Changeset 8351 for code/trunk/src/external/bullet/BulletDynamics

Timestamp:

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

Author:

rgrieder

Message:

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

Notes for updating

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

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

Key new features:

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

There are also some regressions:

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

Location:

code/trunk/src/external/bullet/BulletDynamics

Files:

• CMakeLists.txt (modified) (3 diffs)
• Character/btCharacterControllerInterface.h (modified) (1 diff)
• Character/btKinematicCharacterController.cpp (modified) (26 diffs)
• Character/btKinematicCharacterController.h (modified) (8 diffs)
• ConstraintSolver/btConeTwistConstraint.cpp (modified) (28 diffs)
• ConstraintSolver/btConeTwistConstraint.h (modified) (7 diffs)
• ConstraintSolver/btContactConstraint.cpp (modified) (2 diffs)
• ConstraintSolver/btContactConstraint.h (modified) (2 diffs)
• ConstraintSolver/btContactSolverInfo.h (modified) (3 diffs)
• ConstraintSolver/btGeneric6DofConstraint.cpp (modified) (36 diffs)
• ConstraintSolver/btGeneric6DofConstraint.h (modified) (22 diffs)
• ConstraintSolver/btGeneric6DofSpringConstraint.cpp (copied) (copied from code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp)
• ConstraintSolver/btGeneric6DofSpringConstraint.h (copied) (copied from code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h)
• ConstraintSolver/btHinge2Constraint.cpp (copied) (copied from code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp)
• ConstraintSolver/btHinge2Constraint.h (copied) (copied from code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btHinge2Constraint.h)
• ConstraintSolver/btHingeConstraint.cpp (modified) (22 diffs)
• ConstraintSolver/btHingeConstraint.h (modified) (8 diffs)
• ConstraintSolver/btJacobianEntry.h (modified) (1 diff)
• ConstraintSolver/btPoint2PointConstraint.cpp (modified) (9 diffs)
• ConstraintSolver/btPoint2PointConstraint.h (modified) (6 diffs)
• ConstraintSolver/btSequentialImpulseConstraintSolver.cpp (modified) (41 diffs)
• ConstraintSolver/btSequentialImpulseConstraintSolver.h (modified) (4 diffs)
• ConstraintSolver/btSliderConstraint.cpp (modified) (16 diffs)
• ConstraintSolver/btSliderConstraint.h (modified) (7 diffs)
• ConstraintSolver/btSolverBody.h (modified) (3 diffs)
• ConstraintSolver/btSolverConstraint.h (modified) (3 diffs)
• ConstraintSolver/btTypedConstraint.cpp (modified) (3 diffs)
• ConstraintSolver/btTypedConstraint.h (modified) (8 diffs)
• ConstraintSolver/btUniversalConstraint.cpp (copied) (copied from code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp)
• ConstraintSolver/btUniversalConstraint.h (copied) (copied from code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btUniversalConstraint.h)
• Dynamics/Bullet-C-API.cpp (modified) (4 diffs)
• Dynamics/btActionInterface.h (modified) (2 diffs)
• Dynamics/btContinuousDynamicsWorld.cpp (modified) (1 diff)
• Dynamics/btDiscreteDynamicsWorld.cpp (modified) (33 diffs)
• Dynamics/btDiscreteDynamicsWorld.h (modified) (8 diffs)
• Dynamics/btDynamicsWorld.h (modified) (3 diffs)
• Dynamics/btRigidBody.cpp (modified) (8 diffs)
• Dynamics/btRigidBody.h (modified) (13 diffs)
• Dynamics/btSimpleDynamicsWorld.cpp (modified) (1 diff)
• Dynamics/btSimpleDynamicsWorld.h (modified) (1 diff)
• Vehicle/btRaycastVehicle.cpp (modified) (6 diffs)
• Vehicle/btRaycastVehicle.h (modified) (5 diffs)
• Vehicle/btWheelInfo.h (modified) (3 diffs)

code/trunk/src/external/bullet/BulletDynamics/CMakeLists.txt

@@ -3 +3 @@
 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
@@ -12 +16 @@
         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
@@ -40 +45 @@
         ConstraintSolver/btSolverConstraint.h
         ConstraintSolver/btTypedConstraint.h
-
+        ConstraintSolver/btUniversalConstraint.h
         Dynamics/btActionInterface.h
         Dynamics/btContinuousDynamicsWorld.h

