/*
internal void Initialize( btCollisionAlgorithmConstructionInfo ci)
{
base.Initialize( ci );
}
*/
internal override void processCollision( btCollisionObjectWrapper a
, ref btTransform body0Transform
, btCollisionObjectWrapper body1Wrap
, ref btTransform body1Transform
, btDispatcherInfo c, btManifoldResult d)
{
}
internal override btCollisionAlgorithm CreateCollisionAlgorithm( btCollisionAlgorithmConstructionInfo ci, btCollisionObjectWrapper body0Wrap, btCollisionObjectWrapper body1Wrap )
{
btConvexPlaneCollisionAlgorithm ca = BulletGlobals.ConvexPlaneAlgorithmPool.Get();
if( !m_swapped )
ca.Initialize( null, ci, body0Wrap, body1Wrap, false, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold );
else
ca.Initialize( null, ci, body0Wrap, body1Wrap, true, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold );
return ca;
}
internal override void processCollision( btCollisionObjectWrapper body0Wrap
, ref btTransform body0Transform
, btCollisionObjectWrapper body1Wrap
, ref btTransform body1Transform
, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
{
//(void)dispatchInfo;
if( m_manifoldPtr == null )
return;
resultOut.setPersistentManifold( m_manifoldPtr );
btSphereShape sphere0 = (btSphereShape)body0Wrap.getCollisionShape();
btSphereShape sphere1 = (btSphereShape)body1Wrap.getCollisionShape();
btVector3 diff; body0Wrap.m_collisionObject.m_worldTransform.m_origin.Sub( ref body1Wrap.m_collisionObject.m_worldTransform.m_origin, out diff );
double len = diff.length();
double radius0 = sphere0.getRadius();
double radius1 = sphere1.getRadius();
#if CLEAR_MANIFOLD
m_manifoldPtr.clearManifold(); //don't do this, it disables warmstarting
#endif
///iff distance positive, don't generate a new contact
if( len > ( radius0 + radius1 ) )
{
#if !CLEAR_MANIFOLD
resultOut.refreshContactPoints();
#endif //CLEAR_MANIFOLD
return;
}
///distance (negative means penetration)
double dist = len - ( radius0 + radius1 );
btVector3 normalOnSurfaceB = btVector3.xAxis;
if( len > btScalar.SIMD_EPSILON )
{
normalOnSurfaceB = diff / len;
}
///point on A (worldspace)
///btVector3 pos0 = col0.getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
///point on B (worldspace)
btVector3 pos1; body1Wrap.m_collisionObject.m_worldTransform.m_origin.AddScale( ref normalOnSurfaceB, radius1, out pos1 );
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.addContactPoint( ref normalOnSurfaceB, ref pos1, dist );
#if !CLEAR_MANIFOLD
resultOut.refreshContactPoints();
#endif //CLEAR_MANIFOLD
}
//btSphereSphereCollisionAlgorithm( btCollisionAlgorithmConstructionInfo ci )
// : btActivatingCollisionAlgorithm( ci) { }
internal void Initialize( btPersistentManifold mf, btCollisionAlgorithmConstructionInfo ci, btCollisionObjectWrapper col0Wrap, btCollisionObjectWrapper col1Wrap )
{
base.Initialize( ci, col0Wrap, col1Wrap );
m_ownManifold = ( false );
m_manifoldPtr = ( mf );
if( m_manifoldPtr == null )
{
m_manifoldPtr = m_dispatcher.getNewManifold( col0Wrap.m_collisionObject, col1Wrap.m_collisionObject );
m_ownManifold = true;
}
}
public void Initialize( btCollisionObjectWrapper parent, btCollisionShape shape, btCollisionObject collisionObject
//, ref btTransform worldTransform
, int partId, int index )
{
m_parent = ( parent );
m_shape = ( shape );
m_collisionObject = ( collisionObject );
//m_worldTransform = ( worldTransform );
m_partId = ( partId );
m_index = ( index );
}
void Initialize( btPersistentManifold mf, btCollisionAlgorithmConstructionInfo ci
, btCollisionObjectWrapper body0Wrap, btCollisionObjectWrapper body1Wrap )
{
base.Initialize( ci, body0Wrap, body1Wrap );
m_ownManifold = ( false );
m_manifoldPtr = ( mf );
if( m_manifoldPtr == null && m_dispatcher.needsCollision( body0Wrap.m_collisionObject, body1Wrap.m_collisionObject ) )
{
m_manifoldPtr = m_dispatcher.getNewManifold( body0Wrap.m_collisionObject, body1Wrap.m_collisionObject );
m_ownManifold = true;
}
}
public void Initialize( btDispatcher dispatcher, btCollisionObjectWrapper body0Wrap, btCollisionObjectWrapper body1Wrap, bool isSwapped )
{
m_dispatcher = ( dispatcher );
m_dispatchInfoPtr = ( null );
m_convexBodyWrap = isSwapped ? body1Wrap : body0Wrap;
m_triBodyWrap = isSwapped ? body0Wrap : body1Wrap;
//
// create the manifold from the dispatcher 'manifold pool'
//
m_manifoldPtr = m_dispatcher.getNewManifold( m_convexBodyWrap.m_collisionObject, m_triBodyWrap.m_collisionObject );
clearCache();
}
internal void Initialize( btPersistentManifold mf, btCollisionAlgorithmConstructionInfo ci, btCollisionObjectWrapper col0Wrap, btCollisionObjectWrapper col1Wrap, bool isSwapped, int numPerturbationIterations, int minimumPointsPerturbationThreshold )
{
base.Initialize( ci );
m_ownManifold = ( false );
m_manifoldPtr = ( mf );
m_swapped = ( isSwapped );
m_numPerturbationIterations = ( numPerturbationIterations );
m_minimumPointsPerturbationThreshold = ( minimumPointsPerturbationThreshold );
btCollisionObjectWrapper convexObjWrap = m_swapped ? col1Wrap : col0Wrap;
btCollisionObjectWrapper planeObjWrap = m_swapped ? col0Wrap : col1Wrap;
if( m_manifoldPtr == null && m_dispatcher.needsCollision( convexObjWrap.m_collisionObject, planeObjWrap.m_collisionObject ) )
{
m_manifoldPtr = m_dispatcher.getNewManifold( convexObjWrap.m_collisionObject, planeObjWrap.m_collisionObject );
m_ownManifold = true;
}
}
internal override void processCollision( btCollisionObjectWrapper col0Wrap
, ref btTransform body0Transform
, btCollisionObjectWrapper col1Wrap
, ref btTransform body1Transform
, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
{
if( m_manifoldPtr == null )
return;
btCollisionObjectWrapper sphereObjWrap = m_swapped ? col1Wrap : col0Wrap;
btCollisionObjectWrapper triObjWrap = m_swapped ? col0Wrap : col1Wrap;
btSphereShape sphere = (btSphereShape)sphereObjWrap.getCollisionShape();
btTriangleShape triangle = (btTriangleShape)triObjWrap.getCollisionShape();
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.setPersistentManifold( m_manifoldPtr );
SphereTriangleDetector detector = BulletGlobals.SphereTriangleDetectorPool.Get();
detector.Initialize( sphere, triangle, m_manifoldPtr.getContactBreakingThreshold());
btDiscreteCollisionDetectorInterface.ClosestPointInput input = BulletGlobals.ClosestPointInputPool.Get();
input.m_maximumDistanceSquared = btScalar.BT_LARGE_FLOAT;///@todo: tighter bounds
if( m_swapped )
{
input.m_transformA = body1Transform;
input.m_transformB = body0Transform;
}
else
{
input.m_transformA = body0Transform;
input.m_transformB = body1Transform;
}
bool swapResults = m_swapped;
detector.getClosestPoints( input, resultOut, dispatchInfo.m_debugDraw, swapResults );
BulletGlobals.ClosestPointInputPool.Free( input );
BulletGlobals.SphereTriangleDetectorPool.Free( detector );
if( m_ownManifold )
resultOut.refreshContactPoints();
}
new internal void Initialize( btCollisionAlgorithmConstructionInfo ci, btCollisionObjectWrapper body0Wrap, btCollisionObjectWrapper body1Wrap, bool isSwapped )
{
base.Initialize( ci, body0Wrap, body1Wrap, isSwapped );
m_childCollisionAlgorithmCache = new btHashedSimplePairCache();
btCollisionObjectWrapper col0ObjWrap = body0Wrap;
Debug.Assert( col0ObjWrap.getCollisionShape().isCompound() );
btCollisionObjectWrapper col1ObjWrap = body1Wrap;
Debug.Assert( col1ObjWrap.getCollisionShape().isCompound() );
btCompoundShape compoundShape0 = (btCompoundShape)( col0ObjWrap.getCollisionShape() );
m_compoundShapeRevision0 = compoundShape0.getUpdateRevision();
btCompoundShape compoundShape1 = (btCompoundShape)( col1ObjWrap.getCollisionShape() );
m_compoundShapeRevision1 = compoundShape1.getUpdateRevision();
}
internal override void processCollision( btCollisionObjectWrapper body0Wrap, ref btTransform body0Transform
, btCollisionObjectWrapper body1Wrap, ref btTransform body1Transform, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
{
if( m_manifoldPtr == null )
return;
btBoxShape box0 = (btBoxShape)body0Wrap.m_shape;
btBoxShape box1 = (btBoxShape)body1Wrap.m_shape;
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.setPersistentManifold( m_manifoldPtr );
#if !USE_PERSISTENT_CONTACTS
m_manifoldPtr.clearManifold();
#endif //USE_PERSISTENT_CONTACTS
btDiscreteCollisionDetectorInterface.ClosestPointInput input = new btDiscreteCollisionDetectorInterface.ClosestPointInput();
input.m_maximumDistanceSquared = btScalar.BT_LARGE_FLOAT;
input.m_transformA = body0Transform;
input.m_transformB = body1Transform;
//btBoxBoxDetector detector = BulletGlobals.
// new btBoxBoxDetectors( box0, box1 );
btBoxBoxDetector.getClosestPoints( box0, box1, input, resultOut, dispatchInfo.m_debugDraw );
#if USE_PERSISTENT_CONTACTS
// refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
if( m_ownManifold )
{
resultOut.refreshContactPoints();
}
#endif //USE_PERSISTENT_CONTACTS
}
void preallocateChildAlgorithms( btCollisionObjectWrapper body0Wrap, btCollisionObjectWrapper body1Wrap )
{
btCollisionObjectWrapper colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
btCollisionObjectWrapper otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
Debug.Assert( colObjWrap.getCollisionShape().isCompound() );
btCompoundShape compoundShape = (btCompoundShape)( colObjWrap.getCollisionShape() );
int numChildren = compoundShape.getNumChildShapes();
int i;
m_childCollisionAlgorithms.Count = m_childCollisionAlgorithms.Capacity = ( numChildren );
for( i = 0; i < numChildren; i++ )
{
if( compoundShape.getDynamicAabbTree() != null )
{
m_childCollisionAlgorithms[i] = null;
}
else
{
btCollisionShape childShape = compoundShape.getChildShape( i );
btCollisionObjectWrapper childWrap = BulletGlobals.CollisionObjectWrapperPool.Get();
childWrap.Initialize( colObjWrap, childShape, colObjWrap.m_collisionObject, -1, i );//wrong child trans, but unused (hopefully)
m_childCollisionAlgorithms[i] = m_dispatcher.findAlgorithm( childWrap, otherObjWrap, m_sharedManifold );
}
}
}
internal abstract btCollisionAlgorithm findAlgorithm( btCollisionObjectWrapper body0Wrap, btCollisionObjectWrapper body1Wrap, btPersistentManifold sharedManifold );
internal void Initialize( btCollisionAlgorithmConstructionInfo ci, btCollisionObjectWrapper body0Wrap, btCollisionObjectWrapper body1Wrap )
{
base.Initialize( ci );
}
internal override btCollisionAlgorithm findAlgorithm( btCollisionObjectWrapper body0Wrap, btCollisionObjectWrapper body1Wrap, btPersistentManifold sharedManifold = null )
{
btCollisionAlgorithmConstructionInfo ci;
ci.m_dispatcher1 = this;
ci.m_manifold = sharedManifold;
btCollisionAlgorithm algo = m_doubleDispatch[(int)body0Wrap.getCollisionShape().getShapeType(), (int)body1Wrap.getCollisionShape().getShapeType()].CreateCollisionAlgorithm( ci, body0Wrap, body1Wrap );
return algo;
}
internal abstract btCollisionAlgorithm CreateCollisionAlgorithm( btCollisionAlgorithmConstructionInfo info, btCollisionObjectWrapper body0Wrap, btCollisionObjectWrapper body1Wrap );
internal override btCollisionAlgorithm CreateCollisionAlgorithm( btCollisionAlgorithmConstructionInfo ci, btCollisionObjectWrapper body0Wrap, btCollisionObjectWrapper body1Wrap )
{
btSphereTriangleCollisionAlgorithm ca = BulletGlobals.SphereTriangleCollisionAlgorithmPool.Get();
ca.Initialize( ci.m_manifold, ci, body0Wrap, body1Wrap, m_swapped );
return ca;
}
internal override btCollisionAlgorithm CreateCollisionAlgorithm( btCollisionAlgorithmConstructionInfo ci, btCollisionObjectWrapper body0Wrap, btCollisionObjectWrapper body1Wrap )
{
btCompoundCollisionAlgorithm ca = BulletGlobals.CompoundCollisionAlgorithmPool.Get();
ca.Initialize( ci, body0Wrap, body1Wrap, true );
return ca;
}
internal override btCollisionAlgorithm CreateCollisionAlgorithm( btCollisionAlgorithmConstructionInfo ci, btCollisionObjectWrapper body0Wrap, btCollisionObjectWrapper body1Wrap )
{
btConvexConvexAlgorithm ca = BulletGlobals.ConvexConvexAlgorithmPool.Get();
ca.Initialize( ci.m_manifold, ci, body0Wrap, body1Wrap, m_simplexSolver, m_pdSolver, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold );
return ca;
}
internal abstract double addSingleResult( btManifoldPoint cp, btCollisionObjectWrapper colObj0Wrap, int partId0, int index0, btCollisionObjectWrapper colObj1Wrap, int partId1, int index1 ) ;
void collideSingleContact( bool usePertube, ref btQuaternion perturbeRot
, btCollisionObjectWrapper convexObjWrap
, ref btTransform convexTransform
, btCollisionObjectWrapper planeObjWrap
, ref btTransform planeTransform
, btDispatcherInfo dispatchInfo, btManifoldResult resultOut
, ref btVector3 planeNormal, double planeConstant
)
{
//btCollisionObjectWrapper convexObjWrap = m_swapped ? body1Wrap : body0Wrap;
//btCollisionObjectWrapper planeObjWrap = m_swapped ? body0Wrap : body1Wrap;
btConvexShape convexShape = (btConvexShape)convexObjWrap.getCollisionShape();
btStaticPlaneShape planeShape = (btStaticPlaneShape)planeObjWrap.getCollisionShape();
bool hasCollision = false;
//planeNormal = planeShape.getPlaneNormal().Copy( out planeNormal );
//double planeConstant = planeShape.getPlaneConstant();
btTransform convexWorldTransform = convexTransform;
//btTransform planeWorldTransform = planeObjWrap.m_worldTransform;
btTransform convexInPlaneTrans;
planeTransform.inverseTimes( ref convexWorldTransform, out convexInPlaneTrans );
if( usePertube )
{
//now perturbe the convex-world transform
btMatrix3x3 perturbeMat = new btMatrix3x3( ref perturbeRot );
btMatrix3x3 tmpPerturbe; convexWorldTransform.m_basis.Apply( ref perturbeMat, out tmpPerturbe );
convexWorldTransform.m_basis = tmpPerturbe;
//convexWorldTransform.getBasis() *= btMatrix3x3( perturbeRot );
}
btTransform planeInConvex;
convexTransform.inverseTimes( ref planeObjWrap.m_collisionObject.m_worldTransform, out planeInConvex );
btVector3 tmp, tmp2;
planeNormal.Invert( out tmp );
planeInConvex.m_basis.Apply( ref tmp, out tmp2 );
btVector3 vtx; convexShape.localGetSupportingVertex( ref tmp2, out vtx );
btVector3 vtxInPlane; convexInPlaneTrans.Apply( ref vtx, out vtxInPlane );
double distance = ( planeNormal.dot( ref vtxInPlane ) - planeConstant );
btVector3 vtxInPlaneProjected; vtxInPlane.AddScale( ref planeNormal, -distance, out vtxInPlaneProjected );
btVector3 vtxInPlaneWorld; planeTransform.Apply( ref vtxInPlaneProjected, out vtxInPlaneWorld );
hasCollision = distance < m_manifoldPtr.getContactBreakingThreshold();
resultOut.setPersistentManifold( m_manifoldPtr );
if( hasCollision )
{
/// report a contact. internally this will be kept persistent, and contact reduction is done
btVector3 normalOnSurfaceB; planeTransform.m_basis.Apply( ref planeNormal, out normalOnSurfaceB );
btScalar.Dbg( "Convex plane adds point " + normalOnSurfaceB + " " + vtxInPlaneWorld + " " + distance.ToString( "g17" ) );
resultOut.addContactPoint( ref normalOnSurfaceB, ref vtxInPlaneWorld, distance );
}
}
internal override void processCollision( btCollisionObjectWrapper body0Wrap
, ref btTransform body0Transform
, btCollisionObjectWrapper body1Wrap
, ref btTransform body1Transform
, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
{
btCollisionObjectWrapper col0ObjWrap = body0Wrap;
btCollisionObjectWrapper col1ObjWrap = body1Wrap;
Debug.Assert( col0ObjWrap.getCollisionShape().isCompound() );
Debug.Assert( col1ObjWrap.getCollisionShape().isCompound() );
btCompoundShape compoundShape0 = (btCompoundShape)( col0ObjWrap.getCollisionShape() );
btCompoundShape compoundShape1 = (btCompoundShape)( col1ObjWrap.getCollisionShape() );
btDbvt tree0 = compoundShape0.getDynamicAabbTree();
btDbvt tree1 = compoundShape1.getDynamicAabbTree();
if( tree0 == null || tree1 == null )
{
base.processCollision( body0Wrap, ref body0Transform, body1Wrap, ref body1Transform, dispatchInfo, resultOut );
return;
}
///btCompoundShape might have changed:
////make sure the internal child collision algorithm caches are still valid
if( ( compoundShape0.getUpdateRevision() != m_compoundShapeRevision0 ) || ( compoundShape1.getUpdateRevision() != m_compoundShapeRevision1 ) )
{
///clear all
removeChildAlgorithms();
m_compoundShapeRevision0 = compoundShape0.getUpdateRevision();
m_compoundShapeRevision1 = compoundShape1.getUpdateRevision();
}
///we need to refresh all contact manifolds
///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
{
int i;
btManifoldArray manifoldArray = new btManifoldArray();
btSimplePairArray pairs = m_childCollisionAlgorithmCache.getOverlappingPairArray();
for( i = 0; i < pairs.Count; i++ )
{
if( pairs[i].m_userPointer != null )
{
btCollisionAlgorithm algo = (btCollisionAlgorithm)pairs[i].m_userPointer;
algo.getAllContactManifolds( manifoldArray );
for( int m = 0; m < manifoldArray.Count; m++ )
{
if( manifoldArray[m].m_cachedPoints != 0 )
{
resultOut.setPersistentManifold( manifoldArray[m] );
resultOut.refreshContactPoints();
resultOut.setPersistentManifold( null );
}
}
manifoldArray.Count =( 0 );
}
}
}
btCompoundCompoundLeafCallback callback = new btCompoundCompoundLeafCallback
( col0ObjWrap, col1ObjWrap,this.m_dispatcher, dispatchInfo, resultOut, this.m_childCollisionAlgorithmCache, m_sharedManifold);
btTransform xform; body0Transform.inverseTimes( ref body1Transform, out xform );
MycollideTT( tree0.m_root, tree1.m_root, ref xform, callback );
//Console.WriteLine("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs);
//remove non-overlapping child pairs
{
Debug.Assert( m_removePairs.Count == 0 );
//iterate over all children, perform an AABB check inside ProcessChildShape
btSimplePairArray pairs = m_childCollisionAlgorithmCache.getOverlappingPairArray();
int i;
//btManifoldArray manifoldArray;
btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1;
for( i = 0; i < pairs.Count; i++ )
{
if( pairs[i].m_userPointer != null )
{
btCollisionAlgorithm algo = (btCollisionAlgorithm)pairs[i].m_userPointer;
{
btCollisionShape childShape0 = null;
btTransform newChildWorldTrans0;
//btTransform orgInterpolationTrans0;
childShape0 = compoundShape0.getChildShape( pairs[i].m_indexA );
//orgInterpolationTrans0 = col0ObjWrap.m_worldTransform;
btTransform childTrans0 = compoundShape0.getChildTransform( pairs[i].m_indexA );
body0Transform.Apply( ref childTrans0, out newChildWorldTrans0 );
childShape0.getAabb( ref newChildWorldTrans0, out aabbMin0, out aabbMax0 );
}
{
btCollisionShape childShape1 = null;
btTransform newChildWorldTrans1;
childShape1 = compoundShape1.getChildShape( pairs[i].m_indexB );
btTransform childTrans1 = compoundShape1.getChildTransform( pairs[i].m_indexB );
body1Transform.Apply( ref childTrans1, out newChildWorldTrans1 );
childShape1.getAabb( ref newChildWorldTrans1, out aabbMin1, out aabbMax1 );
}
if( !btAabbUtil.TestAabbAgainstAabb2( ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1 ) )
{
//algo.~btCollisionAlgorithm();
m_dispatcher.freeCollisionAlgorithm( algo );
m_removePairs.Add( new btSimplePair( pairs[i].m_indexA, pairs[i].m_indexB ) );
}
}
}
for( i = 0; i < m_removePairs.Count; i++ )
{
m_childCollisionAlgorithmCache.removeOverlappingPair( m_removePairs[i].m_indexA, m_removePairs[i].m_indexB );
}
m_removePairs.Clear();
}
}
internal btCompoundCompoundLeafCallback( btCollisionObjectWrapper compound1ObjWrap,
btCollisionObjectWrapper compound0ObjWrap,
btDispatcher dispatcher,
btDispatcherInfo dispatchInfo,
btManifoldResult resultOut,
btHashedSimplePairCache childAlgorithmsCache,
btPersistentManifold sharedManifold )
{
m_numOverlapPairs = ( 0 );
m_compound0ColObjWrap = ( compound1ObjWrap );
m_compound1ColObjWrap = ( compound0ObjWrap );
m_dispatcher = ( dispatcher );
m_dispatchInfo = ( dispatchInfo );
m_resultOut = ( resultOut );
m_childCollisionAlgorithmCache = ( childAlgorithmsCache );
m_sharedManifold = ( sharedManifold );
}
internal override void processCollision( btCollisionObjectWrapper body0Wrap
, ref btTransform body0Transform
, btCollisionObjectWrapper body1Wrap
, ref btTransform body1Transform
, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
{
//(void)dispatchInfo;
if( m_manifoldPtr == null )
return;
btCollisionObjectWrapper convexObjWrap = m_swapped ? body1Wrap : body0Wrap;
btCollisionObjectWrapper planeObjWrap = m_swapped ? body0Wrap : body1Wrap;
btConvexShape convexShape = (btConvexShape)convexObjWrap.m_shape;
btStaticPlaneShape planeShape = (btStaticPlaneShape)planeObjWrap.getCollisionShape();
//btVector3 planeNormal;
//planeShape.getPlaneNormal().Copy( out planeNormal );
double planeConstant = planeShape.getPlaneConstant();
if( m_swapped )
collideSingleContact( false, ref btQuaternion.Identity, convexObjWrap
, ref body1Transform
, planeObjWrap
, ref body0Transform
, dispatchInfo, resultOut
, ref planeShape.m_planeNormal, planeConstant );
else
collideSingleContact( false, ref btQuaternion.Identity, convexObjWrap
, ref body0Transform
, planeObjWrap
, ref body1Transform
, dispatchInfo, resultOut
, ref planeShape.m_planeNormal, planeConstant );
/*
btCollisionObjectWrapper convexObjWrap = m_swapped ? body1Wrap : body0Wrap;
btCollisionObjectWrapper planeObjWrap = m_swapped ? body0Wrap : body1Wrap;
btConvexShape convexShape = (btConvexShape)convexObjWrap.m_shape;
btStaticPlaneShape planeShape = (btStaticPlaneShape)planeObjWrap.m_shape;
bool hasCollision = false;
btVector3 planeNormal; planeShape.m_planeNormal.Copy( out planeNormal );
double planeConstant = planeShape.getPlaneConstant();
btTransform planeInConvex;
convexObjWrap.getWorldTransform().inverseTimes( planeObjWrap.getWorldTransform(), out planeInConvex );
btTransform convexInPlaneTrans;
planeObjWrap.getWorldTransform().inverseTimes( convexObjWrap.getWorldTransform(), out convexInPlaneTrans );
btVector3 invPlaneNormal;
planeNormal.Invert( out invPlaneNormal );
btVector3 tmp;
planeInConvex.getBasis().Apply( planeNormal, out tmp );
btVector3 vtx; convexShape.localGetSupportingVertex( ref tmp, out vtx );
btVector3 vtxInPlane; convexInPlaneTrans.Apply( ref vtx, out vtxInPlane );
double distance = ( planeNormal.dot( ref vtxInPlane ) - planeConstant );
btVector3 vtxInPlaneProjected; vtxInPlane.AddScale( planeNormal, -distance, out vtxInPlaneProjected );
btVector3 vtxInPlaneWorld; planeObjWrap.getWorldTransform().Apply( ref vtxInPlaneProjected, out vtxInPlaneWorld );
hasCollision = distance < m_manifoldPtr.getContactBreakingThreshold();
resultOut.setPersistentManifold( m_manifoldPtr );
if( hasCollision )
{
/// report a contact. internally this will be kept persistent, and contact reduction is done
btVector3 normalOnSurfaceB = planeObjWrap.getWorldTransform().getBasis() * planeNormal;
btVector3 pOnB = vtxInPlaneWorld;
resultOut.addContactPoint( ref normalOnSurfaceB, ref pOnB, distance );
}
*/
//the perturbation algorithm doesn't work well with implicit surfaces such as spheres, cylinder and cones:
//they keep on rolling forever because of the additional off-center contact points
//so only enable the feature for polyhedral shapes (btBoxShape, btConvexHullShape etc)
if( convexShape.isPolyhedral() && resultOut.m_manifoldPtr.m_cachedPoints < m_minimumPointsPerturbationThreshold )
{
btVector3 v0, v1;
btVector3.btPlaneSpace1( ref planeShape.m_planeNormal, out v0, out v1 );
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
double angleLimit = 0.125f * btScalar.SIMD_PI;
double perturbeAngle;
double radius = convexShape.getAngularMotionDisc();
perturbeAngle = btPersistentManifold.gContactBreakingThreshold / radius;
if( perturbeAngle > angleLimit )
perturbeAngle = angleLimit;
btQuaternion perturbeRot = new btQuaternion( ref v0, perturbeAngle );
double interval = btScalar.SIMD_2_PI / (double)( m_numPerturbationIterations );
for( int i = 0; i < m_numPerturbationIterations; i++ )
{
double iterationAngle = i * interval;
btQuaternion rotq = new btQuaternion( ref planeShape.m_planeNormal, iterationAngle );
btQuaternion rotqInv;
rotq.inverse( out rotqInv );
btQuaternion tmpq, tmpq2;
rotqInv.Mult( ref perturbeRot, out tmpq );
tmpq.Mult( ref rotq, out tmpq2 );
if( m_swapped )
collideSingleContact( true, ref tmpq2
, convexObjWrap, ref body1Transform
, planeObjWrap, ref body0Transform
, dispatchInfo
, resultOut, ref planeShape.m_planeNormal, planeConstant );
else
collideSingleContact( true, ref tmpq2
, convexObjWrap, ref body0Transform
, planeObjWrap, ref body1Transform, dispatchInfo
, resultOut, ref planeShape.m_planeNormal, planeConstant );
}
}
if( m_ownManifold )
{
if( m_manifoldPtr.m_cachedPoints != 0 )
{
resultOut.refreshContactPoints();
}
}
}
internal void Initialize( btCollisionObjectWrapper compoundObjWrap, btCollisionObjectWrapper otherObjWrap, btDispatcher dispatcher, btDispatcherInfo dispatchInfo, btManifoldResult resultOut, btCollisionAlgorithm[] childCollisionAlgorithms, btPersistentManifold sharedManifold )
{
m_compoundColObjWrap = ( compoundObjWrap );
m_otherObjWrap = ( otherObjWrap );
m_dispatcher = ( dispatcher );
m_dispatchInfo = ( dispatchInfo );
m_resultOut = ( resultOut );
m_childCollisionAlgorithms = ( childCollisionAlgorithms );
m_sharedManifold = ( sharedManifold );
}
public void Initialize( btPersistentManifold mf, btCollisionAlgorithmConstructionInfo ci, btCollisionObjectWrapper body0Wrap, btCollisionObjectWrapper body1Wrap, btSimplexSolverInterface simplexSolver, btConvexPenetrationDepthSolver pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold )
{
base.Initialize( ci, body0Wrap, body1Wrap );
m_simplexSolver = ( simplexSolver );
m_pdSolver = ( pdSolver );
m_ownManifold = ( false );
m_manifoldPtr = ( mf );
m_lowLevelOfDetail = ( false );
#if USE_SEPDISTANCE_UTIL2
m_sepDistance((static_cast<btConvexShape*>(body0.getCollisionShape())).getAngularMotionDisc(),
(static_cast<btConvexShape*>(body1.getCollisionShape())).getAngularMotionDisc()),
#endif
m_numPerturbationIterations = ( numPerturbationIterations );
m_minimumPointsPerturbationThreshold = ( minimumPointsPerturbationThreshold );
}
internal override void processCollision( btCollisionObjectWrapper body0Wrap
, ref btTransform body0Transform
, btCollisionObjectWrapper body1Wrap
, ref btTransform body1Transform
, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
{
btCollisionObjectWrapper colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
btCollisionObjectWrapper otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
Debug.Assert( colObjWrap.getCollisionShape().isCompound() );
btCompoundShape compoundShape = (btCompoundShape)( colObjWrap.getCollisionShape() );
///btCompoundShape might have changed:
////make sure the internal child collision algorithm caches are still valid
if( compoundShape.getUpdateRevision() != m_compoundShapeRevision )
{
///clear and update all
removeChildAlgorithms();
preallocateChildAlgorithms( body0Wrap, body1Wrap );
m_compoundShapeRevision = compoundShape.getUpdateRevision();
}
if( m_childCollisionAlgorithms.Count == 0 )
return;
btDbvt tree = compoundShape.getDynamicAabbTree();
//use a dynamic aabb tree to cull potential child-overlaps
btCompoundLeafCallback callback = BulletGlobals.CompoundLeafCallbackPool.Get();
callback.Initialize( colObjWrap, otherObjWrap, m_dispatcher, dispatchInfo, resultOut, m_childCollisionAlgorithms.InternalArray, m_sharedManifold);
///we need to refresh all contact manifolds
///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
{
int i;
btManifoldArray manifoldArray = new btManifoldArray();
for( i = 0; i < m_childCollisionAlgorithms.Count; i++ )
{
if( m_childCollisionAlgorithms[i] != null )
{
m_childCollisionAlgorithms[i].getAllContactManifolds( manifoldArray );
for( int m = 0; m < manifoldArray.Count; m++ )
{
if( manifoldArray[m].m_cachedPoints > 0 )
{
resultOut.setPersistentManifold( manifoldArray[m] );
resultOut.refreshContactPoints();
resultOut.setPersistentManifold( null );//??necessary?
}
}
manifoldArray.Count = ( 0 );
}
}
}
if( tree != null )
{
btVector3 localAabbMin, localAabbMax;
btTransform otherInCompoundSpace;
if( m_isSwapped )
body1Transform.inverseTimes(ref body0Transform, out otherInCompoundSpace);
else
body0Transform.inverseTimes( ref body1Transform, out otherInCompoundSpace );
otherObjWrap.getCollisionShape().getAabb( ref otherInCompoundSpace, out localAabbMin, out localAabbMax );
btDbvt.btDbvtVolume bounds = btDbvt.btDbvtVolume.FromMM( ref localAabbMin, ref localAabbMax );
//process all children, that overlap with the given AABB bounds
btDbvt.CollideTV( tree.m_root, ref bounds, callback );
}
else
{
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.Count;
int i;
for( i = 0; i < numChildren; i++ )
{
callback.ProcessChildShape( compoundShape.getChildShape( i ), i );
}
}
{
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.Count;
int i;
//btManifoldArray manifoldArray;
btCollisionShape childShape = null;
//btITransform orgTrans;
btTransform newChildWorldTrans;
btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1;
for( i = 0; i < numChildren; i++ )
{
if( m_childCollisionAlgorithms[i] != null )
{
childShape = compoundShape.getChildShape( i );
//if not longer overlapping, remove the algorithm
//orgTrans = colObjWrap.m_worldTransform;
//btTransform childTrans = compoundShape.getChildTransform( i );
( ( m_isSwapped ) ? body1Transform : body0Transform ).Apply( ref compoundShape.m_children.InternalArray[i].m_transform, out newChildWorldTrans );
//perform an AABB check first
childShape.getAabb( ref newChildWorldTrans, out aabbMin0, out aabbMax0 );
if( m_isSwapped )
otherObjWrap.m_shape.getAabb( ref body0Transform, out aabbMin1, out aabbMax1 );
else
otherObjWrap.m_shape.getAabb( ref body1Transform, out aabbMin1, out aabbMax1 );
if( !btAabbUtil.TestAabbAgainstAabb2( ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1 ) )
{
//m_childCollisionAlgorithms[i].~btCollisionAlgorithm();
m_dispatcher.freeCollisionAlgorithm( m_childCollisionAlgorithms[i] );
m_childCollisionAlgorithms[i] = null;
}
}
}
}
}
//
// Convex-Convex collision algorithm
//
internal override void processCollision( btCollisionObjectWrapper body0Wrap
, ref btTransform body0Transform
, btCollisionObjectWrapper body1Wrap
, ref btTransform body1Transform
, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
{
if( m_manifoldPtr == null )
{
//swapped?
m_manifoldPtr = m_dispatcher.getNewManifold( body0Wrap.m_collisionObject, body1Wrap.m_collisionObject );
m_ownManifold = true;
}
resultOut.setPersistentManifold( m_manifoldPtr );
//comment-out next line to test multi-contact generation
//resultOut.getPersistentManifold().clearManifold();
btConvexShape min0 = (btConvexShape)body0Wrap.getCollisionShape();
btConvexShape min1 = (btConvexShape)body1Wrap.getCollisionShape();
btVector3 normalOnB;
btVector3 pointOnBWorld;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
if( ( min0.getShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE )
&& ( min1.getShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE ) )
{
btCapsuleShape capsuleA = (btCapsuleShape)min0;
btCapsuleShape capsuleB = (btCapsuleShape)min1;
// btVector3 localScalingA = capsuleA.getLocalScaling();
// btVector3 localScalingB = capsuleB.getLocalScaling();
double threshold = m_manifoldPtr.getContactBreakingThreshold();
double dist = capsuleCapsuleDistance( out normalOnB, out pointOnBWorld
, capsuleA.getHalfHeight(), capsuleA.getRadius()
, capsuleB.getHalfHeight(), capsuleB.getRadius()
, capsuleA.getUpAxis(), capsuleB.getUpAxis()
, ref body0Wrap.m_collisionObject.m_worldTransform, ref body1Wrap.m_collisionObject.m_worldTransform, threshold );
if( dist < threshold )
{
Debug.Assert( normalOnB.length2() >= ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) );
resultOut.addContactPoint( ref normalOnB, ref pointOnBWorld, dist );
}
resultOut.refreshContactPoints();
return;
}
#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
m_sepDistance.updateSeparatingDistance(body0.getWorldTransform(),body1.getWorldTransform());
}
if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0)
#endif //USE_SEPDISTANCE_UTIL2
{
btGjkPairDetector.ClosestPointInput input = BulletGlobals.ClosestPointInputPool.Get();
input.Initialize();
btGjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get();
gjkPairDetector.Initialize( min0, min1, m_simplexSolver, m_pdSolver );
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.setMinkowskiA( min0 );
gjkPairDetector.setMinkowskiB( min1 );
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
} else
#endif //USE_SEPDISTANCE_UTIL2
{
//if (dispatchInfo.m_convexMaxDistanceUseCPT)
//{
// input.m_maximumDistanceSquared = min0.getMargin() + min1.getMargin() + m_manifoldPtr.getContactProcessingThreshold();
//} else
//{
input.m_maximumDistanceSquared = min0.getMargin() + min1.getMargin() + m_manifoldPtr.getContactBreakingThreshold();
// }
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
input.m_transformA = body0Transform;
input.m_transformB = body1Transform;
#if USE_SEPDISTANCE_UTIL2
double sepDist = 0;
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
sepDist = gjkPairDetector.getCachedSeparatingDistance();
if (sepDist>SIMD_EPSILON)
{
sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
//now perturbe directions to get multiple contact points
}
}
#endif //USE_SEPDISTANCE_UTIL2
if( min0.isPolyhedral() && min1.isPolyhedral() )
{
btDummyResult dummy = new btDummyResult();
///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it
double min0Margin = min0.getShapeType() == BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE ? 0 : min0.getMargin();
double min1Margin = min1.getShapeType() == BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE ? 0 : min1.getMargin();
btWithoutMarginResult withoutMargin = new btWithoutMarginResult( resultOut, min0Margin, min1Margin );
btPolyhedralConvexShape polyhedronA = (btPolyhedralConvexShape)min0;
btPolyhedralConvexShape polyhedronB = (btPolyhedralConvexShape)min1;
if( polyhedronA.getConvexPolyhedron() != null && polyhedronB.getConvexPolyhedron() != null )
{
double threshold = m_manifoldPtr.getContactBreakingThreshold();
double minDist = -1e30f;
btVector3 sepNormalWorldSpace;
bool foundSepAxis = true;
if( dispatchInfo.m_enableSatConvex )
{
foundSepAxis = btPolyhedralContactClipping.findSeparatingAxis(
polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(),
ref body0Wrap.m_collisionObject.m_worldTransform,
ref body1Wrap.m_collisionObject.m_worldTransform,
out sepNormalWorldSpace, resultOut );
}
else
{
#if ZERO_MARGIN
gjkPairDetector.setIgnoreMargin(true);
gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
#else
gjkPairDetector.getClosestPoints( input, withoutMargin, dispatchInfo.m_debugDraw );
//gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
#endif //ZERO_MARGIN
//double l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
//if (l2>SIMD_EPSILON)
{
sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1/l2);
//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0.getMargin()+min1.getMargin();
#if ZERO_MARGIN
foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0;
#else
foundSepAxis = withoutMargin.m_foundResult && minDist < 0;//-(min0.getMargin()+min1.getMargin());
#endif
}
}
if( foundSepAxis )
{
// Console.WriteLine("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z);
btPolyhedralContactClipping.clipHullAgainstHull( ref sepNormalWorldSpace, polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(),
ref body0Wrap.m_collisionObject.m_worldTransform,
ref body1Wrap.m_collisionObject.m_worldTransform
, minDist - threshold, threshold, resultOut );
}
if( m_ownManifold )
{
resultOut.refreshContactPoints();
}
BulletGlobals.ClosestPointInputPool.Free( input );
BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector );
return;
}
else
{
//we can also deal with convex versus triangle (without connectivity data)
if( polyhedronA.getConvexPolyhedron() != null && polyhedronB.getShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE )
{
btVertexArray vertices = new btVertexArray();
btTriangleShape tri = (btTriangleShape)polyhedronB;
btVector3 tmp;
body1Transform.Apply( ref tri.m_vertices1, out tmp );
vertices.Add( ref tmp );
body1Transform.Apply( ref tri.m_vertices2, out tmp );
vertices.Add( ref tmp );
body1Transform.Apply( ref tri.m_vertices3, out tmp );
vertices.Add( ref tmp );
//tri.initializePolyhedralFeatures();
double threshold = m_manifoldPtr.getContactBreakingThreshold();
btVector3 sepNormalWorldSpace;
double minDist = -btScalar.BT_LARGE_FLOAT;
double maxDist = threshold;
bool foundSepAxis = false;
if( false )
{
polyhedronB.initializePolyhedralFeatures();
foundSepAxis = btPolyhedralContactClipping.findSeparatingAxis(
polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(),
ref body0Wrap.m_collisionObject.m_worldTransform,
ref body1Wrap.m_collisionObject.m_worldTransform,
out sepNormalWorldSpace, resultOut );
// Console.WriteLine("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z);
btPolyhedralContactClipping.clipFaceAgainstHull( ref sepNormalWorldSpace
, polyhedronA.getConvexPolyhedron(),
ref body0Wrap.m_collisionObject.m_worldTransform, vertices, minDist - threshold, maxDist, resultOut );
}
else
{
#if ZERO_MARGIN
gjkPairDetector.setIgnoreMargin(true);
gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
#else
gjkPairDetector.getClosestPoints( input, dummy, dispatchInfo.m_debugDraw );
#endif//ZERO_MARGIN
double l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
if( l2 > btScalar.SIMD_EPSILON )
{
gjkPairDetector.getCachedSeparatingAxis().Mult( ( 1 / l2 ), out sepNormalWorldSpace );
//minDist = gjkPairDetector.getCachedSeparatingDistance();
//maxDist = threshold;
minDist = gjkPairDetector.getCachedSeparatingDistance() - min0.getMargin() - min1.getMargin();
//foundSepAxis = true;
btPolyhedralContactClipping.clipFaceAgainstHull( ref sepNormalWorldSpace
, polyhedronA.getConvexPolyhedron(),
ref body0Wrap.m_collisionObject.m_worldTransform, vertices, minDist - threshold, maxDist, resultOut );
}
else
{
//sepNormalWorldSpace = btVector3.Zero;
}
}
if( m_ownManifold )
{
resultOut.refreshContactPoints();
}
BulletGlobals.ClosestPointInputPool.Free( input );
BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector );
return;
}
}
}
gjkPairDetector.getClosestPoints( input, resultOut, dispatchInfo.m_debugDraw );
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
if( m_numPerturbationIterations != 0
&& resultOut.m_manifoldPtr.m_cachedPoints < m_minimumPointsPerturbationThreshold )
{
int i;
btVector3 v0, v1;
btVector3 sepNormalWorldSpace;
double l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
if( l2 > btScalar.SIMD_EPSILON )
{
gjkPairDetector.getCachedSeparatingAxis().Mult( ( 1 / l2 ), out sepNormalWorldSpace );
btVector3.btPlaneSpace1( ref sepNormalWorldSpace, out v0, out v1 );
bool perturbeA = true;
double angleLimit = 0.125f * btScalar.SIMD_PI;
double perturbeAngle;
double radiusA = min0.getAngularMotionDisc();
double radiusB = min1.getAngularMotionDisc();
if( radiusA < radiusB )
{
perturbeAngle = btPersistentManifold.gContactBreakingThreshold / radiusA;
perturbeA = true;
}
else
{
perturbeAngle = btPersistentManifold.gContactBreakingThreshold / radiusB;
perturbeA = false;
}
if( perturbeAngle > angleLimit )
perturbeAngle = angleLimit;
btTransform unPerturbedTransform;
if( perturbeA )
{
unPerturbedTransform = input.m_transformA;
}
else
{
unPerturbedTransform = input.m_transformB;
}
for( i = 0; i < m_numPerturbationIterations; i++ )
{
if( v0.length2() > btScalar.SIMD_EPSILON )
{
btQuaternion perturbeRot = new btQuaternion( ref v0, perturbeAngle );
double iterationAngle = i * ( btScalar.SIMD_2_PI / (double)( m_numPerturbationIterations ) );
btQuaternion rotq = new btQuaternion( ref sepNormalWorldSpace, iterationAngle );
if( perturbeA )
{
btQuaternion tmpq;
btQuaternion tmpq2;
rotq.inverse( out tmpq );
btQuaternion.Mult( ref tmpq, ref perturbeRot, out tmpq2 );
btQuaternion.Mult( ref tmpq2, ref rotq, out tmpq );
btMatrix3x3 m = new btMatrix3x3( ref tmpq );
btMatrix3x3 m2; body0Transform.getBasis( out m2 );
btMatrix3x3 m3;
btMatrix3x3.Mult( ref m, ref m2, out m3 );
input.m_transformA.setBasis( ref m3 );
input.m_transformB = body1Transform;
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.drawTransform(input.m_transformA,10.0);
#endif //DEBUG_CONTACTS
}
else
{
btQuaternion tmpq;
btQuaternion tmpq2;
rotq.inverse( out tmpq );
btQuaternion.Mult( ref tmpq, ref perturbeRot, out tmpq2 );
btQuaternion.Mult( ref tmpq2, ref rotq, out tmpq );
btMatrix3x3 m = new btMatrix3x3( ref tmpq );
btMatrix3x3 m2; body1Transform.getBasis( out m2 );
btMatrix3x3 m3;
btMatrix3x3.Mult( ref m, ref m2, out m3 );
input.m_transformA = body0Transform;
input.m_transformB.setBasis( ref m3 );
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.drawTransform(input.m_transformB,10.0);
#endif
}
btPerturbedContactResult perturbedResultOut = BulletGlobals.PerturbedContactResultPool.Get();
if( perturbeA )
perturbedResultOut.Initialize( resultOut
, ref input.m_transformA, ref input.m_transformB
, ref input.m_transformA
, perturbeA
, dispatchInfo.m_debugDraw );
else
perturbedResultOut.Initialize( resultOut
, ref input.m_transformA, ref input.m_transformB
, ref input.m_transformB
, perturbeA
, dispatchInfo.m_debugDraw );
gjkPairDetector.getClosestPoints( input, perturbedResultOut, dispatchInfo.m_debugDraw );
BulletGlobals.PerturbedContactResultPool.Free( perturbedResultOut );
}
}
}
}
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
{
m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0.getWorldTransform(),body1.getWorldTransform());
}
#endif //USE_SEPDISTANCE_UTIL2
BulletGlobals.ClosestPointInputPool.Free( input );
BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector );
}
if( m_ownManifold )
{
resultOut.refreshContactPoints();
}
}
internal void Initialize( btCollisionAlgorithmConstructionInfo ci, btCollisionObjectWrapper body0Wrap, btCollisionObjectWrapper body1Wrap, bool isSwapped )
{
base.Initialize( ci, body0Wrap, body1Wrap );
m_isSwapped = ( isSwapped );
m_sharedManifold = ( ci.m_manifold );
m_ownsManifold = false;
btCollisionObjectWrapper colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
Debug.Assert( colObjWrap.getCollisionShape().isCompound() );
btCompoundShape compoundShape = (btCompoundShape)( colObjWrap.getCollisionShape() );
m_compoundShapeRevision = compoundShape.getUpdateRevision();
preallocateChildAlgorithms( body0Wrap, body1Wrap );
}
internal void Initialize( btCollisionObjectWrapper obj0Wrap, btCollisionObjectWrapper obj1Wrap, ContactResultCallback resultCallback )
{
base.Initialize( obj0Wrap, obj1Wrap );
m_resultCallback = ( resultCallback );
}