Changeset 2907 for code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver

Timestamp:

Apr 8, 2009, 12:36:08 AM (16 years ago)

Author:

dafrick

Message:

Merging of the current QuestSystem branch.

Location:

code/branches/questsystem5

Files:

• . (modified) (1 prop)
• src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp (modified) (8 diffs)
• src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h (modified) (6 diffs)
• src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp (modified) (6 diffs)
• src/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h (modified) (3 diffs)
• src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp (modified) (9 diffs)
• src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h (modified) (14 diffs)
• src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp (modified) (8 diffs)
• src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h (modified) (4 diffs)
• src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp (modified) (4 diffs)
• src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h (modified) (2 diffs)
• src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp (modified) (17 diffs)
• src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h (modified) (1 diff)
• src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp (modified) (17 diffs)
• src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h (modified) (5 diffs)
• src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h (modified) (3 diffs)
• src/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h (modified) (3 diffs)
• src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp (modified) (5 diffs)
• src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h (modified) (6 diffs)

code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

@@ -23 +23 @@
 #include <new>
 
+//-----------------------------------------------------------------------------
+
+#define CONETWIST_USE_OBSOLETE_SOLVER false
+#define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
+
+//-----------------------------------------------------------------------------
+
 btConeTwistConstraint::btConeTwistConstraint()
-:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE)
+:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
+m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
 {
 }
@@ -32 +40 @@
                                                                                          const btTransform& rbAFrame,const btTransform& rbBFrame)
                                                                                          :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
-                                                                                         m_angularOnly(false)
-{
-        m_swingSpan1 = btScalar(1e30);
-        m_swingSpan2 = btScalar(1e30);
-        m_twistSpan  = btScalar(1e30);
-        m_biasFactor = 0.3f;
-        m_relaxationFactor = 1.0f;
-
+                                                                                         m_angularOnly(false),
+                                                                                         m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
+{
+        init();
+}
+
+btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
+                                                                                        :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
+                                                                                         m_angularOnly(false),
+                                                                                         m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
+{
+        m_rbBFrame = m_rbAFrame;
+        init(); 
+}
+
+
+void btConeTwistConstraint::init()
+{
+        m_angularOnly = false;
         m_solveTwistLimit = false;
         m_solveSwingLimit = false;
-
-}
-
-btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
-                                                                                        :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
-                                                                                         m_angularOnly(false)
-{
-        m_rbBFrame = m_rbAFrame;
+        m_bMotorEnabled = false;
+        m_maxMotorImpulse = btScalar(-1);
+
+        setLimit(btScalar(1e30), btScalar(1e30), btScalar(1e30));
+        m_damping = btScalar(0.01);
+        m_fixThresh = CONETWIST_DEF_FIX_THRESH;
+}
+
+
+//-----------------------------------------------------------------------------
+
+void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
+{
+        if (m_useSolveConstraintObsolete)
+        {
+                info->m_numConstraintRows = 0;
+                info->nub = 0;
+        } 
+        else
+        {
+                info->m_numConstraintRows = 3;
+                info->nub = 3;
+                calcAngleInfo2();
+                if(m_solveSwingLimit)
+                {
+                        info->m_numConstraintRows++;
+                        info->nub--;
+                        if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
+                        {
+                                info->m_numConstraintRows++;
+                                info->nub--;
+                        }
+                }
+                if(m_solveTwistLimit)
+                {
+                        info->m_numConstraintRows++;
+                        info->nub--;
+                }
+        }
+} // btConeTwistConstraint::getInfo1()
         
-        m_swingSpan1 = btScalar(1e30);
-        m_swingSpan2 = btScalar(1e30);
-        m_twistSpan  = btScalar(1e30);
-        m_biasFactor = 0.3f;
-        m_relaxationFactor = 1.0f;
-
-        m_solveTwistLimit = false;
-        m_solveSwingLimit = false;
+//-----------------------------------------------------------------------------
+
+void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
+{
+        btAssert(!m_useSolveConstraintObsolete);
+        //retrieve matrices
+        btTransform body0_trans;
+        body0_trans = m_rbA.getCenterOfMassTransform();
+    btTransform body1_trans;
+        body1_trans = m_rbB.getCenterOfMassTransform();
+    // set jacobian
+    info->m_J1linearAxis[0] = 1;
+    info->m_J1linearAxis[info->rowskip+1] = 1;
+    info->m_J1linearAxis[2*info->rowskip+2] = 1;
+        btVector3 a1 = body0_trans.getBasis() * m_rbAFrame.getOrigin();
+        {
+                btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
+                btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
+                btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
+                btVector3 a1neg = -a1;
+                a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+        }
+        btVector3 a2 = body1_trans.getBasis() * m_rbBFrame.getOrigin();
+        {
+                btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
+                btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
+                btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
+                a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+        }
+    // set right hand side
+    btScalar k = info->fps * info->erp;
+    int j;
+        for (j=0; j<3; j++)
+    {
+        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
+                info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
+                info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
+    }
+        int row = 3;
+    int srow = row * info->rowskip;
+        btVector3 ax1;
+        // angular limits
+        if(m_solveSwingLimit)
+        {
+                btScalar *J1 = info->m_J1angularAxis;
+                btScalar *J2 = info->m_J2angularAxis;
+                if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
+                {
+                        btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
+                        btVector3 p = trA.getBasis().getColumn(1);
+                        btVector3 q = trA.getBasis().getColumn(2);
+                        int srow1 = srow + info->rowskip;
+                        J1[srow+0] = p[0];
+                        J1[srow+1] = p[1];
+                        J1[srow+2] = p[2];
+                        J1[srow1+0] = q[0];
+                        J1[srow1+1] = q[1];
+                        J1[srow1+2] = q[2];
+                        J2[srow+0] = -p[0];
+                        J2[srow+1] = -p[1];
+                        J2[srow+2] = -p[2];
+                        J2[srow1+0] = -q[0];
+                        J2[srow1+1] = -q[1];
+                        J2[srow1+2] = -q[2];
+                        btScalar fact = info->fps * m_relaxationFactor;
+                        info->m_constraintError[srow] =   fact * m_swingAxis.dot(p);
+                        info->m_constraintError[srow1] =  fact * m_swingAxis.dot(q);
+                        info->m_lowerLimit[srow] = -SIMD_INFINITY;
+                        info->m_upperLimit[srow] = SIMD_INFINITY;
+                        info->m_lowerLimit[srow1] = -SIMD_INFINITY;
+                        info->m_upperLimit[srow1] = SIMD_INFINITY;
+                        srow = srow1 + info->rowskip;
+                }
+                else
+                {
+                        ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
+                        J1[srow+0] = ax1[0];
+                        J1[srow+1] = ax1[1];
+                        J1[srow+2] = ax1[2];
+                        J2[srow+0] = -ax1[0];
+                        J2[srow+1] = -ax1[1];
+                        J2[srow+2] = -ax1[2];
+                        btScalar k = info->fps * m_biasFactor;
+
+                        info->m_constraintError[srow] = k * m_swingCorrection;
+                        info->cfm[srow] = 0.0f;
+                        // m_swingCorrection is always positive or 0
+                        info->m_lowerLimit[srow] = 0;
+                        info->m_upperLimit[srow] = SIMD_INFINITY;
+                        srow += info->rowskip;
+                }
+        }
+        if(m_solveTwistLimit)
+        {
+                ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
+                btScalar *J1 = info->m_J1angularAxis;
+                btScalar *J2 = info->m_J2angularAxis;
+                J1[srow+0] = ax1[0];
+                J1[srow+1] = ax1[1];
+                J1[srow+2] = ax1[2];
+                J2[srow+0] = -ax1[0];
+                J2[srow+1] = -ax1[1];
+                J2[srow+2] = -ax1[2];
+                btScalar k = info->fps * m_biasFactor;
+                info->m_constraintError[srow] = k * m_twistCorrection;
+                info->cfm[srow] = 0.0f;
+                if(m_twistSpan > 0.0f)
+                {
+
+                        if(m_twistCorrection > 0.0f)
+                        {
+                                info->m_lowerLimit[srow] = 0;
+                                info->m_upperLimit[srow] = SIMD_INFINITY;
+                        } 
+                        else
+                        {
+                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
+                                info->m_upperLimit[srow] = 0;
+                        } 
+                }
+                else
+                {
+                        info->m_lowerLimit[srow] = -SIMD_INFINITY;
+                        info->m_upperLimit[srow] = SIMD_INFINITY;
+                }
+                srow += info->rowskip;
+        }
+}
         
-}                       
+//-----------------------------------------------------------------------------
 
 void    btConeTwistConstraint::buildJacobian()
 {
-        m_appliedImpulse = btScalar(0.);
-
-        //set bias, sign, clear accumulator
-        m_swingCorrection = btScalar(0.);
-        m_twistLimitSign = btScalar(0.);
-        m_solveTwistLimit = false;
-        m_solveSwingLimit = false;
-        m_accTwistLimitImpulse = btScalar(0.);
-        m_accSwingLimitImpulse = btScalar(0.);
-
-        if (!m_angularOnly)
-        {
-                btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-                btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
-                btVector3 relPos = pivotBInW - pivotAInW;
-
-                btVector3 normal[3];
-                if (relPos.length2() > SIMD_EPSILON)
-                {
-                        normal[0] = relPos.normalized();
-                }
-                else
-                {
-                        normal[0].setValue(btScalar(1.0),0,0);
-                }
-
-                btPlaneSpace1(normal[0], normal[1], normal[2]);
-
-                for (int i=0;i<3;i++)
-                {
-                        new (&m_jac[i]) btJacobianEntry(
+        if (m_useSolveConstraintObsolete)
+        {
+                m_appliedImpulse = btScalar(0.);
+                m_accTwistLimitImpulse = btScalar(0.);
+                m_accSwingLimitImpulse = btScalar(0.);
+
+                if (!m_angularOnly)
+                {
+                        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+                        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+                        btVector3 relPos = pivotBInW - pivotAInW;
+
+                        btVector3 normal[3];
+                        if (relPos.length2() > SIMD_EPSILON)
+                        {
+                                normal[0] = relPos.normalized();
+                        }
+                        else
+                        {
+                                normal[0].setValue(btScalar(1.0),0,0);
+                        }
+
+                        btPlaneSpace1(normal[0], normal[1], normal[2]);
+
+                        for (int i=0;i<3;i++)
+                        {
+                                new (&m_jac[i]) btJacobianEntry(
                                 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
                                 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
@@ -104 +271 @@
                                 m_rbB.getInvInertiaDiagLocal(),
                                 m_rbB.getInvMass());
-                }
-        }
+                        }
+                }
+
+                calcAngleInfo2();
+        }
+}
+
+//-----------------------------------------------------------------------------
+
+void    btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
+{
+        if (m_useSolveConstraintObsolete)
+        {
+                btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+                btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+
+                btScalar tau = btScalar(0.3);
+
+                //linear part
+                if (!m_angularOnly)
+                {
+                        btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+                        btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+
+                        btVector3 vel1;
+                        bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+                        btVector3 vel2;
+                        bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+                        btVector3 vel = vel1 - vel2;
+
+                        for (int i=0;i<3;i++)
+                        {               
+                                const btVector3& normal = m_jac[i].m_linearJointAxis;
+                                btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+
+                                btScalar rel_vel;
+                                rel_vel = normal.dot(vel);
+                                //positional error (zeroth order error)
+                                btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+                                btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
+                                m_appliedImpulse += impulse;
+                                
+                                btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
+                                btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
+                                bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
+                                bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
+                
+                        }
+                }
+
+                // apply motor
+                if (m_bMotorEnabled)
+                {
+                        // compute current and predicted transforms
+                        btTransform trACur = m_rbA.getCenterOfMassTransform();
+                        btTransform trBCur = m_rbB.getCenterOfMassTransform();
+                        btVector3 omegaA; bodyA.getAngularVelocity(omegaA);
+                        btVector3 omegaB; bodyB.getAngularVelocity(omegaB);
+                        btTransform trAPred; trAPred.setIdentity(); 
+                        btVector3 zerovec(0,0,0);
+                        btTransformUtil::integrateTransform(
+                                trACur, zerovec, omegaA, timeStep, trAPred);
+                        btTransform trBPred; trBPred.setIdentity(); 
+                        btTransformUtil::integrateTransform(
+                                trBCur, zerovec, omegaB, timeStep, trBPred);
+
+                        // compute desired transforms in world
+                        btTransform trPose(m_qTarget);
+                        btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
+                        btTransform trADes = trBPred * trABDes;
+                        btTransform trBDes = trAPred * trABDes.inverse();
+
+                        // compute desired omegas in world
+                        btVector3 omegaADes, omegaBDes;
+                        
+                        btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
+                        btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);
+
+                        // compute delta omegas
+                        btVector3 dOmegaA = omegaADes - omegaA;
+                        btVector3 dOmegaB = omegaBDes - omegaB;
+
+                        // compute weighted avg axis of dOmega (weighting based on inertias)
+                        btVector3 axisA, axisB;
+                        btScalar kAxisAInv = 0, kAxisBInv = 0;
+
+                        if (dOmegaA.length2() > SIMD_EPSILON)
+                        {
+                                axisA = dOmegaA.normalized();
+                                kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA);
+                        }
+
+                        if (dOmegaB.length2() > SIMD_EPSILON)
+                        {
+                                axisB = dOmegaB.normalized();
+                                kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB);
+                        }
+
+                        btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
+
+                        static bool bDoTorque = true;
+                        if (bDoTorque && avgAxis.length2() > SIMD_EPSILON)
+                        {
+                                avgAxis.normalize();
+                                kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis);
+                                kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis);
+                                btScalar kInvCombined = kAxisAInv + kAxisBInv;
+
+                                btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
+                                                                        (kInvCombined * kInvCombined);
+
+                                if (m_maxMotorImpulse >= 0)
+                                {
+                                        btScalar fMaxImpulse = m_maxMotorImpulse;
+                                        if (m_bNormalizedMotorStrength)
+                                                fMaxImpulse = fMaxImpulse/kAxisAInv;
+
+                                        btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
+                                        btScalar  newUnclampedMag = newUnclampedAccImpulse.length();
+                                        if (newUnclampedMag > fMaxImpulse)
+                                        {
+                                                newUnclampedAccImpulse.normalize();
+                                                newUnclampedAccImpulse *= fMaxImpulse;
+                                                impulse = newUnclampedAccImpulse - m_accMotorImpulse;
+                                        }
+                                        m_accMotorImpulse += impulse;
+                                }
+
+                                btScalar  impulseMag  = impulse.length();
+                                btVector3 impulseAxis =  impulse / impulseMag;
+
+                                bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
+                                bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+
+                        }
+                }
+                else // no motor: do a little damping
+                {
+                        const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
+                        const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
+                        btVector3 relVel = angVelB - angVelA;
+                        if (relVel.length2() > SIMD_EPSILON)
+                        {
+                                btVector3 relVelAxis = relVel.normalized();
+                                btScalar m_kDamping =  btScalar(1.) /
+                                        (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
+                                         getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
+                                btVector3 impulse = m_damping * m_kDamping * relVel;
+
+                                btScalar  impulseMag  = impulse.length();
+                                btVector3 impulseAxis = impulse / impulseMag;
+                                bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
+                                bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+                        }
+                }
+
+                // joint limits
+                {
+                        ///solve angular part
+                        btVector3 angVelA;
+                        bodyA.getAngularVelocity(angVelA);
+                        btVector3 angVelB;
+                        bodyB.getAngularVelocity(angVelB);
+
+                        // solve swing limit
+                        if (m_solveSwingLimit)
+                        {
+                                btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep;
+                                btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis);
+                                if (relSwingVel > 0)
+                                        amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
+                                btScalar impulseMag = amplitude * m_kSwing;
+
+                                // Clamp the accumulated impulse
+                                btScalar temp = m_accSwingLimitImpulse;
+                                m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
+                                impulseMag = m_accSwingLimitImpulse - temp;
+
+                                btVector3 impulse = m_swingAxis * impulseMag;
+
+                                // don't let cone response affect twist
+                                // (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit)
+                                {
+                                        btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA;
+                                        btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
+                                        impulse = impulseNoTwistCouple;
+                                }
+
+                                impulseMag = impulse.length();
+                                btVector3 noTwistSwingAxis = impulse / impulseMag;
+
+                                bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
+                                bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
+                        }
+
+
+                        // solve twist limit
+                        if (m_solveTwistLimit)
+                        {
+                                btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep;
+                                btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis );
+                                if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important)
+                                        amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
+                                btScalar impulseMag = amplitude * m_kTwist;
+
+                                // Clamp the accumulated impulse
+                                btScalar temp = m_accTwistLimitImpulse;
+                                m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
+                                impulseMag = m_accTwistLimitImpulse - temp;
+
+                                btVector3 impulse = m_twistAxis * impulseMag;
+
+                                bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
+                                bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
+                        }               
+                }
+        }
+
+}
+
+//-----------------------------------------------------------------------------
+
+void    btConeTwistConstraint::updateRHS(btScalar       timeStep)
+{
+        (void)timeStep;
+
+}
+
+//-----------------------------------------------------------------------------
+
+void btConeTwistConstraint::calcAngleInfo()
+{
+        m_swingCorrection = btScalar(0.);
+        m_twistLimitSign = btScalar(0.);
+        m_solveTwistLimit = false;
+        m_solveSwingLimit = false;
 
         btVector3 b1Axis1,b1Axis2,b1Axis3;
@@ -123 +524 @@
         {
                 b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
-//              swing1  = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
                 swx = b2Axis1.dot(b1Axis1);
                 swy = b2Axis1.dot(b1Axis2);
@@ -130 +530 @@
                 fact = fact / (fact + btScalar(1.0));
                 swing1 *= fact; 
-
         }
 
@@ -136 +535 @@
         {
                 b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);                     
-//              swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
                 swx = b2Axis1.dot(b1Axis1);
                 swy = b2Axis1.dot(b1Axis3);
@@ -153 +551 @@
                 m_swingCorrection = EllipseAngle-1.0f;
                 m_solveSwingLimit = true;
-                
                 // Calculate necessary axis & factors
                 m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
                 m_swingAxis.normalize();
-
                 btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
                 m_swingAxis *= swingAxisSign;
-
-                m_kSwing =  btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
-                        getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
-
         }
 
@@ -173 +565 @@
                 btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); 
                 btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
