Vect2 AziEquiProjection::project2(const LatLonAlt& lla) const {
Vect2 p = sphere_to_plane(ref, spherical2xyz(lla.lat(),lla.lon()));
if (p.norm() <= 0.0) {
return Vect2::ZERO;
} else {
return p.Scal(GreatCircle::distance(lla, llaRef)/p.norm());
}
}
/**
* Return distance at time of closest point of approach
**/
double Vect2::dcpa(const Vect2& so, const Vect2& vo, const Vect2& si, const Vect2& vi) {
double t = tcpa(so,vo,si,vi);
Vect2 s = so.Sub(si);
Vect2 v = vo.Sub(vi);
Vect2 st = s.AddScal(t,v);
return st.norm();
}
bool WCV_tvar::horizontal_WCV(const Vect2& s, const Vect2& v) const {
if (s.norm() <= table.getDTHR()) return true;
if (Horizontal::dcpa(s,v) <= table.getDTHR()) {
double tvar = horizontal_tvar(s,v);
return 0 <= tvar && tvar <= table.getTTHR();
}
return false;
}
/**
* 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;
}
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;
}
string fvStr2(const Vect2& v) {
return "("+Units::str("deg",v.compassAngle())+", "+Units::str("knot",v.norm())+")";
}
LatLonAlt AziEquiProjection::inverse(const Vect2& xy, double alt) const {
double d = std::sin(GreatCircle::angle_from_distance(xy.norm(),0.0))*GreatCircle::spherical_earth_radius;
return xyz2spherical(equator_map_inv(ref, plane_to_sphere(xy.Hat().Scal(d))), alt + projAlt);
}
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()));
}
// This appears to use the right-hand rule to determine it returns the inside or outside angle
double VectFuns::angle_between(const Vect2& v1, const Vect2& v2) {
Vect2 VV1 = v1.Scal(1.0/v1.norm());
Vect2 VV2 = v2.Scal(1.0/v2.norm());
return Util::atan2_safe(VV2.y,VV2.x)-Util::atan2_safe(VV1.y,VV1.x);
}
