Changeset 8284 for code/branches/kicklib2/src/external/bullet/BulletDynamics

code/branches/kicklib2

Property svn:mergeinfo changed

/code/branches/kicklib	merged: 7979-7984,8057-8058,8061-8064,8066-8068,8071,8073-8074,8080-8082,8089,8094-8096
/code/branches/mac_osx (added)	merged: 7789,7933-7934,8042-8049,8054-8056,8059-8060,8065,8069-8070,8072
/code/branches/ois_update (added)	merged: 7506-7528,7530,7532,7553,7557-7560,7562-7564,7567,7570-7572,7575-7578,7582,7591-7592,7615,7618-7624,7660,7664,7669,7681-7683,7691,7701,7719,7767,7771,7782

code/branches/kicklib2/src/external/bullet/BulletDynamics/CMakeLists.txt

-                      r5929
+                      r8284
 COMPILATION_BEGIN BulletDynamicsCompilation.cpp
+        Character/btKinematicCharacterController.cpp
+        ConstraintSolver/btConeTwistConstraint.cpp
         ConstraintSolver/btContactConstraint.cpp
-        ConstraintSolver/btConeTwistConstraint.cpp
         ConstraintSolver/btGeneric6DofConstraint.cpp
+        ConstraintSolver/btGeneric6DofSpringConstraint.cpp
+        ConstraintSolver/btHinge2Constraint.cpp
         ConstraintSolver/btHingeConstraint.cpp
         ConstraintSolver/btPoint2PointConstraint.cpp
 …
         ConstraintSolver/btSolve2LinearConstraint.cpp
         ConstraintSolver/btTypedConstraint.cpp
+        ConstraintSolver/btUniversalConstraint.cpp
+        Dynamics/btContinuousDynamicsWorld.cpp
+        Dynamics/btDiscreteDynamicsWorld.cpp
+        Dynamics/btRigidBody.cpp
+        Dynamics/btSimpleDynamicsWorld.cpp
         Dynamics/Bullet-C-API.cpp
-        Dynamics/btDiscreteDynamicsWorld.cpp
-        Dynamics/btSimpleDynamicsWorld.cpp
-        Dynamics/btRigidBody.cpp
         Vehicle/btRaycastVehicle.cpp
         Vehicle/btWheelInfo.cpp
-        Character/btKinematicCharacterController.cpp
 COMPILATION_END
         # Headers
+        ConstraintSolver/btConeTwistConstraint.h
         ConstraintSolver/btConstraintSolver.h
         ConstraintSolver/btContactConstraint.h
         ConstraintSolver/btContactSolverInfo.h
-        ConstraintSolver/btConeTwistConstraint.h
         ConstraintSolver/btGeneric6DofConstraint.h
+        ConstraintSolver/btGeneric6DofSpringConstraint.h
+        ConstraintSolver/btHinge2Constraint.h
         ConstraintSolver/btHingeConstraint.h
         ConstraintSolver/btJacobianEntry.h
 …
         ConstraintSolver/btSolverConstraint.h
         ConstraintSolver/btTypedConstraint.h
+        ConstraintSolver/btUniversalConstraint.h
         Dynamics/btActionInterface.h
         Dynamics/btContinuousDynamicsWorld.h

code/branches/kicklib2/src/external/bullet/BulletDynamics/Character/btCharacterControllerInterface.h

r5781	r8284
31	31
32	32	virtual void setWalkDirection(const btVector3& walkDirection) = 0;
	33	virtual void setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) = 0;
33	34	virtual void reset () = 0;
34	35	virtual void warp (const btVector3& origin) = 0;

code/branches/kicklib2/src/external/bullet/BulletDynamics/Character/btKinematicCharacterController.cpp

-                      r5781
+                      r8284
 . This notice may not be removed or altered from any source distribution.
 */
 #include "LinearMath/btIDebugDraw.h"
 …
 #include "btKinematicCharacterController.h"
+static btVector3 upAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) };
+// static helper method
+static btVector3
+getNormalizedVector(const btVector3& v)
+{
+        btVector3 n = v.normalized();
+        if (n.length() < SIMD_EPSILON) {
+                n.setValue(0, 0, 0);
+        }
+        return n;
+}
 ///@todo Interact with dynamic objects,
 …
+{
 public:
+        btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
+        {
+                m_me = me;
+        btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me, const btVector3& up, btScalar minSlopeDot)
+        : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
+        , m_me(me)
+        , m_up(up)
+        , m_minSlopeDot(minSlopeDot)
+        {
+        }
 …
+        {
                 if (convexResult.m_hitCollisionObject == m_me)
+                        return 1.0;
+                        return btScalar(1.0);
+                btVector3 hitNormalWorld;
+                if (normalInWorldSpace)
+                {
+                        hitNormalWorld = convexResult.m_hitNormalLocal;
+                } else
+                {
+                        ///need to transform normal into worldspace
+                        hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal;
+                }
+                btScalar dotUp = m_up.dot(hitNormalWorld);
+                if (dotUp < m_minSlopeDot) {
+                        return btScalar(1.0);
+                }
                 return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
 …
 protected:
         btCollisionObject* m_me;
+        const btVector3 m_up;
+        btScalar m_minSlopeDot;
 };
 …
+{
         m_upAxis = upAxis;
         m_addedMargin = 0.02f;
+        m_addedMargin = 0.02;
         m_walkDirection.setValue(0,0,0);
         m_useGhostObjectSweepTest = true;
 …
         m_turnAngle = btScalar(0.0);
         m_convexShape=convexShape;
+        m_useWalkDirection = true;      // use walk direction by default, legacy behavior
+        m_velocityTimeInterval = 0.0;
+        m_verticalVelocity = 0.0;
+        m_verticalOffset = 0.0;
+        m_gravity = 9.8 * 3 ; // 3G acceleration.
+        m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s.
+        m_jumpSpeed = 10.0; // ?
+        m_wasOnGround = false;
+        m_wasJumping = false;
+        setMaxSlope(btRadians(45.0));
+}
 …
                                 const btManifoldPoint&pt = manifold->getContactPoint(p);
+                                if (pt.getDistance() < 0.0)
+                                btScalar dist = pt.getDistance();
+                                if (dist < 0.0)
+                                {
                                         if (pt.getDistance() < maxPen)
+                                        if (dist < maxPen)
+                                        {
                                                 maxPen = pt.getDistance();
+                                                maxPen = dist;
                                                 m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
+                                        }
                                         m_currentPosition += pt.m_normalWorldOnB * directionSign * pt.getDistance() * btScalar(0.2);
+                                        m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
                                         penetration = true;
                                 } else {
                                         //printf("touching %f\n", pt.getDistance());
+                                        //printf("touching %f\n", dist);
+                                }
+                        }
 …
         // phase 1: up
         btTransform start, end;
         m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * m_stepHeight;
+        m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f));
         start.setIdentity ();
 …
         /* FIXME: Handle penetration properly */
         start.setOrigin (m_currentPosition + upAxisDirection[m_upAxis] * btScalar(0.1f));
+        start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin));
         end.setOrigin (m_targetPosition);
         btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
+        btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071));
         callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
         callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 …
         if (callback.hasHit())
+        {
+                // we moved up only a fraction of the step height
+                m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
+                m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+                // Only modify the position if the hit was a slope and not a wall or ceiling.
+                if(callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0)
+                {
+                        // we moved up only a fraction of the step height
+                        m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
+                        m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+                }
+                m_verticalVelocity = 0.0;
+                m_verticalOffset = 0.0;
         } else {
                 m_currentStepOffset = m_stepHeight;
 …
 void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove)
+{
+        btVector3 originalDir = walkMove.normalized();
+        if (walkMove.length() < SIMD_EPSILON)
+        {
+                originalDir.setValue(0.f,0.f,0.f);
+        }
+//      printf("originalDir=%f,%f,%f\n",originalDir[0],originalDir[1],originalDir[2]);
+        // printf("m_normalizedDirection=%f,%f,%f\n",
+        //      m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
         // phase 2: forward and strafe
         btTransform start, end;
         m_targetPosition = m_currentPosition + walkMove;
         start.setIdentity ();
         end.setIdentity ();
 …
         if (m_touchingContact)
+        {
+                if (originalDir.dot(m_touchingNormal) > btScalar(0.0))
+                if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
+                {
                         updateTargetPositionBasedOnCollision (m_touchingNormal);
+                }
+        }
 …
                 start.setOrigin (m_currentPosition);
                 end.setOrigin (m_targetPosition);
+                btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
+                btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
+                btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0));
                 callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
                 callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 …
+                {
                         // we moved only a fraction
+                        btScalar hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
+                        if (hitDistance<0.f)
+                        {
+//                              printf("neg dist?\n");
+                        }
+                        /* If the distance is farther than the collision margin, move */
+                        if (hitDistance > m_addedMargin)
+                        {
+//                              printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
+                                m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+                        }
+                        btScalar hitDistance;
+                        hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
+//                      m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
                         updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
 …
                                 currentDir.normalize();
                                 /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
                                 if (currentDir.dot(originalDir) <= btScalar(0.0))
+                                if (currentDir.dot(m_normalizedDirection) <= btScalar(0.0))
+                                {
                                         break;
 …
                                 break;
+                        }
                 } else {
                         // we moved whole way
 …
         // phase 3: down
+        btVector3 step_drop = upAxisDirection[m_upAxis] * m_currentStepOffset;
+        btVector3 gravity_drop = upAxisDirection[m_upAxis] * m_stepHeight;
+        m_targetPosition -= (step_drop + gravity_drop);
+        /*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
+        btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
+        btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
+        btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity;
+        m_targetPosition -= (step_drop + gravity_drop);*/
+        btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
+        if(downVelocity > 0.0 && downVelocity < m_stepHeight
+                && (m_wasOnGround || !m_wasJumping))
+        {
+                downVelocity = m_stepHeight;
+        }
+        btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+        m_targetPosition -= step_drop;
         start.setIdentity ();
 …
         end.setOrigin (m_targetPosition);
         btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
+        btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
         callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
         callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 …
                 // we dropped a fraction of the height -> hit floor
                 m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+                m_verticalVelocity = 0.0;
+                m_verticalOffset = 0.0;
+                m_wasJumping = false;
         } else {
                 // we dropped the full height
 …
+}
+void btKinematicCharacterController::setWalkDirection
+(
+const btVector3& walkDirection
+)
+{
+        m_useWalkDirection = true;
+        m_walkDirection = walkDirection;
+        m_normalizedDirection = getNormalizedVector(m_walkDirection);
+}
+void btKinematicCharacterController::setVelocityForTimeInterval
+(
+const btVector3& velocity,
+btScalar timeInterval
+)
+{
+//      printf("setVelocity!\n");
+//      printf("  interval: %f\n", timeInterval);
+//      printf("  velocity: (%f, %f, %f)\n",
+//               velocity.x(), velocity.y(), velocity.z());
+        m_useWalkDirection = false;
+        m_walkDirection = velocity;
+        m_normalizedDirection = getNormalizedVector(m_walkDirection);
+        m_velocityTimeInterval = timeInterval;
+}
 void btKinematicCharacterController::reset ()
+{
 …
                 if (numPenetrationLoops > 4)
+                {
 //                      printf("character could not recover from penetration = %d\n", numPenetrationLoops);
+                        //printf("character could not recover from penetration = %d\n", numPenetrationLoops);
                         break;
+                }
 …
+}
+#include <stdio.h>
 void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWorld, btScalar dt)
+{
+//      printf("playerStep(): ");
+//      printf("  dt = %f", dt);
+        // quick check...
+        if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0) {
+//              printf("\n");
+                return;         // no motion
+        }
+        m_wasOnGround = onGround();
+        // Update fall velocity.
+        m_verticalVelocity -= m_gravity * dt;
+        if(m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
+        {
+                m_verticalVelocity = m_jumpSpeed;
+        }
+        if(m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
+        {
+                m_verticalVelocity = -btFabs(m_fallSpeed);
+        }
+        m_verticalOffset = m_verticalVelocity * dt;
         btTransform xform;
         xform = m_ghostObject->getWorldTransform ();
 …
         stepUp (collisionWorld);
+        stepForwardAndStrafe (collisionWorld, m_walkDirection);
+        if (m_useWalkDirection) {
+                stepForwardAndStrafe (collisionWorld, m_walkDirection);
+        } else {
+                //printf("  time: %f", m_velocityTimeInterval);
+                // still have some time left for moving!
+                btScalar dtMoving =
+                        (dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval;
+                m_velocityTimeInterval -= dt;
+                // how far will we move while we are moving?
+                btVector3 move = m_walkDirection * dtMoving;
+                //printf("  dtMoving: %f", dtMoving);
+                // okay, step
+                stepForwardAndStrafe(collisionWorld, move);
+        }
         stepDown (collisionWorld, dt);
+        // printf("\n");
         xform.setOrigin (m_currentPosition);
 …
         if (!canJump())
                 return;
+        m_verticalVelocity = m_jumpSpeed;
+        m_wasJumping = true;
 #if 0
 …
+}
+void btKinematicCharacterController::setGravity(btScalar gravity)
+{
+        m_gravity = gravity;
+}
+btScalar btKinematicCharacterController::getGravity() const
+{
+        return m_gravity;
+}
+void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians)
+{
+        m_maxSlopeRadians = slopeRadians;
+        m_maxSlopeCosine = btCos(slopeRadians);
+}
+btScalar btKinematicCharacterController::getMaxSlope() const
+{
+        return m_maxSlopeRadians;
+}
 bool btKinematicCharacterController::onGround () const
+{
+        return true;
+}
+void    btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
+{
+}
+        return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
+}
+btVector3* btKinematicCharacterController::getUpAxisDirections()
+{
+        static btVector3 sUpAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) };
+        return sUpAxisDirection;
+}
+void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
+{
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/Character/btKinematicCharacterController.h

-                      r5781
+                      r8284
 */
 #ifndef KINEMATIC_CHARACTER_CONTROLLER_H
 #define KINEMATIC_CHARACTER_CONTROLLER_H
 …
 #include "btCharacterControllerInterface.h"
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
 class btCollisionShape;
 …
+{
 protected:
         btScalar m_halfHeight;
 …
         btConvexShape*  m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
+        btScalar m_verticalVelocity;
+        btScalar m_verticalOffset;
         btScalar m_fallSpeed;
         btScalar m_jumpSpeed;
         btScalar m_maxJumpHeight;
+        btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value)
+        btScalar m_maxSlopeCosine;  // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization)
+        btScalar m_gravity;
         btScalar m_turnAngle;
 …
         ///this is the desired walk direction, set by the user
         btVector3       m_walkDirection;
+        btVector3       m_normalizedDirection;
         //some internal variables
 …
         btVector3 m_touchingNormal;
+        bool  m_wasOnGround;
+        bool  m_wasJumping;
         bool    m_useGhostObjectSweepTest;
+        bool    m_useWalkDirection;
+        btScalar        m_velocityTimeInterval;
+        int m_upAxis;
         int m_upAxis;
+        static btVector3* getUpAxisDirections();
         btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal);
         btVector3 parallelComponent (const btVector3& direction, const btVector3& normal);
 …
+        }
+        virtual void    setWalkDirection(const btVector3& walkDirection)
+        {
+                m_walkDirection = walkDirection;
+        }
+        /// This should probably be called setPositionIncrementPerSimulatorStep.
+        /// This is neither a direction nor a velocity, but the amount to
+        ///     increment the position each simulation iteration, regardless
+        ///     of dt.
+        /// This call will reset any velocity set by setVelocityForTimeInterval().
+        virtual void    setWalkDirection(const btVector3& walkDirection);
+        /// Caller provides a velocity with which the character should move for
+        ///     the given time period.  After the time period, velocity is reset
+        ///     to zero.
+        /// This call will reset any walk direction set by setWalkDirection().
+        /// Negative time intervals will result in no motion.
+        virtual void setVelocityForTimeInterval(const btVector3& velocity,
+                                btScalar timeInterval);
         void reset ();
 …
         void setMaxJumpHeight (btScalar maxJumpHeight);
         bool canJump () const;
         void jump ();
+        void setGravity(btScalar gravity);
+        btScalar getGravity() const;
+        /// The max slope determines the maximum angle that the controller can walk up.
+        /// The slope angle is measured in radians.
+        void setMaxSlope(btScalar slopeRadians);
+        btScalar getMaxSlope() const;
         btPairCachingGhostObject* getGhostObject();

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

-                      r5781
+                      r8284
 #include <new>
+//-----------------------------------------------------------------------------
+//#define CONETWIST_USE_OBSOLETE_SOLVER true
 #define CONETWIST_USE_OBSOLETE_SOLVER false
 #define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
+//-----------------------------------------------------------------------------
+btConeTwistConstraint::btConeTwistConstraint()
+:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
+m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
+{
+}
+SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld)
+{
+        btVector3 vec = axis * invInertiaWorld;
+        return axis.dot(vec);
+}
 …
         m_maxMotorImpulse = btScalar(-1);
         setLimit(btScalar(1e30), btScalar(1e30), btScalar(1e30));
+        setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
         m_damping = btScalar(0.01);
         m_fixThresh = CONETWIST_DEF_FIX_THRESH;
+}
+//-----------------------------------------------------------------------------
+        m_flags = 0;
+        m_linCFM = btScalar(0.f);
+        m_linERP = btScalar(0.7f);
+        m_angCFM = btScalar(0.f);
+}
 void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
 …
                 info->m_numConstraintRows = 3;
                 info->nub = 3;
                 calcAngleInfo2();
+                calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
                 if(m_solveSwingLimit)
+                {
 …
+                }
+        }
+} // btConeTwistConstraint::getInfo1()
+}
+void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
+{
+        //always reserve 6 rows: object transform is not available on SPU
+        info->m_numConstraintRows = 6;
+        info->nub = 0;
+}
-//-----------------------------------------------------------------------------
 void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
+{
+        getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
+}
+void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
+{
+        calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB);
         btAssert(!m_useSolveConstraintObsolete);
-        //retrieve matrices
-        btTransform body0_trans;
-        body0_trans = m_rbA.getCenterOfMassTransform();
-    btTransform body1_trans;
-        body1_trans = m_rbB.getCenterOfMassTransform();
     // set jacobian
     info->m_J1linearAxis[0] = 1;
     info->m_J1linearAxis[info->rowskip+1] = 1;
     info->m_J1linearAxis[2*info->rowskip+2] = 1;
         btVector3 a1 = body0_trans.getBasis() * m_rbAFrame.getOrigin();
+        btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin();
+        {
                 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
 …
                 a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+        }
         btVector3 a2 = body1_trans.getBasis() * m_rbBFrame.getOrigin();
+        btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin();
+        {
                 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
 …
+        }
     // set right hand side
+    btScalar k = info->fps * info->erp;
+        btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp;
+    btScalar k = info->fps * linERP;
     int j;
         for (j=0; j<3; j++)
+    {
         info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
+        info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
                 info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
                 info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
+                if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
+                {
+                        info->cfm[j*info->rowskip] = m_linCFM;
+                }
+    }
         int row = 3;
 …
                 if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
+                {
                         btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
+                        btTransform trA = transA*m_rbAFrame;
                         btVector3 p = trA.getBasis().getColumn(1);
                         btVector3 q = trA.getBasis().getColumn(2);
 …
                         info->m_constraintError[srow] = k * m_swingCorrection;
+                        info->cfm[srow] = 0.0f;
+                        if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
+                        {
+                                info->cfm[srow] = m_angCFM;
+                        }
                         // m_swingCorrection is always positive or 0
                         info->m_lowerLimit[srow] = 0;
 …
                 btScalar k = info->fps * m_biasFactor;
                 info->m_constraintError[srow] = k * m_twistCorrection;
+                info->cfm[srow] = 0.0f;
+                if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
+                {
+                        info->cfm[srow] = m_angCFM;
+                }
                 if(m_twistSpan > 0.0f)
+                {
 …
+}
+//-----------------------------------------------------------------------------
 void    btConeTwistConstraint::buildJacobian()
 …
                 m_accTwistLimitImpulse = btScalar(0.);
                 m_accSwingLimitImpulse = btScalar(0.);
+                m_accMotorImpulse = btVector3(0.,0.,0.);
                 if (!m_angularOnly)
 …
+                }
+                calcAngleInfo2();
+        }
+}
+//-----------------------------------------------------------------------------
+void    btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
+{
+                calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
+        }
+}
+void    btConeTwistConstraint::solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar   timeStep)
+{
+        #ifndef __SPU__
         if (m_useSolveConstraintObsolete)
+        {
 …
                         btVector3 vel1;
                         bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+                        bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
                         btVector3 vel2;
                         bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+                        bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
                         btVector3 vel = vel1 - vel2;
 …
                                 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);
+                                bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
+                                bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
+                        }
 …
                         btTransform trACur = m_rbA.getCenterOfMassTransform();
                         btTransform trBCur = m_rbB.getCenterOfMassTransform();
                         btVector3 omegaA; bodyA.getAngularVelocity(omegaA);
                         btVector3 omegaB; bodyB.getAngularVelocity(omegaB);
+                        btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA);
+                        btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB);
                         btTransform trAPred; trAPred.setIdentity();
                         btVector3 zerovec(0,0,0);
 …
                                 btVector3 impulseAxis =  impulse / impulseMag;
                                 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
                                 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+                        }
+                }
                 else // no motor: do a little damping
+                {
                         const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
                         const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
+                                bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
+                                bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+                        }
+                }
+                else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
+                {
+                        btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA);
+                        btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB);
                         btVector3 relVel = angVelB - angVelA;
                         if (relVel.length2() > SIMD_EPSILON)
 …
                                 btScalar  impulseMag  = impulse.length();
                                 btVector3 impulseAxis = impulse / impulseMag;
                                 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
                                 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+                                bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
+                                bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+                        }
+                }
 …
                         ///solve angular part
                         btVector3 angVelA;
                         bodyA.getAngularVelocity(angVelA);
+                        bodyA.internalGetAngularVelocity(angVelA);
                         btVector3 angVelB;
                         bodyB.getAngularVelocity(angVelB);
+                        bodyB.internalGetAngularVelocity(angVelB);
                         // solve swing limit
 …
                                 btVector3 noTwistSwingAxis = impulse / impulseMag;
                                 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
                                 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
+                                bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
+                                bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
+                        }
 …
                                 btVector3 impulse = m_twistAxis * impulseMag;
                                 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
                                 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
+                                bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
+                                bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
+                        }
+                }
+        }
+}
+//-----------------------------------------------------------------------------
+#else
+btAssert(0);
+#endif //__SPU__
+}
 void    btConeTwistConstraint::updateRHS(btScalar       timeStep)
 …
+}
+//-----------------------------------------------------------------------------
+#ifndef __SPU__
 void btConeTwistConstraint::calcAngleInfo()
+{
 …
+                }
+        }
 } // btConeTwistConstraint::calcAngleInfo()
+}
+#endif //__SPU__
 static btVector3 vTwist(1,0,0); // twist axis in constraint's space
+//-----------------------------------------------------------------------------
 void btConeTwistConstraint::calcAngleInfo2()
+void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
+{
         m_swingCorrection = btScalar(0.);
 …
         m_solveTwistLimit = false;
         m_solveSwingLimit = false;
+        // compute rotation of A wrt B (in constraint space)
+        if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
+        {       // it is assumed that setMotorTarget() was alredy called
+                // and motor target m_qTarget is within constraint limits
+                // TODO : split rotation to pure swing and pure twist
+                // compute desired transforms in world
+                btTransform trPose(m_qTarget);
+                btTransform trA = transA * m_rbAFrame;
+                btTransform trB = transB * m_rbBFrame;
+                btTransform trDeltaAB = trB * trPose * trA.inverse();
+                btQuaternion qDeltaAB = trDeltaAB.getRotation();
+                btVector3 swingAxis =   btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
+                m_swingAxis = swingAxis;
+                m_swingAxis.normalize();
+                m_swingCorrection = qDeltaAB.getAngle();
+                if(!btFuzzyZero(m_swingCorrection))
+                {
+                        m_solveSwingLimit = true;
+                }
+                return;
+        }
+        {
                 // compute rotation of A wrt B (in constraint space)
                 btQuaternion qA = getRigidBodyA().getCenterOfMassTransform().getRotation() * m_rbAFrame.getRotation();
                 btQuaternion qB = getRigidBodyB().getCenterOfMassTransform().getRotation() * m_rbBFrame.getRotation();
+                btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
+                btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation();
                 btQuaternion qAB = qB.inverse() * qA;
                 // split rotation into cone and twist
                 // (all this is done from B's perspective. Maybe I should be averaging axes...)
 …
                                 m_kSwing =  btScalar(1.) /
                                         (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
                                          getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
+                                        (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) +
+                                         computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB));
+                        }
+                }
 …
                         // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
                         // anyway, we have either hinge or fixed joint
                         btVector3 ivA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
                         btVector3 jvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
                         btVector3 kvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
                         btVector3 ivB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(0);
+                        btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
+                        btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
+                        btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2);
+                        btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0);
                         btVector3 target;
                         btScalar x = ivB.dot(ivA);
 …
                                 m_kTwist = btScalar(1.) /
                                         (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
                                          getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
+                                        (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
+                                         computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
+                        }
 …
+                }
+        }
 } // btConeTwistConstraint::calcAngleInfo2()
+}
 …
