diff --git a/Demos/GyroscopicDemo/GyroscopicSetup.cpp b/Demos/GyroscopicDemo/GyroscopicSetup.cpp index c443bc2d5..e2e617705 100644 --- a/Demos/GyroscopicDemo/GyroscopicSetup.cpp +++ b/Demos/GyroscopicDemo/GyroscopicSetup.cpp @@ -1,19 +1,17 @@ #include "GyroscopicSetup.h" -static int gyroflags[5] = { +static int gyroflags[4] = { 0,//none, no gyroscopic term BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT, BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_EWERT, - BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_CATTO, - BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_COOPER, + BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_CATTO }; -static const char* gyroNames[5] = { +static const char* gyroNames[4] = { "No Coriolis", "Explicit", - "Ewert", - "Catto", - "Cooper", + "Implicit", + "Local Implicit" }; @@ -25,16 +23,15 @@ void GyroscopicSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld); - btVector3 positions[5] = { + btVector3 positions[4] = { btVector3( -10, 8,4), btVector3( -5, 8,4), btVector3( 0, 8,4), btVector3( 5, 8,4), - btVector3( 10, 8,4), }; - for (int i = 0; i<5; i++) + for (int i = 0; i<4; i++) { btCylinderShapeZ* pin = new btCylinderShapeZ(btVector3(0.1,0.1, 0.2)); btBoxShape* box = new btBoxShape(btVector3(1,0.1,0.1)); diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index b80b6632b..dba1fcfe2 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -1271,7 +1271,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btVector3 gyroForce (0,0,0); if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT) { - gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce); + gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce); solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; } if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_EWERT) @@ -1279,11 +1279,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol gyroForce = body->computeGyroscopicImpulseImplicit_Ewert(infoGlobal.m_timeStep); solverBody.m_externalTorqueImpulse += gyroForce; } - 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); diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp index 5df324fe7..f9feb5cf4 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -289,7 +289,7 @@ inline btMatrix3x3 evalEulerEqnDeriv(const btVector3& w1, const btVector3& w0, c return dfw1; } -btVector3 btRigidBody::computeGyroscopicForce(btScalar maxGyroscopicForce) const +btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const { btVector3 inertiaLocal = getLocalInertia(); btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose(); @@ -338,8 +338,9 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Catto(btScalar step) con // Jacobian btMatrix3x3 J = Ib + (skew0*Ib - skew1)*step; - btMatrix3x3 Jinv = J.inverse(); - btVector3 omega_div = Jinv*f; +// btMatrix3x3 Jinv = J.inverse(); +// btVector3 omega_div = Jinv*f; + btVector3 omega_div = J.solve33(f); // Single Newton-Raphson update omegab = omegab - omega_div;//Solve33(J, f); @@ -351,93 +352,6 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Catto(btScalar step) con -btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Cooper(btScalar step) const -{ -#if 0 - dReal h = callContext->m_stepperCallContext->m_stepSize; // Step size - dVector3 L; // Compute angular momentum - dMultiply0_331(L, I, b->avel); -#endif - - btVector3 inertiaLocal = getLocalInertia(); - btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose(); - btVector3 L = inertiaTensorWorld*getAngularVelocity(); - - btMatrix3x3 Itild(0, 0, 0, 0, 0, 0, 0, 0, 0); - -#if 0 - for (int ii = 0; ii<12; ++ii) { - Itild[ii] = Itild[ii] * h + I[ii]; - } -#endif - - btSetCrossMatrixMinus(Itild, L*step); - Itild += inertiaTensorWorld; - - -#if 0 -// Compute a new effective 'inertia tensor' -// for the implicit step: the cross-product -// matrix of the angular momentum plus the -// old tensor scaled by the timestep. -// Itild may not be symmetric pos-definite, -// but we can still use it to compute implicit -// gyroscopic torques. -dMatrix3 Itild = { 0 }; -dSetCrossMatrixMinus(Itild, L, 4); -for (int ii = 0; ii<12; ++ii) { - Itild[ii] = Itild[ii] * h + I[ii]; -} -#endif - - L *= step; - //Itild may not be symmetric pos-definite - btMatrix3x3 itInv = Itild.inverse(); - Itild = inertiaTensorWorld * itInv; - btMatrix3x3 ident(1,0,0,0,1,0,0,0,1); - Itild -= ident; - - - - -#if 0 -// Scale momentum by inverse time to get -// a sort of "torque" -dScaleVector3(L, dRecip(h)); -// Invert the pseudo-tensor -dMatrix3 itInv; -// This is a closed-form inversion. -// It's probably not numerically stable -// when dealing with small masses with -// a large asymmetry. -// An LU decomposition might be better. -if (dInvertMatrix3(itInv, Itild) != 0) { - // "Divide" the original tensor - // by the pseudo-tensor (on the right) - dMultiply0_333(Itild, I, itInv); - // Subtract an identity matrix - Itild[0] -= 1; Itild[5] -= 1; Itild[10] -= 1; - - // This new inertia matrix rotates the - // momentum to get a new set of torques - // that will work correctly when applied - // to the old inertia matrix as explicit - // torques with a semi-implicit update - // step. - dVector3 tau0; - dMultiply0_331(tau0, Itild, L); - - // Add the gyro torques to the torque - // accumulator - for (int ii = 0; ii<3; ++ii) { - b->tacc[ii] += tau0[ii]; - } -#endif - btVector3 tau0 = Itild * L; -// printf("tau0 = %f,%f,%f\n",tau0.x(),tau0.y(),tau0.z()); - return tau0; -} - btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Ewert(btScalar step) const { // use full newton-euler equations. common practice to drop the wxIw term. want it for better tumbling behavior. @@ -462,10 +376,10 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Ewert(btScalar step) con 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; + dw = dfw.solve33(fw); + //const btMatrix3x3 dfw_inv = dfw.inverse(); + //dw = dfw_inv*fw; w1 -= dw; } diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h index ae6c8fd4a..b3c77bb94 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/src/BulletDynamics/Dynamics/btRigidBody.h @@ -44,9 +44,8 @@ enum btRigidBodyFlags ///BT_ENABLE_GYROPSCOPIC_FORCE flags is enabled by default in Bullet 2.83 and onwards. ///See Demos/GyroscopicDemo and computeGyroscopicImpulseImplicit 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, + BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_EWERT=4, + BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_CATTO=8, }; @@ -535,10 +534,10 @@ 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 + ///explicit version is best avoided, it gains energy + btVector3 computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const; btVector3 getLocalInertia() const; /////////////////////////////////////////////// diff --git a/src/LinearMath/btMatrix3x3.h b/src/LinearMath/btMatrix3x3.h index 14fe704f8..41dea6948 100644 --- a/src/LinearMath/btMatrix3x3.h +++ b/src/LinearMath/btMatrix3x3.h @@ -610,6 +610,27 @@ public: /**@brief Return the inverse of the matrix */ btMatrix3x3 inverse() const; + /// Solve A * x = b, where b is a column vector. This is more efficient + /// than computing the inverse in one-shot cases. + ///Solve33 is from Box2d, thanks to Erin Catto, + btVector3 solve33(const btVector3& b) const + { + btVector3 col1 = getColumn(0); + btVector3 col2 = getColumn(1); + btVector3 col3 = getColumn(2); + + btScalar det = btDot(col1, btCross(col2, col3)); + if (btFabs(det)>SIMD_EPSILON) + { + det = 1.0f / det; + } + btVector3 x; + x[0] = det * btDot(b, btCross(col2, col3)); + x[1] = det * btDot(col1, btCross(b, col3)); + x[2] = det * btDot(col1, btCross(col2, b)); + return x; + } + btMatrix3x3 transposeTimes(const btMatrix3x3& m) const; btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;