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

Timestamp:

Apr 28, 2011, 7:15:14 AM (13 years ago)

Author:

rgrieder

Message:

Merged kicklib2 branch back to trunk (includes former branches ois_update, mac_osx and kicklib).

Notes for updating

Linux:
You don't need an extra package for CEGUILua and Tolua, it's already shipped with CEGUI.
However you do need to make sure that the OgreRenderer is installed too with CEGUI 0.7 (may be a separate package).
Also, Orxonox now recognises if you install the CgProgramManager (a separate package available on newer Ubuntu on Debian systems).

Windows:
Download the new dependency packages versioned 6.0 and use these. If you have problems with that or if you don't like the in game console problem mentioned below, you can download the new 4.3 version of the packages (only available for Visual Studio 2005/2008).

Key new features:

• *Support for Mac OS X*
• Visual Studio 2010 support
• Bullet library update to 2.77
• OIS library update to 1.3
• Support for CEGUI 0.7 —> Support for Arch Linux and even SuSE
• Improved install target
• Compiles now with GCC 4.6
• Ogre Cg Shader plugin activated for Linux if available
• And of course lots of bug fixes

There are also some regressions:

• No support for CEGUI 0.5, Ogre 1.4 and boost 1.35 - 1.39 any more
• In game console is not working in main menu for CEGUI 0.7
• Tolua (just the C lib, not the application) and CEGUILua libraries are no longer in our repository. —> You will need to get these as well when compiling Orxonox
• And of course lots of new bugs we don't yet know about

File:

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

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

@@ -23 +23 @@
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btTransformUtil.h"
 #include <new>
 
 
+
 #define D6_USE_OBSOLETE_METHOD false
-//-----------------------------------------------------------------------------
-
-btGeneric6DofConstraint::btGeneric6DofConstraint()
-:btTypedConstraint(D6_CONSTRAINT_TYPE),
-m_useLinearReferenceFrameA(true),
-m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
-{
-}
-
-//-----------------------------------------------------------------------------
+#define D6_USE_FRAME_OFFSET true
+
+
+
+
+
 
 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
@@ -43 +41 @@
 , m_frameInB(frameInB),
 m_useLinearReferenceFrameA(useLinearReferenceFrameA),
+m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
+m_flags(0),
 m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
 {
-
-}
-//-----------------------------------------------------------------------------
+        calculateTransforms();
+}
+
+
+
+btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
+        : btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB),
+                m_frameInB(frameInB),
+                m_useLinearReferenceFrameA(useLinearReferenceFrameB),
+                m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
+                m_flags(0),
+                m_useSolveConstraintObsolete(false)
+{
+        ///not providing rigidbody A means implicitly using worldspace for body A
+        m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
+        calculateTransforms();
+}
+
+
 
 
 #define GENERIC_D6_DISABLE_WARMSTARTING 1
 
-//-----------------------------------------------------------------------------
+
 
 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
@@ -62 +78 @@
 }
 
-//-----------------------------------------------------------------------------
+
 
 ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
@@ -111 +127 @@
                 return 0;
         }
-
         if (test_value < m_loLimit)
         {
@@ -130 +145 @@
 }
 
