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);
}
/**
* 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();
}
Vect2 VectFuns::midPoint(const Vect2& p0, const Vect2& p1) {
return p0.Add(p1).Scal(0.5);
}