code/trunk/src/external/bullet/BulletDynamics/Character/btCharacterControllerInterface.h

@@ -31 +31 @@
         
         virtual void    setWalkDirection(const btVector3& walkDirection) = 0;
+        virtual void    setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) = 0;
         virtual void    reset () = 0;
         virtual void    warp (const btVector3& origin) = 0;

code/trunk/src/external/bullet/BulletDynamics/Character/btKinematicCharacterController.cpp

@@ -13 +13 @@
 3. This notice may not be removed or altered from any source distribution.
 */
+
 
 #include "LinearMath/btIDebugDraw.h"
@@ -23 +24 @@
 #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,
@@ -53 +65 @@
 {
 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)
+        {
         }
 
@@ -61 +76 @@
         {
                 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);
@@ -67 +97 @@
 protected:
         btCollisionObject* m_me;
+        const btVector3 m_up;
+        btScalar m_minSlopeDot;
 };
 
@@ -99 +131 @@
 {
         m_upAxis = upAxis;
-        m_addedMargin = 0.02f;
+        m_addedMargin = 0.02;
         m_walkDirection.setValue(0,0,0);
         m_useGhostObjectSweepTest = true;
@@ -106 +138 @@
         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));
 }
 
@@ -145 +187 @@
                                 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);
                                 }
                         }
@@ -174 +218 @@
         // 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 ();
@@ -180 +224 @@
 
         /* 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;
@@ -198 +242 @@
         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;
@@ -245 +295 @@
 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 ();
@@ -264 +310 @@
         if (m_touchingContact)
         {
-                if (originalDir.dot(m_touchingNormal) > btScalar(0.0))
+                if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
+                {
                         updateTargetPositionBasedOnCollision (m_touchingNormal);
+                }
         }
 
@@ -274 +322 @@
                 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;
@@ -300 +349 @@
                 {       
                         // 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);
@@ -320 +361 @@
                                 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;
@@ -329 +370 @@
                                 break;
                         }
+
                 } else {
                         // we moved whole way
@@ -345 +387 @@
 
         // 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 ();
@@ -355 +409 @@
         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;
@@ -371 +425 @@
                 // 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
@@ -378 +435 @@
 }
 
+
+
+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 ()
 {
@@ -402 +492 @@
                 if (numPenetrationLoops > 4)
                 {
-//                      printf("character could not recover from penetration = %d\n", numPenetrationLoops);
+                        //printf("character could not recover from penetration = %d\n", numPenetrationLoops);
                         break;
                 }
@@ -414 +504 @@
 }
 
+#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 ();
@@ -423 +539 @@
 
         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);
@@ -454 +588 @@
         if (!canJump())
                 return;
+
+        m_verticalVelocity = m_jumpSpeed;
+        m_wasJumping = true;
 
 #if 0
@@ -466 +603 @@
 }
 
+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/trunk/src/external/bullet/BulletDynamics/Character/btKinematicCharacterController.h

@@ -14 +14 @@
 */
 
+
 #ifndef KINEMATIC_CHARACTER_CONTROLLER_H
 #define KINEMATIC_CHARACTER_CONTROLLER_H
@@ -20 +21 @@
 
 #include "btCharacterControllerInterface.h"
+
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+
 
 class btCollisionShape;
@@ -33 +37 @@
 {
 protected:
+
         btScalar m_halfHeight;
         
@@ -38 +43 @@
         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;
@@ -50 +60 @@
         ///this is the desired walk direction, set by the user
         btVector3       m_walkDirection;
+        btVector3       m_normalizedDirection;
 
         //some internal variables
@@ -62 +73 @@
         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);
@@ -99 +115 @@
         }
 
-        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 ();
@@ -114 +140 @@
         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/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

@@ -23 +23 @@
 #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);
+}
+
+
 
 
@@ -64 +66 @@
         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)
@@ -83 +87 @@
                 info->m_numConstraintRows = 3;
                 info->nub = 3;
-                calcAngleInfo2();
+                calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
                 if(m_solveSwingLimit)
                 {
@@ -100 +104 @@
                 }
         }
-} // 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);
@@ -124 +137 @@
                 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);
@@ -132 +145 @@
         }
     // 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;
@@ -150 +168 @@
                 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);
@@ -187 +205 @@
 
                         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;
