updating Bullet 2.x with latest changes. The integration + C-API will follow at some...
authorErwin Coumans <blender@erwincoumans.com>
Sun, 12 Nov 2006 21:05:10 +0000 (21:05 +0000)
committerErwin Coumans <blender@erwincoumans.com>
Sun, 12 Nov 2006 21:05:10 +0000 (21:05 +0000)
57 files changed:
extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h
extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h
extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
extern/bullet2/src/BulletCollision/CMakeLists.txt
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
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/btConvexConcaveCollisionAlgorithm.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.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/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
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.cpp
extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h
extern/bullet2/src/LinearMath/CMakeLists.txt
extern/bullet2/src/LinearMath/btAabbUtil2.h
extern/bullet2/src/LinearMath/btDefaultMotionState.h
extern/bullet2/src/LinearMath/btGeometryUtil.cpp [new file with mode: 0644]
extern/bullet2/src/LinearMath/btGeometryUtil.h [new file with mode: 0644]
extern/bullet2/src/LinearMath/btMotionState.h
extern/bullet2/src/LinearMath/btQuickprof.cpp
extern/bullet2/src/LinearMath/btQuickprof.h
extern/bullet2/src/LinearMath/btScalar.h
extern/bullet2/src/LinearMath/btSimdMinMax.h
extern/bullet2/src/LinearMath/btTransformUtil.h

index 3462e6e..7b35df0 100644 (file)
@@ -44,7 +44,9 @@ CONCAVE_SHAPES_START_HERE,
        TRIANGLE_MESH_SHAPE_PROXYTYPE,
        ///used for demo integration FAST/Swift collision library and Bullet
        FAST_CONCAVE_MESH_PROXYTYPE,
-
+///Used for GIMPACT Trimesh integration
+       GIMPACT_SHAPE_PROXYTYPE,
+       
        EMPTY_SHAPE_PROXYTYPE,
        STATIC_PLANE_PROXYTYPE,
 CONCAVE_SHAPES_END_HERE,
index 3195f99..f9e2205 100644 (file)
@@ -19,7 +19,7 @@ subject to the following restrictions:
 struct btBroadphaseProxy;
 class btDispatcher;
 class btManifoldResult;
-struct btCollisionObject;
+class btCollisionObject;
 struct btDispatcherInfo;
 class  btPersistentManifold;
 
index 75ef338..bbc5bd3 100644 (file)
@@ -19,7 +19,7 @@ subject to the following restrictions:
 class btCollisionAlgorithm;
 struct btBroadphaseProxy;
 class btRigidBody;
-struct btCollisionObject;
+class  btCollisionObject;
 class btOverlappingPairCache;
 
 enum btCollisionDispatcherId
@@ -42,7 +42,8 @@ struct btDispatcherInfo
                m_timeOfImpact(1.f),
                m_useContinuous(false),
                m_debugDraw(0),
-               m_enableSatConvex(false)
+               m_enableSatConvex(false),
+               m_enableSPU(false)
        {
 
        }
@@ -53,6 +54,7 @@ struct btDispatcherInfo
        bool    m_useContinuous;
        class btIDebugDraw*     m_debugDraw;
        bool    m_enableSatConvex;
+       bool    m_enableSPU;
        
 };
 
index 1a24c7a..6281e93 100644 (file)
@@ -101,6 +101,7 @@ protected:
        virtual bool    processOverlap(btBroadphasePair& pair)
        {
                assert(0);
+               return false;
        }
 };
 
index d10bd24..3169f15 100644 (file)
@@ -42,6 +42,7 @@ ADD_LIBRARY(LibBulletCollision
                                CollisionShapes/btStaticPlaneShape.cpp
                                CollisionShapes/btStridingMeshInterface.cpp
                                CollisionShapes/btTriangleCallback.cpp
+                               CollisionShapes/btTriangleBuffer.cpp
                                CollisionShapes/btTriangleIndexVertexArray.cpp
                                CollisionShapes/btTriangleMesh.cpp
                                CollisionShapes/btTriangleMeshShape.cpp
index 6d499f5..1eaa4a0 100644 (file)
@@ -18,9 +18,9 @@ subject to the following restrictions:
 
 #include <vector>
 
-typedef std::vector<struct btCollisionObject*> btCollisionObjectArray;
+typedef std::vector<class btCollisionObject*> btCollisionObjectArray;
 class btCollisionAlgorithm;
-struct btCollisionObject;
+class btCollisionObject;
 
 struct btCollisionAlgorithmConstructionInfo;
 
index ebfccef..c9d21f2 100644 (file)
@@ -14,6 +14,7 @@ subject to the following restrictions:
 */
 
 
+
 #include "btCollisionDispatcher.h"
 
 
@@ -34,8 +35,8 @@ int gNumManifold = 0;
        
 btCollisionDispatcher::btCollisionDispatcher(bool noDefaultAlgorithms)
 :m_useIslands(true),
-m_count(0),
 m_convexConvexCreateFunc(0),
+m_count(0),
 m_convexConcaveCreateFunc(0),
 m_swappedConvexConcaveCreateFunc(0),
 m_compoundCreateFunc(0),
@@ -54,7 +55,7 @@ m_emptyCreateFunc(0)
        }
 }
 
-       
+
 btCollisionDispatcher::btCollisionDispatcher (): 
        m_useIslands(true),
                m_count(0)
@@ -150,7 +151,7 @@ btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* bo
        btCollisionAlgorithmConstructionInfo ci;
        ci.m_dispatcher = this;
        ci.m_manifold = sharedManifold;
-       btCollisionAlgorithm* algo = m_doubleDispatch[body0->m_collisionShape->getShapeType()][body1->m_collisionShape->getShapeType()]
+       btCollisionAlgorithm* algo = m_doubleDispatch[body0->getCollisionShape()->getShapeType()][body1->getCollisionShape()->getShapeType()]
        ->CreateCollisionAlgorithm(ci,body0,body1);
 #else
        btCollisionAlgorithm* algo = internalFindAlgorithm(body0,body1);
@@ -201,27 +202,27 @@ btCollisionAlgorithm* btCollisionDispatcher::internalFindAlgorithm(btCollisionOb
        btCollisionAlgorithmConstructionInfo ci;
        ci.m_dispatcher = this;
        
-       if (body0->m_collisionShape->isConvex() && body1->m_collisionShape->isConvex() )
+       if (body0->getCollisionShape()->isConvex() && body1->getCollisionShape()->isConvex() )
        {
                return new btConvexConvexAlgorithm(sharedManifold,ci,body0,body1);
        }
 
-       if (body0->m_collisionShape->isConvex() && body1->m_collisionShape->isConcave())
+       if (body0->getCollisionShape()->isConvex() && body1->getCollisionShape()->isConcave())
        {
                return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,false);
        }
 
-       if (body1->m_collisionShape->isConvex() && body0->m_collisionShape->isConcave())
+       if (body1->getCollisionShape()->isConvex() && body0->getCollisionShape()->isConcave())
        {
                return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,true);
        }
 
-       if (body0->m_collisionShape->isCompound())
+       if (body0->getCollisionShape()->isCompound())
        {
                return new btCompoundCollisionAlgorithm(ci,body0,body1,false);
        } else
        {
-               if (body1->m_collisionShape->isCompound())
+               if (body1->getCollisionShape()->isCompound())
                {
                        return new btCompoundCollisionAlgorithm(ci,body0,body1,true);
                }
@@ -257,7 +258,7 @@ bool        btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionO
                printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n");
        }
                
-       if ((!body0->IsActive()) && (!body1->IsActive()))
+       if ((!body0->isActive()) && (!body1->isActive()))
                needsCollision = false;
        
        return needsCollision ;
index 4179bc4..9cd2828 100644 (file)
@@ -30,22 +30,22 @@ btCollisionObject::btCollisionObject()
 }
 
 
-void btCollisionObject::SetActivationState(int newState) 
+void btCollisionObject::setActivationState(int newState) 
 { 
        if ( (m_activationState1 != DISABLE_DEACTIVATION) && (m_activationState1 != DISABLE_SIMULATION))
                m_activationState1 = newState;
 }
 
-void btCollisionObject::ForceActivationState(int newState)
+void btCollisionObject::forceActivationState(int newState)
 {
        m_activationState1 = newState;
 }
 
 void btCollisionObject::activate()
 {
-       if (!(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OJBECT)))
+       if (!(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OBJECT)))
        {
-               SetActivationState(ACTIVE_TAG);
+               setActivationState(ACTIVE_TAG);
                m_deactivationTime = 0.f;
        }
 }
index 5df3de4..b85534c 100644 (file)
@@ -34,8 +34,11 @@ class        btCollisionShape;
 /// btCollisionObject can be used to manage collision detection objects. 
 /// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy.
 /// They can be added to the btCollisionWorld.
-struct btCollisionObject
+class  btCollisionObject
 {
+
+protected:
+
        btTransform     m_worldTransform;
        btBroadphaseProxy*      m_broadphaseHandle;
        btCollisionShape*               m_collisionShape;
@@ -48,15 +51,6 @@ struct       btCollisionObject
        btVector3       m_interpolationLinearVelocity;
        btVector3       m_interpolationAngularVelocity;
 
-
-       enum CollisionFlags
-       {
-               CF_STATIC_OBJECT= 1,
-               CF_KINEMATIC_OJBECT= 2,
-               CF_NO_CONTACT_RESPONSE = 4,
-               CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution)
-       };
-
        int                             m_collisionFlags;
 
        int                             m_islandTag1;
@@ -66,7 +60,7 @@ struct        btCollisionObject
        btScalar                m_friction;
        btScalar                m_restitution;
 
-       ///users can point to their objects, m_userPointer is not used by Bullet
+       ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
        void*                   m_userObjectPointer;
 
        ///m_internalOwner is reserved to point to Bullet's btRigidBody. Don't use this, use m_userObjectPointer instead.
@@ -81,10 +75,21 @@ struct      btCollisionObject
        /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold
        float                   m_ccdSquareMotionThreshold;
 
+public:
+
+       enum CollisionFlags
+       {
+               CF_STATIC_OBJECT= 1,
+               CF_KINEMATIC_OBJECT= 2,
+               CF_NO_CONTACT_RESPONSE = 4,
+               CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution)
+       };
+
+
        inline bool mergesSimulationIslands() const
        {
                ///static objects, kinematic and object without contact response don't merge islands
-               return  ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OJBECT | CF_NO_CONTACT_RESPONSE) )==0);
+               return  ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE) )==0);
        }
 
 
@@ -94,12 +99,12 @@ struct      btCollisionObject
 
        inline bool             isKinematicObject() const
        {
-               return (m_collisionFlags & CF_KINEMATIC_OJBECT) != 0;
+               return (m_collisionFlags & CF_KINEMATIC_OBJECT) != 0;
        }
 
        inline bool             isStaticOrKinematicObject() const
        {
-               return (m_collisionFlags & (CF_KINEMATIC_OJBECT | CF_STATIC_OBJECT)) != 0 ;
+               return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0 ;
        }
 
        inline bool             hasContactResponse() const {
@@ -107,27 +112,47 @@ struct    btCollisionObject
        }
 
        
-
-
        btCollisionObject();
 
 
-       void    SetCollisionShape(btCollisionShape* collisionShape)
+       void    setCollisionShape(btCollisionShape* collisionShape)
        {
                m_collisionShape = collisionShape;
        }
 
-       int     GetActivationState() const { return m_activationState1;}
+       const btCollisionShape* getCollisionShape() const
+       {
+               return m_collisionShape;
+       }
+
+       btCollisionShape*       getCollisionShape()
+       {
+               return m_collisionShape;
+       }
+
        
-       void SetActivationState(int newState);
 
-       void ForceActivationState(int newState);
+
+       int     getActivationState() const { return m_activationState1;}
+       
+       void setActivationState(int newState);
+
+       void    setDeactivationTime(float time)
+       {
+               m_deactivationTime = time;
+       }
+       float   getDeactivationTime() const
+       {
+               return m_deactivationTime;
+       }
+
+       void forceActivationState(int newState);
 
        void    activate();
 
-       inline bool IsActive() const
+       inline bool isActive() const
        {
-               return ((GetActivationState() != ISLAND_SLEEPING) && (GetActivationState() != DISABLE_SIMULATION));
+               return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION));
        }
 
                void    setRestitution(float rest)
@@ -147,6 +172,141 @@ struct    btCollisionObject
                return m_friction;
        }
 
+       ///reserved for Bullet internal usage
+       void*   getInternalOwner()
+       {
+               return m_internalOwner;
+       }
+
+       const void*     getInternalOwner() const
+       {
+               return m_internalOwner;
+       }
+
+       btTransform&    getWorldTransform()
+       {
+               return m_worldTransform;
+       }
+
+       const btTransform&      getWorldTransform() const
+       {
+               return m_worldTransform;
+       }
+
+       void    setWorldTransform(const btTransform& worldTrans)
+       {
+               m_worldTransform = worldTrans;
+       }
+
+
+       btBroadphaseProxy*      getBroadphaseHandle()
+       {
+               return m_broadphaseHandle;
+       }
+
+       const btBroadphaseProxy*        getBroadphaseHandle() const
+       {
+               return m_broadphaseHandle;
+       }
+
+       void    setBroadphaseHandle(btBroadphaseProxy* handle)
+       {
+               m_broadphaseHandle = handle;
+       }
+
+
+       const btTransform&      getInterpolationWorldTransform() const
+       {
+               return m_interpolationWorldTransform;
+       }
+
+       btTransform&    getInterpolationWorldTransform()
+       {
+               return m_interpolationWorldTransform;
+       }
+
+       void    setInterpolationWorldTransform(const btTransform&       trans)
+       {
+               m_interpolationWorldTransform = trans;
+       }
+
+
+       const btVector3&        getInterpolationLinearVelocity() const
+       {
+               return m_interpolationLinearVelocity;
+       }
+
+       const btVector3&        getInterpolationAngularVelocity() const
+       {
+               return m_interpolationAngularVelocity;
+       }
+
+       const int getIslandTag() const
+       {
+               return  m_islandTag1;
+       }
+
+       void    setIslandTag(int tag)
+       {
+               m_islandTag1 = tag;
+       }
+
+       const float                     getHitFraction() const
+       {
+               return m_hitFraction; 
+       }
+
+       void    setHitFraction(float hitFraction)
+       {
+               m_hitFraction = hitFraction;
+       }
+
+       
+       const int       getCollisionFlags() const
+       {
+               return m_collisionFlags;
+       }
+
+       void    setCollisionFlags(int flags)
+       {
+               m_collisionFlags = flags;
+       }
+       
+       ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
+       float                   getCcdSweptSphereRadius() const
+       {
+               return m_ccdSweptSphereRadius;
+       }
+
+       ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
+       void    setCcdSweptSphereRadius(float radius)
+       {
+               m_ccdSweptSphereRadius = radius;
+       }
+
+       float   getCcdSquareMotionThreshold() const
+       {
+               return m_ccdSquareMotionThreshold;
+       }
+
+
+       /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold
+       void    setCcdSquareMotionThreshold(float ccdSquareMotionThreshold)
+       {
+               m_ccdSquareMotionThreshold = ccdSquareMotionThreshold;
+       }
+
+       ///users can point to their objects, userPointer is not used by Bullet
+       void*   getUserPointer() const
+       {
+               return m_userObjectPointer;
+       }
+       
+       ///users can point to their objects, userPointer is not used by Bullet
+       void    setUserPointer(void* userPointer)
+       {
+               m_userObjectPointer = userPointer;
+       }
 
 };
 
index e24a5c6..7edd3e8 100644 (file)
@@ -40,6 +40,7 @@ m_ownsBroadphasePairCache(false)
 {
 }
 
+
 btCollisionWorld::btCollisionWorld()
 : m_dispatcher1(new    btCollisionDispatcher()),
 m_broadphasePairCache(new btSimpleBroadphase()),
