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;
}
dgFloat32 dgCollisionChamferCylinder::RayCast(const dgVector& q0, const dgVector& q1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const
{
if (q0.m_x > m_height) {
if (q1.m_x < m_height) {
dgFloat32 t1 = (m_height - q0.m_x) / (q1.m_x - q0.m_x);
dgFloat32 y = q0.m_y + (q1.m_y - q0.m_y) * t1;
dgFloat32 z = q0.m_z + (q1.m_z - q0.m_z) * t1;
if ((y * y + z * z) < m_radius * m_radius) {
contactOut.m_normal = dgVector(dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
return t1;
}
}
}
if (q0.m_x < -m_height) {
if (q1.m_x > -m_height) {
dgFloat32 t1 = (-m_height - q0.m_x) / (q1.m_x - q0.m_x);
dgFloat32 y = q0.m_y + (q1.m_y - q0.m_y) * t1;
dgFloat32 z = q0.m_z + (q1.m_z - q0.m_z) * t1;
if ((y * y + z * z) < m_radius * m_radius) {
contactOut.m_normal = dgVector(dgFloat32(-1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
return t1;
}
}
}
dgVector dq((q1 - q0) & dgVector::m_triplexMask);
// avoid NaN as a result of a division by zero
if (dq.DotProduct(dq).GetScalar() <= 0.0f) {
return dgFloat32(1.2f);
}
//dgVector dir(dq * dq.InvMagSqrt());
dgVector dir(dq.Normalize());
if (dgAbs(dir.m_x) > 0.9999f) {
return dgCollisionConvex::RayCast(q0, q1, maxT, contactOut, body, NULL, NULL);
}
dgVector p0(q0 & dgVector::m_triplexMask);
dgVector p1(q1 & dgVector::m_triplexMask);
p0.m_x = dgFloat32 (0.0f);
p1.m_x = dgFloat32 (0.0f);
dgVector dp (p1 - p0);
dgFloat32 a = dp.DotProduct(dp).GetScalar();
dgFloat32 b = dgFloat32 (2.0f) * dp.DotProduct(p0).GetScalar();
dgFloat32 c = p0.DotProduct(p0).GetScalar() - m_radius * m_radius;
dgFloat32 disc = b * b - dgFloat32 (4.0f) * a * c;
if (disc >= dgFloat32 (0.0f)) {
disc = dgSqrt (disc);
dgVector origin0(p0 + dp.Scale ((-b + disc) / (dgFloat32 (2.0f) * a)));
dgVector origin1(p0 + dp.Scale ((-b - disc) / (dgFloat32 (2.0f) * a)));
dgFloat32 t0 = dgRayCastSphere(q0, q1, origin0, m_height);
dgFloat32 t1 = dgRayCastSphere(q0, q1, origin1, m_height);
if(t1 < t0) {
t0 = t1;
origin0 = origin1;
}
if ((t0 >= 0.0f) && (t0 <= 1.0f)) {
contactOut.m_normal = q0 + dq.Scale(t0) - origin0;
dgAssert(contactOut.m_normal.m_w == dgFloat32(0.0f));
//contactOut.m_normal = contactOut.m_normal * contactOut.m_normal.DotProduct(contactOut.m_normal).InvSqrt();
contactOut.m_normal = contactOut.m_normal.Normalize();
return t0;
}
} else {
dgVector origin0 (dgPointToRayDistance (dgVector::m_zero, p0, p1));
origin0 = origin0.Scale(m_radius / dgSqrt(origin0.DotProduct(origin0).GetScalar()));
dgFloat32 t0 = dgRayCastSphere(q0, q1, origin0, m_height);
if ((t0 >= 0.0f) && (t0 <= 1.0f)) {
contactOut.m_normal = q0 + dq.Scale(t0) - origin0;
dgAssert(contactOut.m_normal.m_w == dgFloat32(0.0f));
//contactOut.m_normal = contactOut.m_normal * contactOut.m_normal.DotProduct(contactOut.m_normal).InvSqrt();
contactOut.m_normal = contactOut.m_normal.Normalize();
return t0;
}
}
return dgFloat32(1.2f);
}
