diff --git a/examples/OpenCL/rigidbody/GpuRigidBodyDemo.cpp b/examples/OpenCL/rigidbody/GpuRigidBodyDemo.cpp index d2ee9794a..c7b821dbf 100644 --- a/examples/OpenCL/rigidbody/GpuRigidBodyDemo.cpp +++ b/examples/OpenCL/rigidbody/GpuRigidBodyDemo.cpp @@ -80,7 +80,18 @@ m_window(0) m_window = helper->getAppInterface()->m_window; m_data = new GpuRigidBodyDemoInternalData; + m_data->m_guiHelper = helper; } + +void GpuRigidBodyDemo::resetCamera() +{ + float dist = 114; + float pitch = 52; + float yaw = 35; + float targetPos[3]={0,0,0}; + m_data->m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); +} + GpuRigidBodyDemo::~GpuRigidBodyDemo() { diff --git a/examples/OpenCL/rigidbody/GpuRigidBodyDemo.h b/examples/OpenCL/rigidbody/GpuRigidBodyDemo.h index 7e58b9e7d..a62fdab9e 100644 --- a/examples/OpenCL/rigidbody/GpuRigidBodyDemo.h +++ b/examples/OpenCL/rigidbody/GpuRigidBodyDemo.h @@ -31,7 +31,8 @@ public: virtual void renderScene(); - + void resetCamera(); + virtual void stepSimulation(float deltaTime); //for picking diff --git a/examples/OpenCL/rigidbody/GpuRigidBodyDemoInternalData.h b/examples/OpenCL/rigidbody/GpuRigidBodyDemoInternalData.h index 9147a5080..8c7b5f5bb 100644 --- a/examples/OpenCL/rigidbody/GpuRigidBodyDemoInternalData.h +++ b/examples/OpenCL/rigidbody/GpuRigidBodyDemoInternalData.h @@ -32,6 +32,7 @@ struct GpuRigidBodyDemoInternalData int m_pickGraphicsShapeIndex; int m_pickGraphicsShapeInstance; b3Config m_config; + GUIHelperInterface* m_guiHelper; GpuRigidBodyDemoInternalData() :m_instancePosOrnColor(0), @@ -45,7 +46,8 @@ struct GpuRigidBodyDemoInternalData m_pickGraphicsShapeInstance(-1), m_pickBody(-1), m_altPressed(0), - m_controlPressed(0) + m_controlPressed(0), + m_guiHelper(0) { } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index b57304ca2..5425100ef 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -717,7 +717,8 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001; m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50; - + m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7; + } void PhysicsServerCommandProcessor::deleteCachedInverseKinematicsBodies() diff --git a/src/Bullet3Collision/NarrowPhaseCollision/b3Config.h b/src/Bullet3Collision/NarrowPhaseCollision/b3Config.h index e23fe11a5..65d4a2161 100644 --- a/src/Bullet3Collision/NarrowPhaseCollision/b3Config.h +++ b/src/Bullet3Collision/NarrowPhaseCollision/b3Config.h @@ -20,7 +20,7 @@ struct b3Config int m_maxTriConvexPairCapacity; b3Config() - :m_maxConvexBodies(32*1024), + :m_maxConvexBodies(128*1024), m_maxVerticesPerFace(64), m_maxFacesPerShape(12), m_maxConvexVertices(8192), diff --git a/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp index 41f37ddf2..27eaec305 100644 --- a/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp @@ -12,6 +12,7 @@ subject to the following restrictions: 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ +#define CLEAR_MANIFOLD 1 #include "btSphereSphereCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index cf310da76..739b066fe 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -58,7 +58,7 @@ struct btContactSolverInfoData int m_minimumSolverBatchSize; btScalar m_maxGyroscopicForce; btScalar m_singleAxisRollingFrictionThreshold; - + btScalar m_leastSquaresResidualThreshold; }; @@ -91,6 +91,7 @@ struct btContactSolverInfo : public btContactSolverInfoData m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit m_maxGyroscopicForce = 100.f; ///it is only used for 'explicit' version of gyroscopic force m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows. + m_leastSquaresResidualThreshold = 0.f; } }; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index caf0258a4..1e63a437d 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -39,7 +39,7 @@ int gNumSplitImpulseRecoveries = 0; #include "BulletDynamics/Dynamics/btRigidBody.h" - +#define VERBOSE_RESIDUAL_PRINTF 1 ///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver ///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check. static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) @@ -298,15 +298,17 @@ btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowe } -void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly( +btScalar btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly( btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) { + btScalar deltaImpulse = 0.f; + if (c.m_rhsPenetration) { gNumSplitImpulseRecoveries++; - btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm; + deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm; const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity()); const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity()); @@ -325,9 +327,10 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri body1.internalApplyPushImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); body2.internalApplyPushImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); } + return deltaImpulse; } - void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +btScalar btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { #ifdef USE_SIMD if (!c.m_rhsPenetration) @@ -357,8 +360,9 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); + return deltaImpulse; #else - resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c); + return resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c); #endif } @@ -1601,6 +1605,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/) { + btScalar leastSquaresResidual = 0.f; int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); int numConstraintPool = m_tmpSolverContactConstraintPool.size(); @@ -1645,7 +1650,10 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration { btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; if (iteration < constraint.m_overrideNumSolverIterations) - resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); + { + btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); + leastSquaresResidual += residual*residual; + } } if (iteration< infoGlobal.m_numIterations) @@ -1674,7 +1682,9 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration { const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]]; - resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + btScalar residual = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + leastSquaresResidual += residual*residual; + totalImpulse = solveManifold.m_appliedImpulse; } bool applyFriction = true; @@ -1689,7 +1699,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + leastSquaresResidual += residual*residual; } } @@ -1703,7 +1714,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + leastSquaresResidual += residual*residual; } } } @@ -1719,8 +1731,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration for (j=0;j=(infoGlobal.m_numIterations-1)) + { +#ifdef VERBOSE_RESIDUAL_PRINTF + printf("residual = %f at iteration #%d\n",leastSquaresResidual,iteration); +#endif + break; + } } } else { for ( iteration = 0;iteration=(infoGlobal.m_numIterations-1)) + { +#ifdef VERBOSE_RESIDUAL_PRINTF + printf("residual = %f at iteration #%d\n",leastSquaresResidual,iteration); +#endif + break; } } } @@ -1888,7 +1926,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations( for ( int iteration = 0 ; iteration< maxIterations ; iteration++) //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--) { - solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); + m_leastSquaresResidual = solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); + + if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration>= (maxIterations-1))) + { +#ifdef VERBOSE_RESIDUAL_PRINTF + printf("residual = %f at iteration #%d\n",m_leastSquaresResidual,iteration); +#endif + break; + } } } diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 7f594208f..3a0995632 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -57,6 +57,8 @@ protected: btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric; btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit; + btScalar m_leastSquaresResidual; + void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB, btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, @@ -90,11 +92,11 @@ protected: void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); - void resolveSplitPenetrationSIMD( + btScalar resolveSplitPenetrationSIMD( btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint); - void resolveSplitPenetrationImpulseCacheFriendly( + btScalar resolveSplitPenetrationImpulseCacheFriendly( btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index aa0755a10..70e6c9922 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -26,7 +26,7 @@ subject to the following restrictions: btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) { - btScalar val = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); + btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); //solve featherstone non-contact constraints @@ -38,7 +38,9 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index]; - resolveSingleConstraintRowGeneric(constraint); + btScalar residual = resolveSingleConstraintRowGeneric(constraint); + leastSquaredResidual += residual*residual; + if(constraint.m_multiBodyA) constraint.m_multiBodyA->setPosUpdated(false); if(constraint.m_multiBodyB) @@ -49,9 +51,15 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl for (int j=0;jsetPosUpdated(false); if(constraint.m_multiBodyB) @@ -71,7 +79,8 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl { frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; - resolveSingleConstraintRowGeneric(frictionConstraint); + btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint); + leastSquaredResidual += residual*residual; if(frictionConstraint.m_multiBodyA) frictionConstraint.m_multiBodyA->setPosUpdated(false); @@ -80,7 +89,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl } } } - return val; + return leastSquaredResidual; } btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) @@ -112,7 +121,7 @@ void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar im m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; } -void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c) +btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c) { btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; @@ -190,7 +199,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult { bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse); } - + return deltaImpulse; } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index 3a89c6373..489347d87 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -43,7 +43,7 @@ protected: btMultiBodyConstraint** m_tmpMultiBodyConstraints; int m_tmpNumMultiBodyConstraints; - void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c); + btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c); void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);