@@ -60,7 +61,7 @@ btCollisionWorld::~btCollisionWorld()
        {
                btCollisionObject* collisionObject= (*i);
                
-               btBroadphaseProxy* bp = collisionObject->m_broadphaseHandle;
+               btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
                if (bp)
                {
                        //
@@ -98,21 +99,22 @@ void        btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho
                m_collisionObjects.push_back(collisionObject);
 
                //calculate new AABB
-               btTransform trans = collisionObject->m_worldTransform;
+               btTransform trans = collisionObject->getWorldTransform();
 
                btVector3       minAabb;
                btVector3       maxAabb;
-               collisionObject->m_collisionShape->getAabb(trans,minAabb,maxAabb);
+               collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
 
-               int type = collisionObject->m_collisionShape->getShapeType();
-               collisionObject->m_broadphaseHandle = getBroadphase()->createProxy(
+               int type = collisionObject->getCollisionShape()->getShapeType();
+               collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
                        minAabb,
                        maxAabb,
                        type,
                        collisionObject,
                        collisionFilterGroup,
                        collisionFilterMask
-                       );
+                       ))      ;
+
                
 
 
@@ -132,8 +134,8 @@ void        btCollisionWorld::performDiscreteCollisionDetection()
        btVector3 aabbMin,aabbMax;
        for (size_t i=0;i<m_collisionObjects.size();i++)
        {
-               m_collisionObjects[i]->m_collisionShape->getAabb(m_collisionObjects[i]->m_worldTransform,aabbMin,aabbMax);
-               m_broadphasePairCache->setAabb(m_collisionObjects[i]->m_broadphaseHandle,aabbMin,aabbMax);
+               m_collisionObjects[i]->getCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),aabbMin,aabbMax);
+               m_broadphasePairCache->setAabb(m_collisionObjects[i]->getBroadphaseHandle(),aabbMin,aabbMax);
        }
 
        m_broadphasePairCache->refreshOverlappingPairs();
@@ -155,7 +157,7 @@ void        btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
        
        {
                
-               btBroadphaseProxy* bp = collisionObject->m_broadphaseHandle;
+               btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
                if (bp)
                {
                        //
@@ -163,7 +165,7 @@ void        btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
                        //
                        getBroadphase()->cleanProxyFromPairs(bp);
                        getBroadphase()->destroyProxy(bp);
-                       collisionObject->m_broadphaseHandle = 0;
+                       collisionObject->setBroadphaseHandle(0);
                }
        }
 
@@ -318,15 +320,7 @@ void       btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r
        
        rayToTrans.setOrigin(rayToWorld);
 
-       //do culling based on aabb (rayFrom/rayTo)
-       btVector3 rayAabbMin = rayFromWorld;
-       btVector3 rayAabbMax = rayFromWorld;
-       rayAabbMin.setMin(rayToWorld);
-       rayAabbMax.setMax(rayToWorld);
-
-
-       /// brute force go over all objects. Once there is a broadphase, use that, or
-       /// add a raycast against aabb first.
+       /// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD)
        
        std::vector<btCollisionObject*>::iterator iter;
        
@@ -338,16 +332,16 @@ void      btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r
 
                //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
                btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
-               collisionObject->m_collisionShape->getAabb(collisionObject->m_worldTransform,collisionObjectAabbMin,collisionObjectAabbMax);
-
-               //check aabb overlap
+               collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
 
-               if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax))
+               float hitLambda = 1.f; //could use resultCallback.m_closestHitFraction, but needs testing
+               btVector3 hitNormal;
+               if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
                {
                        rayTestSingle(rayFromTrans,rayToTrans,
                                collisionObject,
-                                        collisionObject->m_collisionShape,
-                                         collisionObject->m_worldTransform,
+                                        collisionObject->getCollisionShape(),
+                                         collisionObject->getWorldTransform(),
                                          resultCallback);
                        
                }
index a1cf3a0..e2b8eb1 100644 (file)
@@ -194,7 +194,7 @@ public:
                        
                        m_closestHitFraction = rayResult.m_hitFraction;
                        m_collisionObject = rayResult.m_collisionObject;
-                       m_hitNormalWorld = m_collisionObject->m_worldTransform.getBasis()*rayResult.m_hitNormalLocal;
+                       m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal;
                        m_hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction);
                        return rayResult.m_hitFraction;
                }
index febd726..43887d2 100644 (file)
@@ -23,9 +23,9 @@ btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlg
 {
        btCollisionObject* colObj = m_isSwapped? body1 : body0;
        btCollisionObject* otherObj = m_isSwapped? body0 : body1;
-       assert (colObj->m_collisionShape->isCompound());
+       assert (colObj->getCollisionShape()->isCompound());
        
-       btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->m_collisionShape);
+       btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
        int numChildren = compoundShape->getNumChildShapes();
        int i;
        
@@ -33,10 +33,10 @@ btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlg
        for (i=0;i<numChildren;i++)
        {
                btCollisionShape* childShape = compoundShape->getChildShape(i);
-               btCollisionShape* orgShape = colObj->m_collisionShape;
-               colObj->m_collisionShape = childShape;
+               btCollisionShape* orgShape = colObj->getCollisionShape();
+               colObj->setCollisionShape( childShape );
                m_childCollisionAlgorithms[i] = ci.m_dispatcher->findAlgorithm(colObj,otherObj);
-               colObj->m_collisionShape =orgShape;
+               colObj->setCollisionShape( orgShape );
        }
 }
 
@@ -56,8 +56,8 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt
        btCollisionObject* colObj = m_isSwapped? body1 : body0;
        btCollisionObject* otherObj = m_isSwapped? body0 : body1;
 
-       assert (colObj->m_collisionShape->isCompound());
-       btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->m_collisionShape);
+       assert (colObj->getCollisionShape()->isCompound());
+       btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
 
        //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
        //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
@@ -74,18 +74,18 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt
                btCollisionShape* childShape = compoundShape->getChildShape(i);
 
                //backup
-               btTransform     orgTrans = colObj->m_worldTransform;
-               btCollisionShape* orgShape = colObj->m_collisionShape;
+               btTransform     orgTrans = colObj->getWorldTransform();
+               btCollisionShape* orgShape = colObj->getCollisionShape();
 
                btTransform childTrans = compoundShape->getChildTransform(i);
                btTransform     newChildWorldTrans = orgTrans*childTrans ;
-               colObj->m_worldTransform = newChildWorldTrans;
+               colObj->setWorldTransform( newChildWorldTrans );
                //the contactpoint is still projected back using the original inverted worldtrans
-               colObj->m_collisionShape = childShape;
+               colObj->setCollisionShape( childShape );
                m_childCollisionAlgorithms[i]->processCollision(colObj,otherObj,dispatchInfo,resultOut);
                //revert back
-               colObj->m_collisionShape =orgShape;
-               colObj->m_worldTransform = orgTrans;
+               colObj->setCollisionShape( orgShape);
+               colObj->setWorldTransform(  orgTrans );
        }
 }
 
@@ -95,9 +95,9 @@ float btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* bod
        btCollisionObject* colObj = m_isSwapped? body1 : body0;
        btCollisionObject* otherObj = m_isSwapped? body0 : body1;
 
-       assert (colObj->m_collisionShape->isCompound());
+       assert (colObj->getCollisionShape()->isCompound());
        
-       btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->m_collisionShape);
+       btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
 
        //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
        //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
@@ -116,22 +116,22 @@ float     btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* bod
                btCollisionShape* childShape = compoundShape->getChildShape(i);
 
                //backup
-               btTransform     orgTrans = colObj->m_worldTransform;
-               btCollisionShape* orgShape = colObj->m_collisionShape;
+               btTransform     orgTrans = colObj->getWorldTransform();
+               btCollisionShape* orgShape = colObj->getCollisionShape();
 
                btTransform childTrans = compoundShape->getChildTransform(i);
                btTransform     newChildWorldTrans = orgTrans*childTrans ;
-               colObj->m_worldTransform = newChildWorldTrans;
+               colObj->setWorldTransform( newChildWorldTrans );
 
-               colObj->m_collisionShape = childShape;
+               colObj->setCollisionShape( childShape );
                float frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut);
                if (frac<hitFraction)
                {
                        hitFraction = frac;
                }
                //revert back
-               colObj->m_collisionShape =orgShape;
-               colObj->m_worldTransform = orgTrans;
+               colObj->setCollisionShape( orgShape);
+               colObj->setWorldTransform( orgTrans);
        }
        return hitFraction;
 
index 7cb0bba..8ad3967 100644 (file)
@@ -89,7 +89,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i
        if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0)
        {
                btVector3 color(255,255,0);
-               btTransform& tr = ob->m_worldTransform;
+               btTransform& tr = ob->getWorldTransform();
                m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
                m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
                m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
@@ -105,14 +105,14 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i
 
        //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
        
-       if (m_convexBody->m_collisionShape->isConvex())
+       if (m_convexBody->getCollisionShape()->isConvex())
        {
                btTriangleShape tm(triangle[0],triangle[1],triangle[2]);        
                tm.setMargin(m_collisionMarginTriangle);
        
                
-               btCollisionShape* tmpShape = ob->m_collisionShape;
-               ob->m_collisionShape = &tm;
+               btCollisionShape* tmpShape = ob->getCollisionShape();
+               ob->setCollisionShape( &tm );
                
 
                btCollisionAlgorithm* colAlgo = ci.m_dispatcher->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr);
@@ -124,7 +124,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i
 //             cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
                colAlgo->processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
                delete colAlgo;
-               ob->m_collisionShape = tmpShape;
+               ob->setCollisionShape( tmpShape );
 
        }
 
