Added experimental implicit gyroscopic force implementation, one by Michael Ewert, and another by Cooper (from OpenDE)

Will also add Erin Catto's local implicit version from the GDC 2015 tutorial
Added demo for btGeneric6DofSpring2Constraint, thanks to Gabor Puhr
Add gfxBridge.autogenerateGraphicsObjects method for Bullet 2 demos in new framework (need to implement all Bullet 2 collision shape types...)
Use 1,1,1 for local scaling in btStaticPlaneShape

src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp

@@ -21,7 +21,7 @@ subject to the following restrictions:
 btStaticPlaneShape::btStaticPlaneShape(const btVector3& planeNormal,btScalar planeConstant)
 : btConcaveShape (), m_planeNormal(planeNormal.normalized()),
 m_planeConstant(planeConstant),
-m_localScaling(btScalar(0.),btScalar(0.),btScalar(0.))
+m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.))
 {
 	m_shapeType = STATIC_PLANE_PROXYTYPE;
 	//	btAssert( btFuzzyZero(m_planeNormal.length() - btScalar(1.)) );

src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -89,7 +89,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
 		m_restingContactRestitutionThreshold = 2;//unused as of 2.81
 		m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
-		m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their BT_ENABLE_GYROPSCOPIC_FORCE flag set (using btRigidBody::setFlag)
+		m_maxGyroscopicForce = 100.f; ///it is only used for 'explicit' version of gyroscopic force
 		m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
 	}
 };
@@ -111,7 +111,7 @@ struct btContactSolverInfoDoubleData
 	double		m_splitImpulseTurnErp;
 	double		m_linearSlop;
 	double		m_warmstartingFactor;
-	double		m_maxGyroscopicForce;
+	double		m_maxGyroscopicForce;///it is only used for 'explicit' version of gyroscopic force
 	double		m_singleAxisRollingFrictionThreshold;
 
 	int			m_numIterations;

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -1259,6 +1259,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 
 	//convert all bodies
 
+
 	for (int i=0;i<numBodies;i++)
 	{
 		int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep);
@@ -1268,11 +1269,23 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 		{
 			btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
 			btVector3 gyroForce (0,0,0);
-			if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE)
+			if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE_EXPLICIT)
 			{
 				gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce);
 				solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
 			}
+			if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE_IMPLICIT_EWERT)
+			{
+				gyroForce = body->computeGyroscopicImpulseImplicit_Ewert(infoGlobal.m_timeStep);
+				solverBody.m_externalTorqueImpulse += gyroForce;
+			}
+			if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE_IMPLICIT_COOPER)
+			{
+				gyroForce = body->computeGyroscopicImpulseImplicit_Cooper(infoGlobal.m_timeStep);
+				solverBody.m_externalTorqueImpulse += gyroForce;
+			}
+			
+
 		}
 	}
 

src/BulletDynamics/Dynamics/btRigidBody.cpp

@@ -87,7 +87,7 @@ void	btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo&
 	setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
 	updateInertiaTensor();
 
-	m_rigidbodyFlags = 0;
+	m_rigidbodyFlags = BT_ENABLE_GYROPSCOPIC_FORCE_IMPLICIT_EWERT;
 
 
 	m_deltaLinearVelocity.setZero();
@@ -257,12 +257,41 @@ void btRigidBody::updateInertiaTensor()
 }
 
 
