Revert "replace unstable Gyroscopic force calculations with stable back Euler derived"

This reverts commit 0ce687853d.

Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp

@@ -171,6 +171,7 @@ bool	btBulletWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletFile
 		
 			solverInfo.m_linearSlop = btScalar(solverInfoData->m_solverInfo.m_linearSlop);
 			solverInfo.m_warmstartingFactor = btScalar(solverInfoData->m_solverInfo.m_warmstartingFactor);
+			solverInfo.m_maxGyroscopicForce = btScalar(solverInfoData->m_solverInfo.m_maxGyroscopicForce);
 			solverInfo.m_singleAxisRollingFrictionThreshold = btScalar(solverInfoData->m_solverInfo.m_singleAxisRollingFrictionThreshold);
 		
 			solverInfo.m_numIterations = solverInfoData->m_solverInfo.m_numIterations;
@@ -206,6 +207,7 @@ bool	btBulletWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletFile
 		
 			solverInfo.m_linearSlop = solverInfoData->m_solverInfo.m_linearSlop;
 			solverInfo.m_warmstartingFactor = solverInfoData->m_solverInfo.m_warmstartingFactor;
+			solverInfo.m_maxGyroscopicForce = solverInfoData->m_solverInfo.m_maxGyroscopicForce;
 			solverInfo.m_singleAxisRollingFrictionThreshold = solverInfoData->m_solverInfo.m_singleAxisRollingFrictionThreshold;
 		
 			solverInfo.m_numIterations = solverInfoData->m_solverInfo.m_numIterations;

src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h

@@ -56,6 +56,7 @@ struct b3ContactSolverInfoData
 	int			m_solverMode;
 	int	m_restingContactRestitutionThreshold;
 	int			m_minimumSolverBatchSize;
+	b3Scalar	m_maxGyroscopicForce;
 	b3Scalar	m_singleAxisRollingFrictionThreshold;
 
 
@@ -88,6 +89,7 @@ struct b3ContactSolverInfo : public b3ContactSolverInfoData
 		m_solverMode = B3_SOLVER_USE_WARMSTARTING | B3_SOLVER_SIMD;// | B3_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 B3_ENABLE_GYROPSCOPIC_FORCE flag set (using b3RigidBody::setFlag)
 		m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
 	}
 };
@@ -109,6 +111,7 @@ struct b3ContactSolverInfoDoubleData
 	double		m_splitImpulseTurnErp;
 	double		m_linearSlop;
 	double		m_warmstartingFactor;
+	double		m_maxGyroscopicForce;
 	double		m_singleAxisRollingFrictionThreshold;
 
 	int			m_numIterations;
@@ -139,6 +142,7 @@ struct b3ContactSolverInfoFloatData
 
 	float		m_linearSlop;
 	float		m_warmstartingFactor;
+	float		m_maxGyroscopicForce;
 	float		m_singleAxisRollingFrictionThreshold;
 
 	int			m_numIterations;

src/Bullet3Serialize/Bullet2FileLoader/autogenerated/bullet2.h

@@ -782,6 +782,7 @@ typedef struct bInvalidHandle {
         double m_splitImpulseTurnErp;
         double m_linearSlop;
         double m_warmstartingFactor;
+        double m_maxGyroscopicForce;
         double m_singleAxisRollingFrictionThreshold;
         int m_numIterations;
         int m_solverMode;
@@ -810,6 +811,7 @@ typedef struct bInvalidHandle {
         float m_splitImpulseTurnErp;
         float m_linearSlop;
         float m_warmstartingFactor;
+        float m_maxGyroscopicForce;
         float m_singleAxisRollingFrictionThreshold;
         int m_numIterations;
         int m_solverMode;

src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -56,6 +56,7 @@ struct btContactSolverInfoData
 	int			m_solverMode;
 	int	m_restingContactRestitutionThreshold;
 	int			m_minimumSolverBatchSize;
+	btScalar	m_maxGyroscopicForce;
 	btScalar	m_singleAxisRollingFrictionThreshold;
 
 
@@ -88,6 +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_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
 	}
 };
@@ -109,6 +111,7 @@ struct btContactSolverInfoDoubleData
 	double		m_splitImpulseTurnErp;
 	double		m_linearSlop;
 	double		m_warmstartingFactor;
+	double		m_maxGyroscopicForce;
 	double		m_singleAxisRollingFrictionThreshold;
 
 	int			m_numIterations;
@@ -139,6 +142,7 @@ struct btContactSolverInfoFloatData
 
 	float		m_linearSlop;
 	float		m_warmstartingFactor;
+	float		m_maxGyroscopicForce;
 	float		m_singleAxisRollingFrictionThreshold;
 
 	int			m_numIterations;

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -1270,8 +1270,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 			btVector3 gyroForce (0,0,0);
 			if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE)
 			{
-				gyroForce = body->computeGyroscopicImpulse( infoGlobal.m_timeStep ); 
+				gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce);
-                solverBody.m_externalTorqueImpulse += gyroForce; 
+				solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
 			}
 		}
 	}

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -1431,6 +1431,7 @@ void	btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serialize
 
 		worldInfo->m_solverInfo.m_linearSlop = getSolverInfo().m_linearSlop;
 		worldInfo->m_solverInfo.m_warmstartingFactor = getSolverInfo().m_warmstartingFactor;
+		worldInfo->m_solverInfo.m_maxGyroscopicForce = getSolverInfo().m_maxGyroscopicForce;
 		worldInfo->m_solverInfo.m_singleAxisRollingFrictionThreshold = getSolverInfo().m_singleAxisRollingFrictionThreshold;
 
 		worldInfo->m_solverInfo.m_numIterations = getSolverInfo().m_numIterations;

src/BulletDynamics/Dynamics/btRigidBody.cpp

@@ -256,70 +256,21 @@ void btRigidBody::updateInertiaTensor()
 	m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
 }
 
-btVector3 btRigidBody::getLocalInertia() const
-{
 
+btVector3 btRigidBody::computeGyroscopicForce(btScalar maxGyroscopicForce) const
+{
 	btVector3 inertiaLocal;
-    const btVector3 inertia = m_invInertiaLocal;
+	inertiaLocal[0] = 1.f/getInvInertiaDiagLocal()[0];
-    inertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0),
+	inertiaLocal[1] = 1.f/getInvInertiaDiagLocal()[1];
-               inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0),
+	inertiaLocal[2] = 1.f/getInvInertiaDiagLocal()[2];
-               inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
+	btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
-    return inertiaLocal;
+	btVector3 tmp = inertiaTensorWorld*getAngularVelocity();
-}
+	btVector3 gf = getAngularVelocity().cross(tmp);
-
+	btScalar l2 = gf.length2();
-inline btVector3 evalEulerEqn( const btVector3 w1, const btVector3 w0, const btVector3 T, const btScalar dt, 
+	if (l2>maxGyroscopicForce*maxGyroscopicForce)
-                        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::computeGyroscopicImpulse(btScalar step) const
-{
-    // use full newton-euler eqations.  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);
+		gf *= btScalar(1.)/btSqrt(l2)*maxGyroscopicForce;
-        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;
 }
 

src/BulletDynamics/Dynamics/btRigidBody.h

@@ -529,9 +529,7 @@ public:
 		return m_rigidbodyFlags;
 	}
 
-	btVector3 getLocalInertia() const;
+	btVector3 computeGyroscopicForce(btScalar maxGyroscopicForce) const;
-
-	btVector3 computeGyroscopicImpulse(btScalar dt) const; 
 
 	///////////////////////////////////////////////