Update to bullet master (2.90)

11 files changed, 100 insertions, 80 deletions

diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
index e82d1b139e..4356c12abf 100644
--- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
+++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
@@ -46,7 +46,7 @@ struct btContactSolverInfoData
 	btScalar m_sor;          //successive over-relaxation term
 	btScalar m_erp;          //error reduction for non-contact constraints
 	btScalar m_erp2;         //error reduction for contact constraints
-    btScalar m_deformable_erp;          //error reduction for deformable constraints
+	btScalar m_deformable_erp;          //error reduction for deformable constraints
 	btScalar m_globalCfm;    //constraint force mixing for contacts and non-contacts
 	btScalar m_frictionERP;  //error reduction for friction constraints
 	btScalar m_frictionCFM;  //constraint force mixing for friction constraints
@@ -67,6 +67,7 @@ struct btContactSolverInfoData
 	bool m_jointFeedbackInWorldSpace;
 	bool m_jointFeedbackInJointFrame;
 	int m_reportSolverAnalytics;
+	int m_numNonContactInnerIterations;
 };
 
 struct btContactSolverInfo : public btContactSolverInfoData
@@ -82,7 +83,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_numIterations = 10;
 		m_erp = btScalar(0.2);
 		m_erp2 = btScalar(0.2);
-        m_deformable_erp = btScalar(0.);
+		m_deformable_erp = btScalar(0.1);
 		m_globalCfm = btScalar(0.);
 		m_frictionERP = btScalar(0.2);  //positional friction 'anchors' are disabled by default
 		m_frictionCFM = btScalar(0.);
@@ -104,6 +105,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_jointFeedbackInWorldSpace = false;
 		m_jointFeedbackInJointFrame = false;
 		m_reportSolverAnalytics = 0;
+		m_numNonContactInnerIterations = 1;   // the number of inner iterations for solving motor constraint in a single iteration of the constraint solve
 	}
 };
 
diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
index 93626f18ff..74a13c6249 100644
--- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
+++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
@@ -876,7 +876,10 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 		// will we not request a velocity with the wrong direction ?
 		// and the answer is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
 		// so the sign of the force that is really matters
-		info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
+		if (m_flags & BT_6DOF_FLAGS_USE_INFINITE_ERROR)
+			info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
+		else
+			info->m_constraintError[srow] = vel + f / m * (rotational ? -1 : 1);
 
 		btScalar minf = f < fd ? f : fd;
 		btScalar maxf = f < fd ? fd : f;
diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
index 00e24364e0..c86dc373da 100644
--- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
+++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
@@ -265,6 +265,7 @@ enum bt6DofFlags2
 	BT_6DOF_FLAGS_ERP_STOP2 = 2,
 	BT_6DOF_FLAGS_CFM_MOTO2 = 4,
 	BT_6DOF_FLAGS_ERP_MOTO2 = 8,
+	BT_6DOF_FLAGS_USE_INFINITE_ERROR = (1<<16)
 };
 #define BT_6DOF_FLAGS_AXIS_SHIFT2 4  // bits per axis
 
diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index e4da468299..d2641c582f 100644
--- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -14,7 +14,9 @@ subject to the following restrictions:
 */
 
 //#define COMPUTE_IMPULSE_DENOM 1
-//#define BT_ADDITIONAL_DEBUG
+#ifdef BT_DEBUG
+#	define BT_ADDITIONAL_DEBUG
+#endif
 
 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
 
@@ -690,8 +692,10 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 {
 #if BT_THREADSAFE
 	int solverBodyId = -1;
-	bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
-	if (isRigidBodyType && !body.isStaticOrKinematicObject())
+	const bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
+	const bool isStaticOrKinematic = body.isStaticOrKinematicObject();
+	const bool isKinematic = body.isKinematicObject();
+	if (isRigidBodyType && !isStaticOrKinematic)
 	{
 		// dynamic body
 		// Dynamic bodies can only be in one island, so it's safe to write to the companionId
@@ -704,7 +708,7 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 			body.setCompanionId(solverBodyId);
 		}
 	}