@@ -142,8 +142,8 @@ void        btConvexTriangleCallback::setTimeStepAndCounters(float collisionMarginTrian
 
        //recalc aabbs
        btTransform convexInTriangleSpace;
-       convexInTriangleSpace = m_triBody->m_worldTransform.inverse() * m_convexBody->m_worldTransform;
-       btCollisionShape* convexShape = static_cast<btCollisionShape*>(m_convexBody->m_collisionShape);
+       convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * m_convexBody->getWorldTransform();
+       btCollisionShape* convexShape = static_cast<btCollisionShape*>(m_convexBody->getCollisionShape());
        //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
        convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
        float extraMargin = collisionMarginTriangle;
@@ -167,14 +167,14 @@ void btConvexConcaveCollisionAlgorithm::processCollision (btCollisionObject* bod
        btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
        btCollisionObject* triBody = m_isSwapped ? body0 : body1;
 
-       if (triBody->m_collisionShape->isConcave())
+       if (triBody->getCollisionShape()->isConcave())
        {
 
 
                btCollisionObject*      triOb = triBody;
-               ConcaveShape* concaveShape = static_cast<ConcaveShape*>( triOb->m_collisionShape);
+               ConcaveShape* concaveShape = static_cast<ConcaveShape*>( triOb->getCollisionShape());
                
-               if (convexBody->m_collisionShape->isConvex())
+               if (convexBody->getCollisionShape()->isConvex())
                {
                        float collisionMarginTriangle = concaveShape->getMargin();
                                        
@@ -207,8 +207,8 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject
 
        //only perform CCD above a certain threshold, this prevents blocking on the long run
        //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
-       float squareMot0 = (convexbody->m_interpolationWorldTransform.getOrigin() - convexbody->m_worldTransform.getOrigin()).length2();
-       if (squareMot0 < convexbody->m_ccdSquareMotionThreshold)
+       float squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
+       if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
        {
                return 1.f;
        }
@@ -217,9 +217,9 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject
        //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
        //todo: only do if the motion exceeds the 'radius'
 
-       btTransform triInv = triBody->m_worldTransform.inverse();
-       btTransform convexFromLocal = triInv * convexbody->m_worldTransform;
-       btTransform convexToLocal = triInv * convexbody->m_interpolationWorldTransform;
+       btTransform triInv = triBody->getWorldTransform().inverse();
+       btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
+       btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
 
        struct LocalTriangleSphereCastCallback  : public btTriangleCallback
        {
@@ -270,24 +270,25 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject
        
 
        
-       if (triBody->m_collisionShape->isConcave())
+       if (triBody->getCollisionShape()->isConcave())
        {
                btVector3 rayAabbMin = convexFromLocal.getOrigin();
                rayAabbMin.setMin(convexToLocal.getOrigin());
                btVector3 rayAabbMax = convexFromLocal.getOrigin();
                rayAabbMax.setMax(convexToLocal.getOrigin());
-               rayAabbMin -= btVector3(convexbody->m_ccdSweptSphereRadius,convexbody->m_ccdSweptSphereRadius,convexbody->m_ccdSweptSphereRadius);
-               rayAabbMax += btVector3(convexbody->m_ccdSweptSphereRadius,convexbody->m_ccdSweptSphereRadius,convexbody->m_ccdSweptSphereRadius);
+               float ccdRadius0 = convexbody->getCcdSweptSphereRadius();
+               rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
+               rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
 
                float curHitFraction = 1.f; //is this available?
                LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
-               convexbody->m_ccdSweptSphereRadius,curHitFraction);
+                       convexbody->getCcdSweptSphereRadius(),curHitFraction);
 
-               raycastCallback.m_hitFraction = convexbody->m_hitFraction;
+               raycastCallback.m_hitFraction = convexbody->getHitFraction();
 
                btCollisionObject* concavebody = triBody;
 
-               ConcaveShape* triangleMesh = (ConcaveShape*) concavebody->m_collisionShape;
+               ConcaveShape* triangleMesh = (ConcaveShape*) concavebody->getCollisionShape();
                
                if (triangleMesh)
                {
@@ -296,9 +297,9 @@ float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject
        
 
 
-               if (raycastCallback.m_hitFraction < convexbody->m_hitFraction)
+               if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
                {
-                       convexbody->m_hitFraction = raycastCallback.m_hitFraction;
+                       convexbody->setHitFraction( raycastCallback.m_hitFraction);
                        return raycastCallback.m_hitFraction;
                }
        }
index 5347ef0..36b0d15 100644 (file)
@@ -157,8 +157,8 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
 
        checkPenetrationDepthSolver();
 
-       btConvexShape* min0 = static_cast<btConvexShape*>(body0->m_collisionShape);
-       btConvexShape* min1 = static_cast<btConvexShape*>(body1->m_collisionShape);
+       btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
+       btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
        
        btGjkPairDetector::ClosestPointInput input;
 
@@ -170,8 +170,8 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
        
 //     input.m_maximumDistanceSquared = 1e30f;
        
-       input.m_transformA = body0->m_worldTransform;
-       input.m_transformB = body1->m_worldTransform;
+       input.m_transformA = body0->getWorldTransform();
+       input.m_transformB = body1->getWorldTransform();
        
        resultOut->setPersistentManifold(m_manifoldPtr);
        m_gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
@@ -190,14 +190,13 @@ float     btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btC
        float resultFraction = 1.f;
 
 
-       float squareMot0 = (col0->m_interpolationWorldTransform.getOrigin() - col0->m_worldTransform.getOrigin()).length2();
+       float squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
+       float squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
     
-       if (squareMot0 < col0->m_ccdSquareMotionThreshold &&
-               squareMot0 < col0->m_ccdSquareMotionThreshold)
+       if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
+               squareMot1 < col1->getCcdSquareMotionThreshold())
                return resultFraction;
 
-
-
        if (disableCcd)
                return 1.f;
 
@@ -212,26 +211,26 @@ float     btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btC
                
        /// Convex0 against sphere for Convex1
        {
-               btConvexShape* convex0 = static_cast<btConvexShape*>(col0->m_collisionShape);
+               btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
 
-               btSphereShape   sphere1(col1->m_ccdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
+               btSphereShape   sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
                btConvexCast::CastResult result;
                btVoronoiSimplexSolver voronoiSimplex;
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
                ///Simplification, one object is simplified as a sphere
                btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
                //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
-               if (ccd1.calcTimeOfImpact(col0->m_worldTransform,col0->m_interpolationWorldTransform,
-                       col1->m_worldTransform,col1->m_interpolationWorldTransform,result))
+               if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
+                       col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
                {
                
                        //store result.m_fraction in both bodies
                
-                       if (col0->m_hitFraction > result.m_fraction)
-                               col0->m_hitFraction  = result.m_fraction;
+                       if (col0->getHitFraction()> result.m_fraction)
+                               col0->setHitFraction( result.m_fraction );
 
-                       if (col1->m_hitFraction > result.m_fraction)
-                               col1->m_hitFraction  = result.m_fraction;
+                       if (col1->getHitFraction() > result.m_fraction)
+                               col1->setHitFraction( result.m_fraction);
 
                        if (resultFraction > result.m_fraction)
                                resultFraction = result.m_fraction;
@@ -245,26 +244,26 @@ float     btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btC
 
        /// Sphere (for convex0) against Convex1
        {
-               btConvexShape* convex1 = static_cast<btConvexShape*>(col1->m_collisionShape);
+               btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
 
-               btSphereShape   sphere0(col0->m_ccdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
+               btSphereShape   sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
                btConvexCast::CastResult result;
                btVoronoiSimplexSolver voronoiSimplex;
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
                ///Simplification, one object is simplified as a sphere
                btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
                //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
-               if (ccd1.calcTimeOfImpact(col0->m_worldTransform,col0->m_interpolationWorldTransform,
-                       col1->m_worldTransform,col1->m_interpolationWorldTransform,result))
+               if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
+                       col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
                {
                
                        //store result.m_fraction in both bodies
                
-                       if (col0->m_hitFraction > result.m_fraction)
-                               col0->m_hitFraction  = result.m_fraction;
+                       if (col0->getHitFraction()      > result.m_fraction)
+                               col0->setHitFraction( result.m_fraction);
 
-                       if (col1->m_hitFraction > result.m_fraction)
-                               col1->m_hitFraction  = result.m_fraction;
+                       if (col1->getHitFraction() > result.m_fraction)
+                               col1->setHitFraction( result.m_fraction);
 
                        if (resultFraction > result.m_fraction)
                                resultFraction = result.m_fraction;
index 1d39411..cd22f3d 100644 (file)
@@ -48,8 +48,8 @@ btManifoldResult::btManifoldResult(btCollisionObject* body0,btCollisionObject* b
                m_body0(body0),
                m_body1(body1)
 {
-       m_rootTransA = body0->m_worldTransform;
-       m_rootTransB = body1->m_worldTransform;
+       m_rootTransA = body0->getWorldTransform();
+       m_rootTransB = body1->getWorldTransform();
 }
 
 
@@ -81,8 +81,8 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
        //User can override friction and/or restitution
        if (gContactAddedCallback &&
                //and if either of the two bodies requires custom material
-                ((m_body0->m_collisionFlags & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
-                  (m_body1->m_collisionFlags & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)))
+                ((m_body0->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
+                  (m_body1->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)))
        {
                //experimental feature info, for per-triangle material etc.
                btCollisionObject* obj0 = isSwapped? m_body1 : m_body0;
index f9aae4e..3d70689 100644 (file)
@@ -17,10 +17,12 @@ subject to the following restrictions:
 #ifndef MANIFOLD_RESULT_H
 #define MANIFOLD_RESULT_H
 
-#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
-struct btCollisionObject;
+class btCollisionObject;
 class btPersistentManifold;
 class btManifoldPoint;
+
+#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
+
 #include "LinearMath/btTransform.h"
 
 typedef bool (*ContactAddedCallback)(btManifoldPoint& cp,      const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1);
index d6ac86d..be4f868 100644 (file)
@@ -42,8 +42,8 @@ void btSimulationIslandManager::findUnions(btDispatcher* dispatcher)
                                ((colObj1) && ((colObj1)->mergesSimulationIslands())))
                        {
 
-                               m_unionFind.unite((colObj0)->m_islandTag1,
-                                       (colObj1)->m_islandTag1);
+                               m_unionFind.unite((colObj0)->getIslandTag(),
+                                       (colObj1)->getIslandTag());
                        }
                }
        }
@@ -65,8 +65,8 @@ void  btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld
                {
                        
                        btCollisionObject*      collisionObject= (*i);
-                       collisionObject->m_islandTag1 = index;
-                       collisionObject->m_hitFraction = 1.f;
+                       collisionObject->setIslandTag(index);
+                       collisionObject->setHitFraction(1.f);
                        index++;
                        
                }
@@ -98,10 +98,10 @@ void        btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* col
                        
                        if (collisionObject->mergesSimulationIslands())
                        {
-                               collisionObject->m_islandTag1 = m_unionFind.find(index);
+                               collisionObject->setIslandTag( m_unionFind.find(index) );
                        } else
                        {
-                               collisionObject->m_islandTag1 = -1;
+                               collisionObject->setIslandTag(-1);
                        }
                        index++;
                }
@@ -113,7 +113,7 @@ inline      int     getIslandId(const btPersistentManifold* lhs)
        int islandId;
        const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
        const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
-       islandId= rcolObj0->m_islandTag1>=0?rcolObj0->m_islandTag1:rcolObj1->m_islandTag1;
+       islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag();
        return islandId;
 
 }
@@ -158,19 +158,19 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
                        int i = getUnionFind().getElement(idx).m_sz;
 
                        btCollisionObject* colObj0 = collisionObjects[i];
-                       if ((colObj0->m_islandTag1 != islandId) && (colObj0->m_islandTag1 != -1))
+                       if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
                        {
                                printf("error in island management\n");
                        }
 
-                       assert((colObj0->m_islandTag1 == islandId) || (colObj0->m_islandTag1 == -1));
-                       if (colObj0->m_islandTag1 == islandId)
+                       assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+                       if (colObj0->getIslandTag() == islandId)
                        {
-                               if (colObj0->GetActivationState()== ACTIVE_TAG)
+                               if (colObj0->getActivationState()== ACTIVE_TAG)
                                {
                                        allSleeping = false;
                                }
-                               if (colObj0->GetActivationState()== DISABLE_DEACTIVATION)
+                               if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
                                {
                                        allSleeping = false;
                                }
@@ -184,16 +184,16 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
                        {
                                int i = getUnionFind().getElement(idx).m_sz;
                                btCollisionObject* colObj0 = collisionObjects[i];
-                               if ((colObj0->m_islandTag1 != islandId) && (colObj0->m_islandTag1 != -1))
+                               if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
                                {
                                        printf("error in island management\n");
                                }
 
-                               assert((colObj0->m_islandTag1 == islandId) || (colObj0->m_islandTag1 == -1));
+                               assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 
-                               if (colObj0->m_islandTag1 == islandId)
+                               if (colObj0->getIslandTag() == islandId)
                                {
-                                       colObj0->SetActivationState( ISLAND_SLEEPING );
+                                       colObj0->setActivationState( ISLAND_SLEEPING );
                                }
                        }
                } else
@@ -205,18 +205,18 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
                                int i = getUnionFind().getElement(idx).m_sz;
 
                                btCollisionObject* colObj0 = collisionObjects[i];
-                               if ((colObj0->m_islandTag1 != islandId) && (colObj0->m_islandTag1 != -1))
+                               if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
                                {
                                        printf("error in island management\n");
                                }
 
-                               assert((colObj0->m_islandTag1 == islandId) || (colObj0->m_islandTag1 == -1));
+                               assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 
-                               if (colObj0->m_islandTag1 == islandId)
+                               if (colObj0->getIslandTag() == islandId)
                                {
-                                       if ( colObj0->GetActivationState() == ISLAND_SLEEPING)
+                                       if ( colObj0->getActivationState() == ISLAND_SLEEPING)
                                        {
-                                               colObj0->SetActivationState( WANTS_DEACTIVATION);
+                                               colObj0->setActivationState( WANTS_DEACTIVATION);
                                        }
                                }
                        }
@@ -236,17 +236,17 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
                 btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->getBody1());
                
                 //todo: check sleeping conditions!
-                if (((colObj0) && colObj0->GetActivationState() != ISLAND_SLEEPING) ||
-                       ((colObj1) && colObj1->GetActivationState() != ISLAND_SLEEPING))
+                if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
+                       ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
                {
                        //kinematic objects don't merge islands, but wake up all connected objects
-                       if (colObj0->isKinematicObject() && colObj0->GetActivationState() != ISLAND_SLEEPING)
+                       if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
                        {
-                               colObj1->SetActivationState(ACTIVE_TAG);
+                               colObj1->setActivationState(ACTIVE_TAG);
                        }
-                       if (colObj1->isKinematicObject() && colObj1->GetActivationState() != ISLAND_SLEEPING)
+                       if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
                        {
-                               colObj0->SetActivationState(ACTIVE_TAG);
+                               colObj0->setActivationState(ACTIVE_TAG);
                        }
 
                        //filtering for response
index d328184..1423c33 100644 (file)
@@ -57,11 +57,11 @@ void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,b
        btCollisionObject* boxObj = m_isSwapped? body0 : body1;
 
 
-       btSphereShape* sphere0 = (btSphereShape*)sphereObj ->m_collisionShape;
+       btSphereShape* sphere0 = (btSphereShape*)sphereObj->getCollisionShape();
 
        btVector3 normalOnSurfaceB;
        btVector3 pOnBox,pOnSphere;
-       btVector3 sphereCenter = sphereObj->m_worldTransform.getOrigin();
+       btVector3 sphereCenter = sphereObj->getWorldTransform().getOrigin();
        btScalar radius = sphere0->getRadius();
        
        float dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius);
@@ -93,14 +93,14 @@ btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* box
 
        btScalar margins;
        btVector3 bounds[2];
-       btBoxShape* boxShape= (btBoxShape*)boxObj->m_collisionShape;
+       btBoxShape* boxShape= (btBoxShape*)boxObj->getCollisionShape();
        
        bounds[0] = -boxShape->getHalfExtents();
        bounds[1] = boxShape->getHalfExtents();
 
        margins = boxShape->getMargin();//also add sphereShape margin?
 
-       const btTransform&      m44T = boxObj->m_worldTransform;
+       const btTransform&      m44T = boxObj->getWorldTransform();
 
        btVector3       boundsVec[2];
        btScalar        fPenetration;
@@ -209,7 +209,7 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject*
        n[4].setValue(  0.0f,  1.0f,  0.0f );
        n[5].setValue(  0.0f,  0.0f,  1.0f );
 
-       const btTransform&      m44T = boxObj->m_worldTransform;
+       const btTransform&      m44T = boxObj->getWorldTransform();
 
        // convert  point in local space
        prel = m44T.invXform( sphereCenter);
index bb04833..eb05bf9 100644 (file)
@@ -44,10 +44,10 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0
        if (!m_manifoldPtr)
                return;
 
-       btSphereShape* sphere0 = (btSphereShape*)col0->m_collisionShape;
-       btSphereShape* sphere1 = (btSphereShape*)col1->m_collisionShape;
+       btSphereShape* sphere0 = (btSphereShape*)col0->getCollisionShape();
+       btSphereShape* sphere1 = (btSphereShape*)col1->getCollisionShape();
 
-       btVector3 diff = col0->m_worldTransform.getOrigin()-  col1->m_worldTransform.getOrigin();
+       btVector3 diff = col0->getWorldTransform().getOrigin()-  col1->getWorldTransform().getOrigin();
        float len = diff.length();
        btScalar radius0 = sphere0->getRadius();
        btScalar radius1 = sphere1->getRadius();
@@ -61,9 +61,9 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0
 
        btVector3 normalOnSurfaceB = diff / len;
        ///point on A (worldspace)
-       btVector3 pos0 = col0->m_worldTransform.getOrigin() - radius0 * normalOnSurfaceB;
+       btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
        ///point on B (worldspace)
-       btVector3 pos1 = col1->m_worldTransform.getOrigin() + radius1* normalOnSurfaceB;
+       btVector3 pos1 = col1->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB;
 
        /// report a contact. internally this will be kept persistent, and contact reduction is done
        resultOut->setPersistentManifold(m_manifoldPtr);
index ca589ef..6f74c48 100644 (file)
@@ -48,8 +48,8 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co
        if (!m_manifoldPtr)
                return;
 
-       btSphereShape* sphere = (btSphereShape*)col0->m_collisionShape;
-       btTriangleShape* triangle = (btTriangleShape*)col1->m_collisionShape;
+       btSphereShape* sphere = (btSphereShape*)col0->getCollisionShape();
+       btTriangleShape* triangle = (btTriangleShape*)col1->getCollisionShape();
        
        /// report a contact. internally this will be kept persistent, and contact reduction is done
        resultOut->setPersistentManifold(m_manifoldPtr);
@@ -57,8 +57,8 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co
        
        btDiscreteCollisionDetectorInterface::ClosestPointInput input;
        input.m_maximumDistanceSquared = 1e30f;//todo: tighter bounds
-       input.m_transformA = col0->m_worldTransform;
-       input.m_transformB = col1->m_worldTransform;
+       input.m_transformA = col0->getWorldTransform();
+       input.m_transformB = col1->getWorldTransform();
 
        detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
 
index b137eb1..e85a36c 100644 (file)
@@ -255,6 +255,37 @@ public:
                return "Box";
        }
 
+       virtual int             getNumPreferredPenetrationDirections() const
+       {
+               return 6;
+       }
+       
+       virtual void    getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
+       {
+               switch (index)
+               {
+               case 0:
+                       penetrationVector.setValue(1.f,0.f,0.f);
+                       break;
+               case 1:
+                       penetrationVector.setValue(-1.f,0.f,0.f);
+                       break;
+               case 2:
+                       penetrationVector.setValue(0.f,1.f,0.f);
+                       break;
+               case 3:
+                       penetrationVector.setValue(0.f,-1.f,0.f);
+                       break;
+               case 4:
+                       penetrationVector.setValue(0.f,0.f,1.f);
+                       break;
+               case 5:
+                       penetrationVector.setValue(0.f,0.f,-1.f);
+                       break;
+               default:
+                       assert(0);
+               }
+       }
 
 };
 
index 3ffde1b..0de334b 100644 (file)
@@ -26,7 +26,7 @@ subject to the following restrictions:
 
 //todo: get rid of this btConvexCastResult thing!
 struct btConvexCastResult;
-
+#define MAX_PREFERRED_PENETRATION_DIRECTIONS 10
 
 /// btConvexShape is an abstract shape interface.
 /// The explicit part provides plane-equations, the implicit part provides GetClosestPoint interface.
@@ -84,6 +84,17 @@ public:
                return m_collisionMargin;
        }
 
+       virtual int             getNumPreferredPenetrationDirections() const
+       {
+               return 0;
+       }
+       
+       virtual void    getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
+       {
+               assert(0);
+       }
+
+
 
 };
 
index aaadb82..fde4d95 100644 (file)
@@ -20,17 +20,17 @@ subject to the following restrictions:
 btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,const btVector3* positions,const btScalar* radi,int numSpheres)
 :m_inertiaHalfExtents(inertiaHalfExtents)
 {
-       m_minRadius = 1e30f;
+       float startMargin = 1e30f;
 
        m_numSpheres = numSpheres;
        for (int i=0;i<m_numSpheres;i++)
        {
                m_localPositions[i] = positions[i];
                m_radi[i] = radi[i];
-               if (radi[i] < m_minRadius)
-                       m_minRadius = radi[i];
+               if (radi[i] < startMargin)
+                       startMargin = radi[i];
        }
-       setMargin(m_minRadius);
+       setMargin(startMargin);
 
 }
 
