From 38b10137c91be3fb09e96210e0ba1658647b899b Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 16 Dec 2016 14:30:12 -0800 Subject: [PATCH 1/4] allow to terminate btSolveProjectedGaussSeidel MLCP solver based on a least squares residual threshold (m_leastSquaresResidualThreshold) --- .../MLCPSolvers/btSolveProjectedGaussSeidel.h | 30 +++++++++++++++++-- 1 file changed, 27 insertions(+), 3 deletions(-) diff --git a/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h b/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h index 77cc57c6e..c0b40ffd9 100644 --- a/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h +++ b/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h @@ -23,7 +23,18 @@ subject to the following restrictions: ///This solver is mainly for debug/learning purposes: it is functionally equivalent to the btSequentialImpulseConstraintSolver solver, but much slower (it builds the full LCP matrix) class btSolveProjectedGaussSeidel : public btMLCPSolverInterface { + public: + + btScalar m_leastSquaresResidualThreshold; + btScalar m_leastSquaresResidual; + + btSolveProjectedGaussSeidel() + :m_leastSquaresResidualThreshold(0), + m_leastSquaresResidual(0) + { + } + virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray& limitDependency, int numIterations, bool useSparsity = true) { if (!A.rows()) @@ -36,10 +47,11 @@ public: int i, j, numRows = A.rows(); - float delta; + btScalar delta; for (int k = 0; k =0) { @@ -76,6 +89,17 @@ public: x[i]=lo[i]*s; if (x[i]>hi[i]*s) x[i]=hi[i]*s; + btScalar diff = x[i] - xOld; + m_leastSquaresResidual += diff*diff; + } + + btScalar eps = m_leastSquaresResidualThreshold; + if ((m_leastSquaresResidual < eps) || (k >=(numIterations-1))) + { +#ifdef VERBOSE_PRINTF_RESIDUAL + printf("totalLenSqr = %f at iteration #%d\n", m_leastSquaresResidual,k); +#endif + break; } } return true; From dcd02a1e15ed550d931f068aa347bff5c872e395 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 16 Dec 2016 18:09:52 -0800 Subject: [PATCH 2/4] add option to terminate PGS constraint solvers based on a least square residual threshold (for example solverInfo().m_leastSquaresResidualThreshold = 1e-7 and use large m_numSolverIterations disable sphere-sphere contact cache, it is buggy (some contact point stay in the cache, when sphere penetrates more than total margins) tweak some gpu demo settings --- .../OpenCL/rigidbody/GpuRigidBodyDemo.cpp | 11 +++ examples/OpenCL/rigidbody/GpuRigidBodyDemo.h | 3 +- .../rigidbody/GpuRigidBodyDemoInternalData.h | 4 +- .../PhysicsServerCommandProcessor.cpp | 3 +- .../NarrowPhaseCollision/b3Config.h | 2 +- .../btSphereSphereCollisionAlgorithm.cpp | 1 + .../ConstraintSolver/btContactSolverInfo.h | 3 +- .../btSequentialImpulseConstraintSolver.cpp | 88 ++++++++++++++----- .../btSequentialImpulseConstraintSolver.h | 6 +- .../btMultiBodyConstraintSolver.cpp | 25 ++++-- .../btMultiBodyConstraintSolver.h | 2 +- 11 files changed, 111 insertions(+), 37 deletions(-) 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); From 8ff1e55166365e877308f39c15cf4ca7dc560017 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 16 Dec 2016 18:12:33 -0800 Subject: [PATCH 3/4] disable VERBOSE_RESIDUAL_PRINTF --- .../ConstraintSolver/btSequentialImpulseConstraintSolver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 1e63a437d..2dbfcc6c0 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 +//#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) From 379a852f93461067c8d80c243cbb736b2997cad1 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 16 Dec 2016 18:43:21 -0800 Subject: [PATCH 4/4] fix compile issues --- .../ConstraintSolver/btSequentialImpulseConstraintSolver.cpp | 4 ++-- .../ConstraintSolver/btSequentialImpulseConstraintSolver.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 2dbfcc6c0..c7231187b 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -330,11 +330,11 @@ btScalar btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCach return deltaImpulse; } -btScalar btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +btSimdScalar btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { #ifdef USE_SIMD if (!c.m_rhsPenetration) - return; + return 0.f; gNumSplitImpulseRecoveries++; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 3a0995632..0dd31d142 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -92,7 +92,7 @@ protected: void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); - btScalar resolveSplitPenetrationSIMD( + btSimdScalar resolveSplitPenetrationSIMD( btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint);