Upgrade Bullet to version 2.83.
[blender.git] / extern / bullet2 / src / BulletDynamics / Dynamics / btRigidBody.cpp
index 222f9006687280b5d9c618b247662bbc830abbe6..e0e8bc70f5718835999d720b122de0286a4c152d 100644 (file)
@@ -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())
@@ -317,38 +425,50 @@ void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
 }
 
 
-bool btRigidBody::checkCollideWithOverride(const  btCollisionObject* co) const
-{
-       const btRigidBody* otherRb = btRigidBody::upcast(co);
-       if (!otherRb)
-               return true;
-
-       for (int i = 0; i < m_constraintRefs.size(); ++i)
-       {
-               const btTypedConstraint* c = m_constraintRefs[i];
-               if (c->isEnabled())
-                       if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb)
-                               return false;
-       }
-
-       return true;
-}
 
 
 
 void btRigidBody::addConstraintRef(btTypedConstraint* c)
 {
+       ///disable collision with the 'other' body
+
        int index = m_constraintRefs.findLinearSearch(c);
+       //don't add constraints that are already referenced
+       //btAssert(index == m_constraintRefs.size());
        if (index == m_constraintRefs.size())
-               m_constraintRefs.push_back(c); 
-
-       m_checkCollideWith = true;
+       {
+               m_constraintRefs.push_back(c);
+               btCollisionObject* colObjA = &c->getRigidBodyA();
+               btCollisionObject* colObjB = &c->getRigidBodyB();
+               if (colObjA == this)
+               {
+                       colObjA->setIgnoreCollisionCheck(colObjB, true);
+               }
+               else
+               {
+                       colObjB->setIgnoreCollisionCheck(colObjA, true);
+               }
+       } 
 }
 
 void btRigidBody::removeConstraintRef(btTypedConstraint* c)
 {
-       m_constraintRefs.remove(c);
-       m_checkCollideWith = m_constraintRefs.size() > 0;
+       int index = m_constraintRefs.findLinearSearch(c);
+       //don't remove constraints that are not referenced
+       if(index < m_constraintRefs.size())
+    {
+        m_constraintRefs.remove(c);
+        btCollisionObject* colObjA = &c->getRigidBodyA();
+        btCollisionObject* colObjB = &c->getRigidBodyB();
+        if (colObjA == this)
+        {
+            colObjA->setIgnoreCollisionCheck(colObjB, false);
+        }
+        else
+        {
+            colObjB->setIgnoreCollisionCheck(colObjA, false);
+        }
+    }
 }
 
 int    btRigidBody::calculateSerializeBufferSize()     const