@@ -207 +228 @@
                 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)
                 {
@@ -231 +255 @@
 }
         
-//-----------------------------------------------------------------------------
+
 
 void    btConeTwistConstraint::buildJacobian()
@@ -240 +264 @@
                 m_accTwistLimitImpulse = btScalar(0.);
                 m_accSwingLimitImpulse = btScalar(0.);
+                m_accMotorImpulse = btVector3(0.,0.,0.);
 
                 if (!m_angularOnly)
@@ -274 +299 @@
                 }
 
-                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)
         {
@@ -296 +322 @@
 
                         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;
 
@@ -315 +341 @@
                                 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);
                 
                         }
@@ -327 +353 @@
                         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);
@@ -402 +428 @@
                                 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)
@@ -422 +448 @@
                                 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);
                         }
                 }
@@ -431 +457 @@
                         ///solve angular part
                         btVector3 angVelA;
-                        bodyA.getAngularVelocity(angVelA);
+                        bodyA.internalGetAngularVelocity(angVelA);
                         btVector3 angVelB;
-                        bodyB.getAngularVelocity(angVelB);
+                        bodyB.internalGetAngularVelocity(angVelB);
 
                         // solve swing limit
@@ -462 +488 @@
                                 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);
                         }
 
@@ -483 +509 @@
                                 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)
@@ -499 +528 @@
 }
 
-//-----------------------------------------------------------------------------
-
+
+#ifndef __SPU__
 void btConeTwistConstraint::calcAngleInfo()
 {
@@ -585 +614 @@
                 }
         }
-} // 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.);
@@ -598 +627 @@
         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...)
@@ -642 +692 @@
 
                                 m_kSwing =  btScalar(1.) /
-                                        (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
-                                         getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
+                                        (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) +
+                                         computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB));
                         }
                 }
@@ -651 +701 @@
                         // 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);
@@ -745 +795 @@
 
                                 m_kTwist = btScalar(1.) /
-                                        (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
-                                         getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
+                                        (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
+                                         computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
                         }
 
@@ -757 +807 @@
                 }
         }
-} // btConeTwistConstraint::calcAngleInfo2()
+}
 
 
@@ -982 +1032 @@
 }
 
-
-//-----------------------------------------------------------------------------
-//-----------------------------------------------------------------------------
-//-----------------------------------------------------------------------------
-
-
+///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/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h

