bullet: Update to version 2.82 (bullet revision 2705)
authorSergej Reich <sergej.reich@googlemail.com>
Fri, 25 Oct 2013 03:43:20 +0000 (03:43 +0000)
committerSergej Reich <sergej.reich@googlemail.com>
Fri, 25 Oct 2013 03:43:20 +0000 (03:43 +0000)
Remove patch that has been applied upstream.

Fixes several bugs.

101 files changed:
extern/bullet2/CMakeLists.txt
extern/bullet2/patches/ghost_character.patch [deleted file]
extern/bullet2/readme.txt
extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h
extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h
extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp
extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/MLCPSolvers/btPATHSolver.h [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h [new file with mode: 0644]
extern/bullet2/src/BulletSoftBody/btSoftBody.cpp
extern/bullet2/src/BulletSoftBody/btSoftBody.h
extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp
extern/bullet2/src/BulletSoftBody/btSparseSDF.h
extern/bullet2/src/LinearMath/btIDebugDraw.h
extern/bullet2/src/LinearMath/btMatrix3x3.h
extern/bullet2/src/LinearMath/btMatrixX.h [new file with mode: 0644]
extern/bullet2/src/LinearMath/btQuaternion.h
extern/bullet2/src/LinearMath/btScalar.h
extern/bullet2/src/LinearMath/btSerializer.cpp
extern/bullet2/src/LinearMath/btSerializer.h
extern/bullet2/src/LinearMath/btVector3.cpp
extern/bullet2/src/LinearMath/btVector3.h
extern/bullet2/src/btBulletDynamicsCommon.h

index 02ca2cd375591c7d4c4825fa8b04d5e453201c37..2b2c18c06853756b3c68ff17bb20a3885c9ef99e 100644 (file)
@@ -67,6 +67,8 @@ set(SRC
        src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
        src/BulletCollision/CollisionDispatch/btUnionFind.cpp
        src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
+       src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
+       src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp
        src/BulletCollision/CollisionShapes/btBoxShape.cpp
        src/BulletCollision/CollisionShapes/btBox2dShape.cpp
        src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp
@@ -140,6 +142,7 @@ set(SRC
        src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
        src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp
        src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp
+       src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp
        src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
        src/BulletDynamics/Dynamics/btRigidBody.cpp
        src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
@@ -147,6 +150,15 @@ set(SRC
        src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
        src/BulletDynamics/Vehicle/btWheelInfo.cpp
        src/BulletDynamics/Character/btKinematicCharacterController.cpp
+       src/BulletDynamics/Featherstone/btMultiBody.cpp
+       src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
+       src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
+       src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
+       src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
+       src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
+       src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
+       src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp
+       src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
        
        src/BulletSoftBody/btSoftBody.cpp
        src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp
@@ -204,6 +216,8 @@ set(SRC
        src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h
        src/BulletCollision/CollisionDispatch/btUnionFind.h
        src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h
+       src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h
+       src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h
        src/BulletCollision/CollisionShapes/btBoxShape.h
        src/BulletCollision/CollisionShapes/btBox2dShape.h
        src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h
@@ -308,6 +322,7 @@ set(SRC
        src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
        src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h
        src/BulletDynamics/ConstraintSolver/btGearConstraint.h
+       src/BulletDynamics/ConstraintSolver/btFixedConstraint.h
        src/BulletDynamics/Dynamics/btActionInterface.h
        src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
        src/BulletDynamics/Dynamics/btDynamicsWorld.h
@@ -318,6 +333,22 @@ set(SRC
        src/BulletDynamics/Vehicle/btWheelInfo.h
        src/BulletDynamics/Character/btCharacterControllerInterface.h
        src/BulletDynamics/Character/btKinematicCharacterController.h
+       src/BulletDynamics/Featherstone/btMultiBody.h
+       src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
+       src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
+       src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
+       src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h
+       src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
+       src/BulletDynamics/Featherstone/btMultiBodyLink.h
+       src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
+       src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h
+       src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h
+       src/BulletDynamics/MLCPSolvers/btDantzigLCP.h
+       src/BulletDynamics/MLCPSolvers/btDantzigSolver.h
+       src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
+       src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h
+       src/BulletDynamics/MLCPSolvers/btPATHSolver.h
+       src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
        
        src/BulletSoftBody/btSoftBody.h
        src/BulletSoftBody/btSoftBodyInternals.h
@@ -359,6 +390,7 @@ set(SRC
        src/LinearMath/btTransformUtil.h
        src/LinearMath/btVector3.h
        src/LinearMath/btPolarDecomposition.h
+       src/LinearMath/btMatrixX.h
        
        
        src/btBulletCollisionCommon.h
diff --git a/extern/bullet2/patches/ghost_character.patch b/extern/bullet2/patches/ghost_character.patch
deleted file mode 100644 (file)
index d4098cb..0000000
+++ /dev/null
@@ -1,28 +0,0 @@
-Index: extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp
-===================================================================
---- extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp     (revision 49183)
-+++ extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp     (working copy)
-@@ -77,6 +77,9 @@
-               if (convexResult.m_hitCollisionObject == m_me)
-                       return btScalar(1.0);
-+              if (!convexResult.m_hitCollisionObject->hasContactResponse())
-+                      return btScalar(1.0);
-+
-               btVector3 hitNormalWorld;
-               if (normalInWorldSpace)
-               {
-@@ -173,7 +176,12 @@
-               m_manifoldArray.resize(0);
-               btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
--              
-+              btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject);
-+              btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
-+
-+              if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse()))
-+                      continue;
-+
-               if (collisionPair->m_algorithm)
-                       collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
index dd806d40facca042428a0d24eaa6001fa613de87..33430fc8ee32d9a4224cad3fe9291ab468ea7c45 100644 (file)
@@ -7,8 +7,5 @@ Erwin
 Apply patches/ghost_softbody.patch to prevent softbodies being hit by ghost objects.
 Originally committed in blender svn revision: 43905.
 
-Apply patches/ghost_character.patch to prevent characters from colliding with ghost objects.
-Mitchell
-
 Apply patches/convex_hull.patch to add access to the convex hull
 operation, used in the BMesh convex hull operator.
index 1ebb37797d97b2f082f9df4d27c3a5efa617e2b6..89c307d14ca540ece923c5587da23410b0424128 100644 (file)
@@ -25,7 +25,6 @@ class btOverlappingPairCache;
 struct btCollisionObjectWrapper;
 
 class btPersistentManifold;
-class btStackAlloc;
 class btPoolAllocator;
 
 struct btDispatcherInfo
@@ -47,8 +46,7 @@ struct btDispatcherInfo
                m_useEpa(true),
                m_allowedCcdPenetration(btScalar(0.04)),
                m_useConvexConservativeDistanceUtil(false),
-               m_convexConservativeDistanceThreshold(0.0f),
-               m_stackAllocator(0)
+               m_convexConservativeDistanceThreshold(0.0f)
        {
 
        }
@@ -64,7 +62,6 @@ struct btDispatcherInfo
        btScalar        m_allowedCcdPenetration;
        bool            m_useConvexConservativeDistanceUtil;
        btScalar        m_convexConservativeDistanceThreshold;
-       btStackAlloc*   m_stackAllocator;
 };
 
 ///The btDispatcher interface class can be used in combination with broadphase to dispatch calculations for overlapping pairs.
index 041bbe05ae202c34c8576aea9e00efbc00e12418..ae22dadc73ac9ec445ce35569df7abdd01dc1f90 100644 (file)
@@ -53,7 +53,7 @@ btHashedOverlappingPairCache::~btHashedOverlappingPairCache()
 
 void   btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher)
 {
-       if (pair.m_algorithm)
+       if (pair.m_algorithm && dispatcher)
        {
                {
                        pair.m_algorithm->~btCollisionAlgorithm();
index 7a3806c1d2855b865e4178e74a95e18b21d8d479..eee90e473a9488aded22873e56c15a00b1ec67da 100644 (file)
@@ -96,6 +96,12 @@ class btHashedOverlappingPairCache : public btOverlappingPairCache
        btOverlapFilterCallback* m_overlapFilterCallback;
        bool            m_blockedForChanges;
 
+protected:
+       
+       btAlignedObjectArray<int>       m_hashTable;
+       btAlignedObjectArray<int>       m_next;
+       btOverlappingPairCallback*      m_ghostPairCallback;
+
 
 public:
        btHashedOverlappingPairCache();
@@ -265,11 +271,6 @@ private:
        virtual void    sortOverlappingPairs(btDispatcher* dispatcher);
        
 
-protected:
-       
-       btAlignedObjectArray<int>       m_hashTable;
-       btAlignedObjectArray<int>       m_next;
-       btOverlappingPairCallback*      m_ghostPairCallback;
        
 };
 
index f63e0923b783f938d133ace270497d59943f5bcd..66949849448e39c6fe9e5a00196101d4f149dfdf 100644 (file)
@@ -18,7 +18,6 @@ subject to the following restrictions:
 
 struct btCollisionAlgorithmCreateFunc;
 
-class btStackAlloc;
 class btPoolAllocator;
 
 ///btCollisionConfiguration allows to configure Bullet collision detection
@@ -38,7 +37,6 @@ public:
 
        virtual btPoolAllocator* getCollisionAlgorithmPool() = 0;
 
-       virtual btStackAlloc*   getStackAllocator() = 0;
 
        virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0;
 
index cf8ed59a541489d88b0ee06d5610432bdf0e5bea..d0924100058c3de611773e907ecb7d2b8cf88c83 100644 (file)
@@ -38,7 +38,8 @@ btCollisionObject::btCollisionObject()
                m_hitFraction(btScalar(1.)),
                m_ccdSweptSphereRadius(btScalar(0.)),
                m_ccdMotionThreshold(btScalar(0.)),
-               m_checkCollideWith(false)
+               m_checkCollideWith(false),
+               m_updateRevision(0)
 {
        m_worldTransform.setIdentity();
 }
index 2f17967fe0f06a5e056d2ddfcae5b893e6784495..89cad16821093c14d1c56aa9464502220deee09e 100644 (file)
@@ -92,7 +92,11 @@ protected:
        int                             m_internalType;
 
        ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
-       void*                   m_userObjectPointer;
+       union
+       {
+               void*                   m_userObjectPointer;
+               int     m_userIndex;
+       };
 
        ///time of impact calculation
        btScalar                m_hitFraction; 
@@ -106,6 +110,9 @@ protected:
        /// If some object should have elaborate collision filtering by sub-classes
        int                     m_checkCollideWith;
 
+       ///internal update revision number. It will be increased when the object changes. This allows some subsystems to perform lazy evaluation.
+       int                     m_updateRevision;
+
        virtual bool    checkCollideWithOverride(const btCollisionObject* /* co */) const
        {
                return true;
@@ -135,7 +142,8 @@ public:
                CO_GHOST_OBJECT=4,
                CO_SOFT_BODY=8,
                CO_HF_FLUID=16,
-               CO_USER_TYPE=32
+               CO_USER_TYPE=32,
+               CO_FEATHERSTONE_LINK=64
        };
 
        enum AnisotropicFrictionFlags
@@ -202,6 +210,7 @@ public:
 
        virtual void    setCollisionShape(btCollisionShape* collisionShape)
        {
+               m_updateRevision++;
                m_collisionShape = collisionShape;
                m_rootCollisionShape = collisionShape;
        }
@@ -257,6 +266,7 @@ public:
 
        void    setRestitution(btScalar rest)
        {
+               m_updateRevision++;
                m_restitution = rest;
        }
        btScalar        getRestitution() const
@@ -265,6 +275,7 @@ public:
        }
        void    setFriction(btScalar frict)
        {
+               m_updateRevision++;
                m_friction = frict;
        }
        btScalar        getFriction() const
@@ -274,6 +285,7 @@ public:
 
        void    setRollingFriction(btScalar frict)
        {
+               m_updateRevision++;
                m_rollingFriction = frict;
        }
        btScalar        getRollingFriction() const
@@ -300,6 +312,7 @@ public:
 
        void    setWorldTransform(const btTransform& worldTrans)
        {
+               m_updateRevision++;
                m_worldTransform = worldTrans;
        }
 
@@ -332,16 +345,19 @@ public:
 
        void    setInterpolationWorldTransform(const btTransform&       trans)
        {
+               m_updateRevision++;
                m_interpolationWorldTransform = trans;
        }
 
        void    setInterpolationLinearVelocity(const btVector3& linvel)
        {
+               m_updateRevision++;
                m_interpolationLinearVelocity = linvel;
        }
 
        void    setInterpolationAngularVelocity(const btVector3& angvel)
        {
+               m_updateRevision++;
                m_interpolationAngularVelocity = angvel;
        }
 
@@ -431,13 +447,28 @@ public:
        {
                return m_userObjectPointer;
        }
-       
+
+       int     getUserIndex() const
+       {
+               return m_userIndex;
+       }
        ///users can point to their objects, userPointer is not used by Bullet
        void    setUserPointer(void* userPointer)
        {
                m_userObjectPointer = userPointer;
        }
 
+       ///users can point to their objects, userPointer is not used by Bullet
+       void    setUserIndex(int index)
+       {
+               m_userIndex = index;
+       }
+
+       int     getUpdateRevisionInternal() const
+       {
+               return m_updateRevision;
+       }
+
 
        inline bool checkCollideWith(const btCollisionObject* co) const
        {
index 4c09291692d19a1d10fb42e360a35ccccc0b296b..093c6f9b200daad5c392f63518c22313770e18cd 100644 (file)
@@ -31,11 +31,10 @@ subject to the following restrictions:
 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
 #include "LinearMath/btAabbUtil2.h"
 #include "LinearMath/btQuickprof.h"
-#include "LinearMath/btStackAlloc.h"
 #include "LinearMath/btSerializer.h"
 #include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
-
+#include "BulletCollision/Gimpact/btGImpactShape.h"
 //#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
 
 
@@ -73,8 +72,6 @@ m_broadphasePairCache(pairCache),
 m_debugDrawer(0),
 m_forceUpdateAllAabbs(true)
 {
-       m_stackAlloc = collisionConfiguration->getStackAllocator();
-       m_dispatchInfo.m_stackAllocator = m_stackAlloc;
 }
 
 
@@ -290,13 +287,19 @@ void      btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
 
                btConvexShape* convexShape = (btConvexShape*) collisionShape;
                btVoronoiSimplexSolver  simplexSolver;
-#define USE_SUBSIMPLEX_CONVEX_CAST 1
-#ifdef USE_SUBSIMPLEX_CONVEX_CAST
-               btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver);
-#else
-               //btGjkConvexCast       convexCaster(castShape,convexShape,&simplexSolver);
+               btSubsimplexConvexCast subSimplexConvexCaster(castShape,convexShape,&simplexSolver);
+               
+               btGjkConvexCast gjkConvexCaster(castShape,convexShape,&simplexSolver);
+               
                //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
-#endif //#USE_SUBSIMPLEX_CONVEX_CAST
+               bool condition = true;
+               btConvexCast* convexCasterPtr = 0;
+               if (resultCallback.m_flags & btTriangleRaycastCallback::kF_UseSubSimplexConvexCastRaytest)
+                       convexCasterPtr = &subSimplexConvexCaster;
+               else
+                       convexCasterPtr = &gjkConvexCaster;
+               
+               btConvexCast& convexCaster = *convexCasterPtr;
 
                if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
                {
@@ -328,34 +331,26 @@ void      btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
        } else {
                if (collisionShape->isConcave())
                {
-                       //                      BT_PROFILE("rayTestConcave");
-                       if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
-                       {
-                               ///optimized version for btBvhTriangleMeshShape
-                               btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
-                               btTransform worldTocollisionObject = colObjWorldTransform.inverse();
-                               btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
-                               btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
 
-                               //ConvexCast::CastResult
+                       //ConvexCast::CastResult
                                struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback
                                {
                                        btCollisionWorld::RayResultCallback* m_resultCallback;
                                        const btCollisionObject*        m_collisionObject;
-                                       btTriangleMeshShape*    m_triangleMesh;
+                                       const btConcaveShape*   m_triangleMesh;
 
                                        btTransform m_colObjWorldTransform;
 
                                        BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
-                                               btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,btTriangleMeshShape*      triangleMesh,const btTransform& colObjWorldTransform):
-                                       //@BP Mod
-                                       btTriangleRaycastCallback(from,to, resultCallback->m_flags),
-                                               m_resultCallback(resultCallback),
-                                               m_collisionObject(collisionObject),
-                                               m_triangleMesh(triangleMesh),
-                                               m_colObjWorldTransform(colObjWorldTransform)
-                                       {
-                                       }
+                                       btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,const btConcaveShape*     triangleMesh,const btTransform& colObjWorldTransform):
+                                               //@BP Mod
+                                               btTriangleRaycastCallback(from,to, resultCallback->m_flags),
+                                                       m_resultCallback(resultCallback),
+                                                       m_collisionObject(collisionObject),
+                                                       m_triangleMesh(triangleMesh),
+                                                       m_colObjWorldTransform(colObjWorldTransform)
+                                               {
+                                               }
 
 
                                        virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
@@ -378,10 +373,28 @@ void      btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con
 
                                };
 
+                       btTransform worldTocollisionObject = colObjWorldTransform.inverse();
+                       btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
+                       btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
+
+                       //                      BT_PROFILE("rayTestConcave");
+                       if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
+                       {
+                               ///optimized version for btBvhTriangleMeshShape
+                               btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
+                               
                                BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),triangleMesh,colObjWorldTransform);
                                rcb.m_hitFraction = resultCallback.m_closestHitFraction;
                                triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
-                       } else
+                       }
+                       else if(collisionShape->getShapeType()==GIMPACT_SHAPE_PROXYTYPE)
+                       {
+                               btGImpactMeshShape* concaveShape = (btGImpactMeshShape*)collisionShape;
+
+                               BridgeTriangleRaycastCallback   rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),concaveShape, colObjWorldTransform);
+                               rcb.m_hitFraction = resultCallback.m_closestHitFraction;
+                               concaveShape->processAllTrianglesRay(&rcb,rayFromLocal,rayToLocal);
+                       }else
                        {
                                //generic (slower) case
                                btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
index 9412242e8a37742aa0ea1ccf69e4ba02466edfc3..b3fffdecd98ef01b636c9d7355c3bfb614a491fe 100644 (file)
@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://bulletphysics.com/Bullet/
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
@@ -18,13 +18,11 @@ subject to the following restrictions:
  * @mainpage Bullet Documentation
  *
  * @section intro_sec Introduction
- * Bullet Collision Detection & Physics SDK
- *
  * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ).
  *
  * The main documentation is Bullet_User_Manual.pdf, included in the source code distribution.
  * There is the Physics Forum for feedback and general Collision Detection and Physics discussions.
- * Please visit http://www.bulletphysics.com
+ * Please visit http://www.bulletphysics.org
  *
  * @section install_sec Installation
  *
@@ -32,7 +30,16 @@ subject to the following restrictions:
  * You can download the Bullet Physics Library from the Google Code repository: http://code.google.com/p/bullet/downloads/list
  *
  * @subsection step2 Step 2: Building
- * Bullet main build system for all platforms is cmake, you can download http://www.cmake.org
+ * Bullet has multiple build systems, including premake, cmake and autotools. Premake and cmake support all platforms.
+ * Premake is included in the Bullet/build folder for Windows, Mac OSX and Linux. 
+ * Under Windows you can click on Bullet/build/vs2010.bat to create Microsoft Visual Studio projects. 
+ * On Mac OSX and Linux you can open a terminal and generate Makefile, codeblocks or Xcode4 projects:
+ * cd Bullet/build
+ * ./premake4_osx gmake or ./premake4_linux gmake or ./premake4_linux64 gmake or (for Mac) ./premake4_osx xcode4
+ * cd Bullet/build/gmake
+ * make
+ * 
+ * An alternative to premake is cmake. You can download cmake from http://www.cmake.org
  * cmake can autogenerate projectfiles for Microsoft Visual Studio, Apple Xcode, KDevelop and Unix Makefiles.
  * The easiest is to run the CMake cmake-gui graphical user interface and choose the options and generate projectfiles.
  * You can also use cmake in the command-line. Here are some examples for various platforms:
@@ -65,7 +72,6 @@ subject to the following restrictions:
 #ifndef BT_COLLISION_WORLD_H
 #define BT_COLLISION_WORLD_H
 
-class btStackAlloc;
 class btCollisionShape;
 class btConvexShape;
 class btBroadphaseInterface;
@@ -91,8 +97,6 @@ protected:
 
        btDispatcherInfo        m_dispatchInfo;
 
-       btStackAlloc*   m_stackAlloc;
-
        btBroadphaseInterface*  m_broadphasePairCache;
 
        btIDebugDraw*   m_debugDrawer;
index 39b86a289183c52563abe3bbaa2e1b94b28e6bc6..991841ee20be84c2095568f1a379f2131d518f0b 100644 (file)
@@ -11,6 +11,7 @@ subject to the following restrictions:
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
 3. This notice may not be removed or altered from any source distribution.
+
 */
 
 #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
@@ -22,6 +23,8 @@ subject to the following restrictions:
 #include "btManifoldResult.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
 
+btShapePairCallback gCompoundChildShapePairCallback = 0;
+
 btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
 :btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
 m_isSwapped(isSwapped),
@@ -129,6 +132,12 @@ public:
                childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
                m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1);
 
+               if (gCompoundChildShapePairCallback)
+               {
+                       if (!gCompoundChildShapePairCallback(m_otherObjWrap->getCollisionShape(), childShape))
+                               return;
+               }
+
                if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
                {
 
index b16fc524672c9299473c173409a331d266830354..53675145637d9dc4c8fa58c9405263e9d8c2fe20 100644 (file)
@@ -11,6 +11,7 @@ subject to the following restrictions:
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
 3. This notice may not be removed or altered from any source distribution.
+
 */
 
 #ifndef BT_COMPOUND_COLLISION_ALGORITHM_H
@@ -28,6 +29,10 @@ class btDispatcher;
 class btDispatcher;
 class btCollisionObject;
 
+class btCollisionShape;
+typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCollisionShape* pShape1);
+extern btShapePairCallback gCompoundChildShapePairCallback;
+
 /// btCompoundCollisionAlgorithm  supports collision between CompoundCollisionShapes and other collision shapes
 class btCompoundCollisionAlgorithm  : public btActivatingCollisionAlgorithm
 {
@@ -37,6 +42,7 @@ class btCompoundCollisionAlgorithm  : public btActivatingCollisionAlgorithm
        class btPersistentManifold*     m_sharedManifold;
        bool                                    m_ownsManifold;
 
+
        int     m_compoundShapeRevision;//to keep track of changes, so that childAlgorithm array can be updated
        
        void    removeChildAlgorithms();
@@ -49,6 +55,12 @@ public:
 
        virtual ~btCompoundCollisionAlgorithm();
 
+       btCollisionAlgorithm* getChildAlgorithm (int n) const
+       {
+               return m_childCollisionAlgorithms[n];
+       }
+
+
        virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
        btScalar        calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
@@ -63,6 +75,7 @@ public:
                }
        }
 
