// Assumes 0 <= B < T
LossData WCV_tvar::WCV_interval(const Vect3& so, const Velocity& vo, const Vect3& si, const Velocity& vi, double B, double T) const {
double time_in = T;
double time_out = B;
Vect2 so2 = so.vect2();
Vect2 si2 = si.vect2();
Vect2 s2 = so2.Sub(si2);
Vect2 vo2 = vo.vect2();
Vect2 vi2 = vi.vect2();
Vect2 v2 = vo2.Sub(vi2);
double sz = so.z-si.z;
double vz = vo.z-vi.z;
Interval ii = wcv_vertical->vertical_WCV_interval(table.getZTHR(),table.getTCOA(),B,T,sz,vz);
if (ii.low > ii.up) {
return LossData(time_in,time_out);
}
Vect2 step = v2.ScalAdd(ii.low,s2);
if (Util::almost_equals(ii.low,ii.up)) { // [CAM] Changed from == to almost_equals to mitigate numerical problems
if (horizontal_WCV(step,v2)) {
time_in = ii.low;
time_out = ii.up;
}
return LossData(time_in,time_out);
}
LossData ld = horizontal_WCV_interval(ii.up-ii.low,step,v2);
time_in = ld.getTimeIn() + ii.low;
time_out = ld.getTimeOut() + ii.low;
return LossData(time_in,time_out);
}
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);
}
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);
}