/*---------------------------------------------------------------------------
* StGetBackOnTrackAStar
*---------------------------------------------------------------------------*/
StGetBackOnTrackAStar::StGetBackOnTrackAStar(my_context ctx) :
my_base(ctx), StBase<StGetBackOnTrackAStar>(std::string("StGetBackOnTrackAStar")) {
ChsmPlanner& planner = context<ChsmPlanner> ();
Topology* topology = planner.topology_;
assert( ! topology->route_is_finished() );
RndfEdge* edge = (*topology->complete_mission_graph.begin())->edge();
double x, y, psi;
// calculate entry point
Point_2 pp = edge->line().projection(topology->ego_vehicle.point());
Vector_2 nvec = edge->vector();
nvec = nvec / sqrt(nvec.squared_length());
pp = pp + nvec * 12.;
Segment_2 seg = edge->segment();
if (squared_distance(seg, pp) > 0.001) {
if (squared_distance(pp, edge->fromVertex()->point()) < squared_distance(pp, edge->toVertex()->point())) pp = edge->fromVertex()->point();
else pp = edge->toVertex()->point();
}
// check distance to blocking edge
if (sqrt(squared_distance(pp, edge->fromVertex()->point())) < 5.) {
bool bl_edge = false;
for (set<RndfEdge*>::iterator it = edge->fromVertex()->inEdges().begin(); it != edge->fromVertex()->inEdges().end(); ++it) {
if ((*it)->isBlockedEdge()) bl_edge = true;
}
if (bl_edge) pp = edge->toVertex()->point();
}
// set destination point
x = pp.x();
y = pp.y();
psi = atan2(edge->toVertex()->y() - edge->fromVertex()->y(), edge->toVertex()->x() - edge->fromVertex()->x());
// TODO: reactivate gpp / emergency planner
throw VLRException("Recover mode not reimplemented..");
next_replan = drc::Time::current() + GETBACKONTRACK_REPLAN_INTERVAL;
}
FT move(Triangulation& Tp, const FT dt , FT& dd0 ) {
vector<data_kept> prev;
FT dd2=0;
bool first=false; // debug
for(F_v_it fv=Tp.finite_vertices_begin();
fv!=Tp.finite_vertices_end();
fv++) {
data_kept data(fv);
Vector_2 vel = fv->U();
Vector_2 disp = dt * vel;
Periodic_point rr=Tp.periodic_point(fv);
Point rnow=Tp.point(rr); // current point
Point r0=fv->rold(); // starting point
Point rnew= r0 + disp;
Vector_2 disp2 = per_vect(rnew,rnow);
FT rel_disp = sqrt(disp2.squared_length() ) / simu.h();
FT rel_disp0= sqrt( disp.squared_length() ) / simu.h();
if(first) {
cout
<< "r0 " << r0 << " "
<< "rnow " << rnow << " "
<< "rnew " << rnew << " "
<< "disp2 " << disp2 << " "
<< " idx " << fv->idx() << " "
<< "rel_disp " << rel_disp
<< endl ;
first=false;
}
dd2 += rel_disp;
dd0 += rel_disp0;
// cout << "New position: " << r0 ;
data.pos = per_point( rnew );
// cout << " ---> " << data.pos << endl ;
prev.push_back (data);
}
// cout << "relative displacement " << sqrt(dd2)/simu.no_of_points()/simu.h() << endl ;
dd2 /= simu.no_of_particles();
dd0 /= simu.no_of_particles();
// cout << "relative displacement " << dd2 << endl ;
Tp.clear(); // clears the triangulation !!
for(vector<data_kept>::iterator data=prev.begin();
data!=prev.end();
data++) {
// cout << "Inserting back at " << data->pos << endl ;
Vertex_handle fv=Tp.insert(data->pos);
data->restore(fv);
// return info to vertices
}
// cout << "Insertion done" << endl ;
Tp.convert_to_1_sheeted_covering();
return dd2;
}