LossData WCV_TAUMOD::horizontal_WCV_interval(double T, const Vect2& s, const Vect2& v) const {
double time_in = T;
double time_out = 0;
double sqs = s.sqv();
double sdotv = s.dot(v);
double sqD = Util::sq(table.getDTHR());
double a = v.sqv();
double b = 2*sdotv+table.getTTHR()*v.sqv();
double c = sqs+table.getTTHR()*sdotv-sqD;
if (Util::almost_equals(a,0) && sqs <= sqD) { // [CAM] Changed from == to almost_equals to mitigate numerical problems
time_in = 0;
time_out = T;
return LossData(time_in,time_out);
}
if (sqs <= sqD) {
time_in = 0;
time_out = std::min(T,Horizontal::Theta_D(s,v,1,table.getDTHR()));
return LossData(time_in,time_out);
}
double discr = Util::sq(b)-4*a*c;
if (sdotv >= 0 || discr < 0)
return LossData(time_in,time_out);
double t = (-b - std::sqrt(discr))/(2*a);
if (Horizontal::Delta(s, v,table.getDTHR()) >= 0 && t <= T) {
time_in = std::max(0.0,t);
time_out = std::min(T, Horizontal::Theta_D(s,v,1,table.getDTHR()));
}
return LossData(time_in,time_out);
}
LossData WCV_TEP::horizontal_WCV_interval(double T, const Vect2& s, const Vect2& v) const {
double time_in = T;
double time_out = 0;
double sqs = s.sqv();
double sqv = v.sqv();
double sdotv = s.dot(v);
double sqD = Util::sq(table.getDTHR());
if (Util::almost_equals(sqv,0) && sqs <= sqD) { // [CAM] Changed from == to almost_equals to mitigate numerical problems
time_in = 0;
time_out = T;
return LossData(time_in,time_out);
}
if (Util::almost_equals(sqv,0)) // [CAM] Changed from == to almost_equals to mitigate numerical problems
return LossData(time_in,time_out);
if (sqs <= sqD) {
time_in = 0;
time_out = Util::min(T,Horizontal::Theta_D(s,v,1,table.getDTHR()));
return LossData(time_in,time_out);
}
if (sdotv > 0 || Horizontal::Delta(s,v,table.getDTHR()) < 0)
return LossData(time_in,time_out);
double tep = Horizontal::Theta_D(s,v,-1,table.getDTHR());
if (tep-table.getTTHR() > T)
return LossData(time_in,time_out);
time_in = Util::max(0.0,tep-table.getTTHR());
time_out = Util::min(T,Horizontal::Theta_D(s,v,1,table.getDTHR()));
return LossData(time_in,time_out);
}
void TCAS2D::RA2D_interval(double DMOD, double Tau, double B, double T, Vect2 s, Vect2 vo, Vect2 vi) {
t_in = B;
t_out = T;
Vect2 v = vo.Sub(vi);
double sqs = s.sqv();
double sdotv = s.dot(v);
double sqD = Util::sq(DMOD);
if (vo.almostEquals(vi) && sqs <= sqD)
return;
double sqv = v.sqv();
if (sqs <= sqD) {
t_out = Util::root2b(sqv,sdotv,sqs-sqD,1);
return;
}
double b = 2*sdotv+Tau*sqv;
double c = sqs+Tau*sdotv-sqD;
if (sdotv >= 0 || Util::discr(sqv,b,c) < 0) {
t_in = T+1;
t_out = 0;
return;
}
t_in = Util::root(sqv,b,c,-1);
if (Horizontal::Delta(s,v,DMOD) >= 0)
t_out = Horizontal::Theta_D(s,v,1,DMOD);
else
t_out = Util::root(sqv,b,c,1);
}
// Compute modified tau
double TCAS2D::tau_mod(double DMOD, const Vect2& s, const Vect2& v) {
double sdotv = s.dot(v);
if (Util::almost_equals(sdotv,0)) // [CAM] Changed from == to almost_equals to mitigate numerical problems
return 0;
return (Util::sq(DMOD)-s.sqv())/sdotv;
}
double WCV_TEP::horizontal_tvar(const Vect2& s, const Vect2& v) const {
// Time variable is Modified Tau
double TEP = -1;
double sdotv = s.dot(v);
if (sdotv < 0)
return (Util::sq(table.getDTHR())-s.sqv())/sdotv;
return TEP;
}
double TCAS2D::time_of_min_tau(double DMOD, double B, double T, const Vect2& s, const Vect2& v) {
if (v.ScalAdd(B,s).dot(v) >= 0)
return B;
double d = Horizontal::Delta(s,v,DMOD);
double rr = 0;
if (d < 0)
rr = 2*std::sqrt(-d) / v.sqv();
if (v.ScalAdd(T,s).dot(v) < 0)
return T;
return nominal_tau(B,T,s,v,rr);
}
/**
* Return time to closest point approach
* if time is negative or velocities are parallel returns 0
*/
double Vect2::tcpa (const Vect2& so, const Vect2& vo, const Vect2& si, const Vect2& vi){
double t;
Vect2 s = so.Sub(si);
Vect2 v = vo.Sub(vi);
double nv = v.sqv();
if (nv > 0)
t = std::max(0.0,-s.dot(v)/nv);
else
t = 0;
return t;
}
Vect2 TangentLine::Q(const Vect2& s, const double D, const int eps) {
double sq_s = s.sqv();
double sq_D = sq(D);
double delta = sq_s -sq_D;
if (delta >= 0) {
double alpha = sq_D/sq_s;
double beta = D*sqrt_safe(delta)/sq_s;
return Vect2(alpha*s.x+eps*beta*s.y,
alpha*s.y-eps*beta*s.x);
}
return Vect2();
}
LossData WCV_TCPA::horizontal_WCV_interval(double T, const Vect2& s, const Vect2& v) const {
double time_in = T;
double time_out = 0;
double sqs = s.sqv();
double sqv = v.sqv();
double sdotv = s.dot(v);
double sqD = Util::sq(table.getDTHR());
if (Util::almost_equals(sqv,0) && sqs <= sqD) { // [CAM] Changed from == to almost_equals to mitigate numerical problems
time_in = 0;
time_out = T;
return LossData(time_in,time_out);
}
if (Util::almost_equals(sqv,0)) // [CAM] Changed from == to almost_equals to mitigate numerical problems
return LossData(time_in,time_out);
if (sqs <= sqD) {
time_in = 0;
time_out = std::min(T,Horizontal::Theta_D(s,v,1,table.getDTHR()));
return LossData(time_in,time_out);
}
if (sdotv > 0)
return LossData(time_in,time_out);
double tcpa = Horizontal::tcpa(s,v);
if (v.ScalAdd(tcpa, s).norm() > table.getDTHR())
return LossData(time_in,time_out);
double Delta = Horizontal::Delta(s,v,table.getDTHR());
if (Delta < 0 && tcpa - table.getTTHR() > T)
return LossData(time_in,time_out);
if (Delta < 0) {
time_in = std::max(0.0,tcpa-table.getTTHR());
time_out = std::min(T,tcpa);
return LossData(time_in,time_out);
}
double tmin = std::min(Horizontal::Theta_D(s,v,-1,table.getDTHR()),tcpa-table.getTTHR());
if (tmin > T)
return LossData(time_in,time_out);
time_in = std::max(0.0,tmin);
time_out = std::min(T,Horizontal::Theta_D(s,v,1,table.getDTHR()));
return LossData(time_in,time_out);
}
double InitTangentLineY (const Vect2& s, const double D, const int eps) {
// double x;
double y;
double sq_s = s.sqv();
double sq_D = sq(D);
if (Util::almost_equals(sq_s,sq_D)) {
// x = eps*s.y;
y = -eps*s.x;
} else {
Vect2 q = TangentLine::Q(s,D,eps);
// x = q.x;
y = q.y;
if (!q.isZero()) {
// x -= s.x;
y -= s.y;
}
}
return y;
}
double TCAS2D::nominal_tau(double B, double T, const Vect2& s, const Vect2& v, double rr) {
if (v.isZero())
return B;
return std::max(B,std::min(T,-s.dot(v) / v.sqv()-rr/2));
}
/**
* returns the perpendicular distance between line defined vy s,v and point q.
* @param s
* @param v
* @param q
*/
double Vect2::distAlong(const Vect2& s, const Vect2& v, const Vect2& q) {
double tp = q.Sub(s).dot(v)/v.sqv();
//f.pln(" $$$ distAlong: tp = "+tp);
return Util::sign(tp)*v.Scal(tp).norm();
}
/**
* returns the perpendicular distance between line defined vy s,v and point q.
* @param s
* @param v
* @param q
*/
double Vect2::distPerp(const Vect2& s, const Vect2& v, const Vect2& q) {
double tp = q.Sub(s).dot(v)/v.sqv();
return s.Add(v.Scal(tp)).Sub(q).norm();
}