/*! Rebalance the tree after insertion of a node. */
void GsTreeBase::_rebalance ( GsTreeNode *x )
{
GS_TRACE1("Rebalance");
GsTreeNode *y;
while ( x!=_root && RED(x->parent) )
{ // if ( !x->parent->parent ) REPORT_ERROR
if ( x->parent==x->parent->parent->left )
{ y = x->parent->parent->right;
if ( RED(y) )
{ // handle case 1 (see CLR book, pp. 269)
x->parent->color = GsTreeNode::Black;
y->color = GsTreeNode::Black;
x->parent->parent->color = GsTreeNode::Red;
x = x->parent->parent;
}
else
{ if ( x==x->parent->right )
{ // transform case 2 into case 3 (see CLR book, pp. 269)
x = x->parent;
_rotate_left ( x );
}
// handle case 3 (see CLR book, pp. 269)
x->parent->color = GsTreeNode::Black;
x->parent->parent->color = GsTreeNode::Red;
_rotate_right ( x->parent->parent );
}
}
else
{ y = x->parent->parent->left;
if ( RED(y) )
{ // handle case 1 (see CLR book, pp. 269)
x->parent->color = GsTreeNode::Black;
y->color = GsTreeNode::Black;
x->parent->parent->color = GsTreeNode::Red;
x = x->parent->parent;
}
else
{ if ( x==x->parent->left )
{ // transform case 2 into case 3 (see CLR book, pp. 269)
x = x->parent;
_rotate_right ( x );
}
// handle case 3 (see CLR book, pp. 269)
x->parent->color = GsTreeNode::Black;
x->parent->parent->color = GsTreeNode::Red;
_rotate_left ( x->parent->parent );
}
}
}
}
void SeLct::_funnelstart ( FunnelDeque* funnel, const GsPnt2& apex, SeBase* ent, FunnelPath* fpath, float radius, float extclear )
{
// add the first apex:
GS_TRACE1 ( "Adding funnel apex..." );
funnel->pusht().set ( apex.x, apex.y, 'p', 1 ); // initial point becomes the funnel apex
funnel->apex = apex;
fpath->push() = funnel->top();
// check the entrance type:
int i = ent==_ent[0].s? 0: ent==_ent[1].s? 1:2;
GS_TRACE1 ( "Used entrance "<<i<<" is "<<(_ent[i].type==EntTrivial?"trivial":"not trivial") );
// add entrance correction vertices if they exist:
if ( _ent[i].type==EntNotTrivial )
{ GS_TRACE1 ( "Main entrance side: " << (_ent[i].top?"top":"bottom") );
GS_TRACE1 ( (_ent[i].top?"Top":"Bottom") << " disturbances: "<<(_ent[i].fps1-2) );
GS_TRACE1 ( (_ent[i].top?"Bottom":"Top") << " disturbances: "<<(_ent[i].fps2-2) );
Entrance& e = _ent[i];
if ( e.fps1>2 )
{ int max = e.fps1-1;
GS_TRACE1 ( "Adding "<<e.fps1-2<<" disturbance(s) to the funnel..." );
for ( i=1; i<max; i++ )
_funneladd ( funnel, e.top? 't':'b', fpath, e.fp[i], radius );
}
if ( e.fps2>2 )
{ int max = e.fp.size()-1;
GS_TRACE1 ( "Adding "<<e.fps2-2<<" disturbance(s) to the funnel..." );
for ( i=e.fps1+1; i<max; i++ )
_funneladd ( funnel, e.top? 'b':'t', fpath, e.fp[i], radius );
}
}
// add the two vertices of the channel entrance:
GS_TRACE1 ( "Adding channel first edge to the funnel..." );
double x, y;
GsPnt2 a, b;
_man->get_vertex_coordinates ( ent->vtx(), x, y ); a.set((float)x,(float)y);
_man->get_vertex_coordinates ( ent->nxt()->vtx(), x, y ); b.set((float)x,(float)y);
if ( extclear>0 ) // extra clearance is to be completed
{ _funneladd ( funnel, 'b', fpath, a, radius );
_funneladd ( funnel, 't', fpath, b, radius );
}
else
{ _funneladd ( funnel, 'b', fpath, a, radius );
_funneladd ( funnel, 't', fpath, b, radius );
}
}
/*! Method for left rotation of the tree about a given node. */
void GsTreeBase::_rotate_left ( GsTreeNode *x )
{
GS_TRACE1("Rotate Left");
GsTreeNode *y = x->right;
x->right = y->left;
if ( y->left!=NIL ) y->left->parent=x;
y->parent = x->parent;
if ( x->parent!=NIL )
{ if ( x==x->parent->left ) x->parent->left=y;
else x->parent->right=y;
}
else _root = y;
y->left = x;
x->parent = y;
}
void SeLct::_funnelclose ( FunnelDeque* funnel, FunnelPath* fpath, int ei, float radius )
{
// 1. check if additional funnel vertices are needed due corrections detected
// with arrival tests:
int i;
Entrance& ent = _ent[ei];
if ( ent.type==EntNotTrivial )
{ GS_TRACE1 ( "Arrival has "<<ent.fp.size()-4<<" disturbance(s)..." );
GS_TRACE1 ( "Main arrival side: " << (ent.top?"top":"bottom") );
GS_TRACE1 ( (ent.top?"Top":"Bottom") << " disturbances: "<<ent.fps1-2 );
GS_TRACE1 ( (ent.top?"Bottom":"Top") << " disturbances: "<<ent.fps2-2 );
bool intop = !ent.top; // top entrance means bottom arrival
if ( ent.fps1>2 )
{ //int max = ent.fps1-1;
GS_TRACE1 ( "Adding "<<ent.fps1-2<<" disturbance(s) to the funnel..." );
for ( i=ent.fps1-2; i>0; i-- )
_funneladd ( funnel, intop? 't':'b', fpath, ent.fp[i], radius );
}
if ( ent.fps2>2 )
{ //int max = ent.fp.size()-2;
GS_TRACE1 ( "Adding "<<ent.fps2-2<<" disturbance(s) to the funnel..." );
for ( i=ent.fp.size()-2; i>ent.fps1; i-- )
_funneladd ( funnel, intop? 'b':'t', fpath, ent.fp[i], radius );
}
}
// 2. check where is the zone in the funnel that the goal is:
GsPnt2 pg ( (float)_xg,(float)_yg );
FunnelDeque& dq = *funnel;
bool intop=true;
SeFunnelPt fb;
dq.topmode ( intop );
while ( dq.size()>1 )
{ if ( dq.get().apex ) { intop=!intop; dq.topmode(intop); }
if ( isgoalopening(dq.get(1),dq.get(),pg,radius) ) { fb=dq.get(); break; }
dq.pop();
}
// 3. add the needed portion of the funnel side to the funnel path:
for ( i=0; dq.get(i).apex==0; i++ ); // stop at the apex
while ( --i>=0 ) _fpathpush ( fpath, dq.get(i), radius );
// 4. add the final point:
_fpathpush ( fpath, FunnelPt(pg.x,pg.y,'p',0,radius), radius );
}
void SeLct::make_funnel_path ( GsPolygon& path, float radius, float dang, float extclear )
{
GS_TRACE1 ( "Entering funnel path..." );
if ( radius<=0 ) { SeDcdt::make_funnel_path(path); return; }
path.open ( true );
path.size ( 0 );
if ( _path_result==NoPath )
{ GS_TRACE1 ( "No path to compute." );
return;
}
if ( _path_result==TrivialPath )
{ path.push().set((float)_xi,(float)_yi);
path.push().set((float)_xg,(float)_yg);
GS_TRACE1 ( "Trivial path computed." );
return;
}
if ( _path_result==LocalPath )
{ GS_TRACE1 ( "Computing local path..." );
path.push().set((float)_xi,(float)_yi);
GsArray<GsPnt2>& fp = _ent[0].fp;
bool top = _ent[0].top;
if ( !top ) dang=-dang;
GsVec2 c, v1, v2;
int k, max = fp.size()-2;
for ( k=0; k<max; k++ )
{ c = fp[k+1];
if ( top )
{ v1 = (fp[k]-c).ortho();
v2 = (c-fp[k+2]).ortho();
}
else
{ v1 = (c-fp[k]).ortho();
v2 = (fp[k+2]-c).ortho();
}
path.arc ( c, v1, v2, radius, dang );
}
path.push().set((float)_xg,(float)_yg);
GS_TRACE1 ( "Local path has "<<path.size()<<" vertices." );
return;
}
// ok from now on we are treating the global path search case:
GS_TRACE1 ( "Computing global path..." );
// allocate/access used buffers:
// (we reset autolen to false since this is shared with the optimal search)
if ( !_fpath ) _fpath = new FunnelPath(false); else { _fpath->size(0); _fpath->autolen=false; }
if ( !_fdeque ) _fdeque = new FunnelDeque; else _fdeque->init();
// init auxiliary structures if extra clearance is asked:
if ( extclear>0 ) _finitextcl ( radius, extclear );
// check entrances and start funnel:
_funnelstart ( _fdeque, GsPnt2(_xi,_yi), _channel[0], _fpath, radius, extclear );
if (_funnelcb) _funnelcb(_fudata);
// now add remaining vertices of the funnel:
GS_TRACE1 ( "Running funnel algorithm..." );
int i, j, max = _channel.size()-1;
if ( extclear>0 )
{ SeDcdtSymEdge *s;
float r=radius;
for ( i=0; i<max; i++ )
{ j=i+1;
s = (SeDcdtSymEdge*)_channel[j];
if ( _extcl[j].l=='t' ) // add top vertex
{ r = _extcl[j].r;
_funneladd ( _fdeque, 't', _fpath, s->nvtx()->p, r ); // add top edge vertex
}
else // add bot vertex
{ r = _extcl[j].r;
_funneladd ( _fdeque, 'b', _fpath, s->vtx()->p, r ); // add bottom edge vertex
}
if (_funnelcb) _funnelcb(_fudata);
}
}
else
{ SeDcdtSymEdge *s1, *s2;
for ( i=0; i<max; i++ )
{ s1 = (SeDcdtSymEdge*)_channel[i];
s2 = (SeDcdtSymEdge*)_channel[i+1];
if ( s1->vtx()==s2->vtx() ) // add top vertex
{ GS_TRACE2 ( "Updating funnel top with channel edge "<<(i+1) );
_funneladd ( _fdeque, 't', _fpath, s2->nvtx()->p, radius ); // add top edge vertex
}
else // add bot vertex
{ GS_TRACE2 ( "Updating funnel bottom with channel edge "<<(i+1) );
_funneladd ( _fdeque, 'b', _fpath, s2->vtx()->p, radius ); // add bottom edge vertex
}
if (_funnelcb) _funnelcb(_fudata);
}
}
GS_TRACE1 ( "Path has " << _fpath->size() << " vertices before closure." );
GS_TRACE1 ( "Processing closure..." );
_funnelclose ( _fdeque, _fpath, 3, radius );
// build path approximation based on tangents:
GS_TRACE1 ( "Building curved path approximation..." );
_fpathmake ( _fpath, path, radius, dang );
GS_TRACE1 ( "Done." );
}
/*! Method for restoring red-black properties after deletion. */
void GsTreeBase::_fix_remove ( GsTreeNode *x )
{
GS_TRACE1("Fix Remove");
while ( x!=_root && BLACK(x) )
{
if ( x==x->parent->left )
{ GsTreeNode *w = x->parent->right;
if ( RED(w) )
{ w->color = GsTreeNode::Black;
x->parent->color = GsTreeNode::Red;
_rotate_left ( x->parent );
w = x->parent->right;
}
if ( BLACK(w->left) && BLACK(w->right) )
{ w->color = GsTreeNode::Red;
x = x->parent;
}
else
{ if ( BLACK(w->right) )
{ w->left->color = GsTreeNode::Black;
w->color = GsTreeNode::Red;
_rotate_right ( w );
w = x->parent->right;
}
w->color = x->parent->color;
x->parent->color = GsTreeNode::Black;
w->right->color = GsTreeNode::Black;
_rotate_left ( x->parent );
x = _root;
}
}
else
{ GsTreeNode *w = x->parent->left;
if ( RED(w) )
{ w->color = GsTreeNode::Black;
x->parent->color = GsTreeNode::Red;
_rotate_right ( x->parent );
w = x->parent->left;
}
if ( BLACK(w->left) && BLACK(w->right) )
{ w->color = GsTreeNode::Red;
x = x->parent;
}
else
{ if ( BLACK(w->left) )
{ w->right->color = GsTreeNode::Black;
w->color = GsTreeNode::Red;
_rotate_left ( w );
w = x->parent->left;
}
w->color = x->parent->color;
x->parent->color = GsTreeNode::Black;
w->left->color = GsTreeNode::Black;
_rotate_right ( x->parent );
x = _root;
}
}
}
x->color = GsTreeNode::Black;
}