remove Cooper implicit (it was just for comparison, it is buggy)

add btMatrix3x3::solve33, thanks to Erin Catto, and added safety against division by zero

Demos/GyroscopicDemo/GyroscopicSetup.cpp

@@ -1,19 +1,17 @@
 #include "GyroscopicSetup.h"
 
-static int gyroflags[5] = {
+static int gyroflags[4] = {
     0,//none, no gyroscopic term
     BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT,
     BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_EWERT,
-    BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_CATTO,
+    BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_CATTO
-    BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_COOPER,
 };
 
-static const char* gyroNames[5] = {
+static const char* gyroNames[4] = {
     "No Coriolis",
     "Explicit",
-    "Ewert",
+    "Implicit",
-    "Catto",
+    "Local Implicit"
-    "Cooper",
 };
 
 
@@ -25,16 +23,15 @@ void GyroscopicSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 	gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
 
 	
-	btVector3 positions[5] = {
+	btVector3 positions[4] = {
 		btVector3( -10, 8,4),
 		btVector3( -5, 8,4),
 		btVector3( 0, 8,4),
 		btVector3( 5, 8,4),
-		btVector3( 10, 8,4),
 	};
 
 
-	for (int i = 0; i<5; i++)
+	for (int i = 0; i<4; i++)
 	{
 		btCylinderShapeZ* pin = new btCylinderShapeZ(btVector3(0.1,0.1, 0.2));
 		btBoxShape* box = new btBoxShape(btVector3(1,0.1,0.1));

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -1271,7 +1271,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 			btVector3 gyroForce (0,0,0);
 			if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
 			{
-				gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce);
+				gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
 				solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
 			}
 			if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_EWERT)
@@ -1279,11 +1279,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 				gyroForce = body->computeGyroscopicImpulseImplicit_Ewert(infoGlobal.m_timeStep);
 				solverBody.m_externalTorqueImpulse += gyroForce;
 			}
-			if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_COOPER)
-			{
-				gyroForce = body->computeGyroscopicImpulseImplicit_Cooper(infoGlobal.m_timeStep);
-				solverBody.m_externalTorqueImpulse += gyroForce;
-			}
 			if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_CATTO)
 			{
 				gyroForce = body->computeGyroscopicImpulseImplicit_Catto(infoGlobal.m_timeStep);

src/BulletDynamics/Dynamics/btRigidBody.cpp

@@ -289,7 +289,7 @@ inline btMatrix3x3 evalEulerEqnDeriv(const btVector3& w1, const btVector3& w0, c
 	return dfw1;
 }
 
-btVector3 btRigidBody::computeGyroscopicForce(btScalar maxGyroscopicForce) const
+btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const
 {
 	btVector3 inertiaLocal = getLocalInertia();
 	btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
@@ -338,8 +338,9 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Catto(btScalar step) con
 	// Jacobian
 	btMatrix3x3 J = Ib +  (skew0*Ib - skew1)*step;
 	
-	btMatrix3x3 Jinv = J.inverse();
+//	btMatrix3x3 Jinv = J.inverse();
-	btVector3 omega_div = Jinv*f;
+//	btVector3 omega_div = Jinv*f;
+	btVector3 omega_div = J.solve33(f);
 	
 	// Single Newton-Raphson update
 	omegab = omegab - omega_div;//Solve33(J, f);
@@ -351,93 +352,6 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Catto(btScalar step) con
 
 
 
-btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Cooper(btScalar step) const
-{
-#if 0
-	dReal h = callContext->m_stepperCallContext->m_stepSize; // Step size
-	dVector3 L; // Compute angular momentum
-	dMultiply0_331(L, I, b->avel);
-#endif
-
-	btVector3 inertiaLocal = getLocalInertia();
-	btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
-	btVector3 L = inertiaTensorWorld*getAngularVelocity();
-
-	btMatrix3x3 Itild(0, 0, 0, 0, 0, 0, 0, 0, 0);
-
-#if 0
-	for (int ii = 0; ii<12; ++ii) {
-		Itild[ii] = Itild[ii] * h + I[ii];
-	}
-#endif
-
-	btSetCrossMatrixMinus(Itild, L*step);
-	Itild += inertiaTensorWorld;
-	
-
-#if 0
-// Compute a new effective 'inertia tensor'
-// for the implicit step: the cross-product 
-// matrix of the angular momentum plus the
-// old tensor scaled by the timestep.  
-// Itild may not be symmetric pos-definite, 
-// but we can still use it to compute implicit
-// gyroscopic torques.
-dMatrix3 Itild = { 0 };
-dSetCrossMatrixMinus(Itild, L, 4);
-for (int ii = 0; ii<12; ++ii) {
-	Itild[ii] = Itild[ii] * h + I[ii];
-}
-#endif
-
-	L *= step;
-	//Itild may not be symmetric pos-definite
-	btMatrix3x3 itInv = Itild.inverse();
-	Itild =  inertiaTensorWorld * itInv;
-	btMatrix3x3 ident(1,0,0,0,1,0,0,0,1);
-	Itild -= ident;
-
-	
-
-
-#if 0
-// Scale momentum by inverse time to get 
-// a sort of "torque"
-dScaleVector3(L, dRecip(h));
-// Invert the pseudo-tensor
-dMatrix3 itInv;
-// This is a closed-form inversion.
-// It's probably not numerically stable
-// when dealing with small masses with
-// a large asymmetry.
-// An LU decomposition might be better.
-if (dInvertMatrix3(itInv, Itild) != 0) {
-	// "Divide" the original tensor
-	// by the pseudo-tensor (on the right)
-	dMultiply0_333(Itild, I, itInv);
-	// Subtract an identity matrix
-	Itild[0] -= 1; Itild[5] -= 1; Itild[10] -= 1;
-
-	// This new inertia matrix rotates the 
-	// momentum to get a new set of torques
-	// that will work correctly when applied
-	// to the old inertia matrix as explicit
-	// torques with a semi-implicit update
-	// step.
-	dVector3 tau0;
-	dMultiply0_331(tau0, Itild, L);
-
-	// Add the gyro torques to the torque 
-	// accumulator
-	for (int ii = 0; ii<3; ++ii) {
-		b->tacc[ii] += tau0[ii];
-	}
-#endif
-	btVector3 tau0 = Itild * L;
-//	printf("tau0 = %f,%f,%f\n",tau0.x(),tau0.y(),tau0.z());
-	return tau0;
-}
-
 btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Ewert(btScalar step) const
 {
 	// use full newton-euler equations.  common practice to drop the wxIw term. want it for better tumbling behavior.
@@ -462,10 +376,10 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Ewert(btScalar step) con
 		const btVector3 fw = evalEulerEqn(w1, w0, btVector3(0, 0, 0), step, I);
 		const btMatrix3x3 dfw = evalEulerEqnDeriv(w1, w0, step, I);
 
-		const btMatrix3x3 dfw_inv = dfw.inverse();
 		btVector3 dw;
-
+		dw = dfw.solve33(fw);
-		dw = dfw_inv*fw;
+		//const btMatrix3x3 dfw_inv = dfw.inverse();
+		//dw = dfw_inv*fw;
 
 		w1 -= dw;
 	}

src/BulletDynamics/Dynamics/btRigidBody.h

@@ -44,9 +44,8 @@ enum	btRigidBodyFlags
 	///BT_ENABLE_GYROPSCOPIC_FORCE flags is enabled by default in Bullet 2.83 and onwards.
 	///See Demos/GyroscopicDemo and computeGyroscopicImpulseImplicit
 	BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT = 2,
-	BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_COOPER=4,
+	BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_EWERT=4,
-	BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_EWERT=8,
+	BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_CATTO=8,
-	BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_CATTO=16,
 };
 
 
@@ -535,10 +534,10 @@ public:
 	
 
 	btVector3 computeGyroscopicImpulseImplicit_Ewert(btScalar dt) const;
-	btVector3 computeGyroscopicImpulseImplicit_Cooper(btScalar step) const;
 	btVector3 computeGyroscopicImpulseImplicit_Catto(btScalar step) const;
 
-	btVector3 computeGyroscopicForce(btScalar maxGyroscopicForce) const;//explicit version is best avoided, it gains energy
+	///explicit version is best avoided, it gains energy
+	btVector3 computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const;
 	btVector3 getLocalInertia() const;
 
 	///////////////////////////////////////////////

src/LinearMath/btMatrix3x3.h

@@ -610,6 +610,27 @@ public:
 	/**@brief Return the inverse of the matrix */
 	btMatrix3x3 inverse() const; 
 
+	/// Solve A * x = b, where b is a column vector. This is more efficient
+	/// than computing the inverse in one-shot cases.
+	///Solve33 is from Box2d, thanks to Erin Catto,
+	btVector3 solve33(const btVector3& b) const
+	{
+		btVector3 col1 = getColumn(0);
+		btVector3 col2 = getColumn(1);
+		btVector3 col3 = getColumn(2);
+		
+		btScalar det = btDot(col1, btCross(col2, col3));
+		if (btFabs(det)>SIMD_EPSILON)
+		{
+			det = 1.0f / det;
+		}
+		btVector3 x;
+		x[0] = det * btDot(b, btCross(col2, col3));
+		x[1] = det * btDot(col1, btCross(b, col3));
+		x[2] = det * btDot(col1, btCross(col2, b));
+		return x;
+	}
+
 	btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
 	btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;