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);
}
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);
}
bool TCAS2D::min_tau_undef(double DMOD, double B, double T, Vect2 s, Vect2 v) {
return Horizontal::Delta(s,v,DMOD) >= 0 &&
v.ScalAdd(B,s).dot(v) < 0 && v.ScalAdd(T,s).dot(v) >= 0;
}
bool TCAS2D::horizontal_RA_at(double DMOD, double Tau, const Vect2& s, const Vect2& v, double t) {
const Vect2& sat = v.ScalAdd(t,s);
return horizontal_RA(DMOD,Tau,sat,v);
}
