public override void addContactPoint( ref btVector3 normalOnBInWorld, ref btVector3 pointInWorld, double depth ) { bool isSwapped = m_manifoldPtr.m_body0 != m_body0Wrap.m_collisionObject; btVector3 pointA; pointInWorld.AddScale( ref normalOnBInWorld, depth, out pointA ); btVector3 localA; btVector3 localB; if( isSwapped ) { m_body1Wrap.m_collisionObject.m_worldTransform.invXform( ref pointA, out localA ); m_body0Wrap.m_collisionObject.m_worldTransform.invXform( ref pointInWorld, out localB ); } else { m_body0Wrap.m_collisionObject.m_worldTransform.invXform( ref pointA, out localA ); m_body1Wrap.m_collisionObject.m_worldTransform.invXform( ref pointInWorld, out localB ); } btManifoldPoint newPt = BulletGlobals.ManifoldPointPool.Get(); newPt.Initialize( ref localA, ref localB, ref normalOnBInWorld, depth ); btScalar.Dbg( "2 add contact point " + pointA.ToString() + " and " + pointInWorld.ToString() ); newPt.m_positionWorldOnA = pointA; newPt.m_positionWorldOnB = pointInWorld; //BP mod, store contact triangles. if( isSwapped ) { newPt.m_partId0 = m_partId1; newPt.m_partId1 = m_partId0; newPt.m_index0 = m_index1; newPt.m_index1 = m_index0; } else { newPt.m_partId0 = m_partId0; newPt.m_partId1 = m_partId1; newPt.m_index0 = m_index0; newPt.m_index1 = m_index1; } //experimental feature info, for per-triangle material etc. btCollisionObjectWrapper obj0Wrap = isSwapped ? m_body1Wrap : m_body0Wrap; btCollisionObjectWrapper obj1Wrap = isSwapped ? m_body0Wrap : m_body1Wrap; m_resultCallback.addSingleResult( newPt, obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1 ); }