diff --git a/Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp b/Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp index 3f3a35937..9cb2aa395 100644 --- a/Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp +++ b/Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp @@ -171,7 +171,6 @@ 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; @@ -207,7 +206,6 @@ 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; diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h b/src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h index 7a12257b3..b3f5e4ddf 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h +++ b/src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h @@ -56,7 +56,6 @@ struct b3ContactSolverInfoData int m_solverMode; int m_restingContactRestitutionThreshold; int m_minimumSolverBatchSize; - b3Scalar m_maxGyroscopicForce; b3Scalar m_singleAxisRollingFrictionThreshold; @@ -89,7 +88,6 @@ 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. } }; @@ -111,7 +109,6 @@ struct b3ContactSolverInfoDoubleData double m_splitImpulseTurnErp; double m_linearSlop; double m_warmstartingFactor; - double m_maxGyroscopicForce; double m_singleAxisRollingFrictionThreshold; int m_numIterations; @@ -142,7 +139,6 @@ struct b3ContactSolverInfoFloatData float m_linearSlop; float m_warmstartingFactor; - float m_maxGyroscopicForce; float m_singleAxisRollingFrictionThreshold; int m_numIterations; diff --git a/src/Bullet3Serialize/Bullet2FileLoader/autogenerated/bullet2.h b/src/Bullet3Serialize/Bullet2FileLoader/autogenerated/bullet2.h index 6db7b4a91..e701c39d8 100644 --- a/src/Bullet3Serialize/Bullet2FileLoader/autogenerated/bullet2.h +++ b/src/Bullet3Serialize/Bullet2FileLoader/autogenerated/bullet2.h @@ -782,7 +782,6 @@ typedef struct bInvalidHandle { double m_splitImpulseTurnErp; double m_linearSlop; double m_warmstartingFactor; - double m_maxGyroscopicForce; double m_singleAxisRollingFrictionThreshold; int m_numIterations; int m_solverMode; @@ -811,7 +810,6 @@ typedef struct bInvalidHandle { float m_splitImpulseTurnErp; float m_linearSlop; float m_warmstartingFactor; - float m_maxGyroscopicForce; float m_singleAxisRollingFrictionThreshold; int m_numIterations; int m_solverMode; diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index c07e9bbd8..b2158a9fb 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -56,7 +56,6 @@ struct btContactSolverInfoData int m_solverMode; int m_restingContactRestitutionThreshold; int m_minimumSolverBatchSize; - btScalar m_maxGyroscopicForce; btScalar m_singleAxisRollingFrictionThreshold; @@ -89,7 +88,6 @@ 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. } }; @@ -111,7 +109,6 @@ struct btContactSolverInfoDoubleData double m_splitImpulseTurnErp; double m_linearSlop; double m_warmstartingFactor; - double m_maxGyroscopicForce; double m_singleAxisRollingFrictionThreshold; int m_numIterations; @@ -142,7 +139,6 @@ struct btContactSolverInfoFloatData float m_linearSlop; float m_warmstartingFactor; - float m_maxGyroscopicForce; float m_singleAxisRollingFrictionThreshold; int m_numIterations; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index fb06b76c3..6df34ead1 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/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->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce); - solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; + gyroForce = body->computeGyroscopicImpulse( infoGlobal.m_timeStep ); + solverBody.m_externalTorqueImpulse += gyroForce; } } } diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index f80f2a5d3..e71db24ea 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -1431,7 +1431,6 @@ 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; diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp index 732d75963..db0add80c 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -256,22 +256,71 @@ void btRigidBody::updateInertiaTensor() m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose(); } - -btVector3 btRigidBody::computeGyroscopicForce(btScalar maxGyroscopicForce) const +btVector3 btRigidBody::getLocalInertia() const { - btVector3 inertiaLocal; - inertiaLocal[0] = 1.f/getInvInertiaDiagLocal()[0]; - inertiaLocal[1] = 1.f/getInvInertiaDiagLocal()[1]; - inertiaLocal[2] = 1.f/getInvInertiaDiagLocal()[2]; - btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose(); - btVector3 tmp = inertiaTensorWorld*getAngularVelocity(); - btVector3 gf = getAngularVelocity().cross(tmp); - btScalar l2 = gf.length2(); - if (l2>maxGyroscopicForce*maxGyroscopicForce) - { - gf *= btScalar(1.)/btSqrt(l2)*maxGyroscopicForce; - } - return gf; + + 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::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); + 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) diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h index 7ba7b1f23..e715dbf59 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/src/BulletDynamics/Dynamics/btRigidBody.h @@ -529,7 +529,9 @@ public: return m_rigidbodyFlags; } - btVector3 computeGyroscopicForce(btScalar maxGyroscopicForce) const; + btVector3 getLocalInertia() const; + + btVector3 computeGyroscopicImpulse(btScalar dt) const; ///////////////////////////////////////////////