@@ -64,7 +64,7 @@ btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,cons
 
        for (i=0;i<m_numSpheres;i++)
        {
-               vtx = (*pos) +vec*((*rad)-m_minRadius);
+               vtx = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
                pos++;
                rad++;
                newDot = vec.dot(vtx);
@@ -96,7 +96,7 @@ btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,cons
 
                for (int i=0;i<m_numSpheres;i++)
                {
-                       vtx = (*pos) +vec*((*rad)-m_minRadius);
+                       vtx = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
                        pos++;
                        rad++;
                        newDot = vec.dot(vtx);
index 6a9151d..2dde9d3 100644 (file)
@@ -31,9 +31,7 @@ class btMultiSphereShape : public btConvexShape
        btVector3       m_inertiaHalfExtents;
 
        int m_numSpheres;
-       float m_minRadius;
-
-
+       
 
 
 
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp
new file mode 100644 (file)
index 0000000..078ba38
--- /dev/null
@@ -0,0 +1,41 @@
+/*
+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 "btTriangleBuffer.h"
+
+
+///example usage of this class:
+//                     btTriangleBuffer        triBuf;
+//                     concaveShape->processAllTriangles(&triBuf,aabbMin, aabbMax);
+//                     for (int i=0;i<triBuf.getNumTriangles();i++)
+//                     {
+//                             const btTriangle& tri = triBuf.getTriangle(i);
+//                             //do something useful here with the triangle
+//                     }
+
+
+
+
+void btTriangleBuffer::processTriangle(btVector3* triangle,int partId,int  triangleIndex)
+{
+               btTriangle      tri;
+               tri.m_vertex0 = triangle[0];
+               tri.m_vertex1 = triangle[1];
+               tri.m_vertex2 = triangle[2];
+               tri.m_partId = partId;
+               tri.m_triangleIndex = triangleIndex;
+                       
+               m_triangleBuffer.push_back(tri);
+}
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h
new file mode 100644 (file)
index 0000000..4f421a6
--- /dev/null
@@ -0,0 +1,60 @@
+/*
+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_TRIANGLE_BUFFER_H
+#define BT_TRIANGLE_BUFFER_H
+
+#include "btTriangleCallback.h"
+#include <vector>
+
+struct btTriangle
+{
+       btVector3       m_vertex0;
+       btVector3       m_vertex1;
+       btVector3       m_vertex2;
+       int     m_partId;
+       int     m_triangleIndex;
+};
+
+///btTriangleBuffer can be useful to collect and store overlapping triangles between AABB and concave objects that support 'processAllTriangles'
+class btTriangleBuffer : public btTriangleCallback
+{
+
+       std::vector<btTriangle> m_triangleBuffer;
+       
+public:
+
+
+       virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
+       
+       int     getNumTriangles() const
+       {
+               return int(m_triangleBuffer.size());
+       }
+       
+       const btTriangle&       getTriangle(int index) const
+       {
+               return m_triangleBuffer[index];
+       }
+
+       void    clearBuffer()
+       {
+               m_triangleBuffer.clear();
+       }
+       
+};
+
+
+#endif //BT_TRIANGLE_BUFFER_H
\ No newline at end of file
index 690d1e8..171dcf3 100644 (file)
@@ -46,6 +46,15 @@ class btTriangleMesh : public btStridingMeshInterface
                        m_triangles.push_back(tri);
                }
 
+               int getNumTriangles() const
+               {
+                       return m_triangles.size();
+               }
+
+               const btMyTriangle&     getTriangle(int index) const
+               {
+                       return m_triangles[index];
+               }
 
 //StridingMeshInterface interface implementation
 
index 8f0a06f..8e6e2da 100644 (file)
@@ -157,6 +157,18 @@ public:
                        return "Triangle";
                }
 
+               virtual int             getNumPreferredPenetrationDirections() const
+               {
+                       return 2;
+               }
+               
+               virtual void    getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const
+               {
+                       calcNormal(penetrationVector);
+                       if (index)
+                               penetrationVector *= -1.f;
+               }
+
 
 };
 
index 8889699..ef2b480 100644 (file)
@@ -14,8 +14,8 @@ subject to the following restrictions:
 */
 
 
-#ifndef DISCRETE_COLLISION_DETECTOR_INTERFACE_H
-#define DISCRETE_COLLISION_DETECTOR_INTERFACE_H
+#ifndef DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
+#define DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
 #include "LinearMath/btTransform.h"
 #include "LinearMath/btVector3.h"
 
@@ -84,4 +84,4 @@ struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result
                }
 };
 
-#endif //DISCRETE_COLLISION_DETECTOR_INTERFACE_H
+#endif //DISCRETE_COLLISION_DETECTOR_INTERFACE1_H
index 4b38ced..507fcaf 100644 (file)
@@ -22,12 +22,13 @@ subject to the following restrictions:
 #include <stdio.h> //for debug printf
 #endif
 
-static const btScalar rel_error = btScalar(1.0e-5);
-btScalar rel_error2 = rel_error * rel_error;
-float maxdist2 = 1.e30f;
+//must be above the machine epsilon
+#define REL_ERROR2 1.0e-6f
+
 
 #ifdef __SPU__
 #include <spu_printf.h>
+#define printf spu_printf
 #endif //__SPU__
 
 btGjkPairDetector::btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*     penetrationDepthSolver)
@@ -36,7 +37,9 @@ m_penetrationDepthSolver(penetrationDepthSolver),
 m_simplexSolver(simplexSolver),
 m_minkowskiA(objectA),
 m_minkowskiB(objectB),
-m_ignoreMargin(false)
+m_ignoreMargin(false),
+m_lastUsedMethod(-1),
+m_catchDegeneracies(0)
 {
 }
 
@@ -45,6 +48,11 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
        btScalar distance=0.f;
        btVector3       normalInB(0.f,0.f,0.f);
        btVector3 pointOnA,pointOnB;
+       btTransform     localTransA = input.m_transformA;
+       btTransform localTransB = input.m_transformB;
+       btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * 0.5f;
+       localTransA.getOrigin() -= positionOffset;
+       localTransB.getOrigin() -= positionOffset;
 
        float marginA = m_minkowskiA->getMargin();
        float marginB = m_minkowskiB->getMargin();
@@ -56,12 +64,16 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
                marginB = 0.f;
        }
 
-       int curIter = 0;
+       m_curIter = 0;
        int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
+       m_cachedSeparatingAxis.setValue(0,1,0);
 
        bool isValid = false;
        bool checkSimplex = false;
        bool checkPenetration = true;
+       m_degenerateSimplex = false;
+
+       m_lastUsedMethod = -1;
 
        {
                btScalar squaredDistance = SIMD_INFINITY;
@@ -81,8 +93,8 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 
                        btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
                        btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
-                       btPoint3  pWorld = input.m_transformA(pInA);    
-                       btPoint3  qWorld = input.m_transformB(qInB);
+                       btPoint3  pWorld = localTransA(pInA);   
+                       btPoint3  qWorld = localTransB(qInB);
                        
                        btVector3 w     = pWorld - qWorld;
                        delta = m_cachedSeparatingAxis.dot(w);
@@ -101,7 +113,16 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
                                break;
                        }
                        // are we getting any closer ?
-                       if (squaredDistance - delta <= squaredDistance * rel_error2)
+                       float f0 = squaredDistance - delta;
+                       float f1 = squaredDistance * REL_ERROR2;
+
+                       if (f0 <= 0.f)
+                       {
+                               m_degenerateSimplex = 2;
+                       }
+
+                       if (f0 >= 0.f && (f0 <= f1))
+                       //if (f0 <= f1)
                        {
                                checkSimplex = true;
                                break;
@@ -112,6 +133,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
                        //calculate the closest point to the origin (update vector v)
                        if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
                        {
+                               m_degenerateSimplex = 1;
                                checkSimplex = true;
                                break;
                        }
@@ -130,10 +152,11 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
                        }
 
                          //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
-              if (curIter++ > gGjkMaxIter)   
+              if (m_curIter++ > gGjkMaxIter)   
               {   
                       #if defined(DEBUG) || defined (_DEBUG)   
-                              printf("btGjkPairDetector maxIter exceeded:%i\n",curIter);   
+
+                              printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);   
                               printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",   
                               m_cachedSeparatingAxis.getX(),   
                               m_cachedSeparatingAxis.getY(),   
@@ -141,6 +164,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
                               squaredDistance,   
                               m_minkowskiA->getShapeType(),   
                               m_minkowskiB->getShapeType());   
+
                       #endif   
                       break;   
 
@@ -164,20 +188,27 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
                        normalInB = pointOnA-pointOnB;
                        float lenSqr = m_cachedSeparatingAxis.length2();
                        //valid normal
-                       if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
+                       //if (lenSqr > (0.1f*margin)) //SIMD_EPSILON*SIMD_EPSILON))
+                       if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
                        {
                                float rlen = 1.f / btSqrt(lenSqr );
                                normalInB *= rlen; //normalize
                                btScalar s = btSqrt(squaredDistance);
+                       
                                ASSERT(s > btScalar(0.0));
                                pointOnA -= m_cachedSeparatingAxis * (marginA / s);
                                pointOnB += m_cachedSeparatingAxis * (marginB / s);
                                distance = ((1.f/rlen) - margin);
                                isValid = true;
+                               m_lastUsedMethod = 1;
+                       } else
+                       {
+                               m_lastUsedMethod = 2;
                        }
                }
 
-               if (checkPenetration && !isValid)
+               //if (checkPenetration && !isValid)
+               if (checkPenetration && (!isValid || (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex) ))
                {
                        //penetration case
                
@@ -185,27 +216,46 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
                        if (m_penetrationDepthSolver)
                        {
                                // Penetration depth case.
-                               isValid = m_penetrationDepthSolver->calcPenDepth( 
+                               btVector3 tmpPointOnA,tmpPointOnB;
+                               bool isValid2 = m_penetrationDepthSolver->calcPenDepth( 
                                        *m_simplexSolver, 
                                        m_minkowskiA,m_minkowskiB,
-                                       input.m_transformA,input.m_transformB,
-                                       m_cachedSeparatingAxis, pointOnA, pointOnB,
+                                       localTransA,localTransB,
+                                       m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
                                        debugDraw
                                        );
 
-                               if (isValid)
+                               if (isValid2)
                                {
-                                       normalInB = pointOnB-pointOnA;
-                                       float lenSqr = normalInB.length2();
+                                       btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
+                                       float lenSqr = tmpNormalInB.length2();
                                        if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
                                        {
-                                               normalInB /= btSqrt(lenSqr);
-                                               distance = -(pointOnA-pointOnB).length();
+                                               tmpNormalInB /= btSqrt(lenSqr);
+                                               float distance2 = -(tmpPointOnA-tmpPointOnB).length();
+                                               //only replace valid penetrations when the result is deeper (check)
+                                               if (!isValid || (distance2 < distance))
+                                               {
+                                                       distance = distance2;
+                                                       pointOnA = tmpPointOnA;
+                                                       pointOnB = tmpPointOnB;
+                                                       normalInB = tmpNormalInB;
+                                                       isValid = true;
+                                                       m_lastUsedMethod = 3;
+                                               } else
+                                               {
+                                                       
+                                               }
                                        } else
                                        {
-                                               isValid = false;
+                                               //isValid = false;
+                                               m_lastUsedMethod = 4;
                                        }
+                               } else
+                               {
+                                       m_lastUsedMethod = 5;
                                }
+                               
                        }
                }
        }
@@ -219,7 +269,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 
                output.addContactPoint(
                        normalInB,
-                       pointOnB,
+                       pointOnB+positionOffset,
                        distance);
                //printf("gjk add:%f",distance);
        }
index c4842cd..09c1669 100644 (file)
@@ -43,6 +43,12 @@ class btGjkPairDetector : public btDiscreteCollisionDetectorInterface
 
 public:
 
+       //some debugging to fix degeneracy problems
+       int                     m_lastUsedMethod;
+       int                     m_curIter;
+       int                     m_degenerateSimplex;
+       int                     m_catchDegeneracies;
+
 
        btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver);
        virtual ~btGjkPairDetector() {};
@@ -68,11 +74,13 @@ public:
                m_penetrationDepthSolver = penetrationDepthSolver;
        }
 
+       ///don't use setIgnoreMargin, it's for Bullet's internal use
        void    setIgnoreMargin(bool ignoreMargin)
        {
                m_ignoreMargin = ignoreMargin;
        }
 
+
 };
 
 #endif //GJK_PAIR_DETECTOR_H
index 34daacf..74cf2b7 100644 (file)
@@ -20,32 +20,10 @@ subject to the following restrictions:
 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
 
 
-struct MyResult : public btDiscreteCollisionDetectorInterface::Result
-{
 
-       MyResult():m_hasResult(false)
-       {
-       }
-       
-       btVector3 m_normalOnBInWorld;
-       btVector3 m_pointInWorld;
-       float m_depth;
-       bool    m_hasResult;
-
-       virtual void setShapeIdentifiers(int partId0,int index0,        int partId1,int index1)
-       {
-       }
-       void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)
-       {
-               m_normalOnBInWorld = normalOnBInWorld;
-               m_pointInWorld = pointInWorld;
-               m_depth = depth;
-               m_hasResult = true;
-       }
-};
 
 #define NUM_UNITSPHERE_POINTS 42
-static btVector3       sPenetrationDirections[NUM_UNITSPHERE_POINTS] = 
+static btVector3       sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = 
 {
 btVector3(0.000000f , -0.000000f,-1.000000f),
 btVector3(0.723608f , -0.525725f,-0.447219f),
@@ -100,6 +78,31 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
                                                                                                   )
 {
 
+
+       struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
+       {
+
+               btIntermediateResult():m_hasResult(false)
+               {
+               }
+               
+               btVector3 m_normalOnBInWorld;
+               btVector3 m_pointInWorld;
+               float m_depth;
+               bool    m_hasResult;
+
+               virtual void setShapeIdentifiers(int partId0,int index0,        int partId1,int index1)
+               {
+               }
+               void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)
+               {
+                       m_normalOnBInWorld = normalOnBInWorld;
+                       m_pointInWorld = pointInWorld;
+                       m_depth = depth;
+                       m_hasResult = true;
+               }
+       };
+
        //just take fixed number of orientation, and sample the penetration depth in that direction
        float minProj = 1e30f;
        btVector3 minNorm;
@@ -110,22 +113,62 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
 
 #define USE_BATCHED_SUPPORT 1
 #ifdef USE_BATCHED_SUPPORT
-       btVector3       supportVerticesABatch[NUM_UNITSPHERE_POINTS];
-       btVector3       supportVerticesBBatch[NUM_UNITSPHERE_POINTS];
-       btVector3       seperatingAxisInABatch[NUM_UNITSPHERE_POINTS];
-       btVector3       seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS];
+
+       btVector3       supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+       btVector3       supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+       btVector3       seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
+       btVector3       seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
        int i;
 
-       for (i=0;i<NUM_UNITSPHERE_POINTS;i++)
+       int numSampleDirections = NUM_UNITSPHERE_POINTS;
+
+       for (i=0;i<numSampleDirections;i++)
        {
                const btVector3& norm = sPenetrationDirections[i];
-               seperatingAxisInABatch[i] = (-norm)* transA.getBasis();
-               seperatingAxisInBBatch[i] = norm * transB.getBasis();
+               seperatingAxisInABatch[i] =  (-norm) * transA.getBasis() ;
+               seperatingAxisInBBatch[i] =  norm   * transB.getBasis() ;
        }
 
-       convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,NUM_UNITSPHERE_POINTS);
-       convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,NUM_UNITSPHERE_POINTS);
-       for (i=0;i<NUM_UNITSPHERE_POINTS;i++)
+       {
+               int numPDA = convexA->getNumPreferredPenetrationDirections();
+               if (numPDA)
+               {
+                       for (int i=0;i<numPDA;i++)
+                       {
+                               btVector3 norm;
+                               convexA->getPreferredPenetrationDirection(i,norm);
+                               norm  = transA.getBasis() * norm;
+                               sPenetrationDirections[numSampleDirections] = norm;
+                               seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
+                               seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
+                               numSampleDirections++;
+                       }
+               }
+       }
+
+       {
+               int numPDB = convexB->getNumPreferredPenetrationDirections();
+               if (numPDB)
+               {
+                       for (int i=0;i<numPDB;i++)
+                       {
+                               btVector3 norm;
+                               convexB->getPreferredPenetrationDirection(i,norm);
+                               norm  = transB.getBasis() * norm;
+                               sPenetrationDirections[numSampleDirections] = norm;
+                               seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
+                               seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
+                               numSampleDirections++;
+                       }
+               }
+       }
+
+
+
+       convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
+       convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
+
+       for (i=0;i<numSampleDirections;i++)
        {
                const btVector3& norm = sPenetrationDirections[i];
                seperatingAxisInA = seperatingAxisInABatch[i];
@@ -148,7 +191,40 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
                }
        }       
 #else
