Changeset 2882 for code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp

Timestamp:

Mar 31, 2009, 8:05:51 PM (15 years ago)

Author:

rgrieder

Message:

Update from Bullet 2.73 to 2.74.

File:

• code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp (modified) (9 diffs)

code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp

@@ -20 +20 @@
 */
 
-
 #include "btGeneric6DofConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
@@ -27 +26 @@
 
 
+#define D6_USE_OBSOLETE_METHOD false
+//-----------------------------------------------------------------------------
+
+btGeneric6DofConstraint::btGeneric6DofConstraint()
+:btTypedConstraint(D6_CONSTRAINT_TYPE),
+m_useLinearReferenceFrameA(true),
+m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
+{
+}
+
+//-----------------------------------------------------------------------------
+
+btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
+: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
+, m_frameInA(frameInA)
+, m_frameInB(frameInB),
+m_useLinearReferenceFrameA(useLinearReferenceFrameA),
+m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
+{
+
+}
+//-----------------------------------------------------------------------------
+
+
 #define GENERIC_D6_DISABLE_WARMSTARTING 1
+
+//-----------------------------------------------------------------------------
 
 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
@@ -37 +62 @@
 }
 
+//-----------------------------------------------------------------------------
+
 ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
 bool    matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
 bool    matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
 {
-//      // rot =  cy*cz          -cy*sz           sy
-//      //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
-//      //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
-//
-
-                if (btGetMatrixElem(mat,2) < btScalar(1.0))
-                {
-                        if (btGetMatrixElem(mat,2) > btScalar(-1.0))
-                        {
-                                xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
-                                xyz[1] = btAsin(btGetMatrixElem(mat,2));
-                                xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
-                                return true;
-                        }
-                        else
-                        {
-                                // WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
-                                xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
-                                xyz[1] = -SIMD_HALF_PI;
-                                xyz[2] = btScalar(0.0);
-                                return false;
-                        }
+        //      // rot =  cy*cz          -cy*sz           sy
+        //      //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
+        //      //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
+        //
+
+        btScalar fi = btGetMatrixElem(mat,2);
+        if (fi < btScalar(1.0f))
+        {
+                if (fi > btScalar(-1.0f))
+                {
+                        xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
+                        xyz[1] = btAsin(btGetMatrixElem(mat,2));
+                        xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+                        return true;
                 }
                 else
                 {
-                        // WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
-                        xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
-                        xyz[1] = SIMD_HALF_PI;
-                        xyz[2] = 0.0;
-
-                }
-
-
+                        // WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
+                        xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+                        xyz[1] = -SIMD_HALF_PI;
+                        xyz[2] = btScalar(0.0);
+                        return false;
+                }
+        }
+        else
+        {
+                // WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
+                xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+                xyz[1] = SIMD_HALF_PI;
+                xyz[2] = 0.0;
+        }
         return false;
 }
 
-
-
 //////////////////////////// btRotationalLimitMotor ////////////////////////////////////
-
 
 int btRotationalLimitMotor::testLimitValue(btScalar test_value)
@@ -105 +127 @@
         m_currentLimit = 0;//Free from violation
         return 0;
-        
-}
-
+
+}
+
+//-----------------------------------------------------------------------------
 
 btScalar btRotationalLimitMotor::solveAngularLimits(
-                btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
-                btRigidBody * body0, btRigidBody * body1)
-{
-    if (needApplyTorques()==false) return 0.0f;
-
-    btScalar target_velocity = m_targetVelocity;
-    btScalar maxMotorForce = m_maxMotorForce;
+        btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
+        btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB)
+{
+        if (needApplyTorques()==false) return 0.0f;
+
+        btScalar target_velocity = m_targetVelocity;
+        btScalar maxMotorForce = m_maxMotorForce;
 
         //current error correction
-    if (m_currentLimit!=0)
-    {
-        target_velocity = -m_ERP*m_currentLimitError/(timeStep);
-        maxMotorForce = m_maxLimitForce;
-    }
-
-    maxMotorForce *= timeStep;
-
-    // current velocity difference
-    btVector3 vel_diff = body0->getAngularVelocity();
-    if (body1)
-    {
-        vel_diff -= body1->getAngularVelocity();
-    }
-
-
-
-    btScalar rel_vel = axis.dot(vel_diff);
+        if (m_currentLimit!=0)
+        {
+                target_velocity = -m_ERP*m_currentLimitError/(timeStep);
+                maxMotorForce = m_maxLimitForce;
+        }
+
+        maxMotorForce *= timeStep;
+
+        // current velocity difference
+
+        btVector3 angVelA;
+        bodyA.getAngularVelocity(angVelA);
+        btVector3 angVelB;
+        bodyB.getAngularVelocity(angVelB);
+
+        btVector3 vel_diff;
+        vel_diff = angVelA-angVelB;
+
+
+
+        btScalar rel_vel = axis.dot(vel_diff);
 
         // correction velocity
-    btScalar motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);
-
-
-    if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON  )
-    {
-        return 0.0f;//no need for applying force
-    }
+        btScalar motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);
+
+
+        if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON  )
+        {
+                return 0.0f;//no need for applying force
+        }
 
 
         // correction impulse