-
-                btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
+                m_twistAngle = twist;
+
+//              btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
+                btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.);
                 if (twist <= -m_twistSpan*lockedFreeFactor)
                 {
                         m_twistCorrection = -(twist + m_twistSpan);
                         m_solveTwistLimit = true;
-
                         m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
                         m_twistAxis.normalize();
                         m_twistAxis *= -1.0f;
-
-                        m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
-                                getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
-
-                }       else
-                        if (twist >  m_twistSpan*lockedFreeFactor)
-                        {
-                                m_twistCorrection = (twist - m_twistSpan);
+                }
+                else if (twist >  m_twistSpan*lockedFreeFactor)
+                {
+                        m_twistCorrection = (twist - m_twistSpan);
+                        m_solveTwistLimit = true;
+                        m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
+                        m_twistAxis.normalize();
+                }
+        }
+} // btConeTwistConstraint::calcAngleInfo()
+
+
+static btVector3 vTwist(1,0,0); // twist axis in constraint's space
+
+//-----------------------------------------------------------------------------
+
+void btConeTwistConstraint::calcAngleInfo2()
+{
+        m_swingCorrection = btScalar(0.);
+        m_twistLimitSign = btScalar(0.);
+        m_solveTwistLimit = false;
+        m_solveSwingLimit = false;
+
+        {
+                // compute rotation of A wrt B (in constraint space)
+                btQuaternion qA = getRigidBodyA().getCenterOfMassTransform().getRotation() * m_rbAFrame.getRotation();
+                btQuaternion qB = getRigidBodyB().getCenterOfMassTransform().getRotation() * m_rbBFrame.getRotation();
+                btQuaternion qAB = qB.inverse() * qA;
+
+                // split rotation into cone and twist
+                // (all this is done from B's perspective. Maybe I should be averaging axes...)
+                btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
+                btQuaternion qABCone  = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
+                btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
+
+                if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
+                {
+                        btScalar swingAngle, swingLimit = 0; btVector3 swingAxis;
+                        computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);
+
+                        if (swingAngle > swingLimit * m_limitSoftness)
+                        {
+                                m_solveSwingLimit = true;
+
+                                // compute limit ratio: 0->1, where
+                                // 0 == beginning of soft limit
+                                // 1 == hard/real limit
+                                m_swingLimitRatio = 1.f;
+                                if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
+                                {
+                                        m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/
+                                                                                (swingLimit - swingLimit * m_limitSoftness);
+                                }                               
+
+                                // swing correction tries to get back to soft limit
+                                m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
+
+                                // adjustment of swing axis (based on ellipse normal)
+                                adjustSwingAxisToUseEllipseNormal(swingAxis);
+
+                                // Calculate necessary axis & factors           
+                                m_swingAxis = quatRotate(qB, -swingAxis);
+
+                                m_twistAxisA.setValue(0,0,0);
+
+                                m_kSwing =  btScalar(1.) /
+                                        (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
+                                         getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
+                        }
+                }
+                else
+                {
+                        // you haven't set any limits;
+                        // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
+                        // anyway, we have either hinge or fixed joint
+                        btVector3 ivA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
+                        btVector3 jvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
+                        btVector3 kvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+                        btVector3 ivB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(0);
+                        btVector3 target;
+                        btScalar x = ivB.dot(ivA);
+                        btScalar y = ivB.dot(jvA);
+                        btScalar z = ivB.dot(kvA);
+                        if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
+                        { // fixed. We'll need to add one more row to constraint
+                                if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
+                                {
+                                        m_solveSwingLimit = true;
+                                        m_swingAxis = -ivB.cross(ivA);
+                                }
+                        }
+                        else
+                        {
+                                if(m_swingSpan1 < m_fixThresh)
+                                { // hinge around Y axis
+                                        if(!(btFuzzyZero(y)))
+                                        {
+                                                m_solveSwingLimit = true;
+                                                if(m_swingSpan2 >= m_fixThresh)
+                                                {
+                                                        y = btScalar(0.f);
+                                                        btScalar span2 = btAtan2(z, x);
+                                                        if(span2 > m_swingSpan2)
+                                                        {
+                                                                x = btCos(m_swingSpan2);
+                                                                z = btSin(m_swingSpan2);
+                                                        }
+                                                        else if(span2 < -m_swingSpan2)
+                                                        {
+                                                                x =  btCos(m_swingSpan2);
+                                                                z = -btSin(m_swingSpan2);
+                                                        }
+                                                }
+                                        }
+                                }
+                                else
+                                { // hinge around Z axis
+                                        if(!btFuzzyZero(z))
+                                        {
+                                                m_solveSwingLimit = true;
+                                                if(m_swingSpan1 >= m_fixThresh)
+                                                {
+                                                        z = btScalar(0.f);
+                                                        btScalar span1 = btAtan2(y, x);
+                                                        if(span1 > m_swingSpan1)
+                                                        {
+                                                                x = btCos(m_swingSpan1);
+                                                                y = btSin(m_swingSpan1);
+                                                        }
+                                                        else if(span1 < -m_swingSpan1)
+                                                        {
+                                                                x =  btCos(m_swingSpan1);
+                                                                y = -btSin(m_swingSpan1);
+                                                        }
+                                                }
+                                        }
+                                }
+                                target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0];
+                                target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1];
+                                target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
+                                target.normalize();
+                                m_swingAxis = -ivB.cross(target);
+                                m_swingCorrection = m_swingAxis.length();
+                                m_swingAxis.normalize();
+                        }
+                }
+
+                if (m_twistSpan >= btScalar(0.f))
+                {
+                        btVector3 twistAxis;
+                        computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
+
+                        if (m_twistAngle > m_twistSpan*m_limitSoftness)
+                        {
                                 m_solveTwistLimit = true;
 
-                                m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
-                                m_twistAxis.normalize();
-
-                                m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
-                                        getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
-
-                        }
-        }
-}
-
-void    btConeTwistConstraint::solveConstraint(btScalar timeStep)
-{
-
-        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
-
-        btScalar tau = btScalar(0.3);
-
-        //linear part
-        if (!m_angularOnly)
-        {
-                btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
-                btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
-
-                btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
-                btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
-                btVector3 vel = vel1 - vel2;
-
-                for (int i=0;i<3;i++)
-                {               
-                        const btVector3& normal = m_jac[i].m_linearJointAxis;
-                        btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
-
-                        btScalar rel_vel;
-                        rel_vel = normal.dot(vel);
-                        //positional error (zeroth order error)
-                        btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
-                        btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
-                        m_appliedImpulse += impulse;
-                        btVector3 impulse_vector = normal * impulse;
-                        m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
-                        m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
-                }
-        }
-        
-        {
-                ///solve angular part
-                const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
-                const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
-
-                // solve swing limit
-                if (m_solveSwingLimit)
-                {
-                        btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor);
-                        btScalar impulseMag = amplitude * m_kSwing;
-
-                        // Clamp the accumulated impulse
-                        btScalar temp = m_accSwingLimitImpulse;
-                        m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
-                        impulseMag = m_accSwingLimitImpulse - temp;
-
-                        btVector3 impulse = m_swingAxis * impulseMag;
-
-                        m_rbA.applyTorqueImpulse(impulse);
-                        m_rbB.applyTorqueImpulse(-impulse);
-
-                }
-
-                // solve twist limit
-                if (m_solveTwistLimit)
-                {
-                        btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor );
-                        btScalar impulseMag = amplitude * m_kTwist;
-
-                        // Clamp the accumulated impulse
-                        btScalar temp = m_accTwistLimitImpulse;
-                        m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
-                        impulseMag = m_accTwistLimitImpulse - temp;
-
-                        btVector3 impulse = m_twistAxis * impulseMag;
-
-                        m_rbA.applyTorqueImpulse(impulse);
-                        m_rbB.applyTorqueImpulse(-impulse);
-
-                }
-        
-        }
-
-}
-
-void    btConeTwistConstraint::updateRHS(btScalar       timeStep)
-{
-        (void)timeStep;
-
-}
+                                m_twistLimitRatio = 1.f;
+                                if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
+                                {
+                                        m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/
+                                                                                (m_twistSpan  - m_twistSpan * m_limitSoftness);
+                                }
+
+                                // twist correction tries to get back to soft limit
+                                m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
+
+                                m_twistAxis = quatRotate(qB, -twistAxis);
+
+                                m_kTwist = btScalar(1.) /
+                                        (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
+                                         getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
+                        }
+
+                        if (m_solveSwingLimit)
+                                m_twistAxisA = quatRotate(qA, -twistAxis);
+                }
+                else
+                {
+                        m_twistAngle = btScalar(0.f);
+                }
+        }
+} // btConeTwistConstraint::calcAngleInfo2()
+
+
+
+// given a cone rotation in constraint space, (pre: twist must already be removed)
+// this method computes its corresponding swing angle and axis.
+// more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
+void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
+                                                                                                 btScalar& swingAngle, // out
+                                                                                                 btVector3& vSwingAxis, // out
+                                                                                                 btScalar& swingLimit) // out
+{
+        swingAngle = qCone.getAngle();
+        if (swingAngle > SIMD_EPSILON)
+        {
+                vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
+                vSwingAxis.normalize();
+                if (fabs(vSwingAxis.x()) > SIMD_EPSILON)
+                {
+                        // non-zero twist?! this should never happen.
+                        int wtf = 0; wtf = wtf;
+                }
+
+                // Compute limit for given swing. tricky:
+                // Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
+                // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
+
+                // For starters, compute the direction from center to surface of ellipse.
+                // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
+                // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
+                btScalar xEllipse =  vSwingAxis.y();
+                btScalar yEllipse = -vSwingAxis.z();
+
+                // Now, we use the slope of the vector (using x/yEllipse) and find the length
+                // of the line that intersects the ellipse:
+                //  x^2   y^2
+                //  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
+                //  a^2   b^2
+                // Do the math and it should be clear.
+
+                swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
+                if (fabs(xEllipse) > SIMD_EPSILON)
+                {
+                        btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
+                        btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
+                        norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
+                        btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
+                        swingLimit = sqrt(swingLimit2);
+                }
+
+                // test!
+                /*swingLimit = m_swingSpan2;
+                if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
+                {
+                btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
+                btScalar sinphi = m_swingSpan2 / sqrt(mag_2);
+                btScalar phi = asin(sinphi);
+                btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
+                btScalar alpha = 3.14159f - theta - phi;
+                btScalar sinalpha = sin(alpha);
+                swingLimit = m_swingSpan1 * sinphi/sinalpha;
+                }*/
+        }
+        else if (swingAngle < 0)
+        {
+                // this should never happen!
+                int wtf = 0; wtf = wtf;
+        }
+}
+
+btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
+{
+        // compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone)
+        btScalar xEllipse = btCos(fAngleInRadians);
+        btScalar yEllipse = btSin(fAngleInRadians);
+
+        // Use the slope of the vector (using x/yEllipse) and find the length
+        // of the line that intersects the ellipse:
+        //  x^2   y^2
+        //  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
+        //  a^2   b^2
+        // Do the math and it should be clear.
+
+        float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
+        if (fabs(xEllipse) > SIMD_EPSILON)
+        {
+                btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
+                btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
+                norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
+                btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
+                swingLimit = sqrt(swingLimit2);
+        }
+
+        // convert into point in constraint space:
+        // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
+        btVector3 vSwingAxis(0, xEllipse, -yEllipse);
+        btQuaternion qSwing(vSwingAxis, swingLimit);
+        btVector3 vPointInConstraintSpace(fLength,0,0);
+        return quatRotate(qSwing, vPointInConstraintSpace);
+}
+
+// given a twist rotation in constraint space, (pre: cone must already be removed)
+// this method computes its corresponding angle and axis.
+void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
+                                                                                                  btScalar& twistAngle, // out
+                                                                                                  btVector3& vTwistAxis) // out
+{
+        btQuaternion qMinTwist = qTwist;
+        twistAngle = qTwist.getAngle();
+
+        if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
+        {
+                qMinTwist = operator-(qTwist);
+                twistAngle = qMinTwist.getAngle();
+        }
+        if (twistAngle < 0)
+        {
+                // this should never happen
+                int wtf = 0; wtf = wtf;                 
+        }
+
+        vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
+        if (twistAngle > SIMD_EPSILON)
+                vTwistAxis.normalize();
+}
+
+
+void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
+{
+        // the swing axis is computed as the "twist-free" cone rotation,
+        // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
+        // so, if we're outside the limits, the closest way back inside the cone isn't 
+        // along the vector back to the center. better (and more stable) to use the ellipse normal.
+
+        // convert swing axis to direction from center to surface of ellipse
+        // (ie. rotate 2D vector by PI/2)
+        btScalar y = -vSwingAxis.z();
+        btScalar z =  vSwingAxis.y();
+
+        // do the math...
+        if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
+        {
+                // compute gradient/normal of ellipse surface at current "point"
+                btScalar grad = y/z;
+                grad *= m_swingSpan2 / m_swingSpan1;
+
+                // adjust y/z to represent normal at point (instead of vector to point)
+                if (y > 0)
+                        y =  fabs(grad * z);
+                else
+                        y = -fabs(grad * z);
+
+                // convert ellipse direction back to swing axis
+                vSwingAxis.setZ(-y);
+                vSwingAxis.setY( z);
+                vSwingAxis.normalize();
+        }
+}
+
+
+
+void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
+{
+        btTransform trACur = m_rbA.getCenterOfMassTransform();
+        btTransform trBCur = m_rbB.getCenterOfMassTransform();
+        btTransform trABCur = trBCur.inverse() * trACur;
+        btQuaternion qABCur = trABCur.getRotation();
+        btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
+        btQuaternion qConstraintCur = trConstraintCur.getRotation();
+
+        btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
+        setMotorTargetInConstraintSpace(qConstraint);
+}
+
+
+void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q)
+{
+        m_qTarget = q;
+
+        // clamp motor target to within limits
+        {
+                btScalar softness = 1.f;//m_limitSoftness;
+
+                // split into twist and cone
+                btVector3 vTwisted = quatRotate(m_qTarget, vTwist);
+                btQuaternion qTargetCone  = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize();
+                btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize();
+
+                // clamp cone
+                if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
+                {
+                        btScalar swingAngle, swingLimit; btVector3 swingAxis;
+                        computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit);
+
+                        if (fabs(swingAngle) > SIMD_EPSILON)
+                        {
+                                if (swingAngle > swingLimit*softness)
+                                        swingAngle = swingLimit*softness;
+                                else if (swingAngle < -swingLimit*softness)
+                                        swingAngle = -swingLimit*softness;
+                                qTargetCone = btQuaternion(swingAxis, swingAngle);
+                        }
+                }
+
+                // clamp twist
+                if (m_twistSpan >= btScalar(0.05f))
+                {
+                        btScalar twistAngle; btVector3 twistAxis;
+                        computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis);
+
+                        if (fabs(twistAngle) > SIMD_EPSILON)
+                        {
+                                // eddy todo: limitSoftness used here???
+                                if (twistAngle > m_twistSpan*softness)
+                                        twistAngle = m_twistSpan*softness;
+                                else if (twistAngle < -m_twistSpan*softness)
+                                        twistAngle = -m_twistSpan*softness;
+                                qTargetTwist = btQuaternion(twistAxis, twistAngle);
+                        }
+                }
+
+                m_qTarget = qTargetCone * qTargetTwist;
+        }
+}
+
+
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+
+

code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h

@@ -43 +43 @@
         btScalar        m_relaxationFactor;
 
+        btScalar        m_damping;
+
         btScalar        m_swingSpan1;
         btScalar        m_swingSpan2;
         btScalar        m_twistSpan;
 
+        btScalar        m_fixThresh;
+
         btVector3   m_swingAxis;
         btVector3       m_twistAxis;
@@ -57 +61 @@
         btScalar        m_twistCorrection;
 
+        btScalar        m_twistAngle;
+
         btScalar        m_accSwingLimitImpulse;
         btScalar        m_accTwistLimitImpulse;
@@ -64 +70 @@
         bool            m_solveSwingLimit;
 
+        bool    m_useSolveConstraintObsolete;
+
+        // not yet used...
+        btScalar        m_swingLimitRatio;
+        btScalar        m_twistLimitRatio;
+        btVector3   m_twistAxisA;
+
+        // motor
+        bool             m_bMotorEnabled;
+        bool             m_bNormalizedMotorStrength;
+        btQuaternion m_qTarget;
+        btScalar         m_maxMotorImpulse;
+        btVector3        m_accMotorImpulse;
         
 public:
@@ -75 +94 @@
         virtual void    buildJacobian();
 
-        virtual void    solveConstraint(btScalar        timeStep);
+        virtual void getInfo1 (btConstraintInfo1* info);
+        
+        virtual void getInfo2 (btConstraintInfo2* info);
+        
+
+        virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
 
         void    updateRHS(btScalar      timeStep);
@@ -93 +117 @@
         }
 
-        void    setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan,  btScalar _softness = 0.8f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+        void    setLimit(int limitIndex,btScalar limitValue)
+        {
+                switch (limitIndex)
+                {
+                case 3:
+                        {
+                                m_twistSpan = limitValue;
+                                break;
+                        }
+                case 4:
+                        {
+                                m_swingSpan2 = limitValue;
+                                break;
+                        }
+                case 5:
+                        {
+                                m_swingSpan1 = limitValue;
+                                break;
+                        }
+                default:
+                        {
+                        }
+                };
+        }
+
+        void    setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan,  btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
         {
                 m_swingSpan1 = _swingSpan1;
@@ -122 +171 @@
         }
 
+        void calcAngleInfo();
+        void calcAngleInfo2();
+
+        inline btScalar getSwingSpan1()
+        {
+                return m_swingSpan1;
+        }
+        inline btScalar getSwingSpan2()
+        {
+                return m_swingSpan2;
+        }
+        inline btScalar getTwistSpan()
+        {
+                return m_twistSpan;
+        }
+        inline btScalar getTwistAngle()
+        {
+                return m_twistAngle;
+        }
+        bool isPastSwingLimit() { return m_solveSwingLimit; }
+
+
+        void setDamping(btScalar damping) { m_damping = damping; }
+
+        void enableMotor(bool b) { m_bMotorEnabled = b; }
+        void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; }
+        void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; }
+
+        btScalar getFixThresh() { return m_fixThresh; }
+        void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; }
+
+        // setMotorTarget:
+        // q: the desired rotation of bodyA wrt bodyB.
+        // note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
+        // note: don't forget to enableMotor()
+        void setMotorTarget(const btQuaternion &q);
+
+        // same as above, but q is the desired rotation of frameA wrt frameB in constraint space
+        void setMotorTargetInConstraintSpace(const btQuaternion &q);
+
+        btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
+
+
+
+protected:
+        void init();
+
+        void computeConeLimitInfo(const btQuaternion& qCone, // in
+                btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
+
+        void computeTwistLimitInfo(const btQuaternion& qTwist, // in
+                btScalar& twistAngle, btVector3& vTwistAxis); // all outs
+
+        void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
 };
 

code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp

@@ -23 +23 @@
 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
 
-#define ASSERT2 assert
+#define ASSERT2 btAssert
 
 #define USE_INTERNAL_APPLY_IMPULSE 1
@@ -53 +53 @@
         
 
-          btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
+           btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
                 body2.getCenterOfMassTransform().getBasis().transpose(),
                 rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
@@ -115 +115 @@
 
         btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-        assert(cpd);
+        btAssert(cpd);
         btScalar distance = cpd->m_penetration;
         btScalar positionalError = Kcor *-distance;
@@ -167 +167 @@
         
         btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-        assert(cpd);
+        btAssert(cpd);
 
         btScalar combinedFriction = cpd->m_friction;
@@ -256 +256 @@
         
         btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-        assert(cpd);
+        btAssert(cpd);
 
         btScalar combinedFriction = cpd->m_friction;
@@ -338 +338 @@
 
         btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-        assert(cpd);
+        btAssert(cpd);
         btScalar distance = cpd->m_penetration;
         btScalar positionalError = Kcor *-distance;

code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -23 +23 @@
         SOLVER_USE_WARMSTARTING = 4,
         SOLVER_USE_FRICTION_WARMSTARTING = 8,
-        SOLVER_CACHE_FRIENDLY = 16
+        SOLVER_USE_2_FRICTION_DIRECTIONS = 16,
+        SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32,
+        SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64,
+        SOLVER_CACHE_FRIENDLY = 128,
+        SOLVER_SIMD = 256,      //enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version
+        SOLVER_CUDA = 512       //will be open sourced during Game Developers Conference 2009. Much faster.
 };
 
@@ -40 +45 @@
         btScalar        m_erp;//used as Baumgarte factor
         btScalar        m_erp2;//used in Split Impulse
+        btScalar        m_globalCfm;//constraint force mixing
         int                     m_splitImpulse;
         btScalar        m_splitImpulsePenetrationThreshold;
@@ -66 +72 @@
                 m_erp = btScalar(0.2);
                 m_erp2 = btScalar(0.1);
-                m_sor = btScalar(1.3);
+                m_globalCfm = btScalar(0.);
+                m_sor = btScalar(1.);
                 m_splitImpulse = false;
                 m_splitImpulsePenetrationThreshold = -0.02f;
                 m_linearSlop = btScalar(0.0);
                 m_warmstartingFactor=btScalar(0.85);
-                m_solverMode = SOLVER_CACHE_FRIENDLY |  SOLVER_RANDMIZE_ORDER |  SOLVER_USE_WARMSTARTING;
+                m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD ;//SOLVER_RANDMIZE_ORDER
                 m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution
         }

code/branches/questsystem5/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;
+}
+
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------

code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h

@@ -29 +29 @@
 
 class btRigidBody;
+
+
 
 
@@ -93 +95 @@
     bool isLimited()
     {
-        if(m_loLimit>=m_hiLimit) return false;
+        if(m_loLimit > m_hiLimit) return false;
         return true;
     }
@@ -111 +113 @@
 
         //! apply the correction impulses for two bodies
-    btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
-
+    btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB);
 
 };
@@ -130 +131 @@
     btScalar    m_restitution;//! Bounce parameter for linear limit
     //!@}
+        bool            m_enableMotor[3];
+    btVector3   m_targetVelocity;//!< target motor velocity
+    btVector3   m_maxMotorForce;//!< max force on motor
+    btVector3   m_currentLimitError;//!  How much is violated this limit
+    int                 m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
 
     btTranslationalLimitMotor()
@@ -140 +146 @@
         m_damping = btScalar(1.0f);
         m_restitution = btScalar(0.5f);
+                for(int i=0; i < 3; i++) 
+                {
+                        m_enableMotor[i] = false;
+                        m_targetVelocity[i] = btScalar(0.f);
+                        m_maxMotorForce[i] = btScalar(0.f);
+                }
     }
 
@@ -151 +163 @@
         m_damping = other.m_damping;
         m_restitution = other.m_restitution;