-	else if (isRigidBodyType && body.isKinematicObject())
+	else if (isRigidBodyType && isKinematic)
 	{
 		//
 		// NOTE: must test for kinematic before static because some kinematic objects also
diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
index a3c9f42eb9..fb15ae31eb 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
@@ -800,6 +800,14 @@ public:
 		///don't do CCD when the collision filters are not matching
 		if (!ClosestConvexResultCallback::needsCollision(proxy0))
 			return false;
+		if (m_pairCache->getOverlapFilterCallback()) {
+			btBroadphaseProxy* proxy1 = m_me->getBroadphaseHandle();
+			bool collides = m_pairCache->needsBroadphaseCollision(proxy0, proxy1);
+			if (!collides)
+			{
+				return false;
+			}
+		}
 
 		btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject;
 
diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp
index 9e8705b001..27fdead761 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp
+++ b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp
@@ -136,8 +136,13 @@ void btRigidBody::setGravity(const btVector3& acceleration)
 
 void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
 {
-	m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
-	m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+#ifdef BT_USE_OLD_DAMPING_METHOD
+	m_linearDamping = btMax(lin_damping, btScalar(0.0));
+	m_angularDamping = btMax(ang_damping, btScalar(0.0));
+#else
+	m_linearDamping = btClamped(lin_damping, btScalar(0.0), btScalar(1.0));
+	m_angularDamping = btClamped(ang_damping, btScalar(0.0), btScalar(1.0));
+#endif
 }
 
 ///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
@@ -146,10 +151,9 @@ void btRigidBody::applyDamping(btScalar timeStep)
 	//On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
 	//todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
 
-//#define USE_OLD_DAMPING_METHOD 1
-#ifdef USE_OLD_DAMPING_METHOD
-	m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
-	m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+#ifdef BT_USE_OLD_DAMPING_METHOD
+	m_linearVelocity *= btMax((btScalar(1.0) - timeStep * m_linearDamping), btScalar(0.0));
+	m_angularVelocity *= btMax((btScalar(1.0) - timeStep * m_angularDamping), btScalar(0.0));
 #else
 	m_linearVelocity *= btPow(btScalar(1) - m_linearDamping, timeStep);
 	m_angularVelocity *= btPow(btScalar(1) - m_angularDamping, timeStep);
@@ -380,6 +384,9 @@ void btRigidBody::integrateVelocities(btScalar step)
 	{
 		m_angularVelocity *= (MAX_ANGVEL / step) / angvel;
 	}
+	#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+	clampVelocity(m_angularVelocity);
+	#endif
 }
 
 btQuaternion btRigidBody::getOrientation() const
diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h
index 39d47cbbda..943d724cce 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h
+++ b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h
@@ -305,6 +305,9 @@ public:
 	void applyTorque(const btVector3& torque)
 	{
 		m_totalTorque += torque * m_angularFactor;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_totalTorque);
+		#endif
 	}
 
 	void applyForce(const btVector3& force, const btVector3& rel_pos)
@@ -316,11 +319,17 @@ public:
 	void applyCentralImpulse(const btVector3& impulse)
 	{
 		m_linearVelocity += impulse * m_linearFactor * m_inverseMass;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_linearVelocity);
+		#endif
 	}
 
 	void applyTorqueImpulse(const btVector3& torque)
 	{
 		m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_angularVelocity);
+		#endif
 	}
 
 	void applyImpulse(const btVector3& impulse, const btVector3& rel_pos)
@@ -361,20 +370,46 @@ public:
     {
         m_pushVelocity = v;
     }
-    
+
+    #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+    void clampVelocity(btVector3& v) const {
+        v.setX(
+            fmax(-BT_CLAMP_VELOCITY_TO,
+                 fmin(BT_CLAMP_VELOCITY_TO, v.getX()))
+        );
+        v.setY(
+            fmax(-BT_CLAMP_VELOCITY_TO,
+                 fmin(BT_CLAMP_VELOCITY_TO, v.getY()))
+        );
+        v.setZ(
+            fmax(-BT_CLAMP_VELOCITY_TO,
+                 fmin(BT_CLAMP_VELOCITY_TO, v.getZ()))
+        );
+    }
+    #endif
+
     void setTurnVelocity(const btVector3& v)
     {
         m_turnVelocity = v;
+        #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+        clampVelocity(m_turnVelocity);
+        #endif
     }
     
     void applyCentralPushImpulse(const btVector3& impulse)
     {
         m_pushVelocity += impulse * m_linearFactor * m_inverseMass;
+        #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+        clampVelocity(m_pushVelocity);
+        #endif
     }
     
     void applyTorqueTurnImpulse(const btVector3& torque)
     {
         m_turnVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
+        #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+        clampVelocity(m_turnVelocity);
+        #endif
     }
 
 	void clearForces()
@@ -408,12 +443,18 @@ public:
 	{
 		m_updateRevision++;
 		m_linearVelocity = lin_vel;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_linearVelocity);
+		#endif
 	}
 
 	inline void setAngularVelocity(const btVector3& ang_vel)
 	{
 		m_updateRevision++;
 		m_angularVelocity = ang_vel;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_angularVelocity);
+		#endif
 	}
 
 	btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const
diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp
index 5353fe009e..772b774202 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp
+++ b/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp
@@ -171,6 +171,8 @@ void btSimulationIslandManagerMt::initIslandPools()
 
 btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::getIsland(int id)
 {
+	btAssert(id >= 0);
+	btAssert(id < m_lookupIslandFromId.size());
 	Island* island = m_lookupIslandFromId[id];
 	if (island == NULL)
 	{
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++)
 	{