summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp')
-rw-r--r--thirdparty/bullet/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp170
1 files changed, 77 insertions, 93 deletions
diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
index 0c7dbd668b..1ea20edcb2 100644
--- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
+++ b/thirdparty/bullet/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
}
*/
-
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-07 13:08:50 +0000