-    btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
+        btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
 
         // clip correction impulse
-    btScalar clippedMotorImpulse;
-
-    ///@todo: should clip against accumulated impulse
-    if (unclippedMotorImpulse>0.0f)
-    {
-        clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
-    }
-    else
-    {
-        clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
-    }
+        btScalar clippedMotorImpulse;
+
+        ///@todo: should clip against accumulated impulse
+        if (unclippedMotorImpulse>0.0f)
+        {
+                clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
+        }
+        else
+        {
+                clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
+        }
 
 
         // sort with accumulated impulses
-    btScalar    lo = btScalar(-1e30);
-    btScalar    hi = btScalar(1e30);
-
-    btScalar oldaccumImpulse = m_accumulatedImpulse;
-    btScalar sum = oldaccumImpulse + clippedMotorImpulse;
-    m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
-
-    clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
-
-
-
-    btVector3 motorImp = clippedMotorImpulse * axis;
-
-
-    body0->applyTorqueImpulse(motorImp);
-    if (body1) body1->applyTorqueImpulse(-motorImp);
-
-    return clippedMotorImpulse;
+        btScalar        lo = btScalar(-1e30);
+        btScalar        hi = btScalar(1e30);
+
+        btScalar oldaccumImpulse = m_accumulatedImpulse;
+        btScalar sum = oldaccumImpulse + clippedMotorImpulse;
+        m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
+
+        clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
+
+        btVector3 motorImp = clippedMotorImpulse * axis;
+
+        //body0->applyTorqueImpulse(motorImp);
+        //body1->applyTorqueImpulse(-motorImp);
+
+        bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
+        bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
+
+
+        return clippedMotorImpulse;
 
 
@@ -190 +217 @@
 //////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
 
+
+
+
 //////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
