src/gsim/se_lct_tests.cpp
// Parameter s is the symedge with source vertex (x1,y1) and destination vertex (x2,y2)
Parameter s is the symedge with source vertex (x1,y1) and destination vertex (x2,y2)
Parameter s is the symedge with source vertex ( x1 y1 ) and destination vertex ( x2 y2 )
void SeLct::_collect_capsule_disturbances
            ( SeBase* s, double x1, double y1, double x2, double y2, const Capsule& c, const Sector& sec )
 {
   _mesh->mark ( s->edg() );
   double t;
   bool vfound=false;
   SeVertex* v1 = s->vtx();
   SeVertex* v2 = s->nxt()->vtx();
   if ( !_mesh->marked(v1) )
    { _mesh->mark(v1);
      if ( gs_point_segment_dist2 ( x1, y1, c.ax, c.ay, c.bx, c.by, t )<c.r2 )
       { _ent[1].fv.push()=v1; _ent[1].fp.push().set(x1,y1); vfound=true; }
    }
   if ( !_mesh->marked(v2) )
    { _mesh->mark(v2); 
      if ( gs_point_segment_dist2 ( x2, y2, c.ax, c.ay, c.bx, c.by, t )<c.r2 )
       { _ent[1].fv.push()=v2; _ent[1].fp.push().set(x2,y2); vfound=true; }
    }
   if ( !vfound )
    { if ( gs_point_segment_dist2 ( c.ax, c.ay, x1, y1, x2, y2, t )>=c.r2 &&
           gs_point_segment_dist2 ( c.bx, c.by, x1, y1, x2, y2, t )>=c.r2 )
      return; 
    }
   if ( _man->is_constrained(s->edg()) ) return;
   SeBase* sn = s->ret();
   SeBase* sp = sn->nxt();
   double x3, y3;
   _man->get_vertex_coordinates ( sp->vtx(), x3, y3 );
   if ( !_mesh->marked(sp->edg()) )
    if ( gs_ccw(sec.cx,sec.cy,x3,y3,sec.bx,sec.by)>0 ) 
     { _collect_capsule_disturbances ( sp, x3, y3, x2, y2, c, sec );
     }
   if ( !_mesh->marked(sn->edg()) )
    if ( gs_ccw(sec.cx,sec.cy,sec.ax,sec.ay,x3,y3)>0 ) 
     { _collect_capsule_disturbances ( sn, x1, y1, x3, y3, c, sec );
     }
 }
void SeLct :: _collect_capsule_disturbances ( SeBase * s , double x1 , double y1 , double x2 , double y2 , const Capsule & c , const Sector & sec ) { _mesh -> mark ( s -> edg ( ) ) ; double t ; bool vfound = false ; SeVertex * v1 = s -> vtx ( ) ; SeVertex * v2 = s -> nxt ( ) -> vtx ( ) ; if ( ! _mesh -> marked ( v1 ) ) { _mesh -> mark ( v1 ) ; if ( gs_point_segment_dist2 ( x1 , y1 , c . ax , c . ay , c . bx , c . by , t ) < c . r2 ) { _ent [ 1 ] . fv . push ( ) = v1 ; _ent [ 1 ] . fp . push ( ) . set ( x1 , y1 ) ; vfound = true ; } } if ( ! _mesh -> marked ( v2 ) ) { _mesh -> mark ( v2 ) ; if ( gs_point_segment_dist2 ( x2 , y2 , c . ax , c . ay , c . bx , c . by , t ) < c . r2 ) { _ent [ 1 ] . fv . push ( ) = v2 ; _ent [ 1 ] . fp . push ( ) . set ( x2 , y2 ) ; vfound = true ; } } if ( ! vfound ) { if ( gs_point_segment_dist2 ( c . ax , c . ay , x1 , y1 , x2 , y2 , t ) >= c . r2 && gs_point_segment_dist2 ( c . bx , c . by , x1 , y1 , x2 , y2 , t ) >= c . r2 ) return ; } if ( _man -> is_constrained ( s -> edg ( ) ) ) return ; SeBase * sn = s -> ret ( ) ; SeBase * sp = sn -> nxt ( ) ; double x3 , y3 ; _man -> get_vertex_coordinates ( sp -> vtx ( ) , x3 , y3 ) ; if ( ! _mesh -> marked ( sp -> edg ( ) ) ) if ( gs_ccw ( sec . cx , sec . cy , x3 , y3 , sec . bx , sec . by ) > 0 ) { _collect_capsule_disturbances ( sp , x3 , y3 , x2 , y2 , c , sec ) ; } if ( ! _mesh -> marked ( sn -> edg ( ) ) ) if ( gs_ccw ( sec . cx , sec . cy , sec . ax , sec . ay , x3 , y3 ) > 0 ) { _collect_capsule_disturbances ( sn , x1 , y1 , x3 , y3 , c , sec ) ; } }
Parameter s is the symedge with source vertex (x1,y1) and destination vertex (x2,y2)
Parameter s is the symedge with source vertex ( x1 y1 ) and destination vertex ( x2 y2 )
// Note that: 1) the initial and goal discs are known to be valid, and // 2) their centers are on a same triangle so that no edge separates them. // Therefore if a constrained segment intersects the capsule, one of its // vertices has to be inside the capsule and so point-seg tests are enought // and a seg-seg test below is not needed: // if ( gs_segment_segment_dist2(c1x,c1y,c2x,c2y,x1,y1,x2,y2)>=r2 ) return; // ok enought space // check if there are disturbing vertices: // check if current edge is "far away enought" to stop recursion: // stop recursion // test if we are trying to cross an obstacle: // recurse remaining in the test sector: // c-p3-b // c-a-p3