Changeset 2908 for code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

Timestamp:

Apr 8, 2009, 12:58:47 AM (16 years ago)

Author:

dafrick

Message:

Reverted to revision 2906 (because I'm too stupid to merge correctly, 2nd try will follow shortly. )

Location:

code/branches/questsystem5

Files:

• . (modified) (1 prop)
• src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp (modified) (10 diffs)

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_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
-m_useReferenceFrameA(false)
-{
-        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
-}
-
-//-----------------------------------------------------------------------------
+m_enableAngularMotor(false)
+{
+}
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
-                                                                         btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA)
+                                                                         btVector3& axisInA,btVector3& axisInB)
                                                                          :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
                                                                          m_angularOnly(false),
-                                                                         m_enableAngularMotor(false),
-                                                                         m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
-                                                                         m_useReferenceFrameA(useReferenceFrameA)
+                                                                         m_enableAngularMotor(false)
 {
         m_rbAFrame.getOrigin() = pivotInA;
@@ -75 +61 @@
         
         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
@@ -86 +72 @@
         m_limitSoftness = 0.9f;
         m_solveLimit = 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)
+
+}
+
+
+
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false)
 {
 
@@ -107 +91 @@
                                                                         rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
 
-        btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
+        btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;
 
         btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
@@ -126 +110 @@
         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, bool useReferenceFrameA)
+                                                                     const btTransform& rbAFrame, const btTransform& rbBFrame)
 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
 m_angularOnly(false),
-m_enableAngularMotor(false),
-m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
-m_useReferenceFrameA(useReferenceFrameA)
-{
+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.);
+
         //start with free
         m_lowerLimit = btScalar(1e30);
@@ -146 +130 @@
         m_limitSoftness = 0.9f;
         m_solveLimit = false;
-        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
 }                       
 
-//-----------------------------------------------------------------------------
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
+
+
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
 m_angularOnly(false),
-m_enableAngularMotor(false),
-m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
-m_useReferenceFrameA(useReferenceFrameA)
+m_enableAngularMotor(false)
 {
         ///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());
@@ -169 +155 @@
         m_limitSoftness = 0.9f;
         m_solveLimit = false;
-        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
-}
-
-//-----------------------------------------------------------------------------
+}
 
 void    btHingeConstraint::buildJacobian()
 {
-        if (m_useSolveConstraintObsolete)
+        m_appliedImpulse = btScalar(0.);
+
+        if (!m_angularOnly)
         {
-                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(
+                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(),
@@ -210 +191 @@
                                 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);
+                
+        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());
+
+
+        // Compute limit information
+        btScalar hingeAngle = getHingeAngle();  
+
+        //set bias, sign, clear accumulator
+        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);
+                        m_limitSign = 1.0f;
+                        m_solveLimit = true;
+                } 
+//              else if (hingeAngle >= m_upperLimit*m_limitSoftness)
+                else if (hingeAngle >= m_upperLimit)
+                {
+                        m_correction = m_upperLimit - 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;
                         }
-                }
-
-                //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);
+
+                        //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);
                         
-                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)
@@ -623 +391 @@
 
 }
-
-//-----------------------------------------------------------------------------
 
 btScalar btHingeConstraint::getHingeAngle()
@@ -631 +397 @@
         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
-        m_hingeAngle = getHingeAngle();  
-        m_correction = btScalar(0.);
-        m_limitSign = btScalar(0.);
-        m_solveLimit = false;
-        if (m_lowerLimit <= m_upperLimit)
-        {
-                if (m_hingeAngle <= m_lowerLimit)
-                {
-                        m_correction = (m_lowerLimit - m_hingeAngle);
-                        m_limitSign = 1.0f;
-                        m_solveLimit = true;
-                } 
-                else if (m_hingeAngle >= m_upperLimit)
-                {
-                        m_correction = m_upperLimit - m_hingeAngle;
-                        m_limitSign = -1.0f;
-                        m_solveLimit = true;
-                }
-        }
-        return;
-} // btHingeConstraint::testLimit()
-
-//-----------------------------------------------------------------------------
-//-----------------------------------------------------------------------------
-//-----------------------------------------------------------------------------
+
+        return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)  );
+}
+