@@ -18 +18 @@
 
 
+/*
+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
@@ -27 +43 @@
 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)
@@ -84 +106 @@
         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:
 
@@ -90 +131 @@
         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);
@@ -142 +184 @@
         }
 
-        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;
@@ -172 +225 @@
 
         void calcAngleInfo();
-        void calcAngleInfo2();
+        void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
 
         inline btScalar getSwingSpan1()
@@ -213 +266 @@
         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/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp

@@ -13 +13 @@
 3. 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()
+{
+
+}
+
+
+
 
 
@@ -86 +132 @@
 
 
-//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/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.h

@@ -17 +17 @@
 #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,
@@ -95 +65 @@
 
 
-///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/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -36 +36 @@
 
         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;
@@ -53 +53 @@
         int                     m_solverMode;
         int     m_restingContactRestitutionThreshold;
+        int                     m_minimumSolverBatchSize;
 
 
@@ -78 +79 @@
                 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/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp

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

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

@@ -13 +13 @@
 3. 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
+
 /*
 2007-09-09
@@ -46 +50 @@
     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;
@@ -55 +61 @@
     //!@{
     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;
@@ -65 +72 @@
         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;
@@ -83 +92 @@
         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;
@@ -113 +124 @@
 
         //! 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);
 
 };
@@ -130 +141 @@
     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];
@@ -135 +149 @@
     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
 
@@ -142 +157 @@
         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;
@@ -163 +181 @@
         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++) 
                 {
@@ -193 +215 @@
         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,
@@ -201 +223 @@
 
 };
+
+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
@@ -216 +247 @@
 <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>
@@ -273 +306 @@
     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;
+
     //!@}
 
@@ -288 +327 @@
 
 
-        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(
@@ -312 +351 @@
 
     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
         /*!
@@ -320 +358 @@
         \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
@@ -367 +407 @@
         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);
@@ -381 +424 @@
     //! 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);
@@ -404 +454 @@
     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]);
     }
 
@@ -438 +486 @@
         else
         {
+                        lo = btNormalizeAngle(lo);
+                        hi = btNormalizeAngle(hi);
                 m_angularLimits[axis-3].m_loLimit = lo;
                 m_angularLimits[axis-3].m_hiLimit = hi;
@@ -460 +510 @@
     }
 
-    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/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

@@ -22 +22 @@
 #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;
@@ -80 +77 @@
         
         //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;
@@ -89 +86 @@
 }
 
-//-----------------------------------------------------------------------------
-
-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)
 {
 
@@ -120 +119 @@
         
         //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;
@@ -129 +128 @@
 }
 
-//-----------------------------------------------------------------------------
+
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 
@@ -137 +136 @@
 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;
@@ -149 +150 @@
 }                       
 
-//-----------------------------------------------------------------------------
+
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
@@ -156 +157 @@
 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
@@ -163 +166 @@
 
         //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;
@@ -172 +175 @@
 }
 
-//-----------------------------------------------------------------------------
+
 
 void    btHingeConstraint::buildJacobian()
@@ -179 +182 @@
         {
                 m_appliedImpulse = btScalar(0.);
+                m_accMotorImpulse = btScalar(0.);
 
                 if (!m_angularOnly)
@@ -222 +226 @@
                 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;
@@ -249 +252 @@
 
                         // test angular limit
-                        testLimit();
+                        testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
 
                 //Compute K = J*W*J' for hinge axis
@@ -259 +262 @@
 }
 
-//-----------------------------------------------------------------------------
+
+#endif //__SPU__
+
 
 void btHingeConstraint::getInfo1(btConstraintInfo1* info)
@@ -272 +277 @@
                 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())
                 {
@@ -279 +285 @@
                         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
@@ -403 +478 @@
                 }
                 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;
@@ -413 +492 @@
                 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) 
                         {
@@ -436 +518 @@
                         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.
@@ -468 +550 @@
 }
 
-//-----------------------------------------------------------------------------
-
-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)
@@ -624 +561 @@
 }
 
-//-----------------------------------------------------------------------------
 
 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()
 {
@@ -660 +602 @@
         }
         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/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h

@@ -25 +25 @@
 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
@@ -61 +78 @@
         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);
@@ -74 +99 @@
         btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
 
-        btHingeConstraint();
 
         virtual void    buildJacobian();
@@ -80 +104 @@
         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);
@@ -117 +147 @@
         }
 
+        // 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;
@@ -128 +167 @@
         }
 
+        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
         {
@@ -141 +203 @@
         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()
@@ -173 +240 @@
                 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/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btJacobianEntry.h

@@ -29 +29 @@
 /// it can be used in combination with a constraint solver
 /// Can be used to relate the effect of an impulse to the constraint error
-class btJacobianEntry
+ATTRIBUTE_ALIGNED16(class) btJacobianEntry
 {
 public:

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

@@ -21 +21 @@
 
 
-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)
 {
@@ -37 +34 @@
 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)
 {
@@ -44 +42 @@
 void    btPoint2PointConstraint::buildJacobian()
 {
+
         ///we need it for both methods
         {
@@ -67 +66 @@
         }
 
-}
-
+
+}
 
 void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
+{
+        getInfo1NonVirtual(info);
+}
+
+void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
 {
         if (m_useSolveConstraintObsolete)
@@ -83 +87 @@
 }
 
+
+
+
 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.
@@ -97 +105 @@
     // 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();
@@ -127 +135 @@
 
     // 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;//
@@ -145 +160 @@
                 }
         }
-        
-}
-
-
-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)
@@ -229 +172 @@
 }
 
+///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/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h

@@ -23 +23 @@
 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
 {
@@ -36 +45 @@
 };
 
+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
@@ -47 +62 @@
         btVector3       m_pivotInB;
         
-        
+        int                     m_flags;
+        btScalar        m_erp;
+        btScalar        m_cfm;
         
 public:
@@ -60 +77 @@
         btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA);
 
-        btPoint2PointConstraint();
 
         virtual void    buildJacobian();
@@ -66 +82 @@
         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);
@@ -93 +110 @@
         }
 
+        ///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/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -35 +35 @@
 #include <string.h> //for memset
 
+int             gNumSplitImpulseRecoveries = 0;
+
 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
 :m_btSeed2(0)
@@ -56 +58 @@
 
 // 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
@@ -63 +65 @@
         __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)));
@@ -77 +79 @@
         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);
@@ -90 +92 @@
 
 // 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;
@@ -115 +117 @@
                 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
@@ -128 +128 @@
         __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)));
@@ -139 +139 @@
         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);
@@ -152 +152 @@
 
 // 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;
@@ -170 +170 @@
                 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
 }
 
@@ -215 +278 @@
 
 
-
+#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)
@@ -263 +330 @@
 
 
-
-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)
 {
 
@@ -271 +337 @@
         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;
@@ -283 +346 @@
 
         solverConstraint.m_appliedImpulse = 0.f;
-        //      solverConstraint.m_appliedPushImpulse = 0.f;
+        solverConstraint.m_appliedPushImpulse = 0.f;
 
         {
@@ -338 +401 @@
                 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;
 }
@@ -353 +425 @@
 int     btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
 {
+#if 0
         int solverBodyIdA = -1;
 
@@ -374 +447 @@
         }
         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
@@ -467 +506 @@
 
 
-                                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;
@@ -499 +538 @@
                                         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
                                 {
@@ -507 +546 @@
                                 }
 
-                                //                                                      solverConstraint.m_appliedPushImpulse = 0.f;
+                                solverConstraint.m_appliedPushImpulse = 0.f;
 
                                 {
@@ -523 +562 @@
                                         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;
@@ -530 +579 @@
 
 
-                                /////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)
                                         {
@@ -588 +597 @@
                                                                 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
                                                         {
@@ -604 +613 @@
                                                                 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
                                                         {
@@ -622 +631 @@
                                                 }
                                         }
+}
+
+
+
+
+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");
@@ -644 +746 @@
         }
 
+        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)
         {
@@ -653 +782 @@
                 }
         }
-
-        btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
-        initSolverBody(&fixedBody,0);
-
         //btRigidBody* rb0=0,*rb1=0;
 
@@ -665 +790 @@
                         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;
@@ -675 +801 @@
                         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)
                                 {
@@ -697 +821 @@
                                         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++)
@@ -711 +830 @@
                                                 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);
 
 
@@ -733 +852 @@
                                         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);
 
@@ -742 +864 @@
                                         {
                                                 btSolverConstraint& solverConstraint = currentConstraintRow[j];
+                                                solverConstraint.m_originalContactPoint = constraint;
 
                                                 {
@@ -778 +901 @@
                                                         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;
@@ -787 +910 @@
                                         }
                                 }
+                                currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
                         }
                 }
@@ -793 +917 @@
                         int i;
                         btPersistentManifold* manifold = 0;
-                        btCollisionObject* colObj0=0,*colObj1=0;
+//                      btCollisionObject* colObj0=0,*colObj1=0;
 
 
@@ -830 +954 @@
 }
 
-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;
@@ -842 +1109 @@
                 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++)
@@ -993 +1138 @@
         }
 
+        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);
@@ -1018 +1177 @@
 
 
-
-
-
-
-
+/// 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()
@@ -1029 +1200 @@
 }
 
-
+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/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -22 +22 @@
 #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.
@@ -30 +30 @@
 protected:
 
-        btAlignedObjectArray<btSolverBody>      m_tmpSolverBodyPool;
         btConstraintArray                       m_tmpSolverContactConstraintPool;
         btConstraintArray                       m_tmpSolverNonContactConstraintPool;
@@ -36 +35 @@
         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:
 
@@ -72 +100 @@
         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/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp

@@ -19 +19 @@
 */
 
