Changeset 2908 for code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.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/btSliderConstraint.cpp (modified) (17 diffs)

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

@@ -69 +69 @@
 btSliderConstraint::btSliderConstraint()
         :btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
-                m_useLinearReferenceFrameA(true),
-                m_useSolveConstraintObsolete(false)
-//              m_useSolveConstraintObsolete(true)
+                m_useLinearReferenceFrameA(true)
 {
         initParams();
@@ -82 +80 @@
         , m_frameInA(frameInA)
         , m_frameInB(frameInB),
-                m_useLinearReferenceFrameA(useLinearReferenceFrameA),
-                m_useSolveConstraintObsolete(false)
-//              m_useSolveConstraintObsolete(true)
+                m_useLinearReferenceFrameA(useLinearReferenceFrameA)
 {
         initParams();
@@ -93 +89 @@
 void btSliderConstraint::buildJacobian()
 {
-        if (!m_useSolveConstraintObsolete) 
-        {
-                return;
-        }
         if(m_useLinearReferenceFrameA)
         {
@@ -164 +156 @@
 //-----------------------------------------------------------------------------
 
-void btSliderConstraint::getInfo1(btConstraintInfo1* info)
-{
-        if (m_useSolveConstraintObsolete)
-        {
-                info->m_numConstraintRows = 0;
-                info->nub = 0;
+void btSliderConstraint::solveConstraint(btScalar timeStep)
+{
+    m_timeStep = timeStep;
+        if(m_useLinearReferenceFrameA)
+        {
+                solveConstraintInt(m_rbA, m_rbB);
         }
         else
         {
-                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);
-                }
+                solveConstraintInt(m_rbB, m_rbA);
         }
 } // btSliderConstraint::solveConstraint()
@@ -522 +171 @@
 //-----------------------------------------------------------------------------
 
-void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
+void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
 {
     int i;
     // linear
-    btVector3 velA;
-        bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA);
-    btVector3 velB;
-        bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB);
+    btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA);
+    btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB);
     btVector3 vel = velA - velB;
         for(i = 0; i < 3; i++)
@@ -544 +191 @@
                 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);
-                {
-                        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);
-                }
-
-
-
+                rbA.applyImpulse( impulse_vector, m_relPosA);
+                rbB.applyImpulse(-impulse_vector, m_relPosB);
                 if(m_poweredLinMotor && (!i))
                 { // apply linear motor
@@ -581 +218 @@
                                 // apply clamped impulse
                                 impulse_vector = normal * normalImpulse;
-                                //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);
-                                }
-
-
-
+                                rbA.applyImpulse( impulse_vector, m_relPosA);
+                                rbB.applyImpulse(-impulse_vector, m_relPosB);
                         }
                 }
@@ -601 +228 @@
         btVector3 axisB =  m_calculatedTransformB.getBasis().getColumn(0);
 
-        btVector3 angVelA;
-        bodyA.getAngularVelocity(angVelA);
-        btVector3 angVelB;
-        bodyB.getAngularVelocity(angVelB);
+        const btVector3& angVelA = rbA.getAngularVelocity();
+        const btVector3& angVelB = rbB.getAngularVelocity();
 
         btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
@@ -614 +239 @@
         //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;
-                orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
+                velrelOrthog *= (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))
@@ -633 +252 @@
                 btVector3 normal2 = angularError.normalized();
                 btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
-                angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
-                angularError *= angularImpulseMag;
+                angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
         }
         // apply impulse
-        //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);
-
-
+        rbA.applyTorqueImpulse(-velrelOrthog+angularError);
+        rbB.applyTorqueImpulse(velrelOrthog-angularError);
         btScalar impulseMag;
         //solve angular limits
@@ -659 +270 @@
         }
         btVector3 impulse = axisA * impulseMag;
-        //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);
-
-
-
+        rbA.applyTorqueImpulse(impulse);
+        rbB.applyTorqueImpulse(-impulse);
         //apply angular motor
         if(m_poweredAngMotor) 
@@ -697 +302 @@
                         // apply clamped impulse
                         btVector3 motorImp = angImpulse * axisA;
-                        //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);
+                        m_rbA.applyTorqueImpulse(motorImp);
+                        m_rbB.applyTorqueImpulse(-motorImp);
                 }
         }
@@ -711 +313 @@
 
 void btSliderConstraint::calculateTransforms(void){
-        if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
+        if(m_useLinearReferenceFrameA)
         {
                 m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
@@ -724 +326 @@
         m_realPivotBInW = m_calculatedTransformB.getOrigin();
         m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
-        if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
-        {
-                m_delta = m_realPivotBInW - m_realPivotAInW;
-        }
-        else
-        {
-                m_delta = m_realPivotAInW - m_realPivotBInW;
-        }
+        m_delta = m_realPivotBInW - m_realPivotAInW;
         m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
     btVector3 normalWorld;
@@ -773 +368 @@
 
 //-----------------------------------------------------------------------------
+ 
 
 void btSliderConstraint::testAngLimits(void)
@@ -784 +380 @@
                 const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
                 btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));  
-                m_angPos = rot;
                 if(rot < m_lowerAngLimit)
                 {
@@ -797 +392 @@
         }
 } // btSliderConstraint::testAngLimits()
+
         
 //-----------------------------------------------------------------------------
+
+
 
 btVector3 btSliderConstraint::getAncorInA(void)