diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp | 170 |
1 files changed, 77 insertions, 93 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp index 0c7dbd668bb..1ea20edcb2b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp @@ -13,43 +13,38 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #include "btSolve2LinearConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btVector3.h" #include "btJacobianEntry.h" - void btSolve2LinearConstraint::resolveUnilateralPairConstraint( - btRigidBody* body1, - btRigidBody* body2, - - const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - - const btVector3& invInertiaADiag, - const btScalar invMassA, - const btVector3& linvelA,const btVector3& angvelA, - const btVector3& rel_posA1, - const btVector3& invInertiaBDiag, - const btScalar invMassB, - const btVector3& linvelB,const btVector3& angvelB, - const btVector3& rel_posA2, - - btScalar depthA, const btVector3& normalA, - const btVector3& rel_posB1,const btVector3& rel_posB2, - btScalar depthB, const btVector3& normalB, - btScalar& imp0,btScalar& imp1) + btRigidBody* body1, + btRigidBody* body2, + + const btMatrix3x3& world2A, + const btMatrix3x3& world2B, + + const btVector3& invInertiaADiag, + const btScalar invMassA, + const btVector3& linvelA, const btVector3& angvelA, + const btVector3& rel_posA1, + const btVector3& invInertiaBDiag, + const btScalar invMassB, + const btVector3& linvelB, const btVector3& angvelB, + const btVector3& rel_posA2, + + btScalar depthA, const btVector3& normalA, + const btVector3& rel_posB1, const btVector3& rel_posB2, + btScalar depthB, const btVector3& normalB, + btScalar& imp0, btScalar& imp1) { (void)linvelA; (void)linvelB; (void)angvelB; (void)angvelA; - - imp0 = btScalar(0.); imp1 = btScalar(0.); @@ -59,86 +54,76 @@ void btSolve2LinearConstraint::resolveUnilateralPairConstraint( btAssert(len < SIMD_EPSILON); - //this jacobian entry could be re-used for all iterations - btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, - invInertiaBDiag,invMassB); - btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, - invInertiaBDiag,invMassB); - + btJacobianEntry jacA(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, + invInertiaBDiag, invMassB); + btJacobianEntry jacB(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, + invInertiaBDiag, invMassB); + //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); - const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1)); - const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1)); + const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1) - body2->getVelocityInLocalPoint(rel_posA1)); + const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1) - body2->getVelocityInLocalPoint(rel_posB1)); -// btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv + // btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv btScalar massTerm = btScalar(1.) / (invMassA + invMassB); - // calculate rhs (or error) terms - const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping; - const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping; - + const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping; + const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping; // dC/dv * dv = -C - + // jacobian * impulse = -error // //impulse = jacobianInverse * -error // inverting 2x2 symmetric system (offdiagonal are equal!) - // + // + btScalar nonDiag = jacA.getNonDiagonal(jacB, invMassA, invMassB); + btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag); - btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); - btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); - //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; - imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; + imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * -nonDiag * invDet; //[a b] [d -c] //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc) //[jA nD] * [imp0] = [dv0] //[nD jB] [imp1] [dv1] - } - - void btSolve2LinearConstraint::resolveBilateralPairConstraint( - btRigidBody* body1, - btRigidBody* body2, - const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - - const btVector3& invInertiaADiag, - const btScalar invMassA, - const btVector3& linvelA,const btVector3& angvelA, - const btVector3& rel_posA1, - const btVector3& invInertiaBDiag, - const btScalar invMassB, - const btVector3& linvelB,const btVector3& angvelB, - const btVector3& rel_posA2, - - btScalar depthA, const btVector3& normalA, - const btVector3& rel_posB1,const btVector3& rel_posB2, - btScalar depthB, const btVector3& normalB, - btScalar& imp0,btScalar& imp1) + btRigidBody* body1, + btRigidBody* body2, + const btMatrix3x3& world2A, + const btMatrix3x3& world2B, + + const btVector3& invInertiaADiag, + const btScalar invMassA, + const btVector3& linvelA, const btVector3& angvelA, + const btVector3& rel_posA1, + const btVector3& invInertiaBDiag, + const btScalar invMassB, + const btVector3& linvelB, const btVector3& angvelB, + const btVector3& rel_posA2, + + btScalar depthA, const btVector3& normalA, + const btVector3& rel_posB1, const btVector3& rel_posB2, + btScalar depthB, const btVector3& normalB, + btScalar& imp0, btScalar& imp1) { - (void)linvelA; (void)linvelB; (void)angvelA; (void)angvelB; - - imp0 = btScalar(0.); imp1 = btScalar(0.); @@ -148,42 +133,40 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( btAssert(len < SIMD_EPSILON); - //this jacobian entry could be re-used for all iterations - btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, - invInertiaBDiag,invMassB); - btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, - invInertiaBDiag,invMassB); - + btJacobianEntry jacA(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, + invInertiaBDiag, invMassB); + btJacobianEntry jacB(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, + invInertiaBDiag, invMassB); + //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); - const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1)); - const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1)); + const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1) - body2->getVelocityInLocalPoint(rel_posA1)); + const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1) - body2->getVelocityInLocalPoint(rel_posB1)); // calculate rhs (or error) terms - const btScalar dv0 = depthA * m_tau - vel0 * m_damping; - const btScalar dv1 = depthB * m_tau - vel1 * m_damping; + const btScalar dv0 = depthA * m_tau - vel0 * m_damping; + const btScalar dv1 = depthB * m_tau - vel1 * m_damping; // dC/dv * dv = -C - + // jacobian * impulse = -error // //impulse = jacobianInverse * -error // inverting 2x2 symmetric system (offdiagonal are equal!) - // + // + btScalar nonDiag = jacA.getNonDiagonal(jacB, invMassA, invMassB); + btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag); - btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); - btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); - //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; - imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; + imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * -nonDiag * invDet; //[a b] [d -c] //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc) @@ -191,9 +174,9 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( //[jA nD] * [imp0] = [dv0] //[nD jB] [imp1] [dv1] - if ( imp0 > btScalar(0.0)) + if (imp0 > btScalar(0.0)) { - if ( imp1 > btScalar(0.0) ) + if (imp1 > btScalar(0.0)) { //both positive } @@ -203,9 +186,10 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( // now imp0>0 imp1<0 imp0 = dv0 / jacA.getDiagonal(); - if ( imp0 > btScalar(0.0) ) + if (imp0 > btScalar(0.0)) { - } else + } + else { imp0 = btScalar(0.); } @@ -216,24 +200,25 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint( imp0 = btScalar(0.); imp1 = dv1 / jacB.getDiagonal(); - if ( imp1 <= btScalar(0.0) ) + if (imp1 <= btScalar(0.0)) { imp1 = btScalar(0.); // now imp0>0 imp1<0 imp0 = dv0 / jacA.getDiagonal(); - if ( imp0 > btScalar(0.0) ) + if (imp0 > btScalar(0.0)) { - } else + } + else { imp0 = btScalar(0.); } - } else + } + else { } } } - /* void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invInertiaAWS, const btScalar invMassA, @@ -252,4 +237,3 @@ void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invI } */ - |