-       for (int i=0;i<NUM_UNITSPHERE_POINTS;i++)
+
+       int numSampleDirections = NUM_UNITSPHERE_POINTS;
+
+       {
+               int numPDA = convexA->getNumPreferredPenetrationDirections();
+               if (numPDA)
+               {
+                       for (int i=0;i<numPDA;i++)
+                       {
+                               btVector3 norm;
+                               convexA->getPreferredPenetrationDirection(i,norm);
+                               norm  = transA.getBasis() * norm;
+                               sPenetrationDirections[numSampleDirections] = norm;
+                               numSampleDirections++;
+                       }
+               }
+       }
+
+       {
+               int numPDB = convexB->getNumPreferredPenetrationDirections();
+               if (numPDB)
+               {
+                       for (int i=0;i<numPDB;i++)
+                       {
+                               btVector3 norm;
+                               convexB->getPreferredPenetrationDirection(i,norm);
+                               norm  = transB.getBasis() * norm;
+                               sPenetrationDirections[numSampleDirections] = norm;
+                               numSampleDirections++;
+                       }
+               }
+       }
+
+       for (int i=0;i<numSampleDirections;i++)
        {
                const btVector3& norm = sPenetrationDirections[i];
                seperatingAxisInA = (-norm)* transA.getBasis();
@@ -174,10 +250,15 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
 
        minA += minNorm*convexA->getMargin();
        minB -= minNorm*convexB->getMargin();
+       //no penetration
+       if (minProj < 0.f)
+               return false;
+
        minProj += (convexA->getMargin() + convexB->getMargin());
 
 
-       
+
+
 
 //#define DEBUG_DRAW 1
 #ifdef DEBUG_DRAW
@@ -213,7 +294,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
        input.m_transformB = transB;
        input.m_maximumDistanceSquared = 1e30f;//minProj;
        
-       MyResult res;
+       btIntermediateResult res;
        gjkdet.getClosestPoints(input,res,debugDraw);
 
        float correctedMinNorm = minProj - res.m_depth;
index 429ad54..8d22689 100644 (file)
@@ -164,7 +164,8 @@ float resolveSingleFriction(
        float combinedFriction = cpd->m_friction;
        
        btScalar limit = cpd->m_appliedImpulse * combinedFriction;
-       //if (contactPoint.m_appliedImpulse>0.f)
+       
+       if (cpd->m_appliedImpulse>0.f)
        //friction
        {
                //apply friction in the 2 tangential directions
@@ -182,11 +183,12 @@ float resolveSingleFriction(
 
                        // calculate j that moves us to zero relative velocity
                        j1 = -vrel * cpd->m_jacDiagABInvTangent0;
-                       float total = cpd->m_accumulatedTangentImpulse0 + j1;
-                       GEN_set_min(total, limit);
-                       GEN_set_max(total, -limit);
-                       j1 = total - cpd->m_accumulatedTangentImpulse0;
-                       cpd->m_accumulatedTangentImpulse0 = total;
+                       float oldTangentImpulse = cpd->m_accumulatedTangentImpulse0;
+                       cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1;
+                       GEN_set_min(cpd->m_accumulatedTangentImpulse0, limit);
+                       GEN_set_max(cpd->m_accumulatedTangentImpulse0, -limit);
+                       j1 = cpd->m_accumulatedTangentImpulse0 - oldTangentImpulse;
+
                }
                {
                        // 2nd tangent
@@ -195,11 +197,11 @@ float resolveSingleFriction(
                        
                        // calculate j that moves us to zero relative velocity
                        j2 = -vrel * cpd->m_jacDiagABInvTangent1;
-                       float total = cpd->m_accumulatedTangentImpulse1 + j2;
-                       GEN_set_min(total, limit);
-                       GEN_set_max(total, -limit);
-                       j2 = total - cpd->m_accumulatedTangentImpulse1;
-                       cpd->m_accumulatedTangentImpulse1 = total;
+                       float oldTangentImpulse = cpd->m_accumulatedTangentImpulse1;
+                       cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2;
+                       GEN_set_min(cpd->m_accumulatedTangentImpulse1, limit);
+                       GEN_set_max(cpd->m_accumulatedTangentImpulse1, -limit);
+                       j2 = cpd->m_accumulatedTangentImpulse1 - oldTangentImpulse;
                }
 
 #ifdef USE_INTERNAL_APPLY_IMPULSE
@@ -395,4 +397,5 @@ float resolveSingleFrictionEmpty(
        const btContactSolverInfo& solverInfo)
 {
        return 0.f;
-};
\ No newline at end of file
+};
+
index a8ab1bb..b5783f8 100644 (file)
@@ -21,6 +21,7 @@ subject to the following restrictions:
 static const btScalar kSign[] = { 1.0f, -1.0f, 1.0f };
 static const int kAxisA[] = { 1, 0, 0 };
 static const int kAxisB[] = { 2, 2, 1 };
+#define GENERIC_D6_DISABLE_WARMSTARTING 1
 
 btGeneric6DofConstraint::btGeneric6DofConstraint()
 {
@@ -47,7 +48,7 @@ btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody&
 
 void btGeneric6DofConstraint::buildJacobian()
 {
-       btVector3       normal(0,0,0);
+       btVector3       localNormalInA(0,0,0);
 
        const btVector3& pivotInA = m_frameInA.getOrigin();
        const btVector3& pivotInB = m_frameInB.getOrigin();
@@ -64,7 +65,9 @@ void btGeneric6DofConstraint::buildJacobian()
        {
                if (isLimited(i))
                {
-                       normal[i] = 1;
+                       localNormalInA[i] = 1;
+                       btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA;
+
                        
                        // Create linear atom
                        new (&m_jacLinear[i]) btJacobianEntry(
@@ -72,19 +75,24 @@ void btGeneric6DofConstraint::buildJacobian()
                                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
                                m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(),
                                m_rbB.getCenterOfMassTransform()*pivotInB - m_rbB.getCenterOfMassPosition(),
-                               normal,
+                               normalWorld,
                                m_rbA.getInvInertiaDiagLocal(),
                                m_rbA.getInvMass(),
                                m_rbB.getInvInertiaDiagLocal(),
                                m_rbB.getInvMass());
 
+                       //optionally disable warmstarting
+#ifdef GENERIC_D6_DISABLE_WARMSTARTING
+                       m_accumulatedImpulse[i] = 0.f;
+#endif //GENERIC_D6_DISABLE_WARMSTARTING
+
                        // Apply accumulated impulse
-                       btVector3 impulse_vector = m_accumulatedImpulse[i] * normal;
+                       btVector3 impulse_vector = m_accumulatedImpulse[i] * normalWorld;
 
                        m_rbA.applyImpulse( impulse_vector, rel_pos1);
                        m_rbB.applyImpulse(-impulse_vector, rel_pos2);
 
-                       normal[i] = 0;
+                       localNormalInA[i] = 0;
                }
        }
 
@@ -106,6 +114,10 @@ void btGeneric6DofConstraint::buildJacobian()
                                m_rbA.getInvInertiaDiagLocal(),
                                m_rbB.getInvInertiaDiagLocal());
 
+#ifdef GENERIC_D6_DISABLE_WARMSTARTING
+                       m_accumulatedImpulse[i + 3] = 0.f;
+#endif //GENERIC_D6_DISABLE_WARMSTARTING
+
                        // Apply accumulated impulse
                        btVector3 impulse_vector = m_accumulatedImpulse[i + 3] * axis;
 
@@ -115,6 +127,52 @@ void btGeneric6DofConstraint::buildJacobian()
        }
 }
 
+float getMatrixElem(const btMatrix3x3& mat,int index)
+{
+       int row = index%3;
+       int col = index / 3;
+       return mat[row][col];
+}
+
+///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
+bool   MatrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
+{
+    // rot =  cy*cz          -cy*sz           sy
+    //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
+    //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
+
+///    0..8
+
+        if (getMatrixElem(mat,2) < 1.0f)
+    {
+        if (getMatrixElem(mat,2) > -1.0f)
+        {
+            xyz[0] = btAtan2(-getMatrixElem(mat,5),getMatrixElem(mat,8));
+            xyz[1] = btAsin(getMatrixElem(mat,2));
+            xyz[2] = btAtan2(-getMatrixElem(mat,1),getMatrixElem(mat,0));
+            return true;
+        }
+        else
+        {
+            // WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
+            xyz[0] = -btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4));
+            xyz[1] = -SIMD_HALF_PI;
+            xyz[2] = 0.0f;
+            return false;
+        }
+    }
+    else
+    {
+        // WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
+        xyz[0] = btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4));
+        xyz[1] = SIMD_HALF_PI;
+        xyz[2] = 0.0;
+        return false;
+    }
+        return false;
+}
+
+
 void   btGeneric6DofConstraint::solveConstraint(btScalar       timeStep)
 {
        btScalar tau = 0.1f;
@@ -126,7 +184,7 @@ void        btGeneric6DofConstraint::solveConstraint(btScalar       timeStep)
        btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
        btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
        
-       btVector3 normal(0,0,0);
+       btVector3 localNormalInA(0,0,0);
        int i;
 
        // linear
@@ -137,8 +195,10 @@ void       btGeneric6DofConstraint::solveConstraint(btScalar       timeStep)
                        btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
                        btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
                
+                       localNormalInA.setValue(0,0,0);
+                       localNormalInA[i] = 1;
+                       btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA;
 
-                       normal[i] = 1;
                        btScalar jacDiagABInv = 1.f / m_jacLinear[i].getDiagonal();
 
                        //velocity error (first order error)
@@ -146,19 +206,59 @@ void      btGeneric6DofConstraint::solveConstraint(btScalar       timeStep)
                                                                                                                                        m_rbB.getLinearVelocity(),angvelB);
                
                        //positional error (zeroth order error)
-                       btScalar depth = -(pivotAInW - pivotBInW).dot(normal); 
-                       
-                       btScalar impulse = (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv;
-                       m_accumulatedImpulse[i] += impulse;
+                       btScalar depth = -(pivotAInW - pivotBInW).dot(normalWorld); 
+                       btScalar        lo = -1e30f;
+                       btScalar        hi = 1e30f;
+               
+                       //handle the limits
+                       if (m_lowerLimit[i] < m_upperLimit[i])
+                       {
+                               {
+                                       if (depth > m_upperLimit[i])
+                                       {
+                                               depth -= m_upperLimit[i];
+                                               lo = 0.f;
+                                       
+                                       } else
+                                       {
+                                               if (depth < m_lowerLimit[i])
+                                               {
+                                                       depth -= m_lowerLimit[i];
+                                                       hi = 0.f;
+                                               } else
+                                               {
+                                                       continue;
+                                               }
+                                       }
+                               }
+                       }
 
-                       btVector3 impulse_vector = normal * impulse;
+                       btScalar normalImpulse= (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv;
+                       float oldNormalImpulse = m_accumulatedImpulse[i];
+                       float sum = oldNormalImpulse + normalImpulse;
+                       m_accumulatedImpulse[i] = sum > hi ? 0.f : sum < lo ? 0.f : sum;
+                       normalImpulse = m_accumulatedImpulse[i] - oldNormalImpulse;
+
+                       btVector3 impulse_vector = normalWorld * normalImpulse;
                        m_rbA.applyImpulse( impulse_vector, rel_pos1);
                        m_rbB.applyImpulse(-impulse_vector, rel_pos2);
                        
-                       normal[i] = 0;
+                       localNormalInA[i] = 0;
                }
        }
 
