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)+")";
}
