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