+                for(int i=0; i < 3; i++) 
+                {
+                        m_enableMotor[i] = other.m_enableMotor[i];
+                        m_targetVelocity[i] = other.m_targetVelocity[i];
+                        m_maxMotorForce[i] = other.m_maxMotorForce[i];
+                }
     }
 
@@ -164 +182 @@
        return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
     }
+    inline bool needApplyForce(int limitIndex)
+    {
+        if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
+        return true;
+    }
+        int testLimitValue(int limitIndex, btScalar test_value);
 
 
@@ -169 +193 @@
         btScalar timeStep,
         btScalar jacDiagABInv,
-        btRigidBody& body1,const btVector3 &pointInA,
-        btRigidBody& body2,const btVector3 &pointInB,
+        btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
+        btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
         int limit_index,
         const btVector3 & axis_normal_on_a,
@@ -248 +272 @@
     btVector3 m_calculatedAxisAngleDiff;
     btVector3 m_calculatedAxis[3];
+    btVector3 m_calculatedLinearDiff;
     
         btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
@@ -263 +288 @@
 
 
+        int setAngularLimits(btConstraintInfo2 *info, int row_offset);
+
+        int setLinearLimits(btConstraintInfo2 *info);
 
     void buildLinearJacobian(
@@ -270 +298 @@
     void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
 
+        // tests linear limits
+        void calculateLinearInfo();
 
         //! calcs the euler angles between the two bodies.
@@ -277 +307 @@
 
 public:
+
+        ///for backwards compatibility during the transition to 'getInfo/getInfo2'
+        bool            m_useSolveConstraintObsolete;
+
     btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
 
@@ -331 +365 @@
     virtual void        buildJacobian();
 
-    virtual     void    solveConstraint(btScalar        timeStep);
+        virtual void getInfo1 (btConstraintInfo1* info);
+
+        virtual void getInfo2 (btConstraintInfo2* info);
+
+    virtual     void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
 
     void        updateRHS(btScalar      timeStep);
@@ -433 +471 @@
         virtual void calcAnchorPos(void); // overridable
 
+        int get_limit_motor_info2(      btRotationalLimitMotor * limot,
+                                                                btRigidBody * body0, btRigidBody * body1,
+                                                                btConstraintInfo2 *info, int row, btVector3& ax1, int rotational);
+
+
 };
 

code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

@@ -20 +20 @@
 #include "LinearMath/btMinMax.h"
 #include <new>
+#include "btSolverBody.h"
+
+//-----------------------------------------------------------------------------
+
+#define HINGE_USE_OBSOLETE_SOLVER false
+
+//-----------------------------------------------------------------------------
 
 
 btHingeConstraint::btHingeConstraint()
 : btTypedConstraint (HINGE_CONSTRAINT_TYPE),
-m_enableAngularMotor(false)
-{
-}
+m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(false)
+{
+        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
+}
+
+//-----------------------------------------------------------------------------
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
-                                                                         btVector3& axisInA,btVector3& axisInB)
+                                                                         btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA)
                                                                          :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
                                                                          m_angularOnly(false),
-                                                                         m_enableAngularMotor(false)
+                                                                         m_enableAngularMotor(false),
+                                                                         m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+                                                                         m_useReferenceFrameA(useReferenceFrameA)
 {
         m_rbAFrame.getOrigin() = pivotInA;
@@ -61 +75 @@
         
         m_rbBFrame.getOrigin() = pivotInB;
-        m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
-                                                                        rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
-                                                                        rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
+        m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
+                                                                        rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
+                                                                        rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
         
         //start with free
@@ -72 +86 @@
         m_limitSoftness = 0.9f;
         m_solveLimit = false;
-
-}
-
-
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
-:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false)
+        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
+}
+
+//-----------------------------------------------------------------------------
+
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA)
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), 
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
 {
 
@@ -91 +107 @@
                                                                         rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
 
-        btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;
+        btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
 
         btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
@@ -110 +126 @@
         m_limitSoftness = 0.9f;
         m_solveLimit = false;
-}
+        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
+}
+
+//-----------------------------------------------------------------------------
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 
-                                                                     const btTransform& rbAFrame, const btTransform& rbBFrame)
+                                                                     const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
 m_angularOnly(false),
-m_enableAngularMotor(false)
-{
-        // flip axis
-        m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
-        m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
-        m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
-
+m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
+{
         //start with free
         m_lowerLimit = btScalar(1e30);
@@ -130 +146 @@
         m_limitSoftness = 0.9f;
         m_solveLimit = false;
+        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
 }                       
 
-
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
+//-----------------------------------------------------------------------------
+
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
 m_angularOnly(false),
-m_enableAngularMotor(false)
+m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
 {
         ///not providing rigidbody B means implicitly using worldspace for body B
-
-        // flip axis
-        m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
-        m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
-        m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
 
         m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
@@ -155 +169 @@
         m_limitSoftness = 0.9f;
         m_solveLimit = false;
-}
+        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
+}
+
+//-----------------------------------------------------------------------------
 
 void    btHingeConstraint::buildJacobian()
 {
-        m_appliedImpulse = btScalar(0.);
-
-        if (!m_angularOnly)
-        {
-                btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-                btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
-                btVector3 relPos = pivotBInW - pivotAInW;
-
-                btVector3 normal[3];
-                if (relPos.length2() > SIMD_EPSILON)
-                {
-                        normal[0] = relPos.normalized();
-                }
-                else
-                {
-                        normal[0].setValue(btScalar(1.0),0,0);
-                }
-
-                btPlaneSpace1(normal[0], normal[1], normal[2]);
-
-                for (int i=0;i<3;i++)
-                {
-                        new (&m_jac[i]) btJacobianEntry(
+        if (m_useSolveConstraintObsolete)
+        {
+                m_appliedImpulse = btScalar(0.);
+
+                if (!m_angularOnly)
+                {
+                        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+                        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+                        btVector3 relPos = pivotBInW - pivotAInW;
+
+                        btVector3 normal[3];
+                        if (relPos.length2() > SIMD_EPSILON)
+                        {
+                                normal[0] = relPos.normalized();
+                        }
+                        else
+                        {
+                                normal[0].setValue(btScalar(1.0),0,0);
+                        }
+
+                        btPlaneSpace1(normal[0], normal[1], normal[2]);
+
+                        for (int i=0;i<3;i++)
+                        {
+                                new (&m_jac[i]) btJacobianEntry(
                                 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
                                 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
@@ -191 +210 @@
                                 m_rbB.getInvInertiaDiagLocal(),
                                 m_rbB.getInvMass());
+                        }
                 }
-        }
-
-        //calculate two perpendicular jointAxis, orthogonal to hingeAxis
-        //these two jointAxis require equal angular velocities for both bodies
-
-        //this is unused for now, it's a todo
-        btVector3 jointAxis0local;
-        btVector3 jointAxis1local;
-        
-        btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
-
-        getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
-        btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
-        btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
-        btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+
+                //calculate two perpendicular jointAxis, orthogonal to hingeAxis
+                //these two jointAxis require equal angular velocities for both bodies
+
+                //this is unused for now, it's a todo
+                btVector3 jointAxis0local;
+                btVector3 jointAxis1local;
                 
-        new (&m_jacAng[0])      btJacobianEntry(jointAxis0,
-                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-                m_rbA.getInvInertiaDiagLocal(),
-                m_rbB.getInvInertiaDiagLocal());
-
-        new (&m_jacAng[1])      btJacobianEntry(jointAxis1,
-                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-                m_rbA.getInvInertiaDiagLocal(),
-                m_rbB.getInvInertiaDiagLocal());
-
-        new (&m_jacAng[2])      btJacobianEntry(hingeAxisWorld,
-                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-                m_rbA.getInvInertiaDiagLocal(),
-                m_rbB.getInvInertiaDiagLocal());
-
-
+                btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
+
+                getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+                btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
+                btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
+                btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+                        
+                new (&m_jacAng[0])      btJacobianEntry(jointAxis0,
+                        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+                        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+                        m_rbA.getInvInertiaDiagLocal(),
+                        m_rbB.getInvInertiaDiagLocal());
+
+                new (&m_jacAng[1])      btJacobianEntry(jointAxis1,
+                        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+                        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+                        m_rbA.getInvInertiaDiagLocal(),
+                        m_rbB.getInvInertiaDiagLocal());
+
+                new (&m_jacAng[2])      btJacobianEntry(hingeAxisWorld,
+                        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+                        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+                        m_rbA.getInvInertiaDiagLocal(),
+                        m_rbB.getInvInertiaDiagLocal());
+
+                        // clear accumulator
+                        m_accLimitImpulse = btScalar(0.);
+
+                        // test angular limit
+                        testLimit();
+
+                //Compute K = J*W*J' for hinge axis
+                btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
+                m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
+                                                         getRigidBodyB().computeAngularImpulseDenominator(axisA));
+
+        }
+}
+
+//-----------------------------------------------------------------------------
+
+void btHingeConstraint::getInfo1(btConstraintInfo1* info)
+{
+        if (m_useSolveConstraintObsolete)
+        {
+                info->m_numConstraintRows = 0;
+                info->nub = 0;
+        }
+        else
+        {
+                info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
+                info->nub = 1; 
+                //prepare constraint
+                testLimit();
+                if(getSolveLimit() || getEnableAngularMotor())
+                {
+                        info->m_numConstraintRows++; // limit 3rd anguar as well
+                        info->nub--; 
+                }
+        }
+} // btHingeConstraint::getInfo1 ()
+
+//-----------------------------------------------------------------------------
+
+void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
+{
+        btAssert(!m_useSolveConstraintObsolete);
+        int i, s = info->rowskip;
+        // transforms in world space
+        btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
+        btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame;
+        // pivot point
+        btVector3 pivotAInW = trA.getOrigin();
+        btVector3 pivotBInW = trB.getOrigin();
+        // linear (all fixed)
+    info->m_J1linearAxis[0] = 1;
+    info->m_J1linearAxis[s + 1] = 1;
+    info->m_J1linearAxis[2 * s + 2] = 1;
+        btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin();
+        {
+                btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
+                btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s);
+                btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s);
+                btVector3 a1neg = -a1;
+                a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+        }
+        btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin();
+        {
+                btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
+                btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s);
+                btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s);
+                a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+        }
+        // linear RHS
+    btScalar k = info->fps * info->erp;
+        for(i = 0; i < 3; i++)
+    {
+        info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]);
+    }
+        // make rotations around X and Y equal
+        // the hinge axis should be the only unconstrained
+        // rotational axis, the angular velocity of the two bodies perpendicular to
+        // the hinge axis should be equal. thus the constraint equations are
+        //    p*w1 - p*w2 = 0
+        //    q*w1 - q*w2 = 0
+        // where p and q are unit vectors normal to the hinge axis, and w1 and w2
+        // are the angular velocity vectors of the two bodies.
+        // get hinge axis (Z)
+        btVector3 ax1 = trA.getBasis().getColumn(2);
+        // get 2 orthos to hinge axis (X, Y)
+        btVector3 p = trA.getBasis().getColumn(0);
+        btVector3 q = trA.getBasis().getColumn(1);
+        // set the two hinge angular rows 
+    int s3 = 3 * info->rowskip;
+    int s4 = 4 * info->rowskip;
+
+        info->m_J1angularAxis[s3 + 0] = p[0];
+        info->m_J1angularAxis[s3 + 1] = p[1];
+        info->m_J1angularAxis[s3 + 2] = p[2];
+        info->m_J1angularAxis[s4 + 0] = q[0];
+        info->m_J1angularAxis[s4 + 1] = q[1];
+        info->m_J1angularAxis[s4 + 2] = q[2];
+
+        info->m_J2angularAxis[s3 + 0] = -p[0];
+        info->m_J2angularAxis[s3 + 1] = -p[1];
+        info->m_J2angularAxis[s3 + 2] = -p[2];
+        info->m_J2angularAxis[s4 + 0] = -q[0];
+        info->m_J2angularAxis[s4 + 1] = -q[1];
+        info->m_J2angularAxis[s4 + 2] = -q[2];
+    // compute the right hand side of the constraint equation. set relative
+    // body velocities along p and q to bring the hinge back into alignment.
+    // if ax1,ax2 are the unit length hinge axes as computed from body1 and
+    // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
+    // if `theta' is the angle between ax1 and ax2, we need an angular velocity
+    // along u to cover angle erp*theta in one step :
+    //   |angular_velocity| = angle/time = erp*theta / stepsize
+    //                      = (erp*fps) * theta
+    //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
+    //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
+    // ...as ax1 and ax2 are unit length. if theta is smallish,
+    // theta ~= sin(theta), so
+    //    angular_velocity  = (erp*fps) * (ax1 x ax2)
+    // ax1 x ax2 is in the plane space of ax1, so we project the angular
+    // velocity to p and q to find the right hand side.
+    btVector3 ax2 = trB.getBasis().getColumn(2);
+        btVector3 u = ax1.cross(ax2);
+        info->m_constraintError[s3] = k * u.dot(p);
+        info->m_constraintError[s4] = k * u.dot(q);
+        // check angular limits
+        int nrow = 4; // last filled row
+        int srow;
+        btScalar limit_err = btScalar(0.0);
+        int limit = 0;
+        if(getSolveLimit())
+        {
+                limit_err = m_correction * m_referenceSign;
+                limit = (limit_err > btScalar(0.0)) ? 1 : 2;
+        }
+        // if the hinge has joint limits or motor, add in the extra row
+        int powered = 0;
+        if(getEnableAngularMotor())
+        {
+                powered = 1;
+        }
+        if(limit || powered) 
+        {
+                nrow++;
+                srow = nrow * info->rowskip;
+                info->m_J1angularAxis[srow+0] = ax1[0];
+                info->m_J1angularAxis[srow+1] = ax1[1];
+                info->m_J1angularAxis[srow+2] = ax1[2];
+
+                info->m_J2angularAxis[srow+0] = -ax1[0];
+                info->m_J2angularAxis[srow+1] = -ax1[1];
+                info->m_J2angularAxis[srow+2] = -ax1[2];
+
+                btScalar lostop = getLowerLimit();
+                btScalar histop = getUpperLimit();
+                if(limit && (lostop == histop))
+                {  // the joint motor is ineffective
+                        powered = 0;
+                }
+                info->m_constraintError[srow] = btScalar(0.0f);
+                if(powered)
+                {
+            info->cfm[srow] = btScalar(0.0); 
+                        btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp);
+                        info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
+                        info->m_lowerLimit[srow] = - m_maxMotorImpulse;
+                        info->m_upperLimit[srow] =   m_maxMotorImpulse;
+                }
+                if(limit)
+                {
+                        k = info->fps * info->erp;
+                        info->m_constraintError[srow] += k * limit_err;
+                        info->cfm[srow] = btScalar(0.0);
+                        if(lostop == histop) 
+                        {
+                                // limited low and high simultaneously
+                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
+                                info->m_upperLimit[srow] = SIMD_INFINITY;
+                        }
+                        else if(limit == 1) 
+                        { // low limit
+                                info->m_lowerLimit[srow] = 0;
+                                info->m_upperLimit[srow] = SIMD_INFINITY;
+                        }
+                        else 
+                        { // high limit
+                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
+                                info->m_upperLimit[srow] = 0;
+                        }
+                        // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
+                        btScalar bounce = m_relaxationFactor;
+                        if(bounce > btScalar(0.0))
+                        {
+                                btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
+                                vel -= m_rbB.getAngularVelocity().dot(ax1);
+                                // only apply bounce if the velocity is incoming, and if the
+                                // resulting c[] exceeds what we already have.
+                                if(limit == 1)
+                                {       // low limit
+                                        if(vel < 0)
+                                        {
+                                                btScalar newc = -bounce * vel;
+                                                if(newc > info->m_constraintError[srow])
+                                                {
+                                                        info->m_constraintError[srow] = newc;
+                                                }
+                                        }
+                                }
+                                else
+                                {       // high limit - all those computations are reversed
+                                        if(vel > 0)
+                                        {
+                                                btScalar newc = -bounce * vel;
+                                                if(newc < info->m_constraintError[srow])
+                                                {
+                                                        info->m_constraintError[srow] = newc;
+                                                }
+                                        }
+                                }
+                        }
+                        info->m_constraintError[srow] *= m_biasFactor;
+                } // if(limit)
+        } // if angular limit or powered
+}
+
+//-----------------------------------------------------------------------------
+
+void    btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar     timeStep)
+{
+
+        ///for backwards compatibility during the transition to 'getInfo/getInfo2'
+        if (m_useSolveConstraintObsolete)
+        {
+
+                btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+                btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+
+                btScalar tau = btScalar(0.3);
+
+                //linear part
+                if (!m_angularOnly)
+                {
+                        btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+                        btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+
+                        btVector3 vel1,vel2;
+                        bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+                        bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+                        btVector3 vel = vel1 - vel2;
+
+                        for (int i=0;i<3;i++)
+                        {               
+                                const btVector3& normal = m_jac[i].m_linearJointAxis;
+                                btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+
+                                btScalar rel_vel;
+                                rel_vel = normal.dot(vel);
+                                //positional error (zeroth order error)
+                                btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+                                btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
+                                m_appliedImpulse += impulse;
+                                btVector3 impulse_vector = normal * impulse;
+                                btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
+                                btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
+                                bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
+                                bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
+                        }
+                }
+
+                
+                {
+                        ///solve angular part
+
+                        // get axes in world space
+                        btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
+                        btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
+
+                        btVector3 angVelA;
+                        bodyA.getAngularVelocity(angVelA);
+                        btVector3 angVelB;
+                        bodyB.getAngularVelocity(angVelB);
+
+                        btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
+                        btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
+
+                        btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
+                        btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
+                        btVector3 velrelOrthog = angAorthog-angBorthog;
+                        {
+                                
+
+                                //solve orthogonal angular velocity correction
+                                btScalar relaxation = btScalar(1.);
+                                btScalar len = velrelOrthog.length();
+                                if (len > btScalar(0.00001))
+                                {
+                                        btVector3 normal = velrelOrthog.normalized();
+                                        btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
+                                                getRigidBodyB().computeAngularImpulseDenominator(normal);
+                                        // scale for mass and relaxation
+                                        //velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
+
+                                        bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom));
+                                        bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom));
+
+                                }
+
+                                //solve angular positional correction
+                                btVector3 angularError =  axisA.cross(axisB) *(btScalar(1.)/timeStep);
+                                btScalar len2 = angularError.length();
+                                if (len2>btScalar(0.00001))
+                                {
+                                        btVector3 normal2 = angularError.normalized();
+                                        btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
+                                                        getRigidBodyB().computeAngularImpulseDenominator(normal2);
+                                        //angularError *= (btScalar(1.)/denom2) * relaxation;
+                                        
+                                        bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2));
+                                        bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2));
+
+                                }
+                                
+                                
+
+
+
+                                // solve limit
+                                if (m_solveLimit)
+                                {
+                                        btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;
+
+                                        btScalar impulseMag = amplitude * m_kHinge;
+
+                                        // Clamp the accumulated impulse
+                                        btScalar temp = m_accLimitImpulse;
+                                        m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
+                                        impulseMag = m_accLimitImpulse - temp;
+
+
+                                        
+                                        bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign);
+                                        bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign));
+
+                                }
+                        }
+
+                        //apply motor
+                        if (m_enableAngularMotor) 
+                        {
+                                //todo: add limits too
+                                btVector3 angularLimit(0,0,0);
+
+                                btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
+                                btScalar projRelVel = velrel.dot(axisA);
+
+                                btScalar desiredMotorVel = m_motorTargetVelocity;
+                                btScalar motor_relvel = desiredMotorVel - projRelVel;
+
+                                btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
+                                //todo: should clip against accumulated impulse
+                                btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
+                                clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
+                                btVector3 motorImp = clippedMotorImpulse * axisA;
+                        
+                                bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse);
+                                bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse);
+                                
+                        }
+                }
+        }
+
+}
+
+//-----------------------------------------------------------------------------
+
+void    btHingeConstraint::updateRHS(btScalar   timeStep)
+{
+        (void)timeStep;
+
+}
+
+//-----------------------------------------------------------------------------
+
+btScalar btHingeConstraint::getHingeAngle()
+{
+        const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
+        const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
+        const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
+        btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
+        return m_referenceSign * angle;
+}
+
+//-----------------------------------------------------------------------------
+
+void btHingeConstraint::testLimit()
+{
         // Compute limit information
-        btScalar hingeAngle = getHingeAngle();  
-
-        //set bias, sign, clear accumulator
+        m_hingeAngle = getHingeAngle();  
         m_correction = btScalar(0.);
         m_limitSign = btScalar(0.);
         m_solveLimit = false;
-        m_accLimitImpulse = btScalar(0.);
-
-//      if (m_lowerLimit < m_upperLimit)
         if (m_lowerLimit <= m_upperLimit)
         {
-//              if (hingeAngle <= m_lowerLimit*m_limitSoftness)
-                if (hingeAngle <= m_lowerLimit)
-                {
-                        m_correction = (m_lowerLimit - hingeAngle);
+                if (m_hingeAngle <= m_lowerLimit)
+                {
+                        m_correction = (m_lowerLimit - m_hingeAngle);
                         m_limitSign = 1.0f;
                         m_solveLimit = true;
                 } 
-//              else if (hingeAngle >= m_upperLimit*m_limitSoftness)
-                else if (hingeAngle >= m_upperLimit)
-                {
-                        m_correction = m_upperLimit - hingeAngle;
+                else if (m_hingeAngle >= m_upperLimit)
+                {
+                        m_correction = m_upperLimit - m_hingeAngle;
                         m_limitSign = -1.0f;
                         m_solveLimit = true;
                 }
         }
-
-        //Compute K = J*W*J' for hinge axis
-        btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
-        m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
-                                     getRigidBodyB().computeAngularImpulseDenominator(axisA));
-
-}
-
-void    btHingeConstraint::solveConstraint(btScalar     timeStep)
-{
-
-        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
-
-        btScalar tau = btScalar(0.3);
-
-        //linear part
-        if (!m_angularOnly)
-        {
-                btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
-                btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
-
-                btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
-                btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
-                btVector3 vel = vel1 - vel2;
-
-                for (int i=0;i<3;i++)
-                {               
-                        const btVector3& normal = m_jac[i].m_linearJointAxis;
-                        btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
-
-                        btScalar rel_vel;
-                        rel_vel = normal.dot(vel);
-                        //positional error (zeroth order error)
-                        btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
-                        btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
-                        m_appliedImpulse += impulse;
-                        btVector3 impulse_vector = normal * impulse;
-                        m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
-                        m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
-                }
-        }
-
-        
-        {
-                ///solve angular part
-
-                // get axes in world space
-                btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
-                btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
-
-                const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
-                const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
-
-                btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
-                btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
-
-                btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
-                btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
-                btVector3 velrelOrthog = angAorthog-angBorthog;
-                {
-                        //solve orthogonal angular velocity correction
-                        btScalar relaxation = btScalar(1.);
-                        btScalar len = velrelOrthog.length();
-                        if (len > btScalar(0.00001))
-                        {
-                                btVector3 normal = velrelOrthog.normalized();
-                                btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
-                                        getRigidBodyB().computeAngularImpulseDenominator(normal);
-                                // scale for mass and relaxation
-                                velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
-                        }
-
-                        //solve angular positional correction
-                        btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep);
-                        btScalar len2 = angularError.length();
-                        if (len2>btScalar(0.00001))
-                        {
-                                btVector3 normal2 = angularError.normalized();
-                                btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
-                                                getRigidBodyB().computeAngularImpulseDenominator(normal2);
-                                angularError *= (btScalar(1.)/denom2) * relaxation;
-                        }
-
-                        m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
-                        m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
-
-                        // solve limit
-                        if (m_solveLimit)
-                        {
-                                btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;
-
-                                btScalar impulseMag = amplitude * m_kHinge;
-
-                                // Clamp the accumulated impulse
-                                btScalar temp = m_accLimitImpulse;
-                                m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
-                                impulseMag = m_accLimitImpulse - temp;
-
-
-                                btVector3 impulse = axisA * impulseMag * m_limitSign;
-                                m_rbA.applyTorqueImpulse(impulse);
-                                m_rbB.applyTorqueImpulse(-impulse);
-                        }
-                }
-
-                //apply motor
-                if (m_enableAngularMotor) 
-                {
-                        //todo: add limits too
-                        btVector3 angularLimit(0,0,0);
-
-                        btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
-                        btScalar projRelVel = velrel.dot(axisA);
-
-                        btScalar desiredMotorVel = m_motorTargetVelocity;
-                        btScalar motor_relvel = desiredMotorVel - projRelVel;
-
-                        btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
-                        //todo: should clip against accumulated impulse
-                        btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
-                        clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
-                        btVector3 motorImp = clippedMotorImpulse * axisA;
-
-                        m_rbA.applyTorqueImpulse(motorImp+angularLimit);
-                        m_rbB.applyTorqueImpulse(-motorImp-angularLimit);
-                        
-                }
-        }
-
-}
-
-void    btHingeConstraint::updateRHS(btScalar   timeStep)
-{
-        (void)timeStep;
-
-}
-
-btScalar btHingeConstraint::getHingeAngle()
-{
-        const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
-        const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
-        const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
-
-        return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)  );
-}
-
+        return;
+} // btHingeConstraint::testLimit()
+
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------