+}
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+///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 btConeTwistConstraint::setParam(int num, btScalar value, int axis)
+{
+        switch(num)
+        {
+                case BT_CONSTRAINT_ERP :
+                case BT_CONSTRAINT_STOP_ERP :
+                        if((axis >= 0) && (axis < 3))
+                        {
+                                m_linERP = value;
+                                m_flags |= BT_CONETWIST_FLAGS_LIN_ERP;
+                        }
+                        else
+                        {
+                                m_biasFactor = value;
+                        }
+                        break;
+                case BT_CONSTRAINT_CFM :
+                case BT_CONSTRAINT_STOP_CFM :
+                        if((axis >= 0) && (axis < 3))
+                        {
+                                m_linCFM = value;
+                                m_flags |= BT_CONETWIST_FLAGS_LIN_CFM;
+                        }
+                        else
+                        {
+                                m_angCFM = value;
+                                m_flags |= BT_CONETWIST_FLAGS_ANG_CFM;
+                        }
+                        break;
+                default:
+                        btAssertConstrParams(0);
+                        break;
+        }
+}
+///return the local value of parameter
+btScalar btConeTwistConstraint::getParam(int num, int axis) const
+{
+        btScalar retVal = 0;
+        switch(num)
+        {
+                case BT_CONSTRAINT_ERP :
+                case BT_CONSTRAINT_STOP_ERP :
+                        if((axis >= 0) && (axis < 3))
+                        {
+                                btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP);
+                                retVal = m_linERP;
+                        }
+                        else if((axis >= 3) && (axis < 6))
+                        {
+                                retVal = m_biasFactor;
+                        }
+                        else
+                        {
+                                btAssertConstrParams(0);
+                        }
+                        break;
+                case BT_CONSTRAINT_CFM :
+                case BT_CONSTRAINT_STOP_CFM :
+                        if((axis >= 0) && (axis < 3))
+                        {
+                                btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM);
+                                retVal = m_linCFM;
+                        }
+                        else if((axis >= 3) && (axis < 6))
+                        {
+                                btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM);
+                                retVal = m_angCFM;
+                        }
+                        else
+                        {
+                                btAssertConstrParams(0);
+                        }
+                        break;
+                default :
+                        btAssertConstrParams(0);
+        }
+        return retVal;
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h

-                      r5781
+                      r8284
+/*
+Overview:
+btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc).
+It is a fixed translation, 3 degree-of-freedom (DOF) rotational "joint".
+It divides the 3 rotational DOFs into swing (movement within a cone) and twist.
+Swing is divided into swing1 and swing2 which can have different limits, giving an elliptical shape.
+(Note: the cone's base isn't flat, so this ellipse is "embedded" on the surface of a sphere.)
+In the contraint's frame of reference:
+twist is along the x-axis,
+and swing 1 and 2 are along the z and y axes respectively.
+*/
 #ifndef CONETWISTCONSTRAINT_H
 #define CONETWISTCONSTRAINT_H
 …
 class btRigidBody;
+enum btConeTwistFlags
+{
+        BT_CONETWIST_FLAGS_LIN_CFM = 1,
+        BT_CONETWIST_FLAGS_LIN_ERP = 2,
+        BT_CONETWIST_FLAGS_ANG_CFM = 4
+};
 ///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
 …
         btVector3        m_accMotorImpulse;
+        // parameters
+        int                     m_flags;
+        btScalar        m_linCFM;
+        btScalar        m_linERP;
+        btScalar        m_angCFM;
+protected:
+        void init();
+        void computeConeLimitInfo(const btQuaternion& qCone, // in
+                btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
+        void computeTwistLimitInfo(const btQuaternion& qTwist, // in
+                btScalar& twistAngle, btVector3& vTwistAxis); // all outs
+        void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
 public:
 …
         btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
-        btConeTwistConstraint();
         virtual void    buildJacobian();
         virtual void getInfo1 (btConstraintInfo1* info);
+        void    getInfo1NonVirtual(btConstraintInfo1* info);
         virtual void getInfo2 (btConstraintInfo2* info);
+        virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
+        void    getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
+        virtual void    solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar  timeStep);
         void    updateRHS(btScalar      timeStep);
 …
+        }
+        void    setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan,  btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+        // setLimit(), a few notes:
+        // _softness:
+        //              0->1, recommend ~0.8->1.
+        //              describes % of limits where movement is free.
+        //              beyond this softness %, the limit is gradually enforced until the "hard" (1.0) limit is reached.
+        // _biasFactor:
+        //              0->1?, recommend 0.3 +/-0.3 or so.
+        //              strength with which constraint resists zeroth order (angular, not angular velocity) limit violation.
+        // __relaxationFactor:
+        //              0->1, recommend to stay near 1.
+        //              the lower the value, the less the constraint will fight velocities which violate the angular limits.
+        void    setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+        {
                 m_swingSpan1 = _swingSpan1;
 …
         void calcAngleInfo();
         void calcAngleInfo2();
+        void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
         inline btScalar getSwingSpan1()
 …
         btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
+protected:
+        void init();
+        void computeConeLimitInfo(const btQuaternion& qCone, // in
+                btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
+        void computeTwistLimitInfo(const btQuaternion& qTwist, // in
+                btScalar& twistAngle, btVector3& vTwistAxis); // all outs
+        void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
+        ///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.
+        virtual void setParam(int num, btScalar value, int axis = -1);
+        ///return the local value of parameter
+        virtual btScalar getParam(int num, int axis = -1) const;
+        virtual int     calculateSerializeBufferSize() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 };
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btConeTwistConstraintData
+{
+        btTypedConstraintData   m_typeConstraintData;
+        btTransformFloatData m_rbAFrame;
+        btTransformFloatData m_rbBFrame;
+        //limits
+        float   m_swingSpan1;
+        float   m_swingSpan2;
+        float   m_twistSpan;
+        float   m_limitSoftness;
+        float   m_biasFactor;
+        float   m_relaxationFactor;
+        float   m_damping;
+        char m_pad[4];
+};
+SIMD_FORCE_INLINE int   btConeTwistConstraint::calculateSerializeBufferSize() const
+{
+        return sizeof(btConeTwistConstraintData);
+}
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE const char*   btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btConeTwistConstraintData* cone = (btConeTwistConstraintData*) dataBuffer;
+        btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer);
+        m_rbAFrame.serializeFloat(cone->m_rbAFrame);
+        m_rbBFrame.serializeFloat(cone->m_rbBFrame);
+        cone->m_swingSpan1 = float(m_swingSpan1);
+        cone->m_swingSpan2 = float(m_swingSpan2);
+        cone->m_twistSpan = float(m_twistSpan);
+        cone->m_limitSoftness = float(m_limitSoftness);
+        cone->m_biasFactor = float(m_biasFactor);
+        cone->m_relaxationFactor = float(m_relaxationFactor);
+        cone->m_damping = float(m_damping);
+        return "btConeTwistConstraintData";
+}
 #endif //CONETWISTCONSTRAINT_H

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp

-                      r5781
+                      r8284
 . This notice may not be removed or altered from any source distribution.
 */
+#include "btContactConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btContactSolverInfo.h"
+#include "LinearMath/btMinMax.h"
+#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
+btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB)
+:btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB),
+        m_contactManifold(*contactManifold)
+{
+}
+btContactConstraint::~btContactConstraint()
+{
+}
+void    btContactConstraint::setContactManifold(btPersistentManifold* contactManifold)
+{
+        m_contactManifold = *contactManifold;
+}
+void btContactConstraint::getInfo1 (btConstraintInfo1* info)
+{
+}
+void btContactConstraint::getInfo2 (btConstraintInfo2* info)
+{
+}
+void    btContactConstraint::buildJacobian()
+{
+}
 …
-//response  between two dynamic objects with friction
-btScalar resolveSingleCollision(
-        btRigidBody& body1,
-        btRigidBody& body2,
-        btManifoldPoint& contactPoint,
-        const btContactSolverInfo& solverInfo)
+{
-        const btVector3& pos1_ = contactPoint.getPositionWorldOnA();
-        const btVector3& pos2_ = contactPoint.getPositionWorldOnB();
-        const btVector3& normal = contactPoint.m_normalWorldOnB;
-        //constant over all iterations
-        btVector3 rel_pos1 = pos1_ - body1.getCenterOfMassPosition();
-        btVector3 rel_pos2 = pos2_ - body2.getCenterOfMassPosition();
-        btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-        btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-        btVector3 vel = vel1 - vel2;
-        btScalar rel_vel;
-        rel_vel = normal.dot(vel);
-        btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
-        // btScalar damping = solverInfo.m_damping ;
-        btScalar Kerp = solverInfo.m_erp;
-        btScalar Kcor = Kerp *Kfps;
-        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-        btAssert(cpd);
-        btScalar distance = cpd->m_penetration;
-        btScalar positionalError = Kcor *-distance;
-        btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
-        btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
-        btScalar        velocityImpulse = velocityError * cpd->m_jacDiagABInv;
-        btScalar normalImpulse = penetrationImpulse+velocityImpulse;
-        // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
-        btScalar oldNormalImpulse = cpd->m_appliedImpulse;
-        btScalar sum = oldNormalImpulse + normalImpulse;
-        cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
-        normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
-#ifdef USE_INTERNAL_APPLY_IMPULSE
-        if (body1.getInvMass())
+        {
-                body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
+        }
-        if (body2.getInvMass())
+        {
-                body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
+        }
-#else //USE_INTERNAL_APPLY_IMPULSE
-        body1.applyImpulse(normal*(normalImpulse), rel_pos1);
-        body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
-#endif //USE_INTERNAL_APPLY_IMPULSE
-        return normalImpulse;
+}
-btScalar resolveSingleFriction(
-        btRigidBody& body1,
-        btRigidBody& body2,
-        btManifoldPoint& contactPoint,
-        const btContactSolverInfo& solverInfo)
+{
-        (void)solverInfo;
-        const btVector3& pos1 = contactPoint.getPositionWorldOnA();
-        const btVector3& pos2 = contactPoint.getPositionWorldOnB();
-        btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
-        btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
-        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-        btAssert(cpd);
-        btScalar combinedFriction = cpd->m_friction;
-        btScalar limit = cpd->m_appliedImpulse * combinedFriction;
-        if (cpd->m_appliedImpulse>btScalar(0.))
-        //friction
+        {
-                //apply friction in the 2 tangential directions
-                // 1st tangent
-                btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-                btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-                btVector3 vel = vel1 - vel2;
-                btScalar j1,j2;
+                {
-                        btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
-                        // calculate j that moves us to zero relative velocity
-                        j1 = -vrel * cpd->m_jacDiagABInvTangent0;
-                        btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse0;
-                        cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1;
-                        btSetMin(cpd->m_accumulatedTangentImpulse0, limit);
-                        btSetMax(cpd->m_accumulatedTangentImpulse0, -limit);
-                        j1 = cpd->m_accumulatedTangentImpulse0 - oldTangentImpulse;
+                }
+                {
-                        // 2nd tangent
-                        btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
-                        // calculate j that moves us to zero relative velocity
-                        j2 = -vrel * cpd->m_jacDiagABInvTangent1;
-                        btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse1;
-                        cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2;
-                        btSetMin(cpd->m_accumulatedTangentImpulse1, limit);
-                        btSetMax(cpd->m_accumulatedTangentImpulse1, -limit);
-                        j2 = cpd->m_accumulatedTangentImpulse1 - oldTangentImpulse;
+                }
-#ifdef USE_INTERNAL_APPLY_IMPULSE
-        if (body1.getInvMass())
+        {
-                body1.internalApplyImpulse(cpd->m_frictionWorldTangential0*body1.getInvMass(),cpd->m_frictionAngularComponent0A,j1);
-                body1.internalApplyImpulse(cpd->m_frictionWorldTangential1*body1.getInvMass(),cpd->m_frictionAngularComponent1A,j2);
+        }
-        if (body2.getInvMass())
+        {
-                body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1);
-                body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2);
+        }
-#else //USE_INTERNAL_APPLY_IMPULSE
-        body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
-        body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
-#endif //USE_INTERNAL_APPLY_IMPULSE
+        }
-        return cpd->m_appliedImpulse;
+}
-btScalar resolveSingleFrictionOriginal(
-        btRigidBody& body1,
-        btRigidBody& body2,
-        btManifoldPoint& contactPoint,
-        const btContactSolverInfo& solverInfo);
-btScalar resolveSingleFrictionOriginal(
-        btRigidBody& body1,
-        btRigidBody& body2,
-        btManifoldPoint& contactPoint,
-        const btContactSolverInfo& solverInfo)
+{
-        (void)solverInfo;
-        const btVector3& pos1 = contactPoint.getPositionWorldOnA();
-        const btVector3& pos2 = contactPoint.getPositionWorldOnB();
-        btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
-        btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
-        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-        btAssert(cpd);
-        btScalar combinedFriction = cpd->m_friction;
-        btScalar limit = cpd->m_appliedImpulse * combinedFriction;
-        //if (contactPoint.m_appliedImpulse>btScalar(0.))
-        //friction
+        {
-                //apply friction in the 2 tangential directions
+                {
-                        // 1st tangent
-                        btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-                        btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-                        btVector3 vel = vel1 - vel2;
-                        btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
-                        // calculate j that moves us to zero relative velocity
-                        btScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
-                        btScalar total = cpd->m_accumulatedTangentImpulse0 + j;
-                        btSetMin(total, limit);
-                        btSetMax(total, -limit);
-                        j = total - cpd->m_accumulatedTangentImpulse0;
-                        cpd->m_accumulatedTangentImpulse0 = total;
-                        body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1);
-                        body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2);
+                }
+                {
-                        // 2nd tangent
-                        btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-                        btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-                        btVector3 vel = vel1 - vel2;
-                        btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
-                        // calculate j that moves us to zero relative velocity
-                        btScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
-                        btScalar total = cpd->m_accumulatedTangentImpulse1 + j;
-                        btSetMin(total, limit);
-                        btSetMax(total, -limit);
-                        j = total - cpd->m_accumulatedTangentImpulse1;
-                        cpd->m_accumulatedTangentImpulse1 = total;
-                        body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1);
-                        body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2);
+                }
+        }
-        return cpd->m_appliedImpulse;
+}
-//velocity + friction
-//response  between two dynamic objects with friction
-btScalar resolveSingleCollisionCombined(
-        btRigidBody& body1,
-        btRigidBody& body2,
-        btManifoldPoint& contactPoint,
-        const btContactSolverInfo& solverInfo)
+{
-        const btVector3& pos1 = contactPoint.getPositionWorldOnA();
-        const btVector3& pos2 = contactPoint.getPositionWorldOnB();
-        const btVector3& normal = contactPoint.m_normalWorldOnB;
-        btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
-        btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
-        btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-        btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-        btVector3 vel = vel1 - vel2;
-        btScalar rel_vel;
-        rel_vel = normal.dot(vel);
-        btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
-        //btScalar damping = solverInfo.m_damping ;
-        btScalar Kerp = solverInfo.m_erp;
-        btScalar Kcor = Kerp *Kfps;
-        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-        btAssert(cpd);
-        btScalar distance = cpd->m_penetration;
-        btScalar positionalError = Kcor *-distance;
-        btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
-        btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
-        btScalar        velocityImpulse = velocityError * cpd->m_jacDiagABInv;
-        btScalar normalImpulse = penetrationImpulse+velocityImpulse;
-        // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
-        btScalar oldNormalImpulse = cpd->m_appliedImpulse;
-        btScalar sum = oldNormalImpulse + normalImpulse;
-        cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
-        normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
-#ifdef USE_INTERNAL_APPLY_IMPULSE
-        if (body1.getInvMass())
+        {
-                body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
+        }
-        if (body2.getInvMass())
+        {
-                body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
+        }
-#else //USE_INTERNAL_APPLY_IMPULSE
-        body1.applyImpulse(normal*(normalImpulse), rel_pos1);
-        body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
-#endif //USE_INTERNAL_APPLY_IMPULSE
+        {
-                //friction
-                btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-                btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-                btVector3 vel = vel1 - vel2;
-                rel_vel = normal.dot(vel);
-                btVector3 lat_vel = vel - normal * rel_vel;
-                btScalar lat_rel_vel = lat_vel.length();
-                btScalar combinedFriction = cpd->m_friction;
-                if (cpd->m_appliedImpulse > 0)
-                if (lat_rel_vel > SIMD_EPSILON)
+                {
-                        lat_vel /= lat_rel_vel;
-                        btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
-                        btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
-                        btScalar friction_impulse = lat_rel_vel /
-                                (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
-                        btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;
-                        btSetMin(friction_impulse, normal_impulse);
-                        btSetMax(friction_impulse, -normal_impulse);
-                        body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
-                        body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
+                }
+        }
-        return normalImpulse;
+}
-btScalar resolveSingleFrictionEmpty(
-        btRigidBody& body1,
-        btRigidBody& body2,
-        btManifoldPoint& contactPoint,
-        const btContactSolverInfo& solverInfo);
-btScalar resolveSingleFrictionEmpty(
-        btRigidBody& body1,
-        btRigidBody& body2,
-        btManifoldPoint& contactPoint,
-        const btContactSolverInfo& solverInfo)
+{
-        (void)contactPoint;
-        (void)body1;
-        (void)body2;
-        (void)solverInfo;
-        return btScalar(0.);
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.h

-                      r5781
+                      r8284
 #define CONTACT_CONSTRAINT_H
+///@todo: make into a proper class working with the iterative constraint solver
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btRigidBody;
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btScalar.h"
+struct btContactSolverInfo;
+class btManifoldPoint;
+///btContactConstraint can be automatically created to solve contact constraints using the unified btTypedConstraint interface
+ATTRIBUTE_ALIGNED16(class) btContactConstraint : public btTypedConstraint
+{
+protected:
+enum {
+        DEFAULT_CONTACT_SOLVER_TYPE=0,
+        CONTACT_SOLVER_TYPE1,
+        CONTACT_SOLVER_TYPE2,
+        USER_CONTACT_SOLVER_TYPE1,
+        MAX_CONTACT_SOLVER_TYPES
+        btPersistentManifold m_contactManifold;
+public:
+        btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB);
+        void    setContactManifold(btPersistentManifold* contactManifold);
+        btPersistentManifold* getContactManifold()
+        {
+                return &m_contactManifold;
+        }
+        const btPersistentManifold* getContactManifold() const
+        {
+                return &m_contactManifold;
+        }
+        virtual ~btContactConstraint();
+        virtual void getInfo1 (btConstraintInfo1* info);
+        virtual void getInfo2 (btConstraintInfo2* info);
+        ///obsolete methods
+        virtual void    buildJacobian();
 };
+typedef btScalar (*ContactSolverFunc)(btRigidBody& body1,
+                                                                         btRigidBody& body2,
+                                                                         class btManifoldPoint& contactPoint,
+                                                                         const btContactSolverInfo& info);
+///stores some extra information to each contact point. It is not in the contact point, because that want to keep the collision detection independent from the constraint solver.
+struct btConstraintPersistentData
+{
+        inline btConstraintPersistentData()
+        :m_appliedImpulse(btScalar(0.)),
+        m_prevAppliedImpulse(btScalar(0.)),
+        m_accumulatedTangentImpulse0(btScalar(0.)),
+        m_accumulatedTangentImpulse1(btScalar(0.)),
+        m_jacDiagABInv(btScalar(0.)),
+        m_persistentLifeTime(0),
+        m_restitution(btScalar(0.)),
+        m_friction(btScalar(0.)),
+        m_penetration(btScalar(0.)),
+        m_contactSolverFunc(0),
+        m_frictionSolverFunc(0)
+        {
+        }
+                                /// total applied impulse during most recent frame
+                        btScalar        m_appliedImpulse;
+                        btScalar        m_prevAppliedImpulse;
+                        btScalar        m_accumulatedTangentImpulse0;
+                        btScalar        m_accumulatedTangentImpulse1;
+                        btScalar        m_jacDiagABInv;
+                        btScalar        m_jacDiagABInvTangent0;
+                        btScalar        m_jacDiagABInvTangent1;
+                        int             m_persistentLifeTime;
+                        btScalar        m_restitution;
+                        btScalar        m_friction;
+                        btScalar        m_penetration;
+                        btVector3       m_frictionWorldTangential0;
+                        btVector3       m_frictionWorldTangential1;
+                        btVector3       m_frictionAngularComponent0A;
+                        btVector3       m_frictionAngularComponent0B;
+                        btVector3       m_frictionAngularComponent1A;
+                        btVector3       m_frictionAngularComponent1B;
+                        //some data doesn't need to be persistent over frames: todo: clean/reuse this
+                        btVector3       m_angularComponentA;
+                        btVector3       m_angularComponentB;
+                        ContactSolverFunc       m_contactSolverFunc;
+                        ContactSolverFunc       m_frictionSolverFunc;
+};
+///bilateral constraint between two dynamic objects
+///positive distance = separation, negative distance = penetration
+///resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects
 void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
                       btRigidBody& body2, const btVector3& pos2,
 …
-///contact constraint resolution:
-///calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
-///positive distance = separation, negative distance = penetration
-btScalar resolveSingleCollision(
-        btRigidBody& body1,
-        btRigidBody& body2,
-                btManifoldPoint& contactPoint,
-                 const btContactSolverInfo& info);
-btScalar resolveSingleFriction(
-        btRigidBody& body1,
-        btRigidBody& body2,
-        btManifoldPoint& contactPoint,
-        const btContactSolverInfo& solverInfo
-                );
-btScalar resolveSingleCollisionCombined(
-        btRigidBody& body1,
-        btRigidBody& body2,
-        btManifoldPoint& contactPoint,
-        const btContactSolverInfo& solverInfo
-                );
 #endif //CONTACT_CONSTRAINT_H

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

-                      r5781
+                      r8284
         btScalar        m_tau;
         btScalar        m_damping;
+        btScalar        m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
         btScalar        m_friction;
         btScalar        m_timeStep;
 …
         int                     m_solverMode;
         int     m_restingContactRestitutionThreshold;
+        int                     m_minimumSolverBatchSize;
 …
                 m_linearSlop = btScalar(0.0);
                 m_warmstartingFactor=btScalar(0.85);
                 m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD ;//SOLVER_RANDMIZE_ORDER
+                m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
                 m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution
+                m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
+        }
 };

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp

-                      r5781
+                      r8284
 #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)
 …
 , 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);
 …
+}
+//-----------------------------------------------------------------------------
 ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
 …
                 return 0;
+        }
         if (test_value < m_loLimit)
+        {
 …
+}
+//-----------------------------------------------------------------------------
 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;
 …
         if (m_currentLimit!=0)
+        {
                 target_velocity = -m_ERP*m_currentLimitError/(timeStep);
+                target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
                 maxMotorForce = m_maxLimitForce;
+        }
 …
         btVector3 angVelA;
         bodyA.getAngularVelocity(angVelA);
+        body0->internalGetAngularVelocity(angVelA);
         btVector3 angVelB;
         bodyB.getAngularVelocity(angVelB);
+        body1->internalGetAngularVelocity(angVelB);
         btVector3 vel_diff;
 …
         // 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;
 …
         //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);
 …
         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,
 …
         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;
 …
         //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];
 …
         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);
 …
+}
-//-----------------------------------------------------------------------------
 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(
 …
+}
+//-----------------------------------------------------------------------------
 void btGeneric6DofConstraint::buildAngularJacobian(
 …
+}
+//-----------------------------------------------------------------------------
 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);
 …
+}
+//-----------------------------------------------------------------------------
 void btGeneric6DofConstraint::buildJacobian()
+{
+#ifndef __SPU__
         if (m_useSolveConstraintObsolete)
+        {
 …
+                }
                 //calculates transform
                 calculateTransforms();
+                calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
                 //  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
 …
+        }
+}
+//-----------------------------------------------------------------------------
+#endif //__SPU__
+}
 void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
 …
+        {
                 //prepare constraint
                 calculateTransforms();
+                calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
                 info->m_numConstraintRows = 0;
                 info->nub = 6;
 …
+}
+//-----------------------------------------------------------------------------
+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;
 …
                         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;
 …
                         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);
+                        }
+                }
+        }
 …
+}
+//-----------------------------------------------------------------------------
 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;
 …
+                {
                         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);
+                }
+        }
 …
+}
+//-----------------------------------------------------------------------------
+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)
 …
+}
+//-----------------------------------------------------------------------------
 btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
 …
+}
+//-----------------------------------------------------------------------------
+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)
 …
         m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
         return;
 } // btGeneric6DofConstraint::calcAnchorPos()
+//-----------------------------------------------------------------------------
+}
 void btGeneric6DofConstraint::calculateLinearInfo()
 …
         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;
 …
             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
 …
         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;
 …
         if(limit)