+       
        struct CreateFunc :public       btCollisionAlgorithmCreateFunc
        {
                virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
new file mode 100644 (file)
index 0000000..a52dd34
--- /dev/null
@@ -0,0 +1,421 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+*/
+
+#include "btCompoundCompoundCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btAabbUtil2.h"
+#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
+
+
+btShapePairCallback gCompoundCompoundChildShapePairCallback = 0;
+
+btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
+:btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
+m_sharedManifold(ci.m_manifold)
+{
+       m_ownsManifold = false;
+
+       void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16);
+       m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache();
+
+       const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
+       btAssert (col0ObjWrap->getCollisionShape()->isCompound());
+
+       const btCollisionObjectWrapper* col1ObjWrap = body1Wrap;
+       btAssert (col1ObjWrap->getCollisionShape()->isCompound());
+       
+       const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
+       m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
+
+       const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
+       m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
+       
+       
+}
+
+
+btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm()
+{
+       removeChildAlgorithms();
+       m_childCollisionAlgorithmCache->~btHashedSimplePairCache();
+       btAlignedFree(m_childCollisionAlgorithmCache);
+}
+
+void   btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray&   manifoldArray)
+{
+       int i;
+       btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
+       for (i=0;i<pairs.size();i++)
+       {
+               if (pairs[i].m_userPointer)
+               {
+                       
+                       ((btCollisionAlgorithm*)pairs[i].m_userPointer)->getAllContactManifolds(manifoldArray);
+               }
+       }
+}
+
+
+void   btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms()
+{
+       btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
+
+       int numChildren = pairs.size();
+       int i;
+       for (i=0;i<numChildren;i++)
+       {
+               if (pairs[i].m_userPointer)
+               {
+                       btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer;
+                       algo->~btCollisionAlgorithm();
+                       m_dispatcher->freeCollisionAlgorithm(algo);
+               }
+       }
+       m_childCollisionAlgorithmCache->removeAllPairs();
+}
+
+struct btCompoundCompoundLeafCallback : btDbvt::ICollide
+{
+       int m_numOverlapPairs;
+
+
+       const btCollisionObjectWrapper* m_compound0ColObjWrap;
+       const btCollisionObjectWrapper* m_compound1ColObjWrap;
+       btDispatcher* m_dispatcher;
+       const btDispatcherInfo& m_dispatchInfo;
+       btManifoldResult*       m_resultOut;
+       
+       
+       class btHashedSimplePairCache*  m_childCollisionAlgorithmCache;
+       
+       btPersistentManifold*   m_sharedManifold;
+       
+       btCompoundCompoundLeafCallback (const btCollisionObjectWrapper* compound1ObjWrap,
+                                                                       const btCollisionObjectWrapper* compound0ObjWrap,
+                                                                       btDispatcher* dispatcher,
+                                                                       const btDispatcherInfo& dispatchInfo,
+                                                                       btManifoldResult*       resultOut,
+                                                                       btHashedSimplePairCache* childAlgorithmsCache,
+                                                                       btPersistentManifold*   sharedManifold)
+               :m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
+               m_childCollisionAlgorithmCache(childAlgorithmsCache),
+               m_sharedManifold(sharedManifold),
+               m_numOverlapPairs(0)
+       {
+
+       }
+
+
+
+       
+       void            Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1)
+       {
+               m_numOverlapPairs++;
+
+
+               int childIndex0 = leaf0->dataAsInt;
+               int childIndex1 = leaf1->dataAsInt;
+               
+
+               btAssert(childIndex0>=0);
+               btAssert(childIndex1>=0);
+
+
+               const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(m_compound0ColObjWrap->getCollisionShape());
+               btAssert(childIndex0<compoundShape0->getNumChildShapes());
+
+               const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(m_compound1ColObjWrap->getCollisionShape());
+               btAssert(childIndex1<compoundShape1->getNumChildShapes());
+
+               const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0);
+               const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1);
+
+               //backup
+               btTransform     orgTrans0 = m_compound0ColObjWrap->getWorldTransform();
+               const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0);
+               btTransform     newChildWorldTrans0 = orgTrans0*childTrans0 ;
+               
+               btTransform     orgTrans1 = m_compound1ColObjWrap->getWorldTransform();
+               const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1);
+               btTransform     newChildWorldTrans1 = orgTrans1*childTrans1 ;
+               
+
+               //perform an AABB check first
+               btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
+               childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
+               childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
+               
+               if (gCompoundCompoundChildShapePairCallback)
+               {
+                       if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1))
+                               return;
+               }
+
+               if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+               {
+                       btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap,childShape0, m_compound0ColObjWrap->getCollisionObject(),newChildWorldTrans0,-1,childIndex0);
+                       btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap,childShape1,m_compound1ColObjWrap->getCollisionObject(),newChildWorldTrans1,-1,childIndex1);
+                       
+
+                       btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1);
+
+                       btCollisionAlgorithm* colAlgo = 0;
+
+                       if (pair)
+                       {
+                               colAlgo = (btCollisionAlgorithm*)pair->m_userPointer;
+                               
+                       } else
+                       {
+                               colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold);
+                               pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1);
+                               btAssert(pair);
+                               pair->m_userPointer = colAlgo;
+                       }
+
+                       btAssert(colAlgo);
+                                               
+                       const btCollisionObjectWrapper* tmpWrap0 = 0;
+                       const btCollisionObjectWrapper* tmpWrap1 = 0;
+
+                       tmpWrap0 = m_resultOut->getBody0Wrap();
+                       tmpWrap1 = m_resultOut->getBody1Wrap();
+
+                       m_resultOut->setBody0Wrap(&compoundWrap0);
+                       m_resultOut->setBody1Wrap(&compoundWrap1);
+
+                       m_resultOut->setShapeIdentifiersA(-1,childIndex0);
+                       m_resultOut->setShapeIdentifiersB(-1,childIndex1);
+
+
+                       colAlgo->processCollision(&compoundWrap0,&compoundWrap1,m_dispatchInfo,m_resultOut);
+                       
+                       m_resultOut->setBody0Wrap(tmpWrap0);
+                       m_resultOut->setBody1Wrap(tmpWrap1);
+                       
+
+
+               }
+       }
+};
+
+
+static DBVT_INLINE bool                MyIntersect(    const btDbvtAabbMm& a,
+                                                                 const btDbvtAabbMm& b, const btTransform& xform)
+{
+       btVector3 newmin,newmax;
+       btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax);
+       btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax);
+       return Intersect(a,newb);
+}
+
+
+static inline void             MycollideTT(    const btDbvtNode* root0,
+                                                                 const btDbvtNode* root1,
+                                                                 const btTransform& xform,
+                                                                 btCompoundCompoundLeafCallback* callback)
+{
+
+               if(root0&&root1)
+               {
+                       int                                                             depth=1;
+                       int                                                             treshold=btDbvt::DOUBLE_STACKSIZE-4;
+                       btAlignedObjectArray<btDbvt::sStkNN>    stkStack;
+                       stkStack.resize(btDbvt::DOUBLE_STACKSIZE);
+                       stkStack[0]=btDbvt::sStkNN(root0,root1);
+                       do      {
+                               btDbvt::sStkNN  p=stkStack[--depth];
+                               if(MyIntersect(p.a->volume,p.b->volume,xform))
+                               {
+                                       if(depth>treshold)
+                                       {
+                                               stkStack.resize(stkStack.size()*2);
+                                               treshold=stkStack.size()-4;
+                                       }
+                                       if(p.a->isinternal())
+                                       {
+                                               if(p.b->isinternal())
+                                               {                                       
+                                                       stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[0]);
+                                                       stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[0]);
+                                                       stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[1]);
+                                                       stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[1]);
+                                               }
+                                               else
+                                               {
+                                                       stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b);
+                                                       stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b);
+                                               }
+                                       }
+                                       else
+                                       {
+                                               if(p.b->isinternal())
+                                               {
+                                                       stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[0]);
+                                                       stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[1]);
+                                               }
+                                               else
+                                               {
+                                                       callback->Process(p.a,p.b);
+                                               }
+                                       }
+                               }
+                       } while(depth);
+               }
+}
+
+void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+
+       const btCollisionObjectWrapper* col0ObjWrap = body0Wrap;
+       const btCollisionObjectWrapper* col1ObjWrap= body1Wrap;
+
+       btAssert (col0ObjWrap->getCollisionShape()->isCompound());
+       btAssert (col1ObjWrap->getCollisionShape()->isCompound());
+       const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
+       const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
+
+       ///btCompoundShape might have changed:
+       ////make sure the internal child collision algorithm caches are still valid
+       if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1))
+       {
+               ///clear all
+               removeChildAlgorithms();
+       }
+
+
+       ///we need to refresh all contact manifolds
+       ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
+       ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
+       {
+               int i;
+               btManifoldArray manifoldArray;
+               btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
+               for (i=0;i<pairs.size();i++)
+               {
+                       if (pairs[i].m_userPointer)
+                       {
+                               btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer;
+                               algo->getAllContactManifolds(manifoldArray);
+                               for (int m=0;m<manifoldArray.size();m++)
+                               {
+                                       if (manifoldArray[m]->getNumContacts())
+                                       {
+                                               resultOut->setPersistentManifold(manifoldArray[m]);
+                                               resultOut->refreshContactPoints();
+                                               resultOut->setPersistentManifold(0);
+                                       }
+                               }
+                               manifoldArray.resize(0);
+                       }
+               }
+       }
+
+
+       const btDbvt* tree0 = compoundShape0->getDynamicAabbTree();
+       const btDbvt* tree1 = compoundShape1->getDynamicAabbTree();
+
+       btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold);
+
+
+       const btTransform       xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform();
+       MycollideTT(tree0->m_root,tree1->m_root,xform,&callback);
+
+       //printf("#compound-compound child/leaf overlap =%d                      \r",callback.m_numOverlapPairs);
+
+       //remove non-overlapping child pairs
+
+       {
+               btAssert(m_removePairs.size()==0);
+
+               //iterate over all children, perform an AABB check inside ProcessChildShape
+               btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
+               
+               int i;
+               btManifoldArray manifoldArray;
+        
+               
+
+        
+        
+        btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;        
+        
+               for (i=0;i<pairs.size();i++)
+               {
+                       if (pairs[i].m_userPointer)
+                       {
+                               btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer;
+
+                               {
+                                       btTransform     orgTrans0;
+                                       const btCollisionShape* childShape0 = 0;
+                                       
+                                       btTransform     newChildWorldTrans0;
+                                       btTransform     orgInterpolationTrans0;
+                                       childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA);
+                                       orgTrans0 = col0ObjWrap->getWorldTransform();
+                                       orgInterpolationTrans0 = col0ObjWrap->getWorldTransform();
+                                       const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA);
+                                       newChildWorldTrans0 = orgTrans0*childTrans0 ;
+                                       childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0);
+                               }
+
+                               {
+                                       btTransform     orgInterpolationTrans1;
+                                       const btCollisionShape* childShape1 = 0;
+                                       btTransform     orgTrans1;
+                                       btTransform     newChildWorldTrans1;
+
+                                       childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB);
+                                       orgTrans1 = col1ObjWrap->getWorldTransform();
+                                       orgInterpolationTrans1 = col1ObjWrap->getWorldTransform();
+                                       const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB);
+                                       newChildWorldTrans1 = orgTrans1*childTrans1 ;
+                                       childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1);
+                               }
+                               
+                               
+
+                               if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+                               {
+                                       algo->~btCollisionAlgorithm();
+                                       m_dispatcher->freeCollisionAlgorithm(algo);
+                                       m_removePairs.push_back(btSimplePair(pairs[i].m_indexA,pairs[i].m_indexB));
+                               }
+                       }
+               }
+               for (int i=0;i<m_removePairs.size();i++)
+               {
+                       m_childCollisionAlgorithmCache->removeOverlappingPair(m_removePairs[i].m_indexA,m_removePairs[i].m_indexB);
+               }
+               m_removePairs.clear();
+       }
+
+}
+
+btScalar       btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+       btAssert(0);
+       return 0.f;
+
+}
+
+
+
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h
new file mode 100644 (file)
index 0000000..7e2d7ad
--- /dev/null
@@ -0,0 +1,90 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+*/
+
+#ifndef BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H
+#define BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H
+
+#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+class btDispatcher;
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "BulletCollision/CollisionDispatch/btHashedSimplePairCache.h"
+class btDispatcher;
+class btCollisionObject;
+
+class btCollisionShape;
+typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCollisionShape* pShape1);
+extern btShapePairCallback gCompoundCompoundChildShapePairCallback;
+
+/// btCompoundCompoundCollisionAlgorithm  supports collision between two btCompoundCollisionShape shapes
+class btCompoundCompoundCollisionAlgorithm  : public btActivatingCollisionAlgorithm
+{
+
+       class btHashedSimplePairCache*  m_childCollisionAlgorithmCache;
+       btSimplePairArray m_removePairs;
+
+       class btPersistentManifold*     m_sharedManifold;
+       bool                                    m_ownsManifold;
+
+
+       int     m_compoundShapeRevision0;//to keep track of changes, so that childAlgorithm array can be updated
+       int     m_compoundShapeRevision1;
+       
+       void    removeChildAlgorithms();
+       
+//     void    preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
+
+public:
+
+       btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped);
+
+       virtual ~btCompoundCompoundCollisionAlgorithm();
+
+       
+
+       virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+       btScalar        calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
+       virtual void    getAllContactManifolds(btManifoldArray& manifoldArray);
+       
+       
+       struct CreateFunc :public       btCollisionAlgorithmCreateFunc
+       {
+               virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+               {
+                       void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm));
+                       return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,false);
+               }
+       };
+
+       struct SwappedCreateFunc :public        btCollisionAlgorithmCreateFunc
+       {
+               virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+               {
+                       void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm));
+                       return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,true);
+               }
+       };
+
+};
+
+#endif //BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H
index 3e1afede1bf6bf313fcb42af657fae222a714a2a..4ec9ae713862284239f4d7b503f9a1e22b218362 100644 (file)
@@ -132,7 +132,6 @@ void btConvex2dConvex2dAlgorithm ::processCollision (const btCollisionObjectWrap
                        input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
                }
 
-               input.m_stackAlloc = dispatchInfo.m_stackAllocator;
                input.m_transformA = body0Wrap->getWorldTransform();
                input.m_transformB = body1Wrap->getWorldTransform();
 
index 18fde771b14773e6c7fa5cc10a79729dc43f0e6a..e23f5f7a88d688090e410ca1dddec95f37b39b40 100644 (file)
@@ -76,21 +76,27 @@ void        btConvexTriangleCallback::clearCache()
 }
 
 
-
-void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
+void btConvexTriangleCallback::processTriangle(btVector3* triangle,int
+partId, int triangleIndex)
 {
-       //just for debugging purposes
-       //printf("triangle %d",m_triangleCount++);
 
+       if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax))
+       {
+               return;
+       }
+
+        //just for debugging purposes
+        //printf("triangle %d",m_triangleCount++);
 
-       //aabb filter is already applied!       
+        const btCollisionObject* ob = const_cast<btCollisionObject*>(m_triBodyWrap->getCollisionObject());
 
        btCollisionAlgorithmConstructionInfo ci;
        ci.m_dispatcher1 = m_dispatcher;
 
        //const btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBodyWrap->getCollisionObject());
 
+       
+
 
 #if 0  
        ///debug drawing of the overlapping triangles
index 62f98a846f4503e73768e89e69f4e032006aefb2..7f2722aa463c6547a9b2b9161be12325e8668edb 100644 (file)
@@ -373,7 +373,6 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
                input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
        }
 
-       input.m_stackAlloc = dispatchInfo.m_stackAllocator;
        input.m_transformA = body0Wrap->getWorldTransform();
        input.m_transformB = body1Wrap->getWorldTransform();
 
index 7faee6faf50c307fdd319e34d178ff04d7e77ac6..c3cacec4a4f6d391a0edaddeac258defd5eb255d 100644 (file)
@@ -19,6 +19,8 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
+#include "BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h"
+
 #include "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
@@ -32,7 +34,6 @@ subject to the following restrictions:
 
 
 
-#include "LinearMath/btStackAlloc.h"
 #include "LinearMath/btPoolAllocator.h"
 
 
@@ -65,6 +66,10 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault
        m_swappedConvexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::SwappedCreateFunc;
        mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::CreateFunc),16);
        m_compoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::CreateFunc;
+
+       mem = btAlignedAlloc(sizeof(btCompoundCompoundCollisionAlgorithm::CreateFunc),16);
+       m_compoundCompoundCreateFunc = new (mem)btCompoundCompoundCollisionAlgorithm::CreateFunc;
+
        mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc),16);
        m_swappedCompoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::SwappedCreateFunc;
        mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc),16);
@@ -106,16 +111,6 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault
        collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2);
        collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3);
 
