public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
//swapped?
m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
m_ownManifold = true;
}
resultOut.SetPersistentManifold(m_manifoldPtr);
//comment-out next line to test multi-contact generation
//resultOut.getPersistentManifold().clearManifold();
ConvexShape min0 = body0.GetCollisionShape() as ConvexShape;
ConvexShape min1 = body1.GetCollisionShape() as ConvexShape;
IndexedVector3 normalOnB = IndexedVector3.Zero;
IndexedVector3 pointOnBWorld = IndexedVector3.Zero;
{
ClosestPointInput input = ClosestPointInput.Default();
using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get())
{
gjkPairDetector.Initialize(min0, min1, m_simplexSolver, m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.SetMinkowskiA(min0);
gjkPairDetector.SetMinkowskiB(min1);
{
input.m_maximumDistanceSquared = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold();
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
gjkPairDetector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), false);
#if DEBUG
if (BulletGlobals.g_streamWriter != null)
{
BulletGlobals.g_streamWriter.WriteLine("c2dc2d processCollision");
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformA", input.m_transformA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformB", input.m_transformB);
}
#endif
}
//BulletGlobals.GjkPairDetectorPool.Free(gjkPairDetector);
//btVector3 v0,v1;
//btVector3 sepNormalWorldSpace;
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//resultOut = new ManifoldResult();
if (m_manifoldPtr == null)
{
return;
}
CollisionObject sphereObj = m_swapped ? body1 : body0;
CollisionObject triObj = m_swapped ? body0 : body1;
SphereShape sphere = sphereObj.GetCollisionShape() as SphereShape;
TriangleShape triangle = triObj.GetCollisionShape() as TriangleShape;
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.SetPersistentManifold(m_manifoldPtr);
using (SphereTriangleDetector detector = BulletGlobals.SphereTriangleDetectorPool.Get())
{
detector.Initialize(sphere, triangle, m_manifoldPtr.GetContactBreakingThreshold());
ClosestPointInput input = ClosestPointInput.Default();
input.m_maximumDistanceSquared = float.MaxValue;
sphereObj.GetWorldTransform(out input.m_transformA);
triObj.GetWorldTransform(out input.m_transformB);
bool swapResults = m_swapped;
detector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), swapResults);
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
}
//this constructor doesn't own the dispatcher and paircache/broadphase
public CollisionWorld(IDispatcher dispatcher, IBroadphaseInterface broadphasePairCache, ICollisionConfiguration collisionConfiguration)
{
m_dispatcher1 = dispatcher;
m_broadphasePairCache = broadphasePairCache;
m_collisionObjects = new ObjectArray<CollisionObject>();
m_dispatchInfo = new DispatcherInfo();
m_forceUpdateAllAabbs = true;
}
public void Initialize(CollisionObject compoundObj, CollisionObject otherObj, IDispatcher dispatcher, DispatcherInfo dispatchInfo, ManifoldResult resultOut, CompoundCollisionAlgorithm parent, IList <CollisionAlgorithm> childCollisionAlgorithms, PersistentManifold sharedManifold)
{
m_compoundColObj = compoundObj;
m_otherObj = otherObj;
m_dispatcher = dispatcher;
m_dispatchInfo = dispatchInfo;
m_resultOut = resultOut;
m_childCollisionAlgorithms = childCollisionAlgorithms;
m_sharedManifold = sharedManifold;
m_parent = parent;
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
resultOut.SetPersistentManifold(m_manifoldPtr);
SphereShape sphere0 = body0.GetCollisionShape() as SphereShape;
SphereShape sphere1 = body1.GetCollisionShape() as SphereShape;
IndexedVector3 diff = body0.GetWorldTransform()._origin - body1.GetWorldTransform()._origin;
float len = diff.Length();
float radius0 = sphere0.GetRadius();
float 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)
float dist = len - (radius0 + radius1);
IndexedVector3 normalOnSurfaceB = new IndexedVector3(1, 0, 0);
if (len > MathUtil.SIMD_EPSILON)
{
normalOnSurfaceB = diff / len;
}
///point on A (worldspace)
///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
///point on B (worldspace)
IndexedVector3 pos1 = body1.GetWorldTransform()._origin + radius1 * normalOnSurfaceB;
/// 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 void SetTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
m_dispatchInfoPtr = dispatchInfo;
m_collisionMarginTriangle = collisionMarginTriangle;
m_resultOut = resultOut;
//recalc aabbs
//IndexedMatrix convexInTriangleSpace = MathUtil.bulletMatrixMultiply(IndexedMatrix.Invert(m_triBody.getWorldTransform()) , m_convexBody.getWorldTransform());
IndexedMatrix convexInTriangleSpace = m_triBody.GetWorldTransform().Inverse() * m_convexBody.GetWorldTransform();
CollisionShape convexShape = m_convexBody.GetCollisionShape();
convexShape.GetAabb(ref convexInTriangleSpace, out m_aabbMin, out m_aabbMax);
float extraMargin = collisionMarginTriangle;
IndexedVector3 extra = new IndexedVector3(extraMargin);
m_aabbMax += extra;
m_aabbMin -= extra;
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
CollisionObject col0 = body0;
CollisionObject col1 = body1;
Box2dShape box0 = (Box2dShape)col0.GetCollisionShape();
Box2dShape box1 = (Box2dShape)col1.GetCollisionShape();
resultOut.SetPersistentManifold(m_manifoldPtr);
B2CollidePolygons(ref resultOut, box0, col0.GetWorldTransform(), box1, col1.GetWorldTransform());
// refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
ClearCache();
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::ProcessCollision");
}
m_resultOut = resultOut;
m_dispatchInfo = dispatchInfo;
GImpactShapeInterface gimpactshape0;
GImpactShapeInterface gimpactshape1;
if (body0.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE)
{
gimpactshape0 = body0.GetCollisionShape() as GImpactShapeInterface;
if (body1.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE)
{
gimpactshape1 = body1.GetCollisionShape() as GImpactShapeInterface;
GImpactVsGImpact(body0, body1, gimpactshape0, gimpactshape1);
}
else
{
GImpactVsShape(body0, body1, gimpactshape0, body1.GetCollisionShape(), false);
}
}
else if (body1.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE)
{
gimpactshape1 = body1.GetCollisionShape() as GImpactShapeInterface;
GImpactVsShape(body1, body0, gimpactshape1, body0.GetCollisionShape(), true);
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//(void)dispatchInfo;
//(void)resultOut;
if (m_manifoldPtr == null)
{
return;
}
CollisionObject sphereObj = m_isSwapped ? body1 : body0;
CollisionObject boxObj = m_isSwapped ? body0 : body1;
IndexedVector3 pOnBox = new IndexedVector3(); ;
IndexedVector3 normalOnSurfaceB = new IndexedVector3();
float penetrationDepth = 0f;
IndexedVector3 sphereCenter = sphereObj.GetWorldTransform()._origin;
SphereShape sphere0 = sphereObj.GetCollisionShape() as SphereShape;
float radius = sphere0.GetRadius();
float maxContactDistance = m_manifoldPtr.GetContactBreakingThreshold();
resultOut.SetPersistentManifold(m_manifoldPtr);
if (GetSphereDistance(boxObj, ref pOnBox, ref normalOnSurfaceB, ref penetrationDepth, sphereCenter, radius, maxContactDistance))
{
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.AddContactPoint(normalOnSurfaceB, pOnBox, penetrationDepth);
}
if (m_ownManifold)
{
if (m_manifoldPtr.GetNumContacts() > 0)
{
resultOut.RefreshContactPoints();
}
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
CollisionObject col0 = body0;
CollisionObject col1 = body1;
//resultOut = new ManifoldResult(body0, body1);
BoxShape box0 = col0.GetCollisionShape() as BoxShape;
BoxShape box1 = col1.GetCollisionShape() as BoxShape;
//if (((String)col0.getUserPointer()).Contains("Box") &&
// ((String)col1.getUserPointer()).Contains("Box") )
//{
// int ibreak = 0;
//}
/// 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
ClosestPointInput input = ClosestPointInput.Default();
input.m_maximumDistanceSquared = float.MaxValue;
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
BoxBoxDetector.GetClosestPoints(box0,box1,ref input, resultOut, dispatchInfo.getDebugDraw(), false);
#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
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//resultOut = null;
CollisionObject colObj = m_isSwapped ? body1 : body0;
CollisionObject otherObj = m_isSwapped ? body0 : body1;
Debug.Assert(colObj.GetCollisionShape().IsCompound());
CompoundShape compoundShape = (CompoundShape)(colObj.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(body0, body1);
}
Dbvt tree = compoundShape.GetDynamicAabbTree();
//use a dynamic aabb tree to cull potential child-overlaps
using (CompoundLeafCallback callback = BulletGlobals.CompoundLeafCallbackPool.Get())
{
callback.Initialize(colObj, otherObj, m_dispatcher, dispatchInfo, resultOut, this, m_childCollisionAlgorithms, 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
{
m_manifoldArray.Clear();
for (int i = 0; i < m_childCollisionAlgorithms.Count; i++)
{
if (m_childCollisionAlgorithms[i] != null)
{
m_childCollisionAlgorithms[i].GetAllContactManifolds(m_manifoldArray);
for (int m = 0; m < m_manifoldArray.Count; m++)
{
if (m_manifoldArray[m].GetNumContacts() > 0)
{
resultOut.SetPersistentManifold(m_manifoldArray[m]);
resultOut.RefreshContactPoints();
resultOut.SetPersistentManifold(null);//??necessary?
}
}
m_manifoldArray.Clear();
}
}
}
if (tree != null)
{
IndexedVector3 localAabbMin;
IndexedVector3 localAabbMax;
IndexedMatrix otherInCompoundSpace;
//otherInCompoundSpace = MathUtil.BulletMatrixMultiply(colObj.GetWorldTransform(),otherObj.GetWorldTransform());
otherInCompoundSpace = colObj.GetWorldTransform().Inverse() * otherObj.GetWorldTransform();
otherObj.GetCollisionShape().GetAabb(ref otherInCompoundSpace, out localAabbMin, out localAabbMax);
DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
//process all children, that overlap with the given AABB bounds
Dbvt.CollideTV(tree.m_root, ref bounds, callback, tree.CollideTVStack, ref tree.CollideTVCount);
}
else
{
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.Count;
for (int 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;
m_manifoldArray.Clear();
CollisionShape childShape = null;
IndexedMatrix orgTrans;
IndexedMatrix orgInterpolationTrans;
IndexedMatrix newChildWorldTrans;
for (int i = 0; i < numChildren; i++)
{
if (m_childCollisionAlgorithms[i] != null)
{
childShape = compoundShape.GetChildShape(i);
//if not longer overlapping, remove the algorithm
orgTrans = colObj.GetWorldTransform();
orgInterpolationTrans = colObj.GetInterpolationWorldTransform();
IndexedMatrix childTrans = compoundShape.GetChildTransform(i);
newChildWorldTrans = orgTrans * childTrans;
//perform an AABB check first
IndexedVector3 aabbMin0;
IndexedVector3 aabbMax0;
IndexedVector3 aabbMin1;
IndexedVector3 aabbMax1;
childShape.GetAabb(ref newChildWorldTrans, out aabbMin0, out aabbMax0);
otherObj.GetCollisionShape().GetAabb(otherObj.GetWorldTransform(), out aabbMin1, out aabbMax1);
if (!AabbUtil2.TestAabbAgainstAabb2(ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1))
{
m_dispatcher.FreeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
m_childCollisionAlgorithms[i] = null;
}
}
}
}
}
}
public void Initialize(DispatcherInfo dispatchInfo, CollisionDispatcher dispatcher)
{
m_dispatchInfo = dispatchInfo;
m_dispatcher = dispatcher;
}
public abstract float CalculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut);
public virtual void DispatchAllCollisionPairs(IOverlappingPairCache pairCache, DispatcherInfo dispatchInfo, IDispatcher dispatcher)
{
m_collisionCallback.Initialize(dispatchInfo, this);
pairCache.ProcessAllOverlappingPairs(m_collisionCallback, dispatcher);
m_collisionCallback.cleanup();
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//(void)dispatchInfo;
//(void)resultOut;
if (m_manifoldPtr == null)
{
return;
}
CollisionObject sphereObj = m_isSwapped ? body1 : body0;
CollisionObject boxObj = m_isSwapped ? body0 : body1;
IndexedVector3 pOnBox = new IndexedVector3();;
IndexedVector3 normalOnSurfaceB = new IndexedVector3();
float penetrationDepth = 0f;
IndexedVector3 sphereCenter = sphereObj.GetWorldTransform()._origin;
SphereShape sphere0 = sphereObj.GetCollisionShape() as SphereShape;
float radius = sphere0.GetRadius();
float maxContactDistance = m_manifoldPtr.GetContactBreakingThreshold();
resultOut.SetPersistentManifold(m_manifoldPtr);
if (GetSphereDistance(boxObj, ref pOnBox, ref normalOnSurfaceB, ref penetrationDepth, sphereCenter, radius, maxContactDistance))
{
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.AddContactPoint(normalOnSurfaceB, pOnBox, penetrationDepth);
}
if (m_ownManifold)
{
if (m_manifoldPtr.GetNumContacts() > 0)
{
resultOut.RefreshContactPoints();
}
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
CollisionObject col0 = body0;
CollisionObject col1 = body1;
//resultOut = new ManifoldResult(body0, body1);
BoxShape box0 = col0.GetCollisionShape() as BoxShape;
BoxShape box1 = col1.GetCollisionShape() as BoxShape;
//if (((String)col0.getUserPointer()).Contains("Box") &&
// ((String)col1.getUserPointer()).Contains("Box") )
//{
// int ibreak = 0;
//}
/// 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
ClosestPointInput input = ClosestPointInput.Default();
input.m_maximumDistanceSquared = float.MaxValue;
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
BoxBoxDetector.GetClosestPoints(box0, box1, ref input, resultOut, dispatchInfo.getDebugDraw(), false);
#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
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
CollisionObject convexObj = m_isSwapped ? body1 : body0;
CollisionObject planeObj = m_isSwapped ? body0 : body1;
ConvexShape convexShape = convexObj.GetCollisionShape() as ConvexShape;
StaticPlaneShape planeShape = planeObj.GetCollisionShape() as StaticPlaneShape;
bool hasCollision = false;
IndexedVector3 planeNormal = planeShape.GetPlaneNormal();
float planeConstant = planeShape.GetPlaneConstant();
IndexedMatrix planeInConvex;
planeInConvex= convexObj.GetWorldTransform().Inverse() * planeObj.GetWorldTransform();
IndexedMatrix convexInPlaneTrans;
convexInPlaneTrans= planeObj.GetWorldTransform().Inverse() * convexObj.GetWorldTransform();
IndexedVector3 vtx = convexShape.LocalGetSupportingVertex(planeInConvex._basis*-planeNormal);
IndexedVector3 vtxInPlane = convexInPlaneTrans * vtx;
float distance = (planeNormal.Dot(vtxInPlane) - planeConstant);
IndexedVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
IndexedVector3 vtxInPlaneWorld = planeObj.GetWorldTransform() * vtxInPlaneProjected;
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
IndexedVector3 normalOnSurfaceB = planeObj.GetWorldTransform()._basis * planeNormal;
IndexedVector3 pOnB = vtxInPlaneWorld;
resultOut.AddContactPoint(normalOnSurfaceB,pOnB,distance);
}
////first perform a collision query with the non-perturbated collision objects
//{
// IndexedQuaternion rotq = IndexedQuaternion.Identity;
// CollideSingleContact(ref rotq, body0, body1, dispatchInfo, resultOut);
//}
if (convexShape.IsPolyhedral() && resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold)
{
IndexedVector3 v0;
IndexedVector3 v1;
TransformUtil.PlaneSpace1(ref planeNormal, out v0, out v1);
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
float angleLimit = 0.125f * MathUtil.SIMD_PI;
float perturbeAngle;
float radius = convexShape.GetAngularMotionDisc();
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radius;
if (perturbeAngle > angleLimit)
{
perturbeAngle = angleLimit;
}
IndexedQuaternion perturbeRot = new IndexedQuaternion(v0, perturbeAngle);
for (int i = 0; i < m_numPerturbationIterations; i++)
{
float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
IndexedQuaternion rotq = new IndexedQuaternion(planeNormal, iterationAngle);
rotq = IndexedQuaternion.Inverse(rotq) * perturbeRot * rotq;
CollideSingleContact(ref rotq, body0, body1, dispatchInfo, resultOut);
}
}
if (m_ownManifold)
{
if (m_manifoldPtr.GetNumContacts() > 0)
{
resultOut.RefreshContactPoints();
}
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
//swapped?
m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
m_ownManifold = true;
}
//resultOut = new ManifoldResult();
resultOut.SetPersistentManifold(m_manifoldPtr);
//comment-out next line to test multi-contact generation
//resultOut.GetPersistentManifold().ClearManifold();
ConvexShape min0 = body0.GetCollisionShape() as ConvexShape;
ConvexShape min1 = body1.GetCollisionShape() as ConvexShape;
IndexedVector3 normalOnB;
IndexedVector3 pointOnBWorld;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
if ((min0.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
{
CapsuleShape capsuleA = min0 as CapsuleShape;
CapsuleShape capsuleB = min1 as CapsuleShape;
//IndexedVector3 localScalingA = capsuleA.GetLocalScaling();
//IndexedVector3 localScalingB = capsuleB.GetLocalScaling();
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float dist = CapsuleCapsuleDistance(out normalOnB, out pointOnBWorld, capsuleA.GetHalfHeight(), capsuleA.GetRadius(),
capsuleB.GetHalfHeight(), capsuleB.GetRadius(), capsuleA.GetUpAxis(), capsuleB.GetUpAxis(),
body0.GetWorldTransform(), body1.GetWorldTransform(), threshold);
if (dist < threshold)
{
Debug.Assert(normalOnB.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.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.f)
#endif //USE_SEPDISTANCE_UTIL2
{
ClosestPointInput input = ClosestPointInput.Default();
using (GjkPairDetector 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 = float.MaxValue;
}
else
#endif //USE_SEPDISTANCE_UTIL2
{
input.m_maximumDistanceSquared = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold();
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
//input.m_stackAlloc = dispatchInfo.m_stackAllocator;
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
if (min0.IsPolyhedral() && min1.IsPolyhedral())
{
DummyResult dummy = new DummyResult();
PolyhedralConvexShape polyhedronA = min0 as PolyhedralConvexShape;
PolyhedralConvexShape polyhedronB = min1 as PolyhedralConvexShape;
if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetConvexPolyhedron() != null)
{
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float minDist = float.MinValue;
IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);
bool foundSepAxis = true;
if (dispatchInfo.m_enableSatConvex)
{
foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(),
out sepNormalWorldSpace);
}
else
{
#if ZERO_MARGIN
gjkPairDetector.SetIgnoreMargin(true);
gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw);
#else
gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif
float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
if (l2 > MathUtil.SIMD_EPSILON)
{
sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
#if ZERO_MARGIN
foundSepAxis = true; //gjkPairDetector.getCachedSeparatingDistance()<0.f;
#else
foundSepAxis = gjkPairDetector.GetCachedSeparatingDistance() < (min0.GetMargin() + min1.GetMargin());
#endif
}
}
if (foundSepAxis)
{
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
PolyhedralContactClipping.ClipHullAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(), minDist - threshold, threshold, resultOut);
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
return;
}
else
{
//we can also deal with convex versus triangle (without connectivity data)
if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE)
{
m_vertices.Clear();
TriangleShape tri = polyhedronB as TriangleShape;
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[0]);
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[1]);
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[2]);
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);;
float minDist = float.MinValue;
float maxDist = threshold;
bool foundSepAxis = false;
if (false)
{
polyhedronB.InitializePolyhedralFeatures();
foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(),
out sepNormalWorldSpace);
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
}
else
{
#if ZERO_MARGIN
gjkPairDetector.SetIgnoreMargin(true);
gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.m_debugDraw);
#else
gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif//ZERO_MARGIN
float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
if (l2 > MathUtil.SIMD_EPSILON)
{
sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
//minDist = gjkPairDetector.getCachedSeparatingDistance();
//maxDist = threshold;
minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
foundSepAxis = true;
}
}
if (foundSepAxis)
{
PolyhedralContactClipping.ClipFaceAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(),
body0.GetWorldTransform(), m_vertices, minDist - threshold, maxDist, resultOut);
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
return;
}
}
}
gjkPairDetector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), false);
#if USE_SEPDISTANCE_UTIL2
float sepDist = 0.f;
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
sepDist = gjkPairDetector.getCachedSeparatingDistance();
if (sepDist > MathUtil.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.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold)
{
IndexedVector3 v0, v1;
IndexedVector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis();
sepNormalWorldSpace.Normalize();
TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, out v0, out v1);
bool perturbeA = true;
const float angleLimit = 0.125f * MathUtil.SIMD_PI;
float perturbeAngle;
float radiusA = min0.GetAngularMotionDisc();
float radiusB = min1.GetAngularMotionDisc();
if (radiusA < radiusB)
{
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusA;
perturbeA = true;
}
else
{
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusB;
perturbeA = false;
}
if (perturbeAngle > angleLimit)
{
perturbeAngle = angleLimit;
}
IndexedMatrix unPerturbedTransform;
if (perturbeA)
{
unPerturbedTransform = input.m_transformA;
}
else
{
unPerturbedTransform = input.m_transformB;
}
for (int i = 0; i < m_numPerturbationIterations; i++)
{
if (v0.LengthSquared() > MathUtil.SIMD_EPSILON)
{
IndexedQuaternion perturbeRot = new IndexedQuaternion(v0, perturbeAngle);
float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
IndexedQuaternion rotq = new IndexedQuaternion(sepNormalWorldSpace, iterationAngle);
if (perturbeA)
{
input.m_transformA._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body0.GetWorldTransform()._basis);
input.m_transformB = body1.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformA, 10.0f);
#endif //DEBUG_CONTACTS
}
else
{
input.m_transformA = body0.GetWorldTransform();
input.m_transformB._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body1.GetWorldTransform()._basis);
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformB, 10.0f);
#endif
}
PerturbedContactResult perturbedResultOut = new PerturbedContactResult(resultOut, ref input.m_transformA, ref input.m_transformB, ref unPerturbedTransform, perturbeA, dispatchInfo.getDebugDraw());
gjkPairDetector.GetClosestPoints(ref input, perturbedResultOut, dispatchInfo.getDebugDraw(), false);
}
}
}
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist > MathUtil.SIMD_EPSILON))
{
m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(), sepDist, body0.getWorldTransform(), body1.getWorldTransform());
}
#endif //USE_SEPDISTANCE_UTIL2
}
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
public virtual void CollideSingleContact(ref IndexedQuaternion perturbeRot, CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
CollisionObject convexObj = m_isSwapped ? body1 : body0;
CollisionObject planeObj = m_isSwapped ? body0 : body1;
ConvexShape convexShape = convexObj.GetCollisionShape() as ConvexShape;
StaticPlaneShape planeShape = planeObj.GetCollisionShape() as StaticPlaneShape;
bool hasCollision = false;
IndexedVector3 planeNormal = planeShape.GetPlaneNormal();
float planeConstant = planeShape.GetPlaneConstant();
IndexedMatrix convexWorldTransform = convexObj.GetWorldTransform();
IndexedMatrix convexInPlaneTrans = planeObj.GetWorldTransform().Inverse() * convexWorldTransform;
//now perturbe the convex-world transform
convexWorldTransform._basis *= new IndexedBasisMatrix(ref perturbeRot);
IndexedMatrix planeInConvex = convexWorldTransform.Inverse() * planeObj.GetWorldTransform(); ;
IndexedVector3 vtx = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal);
IndexedVector3 vtxInPlane = vtxInPlane = convexInPlaneTrans * vtx;
float distance = (IndexedVector3.Dot(planeNormal, vtxInPlane) - planeConstant);
IndexedVector3 vtxInPlaneProjected = vtxInPlane - (distance * planeNormal);
IndexedVector3 vtxInPlaneWorld = planeObj.GetWorldTransform() * vtxInPlaneProjected;
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
IndexedVector3 normalOnSurfaceB = planeObj.GetWorldTransform()._basis * planeNormal;
IndexedVector3 pOnB = vtxInPlaneWorld;
resultOut.AddContactPoint(ref normalOnSurfaceB, ref pOnB, distance);
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
CollisionObject col0 = body0;
CollisionObject col1 = body1;
Box2dShape box0 = (Box2dShape)col0.GetCollisionShape();
Box2dShape box1 = (Box2dShape)col1.GetCollisionShape();
resultOut.SetPersistentManifold(m_manifoldPtr);
B2CollidePolygons(ref resultOut, box0, col0.GetWorldTransform(), box1, col1.GetWorldTransform());
// refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
public override float CalculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
resultOut = null;
CollisionObject colObj = m_isSwapped ? body1 : body0;
CollisionObject otherObj = m_isSwapped ? body0 : body1;
Debug.Assert(colObj.GetCollisionShape().IsCompound());
CompoundShape compoundShape = (CompoundShape)(colObj.GetCollisionShape());
//We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
//If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
//given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
//determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
//then use each overlapping node AABB against Tree0
//and vise versa.
float hitFraction = 1f;
int numChildren = m_childCollisionAlgorithms.Count;
IndexedMatrix orgTrans;
float frac;
for (int i = 0; i < numChildren; i++)
{
//temporarily exchange parent btCollisionShape with childShape, and recurse
CollisionShape childShape = compoundShape.GetChildShape(i);
//backup
orgTrans = colObj.GetWorldTransform();
IndexedMatrix childTrans = compoundShape.GetChildTransform(i);
IndexedMatrix newChildWorldTrans = orgTrans * childTrans;
colObj.SetWorldTransform(ref newChildWorldTrans);
CollisionShape tmpShape = colObj.GetCollisionShape();
colObj.InternalSetTemporaryCollisionShape(childShape);
frac = m_childCollisionAlgorithms[i].CalculateTimeOfImpact(colObj, otherObj, dispatchInfo, resultOut);
if (frac < hitFraction)
{
hitFraction = frac;
}
//revert back
colObj.InternalSetTemporaryCollisionShape(tmpShape);
colObj.SetWorldTransform(ref orgTrans);
}
return(hitFraction);
}
public override float CalculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
return 1.0f;
}
public abstract float CalculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut);
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
//swapped?
m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
m_ownManifold = true;
}
//resultOut = new ManifoldResult();
resultOut.SetPersistentManifold(m_manifoldPtr);
//comment-out next line to test multi-contact generation
//resultOut.GetPersistentManifold().ClearManifold();
ConvexShape min0 = body0.GetCollisionShape() as ConvexShape;
ConvexShape min1 = body1.GetCollisionShape() as ConvexShape;
IndexedVector3 normalOnB;
IndexedVector3 pointOnBWorld;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
if ((min0.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.GetShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE))
{
CapsuleShape capsuleA = min0 as CapsuleShape;
CapsuleShape capsuleB = min1 as CapsuleShape;
//IndexedVector3 localScalingA = capsuleA.GetLocalScaling();
//IndexedVector3 localScalingB = capsuleB.GetLocalScaling();
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float dist = CapsuleCapsuleDistance(out normalOnB, out pointOnBWorld, capsuleA.GetHalfHeight(), capsuleA.GetRadius(),
capsuleB.GetHalfHeight(), capsuleB.GetRadius(), capsuleA.GetUpAxis(), capsuleB.GetUpAxis(),
body0.GetWorldTransform(), body1.GetWorldTransform(), threshold);
if (dist < threshold)
{
Debug.Assert(normalOnB.LengthSquared() >= (MathUtil.SIMD_EPSILON * MathUtil.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.f)
#endif //USE_SEPDISTANCE_UTIL2
{
ClosestPointInput input = ClosestPointInput.Default();
using (GjkPairDetector 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 = float.MaxValue;
}
else
#endif //USE_SEPDISTANCE_UTIL2
{
input.m_maximumDistanceSquared = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold();
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
//input.m_stackAlloc = dispatchInfo.m_stackAllocator;
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
if (min0.IsPolyhedral() && min1.IsPolyhedral())
{
DummyResult dummy = new DummyResult();
PolyhedralConvexShape polyhedronA = min0 as PolyhedralConvexShape;
PolyhedralConvexShape polyhedronB = min1 as PolyhedralConvexShape;
if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetConvexPolyhedron() != null)
{
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
float minDist = float.MinValue;
IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0);
bool foundSepAxis = true;
if (dispatchInfo.m_enableSatConvex)
{
foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(),
out sepNormalWorldSpace);
}
else
{
#if ZERO_MARGIN
gjkPairDetector.SetIgnoreMargin(true);
gjkPairDetector.GetClosestPoints(input,resultOut,dispatchInfo.m_debugDraw);
#else
gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif
float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
if (l2 > MathUtil.SIMD_EPSILON)
{
sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
#if ZERO_MARGIN
foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f;
#else
foundSepAxis = gjkPairDetector.GetCachedSeparatingDistance() < (min0.GetMargin() + min1.GetMargin());
#endif
}
}
if (foundSepAxis)
{
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
PolyhedralContactClipping.ClipHullAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(), minDist - threshold, threshold, resultOut);
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
return;
}
else
{
//we can also deal with convex versus triangle (without connectivity data)
if (polyhedronA.GetConvexPolyhedron() != null && polyhedronB.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE)
{
m_vertices.Clear();
TriangleShape tri = polyhedronB as TriangleShape;
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[0]);
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[1]);
m_vertices.Add(body1.GetWorldTransform() * tri.m_vertices1[2]);
float threshold = m_manifoldPtr.GetContactBreakingThreshold();
IndexedVector3 sepNormalWorldSpace = new IndexedVector3(0, 1, 0); ;
float minDist = float.MinValue;
float maxDist = threshold;
bool foundSepAxis = false;
if (false)
{
polyhedronB.InitializePolyhedralFeatures();
foundSepAxis = PolyhedralContactClipping.FindSeparatingAxis(
polyhedronA.GetConvexPolyhedron(), polyhedronB.GetConvexPolyhedron(),
body0.GetWorldTransform(),
body1.GetWorldTransform(),
out sepNormalWorldSpace);
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
}
else
{
#if ZERO_MARGIN
gjkPairDetector.SetIgnoreMargin(true);
gjkPairDetector.GetClosestPoints(input,resultOut,dispatchInfo.m_debugDraw);
#else
gjkPairDetector.GetClosestPoints(ref input, dummy, dispatchInfo.m_debugDraw);
#endif//ZERO_MARGIN
float l2 = gjkPairDetector.GetCachedSeparatingAxis().LengthSquared();
if (l2 > MathUtil.SIMD_EPSILON)
{
sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis() * (1.0f / l2);
//minDist = gjkPairDetector.getCachedSeparatingDistance();
//maxDist = threshold;
minDist = gjkPairDetector.GetCachedSeparatingDistance() - min0.GetMargin() - min1.GetMargin();
foundSepAxis = true;
}
}
if (foundSepAxis)
{
PolyhedralContactClipping.ClipFaceAgainstHull(sepNormalWorldSpace, polyhedronA.GetConvexPolyhedron(),
body0.GetWorldTransform(), m_vertices, minDist - threshold, maxDist, resultOut);
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
return;
}
}
}
gjkPairDetector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), false);
#if USE_SEPDISTANCE_UTIL2
float sepDist = 0.f;
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
sepDist = gjkPairDetector.getCachedSeparatingDistance();
if (sepDist>MathUtil.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.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold)
{
IndexedVector3 v0, v1;
IndexedVector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis();
sepNormalWorldSpace.Normalize();
TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, out v0, out v1);
bool perturbeA = true;
const float angleLimit = 0.125f * MathUtil.SIMD_PI;
float perturbeAngle;
float radiusA = min0.GetAngularMotionDisc();
float radiusB = min1.GetAngularMotionDisc();
if (radiusA < radiusB)
{
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusA;
perturbeA = true;
}
else
{
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radiusB;
perturbeA = false;
}
if (perturbeAngle > angleLimit)
{
perturbeAngle = angleLimit;
}
IndexedMatrix unPerturbedTransform;
if (perturbeA)
{
unPerturbedTransform = input.m_transformA;
}
else
{
unPerturbedTransform = input.m_transformB;
}
for (int i = 0; i < m_numPerturbationIterations; i++)
{
if (v0.LengthSquared() > MathUtil.SIMD_EPSILON)
{
IndexedQuaternion perturbeRot = new IndexedQuaternion(v0, perturbeAngle);
float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
IndexedQuaternion rotq = new IndexedQuaternion(sepNormalWorldSpace, iterationAngle);
if (perturbeA)
{
input.m_transformA._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body0.GetWorldTransform()._basis);
input.m_transformB = body1.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformA, 10.0f);
#endif //DEBUG_CONTACTS
}
else
{
input.m_transformA = body0.GetWorldTransform();
input.m_transformB._basis = (new IndexedBasisMatrix(MathUtil.QuaternionInverse(rotq) * perturbeRot * rotq) * body1.GetWorldTransform()._basis);
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.DrawTransform(ref input.m_transformB, 10.0f);
#endif
}
PerturbedContactResult perturbedResultOut = new PerturbedContactResult(resultOut, ref input.m_transformA, ref input.m_transformB, ref unPerturbedTransform, perturbeA, dispatchInfo.getDebugDraw());
gjkPairDetector.GetClosestPoints(ref input, perturbedResultOut, dispatchInfo.getDebugDraw(), false);
}
}
}
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist > MathUtil.SIMD_EPSILON))
{
m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0.getWorldTransform(),body1.getWorldTransform());
}
#endif //USE_SEPDISTANCE_UTIL2
}
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
public abstract void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut);
public void SetTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
m_dispatchInfoPtr = dispatchInfo;
m_collisionMarginTriangle = collisionMarginTriangle;
m_resultOut = resultOut;
//recalc aabbs
//IndexedMatrix convexInTriangleSpace = MathUtil.bulletMatrixMultiply(IndexedMatrix.Invert(m_triBody.getWorldTransform()) , m_convexBody.getWorldTransform());
IndexedMatrix convexInTriangleSpace = m_triBody.GetWorldTransform().Inverse() * m_convexBody.GetWorldTransform();
CollisionShape convexShape = m_convexBody.GetCollisionShape();
convexShape.GetAabb(ref convexInTriangleSpace, out m_aabbMin, out m_aabbMax);
float extraMargin = collisionMarginTriangle;
IndexedVector3 extra = new IndexedVector3(extraMargin);
m_aabbMax += extra;
m_aabbMin -= extra;
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//resultOut = null;
CollisionObject colObj = m_isSwapped ? body1 : body0;
CollisionObject otherObj = m_isSwapped ? body0 : body1;
Debug.Assert(colObj.GetCollisionShape().IsCompound());
CompoundShape compoundShape = (CompoundShape)(colObj.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(body0, body1);
}
Dbvt tree = compoundShape.GetDynamicAabbTree();
//use a dynamic aabb tree to cull potential child-overlaps
using (CompoundLeafCallback callback = BulletGlobals.CompoundLeafCallbackPool.Get())
{
callback.Initialize(colObj, otherObj, m_dispatcher, dispatchInfo, resultOut, this, m_childCollisionAlgorithms, 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
{
m_manifoldArray.Clear();
for (int i = 0; i < m_childCollisionAlgorithms.Count; i++)
{
if (m_childCollisionAlgorithms[i] != null)
{
m_childCollisionAlgorithms[i].GetAllContactManifolds(m_manifoldArray);
for (int m = 0; m < m_manifoldArray.Count; m++)
{
if (m_manifoldArray[m].GetNumContacts() > 0)
{
resultOut.SetPersistentManifold(m_manifoldArray[m]);
resultOut.RefreshContactPoints();
resultOut.SetPersistentManifold(null);//??necessary?
}
}
m_manifoldArray.Clear();
}
}
}
if (tree != null)
{
IndexedVector3 localAabbMin;
IndexedVector3 localAabbMax;
IndexedMatrix otherInCompoundSpace;
//otherInCompoundSpace = MathUtil.BulletMatrixMultiply(colObj.GetWorldTransform(),otherObj.GetWorldTransform());
otherInCompoundSpace = colObj.GetWorldTransform().Inverse() * otherObj.GetWorldTransform();
otherObj.GetCollisionShape().GetAabb(ref otherInCompoundSpace, out localAabbMin, out localAabbMax);
DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
//process all children, that overlap with the given AABB bounds
Dbvt.CollideTV(tree.m_root, ref bounds, callback);
}
else
{
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.Count;
for (int 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;
m_manifoldArray.Clear();
CollisionShape childShape = null;
IndexedMatrix orgTrans;
IndexedMatrix orgInterpolationTrans;
IndexedMatrix newChildWorldTrans;
for (int i = 0; i < numChildren; i++)
{
if (m_childCollisionAlgorithms[i] != null)
{
childShape = compoundShape.GetChildShape(i);
//if not longer overlapping, remove the algorithm
orgTrans = colObj.GetWorldTransform();
orgInterpolationTrans = colObj.GetInterpolationWorldTransform();
IndexedMatrix childTrans = compoundShape.GetChildTransform(i);
newChildWorldTrans = orgTrans * childTrans;
//perform an AABB check first
IndexedVector3 aabbMin0;
IndexedVector3 aabbMax0;
IndexedVector3 aabbMin1;
IndexedVector3 aabbMax1;
childShape.GetAabb(ref newChildWorldTrans, out aabbMin0, out aabbMax0);
otherObj.GetCollisionShape().GetAabb(otherObj.GetWorldTransform(), out aabbMin1, out aabbMax1);
if (!AabbUtil2.TestAabbAgainstAabb2(ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1))
{
m_dispatcher.FreeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
m_childCollisionAlgorithms[i] = null;
}
}
}
}
}
}
public CollisionPairCallback(DispatcherInfo dispatchInfo, CollisionDispatcher dispatcher)
{
m_dispatchInfo = dispatchInfo;
m_dispatcher = dispatcher;
}
public virtual void CollideSingleContact(ref IndexedQuaternion perturbeRot, CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
CollisionObject convexObj = m_isSwapped ? body1 : body0;
CollisionObject planeObj = m_isSwapped ? body0 : body1;
ConvexShape convexShape = convexObj.GetCollisionShape() as ConvexShape;
StaticPlaneShape planeShape = planeObj.GetCollisionShape() as StaticPlaneShape;
bool hasCollision = false;
IndexedVector3 planeNormal = planeShape.GetPlaneNormal();
float planeConstant = planeShape.GetPlaneConstant();
IndexedMatrix convexWorldTransform = convexObj.GetWorldTransform();
IndexedMatrix convexInPlaneTrans = planeObj.GetWorldTransform().Inverse() * convexWorldTransform;
//now perturbe the convex-world transform
convexWorldTransform._basis *= new IndexedBasisMatrix(ref perturbeRot);
IndexedMatrix planeInConvex = convexWorldTransform.Inverse() * planeObj.GetWorldTransform();;
IndexedVector3 vtx = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal);
IndexedVector3 vtxInPlane = vtxInPlane = convexInPlaneTrans * vtx;
float distance = (IndexedVector3.Dot(planeNormal, vtxInPlane) - planeConstant);
IndexedVector3 vtxInPlaneProjected = vtxInPlane - (distance * planeNormal);
IndexedVector3 vtxInPlaneWorld = planeObj.GetWorldTransform() * vtxInPlaneProjected;
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
IndexedVector3 normalOnSurfaceB = planeObj.GetWorldTransform()._basis *planeNormal;
IndexedVector3 pOnB = vtxInPlaneWorld;
resultOut.AddContactPoint(ref normalOnSurfaceB, ref pOnB, distance);
}
}
public void NearCallback(BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo)
{
CollisionObject colObj0 = collisionPair.m_pProxy0.GetClientObject() as CollisionObject;
CollisionObject colObj1 = collisionPair.m_pProxy1.GetClientObject() as CollisionObject;
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, null);
}
if (collisionPair.m_algorithm != null)
{
ManifoldResult contactPointResult = dispatcher.GetNewManifoldResult(colObj0, colObj1);
if (dispatchInfo.GetDispatchFunc() == DispatchFunc.DISPATCH_DISCRETE)
{
//discrete collision detection query
collisionPair.m_algorithm.ProcessCollision(colObj0, colObj1, dispatchInfo, contactPointResult);
}
else
{
//continuous collision detection query, time of impact (toi)
float toi = collisionPair.m_algorithm.CalculateTimeOfImpact(colObj0, colObj1, dispatchInfo, contactPointResult);
if (dispatchInfo.GetTimeOfImpact() > toi)
{
dispatchInfo.SetTimeOfImpact(toi);
}
}
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDispatcher)
{
BulletGlobals.g_streamWriter.WriteLine("NearCallback[{0}][{1}][{2}]", contactPointResult.GetBody0Internal().GetUserPointer(), contactPointResult.GetBody1Internal().GetUserPointer(), contactPointResult.GetPersistentManifold().GetNumContacts());
}
#endif
dispatcher.FreeManifoldResult(contactPointResult);
}
}
}
public override float CalculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
///body0.m_worldTransform,
float resultFraction = 1.0f;
float squareMot0 = (body0.GetInterpolationWorldTransform()._origin - body0.GetWorldTransform()._origin).LengthSquared();
float squareMot1 = (body1.GetInterpolationWorldTransform()._origin - body1.GetWorldTransform()._origin).LengthSquared();
if (squareMot0 < body0.GetCcdSquareMotionThreshold() &&
squareMot1 < body1.GetCcdSquareMotionThreshold())
{
return(resultFraction);
}
//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 = body0.GetCollisionShape() as ConvexShape;
SphereShape sphere1 = BulletGlobals.SphereShapePool.Get();
sphere1.Initialize(body1.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = BulletGlobals.CastResultPool.Get();
VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
{
ccd1.Initialize(convex0, sphere1, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction() > result.m_fraction)
{
body0.SetHitFraction(result.m_fraction);
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction(result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
BulletGlobals.SphereShapePool.Free(sphere1);
result.Cleanup();
}
}
/// Sphere (for convex0) against Convex1
{
ConvexShape convex1 = body1.GetCollisionShape() as ConvexShape;
SphereShape sphere0 = BulletGlobals.SphereShapePool.Get();
sphere0.Initialize(body0.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = BulletGlobals.CastResultPool.Get();
VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
{
ccd1.Initialize(sphere0, convex1, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction() > result.m_fraction)
{
body0.SetHitFraction(result.m_fraction);
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction(result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
BulletGlobals.SphereShapePool.Free(sphere0);
result.Cleanup();
}
}
return(resultFraction);
}
public override float CalculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//not yet
return(1f);
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//resultOut = new ManifoldResult();
if (m_manifoldPtr == null)
{
return;
}
CollisionObject sphereObj = m_swapped ? body1 : body0;
CollisionObject triObj = m_swapped ? body0 : body1;
SphereShape sphere = sphereObj.GetCollisionShape() as SphereShape;
TriangleShape triangle = triObj.GetCollisionShape() as TriangleShape;
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut.SetPersistentManifold(m_manifoldPtr);
using (SphereTriangleDetector detector = BulletGlobals.SphereTriangleDetectorPool.Get())
{
detector.Initialize(sphere, triangle, m_manifoldPtr.GetContactBreakingThreshold());
ClosestPointInput input = ClosestPointInput.Default();
input.m_maximumDistanceSquared = float.MaxValue;
sphereObj.GetWorldTransform(out input.m_transformA);
triObj.GetWorldTransform(out input.m_transformB);
bool swapResults = m_swapped;
detector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), swapResults);
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
CollisionObject convexObj = m_isSwapped ? body1 : body0;
CollisionObject planeObj = m_isSwapped ? body0 : body1;
ConvexShape convexShape = convexObj.GetCollisionShape() as ConvexShape;
StaticPlaneShape planeShape = planeObj.GetCollisionShape() as StaticPlaneShape;
bool hasCollision = false;
IndexedVector3 planeNormal = planeShape.GetPlaneNormal();
float planeConstant = planeShape.GetPlaneConstant();
IndexedMatrix planeInConvex;
planeInConvex = convexObj.GetWorldTransform().Inverse() * planeObj.GetWorldTransform();
IndexedMatrix convexInPlaneTrans;
convexInPlaneTrans = planeObj.GetWorldTransform().Inverse() * convexObj.GetWorldTransform();
IndexedVector3 vtx = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal);
IndexedVector3 vtxInPlane = convexInPlaneTrans * vtx;
float distance = (planeNormal.Dot(vtxInPlane) - planeConstant);
IndexedVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
IndexedVector3 vtxInPlaneWorld = planeObj.GetWorldTransform() * vtxInPlaneProjected;
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
IndexedVector3 normalOnSurfaceB = planeObj.GetWorldTransform()._basis *planeNormal;
IndexedVector3 pOnB = vtxInPlaneWorld;
resultOut.AddContactPoint(normalOnSurfaceB, pOnB, distance);
}
////first perform a collision query with the non-perturbated collision objects
//{
// IndexedQuaternion rotq = IndexedQuaternion.Identity;
// CollideSingleContact(ref rotq, body0, body1, dispatchInfo, resultOut);
//}
if (convexShape.IsPolyhedral() && resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold)
{
IndexedVector3 v0;
IndexedVector3 v1;
TransformUtil.PlaneSpace1(ref planeNormal, out v0, out v1);
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
float angleLimit = 0.125f * MathUtil.SIMD_PI;
float perturbeAngle;
float radius = convexShape.GetAngularMotionDisc();
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radius;
if (perturbeAngle > angleLimit)
{
perturbeAngle = angleLimit;
}
IndexedQuaternion perturbeRot = new IndexedQuaternion(v0, perturbeAngle);
for (int i = 0; i < m_numPerturbationIterations; i++)
{
float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
IndexedQuaternion rotq = new IndexedQuaternion(planeNormal, iterationAngle);
rotq = IndexedQuaternion.Inverse(rotq) * perturbeRot * rotq;
CollideSingleContact(ref rotq, body0, body1, dispatchInfo, resultOut);
}
}
if (m_ownManifold)
{
if (m_manifoldPtr.GetNumContacts() > 0)
{
resultOut.RefreshContactPoints();
}
}
}
public virtual void DispatchAllCollisionPairs(IOverlappingPairCache pairCache, DispatcherInfo dispatchInfo, IDispatcher dispatcher)
{
m_collisionCallback.Initialize(dispatchInfo, this);
pairCache.ProcessAllOverlappingPairs(m_collisionCallback, dispatcher);
m_collisionCallback.cleanup();
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
//swapped?
m_manifoldPtr = m_dispatcher.GetNewManifold(body0, body1);
m_ownManifold = true;
}
resultOut.SetPersistentManifold(m_manifoldPtr);
//comment-out next line to test multi-contact generation
//resultOut.getPersistentManifold().clearManifold();
ConvexShape min0 = body0.GetCollisionShape() as ConvexShape;
ConvexShape min1 = body1.GetCollisionShape() as ConvexShape;
IndexedVector3 normalOnB = IndexedVector3.Zero;
IndexedVector3 pointOnBWorld = IndexedVector3.Zero;
{
ClosestPointInput input = ClosestPointInput.Default();
using (GjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get())
{
gjkPairDetector.Initialize(min0, min1, m_simplexSolver, m_pdSolver);
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.SetMinkowskiA(min0);
gjkPairDetector.SetMinkowskiB(min1);
{
input.m_maximumDistanceSquared = min0.GetMargin() + min1.GetMargin() + m_manifoldPtr.GetContactBreakingThreshold();
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
input.m_transformA = body0.GetWorldTransform();
input.m_transformB = body1.GetWorldTransform();
gjkPairDetector.GetClosestPoints(ref input, resultOut, dispatchInfo.getDebugDraw(), false);
if (BulletGlobals.g_streamWriter != null)
{
BulletGlobals.g_streamWriter.WriteLine("c2dc2d processCollision");
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformA", input.m_transformA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transformB", input.m_transformB);
}
}
//BulletGlobals.GjkPairDetectorPool.Free(gjkPairDetector);
//btVector3 v0,v1;
//btVector3 sepNormalWorldSpace;
}
if (m_ownManifold)
{
resultOut.RefreshContactPoints();
}
}
public void Initialize(DispatcherInfo dispatchInfo, CollisionDispatcher dispatcher)
{
m_dispatchInfo = dispatchInfo;
m_dispatcher = dispatcher;
}
public override float CalculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
resultOut = null;
CollisionObject colObj = m_isSwapped ? body1 : body0;
CollisionObject otherObj = m_isSwapped ? body0 : body1;
Debug.Assert(colObj.GetCollisionShape().IsCompound());
CompoundShape compoundShape = (CompoundShape)(colObj.GetCollisionShape());
//We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
//If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
//given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
//determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
//then use each overlapping node AABB against Tree0
//and vise versa.
float hitFraction = 1f;
int numChildren = m_childCollisionAlgorithms.Count;
IndexedMatrix orgTrans;
float frac;
for (int i = 0; i < numChildren; i++)
{
//temporarily exchange parent btCollisionShape with childShape, and recurse
CollisionShape childShape = compoundShape.GetChildShape(i);
//backup
orgTrans = colObj.GetWorldTransform();
IndexedMatrix childTrans = compoundShape.GetChildTransform(i);
IndexedMatrix newChildWorldTrans = orgTrans * childTrans;
colObj.SetWorldTransform(ref newChildWorldTrans);
CollisionShape tmpShape = colObj.GetCollisionShape();
colObj.InternalSetTemporaryCollisionShape(childShape);
frac = m_childCollisionAlgorithms[i].CalculateTimeOfImpact(colObj, otherObj, dispatchInfo, resultOut);
if (frac < hitFraction)
{
hitFraction = frac;
}
//revert back
colObj.InternalSetTemporaryCollisionShape(tmpShape);
colObj.SetWorldTransform(ref orgTrans);
}
return hitFraction;
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
resultOut.SetPersistentManifold(m_manifoldPtr);
SphereShape sphere0 = body0.GetCollisionShape() as SphereShape;
SphereShape sphere1 = body1.GetCollisionShape() as SphereShape;
IndexedVector3 diff = body0.GetWorldTransform()._origin - body1.GetWorldTransform()._origin;
float len = diff.Length();
float radius0 = sphere0.GetRadius();
float 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)
float dist = len - (radius0 + radius1);
IndexedVector3 normalOnSurfaceB = new IndexedVector3(1, 0, 0);
if (len > MathUtil.SIMD_EPSILON)
{
normalOnSurfaceB = diff / len;
}
///point on A (worldspace)
///btVector3 pos0 = col0.getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
///point on B (worldspace)
IndexedVector3 pos1 = body1.GetWorldTransform()._origin + radius1 * normalOnSurfaceB;
/// 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 void Initialize(CollisionObject compoundObj, CollisionObject otherObj, IDispatcher dispatcher, DispatcherInfo dispatchInfo, ManifoldResult resultOut, CompoundCollisionAlgorithm parent, IList<CollisionAlgorithm> childCollisionAlgorithms, PersistentManifold sharedManifold)
{
m_compoundColObj = compoundObj;
m_otherObj = otherObj;
m_dispatcher = dispatcher;
m_dispatchInfo = dispatchInfo;
m_resultOut = resultOut;
m_childCollisionAlgorithms = childCollisionAlgorithms;
m_sharedManifold = sharedManifold;
m_parent = parent;
}
public CollisionPairCallback(DispatcherInfo dispatchInfo, CollisionDispatcher dispatcher)
{
m_dispatchInfo = dispatchInfo;
m_dispatcher = dispatcher;
}
//public override void processCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
//{
// CollisionObject convexBody = m_isSwapped ? body1 : body0;
// CollisionObject triBody = m_isSwapped ? body0 : body1;
// if (triBody.getCollisionShape().isConcave())
// {
// CollisionObject triOb = triBody;
// ConcaveShape concaveShape = (ConcaveShape)(triOb.getCollisionShape());
// if (convexBody.getCollisionShape().isConvex())
// {
// float collisionMarginTriangle = concaveShape.getMargin();
// resultOut.setPersistentManifold(m_convexTriangleCallback.m_manifoldPtr);
// m_convexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut);
// //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
// //m_dispatcher.clearManifold(m_convexTriangleCallback.m_manifoldPtr);
// m_convexTriangleCallback.m_manifoldPtr.setBodies(convexBody, triBody);
// IndexedVector3 min = m_convexTriangleCallback.getAabbMin();
// IndexedVector3 max = m_convexTriangleCallback.getAabbMax();
// concaveShape.processAllTriangles(m_convexTriangleCallback, ref min,ref max );
// resultOut.refreshContactPoints();
// }
// }
//}
//public override float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
//{
// CollisionObject convexbody = m_isSwapped ? body1 : body0;
// CollisionObject triBody = m_isSwapped ? body0 : body1;
// //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
// //only perform CCD above a certain threshold, this prevents blocking on the long run
// //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
// float squareMot0 = (convexbody.getInterpolationWorldTransform()._origin - convexbody.getWorldTransform()._origin).LengthSquared();
// if (squareMot0 < convexbody.getCcdSquareMotionThreshold())
// {
// return 1f;
// }
// //const IndexedVector3& from = convexbody.m_worldTransform._origin;
// //IndexedVector3 to = convexbody.m_interpolationWorldTransform._origin;
// //todo: only do if the motion exceeds the 'radius'
// //IndexedMatrix triInv = IndexedMatrix.Invert(triBody.getWorldTransform());
// //IndexedMatrix convexFromLocal = MathUtil.bulletMatrixMultiply(triInv , convexbody.getWorldTransform());
// //IndexedMatrix convexToLocal = MathUtil.bulletMatrixMultiply(triInv , convexbody.getInterpolationWorldTransform());
// IndexedMatrix triInv = IndexedMatrix.Invert(triBody.getWorldTransform());
// IndexedMatrix convexFromLocal = MathUtil.inverseTimes(triBody.getWorldTransform(), convexbody.getWorldTransform());
// IndexedMatrix convexToLocal = MathUtil.inverseTimes(triBody.getWorldTransform(), convexbody.getInterpolationWorldTransform());
// if (triBody.getCollisionShape().isConcave())
// {
// IndexedVector3 rayAabbMin = convexFromLocal._origin;
// MathUtil.vectorMin(convexToLocal._origin, ref rayAabbMin);
// IndexedVector3 rayAabbMax = convexFromLocal._origin;
// MathUtil.vectorMax(convexToLocal._origin,ref rayAabbMax);
// float ccdRadius0 = convexbody.getCcdSweptSphereRadius();
// rayAabbMin -= new IndexedVector3(ccdRadius0,ccdRadius0,ccdRadius0);
// rayAabbMax += new IndexedVector3(ccdRadius0,ccdRadius0,ccdRadius0);
// float curHitFraction = 1.0f; //is this available?
// LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(ref convexFromLocal, ref convexToLocal,
// convexbody.getCcdSweptSphereRadius(),curHitFraction);
// raycastCallback.m_hitFraction = convexbody.getHitFraction();
// CollisionObject concavebody = triBody;
// ConcaveShape triangleMesh = (ConcaveShape) concavebody.getCollisionShape();
// if (triangleMesh != null)
// {
// triangleMesh.processAllTriangles(raycastCallback,ref rayAabbMin,ref rayAabbMax);
// }
// if (raycastCallback.m_hitFraction < convexbody.getHitFraction())
// {
// convexbody.setHitFraction( raycastCallback.m_hitFraction);
// float result = raycastCallback.m_hitFraction;
// raycastCallback.cleanup();
// return result;
// }
// raycastCallback.cleanup();
// }
// return 1f;
//}
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//fixme
CollisionObject convexBody = m_isSwapped ? bodyB : bodyA;
CollisionObject triBody = m_isSwapped ? bodyA : bodyB;
if (triBody.GetCollisionShape().IsConcave())
{
CollisionObject triOb = triBody;
ConcaveShape concaveShape = triOb.GetCollisionShape() as ConcaveShape;
if (convexBody.GetCollisionShape().IsConvex())
{
float collisionMarginTriangle = concaveShape.GetMargin();
resultOut.SetPersistentManifold(m_convexTriangleCallback.m_manifoldPtr);
m_convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut);
//Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
//m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);
m_convexTriangleCallback.m_manifoldPtr.SetBodies(convexBody, triBody);
IndexedVector3 min = m_convexTriangleCallback.GetAabbMin();
IndexedVector3 max = m_convexTriangleCallback.GetAabbMax();
concaveShape.ProcessAllTriangles(m_convexTriangleCallback, ref min, ref max);
resultOut.RefreshContactPoints();
}
}
}
public void NearCallback(BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo)
{
CollisionObject colObj0 = collisionPair.m_pProxy0.GetClientObject() as CollisionObject;
CollisionObject colObj1 = collisionPair.m_pProxy1.GetClientObject() as CollisionObject;
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, null);
}
if (collisionPair.m_algorithm != null)
{
ManifoldResult contactPointResult = dispatcher.GetNewManifoldResult(colObj0, colObj1);
if (dispatchInfo.GetDispatchFunc() == DispatchFunc.DISPATCH_DISCRETE)
{
//discrete collision detection query
collisionPair.m_algorithm.ProcessCollision(colObj0, colObj1, dispatchInfo, contactPointResult);
}
else
{
//continuous collision detection query, time of impact (toi)
float toi = collisionPair.m_algorithm.CalculateTimeOfImpact(colObj0, colObj1, dispatchInfo, contactPointResult);
if (dispatchInfo.GetTimeOfImpact() > toi)
{
dispatchInfo.SetTimeOfImpact(toi);
}
}
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDispatcher)
{
BulletGlobals.g_streamWriter.WriteLine("NearCallback[{0}][{1}][{2}]", contactPointResult.GetBody0Internal().GetUserPointer(), contactPointResult.GetBody1Internal().GetUserPointer(),contactPointResult.GetPersistentManifold().GetNumContacts());
}
#endif
dispatcher.FreeManifoldResult(contactPointResult);
}
}
}
public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
CollisionObject convexbody = m_isSwapped ? bodyB : bodyA;
CollisionObject triBody = m_isSwapped ? bodyA : bodyB;
//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
//only perform CCD above a certain threshold, this prevents blocking on the long run
//because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
float squareMot0 = (convexbody.GetInterpolationWorldTransform()._origin - convexbody.GetWorldTransform()._origin).LengthSquared();
if (squareMot0 < convexbody.GetCcdSquareMotionThreshold())
{
return(1);
}
//IndexedMatrix triInv = MathHelper.InvertMatrix(triBody.getWorldTransform());
IndexedMatrix triInv = triBody.GetWorldTransform().Inverse();
IndexedMatrix convexFromLocal = triInv * convexbody.GetWorldTransform();
IndexedMatrix convexToLocal = triInv * convexbody.GetInterpolationWorldTransform();
if (triBody.GetCollisionShape().IsConcave())
{
IndexedVector3 rayAabbMin = convexFromLocal._origin;
MathUtil.VectorMin(convexToLocal._origin, ref rayAabbMin);
IndexedVector3 rayAabbMax = convexFromLocal._origin;
MathUtil.VectorMax(convexToLocal._origin, ref rayAabbMax);
IndexedVector3 ccdRadius0 = new IndexedVector3(convexbody.GetCcdSweptSphereRadius());
rayAabbMin -= ccdRadius0;
rayAabbMax += ccdRadius0;
float curHitFraction = 1f; //is this available?
using (LocalTriangleSphereCastCallback raycastCallback = BulletGlobals.LocalTriangleSphereCastCallbackPool.Get())
{
raycastCallback.Initialize(ref convexFromLocal, ref convexToLocal,
convexbody.GetCcdSweptSphereRadius(), curHitFraction);
raycastCallback.m_hitFraction = convexbody.GetHitFraction();
CollisionObject concavebody = triBody;
ConcaveShape triangleMesh = concavebody.GetCollisionShape() as ConcaveShape;
if (triangleMesh != null)
{
triangleMesh.ProcessAllTriangles(raycastCallback, ref rayAabbMin, ref rayAabbMax);
}
if (raycastCallback.m_hitFraction < convexbody.GetHitFraction())
{
convexbody.SetHitFraction(raycastCallback.m_hitFraction);
return(raycastCallback.m_hitFraction);
}
}
}
return(1);
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
ClearCache();
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::ProcessCollision");
}
m_resultOut = resultOut;
m_dispatchInfo = dispatchInfo;
GImpactShapeInterface gimpactshape0;
GImpactShapeInterface gimpactshape1;
if (body0.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE)
{
gimpactshape0 = body0.GetCollisionShape() as GImpactShapeInterface;
if (body1.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE)
{
gimpactshape1 = body1.GetCollisionShape() as GImpactShapeInterface;
GImpactVsGImpact(body0, body1, gimpactshape0, gimpactshape1);
}
else
{
GImpactVsShape(body0, body1, gimpactshape0, body1.GetCollisionShape(), false);
}
}
else if (body1.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE)
{
gimpactshape1 = body1.GetCollisionShape() as GImpactShapeInterface;
GImpactVsShape(body1, body0, gimpactshape1, body0.GetCollisionShape(), true);
}
}
//public override void processCollision (CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
//{
// CollisionObject convexBody = m_isSwapped ? body1 : body0;
// CollisionObject triBody = m_isSwapped ? body0 : body1;
// if (triBody.getCollisionShape().isConcave())
// {
// CollisionObject triOb = triBody;
// ConcaveShape concaveShape = (ConcaveShape)(triOb.getCollisionShape());
// if (convexBody.getCollisionShape().isConvex())
// {
// float collisionMarginTriangle = concaveShape.getMargin();
// resultOut.setPersistentManifold(m_convexTriangleCallback.m_manifoldPtr);
// m_convexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut);
// //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
// //m_dispatcher.clearManifold(m_convexTriangleCallback.m_manifoldPtr);
// m_convexTriangleCallback.m_manifoldPtr.setBodies(convexBody, triBody);
// IndexedVector3 min = m_convexTriangleCallback.getAabbMin();
// IndexedVector3 max = m_convexTriangleCallback.getAabbMax();
// concaveShape.processAllTriangles(m_convexTriangleCallback, ref min,ref max );
// resultOut.refreshContactPoints();
// }
// }
//}
//public override float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
//{
// CollisionObject convexbody = m_isSwapped ? body1 : body0;
// CollisionObject triBody = m_isSwapped ? body0 : body1;
// //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
// //only perform CCD above a certain threshold, this prevents blocking on the long run
// //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
// float squareMot0 = (convexbody.getInterpolationWorldTransform()._origin - convexbody.getWorldTransform()._origin).LengthSquared();
// if (squareMot0 < convexbody.getCcdSquareMotionThreshold())
// {
// return 1f;
// }
// //const IndexedVector3& from = convexbody.m_worldTransform._origin;
// //IndexedVector3 to = convexbody.m_interpolationWorldTransform._origin;
// //todo: only do if the motion exceeds the 'radius'
// //IndexedMatrix triInv = IndexedMatrix.Invert(triBody.getWorldTransform());
// //IndexedMatrix convexFromLocal = MathUtil.bulletMatrixMultiply(triInv , convexbody.getWorldTransform());
// //IndexedMatrix convexToLocal = MathUtil.bulletMatrixMultiply(triInv , convexbody.getInterpolationWorldTransform());
// IndexedMatrix triInv = IndexedMatrix.Invert(triBody.getWorldTransform());
// IndexedMatrix convexFromLocal = MathUtil.inverseTimes(triBody.getWorldTransform(), convexbody.getWorldTransform());
// IndexedMatrix convexToLocal = MathUtil.inverseTimes(triBody.getWorldTransform(), convexbody.getInterpolationWorldTransform());
// if (triBody.getCollisionShape().isConcave())
// {
// IndexedVector3 rayAabbMin = convexFromLocal._origin;
// MathUtil.vectorMin(convexToLocal._origin, ref rayAabbMin);
// IndexedVector3 rayAabbMax = convexFromLocal._origin;
// MathUtil.vectorMax(convexToLocal._origin,ref rayAabbMax);
// float ccdRadius0 = convexbody.getCcdSweptSphereRadius();
// rayAabbMin -= new IndexedVector3(ccdRadius0,ccdRadius0,ccdRadius0);
// rayAabbMax += new IndexedVector3(ccdRadius0,ccdRadius0,ccdRadius0);
// float curHitFraction = 1.0f; //is this available?
// LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(ref convexFromLocal, ref convexToLocal,
// convexbody.getCcdSweptSphereRadius(),curHitFraction);
// raycastCallback.m_hitFraction = convexbody.getHitFraction();
// CollisionObject concavebody = triBody;
// ConcaveShape triangleMesh = (ConcaveShape) concavebody.getCollisionShape();
// if (triangleMesh != null)
// {
// triangleMesh.processAllTriangles(raycastCallback,ref rayAabbMin,ref rayAabbMax);
// }
// if (raycastCallback.m_hitFraction < convexbody.getHitFraction())
// {
// convexbody.setHitFraction( raycastCallback.m_hitFraction);
// float result = raycastCallback.m_hitFraction;
// raycastCallback.cleanup();
// return result;
// }
// raycastCallback.cleanup();
// }
// return 1f;
//}
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//fixme
CollisionObject convexBody = m_isSwapped ? bodyB : bodyA;
CollisionObject triBody = m_isSwapped ? bodyA : bodyB;
if (triBody.GetCollisionShape().IsConcave())
{
CollisionObject triOb = triBody;
ConcaveShape concaveShape = triOb.GetCollisionShape() as ConcaveShape;
if (convexBody.GetCollisionShape().IsConvex())
{
float collisionMarginTriangle = concaveShape.GetMargin();
resultOut.SetPersistentManifold(m_convexTriangleCallback.m_manifoldPtr);
m_convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut);
//Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
//m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);
m_convexTriangleCallback.m_manifoldPtr.SetBodies(convexBody, triBody);
IndexedVector3 min = m_convexTriangleCallback.GetAabbMin();
IndexedVector3 max = m_convexTriangleCallback.GetAabbMax();
concaveShape.ProcessAllTriangles(m_convexTriangleCallback, ref min, ref max);
resultOut.RefreshContactPoints();
}
}
}
//this constructor doesn't own the dispatcher and paircache/broadphase
public CollisionWorld(IDispatcher dispatcher, IBroadphaseInterface broadphasePairCache, ICollisionConfiguration collisionConfiguration)
{
m_dispatcher1 = dispatcher;
m_broadphasePairCache = broadphasePairCache;
m_collisionObjects = new ObjectArray<CollisionObject>();
m_dispatchInfo = new DispatcherInfo();
m_forceUpdateAllAabbs = true;
WorldSettings.Params = new WorldData.ParamData();
WorldSettings.Params.VoronoiSimplexSolverPool = new PooledType<VoronoiSimplexSolver>();
WorldSettings.Params.SubSimplexConvexCastPool = new PooledType<SubSimplexConvexCast>();
WorldSettings.Params.ManifoldPointPool = new PooledType<ManifoldPoint>();
WorldSettings.Params.CastResultPool = new PooledType<CastResult>();
WorldSettings.Params.SphereShapePool = new PooledType<SphereShape>();
WorldSettings.Params.DbvtNodePool = new PooledType<DbvtNode>();
WorldSettings.Params.SingleRayCallbackPool = new PooledType<SingleRayCallback>();
WorldSettings.Params.SubSimplexClosestResultPool = new PooledType<SubSimplexClosestResult>();
WorldSettings.Params.GjkPairDetectorPool = new PooledType<GjkPairDetector>();
WorldSettings.Params.DbvtTreeColliderPool = new PooledType<DbvtTreeCollider>();
WorldSettings.Params.SingleSweepCallbackPool = new PooledType<SingleSweepCallback>();
WorldSettings.Params.BroadphaseRayTesterPool = new PooledType<BroadphaseRayTester>();
WorldSettings.Params.ClosestNotMeConvexResultCallbackPool = new PooledType<ClosestNotMeConvexResultCallback>();
WorldSettings.Params.GjkEpaPenetrationDepthSolverPool = new PooledType<GjkEpaPenetrationDepthSolver>();
WorldSettings.Params.ContinuousConvexCollisionPool = new PooledType<ContinuousConvexCollision>();
WorldSettings.Params.DbvtStackDataBlockPool = new PooledType<DbvtStackDataBlock>();
WorldSettings.Params.BoxBoxCollisionAlgorithmPool = new PooledType<BoxBoxCollisionAlgorithm>();
WorldSettings.Params.CompoundCollisionAlgorithmPool = new PooledType<CompoundCollisionAlgorithm>();
WorldSettings.Params.ConvexConcaveCollisionAlgorithmPool = new PooledType<ConvexConcaveCollisionAlgorithm>();
WorldSettings.Params.ConvexConvexAlgorithmPool = new PooledType<ConvexConvexAlgorithm>();
WorldSettings.Params.ConvexPlaneAlgorithmPool = new PooledType<ConvexPlaneCollisionAlgorithm>();
WorldSettings.Params.SphereBoxCollisionAlgorithmPool = new PooledType<SphereBoxCollisionAlgorithm>();
WorldSettings.Params.SphereSphereCollisionAlgorithmPool = new PooledType<SphereSphereCollisionAlgorithm>();
WorldSettings.Params.SphereTriangleCollisionAlgorithmPool = new PooledType<SphereTriangleCollisionAlgorithm>();
WorldSettings.Params.GImpactCollisionAlgorithmPool = new PooledType<GImpactCollisionAlgorithm>();
WorldSettings.Params.GjkEpaSolver2MinkowskiDiffPool = new PooledType<GjkEpaSolver2MinkowskiDiff>();
WorldSettings.Params.PersistentManifoldPool = new PooledType<PersistentManifold>();
WorldSettings.Params.ManifoldResultPool = new PooledType<ManifoldResult>();
WorldSettings.Params.GJKPool = new PooledType<GJK>();
WorldSettings.Params.GIM_ShapeRetrieverPool = new PooledType<GIM_ShapeRetriever>();
WorldSettings.Params.TriangleShapePool = new PooledType<TriangleShape>();
WorldSettings.Params.SphereTriangleDetectorPool = new PooledType<SphereTriangleDetector>();
WorldSettings.Params.CompoundLeafCallbackPool = new PooledType<CompoundLeafCallback>();
WorldSettings.Params.GjkConvexCastPool = new PooledType<GjkConvexCast>();
WorldSettings.Params.LocalTriangleSphereCastCallbackPool = new PooledType<LocalTriangleSphereCastCallback>();
WorldSettings.Params.BridgeTriangleRaycastCallbackPool = new PooledType<BridgeTriangleRaycastCallback>();
WorldSettings.Params.BridgeTriangleConcaveRaycastCallbackPool = new PooledType<BridgeTriangleConcaveRaycastCallback>();
WorldSettings.Params.BridgeTriangleConvexcastCallbackPool = new PooledType<BridgeTriangleConvexcastCallback>();
WorldSettings.Params.MyNodeOverlapCallbackPool = new PooledType<MyNodeOverlapCallback>();
WorldSettings.Params.ClosestRayResultCallbackPool = new PooledType<ClosestRayResultCallback>();
WorldSettings.Params.DebugDrawcallbackPool = new PooledType<BulletXNA.DebugDrawcallback>();
}
public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
CollisionObject convexbody = m_isSwapped ? bodyB : bodyA;
CollisionObject triBody = m_isSwapped ? bodyA : bodyB;
//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
//only perform CCD above a certain threshold, this prevents blocking on the long run
//because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
float squareMot0 = (convexbody.GetInterpolationWorldTransform()._origin - convexbody.GetWorldTransform()._origin).LengthSquared();
if (squareMot0 < convexbody.GetCcdSquareMotionThreshold())
{
return 1;
}
//IndexedMatrix triInv = MathHelper.InvertMatrix(triBody.getWorldTransform());
IndexedMatrix triInv = triBody.GetWorldTransform().Inverse();
IndexedMatrix convexFromLocal = triInv * convexbody.GetWorldTransform();
IndexedMatrix convexToLocal = triInv * convexbody.GetInterpolationWorldTransform();
if (triBody.GetCollisionShape().IsConcave())
{
IndexedVector3 rayAabbMin = convexFromLocal._origin;
MathUtil.VectorMin(convexToLocal._origin, ref rayAabbMin);
IndexedVector3 rayAabbMax = convexFromLocal._origin;
MathUtil.VectorMax(convexToLocal._origin, ref rayAabbMax);
IndexedVector3 ccdRadius0 = new IndexedVector3(convexbody.GetCcdSweptSphereRadius());
rayAabbMin -= ccdRadius0;
rayAabbMax += ccdRadius0;
float curHitFraction = 1f; //is this available?
using (LocalTriangleSphereCastCallback raycastCallback = BulletGlobals.LocalTriangleSphereCastCallbackPool.Get())
{
raycastCallback.Initialize(ref convexFromLocal, ref convexToLocal,
convexbody.GetCcdSweptSphereRadius(), curHitFraction);
raycastCallback.m_hitFraction = convexbody.GetHitFraction();
CollisionObject concavebody = triBody;
ConcaveShape triangleMesh = concavebody.GetCollisionShape() as ConcaveShape;
if (triangleMesh != null)
{
triangleMesh.ProcessAllTriangles(raycastCallback, ref rayAabbMin, ref rayAabbMax);
}
if (raycastCallback.m_hitFraction < convexbody.GetHitFraction())
{
convexbody.SetHitFraction(raycastCallback.m_hitFraction);
return raycastCallback.m_hitFraction;
}
}
}
return 1;
}
public override float CalculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//(void)resultOut;
//(void)dispatchInfo;
///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
///body0.m_worldTransform,
float resultFraction = 1.0f;
float squareMot0 = (body0.GetInterpolationWorldTransform()._origin - body0.GetWorldTransform()._origin).LengthSquared();
float squareMot1 = (body1.GetInterpolationWorldTransform()._origin - body1.GetWorldTransform()._origin).LengthSquared();
if (squareMot0 < body0.GetCcdSquareMotionThreshold() &&
squareMot1 < body1.GetCcdSquareMotionThreshold())
{
return resultFraction;
}
if (disableCcd)
{
return 1f;
}
//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 = body0.GetCollisionShape() as ConvexShape;
SphereShape sphere1 = BulletGlobals.SphereShapePool.Get();
sphere1.Initialize(body1.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = BulletGlobals.CastResultPool.Get();
VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
{
ccd1.Initialize(convex0, sphere1, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction() > result.m_fraction)
{
body0.SetHitFraction(result.m_fraction);
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction(result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
BulletGlobals.SphereShapePool.Free(sphere1);
result.Cleanup();
}
}
/// Sphere (for convex0) against Convex1
{
ConvexShape convex1 = body1.GetCollisionShape() as ConvexShape;
SphereShape sphere0 = BulletGlobals.SphereShapePool.Get();
sphere0.Initialize(body0.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = BulletGlobals.CastResultPool.Get();
VoronoiSimplexSolver voronoiSimplex = BulletGlobals.VoronoiSimplexSolverPool.Get();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
using (GjkConvexCast ccd1 = BulletGlobals.GjkConvexCastPool.Get())
{
ccd1.Initialize(sphere0, convex1, voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(), body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(), body1.GetInterpolationWorldTransform(), result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction() > result.m_fraction)
{
body0.SetHitFraction(result.m_fraction);
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction(result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
BulletGlobals.VoronoiSimplexSolverPool.Free(voronoiSimplex);
BulletGlobals.SphereShapePool.Free(sphere0);
result.Cleanup();
}
}
return resultFraction;
}
public abstract void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut);