1 files changed, 80 insertions, 104 deletions

diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
index 3c0430b903..ad399dc57f 100644
--- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
+++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
@@ -13,217 +13,193 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btPoint2PointConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include <new>
 
-
-
-
-
-btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
-:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
-m_flags(0),
-m_useSolveConstraintObsolete(false)
+btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& pivotInA, const btVector3& pivotInB)
+	: btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE, rbA, rbB), m_pivotInA(pivotInA), m_pivotInB(pivotInB), m_flags(0), m_useSolveConstraintObsolete(false)
 {
-
 }
 
-
-btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
-:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
-m_flags(0),
-m_useSolveConstraintObsolete(false)
+btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA, const btVector3& pivotInA)
+	: btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE, rbA), m_pivotInA(pivotInA), m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), m_flags(0), m_useSolveConstraintObsolete(false)
 {
-	
 }
 
-void	btPoint2PointConstraint::buildJacobian()
+void btPoint2PointConstraint::buildJacobian()
 {
-
 	///we need it for both methods
 	{
 		m_appliedImpulse = btScalar(0.);
 
-		btVector3	normal(0,0,0);
+		btVector3 normal(0, 0, 0);
 
-		for (int i=0;i<3;i++)
+		for (int i = 0; i < 3; i++)
 		{
 			normal[i] = 1;
 			new (&m_jac[i]) btJacobianEntry(
-			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-			m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
-			m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
-			normal,
-			m_rbA.getInvInertiaDiagLocal(),
-			m_rbA.getInvMass(),
-			m_rbB.getInvInertiaDiagLocal(),
-			m_rbB.getInvMass());
-		normal[i] = 0;
+				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+				m_rbA.getCenterOfMassTransform() * m_pivotInA - m_rbA.getCenterOfMassPosition(),
+				m_rbB.getCenterOfMassTransform() * m_pivotInB - m_rbB.getCenterOfMassPosition(),
+				normal,
+				m_rbA.getInvInertiaDiagLocal(),
+				m_rbA.getInvMass(),
+				m_rbB.getInvInertiaDiagLocal(),
+				m_rbB.getInvMass());
+			normal[i] = 0;
 		}
 	}
-
-
 }
 
-void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
+void btPoint2PointConstraint::getInfo1(btConstraintInfo1* info)
 {
 	getInfo1NonVirtual(info);
 }
 
-void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
+void btPoint2PointConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
 {
 	if (m_useSolveConstraintObsolete)
 	{
 		info->m_numConstraintRows = 0;
 		info->nub = 0;
-	} else
+	}
+	else
 	{
 		info->m_numConstraintRows = 3;
 		info->nub = 3;
 	}
 }
 
-
-
-
-void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
+void btPoint2PointConstraint::getInfo2(btConstraintInfo2* info)
 {
-	getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+	getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 }
 
