Implement true implicit friction cone, instead of friction pyramid, for btMultiBody vs btMultiBody and btMultiBody vs btRigidBody
See data/sphere_small_zeroinertia.urdf for an example.
This commit is contained in:
erwincoumans
30
data/sphere_small_zeroinertia.urdf
Normal file
30
data/sphere_small_zeroinertia.urdf
Normal file
@@ -0,0 +1,30 @@
|
||||
<?xml version="0.0" ?>
|
||||
<robot name="urdf_robot">
|
||||
<link name="base_link">
|
||||
<contact>
|
||||
<rolling_friction value="0.001"/>
|
||||
<spinning_friction value="0.001"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value=".1"/>
|
||||
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="textured_sphere_smooth.obj" scale="0.03 0.03 0.03"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="0.03"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
|
||||
@@ -70,14 +70,32 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
|
||||
|
||||
//solve featherstone frictional contact
|
||||
|
||||
for (int j1=0;j1<this->m_multiBodyFrictionContactConstraints.size();j1++)
|
||||
for (int j1=0;j1<this->m_multiBodyLateralFrictionContactConstraints.size();j1++)
|
||||
{
|
||||
if (iteration < infoGlobal.m_numIterations)
|
||||
{
|
||||
int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
|
||||
btMultiBodySolverConstraint& frictionConstraint = m_multiBodyLateralFrictionContactConstraints[index];
|
||||
|
||||
btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index];
|
||||
btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
|
||||
{
|
||||
j1++;
|
||||
int index2 = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
|
||||
btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyLateralFrictionContactConstraints[index2];
|
||||
btAssert(frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex);
|
||||
|
||||
if (frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex)
|
||||
{
|
||||
frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
|
||||
frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
|
||||
frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction*totalImpulse);
|
||||
frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction*totalImpulse;
|
||||
resolveConeFrictionConstraintRows(frictionConstraint,frictionConstraintB);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//adjust friction limits here
|
||||
if (totalImpulse > btScalar(0))
|
||||
{
|
||||
@@ -93,6 +111,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return leastSquaredResidual;
|
||||
}
|
||||
|
||||
@@ -100,7 +119,9 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb
|
||||
{
|
||||
m_multiBodyNonContactConstraints.resize(0);
|
||||
m_multiBodyNormalContactConstraints.resize(0);
|
||||
m_multiBodyFrictionContactConstraints.resize(0);
|
||||
m_multiBodyLateralFrictionContactConstraints.resize(0);
|
||||
m_multiBodyTorsionalFrictionContactConstraints.resize(0);
|
||||
|
||||
m_data.m_jacobians.resize(0);
|
||||
m_data.m_deltaVelocitiesUnitImpulse.resize(0);
|
||||
m_data.m_deltaVelocities.resize(0);
|
||||
@@ -141,7 +162,8 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt
|
||||
ndofA = c.m_multiBodyA->getNumDofs() + 6;
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
deltaVelADotn += m_data.m_jacobians[c.m_jacAindex + i] * m_data.m_deltaVelocities[c.m_deltaVelAindex + i];
|
||||
} else if(c.m_solverBodyIdA >= 0)
|
||||
}
|
||||
else if (c.m_solverBodyIdA >= 0)
|
||||
{
|
||||
bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
|
||||
deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
|
||||
@@ -152,7 +174,8 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt
|
||||
ndofB = c.m_multiBodyB->getNumDofs() + 6;
|
||||
for (int i = 0; i < ndofB; ++i)
|
||||
deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex + i] * m_data.m_deltaVelocities[c.m_deltaVelBindex + i];
|
||||
} else if(c.m_solverBodyIdB >= 0)
|
||||
}
|
||||
else if (c.m_solverBodyIdB >= 0)
|
||||
{
|
||||
bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
|
||||
deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
|
||||
@@ -186,7 +209,8 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt
|
||||
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
|
||||
c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse);
|
||||
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||
} else if(c.m_solverBodyIdA >= 0)
|
||||
}
|
||||
else if (c.m_solverBodyIdA >= 0)
|
||||
{
|
||||
bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
|
||||
|
||||
@@ -199,7 +223,8 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt
|
||||
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
|
||||
c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse);
|
||||
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||
} else if(c.m_solverBodyIdB >= 0)
|
||||
}
|
||||
else if (c.m_solverBodyIdB >= 0)
|
||||
{
|
||||
bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
|
||||
}
|
||||
@@ -207,6 +232,185 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt
|
||||
}
|
||||
|
||||
|
||||
btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1,const btMultiBodySolverConstraint& cB)
|
||||
{
|
||||
int ndofA=0;
|
||||
int ndofB=0;
|
||||
btSolverBody* bodyA = 0;
|
||||
btSolverBody* bodyB = 0;
|
||||
btScalar deltaImpulseB = 0.f;
|
||||
btScalar sumB = 0.f;
|
||||
{
|
||||
deltaImpulseB = cB.m_rhs-btScalar(cB.m_appliedImpulse)*cB.m_cfm;
|
||||
btScalar deltaVelADotn=0;
|
||||
btScalar deltaVelBDotn=0;
|
||||
if (cB.m_multiBodyA)
|
||||
{
|
||||
ndofA = cB.m_multiBodyA->getNumDofs() + 6;
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
deltaVelADotn += m_data.m_jacobians[cB.m_jacAindex+i] * m_data.m_deltaVelocities[cB.m_deltaVelAindex+i];
|
||||
} else if(cB.m_solverBodyIdA >= 0)
|
||||
{
|
||||
bodyA = &m_tmpSolverBodyPool[cB.m_solverBodyIdA];
|
||||
deltaVelADotn += cB.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cB.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
|
||||
}
|
||||
|
||||
if (cB.m_multiBodyB)
|
||||
{
|
||||
ndofB = cB.m_multiBodyB->getNumDofs() + 6;
|
||||
for (int i = 0; i < ndofB; ++i)
|
||||
deltaVelBDotn += m_data.m_jacobians[cB.m_jacBindex+i] * m_data.m_deltaVelocities[cB.m_deltaVelBindex+i];
|
||||
} else if(cB.m_solverBodyIdB >= 0)
|
||||
{
|
||||
bodyB = &m_tmpSolverBodyPool[cB.m_solverBodyIdB];
|
||||
deltaVelBDotn += cB.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cB.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
|
||||
}
|
||||
|
||||
|
||||
deltaImpulseB -= deltaVelADotn*cB.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
|
||||
deltaImpulseB -= deltaVelBDotn*cB.m_jacDiagABInv;
|
||||
sumB = btScalar(cB.m_appliedImpulse) + deltaImpulseB;
|
||||
}
|
||||
|
||||
btScalar deltaImpulseA = 0.f;
|
||||
btScalar sumA = 0.f;
|
||||
const btMultiBodySolverConstraint& cA = cA1;
|
||||
{
|
||||
{
|
||||
deltaImpulseA = cA.m_rhs-btScalar(cA.m_appliedImpulse)*cA.m_cfm;
|
||||
btScalar deltaVelADotn=0;
|
||||
btScalar deltaVelBDotn=0;
|
||||
if (cA.m_multiBodyA)
|
||||
{
|
||||
ndofA = cA.m_multiBodyA->getNumDofs() + 6;
|
||||
for (int i = 0; i < ndofA; ++i)
|
||||
deltaVelADotn += m_data.m_jacobians[cA.m_jacAindex+i] * m_data.m_deltaVelocities[cA.m_deltaVelAindex+i];
|
||||
} else if(cA.m_solverBodyIdA >= 0)
|
||||
{
|
||||
bodyA = &m_tmpSolverBodyPool[cA.m_solverBodyIdA];
|
||||
deltaVelADotn += cA.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cA.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
|
||||
}
|
||||
|
||||
if (cA.m_multiBodyB)
|
||||
{
|
||||
ndofB = cA.m_multiBodyB->getNumDofs() + 6;
|
||||
for (int i = 0; i < ndofB; ++i)
|
||||
deltaVelBDotn += m_data.m_jacobians[cA.m_jacBindex+i] * m_data.m_deltaVelocities[cA.m_deltaVelBindex+i];
|
||||
} else if(cA.m_solverBodyIdB >= 0)
|
||||
{
|
||||
bodyB = &m_tmpSolverBodyPool[cA.m_solverBodyIdB];
|
||||
deltaVelBDotn += cA.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cA.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
|
||||
}
|
||||
|
||||
|
||||
deltaImpulseA -= deltaVelADotn*cA.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
|
||||
deltaImpulseA -= deltaVelBDotn*cA.m_jacDiagABInv;
|
||||
sumA = btScalar(cA.m_appliedImpulse) + deltaImpulseA;
|
||||
}
|
||||
}
|
||||
|
||||
if (sumA*sumA+sumB*sumB>=cA.m_lowerLimit*cB.m_lowerLimit)
|
||||
{
|
||||
btScalar angle = btAtan2(sumA,sumB);
|
||||
btScalar sumAclipped = btFabs(cA.m_lowerLimit*btSin(angle));
|
||||
btScalar sumBclipped = btFabs(cB.m_lowerLimit*btCos(angle));
|
||||
|
||||
|
||||
if (sumA < -sumAclipped)
|
||||
{
|
||||
deltaImpulseA = -sumAclipped - cA.m_appliedImpulse;
|
||||
cA.m_appliedImpulse = -sumAclipped;
|
||||
}
|
||||
else if (sumA > sumAclipped)
|
||||
{
|
||||
deltaImpulseA = sumAclipped - cA.m_appliedImpulse;
|
||||
cA.m_appliedImpulse = sumAclipped;
|
||||
}
|
||||
else
|
||||
{
|
||||
cA.m_appliedImpulse = sumA;
|
||||
}
|
||||
|
||||
if (sumB < -sumBclipped)
|
||||
{
|
||||
deltaImpulseB = -sumBclipped - cB.m_appliedImpulse;
|
||||
cB.m_appliedImpulse = -sumBclipped;
|
||||
}
|
||||
else if (sumB > sumBclipped)
|
||||
{
|
||||
deltaImpulseB = sumBclipped - cB.m_appliedImpulse;
|
||||
cB.m_appliedImpulse = sumBclipped;
|
||||
}
|
||||
else
|
||||
{
|
||||
cB.m_appliedImpulse = sumB;
|
||||
}
|
||||
//deltaImpulseA = sumAclipped-cA.m_appliedImpulse;
|
||||
//cA.m_appliedImpulse = sumAclipped;
|
||||
//deltaImpulseB = sumBclipped-cB.m_appliedImpulse;
|
||||
//cB.m_appliedImpulse = sumBclipped;
|
||||
}
|
||||
else
|
||||
{
|
||||
cA.m_appliedImpulse = sumA;
|
||||
cB.m_appliedImpulse = sumB;
|
||||
}
|
||||
|
||||
if (cA.m_multiBodyA)
|
||||
{
|
||||
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex],deltaImpulseA,cA.m_deltaVelAindex,ndofA);
|
||||
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
|
||||
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
|
||||
cA.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex],deltaImpulseA);
|
||||
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||
} else if(cA.m_solverBodyIdA >= 0)
|
||||
{
|
||||
bodyA->internalApplyImpulse(cA.m_contactNormal1*bodyA->internalGetInvMass(),cA.m_angularComponentA,deltaImpulseA);
|
||||
|
||||
}
|
||||
if (cA.m_multiBodyB)
|
||||
{
|
||||
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex],deltaImpulseA,cA.m_deltaVelBindex,ndofB);
|
||||
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
|
||||
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
|
||||
cA.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex],deltaImpulseA);
|
||||
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||
} else if(cA.m_solverBodyIdB >= 0)
|
||||
{
|
||||
bodyB->internalApplyImpulse(cA.m_contactNormal2*bodyB->internalGetInvMass(),cA.m_angularComponentB,deltaImpulseA);
|
||||
}
|
||||
|
||||
if (cB.m_multiBodyA)
|
||||
{
|
||||
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex],deltaImpulseB,cB.m_deltaVelAindex,ndofA);
|
||||
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
|
||||
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
|
||||
cB.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex],deltaImpulseB);
|
||||
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||
} else if(cB.m_solverBodyIdA >= 0)
|
||||
{
|
||||
bodyA->internalApplyImpulse(cB.m_contactNormal1*bodyA->internalGetInvMass(),cB.m_angularComponentA,deltaImpulseB);
|
||||
}
|
||||
if (cB.m_multiBodyB)
|
||||
{
|
||||
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex],deltaImpulseB,cB.m_deltaVelBindex,ndofB);
|
||||
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
|
||||
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
|
||||
cB.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex],deltaImpulseB);
|
||||
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||
} else if(cB.m_solverBodyIdB >= 0)
|
||||
{
|
||||
bodyB->internalApplyImpulse(cB.m_contactNormal2*bodyB->internalGetInvMass(),cB.m_angularComponentB,deltaImpulseB);
|
||||
}
|
||||
|
||||
return deltaImpulseA+deltaImpulseB;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint,
|
||||
@@ -871,7 +1075,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)
|
||||
{
|
||||
BT_PROFILE("addMultiBodyFrictionConstraint");
|
||||
btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
|
||||
btMultiBodySolverConstraint& solverConstraint = m_multiBodyLateralFrictionContactConstraints.expandNonInitializing();
|
||||
solverConstraint.m_orgConstraint = 0;
|
||||
solverConstraint.m_orgDofIndex = -1;
|
||||
|
||||
@@ -908,7 +1112,7 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyTorsionalF
|
||||
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
|
||||
{
|
||||
BT_PROFILE("addMultiBodyRollingFrictionConstraint");
|
||||
btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
|
||||
btMultiBodySolverConstraint& solverConstraint = m_multiBodyTorsionalFrictionContactConstraints.expandNonInitializing();
|
||||
solverConstraint.m_orgConstraint = 0;
|
||||
solverConstraint.m_orgDofIndex = -1;
|
||||
|
||||
@@ -1275,11 +1479,11 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
|
||||
btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[i];
|
||||
writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
|
||||
|
||||
writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],infoGlobal.m_timeStep);
|
||||
writeBackSolverBodyToMultiBody(m_multiBodyLateralFrictionContactConstraints[solverConstraint.m_frictionIndex],infoGlobal.m_timeStep);
|
||||
|
||||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||
{
|
||||
writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],infoGlobal.m_timeStep);
|
||||
writeBackSolverBodyToMultiBody(m_multiBodyLateralFrictionContactConstraints[solverConstraint.m_frictionIndex+1],infoGlobal.m_timeStep);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1300,12 +1504,12 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
|
||||
btManifoldPoint* pt = (btManifoldPoint*) solverConstraint.m_originalContactPoint;
|
||||
btAssert(pt);
|
||||
pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
|
||||
pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
|
||||
pt->m_appliedImpulseLateral1 = m_multiBodyLateralFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
|
||||
|
||||
//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
|
||||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||
{
|
||||
pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse;
|
||||
pt->m_appliedImpulseLateral2 = m_multiBodyLateralFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse;
|
||||
}
|
||||
//do a callback here?
|
||||
}
|
||||
|
||||
@@ -35,7 +35,8 @@ protected:
|
||||
btMultiBodyConstraintArray m_multiBodyNonContactConstraints;
|
||||
|
||||
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints;
|
||||
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints;
|
||||
btMultiBodyConstraintArray m_multiBodyLateralFrictionContactConstraints;
|
||||
btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints;
|
||||
|
||||
btMultiBodyJacobianData m_data;
|
||||
|
||||
@@ -45,6 +46,9 @@ protected:
|
||||
|
||||
btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
|
||||
|
||||
//solve 2 friction directions and clamp against the implicit friction cone
|
||||
btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA, const btMultiBodySolverConstraint& cB);
|
||||
|
||||
|
||||
void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user