-       if (constructionInfo.m_stackAlloc)
-       {
-               m_ownsStackAllocator = false;
-               this->m_stackAlloc = constructionInfo.m_stackAlloc;
-       } else
-       {
-               m_ownsStackAllocator = true;
-               void* mem = btAlignedAlloc(sizeof(btStackAlloc),16);
-               m_stackAlloc = new(mem)btStackAlloc(constructionInfo.m_defaultStackAllocatorSize);
-       }
                
        if (constructionInfo.m_persistentManifoldPool)
        {
@@ -144,12 +139,6 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault
 
 btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration()
 {
-       if (m_ownsStackAllocator)
-       {
-               m_stackAlloc->destroy();
-               m_stackAlloc->~btStackAlloc();
-               btAlignedFree(m_stackAlloc);
-       }
        if (m_ownsCollisionAlgorithmPool)
        {
                m_collisionAlgorithmPool->~btPoolAllocator();
@@ -172,6 +161,9 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration()
        m_compoundCreateFunc->~btCollisionAlgorithmCreateFunc();
        btAlignedFree( m_compoundCreateFunc);
 
+       m_compoundCompoundCreateFunc->~btCollisionAlgorithmCreateFunc();
+       btAlignedFree(m_compoundCompoundCreateFunc);
+
        m_swappedCompoundCreateFunc->~btCollisionAlgorithmCreateFunc();
        btAlignedFree( m_swappedCompoundCreateFunc);
 
@@ -275,6 +267,12 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg
                return m_swappedConvexConcaveCreateFunc;
        }
 
+
+       if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1))
+       {
+               return m_compoundCompoundCreateFunc;
+       }
+
        if (btBroadphaseProxy::isCompound(proxyType0))
        {
                return m_compoundCreateFunc;
index 474785bfc7d089b88ea9276421ad9393e99ea164..2078420e19891b8858d1b0ce637f6f173837c0d6 100644 (file)
@@ -22,23 +22,19 @@ class btConvexPenetrationDepthSolver;
 
 struct btDefaultCollisionConstructionInfo
 {
-       btStackAlloc*           m_stackAlloc;
        btPoolAllocator*        m_persistentManifoldPool;
        btPoolAllocator*        m_collisionAlgorithmPool;
        int                                     m_defaultMaxPersistentManifoldPoolSize;
        int                                     m_defaultMaxCollisionAlgorithmPoolSize;
        int                                     m_customCollisionAlgorithmMaxElementSize;
-       int                                     m_defaultStackAllocatorSize;
        int                                     m_useEpaPenetrationAlgorithm;
 
        btDefaultCollisionConstructionInfo()
-               :m_stackAlloc(0),
-               m_persistentManifoldPool(0),
+               :m_persistentManifoldPool(0),
                m_collisionAlgorithmPool(0),
                m_defaultMaxPersistentManifoldPoolSize(4096),
                m_defaultMaxCollisionAlgorithmPoolSize(4096),
                m_customCollisionAlgorithmMaxElementSize(0),
-               m_defaultStackAllocatorSize(0),
                m_useEpaPenetrationAlgorithm(true)
        {
        }
@@ -56,8 +52,6 @@ protected:
 
        int     m_persistentManifoldPoolSize;
        
-       btStackAlloc*   m_stackAlloc;
-       bool    m_ownsStackAllocator;
 
        btPoolAllocator*        m_persistentManifoldPool;
        bool    m_ownsPersistentManifoldPool;
@@ -75,6 +69,8 @@ protected:
        btCollisionAlgorithmCreateFunc* m_convexConcaveCreateFunc;
        btCollisionAlgorithmCreateFunc* m_swappedConvexConcaveCreateFunc;
        btCollisionAlgorithmCreateFunc* m_compoundCreateFunc;
+       btCollisionAlgorithmCreateFunc* m_compoundCompoundCreateFunc;
+       
        btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc;
        btCollisionAlgorithmCreateFunc* m_emptyCreateFunc;
        btCollisionAlgorithmCreateFunc* m_sphereSphereCF;
@@ -105,10 +101,6 @@ public:
                return m_collisionAlgorithmPool;
        }
 
-       virtual btStackAlloc*   getStackAllocator()
-       {
-               return m_stackAlloc;
-       }
 
        virtual btVoronoiSimplexSolver* getSimplexSolver()
        {
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp
new file mode 100644 (file)
index 0000000..cfcca56
--- /dev/null
@@ -0,0 +1,278 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "btHashedSimplePairCache.h"
+
+
+#include <stdio.h>
+
+int    gOverlappingSimplePairs = 0;
+int gRemoveSimplePairs =0;
+int gAddedSimplePairs =0;
+int gFindSimplePairs =0;
+
+
+
+
+btHashedSimplePairCache::btHashedSimplePairCache():
+       m_blockedForChanges(false)
+{
+       int initialAllocatedSize= 2;
+       m_overlappingPairArray.reserve(initialAllocatedSize);
+       growTables();
+}
+
+
+
+
+btHashedSimplePairCache::~btHashedSimplePairCache()
+{
+}
+
+
+
+
+
+
+void btHashedSimplePairCache::removeAllPairs()
+{
+       m_overlappingPairArray.clear();
+       m_hashTable.clear();
+       m_next.clear();
+
+       int initialAllocatedSize= 2;
+       m_overlappingPairArray.reserve(initialAllocatedSize);
+       growTables();
+}
+
+
+
+btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB)
+{
+       gFindSimplePairs++;
+       
+       
+       /*if (indexA > indexB) 
+               btSwap(indexA, indexB);*/
+
+       int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));
+
+       if (hash >= m_hashTable.size())
+       {
+               return NULL;
+       }
+
+       int index = m_hashTable[hash];
+       while (index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], indexA, indexB) == false)
+       {
+               index = m_next[index];
+       }
+
+       if (index == BT_SIMPLE_NULL_PAIR)
+       {
+               return NULL;
+       }
+
+       btAssert(index < m_overlappingPairArray.size());
+
+       return &m_overlappingPairArray[index];
+}
+
+//#include <stdio.h>
+
+void   btHashedSimplePairCache::growTables()
+{
+
+       int newCapacity = m_overlappingPairArray.capacity();
+
+       if (m_hashTable.size() < newCapacity)
+       {
+               //grow hashtable and next table
+               int curHashtableSize = m_hashTable.size();
+
+               m_hashTable.resize(newCapacity);
+               m_next.resize(newCapacity);
+
+
+               int i;
+
+               for (i= 0; i < newCapacity; ++i)
+               {
+                       m_hashTable[i] = BT_SIMPLE_NULL_PAIR;
+               }
+               for (i = 0; i < newCapacity; ++i)
+               {
+                       m_next[i] = BT_SIMPLE_NULL_PAIR;
+               }
+
+               for(i=0;i<curHashtableSize;i++)
+               {
+       
+                       const btSimplePair& pair = m_overlappingPairArray[i];
+                       int indexA = pair.m_indexA;
+                       int indexB = pair.m_indexB;
+                       
+                       int     hashValue = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));     // New hash value with new mask
+                       m_next[i] = m_hashTable[hashValue];
+                       m_hashTable[hashValue] = i;
+               }
+
+
+       }
+}
+
+btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB)
+{
+
+       int     hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));  // New hash value with new mask
+
+
+       btSimplePair* pair = internalFindPair(indexA, indexB, hash);
+       if (pair != NULL)
+       {
+               return pair;
+       }
+
+       int count = m_overlappingPairArray.size();
+       int oldCapacity = m_overlappingPairArray.capacity();
+       void* mem = &m_overlappingPairArray.expandNonInitializing();
+
+       int newCapacity = m_overlappingPairArray.capacity();
+
+       if (oldCapacity < newCapacity)
+       {
+               growTables();
+               //hash with new capacity
+               hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));
+       }
+       
+       pair = new (mem) btSimplePair(indexA,indexB);
+
+       pair->m_userPointer = 0;
+       
+       m_next[count] = m_hashTable[hash];
+       m_hashTable[hash] = count;
+
+       return pair;
+}
+
+
+
+void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB)
+{
+       gRemoveSimplePairs++;
+       
+
+       /*if (indexA > indexB) 
+               btSwap(indexA, indexB);*/
+
+       int     hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1));
+
+       btSimplePair* pair = internalFindPair(indexA, indexB, hash);
+       if (pair == NULL)
+       {
+               return 0;
+       }
+
+       
+       void* userData = pair->m_userPointer;
+
+
+       int pairIndex = int(pair - &m_overlappingPairArray[0]);
+       btAssert(pairIndex < m_overlappingPairArray.size());
+
+       // Remove the pair from the hash table.
+       int index = m_hashTable[hash];
+       btAssert(index != BT_SIMPLE_NULL_PAIR);
+
+       int previous = BT_SIMPLE_NULL_PAIR;
+       while (index != pairIndex)
+       {
+               previous = index;
+               index = m_next[index];
+       }
+
+       if (previous != BT_SIMPLE_NULL_PAIR)
+       {
+               btAssert(m_next[previous] == pairIndex);
+               m_next[previous] = m_next[pairIndex];
+       }
+       else
+       {
+               m_hashTable[hash] = m_next[pairIndex];
+       }
+
+       // We now move the last pair into spot of the
+       // pair being removed. We need to fix the hash
+       // table indices to support the move.
+
+       int lastPairIndex = m_overlappingPairArray.size() - 1;
+
+       // If the removed pair is the last pair, we are done.
+       if (lastPairIndex == pairIndex)
+       {
+               m_overlappingPairArray.pop_back();
+               return userData;
+       }
+
+       // Remove the last pair from the hash table.
+       const btSimplePair* last = &m_overlappingPairArray[lastPairIndex];
+               /* missing swap here too, Nat. */ 
+       int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_indexA), static_cast<unsigned int>(last->m_indexB)) & (m_overlappingPairArray.capacity()-1));
+
+       index = m_hashTable[lastHash];
+       btAssert(index != BT_SIMPLE_NULL_PAIR);
+
+       previous = BT_SIMPLE_NULL_PAIR;
+       while (index != lastPairIndex)
+       {
+               previous = index;
+               index = m_next[index];
+       }
+
+       if (previous != BT_SIMPLE_NULL_PAIR)
+       {
+               btAssert(m_next[previous] == lastPairIndex);
+               m_next[previous] = m_next[lastPairIndex];
+       }
+       else
+       {
+               m_hashTable[lastHash] = m_next[lastPairIndex];
+       }
+
+       // Copy the last pair into the remove pair's spot.
+       m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex];
+
+       // Insert the last pair into the hash table
+       m_next[pairIndex] = m_hashTable[lastHash];
+       m_hashTable[lastHash] = pairIndex;
+
+       m_overlappingPairArray.pop_back();
+
+       return userData;
+}
+//#include <stdio.h>
+
+
+
+
+
+
+
+
+
+
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h
new file mode 100644 (file)
index 0000000..e88ef97
--- /dev/null
@@ -0,0 +1,174 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_HASHED_SIMPLE_PAIR_CACHE_H
+#define BT_HASHED_SIMPLE_PAIR_CACHE_H
+
+
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+const int BT_SIMPLE_NULL_PAIR=0xffffffff;
+
+struct btSimplePair
+{
+       btSimplePair(int indexA,int indexB)
+               :m_indexA(indexA),
+               m_indexB(indexB),
+               m_userPointer(0)
+       {
+       }
+
+       int m_indexA;
+       int m_indexB;
+       union
+       {
+               void*   m_userPointer;
+               int             m_userValue;
+       };
+};
+
+typedef btAlignedObjectArray<btSimplePair>     btSimplePairArray;
+
+
+
+extern int gOverlappingSimplePairs;
+extern int gRemoveSimplePairs;
+extern int gAddedSimplePairs;
+extern int gFindSimplePairs;
+
+
+
+
+class btHashedSimplePairCache
+{
+       btSimplePairArray       m_overlappingPairArray;
+       
+       bool            m_blockedForChanges;
+       
+
+protected:
+       
+       btAlignedObjectArray<int>       m_hashTable;
+       btAlignedObjectArray<int>       m_next;
+       
+
+public:
+       btHashedSimplePairCache();
+       virtual ~btHashedSimplePairCache();
+       
+       void removeAllPairs();
+
+       virtual void*   removeOverlappingPair(int indexA,int indexB);
+       
+       // Add a pair and return the new pair. If the pair already exists,
+       // no new pair is created and the old one is returned.
+       virtual btSimplePair*   addOverlappingPair(int indexA,int indexB)
+       {
+               gAddedSimplePairs++;
+
+               return internalAddPair(indexA,indexB);
+       }
+
+       
+       virtual btSimplePair*   getOverlappingPairArrayPtr()
+       {
+               return &m_overlappingPairArray[0];
+       }
+
+       const btSimplePair*     getOverlappingPairArrayPtr() const
+       {
+               return &m_overlappingPairArray[0];
+       }
+
+       btSimplePairArray&      getOverlappingPairArray()
+       {
+               return m_overlappingPairArray;
+       }
+
+       const btSimplePairArray&        getOverlappingPairArray() const
+       {
+               return m_overlappingPairArray;
+       }
+
+       
+       btSimplePair* findPair(int indexA,int indexB);
+
+       int GetCount() const { return m_overlappingPairArray.size(); }
+
+       int     getNumOverlappingPairs() const
+       {
+               return m_overlappingPairArray.size();
+       }
+private:
+       
+       btSimplePair*   internalAddPair(int indexA, int indexB);
+
+       void    growTables();
+
+       SIMD_FORCE_INLINE bool equalsPair(const btSimplePair& pair, int indexA, int indexB)
+       {       
+               return pair.m_indexA == indexA && pair.m_indexB == indexB;
+       }
+
+       
+       
+       SIMD_FORCE_INLINE       unsigned int getHash(unsigned int indexA, unsigned int indexB)
+       {
+               int key = static_cast<int>(((unsigned int)indexA) | (((unsigned int)indexB) <<16));
+               // Thomas Wang's hash
+
+               key += ~(key << 15);
+               key ^=  (key >> 10);
+               key +=  (key << 3);
+               key ^=  (key >> 6);
+               key += ~(key << 11);
+               key ^=  (key >> 16);
+               return static_cast<unsigned int>(key);
+       }
+       
+
+
+
+
+       SIMD_FORCE_INLINE btSimplePair* internalFindPair(int proxyIdA , int proxyIdB, int hash)
+       {
+               
+               int index = m_hashTable[hash];
+               
+               while( index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyIdA, proxyIdB) == false)
+               {
+                       index = m_next[index];
+               }
+
+               if ( index == BT_SIMPLE_NULL_PAIR )
+               {
+                       return NULL;
+               }
+
+               btAssert(index < m_overlappingPairArray.size());
+
+               return &m_overlappingPairArray[index];
+       }
+
+       
+};
+
+
+
+
+#endif //BT_HASHED_SIMPLE_PAIR_CACHE_H
+
+
index 12f422f193fbae3b932239bb23e54db0d6c6cd59..0aa75f2bff3b037071ae2c318f81a2d6a0efe762 100644 (file)
@@ -273,6 +273,8 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf
 
 
 
+
+
 void btCompoundShape::setLocalScaling(const btVector3& scaling)
 {
 
@@ -283,7 +285,7 @@ void btCompoundShape::setLocalScaling(const btVector3& scaling)
 //             childScale = childScale * (childTrans.getBasis() * scaling);
                childScale = childScale * scaling / m_localScaling;
                m_children[i].m_childShape->setLocalScaling(childScale);
-               childTrans.setOrigin((childTrans.getOrigin())*scaling);
+               childTrans.setOrigin((childTrans.getOrigin()) * scaling / m_localScaling);
                updateChildTransform(i, childTrans,false);
        }
        
index 5e83087b3200f3d9cfe20e4e5d6e989bc30a1113..2d83c8bfbac5f8b93d4424c46b3c56618b686e37 100644 (file)
@@ -62,6 +62,10 @@ void btConeShape::setConeUpIndex(int upIndex)
        default:
                btAssert(0);
        };
+       
+       m_implicitShapeDimensions[m_coneIndices[0]] = m_radius;
+       m_implicitShapeDimensions[m_coneIndices[1]] = m_height;
+       m_implicitShapeDimensions[m_coneIndices[2]] = m_radius;
 }
 
 btVector3 btConeShape::coneLocalSupport(const btVector3& v) const
index 5966ae48f1144da4aee5c8bdea8f954eb0f8e2db..4a0df0d558bb7598ead55b6052f6a8bfc06633ee 100644 (file)
@@ -90,6 +90,13 @@ public:
        }
 
        virtual void    setLocalScaling(const btVector3& scaling);
+       
+       
+       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;
+       
 
 };
 
@@ -104,19 +111,61 @@ class btConeShapeX : public btConeShape
                return btVector3 (1,0,0);
        }
 
+       //debugging
+       virtual const char*     getName()const
+       {
+               return "ConeX";
+       }
+       
+       
 };
 
 ///btConeShapeZ implements a Cone shape, around the Z axis
 class btConeShapeZ : public btConeShape
 {
-       public:
-               btConeShapeZ(btScalar radius,btScalar height);
+public:
+       btConeShapeZ(btScalar radius,btScalar height);
 
        virtual btVector3       getAnisotropicRollingFrictionDirection() const
        {
                return btVector3 (0,0,1);
        }
 
+       //debugging
+       virtual const char*     getName()const
+       {
+               return "ConeZ";
+       }
+       
+       
 };
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btConeShapeData
+{
+       btConvexInternalShapeData       m_convexInternalShapeData;
+       
+       int     m_upIndex;
+       
+       char    m_padding[4];
+};
+
+SIMD_FORCE_INLINE      int     btConeShape::calculateSerializeBufferSize() const
+{
+       return sizeof(btConeShapeData);
+}
+
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE      const char*     btConeShape::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+       btConeShapeData* shapeData = (btConeShapeData*) dataBuffer;
+       
+       btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer);
+       
+       shapeData->m_upIndex = m_coneIndices[1];
+       
+       return "btConeShapeData";
+}
+
 #endif //BT_CONE_MINKOWSKI_H
 
index 9d40dfdd0dd40beab13960223f1f87ad295412b5..fb0d9fdb75a8dc450b864d3fa561b2c9d6c416e7 100644 (file)
@@ -21,6 +21,7 @@ subject to the following restrictions:
 #include "btTriangleShape.h"
 #include "btSphereShape.h"
 #include "btCylinderShape.h"
+#include "btConeShape.h"
 #include "btCapsuleShape.h"
 #include "btConvexHullShape.h"
 #include "btConvexPointCloudShape.h"
@@ -336,6 +337,11 @@ btScalar btConvexShape::getMarginNonVirtual () const
                btCylinderShape* cylShape = (btCylinderShape*)this;
                return cylShape->getMarginNV();
        }
+       case CONE_SHAPE_PROXYTYPE:
+       {
+               btConeShape* conShape = (btConeShape*)this;
+               return conShape->getMarginNV();
+       }
        case CAPSULE_SHAPE_PROXYTYPE:
        {
                btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
index 8d4080a63a613968dedf2207dbe33abb9dbb8f6d..26322791d041353aca86237740fcf08f54f8b576 100644 (file)
@@ -369,7 +369,7 @@ void        btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback
         getVertex(x+1,j+1,vertices[2]);
         callback->processTriangle(vertices,x,j);
         //second triangle
-        getVertex(x,j,vertices[0]);
+      //  getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman
         getVertex(x+1,j+1,vertices[1]);
         getVertex(x,j+1,vertices[2]);
         callback->processTriangle(vertices,x,j);                               
@@ -382,7 +382,7 @@ void        btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback
         callback->processTriangle(vertices,x,j);
         //second triangle
         getVertex(x+1,j,vertices[0]);
-        getVertex(x,j+1,vertices[1]);
+        //getVertex(x,j+1,vertices[1]);
         getVertex(x+1,j+1,vertices[2]);
         callback->processTriangle(vertices,x,j);
                        }
index 51a2f8a073942b75e738ade6fd12822793edb13d..5fbed334ec13f7766056a1eaabc4795f0f12c4ae 100644 (file)
@@ -111,10 +111,10 @@ int       btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicat
                                        return i/3;
                                }
                        }
-       }
-               m_3componentVertices.push_back((float)vertex.getX());
-               m_3componentVertices.push_back((float)vertex.getY());
-               m_3componentVertices.push_back((float)vertex.getZ());
+               }
+               m_3componentVertices.push_back(vertex.getX());
+               m_3componentVertices.push_back(vertex.getY());
+               m_3componentVertices.push_back(vertex.getZ());
                m_indexedMeshes[0].m_numVertices++;
                m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0];
                return (m_3componentVertices.size()/3)-1;
index 29d1b5cdaa427e36c0efd74a9145006b60598712..0afc2321ff0d728a8bd962fde19b449d19957007 100644 (file)
@@ -27,7 +27,7 @@ subject to the following restrictions:
 class btTriangleMesh : public btTriangleIndexVertexArray
 {
        btAlignedObjectArray<btVector3> m_4componentVertices;
-       btAlignedObjectArray<float>             m_3componentVertices;
+       btAlignedObjectArray<btScalar>  m_3componentVertices;
 
        btAlignedObjectArray<unsigned int>              m_32bitIndices;
        btAlignedObjectArray<unsigned short int>                m_16bitIndices;
index 72eb5aec461a4094bfdef97b42c85712e4fa0ce7..29620abffb9101c1e42f8b3df7c08c57017b29ec 100644 (file)
@@ -17,7 +17,6 @@ subject to the following restrictions:
 #ifndef BT_CONVEX_PENETRATION_DEPTH_H
 #define BT_CONVEX_PENETRATION_DEPTH_H
 
-class btStackAlloc;
 class btVector3;
 #include "btSimplexSolverInterface.h"
 class btConvexShape;
@@ -33,8 +32,7 @@ public:
                const btConvexShape* convexA,const btConvexShape* convexB,
                                        const btTransform& transA,const btTransform& transB,
                                btVector3& v, btVector3& pa, btVector3& pb,
-                               class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
-                               ) = 0;
+                               class btIDebugDraw* debugDraw) = 0;
 
 
 };
index f958cc523efad0edbfdea75adf9572728762b519..46ce1ab75e750ce997354b6e81835e367fb9e618 100644 (file)
@@ -19,7 +19,6 @@ subject to the following restrictions:
 
 #include "LinearMath/btTransform.h"
 #include "LinearMath/btVector3.h"
-class btStackAlloc;
 
 /// This interface is made to be used by an iterative approach to do TimeOfImpact calculations
 /// This interface allows to query for closest points and penetration depth between two (convex) objects
@@ -43,15 +42,13 @@ struct btDiscreteCollisionDetectorInterface
        struct ClosestPointInput
        {
                ClosestPointInput()
-                       :m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT)),
-                       m_stackAlloc(0)
+                       :m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT))
                {
                }
 
                btTransform m_transformA;
                btTransform m_transformB;
                btScalar        m_maximumDistanceSquared;
-               btStackAlloc* m_stackAlloc;
        };
 
        virtual ~btDiscreteCollisionDetectorInterface() {};
index c6dc3f3a67227e91bcfa866eb839c36f75607190..572ec36f5639c164b3407daafad4880630f2d1d5 100644 (file)
@@ -25,7 +25,7 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim
                                                                                          const btConvexShape* pConvexA, const btConvexShape* pConvexB,
                                                                                          const btTransform& transformA, const btTransform& transformB,
                                                                                          btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
-                                                                                         class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc )
+                                                                                         class btIDebugDraw* debugDraw)
 {
 
        (void)debugDraw;
@@ -34,7 +34,7 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim
 
 //     const btScalar                          radialmargin(btScalar(0.));
        
-       btVector3       guessVector(transformA.getOrigin()-transformB.getOrigin());
+       btVector3       guessVector(transformB.getOrigin()-transformA.getOrigin());
        btGjkEpaSolver2::sResults       results;
        
 
index a49689a1501d689d4d70cb7c1ff2f27a5a43d8b6..1ed6340af3b8f4b97e4165247e1af1d876f8dbde 100644 (file)
@@ -33,7 +33,7 @@ class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver
                                                                          const btConvexShape* pConvexA, const btConvexShape* pConvexB,
                                                                          const btTransform& transformA, const btTransform& transformB,
                                                                          btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
-                                                                         class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc );
+                                                                         class btIDebugDraw* debugDraw);
 
        private :
 
index 8af16b9cf6f3857bd4536326c72ca076f7b71ae9..8877579496b06698219918fed07e6a46cc4ed697 100644 (file)
@@ -50,7 +50,8 @@ m_marginA(objectA->getMargin()),
 m_marginB(objectB->getMargin()),
 m_ignoreMargin(false),
 m_lastUsedMethod(-1),
-m_catchDegeneracies(1)
+m_catchDegeneracies(1),
+m_fixContactNormalDirection(1)
 {
 }
 btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*       penetrationDepthSolver)
@@ -65,7 +66,8 @@ m_marginA(marginA),
 m_marginB(marginB),
 m_ignoreMargin(false),
 m_lastUsedMethod(-1),
-m_catchDegeneracies(1)
+m_catchDegeneracies(1),
+m_fixContactNormalDirection(1)
 {
 }
 
@@ -353,7 +355,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
                                        m_minkowskiA,m_minkowskiB,
                                        localTransA,localTransB,
                                        m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
-                                       debugDraw,input.m_stackAlloc
+                                       debugDraw
                                        );
 
 
@@ -438,6 +440,27 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
                }
 #endif 
 
+               if (m_fixContactNormalDirection)
+               {
+                       ///@workaround for sticky convex collisions
+                       //in some degenerate cases (usually when the use uses very small margins) 
+                       //the contact normal is pointing the wrong direction
+                       //so fix it now (until we can deal with all degenerate cases in GJK and EPA)
+                       //contact normals need to point from B to A in all cases, so we can simply check if the contact normal really points from B to A
+                       //We like to use a dot product of the normal against the difference of the centroids, 
+                       //once the centroid is available in the API
+                       //until then we use the center of the aabb to approximate the centroid
+                       btVector3 aabbMin,aabbMax;
+                       m_minkowskiA->getAabb(localTransA,aabbMin,aabbMax);
+                       btVector3 posA  = (aabbMax+aabbMin)*btScalar(0.5);
+               
+                       m_minkowskiB->getAabb(localTransB,aabbMin,aabbMax);
+                       btVector3 posB = (aabbMin+aabbMax)*btScalar(0.5);
+
+                       btVector3 diff = posA-posB;
+                       if (diff.dot(normalInB) < 0.f)
+                               normalInB *= -1.f;
+               }
                m_cachedSeparatingAxis = normalInB;
                m_cachedSeparatingDistance = distance;
 
index f0043b8b9f2eaef7a08e0b3b81c91dab7f0e1586..feeae686213f99c99e9ed8f5b50b1c7ded727e02 100644 (file)
@@ -52,7 +52,7 @@ public:
        int                     m_curIter;
        int                     m_degenerateSimplex;
        int                     m_catchDegeneracies;
-
+       int                     m_fixContactNormalDirection;
 
        btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*     penetrationDepthSolver);
        btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*   penetrationDepthSolver);
index fe31f08d61a6e4e053d402a8407ca2544a851aee..fa45f49037e4a58217061405c7c779f1e1c7d5dd 100644 (file)
@@ -26,11 +26,10 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
                                                                                                   const btConvexShape* convexA,const btConvexShape* convexB,
                                                                                                   const btTransform& transA,const btTransform& transB,
                                                                                                   btVector3& v, btVector3& pa, btVector3& pb,