+
+btVector3 btRigidBody::getLocalInertia() const
+{
+
+	btVector3 inertiaLocal;
+	const btVector3 inertia = m_invInertiaLocal;
+	inertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0),
+		inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0),
+		inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0));
+	return inertiaLocal;
+}
+
+inline btVector3 evalEulerEqn(const btVector3& w1, const btVector3& w0, const btVector3& T, const btScalar dt,
+	const btMatrix3x3 &I)
+{
+	const btVector3 w2 = I*w1 + w1.cross(I*w1)*dt - (T*dt + I*w0);
+	return w2;
+}
+
+inline btMatrix3x3 evalEulerEqnDeriv(const btVector3& w1, const btVector3& w0, const btScalar dt,
+	const btMatrix3x3 &I)
+{
+
+	btMatrix3x3 w1x, Iw1x;
+	const btVector3 Iwi = (I*w1);
+	w1.getSkewSymmetricMatrix(&w1x[0], &w1x[1], &w1x[2]);
+	Iwi.getSkewSymmetricMatrix(&Iw1x[0], &Iw1x[1], &Iw1x[2]);
+
+	const btMatrix3x3 dfw1 = I + (w1x*I - Iw1x)*dt;
+	return dfw1;
+}
+
 btVector3 btRigidBody::computeGyroscopicForce(btScalar maxGyroscopicForce) const
 {
-	btVector3 inertiaLocal;
-	inertiaLocal[0] = 1.f/getInvInertiaDiagLocal()[0];
-	inertiaLocal[1] = 1.f/getInvInertiaDiagLocal()[1];
-	inertiaLocal[2] = 1.f/getInvInertiaDiagLocal()[2];
+	btVector3 inertiaLocal = getLocalInertia();
 	btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
 	btVector3 tmp = inertiaTensorWorld*getAngularVelocity();
 	btVector3 gf = getAngularVelocity().cross(tmp);
@@ -274,6 +303,138 @@ btVector3 btRigidBody::computeGyroscopicForce(btScalar maxGyroscopicForce) const
 	return gf;
 }
 
+void btSetCrossMatrixMinus(btMatrix3x3& res, const btVector3& a)
+{
+	const btScalar a_0 = a[0], a_1 = a[1], a_2 = a[2];
+	res.setValue(0, +a_2, -a_1,
+		-a_2, 0, +a_0,
+		+a_1, -a_0, 0);
+}
+
+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;
+	return tau0;
+	return btVector3(0, 0, 0);
+}
+
+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.
+	// calculate using implicit euler step so it's stable.
+
+	const btVector3 inertiaLocal = getLocalInertia();
+	const btVector3 w0 = getAngularVelocity();
+
+	btMatrix3x3 I;
+
+	I = m_worldTransform.getBasis().scaled(inertiaLocal) *
+		m_worldTransform.getBasis().transpose();
+
+	// use newtons method to find implicit solution for new angular velocity (w')
+	// f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0 
+	// df/dw' = I + 1xIw'*step + w'xI*step
+
+	btVector3 w1 = w0;
+
+	// one step of newton's method
+	{
+		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_inv*fw;
+
+		w1 -= dw;
+	}
+
+	btVector3 gf = (w1 - w0);
+	return gf;
+}
+
+
 void btRigidBody::integrateVelocities(btScalar step) 
 {
 	if (isStaticOrKinematicObject())

src/BulletDynamics/Dynamics/btRigidBody.h

@@ -41,10 +41,11 @@ extern bool gDisableDeactivation;
 enum	btRigidBodyFlags
 {
 	BT_DISABLE_WORLD_GRAVITY = 1,
-	///The BT_ENABLE_GYROPSCOPIC_FORCE can easily introduce instability
-	///So generally it is best to not enable it. 
-	///If really needed, run at a high frequency like 1000 Hertz:	///See Demos/GyroscopicDemo for an example use
-	BT_ENABLE_GYROPSCOPIC_FORCE = 2
+	///BT_ENABLE_GYROPSCOPIC_FORCE flags is enabled by default in Bullet 2.83 and onwards.
+	///See Demos/GyroscopicDemo and computeGyroscopicImpulseImplicit
+	BT_ENABLE_GYROPSCOPIC_FORCE_EXPLICIT = 2,
+	BT_ENABLE_GYROPSCOPIC_FORCE_IMPLICIT_COOPER=4,
+	BT_ENABLE_GYROPSCOPIC_FORCE_IMPLICIT_EWERT=8
 };
 
 
@@ -529,7 +530,13 @@ public:
 		return m_rigidbodyFlags;
 	}
 
-	btVector3 computeGyroscopicForce(btScalar maxGyroscopicForce) const;
+
+	
+
+	btVector3 computeGyroscopicImpulseImplicit_Ewert(btScalar dt) const;
+	btVector3 computeGyroscopicImpulseImplicit_Cooper(btScalar step) const;
+	btVector3 computeGyroscopicForce(btScalar maxGyroscopicForce) const;//explicit version is best avoided, it gains energy
+	btVector3 getLocalInertia() const;
 
 	///////////////////////////////////////////////