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 = (ConvexShape)(body0.GetCollisionShape()); ConvexShape min1 = (ConvexShape)(body1.GetCollisionShape()); Vector3 normalOnB = Vector3.Up; Vector3 pointOnBWorld = Vector3.Zero; #if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER if ((min0.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE) && (min1.ShapeType == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE)) { CapsuleShape capsuleA = (CapsuleShape) min0; CapsuleShape capsuleB = (CapsuleShape) min1; Vector3 localScalingA = capsuleA.GetLocalScaling(); Vector3 localScalingB = capsuleB.GetLocalScaling(); float threshold = m_manifoldPtr.GetContactBreakingThreshold(); float dist = CapsuleCapsuleDistance(ref normalOnB,ref 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 = new ClosestPointInput(); GjkPairDetector gjkPairDetector = new GjkPairDetector(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.Margin + min1.Margin + 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(); gjkPairDetector.GetClosestPoints(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) { Vector3 v0 = Vector3.Zero, v1 = Vector3.Zero; Vector3 sepNormalWorldSpace = gjkPairDetector.GetCachedSeparatingAxis(); sepNormalWorldSpace.Normalize(); TransformUtil.PlaneSpace1(ref sepNormalWorldSpace, ref v0, ref 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; } Matrix unPerturbedTransform = Matrix.Identity; 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) { Quaternion perturbeRot = Quaternion.CreateFromAxisAngle(v0, perturbeAngle); float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations); Quaternion rotq = Quaternion.CreateFromAxisAngle(sepNormalWorldSpace, iterationAngle); if (perturbeA) { //input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0.getWorldTransform().getBasis()); Quaternion temp = MathUtil.QuaternionMultiply(MathUtil.QuaternionInverse(ref rotq),MathUtil.QuaternionMultiply(perturbeRot,rotq)); input.m_transformA = MathUtil.BulletMatrixMultiplyBasis(Matrix.CreateFromQuaternion(temp),body0.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.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1.getWorldTransform().getBasis()); Quaternion temp = MathUtil.QuaternionMultiply(MathUtil.QuaternionInverse(ref rotq),MathUtil.QuaternionMultiply(perturbeRot,rotq)); input.m_transformB = MathUtil.BulletMatrixMultiplyBasis(Matrix.CreateFromQuaternion(temp),body1.GetWorldTransform()); #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(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 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 = (ConvexShape) convexObj.GetCollisionShape(); StaticPlaneShape planeShape = (StaticPlaneShape) planeObj.GetCollisionShape(); //bool hasCollision = false; Vector3 planeNormal = planeShape.GetPlaneNormal(); //float planeConstant = planeShape.getPlaneConstant(); //first perform a collision query with the non-perturbated collision objects { Quaternion rotq = Quaternion.Identity; CollideSingleContact(ref rotq,body0,body1,dispatchInfo,resultOut); } if (resultOut.GetPersistentManifold().GetNumContacts()<m_minimumPointsPerturbationThreshold) { Vector3 v0 = Vector3.Zero; Vector3 v1 = Vector3.Zero; TransformUtil.PlaneSpace1(ref planeNormal,ref v0,ref 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; } Quaternion perturbeRot = Quaternion.CreateFromAxisAngle(v0,perturbeAngle); for (int i=0;i<m_numPerturbationIterations;i++) { float iterationAngle = i*(MathUtil.SIMD_2_PI/(float)m_numPerturbationIterations); Quaternion rotq = Quaternion.CreateFromAxisAngle(planeNormal,iterationAngle); rotq = MathUtil.QuaternionMultiply(Quaternion.Inverse(rotq),MathUtil.QuaternionMultiply(perturbeRot,rotq)); CollideSingleContact(ref rotq,body0,body1,dispatchInfo,resultOut); } } if (m_ownManifold) { if (m_manifoldPtr.GetNumContacts() > 0) { resultOut.RefreshContactPoints(); } } }