double Vector_2::distance( Vector_2 v )
{
double dx = x-v.getX();
double dy = y-v.getY();
return sqrt( dx*dx + dy*dy );
}
/*---------------------------------------------------------------------------
* 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;
}
Vector_2 Vector_2::rayCrossPoint( Vector_2 v1, double angle1, Vector_2 v2, double angle2 )
{
double x1 = v1.getX();
double y1 = v1.getY();
double x2 = v2.getX();
double y2 = v2.getY();
double rx1 = cos(angle1);
double ry1 = sin(angle1);
double rx2 = cos(angle2);
double ry2 = sin(angle2);
double detA = ry1*rx2 - rx1*ry2;
if ( detA == 0 ) return Vector_2(0,0,U);
double dx = x2-x1;
double dy = y2-y1;
double lambda = ( dy*rx2 - dx*ry2 ) / detA;
return Vector_2( x1 + lambda*rx1, y1 + lambda*ry1, K );
}
void load_fields_on_fft( const Triangulation& T , CH_FFT& fft ) {
int Nb = fft.Nx();
size_t align=fft.alignment();
c_array ux( Nb , Nb , align );
c_array uy( Nb , Nb , align );
for(F_v_it vit=T.vertices_begin();
vit != T.vertices_end();
vit++) {
int nx = vit->nx.val();
int ny = vit->ny.val();
// "right" ordering
int i = ( Nb - 1 ) - ny ;
int j = nx;
// "wrong" ordering
// int i = nx;
// int j = ny;
Vector_2 vv = vit->Uold.val();
//FT val = vit->alpha.val();
ux(i,j) = vv.x();
uy(i,j) = vv.y();
}
fft.set_u( ux , uy );
return;
}
int quadratic_line_intersection(const double a, const double b, const double c,
const double d, const double e, const double f,
const Point_2& B, const Vector_2& V,
double t[2], Point_2 p_[2])
{
const double A_ = a*V.x()*V.x()+b*V.x()*V.y()+c*V.y()*V.y();
const double B_ = 2*a*B.x()*V.x()+b*(B.y()*V.x()+B.x()*V.y())+2*c*B.y()*V.y()+d*V.x()+e*V.y();
const double C_ = a*B.x()*B.x()+b*B.x()*B.y()+c*B.y()*B.y()+d*B.x()+e*B.y()+f;
int ret = quadratic(A_, B_, C_, t);
if (ret == 0) {
p_[0] = B + t[0]*V;
p_[1] = p_[0];
}
else if (ret == 1) {
p_[0] = B + t[0]*V;
p_[1] = B + t[1]*V;
}
return ret;
}
Point_2 Origin::operator-(const Vector_2& v) const {return Point_2( CGAL::ORIGIN-v.get_data() ); }
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;
}