code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h

@@ -54 +54 @@
 
         btScalar        m_accLimitImpulse;
+        btScalar        m_hingeAngle;
+        btScalar    m_referenceSign;
 
         bool            m_angularOnly;
         bool            m_enableAngularMotor;
         bool            m_solveLimit;
+        bool            m_useSolveConstraintObsolete;
+        bool            m_useReferenceFrameA;
 
         
 public:
 
-        btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB);
+        btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA = false);
 
-        btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA);
+        btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA = false);
         
-        btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
+        btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
 
-        btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
+        btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
 
         btHingeConstraint();
@@ -74 +78 @@
         virtual void    buildJacobian();
 
-        virtual void    solveConstraint(btScalar        timeStep);
+        virtual void getInfo1 (btConstraintInfo1* info);
+
+        virtual void getInfo2 (btConstraintInfo2* info);
+        
+        virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
 
         void    updateRHS(btScalar      timeStep);
@@ -87 +95 @@
         }
 
+        btRigidBody& getRigidBodyA()    
+        {               
+                return m_rbA;   
+        }       
+
+        btRigidBody& getRigidBodyB()    
+        {               
+                return m_rbB;   
+        }       
+        
         void    setAngularOnly(bool angularOnly)
         {
@@ -123 +141 @@
         btScalar getHingeAngle();
 
+        void testLimit();
+
 
         const btTransform& getAFrame() { return m_rbAFrame; };  

code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp

@@ -22 +22 @@
 
 btPoint2PointConstraint::btPoint2PointConstraint()
-:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE)
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE),
+m_useSolveConstraintObsolete(false)
 {
 }
 
 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
-:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB)
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
+m_useSolveConstraintObsolete(false)
 {
 
@@ -34 +36 @@
 
 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
-:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA))
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
+m_useSolveConstraintObsolete(false)
 {
         
@@ -41 +44 @@
 void    btPoint2PointConstraint::buildJacobian()
 {
-        m_appliedImpulse = btScalar(0.);
-
-        btVector3       normal(0,0,0);
-
-        for (int i=0;i<3;i++)
-        {
-                normal[i] = 1;
-                new (&m_jac[i]) btJacobianEntry(
+        ///we need it for both methods
+        {
+                m_appliedImpulse = btScalar(0.);
+
+                btVector3       normal(0,0,0);
+
+                for (int i=0;i<3;i++)
+                {
+                        normal[i] = 1;
+                        new (&m_jac[i]) btJacobianEntry(
                         m_rbA.getCenterOfMassTransform().getBasis().transpose(),
                         m_rbB.getCenterOfMassTransform().getBasis().transpose(),
@@ -59 +64 @@
                         m_rbB.getInvMass());
                 normal[i] = 0;
-        }
-
-}
-
-void    btPoint2PointConstraint::solveConstraint(btScalar       timeStep)
-{
-        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
-        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
-
-
-        btVector3 normal(0,0,0);
-        
-
-//      btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
-//      btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
-
-        for (int i=0;i<3;i++)
-        {               
-                normal[i] = 1;
-                btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
-
-                btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
-                btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
-                //this jacobian entry could be re-used for all iterations
-                
-                btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
-                btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
-                btVector3 vel = vel1 - vel2;
-                
-                btScalar rel_vel;
-                rel_vel = normal.dot(vel);
-
-        /*
-                //velocity error (first order error)
-                btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
-                                                                                                                m_rbB.getLinearVelocity(),angvelB);
+                }
+        }
+
+}
+
+
+void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
+{
+        if (m_useSolveConstraintObsolete)
+        {
+                info->m_numConstraintRows = 0;
+                info->nub = 0;
+        } else
+        {
+                info->m_numConstraintRows = 3;
+                info->nub = 3;
+        }
+}
+
+void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
+{
+        btAssert(!m_useSolveConstraintObsolete);
+
+         //retrieve matrices
+        btTransform body0_trans;
+        body0_trans = m_rbA.getCenterOfMassTransform();
+    btTransform body1_trans;
+        body1_trans = m_rbB.getCenterOfMassTransform();
+
+        // anchor points in global coordinates with respect to body PORs.
+   
+    // set jacobian
+    info->m_J1linearAxis[0] = 1;
+    info->m_J1linearAxis[info->rowskip+1] = 1;
+    info->m_J1linearAxis[2*info->rowskip+2] = 1;
+
+        btVector3 a1 = body0_trans.getBasis()*getPivotInA();
+        {
+                btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
+                btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
+                btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
+                btVector3 a1neg = -a1;
+                a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+        }
+    
+        /*info->m_J2linearAxis[0] = -1;
+    info->m_J2linearAxis[s+1] = -1;
+    info->m_J2linearAxis[2*s+2] = -1;
         */
         
-                //positional error (zeroth order error)
-                btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+        btVector3 a2 = body1_trans.getBasis()*getPivotInB();
+   
+        {
+                btVector3 a2n = -a2;
+                btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
+                btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
+                btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
+                a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+        }
+    
+
+
+    // set right hand side
+    btScalar k = info->fps * info->erp;
+    int j;
+
+        for (j=0; j<3; j++)
+    {
+        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] -                     a1[j] - body0_trans.getOrigin()[j]);
+                //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
+    }
+
+        btScalar impulseClamp = m_setting.m_impulseClamp;//
+        for (j=0; j<3; j++)
+    {
+                if (m_setting.m_impulseClamp > 0)
+                {
+                        info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
+                        info->m_upperLimit[j*info->rowskip] = impulseClamp;
+                }
+        }
+        
+}
+
+
+void    btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar       timeStep)
+{
+        if (m_useSolveConstraintObsolete)
+        {
+                btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
+                btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
+
+
+                btVector3 normal(0,0,0);
                 
-                btScalar impulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
-
-                btScalar impulseClamp = m_setting.m_impulseClamp;
-                if (impulseClamp > 0)
-                {
-                        if (impulse < -impulseClamp) 
-                                impulse = -impulseClamp;
-                        if (impulse > impulseClamp) 
-                                impulse = impulseClamp;
+
+        //      btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
+        //      btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
+
+                for (int i=0;i<3;i++)
+                {               
+                        normal[i] = 1;
+                        btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+
+                        btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+                        btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+                        //this jacobian entry could be re-used for all iterations
+                        
+                        btVector3 vel1,vel2;
+                        bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+                        bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+                        btVector3 vel = vel1 - vel2;
+                        
+                        btScalar rel_vel;
+                        rel_vel = normal.dot(vel);
+
+                /*
+                        //velocity error (first order error)
+                        btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
+                                                                                                                        m_rbB.getLinearVelocity(),angvelB);
+                */
+                
+                        //positional error (zeroth order error)
+                        btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+                        
+                        btScalar deltaImpulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
+
+                        btScalar impulseClamp = m_setting.m_impulseClamp;
+                        
+                        const btScalar sum = btScalar(m_appliedImpulse) + deltaImpulse;
+                        if (sum < -impulseClamp)
+                        {
+                                deltaImpulse = -impulseClamp-m_appliedImpulse;
+                                m_appliedImpulse = -impulseClamp;
+                        }
+                        else if (sum > impulseClamp) 
+                        {
+                                deltaImpulse = impulseClamp-m_appliedImpulse;
+                                m_appliedImpulse = impulseClamp;
+                        }
+                        else
+                        {
+                                m_appliedImpulse = sum;
+                        }
+
+                        
+                        btVector3 impulse_vector = normal * deltaImpulse;
+                        
+                        btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
+                        btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
+                        bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,deltaImpulse);
+                        bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-deltaImpulse);
+
+
+                        normal[i] = 0;
                 }
-
-                m_appliedImpulse+=impulse;
-                btVector3 impulse_vector = normal * impulse;
-                m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
-                m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
-                
-                normal[i] = 0;
         }
 }

code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h

@@ -51 +51 @@
 public:
 
+        ///for backwards compatibility during the transition to 'getInfo/getInfo2'
+        bool            m_useSolveConstraintObsolete;
+
         btConstraintSetting     m_setting;
 
@@ -61 +64 @@
         virtual void    buildJacobian();
 
+        virtual void getInfo1 (btConstraintInfo1* info);
 
-        virtual void    solveConstraint(btScalar        timeStep);
+        virtual void getInfo2 (btConstraintInfo2* info);
+
+
+        virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
 
         void    updateRHS(btScalar      timeStep);

code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -16 +16 @@
 //#define COMPUTE_IMPULSE_DENOM 1
 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
-//#define FORCE_REFESH_CONTACT_MANIFOLDS 1
 
 #include "btSequentialImpulseConstraintSolver.h"
@@ -33 +32 @@
 #include "btSolverBody.h"
 #include "btSolverConstraint.h"
-
-
 #include "LinearMath/btAlignedObjectArray.h"
-
-
-int totalCpd = 0;
-
-int     gTotalContactPoints = 0;
-
-struct  btOrderIndex
-{
-        int     m_manifoldIndex;
-        int     m_pointIndex;
-};
-
-
-
-#define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384
-static btOrderIndex     gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
+#include <string.h> //for memset
+
+btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
+:m_btSeed2(0)
+{
+
+}
+
+btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
+{
+}
+
+#ifdef USE_SIMD
+#include <emmintrin.h>
+#define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
+static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 )
+{
+        __m128 result = _mm_mul_ps( vec0, vec1);
+        return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) );
+}
+#endif//USE_SIMD
+
+// Project Gauss Seidel or the equivalent Sequential Impulse
+void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+#ifdef USE_SIMD
+        __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
+        __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+        __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+        __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
+        __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
+        __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
+        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+        btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
+        btSimdScalar resultLowerLess,resultUpperLess;
+        resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
+        resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
+        __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
+        deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
+        c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
+        __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
+        deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
+        c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
+        __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
+        __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
+        __m128 impulseMagnitude = deltaImpulse;
+        body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
+        body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
+        body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
+        body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+#else
+        resolveSingleConstraintRowGeneric(body1,body2,c);
+#endif
+}
+
+// Project Gauss Seidel or the equivalent Sequential Impulse
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+        btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
+        const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.m_deltaLinearVelocity)      + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
+        const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
+
+        const btScalar delta_rel_vel    =       deltaVel1Dotn-deltaVel2Dotn;
+        deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
+        deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
+
+        const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
+        if (sum < c.m_lowerLimit)
+        {
+                deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
+                c.m_appliedImpulse = c.m_lowerLimit;
+        }
+        else if (sum > c.m_upperLimit) 
+        {
+                deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
+                c.m_appliedImpulse = c.m_upperLimit;
+        }
+        else
+        {
+                c.m_appliedImpulse = sum;
+        }
+        if (body1.m_invMass)
+                body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
+        if (body2.m_invMass)
+                body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
+}
+
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+#ifdef USE_SIMD
+        __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
+        __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+        __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+        __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
+        __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
+        __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
+        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+        btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
+        btSimdScalar resultLowerLess,resultUpperLess;
+        resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
+        resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
+        __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
+        deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
+        c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
+        __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
+        __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
+        __m128 impulseMagnitude = deltaImpulse;
+        body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
+        body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
+        body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
+        body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+#else
+        resolveSingleConstraintRowLowerLimit(body1,body2,c);
+#endif
+}
+
+// Project Gauss Seidel or the equivalent Sequential Impulse
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+        btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
+        const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.m_deltaLinearVelocity)      + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
+        const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
+
+        deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
+        deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
+        const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
+        if (sum < c.m_lowerLimit)
+        {
+                deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
+                c.m_appliedImpulse = c.m_lowerLimit;
+        }
+        else
+        {
+                c.m_appliedImpulse = sum;
+        }
+        if (body1.m_invMass)
+                body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
+        if (body2.m_invMass)
+                body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
+}
+
 
 
 unsigned long btSequentialImpulseConstraintSolver::btRand2()
 {
-  m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
-  return m_btSeed2;
+        m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
+        return m_btSeed2;
 }
 
@@ -65 +189 @@
 int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
 {
-  // seems good; xor-fold and modulus
-  const unsigned long un = static_cast<unsigned long>(n);
-  unsigned long r = btRand2();
-
-  // note: probably more aggressive than it needs to be -- might be
-  //       able to get away without one or two of the innermost branches.
-  if (un <= 0x00010000UL) {
-    r ^= (r >> 16);
-    if (un <= 0x00000100UL) {
-      r ^= (r >> 8);
-      if (un <= 0x00000010UL) {
-        r ^= (r >> 4);
-        if (un <= 0x00000004UL) {
-          r ^= (r >> 2);
-          if (un <= 0x00000002UL) {
-            r ^= (r >> 1);
-          }
-        }
-     }
-    }
-   }
-
-  return (int) (r % un);
-}
-
-
-
-
-bool  MyContactDestroyedCallback(void* userPersistentData);
-bool  MyContactDestroyedCallback(void* userPersistentData)
-{
-        assert (userPersistentData);
-        btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData;
-        btAlignedFree(cpd);
-        totalCpd--;
-        //printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData);
-        return true;
-}
-
-
-
-btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
-:m_btSeed2(0)
-{
-        gContactDestroyedCallback = &MyContactDestroyedCallback;
-
-        //initialize default friction/contact funcs
-        int i,j;
-        for (i=0;i<MAX_CONTACT_SOLVER_TYPES;i++)
-                for (j=0;j<MAX_CONTACT_SOLVER_TYPES;j++)
-                {
-
-                        m_contactDispatch[i][j] = resolveSingleCollision;
-                        m_frictionDispatch[i][j] = resolveSingleFriction;
-                }
-}
-
-btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
-{
-
-}
-
-void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
-void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
-{
-        btRigidBody* rb = btRigidBody::upcast(collisionObject);
+        // seems good; xor-fold and modulus
+        const unsigned long un = static_cast<unsigned long>(n);
+        unsigned long r = btRand2();
+
+        // note: probably more aggressive than it needs to be -- might be
+        //       able to get away without one or two of the innermost branches.
+        if (un <= 0x00010000UL) {
+                r ^= (r >> 16);
+                if (un <= 0x00000100UL) {
+                        r ^= (r >> 8);
+                        if (un <= 0x00000010UL) {
+                                r ^= (r >> 4);
+                                if (un <= 0x00000004UL) {
+                                        r ^= (r >> 2);
+                                        if (un <= 0x00000002UL) {
+                                                r ^= (r >> 1);
+                                        }
+                                }
+                        }
+                }
+        }
+
+        return (int) (r % un);
+}
+
+
+
+void    btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
+{
+        btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
+
+        solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
+        solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
+
         if (rb)
         {
-                solverBody->m_angularVelocity = rb->getAngularVelocity() ;
-                solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
-                solverBody->m_friction = collisionObject->getFriction();
                 solverBody->m_invMass = rb->getInvMass();
-                solverBody->m_linearVelocity = rb->getLinearVelocity();
                 solverBody->m_originalBody = rb;
                 solverBody->m_angularFactor = rb->getAngularFactor();
         } else
         {
-                solverBody->m_angularVelocity.setValue(0,0,0);
-                solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
-                solverBody->m_friction = collisionObject->getFriction();
                 solverBody->m_invMass = 0.f;
-                solverBody->m_linearVelocity.setValue(0,0,0);
                 solverBody->m_originalBody = 0;
                 solverBody->m_angularFactor = 1.f;
         }
-        solverBody->m_pushVelocity.setValue(0.f,0.f,0.f);
-        solverBody->m_turnVelocity.setValue(0.f,0.f,0.f);
 }
 
@@ -157 +239 @@
 int             gNumSplitImpulseRecoveries = 0;
 
-btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
-btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
+btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
 {
         btScalar rest = restitution * -rel_vel;
@@ -165 +246 @@
 
 
-void    resolveSplitPenetrationImpulseCacheFriendly(
-        btSolverBody& body1,
-        btSolverBody& body2,
-        const btSolverConstraint& contactConstraint,
-        const btContactSolverInfo& solverInfo);
-
-//SIMD_FORCE_INLINE
-void    resolveSplitPenetrationImpulseCacheFriendly(
-        btSolverBody& body1,
-        btSolverBody& body2,
-        const btSolverConstraint& contactConstraint,
-        const btContactSolverInfo& solverInfo)
-{
-        (void)solverInfo;
-
-                if (contactConstraint.m_penetration < solverInfo.m_splitImpulsePenetrationThreshold)
-        {
-
-                                gNumSplitImpulseRecoveries++;
-                btScalar normalImpulse;
-
-                //  Optimized version of projected relative velocity, use precomputed cross products with normal
-                //      body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
-                //      body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
-                //      btVector3 vel = vel1 - vel2;
-                //      btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
-
-                btScalar rel_vel;
-                btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_pushVelocity)
-                + contactConstraint.m_relpos1CrossNormal.dot(body1.m_turnVelocity);
-                btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_pushVelocity)
-                + contactConstraint.m_relpos2CrossNormal.dot(body2.m_turnVelocity);
-
-                rel_vel = vel1Dotn-vel2Dotn;
-
-
-                                btScalar positionalError = -contactConstraint.m_penetration * solverInfo.m_erp2/solverInfo.m_timeStep;
-                //      btScalar positionalError = contactConstraint.m_penetration;
-
-                btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
-
-                btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
-                btScalar        velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
-                normalImpulse = penetrationImpulse+velocityImpulse;
-
-                // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
-                btScalar oldNormalImpulse = contactConstraint.m_appliedPushImpulse;
-                btScalar sum = oldNormalImpulse + normalImpulse;
-                contactConstraint.m_appliedPushImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
-
-                normalImpulse = contactConstraint.m_appliedPushImpulse - oldNormalImpulse;
-
-                                body1.internalApplyPushImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse);
-               
-                                body2.internalApplyPushImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse);
-               
-        }
-
-}
-
-
-//velocity + friction
-//response  between two dynamic objects with friction
-
-btScalar resolveSingleCollisionCombinedCacheFriendly(
-        btSolverBody& body1,
-        btSolverBody& body2,
-        const btSolverConstraint& contactConstraint,
-        const btContactSolverInfo& solverInfo);
-
-//SIMD_FORCE_INLINE 
-btScalar resolveSingleCollisionCombinedCacheFriendly(
-        btSolverBody& body1,
-        btSolverBody& body2,
-        const btSolverConstraint& contactConstraint,
-        const btContactSolverInfo& solverInfo)
-{
-        (void)solverInfo;
-
-        btScalar normalImpulse;
-
-        {
-
-                
-                //  Optimized version of projected relative velocity, use precomputed cross products with normal
-                //      body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
-                //      body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
-                //      btVector3 vel = vel1 - vel2;
-                //      btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
-
-                btScalar rel_vel;
-                btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) 
-                                        + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
-                btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) 
-                                        + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
-
-                rel_vel = vel1Dotn-vel2Dotn;
-
-                btScalar positionalError = 0.f;
-                if (!solverInfo.m_splitImpulse || (contactConstraint.m_penetration > solverInfo.m_splitImpulsePenetrationThreshold))
-                {
-                        positionalError = -contactConstraint.m_penetration * solverInfo.m_erp/solverInfo.m_timeStep;
-                }
-
-                btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
-
-                btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
-                btScalar        velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
-                normalImpulse = penetrationImpulse+velocityImpulse;
-                
-                
-                // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
-                btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
-                btScalar sum = oldNormalImpulse + normalImpulse;
-                contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
-
-                normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
-
-                body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
-                                contactConstraint.m_angularComponentA,normalImpulse);
-                
-                body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
-                                contactConstraint.m_angularComponentB,-normalImpulse);
-        }
-
-        return normalImpulse;
-}
-
-
-#ifndef NO_FRICTION_TANGENTIALS
-
-btScalar resolveSingleFrictionCacheFriendly(
-        btSolverBody& body1,
-        btSolverBody& body2,
-        const btSolverConstraint& contactConstraint,
-        const btContactSolverInfo& solverInfo,
-        btScalar appliedNormalImpulse);
-
-//SIMD_FORCE_INLINE 
-btScalar resolveSingleFrictionCacheFriendly(
-        btSolverBody& body1,
-        btSolverBody& body2,
-        const btSolverConstraint& contactConstraint,
-        const btContactSolverInfo& solverInfo,
-        btScalar appliedNormalImpulse)
-{
-        (void)solverInfo;
-
-        
-        const btScalar combinedFriction = contactConstraint.m_friction;
-        
-        const btScalar limit = appliedNormalImpulse * combinedFriction;
-        
-        if (appliedNormalImpulse>btScalar(0.))
-        //friction
-        {
-                
-                btScalar j1;
-                {
-
-                        btScalar rel_vel;
-                        const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) 
-                                                + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
-                        const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) 
-                                + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
-                        rel_vel = vel1Dotn-vel2Dotn;
-
-                        // calculate j that moves us to zero relative velocity
-                        j1 = -rel_vel * contactConstraint.m_jacDiagABInv;
-#define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1
-#ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE
-                        btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse;
-                        contactConstraint.m_appliedImpulse = oldTangentImpulse + j1;
-                        
-                        if (limit < contactConstraint.m_appliedImpulse)
-                        {
-                                contactConstraint.m_appliedImpulse = limit;
-                        } else
-                        {
-                                if (contactConstraint.m_appliedImpulse < -limit)
-                                        contactConstraint.m_appliedImpulse = -limit;
-                        }
-                        j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse;
-#else
-                        if (limit < j1)
-                        {
-                                j1 = limit;
-                        } else
-                        {
-                                if (j1 < -limit)
-                                        j1 = -limit;
-                        }
-
-#endif //CLAMP_ACCUMULATED_FRICTION_IMPULSE
-
-                        //GEN_set_min(contactConstraint.m_appliedImpulse, limit);
-                        //GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
-
-                        
-
-                }
-        
-                body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
-                
-                body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
-
-        } 
-        return 0.f;
-}
-
-
-#else
-
-//velocity + friction
-//response  between two dynamic objects with friction
-btScalar resolveSingleFrictionCacheFriendly(
-        btSolverBody& body1,
-        btSolverBody& body2,
-        btSolverConstraint& contactConstraint,
-        const btContactSolverInfo& solverInfo)
-{
-
-        btVector3 vel1;
-        btVector3 vel2;
-        btScalar normalImpulse(0.f);
-
-        {
-                const btVector3& normal = contactConstraint.m_contactNormal;
-                if (contactConstraint.m_penetration < 0.f)
-                        return 0.f;
-
-
-                body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
-                body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
-                btVector3 vel = vel1 - vel2;
-                btScalar rel_vel;
-                rel_vel = normal.dot(vel);
-
-                btVector3 lat_vel = vel - normal * rel_vel;
-                btScalar lat_rel_vel = lat_vel.length2();
-
-                btScalar combinedFriction = contactConstraint.m_friction;
-                const btVector3& rel_pos1 = contactConstraint.m_rel_posA;
-                const btVector3& rel_pos2 = contactConstraint.m_rel_posB;
-
-
-                if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON)
-                {
-                        lat_rel_vel = btSqrt(lat_rel_vel);
-
-                        lat_vel /= lat_rel_vel;
-                        btVector3 temp1 = body1.m_invInertiaWorld * rel_pos1.cross(lat_vel);
-                        btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel);
-                        btScalar friction_impulse = lat_rel_vel /
-                                (body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
-                        btScalar normal_impulse = contactConstraint.m_appliedImpulse * combinedFriction;
-
-                        GEN_set_min(friction_impulse, normal_impulse);
-                        GEN_set_max(friction_impulse, -normal_impulse);
-                        body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
-                        body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
-                }
-        }
-
-        return normalImpulse;
-}
-
-#endif //NO_FRICTION_TANGENTIALS
-
-
+
+void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
+void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
+{
+        if (colObj && colObj->hasAnisotropicFriction())
+        {
+                // transform to local coordinates
+                btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
+                const btVector3& friction_scaling = colObj->getAnisotropicFriction();
+                //apply anisotropic friction
+                loc_lateral *= friction_scaling;
+                // ... and transform it back to global coordinates
+                frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
+        }
+}
 
 
@@ -440 +266 @@
 btSolverConstraint&     btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation)
 {
+
 
         btRigidBody* body0=btRigidBody::upcast(colObj0);
         btRigidBody* body1=btRigidBody::upcast(colObj1);
 
-        btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand();
+        btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand();
+        memset(&solverConstraint,0xff,sizeof(btSolverConstraint));
         solverConstraint.m_contactNormal = normalAxis;
 
         solverConstraint.m_solverBodyIdA = solverBodyIdA;
         solverConstraint.m_solverBodyIdB = solverBodyIdB;
-        solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
         solverConstraint.m_frictionIndex = frictionIndex;
 
@@ -455 +282 @@
         solverConstraint.m_originalContactPoint = 0;
 
-        solverConstraint.m_appliedImpulse = btScalar(0.);
-        solverConstraint.m_appliedPushImpulse = 0.f;
-        solverConstraint.m_penetration = 0.f;
+        solverConstraint.m_appliedImpulse = 0.f;
+        //      solverConstraint.m_appliedPushImpulse = 0.f;
+
         {
                 btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
                 solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
-                solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
-        }
-        {
-                btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
+                solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
+        }
+        {
+                btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
                 solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
-                solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
+                solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
         }
 
@@ -483 +310 @@
         if (body1)
         {
-                vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
+                vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
                 denom1 = body1->getInvMass() + normalAxis.dot(vec);
         }
@@ -492 +319 @@
         solverConstraint.m_jacDiagABInv = denom;
 
+#ifdef _USE_JACOBIAN
+        solverConstraint.m_jac =  btJacobianEntry (
+                rel_pos1,rel_pos2,solverConstraint.m_contactNormal,
+                body0->getInvInertiaDiagLocal(),
+                body0->getInvMass(),
+                body1->getInvInertiaDiagLocal(),
+                body1->getInvMass());
+#endif //_USE_JACOBIAN
+
+
+        {
+                btScalar rel_vel;
+                btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0)) 
+                        + solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0));
+                btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0)) 
+                        + solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0));
+
+                rel_vel = vel1Dotn+vel2Dotn;
+
+                btScalar positionalError = 0.f;
+
+                btSimdScalar velocityError =  - rel_vel;
+                btSimdScalar    velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
+                solverConstraint.m_rhs = velocityImpulse;
+                solverConstraint.m_cfm = 0.f;
+                solverConstraint.m_lowerLimit = 0;
+                solverConstraint.m_upperLimit = 1e10f;
+        }
+
         return solverConstraint;
 }
 
+int     btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
+{
+        int solverBodyIdA = -1;
+
+        if (body.getCompanionId() >= 0)
+        {
+                //body has already been converted
+                solverBodyIdA = body.getCompanionId();
+        } else
+        {
+                btRigidBody* rb = btRigidBody::upcast(&body);
+                if (rb && rb->getInvMass())
+                {
+                        solverBodyIdA = m_tmpSolverBodyPool.size();
+                        btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+                        initSolverBody(&solverBody,&body);
+                        body.setCompanionId(solverBodyIdA);
+                } else
+                {
+                        return 0;//assume first one is a fixed solver body
+                }
+        }
+        return solverBodyIdA;
+}
+#include <stdio.h>
+
+
+
+void    btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
+{
+        btCollisionObject* colObj0=0,*colObj1=0;
+
+        colObj0 = (btCollisionObject*)manifold->getBody0();
+        colObj1 = (btCollisionObject*)manifold->getBody1();
+
+        int solverBodyIdA=-1;
+        int solverBodyIdB=-1;
+
+        if (manifold->getNumContacts())
+        {
+                solverBodyIdA = getOrInitSolverBody(*colObj0);
+                solverBodyIdB = getOrInitSolverBody(*colObj1);
+        }
+
+        ///avoid collision response between two static objects
+        if (!solverBodyIdA && !solverBodyIdB)
+                return;
+
+        btVector3 rel_pos1;
+        btVector3 rel_pos2;
+        btScalar relaxation;
+
+        for (int j=0;j<manifold->getNumContacts();j++)
+        {
+
+                btManifoldPoint& cp = manifold->getContactPoint(j);
+
+                if (cp.getDistance() <= manifold->getContactProcessingThreshold())
+                {
+
+                        const btVector3& pos1 = cp.getPositionWorldOnA();
+                        const btVector3& pos2 = cp.getPositionWorldOnB();
+
+                        rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
+                        rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
+
+
+                        relaxation = 1.f;
+                        btScalar rel_vel;
+                        btVector3 vel;
+
+                        int frictionIndex = m_tmpSolverContactConstraintPool.size();
+
+                        {
+                                btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand();
+                                btRigidBody* rb0 = btRigidBody::upcast(colObj0);
+                                btRigidBody* rb1 = btRigidBody::upcast(colObj1);
+
+                                solverConstraint.m_solverBodyIdA = solverBodyIdA;
+                                solverConstraint.m_solverBodyIdB = solverBodyIdB;
+
+                                solverConstraint.m_originalContactPoint = &cp;
+
+                                btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
+                                solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
+                                btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);            
+                                solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+                                {
+#ifdef COMPUTE_IMPULSE_DENOM
+                                        btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
+                                        btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
+#else                                                   
+                                        btVector3 vec;
+                                        btScalar denom0 = 0.f;
+                                        btScalar denom1 = 0.f;
+                                        if (rb0)
+                                        {
+                                                vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+                                                denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
+                                        }
+                                        if (rb1)
+                                        {
+                                                vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
+                                                denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
+                                        }
+#endif //COMPUTE_IMPULSE_DENOM          
+
+                                        btScalar denom = relaxation/(denom0+denom1);
+                                        solverConstraint.m_jacDiagABInv = denom;
+                                }
+
+                                solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
+                                solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
+                                solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB);
+
+
+                                btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
+                                btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
+
+                                vel  = vel1 - vel2;
+
+                                rel_vel = cp.m_normalWorldOnB.dot(vel);
+
+                                btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
+
+
+                                solverConstraint.m_friction = cp.m_combinedFriction;
+
+                                btScalar restitution = 0.f;
+                                
+                                if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
+                                {
+                                        restitution = 0.f;
+                                } else
+                                {
+                                        restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
+                                        if (restitution <= btScalar(0.))
+                                        {
+                                                restitution = 0.f;
+                                        };
+                                }
+
+
+                                ///warm starting (or zero if disabled)
+                                if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+                                {
+                                        solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
+                                        if (rb0)
+                                                m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
+                                        if (rb1)
+                                                m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
+                                } else
+                                {
+                                        solverConstraint.m_appliedImpulse = 0.f;
+                                }
+
+                                //                                                      solverConstraint.m_appliedPushImpulse = 0.f;
+
+                                {
+                                        btScalar rel_vel;
+                                        btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0)) 
+                                                + solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
+                                        btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0)) 
+                                                + solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
+
+                                        rel_vel = vel1Dotn+vel2Dotn;
+
+                                        btScalar positionalError = 0.f;
+                                        positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
+                                        btScalar        velocityError = restitution - rel_vel;// * damping;
+                                        btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
+                                        btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+                                        solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+                                        solverConstraint.m_cfm = 0.f;
+                                        solverConstraint.m_lowerLimit = 0;
+                                        solverConstraint.m_upperLimit = 1e10f;
+                                }
+
+
+                                /////setup the friction constraints
+
+
+
+                                if (1)
+                                {
+                                        solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
+                                        if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
+                                        {
+                                                cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
+                                                btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
+                                                if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
+                                                {
+                                                        cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
+                                                        applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
+                                                        applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
+                                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                                        if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+                                                        {
+                                                                cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
+                                                                cp.m_lateralFrictionDir2.normalize();//??
+                                                                applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
+                                                                applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
+                                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                                        }
+                                                        cp.m_lateralFrictionInitialized = true;
+                                                } else
+                                                {
+                                                        //re-calculate friction direction every frame, todo: check if this is really needed
+                                                        btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
+                                                        applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
+                                                        applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
+
+                                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                                        if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+                                                        {
+                                                                applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
+                                                                applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
+                                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                                        }
+                                                        cp.m_lateralFrictionInitialized = true;
+                                                }
+
+                                        } else
+                                        {
+                                                addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                                if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+                                                        addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                        }
+
+                                        if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
+                                        {
+                                                {
+                                                        btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
+                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+                                                        {
+                                                                frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
+                                                                if (rb0)
+                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
+                                                                if (rb1)
+                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
+                                                        } else
+                                                        {
+                                                                frictionConstraint1.m_appliedImpulse = 0.f;
+                                                        }
+                                                }
+
+                                                if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+                                                {
+                                                        btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
+                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+                                                        {
+                                                                frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
+                                                                if (rb0)
+                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
+                                                                if (rb1)
+                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
+                                                        } else
+                                                        {
+                                                                frictionConstraint2.m_appliedImpulse = 0.f;
+                                                        }
+                                                }
+                                        } else
+                                        {
+                                                btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
+                                                frictionConstraint1.m_appliedImpulse = 0.f;
+                                                if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+                                                {
+                                                        btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
+                                                        frictionConstraint2.m_appliedImpulse = 0.f;
+                                                }
+                                        }
+                                }
+                        }
+
+
+                }
+        }
+}
 
 
@@ -506 +640 @@
         if (!(numConstraints + numManifolds))
         {
-//              printf("empty\n");
+                //              printf("empty\n");
                 return 0.f;
         }