+        {
             btScalar k = info->fps * limot->m_ERP;
+            btScalar k = info->fps * limot->m_stopERP;
                         if(!rotational)
+                        {
 …
                                 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
 …
                     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
 …
+}
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+        ///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;
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h

-                      r5781
+                      r8284
 . This notice may not be removed or altered from any source distribution.
 */
+/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev
+/// Added support for generic constraint solver through getInfo1/getInfo2 methods
 /*
 -09-09
 …
     btScalar m_damping;//!< Damping.
     btScalar m_limitSoftness;//! Relaxation factor
+    btScalar m_ERP;//!< Error tolerance factor when joint is at limit
+    btScalar m_normalCFM;//!< Constraint force mixing factor
+    btScalar m_stopERP;//!< Error tolerance factor when joint is at limit
+    btScalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit
     btScalar m_bounce;//!< restitution factor
     bool m_enableMotor;
 …
     //!@{
     btScalar m_currentLimitError;//!  How much is violated this limit
+    btScalar m_currentPosition;     //!  current value of angle
     int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
     btScalar m_accumulatedImpulse;
 …
         m_maxMotorForce = 0.1f;
         m_maxLimitForce = 300.0f;
+        m_loLimit = -SIMD_INFINITY;
+        m_hiLimit = SIMD_INFINITY;
+        m_ERP = 0.5f;
+        m_loLimit = 1.0f;
+        m_hiLimit = -1.0f;
+                m_normalCFM = 0.f;
+                m_stopERP = 0.2f;
+                m_stopCFM = 0.f;
         m_bounce = 0.0f;
         m_damping = 1.0f;
 …
         m_loLimit = limot.m_loLimit;
         m_hiLimit = limot.m_hiLimit;
+        m_ERP = limot.m_ERP;
+                m_normalCFM = limot.m_normalCFM;
+                m_stopERP = limot.m_stopERP;
+                m_stopCFM =     limot.m_stopCFM;
         m_bounce = limot.m_bounce;
         m_currentLimit = limot.m_currentLimit;
 …
         //! apply the correction impulses for two bodies
     btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB);
+    btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
 };
 …
     btScalar    m_damping;//!< Damping for linear limit
     btScalar    m_restitution;//! Bounce parameter for linear limit
+        btVector3       m_normalCFM;//!< Constraint force mixing factor
+    btVector3   m_stopERP;//!< Error tolerance factor when joint is at limit
+        btVector3       m_stopCFM;//!< Constraint force mixing factor when joint is at limit
     //!@}
         bool            m_enableMotor[3];
 …
     btVector3   m_maxMotorForce;//!< max force on motor
     btVector3   m_currentLimitError;//!  How much is violated this limit
+    btVector3   m_currentLinearDiff;//!  Current relative offset of constraint frames
     int                 m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
 …
         m_upperLimit.setValue(0.f,0.f,0.f);
         m_accumulatedImpulse.setValue(0.f,0.f,0.f);
+                m_normalCFM.setValue(0.f, 0.f, 0.f);
+                m_stopERP.setValue(0.2f, 0.2f, 0.2f);
+                m_stopCFM.setValue(0.f, 0.f, 0.f);
         m_limitSoftness = 0.7f;
 …
         m_damping = other.m_damping;
         m_restitution = other.m_restitution;
+                m_normalCFM = other.m_normalCFM;
+                m_stopERP = other.m_stopERP;
+                m_stopCFM = other.m_stopCFM;
                 for(int i=0; i < 3; i++)
+                {
 …
         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,
 …
 };
+enum bt6DofFlags
+{
+        BT_6DOF_FLAGS_CFM_NORM = 1,
+        BT_6DOF_FLAGS_CFM_STOP = 2,
+        BT_6DOF_FLAGS_ERP_STOP = 4
+};
+#define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
 /// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
 …
 <li> Angulars limits have these possible ranges:
 <table border=1 >
+<tr
+<tr>
         <td><b>AXIS</b></td>
         <td><b>MIN ANGLE</b></td>
         <td><b>MAX ANGLE</b></td>
+</tr><tr>
         <td>X</td>
+                <td>-PI</td>
+                <td>PI</td>
+        <td>-PI</td>
+        <td>PI</td>
+</tr><tr>
         <td>Y</td>
+                <td>-PI/2</td>
+                <td>PI/2</td>
+        <td>-PI/2</td>
+        <td>PI/2</td>
+</tr><tr>
         <td>Z</td>
                 <td>-PI/2</td>
                 <td>PI/2</td>
+        <td>-PI</td>
+        <td>PI</td>
 </tr>
 </table>
 …
     btVector3 m_calculatedAxis[3];
     btVector3 m_calculatedLinearDiff;
+        btScalar        m_factA;
+        btScalar        m_factB;
+        bool            m_hasStaticBody;
         btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
     bool        m_useLinearReferenceFrameA;
+        bool    m_useOffsetForConstraintFrame;
+        int             m_flags;
     //!@}
 …
         int setAngularLimits(btConstraintInfo2 *info, int row_offset);
         int setLinearLimits(btConstraintInfo2 *info);
+        int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
+        int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
     void buildLinearJacobian(
 …
     btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
+    btGeneric6DofConstraint();
+    btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
         //! Calcs global transform of the offsets
         /*!
 …
         \sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
         */
+    void calculateTransforms();
+    void calculateTransforms(const btTransform& transA,const btTransform& transB);
+        void calculateTransforms();
         //! Gets the global transform of the offset for body A
 …
         virtual void getInfo1 (btConstraintInfo1* info);
+        void getInfo1NonVirtual (btConstraintInfo1* info);
         virtual void getInfo2 (btConstraintInfo2* info);
+    virtual     void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
+        void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
     void        updateRHS(btScalar      timeStep);
 …
     //! Get the relative Euler angle
     /*!
         \pre btGeneric6DofConstraint.buildJacobian must be called previously.
+        \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
         */
     btScalar getAngle(int axis_index) const;
+        //! Get the relative position of the constraint pivot
+    /*!
+        \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
+        */
+        btScalar getRelativePivotPosition(int axis_index) const;
         //! Test angular limit.
         /*!
         Calculates angular correction and returns true if limit needs to be corrected.
         \pre btGeneric6DofConstraint.buildJacobian must be called previously.
+        \pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
         */
     bool testAngularLimitMotor(int axis_index);
 …
     void        setAngularLowerLimit(const btVector3& angularLower)
+    {
+        m_angularLimits[0].m_loLimit = angularLower.getX();
+        m_angularLimits[1].m_loLimit = angularLower.getY();
+        m_angularLimits[2].m_loLimit = angularLower.getZ();
+                for(int i = 0; i < 3; i++)
+                        m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
+    }
     void        setAngularUpperLimit(const btVector3& angularUpper)
+    {
+        m_angularLimits[0].m_hiLimit = angularUpper.getX();
+        m_angularLimits[1].m_hiLimit = angularUpper.getY();
+        m_angularLimits[2].m_hiLimit = angularUpper.getZ();
+                for(int i = 0; i < 3; i++)
+                        m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
+    }
 …
         else
+        {
+                        lo = btNormalizeAngle(lo);
+                        hi = btNormalizeAngle(hi);
                 m_angularLimits[axis-3].m_loLimit = lo;
                 m_angularLimits[axis-3].m_hiLimit = hi;
 …
+    }
-    const btRigidBody& getRigidBodyA() const
+    {
-        return m_rbA;
+    }
-    const btRigidBody& getRigidBodyB() const
+    {
-        return m_rbB;
+    }
         virtual void calcAnchorPos(void); // overridable
         int 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 = false);
+        // access for UseFrameOffset
+        bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
+        void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
+        ///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.
+        virtual void setParam(int num, btScalar value, int axis = -1);
+        ///return the local value of parameter
+        virtual btScalar getParam(int num, int axis = -1) const;
+        virtual int     calculateSerializeBufferSize() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 };
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btGeneric6DofConstraintData
+{
+        btTypedConstraintData   m_typeConstraintData;
+        btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+        btTransformFloatData m_rbBFrame;
+        btVector3FloatData      m_linearUpperLimit;
+        btVector3FloatData      m_linearLowerLimit;
+        btVector3FloatData      m_angularUpperLimit;
+        btVector3FloatData      m_angularLowerLimit;
+        int     m_useLinearReferenceFrameA;
+        int m_useOffsetForConstraintFrame;
+};
+SIMD_FORCE_INLINE       int     btGeneric6DofConstraint::calculateSerializeBufferSize() const
+{
+        return sizeof(btGeneric6DofConstraintData);
+}
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE       const char*     btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btGeneric6DofConstraintData* dof = (btGeneric6DofConstraintData*)dataBuffer;
+        btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
+        m_frameInA.serializeFloat(dof->m_rbAFrame);
+        m_frameInB.serializeFloat(dof->m_rbBFrame);
+        int i;
+        for (i=0;i<3;i++)
+        {
+                dof->m_angularLowerLimit.m_floats[i] =  float(m_angularLimits[i].m_loLimit);
+                dof->m_angularUpperLimit.m_floats[i] =  float(m_angularLimits[i].m_hiLimit);
+                dof->m_linearLowerLimit.m_floats[i] = float(m_linearLimits.m_lowerLimit[i]);
+                dof->m_linearUpperLimit.m_floats[i] = float(m_linearLimits.m_upperLimit[i]);
+        }
+        dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
+        dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
+        return "btGeneric6DofConstraintData";
+}
 #endif //GENERIC_6DOF_CONSTRAINT_H

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

-                      r5781
+                      r8284
 #include "btSolverBody.h"
+//-----------------------------------------------------------------------------
+//#define HINGE_USE_OBSOLETE_SOLVER false
 #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);
+}
+//-----------------------------------------------------------------------------
+#define HINGE_USE_FRAME_OFFSET true
+#ifndef __SPU__
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
                                                                          btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA)
+                                                                         const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA)
                                                                          :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
                                                                          m_angularOnly(false),
                                                                          m_enableAngularMotor(false),
                                                                          m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+                                                                         m_useReferenceFrameA(useReferenceFrameA)
+                                                                         m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
+                                                                         m_useReferenceFrameA(useReferenceFrameA),
+                                                                         m_flags(0)
+{
         m_rbAFrame.getOrigin() = pivotInA;
 …
         //start with free
         m_lowerLimit = btScalar(1e30);
         m_upperLimit = btScalar(-1e30);
+        m_lowerLimit = btScalar(1.0f);
+        m_upperLimit = btScalar(-1.0f);
         m_biasFactor = 0.3f;
         m_relaxationFactor = 1.0f;
 …
+}
+//-----------------------------------------------------------------------------
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA)
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA)
 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false),
 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
+m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
+m_useReferenceFrameA(useReferenceFrameA),
+m_flags(0)
+{
 …
         //start with free
         m_lowerLimit = btScalar(1e30);
         m_upperLimit = btScalar(-1e30);
+        m_lowerLimit = btScalar(1.0f);
+        m_upperLimit = btScalar(-1.0f);
         m_biasFactor = 0.3f;
         m_relaxationFactor = 1.0f;
 …
+}
+//-----------------------------------------------------------------------------
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB,
 …
 m_enableAngularMotor(false),
 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
+m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
+m_useReferenceFrameA(useReferenceFrameA),
+m_flags(0)
+{
         //start with free
         m_lowerLimit = btScalar(1e30);
         m_upperLimit = btScalar(-1e30);
+        m_lowerLimit = btScalar(1.0f);
+        m_upperLimit = btScalar(-1.0f);
         m_biasFactor = 0.3f;
         m_relaxationFactor = 1.0f;
 …
+}
+//-----------------------------------------------------------------------------
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
 …
 m_enableAngularMotor(false),
 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
+m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
+m_useReferenceFrameA(useReferenceFrameA),
+m_flags(0)
+{
         ///not providing rigidbody B means implicitly using worldspace for body B
 …
         //start with free
         m_lowerLimit = btScalar(1e30);
         m_upperLimit = btScalar(-1e30);
+        m_lowerLimit = btScalar(1.0f);
+        m_upperLimit = btScalar(-1.0f);
         m_biasFactor = 0.3f;
         m_relaxationFactor = 1.0f;
 …
+}
+//-----------------------------------------------------------------------------
 void    btHingeConstraint::buildJacobian()
 …
+        {
                 m_appliedImpulse = btScalar(0.);
+                m_accMotorImpulse = btScalar(0.);
                 if (!m_angularOnly)
 …
                 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;
 …
                         // test angular limit
                         testLimit();
+                        testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
                 //Compute K = J*W*J' for hinge axis
 …
+}
+//-----------------------------------------------------------------------------
+#endif //__SPU__
 void btHingeConstraint::getInfo1(btConstraintInfo1* info)
 …
                 info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
                 info->nub = 1;
+                //always add the row, to avoid computation (data is not available yet)
                 //prepare constraint
                 testLimit();
+                testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
                 if(getSolveLimit() || getEnableAngularMotor())
+                {
 …
                         info->nub--;
+                }
+        }
+} // btHingeConstraint::getInfo1 ()
+//-----------------------------------------------------------------------------
+        }
+}
+void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
+{
+        if (m_useSolveConstraintObsolete)
+        {
+                info->m_numConstraintRows = 0;
+                info->nub = 0;
+        }
+        else
+        {
+                //always add the 'limit' row, to avoid computation (data is not available yet)
+                info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular
+                info->nub = 0;
+        }
+}
 void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
+{
+        if(m_useOffsetForConstraintFrame)
+        {
+                getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
+        }
+        else
+        {
+                getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
+        }
+}
+void    btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
+{
+        ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now
+        testLimit(transA,transB);
+        getInfo2Internal(info,transA,transB,angVelA,angVelB);
+}
+void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
+{
         btAssert(!m_useSolveConstraintObsolete);
         int i, s = info->rowskip;
+        int i, skip = info->rowskip;
         // transforms in world space
         btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
         btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame;
+        btTransform trA = transA*m_rbAFrame;
+        btTransform trB = transB*m_rbBFrame;
         // pivot point
         btVector3 pivotAInW = trA.getOrigin();
         btVector3 pivotBInW = trB.getOrigin();
+#if 0
+        if (0)
+        {
+                for (i=0;i<6;i++)
+                {
+                        info->m_J1linearAxis[i*skip]=0;
+                        info->m_J1linearAxis[i*skip+1]=0;
+                        info->m_J1linearAxis[i*skip+2]=0;
+                        info->m_J1angularAxis[i*skip]=0;
+                        info->m_J1angularAxis[i*skip+1]=0;
+                        info->m_J1angularAxis[i*skip+2]=0;
+                        info->m_J2angularAxis[i*skip]=0;
+                        info->m_J2angularAxis[i*skip+1]=0;
+                        info->m_J2angularAxis[i*skip+2]=0;
+                        info->m_constraintError[i*skip]=0.f;
+                }
+        }
+#endif //#if 0
         // 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();
+        if (!m_angularOnly)
+        {
+                info->m_J1linearAxis[0] = 1;
+                info->m_J1linearAxis[skip + 1] = 1;
+                info->m_J1linearAxis[2 * skip + 2] = 1;
+        }
+        btVector3 a1 = pivotAInW - transA.getOrigin();
+        {
                 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
                 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s);
                 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s);
+                btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
+                btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
                 btVector3 a1neg = -a1;
                 a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+        }
         btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin();
+        btVector3 a2 = pivotBInW - transB.getOrigin();
+        {
                 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
                 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s);
                 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s);
+                btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
+                btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
                 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]);
+    }
+        if (!m_angularOnly)
+        {
+                for(i = 0; i < 3; i++)
+                {
+                        info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
+                }
+        }
         // make rotations around X and Y equal
         // the hinge axis should be the only unconstrained
 …
+                }
                 info->m_constraintError[srow] = btScalar(0.0f);
+                btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
                 if(powered)
+                {
+            info->cfm[srow] = btScalar(0.0);
+                        btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp);
+                        if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
+                        {
+                                info->cfm[srow] = m_normalCFM;
+                        }
+                        btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
                         info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
                         info->m_lowerLimit[srow] = - m_maxMotorImpulse;
 …
                 if(limit)
+                {
                         k = info->fps * info->erp;
+                        k = info->fps * currERP;
                         info->m_constraintError[srow] += k * limit_err;
+                        info->cfm[srow] = btScalar(0.0);
+                        if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
+                        {
+                                info->cfm[srow] = m_stopCFM;
+                        }
                         if(lostop == histop)
+                        {
 …
                         if(bounce > btScalar(0.0))
+                        {
                                 btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
                                 vel -= m_rbB.getAngularVelocity().dot(ax1);
+                                btScalar vel = angVelA.dot(ax1);
+                                vel -= angVelB.dot(ax1);
                                 // only apply bounce if the velocity is incoming, and if the
                                 // resulting c[] exceeds what we already have.
 …
+}
+//-----------------------------------------------------------------------------
+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)
 …
+}
-//-----------------------------------------------------------------------------
 btScalar btHingeConstraint::getHingeAngle()
+{
+        const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
+        const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
+        const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
+        btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
+        return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+}
+btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB)
+{
+        const btVector3 refAxis0  = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
+        const btVector3 refAxis1  = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
+        const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1);
+//      btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
+        btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
         return m_referenceSign * angle;
+}
+//-----------------------------------------------------------------------------
+#if 0
 void btHingeConstraint::testLimit()
+{
 …
+        }
         return;
+} // btHingeConstraint::testLimit()
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+}
+#else
+void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
+{
+        // Compute limit information
+        m_hingeAngle = getHingeAngle(transA,transB);
+        m_correction = btScalar(0.);
+        m_limitSign = btScalar(0.);
+        m_solveLimit = false;
+        if (m_lowerLimit <= m_upperLimit)
+        {
+                m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, 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;
+}
+#endif
+static btVector3 vHinge(0, 0, btScalar(1));
+void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
+{
+        // convert target from body to constraint space
+        btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation();
+        qConstraint.normalize();
+        // extract "pure" hinge component
+        btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize();
+        btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
+        btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
+        qHinge.normalize();
+        // compute angular target, clamped to limits
+        btScalar targetAngle = qHinge.getAngle();
+        if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
+        {
+                qHinge = operator-(qHinge);
+                targetAngle = qHinge.getAngle();
+        }
+        if (qHinge.getZ() < 0)
+                targetAngle = -targetAngle;
+        setMotorTarget(targetAngle, dt);
+}
+void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
+{
+        if (m_lowerLimit < m_upperLimit)
+        {
+                if (targetAngle < m_lowerLimit)
+                        targetAngle = m_lowerLimit;
+                else if (targetAngle > m_upperLimit)
+                        targetAngle = m_upperLimit;
+        }
+        // compute angular velocity
+        btScalar curAngle  = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+        btScalar dAngle = targetAngle - curAngle;
+        m_motorTargetVelocity = dAngle / dt;
+}
+void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
+{
+        btAssert(!m_useSolveConstraintObsolete);
+        int i, s = info->rowskip;
+        // transforms in world space
+        btTransform trA = transA*m_rbAFrame;
+        btTransform trB = transB*m_rbBFrame;
+        // pivot point
+        btVector3 pivotAInW = trA.getOrigin();
+        btVector3 pivotBInW = trB.getOrigin();
+#if 1
+        // difference between frames in WCS
+        btVector3 ofs = trB.getOrigin() - trA.getOrigin();
+        // now get weight factors depending on masses
+        btScalar miA = getRigidBodyA().getInvMass();
+        btScalar miB = getRigidBodyB().getInvMass();
+        bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
+        btScalar miS = miA + miB;
+        btScalar factA, factB;
+        if(miS > btScalar(0.f))
+        {
+                factA = miB / miS;
+        }
+        else
+        {
+                factA = btScalar(0.5f);
+        }
+        factB = btScalar(1.0f) - factA;
+        // get the desired direction of hinge axis
+        // as weighted sum of Z-orthos of frameA and frameB in WCS
+        btVector3 ax1A = trA.getBasis().getColumn(2);
+        btVector3 ax1B = trB.getBasis().getColumn(2);
+        btVector3 ax1 = ax1A * factA + ax1B * factB;
+        ax1.normalize();
+        // fill first 3 rows
+        // we want: velA + wA x relA == velB + wB x relB
+        btTransform bodyA_trans = transA;
+        btTransform bodyB_trans = transB;
+        int s0 = 0;
+        int s1 = s;
+        int s2 = s * 2;
+        int nrow = 2; // last filled row
+        btVector3 tmpA, tmpB, relA, relB, p, q;
+        // get vector from bodyB to frameB in WCS
+        relB = trB.getOrigin() - bodyB_trans.getOrigin();
+        // get its projection to hinge axis
+        btVector3 projB = ax1 * relB.dot(ax1);
+        // get vector directed from bodyB to hinge axis (and orthogonal to it)
+        btVector3 orthoB = relB - projB;
+        // same for bodyA
+        relA = trA.getOrigin() - bodyA_trans.getOrigin();
+        btVector3 projA = ax1 * relA.dot(ax1);
+        btVector3 orthoA = relA - projA;
+        btVector3 totalDist = projA - projB;
+        // get offset vectors relA and relB
+        relA = orthoA + totalDist * factA;
+        relB = orthoB - totalDist * factB;
+        // now choose average ortho to hinge axis
+        p = orthoB * factA + orthoA * factB;
+        btScalar len2 = p.length2();
+        if(len2 > SIMD_EPSILON)
+        {
+                p /= btSqrt(len2);
+        }
+        else
+        {
+                p = trA.getBasis().getColumn(1);
+        }
+        // make one more ortho
+        q = ax1.cross(p);
+        // fill three rows
+        tmpA = relA.cross(p);
+        tmpB = relB.cross(p);
+    for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i];
+    for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i];
+        tmpA = relA.cross(q);
+        tmpB = relB.cross(q);
+        if(hasStaticBody && getSolveLimit())
+        { // to make constraint between static and dynamic objects more rigid
+                // remove wA (or wB) from equation if angular limit is hit
+                tmpB *= factB;
+                tmpA *= factA;
+        }
+        for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i];
+    for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i];
+        tmpA = relA.cross(ax1);
+        tmpB = relB.cross(ax1);
+        if(hasStaticBody)
+        { // to make constraint between static and dynamic objects more rigid
+                // remove wA (or wB) from equation
+                tmpB *= factB;
+                tmpA *= factA;
+        }
+        for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
+    for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
+        btScalar k = info->fps * info->erp;
+        if (!m_angularOnly)
+        {
+                for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i];
+                for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i];
+                for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i];
+        // compute three elements of right hand side
+                btScalar rhs = k * p.dot(ofs);
+                info->m_constraintError[s0] = rhs;
+                rhs = k * q.dot(ofs);
+                info->m_constraintError[s1] = rhs;
+                rhs = k * ax1.dot(ofs);
+                info->m_constraintError[s2] = rhs;
+        }
+        // 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.
+        int s3 = 3 * s;
+        int s4 = 4 * s;
+        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 ax1A,ax1B are the unit length hinge axes as computed from bodyA and
+        // bodyB, 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.
+        k = info->fps * info->erp;
+        btVector3 u = ax1A.cross(ax1B);
+        info->m_constraintError[s3] = k * u.dot(p);
+        info->m_constraintError[s4] = k * u.dot(q);
+#endif
+        // check angular limits
+        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);
+                btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
+                if(powered)
+                {
+                        if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
+                        {
+                                info->cfm[srow] = m_normalCFM;
+                        }
+                        btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
+                        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 * currERP;
+                        info->m_constraintError[srow] += k * limit_err;
+                        if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
+                        {
+                                info->cfm[srow] = m_stopCFM;
+                        }
+                        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 = angVelA.dot(ax1);
+                                vel -= angVelB.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
+}
+///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 btHingeConstraint::setParam(int num, btScalar value, int axis)
+{
+        if((axis == -1) || (axis == 5))
+        {
+                switch(num)
+                {
+                        case BT_CONSTRAINT_STOP_ERP :
+                                m_stopERP = value;
+                                m_flags |= BT_HINGE_FLAGS_ERP_STOP;
+                                break;
+                        case BT_CONSTRAINT_STOP_CFM :
+                                m_stopCFM = value;
+                                m_flags |= BT_HINGE_FLAGS_CFM_STOP;
+                                break;
+                        case BT_CONSTRAINT_CFM :
+                                m_normalCFM = value;
+                                m_flags |= BT_HINGE_FLAGS_CFM_NORM;
+                                break;
+                        default :
+                                btAssertConstrParams(0);
+                }
+        }
+        else
+        {
+                btAssertConstrParams(0);
+        }
+}
+///return the local value of parameter
+btScalar btHingeConstraint::getParam(int num, int axis) const
+{
+        btScalar retVal = 0;
+        if((axis == -1) || (axis == 5))
+        {
+                switch(num)
+                {
+                        case BT_CONSTRAINT_STOP_ERP :
+                                btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP);
+                                retVal = m_stopERP;
+                                break;
+                        case BT_CONSTRAINT_STOP_CFM :
+                                btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP);
+                                retVal = m_stopCFM;
+                                break;
+                        case BT_CONSTRAINT_CFM :
+                                btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
+                                retVal = m_normalCFM;
+                                break;
+                        default :
+                                btAssertConstrParams(0);
+                }
+        }
+        else
+        {
+                btAssertConstrParams(0);
+        }
+        return retVal;
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h

-                      r5781
+                      r8284
 class btRigidBody;
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btHingeConstraintData   btHingeConstraintDoubleData
+#define btHingeConstraintDataName       "btHingeConstraintDoubleData"
+#else
+#define btHingeConstraintData   btHingeConstraintFloatData
+#define btHingeConstraintDataName       "btHingeConstraintFloatData"
+#endif //BT_USE_DOUBLE_PRECISION
+enum btHingeFlags
+{
+        BT_HINGE_FLAGS_CFM_STOP = 1,
+        BT_HINGE_FLAGS_ERP_STOP = 2,
+        BT_HINGE_FLAGS_CFM_NORM = 4
+};
 /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
 /// axis defines the orientation of the hinge axis
 class btHingeConstraint : public btTypedConstraint
+ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint
+{
 #ifdef IN_PARALLELL_SOLVER
 …
         bool            m_solveLimit;
         bool            m_useSolveConstraintObsolete;
+        bool            m_useOffsetForConstraintFrame;
         bool            m_useReferenceFrameA;
+        btScalar        m_accMotorImpulse;
+        int                     m_flags;
+        btScalar        m_normalCFM;
+        btScalar        m_stopCFM;
+        btScalar        m_stopERP;
 public:
         btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA = false);
         btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA = false);
+        btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false);
+        btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false);
         btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
 …
         btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
-        btHingeConstraint();
         virtual void    buildJacobian();
 …
         virtual void getInfo1 (btConstraintInfo1* info);
+        void getInfo1NonVirtual(btConstraintInfo1* info);
         virtual void getInfo2 (btConstraintInfo2* info);
