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