-void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
+void btPoint2PointConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
 {
 	btAssert(!m_useSolveConstraintObsolete);
 
-	 //retrieve matrices
+	//retrieve matrices
 
 	// anchor points in global coordinates with respect to body PORs.
-   
-    // set jacobian
-    info->m_J1linearAxis[0] = 1;
-	info->m_J1linearAxis[info->rowskip+1] = 1;
-	info->m_J1linearAxis[2*info->rowskip+2] = 1;
 
-	btVector3 a1 = body0_trans.getBasis()*getPivotInA();
+	// set jacobian
+	info->m_J1linearAxis[0] = 1;
+	info->m_J1linearAxis[info->rowskip + 1] = 1;
+	info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
+
+	btVector3 a1 = body0_trans.getBasis() * getPivotInA();
 	{
 		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
-		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
-		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
+		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * info->rowskip);
 		btVector3 a1neg = -a1;
-		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
-    
+
 	info->m_J2linearAxis[0] = -1;
-    info->m_J2linearAxis[info->rowskip+1] = -1;
-    info->m_J2linearAxis[2*info->rowskip+2] = -1;
-	
-	btVector3 a2 = body1_trans.getBasis()*getPivotInB();
-   
+	info->m_J2linearAxis[info->rowskip + 1] = -1;
+	info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
+
+	btVector3 a2 = body1_trans.getBasis() * getPivotInB();
+
 	{
-	//	btVector3 a2n = -a2;
+		//	btVector3 a2n = -a2;
 		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
-		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
-		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
-		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * info->rowskip);
+		a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
-    
-
 
-    // set right hand side
+	// set right hand side
 	btScalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp;
-    btScalar k = info->fps * currERP;
-    int j;
-	for (j=0; j<3; j++)
-    {
-        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
+	btScalar k = info->fps * currERP;
+	int j;
+	for (j = 0; j < 3; j++)
+	{
+		info->m_constraintError[j * info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
 		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
-    }
-	if(m_flags & BT_P2P_FLAGS_CFM)
+	}
+	if (m_flags & BT_P2P_FLAGS_CFM)
 	{
-		for (j=0; j<3; j++)
+		for (j = 0; j < 3; j++)
 		{
-			info->cfm[j*info->rowskip] = m_cfm;
+			info->cfm[j * info->rowskip] = m_cfm;
 		}
 	}
 
-	btScalar impulseClamp = m_setting.m_impulseClamp;//
-	for (j=0; j<3; j++)
-    {
+	btScalar impulseClamp = m_setting.m_impulseClamp;  //
+	for (j = 0; j < 3; j++)
+	{
 		if (m_setting.m_impulseClamp > 0)
 		{
-			info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
-			info->m_upperLimit[j*info->rowskip] = impulseClamp;
+			info->m_lowerLimit[j * info->rowskip] = -impulseClamp;
+			info->m_upperLimit[j * info->rowskip] = impulseClamp;
 		}
 	}
 	info->m_damping = m_setting.m_damping;
-	
 }
 
-
-
-void	btPoint2PointConstraint::updateRHS(btScalar	timeStep)
+void btPoint2PointConstraint::updateRHS(btScalar timeStep)
 {
 	(void)timeStep;
-
 }
 
-///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 ///If no axis is provided, it uses the default axis for this constraint.
 void btPoint2PointConstraint::setParam(int num, btScalar value, int axis)
 {
-	if(axis != -1)
+	if (axis != -1)
 	{
 		btAssertConstrParams(0);
 	}
 	else
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_ERP :
-			case BT_CONSTRAINT_STOP_ERP :
-				m_erp = value; 
+			case BT_CONSTRAINT_ERP:
+			case BT_CONSTRAINT_STOP_ERP:
+				m_erp = value;
 				m_flags |= BT_P2P_FLAGS_ERP;
 				break;
-			case BT_CONSTRAINT_CFM :
-			case BT_CONSTRAINT_STOP_CFM :
-				m_cfm = value; 
+			case BT_CONSTRAINT_CFM:
+			case BT_CONSTRAINT_STOP_CFM:
+				m_cfm = value;
 				m_flags |= BT_P2P_FLAGS_CFM;
 				break;
-			default: 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
 }
 
 ///return the local value of parameter
-btScalar btPoint2PointConstraint::getParam(int num, int axis) const 
+btScalar btPoint2PointConstraint::getParam(int num, int axis) const
 {
 	btScalar retVal(SIMD_INFINITY);
-	if(axis != -1)
+	if (axis != -1)
 	{
 		btAssertConstrParams(0);
 	}
 	else
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_ERP :
-			case BT_CONSTRAINT_STOP_ERP :
+			case BT_CONSTRAINT_ERP:
+			case BT_CONSTRAINT_STOP_ERP:
 				btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP);
-				retVal = m_erp; 
+				retVal = m_erp;
 				break;
-			case BT_CONSTRAINT_CFM :
-			case BT_CONSTRAINT_STOP_CFM :
+			case BT_CONSTRAINT_CFM:
+			case BT_CONSTRAINT_STOP_CFM:
 				btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM);
-				retVal = m_cfm; 
+				retVal = m_cfm;
 				break;
-			default: 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
 	return retVal;
 }
-