+        virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
+        void    getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
+        void    getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
+        void    getInfo2InternalUsingFrameOffset(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
         void    updateRHS(btScalar      timeStep);
 …
+        }
+        // extra motor API, including ability to set a target rotation (as opposed to angular velocity)
+        // note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to
+        //       maintain a given angular target.
+        void enableMotor(bool enableMotor)      { m_enableAngularMotor = enableMotor; }
+        void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; }
+        void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B.
+        void setMotorTarget(btScalar targetAngle, btScalar dt);
         void    setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+        {
                 m_lowerLimit = low;
                 m_upperLimit = high;
+                m_lowerLimit = btNormalizeAngle(low);
+                m_upperLimit = btNormalizeAngle(high);
                 m_limitSoftness =  _softness;
 …
+        }
+        void    setAxis(btVector3& axisInA)
+        {
+                btVector3 rbAxisA1, rbAxisA2;
+                btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
+                btVector3 pivotInA = m_rbAFrame.getOrigin();
+//              m_rbAFrame.getOrigin() = pivotInA;
+                m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
+                                                                                rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
+                                                                                rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
+                btVector3 axisInB = m_rbA.getCenterOfMassTransform().getBasis() * axisInA;
+                btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
+                btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
+                btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
+                m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(pivotInA);
+                m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
+                                                                                rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
+                                                                                rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
+        }
         btScalar        getLowerLimit() const
+        {
 …
         btScalar getHingeAngle();
+        void testLimit();
+        const btTransform& getAFrame() { return m_rbAFrame; };
+        const btTransform& getBFrame() { return m_rbBFrame; };
+        btScalar getHingeAngle(const btTransform& transA,const btTransform& transB);
+        void testLimit(const btTransform& transA,const btTransform& transB);
+        const btTransform& getAFrame() const { return m_rbAFrame; };
+        const btTransform& getBFrame() const { return m_rbBFrame; };
+        btTransform& getAFrame() { return m_rbAFrame; };
+        btTransform& getBFrame() { return m_rbBFrame; };
         inline int getSolveLimit()
 …
                 return m_maxMotorImpulse;
+        }
+        // access for UseFrameOffset
+        bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
+        void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
+        ///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.
+        virtual void    setParam(int num, btScalar value, int axis = -1);
+        ///return the local value of parameter
+        virtual btScalar getParam(int num, int axis = -1) const;
+        virtual int     calculateSerializeBufferSize() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 };
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btHingeConstraintDoubleData
+{
+        btTypedConstraintData   m_typeConstraintData;
+        btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+        btTransformDoubleData m_rbBFrame;
+        int                     m_useReferenceFrameA;
+        int                     m_angularOnly;
+        int                     m_enableAngularMotor;
+        float   m_motorTargetVelocity;
+        float   m_maxMotorImpulse;
+        float   m_lowerLimit;
+        float   m_upperLimit;
+        float   m_limitSoftness;
+        float   m_biasFactor;
+        float   m_relaxationFactor;
+};
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btHingeConstraintFloatData
+{
+        btTypedConstraintData   m_typeConstraintData;
+        btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+        btTransformFloatData m_rbBFrame;
+        int                     m_useReferenceFrameA;
+        int                     m_angularOnly;
+        int                     m_enableAngularMotor;
+        float   m_motorTargetVelocity;
+        float   m_maxMotorImpulse;
+        float   m_lowerLimit;
+        float   m_upperLimit;
+        float   m_limitSoftness;
+        float   m_biasFactor;
+        float   m_relaxationFactor;
+};
+SIMD_FORCE_INLINE       int     btHingeConstraint::calculateSerializeBufferSize() const
+{
+        return sizeof(btHingeConstraintData);
+}
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE       const char*     btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btHingeConstraintData* hingeData = (btHingeConstraintData*)dataBuffer;
+        btTypedConstraint::serialize(&hingeData->m_typeConstraintData,serializer);
+        m_rbAFrame.serialize(hingeData->m_rbAFrame);
+        m_rbBFrame.serialize(hingeData->m_rbBFrame);
+        hingeData->m_angularOnly = m_angularOnly;
+        hingeData->m_enableAngularMotor = m_enableAngularMotor;
+        hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse);
+        hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity);
+        hingeData->m_useReferenceFrameA = m_useReferenceFrameA;
+        hingeData->m_lowerLimit = float(m_lowerLimit);
+        hingeData->m_upperLimit = float(m_upperLimit);
+        hingeData->m_limitSoftness = float(m_limitSoftness);
+        hingeData->m_biasFactor = float(m_biasFactor);
+        hingeData->m_relaxationFactor = float(m_relaxationFactor);
+        return btHingeConstraintDataName;
+}
 #endif //HINGECONSTRAINT_H

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btJacobianEntry.h

r5781	r8284
29	29	/// it can be used in combination with a constraint solver
30	30	/// Can be used to relate the effect of an impulse to the constraint error
31		~~class~~ btJacobianEntry
	31	ATTRIBUTE_ALIGNED16(class) btJacobianEntry
32	32	{
33	33	public:

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp

-                      r5781
+                      r8284
+btPoint2PointConstraint::btPoint2PointConstraint()
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE),
+m_useSolveConstraintObsolete(false)
+{
+}
 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
+m_flags(0),
 m_useSolveConstraintObsolete(false)
+{
 …
 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
+m_flags(0),
 m_useSolveConstraintObsolete(false)
+{
 …
 void    btPoint2PointConstraint::buildJacobian()
+{
         ///we need it for both methods
+        {
 …
+        }
+}
+}
 void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
+{
+        getInfo1NonVirtual(info);
+}
+void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
+{
         if (m_useSolveConstraintObsolete)
 …
+}
 void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
+{
+        getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+}
+void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
+{
         btAssert(!m_useSolveConstraintObsolete);
          //retrieve matrices
-        btTransform body0_trans;
-        body0_trans = m_rbA.getCenterOfMassTransform();
-    btTransform body1_trans;
-        body1_trans = m_rbB.getCenterOfMassTransform();
         // anchor points in global coordinates with respect to body PORs.
 …
     // set jacobian
     info->m_J1linearAxis[0] = 1;
     info->m_J1linearAxis[info->rowskip+1] = 1;
     info->m_J1linearAxis[2*info->rowskip+2] = 1;
+        info->m_J1linearAxis[info->rowskip+1] = 1;
+        info->m_J1linearAxis[2*info->rowskip+2] = 1;
         btVector3 a1 = body0_trans.getBasis()*getPivotInA();
 …
     // set right hand side
+    btScalar k = info->fps * info->erp;
+        btScalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp;
+    btScalar k = info->fps * currERP;
     int j;
         for (j=0; j<3; j++)
+    {
         info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] -                     a1[j] - body0_trans.getOrigin()[j]);
+        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
                 //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
+    }
+        if(m_flags & BT_P2P_FLAGS_CFM)
+        {
+                for (j=0; j<3; j++)
+                {
+                        info->cfm[j*info->rowskip] = m_cfm;
+                }
+        }
         btScalar impulseClamp = m_setting.m_impulseClamp;//
 …
+                }
+        }
+}
+void    btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar       timeStep)
+{
+        if (m_useSolveConstraintObsolete)
+        {
+                btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
+                btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
+                btVector3 normal(0,0,0);
+        //      btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
+        //      btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
+                for (int i=0;i<3;i++)
+                {
+                        normal[i] = 1;
+                        btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+                        btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
+                        btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+                        //this jacobian entry could be re-used for all iterations
+                        btVector3 vel1,vel2;
+                        bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+                        bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+                        btVector3 vel = vel1 - vel2;
+                        btScalar rel_vel;
+                        rel_vel = normal.dot(vel);
+                /*
+                        //velocity error (first order error)
+                        btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
+                                                                                                                        m_rbB.getLinearVelocity(),angvelB);
+                */
+                        //positional error (zeroth order error)
+                        btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+                        btScalar deltaImpulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
+                        btScalar impulseClamp = m_setting.m_impulseClamp;
+                        const btScalar sum = btScalar(m_appliedImpulse) + deltaImpulse;
+                        if (sum < -impulseClamp)
+                        {
+                                deltaImpulse = -impulseClamp-m_appliedImpulse;
+                                m_appliedImpulse = -impulseClamp;
+                        }
+                        else if (sum > impulseClamp)
+                        {
+                                deltaImpulse = impulseClamp-m_appliedImpulse;
+                                m_appliedImpulse = impulseClamp;
+                        }
+                        else
+                        {
+                                m_appliedImpulse = sum;
+                        }
+                        btVector3 impulse_vector = normal * deltaImpulse;
+                        btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
+                        btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
+                        bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,deltaImpulse);
+                        bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-deltaImpulse);
+                        normal[i] = 0;
+                }
+        }
+}
+        info->m_damping = m_setting.m_damping;
+}
 void    btPoint2PointConstraint::updateRHS(btScalar     timeStep)
 …
+}
+///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 btPoint2PointConstraint::setParam(int num, btScalar value, int axis)
+{
+        if(axis != -1)
+        {
+                btAssertConstrParams(0);
+        }
+        else
+        {
+                switch(num)
+                {
+                        case BT_CONSTRAINT_ERP :
+                        case BT_CONSTRAINT_STOP_ERP :
+                                m_erp = value;
+                                m_flags |= BT_P2P_FLAGS_ERP;
+                                break;
+                        case BT_CONSTRAINT_CFM :
+                        case BT_CONSTRAINT_STOP_CFM :
+                                m_cfm = value;
+                                m_flags |= BT_P2P_FLAGS_CFM;
+                                break;
+                        default:
+                                btAssertConstrParams(0);
+                }
+        }
+}
+///return the local value of parameter
+btScalar btPoint2PointConstraint::getParam(int num, int axis) const
+{
+        btScalar retVal(SIMD_INFINITY);
+        if(axis != -1)
+        {
+                btAssertConstrParams(0);
+        }
+        else
+        {
+                switch(num)
+                {
+                        case BT_CONSTRAINT_ERP :
+                        case BT_CONSTRAINT_STOP_ERP :
+                                btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP);
+                                retVal = m_erp;
+                                break;
+                        case BT_CONSTRAINT_CFM :
+                        case BT_CONSTRAINT_STOP_CFM :
+                                btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM);
+                                retVal = m_cfm;
+                                break;
+                        default:
+                                btAssertConstrParams(0);
+                }
+        }
+        return retVal;
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h

-                      r5781
+                      r8284
 class btRigidBody;
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btPoint2PointConstraintData     btPoint2PointConstraintDoubleData
+#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData"
+#else
+#define btPoint2PointConstraintData     btPoint2PointConstraintFloatData
+#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData"
+#endif //BT_USE_DOUBLE_PRECISION
 struct  btConstraintSetting
+{
 …
 };
+enum btPoint2PointFlags
+{
+        BT_P2P_FLAGS_ERP = 1,
+        BT_P2P_FLAGS_CFM = 2
+};
 /// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
 class btPoint2PointConstraint : public btTypedConstraint
+ATTRIBUTE_ALIGNED16(class) btPoint2PointConstraint : public btTypedConstraint
+{
 #ifdef IN_PARALLELL_SOLVER
 …
         btVector3       m_pivotInB;
+        int                     m_flags;
+        btScalar        m_erp;
+        btScalar        m_cfm;
 public:
 …
         btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA);
-        btPoint2PointConstraint();
         virtual void    buildJacobian();
 …
         virtual void getInfo1 (btConstraintInfo1* info);
+        void getInfo1NonVirtual (btConstraintInfo1* info);
         virtual void getInfo2 (btConstraintInfo2* info);
+        virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
+        void getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans);
         void    updateRHS(btScalar      timeStep);
 …
+        }
+        ///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.
+        virtual void    setParam(int num, btScalar value, int axis = -1);
+        ///return the local value of parameter
+        virtual btScalar getParam(int num, int axis = -1) const;
+        virtual int     calculateSerializeBufferSize() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 };
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btPoint2PointConstraintFloatData
+{
+        btTypedConstraintData   m_typeConstraintData;
+        btVector3FloatData      m_pivotInA;
+        btVector3FloatData      m_pivotInB;
+};
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btPoint2PointConstraintDoubleData
+{
+        btTypedConstraintData   m_typeConstraintData;
+        btVector3DoubleData     m_pivotInA;
+        btVector3DoubleData     m_pivotInB;
+};
+SIMD_FORCE_INLINE       int     btPoint2PointConstraint::calculateSerializeBufferSize() const
+{
+        return sizeof(btPoint2PointConstraintData);
+}
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE       const char*     btPoint2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btPoint2PointConstraintData* p2pData = (btPoint2PointConstraintData*)dataBuffer;
+        btTypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer);
+        m_pivotInA.serialize(p2pData->m_pivotInA);
+        m_pivotInB.serialize(p2pData->m_pivotInB);
+        return btPoint2PointConstraintDataName;
+}
 #endif //POINT2POINTCONSTRAINT_H

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

-                      r5781
+                      r8284
 #include <string.h> //for memset
+int             gNumSplitImpulseRecoveries = 0;
 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
 :m_btSeed2(0)
 …
 // Project Gauss Seidel or the equivalent Sequential Impulse
 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
+{
 #ifdef USE_SIMD
 …
         __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
         __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
         __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
         __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
+        __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
+        __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
 …
         deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
         __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
         __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
+        __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
+        __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
         __m128 impulseMagnitude = deltaImpulse;
         body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
         body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
         body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
         body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+        body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
+        body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
+        body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
+        body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
 #else
         resolveSingleConstraintRowGeneric(body1,body2,c);
 …
 // Project Gauss Seidel or the equivalent Sequential Impulse
  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
+{
         btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
         const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.m_deltaLinearVelocity)      + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
         const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
         const btScalar delta_rel_vel    =       deltaVel1Dotn-deltaVel2Dotn;
+        const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity())   + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
+        const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
+//      const btScalar delta_rel_vel    =       deltaVel1Dotn-deltaVel2Dotn;
         deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
         deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
 …
                 c.m_appliedImpulse = sum;
+        }
+        if (body1.m_invMass)
+                body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
+        if (body2.m_invMass)
+                body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
+}
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+                body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
+                body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+}
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
+{
 #ifdef USE_SIMD
 …
         __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
         __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
         __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
         __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
+        __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
+        __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
 …
         deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
         __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
         __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
+        __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
+        __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
         __m128 impulseMagnitude = deltaImpulse;
         body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
         body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
         body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
         body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+        body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
+        body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
+        body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
+        body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
 #else
         resolveSingleConstraintRowLowerLimit(body1,body2,c);
 …
 // Project Gauss Seidel or the equivalent Sequential Impulse
  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
+{
         btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
         const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.m_deltaLinearVelocity)      + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
         const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
+        const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity())   + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
+        const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
         deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
 …
                 c.m_appliedImpulse = sum;
+        }
+        if (body1.m_invMass)
+                body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
+        if (body2.m_invMass)
+                body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
+        body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
+        body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+}
+void    btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
+        btRigidBody& body1,
+        btRigidBody& body2,
+        const btSolverConstraint& c)
+{
+                if (c.m_rhsPenetration)
+        {
+                        gNumSplitImpulseRecoveries++;
+                        btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
+                        const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.internalGetPushVelocity())  + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
+                        const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
+                        deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
+                        deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
+                        const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
+                        if (sum < c.m_lowerLimit)
+                        {
+                                deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse;
+                                c.m_appliedPushImpulse = c.m_lowerLimit;
+                        }
+                        else
+                        {
+                                c.m_appliedPushImpulse = sum;
+                        }
+                        body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
+                        body2.internalApplyPushImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+        }
+}
+ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
+{
+#ifdef USE_SIMD
+        if (!c.m_rhsPenetration)
+                return;
+        gNumSplitImpulseRecoveries++;
+        __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
+        __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+        __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+        __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm)));
+        __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
+        __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128));
+        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+        btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
+        btSimdScalar resultLowerLess,resultUpperLess;
+        resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
+        resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
+        __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
+        deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
+        c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
+        __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
+        __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
+        __m128 impulseMagnitude = deltaImpulse;
+        body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
+        body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
+        body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
+        body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+#else
+        resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
+#endif
+}
 …
+#if 0
 void    btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
+{
         btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
+        solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
+        solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
+        solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
+        solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
+        solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f);
+        solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
         if (rb)
+        {
                 solverBody->m_invMass = rb->getInvMass();
+                solverBody->internalGetInvMass() = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor();
                 solverBody->m_originalBody = rb;
                 solverBody->m_angularFactor = rb->getAngularFactor();
         } else
+        {
                 solverBody->m_invMass = 0.f;
+                solverBody->internalGetInvMass().setValue(0,0,0);
                 solverBody->m_originalBody = 0;
+                solverBody->m_angularFactor = 1.f;
+        }
+}
+int             gNumSplitImpulseRecoveries = 0;
+                solverBody->m_angularFactor.setValue(1,1,1);
+        }
+}
+#endif
 btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
 …
+btSolverConstraint&     btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation)
+void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
+{
 …
         btRigidBody* body1=btRigidBody::upcast(colObj1);
-        btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand();
-        memset(&solverConstraint,0xff,sizeof(btSolverConstraint));
         solverConstraint.m_contactNormal = normalAxis;
+        solverConstraint.m_solverBodyIdA = solverBodyIdA;
+        solverConstraint.m_solverBodyIdB = solverBodyIdB;
+        solverConstraint.m_frictionIndex = frictionIndex;
+        solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody();
+        solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody();
         solverConstraint.m_friction = cp.m_combinedFriction;
 …
         solverConstraint.m_appliedImpulse = 0.f;
         //      solverConstraint.m_appliedPushImpulse = 0.f;
+        solverConstraint.m_appliedPushImpulse = 0.f;
+        {
 …
                 rel_vel = vel1Dotn+vel2Dotn;
                 btScalar positionalError = 0.f;
                 btSimdScalar velocityError =  - rel_vel;
+//              btScalar positionalError = 0.f;
+                btSimdScalar velocityError =  desiredVelocity - rel_vel;
                 btSimdScalar    velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
                 solverConstraint.m_rhs = velocityImpulse;
                 solverConstraint.m_cfm = 0.f;
+                solverConstraint.m_cfm = cfmSlip;
                 solverConstraint.m_lowerLimit = 0;
                 solverConstraint.m_upperLimit = 1e10f;
+        }
+}
+btSolverConstraint&     btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
+{
+        btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
+        solverConstraint.m_frictionIndex = frictionIndex;
+        setupFrictionConstraint(solverConstraint, normalAxis, solverBodyA, solverBodyB, cp, rel_pos1, rel_pos2,
+                                                        colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
         return solverConstraint;
+}
 …
 int     btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
+{
+#if 0
         int solverBodyIdA = -1;
 …
+        }
         return solverBodyIdA;
+#endif
+        return 0;
+}
 #include <stdio.h>
+void    btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
+{
+        btCollisionObject* colObj0=0,*colObj1=0;
+        colObj0 = (btCollisionObject*)manifold->getBody0();
+        colObj1 = (btCollisionObject*)manifold->getBody1();
+        int solverBodyIdA=-1;
+        int solverBodyIdB=-1;
+        if (manifold->getNumContacts())
+        {
+                solverBodyIdA = getOrInitSolverBody(*colObj0);
+                solverBodyIdB = getOrInitSolverBody(*colObj1);
+        }
+        ///avoid collision response between two static objects
+        if (!solverBodyIdA && !solverBodyIdB)
+                return;
+        btVector3 rel_pos1;
+        btVector3 rel_pos2;
+        btScalar relaxation;
+        for (int j=0;j<manifold->getNumContacts();j++)
+        {
+                btManifoldPoint& cp = manifold->getContactPoint(j);
+                if (cp.getDistance() <= manifold->getContactProcessingThreshold())
+                {
+void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
+                                                                                                                                 btCollisionObject* colObj0, btCollisionObject* colObj1,
+                                                                                                                                 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
+                                                                                                                                 btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
+                                                                                                                                 btVector3& rel_pos1, btVector3& rel_pos2)
+{
+                        btRigidBody* rb0 = btRigidBody::upcast(colObj0);
+                        btRigidBody* rb1 = btRigidBody::upcast(colObj1);
                         const btVector3& pos1 = cp.getPositionWorldOnA();
                         const btVector3& pos2 = cp.getPositionWorldOnB();
+//                      btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
+//                      btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
                         rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
                         rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
                         relaxation = 1.f;
+                        btScalar rel_vel;
+                        btVector3 vel;
+                        int frictionIndex = m_tmpSolverContactConstraintPool.size();
+                        {
+                                btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand();
+                                btRigidBody* rb0 = btRigidBody::upcast(colObj0);
+                                btRigidBody* rb1 = btRigidBody::upcast(colObj1);
+                                solverConstraint.m_solverBodyIdA = solverBodyIdA;
+                                solverConstraint.m_solverBodyIdB = solverBodyIdB;
+                                solverConstraint.m_originalContactPoint = &cp;
+                                btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
+                                solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
+                                btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
+                                solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+                        btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
+                        solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
+                        btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
+                        solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+                                {
 #ifdef COMPUTE_IMPULSE_DENOM
 …
+                                btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
+                                btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
                                 vel  = vel1 - vel2;
                                 rel_vel = cp.m_normalWorldOnB.dot(vel);
+                        btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
+                        btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
+                        vel  = vel1 - vel2;
+                        rel_vel = cp.m_normalWorldOnB.dot(vel);
                                 btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
 …
                                         solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
                                         if (rb0)
                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
+                                                rb0->internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
                                         if (rb1)
                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
+                                                rb1->internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
                                 } else
+                                {
 …
+                                }
                                 //                                                      solverConstraint.m_appliedPushImpulse = 0.f;
+                                solverConstraint.m_appliedPushImpulse = 0.f;
+                                {
 …
                                         btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
                                         btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+                                        solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+                                        if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+                                        {
+                                                //combine position and velocity into rhs
+                                                solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+                                                solverConstraint.m_rhsPenetration = 0.f;
+                                        } else
+                                        {
+                                                //split position and velocity into rhs and m_rhsPenetration
+                                                solverConstraint.m_rhs = velocityImpulse;
+                                                solverConstraint.m_rhsPenetration = penetrationImpulse;
+                                        }
                                         solverConstraint.m_cfm = 0.f;
                                         solverConstraint.m_lowerLimit = 0;
 …
+                                /////setup the friction constraints
+                                if (1)
+                                {
+                                        solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
+                                        if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
+                                        {
+                                                cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
+                                                btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
+                                                if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
+                                                {
+                                                        cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
+                                                        applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
+                                                        applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
+                                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                                        if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+                                                        {
+                                                                cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
+                                                                cp.m_lateralFrictionDir2.normalize();//??
+                                                                applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
+                                                                applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
+                                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                                        }
+                                                        cp.m_lateralFrictionInitialized = true;
+                                                } else
+                                                {
+                                                        //re-calculate friction direction every frame, todo: check if this is really needed
+                                                        btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
+                                                        applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
+                                                        applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
+                                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                                        if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+                                                        {
+                                                                applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
+                                                                applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
+                                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                                        }
+                                                        cp.m_lateralFrictionInitialized = true;
+                                                }
+                                        } else
+                                        {
+                                                addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                                if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+                                                        addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                        }
+}
+void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
+                                                                                                                                                btRigidBody* rb0, btRigidBody* rb1,
+                                                                                                                                 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
+{
                                         if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
+                                        {
 …
                                                                 frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
                                                                 if (rb0)
                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
+                                                                        rb0->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
                                                                 if (rb1)
                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
+                                                                        rb1->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
                                                         } else
+                                                        {
 …
                                                                 frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
                                                                 if (rb0)
                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
+                                                                        rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
                                                                 if (rb1)
                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
+                                                                        rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
                                                         } else
+                                                        {
 …
+                                                }
+                                        }
+}
+void    btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
+{
+        btCollisionObject* colObj0=0,*colObj1=0;
+        colObj0 = (btCollisionObject*)manifold->getBody0();
+        colObj1 = (btCollisionObject*)manifold->getBody1();
+        btRigidBody* solverBodyA = btRigidBody::upcast(colObj0);
+        btRigidBody* solverBodyB = btRigidBody::upcast(colObj1);
+        ///avoid collision response between two static objects
+        if ((!solverBodyA || !solverBodyA->getInvMass()) && (!solverBodyB || !solverBodyB->getInvMass()))
+                return;
+        for (int j=0;j<manifold->getNumContacts();j++)
+        {
+                btManifoldPoint& cp = manifold->getContactPoint(j);
+                if (cp.getDistance() <= manifold->getContactProcessingThreshold())
+                {
+                        btVector3 rel_pos1;
+                        btVector3 rel_pos2;
+                        btScalar relaxation;
+                        btScalar rel_vel;
+                        btVector3 vel;
+                        int frictionIndex = m_tmpSolverContactConstraintPool.size();
+                        btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
+                        btRigidBody* rb0 = btRigidBody::upcast(colObj0);
+                        btRigidBody* rb1 = btRigidBody::upcast(colObj1);
+                        solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody();
+                        solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody();
+                        solverConstraint.m_originalContactPoint = &cp;
+                        setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
+//                      const btVector3& pos1 = cp.getPositionWorldOnA();
+//                      const btVector3& pos2 = cp.getPositionWorldOnB();
+                        /////setup the friction constraints
+                        solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
+                        if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
+                        {
+                                cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
+                                btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
+                                if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
+                                {
+                                        cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
+                                        if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+                                        {
+                                                cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
+                                                cp.m_lateralFrictionDir2.normalize();//??
+                                                applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
+                                                applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
+                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                        }
+                                        applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
+                                        applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
+                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                        cp.m_lateralFrictionInitialized = true;
+                                } else
+                                {
+                                        //re-calculate friction direction every frame, todo: check if this is really needed
+                                        btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
+                                        if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+                                        {
+                                                applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
+                                                applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
+                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                        }
+                                        applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
+                                        applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
+                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+                                        cp.m_lateralFrictionInitialized = true;
+                                }
+                        }
+                }
+        }
+}
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+                        } else
+                        {
+                                addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
+                                if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+                                        addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
+                        }
+                        setFrictionConstraintImpulse( solverConstraint, rb0, rb1, cp, infoGlobal);
+                }
+        }
+}
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+{
         BT_PROFILE("solveGroupCacheFriendlySetup");
 …
+        }
+        if (infoGlobal.m_splitImpulse)
+        {
+                for (int i = 0; i < numBodies; i++)
+                {
+                        btRigidBody* body = btRigidBody::upcast(bodies[i]);
+                        if (body)
+                        {
+                                body->internalGetDeltaLinearVelocity().setZero();
+                                body->internalGetDeltaAngularVelocity().setZero();
+                                body->internalGetPushVelocity().setZero();
+                                body->internalGetTurnVelocity().setZero();
+                        }
+                }
+        }
+        else
+        {
+                for (int i = 0; i < numBodies; i++)
+                {
+                        btRigidBody* body = btRigidBody::upcast(bodies[i]);
+                        if (body)
+                        {
+                                body->internalGetDeltaLinearVelocity().setZero();
+                                body->internalGetDeltaAngularVelocity().setZero();
+                        }
+                }
+        }
         if (1)