-//-----------------------------------------------------------------------------
+
 
 btScalar btRotationalLimitMotor::solveAngularLimits(
         btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
-        btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB)
+        btRigidBody * body0, btRigidBody * body1 )
 {
         if (needApplyTorques()==false) return 0.0f;
@@ -144 +159 @@
         if (m_currentLimit!=0)
         {
-                target_velocity = -m_ERP*m_currentLimitError/(timeStep);
+                target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
                 maxMotorForce = m_maxLimitForce;
         }
@@ -153 +168 @@
 
         btVector3 angVelA;
-        bodyA.getAngularVelocity(angVelA);
+        body0->internalGetAngularVelocity(angVelA);
         btVector3 angVelB;
-        bodyB.getAngularVelocity(angVelB);
+        body1->internalGetAngularVelocity(angVelB);
 
         btVector3 vel_diff;
@@ -192 +207 @@
 
         // sort with accumulated impulses
-        btScalar        lo = btScalar(-1e30);
-        btScalar        hi = btScalar(1e30);
+        btScalar        lo = btScalar(-BT_LARGE_FLOAT);
+        btScalar        hi = btScalar(BT_LARGE_FLOAT);
 
         btScalar oldaccumImpulse = m_accumulatedImpulse;
@@ -206 +221 @@
         //body1->applyTorqueImpulse(-motorImp);
 
-        bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
-        bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
+        body0->internalApplyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
+        body1->internalApplyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
 
 
@@ -250 +265 @@
         m_currentLimitError[limitIndex] = btScalar(0.f);
         return 0;
-} // btTranslationalLimitMotor::testLimitValue()
-
-//-----------------------------------------------------------------------------
+}
+
+
 
 btScalar btTranslationalLimitMotor::solveLinearAxis(
         btScalar timeStep,
         btScalar jacDiagABInv,
-        btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
-        btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
+        btRigidBody& body1,const btVector3 &pointInA,
+        btRigidBody& body2,const btVector3 &pointInB,
         int limit_index,
         const btVector3 & axis_normal_on_a,
@@ -271 +286 @@
 
         btVector3 vel1;
-        bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+        body1.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
         btVector3 vel2;
-        bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+        body2.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
         btVector3 vel = vel1 - vel2;
 
@@ -284 +299 @@
         //positional error (zeroth order error)
         btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
-        btScalar        lo = btScalar(-1e30);
-        btScalar        hi = btScalar(1e30);
+        btScalar        lo = btScalar(-BT_LARGE_FLOAT);
+        btScalar        hi = btScalar(BT_LARGE_FLOAT);
 
         btScalar minLimit = m_lowerLimit[limit_index];
@@ -331 +346 @@
         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);
+        body1.internalApplyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
+        body2.internalApplyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
 
 
@@ -373 +388 @@
 }
 
-//-----------------------------------------------------------------------------
-
 void btGeneric6DofConstraint::calculateTransforms()
 {
-        m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
-        m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
+        calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+}
+
+void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
+{
+        m_calculatedTransformA = transA * m_frameInA;
+        m_calculatedTransformB = transB * m_frameInB;
         calculateLinearInfo();
         calculateAngleInfo();
-}
-
-//-----------------------------------------------------------------------------
+        if(m_useOffsetForConstraintFrame)
+        {       //  get weight factors depending on masses
+                btScalar miA = getRigidBodyA().getInvMass();
+                btScalar miB = getRigidBodyB().getInvMass();
+                m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
+                btScalar miS = miA + miB;
+                if(miS > btScalar(0.f))
+                {
+                        m_factA = miB / miS;
+                }
+                else 
+                {
+                        m_factA = btScalar(0.5f);
+                }
+                m_factB = btScalar(1.0f) - m_factA;
+        }
+}
+
+
 
 void btGeneric6DofConstraint::buildLinearJacobian(
@@ -401 +435 @@
 }
 
-//-----------------------------------------------------------------------------
+
 
 void btGeneric6DofConstraint::buildAngularJacobian(
@@ -414 +448 @@
 }
 
-//-----------------------------------------------------------------------------
+
 
 bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
 {
         btScalar angle = m_calculatedAxisAngleDiff[axis_index];
+        angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
+        m_angularLimits[axis_index].m_currentPosition = angle;
         //test limits
         m_angularLimits[axis_index].testLimitValue(angle);
@@ -424 +460 @@
 }
 
-//-----------------------------------------------------------------------------
+
 
 void btGeneric6DofConstraint::buildJacobian()
 {
+#ifndef __SPU__
         if (m_useSolveConstraintObsolete)
         {
@@ -439 +476 @@
                 }
                 //calculates transform
-                calculateTransforms();
+                calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
 
                 //  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
@@ -482 +519 @@
 
         }
