diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp | 118 |
1 files changed, 113 insertions, 5 deletions
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp index 222f9006687..a7882684bf1 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/extern/bullet2/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 = 0; + m_rigidbodyFlags = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY; m_deltaLinearVelocity.setZero(); @@ -257,12 +257,41 @@ void btRigidBody::updateInertiaTensor() } -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]; + 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::computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const +{ + btVector3 inertiaLocal = getLocalInertia(); btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose(); btVector3 tmp = inertiaTensorWorld*getAngularVelocity(); btVector3 gf = getAngularVelocity().cross(tmp); @@ -274,6 +303,85 @@ btVector3 btRigidBody::computeGyroscopicForce(btScalar maxGyroscopicForce) const return gf; } + +btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Body(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; + btVector3 omega_div = J.solve33(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_World(btScalar step) const +{ + // use full newton-euler equations. 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); + + btVector3 dw; + dw = dfw.solve33(fw); + //const btMatrix3x3 dfw_inv = dfw.inverse(); + //dw = dfw_inv*fw; + + w1 -= dw; + } + + btVector3 gf = (w1 - w0); + return gf; +} + + void btRigidBody::integrateVelocities(btScalar step) { if (isStaticOrKinematicObject()) |