+
+
+int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)
+{
+        btScalar loLimit = m_lowerLimit[limitIndex];
+        btScalar hiLimit = m_upperLimit[limitIndex];
+        if(loLimit > hiLimit)
+        {
+                m_currentLimit[limitIndex] = 0;//Free from violation
+                m_currentLimitError[limitIndex] = btScalar(0.f);
+                return 0;
+        }
+
+        if (test_value < loLimit)
+        {
+                m_currentLimit[limitIndex] = 2;//low limit violation
+                m_currentLimitError[limitIndex] =  test_value - loLimit;
+                return 2;
+        }
+        else if (test_value> hiLimit)
+        {
+                m_currentLimit[limitIndex] = 1;//High limit violation
+                m_currentLimitError[limitIndex] = test_value - hiLimit;
+                return 1;
+        };
+
+        m_currentLimit[limitIndex] = 0;//Free from violation
+        m_currentLimitError[limitIndex] = btScalar(0.f);
+        return 0;
+} // btTranslationalLimitMotor::testLimitValue()
+
+//-----------------------------------------------------------------------------
+
 btScalar btTranslationalLimitMotor::solveLinearAxis(
-                btScalar timeStep,
-        btScalar jacDiagABInv,
-        btRigidBody& body1,const btVector3 &pointInA,
-        btRigidBody& body2,const btVector3 &pointInB,
-        int limit_index,
-        const btVector3 & axis_normal_on_a,
-                const btVector3 & anchorPos)
-{
-
-///find relative velocity
-//    btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
-//    btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
-    btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
-    btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
-
-    btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-    btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-    btVector3 vel = vel1 - vel2;
-
-    btScalar rel_vel = axis_normal_on_a.dot(vel);
-
-
-
-/// apply displacement correction
-
-//positional error (zeroth order error)
-    btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
-    btScalar    lo = btScalar(-1e30);
-    btScalar    hi = btScalar(1e30);
-
-    btScalar minLimit = m_lowerLimit[limit_index];
-    btScalar maxLimit = m_upperLimit[limit_index];
-
-    //handle the limits
-    if (minLimit < maxLimit)
-    {
-        {
-            if (depth > maxLimit)
-            {
-                depth -= maxLimit;
-                lo = btScalar(0.);
-
-            }
-            else
-            {
-                if (depth < minLimit)
-                {
-                    depth -= minLimit;
-                    hi = btScalar(0.);
-                }
-                else
-                {
-                    return 0.0f;
-                }
-            }
-        }
-    }
-
-    btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
-
-
-
-
-    btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
-    btScalar sum = oldNormalImpulse + normalImpulse;
-    m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
-    normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
-
-    btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
-    body1.applyImpulse( impulse_vector, rel_pos1);
-    body2.applyImpulse(-impulse_vector, rel_pos2);
-    return normalImpulse;
+        btScalar timeStep,
+        btScalar jacDiagABInv,
+        btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
+        btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
+        int limit_index,
+        const btVector3 & axis_normal_on_a,
+        const btVector3 & anchorPos)
+{
+
+        ///find relative velocity
+        //    btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
+        //    btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
+        btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
+        btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
+
+        btVector3 vel1;
+        bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+        btVector3 vel2;
+        bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+        btVector3 vel = vel1 - vel2;
+
+        btScalar rel_vel = axis_normal_on_a.dot(vel);
+
+
+
+        /// apply displacement correction
+
+        //positional error (zeroth order error)
+        btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
+        btScalar        lo = btScalar(-1e30);
+        btScalar        hi = btScalar(1e30);
+
+        btScalar minLimit = m_lowerLimit[limit_index];
+        btScalar maxLimit = m_upperLimit[limit_index];
+
+        //handle the limits
+        if (minLimit < maxLimit)
+        {
+                {
+                        if (depth > maxLimit)
+                        {
+                                depth -= maxLimit;
+                                lo = btScalar(0.);
+
+                        }
+                        else
+                        {
+                                if (depth < minLimit)
+                                {
+                                        depth -= minLimit;
+                                        hi = btScalar(0.);
+                                }
+                                else
+                                {
+                                        return 0.0f;
+                                }
+                        }
+                }
+        }
+
+        btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
+
+
+
+
+        btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
+        btScalar sum = oldNormalImpulse + normalImpulse;
+        m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
+        normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
+
+        btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
+        //body1.applyImpulse( impulse_vector, rel_pos1);
+        //body2.applyImpulse(-impulse_vector, rel_pos2);
+
+        btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);
+        btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a);
+        bodyA.applyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
+        bodyB.applyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
+
+
+
+
+        return normalImpulse;
 }
 
 //////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
 
