dgBigVector dgPointToTriangleDistance(const dgBigVector& point, const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2)
{
const dgBigVector e10(p1 - p0);
const dgBigVector e20(p2 - p0);
const dgFloat64 a00 = e10.DotProduct(e10).GetScalar();
const dgFloat64 a11 = e20.DotProduct(e20).GetScalar();
const dgFloat64 a01 = e10.DotProduct(e20).GetScalar();
const dgFloat64 det = a00 * a11 - a01 * a01;
dgAssert(det >= dgFloat32(0.0f));
if (dgAbs(det) > dgFloat32(1.0e-24f)) {
dgBigVector p0Point (point - p0);
const dgFloat64 b0 = e10.DotProduct(p0Point).GetScalar();
const dgFloat64 b1 = e20.DotProduct(p0Point).GetScalar();
const dgFloat64 beta = b1 * a00 - a01 * b0;
const dgFloat64 alpha = b0 * a11 - a01 * b1;
if (beta < dgFloat32(0.0f)) {
return dgPointToRayDistance (point, p0, p1);
} else if (alpha < dgFloat32(0.0f)) {
return dgPointToRayDistance (point, p0, p2);
} else if ((alpha + beta) > det) {
return dgPointToRayDistance (point, p1, p2);
}
return p0 + (e10.Scale(alpha) + e20.Scale(beta)).Scale(dgFloat64(1.0f) / det);
}
// this is a degenerated triangle. this should never happens
dgAssert(0);
return p0;
}
dgBigVector dgPointToTetrahedrumDistance (const dgBigVector& point, const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, const dgBigVector& p3)
{
const dgBigVector e10(p1 - p0);
const dgBigVector e20(p2 - p0);
const dgBigVector e30(p3 - p0);
const dgFloat64 d0 = sqrt(e10.DotProduct(e10).GetScalar());
if (d0 > dgFloat64(0.0f)) {
const dgFloat64 invd0 = dgFloat64(1.0f) / d0;
const dgFloat64 l10 = e20.DotProduct(e10).GetScalar() * invd0;
const dgFloat64 l20 = e30.DotProduct(e10).GetScalar() * invd0;
const dgFloat64 desc11 = e20.DotProduct(e20).GetScalar() - l10 * l10;
if (desc11 > dgFloat64(0.0f)) {
const dgFloat64 d1 = sqrt(desc11);
const dgFloat64 invd1 = dgFloat64(1.0f) / d1;
const dgFloat64 l21 = (e30.DotProduct(e20).GetScalar() - l20 * l10) * invd1;
const dgFloat64 desc22 = e30.DotProduct(e30).GetScalar() - l20 * l20 - l21 * l21;
if (desc22 > dgFloat64(0.0f)) {
dgBigVector p0Point (point - p0);
const dgFloat64 d2 = sqrt(desc22);
const dgFloat64 invd2 = dgFloat64(1.0f) / d2;
const dgFloat64 b0 = e10.DotProduct(p0Point).GetScalar();
const dgFloat64 b1 = e20.DotProduct(p0Point).GetScalar();
const dgFloat64 b2 = e30.DotProduct(p0Point).GetScalar();
dgFloat64 u1 = b0 * invd0;
dgFloat64 u2 = (b1 - l10 * u1) * invd1;
dgFloat64 u3 = (b2 - l20 * u1 - l21 * u2) * invd2 * invd2;
u2 = (u2 - l21 * u3) * invd1;
u1 = (u1 - l10 * u2 - l20 * u3) * invd0;
if (u3 < dgFloat64(0.0f)) {
// this looks funny but it is correct
return dgPointToTriangleDistance(point, p0, p1, p2);
} else if (u2 < dgFloat64(0.0f)) {
return dgPointToTriangleDistance(point, p0, p1, p3);
} else if (u1 < dgFloat64(0.0f)) {
return dgPointToTriangleDistance(point, p0, p2, p3);
} else if (u1 + u2 + u3 > dgFloat64(1.0f)) {
return dgPointToTriangleDistance(point, p1, p2, p3);
}
return p0 + e10.Scale(u1) + e20.Scale(u2) + e30.Scale(u3);
}
}
}
// this is a degenerated tetra. this should never happens
dgAssert(0);
return p0;
}
dgBigVector LineTriangleIntersection (const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& A, const dgBigVector& B, const dgBigVector& C)
{
dgHugeVector ph0 (p0);
dgHugeVector ph1 (p1);
dgHugeVector Ah (A);
dgHugeVector Bh (B);
dgHugeVector Ch (C);
dgHugeVector p1p0 (ph1 - ph0);
dgHugeVector Ap0 (Ah - ph0);
dgHugeVector Bp0 (Bh - ph0);
dgHugeVector Cp0 (Ch - ph0);
dgGoogol t0 ((Bp0 * Cp0) % p1p0);
//hacd::HaF64 val0 = t0.GetAproximateValue();
//if (val0 < hacd::HaF64 (0.0f)) {
if (hacd::HaF64(t0) < hacd::HaF64(0.0f)) {
return dgBigVector (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (-1.0f));
}
dgGoogol t1 ((Cp0 * Ap0) % p1p0);
// hacd::HaF64 val1 = t1.GetAproximateValue();
// if (val1 < hacd::HaF64 (0.0f)) {
if (hacd::HaF64 (t1) < hacd::HaF64 (0.0f)) {
return dgBigVector (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (-1.0f));
}
dgGoogol t2 ((Ap0 * Bp0) % p1p0);
//hacd::HaF64 val2 = t2.GetAproximateValue();
//if (val2 < hacd::HaF64 (0.0f)) {
if (hacd::HaF64 (t2) < hacd::HaF64 (0.0f)) {
return dgBigVector (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (-1.0f));
}
dgGoogol sum = t0 + t1 + t2;
//hacd::HaF64 den = sum.GetAproximateValue();
#ifdef _DEBUG
//dgBigVector testpoint (A.Scale (val0 / den) + B.Scale (val1 / den) + C.Scale(val2 / den));
dgBigVector testpoint (A.Scale (t0 / sum) + B.Scale (t1 / sum) + C.Scale(t2 / sum));
hacd::HaF64 volume = ((B - A) * (C - A)) % (testpoint - A);
HACD_ASSERT (fabs (volume) < hacd::HaF64 (1.0e-12f));
#endif
// return dgBigVector (val0 / den, val1 / den, val2 / den, hacd::HaF32 (0.0f));
return dgBigVector (t0 / sum, t1 / sum, t2 / sum, hacd::HaF32 (0.0f));
}
dgBigVector LineTriangleIntersection (const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& A, const dgBigVector& B, const dgBigVector& C)
{
dgHugeVector ph0 (p0);
dgHugeVector ph1 (p1);
dgHugeVector Ah (A);
dgHugeVector Bh (B);
dgHugeVector Ch (C);
dgHugeVector p1p0 (ph1 - ph0);
dgHugeVector Ap0 (Ah - ph0);
dgHugeVector Bp0 (Bh - ph0);
dgHugeVector Cp0 (Ch - ph0);
dgGoogol t0 ((Bp0 * Cp0) % p1p0);
dgFloat64 val0 = t0.GetAproximateValue();
if (val0 < dgFloat64 (0.0f)) {
return dgBigVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (-1.0f));
}
dgGoogol t1 ((Cp0 * Ap0) % p1p0);
dgFloat64 val1 = t1.GetAproximateValue();
if (val1 < dgFloat64 (0.0f)) {
return dgBigVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (-1.0f));
}
dgGoogol t2 ((Ap0 * Bp0) % p1p0);
dgFloat64 val2 = t2.GetAproximateValue();
if (val2 < dgFloat64 (0.0f)) {
return dgBigVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (-1.0f));
}
dgGoogol sum = t0 + t1 + t2;
dgFloat64 den = sum.GetAproximateValue();
#ifdef _DEBUG
dgBigVector testpoint (A.Scale (val0 / den) + B.Scale (val1 / den) + C.Scale(val2 / den));
dgFloat64 volume = ((B - A) * (C - A)) % (testpoint - A);
_ASSERTE (fabs (volume) < dgFloat64 (1.0e-12f));
#endif
return dgBigVector (val0 / den, val1 / den, val2 / den, dgFloat32 (0.0f));
}
dgBigVector dgPointToTriangleDistance (const dgBigVector& point, const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2)
{
// const dgBigVector p (double (0.0f), double (0.0f), double (0.0f));
const dgBigVector p10 (p1 - p0);
const dgBigVector p20 (p2 - p0);
const dgBigVector p_p0 (point - p0);
double alpha1 = p10 % p_p0;
double alpha2 = p20 % p_p0;
if ((alpha1 <= double (0.0f)) && (alpha2 <= double (0.0f))) {
return p0;
}
dgBigVector p_p1 (point - p1);
double alpha3 = p10 % p_p1;
double alpha4 = p20 % p_p1;
if ((alpha3 >= double (0.0f)) && (alpha4 <= alpha3)) {
return p1;
}
double vc = alpha1 * alpha4 - alpha3 * alpha2;
if ((vc <= double (0.0f)) && (alpha1 >= double (0.0f)) && (alpha3 <= double (0.0f))) {
double t = alpha1 / (alpha1 - alpha3);
HACD_ASSERT (t >= double (0.0f));
HACD_ASSERT (t <= double (1.0f));
return p0 + p10.Scale (t);
}
dgBigVector p_p2 (point - p2);
double alpha5 = p10 % p_p2;
double alpha6 = p20 % p_p2;
if ((alpha6 >= double (0.0f)) && (alpha5 <= alpha6)) {
return p2;
}
double vb = alpha5 * alpha2 - alpha1 * alpha6;
if ((vb <= double (0.0f)) && (alpha2 >= double (0.0f)) && (alpha6 <= double (0.0f))) {
double t = alpha2 / (alpha2 - alpha6);
HACD_ASSERT (t >= double (0.0f));
HACD_ASSERT (t <= double (1.0f));
return p0 + p20.Scale (t);
}
double va = alpha3 * alpha6 - alpha5 * alpha4;
if ((va <= double (0.0f)) && ((alpha4 - alpha3) >= double (0.0f)) && ((alpha5 - alpha6) >= double (0.0f))) {
double t = (alpha4 - alpha3) / ((alpha4 - alpha3) + (alpha5 - alpha6));
HACD_ASSERT (t >= double (0.0f));
HACD_ASSERT (t <= double (1.0f));
return p1 + (p2 - p1).Scale (t);
}
double den = float(double (1.0f)) / (va + vb + vc);
double t = vb * den;
double s = vc * den;
HACD_ASSERT (t >= double (0.0f));
HACD_ASSERT (s >= double (0.0f));
HACD_ASSERT (t <= double (1.0f));
HACD_ASSERT (s <= double (1.0f));
return p0 + p10.Scale (t) + p20.Scale (s);
}