janwas
5bf9bca9ef
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.
139 lines
4.7 KiB
C++
139 lines
4.7 KiB
C++
#include "precompiled.h"
|
|
#include "0ad_warning_disable.h"
|
|
# include "sr_cfg_planner.h"
|
|
|
|
//# define SR_USE_TRACE1 // not used
|
|
//# define SR_USE_TRACE2 // update
|
|
//# define SR_USE_TRACE3 // bridge
|
|
# include "sr_trace.h"
|
|
|
|
//============================= SrCfgPlannerBase ========================================
|
|
|
|
bool SrCfgPlannerBase::update_lazy ( float step, int tries, float prec )
|
|
{
|
|
SrCfgNode* n; // new node added to the current tree
|
|
SrCfgNode* nearest1; // nearest node in tree1
|
|
SrCfgNode* nearest2; // nearest node in tree2
|
|
float dist1; // distance from crand to nearest1
|
|
float dist2; // distance from crand to nearest2
|
|
srcfg* crand = _tmpc1; // the random configuration
|
|
|
|
SR_TRACE2 ( "UPDT: expanding tree " << _curtree );
|
|
|
|
if ( _juststarted )
|
|
{ _juststarted = false;
|
|
if ( _test_bridge(_tree1.root(),_tree2.root(),prec) ) return true; // FOUND
|
|
}
|
|
|
|
_cman->random ( crand );
|
|
nearest1 = _tree1.search_nearest ( crand, &dist1 );
|
|
nearest2 = _tree2.search_nearest ( crand, &dist2 );
|
|
|
|
float dist = _cman->dist(nearest1->cfg(),nearest2->cfg());
|
|
if ( dist<=step )
|
|
{ if ( _test_bridge(nearest1,nearest2,prec) ) return true; // FOUND
|
|
}
|
|
|
|
SR_TRACE2 ( "UPDT: nearest1="<<nearest1->id()<<" nearest2="<<nearest2->id() );
|
|
SR_TRACE2 ( "UPDT: expanding..." );
|
|
if ( _curtree==1 )
|
|
{ n = _tree1.expand_node ( nearest1, crand, step, tries, dist1 );
|
|
if ( n )
|
|
{ if ( _test_bridge(n,nearest2,prec) ) return true; // FOUND
|
|
}
|
|
}
|
|
else
|
|
{ n = _tree2.expand_node ( nearest2, crand, step, tries, dist2 );
|
|
if ( n )
|
|
{ if ( _test_bridge(nearest1,n,prec) ) return true; // FOUND
|
|
}
|
|
}
|
|
|
|
_curtree = _curtree==1? 2:1;
|
|
|
|
SR_TRACE2 ( "UPDT: not found." );
|
|
|
|
return false; // not found
|
|
}
|
|
|
|
void SrCfgPlannerBase::_heap_add_branch ( SrCfgTreeBase* tree, SrCfgNode* n )
|
|
{
|
|
HeapEdge e;
|
|
SrCfgNode *parent;
|
|
while ( n->parent() )
|
|
{ parent = n->parent();
|
|
if ( !parent->safe(n->parentlink()) )
|
|
{ e.tree = tree;
|
|
e.n1 = parent; // convention: e.n1 is parent of e.n2
|
|
e.n2 = n;
|
|
_heap.insert ( e, parent->level(n->parentlink()) );
|
|
}
|
|
n = parent; // move to the parent
|
|
}
|
|
}
|
|
|
|
/*! Test if the path formed by connecting node index n1 of tree 1 with
|
|
node index n2 of tree2 is a valid path. The test performs collision
|
|
detection in the edges of the path incrementing their levels. A
|
|
priority queue is used to first test edges in lower levels.
|
|
If all edges in the path become safe, a path is formed and true is returned.
|
|
Otherwise, the edge found to be invalid is deleted, the two trees are
|
|
updated to keep the remaining edges, and false is returned */
|
|
bool SrCfgPlannerBase::_test_bridge ( SrCfgNode* n1, SrCfgNode* n2, float prec )
|
|
{
|
|
// add the cfg of n2 to tree1:
|
|
SrCfgNode* n12 = _tree1.add_node ( n1, n2->cfg() );
|
|
|
|
// make priority heap where the cost is the edge level
|
|
// and add all non-safe path edges of tree 1 and tree 2 to the heap
|
|
SR_TRACE3 ( "BRIDGE: building heap..." );
|
|
_heap.init();
|
|
_heap_add_branch ( &_tree1, n12 );
|
|
_heap_add_branch ( &_tree2, n2 );
|
|
|
|
// test and increment the level of the edges in the heap
|
|
int level;
|
|
HeapEdge e;
|
|
while ( _heap.size()>0 )
|
|
{ level = _heap.lowest_cost();
|
|
e = _heap.top();
|
|
|
|
SR_TRACE3 ( "BRIDGE: heap size="<<_heap.size()<<" level="<<level<<" ..." );
|
|
|
|
if ( !e.tree->increment_edge_level ( e.n1, e.n2, prec ) ) break; // collision found
|
|
|
|
// remove and reinsert edge if not yet safe:
|
|
_heap.remove();
|
|
if ( !e.n1->safe(e.n2->parentlink()) )
|
|
_heap.insert ( e, level+1 );
|
|
}
|
|
|
|
if ( _heap.size()==0 ) // all edges were safe: path found
|
|
{ SR_TRACE3 ( "BRIDGE: path found!" );
|
|
_solved = true;
|
|
_path.init ();
|
|
_tree1.get_branch ( n1, _path );
|
|
_path.revert();
|
|
_tree2.get_branch ( n2, _path );
|
|
SR_TRACE3 ( "BRIDGE: path done." );
|
|
}
|
|
else // failed, reorganize trees
|
|
{ SR_TRACE3 ( "BRIDGE: failed, transferring subtrees..." );
|
|
|
|
if ( e.tree==&_tree1 )
|
|
{ //sr_out<<"\nTRANSFER 1: "<<e.n1->id()<<srspc<< e.n2->id()<<srspc<< n12->id()<<srspc<< n2->id()<<srnl;
|
|
_tree1.transfer_subtree ( e.n1, e.n2, n12/*joint1*/, _tree2, n2/*joint2*/ );
|
|
}
|
|
else
|
|
{ //sr_out<<"\nTRANSFER 2: "<<e.n1->id()<<srspc<< e.n2->id()<<srspc<< n2->id()<<srspc<< n12->id()<<srnl;
|
|
_tree2.transfer_subtree ( e.n1, e.n2, n2/*joint1*/, _tree1, n12/*joint2*/ );
|
|
}
|
|
|
|
SR_TRACE3 ( "BRIDGE: transfer done." );
|
|
_solved = false;
|
|
}
|
|
|
|
return _solved;
|
|
}
|
|
|