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