559 lines
15 KiB
C++
559 lines
15 KiB
C++
|
#include "precompiled.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;
|
||
|
}
|
||
|
|
||
|
|
||
|
|
||
|
*/
|
||
|
|