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