-//-----------------------------------------------------------------------------
+
 
 #include "btSliderConstraint.h"
@@ -26 +26 @@
 #include <new>
 
-//-----------------------------------------------------------------------------
+#define USE_OFFSET_FOR_CONSTANT_FRAME true
 
 void btSliderConstraint::initParams()
@@ -37 +37 @@
         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;
@@ -63 +69 @@
         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;
@@ -127 +187 @@
     {
                 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
@@ -209 +320 @@
         // 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];
@@ -230 +335 @@
         // 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 :
@@ -243 +348 @@
         // 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())
         {
@@ -300 +467 @@
                 limit = (limit_err > btScalar(0.0)) ? 2 : 1;
         }
-        int powered = 0;
+        powered = 0;
         if(getPoweredLinMotor())
         {
@@ -320 +487 @@
                 // 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();
@@ -340 +523 @@
                 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;
@@ -352 +538 @@
                 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
@@ -374 +563 @@
                         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
@@ -437 +626 @@
                         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;
@@ -447 +640 @@
                 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) 
                         {
@@ -500 +696 @@
                 } // 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/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h

@@ -26 +26 @@
 #define SLIDER_CONSTRAINT_H
 
-//-----------------------------------------------------------------------------
+
 
 #include "LinearMath/btVector3.h"
@@ -32 +32 @@
 #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
@@ -49 +66 @@
         ///for backwards compatibility during the transition to 'getInfo/getInfo2'
         bool            m_useSolveConstraintObsolete;
+        bool            m_useOffsetForConstraintFrame;
         btTransform     m_frameInA;
     btTransform m_frameInB;
@@ -68 +86 @@
         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];
