std::pair<Vect3,double> VectFuns::intersectionAvgZ(const Vect3& so1, const Vect3& so2, double dto, const Vect3& si1, const Vect3& si2) { Velocity vo3 = Velocity::genVel(so1, so2, dto); Velocity vi3 = Velocity::genVel(si1, si2, dto); // its ok to use any time here, all times are relative to so std::pair<Vect3,double> iP = intersection(so1,vo3,si1,vi3); Vect3 interSec = iP.first; double do1 = distanceH(so1,interSec); double do2 = distanceH(so2,interSec); double alt_o = so1.z; if (do2 < do1) alt_o = so2.z; double di1 = distanceH(si1,interSec); double di2 = distanceH(si2,interSec); double alt_i = si1.z; if (di2 < di1) alt_i = si2.z; double nZ = (alt_o + alt_i)/2.0; return std::pair<Vect3,double>(interSec.mkZ(nZ),iP.second); }
/** * 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); }