-                                                                                                  class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
+                                                                                                  class btIDebugDraw* debugDraw
                                                                                                   )
 {
 
-       (void)stackAlloc;
        (void)v;
        
        bool check2d= convexA->isConvex2d() && convexB->isConvex2d();
index 6a8fe52f36fe2c4296808db5c2431338d4014817..fd533b4fc3315740c8cada010780e04e21add64c 100644 (file)
@@ -32,7 +32,7 @@ public:
        const btConvexShape* convexA,const btConvexShape* convexB,
                                const btTransform& transA,const btTransform& transB,
                        btVector3& v, btVector3& pa, btVector3& pb,
-                       class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
+                       class btIDebugDraw* debugDraw
                        );
 };
 
index f012889a70e5392930d6ab9bd3d0fd36358c328d..3999d4005030bb3922809bdd2ff8a1efe91ff6db 100644 (file)
@@ -35,7 +35,7 @@ public:
       kF_None                 = 0,
       kF_FilterBackfaces      = 1 << 0,
       kF_KeepUnflippedNormal  = 1 << 1,   // Prevents returned face normal getting flipped when a ray hits a back-facing triangle
-
+         kF_UseSubSimplexConvexCastRaytest =  1 << 2,   // Uses an approximate but faster ray versus convex intersection algorithm
       kF_Terminator        = 0xFFFFFFFF
    };
    unsigned int m_flags;
index c81813c92be2d139fa169e036ebc14196983d1d1..dffb06dfe94d37bc4e1262982e819faf67cae1ee 100644 (file)
@@ -31,7 +31,7 @@ public:
        
        virtual void    setWalkDirection(const btVector3& walkDirection) = 0;
        virtual void    setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) = 0;
-       virtual void    reset () = 0;
+       virtual void    reset ( btCollisionWorld* collisionWorld ) = 0;
        virtual void    warp (const btVector3& origin) = 0;
 
        virtual void    preStep ( btCollisionWorld* collisionWorld) = 0;
@@ -40,6 +40,7 @@ public:
        virtual void    jump () = 0;
 
        virtual bool    onGround () const = 0;
+       virtual void    setUpInterpolate (bool value) = 0;
 };
 
 #endif //BT_CHARACTER_CONTROLLER_INTERFACE_H
index 9db909a546991c61cba48401d67eda7db615276b..8f1cd20bf45754dd9c7cd32bd99daf8ae9fc1db3 100644 (file)
@@ -14,6 +14,7 @@ subject to the following restrictions:
 */
 
 
+#include <stdio.h>
 #include "LinearMath/btIDebugDraw.h"
 #include "BulletCollision/CollisionDispatch/btGhostObject.h"
 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
@@ -79,7 +80,7 @@ public:
 
                if (!convexResult.m_hitCollisionObject->hasContactResponse())
                        return btScalar(1.0);
-               
+
                btVector3 hitNormalWorld;
                if (normalInWorldSpace)
                {
@@ -149,7 +150,11 @@ btKinematicCharacterController::btKinematicCharacterController (btPairCachingGho
        m_jumpSpeed = 10.0; // ?
        m_wasOnGround = false;
        m_wasJumping = false;
+       m_interpolateUp = true;
        setMaxSlope(btRadians(45.0));
+       m_currentStepOffset = 0;
+       full_drop = false;
+       bounce_fix = false;
 }
 
 btKinematicCharacterController::~btKinematicCharacterController ()
@@ -190,9 +195,10 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
                m_manifoldArray.resize(0);
 
                btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
+
                btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject);
-               btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
-               
+                btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
+
                if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse()))
                        continue;
                
@@ -268,7 +274,10 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
                {
                        // 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);
+                       if (m_interpolateUp == true)
+                               m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+                       else
+                               m_currentPosition = m_targetPosition;
                }
                m_verticalVelocity = 0.0;
                m_verticalOffset = 0.0;
@@ -333,7 +342,8 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
        {
                if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
                {
-                       updateTargetPositionBasedOnCollision (m_touchingNormal);
+                       //interferes with step movement
+                       //updateTargetPositionBasedOnCollision (m_touchingNormal);
                }
        }
 
@@ -405,7 +415,8 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
 
 void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt)
 {
-       btTransform start, end;
+       btTransform start, end, end_double;
+       bool runonce = false;
 
        // phase 3: down
        /*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
@@ -414,44 +425,124 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
        btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity; 
        m_targetPosition -= (step_drop + gravity_drop);*/
 
+       btVector3 orig_position = m_targetPosition;
+       
        btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
-       if(downVelocity > 0.0 && downVelocity < m_stepHeight
+
+       if(downVelocity > 0.0 && downVelocity > m_fallSpeed
                && (m_wasOnGround || !m_wasJumping))
-       {
-               downVelocity = m_stepHeight;
-       }
+               downVelocity = m_fallSpeed;
 
        btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
        m_targetPosition -= step_drop;
 
-       start.setIdentity ();
-       end.setIdentity ();
+       btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
+        callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
+        callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 
-       start.setOrigin (m_currentPosition);
-       end.setOrigin (m_targetPosition);
+        btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
+        callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
+        callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 
-       btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
-       callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
-       callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
-       
-       if (m_useGhostObjectSweepTest)
+       while (1)
        {
-               m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
-       } else
-       {
-               collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+               start.setIdentity ();
+               end.setIdentity ();
+
+               end_double.setIdentity ();
+
+               start.setOrigin (m_currentPosition);
+               end.setOrigin (m_targetPosition);
+
+               //set double test for 2x the step drop, to check for a large drop vs small drop
+               end_double.setOrigin (m_targetPosition - step_drop);
+
+               if (m_useGhostObjectSweepTest)
+               {
+                       m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+
+                       if (!callback.hasHit())
+                       {
+                               //test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
+                               m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+                       }
+               } else
+               {
+                       collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+
+                       if (!callback.hasHit())
+                                       {
+                                                       //test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
+                                                       collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+                                       }
+               }
+       
+               btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
+               bool has_hit = false;
+               if (bounce_fix == true)
+                       has_hit = callback.hasHit() || callback2.hasHit();
+               else
+                       has_hit = callback2.hasHit();
+
+               if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false
+                                       && (m_wasOnGround || !m_wasJumping))
+               {
+                       //redo the velocity calculation when falling a small amount, for fast stairs motion
+                       //for larger falls, use the smoother/slower interpolated movement by not touching the target position
+
+                       m_targetPosition = orig_position;
+                                       downVelocity = m_stepHeight;
+
+                               btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+                       m_targetPosition -= step_drop;
+                       runonce = true;
+                       continue; //re-run previous tests
+               }
+               break;
        }
 
-       if (callback.hasHit())
+       if (callback.hasHit() || runonce == true)
        {
                // we dropped a fraction of the height -> hit floor
-               m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+
+               btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2;
+
+               //printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());
+
+               if (bounce_fix == true)
+               {
+                       if (full_drop == true)
+                                m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+                        else
+                                //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
+                                m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
+               }
+               else
+                       m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+
+               full_drop = false;
+
                m_verticalVelocity = 0.0;
                m_verticalOffset = 0.0;
                m_wasJumping = false;
        } else {
                // we dropped the full height
                
+               full_drop = true;
+
+               if (bounce_fix == true)
+               {
+                       downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
+                       if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
+                       {
+                               m_targetPosition += step_drop; //undo previous target change
+                               downVelocity = m_fallSpeed;
+                               step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+                               m_targetPosition -= step_drop;
+                       }
+               }
+               //printf("full drop - %g, %g\n", m_currentPosition.getY(), m_targetPosition.getY());
+
                m_currentPosition = m_targetPosition;
        }
 }
@@ -484,13 +575,24 @@ btScalar timeInterval
        m_useWalkDirection = false;
        m_walkDirection = velocity;
        m_normalizedDirection = getNormalizedVector(m_walkDirection);
-       m_velocityTimeInterval = timeInterval;
+       m_velocityTimeInterval += timeInterval;
 }
 
-
-
-void btKinematicCharacterController::reset ()
-{
+void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld )
+{
+        m_verticalVelocity = 0.0;
+        m_verticalOffset = 0.0;
+        m_wasOnGround = false;
+        m_wasJumping = false;
+        m_walkDirection.setValue(0,0,0);
+        m_velocityTimeInterval = 0.0;
+
+        //clear pair cache
+        btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
+        while (cache->getOverlappingPairArray().size() > 0)
+        {
+                cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
+        }
 }
 
 void btKinematicCharacterController::warp (const btVector3& origin)
@@ -661,3 +763,8 @@ btVector3* btKinematicCharacterController::getUpAxisDirections()
 void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
 {
 }
+
+void btKinematicCharacterController::setUpInterpolate(bool value)
+{
+       m_interpolateUp = value;
+}
index 8ec63735cd8ff02b2101ee9c3069aaee53801593..add6f30a687277a9843d7f3360593a4461a13ce7 100644 (file)
@@ -81,6 +81,9 @@ protected:
        int m_upAxis;
 
        static btVector3* getUpAxisDirections();
+       bool  m_interpolateUp;
+       bool  full_drop;
+       bool  bounce_fix;
 
        btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal);
        btVector3 parallelComponent (const btVector3& direction, const btVector3& normal);
@@ -133,7 +136,7 @@ public:
        virtual void setVelocityForTimeInterval(const btVector3& velocity,
                                btScalar timeInterval);
 
-       void reset ();
+       void reset ( btCollisionWorld* collisionWorld );
        void warp (const btVector3& origin);
 
        void preStep (  btCollisionWorld* collisionWorld);
@@ -161,6 +164,7 @@ public:
        }
 
        bool onGround () const;
+       void setUpInterpolate (bool value);
 };
 
 #endif // BT_KINEMATIC_CHARACTER_CONTROLLER_H
index 09c048beda8bc16bb2011815920229b08fcfb7fd..1735b524dba3c9ac16f43ee58f07798b047ad02b 100644 (file)
@@ -40,6 +40,15 @@ and swing 1 and 2 are along the z and y axes respectively.
 #include "btJacobianEntry.h"
 #include "btTypedConstraint.h"
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btConeTwistConstraintData2     btConeTwistConstraintDoubleData
+#define btConeTwistConstraintDataName  "btConeTwistConstraintDoubleData"
+#else
+#define btConeTwistConstraintData2     btConeTwistConstraintData 
+#define btConeTwistConstraintDataName  "btConeTwistConstraintData" 
+#endif //BT_USE_DOUBLE_PRECISION
+
+
 class btRigidBody;
 
 enum btConeTwistFlags
@@ -295,7 +304,30 @@ public:
 
 };
 
-///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+
+       
+struct btConeTwistConstraintDoubleData
+{
+       btTypedConstraintDoubleData     m_typeConstraintData;
+       btTransformDoubleData m_rbAFrame;
+       btTransformDoubleData m_rbBFrame;
+
+       //limits
+       double  m_swingSpan1;
+       double  m_swingSpan2;
+       double  m_twistSpan;
+       double  m_limitSoftness;
+       double  m_biasFactor;
+       double  m_relaxationFactor;
+
+       double  m_damping;
+               
+       
+
+};
+
+#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
+///this structure is not used, except for loading pre-2.82 .bullet files
 struct btConeTwistConstraintData
 {
        btTypedConstraintData   m_typeConstraintData;
@@ -315,12 +347,12 @@ struct    btConeTwistConstraintData
        char m_pad[4];
 
 };
-       
-
+#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
+//
 
 SIMD_FORCE_INLINE int  btConeTwistConstraint::calculateSerializeBufferSize() const
 {
-       return sizeof(btConeTwistConstraintData);
+       return sizeof(btConeTwistConstraintData2);
 
 }
 
@@ -328,21 +360,21 @@ SIMD_FORCE_INLINE int     btConeTwistConstraint::calculateSerializeBufferSize() cons
        ///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;
+       btConeTwistConstraintData2* cone = (btConeTwistConstraintData2*) dataBuffer;
        btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer);
 
-       m_rbAFrame.serializeFloat(cone->m_rbAFrame);
-       m_rbBFrame.serializeFloat(cone->m_rbBFrame);
+       m_rbAFrame.serialize(cone->m_rbAFrame);
+       m_rbBFrame.serialize(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";
+       cone->m_swingSpan1 = m_swingSpan1;
+       cone->m_swingSpan2 = m_swingSpan2;
+       cone->m_twistSpan = m_twistSpan;
+       cone->m_limitSoftness = m_limitSoftness;
+       cone->m_biasFactor = m_biasFactor;
+       cone->m_relaxationFactor = m_relaxationFactor;
+       cone->m_damping = m_damping;
+
+       return btConeTwistConstraintDataName;
 }
 
 
index 6f673102be284b420ab192bb2083a6ba097d723c..1ba1cd1e839a23ad25161e7c7888d8d539fde985 100644 (file)
@@ -28,6 +28,14 @@ class btIDebugDraw;
 class btStackAlloc;
 class  btDispatcher;
 /// btConstraintSolver provides solver interface
+
+
+enum btConstraintSolverType
+{
+       BT_SEQUENTIAL_IMPULSE_SOLVER=1,
+       BT_MLCP_SOLVER=2
+};
+
 class btConstraintSolver
 {
 
@@ -38,12 +46,16 @@ public:
        virtual void prepareSolve (int /* numBodies */, int /* numManifolds */) {;}
 
        ///solve a group of constraints
-       virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher) = 0;
+       virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer,btDispatcher* dispatcher) = 0;
 
-       virtual void allSolved (const btContactSolverInfo& /* info */,class btIDebugDraw* /* debugDrawer */, btStackAlloc* /* stackAlloc */) {;}
+       virtual void allSolved (const btContactSolverInfo& /* info */,class btIDebugDraw* /* debugDrawer */) {;}
 
        ///clear internal cached data and reset random seed
        virtual void    reset() = 0;
+
+       virtual btConstraintSolverType  getSolverType() const=0;
+
+
 };
 
 
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp
new file mode 100644 (file)
index 0000000..f93a328
--- /dev/null
@@ -0,0 +1,129 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btFixedConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+#include <new>
+
+
+btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB)
+:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB)
+{
+       m_pivotInA = frameInA.getOrigin();
+       m_pivotInB = frameInB.getOrigin();
+       m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse();
+
+}
+
+btFixedConstraint::~btFixedConstraint ()
+{
+}
+
+       
+void btFixedConstraint::getInfo1 (btConstraintInfo1* info)
+{
+       info->m_numConstraintRows = 6;
+       info->nub = 6;
+}
+
+void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
+{
+       //fix the 3 linear degrees of freedom
+
+       
+       const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin();
+       const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis();
+       const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin();
+       const btMatrix3x3& worldOrnB = m_rbB.getCenterOfMassTransform().getBasis();
+       
+
+       info->m_J1linearAxis[0] = 1;
+       info->m_J1linearAxis[info->rowskip+1] = 1;
+       info->m_J1linearAxis[2*info->rowskip+2] = 1;
+
+       btVector3 a1 = worldOrnA*m_pivotInA;
+       {
+               btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
+               btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
+               btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
+               btVector3 a1neg = -a1;
+               a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+       }
+    
+       if (info->m_J2linearAxis)
+       {
+               info->m_J2linearAxis[0] = -1;
+               info->m_J2linearAxis[info->rowskip+1] = -1;
+               info->m_J2linearAxis[2*info->rowskip+2] = -1;
+       }
+       
+       btVector3 a2 = worldOrnB*m_pivotInB;
+   
+       {
+       //      btVector3 a2n = -a2;
+               btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
+               btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
+               btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
+               a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+       }
+
+    // set right hand side for the linear dofs
+       btScalar k = info->fps * info->erp;
+       
+       btVector3 linearError = k*(a2+worldPosB-a1-worldPosA);
+    int j;
+       for (j=0; j<3; j++)
+    {
+
+
+
+        info->m_constraintError[j*info->rowskip] = linearError[j];
+               //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
+    }
+
+               //fix the 3 angular degrees of freedom
+
+       int start_row = 3;
+       int s = info->rowskip;
+    int start_index = start_row * s;
+
+    // 3 rows to make body rotations equal
+       info->m_J1angularAxis[start_index] = 1;
+    info->m_J1angularAxis[start_index + s + 1] = 1;
+    info->m_J1angularAxis[start_index + s*2+2] = 1;
+    if ( info->m_J2angularAxis)
+    {
+        info->m_J2angularAxis[start_index] = -1;
+        info->m_J2angularAxis[start_index + s+1] = -1;
+        info->m_J2angularAxis[start_index + s*2+2] = -1;
+    }
+
+    // set right hand side for the angular dofs
+
+       btVector3 diff;
+       btScalar angle;
+       btMatrix3x3 mrelCur = worldOrnA *worldOrnB.inverse();
+       btQuaternion qrelCur;
+       mrelCur.getRotation(qrelCur);
+       btTransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB,qrelCur,diff,angle);
+       diff*=-angle;
+       for (j=0; j<3; j++)
+    {
+        info->m_constraintError[(3+j)*info->rowskip] = k * diff[j];
+    }
+
+}
\ No newline at end of file
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h
new file mode 100644 (file)
index 0000000..697e319
--- /dev/null
@@ -0,0 +1,49 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_FIXED_CONSTRAINT_H
+#define BT_FIXED_CONSTRAINT_H
+
+#include "btTypedConstraint.h"
+
+ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint
+{
+       btVector3 m_pivotInA;
+       btVector3 m_pivotInB;
+       btQuaternion m_relTargetAB;
+
+public:
+       btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB);
+       
+       virtual ~btFixedConstraint();
+
+       
+       virtual void getInfo1 (btConstraintInfo1* info);
+
+       virtual void getInfo2 (btConstraintInfo2* info);
+
+       virtual void    setParam(int num, btScalar value, int axis = -1)
+       {
+               btAssert(0);
+       }
+       virtual btScalar getParam(int num, int axis = -1) const
+       {
+               btAssert(0);
+               return 0.f;
+       }
+
+};
+
+#endif //BT_FIXED_CONSTRAINT_H
index 60f6009484358af753d7cfa2e4171a286a527ab4..f9afcb91211feda28f2b2274e65d2310f83d8170 100644 (file)
@@ -19,6 +19,18 @@ subject to the following restrictions:
 #define BT_GEAR_CONSTRAINT_H
 
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btGearConstraintData   btGearConstraintDoubleData
+#define btGearConstraintDataName       "btGearConstraintDoubleData"
+#else
+#define btGearConstraintData   btGearConstraintFloatData
+#define btGearConstraintDataName       "btGearConstraintFloatData"
+#endif //BT_USE_DOUBLE_PRECISION
+
+
+
 ///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio.
 ///See Bullet/Demos/ConstraintDemo for an example use.
 class btGearConstraint : public btTypedConstraint
@@ -39,18 +51,102 @@ public:
        ///internal method used by the constraint solver, don't use them directly
        virtual void getInfo2 (btConstraintInfo2* info);
 
+       void setAxisA(btVector3& axisA) 
+       {
+               m_axisInA = axisA;
+       }
+       void setAxisB(btVector3& axisB)
+       {
+               m_axisInB = axisB;
+       }
+       void setRatio(btScalar ratio)
+       {
+               m_ratio = ratio;
+       }
+       const btVector3& getAxisA() const
+       {
+               return m_axisInA;
+       }
+       const btVector3& getAxisB() const
+       {
+               return m_axisInB;
+       }
+       btScalar getRatio() const
+       {
+               return m_ratio;
+       }
+
+
        virtual void    setParam(int num, btScalar value, int axis = -1) 
        {
+               (void) num;
+               (void) value;
+               (void) axis;
                btAssert(0);
-       };
+       }
 
        ///return the local value of parameter
        virtual btScalar getParam(int num, int axis = -1) const 
        { 
+               (void) num;
+               (void) axis;
                btAssert(0);
                return 0.f;
        }
 
+       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 btGearConstraintFloatData
+{
+       btTypedConstraintFloatData      m_typeConstraintData;
+
+       btVector3FloatData                      m_axisInA;
+       btVector3FloatData                      m_axisInB;
+
+       float                                                   m_ratio;
+       char                                                    m_padding[4];
 };
 
+struct btGearConstraintDoubleData
+{
+       btTypedConstraintDoubleData     m_typeConstraintData;
+
+       btVector3DoubleData                     m_axisInA;
+       btVector3DoubleData                     m_axisInB;
+
+       double                                          m_ratio;
+};
+
+SIMD_FORCE_INLINE      int     btGearConstraint::calculateSerializeBufferSize() const
+{
+       return sizeof(btGearConstraintData);
+}
+
+       ///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE      const char*     btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+       btGearConstraintData* gear = (btGearConstraintData*)dataBuffer;
+       btTypedConstraint::serialize(&gear->m_typeConstraintData,serializer);
+
+       m_axisInA.serialize( gear->m_axisInA );
+       m_axisInB.serialize( gear->m_axisInB );
+
+       gear->m_ratio = m_ratio;
+
+       return btGearConstraintDataName;
+}
+
+
+
+
+
+
 #endif //BT_GEAR_CONSTRAINT_H
index 0409f95379b01ef048d1b092081d7d7dc39c8802..431a524169ef20d1d80f60b0bc3ab85b6ff7c5ad 100644 (file)
@@ -35,6 +35,14 @@ class btRigidBody;
 
 
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btGeneric6DofConstraintData2           btGeneric6DofConstraintDoubleData2
+#define btGeneric6DofConstraintDataName        "btGeneric6DofConstraintDoubleData2"
+#else
+#define btGeneric6DofConstraintData2           btGeneric6DofConstraintData
+#define btGeneric6DofConstraintDataName        "btGeneric6DofConstraintData"
+#endif //BT_USE_DOUBLE_PRECISION
+
 
 //! Rotation Limit structure for generic joints
 class btRotationalLimitMotor
@@ -561,7 +569,7 @@ public:
        
 };
 
-///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+
 struct btGeneric6DofConstraintData
 {
        btTypedConstraintData   m_typeConstraintData;
@@ -578,35 +586,51 @@ struct btGeneric6DofConstraintData
        int m_useOffsetForConstraintFrame;
 };
 
