dgInt32 dgCollisionTaperedCapsule::CalculateContacts (const dgVector& point, const dgVector& normal, dgCollisionParamProxy& proxy, dgVector* const contactsOut) const
{
dgVector n (-normal.m_x, -dgSqrt (normal.m_y * normal.m_y + normal.m_z * normal.m_z), dgFloat32 (0.0), dgFloat32 (0.0f));
dgFloat32 project = m_sideNormal % n;
if (project > dgFloat32 (0.9998f)) {
return CalculateContactsGeneric (point, normal, proxy, contactsOut);
} else if (point.m_x > (m_height + m_clip0)){
return CalculateSphereConicContacts ( m_height, m_radio0, normal, point, contactsOut);
} else if (point.m_x < (-m_height + m_clip1)) {
return CalculateSphereConicContacts (-m_height, m_radio1, normal, point, contactsOut);
}
return CalculateContactsGeneric (point, normal, proxy, contactsOut);
}
dgInt32 dgCollisionChamferCylinder::CalculateContacts (const dgVector& point, const dgVector& normal, dgCollisionParamProxy& proxy, dgVector* const contactsOut) const
{
dgFloat32 disc2 = point.m_y * point.m_y + point.m_z * point.m_z;
if (disc2 < m_radius * m_radius) {
dgVector cylinderNormal ((point.m_x >= dgFloat32 (0.0)) ? dgFloat32 (-1.0f) : dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
return CalculateContactsGeneric (point, cylinderNormal, proxy, contactsOut);
} else {
dgVector r (dgFloat32 (0.0f), point.m_y, point.m_z, dgFloat32 (0.0f));
dgAssert ((r % r) > dgFloat32 (0.0f));
// r = r.Scale3(m_radius * dgRsqrt (r % r));
r = r.CompProduct4(r.InvMagSqrt()).Scale4(m_radius);
//dgFloat32 t = normal % (r - point);
dgVector t (normal.DotProduct4(r - point));
//contactsOut[0] = r - normal.Scale3 (t);
contactsOut[0] = r - normal.CompProduct4(t);
return 1;
}
}
dgInt32 dgCollisionTaperedCylinder::CalculateContacts (const dgVector& point, const dgVector& normal, dgCollisionParamProxy& proxy, dgVector* const contactsOut) const
{
dgAssert (dgAbsf (normal % normal - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
return CalculateContactsGeneric (point, normal, proxy, contactsOut);
}
