//
// 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 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
}
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;
}
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 );
}
}
public void setTimeStepAndCounters( double collisionMarginTriangle
, btDispatcherInfo dispatchInfo, btCollisionObjectWrapper convexBodyWrap
, ref btTransform convexTransform
, btCollisionObjectWrapper triBodyWrap
, ref btTransform triBodyTransform
, btManifoldResult resultOut )
{
m_convexBodyWrap = convexBodyWrap;
m_triBodyWrap = triBodyWrap;
m_dispatchInfoPtr = dispatchInfo;
m_collisionMarginTriangle = collisionMarginTriangle;
m_resultOut = resultOut;
//recalc aabbs
btTransform convexInTriangleSpace;
btTransform invTrans;
triBodyTransform.inverse( out invTrans );
invTrans.Apply( ref convexTransform, out convexInTriangleSpace );
btCollisionShape convexShape = m_convexBodyWrap.getCollisionShape();
//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody.m_collisionShape);
convexShape.getAabb( ref convexInTriangleSpace, out m_aabbMin, out m_aabbMax );
double extraMargin = collisionMarginTriangle;
btVector3 extra = new btVector3( extraMargin );
m_aabbMax += extra;
m_aabbMin -= extra;
}