@@ -127 +158 @@
         // 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
@@ -151 +185 @@
     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; }
@@ -211 +245 @@
         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/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h

@@ -106 +106 @@
 
 ///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;
 
         
@@ -146 +145 @@
         }
 
-        
-/*
+        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);
@@ -159 +164 @@
                 }
         }
-        */
 
-        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/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h

@@ -27 +27 @@
 
 ///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();
@@ -59 +59 @@
         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;
         };
         
@@ -78 +78 @@
         btScalar                m_lowerLimit;
         btScalar                m_upperLimit;
+
+        btScalar                m_rhsPenetration;
 
         enum            btSolverConstraintType

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

@@ -17 +17 @@
 #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),
@@ -56 +45 @@
 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)
@@ -110 +97 @@
         }
         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/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h

@@ -1 +1 @@
 /*
 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.
@@ -20 +20 @@
 #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)
@@ -55 +76 @@
         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);
@@ -94 +118 @@
                 // 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
@@ -148 +196 @@
         }
 
+        void    setUserConstraintPtr(void* ptr)
+        {
+                m_userConstraintPtr = ptr;
+        }
+
+        void*   getUserConstraintPtr()
+        {
+                return m_userConstraintPtr;
+        }
+
         int getUid() const
         {
@@ -153 +211 @@
         } 
 
+        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;
         }
@@ -160 +233 @@
         btTypedConstraintType getConstraintType () const
         {
-                return m_constraintType;
+                return btTypedConstraintType(m_objectType);
         }
         
@@ -171 +244 @@
                 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/trunk/src/external/bullet/BulletDynamics/Dynamics/Bullet-C-API.cpp

@@ -44 +44 @@
 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
-#include "LinearMath/btStackAlloc.h"
+
 
 /*
@@ -182 +182 @@
 {
         //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)
@@ -294 +294 @@
         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);
 }
 
@@ -366 +374 @@
         btGjkPairDetector::ClosestPointInput input;
         
-        btStackAlloc gStackAlloc(1024*1024*2);
- 
-        input.m_stackAlloc = &gStackAlloc;
-        
+                
         btTransform tr;
         tr.setIdentity();

code/trunk/src/external/bullet/BulletDynamics/Dynamics/btActionInterface.h

@@ -21 +21 @@
 
 #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()
@@ -38 +44 @@
 
 #endif //_BT_ACTION_INTERFACE_H
+

code/trunk/src/external/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp

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

code/trunk/src/external/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -1 +1 @@
 /*
 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.
@@ -20 +20 @@
 #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"
@@ -36 +37 @@
 #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"
 
 
@@ -56 +45 @@
 #include "LinearMath/btMotionState.h"
 
-
+#include "LinearMath/btSerializer.h"
 
 
@@ -64 +53 @@
 m_constraintSolver(constraintSolver),
 m_gravity(0,-10,0),
-m_localTime(btScalar(1.)/btScalar(60.)),
+m_localTime(0),
+m_synchronizeAllMotionStates(false),
 m_profileTimings(0)
 {
@@ -104 +94 @@
 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);
+                        }
+                }
+        }
+
 }
 
@@ -127 +117 @@
         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())
@@ -169 +143 @@
                 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())
                 {
@@ -218 +156 @@
 {
         ///@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();
         }
 }       
@@ -234 +169 @@
 {
         ///@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();
@@ -270 +203 @@
 {
         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);
+                }
+        }
 }
 
@@ -341 +268 @@
         {
 
-                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++)
                 {
@@ -354 +283 @@
                 }
 
-        } 
-
-        synchronizeMotionStates();
+        } else
+        {
+                synchronizeMotionStates();
+        }
 
         clearForces();
@@ -372 +302 @@
         BT_PROFILE("internalSingleStepSimulation");
 
+        if(0 != m_internalPreTickCallback) {
+                (*m_internalPreTickCallback)(this, timeStep);
+        }       
+
         ///apply gravity, predict motion
         predictUnconstraintMotion(timeStep);
@@ -412 +346 @@
 {
         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);
@@ -428 +361 @@
 }
 
+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);
@@ -443 +391 @@
         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);
@@ -453 +409 @@
 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);
@@ -460 +416 @@
         if (body->getCollisionShape())
         {
+                if (!body->isStaticObject())
+                {
+                        m_nonStaticRigidBodies.push_back(body);
+                }
+                 else
+                {
+                        body->setActivationState(ISLAND_SLEEPING);
+                }
                 addCollisionObject(body,group,mask);
         }
@@ -480 +444 @@
         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)
                 {
@@ -586 +549 @@
                 }
 };
-
 
 
@@ -604 +566 @@
                 btStackAlloc*                   m_stackAlloc;
                 btDispatcher*                   m_dispatcher;
+                
+                btAlignedObjectArray<btCollisionObject*> m_bodies;
+                btAlignedObjectArray<btPersistentManifold*> m_manifolds;
+                btAlignedObjectArray<btTypedConstraint*> m_constraints;
+
 
                 InplaceSolverIslandCallback(
@@ -624 +591 @@
                 }
 
+
                 InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
                 {
@@ -664 +632 @@
                                 }
 
-                                ///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
@@ -699 +698 @@
         m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback);
 
+        solverCallback.processConstraints();
+
         m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
 }
@@ -741 +742 @@
 
 
-#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+
 
 class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
@@ -754 +755 @@
         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)
@@ -829 +830 @@
         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);
                 }
         }
@@ -880 +877 @@
 {
         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());
                 }
         }
@@ -914 +906 @@
 
         
-
-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)
@@ -1350 +1067 @@
                                 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);
@@ -1367 +1084 @@
         }
         return;
-} // btDiscreteDynamicsWorld::debugDrawConstraint()
+}
 
 
@@ -1403 +1120 @@
 
 
+
+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/trunk/src/external/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

@@ -1 +1 @@
 /*
 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.
@@ -13 +13 @@
 3. This notice may not be removed or altered from any source distribution.
 */
