add warmstart
This commit is contained in:
Chuyuan Fu
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user