From 0ce687853def7e564ed71eca66cfe91a1ac604f7 Mon Sep 17 00:00:00 2001 From: emMichael Alexander Ewert/em Date: Fri, 12 Dec 2014 11:49:36 -0800 Subject: [PATCH 1/3] replace unstable Gyroscopic force calculations with stable back Euler derived update. Removed max force clamping. --- .../btBulletWorldImporter.cpp | 2 - .../ConstraintSolver/b3ContactSolverInfo.h | 4 - .../Bullet2FileLoader/autogenerated/bullet2.h | 2 - .../ConstraintSolver/btContactSolverInfo.h | 4 - .../btSequentialImpulseConstraintSolver.cpp | 4 +- .../Dynamics/btDiscreteDynamicsWorld.cpp | 1 - src/BulletDynamics/Dynamics/btRigidBody.cpp | 79 +++++++++++++++---- src/BulletDynamics/Dynamics/btRigidBody.h | 4 +- 8 files changed, 69 insertions(+), 31 deletions(-) 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; /////////////////////////////////////////////// From 92dabdc07d484e668df1b2138408bfbcfe0e0393 Mon Sep 17 00:00:00 2001 From: emMichael Alexander Ewert/em Date: Fri, 12 Dec 2014 15:39:16 -0800 Subject: [PATCH 2/3] Fix a couple of bugs in 2dConvex Hull algorithm. * Need to use atan2 so 3d angles are calculated properly after projection. * Need to handle case where the first tripple of points is non-convex, previously this would cause the algorithm to fail with only 1 point. --- src/LinearMath/btGrahamScan2dConvexHull.h | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/src/LinearMath/btGrahamScan2dConvexHull.h b/src/LinearMath/btGrahamScan2dConvexHull.h index e658c5cf0..13a79aa58 100644 --- a/src/LinearMath/btGrahamScan2dConvexHull.h +++ b/src/LinearMath/btGrahamScan2dConvexHull.h @@ -85,9 +85,17 @@ inline void GrahamScanConvexHull2D(btAlignedObjectArray& original originalPoints[0].m_angle = -1e30f; for (int i=1;i& original else hull.push_back(originalPoints[i]); } + + if( hull.size() == 1 ) + { + hull.push_back( originalPoints[i] ); + } } } From d4be7a48239ede819a9978132aa0fa38db5b8d69 Mon Sep 17 00:00:00 2001 From: emMichael Alexander Ewert/em Date: Fri, 12 Dec 2014 16:35:40 -0800 Subject: [PATCH 3/3] Revert "replace unstable Gyroscopic force calculations with stable back Euler derived" This reverts commit 0ce687853def7e564ed71eca66cfe91a1ac604f7. --- .../btBulletWorldImporter.cpp | 2 + .../ConstraintSolver/b3ContactSolverInfo.h | 4 + .../Bullet2FileLoader/autogenerated/bullet2.h | 2 + .../ConstraintSolver/btContactSolverInfo.h | 4 + .../btSequentialImpulseConstraintSolver.cpp | 4 +- .../Dynamics/btDiscreteDynamicsWorld.cpp | 1 + src/BulletDynamics/Dynamics/btRigidBody.cpp | 79 ++++--------------- src/BulletDynamics/Dynamics/btRigidBody.h | 4 +- 8 files changed, 31 insertions(+), 69 deletions(-) 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; ///////////////////////////////////////////////