double VectFuns::distAtTau(const Vect3& s, const Vect3& vo, const Vect3& vi, bool futureOnly) { double tau = VectFuns::tau(s,vo,vi); if (tau < 0 && futureOnly) return s.norm(); // return distance now else { Vect3 v = vo.Sub(vi); Vect3 sAtTau = s.Add(v.Scal(tau)); return sAtTau.norm(); } }
/** * 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); }
// f should be between 0 and 1 to interpolate Vect3 VectFuns::interpolate(const Vect3& v1, const Vect3& v2, double f) { Vect3 dv = v2.Sub(v1); return v1.Add(dv.Scal(f)); }
Vect3 VectFuns::midPoint(const Vect3& p0, const Vect3& p1) { return p0.Add(p1).Scal(0.5); }