-
-btGeneric6DofConstraint::btGeneric6DofConstraint()
-        :btTypedConstraint(D6_CONSTRAINT_TYPE),
-                m_useLinearReferenceFrameA(true)
-{
-}
-
-btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
-        : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
-        , m_frameInA(frameInA)
-        , m_frameInB(frameInB),
-                m_useLinearReferenceFrameA(useLinearReferenceFrameA)
-{
-
-}
-
-
-
-
-
 void btGeneric6DofConstraint::calculateAngleInfo()
 {
         btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
-
         matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
-
-
-
         // in euler angle mode we do not actually constrain the angular velocity
-  // along the axes axis[0] and axis[2] (although we do use axis[1]) :
-  //
-  //    to get                  constrain w2-w1 along           ...not
-  //    ------                  ---------------------           ------
-  //    d(angle[0])/dt = 0      ax[1] x ax[2]                   ax[0]
-  //    d(angle[1])/dt = 0      ax[1]
-  //    d(angle[2])/dt = 0      ax[0] x ax[1]                   ax[2]
-  //
-  // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
-  // to prove the result for angle[0], write the expression for angle[0] from
-  // GetInfo1 then take the derivative. to prove this for angle[2] it is
-  // easier to take the euler rate expression for d(angle[2])/dt with respect
-  // to the components of w and set that to 0.
-
+        // along the axes axis[0] and axis[2] (although we do use axis[1]) :
+        //
+        //    to get                    constrain w2-w1 along           ...not
+        //    ------                    ---------------------           ------
+        //    d(angle[0])/dt = 0        ax[1] x ax[2]                   ax[0]
+        //    d(angle[1])/dt = 0        ax[1]
+        //    d(angle[2])/dt = 0        ax[0] x ax[1]                   ax[2]
+        //
+        // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
+        // to prove the result for angle[0], write the expression for angle[0] from
+        // GetInfo1 then take the derivative. to prove this for angle[2] it is
+        // easier to take the euler rate expression for d(angle[2])/dt with respect
+        // to the components of w and set that to 0.
         btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
         btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
@@ -318 +367 @@
         m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
 
-
-//    if(m_debugDrawer)
-//    {
-//
-//      char buff[300];
-//              sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
-//              m_calculatedAxisAngleDiff[0],
-//              m_calculatedAxisAngleDiff[1],
-//              m_calculatedAxisAngleDiff[2]);
-//      m_debugDrawer->reportErrorWarning(buff);
-//    }
-
-}
+        m_calculatedAxis[0].normalize();
+        m_calculatedAxis[1].normalize();
+        m_calculatedAxis[2].normalize();
+
+}
+
+//-----------------------------------------------------------------------------
 
 void btGeneric6DofConstraint::calculateTransforms()
 {
-    m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
-    m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
-
-    calculateAngleInfo();
-}
-
+        m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
+        m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
+        calculateLinearInfo();
+        calculateAngleInfo();
+}
+
+//-----------------------------------------------------------------------------
 
 void btGeneric6DofConstraint::buildLinearJacobian(
-    btJacobianEntry & jacLinear,const btVector3 & normalWorld,
-    const btVector3 & pivotAInW,const btVector3 & pivotBInW)
-{
-    new (&jacLinear) btJacobianEntry(
+        btJacobianEntry & jacLinear,const btVector3 & normalWorld,
+        const btVector3 & pivotAInW,const btVector3 & pivotBInW)
+{
+        new (&jacLinear) btJacobianEntry(
         m_rbA.getCenterOfMassTransform().getBasis().transpose(),
         m_rbB.getCenterOfMassTransform().getBasis().transpose(),
@@ -355 +399 @@
         m_rbB.getInvInertiaDiagLocal(),
         m_rbB.getInvMass());
-
-}
+}
+
+//-----------------------------------------------------------------------------
 
 void btGeneric6DofConstraint::buildAngularJacobian(
-    btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
-{
-    new (&jacAngular)   btJacobianEntry(jointAxisW,
+        btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
+{
+         new (&jacAngular)      btJacobianEntry(jointAxisW,
                                       m_rbA.getCenterOfMassTransform().getBasis().transpose(),
                                       m_rbB.getCenterOfMassTransform().getBasis().transpose(),
@@ -369 +414 @@
 }
 
+//-----------------------------------------------------------------------------
+
 bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
 {
-    btScalar angle = m_calculatedAxisAngleDiff[axis_index];
-
-    //test limits
-    m_angularLimits[axis_index].testLimitValue(angle);
-    return m_angularLimits[axis_index].needApplyTorques();
-}
+        btScalar angle = m_calculatedAxisAngleDiff[axis_index];
+        //test limits
+        m_angularLimits[axis_index].testLimitValue(angle);
+        return m_angularLimits[axis_index].needApplyTorques();
+}
+
+//-----------------------------------------------------------------------------
 
 void btGeneric6DofConstraint::buildJacobian()
 {
-
-        // Clear accumulated impulses for the next simulation step
-    m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
-    int i;
-    for(i = 0; i < 3; i++)
-    {
-        m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
-    }
-    //calculates transform
-    calculateTransforms();
-
-//  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
-//  const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
-        calcAnchorPos();
-        btVector3 pivotAInW = m_AnchorPos;
-        btVector3 pivotBInW = m_AnchorPos;
-
-// not used here
-//    btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
-//    btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
-
-    btVector3 normalWorld;
-    //linear part
-    for (i=0;i<3;i++)
-    {
-        if (m_linearLimits.isLimited(i))
-        {
-                        if (m_useLinearReferenceFrameA)
-                    normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
-                        else
-                    normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
-
-            buildLinearJacobian(
-                m_jacLinear[i],normalWorld ,
-                pivotAInW,pivotBInW);
-
-        }
-    }
-
-    // angular part
-    for (i=0;i<3;i++)
-    {
-        //calculates error angle
-        if (testAngularLimitMotor(i))
-        {
-            normalWorld = this->getAxis(i);
-            // Create angular atom
-            buildAngularJacobian(m_jacAng[i],normalWorld);
-        }
-    }
-
-
-}
-
-
-void btGeneric6DofConstraint::solveConstraint(btScalar  timeStep)
-{
-    m_timeStep = timeStep;
-
-    //calculateTransforms();
-
-    int i;
-
-    // linear
-
-    btVector3 pointInA = m_calculatedTransformA.getOrigin();
-        btVector3 pointInB = m_calculatedTransformB.getOrigin();
-
-        btScalar jacDiagABInv;
-        btVector3 linear_axis;
-    for (i=0;i<3;i++)
-    {
-        if (m_linearLimits.isLimited(i))
-        {
-            jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
-
-                        if (m_useLinearReferenceFrameA)
-                    linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
-                        else
-                    linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
-
-            m_linearLimits.solveLinearAxis(
-                m_timeStep,
-                jacDiagABInv,
-                m_rbA,pointInA,
-                m_rbB,pointInB,
-                i,linear_axis, m_AnchorPos);
-
-        }
-    }
-
-    // angular
-    btVector3 angular_axis;
-    btScalar angularJacDiagABInv;
-    for (i=0;i<3;i++)
-    {
-        if (m_angularLimits[i].needApplyTorques())
-        {
-
-                        // get axis
-                        angular_axis = getAxis(i);
-
-                        angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
-
-                        m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,&m_rbB);
-        }
-    }
-}
+        if (m_useSolveConstraintObsolete)
+        {
+
+                // Clear accumulated impulses for the next simulation step
+                m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
+                int i;
+                for(i = 0; i < 3; i++)
+                {
+                        m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
+                }
+                //calculates transform
+                calculateTransforms();
+
+                //  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
+                //  const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
+                calcAnchorPos();
+                btVector3 pivotAInW = m_AnchorPos;
+                btVector3 pivotBInW = m_AnchorPos;
+
+                // not used here
+                //    btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
+                //    btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+
+                btVector3 normalWorld;
+                //linear part
+                for (i=0;i<3;i++)
+                {
+                        if (m_linearLimits.isLimited(i))
+                        {
+                                if (m_useLinearReferenceFrameA)
+                                        normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
+                                else
+                                        normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
+
+                                buildLinearJacobian(
+                                        m_jacLinear[i],normalWorld ,
+                                        pivotAInW,pivotBInW);
+
+                        }
+                }
+
+                // angular part
+                for (i=0;i<3;i++)
+                {
+                        //calculates error angle
+                        if (testAngularLimitMotor(i))
+                        {
+                                normalWorld = this->getAxis(i);
+                                // Create angular atom
+                                buildAngularJacobian(m_jacAng[i],normalWorld);
+                        }
+                }
+
+        }
+}
+
+//-----------------------------------------------------------------------------
+
+void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
+{
+        if (m_useSolveConstraintObsolete)
+        {
+                info->m_numConstraintRows = 0;
+                info->nub = 0;
+        } else
+        {
+                //prepare constraint
+                calculateTransforms();
+                info->m_numConstraintRows = 0;
+                info->nub = 6;
+                int i;
+                //test linear limits
+                for(i = 0; i < 3; i++)
+                {
+                        if(m_linearLimits.needApplyForce(i))
+                        {
+                                info->m_numConstraintRows++;
+                                info->nub--;
+                        }
+                }
+                //test angular limits
+                for (i=0;i<3 ;i++ )
+                {
+                        if(testAngularLimitMotor(i))
+                        {
+                                info->m_numConstraintRows++;
+                                info->nub--;
+                        }
+                }
+        }
+}
+
+//-----------------------------------------------------------------------------
+
+void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
+{
+        btAssert(!m_useSolveConstraintObsolete);
+        int row = setLinearLimits(info);
+        setAngularLimits(info, row);
+}
+
+//-----------------------------------------------------------------------------
+
+int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
+{
+        btGeneric6DofConstraint * d6constraint = this;
+        int row = 0;
+        //solve linear limits
+        btRotationalLimitMotor limot;
+        for (int i=0;i<3 ;i++ )
+        {
+                if(m_linearLimits.needApplyForce(i))
+                { // re-use rotational motor code
+                        limot.m_bounce = btScalar(0.f);
+                        limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
+                        limot.m_currentLimitError  = m_linearLimits.m_currentLimitError[i];
+                        limot.m_damping  = m_linearLimits.m_damping;
+                        limot.m_enableMotor  = m_linearLimits.m_enableMotor[i];
+                        limot.m_ERP  = m_linearLimits.m_restitution;
+                        limot.m_hiLimit  = m_linearLimits.m_upperLimit[i];
+                        limot.m_limitSoftness  = m_linearLimits.m_limitSoftness;
+                        limot.m_loLimit  = m_linearLimits.m_lowerLimit[i];
+                        limot.m_maxLimitForce  = btScalar(0.f);
+                        limot.m_maxMotorForce  = m_linearLimits.m_maxMotorForce[i];
+                        limot.m_targetVelocity  = m_linearLimits.m_targetVelocity[i];
+                        btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
+                        row += get_limit_motor_info2(&limot, &m_rbA, &m_rbB, info, row, axis, 0);
+                }
+        }
+        return row;
+}
+
+//-----------------------------------------------------------------------------
+
+int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset)
+{
+        btGeneric6DofConstraint * d6constraint = this;
+        int row = row_offset;
+        //solve angular limits
+        for (int i=0;i<3 ;i++ )
+        {
+                if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
+                {
+                        btVector3 axis = d6constraint->getAxis(i);
+                        row += get_limit_motor_info2(
+                                d6constraint->getRotationalLimitMotor(i),
+                                &m_rbA,
+                                &m_rbB,
+                                info,row,axis,1);
+                }
+        }
+
+        return row;
+}
+
+//-----------------------------------------------------------------------------
+
+void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar  timeStep)
+{
+        if (m_useSolveConstraintObsolete)
+        {
+
+
+                m_timeStep = timeStep;
+
+                //calculateTransforms();
+
+                int i;
+
+                // linear
+
+                btVector3 pointInA = m_calculatedTransformA.getOrigin();
+                btVector3 pointInB = m_calculatedTransformB.getOrigin();
+
+                btScalar jacDiagABInv;
+                btVector3 linear_axis;
+                for (i=0;i<3;i++)
+                {
+                        if (m_linearLimits.isLimited(i))
+                        {
+                                jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
+
+                                if (m_useLinearReferenceFrameA)
+                                        linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
+                                else
+                                        linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
+
+                                m_linearLimits.solveLinearAxis(
+                                        m_timeStep,
+                                        jacDiagABInv,
+                                        m_rbA,bodyA,pointInA,
+                                        m_rbB,bodyB,pointInB,
+                                        i,linear_axis, m_AnchorPos);
+
+                        }
+                }
+
+                // angular
+                btVector3 angular_axis;
+                btScalar angularJacDiagABInv;
+                for (i=0;i<3;i++)
+                {
+                        if (m_angularLimits[i].needApplyTorques())
+                        {
+
+                                // get axis
+                                angular_axis = getAxis(i);
+
+                                angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
+
+                                m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,bodyA,&m_rbB,bodyB);
+                        }
+                }
+        }
+}
+
+//-----------------------------------------------------------------------------
 
 void    btGeneric6DofConstraint::updateRHS(btScalar     timeStep)
 {
-    (void)timeStep;
-
-}
+        (void)timeStep;
+
+}
+
+//-----------------------------------------------------------------------------
 
 btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
 {
-    return m_calculatedAxis[axis_index];
-}
+        return m_calculatedAxis[axis_index];
+}
+
+//-----------------------------------------------------------------------------
 
 btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
 {
-    return m_calculatedAxisAngleDiff[axis_index];
-}
+        return m_calculatedAxisAngleDiff[axis_index];
+}
+
+//-----------------------------------------------------------------------------
 
 void btGeneric6DofConstraint::calcAnchorPos(void)