+       btVector3       axis;
+       btScalar        angle;
+       btTransform     frameAWorld = m_rbA.getCenterOfMassTransform() * m_frameInA;
+       btTransform     frameBWorld = m_rbB.getCenterOfMassTransform() * m_frameInB;
+
+       btTransformUtil::calculateDiffAxisAngle(frameAWorld,frameBWorld,axis,angle);
+       btQuaternion diff(axis,angle);
+       btMatrix3x3 diffMat (diff);
+       btVector3 xyz;
+       ///this is not perfect, we can first check which axis are limited, and choose a more appropriate order
+       MatrixToEulerXYZ(diffMat,xyz);
+
        // angular
        for (i=0;i<3;i++)
        {
@@ -179,13 +279,46 @@ void      btGeneric6DofConstraint::solveConstraint(btScalar       timeStep)
 
                        btScalar rel_pos = kSign[i] * axisA.dot(axisB);
 
+                       btScalar        lo = -1e30f;
+                       btScalar        hi = 1e30f;
+               
+                       //handle the twist limit
+                       if (m_lowerLimit[i+3] < m_upperLimit[i+3])
+                       {
+                               //clamp the values
+                               btScalar loLimit =  m_upperLimit[i+3] > -3.1415 ? m_lowerLimit[i+3] : -1e30f;
+                               btScalar hiLimit = m_upperLimit[i+3] < 3.1415 ? m_upperLimit[i+3] : 1e30f;
+
+                               float projAngle  = -2.*xyz[i];
+                               
+                               if (projAngle < loLimit)
+                               {
+                                       hi = 0.f;
+                                       rel_pos = (loLimit - projAngle);
+                               } else
+                               {
+                                       if (projAngle > hiLimit)
+                                       {
+                                               lo = 0.f;
+                                               rel_pos = (hiLimit - projAngle);
+                                       } else
+                                       {
+                                               continue;
+                                       }
+                               }
+                       }
+               
                        //impulse
-                       btScalar impulse = -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv;
-                       m_accumulatedImpulse[i + 3] += impulse;
+                       
+                       btScalar normalImpulse= -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv;
+                       float oldNormalImpulse = m_accumulatedImpulse[i+3];
+                       float sum = oldNormalImpulse + normalImpulse;
+                       m_accumulatedImpulse[i+3] = sum > hi ? 0.f : sum < lo ? 0.f : sum;
+                       normalImpulse = m_accumulatedImpulse[i+3] - oldNormalImpulse;
                        
                        // Dirk: Not needed - we could actually project onto Jacobian entry here (same as above)
                        btVector3 axis = kSign[i] * axisA.cross(axisB);
-                       btVector3 impulse_vector = axis * impulse;
+                       btVector3 impulse_vector = axis * normalImpulse;
 
                        m_rbA.applyTorqueImpulse( impulse_vector);
                        m_rbB.applyTorqueImpulse(-impulse_vector);
index ac06f71..568bcc4 100644 (file)
@@ -32,6 +32,27 @@ int totalCpd = 0;
 
 int    gTotalContactPoints = 0;
 
+struct btOrderIndex
+{
+       short int       m_manifoldIndex;
+       short int       m_pointIndex;
+};
+
+#define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384
+static btOrderIndex    gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
+static unsigned long btSeed2 = 0;
+unsigned long btRand2()
+{
+  btSeed2 = (1664525L*btSeed2 + 1013904223L) & 0xffffffff;
+  return btSeed2;
+}
+
+int btRandInt2 (int n)
+{
+  float a = float(n) / 4294967296.0f;
+  return (int) (float(btRand2()) * a);
+}
+
 bool  MyContactDestroyedCallback(void* userPersistentData)
 {
        assert (userPersistentData);
@@ -69,52 +90,62 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
        btProfiler::beginBlock("solve");
 #endif //USE_PROFILE
 
+       int totalPoints = 0;
+
        {
                int j;
                for (j=0;j<numManifolds;j++)
                {
-                       int k=j;
-                       prepareConstraints(manifoldPtr[k],info,debugDrawer);
-                       solve(manifoldPtr[k],info,0,debugDrawer);
+                       prepareConstraints(manifoldPtr[j],info,debugDrawer);
                }
        }
-       
-       
-       //should traverse the contacts random order...
-       int i;
-       for ( i = 0;i<numiter-1;i++)
+
        {
                int j;
                for (j=0;j<numManifolds;j++)
                {
-                       int k=j;
-                       if (i&1)
-                               k=numManifolds-j-1;
-
-                       solve(manifoldPtr[k],info,i,debugDrawer);
+                       for (int p=0;p<manifoldPtr[j]->getNumContacts();p++)
+                       {
+                               gOrder[totalPoints].m_manifoldIndex = j;
+                               gOrder[totalPoints].m_pointIndex = p;
+                               totalPoints++;
+                       }
                }
-               
        }
-#ifdef USE_PROFILE
-       btProfiler::endBlock("solve");
-
-       btProfiler::beginBlock("solveFriction");
-#endif //USE_PROFILE
-
-       //now solve the friction                
-       for (i = 0;i<numiter-1;i++)
+       
+       
+       //should traverse the contacts random order...
+       int iteration;
+       for ( iteration = 0;iteration<numiter-1;iteration++)
        {
                int j;
-       for (j=0;j<numManifolds;j++)
+               if ((iteration & 7) == 0) {
+                       for (j=0; j<totalPoints; ++j) {
+                               btOrderIndex tmp = gOrder[j];
+                               int swapi = btRandInt2(j+1);
+                               gOrder[j] = gOrder[swapi];
+                               gOrder[swapi] = tmp;
+                       }
+               }
+
+               for (j=0;j<totalPoints;j++)
                {
-                       int k = j;
-                       if (i&1)
-                               k=numManifolds-j-1;
-                       solveFriction(manifoldPtr[k],info,i,debugDrawer);
+                       btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
+                       solve( (btRigidBody*)manifold->getBody0(),
+                                                               (btRigidBody*)manifold->getBody1()
+                       ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
+               }
+       
+               for (j=0;j<totalPoints;j++)
+               {
+                       btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
+                       solveFriction((btRigidBody*)manifold->getBody0(),
+                               (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
                }
        }
+               
 #ifdef USE_PROFILE
-       btProfiler::endBlock("solveFriction");
+       btProfiler::endBlock("solve");
 #endif //USE_PROFILE
 
        return 0.f;
@@ -225,7 +256,7 @@ void        btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
                                
                                btScalar penVel = -cpd->m_penetration/info.m_timeStep;
 
-                               if (cpd->m_restitution >= penVel)
+                               if (cpd->m_restitution > penVel)
                                {
                                        cpd->m_penetration = 0.f;
                                }                               
@@ -233,7 +264,7 @@ void        btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
                                
 
                                float relaxation = info.m_damping;
-                               cpd->m_appliedImpulse *= relaxation;
+                               cpd->m_appliedImpulse =0.f;//*= relaxation;
                                //for friction
                                cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
                                
@@ -260,8 +291,8 @@ void        btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
 
                                btVector3 totalImpulse = 
        #ifndef NO_FRICTION_WARMSTART
-                                       cp.m_frictionWorldTangential0*cp.m_accumulatedTangentImpulse0+
-                                       cp.m_frictionWorldTangential1*cp.m_accumulatedTangentImpulse1+
+                                       cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
+                                       cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
        #endif //NO_FRICTION_WARMSTART
                                        cp.m_normalWorldOnB*cpd->m_appliedImpulse;
 
@@ -304,29 +335,16 @@ void      btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
        }
 }
 
-float btSequentialImpulseConstraintSolver::solve(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
+float btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
 {
 
-       btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
-       btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
-
        float maxImpulse = 0.f;
 
        {
-               const int numpoints = manifoldPtr->getNumContacts();
 
                btVector3 color(0,1,0);
-               for (int i=0;i<numpoints ;i++)
                {
-
-                       int j=i;
-                       if (iter % 2)
-                               j = numpoints-1-i;
-                       else
-                               j=i;
-
-                       btManifoldPoint& cp = manifoldPtr->getContactPoint(j);
-                               if (cp.getDistance() <= 0.f)
+                       if (cp.getDistance() <= 0.f)
                        {
 
                                if (iter == 0)
@@ -353,24 +371,15 @@ float btSequentialImpulseConstraintSolver::solve(btPersistentManifold* manifoldP
        return maxImpulse;
 }
 
-float btSequentialImpulseConstraintSolver::solveFriction(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
+float btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
 {
-       btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
-       btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
 
 
        {
-               const int numpoints = manifoldPtr->getNumContacts();
 
                btVector3 color(0,1,0);
-               for (int i=0;i<numpoints ;i++)
                {
-
-                       int j=i;
-                       //if (iter % 2)
-                       //      j = numpoints-1-i;
-
-                       btManifoldPoint& cp = manifoldPtr->getContactPoint(j);
+                       
                        if (cp.getDistance() <= 0.f)
                        {
 
index e399a5c..8264f50 100644 (file)
@@ -29,8 +29,9 @@ class btIDebugDraw;
 /// Applies impulses for combined restitution and penetration recovery and to simulate friction
 class btSequentialImpulseConstraintSolver : public btConstraintSolver
 {
-       float solve(btPersistentManifold* manifold, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
-       float solveFriction(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
+
+       float solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
+       float solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
        void  prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer);
 
        ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
index ea7d5c8..cb3fa72 100644 (file)
@@ -17,7 +17,7 @@ subject to the following restrictions:
 #include "btTypedConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 
-static btRigidBody s_fixed(0, btTransform::getIdentity(),0);
+static btRigidBody s_fixed(0, 0,0);
 
 btTypedConstraint::btTypedConstraint()
 : m_userConstraintType(-1),
index 1bfc9b3..125e933 100644 (file)
@@ -56,9 +56,10 @@ subject to the following restrictions:
 
 #include <algorithm>
 
-btDiscreteDynamicsWorld::btDiscreteDynamicsWorld()
+
+btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btConstraintSolver* constraintSolver)
 :btDynamicsWorld(),
-m_constraintSolver(new btSequentialImpulseConstraintSolver),
+m_constraintSolver(constraintSolver? constraintSolver: new btSequentialImpulseConstraintSolver),
 m_debugDrawer(0),
 m_gravity(0,-10,0),
 m_localTime(1.f/60.f),
@@ -66,10 +67,10 @@ m_profileTimings(0)
 {
        m_islandManager = new btSimulationIslandManager();
        m_ownsIslandManager = true;
-       m_ownsConstraintSolver = true;
-
+       m_ownsConstraintSolver = (constraintSolver==0);
 }
 
+
 btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver)
 :btDynamicsWorld(dispatcher,pairCache),
 m_constraintSolver(constraintSolver? constraintSolver: new btSequentialImpulseConstraintSolver),
@@ -103,7 +104,7 @@ void        btDiscreteDynamicsWorld::saveKinematicState(float timeStep)
                if (body)
                {
                                btTransform predictedTrans;
-                               if (body->GetActivationState() != ISLAND_SLEEPING)
+                               if (body->getActivationState() != ISLAND_SLEEPING)
                                {
                                        if (body->isKinematicObject())
                                        {
@@ -117,42 +118,82 @@ void      btDiscreteDynamicsWorld::saveKinematicState(float timeStep)
 
 void   btDiscreteDynamicsWorld::synchronizeMotionStates()
 {
-       //todo: iterate over awake simulation islands!
-       for (unsigned int i=0;i<m_collisionObjects.size();i++)
+       //debug vehicle wheels
+       
+       
        {
-               btCollisionObject* colObj = m_collisionObjects[i];
-               if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
+               //todo: iterate over awake simulation islands!
+               for (unsigned int i=0;i<m_collisionObjects.size();i++)
                {
-                       btVector3 color(255.f,255.f,255.f);
-                       switch(colObj->GetActivationState())
+                       btCollisionObject* colObj = m_collisionObjects[i];
+                       if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
                        {
-                       case  ACTIVE_TAG:
-                               color = btVector3(255.f,255.f,255.f); break;
-                       case ISLAND_SLEEPING:
-                               color =  btVector3(0.f,255.f,0.f);break;
-                       case WANTS_DEACTIVATION:
-                               color = btVector3(0.f,255.f,255.f);break;
-                       case DISABLE_DEACTIVATION:
-                               color = btVector3(255.f,0.f,0.f);break;
-                       case DISABLE_SIMULATION:
-                               color = btVector3(255.f,255.f,0.f);break;
-                       default:
+                               btVector3 color(255.f,255.f,255.f);
+                               switch(colObj->getActivationState())
                                {
-                                       color = btVector3(255.f,0.f,0.f);
-                               }
-                       };
+                               case  ACTIVE_TAG:
+                                       color = btVector3(255.f,255.f,255.f); break;
+                               case ISLAND_SLEEPING:
+                                       color =  btVector3(0.f,255.f,0.f);break;
+                               case WANTS_DEACTIVATION:
+                                       color = btVector3(0.f,255.f,255.f);break;
+                               case DISABLE_DEACTIVATION:
+                                       color = btVector3(255.f,0.f,0.f);break;
+                               case DISABLE_SIMULATION:
+                                       color = btVector3(255.f,255.f,0.f);break;
+                               default:
+                                       {
+                                               color = btVector3(255.f,0.f,0.f);
+                                       }
+                               };
 
-                       debugDrawObject(colObj->m_worldTransform,colObj->m_collisionShape,color);
+                               debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
+                       }
+                       btRigidBody* body = btRigidBody::upcast(colObj);
+                       if (body && body->getMotionState() && !body->isStaticOrKinematicObject())
+                       {
+                               if (body->getActivationState() != ISLAND_SLEEPING)
+                               {
+                                       btTransform interpolatedTransform;
+                                       btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
+                                               body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime,interpolatedTransform);
+                                       body->getMotionState()->setWorldTransform(interpolatedTransform);
+                               }
+                       }
                }
-               btRigidBody* body = btRigidBody::upcast(colObj);
-               if (body && body->getMotionState() && !body->isStaticOrKinematicObject())
+       }
+
+       if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
+       {
+               for (unsigned int i=0;i<this->m_vehicles.size();i++)
                {
-                       if (body->GetActivationState() != ISLAND_SLEEPING)
+                       for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
                        {
-                               btTransform interpolatedTransform;
-                               btTransformUtil::integrateTransform(body->m_interpolationWorldTransform,
-                                       body->m_interpolationLinearVelocity,body->m_interpolationAngularVelocity,m_localTime,interpolatedTransform);
-                               body->getMotionState()->setWorldTransform(interpolatedTransform);
+                               btVector3 wheelColor(0,255,255);
+                               if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact)
+                               {
+                                       wheelColor.setValue(0,0,255);
+                               } else
+                               {
+                                       wheelColor.setValue(255,0,255);
+                               }
+
+                               //synchronize the wheels with the (interpolated) chassis worldtransform
+                               m_vehicles[i]->updateWheelTransform(v,true);
+                                       
+                               btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin();
+
+                               btVector3 axle = btVector3(     
+                                       m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()],
+                                       m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()],
+                                       m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]);
+
+
+                               //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
+                               //debug wheels (cylinders)
+                               m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
+                               m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
+
                        }
                }
        }
@@ -178,8 +219,15 @@ int        btDiscreteDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, flo
                //variable timestep
                fixedTimeStep = timeStep;
                m_localTime = timeStep;
-               numSimulationSubSteps = 1;
-               maxSubSteps = 1;
+               if (btFuzzyZero(timeStep))
+               {
+                       numSimulationSubSteps = 0;
+                       maxSubSteps = 0;
+               } else
+               {
+                       numSimulationSubSteps = 1;
+                       maxSubSteps = 1;
+               }
        }
 
        //process some debugging flags
@@ -187,7 +235,7 @@ int btDiscreteDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, flo
        {
                gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
        }
-       if (!btFuzzyZero(timeStep) && numSimulationSubSteps)
+       if (numSimulationSubSteps)
        {
 
                saveKinematicState(fixedTimeStep);
@@ -198,6 +246,7 @@ int btDiscreteDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, flo
                for (int i=0;i<clampedSimulationSteps;i++)
                {
                        internalSingleStepSimulation(fixedTimeStep);
+                       synchronizeMotionStates();
                }
 
        } 
@@ -226,20 +275,23 @@ void      btDiscreteDynamicsWorld::internalSingleStepSimulation(float timeStep)
        btContactSolverInfo infoGlobal;
        infoGlobal.m_timeStep = timeStep;
        
+
+
        ///solve non-contact constraints
        solveNoncontactConstraints(infoGlobal);
        
        ///solve contact constraints
        solveContactConstraints(infoGlobal);
 
-       ///update vehicle simulation
-       updateVehicles(timeStep);
-       
        ///CallbackTriggers();
 
        ///integrate transforms
        integrateTransforms(timeStep);
-               
+
+       ///update vehicle simulation
+       updateVehicles(timeStep);
+
+
        updateActivationState( timeStep );
 
        
@@ -268,12 +320,19 @@ void      btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
 
 void   btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
 {
-       body->setGravity(m_gravity);
-       bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
-       short collisionFilterGroup = isDynamic? btBroadphaseProxy::DefaultFilter : btBroadphaseProxy::StaticFilter;
-       short collisionFilterMask = isDynamic?  btBroadphaseProxy::AllFilter :  btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter;
+       if (!body->isStaticOrKinematicObject())
+       {
+               body->setGravity(m_gravity);
+       }
 
-       addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
+       if (body->getCollisionShape())
+       {
+               bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
+               short collisionFilterGroup = isDynamic? btBroadphaseProxy::DefaultFilter : btBroadphaseProxy::StaticFilter;
+               short collisionFilterMask = isDynamic?  btBroadphaseProxy::AllFilter :  btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter;
+
+               addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
+       }
 }
 
 
@@ -303,12 +362,12 @@ void      btDiscreteDynamicsWorld::updateActivationState(float timeStep)
 
                        if (body->wantsSleeping())
                        {
-                               if (body->GetActivationState() == ACTIVE_TAG)
-                                       body->SetActivationState( WANTS_DEACTIVATION );
+                               if (body->getActivationState() == ACTIVE_TAG)
+                                       body->setActivationState( WANTS_DEACTIVATION );
                        } else
                        {
-                               if (body->GetActivationState() != DISABLE_DEACTIVATION)
-                                       body->SetActivationState( ACTIVE_TAG );
+                               if (body->getActivationState() != DISABLE_DEACTIVATION)
+                                       body->setActivationState( ACTIVE_TAG );
                        }
                }
        }
@@ -436,11 +495,11 @@ void      btDiscreteDynamicsWorld::calculateSimulationIslands()
                        if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
                                ((colObj1) && ((colObj1)->mergesSimulationIslands())))
                        {
-                               if (colObj0->IsActive() || colObj1->IsActive())
+                               if (colObj0->isActive() || colObj1->isActive())
                                {
 
-                                       getSimulationIslandManager()->getUnionFind().unite((colObj0)->m_islandTag1,
-                                               (colObj1)->m_islandTag1);
+                                       getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
+                                               (colObj1)->getIslandTag());
                                }
                        }
                }
@@ -501,19 +560,19 @@ void      btDiscreteDynamicsWorld::updateAabbs()
                //      if (body->IsActive() && (!body->IsStatic()))
                        {
                                btPoint3 minAabb,maxAabb;
-                               colObj->m_collisionShape->getAabb(colObj->m_worldTransform, minAabb,maxAabb);
+                               colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
                                btSimpleBroadphase* bp = (btSimpleBroadphase*)m_broadphasePairCache;
 
                                //moving objects should be moderately sized, probably something wrong if not
                                if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < 1e12f))
                                {
-                                       bp->setAabb(body->m_broadphaseHandle,minAabb,maxAabb);
+                                       bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb);
                                } else
                                {
                                        //something went wrong, investigate
                                        //this assert is unwanted in 3D modelers (danger of loosing work)
                                        assert(0);
-                                       body->SetActivationState(DISABLE_SIMULATION);
+                                       body->setActivationState(DISABLE_SIMULATION);
                                        
                                        static bool reportMe = true;
                                        if (reportMe)
@@ -548,7 +607,7 @@ void        btDiscreteDynamicsWorld::integrateTransforms(float timeStep)
                btRigidBody* body = btRigidBody::upcast(colObj);
                if (body)
                {
-                       if (body->IsActive() && (!body->isStaticOrKinematicObject()))
+                       if (body->isActive() && (!body->isStaticOrKinematicObject()))
                        {
                                body->predictIntegratedTransform(timeStep, predictedTrans);
                                body->proceedToTransform( predictedTrans);
@@ -571,11 +630,11 @@ void      btDiscreteDynamicsWorld::predictUnconstraintMotion(float timeStep)
                {
                        if (!body->isStaticOrKinematicObject())
                        {
-                               if (body->IsActive())
+                               if (body->isActive())
                                {
                                        body->applyForces( timeStep);
                                        body->integrateVelocities( timeStep);
-                                       body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform);
+                                       body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
                                }
                        }
                }
@@ -768,3 +827,14 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
                }
        }
 }
+
+void   btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
+{
+       if (m_ownsConstraintSolver)
+       {
+               delete m_constraintSolver;
+       }
+       m_ownsConstraintSolver = false;
+       m_constraintSolver = solver;
+}
+
index 77e4ada..1293975 100644 (file)
@@ -86,7 +86,7 @@ public:
        btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver=0);
 
        ///this btDiscreteDynamicsWorld will create and own dispatcher, pairCache and constraintSolver, and deletes it in the destructor.