+
 
 #ifndef BT_DISCRETE_DYNAMICS_WORLD_H
@@ -42 +43 @@
         btAlignedObjectArray<btTypedConstraint*> m_constraints;
 
+        btAlignedObjectArray<btRigidBody*> m_nonStaticRigidBodies;
+
         btVector3       m_gravity;
 
@@ -50 +53 @@
         bool    m_ownsIslandManager;
         bool    m_ownsConstraintSolver;
+        bool    m_synchronizeAllMotionStates;
 
         btAlignedObjectArray<btActionInterface*>        m_actions;
@@ -74 +78 @@
         virtual void    saveKinematicState(btScalar timeStep);
 
-        void    debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color);
-
+        void    serializeRigidBodies(btSerializer* serializer);
 
 public:
@@ -121 +124 @@
         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);
 
@@ -127 +132 @@
         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);
@@ -175 +182 @@
         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/trunk/src/external/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h

@@ -42 +42 @@
 protected:
                 btInternalTickCallback m_internalTickCallback;
+                btInternalTickCallback m_internalPreTickCallback;
                 void*   m_worldUserInfo;
 
@@ -50 +51 @@
 
                 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)
                 {
                 }
@@ -103 +104 @@
 
                 /// 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/trunk/src/external/bullet/BulletDynamics/Dynamics/btRigidBody.cpp

@@ -20 +20 @@
 #include "LinearMath/btMotionState.h"
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "LinearMath/btSerializer.h"
 
 //'temporarily' global variables
@@ -45 +46 @@
         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));
@@ -86 +88 @@
         updateInertiaTensor();
 
+        m_rigidbodyFlags = 0;
+
+
+        m_deltaLinearVelocity.setZero();
+        m_deltaAngularVelocity.setZero();
+        m_invMass = m_inverseMass*m_linearFactor;
+        m_pushVelocity.setZero();
+        m_turnVelocity.setZero();
+
+        
+
 }
 