@@ -524 +687 @@
 } // btGeneric6DofConstraint::calcAnchorPos()
 
+//-----------------------------------------------------------------------------
+
+void btGeneric6DofConstraint::calculateLinearInfo()
+{
+        m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
+        m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
+        for(int i = 0; i < 3; i++)
+        {
+                m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
+        }
+} // btGeneric6DofConstraint::calculateLinearInfo()
+
+//-----------------------------------------------------------------------------
+
+int btGeneric6DofConstraint::get_limit_motor_info2(
+        btRotationalLimitMotor * limot,
+        btRigidBody * body0, btRigidBody * body1,
+        btConstraintInfo2 *info, int row, btVector3& ax1, int rotational)
+{
+    int srow = row * info->rowskip;
+    int powered = limot->m_enableMotor;
+    int limit = limot->m_currentLimit;
+    if (powered || limit)
+    {   // if the joint is powered, or has joint limits, add in the extra row
+        btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
+        btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
+        J1[srow+0] = ax1[0];
+        J1[srow+1] = ax1[1];
+        J1[srow+2] = ax1[2];
+        if(rotational)
+        {
+            J2[srow+0] = -ax1[0];
+            J2[srow+1] = -ax1[1];
+            J2[srow+2] = -ax1[2];
+        }
+        if((!rotational) && limit)
+        {
+                        btVector3 ltd;  // Linear Torque Decoupling vector
+                        btVector3 c = m_calculatedTransformB.getOrigin() - body0->getCenterOfMassPosition();
+                        ltd = c.cross(ax1);
+            info->m_J1angularAxis[srow+0] = ltd[0];
+            info->m_J1angularAxis[srow+1] = ltd[1];
+            info->m_J1angularAxis[srow+2] = ltd[2];
+
+                        c = m_calculatedTransformB.getOrigin() - body1->getCenterOfMassPosition();
+                        ltd = -c.cross(ax1);
+                        info->m_J2angularAxis[srow+0] = ltd[0];
+            info->m_J2angularAxis[srow+1] = ltd[1];
+            info->m_J2angularAxis[srow+2] = ltd[2];
+        }
+        // if we're limited low and high simultaneously, the joint motor is
+        // ineffective
+        if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
+        info->m_constraintError[srow] = btScalar(0.f);
+        if (powered)
+        {
+            info->cfm[srow] = 0.0f;
+            if(!limit)
+            {
+                info->m_constraintError[srow] += limot->m_targetVelocity;
+                info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
+                info->m_upperLimit[srow] = limot->m_maxMotorForce;
+            }
+        }
+        if(limit)
+        {
+            btScalar k = info->fps * limot->m_ERP;
+                        if(!rotational)
+                        {
+                                info->m_constraintError[srow] += k * limot->m_currentLimitError;
+                        }
+                        else
+                        {
+                                info->m_constraintError[srow] += -k * limot->m_currentLimitError;
+                        }
+            info->cfm[srow] = 0.0f;
+            if (limot->m_loLimit == limot->m_hiLimit)
+            {   // limited low and high simultaneously
+                info->m_lowerLimit[srow] = -SIMD_INFINITY;
+                info->m_upperLimit[srow] = SIMD_INFINITY;
+            }
+            else
+            {
+                if (limit == 1)
+                {
+                    info->m_lowerLimit[srow] = 0;
+                    info->m_upperLimit[srow] = SIMD_INFINITY;
+                }
+                else
+                {
+                    info->m_lowerLimit[srow] = -SIMD_INFINITY;
+                    info->m_upperLimit[srow] = 0;
+                }
+                // deal with bounce
+                if (limot->m_bounce > 0)
+                {
+                    // calculate joint velocity
+                    btScalar vel;
+                    if (rotational)
+                    {
+                        vel = body0->getAngularVelocity().dot(ax1);
+                        if (body1)
+                            vel -= body1->getAngularVelocity().dot(ax1);
+                    }
+                    else
+                    {
+                        vel = body0->getLinearVelocity().dot(ax1);
+                        if (body1)
+                            vel -= body1->getLinearVelocity().dot(ax1);
+                    }
+                    // only apply bounce if the velocity is incoming, and if the
+                    // resulting c[] exceeds what we already have.
+                    if (limit == 1)
+                    {
+                        if (vel < 0)
+                        {
+                            btScalar newc = -limot->m_bounce* vel;
+                            if (newc > info->m_constraintError[srow]) 
+                                                                info->m_constraintError[srow] = newc;
+                        }
+                    }
+                    else
+                    {
+                        if (vel > 0)
+                        {
+                            btScalar newc = -limot->m_bounce * vel;
+                            if (newc < info->m_constraintError[srow]) 
+                                                                info->m_constraintError[srow] = newc;
+                        }
+                    }
+                }
+            }
+        }
+        return 1;
+    }
+    else return 0;
+}
+
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------