// 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;
}
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);
}
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);
}
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;
}
Vect2 VectFuns::closestPoint(const Vect2& a, const Vect2& b, const Vect2& so) {
// translate a to origin, then project so onto the line defined by ab, then translate back to a
Vect2 ab = b.Sub(a);
return ab.Scal(so.Sub(a).dot(ab)/ab.dot(ab)).Add(a);
// if (collinear(a,b,so)) return so;
// Vect2 v = a.Sub(b).PerpL().Hat(); // perpendicular vector to line
// Vect2 s2 = so.AddScal(100, v);
// Vect2 cp = intersection(so,s2,100,a,b).first;
// return cp;
}
/**
* 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;
}
/**
* Return actual time of closest point approach (return negative infinity if parallel)
*/
double Vect2::actual_tcpa (const Vect2& so, const Vect2& vo, const Vect2& si, const Vect2& vi){
double rtn;
Vect2 s = so - si;
Vect2 v = vo - vi;
double nv = v.norm();
if (nv > 0) {
rtn = -s.dot(v)/(nv*nv);
} else {
rtn = NINFINITY;;
}
return rtn;
}
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 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));
}
bool TCAS2D::horizontal_RA(double DMOD, double Tau, const Vect2& s, const Vect2& v) {
if (s.dot(v) >= 0) return s.norm() <= DMOD;
else return s.norm() <= DMOD || tau_mod(DMOD,s,v) <= Tau;
}
bool VectFuns::divergentHorizGt(const Vect2& s, const Vect2& vo, const Vect2& vi, double minRelSpeed) {
Vect2 v = vo.Sub(vi);
bool rtn = s.dot(v) > 0 && v.norm() > minRelSpeed;
return rtn;
}
double VectFuns::angle_between(const Vect2& a, const Vect2& b, const Vect2& c) {
Vect2 A = b.Sub(a);
Vect2 B = b.Sub(c);
return Util::acos_safe(A.dot(B)/(A.norm()*B.norm()));
}