From 96125611139036f697d48179d489c6e062ca5fea Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Tue, 11 Sep 2012 00:56:11 +0000 Subject: [PATCH] add btDiscreteDynamicsWorld::createPredictiveContacts after discrete collision detection and add temporary contact manifolds, for the constraint solver. This should improve 'ccd' handling when using world->getDispatchInfo().m_useContinuous = true;body->setCcdSquareMotionThreshold(...); body->setCcdSquareMotionThreshold(...) shoot smaller boxes (test) use yellow instead of orange for contact point normals tweak default erp and erp2 values, now split impulse is on by default (need to check it) --- Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp | 2 - Demos/OpenGL/DemoApplication.cpp | 6 +- .../CollisionDispatch/btCollisionWorld.cpp | 2 +- .../btPersistentManifold.cpp | 9 +- .../btPersistentManifold.h | 2 +- .../ConstraintSolver/btContactSolverInfo.h | 4 +- .../Dynamics/btDiscreteDynamicsWorld.cpp | 99 +++++++++++++++++++ .../Dynamics/btDiscreteDynamicsWorld.h | 5 +- 8 files changed, 116 insertions(+), 13 deletions(-) diff --git a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp index da2b9cb86..d64aebcb6 100644 --- a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp +++ b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp @@ -182,8 +182,6 @@ void CcdPhysicsDemo::initPhysics() m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration); m_dynamicsWorld ->setDebugDrawer(&sDebugDrawer); - m_dynamicsWorld->getSolverInfo().m_splitImpulse=true; - m_dynamicsWorld->getSolverInfo().m_numIterations = 20; if (m_ccdMode==USE_CCD) diff --git a/Demos/OpenGL/DemoApplication.cpp b/Demos/OpenGL/DemoApplication.cpp index d8b8f17ef..18d8b21ff 100644 --- a/Demos/OpenGL/DemoApplication.cpp +++ b/Demos/OpenGL/DemoApplication.cpp @@ -534,7 +534,7 @@ void DemoApplication::setShootBoxShape () { if (!m_shootBoxShape) { - btBoxShape* box = new btBoxShape(btVector3(.5f,.5f,.5f)); + btBoxShape* box = new btBoxShape(btVector3(.15f,.15f,.15f)); box->initializePolyhedralFeatures(); m_shootBoxShape = box; } @@ -565,8 +565,8 @@ void DemoApplication::shootBox(const btVector3& destination) body->getWorldTransform().setRotation(btQuaternion(0,0,0,1)); body->setLinearVelocity(linVel); body->setAngularVelocity(btVector3(0,0,0)); - body->setCcdMotionThreshold(0.5); - body->setCcdSweptSphereRadius(0.9f); + body->setCcdMotionThreshold(0.1); + body->setCcdSweptSphereRadius(0.1f); // printf("shootBox uid=%d\n", body->getBroadphaseHandle()->getUid()); // printf("camPos=%f,%f,%f\n",camPos.getX(),camPos.getY(),camPos.getZ()); // printf("destination=%f,%f,%f\n",destination.getX(),destination.getY(),destination.getZ()); diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 6c1be6c55..f88234fe9 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -1417,7 +1417,7 @@ void btCollisionWorld::debugDrawWorld() if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) { int numManifolds = getDispatcher()->getNumManifolds(); - btVector3 color(1,0.65,0); + btVector3 color(1,1,0); for (int i=0;igetManifoldByIndexInternal(i); diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index ec8735614..4d92e853d 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -205,10 +205,13 @@ int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const return nearestPoint; } -int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint) +int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool isPredictive) { - btAssert(validContactDistance(newPoint)); - + if (!isPredictive) + { + btAssert(validContactDistance(newPoint)); + } + int insertIndex = getNumContacts(); if (insertIndex == MANIFOLD_CACHE_SIZE) { diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index f343ab992..2ceaab750 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -149,7 +149,7 @@ public: int getCacheEntry(const btManifoldPoint& newPoint) const; - int addManifoldPoint( const btManifoldPoint& newPoint); + int addManifoldPoint( const btManifoldPoint& newPoint, bool isPredictive=false); void removeContactPoint (int index) { diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 7ddc25ecc..19a2203f2 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -74,8 +74,8 @@ struct btContactSolverInfo : public btContactSolverInfoData m_restitution = btScalar(0.); m_maxErrorReduction = btScalar(20.); m_numIterations = 10; - m_erp = btScalar(0.1); - m_erp2 = btScalar(0.1); + m_erp = btScalar(0.2); + m_erp2 = btScalar(0.8); m_globalCfm = btScalar(0.); m_sor = btScalar(1.); m_splitImpulse = true; diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 3f6f5c015..b7a32edc0 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -484,6 +484,7 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) ///perform collision detection performDiscreteCollisionDetection(); + createPredictiveContacts(timeStep); calculateSimulationIslands(); @@ -498,6 +499,7 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) ///CallbackTriggers(); ///integrate transforms + integrateTransforms(timeStep); ///update vehicle simulation @@ -851,6 +853,101 @@ public: ///internal debugging variable. this value shouldn't be too high int gNumClampedCcdMotions=0; + +void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep) +{ + BT_PROFILE("createPredictiveContacts"); + + { + BT_PROFILE("release predictive contact manifolds"); + + for (int i=0;im_dispatcher1->releaseManifold(manifold); + } + m_predictiveManifolds.clear(); + } + + btTransform predictedTrans; + for ( int i=0;isetHitFraction(1.f); + + if (body->isActive() && (!body->isStaticOrKinematicObject())) + { + + body->predictIntegratedTransform(timeStep, predictedTrans); + + btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); + + if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) + { + BT_PROFILE("predictive convexSweepTest"); + if (body->getCollisionShape()->isConvex()) + { + gNumClampedCcdMotions++; +#ifdef PREDICTIVE_CONTACT_USE_STATIC_ONLY + class StaticOnlyCallback : public btClosestNotMeConvexResultCallback + { + public: + + StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : + btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher) + { + } + + virtual bool needsCollision(btBroadphaseProxy* proxy0) const + { + btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; + if (!otherObj->isStaticOrKinematicObject()) + return false; + return btClosestNotMeConvexResultCallback::needsCollision(proxy0); + } + }; + + StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); +#else + btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); +#endif + //btConvexShape* convexShape = static_cast(body->getCollisionShape()); + btSphereShape tmpSphere(0.1);//body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast(body->getCollisionShape()); + sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration; + + sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; + sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; + btTransform modifiedPredictedTrans = predictedTrans; + modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis()); + + convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults); + if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) + { + + btVector3 distVec = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin())*sweepResults.m_closestHitFraction; + btScalar distance = distVec.dot(-sweepResults.m_hitNormalWorld); + + + btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body,sweepResults.m_hitCollisionObject); + m_predictiveManifolds.push_back(manifold); + + btVector3 worldPointB = body->getWorldTransform().getOrigin()+distVec; + btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse()*worldPointB; + + btManifoldPoint newPoint(btVector3(0,0,0), localPointB,sweepResults.m_hitNormalWorld,distance); + + bool isPredictive = true; + int index = manifold->addManifoldPoint(newPoint, isPredictive); + btManifoldPoint& pt = manifold->getContactPoint(index); + pt.m_positionWorldOnA = body->getWorldTransform().getOrigin(); + pt.m_positionWorldOnB = worldPointB; + + } + } + } + } + } +} void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) { BT_PROFILE("integrateTransforms"); @@ -951,7 +1048,9 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) body->proceedToTransform( predictedTrans); + } + } } diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index 15fb0d95f..3fa49e78f 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -25,7 +25,7 @@ class btConstraintSolver; class btSimulationIslandManager; class btTypedConstraint; class btActionInterface; - +class btPersistentManifold; class btIDebugDraw; struct InplaceSolverIslandCallback; @@ -63,6 +63,8 @@ protected: int m_profileTimings; + btAlignedObjectArray m_predictiveManifolds; + virtual void predictUnconstraintMotion(btScalar timeStep); virtual void integrateTransforms(btScalar timeStep); @@ -79,6 +81,7 @@ protected: virtual void internalSingleStepSimulation( btScalar timeStep); + void createPredictiveContacts(btScalar timeStep); virtual void saveKinematicState(btScalar timeStep);