public override void AddContactPoint(ref IndexedVector3 normalOnBInWorld, ref IndexedVector3 pointInWorld, float orgDepth)
{
IndexedVector3 endPt, startPt;
float newDepth;
IndexedVector3 newNormal = new IndexedVector3(0, 1, 0);
if (m_perturbA)
{
IndexedVector3 endPtOrg = pointInWorld + normalOnBInWorld * orgDepth;
endPt = (m_unPerturbedTransform * m_transformA.Inverse()) * (endPtOrg);
newDepth = IndexedVector3.Dot((endPt - pointInWorld), normalOnBInWorld);
startPt = endPt + normalOnBInWorld * newDepth;
}
else
{
endPt = pointInWorld + normalOnBInWorld * orgDepth;
startPt = (m_unPerturbedTransform * m_transformB.Inverse()) * (pointInWorld);
newDepth = IndexedVector3.Dot((endPt - startPt), normalOnBInWorld);
}
//#define DEBUG_CONTACTS 1
#if DEBUG_CONTACTS
m_debugDrawer.DrawLine(startPt, endPt, new IndexedVector3(1, 0, 0));
m_debugDrawer.DrawSphere(startPt, 0.5f, new IndexedVector3(0, 1, 0));
m_debugDrawer.DrawSphere(endPt, 0.5f, new IndexedVector3(0, 0, 1));
#endif //DEBUG_CONTACTS
m_originalManifoldResult.AddContactPoint(ref normalOnBInWorld, ref startPt, newDepth);
}
