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