-        btPersistentManifold* manifold = 0;
-        btCollisionObject* colObj0=0,*colObj1=0;
-
-        //btRigidBody* rb0=0,*rb1=0;
-
-
-#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
-
-        BEGIN_PROFILE("refreshManifolds");
-
-        int i;
-        
-        
-
-        for (i=0;i<numManifolds;i++)
-        {
-                manifold = manifoldPtr[i];
-                rb1 = (btRigidBody*)manifold->getBody1();
-                rb0 = (btRigidBody*)manifold->getBody0();
-                
-                manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
-
-        }
-
-        END_PROFILE("refreshManifolds");
-#endif //FORCE_REFESH_CONTACT_MANIFOLDS
-
-        
-
-
-
-        //int sizeofSB = sizeof(btSolverBody);
-        //int sizeofSC = sizeof(btSolverConstraint);
-
-
-        //if (1)
-        {
-                //if m_stackAlloc, try to pack bodies/constraints to speed up solving
-//              btBlock*                                        sablock;
-//              sablock = stackAlloc->beginBlock();
-
-        //      int memsize = 16;
-//              unsigned char* stackMemory = stackAlloc->allocate(memsize);
-
-                
-                //todo: use stack allocator for this temp memory
-//              int minReservation = numManifolds*2;
-
-                //m_tmpSolverBodyPool.reserve(minReservation);
-
-                //don't convert all bodies, only the one we need so solver the constraints
-/*
-                {
-                        for (int i=0;i<numBodies;i++)
-                        {
-                                btRigidBody* rb = btRigidBody::upcast(bodies[i]);
-                                if (rb &&       (rb->getIslandTag() >= 0))
-                                {
-                                        btAssert(rb->getCompanionId() < 0);
-                                        int solverBodyId = m_tmpSolverBodyPool.size();
-                                        btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-                                        initSolverBody(&solverBody,rb);
-                                        rb->setCompanionId(solverBodyId);
-                                } 
-                        }
-                }
-*/
-                
-                //m_tmpSolverConstraintPool.reserve(minReservation);
-                //m_tmpSolverFrictionConstraintPool.reserve(minReservation);
-
-                {
-                        int i;
-
-                        for (i=0;i<numManifolds;i++)
-                        {
-                                manifold = manifoldPtr[i];
-                                colObj0 = (btCollisionObject*)manifold->getBody0();
-                                colObj1 = (btCollisionObject*)manifold->getBody1();
-                        
-                                int solverBodyIdA=-1;
-                                int solverBodyIdB=-1;
-
-                                if (manifold->getNumContacts())
-                                {
-
-                                        
-
-                                        if (colObj0->getIslandTag() >= 0)
-                                        {
-                                                if (colObj0->getCompanionId() >= 0)
-                                                {
-                                                        //body has already been converted
-                                                        solverBodyIdA = colObj0->getCompanionId();
-                                                } else
-                                                {
-                                                        solverBodyIdA = m_tmpSolverBodyPool.size();
-                                                        btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-                                                        initSolverBody(&solverBody,colObj0);
-                                                        colObj0->setCompanionId(solverBodyIdA);
-                                                }
-                                        } else
-                                        {
-                                                //create a static body
-                                                solverBodyIdA = m_tmpSolverBodyPool.size();
-                                                btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-                                                initSolverBody(&solverBody,colObj0);
-                                        }
-
-                                        if (colObj1->getIslandTag() >= 0)
-                                        {
-                                                if (colObj1->getCompanionId() >= 0)
-                                                {
-                                                        solverBodyIdB = colObj1->getCompanionId();
-                                                } else
-                                                {
-                                                        solverBodyIdB = m_tmpSolverBodyPool.size();
-                                                        btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-                                                        initSolverBody(&solverBody,colObj1);
-                                                        colObj1->setCompanionId(solverBodyIdB);
-                                                }
-                                        } else
-                                        {
-                                                //create a static body
-                                                solverBodyIdB = m_tmpSolverBodyPool.size();
-                                                btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-                                                initSolverBody(&solverBody,colObj1);
-                                        }
-                                }
-
-                                btVector3 rel_pos1;
-                                btVector3 rel_pos2;
-                                btScalar relaxation;
-
-                                for (int j=0;j<manifold->getNumContacts();j++)
-                                {
-                                        
-                                        btManifoldPoint& cp = manifold->getContactPoint(j);
-                                        
-                                        if (cp.getDistance() <= btScalar(0.))
-                                        {
-                                                
-                                                const btVector3& pos1 = cp.getPositionWorldOnA();
-                                                const btVector3& pos2 = cp.getPositionWorldOnB();
-
-                                                 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
-                                                 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
-
-                                                
-                                                relaxation = 1.f;
-                                                btScalar rel_vel;
-                                                btVector3 vel;
-
-                                                int frictionIndex = m_tmpSolverConstraintPool.size();
-
-                                                {
-                                                        btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand();
-                                                        btRigidBody* rb0 = btRigidBody::upcast(colObj0);
-                                                        btRigidBody* rb1 = btRigidBody::upcast(colObj1);
-
-                                                        solverConstraint.m_solverBodyIdA = solverBodyIdA;
-                                                        solverConstraint.m_solverBodyIdB = solverBodyIdB;
-                                                        solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D;
-
-                                                        solverConstraint.m_originalContactPoint = &cp;
-
-                                                        btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
-                                                        solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0);
-                                                        btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);            
-                                                        solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1 : btVector3(0,0,0);
-                                                        {
-#ifdef COMPUTE_IMPULSE_DENOM
-                                                                btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
-                                                                btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
-#else                                                   
-                                                                btVector3 vec;
-                                                                btScalar denom0 = 0.f;
-                                                                btScalar denom1 = 0.f;
-                                                                if (rb0)
-                                                                {
-                                                                        vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
-                                                                        denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
-                                                                }
-                                                                if (rb1)
-                                                                {
-                                                                        vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
-                                                                        denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
-                                                                }
-#endif //COMPUTE_IMPULSE_DENOM          
-                                                                
-                                                                btScalar denom = relaxation/(denom0+denom1);
-                                                                solverConstraint.m_jacDiagABInv = denom;
-                                                        }
-
-                                                        solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
-                                                        solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
-                                                        solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB);
-
-
-                                                        btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
-                                                        btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
-                                
-                                                        vel  = vel1 - vel2;
-                                                        
-                                                        rel_vel = cp.m_normalWorldOnB.dot(vel);
-                                                        
-                                                        solverConstraint.m_penetration = btMin(cp.getDistance()+infoGlobal.m_linearSlop,btScalar(0.));
-                                                        //solverConstraint.m_penetration = cp.getDistance();
-
-                                                        solverConstraint.m_friction = cp.m_combinedFriction;
-
-                                                        
-                                                        if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
-                                                        {
-                                                                solverConstraint.m_restitution = 0.f;
-                                                        } else
-                                                        {
-                                                                solverConstraint.m_restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
-                                                                if (solverConstraint.m_restitution <= btScalar(0.))
-                                                                {
-                                                                        solverConstraint.m_restitution = 0.f;
-                                                                };
-                                                        }
-
-                                                        
-                                                        btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep;
-
-                                                        
-
-                                                        if (solverConstraint.m_restitution > penVel)
-                                                        {
-                                                                solverConstraint.m_penetration = btScalar(0.);
-                                                        }
-                                                         
-                                                        
-                                                        
-                                                        ///warm starting (or zero if disabled)
-                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-                                                        {
-                                                                solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
-                                                                if (rb0)
-                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
-                                                                if (rb1)
-                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
-                                                        } else
-                                                        {
-                                                                solverConstraint.m_appliedImpulse = 0.f;
-                                                        }
-
-                                                        solverConstraint.m_appliedPushImpulse = 0.f;
-                                                        
-                                                        solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size();
-                                                        if (!cp.m_lateralFrictionInitialized)
-                                                        {
-                                                                cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
-                                                                btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
-                                                                if (lat_rel_vel > SIMD_EPSILON)//0.0f)
-                                                                {
-                                                                        cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
-                                                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-                                                                        if(infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
-                                                                        {
-                                                                                cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
-                                                                                cp.m_lateralFrictionDir2.normalize();//??
-                                                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-                                                                                cp.m_lateralFrictionInitialized = true;
-                                                                        }
-                                                                } else
-                                                                {
-                                                                        //re-calculate friction direction every frame, todo: check if this is really needed
-                                                                        btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
-                                                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-                                                                        addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-                                                                        if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
-                                                                        {
-                                                                                cp.m_lateralFrictionInitialized = true;
-                                                                        }
-                                                                }
-                                                                
-                                                        } else
-                                                        {
-                                                                addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-                                                                if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
-                                                                        addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-                                                        }
-
-                                                        if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
-                                                        {
-                                                                {
-                                                                        btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex];
-                                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-                                                                        {
-                                                                                frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
-                                                                                if (rb0)
-                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
-                                                                                if (rb1)
-                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
-                                                                        } else
-                                                                        {
-                                                                                frictionConstraint1.m_appliedImpulse = 0.f;
-                                                                        }
-                                                                }
-                                                                {
-                                                                        btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
-                                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-                                                                        {
-                                                                                frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
-                                                                                if (rb0)
-                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
-                                                                                if (rb1)
-                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
-                                                                        } else
-                                                                        {
-                                                                                frictionConstraint2.m_appliedImpulse = 0.f;
-                                                                        }
-                                                                }
-                                                        }
-                                                }
-
-
-                                        }
-                                }
-                        }
-                }
-        }
-        
-        btContactSolverInfo info = infoGlobal;
-
+
+        if (1)
         {
                 int j;
@@ -845 +653 @@
                 }
         }
-        
-        
-
-        int numConstraintPool = m_tmpSolverConstraintPool.size();
-        int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
+
+        btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+        initSolverBody(&fixedBody,0);
+
+        //btRigidBody* rb0=0,*rb1=0;
+
+        //if (1)
+        {
+                {
+
+                        int totalNumRows = 0;
+                        int i;
+                        //calculate the total number of contraint rows
+                        for (i=0;i<numConstraints;i++)
+                        {
+
+                                btTypedConstraint::btConstraintInfo1 info1;
+                                constraints[i]->getInfo1(&info1);
+                                totalNumRows += info1.m_numConstraintRows;
+                        }
+                        m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
+
+                        btTypedConstraint::btConstraintInfo1 info1;
+                        info1.m_numConstraintRows = 0;
+
+
+                        ///setup the btSolverConstraints
+                        int currentRow = 0;
+
+                        for (i=0;i<numConstraints;i++,currentRow+=info1.m_numConstraintRows)
+                        {
+                                constraints[i]->getInfo1(&info1);
+                                if (info1.m_numConstraintRows)
+                                {
+                                        btAssert(currentRow<totalNumRows);
+
+                                        btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
+                                        btTypedConstraint* constraint = constraints[i];
+
+
+
+                                        btRigidBody& rbA = constraint->getRigidBodyA();
+                                        btRigidBody& rbB = constraint->getRigidBodyB();
+
+                                        int solverBodyIdA = getOrInitSolverBody(rbA);
+                                        int solverBodyIdB = getOrInitSolverBody(rbB);
+
+                                        btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
+                                        btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
+
+                                        int j;
+                                        for ( j=0;j<info1.m_numConstraintRows;j++)
+                                        {
+                                                memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
+                                                currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
+                                                currentConstraintRow[j].m_upperLimit = FLT_MAX;
+                                                currentConstraintRow[j].m_appliedImpulse = 0.f;
+                                                currentConstraintRow[j].m_appliedPushImpulse = 0.f;
+                                                currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
+                                                currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
+                                        }
+
+                                        bodyAPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
+                                        bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
+                                        bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
+                                        bodyBPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
+
+
+
+                                        btTypedConstraint::btConstraintInfo2 info2;
+                                        info2.fps = 1.f/infoGlobal.m_timeStep;
+                                        info2.erp = infoGlobal.m_erp;
+                                        info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
+                                        info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
+                                        info2.m_J2linearAxis = 0;
+                                        info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
+                                        info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
+                                        ///the size of btSolverConstraint needs be a multiple of btScalar
+                                        btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
+                                        info2.m_constraintError = &currentConstraintRow->m_rhs;
+                                        info2.cfm = &currentConstraintRow->m_cfm;
+                                        info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
+                                        info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
+                                        constraints[i]->getInfo2(&info2);
+
+                                        ///finalize the constraint setup
+                                        for ( j=0;j<info1.m_numConstraintRows;j++)
+                                        {
+                                                btSolverConstraint& solverConstraint = currentConstraintRow[j];
+
+                                                {
+                                                        const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
+                                                        solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
+                                                }
+                                                {
+                                                        const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
+                                                        solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
+                                                }
+
+                                                {
+                                                        btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
+                                                        btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
+                                                        btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
+                                                        btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
+
+                                                        btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
+                                                        sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
+                                                        sum += iMJlB.dot(solverConstraint.m_contactNormal);
+                                                        sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
+
+                                                        solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
+                                                }
+
+
+                                                ///fix rhs
+                                                ///todo: add force/torque accelerators
+                                                {
+                                                        btScalar rel_vel;
+                                                        btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
+                                                        btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
+
+                                                        rel_vel = vel1Dotn+vel2Dotn;
+
+                                                        btScalar restitution = 0.f;
+                                                        btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
+                                                        btScalar        velocityError = restitution - rel_vel;// * damping;
+                                                        btScalar        penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
+                                                        btScalar        velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+                                                        solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+                                                        solverConstraint.m_appliedImpulse = 0.f;
+
+                                                }
+                                        }
+                                }
+                        }
+                }
+
+                {
+                        int i;
+                        btPersistentManifold* manifold = 0;
+                        btCollisionObject* colObj0=0,*colObj1=0;
+
+
+                        for (i=0;i<numManifolds;i++)
+                        {
+                                manifold = manifoldPtr[i];
+                                convertContact(manifold,infoGlobal);
+                        }
+                }
+        }
+
+        btContactSolverInfo info = infoGlobal;
+
+
+
+        int numConstraintPool = m_tmpSolverContactConstraintPool.size();
+        int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
 
         ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
@@ -866 +826 @@
         }
 
-
-
         return 0.f;
 
@@ -875 +833 @@
 {
         BT_PROFILE("solveGroupCacheFriendlyIterations");
-        int numConstraintPool = m_tmpSolverConstraintPool.size();
-        int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
+
+        int numConstraintPool = m_tmpSolverContactConstraintPool.size();
+        int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
 
         //should traverse the contacts random order...
@@ -904 +863 @@
                         }
 
-                        for (j=0;j<numConstraints;j++)
+                        if (infoGlobal.m_solverMode & SOLVER_SIMD)
                         {
-                                btTypedConstraint* constraint = constraints[j];
-                                ///todo: use solver bodies, so we don't need to copy from/to btRigidBody
-
-                                if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
-                                {
-                                        m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
-                                }
-                                if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
-                                {
-                                        m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
-                                }
-
-                                constraint->solveConstraint(infoGlobal.m_timeStep);
-
-                                if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
-                                {
-                                        m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
-                                }
-                                if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
-                                {
-                                        m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
-                                }
-
+                                ///solve all joint constraints, using SIMD, if available
+                                for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+                                {
+                                        btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
+                                        resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+                                }
+
+                                for (j=0;j<numConstraints;j++)
+                                {
+                                        int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
+                                        int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
+                                        btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
+                                        btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
+                                        constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
+                                }
+
+                                ///solve all contact constraints using SIMD, if available
+                                int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+                                for (j=0;j<numPoolConstraints;j++)
+                                {
+                                        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+                                        resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+
+                                }
+                                ///solve all friction constraints, using SIMD, if available
+                                int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+                                for (j=0;j<numFrictionPoolConstraints;j++)
+                                {
+                                        btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+                                        btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+                                        if (totalImpulse>btScalar(0))
+                                        {
+                                                solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+                                                solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+                                                resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],       m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+                                        }
+                                }
+                        } else
+                        {
+
+                                ///solve all joint constraints
+                                for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+                                {
+                                        btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
+                                        resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+                                }
+
+                                for (j=0;j<numConstraints;j++)
+                                {
+                                        int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
+                                        int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
+                                        btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
+                                        btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
+
+                                        constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
+                                }
+
+                                ///solve all contact constraints
+                                int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+                                for (j=0;j<numPoolConstraints;j++)
+                                {
+                                        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+                                        resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+                                }
+                                ///solve all friction constraints
+                                int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+                                for (j=0;j<numFrictionPoolConstraints;j++)
+                                {
+                                        btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+                                        btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+                                        if (totalImpulse>btScalar(0))
+                                        {
+                                                solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+                                                solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+                                                resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],                                                   m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+                                        }
+                                }
                         }
 
-                        {
-                                int numPoolConstraints = m_tmpSolverConstraintPool.size();
-                                for (j=0;j<numPoolConstraints;j++)
-                                {
-                                        
-                                        const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
-                                        resolveSingleCollisionCombinedCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
-                                                m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
-                                }
-                        }
-
-                        {
-                                 int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size();
-                                
-                                 for (j=0;j<numFrictionPoolConstraints;j++)
-                                {
-                                        const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
-                                        btScalar totalImpulse = m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse+
-                                                                m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedPushImpulse;                  
-
-                                                resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
-                                                        m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
-                                                        totalImpulse);
-                                }
-                        }
-                        
-
-
-                }
+
+
+                }
+        }
+        return 0.f;
+}
+
+
+
+/// btSequentialImpulseConstraintSolver Sequentially applies impulses
+btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
+{
+
         
-                if (infoGlobal.m_splitImpulse)
-                {
-                        
-                        for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
-                        {
-                                {
-                                        int numPoolConstraints = m_tmpSolverConstraintPool.size();
-                                        int j;
-                                        for (j=0;j<numPoolConstraints;j++)
-                                        {
-                                                const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
-
-                                                resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
-                                                        m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
-                                        }
-                                }
-                        }
-
-                }
-
-        }
-
-        return 0.f;
-}
-
-
-btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
-{
+
+        BT_PROFILE("solveGroup");
+        //we only implement SOLVER_CACHE_FRIENDLY now
+        //you need to provide at least some bodies
+        btAssert(bodies);
+        btAssert(numBodies);
+
         int i;
 
@@ -994 +974 @@
         solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
 
-        int numPoolConstraints = m_tmpSolverConstraintPool.size();
+        int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
         int j;
+
         for (j=0;j<numPoolConstraints;j++)
         {
-                
-                const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
+
+                const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
                 btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
                 btAssert(pt);
@@ -1005 +986 @@
                 if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
                 {
-                        pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
-                        pt->m_appliedImpulseLateral2 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
+                        pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+                        pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
                 }
 
                 //do a callback here?
-
         }
 
@@ -1022 +1002 @@
         {
                 for ( i=0;i<m_tmpSolverBodyPool.size();i++)
-        {
-                m_tmpSolverBodyPool[i].writebackVelocity();
-        }
-        }
-
-//      printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
-
-/*
-        printf("m_tmpSolverBodyPool.size() = %i\n",m_tmpSolverBodyPool.size());
-        printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
-        printf("m_tmpSolverFrictionConstraintPool.size() = %i\n",m_tmpSolverFrictionConstraintPool.size());
-
-        
-        printf("m_tmpSolverBodyPool.capacity() = %i\n",m_tmpSolverBodyPool.capacity());
-        printf("m_tmpSolverConstraintPool.capacity() = %i\n",m_tmpSolverConstraintPool.capacity());
-        printf("m_tmpSolverFrictionConstraintPool.capacity() = %i\n",m_tmpSolverFrictionConstraintPool.capacity());
-*/
+                {
+                        m_tmpSolverBodyPool[i].writebackVelocity();
+                }
+        }
+
 
         m_tmpSolverBodyPool.resize(0);
-        m_tmpSolverConstraintPool.resize(0);
-        m_tmpSolverFrictionConstraintPool.resize(0);
-
+        m_tmpSolverContactConstraintPool.resize(0);
+        m_tmpSolverNonContactConstraintPool.resize(0);
+        m_tmpSolverContactFrictionConstraintPool.resize(0);
 
         return 0.f;
 }
 