-}
-
-//-----------------------------------------------------------------------------
+#endif //__SPU__
+
+}
+
 
 void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
@@ -495 +533 @@
         {
                 //prepare constraint
-                calculateTransforms();
+                calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
                 info->m_numConstraintRows = 0;
                 info->nub = 6;
@@ -520 +558 @@
 }
 
-//-----------------------------------------------------------------------------
+void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
+{
+        if (m_useSolveConstraintObsolete)
+        {
+                info->m_numConstraintRows = 0;
+                info->nub = 0;
+        } else
+        {
+                //pre-allocate all 6
+                info->m_numConstraintRows = 6;
+                info->nub = 0;
+        }
+}
+
 
 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;
+
+        const btTransform& transA = m_rbA.getCenterOfMassTransform();
+        const btTransform& transB = m_rbB.getCenterOfMassTransform();
+        const btVector3& linVelA = m_rbA.getLinearVelocity();
+        const btVector3& linVelB = m_rbB.getLinearVelocity();
+        const btVector3& angVelA = m_rbA.getAngularVelocity();
+        const btVector3& angVelB = m_rbB.getAngularVelocity();
+
+        if(m_useOffsetForConstraintFrame)
+        { // for stability better to solve angular limits first
+                int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
+                setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
+        }
+        else
+        { // leave old version for compatibility
+                int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
+                setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
+        }
+
+}
+
+
+void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+{
+        
+        btAssert(!m_useSolveConstraintObsolete);
+        //prepare constraint
+        calculateTransforms(transA,transB);
+
+        int i;
+        for (i=0;i<3 ;i++ )
+        {
+                testAngularLimitMotor(i);
+        }
+
+        if(m_useOffsetForConstraintFrame)
+        { // for stability better to solve angular limits first
+                int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
+                setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
+        }
+        else
+        { // leave old version for compatibility
+                int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
+                setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
+        }
+}
+
+
+
+int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+{
+//      int row = 0;
         //solve linear limits
         btRotationalLimitMotor limot;
@@ -543 +636 @@
                         limot.m_bounce = btScalar(0.f);
                         limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
+                        limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[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;
@@ -554 +647 @@
                         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);
+                        int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT);
+                        limot.m_normalCFM       = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
+                        limot.m_stopCFM         = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
+                        limot.m_stopERP         = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
+                        if(m_useOffsetForConstraintFrame)
+                        {
+                                int indx1 = (i + 1) % 3;
+                                int indx2 = (i + 2) % 3;
+                                int rotAllowed = 1; // rotations around orthos to current axis
+                                if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
+                                {
+                                        rotAllowed = 0;
+                                }
+                                row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
+                        }
+                        else
+                        {
+                                row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
+                        }
                 }
         }
@@ -560 +671 @@
 }
 
-//-----------------------------------------------------------------------------
-
-int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset)
+
+
+int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
 {
         btGeneric6DofConstraint * d6constraint = this;
@@ -572 +683 @@
                 {
                         btVector3 axis = d6constraint->getAxis(i);
-                        row += get_limit_motor_info2(
-                                d6constraint->getRotationalLimitMotor(i),
-                                &m_rbA,
-                                &m_rbB,
-                                info,row,axis,1);
+                        int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
+                        if(!(flags & BT_6DOF_FLAGS_CFM_NORM))
+                        {
+                                m_angularLimits[i].m_normalCFM = info->cfm[0];
+                        }
+                        if(!(flags & BT_6DOF_FLAGS_CFM_STOP))
+                        {
+                                m_angularLimits[i].m_stopCFM = info->cfm[0];
+                        }
+                        if(!(flags & BT_6DOF_FLAGS_ERP_STOP))
+                        {
+                                m_angularLimits[i].m_stopERP = info->erp;
+                        }
+                        row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
+                                                                                                transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
                 }
         }
@@ -583 +704 @@
 }
 
