/**
* 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();
}
Vect2 VectFuns::closestPoint(const Vect2& a, const Vect2& b, const Vect2& so) {
// translate a to origin, then project so onto the line defined by ab, then translate back to a
Vect2 ab = b.Sub(a);
return ab.Scal(so.Sub(a).dot(ab)/ab.dot(ab)).Add(a);
// if (collinear(a,b,so)) return so;
// Vect2 v = a.Sub(b).PerpL().Hat(); // perpendicular vector to line
// Vect2 s2 = so.AddScal(100, v);
// Vect2 cp = intersection(so,s2,100,a,b).first;
// return cp;
}
/**
* Return time to closest point approach
* if time is negative or velocities are parallel returns 0
*/
double Vect2::tcpa (const Vect2& so, const Vect2& vo, const Vect2& si, const Vect2& vi){
double t;
Vect2 s = so.Sub(si);
Vect2 v = vo.Sub(vi);
double nv = v.sqv();
if (nv > 0)
t = std::max(0.0,-s.dot(v)/nv);
else
t = 0;
return t;
}
void TCAS2D::RA2D_interval(double DMOD, double Tau, double B, double T, Vect2 s, Vect2 vo, Vect2 vi) {
t_in = B;
t_out = T;
Vect2 v = vo.Sub(vi);
double sqs = s.sqv();
double sdotv = s.dot(v);
double sqD = Util::sq(DMOD);
if (vo.almostEquals(vi) && sqs <= sqD)
return;
double sqv = v.sqv();
if (sqs <= sqD) {
t_out = Util::root2b(sqv,sdotv,sqs-sqD,1);
return;
}
double b = 2*sdotv+Tau*sqv;
double c = sqs+Tau*sdotv-sqD;
if (sdotv >= 0 || Util::discr(sqv,b,c) < 0) {
t_in = T+1;
t_out = 0;
return;
}
t_in = Util::root(sqv,b,c,-1);
if (Horizontal::Delta(s,v,DMOD) >= 0)
t_out = Horizontal::Theta_D(s,v,1,DMOD);
else
t_out = Util::root(sqv,b,c,1);
}
Vect2 Vect2::intersect_pt(const Vect2& s0, const Vect2& v0, const Vect2& s1, const Vect2& v1) {
if (Util::almost_equals(v0.det(v1),0.0)) {
//fpln(" $$$$$$$$ ERROR $$$$$$$$$");
return Vect2::INVALID();
} else {
Vect2 delta = s1.Sub(s0);
double ss = delta.det(v1)/v0.det(v1);
return s0.Add(v0.Scal(ss));
}
}
std::pair<Vect2,double> VectFuns::intersection(const Vect2& so, const Vect2& vo, const Vect2& si, const Vect2& vi) {
Vect2 ds = si.Sub(so);
if (vo.det(vi) == 0) {
//f.pln(" $$$ intersection: lines are parallel");
return std::pair<Vect2,double>(Vect2::ZERO(), NaN);
}
double tt = ds.det(vi)/vo.det(vi);
Vect2 intersec = so.Add(vo.Scal(tt));
return std::pair<Vect2,double>(intersec,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).
*/
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;
}
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);
}
/**
* 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);
}
bool VectFuns::collinear(const Vect2& p0, const Vect2& p1, const Vect2& p2) {
// area of triangle = 0? (same as det of sides = 0?)
return p1.Sub(p0).det(p2.Sub(p0)) == 0;
}
/**
* returns the perpendicular distance between line defined vy s,v and point q.
* @param s
* @param v
* @param q
*/
double Vect2::distAlong(const Vect2& s, const Vect2& v, const Vect2& q) {
double tp = q.Sub(s).dot(v)/v.sqv();
//f.pln(" $$$ distAlong: tp = "+tp);
return Util::sign(tp)*v.Scal(tp).norm();
}
std::pair<Vect2,double> VectFuns::intersection(const Vect2& so1, const Vect2& so2, double dto, const Vect2& si1, const Vect2& si2) {
Vect2 vo = so2.Sub(so1).Scal(1/dto);
Vect2 vi = si2.Sub(si1).Scal(1/dto); // its ok to use any time here, all times are relative to so
return intersection(so1,vo,si1,vi);
}
bool VectFuns::rightOfLine(const Vect2& so, const Vect2& vo, const Vect2& si) {
return si.Sub(so).dot(vo.PerpR()) >= 0;
}
/**
* returns the perpendicular distance between line defined vy s,v and point q.
* @param s
* @param v
* @param q
*/
double Vect2::distPerp(const Vect2& s, const Vect2& v, const Vect2& q) {
double tp = q.Sub(s).dot(v)/v.sqv();
return s.Add(v.Scal(tp)).Sub(q).norm();
}
/**
* Returns true if x is "behind" so , that is, x is within the region behind the perpendicular line to vo through so.
*/
bool VectFuns::behind(const Vect2& x, const Vect2& so, const Vect2& vo) {
return Util::turnDelta(vo.trk(), x.Sub(so).trk()) > M_PI/2.0;
}
bool VectFuns::divergent(const Vect2& so, const Vect2& vo, const Vect2& si, const Vect2& vi) {
return so.Sub(si).dot(vo.Sub(vi)) > 0;
}
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()));
}
int VectFuns::rightOfLinePoints(const Vect2& a, const Vect2& b, const Vect2& p) {
Vect2 AB = b.Sub(a);
Vect2 AP = p.Sub(a);
//The calculation below is the 2-D cross product.
return Util::sign(AP.x*AB.y - AP.y*AB.x);
}
