// 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);
}
/* Solve the following equation on vz:
* sz+t*vz = eps*H,
*
* where t = Theta_D(s,v,epsp).
* eps determines the bottom, i.e.,-1, or top, i.e., 1, circle.
*/
Vertical Vertical::vs_circle(const Vect3& s, const Vect3& vo, const Vect3& vi,
const int eps, const double D, const double H) {
Vect2 s2 = s.vect2();
Vect2 vo2 = vo.vect2();
Vect2 vi2 = vi.vect2();
Vect2 v2 = vo2-vi2;
if (vo2.almostEquals(vi2) && eps == sign(s.z))
return Vertical(vi.z);
else if (Horizontal::Delta(s2,v2,D) > 0)
return vs_only(s,v2,Horizontal::Theta_D(s2,v2,larcfm::Exit,D),eps,D,H).add_this(vi.z);
return NoVerticalSolution;
}
/**
* 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;
}
Vertical vs_only(const Vect3& s, const Vect2& v, const double t,
const int eps, const double D, const double H) {
Vect2 s2 = s.vect2();
if (eps*s.z < H && s2.sqv() > sq(D) && Horizontal::Delta(s2,v,D) > 0)
return vs_at(s.z,Horizontal::Theta_D(s2,v,larcfm::Entry,D),eps,H);
else if (eps*s.z >= H && t > 0)
return vs_at(s.z,t,eps,H);
return Vertical::NoVerticalSolution;
}
/**
* 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);
}
Vect3 VectFuns::closestPoint(const Vect3& a, const Vect3& b, const Vect3& so) {
if (a.almostEquals(b)) return Vect3::INVALID();
Vect2 c = closestPoint(a.vect2(), b.vect2(), so.vect2());
Vect3 v = b.Sub(a);
double d1 = v.vect2().norm();
double d2 = c.Sub(a.vect2()).norm();
double d3 = c.Sub(b.vect2()).norm();
double f = d2/d1;
if (d3 > d1 && d3 > d2) { // negative direction
f = -f;
}
return a.AddScal(f, v);
// Vect3 v = a.Sub(b).PerpL().Hat2D(); // perpendicular vector to line
// Vect3 s2 = so.AddScal(100, v);
// std::pair<Vect3, double> i = intersectionAvgZ(a,b,100,so,s2);
// // need to fix altitude to be along the a-b line
// return interpolate(a,b,i.second/100.0);
}
string fvStr2(const Vect3& v) {
return "("+Units::str("deg",v.vect2().compassAngle())+", "+Units::str("knot",v.norm())+", "+Units::str("fpm",v.z)+")";
}
LatLonAlt AziEquiProjection::inverse(const Vect3& xyz) const {
return inverse(xyz.vect2(), xyz.z);
}
bool VectFuns::divergentHorizGt(const Vect3& s, const Vect3& vo, const Vect3& vi, double minRelSpeed) {
return divergentHorizGt(s.vect2(), vo.vect2(), vi.vect2(), minRelSpeed);
}
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());
}