-//-----------------------------------------------------------------------------
-
-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)
@@ -652 +713 @@
 }
 
-//-----------------------------------------------------------------------------
+
 
 btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
@@ -659 +720 @@
 }
 
-//-----------------------------------------------------------------------------
-
-btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
-{
-        return m_calculatedAxisAngleDiff[axis_index];
-}
-
-//-----------------------------------------------------------------------------
+
+btScalar        btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
+{
+        return m_calculatedLinearDiff[axisIndex];
+}
+
+
+btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
+{
+        return m_calculatedAxisAngleDiff[axisIndex];
+}
+
+
 
 void btGeneric6DofConstraint::calcAnchorPos(void)
@@ -685 +751 @@
         m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
         return;
-} // btGeneric6DofConstraint::calcAnchorPos()
-
-//-----------------------------------------------------------------------------
+}
+
+
 
 void btGeneric6DofConstraint::calculateLinearInfo()
@@ -695 +761 @@
         for(int i = 0; i < 3; i++)
         {
+                m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[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)
+        const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
+        btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
 {
     int srow = row * info->rowskip;
@@ -722 +789 @@
             J2[srow+2] = -ax1[2];
         }
-        if((!rotational) && limit)
+        if((!rotational))
         {
-                        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 (m_useOffsetForConstraintFrame)
+                        {
+                                btVector3 tmpA, tmpB, relA, relB;
+                                // get vector from bodyB to frameB in WCS
+                                relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
+                                // get its projection to constraint axis
+                                btVector3 projB = ax1 * relB.dot(ax1);
+                                // get vector directed from bodyB to constraint axis (and orthogonal to it)
+                                btVector3 orthoB = relB - projB;
+                                // same for bodyA
+                                relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
+                                btVector3 projA = ax1 * relA.dot(ax1);
+                                btVector3 orthoA = relA - projA;
+                                // get desired offset between frames A and B along constraint axis
+                                btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
+                                // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
+                                btVector3 totalDist = projA + ax1 * desiredOffs - projB;
+                                // get offset vectors relA and relB
+                                relA = orthoA + totalDist * m_factA;
+                                relB = orthoB - totalDist * m_factB;
+                                tmpA = relA.cross(ax1);
+                                tmpB = relB.cross(ax1);
+                                if(m_hasStaticBody && (!rotAllowed))
+                                {
+                                        tmpA *= m_factA;
+                                        tmpB *= m_factB;
+                                }
+                                int i;
+                                for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
+                                for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
+                        } else
+                        {
+                                btVector3 ltd;  // Linear Torque Decoupling vector
+                                btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
+                                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() - transB.getOrigin();
+                                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
@@ -743 +843 @@
         if (powered)
         {
-            info->cfm[srow] = 0.0f;
+                        info->cfm[srow] = limot->m_normalCFM;
             if(!limit)
             {
-                info->m_constraintError[srow] += limot->m_targetVelocity;
+                                btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
+
+                                btScalar mot_fact = getMotorFactor(     limot->m_currentPosition, 
+                                                                                                        limot->m_loLimit,
+                                                                                                        limot->m_hiLimit, 
+                                                                                                        tag_vel, 
+                                                                                                        info->fps * limot->m_stopERP);
+                                info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
                 info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
                 info->m_upperLimit[srow] = limot->m_maxMotorForce;
@@ -753 +860 @@
         if(limit)
         {
-            btScalar k = info->fps * limot->m_ERP;
+            btScalar k = info->fps * limot->m_stopERP;
                         if(!rotational)
                         {
@@ -762 +869 @@
                                 info->m_constraintError[srow] += -k * limot->m_currentLimitError;
                         }
-            info->cfm[srow] = 0.0f;
+                        info->cfm[srow] = limot->m_stopCFM;
             if (limot->m_loLimit == limot->m_hiLimit)
             {   // limited low and high simultaneously
@@ -787 +894 @@
                     if (rotational)
                     {
-                        vel = body0->getAngularVelocity().dot(ax1);
-                        if (body1)
-                            vel -= body1->getAngularVelocity().dot(ax1);
+                        vel = angVelA.dot(ax1);
+//make sure that if no body -> angVelB == zero vec
+//                        if (body1)
+                            vel -= angVelB.dot(ax1);
                     }
                     else
                     {
-                        vel = body0->getLinearVelocity().dot(ax1);
-                        if (body1)
-                            vel -= body1->getLinearVelocity().dot(ax1);
+                        vel = linVelA.dot(ax1);
+//make sure that if no body -> angVelB == zero vec
+//                        if (body1)
+                            vel -= linVelB.dot(ax1);
                     }
                     // only apply bounce if the velocity is incoming, and if the
@@ -825 +934 @@
 }
 
-//-----------------------------------------------------------------------------
-//-----------------------------------------------------------------------------
-//-----------------------------------------------------------------------------
+
+
+
+
+
+        ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+        ///If no axis is provided, it uses the default axis for this constraint.
+void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
+{
+        if((axis >= 0) && (axis < 3))
+        {
+                switch(num)
+                {
+                        case BT_CONSTRAINT_STOP_ERP : 
+                                m_linearLimits.m_stopERP[axis] = value;
+                                m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+                                break;
+                        case BT_CONSTRAINT_STOP_CFM : 
+                                m_linearLimits.m_stopCFM[axis] = value;
+                                m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+                                break;
+                        case BT_CONSTRAINT_CFM : 
+                                m_linearLimits.m_normalCFM[axis] = value;
+                                m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+                                break;
+                        default : 
+                                btAssertConstrParams(0);
+                }
+        }
+        else if((axis >=3) && (axis < 6))
+        {
+                switch(num)
+                {
+                        case BT_CONSTRAINT_STOP_ERP : 
+                                m_angularLimits[axis - 3].m_stopERP = value;
+                                m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+                                break;
+                        case BT_CONSTRAINT_STOP_CFM : 
+                                m_angularLimits[axis - 3].m_stopCFM = value;
+                                m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+                                break;
+                        case BT_CONSTRAINT_CFM : 
+                                m_angularLimits[axis - 3].m_normalCFM = value;
+                                m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+                                break;
+                        default : 
+                                btAssertConstrParams(0);
+                }
+        }
+        else
+        {
+                btAssertConstrParams(0);
+        }
+}
+
+        ///return the local value of parameter
+btScalar btGeneric6DofConstraint::getParam(int num, int axis) const 
+{
+        btScalar retVal = 0;
+        if((axis >= 0) && (axis < 3))
+        {
+                switch(num)
+                {
+                        case BT_CONSTRAINT_STOP_ERP : 
+                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+                                retVal = m_linearLimits.m_stopERP[axis];
+                                break;
+                        case BT_CONSTRAINT_STOP_CFM : 
+                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+                                retVal = m_linearLimits.m_stopCFM[axis];
+                                break;
+                        case BT_CONSTRAINT_CFM : 
+                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+                                retVal = m_linearLimits.m_normalCFM[axis];
+                                break;
+                        default : 
+                                btAssertConstrParams(0);
+                }
+        }
+        else if((axis >=3) && (axis < 6))
+        {
+                switch(num)
+                {
+                        case BT_CONSTRAINT_STOP_ERP : 
+                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+                                retVal = m_angularLimits[axis - 3].m_stopERP;
+                                break;
+                        case BT_CONSTRAINT_STOP_CFM : 
+                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+                                retVal = m_angularLimits[axis - 3].m_stopCFM;
+                                break;
+                        case BT_CONSTRAINT_CFM : 
+                                btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+                                retVal = m_angularLimits[axis - 3].m_normalCFM;
+                                break;
+                        default : 
+                                btAssertConstrParams(0);
+                }
+        }
+        else
+        {
+                btAssertConstrParams(0);
+        }
+        return retVal;
+}