Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added LCP clamp for linear limit on the generic D6 joint

Browse Source
This commit is contained in:
ejcoumans
2006-11-07 22:11:15 +00:00
parent 2a1546764e
commit db65601f9a
1 changed files with 13 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
View File

@@ -161,7 +161,9 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep)
//positional error (zeroth order error) //positional error (zeroth order error)
btScalar depth = -(pivotAInW - pivotBInW).dot(normalWorld); btScalar depth = -(pivotAInW - pivotBInW).dot(normalWorld);
btScalar lo = -1e30f;
btScalar hi = 1e30f;
//handle the limits //handle the limits
if (m_lowerLimit[i] < m_upperLimit[i]) if (m_lowerLimit[i] < m_upperLimit[i])
{ {
@@ -169,11 +171,14 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep)
if (depth > m_upperLimit[i]) if (depth > m_upperLimit[i])
{ {
depth -= m_upperLimit[i]; depth -= m_upperLimit[i];
lo = 0.f;
} else } else
{ {
if (depth < m_lowerLimit[i]) if (depth < m_lowerLimit[i])
{ {
depth -= m_lowerLimit[i]; depth -= m_lowerLimit[i];
hi = 0.f;
} else } else
{ {
continue; continue;
@@ -181,12 +186,14 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep)
} }
} }
} }
btScalar impulse = (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv;
m_accumulatedImpulse[i] += impulse;
btVector3 impulse_vector = normalWorld * impulse; btScalar normalImpulse= (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv;
float oldNormalImpulse = m_accumulatedImpulse[i];
float sum = oldNormalImpulse + normalImpulse;
m_accumulatedImpulse[i] = sum > hi ? 0.f : sum < lo ? 0.f : sum;
normalImpulse = m_accumulatedImpulse[i] - oldNormalImpulse;
btVector3 impulse_vector = normalWorld * normalImpulse;
m_rbA.applyImpulse( impulse_vector, rel_pos1); m_rbA.applyImpulse( impulse_vector, rel_pos1);
m_rbB.applyImpulse(-impulse_vector, rel_pos2); m_rbB.applyImpulse(-impulse_vector, rel_pos2);