-/// btSequentialImpulseConstraintSolver Sequentially applies impulses
-btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
-{
-        BT_PROFILE("solveGroup");
-        if (infoGlobal.m_solverMode & SOLVER_CACHE_FRIENDLY)
-        {
-                //you need to provide at least some bodies
-                //btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY
-                btAssert(bodies);
-                btAssert(numBodies);
-                return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
-        }
-
-        
-
-        btContactSolverInfo info = infoGlobal;
-
-        int numiter = infoGlobal.m_numIterations;
-
-        int totalPoints = 0;
-
-
-        {
-                short j;
-                for (j=0;j<numManifolds;j++)
-                {
-                        btPersistentManifold* manifold = manifoldPtr[j];
-                        prepareConstraints(manifold,info,debugDrawer);
-
-                        for (short p=0;p<manifoldPtr[j]->getNumContacts();p++)
-                        {
-                                gOrder[totalPoints].m_manifoldIndex = j;
-                                gOrder[totalPoints].m_pointIndex = p;
-                                totalPoints++;
-                        }
-                }
-        }
-
-        {
-                int j;
-                for (j=0;j<numConstraints;j++)
-                {
-                        btTypedConstraint* constraint = constraints[j];
-                        constraint->buildJacobian();
-                }
-        }
-        
-
-        //should traverse the contacts random order...
-        int iteration;
-
-        {
-                for ( iteration = 0;iteration<numiter;iteration++)
-                {
-                        int j;
-                        if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
-                        {
-                                if ((iteration & 7) == 0) {
-                                        for (j=0; j<totalPoints; ++j) {
-                                                btOrderIndex tmp = gOrder[j];
-                                                int swapi = btRandInt2(j+1);
-                                                gOrder[j] = gOrder[swapi];
-                                                gOrder[swapi] = tmp;
-                                        }
-                                }
-                        }
-
-                        for (j=0;j<numConstraints;j++)
-                        {
-                                btTypedConstraint* constraint = constraints[j];
-                                constraint->solveConstraint(info.m_timeStep);
-                        }
-
-                        for (j=0;j<totalPoints;j++)
-                        {
-                                btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
-                                solve( (btRigidBody*)manifold->getBody0(),
-                                                                        (btRigidBody*)manifold->getBody1()
-                                ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
-                        }
-                
-                        for (j=0;j<totalPoints;j++)
-                        {
-                                btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
-                                solveFriction((btRigidBody*)manifold->getBody0(),
-                                        (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
-                        }
-                        
-                }
-        }
-                
-
-
-
-        return btScalar(0.);
-}
-
-
-
-
-
-
-
-void    btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer)
-{
-
-        (void)debugDrawer;
-
-        btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
-        btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
-
-
-        //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
-        {
-#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
-                manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
-#endif //FORCE_REFESH_CONTACT_MANIFOLDS         
-                int numpoints = manifoldPtr->getNumContacts();
-
-                gTotalContactPoints += numpoints;
-
-                
-                for (int i=0;i<numpoints ;i++)
-                {
-                        btManifoldPoint& cp = manifoldPtr->getContactPoint(i);
-                        if (cp.getDistance() <= btScalar(0.))
-                        {
-                                const btVector3& pos1 = cp.getPositionWorldOnA();
-                                const btVector3& pos2 = cp.getPositionWorldOnB();
-
-                                btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); 
-                                btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
-                                
-
-                                //this jacobian entry is re-used for all iterations
-                                btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
-                                        body1->getCenterOfMassTransform().getBasis().transpose(),
-                                        rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
-                                        body1->getInvInertiaDiagLocal(),body1->getInvMass());
-
-                                
-                                btScalar jacDiagAB = jac.getDiagonal();
-
-                                btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
-                                if (cpd)
-                                {
-                                        //might be invalid
-                                        cpd->m_persistentLifeTime++;
-                                        if (cpd->m_persistentLifeTime != cp.getLifeTime())
-                                        {
-                                                //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
-                                                new (cpd) btConstraintPersistentData;
-                                                cpd->m_persistentLifeTime = cp.getLifeTime();
-
-                                        } else
-                                        {
-                                                //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
-                                                
-                                        }
-                                } else
-                                {
-                                                
-                                        //todo: should this be in a pool?
-                                        void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16);
-                                        cpd = new (mem)btConstraintPersistentData;
-                                        assert(cpd);
-
-                                        totalCpd ++;
-                                        //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
-                                        cp.m_userPersistentData = cpd;
-                                        cpd->m_persistentLifeTime = cp.getLifeTime();
-                                        //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
-                                        
-                                }
-                                assert(cpd);
-
-                                cpd->m_jacDiagABInv = btScalar(1.) / jacDiagAB;
-
-                                //Dependent on Rigidbody A and B types, fetch the contact/friction response func
-                                //perhaps do a similar thing for friction/restutution combiner funcs...
-                                
-                                cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType];
-                                cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType];
-                                
-                                btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
-                                btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
-                                btVector3 vel = vel1 - vel2;
-                                btScalar rel_vel;
-                                rel_vel = cp.m_normalWorldOnB.dot(vel);
-                                
-                                btScalar combinedRestitution = cp.m_combinedRestitution;
-                                
-                                cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations);
-                                cpd->m_friction = cp.m_combinedFriction;
-                                if (cp.m_lifeTime>info.m_restingContactRestitutionThreshold)
-                                {
-                                        cpd->m_restitution = 0.f;
-                                } else
-                                {
-                                        cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
-                                        if (cpd->m_restitution <= btScalar(0.))
-                                        {
-                                                cpd->m_restitution = btScalar(0.0);
-                                        };
-                                }
-                                
-                                //restitution and penetration work in same direction so
-                                //rel_vel 
-                                
-                                btScalar penVel = -cpd->m_penetration/info.m_timeStep;
-
-                                if (cpd->m_restitution > penVel)
-                                {
-                                        cpd->m_penetration = btScalar(0.);
-                                }                               
-                                
-
-                                btScalar relaxation = info.m_damping;
-                                if (info.m_solverMode & SOLVER_USE_WARMSTARTING)
-                                {
-                                        cpd->m_appliedImpulse *= relaxation;
-                                } else
-                                {
-                                        cpd->m_appliedImpulse =btScalar(0.);
-                                }
-        
-                                //for friction
-                                cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
-                                
-                                //re-calculate friction direction every frame, todo: check if this is really needed
-                                btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);
-
-
-#define NO_FRICTION_WARMSTART 1
-
-        #ifdef NO_FRICTION_WARMSTART
-                                cpd->m_accumulatedTangentImpulse0 = btScalar(0.);
-                                cpd->m_accumulatedTangentImpulse1 = btScalar(0.);
-        #endif //NO_FRICTION_WARMSTART
-                                btScalar denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0);
-                                btScalar denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0);
-                                btScalar denom = relaxation/(denom0+denom1);
-                                cpd->m_jacDiagABInvTangent0 = denom;
-
-
-                                denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
-                                denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
-                                denom = relaxation/(denom0+denom1);
-                                cpd->m_jacDiagABInvTangent1 = denom;
-
-                                btVector3 totalImpulse = 
-        #ifndef NO_FRICTION_WARMSTART
-                                        cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
-                                        cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
-        #endif //NO_FRICTION_WARMSTART
-                                        cp.m_normalWorldOnB*cpd->m_appliedImpulse;
-
-
-
-                                ///
-                                {
-                                btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
-                                cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0;
-                                btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);            
-                                cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1;
-                                }
-                                {
-                                        btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0);
-                                        cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0;
-                                }
-                                {
-                                        btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1);
-                                        cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1;
-                                }
-                                {
-                                        btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0);
-                                        cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0;
-                                }
-                                {
-                                        btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1);
-                                        cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1;
-                                }
-                                
-                                ///
-
-
-
-                                //apply previous frames impulse on both bodies
-                                body0->applyImpulse(totalImpulse, rel_pos1);
-                                body1->applyImpulse(-totalImpulse, rel_pos2);
-                        }
-                        
-                }
-        }
-}
-
-
-btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
-{
-        btScalar maxImpulse = btScalar(0.);
-
-        {
-
-                
-                {
-                        if (cp.getDistance() <= btScalar(0.))
-                        {
-
-                                
-
-                                {
-
-                                        //btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
-                                        btScalar impulse = resolveSingleCollisionCombined(
-                                                *body0,*body1,
-                                                cp,
-                                                info);
-
-                                        if (maxImpulse < impulse)
-                                                maxImpulse  = impulse;
-
-                                }
-                        }
-                }
-        }
-        return maxImpulse;
-}
-
-
-
-btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
-{
-
-        btScalar maxImpulse = btScalar(0.);
-
-        {
-
-                
-                {
-                        if (cp.getDistance() <= btScalar(0.))
-                        {
-
-                                
-
-                                {
-
-                                        btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
-                                        btScalar impulse = cpd->m_contactSolverFunc(
-                                                *body0,*body1,
-                                                cp,
-                                                info);
-
-                                        if (maxImpulse < impulse)
-                                                maxImpulse  = impulse;
-
-                                }
-                        }
-                }
-        }
-        return maxImpulse;
-}
-
-btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
-{
-
-        (void)debugDrawer;
-        (void)iter;
-
-
-        {
-
-                
-                {
-                        
-                        if (cp.getDistance() <= btScalar(0.))
-                        {
-
-                                btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
-                                cpd->m_frictionSolverFunc(
-                                        *body0,*body1,
-                                        cp,
-                                        info);
-
-                                
-                        }
-                }
-
-        
-        }
-        return btScalar(0.);
-}
+
+
+
+
+
+
 
 

code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -24 +24 @@
 
 
-///The btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses
-///The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com
-///Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP)
-///Applies impulses for combined restitution and penetration recovery and to simulate friction
+
+///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
 class btSequentialImpulseConstraintSolver : public btConstraintSolver
 {
+protected:
 
         btAlignedObjectArray<btSolverBody>      m_tmpSolverBodyPool;
-        btAlignedObjectArray<btSolverConstraint>        m_tmpSolverConstraintPool;
-        btAlignedObjectArray<btSolverConstraint>        m_tmpSolverFrictionConstraintPool;
+        btConstraintArray                       m_tmpSolverContactConstraintPool;
+        btConstraintArray                       m_tmpSolverNonContactConstraintPool;
+        btConstraintArray                       m_tmpSolverContactFrictionConstraintPool;
         btAlignedObjectArray<int>       m_orderTmpConstraintPool;
         btAlignedObjectArray<int>       m_orderFrictionConstraintPool;
 
-
-protected:
-        btScalar solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
-        btScalar solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
-        void  prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer);
         btSolverConstraint&     addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation);
-
-        ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
-        ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
-
         
         ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
         unsigned long   m_btSeed2;
 
+        void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
+        btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
+
+        void    convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
+
+        void    resolveSplitPenetrationImpulseCacheFriendly(
+        btSolverBody& body1,
+        btSolverBody& body2,
+        const btSolverConstraint& contactConstraint,
+        const btContactSolverInfo& solverInfo);
+
+        //internal method
+        int     getOrInitSolverBody(btCollisionObject& body);
+
+        void    resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+
+        void    resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+        
+        void    resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+        
+        void    resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+                
 public:
 
         
         btSequentialImpulseConstraintSolver();
+        virtual ~btSequentialImpulseConstraintSolver();
 
-        ///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
-        ///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
-        void    setContactSolverFunc(ContactSolverFunc func,int type0,int type1)
-        {
-                m_contactDispatch[type0][type1] = func;
-        }
+        virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
         
-        ///Advanced: Override the default friction solving function for contacts, for certain types of rigidbody
-        ///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
-        void    SetFrictionSolverFunc(ContactSolverFunc func,int type0,int type1)
-        {
-                m_frictionDispatch[type0][type1] = func;
-        }
-
-        virtual ~btSequentialImpulseConstraintSolver();
-        
-        virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
-
-        virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+        btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
         btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
-        btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
-
 
         ///clear internal cached data and reset random seed
         virtual void    reset();
-
-        btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
-
-
         
         unsigned long btRand2();

code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp

@@ -69 +69 @@
 btSliderConstraint::btSliderConstraint()
         :btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
-                m_useLinearReferenceFrameA(true)
+                m_useLinearReferenceFrameA(true),
+                m_useSolveConstraintObsolete(false)
+//              m_useSolveConstraintObsolete(true)
 {
         initParams();
@@ -80 +82 @@
         , m_frameInA(frameInA)
         , m_frameInB(frameInB),
-                m_useLinearReferenceFrameA(useLinearReferenceFrameA)
+                m_useLinearReferenceFrameA(useLinearReferenceFrameA),
+                m_useSolveConstraintObsolete(false)
+//              m_useSolveConstraintObsolete(true)
 {
         initParams();
@@ -89 +93 @@
 void btSliderConstraint::buildJacobian()
 {
+        if (!m_useSolveConstraintObsolete) 
+        {
+                return;
+        }
         if(m_useLinearReferenceFrameA)
         {
@@ -156 +164 @@
 //-----------------------------------------------------------------------------
 
-void btSliderConstraint::solveConstraint(btScalar timeStep)
-{
-    m_timeStep = timeStep;
-        if(m_useLinearReferenceFrameA)
-        {
-                solveConstraintInt(m_rbA, m_rbB);
+void btSliderConstraint::getInfo1(btConstraintInfo1* info)
+{
+        if (m_useSolveConstraintObsolete)
+        {
+                info->m_numConstraintRows = 0;
+                info->nub = 0;
         }
         else
         {
-                solveConstraintInt(m_rbB, m_rbA);
+                info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
+                info->nub = 2; 
+                //prepare constraint
+                calculateTransforms();
+                testLinLimits();
+                if(getSolveLinLimit() || getPoweredLinMotor())
+                {
+                        info->m_numConstraintRows++; // limit 3rd linear as well
+                        info->nub--; 
+                }
+                testAngLimits();
+                if(getSolveAngLimit() || getPoweredAngMotor())
+                {
+                        info->m_numConstraintRows++; // limit 3rd angular as well
+                        info->nub--; 
+                }
+        }
+} // btSliderConstraint::getInfo1()
+
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::getInfo2(btConstraintInfo2* info)
+{
+        btAssert(!m_useSolveConstraintObsolete);
+        int i, s = info->rowskip;
+        const btTransform& trA = getCalculatedTransformA();
+        const btTransform& trB = getCalculatedTransformB();
+        btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
+        // make rotations around Y and Z equal
+        // the slider axis should be the only unconstrained
+        // rotational axis, the angular velocity of the two bodies perpendicular to
+        // the slider axis should be equal. thus the constraint equations are
+        //    p*w1 - p*w2 = 0
+        //    q*w1 - q*w2 = 0
+        // where p and q are unit vectors normal to the slider axis, and w1 and w2
+        // are the angular velocity vectors of the two bodies.
+        // get slider axis (X)
+        btVector3 ax1 = trA.getBasis().getColumn(0);
+        // get 2 orthos to slider axis (Y, Z)
+        btVector3 p = trA.getBasis().getColumn(1);
+        btVector3 q = trA.getBasis().getColumn(2);
+        // set the two slider rows 
+        info->m_J1angularAxis[0] = p[0];
+        info->m_J1angularAxis[1] = p[1];
+        info->m_J1angularAxis[2] = p[2];
+        info->m_J1angularAxis[s+0] = q[0];
+        info->m_J1angularAxis[s+1] = q[1];
+        info->m_J1angularAxis[s+2] = q[2];
+
+        info->m_J2angularAxis[0] = -p[0];
+        info->m_J2angularAxis[1] = -p[1];
+        info->m_J2angularAxis[2] = -p[2];
+        info->m_J2angularAxis[s+0] = -q[0];
+        info->m_J2angularAxis[s+1] = -q[1];
+        info->m_J2angularAxis[s+2] = -q[2];
+        // compute the right hand side of the constraint equation. set relative
+        // body velocities along p and q to bring the slider back into alignment.
+        // if ax1,ax2 are the unit length slider axes as computed from body1 and
+        // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
+        // if "theta" is the angle between ax1 and ax2, we need an angular velocity
+        // along u to cover angle erp*theta in one step :
+        //   |angular_velocity| = angle/time = erp*theta / stepsize
+        //                      = (erp*fps) * theta
+        //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
+        //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
+        // ...as ax1 and ax2 are unit length. if theta is smallish,
+        // theta ~= sin(theta), so
+        //    angular_velocity  = (erp*fps) * (ax1 x ax2)
+        // ax1 x ax2 is in the plane space of ax1, so we project the angular
+        // velocity to p and q to find the right hand side.
+        btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
+    btVector3 ax2 = trB.getBasis().getColumn(0);
+        btVector3 u = ax1.cross(ax2);
+        info->m_constraintError[0] = k * u.dot(p);
+        info->m_constraintError[s] = k * u.dot(q);
+        // pull out pos and R for both bodies. also get the connection
+        // vector c = pos2-pos1.
+        // next two rows. we want: vel2 = vel1 + w1 x c ... but this would
+        // result in three equations, so we project along the planespace vectors
+        // so that sliding along the slider axis is disregarded. for symmetry we
+        // also consider rotation around center of mass of two bodies (factA and factB).
+        btTransform bodyA_trans = m_rbA.getCenterOfMassTransform();
+        btTransform bodyB_trans = m_rbB.getCenterOfMassTransform();
+        int s2 = 2 * s, s3 = 3 * s;
+        btVector3 c;
+        btScalar miA = m_rbA.getInvMass();
+        btScalar miB = m_rbB.getInvMass();
+        btScalar miS = miA + miB;
+        btScalar factA, factB;
+        if(miS > btScalar(0.f))
+        {
+                factA = miB / miS;
+        }
+        else 
+        {
+                factA = btScalar(0.5f);
+        }
+        if(factA > 0.99f) factA = 0.99f;
+        if(factA < 0.01f) factA = 0.01f;
+        factB = btScalar(1.0f) - factA;
+        c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
+        btVector3 tmp = c.cross(p);
+        for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
+        for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
+        tmp = c.cross(q);
+        for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
+        for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
+
+        for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
+        for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
+        // compute two elements of right hand side. we want to align the offset
+        // point (in body 2's frame) with the center of body 1.
+        btVector3 ofs; // offset point in global coordinates
+        ofs = trB.getOrigin() - trA.getOrigin();
+        k = info->fps * info->erp * getSoftnessOrthoLin();
+        info->m_constraintError[s2] = k * p.dot(ofs);
+        info->m_constraintError[s3] = k * q.dot(ofs);
+        int nrow = 3; // last filled row
+        int srow;
+        // check linear limits linear
+        btScalar limit_err = btScalar(0.0);
+        int limit = 0;
+        if(getSolveLinLimit())
+        {
+                limit_err = getLinDepth() *  signFact;
+                limit = (limit_err > btScalar(0.0)) ? 2 : 1;
+        }
+        int powered = 0;
+        if(getPoweredLinMotor())
+        {
+                powered = 1;
+        }
+        // if the slider has joint limits or motor, add in the extra row
+        if (limit || powered) 
+        {
+                nrow++;
+                srow = nrow * info->rowskip;
+                info->m_J1linearAxis[srow+0] = ax1[0];
+                info->m_J1linearAxis[srow+1] = ax1[1];
+                info->m_J1linearAxis[srow+2] = ax1[2];
+                // linear torque decoupling step:
+                //
+                // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
+                // do not create a torque couple. in other words, the points that the
+                // constraint force is applied at must lie along the same ax1 axis.
+                // a torque couple will result in limited slider-jointed free
+                // bodies from gaining angular momentum.
+                // the solution used here is to apply the constraint forces at the center of mass of the two bodies
+                btVector3 ltd;  // Linear Torque Decoupling vector (a torque)
+//              c = btScalar(0.5) * c;
+                ltd = c.cross(ax1);
+                info->m_J1angularAxis[srow+0] = factA*ltd[0];
+                info->m_J1angularAxis[srow+1] = factA*ltd[1];
+                info->m_J1angularAxis[srow+2] = factA*ltd[2];
+                info->m_J2angularAxis[srow+0] = factB*ltd[0];
+                info->m_J2angularAxis[srow+1] = factB*ltd[1];
+                info->m_J2angularAxis[srow+2] = factB*ltd[2];
+                // right-hand part
+                btScalar lostop = getLowerLinLimit();
+                btScalar histop = getUpperLinLimit();
+                if(limit && (lostop == histop))
+                {  // the joint motor is ineffective
+                        powered = 0;
+                }
+                info->m_constraintError[srow] = 0.;
+                info->m_lowerLimit[srow] = 0.;
+                info->m_upperLimit[srow] = 0.;
+                if(powered)
+                {
+            info->cfm[nrow] = btScalar(0.0); 
+                        btScalar tag_vel = getTargetLinMotorVelocity();
+                        btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp);
+//                      info->m_constraintError[srow] += mot_fact * getTargetLinMotorVelocity();
+                        info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
+                        info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
+                        info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
+                }
+                if(limit)
+                {
+                        k = info->fps * info->erp;
+                        info->m_constraintError[srow] += k * limit_err;
+                        info->cfm[srow] = btScalar(0.0); // stop_cfm;
+                        if(lostop == histop) 
+                        {       // limited low and high simultaneously
+                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
+                                info->m_upperLimit[srow] = SIMD_INFINITY;
+                        }
+                        else if(limit == 1) 
+                        { // low limit
+                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
+                                info->m_upperLimit[srow] = 0;
+                        }
+                        else 
+                        { // high limit
+                                info->m_lowerLimit[srow] = 0;
+                                info->m_upperLimit[srow] = SIMD_INFINITY;
+                        }
+                        // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
+                        btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
+                        if(bounce > btScalar(0.0))
+                        {
+                                btScalar vel = m_rbA.getLinearVelocity().dot(ax1);
+                                vel -= m_rbB.getLinearVelocity().dot(ax1);
+                                vel *= signFact;
+                                // only apply bounce if the velocity is incoming, and if the
+                                // resulting c[] exceeds what we already have.
+                                if(limit == 1)
+                                {       // low limit
+                                        if(vel < 0)
+                                        {
+                                                btScalar newc = -bounce * vel;
+                                                if (newc > info->m_constraintError[srow])
+                                                {
+                                                        info->m_constraintError[srow] = newc;
+                                                }
+                                        }
+                                }
+                                else
+                                { // high limit - all those computations are reversed
+                                        if(vel > 0)
+                                        {
+                                                btScalar newc = -bounce * vel;
+                                                if(newc < info->m_constraintError[srow]) 
+                                                {
+                                                        info->m_constraintError[srow] = newc;
+                                                }
+                                        }
+                                }
+                        }
+                        info->m_constraintError[srow] *= getSoftnessLimLin();
+                } // if(limit)
+        } // if linear limit
+        // check angular limits
+        limit_err = btScalar(0.0);
+        limit = 0;
+        if(getSolveAngLimit())
+        {
+                limit_err = getAngDepth();
+                limit = (limit_err > btScalar(0.0)) ? 1 : 2;
+        }
+        // if the slider has joint limits, add in the extra row
+        powered = 0;
+        if(getPoweredAngMotor())
+        {
+                powered = 1;
+        }
+        if(limit || powered) 
+        {
+                nrow++;
+                srow = nrow * info->rowskip;
+                info->m_J1angularAxis[srow+0] = ax1[0];
+                info->m_J1angularAxis[srow+1] = ax1[1];
+                info->m_J1angularAxis[srow+2] = ax1[2];
+
+                info->m_J2angularAxis[srow+0] = -ax1[0];
+                info->m_J2angularAxis[srow+1] = -ax1[1];
+                info->m_J2angularAxis[srow+2] = -ax1[2];
+
+                btScalar lostop = getLowerAngLimit();
+                btScalar histop = getUpperAngLimit();
+                if(limit && (lostop == histop))
+                {  // the joint motor is ineffective
+                        powered = 0;
+                }
+                if(powered)
+                {
+            info->cfm[srow] = btScalar(0.0); 
+                        btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp);
+                        info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
+                        info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
+                        info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
+                }
+                if(limit)
+                {
+                        k = info->fps * info->erp;
+                        info->m_constraintError[srow] += k * limit_err;
+                        info->cfm[srow] = btScalar(0.0); // stop_cfm;
+                        if(lostop == histop) 
+                        {
+                                // limited low and high simultaneously
+                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
+                                info->m_upperLimit[srow] = SIMD_INFINITY;
+                        }
+                        else if(limit == 1) 
+                        { // low limit
+                                info->m_lowerLimit[srow] = 0;
+                                info->m_upperLimit[srow] = SIMD_INFINITY;
+                        }
+                        else 
+                        { // high limit
+                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
+                                info->m_upperLimit[srow] = 0;
+                        }
+                        // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
+                        btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
+                        if(bounce > btScalar(0.0))
+                        {
+                                btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
+                                vel -= m_rbB.getAngularVelocity().dot(ax1);
+                                // only apply bounce if the velocity is incoming, and if the
+                                // resulting c[] exceeds what we already have.
+                                if(limit == 1)
+                                {       // low limit
+                                        if(vel < 0)
+                                        {
+                                                btScalar newc = -bounce * vel;
+                                                if(newc > info->m_constraintError[srow])
+                                                {
+                                                        info->m_constraintError[srow] = newc;
+                                                }
+                                        }
+                                }
+                                else
+                                {       // high limit - all those computations are reversed
+                                        if(vel > 0)
+                                        {
+                                                btScalar newc = -bounce * vel;
+                                                if(newc < info->m_constraintError[srow])
+                                                {
+                                                        info->m_constraintError[srow] = newc;
+                                                }
+                                        }
+                                }
+                        }
+                        info->m_constraintError[srow] *= getSoftnessLimAng();
+                } // if(limit)
+        } // if angular limit or powered
+} // btSliderConstraint::getInfo2()
+
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
+{
+        if (m_useSolveConstraintObsolete)
+        {
+                m_timeStep = timeStep;
+                if(m_useLinearReferenceFrameA)
+                {
+                        solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB);
+                }
+                else
+                {
+                        solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA);
+                }
         }
 } // btSliderConstraint::solveConstraint()
@@ -171 +522 @@
 //-----------------------------------------------------------------------------
 
-void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
+void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
 {
     int i;
     // linear
-    btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA);
-    btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB);
+    btVector3 velA;
+        bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA);
+    btVector3 velB;
+        bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB);
     btVector3 vel = velA - velB;
         for(i = 0; i < 3; i++)
