Update to bullet master (2.90)

3 files changed, 18 insertions, 66 deletions

diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp
index bdaa473476..a1d5bb9ca8 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -583,52 +583,6 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
 	}
 }
 
-btScalar btMultiBody::getKineticEnergy() const
-{
-	int num_links = getNumLinks();
-	// TODO: would be better not to allocate memory here
-	btAlignedObjectArray<btVector3> omega;
-	omega.resize(num_links + 1);
-	btAlignedObjectArray<btVector3> vel;
-	vel.resize(num_links + 1);
-	compTreeLinkVelocities(&omega[0], &vel[0]);
-
-	// we will do the factor of 0.5 at the end
-	btScalar result = m_baseMass * vel[0].dot(vel[0]);
-	result += omega[0].dot(m_baseInertia * omega[0]);
-
-	for (int i = 0; i < num_links; ++i)
-	{
-		result += m_links[i].m_mass * vel[i + 1].dot(vel[i + 1]);
-		result += omega[i + 1].dot(m_links[i].m_inertiaLocal * omega[i + 1]);
-	}
-
-	return 0.5f * result;
-}
-
-btVector3 btMultiBody::getAngularMomentum() const
-{
-	int num_links = getNumLinks();
-	// TODO: would be better not to allocate memory here
-	btAlignedObjectArray<btVector3> omega;
-	omega.resize(num_links + 1);
-	btAlignedObjectArray<btVector3> vel;
-	vel.resize(num_links + 1);
-	btAlignedObjectArray<btQuaternion> rot_from_world;
-	rot_from_world.resize(num_links + 1);
-	compTreeLinkVelocities(&omega[0], &vel[0]);
-
-	rot_from_world[0] = m_baseQuat;
-	btVector3 result = quatRotate(rot_from_world[0].inverse(), (m_baseInertia * omega[0]));
-
-	for (int i = 0; i < num_links; ++i)
-	{
-		rot_from_world[i + 1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent + 1];
-		result += (quatRotate(rot_from_world[i + 1].inverse(), (m_links[i].m_inertiaLocal * omega[i + 1])));
-	}
-
-	return result;
-}
 
 void btMultiBody::clearConstraintForces()
 {
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h
index afed669a7b..be795633fd 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h
@@ -307,13 +307,6 @@ public:
 	//
 	btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const;
 
-	//
-	// calculate kinetic energy and angular momentum
-	// useful for debugging.
-	//
-
-	btScalar getKineticEnergy() const;
-	btVector3 getAngularMomentum() const;
 
 	//
 	// set external forces and torques. Note all external forces/torques are given in the WORLD frame.
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
index ffae5300f0..2788367431 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
@@ -30,23 +30,28 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 	btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
 	//solve featherstone non-contact constraints
-
+	btScalar nonContactResidual = 0;
 	//printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
-
-	for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++)
+	for (int i = 0; i < infoGlobal.m_numNonContactInnerIterations; ++i)
 	{
-		int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j;
+		// reset the nonContactResdual to 0 at start of each inner iteration
+		nonContactResidual = 0;
+		for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++)
+		{
+			int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j;
 
-		btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
+			btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
 
-		btScalar residual = resolveSingleConstraintRowGeneric(constraint);
-		leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
+			btScalar residual = resolveSingleConstraintRowGeneric(constraint);
+			nonContactResidual = btMax(nonContactResidual, residual * residual);
 
-		if (constraint.m_multiBodyA)
-			constraint.m_multiBodyA->setPosUpdated(false);
-		if (constraint.m_multiBodyB)
-			constraint.m_multiBodyB->setPosUpdated(false);
+			if (constraint.m_multiBodyA)
+				constraint.m_multiBodyA->setPosUpdated(false);
+			if (constraint.m_multiBodyB)
+				constraint.m_multiBodyB->setPosUpdated(false);
+		}
 	}
+	leastSquaredResidual = btMax(leastSquaredResidual, nonContactResidual);
 
 	//solve featherstone normal contact
 	for (int j0 = 0; j0 < m_multiBodyNormalContactConstraints.size(); j0++)
@@ -1250,7 +1255,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 {
 	const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
 	const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
-
+	
 	btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
 	btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
 
@@ -1270,7 +1275,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 	//	return;
 
 	//only a single rollingFriction per manifold
-	int rollingFriction = 1;
+	int rollingFriction = 4;
 
 	for (int j = 0; j < manifold->getNumContacts(); j++)
 	{