+        {
 …
+                }
+        }
-        btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
-        initSolverBody(&fixedBody,0);
         //btRigidBody* rb0=0,*rb1=0;
 …
                         int totalNumRows = 0;
                         int i;
+                        m_tmpConstraintSizesPool.resize(numConstraints);
                         //calculate the total number of contraint rows
                         for (i=0;i<numConstraints;i++)
+                        {
+                                btTypedConstraint::btConstraintInfo1 info1;
+                                btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
                                 constraints[i]->getInfo1(&info1);
                                 totalNumRows += info1.m_numConstraintRows;
 …
                         m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
+                        btTypedConstraint::btConstraintInfo1 info1;
+                        info1.m_numConstraintRows = 0;
                         ///setup the btSolverConstraints
                         int currentRow = 0;
                         for (i=0;i<numConstraints;i++,currentRow+=info1.m_numConstraintRows)
+                        for (i=0;i<numConstraints;i++)
+                        {
+                                constraints[i]->getInfo1(&info1);
+                                const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
                                 if (info1.m_numConstraintRows)
+                                {
 …
                                         btRigidBody& rbB = constraint->getRigidBodyB();
+                                        int solverBodyIdA = getOrInitSolverBody(rbA);
+                                        int solverBodyIdB = getOrInitSolverBody(rbB);
+                                        btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
+                                        btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
                                         int j;
                                         for ( j=0;j<info1.m_numConstraintRows;j++)
 …
                                                 currentConstraintRow[j].m_appliedImpulse = 0.f;
                                                 currentConstraintRow[j].m_appliedPushImpulse = 0.f;
                                                 currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
                                                 currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
+                                                currentConstraintRow[j].m_solverBodyA = &rbA;
+                                                currentConstraintRow[j].m_solverBodyB = &rbB;
+                                        }
                                         bodyAPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
                                         bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
                                         bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
                                         bodyBPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
+                                        rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
+                                        rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
+                                        rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
+                                        rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
 …
                                         btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
                                         info2.m_constraintError = &currentConstraintRow->m_rhs;
+                                        currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
+                                        info2.m_damping = infoGlobal.m_damping;
                                         info2.cfm = &currentConstraintRow->m_cfm;
                                         info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
                                         info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
+                                        info2.m_numIterations = infoGlobal.m_numIterations;
                                         constraints[i]->getInfo2(&info2);
 …
+                                        {
                                                 btSolverConstraint& solverConstraint = currentConstraintRow[j];
+                                                solverConstraint.m_originalContactPoint = constraint;
+                                                {
 …
                                                         btScalar restitution = 0.f;
                                                         btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
                                                         btScalar        velocityError = restitution - rel_vel;// * damping;
+                                                        btScalar        velocityError = restitution - rel_vel * info2.m_damping;
                                                         btScalar        penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
                                                         btScalar        velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
 …
+                                        }
+                                }
+                                currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
+                        }
+                }
 …
                         int i;
                         btPersistentManifold* manifold = 0;
                         btCollisionObject* colObj0=0,*colObj1=0;
+//                      btCollisionObject* colObj0=0,*colObj1=0;
 …
+}
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
+{
+        BT_PROFILE("solveGroupCacheFriendlyIterations");
+btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
+{
         int numConstraintPool = m_tmpSolverContactConstraintPool.size();
         int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
+        int j;
+        if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
+        {
+                if ((iteration & 7) == 0) {
+                        for (j=0; j<numConstraintPool; ++j) {
+                                int tmp = m_orderTmpConstraintPool[j];
+                                int swapi = btRandInt2(j+1);
+                                m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
+                                m_orderTmpConstraintPool[swapi] = tmp;
+                        }
+                        for (j=0; j<numFrictionPool; ++j) {
+                                int tmp = m_orderFrictionConstraintPool[j];
+                                int swapi = btRandInt2(j+1);
+                                m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
+                                m_orderFrictionConstraintPool[swapi] = tmp;
+                        }
+                }
+        }
+        if (infoGlobal.m_solverMode & SOLVER_SIMD)
+        {
+                ///solve all joint constraints, using SIMD, if available
+                for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+                {
+                        btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
+                        resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
+                }
+                for (j=0;j<numConstraints;j++)
+                {
+                        constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
+                }
+                ///solve all contact constraints using SIMD, if available
+                int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+                for (j=0;j<numPoolConstraints;j++)
+                {
+                        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+                        resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
+                }
+                ///solve all friction constraints, using SIMD, if available
+                int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+                for (j=0;j<numFrictionPoolConstraints;j++)
+                {
+                        btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+                        btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+                        if (totalImpulse>btScalar(0))
+                        {
+                                solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+                                solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+                                resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA,     *solveManifold.m_solverBodyB,solveManifold);
+                        }
+                }
+        } else
+        {
+                ///solve all joint constraints
+                for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+                {
+                        btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
+                        resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
+                }
+                for (j=0;j<numConstraints;j++)
+                {
+                        constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
+                }
+                ///solve all contact constraints
+                int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+                for (j=0;j<numPoolConstraints;j++)
+                {
+                        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+                        resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
+                }
+                ///solve all friction constraints
+                int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+                for (j=0;j<numFrictionPoolConstraints;j++)
+                {
+                        btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+                        btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+                        if (totalImpulse>btScalar(0))
+                        {
+                                solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+                                solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+                                resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
+                        }
+                }
+        }
+        return 0.f;
+}
+void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+{
+        int iteration;
+        if (infoGlobal.m_splitImpulse)
+        {
+                if (infoGlobal.m_solverMode & SOLVER_SIMD)
+                {
+                        for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
+                        {
+                                {
+                                        int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+                                        int j;
+                                        for (j=0;j<numPoolConstraints;j++)
+                                        {
+                                                const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+                                                resolveSplitPenetrationSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
+                                        }
+                                }
+                        }
+                }
+                else
+                {
+                        for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
+                        {
+                                {
+                                        int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+                                        int j;
+                                        for (j=0;j<numPoolConstraints;j++)
+                                        {
+                                                const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+                                                resolveSplitPenetrationImpulseCacheFriendly(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
+                                        }
+                                }
+                        }
+                }
+        }
+}
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+{
+        BT_PROFILE("solveGroupCacheFriendlyIterations");
         //should traverse the contacts random order...
         int iteration;
 …
                 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
+                {
+                        int j;
+                        if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
+                        {
+                                if ((iteration & 7) == 0) {
+                                        for (j=0; j<numConstraintPool; ++j) {
+                                                int tmp = m_orderTmpConstraintPool[j];
+                                                int swapi = btRandInt2(j+1);
+                                                m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
+                                                m_orderTmpConstraintPool[swapi] = tmp;
+                                        }
+                                        for (j=0; j<numFrictionPool; ++j) {
+                                                int tmp = m_orderFrictionConstraintPool[j];
+                                                int swapi = btRandInt2(j+1);
+                                                m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
+                                                m_orderFrictionConstraintPool[swapi] = tmp;
+                                        }
+                                }
+                        }
+                        if (infoGlobal.m_solverMode & SOLVER_SIMD)
+                        {
+                                ///solve all joint constraints, using SIMD, if available
+                                for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+                                {
+                                        btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
+                                        resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+                                }
+                                for (j=0;j<numConstraints;j++)
+                                {
+                                        int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
+                                        int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
+                                        btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
+                                        btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
+                                        constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
+                                }
+                                ///solve all contact constraints using SIMD, if available
+                                int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+                                for (j=0;j<numPoolConstraints;j++)
+                                {
+                                        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+                                        resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+                                }
+                                ///solve all friction constraints, using SIMD, if available
+                                int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+                                for (j=0;j<numFrictionPoolConstraints;j++)
+                                {
+                                        btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+                                        btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+                                        if (totalImpulse>btScalar(0))
+                                        {
+                                                solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+                                                solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+                                                resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],       m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+                                        }
+                                }
+                        } else
+                        {
+                                ///solve all joint constraints
+                                for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+                                {
+                                        btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
+                                        resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+                                }
+                                for (j=0;j<numConstraints;j++)
+                                {
+                                        int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
+                                        int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
+                                        btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
+                                        btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
+                                        constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
+                                }
+                                ///solve all contact constraints
+                                int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+                                for (j=0;j<numPoolConstraints;j++)
+                                {
+                                        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+                                        resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+                                }
+                                ///solve all friction constraints
+                                int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+                                for (j=0;j<numFrictionPoolConstraints;j++)
+                                {
+                                        btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+                                        btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+                                        if (totalImpulse>btScalar(0))
+                                        {
+                                                solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+                                                solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+                                                resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],                                                   m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+                                        }
+                                }
+                        }
+                }
+                        solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
+                }
+                solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
+        }
         return 0.f;
+}
+/// btSequentialImpulseConstraintSolver Sequentially applies impulses
+btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
+{
+        BT_PROFILE("solveGroup");
+        //we only implement SOLVER_CACHE_FRIENDLY now
+        //you need to provide at least some bodies
+        btAssert(bodies);
+        btAssert(numBodies);
+        int i;
+        solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
+        solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** /*constraints*/,int /* numConstraints*/,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
+{
         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
         int j;
+        int i,j;
         for (j=0;j<numPoolConstraints;j++)
 …
+        }
+        numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
+        for (j=0;j<numPoolConstraints;j++)
+        {
+                const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
+                btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
+                btScalar sum = constr->internalGetAppliedImpulse();
+                sum += solverConstr.m_appliedImpulse;
+                constr->internalSetAppliedImpulse(sum);
+        }
         if (infoGlobal.m_splitImpulse)
+        {
+                for ( i=0;i<m_tmpSolverBodyPool.size();i++)
+                {
+                        m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep);
+                for ( i=0;i<numBodies;i++)
+                {
+                        btRigidBody* body = btRigidBody::upcast(bodies[i]);
+                        if (body)
+                                body->internalWritebackVelocity(infoGlobal.m_timeStep);
+                }
         } else
+        {
+                for ( i=0;i<m_tmpSolverBodyPool.size();i++)
+                {
+                        m_tmpSolverBodyPool[i].writebackVelocity();
+                }
+        }
+        m_tmpSolverBodyPool.resize(0);
+                for ( i=0;i<numBodies;i++)
+                {
+                        btRigidBody* body = btRigidBody::upcast(bodies[i]);
+                        if (body)
+                                body->internalWritebackVelocity();
+                }
+        }
         m_tmpSolverContactConstraintPool.resize(0);
         m_tmpSolverNonContactConstraintPool.resize(0);
 …
+/// btSequentialImpulseConstraintSolver Sequentially applies impulses
+btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
+{
+        BT_PROFILE("solveGroup");
+        //you need to provide at least some bodies
+        btAssert(bodies);
+        btAssert(numBodies);
+        solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
+        solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
+        solveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
+        return 0.f;
+}
 void    btSequentialImpulseConstraintSolver::reset()
 …
+}
+btRigidBody& btSequentialImpulseConstraintSolver::getFixedBody()
+{
+        static btRigidBody s_fixed(0, 0,0);
+        s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+        return s_fixed;
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

-                      r5781
+                      r8284
 #include "btSolverBody.h"
 #include "btSolverConstraint.h"
+#include "btTypedConstraint.h"
+#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
 ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
 …
 protected:
-        btAlignedObjectArray<btSolverBody>      m_tmpSolverBodyPool;
         btConstraintArray                       m_tmpSolverContactConstraintPool;
         btConstraintArray                       m_tmpSolverNonContactConstraintPool;
 …
         btAlignedObjectArray<int>       m_orderTmpConstraintPool;
         btAlignedObjectArray<int>       m_orderFrictionConstraintPool;
+        btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
+        btSolverConstraint&     addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation);
+        void setupFrictionConstraint(   btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyIdB,
+                                                                        btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
+                                                                        btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
+                                                                        btScalar desiredVelocity=0., btScalar cfmSlip=0.);
+        btSolverConstraint&     addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
+        void setupContactConstraint(btSolverConstraint& solverConstraint, btCollisionObject* colObj0, btCollisionObject* colObj1, btManifoldPoint& cp,
+                                                                const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
+                                                                btVector3& rel_pos1, btVector3& rel_pos2);
+        void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, btRigidBody* rb0, btRigidBody* rb1,
+                                                                                 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
         ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
         unsigned long   m_btSeed2;
         void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
+//      void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
         btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
         void    convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
+        void    resolveSplitPenetrationSIMD(
+        btRigidBody& body1,
+        btRigidBody& body2,
+        const btSolverConstraint& contactConstraint);
         void    resolveSplitPenetrationImpulseCacheFriendly(
+        btSolverBody& body1,
+        btSolverBody& body2,
+        const btSolverConstraint& contactConstraint,
+        const btContactSolverInfo& solverInfo);
+        btRigidBody& body1,
+        btRigidBody& body2,
+        const btSolverConstraint& contactConstraint);
         //internal method
         int     getOrInitSolverBody(btCollisionObject& body);
         void    resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+        void    resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
         void    resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+        void    resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
         void    resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+        void    resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
         void    resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+        void    resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
+protected:
+        static btRigidBody& getFixedBody();
+        virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+        virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+        btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+        virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+        virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
 public:
 …
         virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
-        btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
-        btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
         ///clear internal cached data and reset random seed
         virtual void    reset();

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp

-                      r5781
+                      r8284
 */
+//-----------------------------------------------------------------------------
 #include "btSliderConstraint.h"
 …
 #include <new>
+//-----------------------------------------------------------------------------
+#define USE_OFFSET_FOR_CONSTANT_FRAME true
 void btSliderConstraint::initParams()
 …
         m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
         m_dampingDirLin = btScalar(0.);
+        m_cfmDirLin = SLIDER_CONSTRAINT_DEF_CFM;
         m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
         m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
         m_dampingDirAng = btScalar(0.);
+        m_cfmDirAng = SLIDER_CONSTRAINT_DEF_CFM;
         m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
         m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
         m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
+        m_cfmOrthoLin = SLIDER_CONSTRAINT_DEF_CFM;
         m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
         m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
         m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
+        m_cfmOrthoAng = SLIDER_CONSTRAINT_DEF_CFM;
         m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
         m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
         m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
+        m_cfmLimLin = SLIDER_CONSTRAINT_DEF_CFM;
         m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
         m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
         m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
+        m_cfmLimAng = SLIDER_CONSTRAINT_DEF_CFM;
         m_poweredLinMotor = false;
 …
         m_accumulatedAngMotorImpulse = btScalar(0.0);
+} // btSliderConstraint::initParams()
+//-----------------------------------------------------------------------------
+btSliderConstraint::btSliderConstraint()
+        :btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
+                m_useLinearReferenceFrameA(true),
+                m_useSolveConstraintObsolete(false)
+//              m_useSolveConstraintObsolete(true)
+        m_flags = 0;
+        m_flags = 0;
+        m_useOffsetForConstraintFrame = USE_OFFSET_FOR_CONSTANT_FRAME;
+        calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+}
+btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
+        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB),
+                m_useSolveConstraintObsolete(false),
+                m_frameInA(frameInA),
+        m_frameInB(frameInB),
+                m_useLinearReferenceFrameA(useLinearReferenceFrameA)
+{
         initParams();
+} // btSliderConstraint::btSliderConstraint()
+//-----------------------------------------------------------------------------
+btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
+        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB)
+        , m_frameInA(frameInA)
+        , m_frameInB(frameInB),
+                m_useLinearReferenceFrameA(useLinearReferenceFrameA),
+                m_useSolveConstraintObsolete(false)
+//              m_useSolveConstraintObsolete(true)
+{
+}
+btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA)
+        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB),
+                m_useSolveConstraintObsolete(false),
+                m_frameInB(frameInB),
+                m_useLinearReferenceFrameA(useLinearReferenceFrameA)
+{
+        ///not providing rigidbody A means implicitly using worldspace for body A
+        m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
+//      m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin());
         initParams();
 } // btSliderConstraint::btSliderConstraint()
+//-----------------------------------------------------------------------------
+void btSliderConstraint::buildJacobian()
+{
+        if (!m_useSolveConstraintObsolete)
+        {
+                return;
+        }
         if(m_useLinearReferenceFrameA)
+        {
                 buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB);
+}
+void btSliderConstraint::getInfo1(btConstraintInfo1* info)
+{
+        if (m_useSolveConstraintObsolete)
+        {
+                info->m_numConstraintRows = 0;
+                info->nub = 0;
+        }
         else
+        {
+                buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA);
+        }
+} // btSliderConstraint::buildJacobian()
+//-----------------------------------------------------------------------------
+void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
+{
+        //calculate transforms
+    m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA;
+    m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB;
+                info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
+                info->nub = 2;
+                //prepare constraint
+                calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+                testAngLimits();
+                testLinLimits();
+                if(getSolveLinLimit() || getPoweredLinMotor())
+                {
+                        info->m_numConstraintRows++; // limit 3rd linear as well
+                        info->nub--;
+                }
+                if(getSolveAngLimit() || getPoweredAngMotor())
+                {
+                        info->m_numConstraintRows++; // limit 3rd angular as well
+                        info->nub--;
+                }
+        }
+}
+void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
+{
+        info->m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used)
+        info->nub = 0;
+}
+void btSliderConstraint::getInfo2(btConstraintInfo2* info)
+{
+        getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
+}
+void btSliderConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
+{
+        if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
+        {
+                m_calculatedTransformA = transA * m_frameInA;
+                m_calculatedTransformB = transB * m_frameInB;
+        }
+        else
+        {
+                m_calculatedTransformA = transB * m_frameInB;
+                m_calculatedTransformB = transA * m_frameInA;
+        }
         m_realPivotAInW = m_calculatedTransformA.getOrigin();
         m_realPivotBInW = m_calculatedTransformB.getOrigin();
         m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
+        m_delta = m_realPivotBInW - m_realPivotAInW;
+        if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
+        {
+                m_delta = m_realPivotBInW - m_realPivotAInW;
+        }
+        else
+        {
+                m_delta = m_realPivotAInW - m_realPivotBInW;
+        }
         m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
-        m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition();
-        m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition();
     btVector3 normalWorld;
     int i;
 …
+    {
                 normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
-                new (&m_jacLin[i]) btJacobianEntry(
-                        rbA.getCenterOfMassTransform().getBasis().transpose(),
-                        rbB.getCenterOfMassTransform().getBasis().transpose(),
-                        m_relPosA,
-                        m_relPosB,
-                        normalWorld,
-                        rbA.getInvInertiaDiagLocal(),
-                        rbA.getInvMass(),
-                        rbB.getInvInertiaDiagLocal(),
-                        rbB.getInvMass()
-                        );
-                m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal();
                 m_depth[i] = m_delta.dot(normalWorld);
+    }
+        testLinLimits();
+    // angular part
+    for(i = 0; i < 3; i++)
+    {
+                normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
+                new (&m_jacAng[i])      btJacobianEntry(
+                        normalWorld,
+            rbA.getCenterOfMassTransform().getBasis().transpose(),
+            rbB.getCenterOfMassTransform().getBasis().transpose(),
+            rbA.getInvInertiaDiagLocal(),
+            rbB.getInvInertiaDiagLocal()
+                        );
+        }
+        testAngLimits();
+        btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0);
+        m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA));
+        // clear accumulator for motors
+        m_accumulatedLinMotorImpulse = btScalar(0.0);
+        m_accumulatedAngMotorImpulse = btScalar(0.0);
+} // btSliderConstraint::buildJacobianInt()
+//-----------------------------------------------------------------------------
+void btSliderConstraint::getInfo1(btConstraintInfo1* info)
+{
+        if (m_useSolveConstraintObsolete)
+        {
+                info->m_numConstraintRows = 0;
+                info->nub = 0;
+}
+void btSliderConstraint::testLinLimits(void)
+{
+        m_solveLinLim = false;
+        m_linPos = m_depth[0];
+        if(m_lowerLinLimit <= m_upperLinLimit)
+        {
+                if(m_depth[0] > m_upperLinLimit)
+                {
+                        m_depth[0] -= m_upperLinLimit;
+                        m_solveLinLim = true;
+                }
+                else if(m_depth[0] < m_lowerLinLimit)
+                {
+                        m_depth[0] -= m_lowerLinLimit;
+                        m_solveLinLim = true;
+                }
+                else
+                {
+                        m_depth[0] = btScalar(0.);
+                }
+        }
         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)
+{
+                m_depth[0] = btScalar(0.);
+        }
+}
+void btSliderConstraint::testAngLimits(void)
+{
+        m_angDepth = btScalar(0.);
+        m_solveAngLim = false;
+        if(m_lowerAngLimit <= m_upperAngLimit)
+        {
+                const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
+                const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
+                const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
+//              btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));
+                btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0));
+                rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit);
+                m_angPos = rot;
+                if(rot < m_lowerAngLimit)
+                {
+                        m_angDepth = rot - m_lowerAngLimit;
+                        m_solveAngLim = true;
+                }
+                else if(rot > m_upperAngLimit)
+                {
+                        m_angDepth = rot - m_upperAngLimit;
+                        m_solveAngLim = true;
+                }
+        }
+}
+btVector3 btSliderConstraint::getAncorInA(void)
+{
+        btVector3 ancorInA;
+        ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis;
+        ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA;
+        return ancorInA;
+}
+btVector3 btSliderConstraint::getAncorInB(void)
+{
+        btVector3 ancorInB;
+        ancorInB = m_frameInB.getOrigin();
+        return ancorInB;
+}
+void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass  )
+{
+        const btTransform& trA = getCalculatedTransformA();
+        const btTransform& trB = getCalculatedTransformB();
         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
+        // difference between frames in WCS
+        btVector3 ofs = trB.getOrigin() - trA.getOrigin();
+        // now get weight factors depending on masses
+        btScalar miA = rbAinvMass;
+        btScalar miB = rbBinvMass;
+        bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
+        btScalar miS = miA + miB;
+        btScalar factA, factB;
+        if(miS > btScalar(0.f))
+        {
+                factA = miB / miS;
+        }
+        else
+        {
+                factA = btScalar(0.5f);
+        }
+        factB = btScalar(1.0f) - factA;
+        btVector3 ax1, p, q;
+        btVector3 ax1A = trA.getBasis().getColumn(0);
+        btVector3 ax1B = trB.getBasis().getColumn(0);
+        if(m_useOffsetForConstraintFrame)
+        {
+                // get the desired direction of slider axis
+                // as weighted sum of X-orthos of frameA and frameB in WCS
+                ax1 = ax1A * factA + ax1B * factB;
+                ax1.normalize();
+                // construct two orthos to slider axis
+                btPlaneSpace1 (ax1, p, q);
+        }
+        else
+        { // old way - use frameA
+                ax1 = trA.getBasis().getColumn(0);
+                // get 2 orthos to slider axis (Y, Z)
+                p = trA.getBasis().getColumn(1);
+                q = trA.getBasis().getColumn(2);
+        }
+        // make rotations around these orthos equal
         // the slider axis should be the only unconstrained
         // rotational axis, the angular velocity of the two bodies perpendicular to
 …
         // 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];
 …
         // 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 ax1A,ax1B are the unit length slider axes as computed from bodyA and
+        // bodyB, 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 :
 …
         // 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);
+//      btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
+        btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp;
+        btScalar k = info->fps * currERP;
+        btVector3 u = ax1A.cross(ax1B);
         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
+        if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG)
+        {
+                info->cfm[0] = m_cfmOrthoAng;
+                info->cfm[s] = m_cfmOrthoAng;
+        }
+        int nrow = 1; // last filled row
         int srow;