@@ -136 +149 @@
 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));
 }
 
@@ -227 +240 @@
                 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),
@@ -232 +248 @@
                                    inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
 
+        m_invMass = m_linearFactor*m_inverseMass;
 }
 
@@ -301 +318 @@
 }
 
+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)
 {
@@ -315 +354 @@
         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/trunk/src/external/bullet/BulletDynamics/Dynamics/btRigidBody.h

@@ -29 +29 @@
 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
+};
 
 
@@ -46 +60 @@
         btVector3               m_angularVelocity;
         btScalar                m_inverseMass;
-        btScalar                m_angularFactor;
+        btVector3               m_linearFactor;
 
         btVector3               m_gravity;      
@@ -72 +86 @@
         //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:
@@ -111 +140 @@
                 btScalar                        m_additionalAngularDampingFactor;
 
-                
                 btRigidBodyConstructionInfo(    btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)):
                 m_mass(mass),
@@ -161 +189 @@
         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;
@@ -167 +195 @@
         static btRigidBody*     upcast(btCollisionObject* colObj)
         {
-                if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
+                if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
                         return (btRigidBody*)colObj;
                 return 0;
@@ -220 +248 @@
         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 { 
@@ -231 +268 @@
         void                    applyCentralForce(const btVector3& force)
         {
-                m_totalForce += force;
+                m_totalForce += force*m_linearFactor;
         }
 
@@ -262 +299 @@
         void    applyTorque(const btVector3& torque)
         {
-                m_totalTorque += torque;
+                m_totalTorque += torque*m_angularFactor;
         }
         
@@ -268 +305 @@
         {
                 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;
         }
         
@@ -288 +325 @@
                         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() 
         {
@@ -451 +475 @@
         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;
@@ -481 +510 @@
         }
 
-        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/trunk/src/external/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp

@@ -133 +133 @@
 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/trunk/src/external/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h

@@ -58 +58 @@
 
         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/trunk/src/external/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp

@@ -23 +23 @@
 #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 )
@@ -71 +76 @@
         ci.m_bIsFrontWheel = isFrontWheel;
         ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm;
+        ci.m_maxSuspensionForce = tuning.m_maxSuspensionForce;
 
         m_wheelInfo.push_back( btWheelInfo(ci));
@@ -187 +193 @@
                 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;
 
@@ -302 +308 @@
                 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;
@@ -690 +695 @@
                                         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);
 
@@ -709 +714 @@
         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/trunk/src/external/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h

@@ -30 +30 @@
                 btAlignedObjectArray<btScalar>  m_forwardImpulse;
                 btAlignedObjectArray<btScalar>  m_sideImpulse;
+        
+                ///backwards compatibility
+                int     m_userConstraintType;
+                int     m_userConstraintId;
 
 public:
@@ -41 +45 @@
                                 m_suspensionDamping(btScalar(0.88)),
                                 m_maxSuspensionTravelCm(btScalar(500.)),
-                                m_frictionSlip(btScalar(10.5))
+                                m_frictionSlip(btScalar(10.5)),
+                                m_maxSuspensionForce(btScalar(6000.))
                         {
                         }
@@ -49 +54 @@
                         btScalar        m_maxSuspensionTravelCm;
                         btScalar        m_frictionSlip;
+                        btScalar        m_maxSuspensionForce;
 
                 };
@@ -79 +85 @@
         virtual void updateAction( btCollisionWorld* collisionWorld, btScalar step)
         {
+        (void) collisionWorld;
                 updateVehicle(step);
         }
@@ -189 +196 @@
 
 
+        ///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/trunk/src/external/bullet/BulletDynamics/Vehicle/btWheelInfo.h

@@ -30 +30 @@
         btScalar                m_wheelsDampingRelaxation;
         btScalar                m_frictionSlip;
+        btScalar                m_maxSuspensionForce;
         bool m_bIsFrontWheel;
         
@@ -69 +70 @@
         btScalar        m_deltaRotation;
         btScalar        m_rollInfluence;
+        btScalar        m_maxSuspensionForce;
 
         btScalar        m_engineForce;
@@ -100 +102 @@
                 m_rollInfluence = btScalar(0.1);
                 m_bIsFrontWheel = ci.m_bIsFrontWheel;
+                m_maxSuspensionForce = ci.m_maxSuspensionForce;
 
         }