-       btDiscreteDynamicsWorld();
+       btDiscreteDynamicsWorld(btConstraintSolver* constraintSolver = 0);
                
        virtual ~btDiscreteDynamicsWorld();
 
@@ -136,6 +136,7 @@ public:
 
        void    debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
 
+       virtual void    setConstraintSolver(btConstraintSolver* solver);
 
 };
 
index fa916f6..6023f29 100644 (file)
@@ -19,6 +19,8 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
 class btTypedConstraint;
 class btRaycastVehicle;
+class btConstraintSolver;
+
 
 ///btDynamicsWorld is the baseclass for several dynamics implementation, basic, discrete, parallel, and continuous
 class btDynamicsWorld : public btCollisionWorld
@@ -65,6 +67,8 @@ class btDynamicsWorld : public btCollisionWorld
 
                virtual void    removeRigidBody(btRigidBody* body) = 0;
 
+               virtual void    setConstraintSolver(btConstraintSolver* solver) = 0;
+
 };
 
 #endif //BT_DYNAMICS_WORLD_H
index a434f5e..db05f37 100644 (file)
@@ -42,7 +42,13 @@ btRigidBody::btRigidBody(float mass, btMotionState* motionState, btCollisionShap
        m_frictionSolverType(0)
 {
 
-       motionState->getWorldTransform(m_worldTransform);
+       if (motionState)
+       {
+               motionState->getWorldTransform(m_worldTransform);
+       } else
+       {
+               m_worldTransform = btTransform::getIdentity();
+       }
 
        m_interpolationWorldTransform = m_worldTransform;
        m_interpolationLinearVelocity.setValue(0,0,0);
@@ -64,7 +70,7 @@ btRigidBody::btRigidBody(float mass, btMotionState* motionState, btCollisionShap
 
 }
 
-
+#ifdef OBSOLETE_MOTIONSTATE_LESS
 btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisionShape* collisionShape,const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution)
 : 
        m_gravity(0.0f, 0.0f, 0.0f),
@@ -100,30 +106,34 @@ btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisi
 
 }
 
+#endif //OBSOLETE_MOTIONSTATE_LESS
+
+
 #define EXPERIMENTAL_JITTER_REMOVAL 1
 #ifdef EXPERIMENTAL_JITTER_REMOVAL
-//Bullet 2.20b has experimental code to reduce jitter just before objects fall asleep/deactivate
-//doesn't work very well yet (value 0 only reduces performance a bit, no difference in functionality)
-float gClippedAngvelThresholdSqr = 0.f;
-float  gClippedLinearThresholdSqr = 0.f;
+//Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate
+//doesn't work very well yet (value 0 disabled this damping)
+//note there this influences deactivation thresholds!
+float gClippedAngvelThresholdSqr = 0.01f;
+float  gClippedLinearThresholdSqr = 0.01f;
+float  gJitterVelocityDampingFactor = 1.f;
 #endif //EXPERIMENTAL_JITTER_REMOVAL
 
 void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 
 {
 
 #ifdef EXPERIMENTAL_JITTER_REMOVAL
-       //clip to avoid jitter
-       if (m_angularVelocity.length2() < gClippedAngvelThresholdSqr)
+       //if (wantsSleeping())
        {
-               m_angularVelocity.setValue(0,0,0);
-               printf("clipped!\n");
+               //clip to avoid jitter
+               if ((m_angularVelocity.length2() < gClippedAngvelThresholdSqr) &&
+                       (m_linearVelocity.length2() < gClippedLinearThresholdSqr))
+               {
+                       m_angularVelocity *= gJitterVelocityDampingFactor;
+                       m_linearVelocity *= gJitterVelocityDampingFactor;
+               }
        }
        
-       if (m_linearVelocity.length2() < gClippedLinearThresholdSqr)
-       {
-               m_linearVelocity.setValue(0,0,0);
-               printf("clipped!\n");
-       }
 #endif //EXPERIMENTAL_JITTER_REMOVAL
 
        btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
index fca3658..c5fecdb 100644 (file)
@@ -62,7 +62,11 @@ class btRigidBody  : public btCollisionObject
 
 public:
 
+#ifdef OBSOLETE_MOTIONSTATE_LESS
+       //not supported, please use btMotionState
        btRigidBody(float mass, const btTransform& worldTransform, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=0.f,btScalar angularDamping=0.f,btScalar friction=0.5f,btScalar restitution=0.f);
+#endif //OBSOLETE_MOTIONSTATE_LESS
+
        btRigidBody(float mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=0.f,btScalar angularDamping=0.f,btScalar friction=0.5f,btScalar restitution=0.f);
 
        void                    proceedToTransform(const btTransform& newTrans); 
@@ -71,11 +75,11 @@ public:
        ///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast
        static const btRigidBody*       upcast(const btCollisionObject* colObj)
        {
-               return (const btRigidBody*)colObj->m_internalOwner;
+               return (const btRigidBody*)colObj->getInternalOwner();
        }
        static btRigidBody*     upcast(btCollisionObject* colObj)
        {
-               return (btRigidBody*)colObj->m_internalOwner;
+               return (btRigidBody*)colObj->getInternalOwner();
        }
        
        /// continuous collision detection needs prediction
@@ -247,7 +251,7 @@ public:
 
        inline void     updateDeactivation(float timeStep)
        {
-               if ( (GetActivationState() == ISLAND_SLEEPING) || (GetActivationState() == DISABLE_DEACTIVATION))
+               if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
                        return;
 
                if ((getLinearVelocity().length2() < gLinearSleepingThreshold*gLinearSleepingThreshold) &&
@@ -257,7 +261,7 @@ public:
                } else
                {
                        m_deactivationTime=0.f;
-                       SetActivationState(0);
+                       setActivationState(0);
                }
 
        }
@@ -265,14 +269,14 @@ public:
        inline bool     wantsSleeping()
        {
 
-               if (GetActivationState() == DISABLE_DEACTIVATION)
+               if (getActivationState() == DISABLE_DEACTIVATION)
                        return false;
 
                //disable deactivation
                if (gDisableDeactivation || (gDeactivationTime == 0.f))
                        return false;
 
-               if ( (GetActivationState() == ISLAND_SLEEPING) || (GetActivationState() == WANTS_DEACTIVATION))
+               if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
                        return true;
 
                if (m_deactivationTime> gDeactivationTime)
@@ -309,6 +313,8 @@ public:
        void    setMotionState(btMotionState* motionState)
        {
                m_optionalMotionState = motionState;
+               if (m_optionalMotionState)
+                       motionState->getWorldTransform(m_worldTransform);
        }
 
        //for experimental overriding of friction/contact solver func
index 53a088d..dabf291 100644 (file)
@@ -114,12 +114,12 @@ void      btSimpleDynamicsWorld::updateAabbs()
                btRigidBody* body = btRigidBody::upcast(colObj);
                if (body)
                {
-                       if (body->IsActive() && (!body->isStaticObject()))
+                       if (body->isActive() && (!body->isStaticObject()))
                        {
                                btPoint3 minAabb,maxAabb;
-                               colObj->m_collisionShape->getAabb(colObj->m_worldTransform, minAabb,maxAabb);
+                               colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
                                btBroadphaseInterface* bp = getBroadphase();
-                               bp->setAabb(body->m_broadphaseHandle,minAabb,maxAabb);
+                               bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb);
                        }
                }
        }
@@ -134,7 +134,7 @@ void        btSimpleDynamicsWorld::integrateTransforms(float timeStep)
                btRigidBody* body = btRigidBody::upcast(colObj);
                if (body)
                {
-                       if (body->IsActive() && (!body->isStaticObject()))
+                       if (body->isActive() && (!body->isStaticObject()))
                        {
                                body->predictIntegratedTransform(timeStep, predictedTrans);
                                body->proceedToTransform( predictedTrans);
@@ -155,11 +155,11 @@ void      btSimpleDynamicsWorld::predictUnconstraintMotion(float timeStep)
                {
                        if (!body->isStaticObject())
                        {
-                               if (body->IsActive())
+                               if (body->isActive())
                                {
                                        body->applyForces( timeStep);
                                        body->integrateVelocities( timeStep);
-                                       body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform);
+                                       body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
                                }
                        }
                }
@@ -176,11 +176,22 @@ void      btSimpleDynamicsWorld::synchronizeMotionStates()
                btRigidBody* body = btRigidBody::upcast(colObj);
                if (body && body->getMotionState())
                {
-                       if (body->GetActivationState() != ISLAND_SLEEPING)
+                       if (body->getActivationState() != ISLAND_SLEEPING)
                        {
-                               body->getMotionState()->setWorldTransform(body->m_worldTransform);
+                               body->getMotionState()->setWorldTransform(body->getWorldTransform());
                        }
                }
        }
 
 }
+
+
+void   btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
+{
+       if (m_ownsConstraintSolver)
+       {
+               delete m_constraintSolver;
+       }
+       m_ownsConstraintSolver = false;
+       m_constraintSolver = solver;
+}
index a65b56c..f74e246 100644 (file)
@@ -77,6 +77,8 @@ public:
 
        void    synchronizeMotionStates();
 
+       virtual void    setConstraintSolver(btConstraintSolver* solver);
+
 };
 
 #endif //BT_SIMPLE_DYNAMICS_WORLD_H
index 7a2043e..700bdb8 100644 (file)
@@ -11,6 +11,7 @@
 
 #include "LinearMath/btVector3.h"
 #include "btRaycastVehicle.h"
+
 #include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
 #include "LinearMath/btQuaternion.h"
@@ -23,7 +24,7 @@
 
 
 
-static btRigidBody s_fixedObject( 0,btTransform(btQuaternion(0,0,0,1)),0);
+static btRigidBody s_fixedObject( 0,0,0);
 
 btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster )
 :m_vehicleRaycaster(raycaster),
@@ -75,8 +76,8 @@ btWheelInfo&  btRaycastVehicle::addWheel( const btVector3& connectionPointCS, con
        
        btWheelInfo& wheel = m_wheelInfo[getNumWheels()-1];
        
-       updateWheelTransformsWS( wheel );
-       updateWheelTransform(getNumWheels()-1);
+       updateWheelTransformsWS( wheel , false );
+       updateWheelTransform(getNumWheels()-1,false);
        return wheel;
 }
 
@@ -91,15 +92,18 @@ const btTransform&  btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const
 
 }
 
-void   btRaycastVehicle::updateWheelTransform( int wheelIndex )
+void   btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform)
 {
        
        btWheelInfo& wheel = m_wheelInfo[ wheelIndex ];
-       updateWheelTransformsWS(wheel);
+       updateWheelTransformsWS(wheel,interpolatedTransform);
        btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
        const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS;
        btVector3 fwd = up.cross(right);
        fwd = fwd.normalize();
+//     up = right.cross(fwd);
+//     up.normalize();
+
        //rotate around steering over de wheelAxleWS
        float steering = wheel.m_steering;
        
@@ -138,16 +142,16 @@ void btRaycastVehicle::resetSuspension()
        }
 }
 
-void   btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel )
+void   btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform)
 {
        wheel.m_raycastInfo.m_isInContact = false;
 
-       btTransform chassisTrans;
-       if (getRigidBody()->getMotionState())
+       btTransform chassisTrans = getChassisWorldTransform();
+       if (interpolatedTransform && (getRigidBody()->getMotionState()))
+       {
                getRigidBody()->getMotionState()->getWorldTransform(chassisTrans);
-       else
-               chassisTrans = getRigidBody()->getCenterOfMassTransform();
-       
+       }
+
        wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS );
        wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() *  wheel.m_wheelDirectionCS ;
        wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS;
@@ -155,7 +159,7 @@ void        btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel )
 
 btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
 {
-       updateWheelTransformsWS( wheel );
+       updateWheelTransformsWS( wheel,false);
 
        
        btScalar depth = -1;
@@ -239,12 +243,35 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
 }
 
 
+const btTransform& btRaycastVehicle::getChassisWorldTransform() const
+{
+       /*if (getRigidBody()->getMotionState())
+       {
+               btTransform chassisWorldTrans;
+               getRigidBody()->getMotionState()->getWorldTransform(chassisWorldTrans);
+               return chassisWorldTrans;
+       }
+       */
+
+       
+       return getRigidBody()->getCenterOfMassTransform();
+}
+
+
 void btRaycastVehicle::updateVehicle( btScalar step )
 {
+       {
+               for (int i=0;i<getNumWheels();i++)
+               {
+                       updateWheelTransform(i,false);
+               }
+       }
+
 
        m_currentVehicleSpeedKmHour = 3.6f * getRigidBody()->getLinearVelocity().length();
        
-       const btTransform& chassisTrans = getRigidBody()->getCenterOfMassTransform();
+       const btTransform& chassisTrans = getChassisWorldTransform();
+
        btVector3 forwardW (
                chassisTrans.getBasis()[0][m_indexForwardAxis],
                chassisTrans.getBasis()[1][m_indexForwardAxis],
@@ -304,10 +331,12 @@ void btRaycastVehicle::updateVehicle( btScalar step )
 
                if (wheel.m_raycastInfo.m_isInContact)
                {
+                       const btTransform&      chassisWorldTransform = getChassisWorldTransform();
+
                        btVector3 fwd (
-                               getRigidBody()->getCenterOfMassTransform().getBasis()[0][m_indexForwardAxis],
-                               getRigidBody()->getCenterOfMassTransform().getBasis()[1][m_indexForwardAxis],
-                               getRigidBody()->getCenterOfMassTransform().getBasis()[2][m_indexForwardAxis]);
+                               chassisWorldTransform.getBasis()[0][m_indexForwardAxis],
+                               chassisWorldTransform.getBasis()[1][m_indexForwardAxis],
+                               chassisWorldTransform.getBasis()[2][m_indexForwardAxis]);
 
                        btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
                        fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
index a7534c6..b928248 100644 (file)
@@ -68,9 +68,8 @@ public:
        virtual ~btRaycastVehicle() ;
 
                
-
-
-
+       const btTransform& getChassisWorldTransform() const;
+       
        btScalar rayCast(btWheelInfo& wheel);
 
        virtual void updateVehicle(btScalar step);
@@ -86,7 +85,7 @@ public:
 
        const btTransform&      getWheelTransformWS( int wheelIndex ) const;
 
-       void    updateWheelTransform( int wheelIndex );
+       void    updateWheelTransform( int wheelIndex, bool interpolatedTransform = true );
        
        void    setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth);
 
@@ -103,7 +102,7 @@ public:
 
        btWheelInfo&    getWheelInfo(int index);
 
-       void    updateWheelTransformsWS(btWheelInfo& wheel );
+       void    updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform = true);
 
        
        void setBrake(float brake,int wheelIndex);
index b954616..207eed9 100644 (file)
@@ -5,5 +5,6 @@ ${BULLET_PHYSICS_SOURCE_DIR}/src }
 
 ADD_LIBRARY(LibLinearMath
 btQuickprof.cpp
+btGeometryUtil.cpp
 )
 
index 1cc7d4e..2eacb8e 100644 (file)
@@ -18,6 +18,8 @@ subject to the following restrictions:
 #define AABB_UTIL2
 
 #include "LinearMath/btVector3.h"
+#include "LinearMath/btSimdMinMax.h"
+
 
 #define btMin(a,b) ((a < b ? a : b))
 #define btMax(a,b) ((a > b ? a : b))
@@ -53,5 +55,73 @@ SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3 *vertices,
        return true;
 }
 