+        // check linear limits linear
+        btScalar limit_err = btScalar(0.0);
+        int limit = 0;
+        btScalar limit_err;
+        int limit;
+        int powered;
+        // next two rows.
+        // we want: velA + wA x relA == velB + wB x relB ... but this would
+        // result in three equations, so we project along two orthos to the slider axis
+        btTransform bodyA_trans = transA;
+        btTransform bodyB_trans = transB;
+        nrow++;
+        int s2 = nrow * s;
+        nrow++;
+        int s3 = nrow * s;
+        btVector3 tmpA(0,0,0), tmpB(0,0,0), relA(0,0,0), relB(0,0,0), c(0,0,0);
+        if(m_useOffsetForConstraintFrame)
+        {
+                // get vector from bodyB to frameB in WCS
+                relB = trB.getOrigin() - bodyB_trans.getOrigin();
+                // get its projection to slider axis
+                btVector3 projB = ax1 * relB.dot(ax1);
+                // get vector directed from bodyB to slider axis (and orthogonal to it)
+                btVector3 orthoB = relB - projB;
+                // same for bodyA
+                relA = trA.getOrigin() - bodyA_trans.getOrigin();
+                btVector3 projA = ax1 * relA.dot(ax1);
+                btVector3 orthoA = relA - projA;
+                // get desired offset between frames A and B along slider axis
+                btScalar sliderOffs = m_linPos - m_depth[0];
+                // desired vector from projection of center of bodyA to projection of center of bodyB to slider axis
+                btVector3 totalDist = projA + ax1 * sliderOffs - projB;
+                // get offset vectors relA and relB
+                relA = orthoA + totalDist * factA;
+                relB = orthoB - totalDist * factB;
+                // now choose average ortho to slider axis
+                p = orthoB * factA + orthoA * factB;
+                btScalar len2 = p.length2();
+                if(len2 > SIMD_EPSILON)
+                {
+                        p /= btSqrt(len2);
+                }
+                else
+                {
+                        p = trA.getBasis().getColumn(1);
+                }
+                // make one more ortho
+                q = ax1.cross(p);
+                // fill two rows
+                tmpA = relA.cross(p);
+                tmpB = relB.cross(p);
+                for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
+                for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
+                tmpA = relA.cross(q);
+                tmpB = relB.cross(q);
+                if(hasStaticBody && getSolveAngLimit())
+                { // to make constraint between static and dynamic objects more rigid
+                        // remove wA (or wB) from equation if angular limit is hit
+                        tmpB *= factB;
+                        tmpA *= factA;
+                }
+                for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmpA[i];
+                for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[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];
+        }
+        else
+        {       // old way - maybe incorrect if bodies are not on the slider axis
+                // see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
+                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
+        //      k = info->fps * info->erp * getSoftnessOrthoLin();
+        currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp;
+        k = info->fps * currERP;
+        btScalar rhs = k * p.dot(ofs);
+        info->m_constraintError[s2] = rhs;
+        rhs = k * q.dot(ofs);
+        info->m_constraintError[s3] = rhs;
+        if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN)
+        {
+                info->cfm[s2] = m_cfmOrthoLin;
+                info->cfm[s3] = m_cfmOrthoLin;
+        }
+        // check linear limits
+        limit_err = btScalar(0.0);
+        limit = 0;
         if(getSolveLinLimit())
+        {
 …
                 limit = (limit_err > btScalar(0.0)) ? 2 : 1;
+        }
         int powered = 0;
+        powered = 0;
         if(getPoweredLinMotor())
+        {
 …
                 // 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];
+                if(m_useOffsetForConstraintFrame)
+                {
+                        // this is needed only when bodyA and bodyB are both dynamic.
+                        if(!hasStaticBody)
+                        {
+                                tmpA = relA.cross(ax1);
+                                tmpB = relB.cross(ax1);
+                                info->m_J1angularAxis[srow+0] = tmpA[0];
+                                info->m_J1angularAxis[srow+1] = tmpA[1];
+                                info->m_J1angularAxis[srow+2] = tmpA[2];
+                                info->m_J2angularAxis[srow+0] = -tmpB[0];
+                                info->m_J2angularAxis[srow+1] = -tmpB[1];
+                                info->m_J2angularAxis[srow+2] = -tmpB[2];
+                        }
+                }
+                else
+                { // The old way. May be incorrect if bodies are not on the slider axis
+                        btVector3 ltd;  // Linear Torque Decoupling vector (a torque)
+                        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();
 …
                 info->m_lowerLimit[srow] = 0.;
                 info->m_upperLimit[srow] = 0.;
+                currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp;
                 if(powered)
+                {
+            info->cfm[nrow] = btScalar(0.0);
+                        if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
+                        {
+                                info->cfm[srow] = m_cfmDirLin;
+                        }
                         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();
+                        btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP);
                         info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
                         info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
 …
                 if(limit)
+                {
                         k = info->fps * info->erp;
+                        k = info->fps * currERP;
                         info->m_constraintError[srow] += k * limit_err;
+                        info->cfm[srow] = btScalar(0.0); // stop_cfm;
+                        if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN)
+                        {
+                                info->cfm[srow] = m_cfmLimLin;
+                        }
                         if(lostop == histop)
                         {       // limited low and high simultaneously
 …
                         if(bounce > btScalar(0.0))
+                        {
                                 btScalar vel = m_rbA.getLinearVelocity().dot(ax1);
                                 vel -= m_rbB.getLinearVelocity().dot(ax1);
+                                btScalar vel = linVelA.dot(ax1);
+                                vel -= linVelB.dot(ax1);
                                 vel *= signFact;
                                 // only apply bounce if the velocity is incoming, and if the
 …
                         powered = 0;
+                }
+                currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp;
                 if(powered)
+                {
+            info->cfm[srow] = btScalar(0.0);
+                        btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp);
+                        if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG)
+                        {
+                                info->cfm[srow] = m_cfmDirAng;
+                        }
+                        btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP);
                         info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
                         info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
 …
                 if(limit)
+                {
                         k = info->fps * info->erp;
+                        k = info->fps * currERP;
                         info->m_constraintError[srow] += k * limit_err;
+                        info->cfm[srow] = btScalar(0.0); // stop_cfm;
+                        if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
+                        {
+                                info->cfm[srow] = m_cfmLimAng;
+                        }
                         if(lostop == histop)
+                        {
 …
                 } // 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);
+}
+///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 btSliderConstraint::setParam(int num, btScalar value, int axis)
+{
+        switch(num)
+        {
+        case BT_CONSTRAINT_STOP_ERP :
+                if(axis < 1)
+                {
+                        m_softnessLimLin = value;
+                        m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN;
+                }
+                else if(axis < 3)
+                {
+                        m_softnessOrthoLin = value;
+                        m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN;
+                }
+                else if(axis == 3)
+                {
+                        m_softnessLimAng = value;
+                        m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG;
+                }
+                else if(axis < 6)
+                {
+                        m_softnessOrthoAng = value;
+                        m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG;
+                }
                 else
+                {
+                        solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA);
+                }
+        }
+} // btSliderConstraint::solveConstraint()
+//-----------------------------------------------------------------------------
+void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
+{
+    int i;
+    // linear
+    btVector3 velA;
+        bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA);
+    btVector3 velB;
+        bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB);
+    btVector3 vel = velA - velB;
+        for(i = 0; i < 3; i++)
+    {
+                const btVector3& normal = m_jacLin[i].m_linearJointAxis;
+                btScalar rel_vel = normal.dot(vel);
+                // calculate positional error
+                btScalar depth = m_depth[i];
+                // get parameters
+                btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin);
+                btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin);
+                btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin);
+                // calcutate and apply impulse
+                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);
+                }
+                if(m_poweredLinMotor && (!i))
+                { // apply linear motor
+                        if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce)
+                        {
+                                btScalar desiredMotorVel = m_targetLinMotorVelocity;
+                                btScalar motor_relvel = desiredMotorVel + rel_vel;
+                                normalImpulse = -motor_relvel * m_jacLinDiagABInv[i];
+                                // clamp accumulated impulse
+                                btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse);
+                                if(new_acc  > m_maxLinMotorForce)
+                                {
+                                        new_acc = m_maxLinMotorForce;
+                                }
+                                btScalar del = new_acc  - m_accumulatedLinMotorImpulse;
+                                if(normalImpulse < btScalar(0.0))
+                                {
+                                        normalImpulse = -del;
+                                }
+                                else
+                                {
+                                        normalImpulse = del;
+                                }
+                                m_accumulatedLinMotorImpulse = new_acc;
+                                // 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);
+                                }
+                        }
+                }
+    }
+        // angular
+        // get axes in world space
+        btVector3 axisA =  m_calculatedTransformA.getBasis().getColumn(0);
+        btVector3 axisB =  m_calculatedTransformB.getBasis().getColumn(0);
+        btVector3 angVelA;
+        bodyA.getAngularVelocity(angVelA);
+        btVector3 angVelB;
+        bodyB.getAngularVelocity(angVelB);
+        btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
+        btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
+        btVector3 angAorthog = angVelA - angVelAroundAxisA;
+        btVector3 angBorthog = angVelB - angVelAroundAxisB;
+        btVector3 velrelOrthog = angAorthog-angBorthog;
+        //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;
+        }
+        //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))
+        {
+                btVector3 normal2 = angularError.normalized();
+                btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
+                angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
+                angularError *= angularImpulseMag;
+        }
+        // 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);
+        btScalar impulseMag;
+        //solve angular limits
+        if(m_solveAngLim)
+        {
+                impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep;
+                impulseMag *= m_kAngle * m_softnessLimAng;
+        }
+        else
+        {
+                impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep;
+                impulseMag *= m_kAngle * m_softnessDirAng;
+        }
+        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);
+        //apply angular motor
+        if(m_poweredAngMotor)
+        {
+                if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce)
+                {
+                        btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB;
+                        btScalar projRelVel = velrel.dot(axisA);
+                        btScalar desiredMotorVel = m_targetAngMotorVelocity;
+                        btScalar motor_relvel = desiredMotorVel - projRelVel;
+                        btScalar angImpulse = m_kAngle * motor_relvel;
+                        // clamp accumulated impulse
+                        btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse);
+                        if(new_acc  > m_maxAngMotorForce)
+                        {
+                                new_acc = m_maxAngMotorForce;
+                        }
+                        btScalar del = new_acc  - m_accumulatedAngMotorImpulse;
+                        if(angImpulse < btScalar(0.0))
+                        {
+                                angImpulse = -del;
+                        }
+                        else
+                        {
+                                angImpulse = del;
+                        }
+                        m_accumulatedAngMotorImpulse = new_acc;
+                        // 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);
+                }
+        }
+} // btSliderConstraint::solveConstraint()
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+void btSliderConstraint::calculateTransforms(void){
+        if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
+        {
+                m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
+                m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
+        }
+        else
+        {
+                m_calculatedTransformA = m_rbB.getCenterOfMassTransform() * m_frameInB;
+                m_calculatedTransformB = m_rbA.getCenterOfMassTransform() * m_frameInA;
+        }
+        m_realPivotAInW = m_calculatedTransformA.getOrigin();
+        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_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
+    btVector3 normalWorld;
+    int i;
+    //linear part
+    for(i = 0; i < 3; i++)
+    {
+                normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
+                m_depth[i] = m_delta.dot(normalWorld);
+    }
+} // btSliderConstraint::calculateTransforms()
+//-----------------------------------------------------------------------------
+void btSliderConstraint::testLinLimits(void)
+{
+        m_solveLinLim = false;
+        m_linPos = m_depth[0];
+        if(m_lowerLinLimit <= m_upperLinLimit)
+        {
+                if(m_depth[0] > m_upperLinLimit)
+                {
+                        m_depth[0] -= m_upperLinLimit;
+                        m_solveLinLim = true;
+                }
+                else if(m_depth[0] < m_lowerLinLimit)
+                {
+                        m_depth[0] -= m_lowerLinLimit;
+                        m_solveLinLim = true;
+                        btAssertConstrParams(0);
+                }
+                break;
+        case BT_CONSTRAINT_CFM :
+                if(axis < 1)
+                {
+                        m_cfmDirLin = value;
+                        m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN;
+                }
+                else if(axis == 3)
+                {
+                        m_cfmDirAng = value;
+                        m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG;
+                }
                 else
+                {
+                        m_depth[0] = btScalar(0.);
+                }
+        }
+        else
+        {
+                m_depth[0] = btScalar(0.);
+        }
+} // btSliderConstraint::testLinLimits()
+//-----------------------------------------------------------------------------
+void btSliderConstraint::testAngLimits(void)
+{
+        m_angDepth = btScalar(0.);
+        m_solveAngLim = false;
+        if(m_lowerAngLimit <= m_upperAngLimit)
+        {
+                const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
+                const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
+                const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
+                btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));
+                m_angPos = rot;
+                if(rot < m_lowerAngLimit)
+                {
+                        m_angDepth = rot - m_lowerAngLimit;
+                        m_solveAngLim = true;
+                }
+                else if(rot > m_upperAngLimit)
+                {
+                        m_angDepth = rot - m_upperAngLimit;
+                        m_solveAngLim = true;
+                }
+        }
+} // btSliderConstraint::testAngLimits()
+//-----------------------------------------------------------------------------
+btVector3 btSliderConstraint::getAncorInA(void)
+{
+        btVector3 ancorInA;
+        ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis;
+        ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA;
+        return ancorInA;
+} // btSliderConstraint::getAncorInA()
+//-----------------------------------------------------------------------------
+btVector3 btSliderConstraint::getAncorInB(void)
+{
+        btVector3 ancorInB;
+        ancorInB = m_frameInB.getOrigin();
+        return ancorInB;
+} // btSliderConstraint::getAncorInB();
+                        btAssertConstrParams(0);
+                }
+                break;
+        case BT_CONSTRAINT_STOP_CFM :
+                if(axis < 1)
+                {
+                        m_cfmLimLin = value;
+                        m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN;
+                }
+                else if(axis < 3)
+                {
+                        m_cfmOrthoLin = value;
+                        m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN;
+                }
+                else if(axis == 3)
+                {
+                        m_cfmLimAng = value;
+                        m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG;
+                }
+                else if(axis < 6)
+                {
+                        m_cfmOrthoAng = value;
+                        m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG;
+                }
+                else
+                {
+                        btAssertConstrParams(0);
+                }
+                break;
+        }
+}
+///return the local value of parameter
+btScalar btSliderConstraint::getParam(int num, int axis) const
+{
+        btScalar retVal(SIMD_INFINITY);
+        switch(num)
+        {
+        case BT_CONSTRAINT_STOP_ERP :
+                if(axis < 1)
+                {
+                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN);
+                        retVal = m_softnessLimLin;
+                }
+                else if(axis < 3)
+                {
+                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN);
+                        retVal = m_softnessOrthoLin;
+                }
+                else if(axis == 3)
+                {
+                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG);
+                        retVal = m_softnessLimAng;
+                }
+                else if(axis < 6)
+                {
+                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG);
+                        retVal = m_softnessOrthoAng;
+                }
+                else
+                {
+                        btAssertConstrParams(0);
+                }
+                break;
+        case BT_CONSTRAINT_CFM :
+                if(axis < 1)
+                {
+                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN);
+                        retVal = m_cfmDirLin;
+                }
+                else if(axis == 3)
+                {
+                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG);
+                        retVal = m_cfmDirAng;
+                }
+                else
+                {
+                        btAssertConstrParams(0);
+                }
+                break;
+        case BT_CONSTRAINT_STOP_CFM :
+                if(axis < 1)
+                {
+                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN);
+                        retVal = m_cfmLimLin;
+                }
+                else if(axis < 3)
+                {
+                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN);
+                        retVal = m_cfmOrthoLin;
+                }
+                else if(axis == 3)
+                {
+                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG);
+                        retVal = m_cfmLimAng;
+                }
+                else if(axis < 6)
+                {
+                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG);
+                        retVal = m_cfmOrthoAng;
+                }
+                else
+                {
+                        btAssertConstrParams(0);
+                }
+                break;
+        }
+        return retVal;
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h

-                      r5781
+                      r8284
 #define SLIDER_CONSTRAINT_H
+//-----------------------------------------------------------------------------
 #include "LinearMath/btVector3.h"
 …
 #include "btTypedConstraint.h"
+//-----------------------------------------------------------------------------
 class btRigidBody;
+//-----------------------------------------------------------------------------
 #define SLIDER_CONSTRAINT_DEF_SOFTNESS          (btScalar(1.0))
 #define SLIDER_CONSTRAINT_DEF_DAMPING           (btScalar(1.0))
 #define SLIDER_CONSTRAINT_DEF_RESTITUTION       (btScalar(0.7))
+//-----------------------------------------------------------------------------
+#define SLIDER_CONSTRAINT_DEF_CFM                       (btScalar(0.f))
+enum btSliderFlags
+{
+        BT_SLIDER_FLAGS_CFM_DIRLIN = (1 << 0),
+        BT_SLIDER_FLAGS_ERP_DIRLIN = (1 << 1),
+        BT_SLIDER_FLAGS_CFM_DIRANG = (1 << 2),
+        BT_SLIDER_FLAGS_ERP_DIRANG = (1 << 3),
+        BT_SLIDER_FLAGS_CFM_ORTLIN = (1 << 4),
+        BT_SLIDER_FLAGS_ERP_ORTLIN = (1 << 5),
+        BT_SLIDER_FLAGS_CFM_ORTANG = (1 << 6),
+        BT_SLIDER_FLAGS_ERP_ORTANG = (1 << 7),
+        BT_SLIDER_FLAGS_CFM_LIMLIN = (1 << 8),
+        BT_SLIDER_FLAGS_ERP_LIMLIN = (1 << 9),
+        BT_SLIDER_FLAGS_CFM_LIMANG = (1 << 10),
+        BT_SLIDER_FLAGS_ERP_LIMANG = (1 << 11)
+};
 class btSliderConstraint : public btTypedConstraint
 …
         ///for backwards compatibility during the transition to 'getInfo/getInfo2'
         bool            m_useSolveConstraintObsolete;
+        bool            m_useOffsetForConstraintFrame;
         btTransform     m_frameInA;
     btTransform m_frameInB;
 …
         btScalar m_restitutionDirLin;
         btScalar m_dampingDirLin;
+        btScalar m_cfmDirLin;
         btScalar m_softnessDirAng;
         btScalar m_restitutionDirAng;
         btScalar m_dampingDirAng;
+        btScalar m_cfmDirAng;
         btScalar m_softnessLimLin;
         btScalar m_restitutionLimLin;
         btScalar m_dampingLimLin;
+        btScalar m_cfmLimLin;
         btScalar m_softnessLimAng;
         btScalar m_restitutionLimAng;
         btScalar m_dampingLimAng;
+        btScalar m_cfmLimAng;
         btScalar m_softnessOrthoLin;
         btScalar m_restitutionOrthoLin;
         btScalar m_dampingOrthoLin;
+        btScalar m_cfmOrthoLin;
         btScalar m_softnessOrthoAng;
         btScalar m_restitutionOrthoAng;
         btScalar m_dampingOrthoAng;
+        btScalar m_cfmOrthoAng;
         // for interlal use
         bool m_solveLinLim;
         bool m_solveAngLim;
+        int m_flags;
         btJacobianEntry m_jacLin[3];
 …
         // constructors
     btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
+    btSliderConstraint();
+    btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA);
         // overrides
+    virtual void        buildJacobian();
     virtual void getInfo1 (btConstraintInfo1* info);
+        void getInfo1NonVirtual(btConstraintInfo1* info);
         virtual void getInfo2 (btConstraintInfo2* info);
     virtual     void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep);
+        void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass);
         // access
 …
     void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
     btScalar getLowerAngLimit() { return m_lowerAngLimit; }
     void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = lowerLimit; }
+    void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); }
     btScalar getUpperAngLimit() { return m_upperAngLimit; }
     void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = upperLimit; }
+    void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); }
         bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; }
         btScalar getSoftnessDirLin() { return m_softnessDirLin; }
 …
         bool getSolveAngLimit() { return m_solveAngLim; }
         btScalar getAngDepth() { return m_angDepth; }
-        // internal
-    void        buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
-    void        solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB);
         // shared code used by ODE solver
+        void    calculateTransforms(void);
+        void    testLinLimits(void);
+        void    testLinLimits2(btConstraintInfo2* info);
+        void    testAngLimits(void);
+        void    calculateTransforms(const btTransform& transA,const btTransform& transB);
+        void    testLinLimits();
+        void    testAngLimits();
         // access for PE Solver
+        btVector3 getAncorInA(void);
+        btVector3 getAncorInB(void);
+        btVector3 getAncorInA();
+        btVector3 getAncorInB();
+        // access for UseFrameOffset
+        bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
+        void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
+        ///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.
+        virtual void    setParam(int num, btScalar value, int axis = -1);
+        ///return the local value of parameter
+        virtual btScalar getParam(int num, int axis = -1) const;
+        virtual int     calculateSerializeBufferSize() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 };
+//-----------------------------------------------------------------------------
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btSliderConstraintData
+{
+        btTypedConstraintData   m_typeConstraintData;
+        btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+        btTransformFloatData m_rbBFrame;
+        float   m_linearUpperLimit;
+        float   m_linearLowerLimit;
+        float   m_angularUpperLimit;
+        float   m_angularLowerLimit;
+        int     m_useLinearReferenceFrameA;
+        int m_useOffsetForConstraintFrame;
+};
+SIMD_FORCE_INLINE               int     btSliderConstraint::calculateSerializeBufferSize() const
+{
+        return sizeof(btSliderConstraintData);
+}
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE       const char*     btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btSliderConstraintData* sliderData = (btSliderConstraintData*) dataBuffer;
+        btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer);
+        m_frameInA.serializeFloat(sliderData->m_rbAFrame);
+        m_frameInB.serializeFloat(sliderData->m_rbBFrame);
+        sliderData->m_linearUpperLimit = float(m_upperLinLimit);
+        sliderData->m_linearLowerLimit = float(m_lowerLinLimit);
+        sliderData->m_angularUpperLimit = float(m_upperAngLimit);
+        sliderData->m_angularLowerLimit = float(m_lowerAngLimit);
+        sliderData->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA;
+        sliderData->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame;
+        return "btSliderConstraintData";
+}
 #endif //SLIDER_CONSTRAINT_H

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h

-                      r5781
+                      r8284
 ///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
 ATTRIBUTE_ALIGNED16 (struct)    btSolverBody
+ATTRIBUTE_ALIGNED64 (struct)    btSolverBodyObsolete
+{
         BT_DECLARE_ALIGNED_ALLOCATOR();
         btVector3               m_deltaLinearVelocity;
         btVector3               m_deltaAngularVelocity;
+        btScalar                m_angularFactor;
+        btScalar                m_invMass;
+        btScalar                m_friction;
+        btVector3               m_angularFactor;
+        btVector3               m_invMass;
         btRigidBody*    m_originalBody;
         btVector3               m_pushVelocity;
         //btVector3             m_turnVelocity;
+        btVector3               m_turnVelocity;
 …
+        }
+/*
+        SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+        {
+                if (m_originalBody)
+                {
+                        m_pushVelocity += linearComponent*impulseMagnitude;
+                        m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+                }
+        }
         void    writebackVelocity()
+        {
                 if (m_invMass)
+                if (m_originalBody)
+                {
                         m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
 …
+                }
+        }
-        */
+        void    writebackVelocity(btScalar timeStep=0)
+        void    writebackVelocity(btScalar timeStep)
+        {
+                if (m_invMass)
+        (void) timeStep;
+                if (m_originalBody)
+                {
                         m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
+                        m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
                         m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
+                        //correct the position/orientation based on push/turn recovery
+                        btTransform newTransform;
+                        btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
+                        m_originalBody->setWorldTransform(newTransform);
                         //m_originalBody->setCompanionId(-1);
+                }

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h

-                      r5781
+                      r8284
 ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
 ATTRIBUTE_ALIGNED16 (struct)    btSolverConstraint
+ATTRIBUTE_ALIGNED64 (struct)    btSolverConstraint
+{
         BT_DECLARE_ALIGNED_ALLOCATOR();
 …
         union
+        {
                 int                     m_solverBodyIdA;
                 btScalar        m_unusedPadding2;
+                btRigidBody*    m_solverBodyA;
+                int                             m_companionIdA;
         };
         union
+        {
                 int                     m_solverBodyIdB;
                 btScalar        m_unusedPadding3;
+                btRigidBody*    m_solverBodyB;
+                int                             m_companionIdB;
         };
 …
         btScalar                m_lowerLimit;
         btScalar                m_upperLimit;
+        btScalar                m_rhsPenetration;
         enum            btSolverConstraintType

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp

-                      r5781
+                      r8284
 #include "btTypedConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btSerializer.h"
-static btRigidBody s_fixed(0, 0,0);
 #define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
+:m_userConstraintType(-1),
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
+:btTypedObject(type),
+m_userConstraintType(-1),
 m_userConstraintId(-1),
 m_constraintType (type),
 m_rbA(s_fixed),
 m_rbB(s_fixed),
+m_needsFeedback(false),
+m_rbA(rbA),
+m_rbB(getFixedBody()),
 m_appliedImpulse(btScalar(0.)),
 m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
+{
-        s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+}
-btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
-:m_userConstraintType(-1),
-m_userConstraintId(-1),
-m_constraintType (type),
-m_rbA(rbA),
-m_rbB(s_fixed),
-m_appliedImpulse(btScalar(0.)),
-m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
+{
-                s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+}
 btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
+:m_userConstraintType(-1),
+:btTypedObject(type),
+m_userConstraintType(-1),
 m_userConstraintId(-1),
 m_constraintType (type),
+m_needsFeedback(false),
 m_rbA(rbA),
 m_rbB(rbB),
 …
 m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
+{
-                s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+}
+//-----------------------------------------------------------------------------
 btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
 …
+        }
         return lim_fact;
 } // btTypedConstraint::getMotorFactor()
