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.
130 lines
3.4 KiB
C++
130 lines
3.4 KiB
C++
#include "precompiled.h"
|
|
#include "0ad_warning_disable.h"
|
|
# include "sr_cfg_planner.h"
|
|
|
|
//# define SR_USE_TRACE1 // start
|
|
//# define SR_USE_TRACE2 // update
|
|
//# define SR_USE_TRACE3 // bridge
|
|
# include "sr_trace.h"
|
|
|
|
//============================= SrCfgPlannerBase ========================================
|
|
|
|
SrCfgPlannerBase::SrCfgPlannerBase ( SrCfgManagerBase* cman )
|
|
:_tree1 ( cman ), _tree2 ( cman ), _path ( cman )
|
|
{
|
|
_cman = cman;
|
|
|
|
_tmpc1 = _cman->alloc();
|
|
_tmpc2 = _cman->alloc();
|
|
|
|
_solved = false;
|
|
_juststarted = false;
|
|
_curtree = 1;
|
|
}
|
|
|
|
SrCfgPlannerBase::~SrCfgPlannerBase ()
|
|
{
|
|
init ();
|
|
_cman->free ( _tmpc1 );
|
|
_cman->free ( _tmpc2 );
|
|
}
|
|
|
|
void SrCfgPlannerBase::init ()
|
|
{
|
|
_tree1.init ();
|
|
_tree2.init ();
|
|
_path.init ();
|
|
_solved = false;
|
|
_juststarted = false;
|
|
_curtree = 1;
|
|
}
|
|
|
|
void SrCfgPlannerBase::start ( const srcfg* c1, const srcfg* c2 )
|
|
{
|
|
SR_TRACE1 ( "Start...");
|
|
init ();
|
|
|
|
SR_TRACE1 ( "Tree Init...");
|
|
_tree1.init ( c1 );
|
|
_tree2.init ( c2 );
|
|
_curtree = 1; // could be 1 or 2
|
|
_juststarted = true;
|
|
|
|
SR_TRACE1 ( "Start OK.");
|
|
}
|
|
|
|
bool SrCfgPlannerBase::update_rrt ( 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 ( _try_to_join(_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 ( _try_to_join(nearest1,nearest2,prec) ) return true; // FOUND
|
|
SR_TRACE2 ( "UPDT: not found." );
|
|
return false; // not found
|
|
}
|
|
|
|
SR_TRACE2 ( "UPDT: nearest1="<<nearest1->id()<<" nearest2="<<nearest2->id() );
|
|
SR_TRACE2 ( "UPDT: expanding..." );
|
|
if ( _curtree==1 )
|
|
{ if ( _cman->monotone ( nearest1->cfg(), crand ) )
|
|
{ n = _tree1.expand_node_safe ( nearest1, crand, step, tries, prec, dist1 );
|
|
if ( n )
|
|
{ if ( _try_to_join(n,nearest2,prec) ) return true; // FOUND
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{ if ( _cman->monotone ( crand, nearest2->cfg() ) )
|
|
{ n = _tree2.expand_node_safe ( nearest2, crand, step, tries, prec, dist2 );
|
|
if ( n )
|
|
{ if ( _try_to_join(nearest1,n,prec) ) return true; // FOUND
|
|
}
|
|
}
|
|
}
|
|
|
|
_curtree = _curtree==1? 2:1;
|
|
|
|
SR_TRACE2 ( "UPDT: not found." );
|
|
|
|
return false; // not found
|
|
}
|
|
|
|
bool SrCfgPlannerBase::_try_to_join ( SrCfgNode* n1, SrCfgNode* n2, float prec )
|
|
{
|
|
bool b;
|
|
|
|
b = _cman->monotone ( n1->cfg(), n2->cfg() );
|
|
if ( !b ) return false;
|
|
|
|
b = _cman->visible ( n1->cfg(), n2->cfg(), _tmpc2, prec );
|
|
if ( !b ) return false;
|
|
|
|
_path.init ();
|
|
_tree1.get_branch ( n1, _path );
|
|
_path.revert();
|
|
_tree2.get_branch ( n2, _path );
|
|
|
|
_solved = true;
|
|
return _solved;
|
|
}
|
|
|
|
//============================= End of File ===========================================
|
|
|