+struct btGeneric6DofConstraintDoubleData2
+{
+       btTypedConstraintDoubleData     m_typeConstraintData;
+       btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+       btTransformDoubleData m_rbBFrame;
+       
+       btVector3DoubleData     m_linearUpperLimit;
+       btVector3DoubleData     m_linearLowerLimit;
+
+       btVector3DoubleData     m_angularUpperLimit;
+       btVector3DoubleData     m_angularLowerLimit;
+       
+       int     m_useLinearReferenceFrameA;
+       int m_useOffsetForConstraintFrame;
+};
+
 SIMD_FORCE_INLINE      int     btGeneric6DofConstraint::calculateSerializeBufferSize() const
 {
-       return sizeof(btGeneric6DofConstraintData);
+       return sizeof(btGeneric6DofConstraintData2);
 }
 
        ///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;
+       btGeneric6DofConstraintData2* dof = (btGeneric6DofConstraintData2*)dataBuffer;
        btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
 
-       m_frameInA.serializeFloat(dof->m_rbAFrame);
-       m_frameInB.serializeFloat(dof->m_rbBFrame);
+       m_frameInA.serialize(dof->m_rbAFrame);
+       m_frameInB.serialize(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_angularLowerLimit.m_floats[i] =  m_angularLimits[i].m_loLimit;
+               dof->m_angularUpperLimit.m_floats[i] =  m_angularLimits[i].m_hiLimit;
+               dof->m_linearLowerLimit.m_floats[i] = m_linearLimits.m_lowerLimit[i];
+               dof->m_linearUpperLimit.m_floats[i] = m_linearLimits.m_upperLimit[i];
        }
        
        dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
        dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
 
-       return "btGeneric6DofConstraintData";
+       return btGeneric6DofConstraintDataName;
 }
 
 
index 6fabb30369b7ee472bb03419f704c94d7f8d5c25..1b2e0f62ca8cf3fdea8e9a60da360f99c970610f 100644 (file)
@@ -21,6 +21,15 @@ subject to the following restrictions:
 #include "btTypedConstraint.h"
 #include "btGeneric6DofConstraint.h"
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btGeneric6DofSpringConstraintData2             btGeneric6DofSpringConstraintDoubleData2
+#define btGeneric6DofSpringConstraintDataName  "btGeneric6DofSpringConstraintDoubleData2"
+#else
+#define btGeneric6DofSpringConstraintData2             btGeneric6DofSpringConstraintData
+#define btGeneric6DofSpringConstraintDataName  "btGeneric6DofSpringConstraintData"
+#endif //BT_USE_DOUBLE_PRECISION
+
+
 
 /// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF
 
@@ -65,7 +74,6 @@ public:
 };
 
 
-///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct btGeneric6DofSpringConstraintData
 {
        btGeneric6DofConstraintData     m_6dofData;
@@ -76,26 +84,37 @@ struct btGeneric6DofSpringConstraintData
        float           m_springDamping[6];
 };
 
+struct btGeneric6DofSpringConstraintDoubleData2
+{
+       btGeneric6DofConstraintDoubleData2      m_6dofData;
+       
+       int                     m_springEnabled[6];
+       double          m_equilibriumPoint[6];
+       double          m_springStiffness[6];
+       double          m_springDamping[6];
+};
+
+
 SIMD_FORCE_INLINE      int     btGeneric6DofSpringConstraint::calculateSerializeBufferSize() const
 {
-       return sizeof(btGeneric6DofSpringConstraintData);
+       return sizeof(btGeneric6DofSpringConstraintData2);
 }
 
        ///fills the dataBuffer and returns the struct name (and 0 on failure)
 SIMD_FORCE_INLINE      const char*     btGeneric6DofSpringConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-       btGeneric6DofSpringConstraintData* dof = (btGeneric6DofSpringConstraintData*)dataBuffer;
+       btGeneric6DofSpringConstraintData2* dof = (btGeneric6DofSpringConstraintData2*)dataBuffer;
        btGeneric6DofConstraint::serialize(&dof->m_6dofData,serializer);
 
        int i;
        for (i=0;i<6;i++)
        {
-               dof->m_equilibriumPoint[i] = (float)m_equilibriumPoint[i];
-               dof->m_springDamping[i] = (float)m_springDamping[i];
+               dof->m_equilibriumPoint[i] = m_equilibriumPoint[i];
+               dof->m_springDamping[i] = m_springDamping[i];
                dof->m_springEnabled[i] = m_springEnabled[i]? 1 : 0;
-               dof->m_springStiffness[i] = (float)m_springStiffness[i];
+               dof->m_springStiffness[i] = m_springStiffness[i];
        }
-       return "btGeneric6DofSpringConstraintData";
+       return btGeneric6DofSpringConstraintDataName;
 }
 
 #endif // BT_GENERIC_6DOF_SPRING_CONSTRAINT_H
index a7f2cca5500828e289d2b2709c7b92ab4581b761..7c33ac24e05752f15d599a6a3d8d5cf53eaeb4b3 100644 (file)
@@ -28,8 +28,8 @@ subject to the following restrictions:
 class btRigidBody;
 
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btHingeConstraintData  btHingeConstraintDoubleData
-#define btHingeConstraintDataName      "btHingeConstraintDoubleData"
+#define btHingeConstraintData  btHingeConstraintDoubleData2 //rename to 2 for backwards compatibility, so we can still load the 'btHingeConstraintDoubleData' version
+#define btHingeConstraintDataName      "btHingeConstraintDoubleData2" 
 #else
 #define btHingeConstraintData  btHingeConstraintFloatData
 #define btHingeConstraintDataName      "btHingeConstraintFloatData"
@@ -302,7 +302,10 @@ public:
 
 };
 
-///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+
+//only for backward compatibility
+#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
+///this structure is not used, except for loading pre-2.82 .bullet files
 struct btHingeConstraintDoubleData
 {
        btTypedConstraintData   m_typeConstraintData;
@@ -321,7 +324,9 @@ struct      btHingeConstraintDoubleData
        float   m_relaxationFactor;
 
 };
-///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
+
+
 struct btHingeConstraintFloatData
 {
        btTypedConstraintData   m_typeConstraintData;
@@ -344,6 +349,30 @@ struct     btHingeConstraintFloatData
 
 
 
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btHingeConstraintDoubleData2
+{
+       btTypedConstraintDoubleData     m_typeConstraintData;
+       btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+       btTransformDoubleData m_rbBFrame;
+       int                     m_useReferenceFrameA;
+       int                     m_angularOnly;
+       int                     m_enableAngularMotor;
+       double          m_motorTargetVelocity;
+       double          m_maxMotorImpulse;
+
+       double          m_lowerLimit;
+       double          m_upperLimit;
+       double          m_limitSoftness;
+       double          m_biasFactor;
+       double          m_relaxationFactor;
+       char    m_padding1[4];
+
+};
+
+
+
+
 SIMD_FORCE_INLINE      int     btHingeConstraint::calculateSerializeBufferSize() const
 {
        return sizeof(btHingeConstraintData);
index 1e13416dfeb149ba47bb95dde13a00fcbb5d4cfe..91218949498f2135941e7babf2f37c7bedd6713f 100644 (file)
@@ -24,10 +24,10 @@ class btRigidBody;
 
 
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btPoint2PointConstraintData    btPoint2PointConstraintDoubleData
-#define btPoint2PointConstraintDataName        "btPoint2PointConstraintDoubleData"
+#define btPoint2PointConstraintData2   btPoint2PointConstraintDoubleData2
+#define btPoint2PointConstraintDataName        "btPoint2PointConstraintDoubleData2"
 #else
-#define btPoint2PointConstraintData    btPoint2PointConstraintFloatData
+#define btPoint2PointConstraintData2   btPoint2PointConstraintFloatData
 #define btPoint2PointConstraintDataName        "btPoint2PointConstraintFloatData"
 #endif //BT_USE_DOUBLE_PRECISION
 
@@ -133,6 +133,17 @@ struct     btPoint2PointConstraintFloatData
        btVector3FloatData      m_pivotInB;
 };
 
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btPoint2PointConstraintDoubleData2
+{
+       btTypedConstraintDoubleData     m_typeConstraintData;
+       btVector3DoubleData     m_pivotInA;
+       btVector3DoubleData     m_pivotInB;
+};
+
+#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+///this structure is not used, except for loading pre-2.82 .bullet files
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct btPoint2PointConstraintDoubleData
 {
@@ -140,18 +151,19 @@ struct    btPoint2PointConstraintDoubleData
        btVector3DoubleData     m_pivotInA;
        btVector3DoubleData     m_pivotInB;
 };
+#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
 
 
 SIMD_FORCE_INLINE      int     btPoint2PointConstraint::calculateSerializeBufferSize() const
 {
-       return sizeof(btPoint2PointConstraintData);
+       return sizeof(btPoint2PointConstraintData2);
 
 }
 
        ///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;
+       btPoint2PointConstraintData2* p2pData = (btPoint2PointConstraintData2*)dataBuffer;
 
        btTypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer);
        m_pivotInA.serialize(p2pData->m_pivotInA);
index 5f9437b7add1ed7258dc226eb4722e31a5ad7b21..be93e35434cead775ed68b49d6a1caafc8e930cc 100644 (file)
@@ -152,7 +152,7 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
 #endif
 }
 
-// Project Gauss Seidel or the equivalent Sequential Impulse
+// Projected Gauss Seidel or the equivalent Sequential Impulse
  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
        btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
@@ -280,7 +280,7 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
 
 
 
-void   btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
+void   btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep)
 {
 
        btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
@@ -299,6 +299,9 @@ void        btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod
                solverBody->m_linearFactor = rb->getLinearFactor();
                solverBody->m_linearVelocity = rb->getLinearVelocity();
                solverBody->m_angularVelocity = rb->getAngularVelocity();
+               solverBody->m_externalForceImpulse = rb->getTotalForce()*rb->getInvMass()*timeStep;
+               solverBody->m_externalTorqueImpulse = rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*timeStep ;
+               
        } else
        {
                solverBody->m_worldTransform.setIdentity();
@@ -308,6 +311,8 @@ void        btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod
                solverBody->m_linearFactor.setValue(1,1,1);
                solverBody->m_linearVelocity.setValue(0,0,0);
                solverBody->m_angularVelocity.setValue(0,0,0);
+               solverBody->m_externalForceImpulse.setValue(0,0,0);
+               solverBody->m_externalTorqueImpulse.setValue(0,0,0);
        }
 
 
@@ -326,8 +331,7 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel,
 
 
 
-static void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode);
-static void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode)
+void   btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode)
 {
        
 
@@ -351,8 +355,6 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 {
 
        
-       solverConstraint.m_contactNormal1 = normalAxis;
-       solverConstraint.m_contactNormal2 = -normalAxis;
        btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
        btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
 
@@ -368,15 +370,30 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
        solverConstraint.m_appliedImpulse = 0.f;
        solverConstraint.m_appliedPushImpulse = 0.f;
 
+       if (body0)
        {
+               solverConstraint.m_contactNormal1 = normalAxis;
                btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1);
                solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
-               solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
+               solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor();
+       }else
+       {
+               solverConstraint.m_contactNormal1.setZero();
+               solverConstraint.m_relpos1CrossNormal.setZero();
+               solverConstraint.m_angularComponentA .setZero();
        }
+
+       if (body1)
        {
+               solverConstraint.m_contactNormal2 = -normalAxis;
                btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2);
                solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
-               solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
+               solverConstraint.m_angularComponentB = body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor();
+       } else
+       {
+               solverConstraint.m_contactNormal2.setZero();
+               solverConstraint.m_relpos2CrossNormal.setZero();
+               solverConstraint.m_angularComponentB.setZero();
        }
 
        {
@@ -401,9 +418,9 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
                
 
                btScalar rel_vel;
-               btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity:btVector3(0,0,0)) 
+               btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) 
                        + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
-               btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity:btVector3(0,0,0)) 
+               btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) 
                        + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
 
                rel_vel = vel1Dotn+vel2Dotn;
@@ -414,8 +431,8 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
                btSimdScalar    velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
                solverConstraint.m_rhs = velocityImpulse;
                solverConstraint.m_cfm = cfmSlip;
-               solverConstraint.m_lowerLimit = 0;
-               solverConstraint.m_upperLimit = 1e10f;
+               solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
+               solverConstraint.m_upperLimit = solverConstraint.m_friction;
                
        }
 }
@@ -481,9 +498,9 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(   btSolv
                
 
                btScalar rel_vel;
-               btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity:btVector3(0,0,0)) 
+               btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) 
                        + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
-               btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity:btVector3(0,0,0)) 
+               btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) 
                        + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
 
                rel_vel = vel1Dotn+vel2Dotn;
@@ -494,8 +511,8 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(   btSolv
                btSimdScalar    velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
                solverConstraint.m_rhs = velocityImpulse;
                solverConstraint.m_cfm = cfmSlip;
-               solverConstraint.m_lowerLimit = 0;
-               solverConstraint.m_upperLimit = 1e10f;
+               solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
+               solverConstraint.m_upperLimit = solverConstraint.m_friction;
                
        }
 }
@@ -517,7 +534,7 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addRollingFrictionConst
 }
 
 
-int    btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
+int    btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body,btScalar timeStep)
 {
 
        int solverBodyIdA = -1;
@@ -535,11 +552,19 @@ int       btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
                {
                        solverBodyIdA = m_tmpSolverBodyPool.size();
                        btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-                       initSolverBody(&solverBody,&body);
+                       initSolverBody(&solverBody,&body,timeStep);
                        body.setCompanionId(solverBodyIdA);
                } else
                {
-                       return 0;//assume first one is a fixed solver body
+                       
+                       if (m_fixedBodyId<0)
+                       {
+                               m_fixedBodyId = m_tmpSolverBodyPool.size();
+                               btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+                               initSolverBody(&fixedBody,0,timeStep);
+                       }
+                       return m_fixedBodyId;
+//                     return 0;//assume first one is a fixed solver body
                }
        }
 
@@ -552,8 +577,8 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, 
                                                                                                                                 int solverBodyIdA, int solverBodyIdB,
                                                                                                                                 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
-                                                                                                                                btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
-                                                                                                                                btVector3& rel_pos1, btVector3& rel_pos2)
+                                                                                                                                btScalar& relaxation,
+                                                                                                                                const btVector3& rel_pos1, const btVector3& rel_pos2)
 {
                        
                        const btVector3& pos1 = cp.getPositionWorldOnA();
@@ -567,8 +592,8 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 
 //                     btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
 //                     btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
-                       rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); 
-                       rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
+                       //rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); 
+                       //rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
 
                        relaxation = 1.f;
 
@@ -601,10 +626,24 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
                                        solverConstraint.m_jacDiagABInv = denom;
                                }
 
-                               solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
-                               solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
-                               solverConstraint.m_relpos1CrossNormal = torqueAxis0;
-                               solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+                               if (rb0)
+                               {
+                                       solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
+                                       solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+                               } else
+                               {
+                                       solverConstraint.m_contactNormal1.setZero();
+                                       solverConstraint.m_relpos1CrossNormal.setZero();
+                               }
+                               if (rb1)
+                               {
+                                       solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
+                                       solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+                               }else
+                               {
+                                       solverConstraint.m_contactNormal2.setZero();
+                                       solverConstraint.m_relpos2CrossNormal.setZero();
+                               }
 
                                btScalar restitution = 0.f;
                                btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
@@ -616,8 +655,8 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
                                        vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : 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 vel  = vel1 - vel2;
+                                       btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
 
                                        
 
@@ -648,10 +687,17 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
                                solverConstraint.m_appliedPushImpulse = 0.f;
 
                                {
-                                       btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rb0?bodyA->m_linearVelocity:btVector3(0,0,0)) 
-                                               + solverConstraint.m_relpos1CrossNormal.dot(rb0?bodyA->m_angularVelocity:btVector3(0,0,0));
-                                       btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rb1?bodyB->m_linearVelocity:btVector3(0,0,0)) 
-                                               + solverConstraint.m_relpos2CrossNormal.dot(rb1?bodyB->m_angularVelocity:btVector3(0,0,0));
+
+                                       btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse: btVector3(0,0,0);
+                                       btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse: btVector3(0,0,0);
+                                       btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse: btVector3(0,0,0);
+                                       btVector3 externalTorqueImpulseB = bodyB->m_originalBody ?bodyB->m_externalTorqueImpulse : btVector3(0,0,0);
+                                               
+
+                                       btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA) 
+                                               + solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity+externalTorqueImpulseA);
+                                       btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB) 
+                                               + solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity+externalTorqueImpulseB);
                                        btScalar rel_vel = vel1Dotn+vel2Dotn;
 
                                        btScalar positionalError = 0.f;
@@ -680,7 +726,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
                                        if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
                                        {
                                                //combine position and velocity into rhs
-                                               solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+                                               solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv;
                                                solverConstraint.m_rhsPenetration = 0.f;
 
                                        } else
@@ -754,8 +800,8 @@ void        btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
        colObj0 = (btCollisionObject*)manifold->getBody0();
        colObj1 = (btCollisionObject*)manifold->getBody1();
 
-       int solverBodyIdA = getOrInitSolverBody(*colObj0);
-       int solverBodyIdB = getOrInitSolverBody(*colObj1);
+       int solverBodyIdA = getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
+       int solverBodyIdB = getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
 
 //     btRigidBody* bodyA = btRigidBody::upcast(colObj0);
 //     btRigidBody* bodyB = btRigidBody::upcast(colObj1);
@@ -780,19 +826,35 @@ void      btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
                        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);
+                       btRigidBody* rb0 = btRigidBody::upcast(colObj0);
+                       btRigidBody* rb1 = btRigidBody::upcast(colObj1);
                        solverConstraint.m_solverBodyIdA = solverBodyIdA;
                        solverConstraint.m_solverBodyIdB = solverBodyIdB;
 
                        solverConstraint.m_originalContactPoint = &cp;
 
-                       setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
+                       const btVector3& pos1 = cp.getPositionWorldOnA();
+                       const btVector3& pos2 = cp.getPositionWorldOnB();
+
+                       rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
+                       rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
+
+                       btVector3 vel1;// = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
+                       btVector3 vel2;// = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
+
+                       solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1);
+                       solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 );
+                       
+                       btVector3 vel  = vel1 - vel2;
+                       btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
+
+                       setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
+
+                       
 
 //                     const btVector3& pos1 = cp.getPositionWorldOnA();
 //                     const btVector3& pos2 = cp.getPositionWorldOnB();
@@ -801,9 +863,11 @@ void       btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
                        solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
 
-                       btVector3 angVelA,angVelB;
-                       solverBodyA->getAngularVelocity(angVelA);
-                       solverBodyB->getAngularVelocity(angVelB);                       
+                       btVector3 angVelA(0,0,0),angVelB(0,0,0);
+                       if (rb0)
+                               angVelA = rb0->getAngularVelocity();
+                       if (rb1)
+                               angVelB = rb1->getAngularVelocity();
                        btVector3 relAngVel = angVelB-angVelA;
 
                        if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
@@ -857,6 +921,10 @@ void       btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
                                if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
                                {
                                        cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
+                                       applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
+                                       applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
+                                       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);
@@ -864,17 +932,16 @@ void      btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
                                                applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
                                                applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-
                                        }
 
-                                       applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-                                       applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-                                       addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-
                                } else
                                {
                                        btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
 
+                                       applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
+                                       applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
+                                       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,btCollisionObject::CF_ANISOTROPIC_FRICTION);
@@ -882,9 +949,6 @@ void        btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
                                        }
 
-                                       applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-                                       applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-                                       addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 
                                        if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
                                        {
@@ -899,8 +963,8 @@ void        btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
                                if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
                                        addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
 
-                               setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
                        }
+                       setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
                
 
                        
@@ -909,10 +973,24 @@ void      btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
        }
 }
 
-btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
+{
+       int i;
+       btPersistentManifold* manifold = 0;
+//                     btCollisionObject* colObj0=0,*colObj1=0;
+
+
+       for (i=0;i<numManifolds;i++)
+       {
+               manifold = manifoldPtr[i];
+               convertContact(manifold,infoGlobal);
+       }
+}
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
 {
+       m_fixedBodyId = -1;
        BT_PROFILE("solveGroupCacheFriendlySetup");
-       (void)stackAlloc;
        (void)debugDrawer;
 
        m_maxOverrideNumSolverIterations = 0;
@@ -996,14 +1074,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
        m_tmpSolverBodyPool.reserve(numBodies+1);
        m_tmpSolverBodyPool.resize(0);
 
-       btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
-    initSolverBody(&fixedBody,0);
+       //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+    //initSolverBody(&fixedBody,0);
 
        //convert all bodies
 
        for (int i=0;i<numBodies;i++)
        {
-               int bodyId = getOrInitSolverBody(*bodies[i]);
+               int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep);
+
                btRigidBody* body = btRigidBody::upcast(bodies[i]);
                if (body && body->getInvMass())
                {
@@ -1012,9 +1091,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
                        if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE)
                        {
                                gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce);
+                               solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
                        }
-                       solverBody.m_linearVelocity += body->getTotalForce()*body->getInvMass()*infoGlobal.m_timeStep;
-                       solverBody.m_angularVelocity += (body->getTotalTorque()-gyroForce)*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
                }
        }
        
@@ -1084,8 +1162,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
                                        btRigidBody& rbA = constraint->getRigidBodyA();
                                        btRigidBody& rbB = constraint->getRigidBodyB();
 
-                    int solverBodyIdA = getOrInitSolverBody(rbA);
-                    int solverBodyIdB = getOrInitSolverBody(rbB);
+                                       int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep);
+                    int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep);
 
                     btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
                     btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
@@ -1182,15 +1260,22 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
                                                }
 
 
-                                               ///fix rhs
-                                               ///todo: add force/torque accelerators
+                                               
                                                {
                                                        btScalar rel_vel;
-                                                       btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
-                                                       btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
+                                                       btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0);
+                                                       btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0);
+
+                                                       btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0);
+                                                       btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0);
+                                                       
+                                                       btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA) 
+                                                                                               + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA);
+                                                       
+                                                       btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB) 
+                                                                                                                               + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB);
 
                                                        rel_vel = vel1Dotn+vel2Dotn;
-
                                                        btScalar restitution = 0.f;
                                                        btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
                                                        btScalar        velocityError = restitution - rel_vel * info2.m_damping;
@@ -1199,6 +1284,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
                                                        solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
                                                        solverConstraint.m_appliedImpulse = 0.f;
 