+}
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char*     btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+        btTypedConstraintData* tcd = (btTypedConstraintData*) dataBuffer;
+        tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA);
+        tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB);
+        char* name = (char*) serializer->findNameForPointer(this);
+        tcd->m_name = (char*)serializer->getUniquePointer(name);
+        if (tcd->m_name)
+        {
+                serializer->serializeName(name);
+        }
+        tcd->m_objectType = m_objectType;
+        tcd->m_needsFeedback = m_needsFeedback;
+        tcd->m_userConstraintId =m_userConstraintId;
+        tcd->m_userConstraintType =m_userConstraintType;
+        tcd->m_appliedImpulse = float(m_appliedImpulse);
+        tcd->m_dbgDrawSize = float(m_dbgDrawSize );
+        tcd->m_disableCollisionsBetweenLinkedBodies = false;
+        int i;
+        for (i=0;i<m_rbA.getNumConstraintRefs();i++)
+                if (m_rbA.getConstraintRef(i) == this)
+                        tcd->m_disableCollisionsBetweenLinkedBodies = true;
+        for (i=0;i<m_rbB.getNumConstraintRefs();i++)
+                if (m_rbB.getConstraintRef(i) == this)
+                        tcd->m_disableCollisionsBetweenLinkedBodies = true;
+        return "btTypedConstraintData";
+}
+btRigidBody& btTypedConstraint::getFixedBody()
+{
+        static btRigidBody s_fixed(0, 0,0);
+        s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+        return s_fixed;
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2010 Erwin Coumans  http://continuousphysics.com/Bullet/
 This software is provided 'as-is', without any express or implied warranty.
 …
 #include "LinearMath/btScalar.h"
 #include "btSolverConstraint.h"
+struct  btSolverBody;
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btSerializer;
 enum btTypedConstraintType
+{
         POINT2POINT_CONSTRAINT_TYPE,
+        POINT2POINT_CONSTRAINT_TYPE=MAX_CONTACT_MANIFOLD_TYPE+1,
         HINGE_CONSTRAINT_TYPE,
         CONETWIST_CONSTRAINT_TYPE,
         D6_CONSTRAINT_TYPE,
+        SLIDER_CONSTRAINT_TYPE
+        SLIDER_CONSTRAINT_TYPE,
+        CONTACT_CONSTRAINT_TYPE
 };
+enum btConstraintParams
+{
+        BT_CONSTRAINT_ERP=1,
+        BT_CONSTRAINT_STOP_ERP,
+        BT_CONSTRAINT_CFM,
+        BT_CONSTRAINT_STOP_CFM
+};
+#if 1
+        #define btAssertConstrParams(_par) btAssert(_par)
+#else
+        #define btAssertConstrParams(_par)
+#endif
 ///TypedConstraint is the baseclass for Bullet constraints and vehicles
 class btTypedConstraint
+class btTypedConstraint : public btTypedObject
+{
         int     m_userConstraintType;
+        int     m_userConstraintId;
+        btTypedConstraintType m_constraintType;
+        union
+        {
+                int     m_userConstraintId;
+                void* m_userConstraintPtr;
+        };
+        bool m_needsFeedback;
         btTypedConstraint&      operator=(btTypedConstraint&    other)
 …
         btScalar        m_dbgDrawSize;
+        ///internal method used by the constraint solver, don't use them directly
+        btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
+        static btRigidBody& getFixedBody();
 public:
-        btTypedConstraint(btTypedConstraintType type);
         virtual ~btTypedConstraint() {};
         btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
 …
                 // the constraint.
                 int *findex;
+                // number of solver iterations
+                int m_numIterations;
+                //damping of the velocity
+                btScalar        m_damping;
         };
+        virtual void    buildJacobian() = 0;
+        ///internal method used by the constraint solver, don't use them directly
+        virtual void    buildJacobian() {};
+        ///internal method used by the constraint solver, don't use them directly
         virtual void    setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
+        {
+        }
+        (void)ca;
+        (void)solverBodyA;
+        (void)solverBodyB;
+        (void)timeStep;
+        }
+        ///internal method used by the constraint solver, don't use them directly
         virtual void getInfo1 (btConstraintInfo1* info)=0;
+        ///internal method used by the constraint solver, don't use them directly
         virtual void getInfo2 (btConstraintInfo2* info)=0;
+        virtual void    solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar        timeStep) = 0;
+        btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
+        ///internal method used by the constraint solver, don't use them directly
+        void    internalSetAppliedImpulse(btScalar appliedImpulse)
+        {
+                m_appliedImpulse = appliedImpulse;
+        }
+        ///internal method used by the constraint solver, don't use them directly
+        btScalar        internalGetAppliedImpulse()
+        {
+                return m_appliedImpulse;
+        }
+        ///internal method used by the constraint solver, don't use them directly
+        virtual void    solveConstraintObsolete(btRigidBody& /*bodyA*/,btRigidBody& /*bodyB*/,btScalar  /*timeStep*/) {};
         const btRigidBody& getRigidBodyA() const
 …
+        }
+        void    setUserConstraintPtr(void* ptr)
+        {
+                m_userConstraintPtr = ptr;
+        }
+        void*   getUserConstraintPtr()
+        {
+                return m_userConstraintPtr;
+        }
         int getUid() const
+        {
 …
+        }
+        bool    needsFeedback() const
+        {
+                return m_needsFeedback;
+        }
+        ///enableFeedback will allow to read the applied linear and angular impulse
+        ///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information
+        void    enableFeedback(bool needsFeedback)
+        {
+                m_needsFeedback = needsFeedback;
+        }
+        ///getAppliedImpulse is an estimated total applied impulse.
+        ///This feedback could be used to determine breaking constraints or playing sounds.
         btScalar        getAppliedImpulse() const
+        {
+                btAssert(m_needsFeedback);
                 return m_appliedImpulse;
+        }
 …
         btTypedConstraintType getConstraintType () const
+        {
                 return m_constraintType;
+                return btTypedConstraintType(m_objectType);
+        }
 …
                 return m_dbgDrawSize;
+        }
+        ///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.
+        virtual void    setParam(int num, btScalar value, int axis = -1) = 0;
+        ///return the local value of parameter
+        virtual btScalar getParam(int num, int axis = -1) const = 0;
+        virtual int     calculateSerializeBufferSize() const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, btSerializer* serializer) const;
 };
+// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits
+// all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI])
+SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
+{
+        if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
+        {
+                return angleInRadians;
+        }
+        else if(angleInRadians < angleLowerLimitInRadians)
+        {
+                btScalar diffLo = btFabs(btNormalizeAngle(angleLowerLimitInRadians - angleInRadians));
+                btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians));
+                return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
+        }
+        else if(angleInRadians > angleUpperLimitInRadians)
+        {
+                btScalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians));
+                btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians));
+                return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians;
+        }
+        else
+        {
+                return angleInRadians;
+        }
+}
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btTypedConstraintData
+{
+        btRigidBodyData         *m_rbA;
+        btRigidBodyData         *m_rbB;
+        char    *m_name;
+        int     m_objectType;
+        int     m_userConstraintType;
+        int     m_userConstraintId;
+        int     m_needsFeedback;
+        float   m_appliedImpulse;
+        float   m_dbgDrawSize;
+        int     m_disableCollisionsBetweenLinkedBodies;
+        char    m_pad4[4];
+};
+SIMD_FORCE_INLINE       int     btTypedConstraint::calculateSerializeBufferSize() const
+{
+        return sizeof(btTypedConstraintData);
+}
 #endif //TYPED_CONSTRAINT_H

code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/Bullet-C-API.cpp

-                      r5781
+                      r8284
 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
+#include "LinearMath/btStackAlloc.h"
 /*
 …
+{
         //capsule is convex hull of 2 spheres, so use btMultiSphereShape
         btVector3 inertiaHalfExtents(radius,height,radius);
         const int numSpheres = 2;
         btVector3 positions[numSpheres] = {btVector3(0,height,0),btVector3(0,-height,0)};
         btScalar radi[numSpheres] = {radius,radius};
         void* mem = btAlignedAlloc(sizeof(btMultiSphereShape),16);
         return (plCollisionShapeHandle) new (mem)btMultiSphereShape(inertiaHalfExtents,positions,radi,numSpheres);
+        return (plCollisionShapeHandle) new (mem)btMultiSphereShape(positions,radi,numSpheres);
+}
 plCollisionShapeHandle plNewConeShape(plReal radius, plReal height)
 …
         worldTrans.setRotation(orn);
         body->setWorldTransform(worldTrans);
+}
+void    plSetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix)
+{
+        btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+        btAssert(body);
+        btTransform& worldTrans = body->getWorldTransform();
+        worldTrans.setFromOpenGLMatrix(matrix);
+}
 …
         btGjkPairDetector::ClosestPointInput input;
+        btStackAlloc gStackAlloc(1024*1024*2);
+        input.m_stackAlloc = &gStackAlloc;
         btTransform tr;
         tr.setIdentity();

code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btActionInterface.h

-                      r5781
+                      r8284
 #include "LinearMath/btScalar.h"
+#include "btRigidBody.h"
 ///Basic interface to allow actions such as vehicles and characters to be updated inside a btDynamicsWorld
 class btActionInterface
+{
+        public:
+protected:
+        static btRigidBody& getFixedBody();
+public:
         virtual ~btActionInterface()
 …
 #endif //_BT_ACTION_INTERFACE_H

code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp

-                      r5781
+                      r8284
         startProfiling(timeStep);
+        if(0 != m_internalPreTickCallback) {
+                (*m_internalPreTickCallback)(this, timeStep);
+        }
         ///update aabbs information

code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
 …
 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
+//for debug rendering
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/CollisionShapes/btConeShape.h"
+#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btCylinderShape.h"
+#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
+#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
+#include "LinearMath/btIDebugDraw.h"
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
-#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
-#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
-#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
-#include "LinearMath/btIDebugDraw.h"
 …
 #include "LinearMath/btMotionState.h"
+#include "LinearMath/btSerializer.h"
 …
 m_constraintSolver(constraintSolver),
 m_gravity(0,-10,0),
+m_localTime(btScalar(1.)/btScalar(60.)),
+m_localTime(0),
+m_synchronizeAllMotionStates(false),
 m_profileTimings(0)
+{
 …
 void    btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
+{
+///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows
+///to switch status _after_ adding kinematic objects to the world
+///fix it for Bullet 3.x release
         for (int i=0;i<m_collisionObjects.size();i++)
+        {
                 btCollisionObject* colObj = m_collisionObjects[i];
                 btRigidBody* body = btRigidBody::upcast(colObj);
+                if (body)
+                {
+                                if (body->getActivationState() != ISLAND_SLEEPING)
+                                {
+                                        if (body->isKinematicObject())
+                                        {
+                                                //to calculate velocities next frame
+                                                body->saveKinematicState(timeStep);
+                                        }
+                                }
+                }
+        }
+                if (body && body->getActivationState() != ISLAND_SLEEPING)
+                {
+                        if (body->isKinematicObject())
+                        {
+                                //to calculate velocities next frame
+                                body->saveKinematicState(timeStep);
+                        }
+                }
+        }
+}
 …
         BT_PROFILE("debugDrawWorld");
+        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
+        {
+                int numManifolds = getDispatcher()->getNumManifolds();
+                btVector3 color(0,0,0);
+                for (int i=0;i<numManifolds;i++)
+                {
+                        btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
+                        //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
+                        //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
+                        int numContacts = contactManifold->getNumContacts();
+                        for (int j=0;j<numContacts;j++)
+                        {
+                                btManifoldPoint& cp = contactManifold->getContactPoint(j);
+                                getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
+                        }
+                }
+        }
+        btCollisionWorld::debugDrawWorld();
         bool drawConstraints = false;
         if (getDebugDrawer())
 …
                 int i;
-                for (  i=0;i<m_collisionObjects.size();i++)
+                {
-                        btCollisionObject* colObj = m_collisionObjects[i];
-                        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
+                        {
-                                btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.));
-                                switch(colObj->getActivationState())
+                                {
-                                case  ACTIVE_TAG:
-                                        color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break;
-                                case ISLAND_SLEEPING:
-                                        color =  btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break;
-                                case WANTS_DEACTIVATION:
-                                        color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break;
-                                case DISABLE_DEACTIVATION:
-                                        color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break;
-                                case DISABLE_SIMULATION:
-                                        color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break;
-                                default:
+                                        {
-                                                color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));
+                                        }
-                                };
-                                debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
+                        }
-                        if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+                        {
-                                btVector3 minAabb,maxAabb;
-                                btVector3 colorvec(1,0,0);
-                                colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
-                                m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
+                        }
+                }
                 if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
+                {
 …
+{
         ///@todo: iterate over awake simulation islands!
+        for ( int i=0;i<m_collisionObjects.size();i++)
+        {
+                btCollisionObject* colObj = m_collisionObjects[i];
+                btRigidBody* body = btRigidBody::upcast(colObj);
+                if (body)
+                {
+                        body->clearForces();
+                }
+        for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+        {
+                btRigidBody* body = m_nonStaticRigidBodies[i];
+                //need to check if next line is ok
+                //it might break backward compatibility (people applying forces on sleeping objects get never cleared and accumulate on wake-up
+                body->clearForces();
+        }
+}
 …
+{
         ///@todo: iterate over awake simulation islands!
+        for ( int i=0;i<m_collisionObjects.size();i++)
+        {
+                btCollisionObject* colObj = m_collisionObjects[i];
+                btRigidBody* body = btRigidBody::upcast(colObj);
+                if (body && body->isActive())
+        for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+        {
+                btRigidBody* body = m_nonStaticRigidBodies[i];
+                if (body->isActive())
+                {
                         body->applyGravity();
 …
+{
         BT_PROFILE("synchronizeMotionStates");
+        {
+                //todo: iterate over awake simulation islands!
+        if (m_synchronizeAllMotionStates)
+        {
+                //iterate  over all collision objects
                 for ( int i=0;i<m_collisionObjects.size();i++)
+                {
                         btCollisionObject* colObj = m_collisionObjects[i];
                         btRigidBody* body = btRigidBody::upcast(colObj);
                         if (body)
                                 synchronizeSingleMotionState(body);
+                }
+        }
+/*
+        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
+        {
+                for ( int i=0;i<this->m_vehicles.size();i++)
+                {
+                        for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
+                        {
+                                //synchronize the wheels with the (interpolated) chassis worldtransform
+                                m_vehicles[i]->updateWheelTransform(v,true);
+                        }
+                }
+        }
+        */
+        } else
+        {
+                //iterate over all active rigid bodies
+                for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+                {
+                        btRigidBody* body = m_nonStaticRigidBodies[i];
+                        if (body->isActive())
+                                synchronizeSingleMotionState(body);
+                }
+        }
+}
 …
+        {
-                saveKinematicState(fixedTimeStep);
-                applyGravity();
                 //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
                 int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
+                saveKinematicState(fixedTimeStep*clampedSimulationSteps);
+                applyGravity();
                 for (int i=0;i<clampedSimulationSteps;i++)
+                {
 …
+                }
+        }
+        synchronizeMotionStates();
+        } else
+        {
+                synchronizeMotionStates();
+        }
         clearForces();
 …
         BT_PROFILE("internalSingleStepSimulation");
+        if(0 != m_internalPreTickCallback) {
+                (*m_internalPreTickCallback)(this, timeStep);
+        }
         ///apply gravity, predict motion
         predictUnconstraintMotion(timeStep);
 …
+{
         m_gravity = gravity;
+        for ( int i=0;i<m_collisionObjects.size();i++)
+        {
+                btCollisionObject* colObj = m_collisionObjects[i];
+                btRigidBody* body = btRigidBody::upcast(colObj);
+                if (body)
+        for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+        {
+                btRigidBody* body = m_nonStaticRigidBodies[i];
+                if (body->isActive() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
+                {
                         body->setGravity(gravity);
 …
+}
+void    btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
+{
+        btCollisionWorld::addCollisionObject(collisionObject,collisionFilterGroup,collisionFilterMask);
+}
+void    btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+{
+        btRigidBody* body = btRigidBody::upcast(collisionObject);
+        if (body)
+                removeRigidBody(body);
+        else
+                btCollisionWorld::removeCollisionObject(collisionObject);
+}
 void    btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
+{
+        removeCollisionObject(body);
+}
+        m_nonStaticRigidBodies.remove(body);
+        btCollisionWorld::removeCollisionObject(body);
+}
 void    btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
+{
         if (!body->isStaticOrKinematicObject())
+        if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
+        {
                 body->setGravity(m_gravity);
 …
         if (body->getCollisionShape())
+        {
+                if (!body->isStaticObject())
+                {
+                        m_nonStaticRigidBodies.push_back(body);
+                } else
+                {
+                        body->setActivationState(ISLAND_SLEEPING);
+                }
                 bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
                 short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
 …
 void    btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
+{
         if (!body->isStaticOrKinematicObject())
+        if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
+        {
                 body->setGravity(m_gravity);
 …
         if (body->getCollisionShape())
+        {
+                if (!body->isStaticObject())
+                {
+                        m_nonStaticRigidBodies.push_back(body);
+                }
+                 else
+                {
+                        body->setActivationState(ISLAND_SLEEPING);
+                }
                 addCollisionObject(body,group,mask);
+        }
 …
         BT_PROFILE("updateActivationState");
+        for ( int i=0;i<m_collisionObjects.size();i++)
+        {
+                btCollisionObject* colObj = m_collisionObjects[i];
+                btRigidBody* body = btRigidBody::upcast(colObj);
+        for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+        {
+                btRigidBody* body = m_nonStaticRigidBodies[i];
                 if (body)
+                {
 …
+                }
 };
 …
                 btStackAlloc*                   m_stackAlloc;
                 btDispatcher*                   m_dispatcher;
+                btAlignedObjectArray<btCollisionObject*> m_bodies;
+                btAlignedObjectArray<btPersistentManifold*> m_manifolds;
+                btAlignedObjectArray<btTypedConstraint*> m_constraints;
                 InplaceSolverIslandCallback(
 …
+                }
                 InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
+                {
 …
+                                }
+                                ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
+                                if (numManifolds + numCurConstraints)
+                                {
+                                        m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+                                }
+                        }
+                                if (m_solverInfo.m_minimumSolverBatchSize<=1)
+                                {
+                                        ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
+                                        if (numManifolds + numCurConstraints)
+                                        {
+                                                m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+                                        }
+                                } else
+                                {
+                                        for (i=0;i<numBodies;i++)
+                                                m_bodies.push_back(bodies[i]);
+                                        for (i=0;i<numManifolds;i++)
+                                                m_manifolds.push_back(manifolds[i]);
+                                        for (i=0;i<numCurConstraints;i++)
+                                                m_constraints.push_back(startConstraint[i]);
+                                        if ((m_constraints.size()+m_manifolds.size())>m_solverInfo.m_minimumSolverBatchSize)
+                                        {
+                                                processConstraints();
+                                        } else
+                                        {
+                                                //printf("deferred\n");
+                                        }
+                                }
+                        }
+                }
+                void    processConstraints()
+                {
+                        if (m_manifolds.size() + m_constraints.size()>0)
+                        {
+                                m_solver->solveGroup( &m_bodies[0],m_bodies.size(), &m_manifolds[0], m_manifolds.size(), &m_constraints[0], m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+                        }
+                        m_bodies.resize(0);
+                        m_manifolds.resize(0);
+                        m_constraints.resize(0);
+                }
         };
         //sorted version of all btTypedConstraint, based on islandId
 …
         m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback);
+        solverCallback.processConstraints();
         m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
+}
 …
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
 class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
 …
         btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
           btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
+                m_me(me),
                 m_allowedPenetration(0.0f),
-                m_me(me),
                 m_pairCache(pairCache),
                 m_dispatcher(dispatcher)
 …
         BT_PROFILE("integrateTransforms");
         btTransform predictedTrans;
+        for ( int i=0;i<m_collisionObjects.size();i++)
+        {
+                btCollisionObject* colObj = m_collisionObjects[i];
+                btRigidBody* body = btRigidBody::upcast(colObj);
+                if (body)
+                {
+                        body->setHitFraction(1.f);
+                        if (body->isActive() && (!body->isStaticOrKinematicObject()))
+                        {
+                                body->predictIntegratedTransform(timeStep, predictedTrans);
+                                btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
+                                if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
+                                {
+                                        BT_PROFILE("CCD motion clamping");
+                                        if (body->getCollisionShape()->isConvex())
+                                        {
+                                                gNumClampedCcdMotions++;
+                                                btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
+                                                btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
+                                                btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
+                                                sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
+                                                sweepResults.m_collisionFilterMask  = body->getBroadphaseProxy()->m_collisionFilterMask;
+                                                convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults);
+                                                if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
+                                                {
+                                                        body->setHitFraction(sweepResults.m_closestHitFraction);
+                                                        body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
+                                                        body->setHitFraction(0.f);
+        for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+        {
+                btRigidBody* body = m_nonStaticRigidBodies[i];
+                body->setHitFraction(1.f);
+                if (body->isActive() && (!body->isStaticOrKinematicObject()))
+                {
+                        body->predictIntegratedTransform(timeStep, predictedTrans);
+                        btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
+                        if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
+                        {
+                                BT_PROFILE("CCD motion clamping");
+                                if (body->getCollisionShape()->isConvex())
+                                {
+                                        gNumClampedCcdMotions++;
+                                        btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
+                                        //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
+                                        btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
+                                        sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
+                                        sweepResults.m_collisionFilterMask  = body->getBroadphaseProxy()->m_collisionFilterMask;
+                                        convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults);
+                                        if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
+                                        {
+                                                body->setHitFraction(sweepResults.m_closestHitFraction);
+                                                body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
+                                                body->setHitFraction(0.f);
 //                                                      printf("clamped integration to hit fraction = %f\n",fraction);
+                                                }
+                                        }
+                                }
                                 body->proceedToTransform( predictedTrans);
+                        }
+                        }
+                        body->proceedToTransform( predictedTrans);
+                }
+        }
 …
+{
         BT_PROFILE("predictUnconstraintMotion");
+        for ( int i=0;i<m_collisionObjects.size();i++)
+        {
+                btCollisionObject* colObj = m_collisionObjects[i];
+                btRigidBody* body = btRigidBody::upcast(colObj);
+                if (body)
+                {
+                        if (!body->isStaticOrKinematicObject())
+                        {
+                                body->integrateVelocities( timeStep);
+                                //damping
+                                body->applyDamping(timeStep);
+                                body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
+                        }
+        for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+        {
+                btRigidBody* body = m_nonStaticRigidBodies[i];
+                if (!body->isStaticOrKinematicObject())
+                {
+                        body->integrateVelocities( timeStep);
+                        //damping
+                        body->applyDamping(timeStep);
+                        body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
+                }
+        }
 …
-class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
+{
-        btIDebugDraw*   m_debugDrawer;
-        btVector3       m_color;
-        btTransform     m_worldTrans;
-public:
-        DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
-                m_debugDrawer(debugDrawer),
-                m_color(color),
-                m_worldTrans(worldTrans)
+        {
+        }
-        virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
+        {
-                processTriangle(triangle,partId,triangleIndex);
+        }
-        virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
+        {
-                (void)partId;
-                (void)triangleIndex;
-                btVector3 wv0,wv1,wv2;
-                wv0 = m_worldTrans*triangle[0];
-                wv1 = m_worldTrans*triangle[1];
-                wv2 = m_worldTrans*triangle[2];
-                m_debugDrawer->drawLine(wv0,wv1,m_color);
-                m_debugDrawer->drawLine(wv1,wv2,m_color);
-                m_debugDrawer->drawLine(wv2,wv0,m_color);
+        }
-};
-void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
+{
-        btVector3 start = transform.getOrigin();
-        const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
-        const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
-        const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
-        // XY
-        getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color);
-        getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color);
-        getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color);
-        getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color);
-        // XZ
-        getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color);
-        getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color);
-        getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color);
-        getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color);
-        // YZ
-        getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color);
-        getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color);
-        getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color);
-        getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color);
+}
-void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
+{
-        // Draw a small simplex at the center of the object
+        {
-                btVector3 start = worldTransform.getOrigin();
-                getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0));
-                getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0));
-                getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1));
+        }
-        if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
+        {
-                const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
-                for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
+                {
-                        btTransform childTrans = compoundShape->getChildTransform(i);
-                        const btCollisionShape* colShape = compoundShape->getChildShape(i);
-                        debugDrawObject(worldTransform*childTrans,colShape,color);
+                }
-        } else
+        {
-                switch (shape->getShapeType())
+                {
-                case SPHERE_SHAPE_PROXYTYPE:
+                        {
-                                const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
-                                btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
-                                debugDrawSphere(radius, worldTransform, color);
-                                break;
+                        }
-                case MULTI_SPHERE_SHAPE_PROXYTYPE:
+                        {
-                                const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
-                                for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
+                                {
-                                        btTransform childTransform = worldTransform;
-                                        childTransform.getOrigin() += multiSphereShape->getSpherePosition(i);
-                                        debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color);
+                                }
-                                break;
+                        }
-                case CAPSULE_SHAPE_PROXYTYPE:
+                        {
-                                const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
-                                btScalar radius = capsuleShape->getRadius();
-                                btScalar halfHeight = capsuleShape->getHalfHeight();
-                                int upAxis = capsuleShape->getUpAxis();
-                                btVector3 capStart(0.f,0.f,0.f);
-                                capStart[upAxis] = -halfHeight;
-                                btVector3 capEnd(0.f,0.f,0.f);
-                                capEnd[upAxis] = halfHeight;
-                                // Draw the ends
+                                {
-                                        btTransform childTransform = worldTransform;
-                                        childTransform.getOrigin() = worldTransform * capStart;
-                                        debugDrawSphere(radius, childTransform, color);
+                                }
+                                {
-                                        btTransform childTransform = worldTransform;
-                                        childTransform.getOrigin() = worldTransform * capEnd;
-                                        debugDrawSphere(radius, childTransform, color);
+                                }
-                                // Draw some additional lines
-                                btVector3 start = worldTransform.getOrigin();
-                                capStart[(upAxis+1)%3] = radius;
-                                capEnd[(upAxis+1)%3] = radius;
-                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
-                                capStart[(upAxis+1)%3] = -radius;
-                                capEnd[(upAxis+1)%3] = -radius;
-                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
-                                capStart[(upAxis+1)%3] = 0.f;
-                                capEnd[(upAxis+1)%3] = 0.f;
-                                capStart[(upAxis+2)%3] = radius;
-                                capEnd[(upAxis+2)%3] = radius;
-                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
-                                capStart[(upAxis+2)%3] = -radius;
-                                capEnd[(upAxis+2)%3] = -radius;
-                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
-                                break;
+                        }
-                case CONE_SHAPE_PROXYTYPE:
+                        {
-                                const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
-                                btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
-                                btScalar height = coneShape->getHeight();//+coneShape->getMargin();
-                                btVector3 start = worldTransform.getOrigin();
-                                int upAxis= coneShape->getConeUpIndex();
-                                btVector3       offsetHeight(0,0,0);
-                                offsetHeight[upAxis] = height * btScalar(0.5);
-                                btVector3       offsetRadius(0,0,0);
-                                offsetRadius[(upAxis+1)%3] = radius;
-                                btVector3       offset2Radius(0,0,0);
-                                offset2Radius[(upAxis+2)%3] = radius;
-                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
-                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
-                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
-                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
-                                break;
+                        }
-                case CYLINDER_SHAPE_PROXYTYPE:
+                        {
-                                const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
-                                int upAxis = cylinder->getUpAxis();
-                                btScalar radius = cylinder->getRadius();
-                                btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
-                                btVector3 start = worldTransform.getOrigin();
-                                btVector3       offsetHeight(0,0,0);
-                                offsetHeight[upAxis] = halfHeight;
-                                btVector3       offsetRadius(0,0,0);
-                                offsetRadius[(upAxis+1)%3] = radius;
-                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
-                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
-                                break;
+                        }
-                        case STATIC_PLANE_PROXYTYPE:
+                                {
-                                        const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
-                                        btScalar planeConst = staticPlaneShape->getPlaneConstant();
-                                        const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
-                                        btVector3 planeOrigin = planeNormal * planeConst;
-                                        btVector3 vec0,vec1;
-                                        btPlaneSpace1(planeNormal,vec0,vec1);
-                                        btScalar vecLen = 100.f;
-                                        btVector3 pt0 = planeOrigin + vec0*vecLen;
-                                        btVector3 pt1 = planeOrigin - vec0*vecLen;
-                                        btVector3 pt2 = planeOrigin + vec1*vecLen;
-                                        btVector3 pt3 = planeOrigin - vec1*vecLen;
-                                        getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color);
-                                        getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color);
-                                        break;
+                                }
-                default:
+                        {
-                                if (shape->isConcave())
+                                {
-                                        btConcaveShape* concaveMesh = (btConcaveShape*) shape;
-                                        ///@todo pass camera, for some culling? no -> we are not a graphics lib
-                                        btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
-                                        btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
-                                        DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
-                                        concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
+                                }
-                                if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
+                                {
-                                        btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
-                                        //todo: pass camera for some culling
-                                        btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
-                                        btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
-                                        //DebugDrawcallback drawCallback;
-                                        DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
-                                        convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
+                                }
-                                /// for polyhedral shapes
-                                if (shape->isPolyhedral())
+                                {
-                                        btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
-                                        int i;
-                                        for (i=0;i<polyshape->getNumEdges();i++)
+                                        {
-                                                btVector3 a,b;
-                                                polyshape->getEdge(i,a,b);
-                                                btVector3 wa = worldTransform * a;
-                                                btVector3 wb = worldTransform * b;
-                                                getDebugDrawer()->drawLine(wa,wb,color);
+                                        }
+                                }
+                        }
+                }
+        }
+}
 void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 …
                                 if(drawLimits)
