diff --git a/Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp b/Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp index 9cb2aa395..3f3a35937 100644 --- a/Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp +++ b/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; diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h b/src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h index b3f5e4ddf..7a12257b3 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h +++ b/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; diff --git a/src/Bullet3Serialize/Bullet2FileLoader/autogenerated/bullet2.h b/src/Bullet3Serialize/Bullet2FileLoader/autogenerated/bullet2.h index e701c39d8..6db7b4a91 100644 --- a/src/Bullet3Serialize/Bullet2FileLoader/autogenerated/bullet2.h +++ b/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; diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index b2158a9fb..c07e9bbd8 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/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; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 6df34ead1..fb06b76c3 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->computeGyroscopicImpulse( infoGlobal.m_timeStep ); - solverBody.m_externalTorqueImpulse += gyroForce; + gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce); + solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; } } } diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index e71db24ea..f80f2a5d3 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/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; diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp index db0add80c..732d75963 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -256,71 +256,22 @@ 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.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; + 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; } void btRigidBody::integrateVelocities(btScalar step) diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h index e715dbf59..7ba7b1f23 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/src/BulletDynamics/Dynamics/btRigidBody.h @@ -529,9 +529,7 @@ public: return m_rigidbodyFlags; } - btVector3 getLocalInertia() const; - - btVector3 computeGyroscopicImpulse(btScalar dt) const; + btVector3 computeGyroscopicForce(btScalar maxGyroscopicForce) const; ///////////////////////////////////////////////