phosit/0ad
1
0
Fork 0
forked from 0ad/0ad
Code Pull Requests Activity
d83945e1e7
0ad/source/dcdt/se/sr_cfg_tree.cpp
janwas 5bf9bca9ef fix/disable warnings. 
there are too many W4 and "potentially uninitialized", so those are
disabled by 0ad_warning_disable.h.

the silly "int x = strlen" and very dangerous "int x = (void*)p" (and
vice versa) problems are fixed.

This was SVN commit r5526.
2007-12-23 12:18:57 +00:00

560 lines
15 KiB
C++
Raw Blame History

#include "precompiled.h"
#include "0ad_warning_disable.h"
# include "sr_cfg_tree.h"
//# define SR_USE_TRACE1 // expand node
# include "sr_trace.h"
//=============================== SrCfgNode ========================================
float SrCfgNode::prec ( int i ) const
{
int lev = level ( i );
return dist(i)/(float)sr_pow(2,SR_ABS(lev));
}
void SrCfgNode::get_subtree ( SrArray<SrCfgNode*>& nodes )
{
int i;
nodes.push() = this;
for ( i=0; i<_children.size(); i++ ) _children[i].node->get_subtree ( nodes );
}
void SrCfgNode::_deledge ( int e )
{
_children.remove ( e );
int i, size = _children.size();
for ( i=e; i<size; i++ )
_children[i].node->_parentlink = i;
}
void SrCfgNode::_fixplinks ()
{
int i;
for ( i=0; i<_children.size(); i++ )
_children[i].node->_parentlink = i;
for ( i=0; i<_children.size(); i++ )
_children[i].node->_fixplinks();
}
void SrCfgNode::_reroot ()
{
if ( !_parent ) return;
// new parent will have its old parent as new child:
SrCfgNode* curnode = this;
SrCfgNode* curparent = _parent;
int newparentlink = _children.size();
int oldparentlink = _parentlink;
_children.push() = _parent->_children[oldparentlink];
_children.top().node = _parent;
_parent = 0;
_parentlink = -1;
// walk towards the old root, swaping old/new parents:
int tmp;
SrCfgNode* newparent = this;
curnode = curparent;
curparent = curnode->_parent; // move to parent
while ( curparent )
{ // modify curnode:
curnode->_children[oldparentlink] = curparent->_children[curnode->_parentlink];
curnode->_children[oldparentlink].node = curparent; // parent becomes child
tmp = oldparentlink;
oldparentlink = curnode->_parentlink;
curnode->_parentlink = newparentlink;
newparentlink = tmp;
curnode->_parent = newparent;
// move to parent:
newparent = curnode;
curnode = curparent;
curparent = curparent->_parent;
}
// delete extra child of the old root:
curnode->_deledge ( oldparentlink );
curnode->_parentlink = newparentlink;
curnode->_parent = newparent;
}
//============================= SrCfgTreeBase ======================================
SrCfgTreeBase::SrCfgTreeBase ( SrCfgManagerBase* cman )
{
_cman = cman;
_cman->ref ();
_root = 0;
}
SrCfgTreeBase::~SrCfgTreeBase ()
{
int i;
for ( i=0; i<_buffer.size(); i++ )
{ _cman->free ( _buffer[i]->_cfg );
delete _buffer[i];
}
_cman->unref();
}
bool SrCfgTreeBase::check_all ( SrOutput& o )
{
int i, j;
o << "Starting check:\n";
if ( !_root ) { o<<"Check ok, but empty.\n"; return true; }
o << "Buffer size...\n";
_nodes.size ( 0 );
_root->get_subtree ( _nodes );
if ( _nodes.size()!=_buffer.size()-_freepos.size() ) goto error;
o << "Buffer indices...\n";
for ( i=0; i<_buffer.size(); i++ )
if ( _buffer[i]->_bufferid!=i ) goto error;
o << "Parent-child pointers...\n";
for ( i=0; i<_nodes.size(); i++ )
{ if ( !_nodes[i]->_parent )
{ if ( _nodes[i]!=_root ) { o<<"wrong root "; goto error; } }
else
{ if ( _nodes[i]->_parent->child(_nodes[i]->_parentlink)!=_nodes[i] )
{ o<< "wrong parent link n:" <<
_nodes[i]->id()<<" p:"<<_nodes[i]->_parent->id()<<srspc; goto error; }
}
}
o << "Children pointers...\n";
for ( i=0; i<_nodes.size(); i++ )
{ for ( j=0; j<_nodes[i]->children(); j++ )
{ if ( _nodes[i]->_children[j].node->_parent!=_nodes[i] )
{ o<<"wrong child->_parent pointer "; goto error; }
if ( _nodes[i]->_children[j].node->_parentlink!=j )
{ o<<"wrong child->_parentlink index "<<
_nodes[i]->id()<<"/"<<j<<srspc; goto error; }
}
}
o << "Check ok.\n";
return true;
error:
o << "error!\n";
return false;
}
void SrCfgTreeBase::init ()
{
_root = 0;
_freepos.size ( _buffer.size() );
int i, max = _buffer.size()-1;
for ( i=0; i<=max; i++ ) _freepos[i] = max-i;
}
void SrCfgTreeBase::init ( const srcfg* cfg )
{
init ();
add_node ( 0, cfg, 0 );
}
SrCfgNode* SrCfgTreeBase::_newnode ()
{
if ( _freepos.size()>0 )
{ return _buffer[_freepos.pop()];
}
else
{ int id = _buffer.size();
_buffer.push() = new SrCfgNode;
_buffer[id]->_cfg = _cman->alloc();
_buffer[id]->_bufferid = id;
return _buffer[id];
}
}
void SrCfgTreeBase::_delnode ( SrCfgNode* n )
{
_freepos.push() = n->_bufferid;
}
SrCfgNode* SrCfgTreeBase::add_node ( SrCfgNode* parent, const srcfg* cfg, float dist )
{
SrCfgNode* newn = _newnode ();
if ( parent ) // set parent data
{ newn->_parentlink = parent->children();
SrCfgNode::Link& l = parent->_children.push();
if ( dist<0 ) dist = _cman->dist ( parent->_cfg, cfg );
l.node = newn;
l.dist = dist;
l.level = 0;
}
else // this is the root node
{ _root = newn;
newn->_parentlink = -1;
}
_cman->copy ( newn->_cfg, cfg );
newn->_parent = parent;
newn->_children.size(0);
return newn;
}
void SrCfgTreeBase::_nearest ( SrCfgNode* n, const srcfg* c, SrCfgNode*& nearest, float& mindist )
{
// check distance:
float dist = _cman->dist ( n->cfg(), c );
if ( dist<mindist )
{ nearest = n;
mindist = dist;
}
// recurse:
int i, chsize=n->children();
for ( i=0; i<chsize; i++ )
_nearest ( n->child(i), c, nearest, mindist );
}
SrCfgNode* SrCfgTreeBase::search_nearest ( const srcfg* c, float* d )
{
if ( !_root ) return _root;
SrCfgNode* nearest;
float mindist = 1E+30f; // float range in visualc is: 3.4E +/- 38
_nearest ( _root, c, nearest, mindist );
if (d) *d = mindist;
return nearest;
}
SrCfgNode* SrCfgTreeBase::expand_node ( SrCfgNode* source, const srcfg* direction,
float step, int maxtries, float dist )
{
srcfg* csource = source->cfg();
if ( dist<0 ) dist = _cman->dist ( csource, direction );
if ( dist<0.00001f ) return 0; // too close
SrCfgNode* nnew = _newnode();
srcfg* cnew = nnew->cfg();
float t = step/dist;
if ( t>1.0f ) t = 1.0f;
while ( maxtries-->0 )
{ SR_TRACE1 ( "INS: trying to insert...");
_cman->interp ( csource, direction, t, cnew );
t /= 2;
if ( _cman->valid(cnew) )
{ SR_TRACE1 ( "INS: inserting 1 node.");
_delnode ( nnew );
return add_node ( source, cnew, -1 ); // instead of -1, could use aprox distance dist*t...
}
}
SR_TRACE1 ( "INS: no nodes inserted.");
_delnode ( nnew );
return 0;
}
SrCfgNode* SrCfgTreeBase::expand_node_safe ( SrCfgNode* source, const srcfg* direction,
float step, int maxtries, float prec, float dist )
{
srcfg* csource = source->cfg();
if ( dist<0 ) dist = _cman->dist ( csource, direction );
if ( dist<0.00001f ) return 0; // too close
SrCfgNode* nnew = _newnode();
srcfg* cnew = nnew->cfg();
SrCfgNode* ntmp = _newnode();
srcfg* ctmp = ntmp->cfg();
float t = step/dist;
if ( t>1.0f ) t = 1.0f;
if ( 0 ) // test "long" expansion
{ float dt = t;
maxtries=5;
while ( t<=1 && maxtries-->0 )
{ SR_TRACE1 ( "INS: trying to insert...");
_cman->interp ( csource, direction, t, cnew );
t += dt;
if ( _cman->valid(cnew) )
if ( _cman->visible(source->cfg(),cnew,ctmp,prec) )
{ SR_TRACE1 ( "INS: inserting 1 node.");
_delnode ( nnew );
_delnode ( ntmp );
nnew = add_node ( source, cnew, -1 ); // instead of -1, could use aprox distance dist*t...
source->_children.top().level = -1; // mark as safe (can be any <0 number)
_cman->child_added ( source->cfg(), cnew ); // notify configuration manager
SR_TRACE1 ( "INS: Ok.");
source = nnew;
if ( t>1 ) return nnew; // end
nnew = _newnode();
cnew = nnew->cfg();
ntmp = _newnode();
ctmp = ntmp->cfg();
}
}
}
else
while ( maxtries-->0 )
{ SR_TRACE1 ( "INS: trying to insert...");
_cman->interp ( csource, direction, t, cnew );
t /= 2;
if ( _cman->valid(cnew) )
if ( _cman->visible(source->cfg(),cnew,ctmp,prec) )
{ SR_TRACE1 ( "INS: inserting 1 node.");
_delnode ( nnew );
_delnode ( ntmp );
nnew = add_node ( source, cnew, -1 ); // instead of -1, could use aprox distance dist*t...
source->_children.top().level = -1; // mark as safe (can be any <0 number)
_cman->child_added ( source->cfg(), cnew ); // notify configuration manager
SR_TRACE1 ( "INS: Ok.");
return nnew;
//following test not ok:
//return expand_node_safe ( nnew, direction, step, maxtries, prec, -1 );
}
}
SR_TRACE1 ( "INS: no nodes inserted.");
_delnode ( nnew );
_delnode ( ntmp );
return 0;
}
void SrCfgTreeBase::get_branch ( SrCfgNode* n, SrArray<SrCfgNode*>& nodes )
{
while ( n )
{ nodes.push() = n;
n = n->parent();
}
}
void SrCfgTreeBase::get_branch ( SrCfgNode* n, SrCfgPathBase& path )
{
while ( n )
{ path.push ( n->cfg() );
n = n->parent();
}
}
void SrCfgTreeBase::get_nodes ( SrArray<SrCfgNode*>& nodes )
{
nodes.size(0);
if ( !_root ) return;
_root->get_subtree ( nodes );
}
// level k : (2^k)+1 tot tests, 2^(k-1) new tests, 2^k segments
// safe edge : dist/segments < collision_precision
bool SrCfgTreeBase::increment_edge_level ( SrCfgNode* n1, SrCfgNode* n2, float prec )
{
SrCfgNode::Link& l = n1->_children[n2->parentlink()];
SrCfgNode* tmpnode = _newnode();
srcfg* ct = tmpnode->cfg();
srcfg* ct0 = n1->cfg();
srcfg* ct1 = n2->cfg();
// we will test the next level k:
int k = 1 + l.level;
//sr_out<<"K: "<<k<<srnl;
// test the 2^(k-1) new tests of the new level k:
float segs = (float) sr_pow ( 2, k );
//float dseg = (l.dist/segs<=prec)? prec : 1.0f/segs; // test if last pass
float dseg = 1.0f/segs;
float dt = dseg*2.0f;
float t;
for ( t=dseg; t<1.0f; t+=dt )
{ //sr_out<<"t: "<<t<<srnl;
_cman->interp ( ct0, ct1, t, ct );
if ( !_cman->valid(ct) ) { _delnode(tmpnode); return false; }
}
// ok, promote edge to level k and check if it can be marked as safe:
l.level = k;
//sr_out<<"dist:"<<l.dist<<" segs:"<<segs<<" prec:"<<prec<<srnl;
if ( l.dist/segs<=prec ) l.level=-l.level; // mark as safe
_delnode(tmpnode);
return true;
}
void SrCfgTreeBase::transfer_subtree ( SrCfgNode* n1, SrCfgNode* n2, SrCfgNode* joint1,
SrCfgTreeBase& tree2, SrCfgNode* joint2 )
{
// delete edge [n1,n2]:
n1->_deledge ( n2->parentlink() );
n2->_parent=0; // n2 is now the root of a disconnected subtree, still using _buffer
n2->_parentlink=-1;
// save the nodes of subtree n2:
_nodes.size ( 0 );
n2->get_subtree ( _nodes );
// reorder subtree to make joint1 the new root:
joint1->_reroot();
// attach joint1 children as children of joint2:
while ( joint1->_children.size() )
{ SrCfgNode::Link& l = joint1->_children.pop();
l.node->_parent = joint2;
l.node->_parentlink = joint2->_children.size();
joint2->_children.push() = l;
}
// finally reorganize buffers; all in _nodes change buffer, except joint1 deleted:
int tmpi;
SrCfgNode *n, *newn, *tmpn;
while ( _nodes.size() )
{ n = _nodes.pop();
_delnode (n);
if ( n!=joint1 ) // "swap" with a new entry in tree2.buffer
{ newn = tree2._newnode();
SR_SWAPT(_buffer[n->_bufferid],tree2._buffer[newn->_bufferid],tmpn);
SR_SWAPT(n->_bufferid,newn->_bufferid,tmpi);
}
}
}
void SrCfgTreeBase::output ( SrOutput& o, bool printcfg, SrCfgNode* n )
{
int i, j;
_nodes.size(0);
if ( !n ) n = _root;
n->get_subtree ( _nodes );
o << "\nSrCfgTree " << _nodes.size() << srnl;
for ( i=0; i<_nodes.size(); i++ )
{
n = _nodes[i];
//t._cman->output ( o, t.cfg(i) );
o << "Node:" << n->id();
if ( n->parent() ) o << " parent:"<<n->parent()->id();
else o<<" root ";
o << " Children:";
for ( j=0; j<n->children(); j++ )
{ o << srspc << n->child(j)->id()
<< ":" << j
<< "/" << n->child(j)->parentlink();
}
if ( printcfg )
{ o << "\nData: ";
_cman->output(o,n->cfg());
o <<"\n";
}
o << srnl;
}
}
//================================ friends =================================================
/*
void _inpnode ( SrInput& i, SrCfgNode* n );
void SrCfgTreeBase::_inpnode ( SrInput& in, SrCfgNode* n )
{
int i, size;
_cman->input ( in, n->_cfg );
in.get_token(); // "p"
in >> n->_parent;
in.get_token(); // "c"
in >> size;
in.get_token(); // ":"
n->_children.size(size);
for ( i=0; i<size; i++ )
{ in >> n->_children[i].dist;
in >> n->_children[i].node;
in >> n->_children[i].level;
}
}*/
/*! read the roadmap */
/*friend SrInput& operator>> ( SrInput& in, SrCfgTreeBase& t );
SrInput& operator>> ( SrInput& in, SrCfgTreeBase& t )
{
int i, size;
t.init();
in.get_token();
if ( in.last_token()!="SrCfgTree" ) return in;
in >> size;
for ( i=0; i<size; i++ )
{
// make sure next entry is valid
if ( i==t._nodes.size() ) t._pushnode();
// read node
t._cman->input ( in, t.cfg(i) );
t._inpnode ( in, t.node(i) );
}
t._nsize = size;
return in;
}
*/
//================================ End of File =================================================
/*
This method takes O(k*n) time. It looks into all nodes of the graph,
updating the sorted list with the k closest nodes, which is returned.
SrArray<SrCfgTreeBase::NodeDist>& SrCfgTreeBase::search_k_nearests ( const srcfg* c, int k )
{
_snodes.ensure_capacity(k);
_snodes.size(0);
SrRoadmapNode* n = _graph.first_node();
if ( !n ) return _snodes;
NodeDist nd;
int i, worse=-1;
SrListIterator<SrRoadmapNode> it(n);
for ( it.first(); it.inrange(); it.next() )
{ if ( it.get()->blocked() ) continue;
nd.n = it.get();
nd.d = _cman->get_distance ( it.get()->c, c );
if ( _snodes.size()<k )
{ if ( worse<0 )
worse=_snodes.size();
else
if ( nd.d < _snodes[worse].d ) worse=_snodes.size();
_snodes.push() = nd;
}
else if ( nd.d < _snodes[worse].d )
{ _snodes[worse] = nd;
worse=0;
for ( i=1; i<k; i++ )
if ( _snodes[i].d > _snodes[worse].d ) worse=i;
}
}
if ( _snodes.size()>1 ) _snodes.sort(nd_compare);
return _snodes;
}
*/
View Git Blame Copy Permalink