Merge pull request #24740 from OBKF/update-bullet-physics

Update Bullet physics to commit 126b676

1 files changed, 40 insertions, 54 deletions

diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
index 35c929f7ce..8791ad2868 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
@@ -20,21 +20,18 @@ subject to the following restrictions:
 #include "btMultiBodyLinkCollider.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
-
-
 btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
 	//:btMultiBodyConstraint(body,0,link,-1,2,true),
-	:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,2,true),
-	m_lowerBound(lower),
-	m_upperBound(upper)
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true),
+	  m_lowerBound(lower),
+	  m_upperBound(upper)
 {
-
 }
 
 void btMultiBodyJointLimitConstraint::finalizeMultiDof()
 {
 	// the data.m_jacobians never change, so may as well
-    // initialize them here
+	// initialize them here
 
 	allocateJacobiansMultiDof();
 
@@ -53,10 +50,8 @@ btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
 {
 }
 
-
 int btMultiBodyJointLimitConstraint::getIslandIdA() const
 {
-
 	if (m_bodyA)
 	{
 		if (m_linkA < 0)
@@ -93,72 +88,69 @@ int btMultiBodyJointLimitConstraint::getIslandIdB() const
 	return -1;
 }
 
-
 void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal)
+														   btMultiBodyJacobianData& data,
+														   const btContactSolverInfo& infoGlobal)
 {
-	
-    // only positions need to be updated -- data.m_jacobians and force
-    // directions were set in the ctor and never change.
+	// only positions need to be updated -- data.m_jacobians and force
+	// directions were set in the ctor and never change.
 
 	if (m_numDofsFinalized != m_jacSizeBoth)
 	{
-        finalizeMultiDof();
+		finalizeMultiDof();
 	}
 
+	// row 0: the lower bound
+	setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound);  //multidof: this is joint-type dependent
 
-    // row 0: the lower bound
-    setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound);			//multidof: this is joint-type dependent
+	// row 1: the upper bound
+	setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
 
-    // row 1: the upper bound
-    setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
-	
-	for (int row=0;row<getNumRows();row++)
+	for (int row = 0; row < getNumRows(); row++)
 	{
 		btScalar penetration = getPosition(row);
 
 		//todo: consider adding some safety threshold here
-		if (penetration>0)
+		if (penetration > 0)
 		{
 			continue;
 		}
-		btScalar direction = row? -1 : 1;
+		btScalar direction = row ? -1 : 1;
 
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
-        constraintRow.m_orgConstraint = this;
-        constraintRow.m_orgDofIndex = row;
-        
+		constraintRow.m_orgConstraint = this;
+		constraintRow.m_orgDofIndex = row;
+
 		constraintRow.m_multiBodyA = m_bodyA;
 		constraintRow.m_multiBodyB = m_bodyB;
-		const btScalar posError = 0;						//why assume it's zero?
+		const btScalar posError = 0;  //why assume it's zero?
 		const btVector3 dummy(0, 0, 0);
 
-		btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
+		btScalar rel_vel = fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, 0, m_maxAppliedImpulse);
 
 		{
 			//expect either prismatic or revolute joint type for now
-			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
+			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
 			switch (m_bodyA->getLink(m_linkA).m_jointType)
 			{
 				case btMultibodyLink::eRevolute:
 				{
 					constraintRow.m_contactNormal1.setZero();
 					constraintRow.m_contactNormal2.setZero();
-					btVector3 revoluteAxisInWorld = direction*quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
-					constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
-					constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
-					
+					btVector3 revoluteAxisInWorld = direction * quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
+					constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
+					constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
+
 					break;
 				}
 				case btMultibodyLink::ePrismatic:
 				{
-					btVector3 prismaticAxisInWorld = direction* quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
-					constraintRow.m_contactNormal1=prismaticAxisInWorld;
-					constraintRow.m_contactNormal2=-prismaticAxisInWorld;
+					btVector3 prismaticAxisInWorld = direction * quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
+					constraintRow.m_contactNormal1 = prismaticAxisInWorld;
+					constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
 					constraintRow.m_relpos1CrossNormal.setZero();
 					constraintRow.m_relpos2CrossNormal.setZero();
-					
+
 					break;
 				}
 				default:
@@ -166,36 +158,35 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
 					btAssert(0);
 				}
 			};
-			
 		}
 
 		{
-			
 			btScalar positionalError = 0.f;
-			btScalar	velocityError =  - rel_vel;// * damping;
+			btScalar velocityError = -rel_vel;  // * damping;
 			btScalar erp = infoGlobal.m_erp2;
 			if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
 			{
 				erp = infoGlobal.m_erp;
 			}
-			if (penetration>0)
+			if (penetration > 0)
 			{
 				positionalError = 0;
 				velocityError = -penetration / infoGlobal.m_timeStep;
-			} else
+			}
+			else
 			{
-				positionalError = -penetration * erp/infoGlobal.m_timeStep;
+				positionalError = -penetration * erp / infoGlobal.m_timeStep;
 			}
 
-			btScalar  penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
-			btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
+			btScalar penetrationImpulse = positionalError * constraintRow.m_jacDiagABInv;
+			btScalar velocityImpulse = velocityError * constraintRow.m_jacDiagABInv;
 			if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
 			{
 				//combine position and velocity into rhs
-				constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
+				constraintRow.m_rhs = penetrationImpulse + velocityImpulse;
 				constraintRow.m_rhsPenetration = 0.f;
-
-			} else
+			}
+			else
 			{
 				//split position and velocity into rhs and m_rhsPenetration
 				constraintRow.m_rhs = velocityImpulse;
@@ -203,9 +194,4 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
 			}
 		}
 	}
-
 }
-
-
-
-