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;
}