+
+SIMD_FORCE_INLINE int  btOutcode(const btVector3& p,const btVector3& halfExtent) 
+{
+       return (p.getX()  < -halfExtent.getX() ? 0x01 : 0x0) |    
+                  (p.getX() >  halfExtent.getX() ? 0x08 : 0x0) |
+                  (p.getY() < -halfExtent.getY() ? 0x02 : 0x0) |    
+                  (p.getY() >  halfExtent.getY() ? 0x10 : 0x0) |
+                  (p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) |    
+                  (p.getZ() >  halfExtent.getZ() ? 0x20 : 0x0);
+}
+
+
+SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, 
+                                                                const btVector3& rayTo, 
+                                                                const btVector3& aabbMin, 
+                                                                const btVector3& aabbMax,
+                                         btScalar& param, btVector3& normal) 
+{
+       btVector3 aabbHalfExtent = (aabbMax-aabbMin)* 0.5f;
+       btVector3 aabbCenter = (aabbMax+aabbMin)* 0.5f;
+       btVector3       source = rayFrom - aabbCenter;
+       btVector3       target = rayTo - aabbCenter;
+       int     sourceOutcode = btOutcode(source,aabbHalfExtent);
+       int targetOutcode = btOutcode(target,aabbHalfExtent);
+       if ((sourceOutcode & targetOutcode) == 0x0)
+       {
+               btScalar lambda_enter = btScalar(0.0);
+               btScalar lambda_exit  = param;
+               btVector3 r = target - source;
+               int i;
+               btScalar        normSign = 1;
+               btVector3       hitNormal(0,0,0);
+               int bit=1;
+
+               for (int j=0;j<2;j++)
+               {
+                       for (i = 0; i != 3; ++i)
+                       {
+                               if (sourceOutcode & bit)
+                               {
+                                       btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
+                                       if (lambda_enter <= lambda)
+                                       {
+                                               lambda_enter = lambda;
+                                               hitNormal.setValue(0,0,0);
+                                               hitNormal[i] = normSign;
+                                       }
+                               }
+                               else if (targetOutcode & bit) 
+                               {
+                                       btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
+                                       btSetMin(lambda_exit, lambda);
+                               }
+                               bit<<=1;
+                       }
+                       normSign = -1.f;
+               }
+               if (lambda_enter <= lambda_exit)
+               {
+                       param = lambda_enter;
+                       normal = hitNormal;
+                       return true;
+               }
+       }
+       return false;
+}
+
+
 #endif
 
index 805631a..6b85b37 100644 (file)
@@ -19,16 +19,25 @@ struct      btDefaultMotionState : public btMotionState
        }
 
        ///synchronizes world transform from user to physics
-       virtual void    getWorldTransform(btTransform& centerOfMassWorldTrans )
+       virtual void    getWorldTransform(btTransform& centerOfMassWorldTrans ) const 
        {
                        centerOfMassWorldTrans =        m_centerOfMassOffset.inverse() * m_graphicsWorldTrans ;
        }
 
        ///synchronizes world transform from physics to user
+       ///Bullet only calls the update of worldtransform for active objects
        virtual void    setWorldTransform(const btTransform& centerOfMassWorldTrans)
        {
                        m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset ;
        }
+
+       ///Bullet gives a callback for objects that are about to be deactivated (put asleep)
+       /// You can intercept this callback for your own bookkeeping. 
+       ///Also you can return false to disable deactivation for this object this frame.
+       virtual bool deactivationCallback(void* userPointer) {
+               return true;
+       }
+
 };
 
 #endif //DEFAULT_MOTION_STATE_H
diff --git a/extern/bullet2/src/LinearMath/btGeometryUtil.cpp b/extern/bullet2/src/LinearMath/btGeometryUtil.cpp
new file mode 100644 (file)
index 0000000..69d061d
--- /dev/null
@@ -0,0 +1,168 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / 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 "btGeometryUtil.h"
+
+bool   btGeometryUtil::isPointInsidePlanes(const std::vector<btVector3>& planeEquations, const btVector3& point, float margin)
+{
+       int numbrushes = planeEquations.size();
+       for (int i=0;i<numbrushes;i++)
+       {
+               const btVector3& N1 = planeEquations[i];
+               float dist = float(N1.dot(point))+float(N1[3])-margin;
+               if (dist>0.f)
+               {
+                       return false;
+               }
+       }
+       return true;
+               
+}
+
+
+bool   btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const std::vector<btVector3>& vertices, float      margin)
+{
+       int numvertices = vertices.size();
+       for (int i=0;i<numvertices;i++)
+       {
+               const btVector3& N1 = vertices[i];
+               float dist = float(planeNormal.dot(N1))+float(planeNormal[3])-margin;
+               if (dist>0.f)
+               {
+                       return false;
+               }
+       }
+       return true;
+}
+
+bool notExist(const btVector3& planeEquation,const std::vector<btVector3>& planeEquations)
+{
+       int numbrushes = planeEquations.size();
+       for (int i=0;i<numbrushes;i++)
+       {
+               const btVector3& N1 = planeEquations[i];
+               if (planeEquation.dot(N1) > 0.999f)
+               {
+                       return false;
+               } 
+       }
+       return true;
+}
+
+void   btGeometryUtil::getPlaneEquationsFromVertices(std::vector<btVector3>& vertices, std::vector<btVector3>& planeEquationsOut )
+{
+               const int numvertices = vertices.size();
+       // brute force:
+       for (int i=0;i<numvertices;i++)
+       {
+               const btVector3& N1 = vertices[i];
+               
+
+               for (int j=i+1;j<numvertices;j++)
+               {
+                       const btVector3& N2 = vertices[j];
+                               
+                       for (int k=j+1;k<numvertices;k++)
+                       {
+
+                               const btVector3& N3 = vertices[k];
+
+                               btVector3 planeEquation,edge0,edge1;
+                               edge0 = N2-N1;
+                               edge1 = N3-N1;
+                               float normalSign = 1.f;
+                               for (int ww=0;ww<2;ww++)
+                               {
+                                       planeEquation = normalSign * edge0.cross(edge1);
+                                       if (planeEquation.length2() > 0.0001f)
+                                       {
+                                               planeEquation.normalize();
+                                               if (notExist(planeEquation,planeEquationsOut))
+                                               {
+                                                       planeEquation[3] = -planeEquation.dot(N1);
+                                                       
+                                                               //check if inside, and replace supportingVertexOut if needed
+                                                               if (areVerticesBehindPlane(planeEquation,vertices,0.01f))
+                                                               {
+                                                                       planeEquationsOut.push_back(planeEquation);
+                                                               }
+                                               }
+                                       }
+                                       normalSign = -1.f;
+                               }
+                       
+                       }
+               }
+       }
+
+}
+
+void   btGeometryUtil::getVerticesFromPlaneEquations(const std::vector<btVector3>& planeEquations , std::vector<btVector3>& verticesOut )
+{
+       const int numbrushes = planeEquations.size();
+       // brute force:
+       for (int i=0;i<numbrushes;i++)
+       {
+               const btVector3& N1 = planeEquations[i];
+               
+
+               for (int j=i+1;j<numbrushes;j++)
+               {
+                       const btVector3& N2 = planeEquations[j];
+                               
+                       for (int k=j+1;k<numbrushes;k++)
+                       {
+
+                               const btVector3& N3 = planeEquations[k];
+
+                               btVector3 n2n3; n2n3 = N2.cross(N3);
+                               btVector3 n3n1; n3n1 = N3.cross(N1);
+                               btVector3 n1n2; n1n2 = N1.cross(N2);
+                               
+                               if ( ( n2n3.length2() > 0.0001f ) &&
+                                        ( n3n1.length2() > 0.0001f ) &&
+                                        ( n1n2.length2() > 0.0001f ) )
+                               {
+                                       //point P out of 3 plane equations:
+
+                                       //      d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )  
+                                       //P =  -------------------------------------------------------------------------  
+                                       //   N1 . ( N2 * N3 )  
+
+
+                                       float quotient = (N1.dot(n2n3));
+                                       if (btFabs(quotient) > 0.000001f)
+                                       {
+                                               quotient = -1.f / quotient;
+                                               n2n3 *= N1[3];
+                                               n3n1 *= N2[3];
+                                               n1n2 *= N3[3];
+                                               btVector3 potentialVertex = n2n3;
+                                               potentialVertex += n3n1;
+                                               potentialVertex += n1n2;
+                                               potentialVertex *= quotient;
+
+                                               //check if inside, and replace supportingVertexOut if needed
+                                               if (isPointInsidePlanes(planeEquations,potentialVertex,0.01f))
+                                               {
+                                                       verticesOut.push_back(potentialVertex);
+                                               }
+                                       }
+                               }
+                       }
+               }
+       }
+}
\ No newline at end of file
diff --git a/extern/bullet2/src/LinearMath/btGeometryUtil.h b/extern/bullet2/src/LinearMath/btGeometryUtil.h
new file mode 100644 (file)
index 0000000..3170c21
--- /dev/null
@@ -0,0 +1,39 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / 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_GEOMETRY_UTIL_H
+#define BT_GEOMETRY_UTIL_H
+#include <vector>
+#include "btVector3.h"
+
+class btGeometryUtil
+{
+       public:
+       
+       
+               static void     getPlaneEquationsFromVertices(std::vector<btVector3>& vertices, std::vector<btVector3>& planeEquationsOut );
+
+               static void     getVerticesFromPlaneEquations(const std::vector<btVector3>& planeEquations , std::vector<btVector3>& verticesOut );
+       
+               static bool     isInside(const std::vector<btVector3>& vertices, const btVector3& planeNormal, float    margin);
+               
+               static bool     isPointInsidePlanes(const std::vector<btVector3>& planeEquations, const btVector3& point, float margin);
+
+               static bool     areVerticesBehindPlane(const btVector3& planeNormal, const std::vector<btVector3>& vertices, float      margin);
+
+};
+
+
+#endif //BT_GEOMETRY_UTIL_H
\ No newline at end of file
index 4bbb3d4..a9e212d 100644 (file)
@@ -29,10 +29,15 @@ class       btMotionState
                        
                }
                
-               virtual void    getWorldTransform(btTransform& worldTrans )=0;
+               virtual void    getWorldTransform(btTransform& worldTrans ) const =0;
 
+               //Bullet only calls the update of worldtransform for active objects
                virtual void    setWorldTransform(const btTransform& worldTrans)=0;
-
+               
+               //future: when Bullet makes attempt to deactivate object, you can intercept this callback (return false to disable deactivation for this object this frame)
+               virtual bool deactivationCallback(void* userPointer) {
+                       return true;
+               }
 };
 
 #endif //BT_MOTIONSTATE_H
index 23b8b8b..4d013a6 100644 (file)
@@ -1,25 +1,18 @@
-/************************************************************************
-* QuickProf                                                             *
-* Copyright (C) 2006                                                    *
-* Tyler Streeter  tylerstreeter@gmail.com                               *
-* All rights reserved.                                                  *
-* Web: http://quickprof.sourceforge.net                                 *
-*                                                                       *
-* This library is free software; you can redistribute it and/or         *
-* modify it under the terms of EITHER:                                  *
-*   (1) The GNU Lesser bteral Public License as published by the Free  *
-*       Software Foundation; either version 2.1 of the License, or (at  *
-*       your option) any later version. The text of the GNU Lesser      *
-*       bteral Public License is included with this library in the     *
-*       file license-LGPL.txt.                                          *
-*   (2) The BSD-style license that is included with this library in     *
-*       the file license-BSD.txt.                                       *
-*                                                                       *
-* This library is distributed in the hope that it will be useful,       *
-* but WITHOUT ANY WARRANTY; without even the implied warranty of        *
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
-* license-LGPL.txt and license-BSD.txt for more details.                *
-************************************************************************/
+/*
+Copyright (c) 2006 Tyler Streeter
+
+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.
+
+*/
+
 
 // Please visit the project website (http://quickprof.sourceforge.net) 
 // for usage instructions.
index 801ef07..ec560c8 100644 (file)
@@ -1,26 +1,18 @@
-/************************************************************************
-* QuickProf                                                             *
-* Copyright (C) 2006                                                    *
-* Tyler Streeter  tylerstreeter@gmail.com                               *
-* All rights reserved.                                                  *
-* Web: http://quickprof.sourceforge.net                                 *
-*                                                                       *
-* This library is free software; you can redistribute it and/or         *
-* modify it under the terms of EITHER:                                  *
-*   (1) The GNU Lesser bteral Public License as published by the Free  *
-*       Software Foundation; either version 2.1 of the License, or (at  *
-*       your option) any later version. The text of the GNU Lesser      *
-*       bteral Public License is included with this library in the     *
-*       file license-LGPL.txt.                                          *
-*   (2) The BSD-style license that is included with this library in     *
-*       the file license-BSD.txt.                                       *
-*                                                                       *
-* This library is distributed in the hope that it will be useful,       *
-* but WITHOUT ANY WARRANTY; without even the implied warranty of        *
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
-* license-LGPL.txt and license-BSD.txt for more details.                *
-************************************************************************/
+/*
+Copyright (c) 2006 Tyler Streeter
 
+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.
+
+*/
+       
 // Please visit the project website (http://quickprof.sourceforge.net) 
 // for usage instructions.
 
 typedef uint64_t __int64;
 #endif
 
+#if defined (SUNOS) || defined (__SUNOS__) 
+        #include <stdio.h> 
+#endif
+
 #if defined(WIN32) || defined(_WIN32)
        #define USE_WINDOWS_TIMERS
        #include <windows.h>
index dea040a..aa046b8 100644 (file)
@@ -88,13 +88,13 @@ SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
 #endif
 
 
-const btScalar  SIMD_2_PI         = 6.283185307179586232f;
-const btScalar  SIMD_PI           = SIMD_2_PI * btScalar(0.5f);
-const btScalar  SIMD_HALF_PI            = SIMD_2_PI * btScalar(0.25f);
-const btScalar  SIMD_RADS_PER_DEG = SIMD_2_PI / btScalar(360.0f);
-const btScalar  SIMD_DEGS_PER_RAD = btScalar(360.0f) / SIMD_2_PI;
-const btScalar  SIMD_EPSILON      = FLT_EPSILON;
-const btScalar  SIMD_INFINITY     = FLT_MAX;
+#define SIMD_2_PI         6.283185307179586232f
+#define SIMD_PI           (SIMD_2_PI * btScalar(0.5f))
+#define SIMD_HALF_PI      (SIMD_2_PI * btScalar(0.25f))
+#define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0f))
+#define SIMD_DEGS_PER_RAD  (btScalar(360.0f) / SIMD_2_PI)
+#define SIMD_EPSILON      FLT_EPSILON
+#define SIMD_INFINITY     FLT_MAX
 
 SIMD_FORCE_INLINE bool      btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; }
 
@@ -114,13 +114,14 @@ SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); }
 SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
 */
 
-SIMD_FORCE_INLINE int       btSign(btScalar x) {
-    return x < 0.0f ? -1 : x > 0.0f ? 1 : 0;
+SIMD_FORCE_INLINE int       btIsNegative(btScalar x) {
+    return x < 0.0f ? 1 : 0;
 }
 
 SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; }
 SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; }
 
+#define BT_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
 
 
 #endif //SIMD___SCALAR_H
index 16c3155..2731c8b 100644 (file)
@@ -16,6 +16,7 @@ subject to the following restrictions:
 
 #ifndef SIMD_MINMAX_H
 #define SIMD_MINMAX_H
+#include "LinearMath/btScalar.h"
 
 template <class T>
 SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b) {
index 12ca634..39fa830 100644 (file)
@@ -108,7 +108,16 @@ public:
        static void     calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
        {
                linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
-#ifdef USE_QUATERNION_DIFF
+               btVector3 axis;
+               btScalar  angle;
+               calculateDiffAxisAngle(transform0,transform1,axis,angle);
+               angVel = axis * angle / timeStep;
+       }
+
+       static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle)
+       {
+       
+       #ifdef USE_QUATERNION_DIFF
                btQuaternion orn0 = transform0.getRotation();
                btQuaternion orn1a = transform1.getRotation();
                btQuaternion orn1 = orn0.farthest(orn1a);
@@ -118,9 +127,7 @@ public:
                btQuaternion dorn;
                dmat.getRotation(dorn);
 #endif//USE_QUATERNION_DIFF
-
-               btVector3 axis;
-               btScalar  angle;
+       
                angle = dorn.getAngle();
                axis = btVector3(dorn.x(),dorn.y(),dorn.z());
                axis[3] = 0.f;
@@ -130,13 +137,8 @@ public:
                        axis = btVector3(1.f,0.f,0.f);
                else
                        axis /= btSqrt(len);
-
-               
-               angVel = axis * angle / timeStep;
-
        }
 
-
 };
 
 #endif //SIMD_TRANSFORM_UTIL_H