+                                {
                                         btTransform tr = pSlider->getCalculatedTransformA();
+                                        btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB();
                                         btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
                                         btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
 …
+        }
         return;
 } // btDiscreteDynamicsWorld::debugDrawConstraint()
+}
 …
+void    btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer)
+{
+        int i;
+        //serialize all collision objects
+        for (i=0;i<m_collisionObjects.size();i++)
+        {
+                btCollisionObject* colObj = m_collisionObjects[i];
+                if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY)
+                {
+                        int len = colObj->calculateSerializeBufferSize();
+                        btChunk* chunk = serializer->allocate(len,1);
+                        const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
+                        serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,colObj);
+                }
+        }
+        for (i=0;i<m_constraints.size();i++)
+        {
+                btTypedConstraint* constraint = m_constraints[i];
+                int size = constraint->calculateSerializeBufferSize();
+                btChunk* chunk = serializer->allocate(size,1);
+                const char* structType = constraint->serialize(chunk->m_oldPtr,serializer);
+                serializer->finalizeChunk(chunk,structType,BT_CONSTRAINT_CODE,constraint);
+        }
+}
+void    btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
+{
+        serializer->startSerialization();
+        serializeRigidBodies(serializer);
+        serializeCollisionObjects(serializer);
+        serializer->finishSerialization();
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

-                      r5781
+                      r8284
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 …
 . This notice may not be removed or altered from any source distribution.
 */
 #ifndef BT_DISCRETE_DYNAMICS_WORLD_H
 …
         btAlignedObjectArray<btTypedConstraint*> m_constraints;
+        btAlignedObjectArray<btRigidBody*> m_nonStaticRigidBodies;
         btVector3       m_gravity;
 …
         bool    m_ownsIslandManager;
         bool    m_ownsConstraintSolver;
+        bool    m_synchronizeAllMotionStates;
         btAlignedObjectArray<btActionInterface*>        m_actions;
 …
         virtual void    saveKinematicState(btScalar timeStep);
+        void    debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color);
+        void    serializeRigidBodies(btSerializer* serializer);
 public:
 …
         virtual btVector3 getGravity () const;
+        virtual void    addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::StaticFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
         virtual void    addRigidBody(btRigidBody* body);
 …
         virtual void    removeRigidBody(btRigidBody* body);
+        void    debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
+        ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
+        virtual void    removeCollisionObject(btCollisionObject* collisionObject);
         void    debugDrawConstraint(btTypedConstraint* constraint);
 …
         virtual void    removeCharacter(btActionInterface* character);
+        void    setSynchronizeAllMotionStates(bool synchronizeAll)
+        {
+                m_synchronizeAllMotionStates = synchronizeAll;
+        }
+        bool getSynchronizeAllMotionStates() const
+        {
+                return m_synchronizeAllMotionStates;
+        }
+        ///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see Bullet/Demos/SerializeDemo)
+        virtual void    serialize(btSerializer* serializer);
 };

code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h

-                      r5781
+                      r8284
 protected:
                 btInternalTickCallback m_internalTickCallback;
+                btInternalTickCallback m_internalPreTickCallback;
                 void*   m_worldUserInfo;
 …
                 btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration)
                 :btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0), m_worldUserInfo(0)
+                :btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0),m_internalPreTickCallback(0), m_worldUserInfo(0)
+                {
+                }
 …
                 /// Set the callback for when an internal tick (simulation substep) happens, optional user info
                 void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo=0)
+                void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo=0,bool isPreTick=false)
+                {
+                        m_internalTickCallback = cb;
+                        if (isPreTick)
+                        {
+                                m_internalPreTickCallback = cb;
+                        } else
+                        {
+                                m_internalTickCallback = cb;
+                        }
                         m_worldUserInfo = worldUserInfo;
+                }

code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btRigidBody.cpp

-                      r5781
+                      r8284
 #include "LinearMath/btMotionState.h"
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "LinearMath/btSerializer.h"
 //'temporarily' global variables
 …
         m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
         m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+        m_angularFactor = btScalar(1.);
+        m_angularFactor.setValue(1,1,1);
+        m_linearFactor.setValue(1,1,1);
         m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
         m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 …
         updateInertiaTensor();
+        m_rigidbodyFlags = 0;
+        m_deltaLinearVelocity.setZero();
+        m_deltaAngularVelocity.setZero();
+        m_invMass = m_inverseMass*m_linearFactor;
+        m_pushVelocity.setZero();
+        m_turnVelocity.setZero();
+}
 …
 void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
+{
         m_linearDamping = GEN_clamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
         m_angularDamping = GEN_clamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+        m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+        m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+}
 …
                 m_inverseMass = btScalar(1.0) / mass;
+        }
+        //Fg = m * a
+        m_gravity = mass * m_gravity_acceleration;
         m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0),
 …
                                    inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
+        m_invMass = m_linearFactor*m_inverseMass;
+}
 …
+}
+void    btRigidBody::internalWritebackVelocity(btScalar timeStep)
+{
+    (void) timeStep;
+        if (m_inverseMass)
+        {
+                setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity);
+                setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity);
+                //correct the position/orientation based on push/turn recovery
+                btTransform newTransform;
+                btTransformUtil::integrateTransform(getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
+                setWorldTransform(newTransform);
+                //m_originalBody->setCompanionId(-1);
+        }
+//      m_deltaLinearVelocity.setZero();
+//      m_deltaAngularVelocity .setZero();
+//      m_pushVelocity.setZero();
+//      m_turnVelocity.setZero();
+}
 void btRigidBody::addConstraintRef(btTypedConstraint* c)
+{
 …
         m_checkCollideWith = m_constraintRefs.size() > 0;
+}
+int     btRigidBody::calculateSerializeBufferSize()     const
+{
+        int sz = sizeof(btRigidBodyData);
+        return sz;
+}
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char*     btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const
+{
+        btRigidBodyData* rbd = (btRigidBodyData*) dataBuffer;
+        btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer);
+        m_invInertiaTensorWorld.serialize(rbd->m_invInertiaTensorWorld);
+        m_linearVelocity.serialize(rbd->m_linearVelocity);
+        m_angularVelocity.serialize(rbd->m_angularVelocity);
+        rbd->m_inverseMass = m_inverseMass;
+        m_angularFactor.serialize(rbd->m_angularFactor);
+        m_linearFactor.serialize(rbd->m_linearFactor);
+        m_gravity.serialize(rbd->m_gravity);
+        m_gravity_acceleration.serialize(rbd->m_gravity_acceleration);
+        m_invInertiaLocal.serialize(rbd->m_invInertiaLocal);
+        m_totalForce.serialize(rbd->m_totalForce);
+        m_totalTorque.serialize(rbd->m_totalTorque);
+        rbd->m_linearDamping = m_linearDamping;
+        rbd->m_angularDamping = m_angularDamping;
+        rbd->m_additionalDamping = m_additionalDamping;
+        rbd->m_additionalDampingFactor = m_additionalDampingFactor;
+        rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr;
+        rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr;
+        rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor;
+        rbd->m_linearSleepingThreshold=m_linearSleepingThreshold;
+        rbd->m_angularSleepingThreshold = m_angularSleepingThreshold;
+        return btRigidBodyDataName;
+}
+void btRigidBody::serializeSingleObject(class btSerializer* serializer) const
+{
+        btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1);
+        const char* structType = serialize(chunk->m_oldPtr, serializer);
+        serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this);
+}

code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btRigidBody.h

-                      r5781
+                      r8284
 extern btScalar gDeactivationTime;
 extern bool gDisableDeactivation;
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btRigidBodyData btRigidBodyDoubleData
+#define btRigidBodyDataName     "btRigidBodyDoubleData"
+#else
+#define btRigidBodyData btRigidBodyFloatData
+#define btRigidBodyDataName     "btRigidBodyFloatData"
+#endif //BT_USE_DOUBLE_PRECISION
+enum    btRigidBodyFlags
+{
+        BT_DISABLE_WORLD_GRAVITY = 1
+};
 …
         btVector3               m_angularVelocity;
         btScalar                m_inverseMass;
         btScalar                m_angularFactor;
+        btVector3               m_linearFactor;
         btVector3               m_gravity;
 …
         //keep track of typed constraints referencing this rigid body
         btAlignedObjectArray<btTypedConstraint*> m_constraintRefs;
+        int                             m_rigidbodyFlags;
+        int                             m_debugBodyId;
+protected:
+        ATTRIBUTE_ALIGNED64(btVector3           m_deltaLinearVelocity);
+        btVector3               m_deltaAngularVelocity;
+        btVector3               m_angularFactor;
+        btVector3               m_invMass;
+        btVector3               m_pushVelocity;
+        btVector3               m_turnVelocity;
 public:
 …
                 btScalar                        m_additionalAngularDampingFactor;
                 btRigidBodyConstructionInfo(    btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)):
                 m_mass(mass),
 …
         static const btRigidBody*       upcast(const btCollisionObject* colObj)
+        {
                 if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
+                if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
                         return (const btRigidBody*)colObj;
                 return 0;
 …
         static btRigidBody*     upcast(btCollisionObject* colObj)
+        {
                 if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
+                if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
                         return (btRigidBody*)colObj;
                 return 0;
 …
         void                    setMassProps(btScalar mass, const btVector3& inertia);
+        const btVector3& getLinearFactor() const
+        {
+                return m_linearFactor;
+        }
+        void setLinearFactor(const btVector3& linearFactor)
+        {
+                m_linearFactor = linearFactor;
+                m_invMass = m_linearFactor*m_inverseMass;
+        }
         btScalar                getInvMass() const { return m_inverseMass; }
         const btMatrix3x3& getInvInertiaTensorWorld() const {
 …
         void                    applyCentralForce(const btVector3& force)
+        {
                 m_totalForce += force;
+                m_totalForce += force*m_linearFactor;
+        }
 …
         void    applyTorque(const btVector3& torque)
+        {
                 m_totalTorque += torque;
+                m_totalTorque += torque*m_angularFactor;
+        }
 …
+        {
                 applyCentralForce(force);
                 applyTorque(rel_pos.cross(force)*m_angularFactor);
+                applyTorque(rel_pos.cross(force*m_linearFactor));
+        }
         void applyCentralImpulse(const btVector3& impulse)
+        {
                 m_linearVelocity += impulse * m_inverseMass;
+                m_linearVelocity += impulse *m_linearFactor * m_inverseMass;
+        }
         void applyTorqueImpulse(const btVector3& torque)
+        {
                         m_angularVelocity += m_invInertiaTensorWorld * torque;
+                        m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
+        }
 …
                         if (m_angularFactor)
+                        {
                                 applyTorqueImpulse(rel_pos.cross(impulse)*m_angularFactor);
+                                applyTorqueImpulse(rel_pos.cross(impulse*m_linearFactor));
+                        }
+                }
+        }
-        //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-        SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+        {
-                if (m_inverseMass != btScalar(0.))
+                {
-                        m_linearVelocity += linearComponent*impulseMagnitude;
-                        if (m_angularFactor)
+                        {
-                                m_angularVelocity += angularComponent*impulseMagnitude*m_angularFactor;
+                        }
+                }
+        }
         void clearForces()
+        {
 …
         int     m_frictionSolverType;
+        void    setAngularFactor(const btVector3& angFac)
+        {
+                m_angularFactor = angFac;
+        }
         void    setAngularFactor(btScalar angFac)
+        {
                 m_angularFactor = angFac;
+        }
         btScalar        getAngularFactor() const
+                m_angularFactor.setValue(angFac,angFac,angFac);
+        }
+        const btVector3&        getAngularFactor() const
+        {
                 return m_angularFactor;
 …
+        }
+        int     m_debugBodyId;
+        void    setFlags(int flags)
+        {
+                m_rigidbodyFlags = flags;
+        }
+        int getFlags() const
+        {
+                return m_rigidbodyFlags;
+        }
+        const btVector3& getDeltaLinearVelocity() const
+        {
+                return m_deltaLinearVelocity;
+        }
+        const btVector3& getDeltaAngularVelocity() const
+        {
+                return m_deltaAngularVelocity;
+        }
+        const btVector3& getPushVelocity() const
+        {
+                return m_pushVelocity;
+        }
+        const btVector3& getTurnVelocity() const
+        {
+                return m_turnVelocity;
+        }
+        ////////////////////////////////////////////////
+        ///some internal methods, don't use them
+        btVector3& internalGetDeltaLinearVelocity()
+        {
+                return m_deltaLinearVelocity;
+        }
+        btVector3& internalGetDeltaAngularVelocity()
+        {
+                return m_deltaAngularVelocity;
+        }
+        const btVector3& internalGetAngularFactor() const
+        {
+                return m_angularFactor;
+        }
+        const btVector3& internalGetInvMass() const
+        {
+                return m_invMass;
+        }
+        btVector3& internalGetPushVelocity()
+        {
+                return m_pushVelocity;
+        }
+        btVector3& internalGetTurnVelocity()
+        {
+                return m_turnVelocity;
+        }
+        SIMD_FORCE_INLINE void  internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
+        {
+                velocity = getLinearVelocity()+m_deltaLinearVelocity + (getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
+        }
+        SIMD_FORCE_INLINE void  internalGetAngularVelocity(btVector3& angVel) const
+        {
+                angVel = getAngularVelocity()+m_deltaAngularVelocity;
+        }
+        //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
+        SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
+        {
+                if (m_inverseMass)
+                {
+                        m_deltaLinearVelocity += linearComponent*impulseMagnitude;
+                        m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+                }
+        }
+        SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+        {
+                if (m_inverseMass)
+                {
+                        m_pushVelocity += linearComponent*impulseMagnitude;
+                        m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+                }
+        }
+        void    internalWritebackVelocity()
+        {
+                if (m_inverseMass)
+                {
+                        setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity);
+                        setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity);
+                        //m_deltaLinearVelocity.setZero();
+                        //m_deltaAngularVelocity .setZero();
+                        //m_originalBody->setCompanionId(-1);
+                }
+        }
+        void    internalWritebackVelocity(btScalar timeStep);
+        ///////////////////////////////////////////////
+        virtual int     calculateSerializeBufferSize()  const;
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer,  class btSerializer* serializer) const;
+        virtual void serializeSingleObject(class btSerializer* serializer) const;
 };
+//@todo add m_optionalMotionState and m_constraintRefs to btRigidBodyData
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btRigidBodyFloatData
+{
+        btCollisionObjectFloatData      m_collisionObjectData;
+        btMatrix3x3FloatData            m_invInertiaTensorWorld;
+        btVector3FloatData              m_linearVelocity;
+        btVector3FloatData              m_angularVelocity;
+        btVector3FloatData              m_angularFactor;
+        btVector3FloatData              m_linearFactor;
+        btVector3FloatData              m_gravity;
+        btVector3FloatData              m_gravity_acceleration;
+        btVector3FloatData              m_invInertiaLocal;
+        btVector3FloatData              m_totalForce;
+        btVector3FloatData              m_totalTorque;
+        float                                   m_inverseMass;
+        float                                   m_linearDamping;
+        float                                   m_angularDamping;
+        float                                   m_additionalDampingFactor;
+        float                                   m_additionalLinearDampingThresholdSqr;
+        float                                   m_additionalAngularDampingThresholdSqr;
+        float                                   m_additionalAngularDampingFactor;
+        float                                   m_linearSleepingThreshold;
+        float                                   m_angularSleepingThreshold;
+        int                                             m_additionalDamping;
+};
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btRigidBodyDoubleData
+{
+        btCollisionObjectDoubleData     m_collisionObjectData;
+        btMatrix3x3DoubleData           m_invInertiaTensorWorld;
+        btVector3DoubleData             m_linearVelocity;
+        btVector3DoubleData             m_angularVelocity;
+        btVector3DoubleData             m_angularFactor;
+        btVector3DoubleData             m_linearFactor;
+        btVector3DoubleData             m_gravity;
+        btVector3DoubleData             m_gravity_acceleration;
+        btVector3DoubleData             m_invInertiaLocal;
+        btVector3DoubleData             m_totalForce;
+        btVector3DoubleData             m_totalTorque;
+        double                                  m_inverseMass;
+        double                                  m_linearDamping;
+        double                                  m_angularDamping;
+        double                                  m_additionalDampingFactor;
+        double                                  m_additionalLinearDampingThresholdSqr;
+        double                                  m_additionalAngularDampingThresholdSqr;
+        double                                  m_additionalAngularDampingFactor;
+        double                                  m_linearSleepingThreshold;
+        double                                  m_angularSleepingThreshold;
+        int                                             m_additionalDamping;
+        char    m_padding[4];
+};

code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp

-                      r5781
+                      r8284
 void    btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body)
+{
+        removeCollisionObject(body);
+}
+        btCollisionWorld::removeCollisionObject(body);
+}
+void    btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+{
+        btRigidBody* body = btRigidBody::upcast(collisionObject);
+        if (body)
+                removeRigidBody(body);
+        else
+                btCollisionWorld::removeCollisionObject(collisionObject);
+}
 void    btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)

code/branches/kicklib2/src/external/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h

-                      r5781
+                      r8284
         virtual void    removeRigidBody(btRigidBody* body);
+        ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
+        virtual void    removeCollisionObject(btCollisionObject* collisionObject);
         virtual void    updateAabbs();

code/branches/kicklib2/src/external/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp

-                      r5781
+                      r8284
 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
+static btRigidBody s_fixedObject( 0,0,0);
+btRigidBody& btActionInterface::getFixedBody()
+{
+        static btRigidBody s_fixed(0, 0,0);
+        s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+        return s_fixed;
+}
 btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis,  btVehicleRaycaster* raycaster )
 …
         ci.m_bIsFrontWheel = isFrontWheel;
         ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm;
+        ci.m_maxSuspensionForce = tuning.m_maxSuspensionForce;
         m_wheelInfo.push_back( btWheelInfo(ci));
 …
                 wheel.m_raycastInfo.m_isInContact = true;
                 wheel.m_raycastInfo.m_groundObject = &s_fixedObject;///@todo for driving on dynamic/movable objects!;
+                wheel.m_raycastInfo.m_groundObject = &getFixedBody();///@todo for driving on dynamic/movable objects!;
                 //wheel.m_raycastInfo.m_groundObject = object;
 …
                 btScalar suspensionForce = wheel.m_wheelsSuspensionForce;
+                btScalar gMaxSuspensionForce = btScalar(6000.);
+                if (suspensionForce > gMaxSuspensionForce)
+                {
+                        suspensionForce = gMaxSuspensionForce;
+                if (suspensionForce > wheel.m_maxSuspensionForce)
+                {
+                        suspensionForce = wheel.m_maxSuspensionForce;
+                }
                 btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
 …
                                         btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
                                         rel_pos[m_indexForwardAxis] *= wheelInfo.m_rollInfluence;
+                                        rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
                                         m_chassisBody->applyImpulse(sideImp,rel_pos);
 …
         for (int v=0;v<this->getNumWheels();v++)
+        {
                 btVector3 wheelColor(0,255,255);
+                btVector3 wheelColor(0,1,1);
                 if (getWheelInfo(v).m_raycastInfo.m_isInContact)
+                {
                         wheelColor.setValue(0,0,255);
+                        wheelColor.setValue(0,0,1);
                 } else
+                {
                         wheelColor.setValue(255,0,255);
+                        wheelColor.setValue(1,0,1);
+                }

code/branches/kicklib2/src/external/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h

-                      r5781
+                      r8284
                 btAlignedObjectArray<btScalar>  m_forwardImpulse;
                 btAlignedObjectArray<btScalar>  m_sideImpulse;
+                ///backwards compatibility
+                int     m_userConstraintType;
+                int     m_userConstraintId;
 public:
 …
                                 m_suspensionDamping(btScalar(0.88)),
                                 m_maxSuspensionTravelCm(btScalar(500.)),
+                                m_frictionSlip(btScalar(10.5))
+                                m_frictionSlip(btScalar(10.5)),
+                                m_maxSuspensionForce(btScalar(6000.))
+                        {
+                        }
 …
                         btScalar        m_maxSuspensionTravelCm;
                         btScalar        m_frictionSlip;
+                        btScalar        m_maxSuspensionForce;
                 };
 …
         virtual void updateAction( btCollisionWorld* collisionWorld, btScalar step)
+        {
+        (void) collisionWorld;
                 updateVehicle(step);
+        }
 …
+        ///backwards compatibility
+        int getUserConstraintType() const
+        {
+                return m_userConstraintType ;
+        }
+        void    setUserConstraintType(int userConstraintType)
+        {
+                m_userConstraintType = userConstraintType;
+        };
+        void    setUserConstraintId(int uid)
+        {
+                m_userConstraintId = uid;
+        }
+        int getUserConstraintId() const
+        {
+                return m_userConstraintId;
+        }
 };

code/branches/kicklib2/src/external/bullet/BulletDynamics/Vehicle/btWheelInfo.h

-                      r5781
+                      r8284
         btScalar                m_wheelsDampingRelaxation;
         btScalar                m_frictionSlip;
+        btScalar                m_maxSuspensionForce;
         bool m_bIsFrontWheel;
 …
         btScalar        m_deltaRotation;
         btScalar        m_rollInfluence;
+        btScalar        m_maxSuspensionForce;
         btScalar        m_engineForce;
 …
                 m_rollInfluence = btScalar(0.1);
                 m_bIsFrontWheel = ci.m_bIsFrontWheel;
+                m_maxSuspensionForce = ci.m_maxSuspensionForce;
+        }

Context Navigation

Legend:

Download in other formats: