public override void processCollision(CollisionObject col0, CollisionObject col1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
return;
resultOut.PersistentManifold = m_manifoldPtr;
SphereShape sphere0 = (SphereShape)col0.CollisionShape;
SphereShape sphere1 = (SphereShape)col1.CollisionShape;
btVector3 diff = col0.WorldTransform.Origin - col1.WorldTransform.Origin;
float len = diff.Length;
float radius0 = sphere0.Radius;
float radius1 = sphere1.Radius;
#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)
float dist = len - (radius0 + radius1);
btVector3 normalOnSurfaceB = new btVector3(1, 0, 0);
if (len > BulletGlobal.SIMD_EPSILON)
{
//normalOnSurfaceB = diff / len;
btVector3.Divide(ref diff, len, out normalOnSurfaceB);
}
///point on A (worldspace)
///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
///point on B (worldspace)
btVector3 pos1;// = col1.WorldTransform.Origin + radius1 * normalOnSurfaceB;
{
btVector3 temp;
btVector3.Multiply(ref normalOnSurfaceB, radius1, out temp);
btVector3.Add(ref col1.WorldTransform.Origin, ref temp, 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
}
public override void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
return;
CollisionObject convexObj = m_isSwapped ? body1 : body0;
CollisionObject planeObj = m_isSwapped ? body0 : body1;
ConvexShape convexShape = (ConvexShape)convexObj.CollisionShape;
StaticPlaneShape planeShape = (StaticPlaneShape)planeObj.CollisionShape;
btVector3 planeNormal = planeShape.PlaneNormal;
//const btScalar& planeConstant = planeShape->getPlaneConstant();
//first perform a collision query with the non-perturbated collision objects
{
btQuaternion rotq = new btQuaternion(0, 0, 0, 1);
collideSingleContact(rotq, body0, body1, dispatchInfo, ref resultOut);
}
if (resultOut.PersistentManifold.NumContacts < m_minimumPointsPerturbationThreshold)
{
btVector3 v0, v1;
btVector3.PlaneSpace1(ref planeNormal, out v0, out v1);
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
float angleLimit = 0.125f * BulletGlobal.SIMD_PI;
float perturbeAngle;
float radius = convexShape.getAngularMotionDisc();
perturbeAngle = PersistentManifold.gContactBreakingThreshold / radius;
if (perturbeAngle > angleLimit)
perturbeAngle = angleLimit;
btQuaternion perturbeRot = new btQuaternion(v0, perturbeAngle);
for (int i = 0; i < m_numPerturbationIterations; i++)
{
float iterationAngle = (float)i * (BulletGlobal.SIMD_2_PI / (float)m_numPerturbationIterations);
btQuaternion rotq = new btQuaternion(planeNormal, iterationAngle);
collideSingleContact(rotq.inverse() * perturbeRot * rotq, body0, body1, dispatchInfo, ref resultOut);
}
}
if (m_ownManifold)
{
if (m_manifoldPtr.NumContacts != 0)
{
resultOut.refreshContactPoints();
}
}
}
public override void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
return;
CollisionObject sphereObj = m_isSwapped ? body1 : body0;
CollisionObject boxObj = m_isSwapped ? body0 : body1;
SphereShape sphere0 = (SphereShape)sphereObj.CollisionShape;
//btVector3 normalOnSurfaceB;
btVector3 pOnBox=btVector3.Zero, pOnSphere=btVector3.Zero;
btVector3 sphereCenter = sphereObj.WorldTransform.Origin;
float radius = sphere0.Radius;
float dist = getSphereDistance(boxObj, ref pOnBox, ref pOnSphere, sphereCenter, radius);
resultOut.PersistentManifold = m_manifoldPtr;
if (dist < BulletGlobal.SIMD_EPSILON)
{
btVector3 normalOnSurfaceB;// = (pOnBox - pOnSphere).normalize();
(pOnBox - pOnSphere).normalize(out normalOnSurfaceB);
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.addContactPoint(ref normalOnSurfaceB, ref pOnBox, dist);
}
if (m_ownManifold)
{
if (m_manifoldPtr.NumContacts != 0)
{
resultOut.refreshContactPoints();
}
}
}
public abstract float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut);
public abstract void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut);
public override float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
{
//not yet
return 1f;
}
public void Constructor(DispatcherInfo dispatchInfo, CollisionDispatcher dispatcher)
{
m_dispatchInfo = dispatchInfo;
m_dispatcher = dispatcher;
}
public override float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
{
throw new NotImplementedException();
}
public override void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
{
throw new NotImplementedException();
}
public override void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
//swapped?
m_manifoldPtr = m_dispatcher.getNewManifold(body0, body1);
m_ownManifold = true;
}
resultOut.PersistentManifold = m_manifoldPtr;
//comment-out next line to test multi-contact generation
//resultOut->getPersistentManifold()->clearManifold();
ConvexShape min0 = (ConvexShape)(body0.CollisionShape);
ConvexShape min1 = (ConvexShape)(body1.CollisionShape);
btVector3 normalOnB;
btVector3 pointOnBWorld;
if ((min0.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
{
CapsuleShape capsuleA = (CapsuleShape)min0;
CapsuleShape capsuleB = (CapsuleShape)min1;
btVector3 localScalingA = capsuleA.LocalScaling;
btVector3 localScalingB = capsuleB.LocalScaling;
float threshold = m_manifoldPtr.ContactBreakingThreshold;
float dist = capsuleCapsuleDistance(out normalOnB, out pointOnBWorld, capsuleA.HalfHeight, capsuleA.Radius,
capsuleB.HalfHeight, capsuleB.Radius, capsuleA.UpAxis, capsuleB.UpAxis,
body0.WorldTransform, body1.WorldTransform, threshold);
if (dist < threshold)
{
Debug.Assert(normalOnB.Length2 >= (BulletGlobal.SIMD_EPSILON * BulletGlobal.SIMD_EPSILON));
resultOut.addContactPoint(ref normalOnB, ref pointOnBWorld, dist);
}
resultOut.refreshContactPoints();
return;
}
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
}
if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
#endif //USE_SEPDISTANCE_UTIL2
{
ClosestPointInput input;
GjkPairDetector gjkPairDetector = new GjkPairDetector(min0, min1, m_simplexSolver, m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.MinkowskiA = min0;
gjkPairDetector.MinkowskiB = min1;
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
} else
#endif //USE_SEPDISTANCE_UTIL2
{
input.m_maximumDistanceSquared = min0.Margin + min1.Margin + m_manifoldPtr.ContactBreakingThreshold;
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
//input.m_stackAlloc = dispatchInfo.m_stackAllocator;
input.m_transformA = body0.WorldTransform;
input.m_transformB = body1.WorldTransform;
gjkPairDetector.getClosestPoints(ref input, ref resultOut, dispatchInfo.m_debugDraw);
#if USE_SEPDISTANCE_UTIL2
btScalar sepDist = 0.f;
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
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
if (m_numPerturbationIterations != 0 && resultOut.PersistentManifold.NumContacts < m_minimumPointsPerturbationThreshold)
{
int i;
btVector3 v0, v1;
btVector3 sepNormalWorldSpace;
//sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
gjkPairDetector.getCachedSeparatingAxis().normalized(out sepNormalWorldSpace);
btVector3.PlaneSpace1(ref sepNormalWorldSpace, out v0, out v1);
bool perturbeA = true;
float angleLimit = 0.125f * BulletGlobal.SIMD_PI;
float perturbeAngle;
float radiusA = min0.getAngularMotionDisc();
float radiusB = min1.getAngularMotionDisc();
if (radiusA < radiusB)
{
perturbeAngle = PersistentManifold.gContactBreakingThreshold / radiusA;
perturbeA = true;
}
else
{
perturbeAngle = PersistentManifold.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 > BulletGlobal.SIMD_EPSILON)
{
btQuaternion perturbeRot = new btQuaternion(v0, perturbeAngle);
float iterationAngle = i * (BulletGlobal.SIMD_2_PI / m_numPerturbationIterations);
btQuaternion rotq = new btQuaternion(sepNormalWorldSpace, iterationAngle);
if (perturbeA)
{
#region input.m_transformA.Basis = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq) * body0.WorldTransform.Basis;
{
btMatrix3x3 temp = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq);
btMatrix3x3.Multiply(ref temp, ref body0.WorldTransform.Basis, out input.m_transformA.Basis);
}
#endregion
input.m_transformB = body1.WorldTransform;
#if DEBUG
dispatchInfo.m_debugDraw.drawTransform(ref input.m_transformA, 10.0f);
#endif //DEBUG_CONTACTS
}
else
{
input.m_transformA = body0.WorldTransform;
#region input.m_transformB.Basis = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq) * body1.WorldTransform.Basis;
{
btMatrix3x3 temp = new btMatrix3x3(rotq.inverse() * perturbeRot * rotq);
btMatrix3x3.Multiply(ref temp, ref body1.WorldTransform.Basis, out input.m_transformB.Basis);
}
#endregion
#if DEBUG
dispatchInfo.m_debugDraw.drawTransform(ref input.m_transformB, 10.0f);
#endif
}
PerturbedContactResult perturbedResultOut = new PerturbedContactResult(input.m_transformA, input.m_transformB, unPerturbedTransform, perturbeA, dispatchInfo.m_debugDraw);
gjkPairDetector.getClosestPoints(ref input, ref perturbedResultOut, ref resultOut, dispatchInfo.m_debugDraw);
}
}
}
#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
}
if (m_ownManifold)
{
resultOut.refreshContactPoints();
}
}
public override float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
{
throw new NotImplementedException();
#if false//未移植
///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
///col0->m_worldTransform,
float resultFraction = btScalar(1.);
float squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
float squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
squareMot1 < col1->getCcdSquareMotionThreshold())
return resultFraction;
if (disableCcd)
return btScalar(1.);
//An adhoc way of testing the Continuous Collision Detection algorithms
//One object is approximated as a sphere, to simplify things
//Starting in penetration should report no time of impact
//For proper CCD, better accuracy and handling of 'allowed' penetration should be added
//also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
/// Convex0 against sphere for Convex1
{
ConvexShape convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
SphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
btConvexCast::CastResult result;
btVoronoiSimplexSolver voronoiSimplex;
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
{
//store result.m_fraction in both bodies
if (col0->getHitFraction()> result.m_fraction)
col0->setHitFraction( result.m_fraction );
if (col1->getHitFraction() > result.m_fraction)
col1->setHitFraction( result.m_fraction);
if (resultFraction > result.m_fraction)
resultFraction = result.m_fraction;
}
}
/// Sphere (for convex0) against Convex1
{
btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
btConvexCast::CastResult result;
btVoronoiSimplexSolver voronoiSimplex;
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
{
//store result.m_fraction in both bodies
if (col0->getHitFraction() > result.m_fraction)
col0->setHitFraction( result.m_fraction);
if (col1->getHitFraction() > result.m_fraction)
col1->setHitFraction( result.m_fraction);
if (resultFraction > result.m_fraction)
resultFraction = result.m_fraction;
}
}
return resultFraction;
#endif
}
public void collideSingleContact(btQuaternion perturbeRot, CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
{
CollisionObject convexObj = m_isSwapped ? body1 : body0;
CollisionObject planeObj = m_isSwapped ? body0 : body1;
ConvexShape convexShape = (ConvexShape)convexObj.CollisionShape;
StaticPlaneShape planeShape = (StaticPlaneShape)planeObj.CollisionShape;
bool hasCollision = false;
btVector3 planeNormal = planeShape.PlaneNormal;
float planeConstant = planeShape.PlaneConstant;
btTransform convexWorldTransform = convexObj.WorldTransform;
btTransform convexInPlaneTrans;
convexInPlaneTrans = planeObj.WorldTransform.inverse() * convexWorldTransform;
//now perturbe the convex-world transform
#region convexWorldTransform.Basis *= new btMatrix3x3(perturbeRot);
{
btMatrix3x3 temp1 = convexWorldTransform.Basis, temp2 = new btMatrix3x3(perturbeRot);
btMatrix3x3.Multiply(ref temp1, ref temp2, out convexWorldTransform.Basis);
}
#endregion
btTransform planeInConvex;
planeInConvex = convexWorldTransform.inverse() * planeObj.WorldTransform;
#region btVector3 vtx = convexShape.localGetSupportingVertex(planeInConvex.Basis * -planeNormal);
btVector3 vtx;
{
btVector3 temp, temp2;
temp2 = -planeNormal;
btMatrix3x3.Multiply(ref planeInConvex.Basis, ref temp2, out temp);
//vtx = convexShape.localGetSupportingVertex(temp);
convexShape.localGetSupportingVertex(ref temp, out vtx);
}
#endregion
btVector3 vtxInPlane = convexInPlaneTrans * vtx;
float distance = (planeNormal.dot(vtxInPlane) - planeConstant);
btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
btVector3 vtxInPlaneWorld = planeObj.WorldTransform * vtxInPlaneProjected;
hasCollision = distance < m_manifoldPtr.ContactBreakingThreshold;
resultOut.PersistentManifold = m_manifoldPtr;
if (hasCollision)
{
/// report a contact. internally this will be kept persistent, and contact reduction is done
btVector3 normalOnSurfaceB;// = planeObj.WorldTransform.Basis * planeNormal;
btMatrix3x3.Multiply(ref planeObj.WorldTransform.Basis, ref planeNormal, out normalOnSurfaceB);
btVector3 pOnB = vtxInPlaneWorld;
resultOut.addContactPoint(ref normalOnSurfaceB, ref pOnB, distance);
}
}
//by default, Bullet will use this near callback
static void DefaultNearCallback(BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo)
{
CollisionObject colObj0 = (CollisionObject)collisionPair.m_pProxy0.m_clientObject;
CollisionObject colObj1 = (CollisionObject)collisionPair.m_pProxy1.m_clientObject;
if (dispatcher.needsCollision(colObj0,colObj1))
{
//dispatcher will keep algorithms persistent in the collision pair
if (collisionPair.m_algorithm==null)
{
collisionPair.m_algorithm = dispatcher.findAlgorithm(colObj0,colObj1);
}
if (collisionPair.m_algorithm!=null)
{
ManifoldResult contactPointResult=new ManifoldResult(colObj0,colObj1);
if (dispatchInfo.m_dispatchFunc == DispatchFunc.DISPATCH_DISCRETE)
{
//discrete collision detection query
collisionPair.m_algorithm.processCollision(colObj0,colObj1,dispatchInfo,ref contactPointResult);
} else
{
//continuous collision detection query, time of impact (toi)
float toi = collisionPair.m_algorithm.calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,ref contactPointResult);
if (dispatchInfo.m_timeOfImpact > toi)
dispatchInfo.m_timeOfImpact = toi;
}
}
}
}
public virtual void dispatchAllCollisionPairs(IOverlappingPairCache pairCache, DispatcherInfo dispatchInfo, IDispatcher dispatcher)
{
collisionCallback.Constructor(dispatchInfo, this);
pairCache.processAllOverlappingPairs(collisionCallback, dispatcher);
}