+
                                                }
                                        }
                                }
@@ -1206,18 +1292,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
                        }
                }
 
-               {
-                       int i;
-                       btPersistentManifold* manifold = 0;
-//                     btCollisionObject* colObj0=0,*colObj1=0;
-
+               convertContacts(manifoldPtr,numManifolds,infoGlobal);
 
-                       for (i=0;i<numManifolds;i++)
-                       {
-                               manifold = manifoldPtr[i];
-                               convertContact(manifold,infoGlobal);
-                       }
-               }
        }
 
 //     btContactSolverInfo info = infoGlobal;
@@ -1256,7 +1332,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 }
 
 
-btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
+btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
 {
 
        int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
@@ -1309,14 +1385,14 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
                {
                        for (int j=0;j<numConstraints;j++)
                        {
-                if (constraints[j]->isEnabled())
-                {
-                    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);
-                }
+                               if (constraints[j]->isEnabled())
+                               {
+                                       int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
+                                       int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
+                                       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
@@ -1376,7 +1452,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
                                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);
+                                       //resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+                                       resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 
                                }
                
@@ -1395,7 +1472,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
                                                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);
+                                               //resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+                                               resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
                                        }
                                }
 
@@ -1437,14 +1515,14 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
                {
                        for (int j=0;j<numConstraints;j++)
                        {
-                if (constraints[j]->isEnabled())
-                {
-                    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);
-                }
+                               if (constraints[j]->isEnabled())
+                               {
+                                       int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
+                                       int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
+                                       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();
@@ -1492,7 +1570,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 }
 
 
-void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
 {
        int iteration;
        if (infoGlobal.m_splitImpulse)
@@ -1532,20 +1610,20 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
        }
 }
 
-btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
 {
        BT_PROFILE("solveGroupCacheFriendlyIterations");
 
        {
                ///this is a special step to resolve penetrations (just for contacts)
-               solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
+               solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
 
                int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
 
                for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
                //for ( int iteration = maxIterations-1  ; iteration >= 0;iteration--)
                {                       
-                       solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
+                       solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
                }
                
        }
@@ -1610,9 +1688,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
                                m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
                        else
                                m_tmpSolverBodyPool[i].writebackVelocity();
+                       
+                       m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(
+                               m_tmpSolverBodyPool[i].m_linearVelocity+
+                               m_tmpSolverBodyPool[i].m_externalForceImpulse);
+
+                       m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(
+                               m_tmpSolverBodyPool[i].m_angularVelocity+
+                               m_tmpSolverBodyPool[i].m_externalTorqueImpulse);
 
-                       m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity);
-                       m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity);
                        if (infoGlobal.m_splitImpulse)
                                m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform);
 
@@ -1632,15 +1716,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
 
 
 /// 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*/)
+btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btDispatcher* /*dispatcher*/)
 {
 
        BT_PROFILE("solveGroup");
        //you need to provide at least some bodies
        
-       solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
+       solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
 
-       solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
+       solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
 
        solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
        
index 2eea6be0db296a44d8df036985fe3881cd8bab63..180d2a385d3c98a9147149c13c6da45f204da82f 100644 (file)
@@ -18,7 +18,6 @@ subject to the following restrictions:
 
 class btIDebugDraw;
 class btPersistentManifold;
-class btStackAlloc;
 class btDispatcher;
 class btCollisionObject;
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
@@ -43,7 +42,7 @@ protected:
        btAlignedObjectArray<int>       m_orderFrictionConstraintPool;
        btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
        int                                                     m_maxOverrideNumSolverIterations;
-
+       int m_fixedBodyId;
        void setupFrictionConstraint(   btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
                                                                        btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
                                                                        btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 
@@ -57,10 +56,11 @@ protected:
        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, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
        btSolverConstraint&     addRollingFrictionConstraint(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, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);
 
-
+       
        void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, 
-                                                               const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation, 
-                                                               btVector3& rel_pos1, btVector3& rel_pos2);
+                                                               const btContactSolverInfo& infoGlobal,btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2);
+
+       static void     applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode);
 
        void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, 
                                                                                 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
@@ -71,6 +71,8 @@ protected:
        
        btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
 
+       virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
+
        void    convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
 
 
@@ -83,8 +85,8 @@ protected:
         const btSolverConstraint& contactConstraint);
 
        //internal method
-       int             getOrInitSolverBody(btCollisionObject& body);
-       void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
+       int             getOrInitSolverBody(btCollisionObject& body,btScalar timeStep);
+       void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep);
 
        void    resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
 
@@ -97,12 +99,12 @@ protected:
 protected:
        
        
-       virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+       virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
        virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
-       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 solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
 
-       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);
+       virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+       virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
 
 
 public:
@@ -112,7 +114,7 @@ public:
        btSequentialImpulseConstraintSolver();
        virtual ~btSequentialImpulseConstraintSolver();
 
-       virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
+       virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
        
 
        
@@ -132,6 +134,11 @@ public:
                return m_btSeed2;
        }
 
+       
+       virtual btConstraintSolverType  getSolverType() const
+       {
+               return BT_SEQUENTIAL_IMPULSE_SOLVER;
+       }
 };
 
 
index ca8e715bc47ef0a79bf7a907e0819544b19595b1..57ebb47d8e720aa35aa2369a62347e386f2173ef 100755 (executable)
@@ -25,7 +25,13 @@ TODO:
 #ifndef BT_SLIDER_CONSTRAINT_H
 #define BT_SLIDER_CONSTRAINT_H
 
-
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btSliderConstraintData2                btSliderConstraintDoubleData
+#define btSliderConstraintDataName  "btSliderConstraintDoubleData"
+#else
+#define btSliderConstraintData2                btSliderConstraintData 
+#define btSliderConstraintDataName     "btSliderConstraintData"
+#endif //BT_USE_DOUBLE_PRECISION
 
 #include "LinearMath/btVector3.h"
 #include "btJacobianEntry.h"
@@ -283,7 +289,10 @@ public:
 
 };
 
+
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+
+
 struct btSliderConstraintData
 {
        btTypedConstraintData   m_typeConstraintData;
@@ -302,31 +311,48 @@ struct btSliderConstraintData
 };
 
 
+struct btSliderConstraintDoubleData
+{
+       btTypedConstraintDoubleData     m_typeConstraintData;
+       btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+       btTransformDoubleData m_rbBFrame;
+       
+       double  m_linearUpperLimit;
+       double  m_linearLowerLimit;
+
+       double  m_angularUpperLimit;
+       double  m_angularLowerLimit;
+
+       int     m_useLinearReferenceFrameA;
+       int m_useOffsetForConstraintFrame;
+
+};
+
 SIMD_FORCE_INLINE              int     btSliderConstraint::calculateSerializeBufferSize() const
 {
-       return sizeof(btSliderConstraintData);
+       return sizeof(btSliderConstraintData2);
 }
 
        ///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;
+       btSliderConstraintData2* sliderData = (btSliderConstraintData2*) dataBuffer;
        btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer);
 
-       m_frameInA.serializeFloat(sliderData->m_rbAFrame);
-       m_frameInB.serializeFloat(sliderData->m_rbBFrame);
+       m_frameInA.serialize(sliderData->m_rbAFrame);
+       m_frameInB.serialize(sliderData->m_rbBFrame);
 
-       sliderData->m_linearUpperLimit = float(m_upperLinLimit);
-       sliderData->m_linearLowerLimit = float(m_lowerLinLimit);
+       sliderData->m_linearUpperLimit = m_upperLinLimit;
+       sliderData->m_linearLowerLimit = m_lowerLinLimit;
 
-       sliderData->m_angularUpperLimit = float(m_upperAngLimit);
-       sliderData->m_angularLowerLimit = float(m_lowerAngLimit);
+       sliderData->m_angularUpperLimit = m_upperAngLimit;
+       sliderData->m_angularLowerLimit = m_lowerAngLimit;
 
        sliderData->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA;
        sliderData->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame;
 
-       return "btSliderConstraintData";
+       return btSliderConstraintDataName;
 }
 
 
index ccc45996c8b727f4416ce3981825908f0e680c04..27ccefe41690425b83168383af17751416fdaeda 100644 (file)
@@ -118,6 +118,8 @@ ATTRIBUTE_ALIGNED16 (struct)        btSolverBody
        btVector3               m_turnVelocity;
        btVector3               m_linearVelocity;
        btVector3               m_angularVelocity;
+       btVector3               m_externalForceImpulse;
+       btVector3               m_externalTorqueImpulse;
 
        btRigidBody*    m_originalBody;
        void    setWorldTransform(const btTransform& worldTransform)
@@ -130,6 +132,17 @@ ATTRIBUTE_ALIGNED16 (struct)       btSolverBody
                return m_worldTransform;
        }
        
+       
+
+       SIMD_FORCE_INLINE void  getVelocityInLocalPointNoDelta(const btVector3& rel_pos, btVector3& velocity ) const
+       {
+               if (m_originalBody)
+                       velocity = m_linearVelocity + m_externalForceImpulse + (m_angularVelocity+m_externalTorqueImpulse).cross(rel_pos);
+               else
+                       velocity.setValue(0,0,0);
+       }
+
+
        SIMD_FORCE_INLINE void  getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
        {
                if (m_originalBody)
index 2ade61b2f69529f26874826d03249a05be260ee8..5515e6b311c721ba2aab0a79e31e9ae06a5db6b5 100644 (file)
@@ -55,6 +55,7 @@ ATTRIBUTE_ALIGNED16 (struct)  btSolverConstraint
        {
                void*           m_originalContactPoint;
                btScalar        m_unusedPadding4;
+               int                     m_numRowsForNonContactConstraint;
        };
 
        int     m_overrideNumSolverIterations;
index 465c0746c584bda852043d2b39129c05672c002b..27fdd9d3df8c31f54b5d3612645a94817822014c 100644 (file)
@@ -109,7 +109,7 @@ btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScal
 ///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;
+       btTypedConstraintData2* tcd = (btTypedConstraintData2*) dataBuffer;
 
        tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA);
        tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB);
@@ -123,14 +123,14 @@ const char*       btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali
        tcd->m_objectType = m_objectType;
        tcd->m_needsFeedback = m_needsFeedback;
        tcd->m_overrideNumSolverIterations = m_overrideNumSolverIterations;
-       tcd->m_breakingImpulseThreshold = float(m_breakingImpulseThreshold);
+       tcd->m_breakingImpulseThreshold = m_breakingImpulseThreshold;
        tcd->m_isEnabled = m_isEnabled? 1: 0;
        
        tcd->m_userConstraintId =m_userConstraintId;
        tcd->m_userConstraintType =m_userConstraintType;
 
-       tcd->m_appliedImpulse = float(m_appliedImpulse);
-       tcd->m_dbgDrawSize = float(m_dbgDrawSize );
+       tcd->m_appliedImpulse = m_appliedImpulse;
+       tcd->m_dbgDrawSize = m_dbgDrawSize;
 
        tcd->m_disableCollisionsBetweenLinkedBodies = false;
 
@@ -142,7 +142,7 @@ const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali
                if (m_rbB.getConstraintRef(i) == this)
                        tcd->m_disableCollisionsBetweenLinkedBodies = true;
 
-       return "btTypedConstraintData";
+       return btTypedConstraintDataName;
 }
 
 btRigidBody& btTypedConstraint::getFixedBody()
index 441fa375050a49cbabecd1baf723788c9262ec81..b58f984d0fb83059f0597871b888b67a7330f963 100644 (file)
@@ -21,6 +21,15 @@ subject to the following restrictions:
 #include "btSolverConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btTypedConstraintData2         btTypedConstraintDoubleData
+#define btTypedConstraintDataName      "btTypedConstraintDoubleData"
+#else
+#define btTypedConstraintData2                 btTypedConstraintFloatData
+#define btTypedConstraintDataName  "btTypedConstraintFloatData" 
+#endif //BT_USE_DOUBLE_PRECISION
+
+
 class btSerializer;
 
 //Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
@@ -34,6 +43,7 @@ enum btTypedConstraintType
        CONTACT_CONSTRAINT_TYPE,
        D6_SPRING_CONSTRAINT_TYPE,
        GEAR_CONSTRAINT_TYPE,
+       FIXED_CONSTRAINT_TYPE,
        MAX_CONSTRAINT_TYPE
 };
 
@@ -356,6 +366,33 @@ SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScal
 }
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btTypedConstraintFloatData
+{
+       btRigidBodyFloatData            *m_rbA;
+       btRigidBodyFloatData            *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;
+       int     m_overrideNumSolverIterations;
+
+       float   m_breakingImpulseThreshold;
+       int             m_isEnabled;
+       
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+
+#define BT_BACKWARDS_COMPATIBLE_SERIALIZATION
+#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
+///this structure is not used, except for loading pre-2.82 .bullet files
 struct btTypedConstraintData
 {
        btRigidBodyData         *m_rbA;
@@ -377,10 +414,35 @@ struct    btTypedConstraintData
        int             m_isEnabled;
        
 };
+#endif //BACKWARDS_COMPATIBLE
+
+struct btTypedConstraintDoubleData
+{
+       btRigidBodyDoubleData           *m_rbA;
+       btRigidBodyDoubleData           *m_rbB;
+       char    *m_name;
+
+       int     m_objectType;
+       int     m_userConstraintType;
+       int     m_userConstraintId;
+       int     m_needsFeedback;
+
+       double  m_appliedImpulse;
+       double  m_dbgDrawSize;
+
+       int     m_disableCollisionsBetweenLinkedBodies;
+       int     m_overrideNumSolverIterations;
+
+       double  m_breakingImpulseThreshold;
+       int             m_isEnabled;
+       char    padding[4];
+       
+};
+
 
 SIMD_FORCE_INLINE      int     btTypedConstraint::calculateSerializeBufferSize() const
 {
-       return sizeof(btTypedConstraintData);
+       return sizeof(btTypedConstraintData2);
 }
 
 
index 9ff2d9f1173d4c4ac853053afc3f35709d7bc477..fb8a4068e2386a0e56c6e64334b470b5ab963cbb 100644 (file)
@@ -87,7 +87,6 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
        btTypedConstraint**             m_sortedConstraints;
        int                                             m_numConstraints;
        btIDebugDraw*                   m_debugDrawer;
-       btStackAlloc*                   m_stackAlloc;
        btDispatcher*                   m_dispatcher;
        
        btAlignedObjectArray<btCollisionObject*> m_bodies;
@@ -104,7 +103,6 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
                m_sortedConstraints(NULL),
                m_numConstraints(0),
                m_debugDrawer(NULL),
-               m_stackAlloc(stackAlloc),
                m_dispatcher(dispatcher)
        {
 
@@ -135,7 +133,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
                if (islandId<0)
                {
                        ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
-                       m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+                       m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
                } else
                {
                                //also add all non-contact constraints/joints for this island
@@ -163,7 +161,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 
                        if (m_solverInfo->m_minimumSolverBatchSize<=1)
                        {
-                               m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+                               m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
                        } else
                        {
                                
@@ -190,7 +188,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
                btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
                btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
                        
-               m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+               m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_dispatcher);
                m_bodies.resize(0);
                m_manifolds.resize(0);
                m_constraints.resize(0);
@@ -210,7 +208,9 @@ m_gravity(0,-10,0),
 m_localTime(0),
 m_synchronizeAllMotionStates(false),
 m_applySpeculativeContactRestitution(false),
-m_profileTimings(0)
+m_profileTimings(0),
+m_fixedTimeStep(0),
+m_latencyMotionStateInterpolation(true)
 
 {
        if (!m_constraintSolver)
@@ -232,7 +232,7 @@ m_profileTimings(0)
 
        {
                void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallback),16);
-               m_solverIslandCallback = new (mem) InplaceSolverIslandCallback (m_constraintSolver, m_stackAlloc, dispatcher);
+               m_solverIslandCallback = new (mem) InplaceSolverIslandCallback (m_constraintSolver, 0, dispatcher);
        }
 }
 
@@ -359,7 +359,9 @@ void        btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
                {
                        btTransform interpolatedTransform;
                        btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
-                               body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform);
+                               body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),
+                               (m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime*body->getHitFraction(),
+                               interpolatedTransform);
                        body->getMotionState()->setWorldTransform(interpolatedTransform);
                }
        }
@@ -403,6 +405,7 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
        if (maxSubSteps)
        {
                //fixed timestep with interpolation
+               m_fixedTimeStep = fixedTimeStep;
                m_localTime += timeStep;
                if (m_localTime >= fixedTimeStep)
                {
@@ -413,7 +416,8 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
        {
                //variable timestep
                fixedTimeStep = timeStep;
-               m_localTime = timeStep;
+               m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep;
+               m_fixedTimeStep = 0;
                if (btFuzzyZero(timeStep))
                {
                        numSimulationSubSteps = 0;
@@ -724,7 +728,7 @@ void        btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 
        m_solverIslandCallback->processConstraints();
 
-       m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
+       m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
 }
 
 
@@ -746,12 +750,7 @@ void       btDiscreteDynamicsWorld::calculateSimulationIslands()
             if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
                 ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
             {
-                if (colObj0->isActive() || colObj1->isActive())
-                {
-                    
-                    getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
-                                                                       (colObj1)->getIslandTag());
-                }
+                               getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
             }
         }
     }
@@ -770,12 +769,7 @@ void       btDiscreteDynamicsWorld::calculateSimulationIslands()
                                if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
                                        ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
                                {
-                                       if (colObj0->isActive() || colObj1->isActive())
-                                       {
-
-                                               getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
-                                                       (colObj1)->getIslandTag());
-                                       }
+                                       getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
                                }
                        }
                }
@@ -1131,7 +1125,6 @@ void      btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
                {
                        //don't integrate/update velocities here, it happens in the constraint solver
 
-                       //damping
                        body->applyDamping(timeStep);
 
                        body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
index fa934c49d2b8bb917f8639b79ea44ddde0917b14..d8a34b7da3d1ce2537eab071fb1c3c0dad44cf6b 100644 (file)
@@ -53,6 +53,7 @@ protected:
 
        //for variable timesteps
        btScalar        m_localTime;
+       btScalar        m_fixedTimeStep;
        //for variable timesteps
 
        bool    m_ownsIslandManager;
@@ -64,6 +65,8 @@ protected:
        
        int     m_profileTimings;
 
+       bool    m_latencyMotionStateInterpolation;
+
        btAlignedObjectArray<btPersistentManifold*>     m_predictiveManifolds;
 
        virtual void    predictUnconstraintMotion(btScalar timeStep);
@@ -74,7 +77,7 @@ protected:
 
        virtual void    solveConstraints(btContactSolverInfo& solverInfo);
        
-       void    updateActivationState(btScalar timeStep);
+       virtual void    updateActivationState(btScalar timeStep);
 
        void    updateActions(btScalar timeStep);
 
@@ -216,6 +219,16 @@ public:
        ///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see Bullet/Demos/SerializeDemo)
        virtual void    serialize(btSerializer* serializer);
 
+       ///Interpolate motion state between previous and current transform, instead of current and next transform.
+       ///This can relieve discontinuities in the rendering, due to penetrations
+       void setLatencyMotionStateInterpolation(bool latencyInterpolation )
+       {
+               m_latencyMotionStateInterpolation = latencyInterpolation;
+       }
+       bool getLatencyMotionStateInterpolation() const
+       {
+               return m_latencyMotionStateInterpolation;
+       }
 };
 
 #endif //BT_DISCRETE_DYNAMICS_WORLD_H
index 7d5c621f850908a103028234ba84c421eb581484..35dd1400fe7e11e482a5e3b613b5e47e373e31ef 100644 (file)
@@ -33,7 +33,8 @@ enum btDynamicsWorldType
        BT_SIMPLE_DYNAMICS_WORLD=1,
        BT_DISCRETE_DYNAMICS_WORLD=2,
        BT_CONTINUOUS_DYNAMICS_WORLD=3,
-       BT_SOFT_RIGID_DYNAMICS_WORLD=4
+       BT_SOFT_RIGID_DYNAMICS_WORLD=4,
+       BT_GPU_DYNAMICS_WORLD=5
 };
 
 ///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc.
index f0e07f942afb85705e893ab9fb5ac921ab025a0c..ed90fb44115db47b21fb146570bd7ef00d91cd15 100644 (file)
@@ -363,11 +363,13 @@ public:
 
        inline void setLinearVelocity(const btVector3& lin_vel)
        { 
+               m_updateRevision++;
                m_linearVelocity = lin_vel; 
        }
 
        inline void setAngularVelocity(const btVector3& ang_vel) 
        { 
+               m_updateRevision++;
                m_angularVelocity = ang_vel; 
        }
 
@@ -484,11 +486,13 @@ public:
 
        void    setAngularFactor(const btVector3& angFac)
        {
+               m_updateRevision++;
                m_angularFactor = angFac;
        }
 
        void    setAngularFactor(btScalar angFac)
        {
+               m_updateRevision++;
                m_angularFactor.setValue(angFac,angFac,angFac);
        }
        const btVector3&        getAngularFactor() const
index 5fc2f3cf8f7aed4aa3912e9e11ccbe3a828fb43b..35dd38840f608927f78c1cbe659d4fa80a55c4d2 100644 (file)
@@ -78,8 +78,8 @@ int           btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b
                btContactSolverInfo infoGlobal;
                infoGlobal.m_timeStep = timeStep;
                m_constraintSolver->prepareSolve(0,numManifolds);
