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(); } }
// time of closest approach double VectFuns::tau(const Vect3& s, const Vect3& vo, const Vect3& vi) { double rtn; Vect3 v = vo.Sub(vi); double nv = v.norm(); if (Util::almost_equals(nv,0.0)) { rtn = std::numeric_limits<double>::max(); // pseudo infinity } else rtn = -s.dot(v)/(nv*nv); return rtn; }// tau
static double dpc(const Vect3& m, const Vect3 *pts, const Triangle& cell, Vect3& alphas, int nb, int* idx, bool& inside) { if(nb == 1) { alphas(idx[0]) = 1.0; return (m - pts[cell[idx[0]]]).norm(); } // Solves H=sum(alpha_i A_i), sum(alpha_i)=1, et HM.(A_i-A_0)=0 Vect3 A0Ai[3]; // A_i-A_0 for(int i=1; i<nb; i++) { A0Ai[i] = pts[cell[idx[i]]] - pts[cell[idx[0]]]; } Vect3 A0M = m - pts[cell[idx[0]]]; // M-A_0 if(nb == 2) { alphas(idx[1]) = (A0M * A0Ai[1]) / (A0Ai[1] * A0Ai[1]); alphas(idx[0]) = 1.0 - alphas(idx[1]); } else if (nb==3) { // direct inversion (2x2 linear system) double a00 = A0Ai[1] * A0Ai[1]; double a10 = A0Ai[1] * A0Ai[2]; double a11 = A0Ai[2] * A0Ai[2]; double b0 = A0M * A0Ai[1]; double b1 = A0M * A0Ai[2]; double d = a00 * a11 - a10 * a10; assert(d != 0); alphas(idx[1]) = (b0 * a11 - b1 * a10) / d; alphas(idx[2]) = (a00 * b1 - a10 * b0) / d; alphas(idx[0]) = 1.0 - alphas(idx[1]) - alphas(idx[2]); } else { // 3 unknowns or more -> solve system // Ax=b with: A(i, j)=A0Ai.AjA0, x=(alpha_1, alpha_2, ...), b=A0M.A0Ai cerr << "Error : dim>=4 in danielsson !" << endl; exit(0); } // If alpha_i<0 -> brought to 0 and recursion // NB: also takes care of alpha > 1 because if alpha_i>1 then alpha_j<0 for at least one j for (int i=0; i<nb; i++) { if (alphas(idx[i])<0) { inside = false; alphas(idx[i]) = 0; swap(idx[i], idx[nb-1]); return dpc(m, pts, cell, alphas, nb-1, idx, inside); } } // Sinon: distance HM Vect3 MH = -A0M; for (int i=1; i<nb; i++) { MH = MH + alphas(idx[i]) * A0Ai[i]; } return MH.norm(); }
string fvStr2(const Vect3& v) { return "("+Units::str("deg",v.vect2().compassAngle())+", "+Units::str("knot",v.norm())+", "+Units::str("fpm",v.z)+")"; }