diff --git a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index 6d21f6620..12ce02e87 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -55,6 +55,7 @@ public: : m_userPersistentData(0), m_contactPointFlags(0), m_appliedImpulse(0.f), + m_prevRHS(0.f), m_appliedImpulseLateral1(0.f), m_appliedImpulseLateral2(0.f), m_contactMotion1(0.f), @@ -79,6 +80,7 @@ public: m_userPersistentData(0), m_contactPointFlags(0), m_appliedImpulse(0.f), + m_prevRHS(0.f), m_appliedImpulseLateral1(0.f), m_appliedImpulseLateral2(0.f), m_contactMotion1(0.f), @@ -114,7 +116,8 @@ public: int m_contactPointFlags; btScalar m_appliedImpulse; - btScalar m_appliedImpulseLateral1; + btScalar m_prevRHS; + btScalar m_appliedImpulseLateral1; btScalar m_appliedImpulseLateral2; btScalar m_contactMotion1; btScalar m_contactMotion2; diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index f1422cad4..f378ba73b 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -325,7 +325,8 @@ const char* btPersistentManifold::serialize(const class btPersistentManifold* ma { const btManifoldPoint& pt = manifold->getContactPoint(i); dataOut->m_pointCacheAppliedImpulse[i] = pt.m_appliedImpulse; - dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1; + dataOut->m_pointCachePrevRHS[i] = pt.m_prevRHS; + dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1; dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2; pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]); pt.m_localPointB.serialize(dataOut->m_pointCacheLocalPointB[i]); @@ -337,7 +338,6 @@ const char* btPersistentManifold::serialize(const class btPersistentManifold* ma dataOut->m_pointCacheFrictionCFM[i] = pt.m_frictionCFM; dataOut->m_pointCacheContactERP[i] = pt.m_contactERP; dataOut->m_pointCacheContactCFM[i] = pt.m_contactCFM; - dataOut->m_pointCacheContactPointFlags[i] = pt.m_contactPointFlags; dataOut->m_pointCacheIndex0[i] = pt.m_index0; dataOut->m_pointCacheIndex1[i] = pt.m_index1; dataOut->m_pointCachePartId0[i] = pt.m_partId0; @@ -371,6 +371,7 @@ void btPersistentManifold::deSerialize(const struct btPersistentManifoldDoubleDa btManifoldPoint& pt = m_pointCache[i]; pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i]; + pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i]; pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i]; pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i]; pt.m_localPointA.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointA[i]); @@ -416,6 +417,7 @@ void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatDat btManifoldPoint& pt = m_pointCache[i]; pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i]; + pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i]; pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i]; pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i]; pt.m_localPointA.deSerialize(manifoldDataPtr->m_pointCacheLocalPointA[i]); @@ -444,4 +446,4 @@ void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatDat pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i]; pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i]; } -} \ No newline at end of file +} diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index 8a9134c95..feb149af6 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -173,6 +173,7 @@ public: //get rid of duplicated userPersistentData pointer m_pointCache[lastUsedIndex].m_userPersistentData = 0; m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f; + m_pointCache[lastUsedIndex].m_prevRHS = 0.f; m_pointCache[lastUsedIndex].m_contactPointFlags = 0; m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f; m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f; @@ -195,6 +196,7 @@ public: #ifdef MAINTAIN_PERSISTENCY int lifeTime = m_pointCache[insertIndex].getLifeTime(); btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse; + btScalar prevRHS = m_pointCache[insertIndex].m_prevRHS; btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1; btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2; @@ -223,6 +225,7 @@ public: m_pointCache[insertIndex] = newPoint; m_pointCache[insertIndex].m_userPersistentData = cache; m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse; + m_pointCache[insertIndex].m_prevRHS = prevRHS; m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1; m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2; } @@ -276,7 +279,8 @@ struct btPersistentManifoldDoubleData btVector3DoubleData m_pointCacheLateralFrictionDir2[4]; double m_pointCacheDistance[4]; double m_pointCacheAppliedImpulse[4]; - double m_pointCacheCombinedFriction[4]; + double m_pointCachePrevRHS[4]; + double m_pointCacheCombinedFriction[4]; double m_pointCacheCombinedRollingFriction[4]; double m_pointCacheCombinedSpinningFriction[4]; double m_pointCacheCombinedRestitution[4]; @@ -322,6 +326,7 @@ struct btPersistentManifoldFloatData btVector3FloatData m_pointCacheLateralFrictionDir2[4]; float m_pointCacheDistance[4]; float m_pointCacheAppliedImpulse[4]; + float m_pointCachePrevRHS[4]; float m_pointCacheCombinedFriction[4]; float m_pointCacheCombinedRollingFriction[4]; float m_pointCacheCombinedSpinningFriction[4]; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index e17ab94d9..d7ed05ce5 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -342,40 +342,6 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(btMultiBodySolverConstra solverConstraint.m_friction = 0.f; //cp.m_combinedFriction; } - ///warm starting (or zero if disabled) - /* - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - - if (solverConstraint.m_appliedImpulse) - { - if (multiBodyA) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); - } else - { - if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); - } - if (multiBodyB) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - multiBodyB->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); - } else - { - if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); - } - } - } else - */ - solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedPushImpulse = 0.f; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 45c893b27..e10fea5a6 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -22,6 +22,8 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" #include "LinearMath/btQuickprof.h" +#include "BulletDynamics/Featherstone/btMultiBodySolverConstraint.h" +#include "LinearMath/btScalar.h" btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { @@ -491,11 +493,7 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt return deltaVel; } -void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, - const btVector3& contactNormal, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, const btVector3& contactNormal, const btScalar& appliedImpulse, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) { BT_PROFILE("setupMultiBodyContactConstraint"); btVector3 rel_pos1; @@ -781,48 +779,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } } - ///warm starting (or zero if disabled) - //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion) - if (/* DISABLES CODE */ (0)) //infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - - if (solverConstraint.m_appliedImpulse) - { - if (multiBodyA) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVeeMultiDof(deltaV, impulse); - - applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA); - } - else - { - if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse); - } - if (multiBodyB) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - multiBodyB->applyDeltaVeeMultiDof(deltaV, impulse); - applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB); - } - else - { - if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse); - } - } - } - else - { - solverConstraint.m_appliedImpulse = 0.f; - } - - solverConstraint.m_appliedPushImpulse = 0.f; - { btScalar positionalError = 0.f; btScalar velocityError = restitution - rel_vel; // * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction @@ -874,6 +830,53 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv; } + + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + if (btFabs(cp.m_prevRHS) > 1e-5) + { + solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse / cp.m_prevRHS * solverConstraint.m_rhs * infoGlobal.m_warmstartingFactor; + if (solverConstraint.m_appliedImpulse < 0) + solverConstraint.m_appliedImpulse = 0; + } + else + { + solverConstraint.m_appliedImpulse = 0.f; + } + + if (solverConstraint.m_appliedImpulse) + { + if (multiBodyA) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse); + + applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA); + } + else + { + if (rb0) + bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse); + } + if (multiBodyB) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse); + applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB); + } + else + { + if (rb1) + bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse); + } + } + } + else + { + solverConstraint.m_appliedImpulse = 0.f; + } } void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint, @@ -1130,7 +1133,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu } } -btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { BT_PROFILE("addMultiBodyFrictionConstraint"); btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); @@ -1161,7 +1164,7 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo solverConstraint.m_originalContactPoint = &cp; - setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip); + setupMultiBodyContactConstraint(solverConstraint, normalAxis, 0, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip); return solverConstraint; } @@ -1297,7 +1300,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* solverConstraint.m_originalContactPoint = &cp; bool isFriction = false; - setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp, infoGlobal, relaxation, isFriction); + setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp.m_appliedImpulse, cp, infoGlobal, relaxation, isFriction); // const btVector3& pos1 = cp.getPositionWorldOnA(); // const btVector3& pos2 = cp.getPositionWorldOnB(); @@ -1371,13 +1374,13 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* { applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal); } if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) @@ -1388,26 +1391,25 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* } else { - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM); - - //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); - //todo: - solverConstraint.m_appliedImpulse = 0.f; - solverConstraint.m_appliedPushImpulse = 0.f; + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM); } #endif //ENABLE_FRICTION } + else + { + // Reset quantities related to warmstart as 0. + cp.m_appliedImpulse = 0; + cp.m_prevRHS = 0; + } } } void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) { - //btPersistentManifold* manifold = 0; - for (int i = 0; i < numManifolds; i++) { btPersistentManifold* manifold = manifoldPtr[i]; @@ -1434,6 +1436,51 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol c->createConstraintRows(m_multiBodyNonContactConstraints, m_data, infoGlobal); } + + // Warmstart for noncontact constraints + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++) + { + btMultiBodySolverConstraint& solverConstraint = + m_multiBodyNonContactConstraints[i]; + solverConstraint.m_appliedImpulse = + solverConstraint.m_orgConstraint->getAppliedImpulse(solverConstraint.m_orgDofIndex) * + infoGlobal.m_warmstartingFactor; + + btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; + btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; + if (solverConstraint.m_appliedImpulse) + { + if (multiBodyA) + { + int ndofA = multiBodyA->getNumDofs() + 6; + btScalar* deltaV = + &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + btScalar impulse = solverConstraint.m_appliedImpulse; + multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse); + applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA); + } + if (multiBodyB) + { + int ndofB = multiBodyB->getNumDofs() + 6; + btScalar* deltaV = + &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + btScalar impulse = solverConstraint.m_appliedImpulse; + multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse); + applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB); + } + } + } + } + else + { + for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++) + { + btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i]; + solverConstraint.m_appliedImpulse = 0; + } + } } btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) @@ -1678,10 +1725,22 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO } #endif #endif - + writeBackMultiBodyContacts(0, numPoolConstraints); return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal); } +void btMultiBodyConstraintSolver::writeBackMultiBodyContacts(int iBegin, int iEnd) +{ + for (int j = iBegin; j < iEnd; j++) + { + const btMultiBodySolverConstraint& solveManifold = m_multiBodyNormalContactConstraints[j]; + btManifoldPoint* pt = (btManifoldPoint*)solveManifold.m_originalContactPoint; + btAssert(pt); + pt->m_appliedImpulse = solveManifold.m_appliedImpulse; + pt->m_prevRHS = solveManifold.m_rhs; + } +} + void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) { //printf("solveMultiBodyGroup: numBodies=%d, numConstraints=%d, numManifolds=%d, numMultiBodyConstraints=%d\n", numBodies, numConstraints, numManifolds, numMultiBodyConstraints); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index abf571883..2f5a6535c 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -49,7 +49,7 @@ protected: void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); - btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); + btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); btMultiBodySolverConstraint& addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, @@ -66,7 +66,9 @@ protected: void setupMultiBodyContactConstraint(btMultiBodySolverConstraint & solverConstraint, const btVector3& contactNormal, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + const btScalar& appliedImpulse, + btManifoldPoint& cp, + const btContactSolverInfo& infoGlobal, btScalar& relaxation, bool isFriction, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); @@ -82,10 +84,10 @@ protected: void convertMultiBodyContact(btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal); virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); // virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); void applyDeltaVee(btScalar * deltaV, btScalar impulse, int velocityIndex, int ndof); void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint & constraint, btScalar deltaTime); + void writeBackMultiBodyContacts(int iBegin, int iEnd); public: BT_DECLARE_ALIGNED_ALLOCATOR();