-               m_constraintSolver->solveGroup(&getCollisionObjectArray()[0],getNumCollisionObjects(),manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc,m_dispatcher1);
-               m_constraintSolver->allSolved(infoGlobal,m_debugDrawer, m_stackAlloc);
+               m_constraintSolver->solveGroup(&getCollisionObjectArray()[0],getNumCollisionObjects(),manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_dispatcher1);
+               m_constraintSolver->allSolved(infoGlobal,m_debugDrawer);
        }
 
        ///integrate transforms
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp
new file mode 100644 (file)
index 0000000..56a1c55
--- /dev/null
@@ -0,0 +1,1009 @@
+/*
+ * PURPOSE:
+ *   Class representing an articulated rigid body. Stores the body's
+ *   current state, allows forces and torques to be set, handles
+ *   timestepping and implements Featherstone's algorithm.
+ *   
+ * COPYRIGHT:
+ *   Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
+ *   Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
+
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+
+#include "btMultiBody.h"
+#include "btMultiBodyLink.h"
+#include "btMultiBodyLinkCollider.h"
+
+// #define INCLUDE_GYRO_TERM 
+
+namespace {
+    const btScalar SLEEP_EPSILON = btScalar(0.05);  // this is a squared velocity (m^2 s^-2)
+    const btScalar SLEEP_TIMEOUT = btScalar(2);     // in seconds
+}
+
+
+
+
+//
+// Various spatial helper functions
+//
+
+namespace {
+    void SpatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in 'from' frame to vectors in 'to' frame
+                          const btVector3 &displacement,     // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
+                          const btVector3 &top_in,       // top part of input vector
+                          const btVector3 &bottom_in,    // bottom part of input vector
+                          btVector3 &top_out,         // top part of output vector
+                          btVector3 &bottom_out)      // bottom part of output vector
+    {
+        top_out = rotation_matrix * top_in;
+        bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in;
+    }
+
+    void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix,
+                                 const btVector3 &displacement,
+                                 const btVector3 &top_in,
+                                 const btVector3 &bottom_in,
+                                 btVector3 &top_out,
+                                 btVector3 &bottom_out)
+    {
+        top_out = rotation_matrix.transpose() * top_in;
+        bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in));
+    }
+
+    btScalar SpatialDotProduct(const btVector3 &a_top,
+                            const btVector3 &a_bottom,
+                            const btVector3 &b_top,
+                            const btVector3 &b_bottom)
+    {
+        return a_bottom.dot(b_top) + a_top.dot(b_bottom);
+    }
+}
+
+
+//
+// Implementation of class btMultiBody
+//
+
+btMultiBody::btMultiBody(int n_links,
+                     btScalar mass,
+                     const btVector3 &inertia,
+                     bool fixed_base_,
+                     bool can_sleep_)
+    : base_quat(0, 0, 0, 1),
+      base_mass(mass),
+      base_inertia(inertia),
+    
+               fixed_base(fixed_base_),
+               awake(true),
+               can_sleep(can_sleep_),
+               sleep_timer(0),
+               m_baseCollider(0),
+               m_linearDamping(0.04f),
+               m_angularDamping(0.04f),
+               m_useGyroTerm(true),
+               m_maxAppliedImpulse(1000.f),
+               m_hasSelfCollision(true)
+{
+        links.resize(n_links);
+
+       vector_buf.resize(2*n_links);
+    matrix_buf.resize(n_links + 1);
+       m_real_buf.resize(6 + 2*n_links);
+    base_pos.setValue(0, 0, 0);
+    base_force.setValue(0, 0, 0);
+    base_torque.setValue(0, 0, 0);
+}
+
+btMultiBody::~btMultiBody()
+{
+}
+
+void btMultiBody::setupPrismatic(int i,
+                               btScalar mass,
+                               const btVector3 &inertia,
+                               int parent,
+                               const btQuaternion &rot_parent_to_this,
+                               const btVector3 &joint_axis,
+                               const btVector3 &r_vector_when_q_zero,
+                                                          bool disableParentCollision)
+{
+    links[i].mass = mass;
+    links[i].inertia = inertia;
+    links[i].parent = parent;
+    links[i].zero_rot_parent_to_this = rot_parent_to_this;
+    links[i].axis_top.setValue(0,0,0);
+    links[i].axis_bottom = joint_axis;
+    links[i].e_vector = r_vector_when_q_zero;
+    links[i].is_revolute = false;
+    links[i].cached_rot_parent_to_this = rot_parent_to_this;
+       if (disableParentCollision)
+               links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
+
+    links[i].updateCache();
+}
+
+void btMultiBody::setupRevolute(int i,
+                              btScalar mass,
+                              const btVector3 &inertia,
+                              int parent,
+                              const btQuaternion &zero_rot_parent_to_this,
+                              const btVector3 &joint_axis,
+                              const btVector3 &parent_axis_position,
+                              const btVector3 &my_axis_position,
+                                                         bool disableParentCollision)
+{
+    links[i].mass = mass;
+    links[i].inertia = inertia;
+    links[i].parent = parent;
+    links[i].zero_rot_parent_to_this = zero_rot_parent_to_this;
+    links[i].axis_top = joint_axis;
+    links[i].axis_bottom = joint_axis.cross(my_axis_position);
+    links[i].d_vector = my_axis_position;
+    links[i].e_vector = parent_axis_position;
+    links[i].is_revolute = true;
+       if (disableParentCollision)
+               links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
+    links[i].updateCache();
+}
+
+
+
+
+       
+int btMultiBody::getParent(int i) const
+{
+    return links[i].parent;
+}
+
+btScalar btMultiBody::getLinkMass(int i) const
+{
+    return links[i].mass;
+}
+
+const btVector3 & btMultiBody::getLinkInertia(int i) const
+{
+    return links[i].inertia;
+}
+
+btScalar btMultiBody::getJointPos(int i) const
+{
+    return links[i].joint_pos;
+}
+
+btScalar btMultiBody::getJointVel(int i) const
+{
+    return m_real_buf[6 + i];
+}
+
+void btMultiBody::setJointPos(int i, btScalar q)
+{
+    links[i].joint_pos = q;
+    links[i].updateCache();
+}
+
+void btMultiBody::setJointVel(int i, btScalar qdot)
+{
+    m_real_buf[6 + i] = qdot;
+}
+
+const btVector3 & btMultiBody::getRVector(int i) const
+{
+    return links[i].cached_r_vector;
+}
+
+const btQuaternion & btMultiBody::getParentToLocalRot(int i) const
+{
+    return links[i].cached_rot_parent_to_this;
+}
+
+btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
+{
+    btVector3 result = local_pos;
+    while (i != -1) {
+        // 'result' is in frame i. transform it to frame parent(i)
+        result += getRVector(i);
+        result = quatRotate(getParentToLocalRot(i).inverse(),result);
+        i = getParent(i);
+    }
+
+    // 'result' is now in the base frame. transform it to world frame
+    result = quatRotate(getWorldToBaseRot().inverse() ,result);
+    result += getBasePos();
+
+    return result;
+}
+
+btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
+{
+    if (i == -1) {
+        // world to base
+        return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos()));
+    } else {
+        // find position in parent frame, then transform to current frame
+        return quatRotate(getParentToLocalRot(i),worldPosToLocal(getParent(i), world_pos)) - getRVector(i);
+    }
+}
+
+btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
+{
+    btVector3 result = local_dir;
+    while (i != -1) {
+        result = quatRotate(getParentToLocalRot(i).inverse() , result);
+        i = getParent(i);
+    }
+    result = quatRotate(getWorldToBaseRot().inverse() , result);
+    return result;
+}
+
+btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
+{
+    if (i == -1) {
+        return quatRotate(getWorldToBaseRot(), world_dir);
+    } else {
+        return quatRotate(getParentToLocalRot(i) ,worldDirToLocal(getParent(i), world_dir));
+    }
+}
+
+void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
+{
+       int num_links = getNumLinks();
+    // Calculates the velocities of each link (and the base) in its local frame
+    omega[0] = quatRotate(base_quat ,getBaseOmega());
+    vel[0] = quatRotate(base_quat ,getBaseVel());
+    
+    for (int i = 0; i < num_links; ++i) {
+        const int parent = links[i].parent;
+
+        // transform parent vel into this frame, store in omega[i+1], vel[i+1]
+        SpatialTransform(btMatrix3x3(links[i].cached_rot_parent_to_this), links[i].cached_r_vector,
+                         omega[parent+1], vel[parent+1],
+                         omega[i+1], vel[i+1]);
+
+        // now add qidot * shat_i
+        omega[i+1] += getJointVel(i) * links[i].axis_top;
+        vel[i+1] += getJointVel(i) * links[i].axis_bottom;
+    }
+}
+
+btScalar btMultiBody::getKineticEnergy() const
+{
+       int num_links = getNumLinks();
+    // TODO: would be better not to allocate memory here
+    btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
+       btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
+    compTreeLinkVelocities(&omega[0], &vel[0]);
+
+    // we will do the factor of 0.5 at the end
+    btScalar result = base_mass * vel[0].dot(vel[0]);
+    result += omega[0].dot(base_inertia * omega[0]);
+    
+    for (int i = 0; i < num_links; ++i) {
+        result += links[i].mass * vel[i+1].dot(vel[i+1]);
+        result += omega[i+1].dot(links[i].inertia * omega[i+1]);
+    }
+
+    return 0.5f * result;
+}
+
+btVector3 btMultiBody::getAngularMomentum() const
+{
+       int num_links = getNumLinks();
+    // TODO: would be better not to allocate memory here
+    btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
+       btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
+    btAlignedObjectArray<btQuaternion> rot_from_world;rot_from_world.resize(num_links+1);
+    compTreeLinkVelocities(&omega[0], &vel[0]);
+
+    rot_from_world[0] = base_quat;
+    btVector3 result = quatRotate(rot_from_world[0].inverse() , (base_inertia * omega[0]));
+
+    for (int i = 0; i < num_links; ++i) {
+        rot_from_world[i+1] = links[i].cached_rot_parent_to_this * rot_from_world[links[i].parent+1];
+        result += (quatRotate(rot_from_world[i+1].inverse() , (links[i].inertia * omega[i+1])));
+    }
+
+    return result;
+}
+
+
+void btMultiBody::clearForcesAndTorques()
+{
+    base_force.setValue(0, 0, 0);
+    base_torque.setValue(0, 0, 0);
+
+    for (int i = 0; i < getNumLinks(); ++i) {
+        links[i].applied_force.setValue(0, 0, 0);
+        links[i].applied_torque.setValue(0, 0, 0);
+        links[i].joint_torque = 0;
+    }
+}
+
+void btMultiBody::clearVelocities()
+{
+       for (int i = 0; i < 6 + getNumLinks(); ++i) 
+       {
+               m_real_buf[i] = 0.f;
+       }
+}
+void btMultiBody::addLinkForce(int i, const btVector3 &f)
+{
+    links[i].applied_force += f;
+}
+
+void btMultiBody::addLinkTorque(int i, const btVector3 &t)
+{
+    links[i].applied_torque += t;
+}
+
+void btMultiBody::addJointTorque(int i, btScalar Q)
+{
+    links[i].joint_torque += Q;
+}
+
+const btVector3 & btMultiBody::getLinkForce(int i) const
+{
+    return links[i].applied_force;
+}
+
+const btVector3 & btMultiBody::getLinkTorque(int i) const
+{
+    return links[i].applied_torque;
+}
+
+btScalar btMultiBody::getJointTorque(int i) const
+{
+    return links[i].joint_torque;
+}
+
+
+inline btMatrix3x3 vecMulVecTranspose(const btVector3& v0, const btVector3& v1Transposed)
+{
+               btVector3 row0 = btVector3( 
+                       v0.x() * v1Transposed.x(),
+                       v0.x() * v1Transposed.y(),
+                       v0.x() * v1Transposed.z());
+               btVector3 row1 = btVector3( 
+                       v0.y() * v1Transposed.x(),
+                       v0.y() * v1Transposed.y(),
+                       v0.y() * v1Transposed.z());
+               btVector3 row2 = btVector3( 
+                       v0.z() * v1Transposed.x(),
+                       v0.z() * v1Transposed.y(),
+                       v0.z() * v1Transposed.z());
+
+        btMatrix3x3 m(row0[0],row0[1],row0[2],
+                                               row1[0],row1[1],row1[2],
+                                               row2[0],row2[1],row2[2]);
+               return m;
+}
+
+
+void btMultiBody::stepVelocities(btScalar dt,
+                               btAlignedObjectArray<btScalar> &scratch_r,
+                               btAlignedObjectArray<btVector3> &scratch_v,
+                               btAlignedObjectArray<btMatrix3x3> &scratch_m)
+{
+    // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
+    // and the base linear & angular accelerations.
+
+    // We apply damping forces in this routine as well as any external forces specified by the 
+    // caller (via addBaseForce etc).
+
+    // output should point to an array of 6 + num_links reals.
+    // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
+    // num_links joint acceleration values.
+    
+       int num_links = getNumLinks();
+
+    const btScalar DAMPING_K1_LINEAR = m_linearDamping;
+       const btScalar DAMPING_K2_LINEAR = m_linearDamping;
+
+       const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
+       const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
+
+    btVector3 base_vel = getBaseVel();
+    btVector3 base_omega = getBaseOmega();
+
+    // Temporary matrices/vectors -- use scratch space from caller
+    // so that we don't have to keep reallocating every frame
+
+    scratch_r.resize(2*num_links + 6);
+    scratch_v.resize(8*num_links + 6);
+    scratch_m.resize(4*num_links + 4);
+
+    btScalar * r_ptr = &scratch_r[0];
+    btScalar * output = &scratch_r[num_links];  // "output" holds the q_double_dot results
+    btVector3 * v_ptr = &scratch_v[0];
+    
+    // vhat_i  (top = angular, bottom = linear part)
+    btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1;
+    btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;
+
+    // zhat_i^A
+    btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
+    btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
+
+    // chat_i  (note NOT defined for the base)
+    btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
+    btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links;
+
+    // top left, top right and bottom left blocks of Ihat_i^A.
+    // bottom right block = transpose of top left block and is not stored.
+    // Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently.
+    btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1];
+    btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2];
+    btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3];
+
+    // Cached 3x3 rotation matrices from parent frame to this frame.
+    btMatrix3x3 * rot_from_parent = &matrix_buf[0];
+    btMatrix3x3 * rot_from_world = &scratch_m[0];
+
+    // hhat_i, ahat_i
+    // hhat is NOT stored for the base (but ahat is)
+    btVector3 * h_top = num_links > 0 ? &vector_buf[0] : 0;
+    btVector3 * h_bottom = num_links > 0 ? &vector_buf[num_links] : 0;
+    btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
+    btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
+
+    // Y_i, D_i
+    btScalar * Y = r_ptr; r_ptr += num_links;
+    btScalar * D = num_links > 0 ? &m_real_buf[6 + num_links] : 0;
+
+    // ptr to the joint accel part of the output
+    btScalar * joint_accel = output + 6;
+
+
+    // Start of the algorithm proper.
+    
+    // First 'upward' loop.
+    // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
+
+    rot_from_parent[0] = btMatrix3x3(base_quat);
+
+    vel_top_angular[0] = rot_from_parent[0] * base_omega;
+    vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
+
+    if (fixed_base) {
+        zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
+    } else {
+        zero_acc_top_angular[0] = - (rot_from_parent[0] * (base_force 
+                                                   - base_mass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
+               
+        zero_acc_bottom_linear[0] =
+            - (rot_from_parent[0] * base_torque);
+
+               if (m_useGyroTerm)
+                       zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( base_inertia * vel_top_angular[0] );
+
+        zero_acc_bottom_linear[0] += base_inertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
+
+    }
+
+
+
+    inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
+       
+       
+    inertia_top_right[0].setValue(base_mass, 0, 0,
+                            0, base_mass, 0,
+                            0, 0, base_mass);
+    inertia_bottom_left[0].setValue(base_inertia[0], 0, 0,
+                              0, base_inertia[1], 0,
+                              0, 0, base_inertia[2]);
+
+    rot_from_world[0] = rot_from_parent[0];
+
+    for (int i = 0; i < num_links; ++i) {
+        const int parent = links[i].parent;
+        rot_from_parent[i+1] = btMatrix3x3(links[i].cached_rot_parent_to_this);
+               
+
+        rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
+        
+        // vhat_i = i_xhat_p(i) * vhat_p(i)
+        SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
+                         vel_top_angular[parent+1], vel_bottom_linear[parent+1],
+                         vel_top_angular[i+1], vel_bottom_linear[i+1]);
+
+        // we can now calculate chat_i
+        // remember vhat_i is really vhat_p(i) (but in current frame) at this point
+        coriolis_bottom_linear[i] = vel_top_angular[i+1].cross(vel_top_angular[i+1].cross(links[i].cached_r_vector))
+            + 2 * vel_top_angular[i+1].cross(links[i].axis_bottom) * getJointVel(i);
+        if (links[i].is_revolute) {
+            coriolis_top_angular[i] = vel_top_angular[i+1].cross(links[i].axis_top) * getJointVel(i);
+            coriolis_bottom_linear[i] += (getJointVel(i) * getJointVel(i)) * links[i].axis_top.cross(links[i].axis_bottom);
+        } else {
+            coriolis_top_angular[i] = btVector3(0,0,0);
+        }
+        
+        // now set vhat_i to its true value by doing
+        // vhat_i += qidot * shat_i
+        vel_top_angular[i+1] += getJointVel(i) * links[i].axis_top;
+        vel_bottom_linear[i+1] += getJointVel(i) * links[i].axis_bottom;
+
+        // calculate zhat_i^A
+        zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (links[i].applied_force));
+        zero_acc_top_angular[i+1] += links[i].mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1];
+
+        zero_acc_bottom_linear[i+1] =
+            - (rot_from_world[i+1] * links[i].applied_torque);
+               if (m_useGyroTerm)
+               {
+                       zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( links[i].inertia * vel_top_angular[i+1] );
+               }
+
+        zero_acc_bottom_linear[i+1] += links[i].inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
+
+        // calculate Ihat_i^A
+        inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
+        inertia_top_right[i+1].setValue(links[i].mass, 0, 0,
+                                  0, links[i].mass, 0,
+                                  0, 0, links[i].mass);
+        inertia_bottom_left[i+1].setValue(links[i].inertia[0], 0, 0,
+                                    0, links[i].inertia[1], 0,
+                                    0, 0, links[i].inertia[2]);
+    }
+
+
+    // 'Downward' loop.
+    // (part of TreeForwardDynamics in Mirtich.)
+    for (int i = num_links - 1; i >= 0; --i) {
+
+        h_top[i] = inertia_top_left[i+1] * links[i].axis_top + inertia_top_right[i+1] * links[i].axis_bottom;
+        h_bottom[i] = inertia_bottom_left[i+1] * links[i].axis_top + inertia_top_left[i+1].transpose() * links[i].axis_bottom;
+               btScalar val = SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, h_top[i], h_bottom[i]);
+        D[i] = val;
+               Y[i] = links[i].joint_torque
+            - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1])
+            - SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]);
+
+        const int parent = links[i].parent;
+
+        
+        // Ip += pXi * (Ii - hi hi' / Di) * iXp
+        const btScalar one_over_di = 1.0f / D[i];
+
+               
+
+
+               const btMatrix3x3 TL = inertia_top_left[i+1]   - vecMulVecTranspose(one_over_di * h_top[i] , h_bottom[i]);
+        const btMatrix3x3 TR = inertia_top_right[i+1]  - vecMulVecTranspose(one_over_di * h_top[i] , h_top[i]);
+        const btMatrix3x3 BL = inertia_bottom_left[i+1]- vecMulVecTranspose(one_over_di * h_bottom[i] , h_bottom[i]);
+
+
+        btMatrix3x3 r_cross;
+        r_cross.setValue(
+            0, -links[i].cached_r_vector[2], links[i].cached_r_vector[1],
+            links[i].cached_r_vector[2], 0, -links[i].cached_r_vector[0],
+            -links[i].cached_r_vector[1], links[i].cached_r_vector[0], 0);
+        
+        inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1];
+        inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1];
+        inertia_bottom_left[parent+1] += rot_from_parent[i+1].transpose() *
+            (r_cross * (TL - TR * r_cross) + BL - TL.transpose() * r_cross) * rot_from_parent[i+1];
+        
+        
+        // Zp += pXi * (Zi + Ii*ci + hi*Yi/Di)
+        btVector3 in_top, in_bottom, out_top, out_bottom;
+        const btScalar Y_over_D = Y[i] * one_over_di;
+        in_top = zero_acc_top_angular[i+1]
+            + inertia_top_left[i+1] * coriolis_top_angular[i]
+            + inertia_top_right[i+1] * coriolis_bottom_linear[i]
+            + Y_over_D * h_top[i];
+        in_bottom = zero_acc_bottom_linear[i+1]
+            + inertia_bottom_left[i+1] * coriolis_top_angular[i]
+            + inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i]
+            + Y_over_D * h_bottom[i];
+        InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
+                                in_top, in_bottom, out_top, out_bottom);
+        zero_acc_top_angular[parent+1] += out_top;
+        zero_acc_bottom_linear[parent+1] += out_bottom;
+    }
+
+
+    // Second 'upward' loop
+    // (part of TreeForwardDynamics in Mirtich)
+
+    if (fixed_base) 
+       {
+        accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
+    } 
+       else 
+       {
+        if (num_links > 0) 
+               {
+            //Matrix<btScalar, 6, 6> Imatrix;
+            //Imatrix.block<3,3>(0,0) = inertia_top_left[0];
+            //Imatrix.block<3,3>(3,0) = inertia_bottom_left[0];
+            //Imatrix.block<3,3>(0,3) = inertia_top_right[0];
+            //Imatrix.block<3,3>(3,3) = inertia_top_left[0].transpose();
+            //cached_imatrix_lu.reset(new Eigen::LU<Matrix<btScalar, 6, 6> >(Imatrix));  // TODO: Avoid memory allocation here?
+
+                       cached_inertia_top_left = inertia_top_left[0];
+                       cached_inertia_top_right = inertia_top_right[0];
+                       cached_inertia_lower_left = inertia_bottom_left[0];
+                       cached_inertia_lower_right= inertia_top_left[0].transpose();
+
+        }
+               btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
+               btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
+        float result[6];
+
+               solveImatrix(rhs_top, rhs_bot, result);
+//             printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
+        for (int i = 0; i < 3; ++i) {
+            accel_top[0][i] = -result[i];
+            accel_bottom[0][i] = -result[i+3];
+        }
+
+    }
+
+    // now do the loop over the links
+    for (int i = 0; i < num_links; ++i) {
+        const int parent = links[i].parent;
+        SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
+                         accel_top[parent+1], accel_bottom[parent+1],
+                         accel_top[i+1], accel_bottom[i+1]);
+        joint_accel[i] = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
+        accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * links[i].axis_top;
+        accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * links[i].axis_bottom;
+    }
+
+    // transform base accelerations back to the world frame.
+    btVector3 omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
+       output[0] = omegadot_out[0];
+       output[1] = omegadot_out[1];
+       output[2] = omegadot_out[2];
+
+    btVector3 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
+       output[3] = vdot_out[0];
+       output[4] = vdot_out[1];
+       output[5] = vdot_out[2];
+    // Final step: add the accelerations (times dt) to the velocities.
+    applyDeltaVee(output, dt);
+
+       
+}
+
+
+
+void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const
+{
+       int num_links = getNumLinks();
+       ///solve I * x = rhs, so the result = invI * rhs
+    if (num_links == 0) 
+       {
+               // in the case of 0 links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
+        result[0] = rhs_bot[0] / base_inertia[0];
+        result[1] = rhs_bot[1] / base_inertia[1];
+        result[2] = rhs_bot[2] / base_inertia[2];
+        result[3] = rhs_top[0] / base_mass;
+        result[4] = rhs_top[1] / base_mass;
+        result[5] = rhs_top[2] / base_mass;
+    } else 
+       {
+               /// Special routine for calculating the inverse of a spatial inertia matrix
+               ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
+               btMatrix3x3 Binv = cached_inertia_top_right.inverse()*-1.f;
+               btMatrix3x3 tmp = cached_inertia_lower_right * Binv;
+               btMatrix3x3 invIupper_right = (tmp * cached_inertia_top_left + cached_inertia_lower_left).inverse();
+               tmp = invIupper_right * cached_inertia_lower_right;
+               btMatrix3x3 invI_upper_left = (tmp * Binv);
+               btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
+               tmp = cached_inertia_top_left  * invI_upper_left;
+               tmp[0][0]-= 1.0;
+               tmp[1][1]-= 1.0;
+               tmp[2][2]-= 1.0;
+               btMatrix3x3 invI_lower_left = (Binv * tmp);
+
+               //multiply result = invI * rhs
+               {
+                 btVector3 vtop = invI_upper_left*rhs_top;
+                 btVector3 tmp;
+                 tmp = invIupper_right * rhs_bot;
+                 vtop += tmp;
+                 btVector3 vbot = invI_lower_left*rhs_top;
+                 tmp = invI_lower_right * rhs_bot;
+                 vbot += tmp;
+                 result[0] = vtop[0];
+                 result[1] = vtop[1];
+                 result[2] = vtop[2];
+                 result[3] = vbot[0];
+                 result[4] = vbot[1];
+                 result[5] = vbot[2];
+               }
+               
+    }
+}
+
+
+void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output,
+                                       btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
+{
+    // Temporary matrices/vectors -- use scratch space from caller
+    // so that we don't have to keep reallocating every frame
+       int num_links = getNumLinks();
+    scratch_r.resize(num_links);
+    scratch_v.resize(4*num_links + 4);
+
+    btScalar * r_ptr = num_links == 0 ? 0 : &scratch_r[0];
+    btVector3 * v_ptr = &scratch_v[0];
+    
+    // zhat_i^A (scratch space)
+    btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
+    btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
+
+    // rot_from_parent (cached from calcAccelerations)
+    const btMatrix3x3 * rot_from_parent = &matrix_buf[0];
+
+    // hhat (cached), accel (scratch)
+    const btVector3 * h_top = num_links > 0 ? &vector_buf[0] : 0;
+    const btVector3 * h_bottom = num_links > 0 ? &vector_buf[num_links] : 0;
+    btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
+    btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
+
+    // Y_i (scratch), D_i (cached)
+    btScalar * Y = r_ptr; r_ptr += num_links;
+    const btScalar * D = num_links > 0 ? &m_real_buf[6 + num_links] : 0;
+
+    btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.size());
+    btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
+
+
+    
+    // First 'upward' loop.
+    // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
+
+    btVector3 input_force(force[3],force[4],force[5]);
+    btVector3 input_torque(force[0],force[1],force[2]);
+    
+    // Fill in zero_acc
+    // -- set to force/torque on the base, zero otherwise
+    if (fixed_base) 
+       {
+        zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
+    } else 
+       {
+        zero_acc_top_angular[0] = - (rot_from_parent[0] * input_force);
+        zero_acc_bottom_linear[0] =  - (rot_from_parent[0] * input_torque);
+    }
+    for (int i = 0; i < num_links; ++i) 
+       {
+        zero_acc_top_angular[i+1] = zero_acc_bottom_linear[i+1] = btVector3(0,0,0);
+    }
+
+    // 'Downward' loop.
+    for (int i = num_links - 1; i >= 0; --i) 
+       {
+
+        Y[i] = - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
+        Y[i] += force[6 + i];  // add joint torque
+        
+        const int parent = links[i].parent;
+        
+        // Zp += pXi * (Zi + hi*Yi/Di)
+        btVector3 in_top, in_bottom, out_top, out_bottom;
+        const btScalar Y_over_D = Y[i] / D[i];
+        in_top = zero_acc_top_angular[i+1] + Y_over_D * h_top[i];
+        in_bottom = zero_acc_bottom_linear[i+1] + Y_over_D * h_bottom[i];
+        InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
+                                in_top, in_bottom, out_top, out_bottom);
+        zero_acc_top_angular[parent+1] += out_top;
+        zero_acc_bottom_linear[parent+1] += out_bottom;
+    }
+
+    // ptr to the joint accel part of the output
+    btScalar * joint_accel = output + 6;
+
+    // Second 'upward' loop
+    if (fixed_base) 
+       {
+        accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
+    } else 
+       {
+               btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
+               btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
+               
+               float result[6];
+        solveImatrix(rhs_top,rhs_bot, result);
+       //      printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
+
+        for (int i = 0; i < 3; ++i) {
+            accel_top[0][i] = -result[i];
+            accel_bottom[0][i] = -result[i+3];
+        }
+
+    }
+    
+    // now do the loop over the links
+    for (int i = 0; i < num_links; ++i) {
+        const int parent = links[i].parent;
+        SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
+                         accel_top[parent+1], accel_bottom[parent+1],
+                         accel_top[i+1], accel_bottom[i+1]);
+        joint_accel[i] = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
+        accel_top[i+1] += joint_accel[i] * links[i].axis_top;
+        accel_bottom[i+1] += joint_accel[i] * links[i].axis_bottom;
+    }
+
+    // transform base accelerations back to the world frame.
+    btVector3 omegadot_out;
+    omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
+       output[0] = omegadot_out[0];
+       output[1] = omegadot_out[1];
+       output[2] = omegadot_out[2];
+
+    btVector3 vdot_out;
+    vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
+
+       output[3] = vdot_out[0];
+       output[4] = vdot_out[1];
+       output[5] = vdot_out[2];
+}
+
+void btMultiBody::stepPositions(btScalar dt)
+{
+       int num_links = getNumLinks();
+    // step position by adding dt * velocity
+       btVector3 v = getBaseVel();
+    base_pos += dt * v;
+
+    // "exponential map" method for the rotation
+    btVector3 base_omega = getBaseOmega();
+    const btScalar omega_norm = base_omega.norm();
+    const btScalar omega_times_dt = omega_norm * dt;
+    const btScalar SMALL_ROTATION_ANGLE = 0.02f; // Theoretically this should be ~ pow(FLT_EPSILON,0.25) which is ~ 0.0156
+    if (fabs(omega_times_dt) < SMALL_ROTATION_ANGLE) 
+       {
+        const btScalar xsq = omega_times_dt * omega_times_dt;     // |omega|^2 * dt^2
+        const btScalar sin_term = dt * (xsq / 48.0f - 0.5f);      // -sin(0.5*dt*|omega|) / |omega|
+        const btScalar cos_term = 1.0f - xsq / 8.0f;              // cos(0.5*dt*|omega|)
+        base_quat = base_quat * btQuaternion(sin_term * base_omega[0],sin_term * base_omega[1],sin_term * base_omega[2],cos_term);
+    } else 
+       {
+        base_quat = base_quat * btQuaternion(base_omega / omega_norm,-omega_times_dt);
+    }
+
+    // Make sure the quaternion represents a valid rotation.
+    // (Not strictly necessary, but helps prevent any round-off errors from building up.)
+    base_quat.normalize();
+
+    // Finally we can update joint_pos for each of the links
+    for (int i = 0; i < num_links; ++i) 
+       {
+               float jointVel = getJointVel(i);
+        links[i].joint_pos += dt * jointVel;
+        links[i].updateCache();
+    }
+}
+
+void btMultiBody::fillContactJacobian(int link,
+                                    const btVector3 &contact_point,
+                                    const btVector3 &normal,
+                                    btScalar *jac,
+                                    btAlignedObjectArray<btScalar> &scratch_r,
+                                    btAlignedObjectArray<btVector3> &scratch_v,
+                                    btAlignedObjectArray<btMatrix3x3> &scratch_m) const
+{
+    // temporary space
+       int num_links = getNumLinks();
+    scratch_v.resize(2*num_links + 2);
+    scratch_m.resize(num_links + 1);
+
+    btVector3 * v_ptr = &scratch_v[0];
+    btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1;
+    btVector3 * n_local = v_ptr; v_ptr += num_links + 1;
+    btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
+
+    scratch_r.resize(num_links);
+    btScalar * results = num_links > 0 ? &scratch_r[0] : 0;
+
+    btMatrix3x3 * rot_from_world = &scratch_m[0];
+
+    const btVector3 p_minus_com_world = contact_point - base_pos;
+
+    rot_from_world[0] = btMatrix3x3(base_quat);
+
+    p_minus_com[0] = rot_from_world[0] * p_minus_com_world;
+    n_local[0] = rot_from_world[0] * normal;
+    
+    // omega coeffients first.
+    btVector3 omega_coeffs;
+    omega_coeffs = p_minus_com_world.cross(normal);
+       jac[0] = omega_coeffs[0];
+       jac[1] = omega_coeffs[1];
+       jac[2] = omega_coeffs[2];
+    // then v coefficients
+    jac[3] = normal[0];
+    jac[4] = normal[1];
+    jac[5] = normal[2];
+
+    // Set remaining jac values to zero for now.
+    for (int i = 6; i < 6 + num_links; ++i) {
+        jac[i] = 0;
+    }
+
+    // Qdot coefficients, if necessary.
+    if (num_links > 0 && link > -1) {
+
+        // TODO: speed this up -- don't calculate for links we don't need.
+        // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
+        // which is resulting in repeated work being done...)
+
+        // calculate required normals & positions in the local frames.
+        for (int i = 0; i < num_links; ++i) {
+
+            // transform to local frame
+            const int parent = links[i].parent;
+            const btMatrix3x3 mtx(links[i].cached_rot_parent_to_this);
+            rot_from_world[i+1] = mtx * rot_from_world[parent+1];
+
+            n_local[i+1] = mtx * n_local[parent+1];
+            p_minus_com[i+1] = mtx * p_minus_com[parent+1] - links[i].cached_r_vector;
+
+            // calculate the jacobian entry
+            if (links[i].is_revolute) {
+                results[i] = n_local[i+1].dot( links[i].axis_top.cross(p_minus_com[i+1]) + links[i].axis_bottom );
+            } else {
+                results[i] = n_local[i+1].dot( links[i].axis_bottom );
+            }
+        }
+
+        // Now copy through to output.
+        while (link != -1) {
+            jac[6 + link] = results[link];
+            link = links[link].parent;
+        }
+    }
+}
+
+void btMultiBody::wakeUp()
+{
+    awake = true;
+}
+
+void btMultiBody::goToSleep()
+{
+    awake = false;
+}
+
+void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
+{
+       int num_links = getNumLinks();
+       extern bool gDisableDeactivation;
+    if (!can_sleep || gDisableDeactivation) 
+       {
+               awake = true;
+               sleep_timer = 0;
+               return;
+       }
+
+    // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
+    btScalar motion = 0;
+    for (int i = 0; i < 6 + num_links; ++i) {
+        motion += m_real_buf[i] * m_real_buf[i];
+    }
+
+    if (motion < SLEEP_EPSILON) {
+        sleep_timer += timestep;
+        if (sleep_timer > SLEEP_TIMEOUT) {
+            goToSleep();
+        }
+    } else {
+        sleep_timer = 0;
+               if (!awake)
+                       wakeUp();
+    }
+}
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h
new file mode 100644 (file)
index 0000000..7177beb
--- /dev/null
@@ -0,0 +1,466 @@
+/*
+ * PURPOSE:
+ *   Class representing an articulated rigid body. Stores the body's
+ *   current state, allows forces and torques to be set, handles
+ *   timestepping and implements Featherstone's algorithm.
+ *   
+ * COPYRIGHT:
+ *   Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
+ *   Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
+
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+
+#ifndef BT_MULTIBODY_H
+#define BT_MULTIBODY_H
+
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btQuaternion.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+
+#include "btMultiBodyLink.h"
+class btMultiBodyLinkCollider;
+
+class btMultiBody 
+{
+public:
+
+    
+       BT_DECLARE_ALIGNED_ALLOCATOR();
+
+    //
+    // initialization
+    //
+    
+    btMultiBody(int n_links,                // NOT including the base
+              btScalar mass,                // mass of base
+              const btVector3 &inertia,    // inertia of base, in base frame; assumed diagonal
+              bool fixed_base_,           // whether the base is fixed (true) or can move (false)
+              bool can_sleep_);
+
+    ~btMultiBody();
+    
+    void setupPrismatic(int i,             // 0 to num_links-1
+                        btScalar mass,
+                        const btVector3 &inertia,       // in my frame; assumed diagonal
+                        int parent,
+                        const btQuaternion &rot_parent_to_this,  // rotate points in parent frame to my frame.
+                        const btVector3 &joint_axis,             // in my frame
+                        const btVector3 &r_vector_when_q_zero,  // vector from parent COM to my COM, in my frame, when q = 0.
+                                               bool disableParentCollision=false
+                                               );
+
+    void setupRevolute(int i,            // 0 to num_links-1
+                       btScalar mass,
+                       const btVector3 &inertia,
+                       int parent,
+                       const btQuaternion &zero_rot_parent_to_this,  // rotate points in parent frame to this frame, when q = 0
+                       const btVector3 &joint_axis,    // in my frame
+                       const btVector3 &parent_axis_position,    // vector from parent COM to joint axis, in PARENT frame
+                       const btVector3 &my_axis_position,       // vector from joint axis to my COM, in MY frame
+                                          bool disableParentCollision=false);
+       
+       const btMultibodyLink& getLink(int index) const
+       {
+               return links[index];
+       }
+
+       btMultibodyLink& getLink(int index)
+       {
+               return links[index];
+       }
+
+
+       void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base
+       {
+               m_baseCollider = collider;
+       }
+       const btMultiBodyLinkCollider* getBaseCollider() const
+       {
+               return m_baseCollider;
+       }
+       btMultiBodyLinkCollider* getBaseCollider()
+       {
+               return m_baseCollider;
+       }
+
+    //
+    // get parent
+    // input: link num from 0 to num_links-1
+    // output: link num from 0 to num_links-1, OR -1 to mean the base.
+    //
+    int getParent(int link_num) const;
+    
+    
+    //
+    // get number of links, masses, moments of inertia
+    //
+
+    int getNumLinks() const { return links.size(); }
+    btScalar getBaseMass() const { return base_mass; }
+    const btVector3 & getBaseInertia() const { return base_inertia; }
+    btScalar getLinkMass(int i) const;
+    const btVector3 & getLinkInertia(int i) const;
+    
+
+    //
+    // change mass (incomplete: can only change base mass and inertia at present)
+    //
+
+    void setBaseMass(btScalar mass) { base_mass = mass; }
+    void setBaseInertia(const btVector3 &inertia) { base_inertia = inertia; }
+
+
+    //
+    // get/set pos/vel/rot/omega for the base link
+    //
+
+    const btVector3 & getBasePos() const { return base_pos; }    // in world frame
+    const btVector3 getBaseVel() const 
+       { 
+               return btVector3(m_real_buf[3],m_real_buf[4],m_real_buf[5]); 
+       }     // in world frame
+    const btQuaternion & getWorldToBaseRot() const 
+       { 
+               return base_quat; 
+       }     // rotates world vectors into base frame
+    btVector3 getBaseOmega() const { return btVector3(m_real_buf[0],m_real_buf[1],m_real_buf[2]); }   // in world frame
+
+    void setBasePos(const btVector3 &pos) 
+       { 
+               base_pos = pos; 
+       }
+    void setBaseVel(const btVector3 &vel) 
+       { 
+
+               m_real_buf[3]=vel[0]; m_real_buf[4]=vel[1]; m_real_buf[5]=vel[2]; 
+       }
+    void setWorldToBaseRot(const btQuaternion &rot) 
+       { 
+               base_quat = rot; 
+       }
+    void setBaseOmega(const btVector3 &omega) 
+       { 
+               m_real_buf[0]=omega[0]; 
+               m_real_buf[1]=omega[1]; 
+               m_real_buf[2]=omega[2]; 
+       }
+
+
+    //
+    // get/set pos/vel for child links (i = 0 to num_links-1)
+    //
+
+    btScalar getJointPos(int i) const;
+    btScalar getJointVel(int i) const;
+
+    void setJointPos(int i, btScalar q);
+    void setJointVel(int i, btScalar qdot);
+
+    //
+    // direct access to velocities as a vector of 6 + num_links elements.
+    // (omega first, then v, then joint velocities.)
+    //
+    const btScalar * getVelocityVector() const 
+       { 
+               return &m_real_buf[0]; 
+       }
+/*    btScalar * getVelocityVector() 
+       { 
+               return &real_buf[0]; 
+       }
+  */  
+
+    //
+    // get the frames of reference (positions and orientations) of the child links
+    // (i = 0 to num_links-1)
+    //
+
+    const btVector3 & getRVector(int i) const;   // vector from COM(parent(i)) to COM(i), in frame i's coords
+    const btQuaternion & getParentToLocalRot(int i) const;   // rotates vectors in frame parent(i) to vectors in frame i.
+
+
+    //
+    // transform vectors in local frame of link i to world frame (or vice versa)
+    //
+    btVector3 localPosToWorld(int i, const btVector3 &vec) const;
+    btVector3 localDirToWorld(int i, const btVector3 &vec) const;
+    btVector3 worldPosToLocal(int i, const btVector3 &vec) const;
+    btVector3 worldDirToLocal(int i, const btVector3 &vec) const;
+    
+
+    //
+    // calculate kinetic energy and angular momentum
+    // useful for debugging.
+    //
+
+    btScalar getKineticEnergy() const;
+    btVector3 getAngularMomentum() const;
+    
+
+    //
+    // set external forces and torques. Note all external forces/torques are given in the WORLD frame.
+    //
+
+    void clearForcesAndTorques();
+       void clearVelocities();
+
+    void addBaseForce(const btVector3 &f) 
+       { 
+               base_force += f; 
+       }
+    void addBaseTorque(const btVector3 &t) { base_torque += t; }
+    void addLinkForce(int i, const btVector3 &f);
+    void addLinkTorque(int i, const btVector3 &t);
+    void addJointTorque(int i, btScalar Q);
+
+    const btVector3 & getBaseForce() const { return base_force; }
+    const btVector3 & getBaseTorque() const { return base_torque; }
+    const btVector3 & getLinkForce(int i) const;
+    const btVector3 & getLinkTorque(int i) const;
+    btScalar getJointTorque(int i) const;
+
+
+    //
+    // dynamics routines.
+    //
+
+    // timestep the velocities (given the external forces/torques set using addBaseForce etc).
+    // also sets up caches for calcAccelerationDeltas.
+    //
+    // Note: the caller must provide three vectors which are used as
+    // temporary scratch space. The idea here is to reduce dynamic
+    // memory allocation: the same scratch vectors can be re-used
+    // again and again for different Multibodies, instead of each
+    // btMultiBody allocating (and then deallocating) their own
+    // individual scratch buffers. This gives a considerable speed
+    // improvement, at least on Windows (where dynamic memory
+    // allocation appears to be fairly slow).
+    //
+    void stepVelocities(btScalar dt,
+                        btAlignedObjectArray<btScalar> &scratch_r,
+                        btAlignedObjectArray<btVector3> &scratch_v,
+                        btAlignedObjectArray<btMatrix3x3> &scratch_m);
+
+    // calcAccelerationDeltas
+    // input: force vector (in same format as jacobian, i.e.:
+    //                      3 torque values, 3 force values, num_links joint torque values)
+    // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values
+    // (existing contents of output array are replaced)
+    // stepVelocities must have been called first.
+    void calcAccelerationDeltas(const btScalar *force, btScalar *output,
+                                btAlignedObjectArray<btScalar> &scratch_r,
+                                btAlignedObjectArray<btVector3> &scratch_v) const;
+
+    // apply a delta-vee directly. used in sequential impulses code.
+    void applyDeltaVee(const btScalar * delta_vee) 
+       {
+
+        for (int i = 0; i < 6 + getNumLinks(); ++i) 
+               {
+                       m_real_buf[i] += delta_vee[i];
+               }
+               
+    }
+    void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier) 
+       {
+               btScalar sum = 0;
+        for (int i = 0; i < 6 + getNumLinks(); ++i)
+               {
+                       sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
+               }
+               btScalar l = btSqrt(sum);
+               /*
+               static btScalar maxl = -1e30f;
+               if (l>maxl)
+               {
+                       maxl=l;
+       //              printf("maxl=%f\n",maxl);
+               }
+               */
+               if (l>m_maxAppliedImpulse)
+               {
+//                     printf("exceeds 100: l=%f\n",maxl);
+                       multiplier *= m_maxAppliedImpulse/l;
+               }
+
+        for (int i = 0; i < 6 + getNumLinks(); ++i)
+               {
+                       sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
+                       m_real_buf[i] += delta_vee[i] * multiplier;
+               }
+    }
+
+    // timestep the positions (given current velocities).
+    void stepPositions(btScalar dt);
+
+
+    //
+    // contacts
+    //
+
+    // This routine fills out a contact constraint jacobian for this body.
+    // the 'normal' supplied must be -n for body1 or +n for body2 of the contact.
+    // 'normal' & 'contact_point' are both given in world coordinates.
+    void fillContactJacobian(int link,
+                             const btVector3 &contact_point,
+                             const btVector3 &normal,
+                             btScalar *jac,
+                             btAlignedObjectArray<btScalar> &scratch_r,
+                             btAlignedObjectArray<btVector3> &scratch_v,
+                             btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
+
+
+    //
+    // sleeping
+    //
+       void    setCanSleep(bool canSleep)
+       {
+               can_sleep = canSleep;
+       }
+
+    bool isAwake() const { return awake; }
+    void wakeUp();
+    void goToSleep();
+    void checkMotionAndSleepIfRequired(btScalar timestep);
+    
+       bool hasFixedBase() const
+       {
+                   return fixed_base;
+       }
+
+       int getCompanionId() const
+       {
+               return m_companionId;
+       }
+       void setCompanionId(int id)
+       {
+