// 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);
}
bool WCV_tvar::violation(const Vect3& so, const Velocity& vo, const Vect3& si, const Velocity& vi) const {
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);
return horizontal_WCV(s2,v2) &&
wcv_vertical->vertical_WCV(table.getZTHR(),table.getTCOA(),so.z-si.z,vo.z-vi.z);
}
/**
* Computes 2D intersection point of two lines, but also finds z component (projected by time from line 1)
* @param s0 starting point of line 1
* @param v0 direction vector for line 1
* @param s1 starting point of line 2
* @param v1 direction vector of line 2
* @return Pair (2-dimensional point of intersection with 3D projection, relative time of intersection, relative to the so3)
* If the lines are parallel, this returns the pair (0,NaN).
*/
double VectFuns::timeOfIntersection(const Vect3& so3, const Velocity& vo3, const Vect3& si3, const Velocity& vi3) {
Vect2 so = so3.vect2();
Vect2 vo = vo3.vect2();
Vect2 si = si3.vect2();
Vect2 vi = vi3.vect2();
Vect2 ds = si.Sub(so);
if (vo.det(vi) == 0) {
//f.pln(" $$$ intersection: lines are parallel");
return NaN;
}
double tt = ds.det(vi)/vo.det(vi);
//f.pln(" $$$ intersection: tt = "+tt);
return tt;
}
/**
* Computes 2D intersection point of two lines, but also finds z component (projected by time from line 1)
* @param s0 starting point of line 1
* @param v0 direction vector for line 1
* @param s1 starting point of line 2
* @param v1 direction vector of line 2
* @return Pair (2-dimensional point of intersection with 3D projection, relative time of intersection, relative to the so3)
* If the lines are parallel, this returns the pair (0,NaN).
*/
std::pair<Vect3,double> VectFuns::intersection(const Vect3& so3, const Velocity& vo3, const Vect3& si3, const Velocity& vi3) {
Vect2 so = so3.vect2();
Vect2 vo = vo3.vect2();
Vect2 si = si3.vect2();
Vect2 vi = vi3.vect2();
Vect2 ds = si.Sub(so);
if (vo.det(vi) == 0) {
//f.pln(" $$$ intersection: lines are parallel");
return std::pair<Vect3,double>(Vect3::ZERO(), NaN);
}
double tt = ds.det(vi)/vo.det(vi);
//f.pln(" $$$ intersection: tt = "+tt);
Vect3 intersec = so3.Add(vo3.Scal(tt));
double nZ = intersec.z;
double maxZ = Util::max(so3.z,si3.z);
double minZ = Util::min(so3.z,si3.z);
if (nZ > maxZ) nZ = maxZ;
if (nZ < minZ) nZ = minZ;
return std::pair<Vect3,double>(intersec.mkZ(nZ),tt);
}
/**
* Returns values indicating whether the ownship state will pass in front of or behind the intruder (from a horizontal perspective)
* @param so ownship position
* @param vo ownship velocity
* @param si intruder position
* @param vi intruder velocity
* @return 1 if ownship will pass in front (or collide, from a horizontal sense), -1 if ownship will pass behind, 0 if divergent or parallel
*/
int VectFuns::passingDirection(const Vect3& so, const Velocity& vo, const Vect3& si, const Velocity& vi) {
double toi = timeOfIntersection(so,vo,si,vi);
double tii = timeOfIntersection(si,vi,so,vo); // these values may have opposite sign!
//fpln("toi="+toi);
//fpln("int = "+ intersection(so,vo,si,vi));
if (ISNAN(toi) || toi < 0 || tii < 0) return 0;
Vect3 so3 = so.linear(vo, toi);
Vect3 si3 = si.linear(vi, toi);
//fpln("so3="+so3);
//fpln("si3="+si3);
if (behind(so3.vect2(), si3.vect2(), vi.vect2())) return -1;
return 1;
}
int VectFuns::dirForBehind(const Vect3& so, const Velocity& vo, const Vect3& si, const Velocity& vi) {
return dirForBehind(so.vect2(),vo.vect2(),si.vect2(),vi.vect2());
}