From 0a04a745dd35836e23a4e452a0d513cf147a397c Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 24 Mar 2015 23:16:45 -0700 Subject: [PATCH] added Erin Catto's 'local' implicit coriolis/gyroscopic force, next to 'Ewert', Cooper, explicit and none Configured the gyroscopic demo to show the Dzhanibekov effect see also https://www.youtube.com/watch?v=L2o9eBl_Gzw --- Demos/GyroscopicDemo/GyroscopicSetup.cpp | 46 +++++++++++-------- .../btSequentialImpulseConstraintSolver.cpp | 12 +++-- src/BulletDynamics/Dynamics/btRigidBody.cpp | 43 ++++++++++++++++- src/BulletDynamics/Dynamics/btRigidBody.h | 9 ++-- 4 files changed, 83 insertions(+), 27 deletions(-) diff --git a/Demos/GyroscopicDemo/GyroscopicSetup.cpp b/Demos/GyroscopicDemo/GyroscopicSetup.cpp index af0cb123b..065d1a624 100644 --- a/Demos/GyroscopicDemo/GyroscopicSetup.cpp +++ b/Demos/GyroscopicDemo/GyroscopicSetup.cpp @@ -17,43 +17,49 @@ void GyroscopicSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) btRigidBody* groundBody; groundBody = createRigidBody(0, groundTransform, groundShape); groundBody->setFriction(btSqrt(2)); - btVector3 positions[4] = { - btVector3(0.8, -5, 4), - btVector3(0.8, -2, 4), - btVector3(0.8, 2, 4), - btVector3(0.8, 5, 4) - + btVector3 positions[5] = { + btVector3( -10, 8,4), + btVector3( -5, 8,4), + btVector3( 0, 8,4), + btVector3( 5, 8,4), + btVector3( 10, 8,4), }; - int gyroflags[4] = { + int gyroflags[5] = { 0,//none, no gyroscopic term - BT_ENABLE_GYROPSCOPIC_FORCE_EXPLICIT, - BT_ENABLE_GYROPSCOPIC_FORCE_IMPLICIT_EWERT, - BT_ENABLE_GYROPSCOPIC_FORCE_IMPLICIT_COOPER, + BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT, + BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_EWERT, + BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_COOPER, + BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_CATTO, + }; - for (int i = 0; i<4; i++) + for (int i = 0; i<5; i++) { - btCylinderShapeZ* top = new btCylinderShapeZ(btVector3(1, 1, 0.125)); - btCapsuleShapeZ* pin = new btCapsuleShapeZ(0.05, 1.5); - top->setMargin(0.01); + btCylinderShapeZ* pin = new btCylinderShapeZ(btVector3(0.1,0.1, 0.2)); + btBoxShape* box = new btBoxShape(btVector3(1,0.1,0.1)); + box->setMargin(0.01); pin->setMargin(0.01); btCompoundShape* compound = new btCompoundShape(); - compound->addChildShape(btTransform::getIdentity(), top); compound->addChildShape(btTransform::getIdentity(), pin); + btTransform offsetBox(btMatrix3x3::getIdentity(),btVector3(0,0,0.2)); + compound->addChildShape(offsetBox, box); + btScalar masses[2] = {0.3,0.1}; btVector3 localInertia; - top->calculateLocalInertia(1, localInertia); + btTransform principal; + compound->calculatePrincipalAxisTransform(masses,principal,localInertia); + btRigidBody* body = new btRigidBody(1, 0, compound, localInertia); btTransform tr; tr.setIdentity(); tr.setOrigin(positions[i]); body->setCenterOfMassTransform(tr); - body->setAngularVelocity(btVector3(1, 17, 3)); - body->setLinearVelocity(btVector3(0, 0, 0)); + body->setAngularVelocity(btVector3(0, 0.1, 10));//51)); + //body->setLinearVelocity(btVector3(3, 0, 0)); body->setFriction(btSqrt(1)); m_dynamicsWorld->addRigidBody(body); body->setFlags(gyroflags[i]); - - body->setDamping(0.00001f, 0.0001f); + m_dynamicsWorld->getSolverInfo().m_maxGyroscopicForce = 10.f; + body->setDamping(0.0000f, 0.000f); } diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index f01a22fc1..b80b6632b 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -1269,21 +1269,27 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol { btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; btVector3 gyroForce (0,0,0); - if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE_EXPLICIT) + if (body->getFlags()&BT_ENABLE_GYROSCOPIC_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) + if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_EWERT) { gyroForce = body->computeGyroscopicImpulseImplicit_Ewert(infoGlobal.m_timeStep); solverBody.m_externalTorqueImpulse += gyroForce; } - if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE_IMPLICIT_COOPER) + if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_COOPER) { gyroForce = body->computeGyroscopicImpulseImplicit_Cooper(infoGlobal.m_timeStep); solverBody.m_externalTorqueImpulse += gyroForce; } + if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_CATTO) + { + gyroForce = body->computeGyroscopicImpulseImplicit_Catto(infoGlobal.m_timeStep); + solverBody.m_externalTorqueImpulse += gyroForce; + + } } diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp index 48186d5d4..1a71887c6 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/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 = BT_ENABLE_GYROPSCOPIC_FORCE_IMPLICIT_EWERT; + m_rigidbodyFlags = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_EWERT; m_deltaLinearVelocity.setZero(); @@ -311,6 +311,46 @@ void btSetCrossMatrixMinus(btMatrix3x3& res, const btVector3& a) +a_1, -a_0, 0); } +btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Catto(btScalar step) const +{ + btVector3 idl = getLocalInertia(); + btVector3 omega1 = getAngularVelocity(); + btQuaternion q = getWorldTransform().getRotation(); + + // Convert to body coordinates + btVector3 omegab = quatRotate(q.inverse(), omega1); + btMatrix3x3 Ib; + Ib.setValue(idl.x(),0,0, + 0,idl.y(),0, + 0,0,idl.z()); + + btVector3 ibo = Ib*omegab; + + // Residual vector + btVector3 f = step * omegab.cross(ibo); + + btMatrix3x3 skew0; + omegab.getSkewSymmetricMatrix(&skew0[0], &skew0[1], &skew0[2]); + btVector3 om = Ib*omegab; + btMatrix3x3 skew1; + om.getSkewSymmetricMatrix(&skew1[0],&skew1[1],&skew1[2]); + + // Jacobian + btMatrix3x3 J = Ib + (skew0*Ib - skew1)*step; + + btMatrix3x3 Jinv = J.inverse(); + btVector3 omega_div = Jinv*f; + + // Single Newton-Raphson update + omegab = omegab - omega_div;//Solve33(J, f); + // Back to world coordinates + btVector3 omega2 = quatRotate(q,omegab); + btVector3 gf = omega2-omega1; + return gf; +} + + + btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Cooper(btScalar step) const { #if 0 @@ -394,6 +434,7 @@ if (dInvertMatrix3(itInv, Itild) != 0) { } #endif btVector3 tau0 = Itild * L; + printf("tau0 = %f,%f,%f\n",tau0.x(),tau0.y(),tau0.z()); return tau0; return btVector3(0, 0, 0); } diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h index b3934e99e..ae6c8fd4a 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/src/BulletDynamics/Dynamics/btRigidBody.h @@ -43,9 +43,10 @@ enum btRigidBodyFlags BT_DISABLE_WORLD_GRAVITY = 1, ///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 + BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT = 2, + BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_COOPER=4, + BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_EWERT=8, + BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_CATTO=16, }; @@ -535,6 +536,8 @@ public: btVector3 computeGyroscopicImpulseImplicit_Ewert(btScalar dt) const; btVector3 computeGyroscopicImpulseImplicit_Cooper(btScalar step) const; + btVector3 computeGyroscopicImpulseImplicit_Catto(btScalar step) const; + btVector3 computeGyroscopicForce(btScalar maxGyroscopicForce) const;//explicit version is best avoided, it gains energy btVector3 getLocalInertia() const;