@@ -191 +544 @@
                 btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
                 btVector3 impulse_vector = normal * normalImpulse;
-                rbA.applyImpulse( impulse_vector, m_relPosA);
-                rbB.applyImpulse(-impulse_vector, m_relPosB);
+                
+                //rbA.applyImpulse( impulse_vector, m_relPosA);
+                //rbB.applyImpulse(-impulse_vector, m_relPosB);
+                {
+                        btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
+                        btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
+                        bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
+                        bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
+                }
+
+
+
                 if(m_poweredLinMotor && (!i))
                 { // apply linear motor
@@ -218 +581 @@
                                 // apply clamped impulse
                                 impulse_vector = normal * normalImpulse;
-                                rbA.applyImpulse( impulse_vector, m_relPosA);
-                                rbB.applyImpulse(-impulse_vector, m_relPosB);
+                                //rbA.applyImpulse( impulse_vector, m_relPosA);
+                                //rbB.applyImpulse(-impulse_vector, m_relPosB);
+
+                                {
+                                        btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
+                                        btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
+                                        bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
+                                        bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
+                                }
+
+
+
                         }
                 }
@@ -228 +601 @@
         btVector3 axisB =  m_calculatedTransformB.getBasis().getColumn(0);
 
-        const btVector3& angVelA = rbA.getAngularVelocity();
-        const btVector3& angVelB = rbB.getAngularVelocity();
+        btVector3 angVelA;
+        bodyA.getAngularVelocity(angVelA);
+        btVector3 angVelB;
+        bodyB.getAngularVelocity(angVelB);
 
         btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
@@ -239 +614 @@
         //solve orthogonal angular velocity correction
         btScalar len = velrelOrthog.length();
+        btScalar orthorImpulseMag = 0.f;
+
         if (len > btScalar(0.00001))
         {
                 btVector3 normal = velrelOrthog.normalized();
                 btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
-                velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
+                //velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
+                orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
         }
         //solve angular positional correction
         btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep);
+        btVector3 angularAxis = angularError;
+        btScalar angularImpulseMag = 0;
+
         btScalar len2 = angularError.length();
         if (len2>btScalar(0.00001))
@@ -252 +633 @@
                 btVector3 normal2 = angularError.normalized();
                 btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
-                angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
+                angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
+                angularError *= angularImpulseMag;
         }
         // apply impulse
-        rbA.applyTorqueImpulse(-velrelOrthog+angularError);
-        rbB.applyTorqueImpulse(velrelOrthog-angularError);
+        //rbA.applyTorqueImpulse(-velrelOrthog+angularError);
+        //rbB.applyTorqueImpulse(velrelOrthog-angularError);
+
+        bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*velrelOrthog,-orthorImpulseMag);
+        bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*velrelOrthog,orthorImpulseMag);
+        bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*angularAxis,angularImpulseMag);
+        bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*angularAxis,-angularImpulseMag);
+
+
         btScalar impulseMag;
         //solve angular limits
@@ -270 +659 @@
         }
         btVector3 impulse = axisA * impulseMag;
-        rbA.applyTorqueImpulse(impulse);
-        rbB.applyTorqueImpulse(-impulse);
+        //rbA.applyTorqueImpulse(impulse);
+        //rbB.applyTorqueImpulse(-impulse);
+
+        bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,impulseMag);
+        bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-impulseMag);
+
+
+
         //apply angular motor
         if(m_poweredAngMotor) 
@@ -302 +697 @@
                         // apply clamped impulse
                         btVector3 motorImp = angImpulse * axisA;
-                        m_rbA.applyTorqueImpulse(motorImp);
-                        m_rbB.applyTorqueImpulse(-motorImp);
+                        //rbA.applyTorqueImpulse(motorImp);
+                        //rbB.applyTorqueImpulse(-motorImp);
+
+                        bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,angImpulse);
+                        bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse);
                 }
         }
@@ -313 +711 @@
 
 void btSliderConstraint::calculateTransforms(void){
-        if(m_useLinearReferenceFrameA)
+        if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
         {
                 m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
@@ -326 +724 @@
         m_realPivotBInW = m_calculatedTransformB.getOrigin();
         m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
-        m_delta = m_realPivotBInW - m_realPivotAInW;
+        if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
+        {
+                m_delta = m_realPivotBInW - m_realPivotAInW;
+        }
+        else
+        {
+                m_delta = m_realPivotAInW - m_realPivotBInW;
+        }
         m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
     btVector3 normalWorld;
@@ -368 +773 @@
 
 //-----------------------------------------------------------------------------
- 
 
 void btSliderConstraint::testAngLimits(void)
@@ -380 +784 @@
                 const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
                 btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));  
+                m_angPos = rot;
                 if(rot < m_lowerAngLimit)
                 {
@@ -392 +797 @@
         }
 } // btSliderConstraint::testAngLimits()
-
         
 //-----------------------------------------------------------------------------
-
-
 
 btVector3 btSliderConstraint::getAncorInA(void)

code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h

@@ -47 +47 @@
 {
 protected:
+        ///for backwards compatibility during the transition to 'getInfo/getInfo2'
+        bool            m_useSolveConstraintObsolete;
         btTransform     m_frameInA;
     btTransform m_frameInB;
@@ -105 +107 @@
 
         btScalar m_linPos;
+        btScalar m_angPos;
 
         btScalar m_angDepth;
@@ -127 +130 @@
         // overrides
     virtual void        buildJacobian();
-    virtual     void    solveConstraint(btScalar        timeStep);
+    virtual void getInfo1 (btConstraintInfo1* info);
+        
+        virtual void getInfo2 (btConstraintInfo2* info);
+
+    virtual     void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
+        
+
         // access
     const btRigidBody& getRigidBodyA() const { return m_rbA; }
@@ -195 +204 @@
         btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; }
         btScalar getLinearPos() { return m_linPos; }
+        
 
         // access for ODE solver
@@ -203 +213 @@
         // internal
     void        buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
-    void        solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB);
+    void        solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB);
         // shared code used by ODE solver
         void    calculateTransforms(void);
         void    testLinLimits(void);
+        void    testLinLimits2(btConstraintInfo2* info);
         void    testAngLimits(void);
         // access for PE Solver

code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h

@@ -24 +24 @@
 #include "LinearMath/btTransformUtil.h"
 
+///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
+#ifdef BT_USE_SSE
+#define USE_SIMD 1
+#endif //
+
+
+#ifdef USE_SIMD
+
+struct  btSimdScalar
+{
+        SIMD_FORCE_INLINE       btSimdScalar()
+        {
+
+        }
+
+        SIMD_FORCE_INLINE       btSimdScalar(float      fl)
+        :m_vec128 (_mm_set1_ps(fl))
+        {
+        }
+
+        SIMD_FORCE_INLINE       btSimdScalar(__m128 v128)
+                :m_vec128(v128)
+        {
+        }
+        union
+        {
+                __m128          m_vec128;
+                float           m_floats[4];
+                int                     m_ints[4];
+                btScalar        m_unusedPadding;
+        };
+        SIMD_FORCE_INLINE       __m128  get128()
+        {
+                return m_vec128;
+        }
+
+        SIMD_FORCE_INLINE       const __m128    get128() const
+        {
+                return m_vec128;
+        }
+
+        SIMD_FORCE_INLINE       void    set128(__m128 v128)
+        {
+                m_vec128 = v128;
+        }
+
+        SIMD_FORCE_INLINE       operator       __m128()       
+        { 
+                return m_vec128; 
+        }
+        SIMD_FORCE_INLINE       operator const __m128() const 
+        { 
+                return m_vec128; 
+        }
+        
+        SIMD_FORCE_INLINE       operator float() const 
+        { 
+                return m_floats[0]; 
+        }
+
+};
+
+///@brief Return the elementwise product of two btSimdScalar
+SIMD_FORCE_INLINE btSimdScalar 
+operator*(const btSimdScalar& v1, const btSimdScalar& v2) 
+{
+        return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
+}
+
+///@brief Return the elementwise product of two btSimdScalar
+SIMD_FORCE_INLINE btSimdScalar 
+operator+(const btSimdScalar& v1, const btSimdScalar& v2) 
+{
+        return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
+}
+
+
+#else
+#define btSimdScalar btScalar
+#endif
 
 ///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
@@ -29 +109 @@
 {
         BT_DECLARE_ALIGNED_ALLOCATOR();
+        btVector3               m_deltaLinearVelocity;
+        btVector3               m_deltaAngularVelocity;
+        btScalar                m_angularFactor;
+        btScalar                m_invMass;
+        btScalar                m_friction;
+        btRigidBody*    m_originalBody;
+        btVector3               m_pushVelocity;
+        //btVector3             m_turnVelocity; 
+
         
-        btMatrix3x3             m_worldInvInertiaTensor;
-
-        btVector3               m_angularVelocity;
-        btVector3               m_linearVelocity;
-        btVector3               m_centerOfMassPosition;
-        btVector3               m_pushVelocity;
-        btVector3               m_turnVelocity;
-
-        float                   m_angularFactor;
-        float                   m_invMass;
-        float                   m_friction;
-        btRigidBody*    m_originalBody;
-        
-        
-        SIMD_FORCE_INLINE void  getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
+        SIMD_FORCE_INLINE void  getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
         {
-                velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
+                if (m_originalBody)
+                        velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
+                else
+                        velocity.setValue(0,0,0);
         }
 
+        SIMD_FORCE_INLINE void  getAngularVelocity(btVector3& angVel) const
+        {
+                if (m_originalBody)
+                        angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity;
+                else
+                        angVel.setValue(0,0,0);
+        }
+
+
         //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-        SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+        SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
         {
-                if (m_invMass)
+                //if (m_invMass)
                 {
-                        m_linearVelocity += linearComponent*impulseMagnitude;
-                        m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
-                }
-        }
-        
-        SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
-        {
-                if (m_invMass)
-                {
-                        m_pushVelocity += linearComponent*impulseMagnitude;
-                        m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+                        m_deltaLinearVelocity += linearComponent*impulseMagnitude;
+                        m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
                 }
         }
 
+        
+/*
         
         void    writebackVelocity()
@@ -73 +153 @@
                 if (m_invMass)
                 {
-                        m_originalBody->setLinearVelocity(m_linearVelocity);
-                        m_originalBody->setAngularVelocity(m_angularVelocity);
+                        m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
+                        m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
                         
                         //m_originalBody->setCompanionId(-1);
                 }
         }
-        
+        */
 
-        void    writebackVelocity(btScalar timeStep)
+        void    writebackVelocity(btScalar timeStep=0)
         {
                 if (m_invMass)
                 {
-                        m_originalBody->setLinearVelocity(m_linearVelocity);
-                        m_originalBody->setAngularVelocity(m_angularVelocity);
-                        
-                        //correct the position/orientation based on push/turn recovery
-                        btTransform newTransform;
-                        btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
-                        m_originalBody->setWorldTransform(newTransform);
-                        
+                        m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
+                        m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
                         //m_originalBody->setCompanionId(-1);
                 }
         }
-
-        void    readVelocity()
-        {
-                if (m_invMass)
-                {
-                        m_linearVelocity = m_originalBody->getLinearVelocity();
-                        m_angularVelocity = m_originalBody->getAngularVelocity();
-                }
-        }
-
         
 

code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h

@@ -20 +20 @@
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btMatrix3x3.h"
+#include "btJacobianEntry.h"
 
 //#define NO_FRICTION_TANGENTIALS 1
+#include "btSolverBody.h"
+
 
 ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
@@ -28 +31 @@
         BT_DECLARE_ALIGNED_ALLOCATOR();
 
-        btVector3       m_relpos1CrossNormal;
-        btVector3       m_contactNormal;
+        btVector3               m_relpos1CrossNormal;
+        btVector3               m_contactNormal;
 
-        btVector3       m_relpos2CrossNormal;
-        btVector3       m_angularComponentA;
+        btVector3               m_relpos2CrossNormal;
+        //btVector3             m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal
 
-        btVector3       m_angularComponentB;
-
-        mutable btScalar        m_appliedPushImpulse;
+        btVector3               m_angularComponentA;
+        btVector3               m_angularComponentB;
         
-        mutable btScalar        m_appliedImpulse;
-        int                     m_solverBodyIdA;
-        int                     m_solverBodyIdB;
+        mutable btSimdScalar    m_appliedPushImpulse;
+        mutable btSimdScalar    m_appliedImpulse;
+        
         
         btScalar        m_friction;
-        btScalar        m_restitution;
         btScalar        m_jacDiagABInv;
-        btScalar        m_penetration;
+        union
+        {
+                int     m_numConsecutiveRowsPerKernel;
+                btScalar        m_unusedPadding0;
+        };
+
+        union
+        {
+                int                     m_frictionIndex;
+                btScalar        m_unusedPadding1;
+        };
+        union
+        {
+                int                     m_solverBodyIdA;
+                btScalar        m_unusedPadding2;
+        };
+        union
+        {
+                int                     m_solverBodyIdB;
+                btScalar        m_unusedPadding3;
+        };
         
+        union
+        {
+                void*           m_originalContactPoint;
+                btScalar        m_unusedPadding4;
+        };
 
-        
-        int                     m_constraintType;
-        int                     m_frictionIndex;
-        void*           m_originalContactPoint;
-        int                     m_unusedPadding[1];
-
+        btScalar                m_rhs;
+        btScalar                m_cfm;
+        btScalar                m_lowerLimit;
+        btScalar                m_upperLimit;
 
         enum            btSolverConstraintType
@@ -62 +86 @@
 };
 
-
-
-
+typedef btAlignedObjectArray<btSolverConstraint>        btConstraintArray;
 
 

code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp

@@ -20 +20 @@
 static btRigidBody s_fixed(0, 0,0);
 
+#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
+
 btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
 :m_userConstraintType(-1),
@@ -26 +28 @@
 m_rbA(s_fixed),
 m_rbB(s_fixed),
-m_appliedImpulse(btScalar(0.))
+m_appliedImpulse(btScalar(0.)),
+m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
 {
         s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
@@ -36 +39 @@
 m_rbA(rbA),
 m_rbB(s_fixed),
-m_appliedImpulse(btScalar(0.))
+m_appliedImpulse(btScalar(0.)),
+m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
 {
                 s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
@@ -49 +53 @@
 m_rbA(rbA),
 m_rbB(rbB),
-m_appliedImpulse(btScalar(0.))
+m_appliedImpulse(btScalar(0.)),
+m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
 {
                 s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
@@ -55 +60 @@
 }
 
+
+//-----------------------------------------------------------------------------
+
+btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
+{
+        if(lowLim > uppLim)
+        {
+                return btScalar(1.0f);
+        }
+        else if(lowLim == uppLim)
+        {
+                return btScalar(0.0f);
+        }
+        btScalar lim_fact = btScalar(1.0f);
+        btScalar delta_max = vel / timeFact;
+        if(delta_max < btScalar(0.0f))
+        {
+                if((pos >= lowLim) && (pos < (lowLim - delta_max)))
+                {
+                        lim_fact = (lowLim - pos) / delta_max;
+                }
+                else if(pos  < lowLim)
+                {
+                        lim_fact = btScalar(0.0f);
+                }
+                else
+                {
+                        lim_fact = btScalar(1.0f);
+                }
+        }
+        else if(delta_max > btScalar(0.0f))
+        {
+                if((pos <= uppLim) && (pos > (uppLim - delta_max)))
+                {
+                        lim_fact = (uppLim - pos) / delta_max;
+                }
+                else if(pos  > uppLim)
+                {
+                        lim_fact = btScalar(0.0f);
+                }
+                else
+                {
+                        lim_fact = btScalar(1.0f);
+                }
+        }
+        else
+        {
+                        lim_fact = btScalar(0.0f);
+        }
+        return lim_fact;
+} // btTypedConstraint::getMotorFactor()
+
+

code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h

@@ -19 +19 @@
 class btRigidBody;
 #include "LinearMath/btScalar.h"
+#include "btSolverConstraint.h"
+struct  btSolverBody;
+
+
+
 
 enum btTypedConstraintType
@@ -26 +31 @@
         CONETWIST_CONSTRAINT_TYPE,
         D6_CONSTRAINT_TYPE,
-        VEHICLE_CONSTRAINT_TYPE,
         SLIDER_CONSTRAINT_TYPE
 };
@@ -49 +53 @@
         btRigidBody&    m_rbB;
         btScalar        m_appliedImpulse;
+        btScalar        m_dbgDrawSize;
 
 
@@ -56 +61 @@
         virtual ~btTypedConstraint() {};
         btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
+        btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
 
-        btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
+        struct btConstraintInfo1 {
+                int m_numConstraintRows,nub;
+        };
+
+        struct btConstraintInfo2 {
+                // integrator parameters: frames per second (1/stepsize), default error
+                // reduction parameter (0..1).
+                btScalar fps,erp;
+
+                // for the first and second body, pointers to two (linear and angular)
+                // n*3 jacobian sub matrices, stored by rows. these matrices will have
+                // been initialized to 0 on entry. if the second body is zero then the
+                // J2xx pointers may be 0.
+                btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
+
+                // elements to jump from one row to the next in J's
+                int rowskip;
+
+                // right hand sides of the equation J*v = c + cfm * lambda. cfm is the
+                // "constraint force mixing" vector. c is set to zero on entry, cfm is
+                // set to a constant value (typically very small or zero) value on entry.
+                btScalar *m_constraintError,*cfm;
+
+                // lo and hi limits for variables (set to -/+ infinity on entry).
+                btScalar *m_lowerLimit,*m_upperLimit;
+
+                // findex vector for variables. see the LCP solver interface for a
+                // description of what this does. this is set to -1 on entry.
+                // note that the returned indexes are relative to the first index of
+                // the constraint.
+                int *findex;
+        };
+
 
         virtual void    buildJacobian() = 0;
 
-        virtual void    solveConstraint(btScalar        timeStep) = 0;
+        virtual void    setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
+        {
+        }
+        virtual void getInfo1 (btConstraintInfo1* info)=0;
 
+        virtual void getInfo2 (btConstraintInfo2* info)=0;
+
+        virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep) = 0;
+
+        btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
+        
         const btRigidBody& getRigidBodyA() const
         {
@@ -95 +142 @@
                 m_userConstraintId = uid;
         }
-        
+
         int getUserConstraintId() const
         {
@@ -115 +162 @@
                 return m_constraintType;
         }
-
+        
+        void setDbgDrawSize(btScalar dbgDrawSize)
+        {
+                m_dbgDrawSize = dbgDrawSize;
+        }
+        btScalar getDbgDrawSize()
+        {
+                return m_dbgDrawSize;
+        }
+        
 };