upgraded Bullet rigidbody physics to latest version 1.9
authorErwin Coumans <blender@erwincoumans.com>
Mon, 28 Aug 2006 06:44:29 +0000 (06:44 +0000)
committerErwin Coumans <blender@erwincoumans.com>
Mon, 28 Aug 2006 06:44:29 +0000 (06:44 +0000)
95 files changed:
extern/bullet/Bullet/BroadphaseCollision/AxisSweep3.cpp
extern/bullet/Bullet/BroadphaseCollision/AxisSweep3.h
extern/bullet/Bullet/BroadphaseCollision/BroadphaseInterface.h
extern/bullet/Bullet/BroadphaseCollision/BroadphaseProxy.h
extern/bullet/Bullet/BroadphaseCollision/Dispatcher.h
extern/bullet/Bullet/BroadphaseCollision/OverlappingPairCache.cpp [new file with mode: 0644]
extern/bullet/Bullet/BroadphaseCollision/OverlappingPairCache.h [new file with mode: 0644]
extern/bullet/Bullet/BroadphaseCollision/SimpleBroadphase.cpp
extern/bullet/Bullet/BroadphaseCollision/SimpleBroadphase.h
extern/bullet/Bullet/CollisionDispatch/CollisionCreateFunc.h [new file with mode: 0644]
extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp
extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.h
extern/bullet/Bullet/CollisionDispatch/CollisionObject.cpp
extern/bullet/Bullet/CollisionDispatch/CollisionObject.h
extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp
extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h
extern/bullet/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.cpp [new file with mode: 0644]
extern/bullet/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.h [new file with mode: 0644]
extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp
extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.h
extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp
extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.h
extern/bullet/Bullet/CollisionDispatch/ManifoldResult.cpp
extern/bullet/Bullet/CollisionDispatch/ManifoldResult.h
extern/bullet/Bullet/CollisionDispatch/SimulationIslandManager.cpp [new file with mode: 0644]
extern/bullet/Bullet/CollisionDispatch/SimulationIslandManager.h [new file with mode: 0644]
extern/bullet/Bullet/CollisionShapes/BoxShape.h
extern/bullet/Bullet/CollisionShapes/CollisionShape.h
extern/bullet/Bullet/CollisionShapes/CompoundShape.cpp [new file with mode: 0644]
extern/bullet/Bullet/CollisionShapes/CompoundShape.h [new file with mode: 0644]
extern/bullet/Bullet/CollisionShapes/ConcaveShape.cpp [new file with mode: 0644]
extern/bullet/Bullet/CollisionShapes/ConcaveShape.h [new file with mode: 0644]
extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp [deleted file]
extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.h [deleted file]
extern/bullet/Bullet/CollisionShapes/ConvexTriangleMeshShape.cpp [new file with mode: 0644]
extern/bullet/Bullet/CollisionShapes/ConvexTriangleMeshShape.h [new file with mode: 0644]
extern/bullet/Bullet/CollisionShapes/CylinderShape.h
extern/bullet/Bullet/CollisionShapes/EmptyShape.h
extern/bullet/Bullet/CollisionShapes/OptimizedBvh.cpp
extern/bullet/Bullet/CollisionShapes/StaticPlaneShape.cpp [new file with mode: 0644]
extern/bullet/Bullet/CollisionShapes/StaticPlaneShape.h [new file with mode: 0644]
extern/bullet/Bullet/CollisionShapes/TriangleIndexVertexArray.cpp
extern/bullet/Bullet/CollisionShapes/TriangleIndexVertexArray.h
extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.cpp
extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.h
extern/bullet/Bullet/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h
extern/bullet/Bullet/NarrowPhaseCollision/GjkConvexCast.cpp
extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.cpp
extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.h
extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h
extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp
extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp
extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h
extern/bullet/Bullet/NarrowPhaseCollision/PointCollector.h
extern/bullet/Bullet/NarrowPhaseCollision/SimplexSolverInterface.h
extern/bullet/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.cpp
extern/bullet/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.h
extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp
extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.h
extern/bullet/BulletDynamics/ConstraintSolver/ContactSolverInfo.h
extern/bullet/BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp [new file with mode: 0644]
extern/bullet/BulletDynamics/ConstraintSolver/Generic6DofConstraint.h [new file with mode: 0644]
extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp
extern/bullet/BulletDynamics/ConstraintSolver/JacobianEntry.h
extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp [deleted file]
extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.h [deleted file]
extern/bullet/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cpp [moved from extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp with 91% similarity]
extern/bullet/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.h [moved from extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h with 81% similarity]
extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.cpp [deleted file]
extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.h [deleted file]
extern/bullet/BulletDynamics/Dynamics/ContactJoint.cpp
extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp
extern/bullet/BulletDynamics/Dynamics/RigidBody.h
extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp
extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h
extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h
extern/bullet/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.cpp [new file with mode: 0644]
extern/bullet/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.h [new file with mode: 0644]
extern/bullet/Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.cpp [new file with mode: 0644]
extern/bullet/Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.h [new file with mode: 0644]
extern/bullet/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp [new file with mode: 0644]
extern/bullet/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.h [new file with mode: 0644]
extern/bullet/Extras/PhysicsInterface/Common/PHY_DynamicTypes.h
extern/bullet/Extras/PhysicsInterface/Common/PHY_IPhysicsEnvironment.h
extern/bullet/LinearMath/SimdScalar.h
extern/bullet/LinearMath/SimdTransformUtil.h
extern/bullet/LinearMath/SimdVector3.h
source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp
source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
source/gameengine/Physics/Bullet/CcdPhysicsController.h
source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
source/gameengine/Physics/common/PHY_DynamicTypes.h
source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h

index 25abf92aea004c0320fb15e033d047b2ec52938a..235afa1f9fe1362f39fdd2dbf9ee416a72e7e670 100644 (file)
 
 #include <assert.h>
 
-BroadphaseProxy*       AxisSweep3::CreateProxy(  const SimdVector3& min,  const SimdVector3& max,int shapeType,void* userPtr )
+BroadphaseProxy*       AxisSweep3::CreateProxy(  const SimdVector3& min,  const SimdVector3& max,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask)
 {
-               unsigned short handleId = AddHandle(min,max, userPtr);
+               unsigned short handleId = AddHandle(min,max, userPtr,collisionFilterGroup,collisionFilterMask);
                
                Handle* handle = GetHandle(handleId);
+                               
                return handle;
 }
 
@@ -47,6 +48,7 @@ void  AxisSweep3::SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const
 
 
 AxisSweep3::AxisSweep3(const SimdPoint3& worldAabbMin,const SimdPoint3& worldAabbMax, int maxHandles, int maxOverlaps)
+:OverlappingPairCache(maxOverlaps)
 {
        //assert(bounds.HasVolume());
 
@@ -156,7 +158,7 @@ void AxisSweep3::FreeHandle(unsigned short handle)
 
 
 
-unsigned short AxisSweep3::AddHandle(const SimdPoint3& aabbMin,const SimdPoint3& aabbMax, void* pOwner)
+unsigned short AxisSweep3::AddHandle(const SimdPoint3& aabbMin,const SimdPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask)
 {
        // quantize the bounds
        unsigned short min[3], max[3];
@@ -169,10 +171,11 @@ unsigned short AxisSweep3::AddHandle(const SimdPoint3& aabbMin,const SimdPoint3&
 
        Handle* pHandle = GetHandle(handle);
        
-
        pHandle->m_handleId = handle;
        //pHandle->m_pOverlaps = 0;
        pHandle->m_clientObject = pOwner;
+       pHandle->m_collisionFilterGroup = collisionFilterGroup;
+       pHandle->m_collisionFilterMask = collisionFilterMask;
 
        // compute current limit of edge arrays
        int limit = m_numHandles * 2;
index 9591bdbb40e2f0a4a94b29f778d5750ce860ccf5..0a4a75d523af8c044ccfacc47de3ceacce21c623 100644 (file)
 
 #include "SimdPoint3.h"
 #include "SimdVector3.h"
-#include "SimpleBroadphase.h"
+#include "OverlappingPairCache.h"
 #include "BroadphaseProxy.h"
 
-class AxisSweep3 : public SimpleBroadphase
+/// AxisSweep3 is an efficient implementation of the 3d axis sweep and prune broadphase.
+/// It uses arrays rather then lists for storage of the 3 axis. Also it operates using integer coordinates instead of floats.
+/// The TestOverlap check is optimized to check the array index, rather then the actual AABB coordinates/pos
+class AxisSweep3 : public OverlappingPairCache
 {
 
 public:
@@ -96,14 +99,14 @@ public:
                //this is replace by sweep and prune
        }
        
-       unsigned short AddHandle(const SimdPoint3& aabbMin,const SimdPoint3& aabbMax, void* pOwner);
+       unsigned short AddHandle(const SimdPoint3& aabbMin,const SimdPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask);
        void RemoveHandle(unsigned short handle);
        void UpdateHandle(unsigned short handle, const SimdPoint3& aabbMin,const SimdPoint3& aabbMax);
        inline Handle* GetHandle(unsigned short index) const {return m_pHandles + index;}
 
 
        //Broadphase Interface
-       virtual BroadphaseProxy*        CreateProxy(  const SimdVector3& min,  const SimdVector3& max,int shapeType,void* userPtr );
+       virtual BroadphaseProxy*        CreateProxy(  const SimdVector3& min,  const SimdVector3& max,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask);
        virtual void    DestroyProxy(BroadphaseProxy* proxy);
        virtual void    SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const SimdVector3& aabbMax);
 
index 5b9efc6b92a6306abd10355b05039383248eca1a..3ca273bde54fa2cd221053c9ba9236d8f7d03b4f 100644 (file)
@@ -29,11 +29,11 @@ class BroadphaseInterface
 public:
        virtual ~BroadphaseInterface() {}
 
-       virtual BroadphaseProxy*        CreateProxy(  const SimdVector3& min,  const SimdVector3& max,int shapeType,void* userPtr ) =0;
+       virtual BroadphaseProxy*        CreateProxy(  const SimdVector3& min,  const SimdVector3& max,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask) =0;
        virtual void    DestroyProxy(BroadphaseProxy* proxy)=0;
        virtual void    SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const SimdVector3& aabbMax)=0;
        virtual void    CleanProxyFromPairs(BroadphaseProxy* proxy)=0;
-       virtual void    DispatchAllCollisionPairs(Dispatcher&   dispatcher,DispatcherInfo& dispatchInfo)=0;
+       
 
 };
 
index 78bd7ab7697ffd0cd6935f1eb448d820f84740d2..35b2dc866f1d3cc1a336583d26fcc1743de6e31f 100644 (file)
@@ -27,6 +27,7 @@ enum BroadphaseNativeTypes
        BOX_SHAPE_PROXYTYPE,
        TRIANGLE_SHAPE_PROXYTYPE,
        TETRAHEDRAL_SHAPE_PROXYTYPE,
+       CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE,
        CONVEX_HULL_SHAPE_PROXYTYPE,
 //implicit convex shapes
 IMPLICIT_CONVEX_SHAPES_START_HERE,
@@ -42,6 +43,10 @@ CONCAVE_SHAPES_START_HERE,
        //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy!
        TRIANGLE_MESH_SHAPE_PROXYTYPE,
        EMPTY_SHAPE_PROXYTYPE,
+       STATIC_PLANE_PROXYTYPE,
+CONCAVE_SHAPES_END_HERE,
+
+       COMPOUND_SHAPE_PROXYTYPE,
 
        MAX_BROADPHASE_COLLISION_TYPES
 };
@@ -53,12 +58,16 @@ struct BroadphaseProxy
        
        //Usually the client CollisionObject or Rigidbody class
        void*   m_clientObject;
+       short int m_collisionFilterGroup;
+       short int m_collisionFilterMask;
 
-
+       //used for memory pools
        BroadphaseProxy() :m_clientObject(0){}
-       BroadphaseProxy(int shapeType,void* userPtr)
-               :m_clientObject(userPtr)
-               //m_clientObjectType(shapeType)
+
+       BroadphaseProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask)
+               :m_clientObject(userPtr),
+               m_collisionFilterGroup(collisionFilterGroup),
+               m_collisionFilterMask(collisionFilterMask)
        {
        }
        
index 7f8351bcd93a5c02a889cde11c8ebe282ee95e75..5edfbbdd3b5b21f2537c2980237f0d8024da1ff8 100644 (file)
@@ -21,6 +21,7 @@ struct BroadphaseProxy;
 class RigidBody;
 struct CollisionObject;
 class ManifoldResult;
+class OverlappingPairCache;
 
 enum CollisionDispatcherId
 {
@@ -80,10 +81,17 @@ public:
 
        virtual bool    NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1) = 0;
 
+       virtual bool    NeedsResponse(const CollisionObject& colObj0,const CollisionObject& colObj1)=0;
+
        virtual ManifoldResult* GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold) =0;
 
        virtual void    ReleaseManifoldResult(ManifoldResult*)=0;
 
+       virtual void    DispatchAllCollisionPairs(struct BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo)=0;
+
+       virtual int GetNumManifolds() const = 0;
+
+       virtual PersistentManifold* GetManifoldByIndexInternal(int index) = 0;
 
 };
 
diff --git a/extern/bullet/Bullet/BroadphaseCollision/OverlappingPairCache.cpp b/extern/bullet/Bullet/BroadphaseCollision/OverlappingPairCache.cpp
new file mode 100644 (file)
index 0000000..df53504
--- /dev/null
@@ -0,0 +1,151 @@
+
+/*
+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 "OverlappingPairCache.h"
+
+#include "Dispatcher.h"
+#include "CollisionAlgorithm.h"
+
+
+OverlappingPairCache::OverlappingPairCache(int maxOverlap):
+m_blockedForChanges(false),
+m_NumOverlapBroadphasePair(0),
+m_maxOverlap(maxOverlap)
+{
+       m_OverlappingPairs = new BroadphasePair[maxOverlap];
+}
+
+
+OverlappingPairCache::~OverlappingPairCache()
+{
+       delete [] m_OverlappingPairs;
+}
+
+
+void   OverlappingPairCache::RemoveOverlappingPair(BroadphasePair& pair)
+{
+       CleanOverlappingPair(pair);
+       int     index = &pair - &m_OverlappingPairs[0];
+       //remove efficiently, swap with the last
+       m_OverlappingPairs[index] = m_OverlappingPairs[m_NumOverlapBroadphasePair-1];
+       m_NumOverlapBroadphasePair--;
+}
+
+
+void   OverlappingPairCache::CleanOverlappingPair(BroadphasePair& pair)
+{
+       for (int dispatcherId=0;dispatcherId<SIMPLE_MAX_ALGORITHMS;dispatcherId++)
+       {
+               if (pair.m_algorithms[dispatcherId])
+               {
+                       {
+                               delete pair.m_algorithms[dispatcherId];
+                               pair.m_algorithms[dispatcherId]=0;
+                       }
+               }
+       }
+}
+
+
+
+
+
+void   OverlappingPairCache::AddOverlappingPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
+{
+       //don't add overlap with own
+       assert(proxy0 != proxy1);
+
+       if (!NeedsCollision(proxy0,proxy1))
+               return;
+
+
+       BroadphasePair pair(*proxy0,*proxy1);
+       m_OverlappingPairs[m_NumOverlapBroadphasePair] = pair;
+
+       int i;
+       for (i=0;i<SIMPLE_MAX_ALGORITHMS;i++)
+       {
+               assert(!m_OverlappingPairs[m_NumOverlapBroadphasePair].m_algorithms[i]);
+               m_OverlappingPairs[m_NumOverlapBroadphasePair].m_algorithms[i] = 0;
+       }
+
+       if (m_NumOverlapBroadphasePair >= m_maxOverlap)
+       {
+               //printf("Error: too many overlapping objects: m_NumOverlapBroadphasePair: %d\n",m_NumOverlapBroadphasePair);
+#ifdef DEBUG
+               assert(0);
+#endif 
+       } else
+       {
+               m_NumOverlapBroadphasePair++;
+       }
+
+
+}
+
+
+BroadphasePair*        OverlappingPairCache::FindPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
+{
+       BroadphasePair* foundPair = 0;
+
+       int i;
+       for (i=m_NumOverlapBroadphasePair-1;i>=0;i--)
+       {
+               BroadphasePair& pair = m_OverlappingPairs[i];
+               if (((pair.m_pProxy0 == proxy0) && (pair.m_pProxy1 == proxy1)) ||
+                       ((pair.m_pProxy0 == proxy1) && (pair.m_pProxy1 == proxy0)))
+               {
+                       foundPair = &pair;
+                       return foundPair;
+               }
+       }       
+
+       return foundPair;
+}
+
+
+
+void   OverlappingPairCache::CleanProxyFromPairs(BroadphaseProxy* proxy)
+{
+       for (int i=0;i<m_NumOverlapBroadphasePair;i++)
+       {
+               BroadphasePair& pair = m_OverlappingPairs[i];
+               if (pair.m_pProxy0 == proxy ||
+                       pair.m_pProxy1 == proxy)
+               {
+                       CleanOverlappingPair(pair);
+               }
+       }
+}
+
+void   OverlappingPairCache::RemoveOverlappingPairsContainingProxy(BroadphaseProxy* proxy)
+{
+       int i;
+       for ( i=m_NumOverlapBroadphasePair-1;i>=0;i--)
+       {
+               BroadphasePair& pair = m_OverlappingPairs[i];
+               if (pair.m_pProxy0 == proxy ||
+                       pair.m_pProxy1 == proxy)
+               {
+                       RemoveOverlappingPair(pair);
+               }
+       }
+}
+
+
+
diff --git a/extern/bullet/Bullet/BroadphaseCollision/OverlappingPairCache.h b/extern/bullet/Bullet/BroadphaseCollision/OverlappingPairCache.h
new file mode 100644 (file)
index 0000000..0fba0b2
--- /dev/null
@@ -0,0 +1,85 @@
+
+/*
+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 OVERLAPPING_PAIR_CACHE_H
+#define OVERLAPPING_PAIR_CACHE_H
+
+
+#include "BroadphaseInterface.h"
+#include "BroadphaseProxy.h"
+#include "SimdPoint3.h"
+
+
+///OverlappingPairCache maintains the objects with overlapping AABB
+///Typically managed by the Broadphase, Axis3Sweep or SimpleBroadphase
+class  OverlappingPairCache : public BroadphaseInterface
+{
+
+       BroadphasePair* m_OverlappingPairs;
+       int     m_NumOverlapBroadphasePair;
+       int m_maxOverlap;
+       
+       //during the dispatch, check that user doesn't destroy/create proxy
+       bool            m_blockedForChanges;
+
+       
+       public:
+               
+       OverlappingPairCache(int maxOverlap);   
+       virtual ~OverlappingPairCache();
+       
+       int             GetNumOverlappingPairs() const
+       {
+               return m_NumOverlapBroadphasePair;
+       }
+
+       BroadphasePair& GetOverlappingPair(int index)
+       {
+               return m_OverlappingPairs[index];
+       }
+
+       void    RemoveOverlappingPair(BroadphasePair& pair);
+
+       void    CleanOverlappingPair(BroadphasePair& pair);
+       
+       void    AddOverlappingPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
+
+       BroadphasePair* FindPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
+       
+       
+       
+       void    CleanProxyFromPairs(BroadphaseProxy* proxy);
+
+       void    RemoveOverlappingPairsContainingProxy(BroadphaseProxy* proxy);
+
+
+       inline bool NeedsCollision(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1) const
+       {
+               bool collides = proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask;
+               collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+               
+               return collides;
+       }
+               
+       
+
+       virtual void    RefreshOverlappingPairs() =0;
+
+
+
+
+};
+#endif //OVERLAPPING_PAIR_CACHE_H
\ No newline at end of file
index c0796f9b9f3bfaaaa497df4f8702e94029340b22..5a648e1d3067673ff8255cfb2726ec220b915d75 100644 (file)
@@ -14,8 +14,8 @@ subject to the following restrictions:
 */
 
 #include "SimpleBroadphase.h"
-#include "BroadphaseCollision/Dispatcher.h"
-#include "BroadphaseCollision/CollisionAlgorithm.h"
+#include <BroadphaseCollision/Dispatcher.h>
+#include <BroadphaseCollision/CollisionAlgorithm.h>
 
 #include "SimdVector3.h"
 #include "SimdTransform.h"
@@ -36,19 +36,16 @@ void        SimpleBroadphase::validate()
 }
 
 SimpleBroadphase::SimpleBroadphase(int maxProxies,int maxOverlap)
-       :m_firstFreeProxy(0),
+       :OverlappingPairCache(maxOverlap),
+       m_firstFreeProxy(0),
        m_numProxies(0),
-       m_blockedForChanges(false),
-       m_NumOverlapBroadphasePair(0),
-       m_maxProxies(maxProxies),
-       m_maxOverlap(maxOverlap)
+       m_maxProxies(maxProxies)
 {
 
        m_proxies = new SimpleBroadphaseProxy[maxProxies];
        m_freeProxies = new int[maxProxies];
        m_pProxies = new SimpleBroadphaseProxy*[maxProxies];
-       m_OverlappingPairs = new BroadphasePair[maxOverlap];
-
+       
 
        int i;
        for (i=0;i<m_maxProxies;i++)
@@ -62,7 +59,6 @@ SimpleBroadphase::~SimpleBroadphase()
        delete[] m_proxies;
        delete []m_freeProxies;
        delete [] m_pProxies;
-       delete [] m_OverlappingPairs;
 
        /*int i;
        for (i=m_numProxies-1;i>=0;i--)
@@ -74,7 +70,7 @@ SimpleBroadphase::~SimpleBroadphase()
 }
 
 
-BroadphaseProxy*       SimpleBroadphase::CreateProxy(  const SimdVector3& min,  const SimdVector3& max,int shapeType,void* userPtr)
+BroadphaseProxy*       SimpleBroadphase::CreateProxy(  const SimdVector3& min,  const SimdVector3& max,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask)
 {
        if (m_numProxies >= m_maxProxies)
        {
@@ -84,7 +80,7 @@ BroadphaseProxy*      SimpleBroadphase::CreateProxy(  const SimdVector3& min,  const
        assert(min[0]<= max[0] && min[1]<= max[1] && min[2]<= max[2]);
 
        int freeIndex= m_freeProxies[m_firstFreeProxy];
-       SimpleBroadphaseProxy* proxy = new (&m_proxies[freeIndex])SimpleBroadphaseProxy(min,max,shapeType,userPtr);
+       SimpleBroadphaseProxy* proxy = new (&m_proxies[freeIndex])SimpleBroadphaseProxy(min,max,shapeType,userPtr,collisionFilterGroup,collisionFilterMask);
        m_firstFreeProxy++;
 
        SimpleBroadphaseProxy* proxy1 = &m_proxies[0];
@@ -99,19 +95,7 @@ BroadphaseProxy*     SimpleBroadphase::CreateProxy(  const SimdVector3& min,  const
        return proxy;
 }
 
-void   SimpleBroadphase::RemoveOverlappingPairsContainingProxy(BroadphaseProxy* proxy)
-{
-       int i;
-               for ( i=m_NumOverlapBroadphasePair-1;i>=0;i--)
-               {
-                       BroadphasePair& pair = m_OverlappingPairs[i];
-                       if (pair.m_pProxy0 == proxy ||
-                                       pair.m_pProxy1 == proxy)
-                       {
-                               RemoveOverlappingPair(pair);
-                       }
-               }
-}
+
 
 void   SimpleBroadphase::DestroyProxy(BroadphaseProxy* proxyOrg)
 {
@@ -148,89 +132,13 @@ void      SimpleBroadphase::SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin
        sbp->m_max = aabbMax;
 }
 
-void   SimpleBroadphase::CleanOverlappingPair(BroadphasePair& pair)
-{
-       for (int dispatcherId=0;dispatcherId<SIMPLE_MAX_ALGORITHMS;dispatcherId++)
-       {
-               if (pair.m_algorithms[dispatcherId])
-               {
-                       {
-                               delete pair.m_algorithms[dispatcherId];
-                               pair.m_algorithms[dispatcherId]=0;
-                       }
-               }
-       }
-}
-
 
-void   SimpleBroadphase::CleanProxyFromPairs(BroadphaseProxy* proxy)
-{
-       for (int i=0;i<m_NumOverlapBroadphasePair;i++)
-       {
-               BroadphasePair& pair = m_OverlappingPairs[i];
-               if (pair.m_pProxy0 == proxy ||
-                               pair.m_pProxy1 == proxy)
-               {
-                       CleanOverlappingPair(pair);
-               }
-       }
-}
 
-void   SimpleBroadphase::AddOverlappingPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
-{
-       //don't add overlap with own
-       assert(proxy0 != proxy1);
 
-       BroadphasePair pair(*proxy0,*proxy1);
-       m_OverlappingPairs[m_NumOverlapBroadphasePair] = pair;
 
-       int i;
-       for (i=0;i<SIMPLE_MAX_ALGORITHMS;i++)
-       {
-               assert(!m_OverlappingPairs[m_NumOverlapBroadphasePair].m_algorithms[i]);
-               m_OverlappingPairs[m_NumOverlapBroadphasePair].m_algorithms[i] = 0;
-       }
-
-       if (m_NumOverlapBroadphasePair >= m_maxOverlap)
-       {
-               //printf("Error: too many overlapping objects: m_NumOverlapBroadphasePair: %d\n",m_NumOverlapBroadphasePair);
-#ifdef DEBUG
-               assert(0);
-#endif 
-       } else
-       {
-               m_NumOverlapBroadphasePair++;
-       }
-
-       
-}
        
-BroadphasePair*        SimpleBroadphase::FindPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
-{
-       BroadphasePair* foundPair = 0;
 
-       int i;
-       for (i=m_NumOverlapBroadphasePair-1;i>=0;i--)
-       {
-               BroadphasePair& pair = m_OverlappingPairs[i];
-               if (((pair.m_pProxy0 == proxy0) && (pair.m_pProxy1 == proxy1)) ||
-                       ((pair.m_pProxy0 == proxy1) && (pair.m_pProxy1 == proxy0)))
-               {
-                       foundPair = &pair;
-                       return foundPair;
-               }
-       }       
 
-       return foundPair;
-}
-void   SimpleBroadphase::RemoveOverlappingPair(BroadphasePair& pair)
-{
-    CleanOverlappingPair(pair);
-       int     index = &pair - &m_OverlappingPairs[0];
-       //remove efficiently, swap with the last
-       m_OverlappingPairs[index] = m_OverlappingPairs[m_NumOverlapBroadphasePair-1];
-       m_NumOverlapBroadphasePair--;
-}
 
 bool   SimpleBroadphase::AabbOverlap(SimpleBroadphaseProxy* proxy0,SimpleBroadphaseProxy* proxy1)
 {
@@ -265,9 +173,10 @@ void       SimpleBroadphase::RefreshOverlappingPairs()
        }
 
        //then remove non-overlapping ones
-       for (i=0;i<m_NumOverlapBroadphasePair;i++)
+       for (i=0;i<GetNumOverlappingPairs();i++)
        {
-               BroadphasePair& pair = m_OverlappingPairs[i];
+               BroadphasePair& pair = GetOverlappingPair(i);
+
                SimpleBroadphaseProxy* proxy0 = GetSimpleProxyFromProxy(pair.m_pProxy0);
                SimpleBroadphaseProxy* proxy1 = GetSimpleProxyFromProxy(pair.m_pProxy1);
                if (!AabbOverlap(proxy0,proxy1))
@@ -280,69 +189,4 @@ void       SimpleBroadphase::RefreshOverlappingPairs()
 
 }
 
-void   SimpleBroadphase::DispatchAllCollisionPairs(Dispatcher& dispatcher,DispatcherInfo& dispatchInfo)
-{
-       m_blockedForChanges = true;
-
-       int i;
-       
-       int dispatcherId = dispatcher.GetUniqueId();
-
-       RefreshOverlappingPairs();
-
-       for (i=0;i<m_NumOverlapBroadphasePair;i++)
-       {
-               
-               BroadphasePair& pair = m_OverlappingPairs[i];
-               
-               if (dispatcherId>= 0)
-               {
-                       //dispatcher will keep algorithms persistent in the collision pair
-                       if (!pair.m_algorithms[dispatcherId])
-                       {
-                               pair.m_algorithms[dispatcherId] = dispatcher.FindAlgorithm(
-                                       *pair.m_pProxy0,
-                                       *pair.m_pProxy1);
-                       }
-
-                       if (pair.m_algorithms[dispatcherId])
-                       {
-                               if (dispatchInfo.m_dispatchFunc ==              DispatcherInfo::DISPATCH_DISCRETE)
-                               {
-                                       pair.m_algorithms[dispatcherId]->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
-                               } else
-                               {
-                                       float toi = pair.m_algorithms[dispatcherId]->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
-                                       if (dispatchInfo.m_timeOfImpact > toi)
-                                               dispatchInfo.m_timeOfImpact = toi;
-
-                               }
-                       }
-               } else
-               {
-                       //non-persistent algorithm dispatcher
-                               CollisionAlgorithm* algo = dispatcher.FindAlgorithm(
-                                       *pair.m_pProxy0,
-                                       *pair.m_pProxy1);
-
-                               if (algo)
-                               {
-                                       if (dispatchInfo.m_dispatchFunc ==              DispatcherInfo::DISPATCH_DISCRETE)
-                                       {
-                                               algo->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
-                                       } else
-                                       {
-                                               float toi = algo->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
-                                               if (dispatchInfo.m_timeOfImpact > toi)
-                                                       dispatchInfo.m_timeOfImpact = toi;
-                                       }
-                               }
-               }
-
-       }
-
-       m_blockedForChanges = false;
-
-}
-
 
index b61f82b62a8b78b132879b6af20430cd0093bfd1..e67c71056871a02992f4676652ee773ebb7830bf 100644 (file)
@@ -16,12 +16,9 @@ subject to the following restrictions:
 #ifndef SIMPLE_BROADPHASE_H
 #define SIMPLE_BROADPHASE_H
 
-//#define SIMPLE_MAX_PROXIES 8192
-//#define SIMPLE_MAX_OVERLAP 4096
 
-#include "BroadphaseInterface.h"
-#include "BroadphaseProxy.h"
-#include "SimdPoint3.h"
+#include "OverlappingPairCache.h"
+
 
 struct SimpleBroadphaseProxy : public BroadphaseProxy
 {
@@ -30,8 +27,8 @@ struct SimpleBroadphaseProxy : public BroadphaseProxy
        
        SimpleBroadphaseProxy() {};
 
-       SimpleBroadphaseProxy(const SimdPoint3& minpt,const SimdPoint3& maxpt,int shapeType,void* userPtr)
-       :BroadphaseProxy(shapeType,userPtr),
+       SimpleBroadphaseProxy(const SimdPoint3& minpt,const SimdPoint3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask)
+       :BroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask),
        m_min(minpt),m_max(maxpt)               
        {
        }
@@ -40,7 +37,7 @@ struct SimpleBroadphaseProxy : public BroadphaseProxy
 };
 
 ///SimpleBroadphase is a brute force aabb culling broadphase based on O(n^2) aabb checks
-class SimpleBroadphase : public BroadphaseInterface
+class SimpleBroadphase : public OverlappingPairCache
 {
 
        SimpleBroadphaseProxy*  m_proxies;
@@ -50,14 +47,10 @@ class SimpleBroadphase : public BroadphaseInterface
        SimpleBroadphaseProxy** m_pProxies;
        int                             m_numProxies;
 
-       //during the dispatch, check that user doesn't destroy/create proxy
-       bool            m_blockedForChanges;
-
-       BroadphasePair* m_OverlappingPairs;
-       int     m_NumOverlapBroadphasePair;
+       
 
        int m_maxProxies;
-       int m_maxOverlap;
+       
        
        inline SimpleBroadphaseProxy*   GetSimpleProxyFromProxy(BroadphaseProxy* proxy)
        {
@@ -70,24 +63,24 @@ class SimpleBroadphase : public BroadphaseInterface
        void    validate();
 
 protected:
-       void    RemoveOverlappingPair(BroadphasePair& pair);
-       void    CleanOverlappingPair(BroadphasePair& pair);
 
-       void    RemoveOverlappingPairsContainingProxy(BroadphaseProxy* proxy);
 
-       void    AddOverlappingPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
-       BroadphasePair* FindPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
        virtual void    RefreshOverlappingPairs();
 public:
        SimpleBroadphase(int maxProxies=4096,int maxOverlap=8192);
        virtual ~SimpleBroadphase();
 
-       virtual BroadphaseProxy*        CreateProxy(  const SimdVector3& min,  const SimdVector3& max,int shapeType,void* userPtr);
+
+       virtual BroadphaseProxy*        CreateProxy(  const SimdVector3& min,  const SimdVector3& max,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask);
+
 
        virtual void    DestroyProxy(BroadphaseProxy* proxy);
        virtual void    SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const SimdVector3& aabbMax);
-       virtual void    CleanProxyFromPairs(BroadphaseProxy* proxy);
-       virtual void    DispatchAllCollisionPairs(Dispatcher&   dispatcher,DispatcherInfo& dispatchInfo);
+               
+       
+       
+
+
 
 };
 
diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionCreateFunc.h b/extern/bullet/Bullet/CollisionDispatch/CollisionCreateFunc.h
new file mode 100644 (file)
index 0000000..5df073b
--- /dev/null
@@ -0,0 +1,42 @@
+/*
+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 COLLISION_CREATE_FUNC
+#define COLLISION_CREATE_FUNC
+
+#include <vector>
+
+typedef std::vector<struct CollisionObject*> CollisionObjectArray;
+class CollisionAlgorithm;
+struct BroadphaseProxy;
+
+
+
+struct CollisionAlgorithmCreateFunc
+{
+       bool m_swapped;
+       
+       CollisionAlgorithmCreateFunc()
+               :m_swapped(false)
+       {
+       }
+       virtual ~CollisionAlgorithmCreateFunc(){};
+
+       virtual CollisionAlgorithm* CreateCollisionAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1)
+       {
+               return 0;
+       }
+};
+#endif //COLLISION_CREATE_FUNC
\ No newline at end of file
index 5749db20a0504af9bd9b856e7ee5e09a505b3c3b..06e999fbe0dea7d7c5fc6c5039248360a2510075 100644 (file)
@@ -21,40 +21,15 @@ subject to the following restrictions:
 #include "CollisionDispatch/ConvexConvexAlgorithm.h"
 #include "CollisionDispatch/EmptyCollisionAlgorithm.h"
 #include "CollisionDispatch/ConvexConcaveCollisionAlgorithm.h"
+#include "CollisionDispatch/CompoundCollisionAlgorithm.h"
 #include "CollisionShapes/CollisionShape.h"
 #include "CollisionDispatch/CollisionObject.h"
 #include <algorithm>
+#include "BroadphaseCollision/OverlappingPairCache.h"
 
 int gNumManifold = 0;
 
-void CollisionDispatcher::FindUnions()
-{
-       if (m_useIslands)
-       {
-               for (int i=0;i<GetNumManifolds();i++)
-               {
-                       const PersistentManifold* manifold = this->GetManifoldByIndexInternal(i);
-                       //static objects (invmass 0.f) don't merge !
-
-                        const  CollisionObject* colObj0 = static_cast<const CollisionObject*>(manifold->GetBody0());
-                        const  CollisionObject* colObj1 = static_cast<const CollisionObject*>(manifold->GetBody1());
-
-                        if (colObj0 && colObj1 && NeedsResponse(*colObj0,*colObj1))
-                        {
-                               if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
-                                       ((colObj1) && ((colObj1)->mergesSimulationIslands())))
-                               {
 
-                                       m_unionFind.unite((colObj0)->m_islandTag1,
-                                               (colObj1)->m_islandTag1);
-                               }
-                        }
-                       
-                       
-               }
-       }
-       
-}
        
 
        
@@ -79,7 +54,9 @@ CollisionDispatcher::CollisionDispatcher ():
 PersistentManifold*    CollisionDispatcher::GetNewManifold(void* b0,void* b1) 
 { 
        gNumManifold++;
-       //printf("GetNewManifoldResult: gNumManifold %d\n",gNumManifold);
+       
+       //ASSERT(gNumManifold < 65535);
+       
 
        CollisionObject* body0 = (CollisionObject*)b0;
        CollisionObject* body1 = (CollisionObject*)b1;
@@ -119,96 +96,6 @@ void CollisionDispatcher::ReleaseManifold(PersistentManifold* manifold)
 }
 
        
-//
-// todo: this is random access, it can be walked 'cache friendly'!
-//
-void CollisionDispatcher::BuildAndProcessIslands(CollisionObjectArray& collisionObjects, IslandCallback* callback)
-{
-       int numBodies  = collisionObjects.size();
-
-       for (int islandId=0;islandId<numBodies;islandId++)
-       {
-
-               std::vector<PersistentManifold*>  islandmanifold;
-               
-               //int numSleeping = 0;
-
-               bool allSleeping = true;
-
-               int i;
-               for (i=0;i<numBodies;i++)
-               {
-                       CollisionObject* colObj0 = collisionObjects[i];
-                       if (colObj0->m_islandTag1 == islandId)
-                       {
-                               if (colObj0->GetActivationState()== ACTIVE_TAG)
-                               {
-                                       allSleeping = false;
-                               }
-                               if (colObj0->GetActivationState()== DISABLE_DEACTIVATION)
-                               {
-                                       allSleeping = false;
-                               }
-                       }
-               }
-
-               
-               for (i=0;i<GetNumManifolds();i++)
-               {
-                        PersistentManifold* manifold = this->GetManifoldByIndexInternal(i);
-                        
-                        //filtering for response
-
-                        CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0());
-                        CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1());
-                        {
-                               if (((colObj0) && (colObj0)->m_islandTag1 == (islandId)) ||
-                                       ((colObj1) && (colObj1)->m_islandTag1 == (islandId)))
-                               {
-
-                                       if (NeedsResponse(*colObj0,*colObj1))
-                                               islandmanifold.push_back(manifold);
-                               }
-                        }
-               }
-               if (allSleeping)
-               {
-                       int i;
-                       for (i=0;i<numBodies;i++)
-                       {
-                               CollisionObject* colObj0 = collisionObjects[i];
-                               if (colObj0->m_islandTag1 == islandId)
-                               {
-                                       colObj0->SetActivationState( ISLAND_SLEEPING );
-                               }
-                       }
-
-                       
-               } else
-               {
-
-                       int i;
-                       for (i=0;i<numBodies;i++)
-                       {
-                               CollisionObject* colObj0 = collisionObjects[i];
-                               if (colObj0->m_islandTag1 == islandId)
-                               {
-                                       if ( colObj0->GetActivationState() == ISLAND_SLEEPING)
-                                       {
-                                               colObj0->SetActivationState( WANTS_DEACTIVATION);
-                                       }
-                               }
-                       }
-
-                       /// Process the actual simulation, only if not sleeping/deactivated
-                       if (islandmanifold.size())
-                       {
-                               callback->ProcessIsland(&islandmanifold[0],islandmanifold.size());
-                       }
-
-               }
-       }
-}
 
 
 
@@ -236,6 +123,17 @@ CollisionAlgorithm* CollisionDispatcher::InternalFindAlgorithm(BroadphaseProxy&
                return new ConvexConcaveCollisionAlgorithm(ci,&proxy1,&proxy0);
        }
 
+       if (body0->m_collisionShape->IsCompound())
+       {
+               return new CompoundCollisionAlgorithm(ci,&proxy0,&proxy1);
+       } else
+       {
+               if (body1->m_collisionShape->IsCompound())
+               {
+                       return new CompoundCollisionAlgorithm(ci,&proxy1,&proxy0);
+               }
+       }
+
        //failed to find an algorithm
        return new EmptyAlgorithm(ci);
        
@@ -291,3 +189,69 @@ void       CollisionDispatcher::ReleaseManifoldResult(ManifoldResult*)
 {
 
 }
+
+
+void   CollisionDispatcher::DispatchAllCollisionPairs(BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo)
+{
+       //m_blockedForChanges = true;
+
+       int i;
+
+       int dispatcherId = GetUniqueId();
+
+       
+
+       for (i=0;i<numPairs;i++)
+       {
+
+               BroadphasePair& pair = pairs[i];
+
+               if (dispatcherId>= 0)
+               {
+                       //dispatcher will keep algorithms persistent in the collision pair
+                       if (!pair.m_algorithms[dispatcherId])
+                       {
+                               pair.m_algorithms[dispatcherId] = FindAlgorithm(
+                                       *pair.m_pProxy0,
+                                       *pair.m_pProxy1);
+                       }
+
+                       if (pair.m_algorithms[dispatcherId])
+                       {
+                               if (dispatchInfo.m_dispatchFunc ==              DispatcherInfo::DISPATCH_DISCRETE)
+                               {
+                                       pair.m_algorithms[dispatcherId]->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
+                               } else
+                               {
+                                       float toi = pair.m_algorithms[dispatcherId]->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
+                                       if (dispatchInfo.m_timeOfImpact > toi)
+                                               dispatchInfo.m_timeOfImpact = toi;
+
+                               }
+                       }
+               } else
+               {
+                       //non-persistent algorithm dispatcher
+                       CollisionAlgorithm* algo = FindAlgorithm(
+                               *pair.m_pProxy0,
+                               *pair.m_pProxy1);
+
+                       if (algo)
+                       {
+                               if (dispatchInfo.m_dispatchFunc ==              DispatcherInfo::DISPATCH_DISCRETE)
+                               {
+                                       algo->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
+                               } else
+                               {
+                                       float toi = algo->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
+                                       if (dispatchInfo.m_timeOfImpact > toi)
+                                               dispatchInfo.m_timeOfImpact = toi;
+                               }
+                       }
+               }
+
+       }
+
+       //m_blockedForChanges = false;
+
+}
\ No newline at end of file
index d559f77bebe221e3177590d20fafdf97334f9881..773978b921807d5924de0e379b94bc5dfc330db6 100644 (file)
@@ -18,32 +18,19 @@ subject to the following restrictions:
 
 #include "BroadphaseCollision/Dispatcher.h"
 #include "NarrowPhaseCollision/PersistentManifold.h"
-#include "CollisionDispatch/UnionFind.h"
+
 #include "CollisionDispatch/ManifoldResult.h"
 
 #include "BroadphaseCollision/BroadphaseProxy.h"
-#include <vector>
+
 
 class IDebugDraw;
+class OverlappingPairCache;
 
-typedef std::vector<struct CollisionObject*> CollisionObjectArray;
 
+#include "CollisionCreateFunc.h"
 
-struct CollisionAlgorithmCreateFunc
-{
-       bool m_swapped;
-       
-       CollisionAlgorithmCreateFunc()
-               :m_swapped(false)
-       {
-       }
-       virtual ~CollisionAlgorithmCreateFunc(){};
 
-       virtual CollisionAlgorithm* CreateCollisionAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1)
-       {
-               return 0;
-       }
-};
 
 
 ///CollisionDispatcher supports algorithms that handle ConvexConvex and ConvexConcave collision pairs.
@@ -53,7 +40,7 @@ class CollisionDispatcher : public Dispatcher
        
        std::vector<PersistentManifold*>        m_manifoldsPtr;
 
-       UnionFind m_unionFind;
+       
 
        bool m_useIslands;
        
@@ -63,14 +50,9 @@ class CollisionDispatcher : public Dispatcher
        
 public:
        
-       UnionFind& GetUnionFind() { return m_unionFind;}
-
-       struct  IslandCallback
-       {
-               virtual ~IslandCallback() {};
+       
 
-               virtual void    ProcessIsland(PersistentManifold**      manifolds,int numManifolds) = 0;
-       };
+       
 
 
        int     GetNumManifolds() const
@@ -88,14 +70,6 @@ public:
                return m_manifoldsPtr[index];
        }
 
-       void InitUnionFind(int n)
-       {
-               if (m_useIslands)
-                       m_unionFind.reset(n);
-       }
-       
-       void FindUnions();
-       
        int m_count;
        
        CollisionDispatcher ();
@@ -106,8 +80,6 @@ public:
        virtual void ReleaseManifold(PersistentManifold* manifold);
 
        
-       virtual void BuildAndProcessIslands(CollisionObjectArray& collisionObjects, IslandCallback* callback);
-
        ///allows the user to get contact point callbacks 
        virtual ManifoldResult* GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold);
 
@@ -131,6 +103,8 @@ public:
 
        virtual int GetUniqueId() { return RIGIDBODY_DISPATCHER;}
        
+       virtual void    DispatchAllCollisionPairs(BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo);
+
        
 
 };
index 056dce59856f0a4d2816fa469bd804d585c10baf..a96707f5e86e56c95429a9c63173f74b419ade02 100644 (file)
@@ -21,8 +21,11 @@ CollisionObject::CollisionObject()
                m_deactivationTime(0.f),
                m_broadphaseHandle(0),
                m_collisionShape(0),
-               m_hitFraction(1.f)
+               m_hitFraction(1.f),
+               m_ccdSweptShereRadius(0.f),
+               m_ccdSquareMotionTreshold(0.f)
 {
+       m_cachedInvertedWorldTransform.setIdentity();
 
 }
 
index 0c2d0be460ce9055b7a20bd30412a6da095e1416..da4e9b934f5336ee041a296246f8c8a7e03717a6 100644 (file)
@@ -28,6 +28,9 @@ subject to the following restrictions:
 struct BroadphaseProxy;
 class  CollisionShape;
 
+/// CollisionObject can be used to manage collision detection objects. 
+/// CollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy.
+/// They can be added to the CollisionWorld.
 struct CollisionObject
 {
        SimdTransform   m_worldTransform;
@@ -35,12 +38,14 @@ struct      CollisionObject
        //m_interpolationWorldTransform is used for CCD and interpolation
        //it can be either previous or future (predicted) transform
        SimdTransform   m_interpolationWorldTransform;
-       
+
+       SimdTransform   m_cachedInvertedWorldTransform;
+
        enum CollisionFlags
        {
                isStatic = 1,
                noContactResponse = 2,
-
+               customMaterialCallback = 4,//this allows per-triangle material (friction/restitution)
        };
 
        int                             m_collisionFlags;
@@ -49,13 +54,22 @@ struct      CollisionObject
        int                             m_activationState1;
        float                   m_deactivationTime;
 
+       SimdScalar              m_friction;
+       SimdScalar              m_restitution;
+
        BroadphaseProxy*        m_broadphaseHandle;
        CollisionShape*         m_collisionShape;
 
        void*                   m_userPointer;//not use by Bullet internally
 
-       //time of impact calculation
+       ///time of impact calculation
        float                   m_hitFraction; 
+       
+       ///Swept sphere radius (0.0 by default), see ConvexConvexAlgorithm::
+       float                   m_ccdSweptShereRadius;
+
+       /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionTreshold
+       float                   m_ccdSquareMotionTreshold;
 
        bool                    mergesSimulationIslands() const;
 
@@ -91,6 +105,23 @@ struct      CollisionObject
                return ((GetActivationState() != ISLAND_SLEEPING) && (GetActivationState() != DISABLE_SIMULATION));
        }
 
+               void    setRestitution(float rest)
+       {
+               m_restitution = rest;
+       }
+       float   getRestitution() const
+       {
+               return m_restitution;
+       }
+       void    setFriction(float frict)
+       {
+               m_friction = frict;
+       }
+       float   getFriction() const
+       {
+               return m_friction;
+       }
+
 
 };
 
index e8f73b2cc0784a918044c255b9657c2b4a23ca8b..bc4413c3427f202fa9c16a7dd05807d34ac017b9 100644 (file)
@@ -20,6 +20,7 @@ subject to the following restrictions:
 #include "CollisionShapes/SphereShape.h" //for raycasting
 #include "CollisionShapes/TriangleMeshShape.h" //for raycasting
 #include "NarrowPhaseCollision/RaycastCallback.h"
+#include "CollisionShapes/CompoundShape.h"
 
 #include "NarrowPhaseCollision/SubSimplexConvexCast.h"
 #include "BroadphaseCollision/BroadphaseInterface.h"
@@ -52,67 +53,16 @@ CollisionWorld::~CollisionWorld()
 
 }
 
-void   CollisionWorld::UpdateActivationState()
-{
-       m_dispatcher->InitUnionFind(m_collisionObjects.size());
-       
-       // put the index into m_controllers into m_tag  
-       {
-               std::vector<CollisionObject*>::iterator i;
-               
-               int index = 0;
-               for (i=m_collisionObjects.begin();
-               !(i==m_collisionObjects.end()); i++)
-               {
-                       
-                       CollisionObject*        collisionObject= (*i);
-                       collisionObject->m_islandTag1 = index;
-                       collisionObject->m_hitFraction = 1.f;
-                       index++;
-                       
-               }
-       }
-       // do the union find
-       
-       m_dispatcher->FindUnions();
-       
 
-       
-}
 
 
 
-void   CollisionWorld::StoreIslandActivationState()
-{
-       // put the islandId ('find' value) into m_tag   
-       {
-               UnionFind& unionFind = m_dispatcher->GetUnionFind();
-               
-               std::vector<CollisionObject*>::iterator i;
-               
-               int index = 0;
-               for (i=m_collisionObjects.begin();
-               !(i==m_collisionObjects.end()); i++)
-               {
-                       CollisionObject* collisionObject= (*i);
-                       
-                       if (collisionObject->mergesSimulationIslands())
-                       {
-                               collisionObject->m_islandTag1 = unionFind.find(index);
-                       } else
-                       {
-                               collisionObject->m_islandTag1 = -1;
-                       }
-                       index++;
-               }
-       }
-}
 
 
 
 
 
-void   CollisionWorld::AddCollisionObject(CollisionObject* collisionObject)
+void   CollisionWorld::AddCollisionObject(CollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
 {
                m_collisionObjects.push_back(collisionObject);
 
@@ -128,7 +78,9 @@ void CollisionWorld::AddCollisionObject(CollisionObject* collisionObject)
                        minAabb,
                        maxAabb,
                        type,
-                       collisionObject
+                       collisionObject,
+                       collisionFilterGroup,
+                       collisionFilterMask
                        );
                
 
@@ -148,10 +100,13 @@ void      CollisionWorld::PerformDiscreteCollisionDetection()
        for (size_t i=0;i<m_collisionObjects.size();i++)
        {
                m_collisionObjects[i]->m_collisionShape->GetAabb(m_collisionObjects[i]->m_worldTransform,aabbMin,aabbMax);
-               m_broadphase->SetAabb(m_collisionObjects[i]->m_broadphaseHandle,aabbMin,aabbMax);
+               m_pairCache->SetAabb(m_collisionObjects[i]->m_broadphaseHandle,aabbMin,aabbMax);
        }
 
-       m_broadphase->DispatchAllCollisionPairs(*GetDispatcher(),dispatchInfo);
+       Dispatcher* dispatcher = GetDispatcher();
+       if (dispatcher)
+               dispatcher->DispatchAllCollisionPairs(&m_pairCache->GetOverlappingPair(0),m_pairCache->GetNumOverlappingPairs(),dispatchInfo);
+
 }
 
 
@@ -185,58 +140,27 @@ void      CollisionWorld::RemoveCollisionObject(CollisionObject* collisionObject)
                }
 }
 
-
-
-void   CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback)
+void   RayTestSingle(const SimdTransform& rayFromTrans,const SimdTransform& rayToTrans,
+                                         CollisionObject* collisionObject,
+                                         const CollisionShape* collisionShape,
+                                         const SimdTransform& colObjWorldTransform,
+                                         CollisionWorld::RayResultCallback& resultCallback)
 {
-
-       
-       SimdTransform   rayFromTrans,rayToTrans;
-       rayFromTrans.setIdentity();
-       rayFromTrans.setOrigin(rayFromWorld);
-       rayToTrans.setIdentity();
        
-       rayToTrans.setOrigin(rayToWorld);
-
-       //do culling based on aabb (rayFrom/rayTo)
-       SimdVector3 rayAabbMin = rayFromWorld;
-       SimdVector3 rayAabbMax = rayFromWorld;
-       rayAabbMin.setMin(rayToWorld);
-       rayAabbMax.setMax(rayToWorld);
-
        SphereShape pointShape(0.0f);
 
-       /// brute force go over all objects. Once there is a broadphase, use that, or
-       /// add a raycast against aabb first.
-       
-       std::vector<CollisionObject*>::iterator iter;
-       
-       for (iter=m_collisionObjects.begin();
-       !(iter==m_collisionObjects.end()); iter++)
-       {
-               
-               CollisionObject*        collisionObject= (*iter);
-
-               //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
-               SimdVector3 collisionObjectAabbMin,collisionObjectAabbMax;
-               collisionObject->m_collisionShape->GetAabb(collisionObject->m_worldTransform,collisionObjectAabbMin,collisionObjectAabbMax);
-
-               //check aabb overlap
-
-               if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax))
-               {
-                       if (collisionObject->m_collisionShape->IsConvex())
+       if (collisionShape->IsConvex())
                        {
                                ConvexCast::CastResult castResult;
                                castResult.m_fraction = 1.f;//??
 
-                               ConvexShape* convexShape = (ConvexShape*) collisionObject->m_collisionShape;
+                               ConvexShape* convexShape = (ConvexShape*) collisionShape;
                                VoronoiSimplexSolver    simplexSolver;
                                SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
                                //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
                                //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
                                
-                               if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,collisionObject->m_worldTransform,collisionObject->m_worldTransform,castResult))
+                               if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
                                {
                                        //add hit
                                        if (castResult.m_normal.length2() > 0.0001f)
@@ -263,12 +187,12 @@ void      CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3&
                        else
                        {
                                
-                               if (collisionObject->m_collisionShape->IsConcave())
+                               if (collisionShape->IsConcave())
                                        {
 
-                                               TriangleMeshShape* triangleMesh = (TriangleMeshShape*)collisionObject->m_collisionShape;
+                                               TriangleMeshShape* triangleMesh = (TriangleMeshShape*)collisionShape;
                                                
-                                               SimdTransform worldTocollisionObject = collisionObject->m_worldTransform.inverse();
+                                               SimdTransform worldTocollisionObject = colObjWorldTransform.inverse();
 
                                                SimdVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
                                                SimdVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
@@ -277,12 +201,12 @@ void      CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3&
 
                                                struct BridgeTriangleRaycastCallback : public TriangleRaycastCallback 
                                                {
-                                                       RayResultCallback* m_resultCallback;
+                                                       CollisionWorld::RayResultCallback* m_resultCallback;
                                                        CollisionObject*        m_collisionObject;
                                                        TriangleMeshShape*      m_triangleMesh;
 
                                                        BridgeTriangleRaycastCallback( const SimdVector3& from,const SimdVector3& to,
-                                                               RayResultCallback* resultCallback, CollisionObject* collisionObject,TriangleMeshShape*  triangleMesh):
+                                                               CollisionWorld::RayResultCallback* resultCallback, CollisionObject* collisionObject,TriangleMeshShape*  triangleMesh):
                                                                TriangleRaycastCallback(from,to),
                                                                        m_resultCallback(resultCallback),
                                                                        m_collisionObject(collisionObject),
@@ -321,10 +245,75 @@ void      CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3&
 
                                                triangleMesh->ProcessAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
                                                                                        
-                                       }
-                                       
+                                       } else
+                                       {
+                                               //todo: use AABB tree or other BVH acceleration structure!
+                                               if (collisionShape->IsCompound())
+                                               {
+                                                       const CompoundShape* compoundShape = static_cast<const CompoundShape*>(collisionShape);
+                                                       int i=0;
+                                                       for (i=0;i<compoundShape->GetNumChildShapes();i++)
+                                                       {
+                                                               SimdTransform childTrans = compoundShape->GetChildTransform(i);
+                                                               const CollisionShape* childCollisionShape = compoundShape->GetChildShape(i);
+                                                               SimdTransform childWorldTrans = colObjWorldTransform * childTrans;
+                                                               RayTestSingle(rayFromTrans,rayToTrans,
+                                                                       collisionObject,
+                                                                       childCollisionShape,
+                                                                       childWorldTrans,
+                                                                       resultCallback);
 
+                                                       }
+
+
+                                               }
+                                       }
                        }
+}
+
+void   CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback)
+{
+
+       
+       SimdTransform   rayFromTrans,rayToTrans;
+       rayFromTrans.setIdentity();
+       rayFromTrans.setOrigin(rayFromWorld);
+       rayToTrans.setIdentity();
+       
+       rayToTrans.setOrigin(rayToWorld);
+
+       //do culling based on aabb (rayFrom/rayTo)
+       SimdVector3 rayAabbMin = rayFromWorld;
+       SimdVector3 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.
+       
+       std::vector<CollisionObject*>::iterator iter;
+       
+       for (iter=m_collisionObjects.begin();
+       !(iter==m_collisionObjects.end()); iter++)
+       {
+               
+               CollisionObject*        collisionObject= (*iter);
+
+               //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
+               SimdVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+               collisionObject->m_collisionShape->GetAabb(collisionObject->m_worldTransform,collisionObjectAabbMin,collisionObjectAabbMax);
+
+               //check aabb overlap
+
+               if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax))
+               {
+                       RayTestSingle(rayFromTrans,rayToTrans,
+                               collisionObject,
+                                        collisionObject->m_collisionShape,
+                                         collisionObject->m_worldTransform,
+                                         resultCallback);
+                       
                }
        }
 
index 1589da6da6933e4819cb738d0651d26b78c25be3..8414ba4e4590a895772a8ecfc2b3fce23fbf4202 100644 (file)
@@ -70,6 +70,7 @@ class BroadphaseInterface;
 #include "SimdTransform.h"
 #include "CollisionObject.h"
 #include "CollisionDispatcher.h" //for definition of CollisionObjectArray
+#include "BroadphaseCollision/OverlappingPairCache.h"
 
 #include <vector>
 
@@ -83,31 +84,36 @@ class CollisionWorld
 
        std::vector<CollisionObject*>   m_collisionObjects;
        
-       CollisionDispatcher*    m_dispatcher;
+       Dispatcher*     m_dispatcher1;
 
-       BroadphaseInterface*    m_broadphase;
+       OverlappingPairCache*   m_pairCache;
+       
 
 public:
 
-       CollisionWorld(CollisionDispatcher* dispatcher,BroadphaseInterface* broadphase)
-               :m_dispatcher(dispatcher),
-               m_broadphase(broadphase)
+       CollisionWorld(Dispatcher* dispatcher,OverlappingPairCache* pairCache)
+               :m_dispatcher1(dispatcher),
+               m_pairCache(pairCache)
        {
 
        }
        virtual ~CollisionWorld();
 
-       virtual void    UpdateActivationState();
-       virtual void    StoreIslandActivationState();
 
        BroadphaseInterface*    GetBroadphase()
        {
-               return m_broadphase;
+               return m_pairCache;
        }
 
-       CollisionDispatcher*    GetDispatcher()
+       OverlappingPairCache*   GetPairCache()
+       {
+               return m_pairCache;
+       }
+
+
+       Dispatcher*     GetDispatcher()
        {
-               return m_dispatcher;
+               return m_dispatcher1;
        }
 
        ///LocalShapeInfo gives extra information for complex shapes
@@ -201,7 +207,7 @@ public:
        void    RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback);
 
 
-       void    AddCollisionObject(CollisionObject* collisionObject);
+       void    AddCollisionObject(CollisionObject* collisionObject,short int collisionFilterGroup=1,short int collisionFilterMask=1);
 
        CollisionObjectArray& GetCollisionObjectArray()
        {
diff --git a/extern/bullet/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.cpp b/extern/bullet/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.cpp
new file mode 100644 (file)
index 0000000..0a79d2b
--- /dev/null
@@ -0,0 +1,144 @@
+/*
+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 "CollisionDispatch/CompoundCollisionAlgorithm.h"
+#include "CollisionDispatch/CollisionObject.h"
+#include "CollisionShapes/CompoundShape.h"
+
+
+CompoundCollisionAlgorithm::CompoundCollisionAlgorithm( const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
+:m_dispatcher(ci.m_dispatcher),
+m_compoundProxy(*proxy0),
+m_otherProxy(*proxy1)
+{
+       CollisionObject* colObj = static_cast<CollisionObject*>(m_compoundProxy.m_clientObject);
+       assert (colObj->m_collisionShape->IsCompound());
+       
+       CompoundShape* compoundShape = static_cast<CompoundShape*>(colObj->m_collisionShape);
+       int numChildren = compoundShape->GetNumChildShapes();
+       m_childProxies.resize( numChildren );
+       int i;
+       for (i=0;i<numChildren;i++)
+       {
+               m_childProxies[i] = BroadphaseProxy(*proxy0);
+       }
+
+       m_childCollisionAlgorithms.resize(numChildren);
+       for (i=0;i<numChildren;i++)
+       {
+               CollisionShape* childShape = compoundShape->GetChildShape(i);
+               CollisionObject* colObj = static_cast<CollisionObject*>(m_childProxies[i].m_clientObject);
+               CollisionShape* orgShape = colObj->m_collisionShape;
+               colObj->m_collisionShape = childShape;
+               m_childCollisionAlgorithms[i] = m_dispatcher->FindAlgorithm(m_childProxies[i],m_otherProxy);
+               colObj->m_collisionShape =orgShape;
+       }
+}
+
+
+CompoundCollisionAlgorithm::~CompoundCollisionAlgorithm()
+{
+       int numChildren = m_childCollisionAlgorithms.size();
+       int i;
+       for (i=0;i<numChildren;i++)
+       {
+               delete m_childCollisionAlgorithms[i];
+       }
+}
+
+void CompoundCollisionAlgorithm::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy* ,const DispatcherInfo& dispatchInfo)
+{
+       CollisionObject* colObj = static_cast<CollisionObject*>(m_compoundProxy.m_clientObject);
+       assert (colObj->m_collisionShape->IsCompound());
+       
+       CompoundShape* compoundShape = static_cast<CompoundShape*>(colObj->m_collisionShape);
+
+       //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
+       //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
+       //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
+       //then use each overlapping node AABB against Tree0
+       //and vise versa.
+
+
+       int numChildren = m_childCollisionAlgorithms.size();
+       int i;
+       for (i=0;i<numChildren;i++)
+       {
+               //temporarily exchange parent CollisionShape with childShape, and recurse
+               CollisionShape* childShape = compoundShape->GetChildShape(i);
+               CollisionObject* colObj = static_cast<CollisionObject*>(m_childProxies[i].m_clientObject);
+
+               //backup
+               SimdTransform   orgTrans = colObj->m_worldTransform;
+               CollisionShape* orgShape = colObj->m_collisionShape;
+
+               SimdTransform childTrans = compoundShape->GetChildTransform(i);
+               SimdTransform   newChildWorldTrans = orgTrans*childTrans ;
+               colObj->m_worldTransform = newChildWorldTrans;
+
+               colObj->m_collisionShape = childShape;
+               m_childCollisionAlgorithms[i]->ProcessCollision(&m_childProxies[i],&m_otherProxy,dispatchInfo);
+               //revert back
+               colObj->m_collisionShape =orgShape;
+               colObj->m_worldTransform = orgTrans;
+       }
+}
+
+float  CompoundCollisionAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo)
+{
+       CollisionObject* colObj = static_cast<CollisionObject*>(m_compoundProxy.m_clientObject);
+       assert (colObj->m_collisionShape->IsCompound());
+       
+       CompoundShape* compoundShape = static_cast<CompoundShape*>(colObj->m_collisionShape);
+
+       //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
+       //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
+       //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
+       //then use each overlapping node AABB against Tree0
+       //and vise versa.
+
+       float hitFraction = 1.f;
+
+       int numChildren = m_childCollisionAlgorithms.size();
+       int i;
+       for (i=0;i<numChildren;i++)
+       {
+               //temporarily exchange parent CollisionShape with childShape, and recurse
+               CollisionShape* childShape = compoundShape->GetChildShape(i);
+               CollisionObject* colObj = static_cast<CollisionObject*>(m_childProxies[i].m_clientObject);
+
+               //backup
+               SimdTransform   orgTrans = colObj->m_worldTransform;
+               CollisionShape* orgShape = colObj->m_collisionShape;
+
+               SimdTransform childTrans = compoundShape->GetChildTransform(i);
+               SimdTransform   newChildWorldTrans = orgTrans*childTrans ;
+               colObj->m_worldTransform = newChildWorldTrans;
+
+               colObj->m_collisionShape = childShape;
+               float frac = m_childCollisionAlgorithms[i]->CalculateTimeOfImpact(&m_childProxies[i],&m_otherProxy,dispatchInfo);
+               if (frac<hitFraction)
+               {
+                       hitFraction = frac;
+               }
+               //revert back
+               colObj->m_collisionShape =orgShape;
+               colObj->m_worldTransform = orgTrans;
+       }
+       return hitFraction;
+
+}
\ No newline at end of file
diff --git a/extern/bullet/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.h b/extern/bullet/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.h
new file mode 100644 (file)
index 0000000..a41d3a8
--- /dev/null
@@ -0,0 +1,56 @@
+/*
+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 COMPOUND_COLLISION_ALGORITHM_H
+#define COMPOUND_COLLISION_ALGORITHM_H
+
+#include "BroadphaseCollision/CollisionAlgorithm.h"
+#include "BroadphaseCollision/Dispatcher.h"
+#include "BroadphaseCollision/BroadphaseInterface.h"
+
+#include "NarrowPhaseCollision/PersistentManifold.h"
+class Dispatcher;
+#include "BroadphaseCollision/BroadphaseProxy.h"
+#include <vector>
+
+
+/// CompoundCollisionAlgorithm  supports collision between CompoundCollisionShapes and other collision shapes
+/// Place holder, not fully implemented yet
+class CompoundCollisionAlgorithm  : public CollisionAlgorithm
+{
+       BroadphaseProxy m_compoundProxy;
+       BroadphaseProxy m_otherProxy;
+       std::vector<BroadphaseProxy> m_childProxies;
+       std::vector<CollisionAlgorithm*> m_childCollisionAlgorithms;
+
+       Dispatcher*             m_dispatcher;
+       BroadphaseProxy m_compound;
+
+       BroadphaseProxy m_other;
+
+       
+public:
+
+       CompoundCollisionAlgorithm( const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
+
+       virtual ~CompoundCollisionAlgorithm();
+
+       virtual void ProcessCollision (BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo);
+
+       float   CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo);
+
+};
+
+#endif //COMPOUND_COLLISION_ALGORITHM_H
index a72432ef9525a886ea25018565e55eb71bc350ed..63ed29888ab317b982f27421d68f1ab759b2901e 100644 (file)
@@ -17,18 +17,19 @@ subject to the following restrictions:
 #include "ConvexConcaveCollisionAlgorithm.h"
 #include "CollisionDispatch/CollisionObject.h"
 #include "CollisionShapes/MultiSphereShape.h"
-#include "CollisionShapes/BoxShape.h"
 #include "ConvexConvexAlgorithm.h"
 #include "BroadphaseCollision/BroadphaseProxy.h"
-#include "CollisionShapes/TriangleShape.h"
+#include "CollisionShapes/ConcaveShape.h"
 #include "CollisionDispatch/ManifoldResult.h"
 #include "NarrowPhaseCollision/RaycastCallback.h"
-#include "CollisionShapes/TriangleMeshShape.h"
-
+#include "CollisionShapes/TriangleShape.h"
+#include "CollisionShapes/SphereShape.h"
+#include "IDebugDraw.h"
+#include "NarrowPhaseCollision/SubSimplexConvexCast.h"
 
 ConvexConcaveCollisionAlgorithm::ConvexConcaveCollisionAlgorithm( const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
 : CollisionAlgorithm(ci),m_convex(*proxy0),m_concave(*proxy1),
-m_boxTriangleCallback(ci.m_dispatcher,proxy0,proxy1)
+m_ConvexTriangleCallback(ci.m_dispatcher,proxy0,proxy1)
 
 {
 }
@@ -39,8 +40,8 @@ ConvexConcaveCollisionAlgorithm::~ConvexConcaveCollisionAlgorithm()
 
 
 
-BoxTriangleCallback::BoxTriangleCallback(Dispatcher*  dispatcher,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1):
-  m_boxProxy(proxy0),m_triangleProxy(*proxy1),m_dispatcher(dispatcher),
+ConvexTriangleCallback::ConvexTriangleCallback(Dispatcher*  dispatcher,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1):
+  m_convexProxy(proxy0),m_triangleProxy(*proxy1),m_dispatcher(dispatcher),
        m_dispatchInfoPtr(0)
 {
 
@@ -52,7 +53,7 @@ BoxTriangleCallback::BoxTriangleCallback(Dispatcher*  dispatcher,BroadphaseProxy
          ClearCache();
 }
 
-BoxTriangleCallback::~BoxTriangleCallback()
+ConvexTriangleCallback::~ConvexTriangleCallback()
 {
        ClearCache();
        m_dispatcher->ReleaseManifold( m_manifoldPtr );
@@ -60,14 +61,14 @@ BoxTriangleCallback::~BoxTriangleCallback()
 }
   
 
-void   BoxTriangleCallback::ClearCache()
+void   ConvexTriangleCallback::ClearCache()
 {
        m_dispatcher->ClearManifold(m_manifoldPtr);
 };
 
 
 
-void BoxTriangleCallback::ProcessTriangle(SimdVector3* triangle,int partId, int triangleIndex)
+void ConvexTriangleCallback::ProcessTriangle(SimdVector3* triangle,int partId, int triangleIndex)
 {
  
        //just for debugging purposes
@@ -79,22 +80,42 @@ void BoxTriangleCallback::ProcessTriangle(SimdVector3* triangle,int partId, int
        CollisionAlgorithmConstructionInfo ci;
        ci.m_dispatcher = m_dispatcher;
 
+       CollisionObject* ob = static_cast<CollisionObject*>(m_triangleProxy.m_clientObject);
+
+
        
+       ///debug drawing of the overlapping triangles
+       if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->GetDebugMode() > 0)
+       {
+               SimdVector3 color(255,255,0);
+               SimdTransform& tr = ob->m_worldTransform;
+               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);
+
+               //SimdVector3 center = triangle[0] + triangle[1]+triangle[2];
+               //center *= 0.333333f;
+               //m_dispatchInfoPtr->m_debugDraw->DrawLine(tr(triangle[0]),tr(center),color);
+               //m_dispatchInfoPtr->m_debugDraw->DrawLine(tr(triangle[1]),tr(center),color);
+               //m_dispatchInfoPtr->m_debugDraw->DrawLine(tr(triangle[2]),tr(center),color);
+
+       }
+
 
-       CollisionObject* colObj = static_cast<CollisionObject*>(m_boxProxy->m_clientObject);
+       CollisionObject* colObj = static_cast<CollisionObject*>(m_convexProxy->m_clientObject);
        
        if (colObj->m_collisionShape->IsConvex())
        {
                TriangleShape tm(triangle[0],triangle[1],triangle[2]);  
                tm.SetMargin(m_collisionMarginTriangle);
        
-               CollisionObject* ob = static_cast<CollisionObject*>(m_triangleProxy.m_clientObject);
-
+               
                CollisionShape* tmpShape = ob->m_collisionShape;
                ob->m_collisionShape = &tm;
                
-               ConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_boxProxy,&m_triangleProxy);
-               cvxcvxalgo.ProcessCollision(m_boxProxy,&m_triangleProxy,*m_dispatchInfoPtr);
+               ConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexProxy,&m_triangleProxy);
+               cvxcvxalgo.SetShapeIdentifiers(-1,-1,partId,triangleIndex);
+               cvxcvxalgo.ProcessCollision(m_convexProxy,&m_triangleProxy,*m_dispatchInfoPtr);
                ob->m_collisionShape = tmpShape;
 
        }
@@ -105,22 +126,22 @@ void BoxTriangleCallback::ProcessTriangle(SimdVector3* triangle,int partId, int
 
 
 
-void   BoxTriangleCallback::SetTimeStepAndCounters(float collisionMarginTriangle,const DispatcherInfo& dispatchInfo)
+void   ConvexTriangleCallback::SetTimeStepAndCounters(float collisionMarginTriangle,const DispatcherInfo& dispatchInfo)
 {
        m_dispatchInfoPtr = &dispatchInfo;
        m_collisionMarginTriangle = collisionMarginTriangle;
 
        //recalc aabbs
-       CollisionObject* boxBody = (CollisionObject* )m_boxProxy->m_clientObject;
+       CollisionObject* convexBody = (CollisionObject* )m_convexProxy->m_clientObject;
        CollisionObject* triBody = (CollisionObject* )m_triangleProxy.m_clientObject;
 
-       SimdTransform boxInTriangleSpace;
-       boxInTriangleSpace = triBody->m_worldTransform.inverse() * boxBody->m_worldTransform;
+       SimdTransform convexInTriangleSpace;
+       convexInTriangleSpace = triBody->m_worldTransform.inverse() * convexBody->m_worldTransform;
 
-       CollisionShape* boxshape = static_cast<CollisionShape*>(boxBody->m_collisionShape);
+       CollisionShape* convexShape = static_cast<CollisionShape*>(convexBody->m_collisionShape);
        //CollisionShape* triangleShape = static_cast<CollisionShape*>(triBody->m_collisionShape);
 
-       boxshape->GetAabb(boxInTriangleSpace,m_aabbMin,m_aabbMax);
+       convexShape->GetAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
 
        float extraMargin = collisionMarginTriangle;//CONVEX_DISTANCE_MARGIN;//+0.1f;
 
@@ -133,17 +154,17 @@ void      BoxTriangleCallback::SetTimeStepAndCounters(float collisionMarginTriangle,c
 
 void ConvexConcaveCollisionAlgorithm::ClearCache()
 {
-       m_boxTriangleCallback.ClearCache();
+       m_ConvexTriangleCallback.ClearCache();
 
 }
 
 void ConvexConcaveCollisionAlgorithm::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy* ,const DispatcherInfo& dispatchInfo)
 {
-
-       CollisionObject* boxBody = static_cast<CollisionObject* >(m_convex.m_clientObject);
+       
+       CollisionObject* convexBody = static_cast<CollisionObject* >(m_convex.m_clientObject);
        CollisionObject* triBody = static_cast<CollisionObject* >(m_concave.m_clientObject);
 
-       if (triBody->m_collisionShape->GetShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
+       if (triBody->m_collisionShape->IsConcave())
        {
 
                if (!m_dispatcher->NeedsCollision(m_convex,m_concave))
@@ -152,19 +173,21 @@ void ConvexConcaveCollisionAlgorithm::ProcessCollision (BroadphaseProxy* ,Broadp
                
 
                CollisionObject*        triOb = static_cast<CollisionObject*>(m_concave.m_clientObject);
-               TriangleMeshShape* triangleMesh = static_cast<TriangleMeshShape*>( triOb->m_collisionShape);
+               ConcaveShape* concaveShape = static_cast<ConcaveShape*>( triOb->m_collisionShape);
                
-               if (boxBody->m_collisionShape->IsConvex())
+               if (convexBody->m_collisionShape->IsConvex())
                {
-                       float collisionMarginTriangle = triangleMesh->GetMargin();
+                       float collisionMarginTriangle = concaveShape->GetMargin();
                                        
-                       m_boxTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle,dispatchInfo);
-#ifdef USE_BOX_TRIANGLE
-                       m_dispatcher->ClearManifold(m_boxTriangleCallback.m_manifoldPtr);
-#endif
-                       m_boxTriangleCallback.m_manifoldPtr->SetBodies(m_convex.m_clientObject,m_concave.m_clientObject);
+                       m_ConvexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle,dispatchInfo);
+
+                       //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
+                       //m_dispatcher->ClearManifold(m_ConvexTriangleCallback.m_manifoldPtr);
 
-                       triangleMesh->ProcessAllTriangles( &m_boxTriangleCallback,m_boxTriangleCallback.GetAabbMin(),m_boxTriangleCallback.GetAabbMax());
+
+                       m_ConvexTriangleCallback.m_manifoldPtr->SetBodies(m_convex.m_clientObject,m_concave.m_clientObject);
+
+                       concaveShape->ProcessAllTriangles( &m_ConvexTriangleCallback,m_ConvexTriangleCallback.GetAabbMin(),m_ConvexTriangleCallback.GetAabbMax());
                        
        
                }
@@ -181,52 +204,101 @@ float ConvexConcaveCollisionAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* ,B
        CollisionObject* convexbody = (CollisionObject* )m_convex.m_clientObject;
        CollisionObject* triBody = static_cast<CollisionObject* >(m_concave.m_clientObject);
 
-       const SimdVector3& from = convexbody->m_worldTransform.getOrigin();
-       
-       SimdVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
+       //only perform CCD above a certain treshold, 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_ccdSquareMotionTreshold)
+       {
+               return 1.f;
+       }
+
+       //const SimdVector3& from = convexbody->m_worldTransform.getOrigin();
+       //SimdVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
        //todo: only do if the motion exceeds the 'radius'
 
-       struct LocalTriangleRaycastCallback     : public TriangleRaycastCallback
+       SimdTransform convexFromLocal = triBody->m_cachedInvertedWorldTransform * convexbody->m_worldTransform;
+       SimdTransform convexToLocal = triBody->m_cachedInvertedWorldTransform * convexbody->m_interpolationWorldTransform;
+
+       struct LocalTriangleSphereCastCallback  : public TriangleCallback
        {
-               LocalTriangleRaycastCallback(const SimdVector3& from,const SimdVector3& to)
-                       :TriangleRaycastCallback(from,to)
-               {
+               SimdTransform m_ccdSphereFromTrans;
+               SimdTransform m_ccdSphereToTrans;
+               SimdTransform   m_meshTransform;
+
+               float   m_ccdSphereRadius;
+               float   m_hitFraction;
+       
+
+               LocalTriangleSphereCastCallback(const SimdTransform& from,const SimdTransform& to,float ccdSphereRadius,float hitFraction)
+                       :m_ccdSphereFromTrans(from),
+                       m_ccdSphereToTrans(to),
+                       m_ccdSphereRadius(ccdSphereRadius),
+                       m_hitFraction(hitFraction)
+               {                       
                }
                
-               virtual float ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex )
+               
+               virtual void ProcessTriangle(SimdVector3* triangle, int partId, int triangleIndex)
                {
-                       //todo: handle ccd here
-                       return 0.f;
+                       //do a swept sphere for now
+                       SimdTransform ident;
+                       ident.setIdentity();
+                       ConvexCast::CastResult castResult;
+                       castResult.m_fraction = m_hitFraction;
+                       SphereShape     pointShape(m_ccdSphereRadius);
+                       TriangleShape   triShape(triangle[0],triangle[1],triangle[2]);
+                       VoronoiSimplexSolver    simplexSolver;
+                       SubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
+                       //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
+                       //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
+                       //local space?
+
+                       if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
+                               ident,ident,castResult))
+                       {
+                               if (m_hitFraction > castResult.m_fraction)
+                                       m_hitFraction = castResult.m_fraction;
+                       }
 
                }
+
        };
 
 
-       LocalTriangleRaycastCallback raycastCallback(from,to);
+       
 
-       raycastCallback.m_hitFraction = convexbody->m_hitFraction;
+       
+       if (triBody->m_collisionShape->IsConcave())
+       {
+               SimdVector3 rayAabbMin = convexFromLocal.getOrigin();
+               rayAabbMin.setMin(convexToLocal.getOrigin());
+               SimdVector3 rayAabbMax = convexFromLocal.getOrigin();
+               rayAabbMax.setMax(convexToLocal.getOrigin());
+               rayAabbMin -= SimdVector3(convexbody->m_ccdSweptShereRadius,convexbody->m_ccdSweptShereRadius,convexbody->m_ccdSweptShereRadius);
+               rayAabbMax += SimdVector3(convexbody->m_ccdSweptShereRadius,convexbody->m_ccdSweptShereRadius,convexbody->m_ccdSweptShereRadius);
 
-       SimdVector3 aabbMin (-1e30f,-1e30f,-1e30f);
-       SimdVector3 aabbMax (SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
+               float curHitFraction = 1.f; //is this available?
+               LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
+               convexbody->m_ccdSweptShereRadius,curHitFraction);
 
-       if (triBody->m_collisionShape->GetShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
-       {
+               raycastCallback.m_hitFraction = convexbody->m_hitFraction;
 
                CollisionObject* concavebody = (CollisionObject* )m_concave.m_clientObject;
 
-               TriangleMeshShape* triangleMesh = (TriangleMeshShape*) concavebody->m_collisionShape;
+               ConcaveShape* triangleMesh = (ConcaveShape*) concavebody->m_collisionShape;
                
                if (triangleMesh)
                {
-                       triangleMesh->ProcessAllTriangles(&raycastCallback,aabbMin,aabbMax);
+                       triangleMesh->ProcessAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
                }
-       }
+       
 
 
-       if (raycastCallback.m_hitFraction < convexbody->m_hitFraction)
-       {
-               convexbody->m_hitFraction = raycastCallback.m_hitFraction;
-               return raycastCallback.m_hitFraction;
+               if (raycastCallback.m_hitFraction < convexbody->m_hitFraction)
+               {
+                       convexbody->m_hitFraction = raycastCallback.m_hitFraction;
+                       return raycastCallback.m_hitFraction;
+               }
        }
 
        return 1.f;
index 18652334b4ed4db89a085bbc7a896c63f66483e9..47b4de3bbe14236c47fecf5b433bc761c24d6336 100644 (file)
@@ -25,10 +25,10 @@ class Dispatcher;
 #include "BroadphaseCollision/BroadphaseProxy.h"
 
 
-
-class BoxTriangleCallback : public TriangleCallback
+///For each triangle in the concave mesh that overlaps with the AABB of a convex (m_convexProxy), ProcessTriangle is called.
+class ConvexTriangleCallback : public TriangleCallback
 {
-       BroadphaseProxy* m_boxProxy;
+       BroadphaseProxy* m_convexProxy;
        BroadphaseProxy m_triangleProxy;
 
        SimdVector3     m_aabbMin;
@@ -43,11 +43,11 @@ int m_triangleCount;
        
        PersistentManifold*     m_manifoldPtr;
 
-       BoxTriangleCallback(Dispatcher* dispatcher,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
+       ConvexTriangleCallback(Dispatcher* dispatcher,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
 
        void    SetTimeStepAndCounters(float collisionMarginTriangle,const DispatcherInfo& dispatchInfo);
 
-       virtual ~BoxTriangleCallback();
+       virtual ~ConvexTriangleCallback();
 
        virtual void ProcessTriangle(SimdVector3* triangle, int partId, int triangleIndex);
        
@@ -75,7 +75,7 @@ class ConvexConcaveCollisionAlgorithm  : public CollisionAlgorithm
 
        BroadphaseProxy m_concave;
 
-       BoxTriangleCallback m_boxTriangleCallback;
+       ConvexTriangleCallback m_ConvexTriangleCallback;
 
 
 public:
index 9c20f28d1ec9c492dc82b4e522d609e2449db9bb..765ae8625f6f910cafa6e7d8850084e0d33e6716 100644 (file)
@@ -233,8 +233,9 @@ public:
 };
 #endif //USE_HULL
 
+
 //
-// box-box collision algorithm, for simplicity also applies resolution-impulse
+// Convex-Convex collision algorithm
 //
 void ConvexConvexAlgorithm ::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy* ,const DispatcherInfo& dispatchInfo)
 {
@@ -314,7 +315,7 @@ void ConvexConvexAlgorithm ::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy
        input.m_maximumDistanceSquared = min0->GetMargin() + min1->GetMargin() + m_manifoldPtr->GetContactBreakingTreshold();
        input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
        
-       //input.m_maximumDistanceSquared = 1e30f;//
+//     input.m_maximumDistanceSquared = 1e30f;
        
        input.m_transformA = col0->m_worldTransform;
        input.m_transformB = col1->m_worldTransform;
@@ -323,15 +324,37 @@ void ConvexConvexAlgorithm ::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy
 
        m_dispatcher->ReleaseManifoldResult(resultOut);
 }
+
+
+
 bool disableCcd = false;
 float  ConvexConvexAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo)
 {
+       ///Rather then checking ALL pairs, only calculate TOI when motion exceeds treshold
+    
+       ///Linear motion for one of objects needs to exceed m_ccdSquareMotionTreshold
+       ///col0->m_worldTransform,
+       float resultFraction = 1.f;
+
+       CollisionObject* col1 = static_cast<CollisionObject*>(m_box1.m_clientObject);
+       CollisionObject* col0 = static_cast<CollisionObject*>(m_box0.m_clientObject);
+
+       float squareMot0 = (col0->m_interpolationWorldTransform.getOrigin() - col0->m_worldTransform.getOrigin()).length2();
+       float squareMot1 = (col1->m_interpolationWorldTransform.getOrigin() - col1->m_worldTransform.getOrigin()).length2();
+    
+       if (squareMot0 < col0->m_ccdSquareMotionTreshold &&
+               squareMot0 < col0->m_ccdSquareMotionTreshold)
+               return resultFraction;
+
 
 
+       if (disableCcd)
+               return 1.f;
+
        CheckPenetrationDepthSolver();
 
        //An adhoc way of testing the Continuous Collision Detection algorithms
-       //One object is approximated as a point, to simplify things
+       //One object is approximated as a sphere, to simplify things
        //Starting in penetration should report no time of impact
        //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
        //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
@@ -340,48 +363,70 @@ float     ConvexConvexAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,Broad
 
        if (!needsCollision)
                return 1.f;
-
        
-       CollisionObject* col0 = static_cast<CollisionObject*>(m_box0.m_clientObject);
-       CollisionObject* col1 = static_cast<CollisionObject*>(m_box1.m_clientObject);
-       
-       SphereShape     sphere(0.f);
+               
+       /// Convex0 against sphere for Convex1
+       {
+               ConvexShape* convex0 = static_cast<ConvexShape*>(col0->m_collisionShape);
+
+               SphereShape     sphere1(col1->m_ccdSweptShereRadius); //todo: allow non-zero sphere sizes, for better approximation
+               ConvexCast::CastResult result;
+               VoronoiSimplexSolver voronoiSimplex;
+               //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+               ///Simplification, one object is simplified as a sphere
+               GjkConvexCast 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))
+               {
+               
+                       //store result.m_fraction in both bodies
+               
+                       if (col0->m_hitFraction > result.m_fraction)
+                               col0->m_hitFraction  = result.m_fraction;
 
-       ConvexShape* min0 = static_cast<ConvexShape*>(col0->m_collisionShape);
-       ConvexShape* min1 = static_cast<ConvexShape*>(col1->m_collisionShape);
-       
-       ConvexCast::CastResult result;
+                       if (col1->m_hitFraction > result.m_fraction)
+                               col1->m_hitFraction  = result.m_fraction;
 
+                       if (resultFraction > result.m_fraction)
+                               resultFraction = result.m_fraction;
 
-       VoronoiSimplexSolver voronoiSimplex;
-       SubsimplexConvexCast ccd0(&sphere,min1,&voronoiSimplex);
+               }
+               
+               
 
-       ///Simplification, one object is simplified as a sphere
-       GjkConvexCast ccd1(&sphere,min0,&voronoiSimplex);
-       //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
 
-       if (disableCcd)
-               return 1.f;
+       }
 
-       if (ccd1.calcTimeOfImpact(col0->m_worldTransform,col0->m_interpolationWorldTransform,
-               col1->m_worldTransform,col1->m_interpolationWorldTransform,result))
+       /// Sphere (for convex0) against Convex1
        {
-       
-               //store result.m_fraction in both bodies
-       
-               if (col0->m_hitFraction > result.m_fraction)
-                       col0->m_hitFraction  = result.m_fraction;
-
-               if (col1->m_hitFraction > result.m_fraction)
-                       col1->m_hitFraction  = result.m_fraction;
+               ConvexShape* convex1 = static_cast<ConvexShape*>(col1->m_collisionShape);
+
+               SphereShape     sphere0(col0->m_ccdSweptShereRadius); //todo: allow non-zero sphere sizes, for better approximation
+               ConvexCast::CastResult result;
+               VoronoiSimplexSolver voronoiSimplex;
+               //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+               ///Simplification, one object is simplified as a sphere
+               GjkConvexCast 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))
+               {
+               
+                       //store result.m_fraction in both bodies
+               
+                       if (col0->m_hitFraction > result.m_fraction)
+                               col0->m_hitFraction  = result.m_fraction;
 
-               return result.m_fraction;
-       }
+                       if (col1->m_hitFraction > result.m_fraction)
+                               col1->m_hitFraction  = result.m_fraction;
 
+                       if (resultFraction > result.m_fraction)
+                               resultFraction = result.m_fraction;
 
+               }
+       }
        
-
-
-       return 1.f;
+       return resultFraction;
 
 }
index c8ace81dda16e2467071a62b85755046fce9245c..a1cadabe7ebc27a11d1b31c7e77337b80887ed7d 100644 (file)
@@ -55,7 +55,13 @@ public:
 
        void    SetLowLevelOfDetail(bool useLowLevel);
 
-       
+       virtual void SetShapeIdentifiers(int partId0,int index0,        int partId1,int index1)
+       {
+                       m_gjkPairDetector.m_partId0=partId0;
+                       m_gjkPairDetector.m_partId1=partId1;
+                       m_gjkPairDetector.m_index0=index0;
+                       m_gjkPairDetector.m_index1=index1;              
+       }
 
        const PersistentManifold*       GetManifold()
        {
index 15b6512a5380c6dfce4af8d9b1929481b1040de5..36f7255a7373bf4d62f6dc0996fff64beea3e51a 100644 (file)
@@ -18,6 +18,31 @@ subject to the following restrictions:
 #include "NarrowPhaseCollision/PersistentManifold.h"
 #include "CollisionDispatch/CollisionObject.h"
 
+
+///This is to allow MaterialCombiner/Custom Friction/Restitution values
+ContactAddedCallback           gContactAddedCallback=0;
+
+///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= CollisionObject::customMaterialCallback;
+inline SimdScalar      calculateCombinedFriction(const CollisionObject* body0,const CollisionObject* body1)
+{
+       SimdScalar friction = body0->getFriction() * body1->getFriction();
+
+       const SimdScalar MAX_FRICTION  = 10.f;
+       if (friction < -MAX_FRICTION)
+               friction = -MAX_FRICTION;
+       if (friction > MAX_FRICTION)
+               friction = MAX_FRICTION;
+       return friction;
+
+}
+
+inline SimdScalar      calculateCombinedRestitution(const CollisionObject* body0,const CollisionObject* body1)
+{
+       return body0->getRestitution() * body1->getRestitution();
+}
+
+
+
 ManifoldResult::ManifoldResult(CollisionObject* body0,CollisionObject* body1,PersistentManifold* manifoldPtr)
                :m_manifoldPtr(manifoldPtr),
                m_body0(body0),
@@ -31,8 +56,12 @@ void ManifoldResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const S
        if (depth > m_manifoldPtr->GetContactBreakingTreshold())
                return;
 
-       SimdTransform transAInv = m_body0->m_worldTransform.inverse();
-       SimdTransform transBInv= m_body1->m_worldTransform.inverse();
+
+       SimdTransform transAInv = m_body0->m_cachedInvertedWorldTransform;
+       SimdTransform transBInv= m_body1->m_cachedInvertedWorldTransform;
+
+       //transAInv = m_body0->m_worldTransform.inverse();
+       //transBInv= m_body1->m_worldTransform.inverse();
        SimdVector3 pointA = pointInWorld + normalOnBInWorld * depth;
        SimdVector3 localA = transAInv(pointA );
        SimdVector3 localB = transBInv(pointInWorld);
@@ -52,6 +81,20 @@ void ManifoldResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const S
 
        } else
        {
+
+               newPt.m_combinedFriction = calculateCombinedFriction(m_body0,m_body1);
+               newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0,m_body1);
+
+               //User can override friction and/or restitution
+               if (gContactAddedCallback &&
+                       //and if either of the two bodies requires custom material
+                        ((m_body0->m_collisionFlags & CollisionObject::customMaterialCallback) ||
+                          (m_body1->m_collisionFlags & CollisionObject::customMaterialCallback)))
+               {
+                       //experimental feature info, for per-triangle material etc.
+                       (*gContactAddedCallback)(newPt,m_body0,m_partId0,m_index0,m_body1,m_partId1,m_index1);
+               }
+
                m_manifoldPtr->AddManifoldPoint(newPt);
        }
 }
index 6126a7597440f1375b223deb36a09b07172e3ae7..fe2438980a1e7518e75739b02dd0f1d7b4bc0785 100644 (file)
@@ -20,22 +20,41 @@ subject to the following restrictions:
 #include "NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h"
 struct CollisionObject;
 class PersistentManifold;
+class ManifoldPoint;
 
+typedef bool (*ContactAddedCallback)(ManifoldPoint& cp,        const CollisionObject* colObj0,int partId0,int index0,const CollisionObject* colObj1,int partId1,int index1);
+extern ContactAddedCallback            gContactAddedCallback;
 
+
+
+///ManifoldResult is a helper class to manage  contact results.
 class ManifoldResult : public DiscreteCollisionDetectorInterface::Result
 {
        PersistentManifold* m_manifoldPtr;
        CollisionObject* m_body0;
        CollisionObject* m_body1;
-
+       int     m_partId0;
+       int m_partId1;
+       int m_index0;
+       int m_index1;
 public:
 
        ManifoldResult(CollisionObject* body0,CollisionObject* body1,PersistentManifold* manifoldPtr);
 
        virtual ~ManifoldResult() {};
 
+       virtual void SetShapeIdentifiers(int partId0,int index0,        int partId1,int index1)
+       {
+                       m_partId0=partId0;
+                       m_partId1=partId1;
+                       m_index0=index0;
+                       m_index1=index1;                
+       }
+
        virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth);
 
+
+
 };
 
 #endif //MANIFOLD_RESULT_H
diff --git a/extern/bullet/Bullet/CollisionDispatch/SimulationIslandManager.cpp b/extern/bullet/Bullet/CollisionDispatch/SimulationIslandManager.cpp
new file mode 100644 (file)
index 0000000..10a7f0b
--- /dev/null
@@ -0,0 +1,203 @@
+
+#include "SimulationIslandManager.h"
+#include "BroadphaseCollision/Dispatcher.h"
+#include "NarrowPhaseCollision/PersistentManifold.h"
+#include "CollisionDispatch/CollisionObject.h"
+#include "CollisionDispatch/CollisionWorld.h"
+
+
+
+SimulationIslandManager::SimulationIslandManager()
+{
+}
+
+void SimulationIslandManager::InitUnionFind(int n)
+{
+               m_unionFind.reset(n);
+}
+               
+
+void SimulationIslandManager::FindUnions(Dispatcher* dispatcher)
+{
+       
+       {
+               for (int i=0;i<dispatcher->GetNumManifolds();i++)
+               {
+                       const PersistentManifold* manifold = dispatcher->GetManifoldByIndexInternal(i);
+                       //static objects (invmass 0.f) don't merge !
+
+                        const  CollisionObject* colObj0 = static_cast<const CollisionObject*>(manifold->GetBody0());
+                        const  CollisionObject* colObj1 = static_cast<const CollisionObject*>(manifold->GetBody1());
+
+                        if (colObj0 && colObj1 && dispatcher->NeedsResponse(*colObj0,*colObj1))
+                        {
+                               if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
+                                       ((colObj1) && ((colObj1)->mergesSimulationIslands())))
+                               {
+
+                                       m_unionFind.unite((colObj0)->m_islandTag1,
+                                               (colObj1)->m_islandTag1);
+                               }
+                        }
+                       
+                       
+               }
+       }
+       
+}
+
+
+void   SimulationIslandManager::UpdateActivationState(CollisionWorld* colWorld,Dispatcher* dispatcher)
+{
+       
+       InitUnionFind(colWorld->GetCollisionObjectArray().size());
+       
+       // put the index into m_controllers into m_tag  
+       {
+               std::vector<CollisionObject*>::iterator i;
+               
+               int index = 0;
+               for (i=colWorld->GetCollisionObjectArray().begin();
+               !(i==colWorld->GetCollisionObjectArray().end()); i++)
+               {
+                       
+                       CollisionObject*        collisionObject= (*i);
+                       collisionObject->m_islandTag1 = index;
+                       collisionObject->m_hitFraction = 1.f;
+                       index++;
+                       
+               }
+       }
+       // do the union find
+       
+       FindUnions(dispatcher);
+       
+
+       
+}
+
+
+
+
+void   SimulationIslandManager::StoreIslandActivationState(CollisionWorld* colWorld)
+{
+       // put the islandId ('find' value) into m_tag   
+       {
+               
+               
+               std::vector<CollisionObject*>::iterator i;
+               
+               int index = 0;
+               for (i=colWorld->GetCollisionObjectArray().begin();
+               !(i==colWorld->GetCollisionObjectArray().end()); i++)
+               {
+                       CollisionObject* collisionObject= (*i);
+                       
+                       if (collisionObject->mergesSimulationIslands())
+                       {
+                               collisionObject->m_islandTag1 = m_unionFind.find(index);
+                       } else
+                       {
+                               collisionObject->m_islandTag1 = -1;
+                       }
+                       index++;
+               }
+       }
+}
+
+
+
+
+//
+// todo: this is random access, it can be walked 'cache friendly'!
+//
+void SimulationIslandManager::BuildAndProcessIslands(Dispatcher* dispatcher,CollisionObjectArray& collisionObjects, IslandCallback* callback)
+{
+       
+       int numBodies  = collisionObjects.size();
+
+       for (int islandId=0;islandId<numBodies;islandId++)
+       {
+
+               std::vector<PersistentManifold*>  islandmanifold;
+               
+               //int numSleeping = 0;
+
+               bool allSleeping = true;
+
+               int i;
+               for (i=0;i<numBodies;i++)
+               {
+                       CollisionObject* colObj0 = collisionObjects[i];
+               
+                       if (colObj0->m_islandTag1 == islandId)
+                       {
+                       
+                               if (colObj0->GetActivationState()== ACTIVE_TAG)
+                               {
+                                       allSleeping = false;
+                               }
+                               if (colObj0->GetActivationState()== DISABLE_DEACTIVATION)
+                               {
+                                       allSleeping = false;
+                               }
+                       }
+               }
+
+               
+               for (i=0;i<dispatcher->GetNumManifolds();i++)
+               {
+                        PersistentManifold* manifold = dispatcher->GetManifoldByIndexInternal(i);
+                        
+                        //filtering for response
+
+                        CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0());
+                        CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1());
+                        {
+                               if (((colObj0) && (colObj0)->m_islandTag1 == (islandId)) ||
+                                       ((colObj1) && (colObj1)->m_islandTag1 == (islandId)))
+                               {
+
+                                       if (dispatcher->NeedsResponse(*colObj0,*colObj1))
+                                               islandmanifold.push_back(manifold);
+                               }
+                        }
+               }
+               if (allSleeping)
+               {
+                       int i;
+                       for (i=0;i<numBodies;i++)
+                       {
+                               CollisionObject* colObj0 = collisionObjects[i];
+                               if (colObj0->m_islandTag1 == islandId)
+                               {
+                                       colObj0->SetActivationState( ISLAND_SLEEPING );
+                               }
+                       }
+
+                       
+               } else
+               {
+
+                       int i;
+                       for (i=0;i<numBodies;i++)
+                       {
+                               CollisionObject* colObj0 = collisionObjects[i];
+                               if (colObj0->m_islandTag1 == islandId)
+                               {
+                                       if ( colObj0->GetActivationState() == ISLAND_SLEEPING)
+                                       {
+                                               colObj0->SetActivationState( WANTS_DEACTIVATION);
+                                       }
+                               }
+                       }
+
+                       /// Process the actual simulation, only if not sleeping/deactivated
+                       if (islandmanifold.size())
+                       {
+                               callback->ProcessIsland(&islandmanifold[0],islandmanifold.size());
+                       }
+
+               }
+       }
+}
diff --git a/extern/bullet/Bullet/CollisionDispatch/SimulationIslandManager.h b/extern/bullet/Bullet/CollisionDispatch/SimulationIslandManager.h
new file mode 100644 (file)
index 0000000..8428528
--- /dev/null
@@ -0,0 +1,58 @@
+/*
+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 SIMULATION_ISLAND_MANAGER_H
+#define SIMULATION_ISLAND_MANAGER_H
+
+#include "CollisionDispatch/UnionFind.h"
+#include "CollisionCreateFunc.h"
+
+class CollisionWorld;
+class Dispatcher;
+
+///SimulationIslandManager creates and handles simulation islands, using UnionFind
+class SimulationIslandManager
+{
+       UnionFind m_unionFind;
+
+public:
+       SimulationIslandManager();
+
+
+       void InitUnionFind(int n);      
+       
+               
+       UnionFind& GetUnionFind() { return m_unionFind;}
+
+       virtual void    UpdateActivationState(CollisionWorld* colWorld,Dispatcher* dispatcher);
+       virtual void    StoreIslandActivationState(CollisionWorld* world);
+
+
+       void    FindUnions(Dispatcher* dispatcher);
+
+       
+
+       struct  IslandCallback
+       {
+               virtual ~IslandCallback() {};
+
+               virtual void    ProcessIsland(class PersistentManifold**        manifolds,int numManifolds) = 0;
+       };
+
+       void    BuildAndProcessIslands(Dispatcher* dispatcher,CollisionObjectArray& collisionObjects, IslandCallback* callback);
+
+};
+
+#endif //SIMULATION_ISLAND_MANAGER_H
index f47f982112d0f337c88b98b93f71963bd3592d84..67ce82078f36ba49775002245a0a0a92b1ab7e54 100644 (file)
@@ -48,24 +48,12 @@ public:
        {
                
                SimdVector3 halfExtents = GetHalfExtents();
-               SimdVector3 margin(GetMargin(),GetMargin(),GetMargin());
-               halfExtents -= margin;
                
                SimdVector3 supVertex;
                supVertex = SimdPoint3(vec.x() < SimdScalar(0.0f) ? -halfExtents.x() : halfExtents.x(),
                      vec.y() < SimdScalar(0.0f) ? -halfExtents.y() : halfExtents.y(),
                      vec.z() < SimdScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); 
   
-               if ( GetMargin()!=0.f )
-               {
-                       SimdVector3 vecnorm = vec;
-                       if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
-                       {
-                               vecnorm.setValue(-1.f,-1.f,-1.f);
-                       } 
-                       vecnorm.normalize();
-                       supVertex+= GetMargin() * vecnorm;
-               }
                return supVertex;
        }
 
index 0c0f6984f9b9b4d4e81668122b5bb99c5fe4d5fa..57bf97874b21b18052220a30eb66d36aec2b80a7 100644 (file)
@@ -59,9 +59,13 @@ public:
        }
        bool    IsConcave() const
        {
-               return (GetShapeType() > CONCAVE_SHAPES_START_HERE);
+               return ((GetShapeType() > CONCAVE_SHAPES_START_HERE) &&
+                       (GetShapeType() < CONCAVE_SHAPES_END_HERE));
+       }
+       bool    IsCompound() const
+       {
+               return (GetShapeType() == COMPOUND_SHAPE_PROXYTYPE);
        }
-
 
        virtual void    setLocalScaling(const SimdVector3& scaling) =0;
        virtual const SimdVector3& getLocalScaling() const =0;
diff --git a/extern/bullet/Bullet/CollisionShapes/CompoundShape.cpp b/extern/bullet/Bullet/CollisionShapes/CompoundShape.cpp
new file mode 100644 (file)
index 0000000..ecb1170
--- /dev/null
@@ -0,0 +1,100 @@
+/*
+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 "CompoundShape.h"
+
+
+#include "CollisionShape.h"
+
+
+CompoundShape::CompoundShape()
+:m_localAabbMin(1e30f,1e30f,1e30f),
+m_localAabbMax(-1e30f,-1e30f,-1e30f),
+m_aabbTree(0),
+m_collisionMargin(0.f),
+m_localScaling(1.f,1.f,1.f)
+{
+}
+
+
+CompoundShape::~CompoundShape()
+{
+}
+
+void   CompoundShape::AddChildShape(const SimdTransform& localTransform,CollisionShape* shape)
+{
+       m_childTransforms.push_back(localTransform);
+       m_childShapes.push_back(shape);
+
+       //extend the local aabbMin/aabbMax
+       SimdVector3 localAabbMin,localAabbMax;
+       shape->GetAabb(localTransform,localAabbMin,localAabbMax);
+       for (int i=0;i<3;i++)
+       {
+               if (m_localAabbMin[i] > localAabbMin[i])
+               {
+                       m_localAabbMin[i] = localAabbMin[i];
+               }
+               if (m_localAabbMax[i] < localAabbMax[i])
+               {
+                       m_localAabbMax[i] = localAabbMax[i];
+               }
+
+       }
+}
+
+
+
+       ///GetAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+void CompoundShape::GetAabb(const SimdTransform& trans,SimdVector3& aabbMin,SimdVector3& aabbMax) const
+{
+       SimdVector3 localHalfExtents = 0.5f*(m_localAabbMax-m_localAabbMin);
+       SimdVector3 localCenter = 0.5f*(m_localAabbMax+m_localAabbMin);
+       
+       SimdMatrix3x3 abs_b = trans.getBasis().absolute();  
+
+       SimdPoint3 center = trans(localCenter);
+
+       SimdVector3 extent = SimdVector3(abs_b[0].dot(localHalfExtents),
+                  abs_b[1].dot(localHalfExtents),
+                 abs_b[2].dot(localHalfExtents));
+       extent += SimdVector3(GetMargin(),GetMargin(),GetMargin());
+
+       aabbMin = center - extent;
+       aabbMax = center + extent;
+}
+
+void   CompoundShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
+{
+       //approximation: take the inertia from the aabb for now
+       SimdTransform ident;
+       ident.setIdentity();
+       SimdVector3 aabbMin,aabbMax;
+       GetAabb(ident,aabbMin,aabbMax);
+       
+       SimdVector3 halfExtents = (aabbMax-aabbMin)*0.5f;
+       
+       SimdScalar lx=2.f*(halfExtents.x());
+       SimdScalar ly=2.f*(halfExtents.y());
+       SimdScalar lz=2.f*(halfExtents.z());
+
+       inertia[0] = mass/(12.0f) * (ly*ly + lz*lz);
+       inertia[1] = mass/(12.0f) * (lx*lx + lz*lz);
+       inertia[2] = mass/(12.0f) * (lx*lx + ly*ly);
+
+}
+
+       
+       
diff --git a/extern/bullet/Bullet/CollisionShapes/CompoundShape.h b/extern/bullet/Bullet/CollisionShapes/CompoundShape.h
new file mode 100644 (file)
index 0000000..7a43e44
--- /dev/null
@@ -0,0 +1,117 @@
+/*
+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 COMPOUND_SHAPE_H
+#define COMPOUND_SHAPE_H
+
+#include "CollisionShape.h"
+
+#include "SimdVector3.h"
+#include "SimdTransform.h"
+#include "SimdMatrix3x3.h"
+#include <vector>
+#include "CollisionShapes/CollisionMargin.h"
+
+class OptimizedBvh;
+
+/// CompoundShape allows to store multiple other CollisionShapes
+/// This allows for concave collision objects. This is more general then the Static Concave TriangleMeshShape.
+class CompoundShape    : public CollisionShape
+{
+       std::vector<SimdTransform>              m_childTransforms;
+       std::vector<CollisionShape*>    m_childShapes;
+       SimdVector3                                             m_localAabbMin;
+       SimdVector3                                             m_localAabbMax;
+
+       OptimizedBvh*                                   m_aabbTree;
+
+public:
+       CompoundShape();
+
+       virtual ~CompoundShape();
+
+       void    AddChildShape(const SimdTransform& localTransform,CollisionShape* shape);
+
+       int             GetNumChildShapes() const
+       {
+               return m_childShapes.size();
+       }
+
+       CollisionShape* GetChildShape(int index)
+       {
+               return m_childShapes[index];
+       }
+       const CollisionShape* GetChildShape(int index) const
+       {
+               return m_childShapes[index];
+       }
+
+       SimdTransform   GetChildTransform(int index)
+       {
+               return m_childTransforms[index];
+       }
+       const SimdTransform     GetChildTransform(int index) const
+       {
+               return m_childTransforms[index];
+       }
+
+       ///GetAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+       void GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const;
+
+
+       virtual void    setLocalScaling(const SimdVector3& scaling)
+       {
+               m_localScaling = scaling;
+       }
+       virtual const SimdVector3& getLocalScaling() const 
+       {
+               return m_localScaling;
+       }
+
+       virtual void    CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia);
+       
+       virtual int     GetShapeType() const { return COMPOUND_SHAPE_PROXYTYPE;}
+
+       virtual void    SetMargin(float margin)
+       {
+               m_collisionMargin = margin;
+       }
+       virtual float   GetMargin() const
+       {
+               return m_collisionMargin;
+       }
+       virtual char*   GetName()const
+       {
+               return "Compound";
+       }
+
+       //this is optional, but should make collision queries faster, by culling non-overlapping nodes
+       void    CreateAabbTreeFromChildren();
+
+       const OptimizedBvh*                                     GetAabbTree() const
+       {
+               return m_aabbTree;
+       }
+
+private:
+       SimdScalar      m_collisionMargin;
+protected:
+       SimdVector3     m_localScaling;
+
+};
+
+
+
+#endif //COMPOUND_SHAPE_H
diff --git a/extern/bullet/Bullet/CollisionShapes/ConcaveShape.cpp b/extern/bullet/Bullet/CollisionShapes/ConcaveShape.cpp
new file mode 100644 (file)
index 0000000..c060a4d
--- /dev/null
@@ -0,0 +1,28 @@
+
+/*
+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 "ConcaveShape.h"
+
+ConcaveShape::ConcaveShape() : m_collisionMargin(0.f)
+{
+
+}
+
+ConcaveShape::~ConcaveShape()
+{
+
+}
diff --git a/extern/bullet/Bullet/CollisionShapes/ConcaveShape.h b/extern/bullet/Bullet/CollisionShapes/ConcaveShape.h
new file mode 100644 (file)
index 0000000..d6c589d
--- /dev/null
@@ -0,0 +1,51 @@
+/*
+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 CONCAVE_SHAPE_H
+#define CONCAVE_SHAPE_H
+
+#include "CollisionShapes/CollisionShape.h"
+#include "BroadphaseCollision/BroadphaseProxy.h" // for the types
+
+#include "TriangleCallback.h"
+
+
+///Concave shape proves an interface concave shapes that can produce triangles that overlapping a given AABB.
+///Static triangle mesh, infinite plane, height field/landscapes are example that implement this interface.
+class ConcaveShape : public CollisionShape
+{
+protected:
+       float m_collisionMargin;
+
+public:
+       ConcaveShape();
+
+       virtual ~ConcaveShape();
+
+       virtual void    ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const = 0;
+
+       virtual float GetMargin() const {
+               return m_collisionMargin;
+       }
+       virtual void SetMargin(float collisionMargin)
+       {
+               m_collisionMargin = collisionMargin;
+       }
+
+
+
+};
+
+#endif //CONCAVE_SHAPE_H
diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp
deleted file mode 100644 (file)
index a34add4..0000000
+++ /dev/null
@@ -1,96 +0,0 @@
-/*
-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 "ConvexTriangleCallback.h"
-#include "NarrowPhaseCollision/PersistentManifold.h"
-#include "NarrowPhaseCollision/ManifoldContactAddResult.h"
-#include "NarrowPhaseCollision/GjkPairDetector.h"
-#include "NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.h"
-
-
-
-#include "TriangleShape.h"
-
-//m_manifoldPtr = m_dispatcher->GetNewManifold(proxy0->m_clientObject,proxy1->m_clientObject);
-       //m_dispatcher->ReleaseManifold( m_manifoldPtr );
-
-ConvexTriangleCallback::ConvexTriangleCallback(PersistentManifold* manifold,ConvexShape* convexShape,const SimdTransform&convexTransform,const SimdTransform& triangleMeshTransform)
-:m_triangleMeshTransform(triangleMeshTransform),
-       m_convexTransform(convexTransform),
-       m_triangleCount(0),
-       m_convexShape(convexShape),
-       m_manifoldPtr(manifold)
-{
-}
-
-ConvexTriangleCallback::~ConvexTriangleCallback()
-{
-  
-}
-  
-
-
-
-void ConvexTriangleCallback::ProcessTriangle(SimdVector3* triangle)
-{
-
-       //triangle, convex
-       TriangleShape tm(triangle[0],triangle[1],triangle[2]);  
-       tm.SetMargin(m_collisionMarginTriangle);
-       GjkPairDetector::ClosestPointInput input;
-       input.m_transformA = m_triangleMeshTransform;
-       input.m_transformB = m_convexTransform;
-       
-       ManifoldContactAddResult output(m_triangleMeshTransform,m_convexTransform,m_manifoldPtr);
-       
-       
-       VoronoiSimplexSolver simplexSolver;
-       MinkowskiPenetrationDepthSolver penetrationDepthSolver;
-       
-       GjkPairDetector gjkDetector(&tm,m_convexShape,&simplexSolver,&penetrationDepthSolver);
-
-       gjkDetector.SetMinkowskiA(&tm);
-       gjkDetector.SetMinkowskiB(m_convexShape);
-       input.m_maximumDistanceSquared = tm.GetMargin()+ m_convexShape->GetMargin() + m_manifoldPtr->GetContactBreakingTreshold();
-       input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
-
-       input.m_maximumDistanceSquared = 1e30f;//?
-       
-               
-       gjkDetector.GetClosestPoints(input,output,0);
-
-
-}
-
-
-
-void   ConvexTriangleCallback::Update(float collisionMarginTriangle)
-{
-       m_triangleCount = 0;
-       m_collisionMarginTriangle = collisionMarginTriangle;
-
-       SimdTransform boxInTriangleSpace;
-       boxInTriangleSpace = m_triangleMeshTransform.inverse() * m_convexTransform;
-
-       m_convexShape->GetAabb(boxInTriangleSpace,m_aabbMin,m_aabbMax);
-
-       float extraMargin = CONVEX_DISTANCE_MARGIN;
-
-       SimdVector3 extra(extraMargin,extraMargin,extraMargin);
-
-       m_aabbMax += extra;
-       m_aabbMin -= extra;
-       
-}
diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.h b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.h
deleted file mode 100644 (file)
index 9160d1f..0000000
+++ /dev/null
@@ -1,64 +0,0 @@
-/*
-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 CONVEX_TRIANGLE_CALLBACK_H
-#define CONVEX_TRIANGLE_CALLBACK_H
-
-#include "TriangleCallback.h"
-class ConvexShape;
-class PersistentManifold;
-#include "SimdTransform.h"
-///ConvexTriangleCallback processes the narrowphase convex-triangle collision detection
-class ConvexTriangleCallback: public TriangleCallback
-{
-       SimdVector3     m_aabbMin;
-       SimdVector3     m_aabbMax ;
-
-       SimdTransform   m_triangleMeshTransform;
-       SimdTransform   m_convexTransform;
-
-//     bool m_useContinuous;
-       float m_collisionMarginTriangle;
-       
-public:
-int    m_triangleCount;
-       
-       ConvexShape*    m_convexShape;
-
-       PersistentManifold*     m_manifoldPtr;
-
-       ConvexTriangleCallback(PersistentManifold* manifold,ConvexShape* convexShape,const SimdTransform&convexTransform,const SimdTransform& triangleMeshTransform);
-
-       void    Update(float collisionMarginTriangle);
-
-       virtual ~ConvexTriangleCallback();
-
-       virtual void ProcessTriangle(SimdVector3* triangle);
-       
-
-       inline const SimdVector3& GetAabbMin() const
-       {
-               return m_aabbMin;
-       }
-       inline const SimdVector3& GetAabbMax() const
-       {
-               return m_aabbMax;
-       }
-
-};
-
-
-#endif //CONVEX_TRIANGLE_CALLBACK_H
-
diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleMeshShape.cpp b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleMeshShape.cpp
new file mode 100644 (file)
index 0000000..44bbaeb
--- /dev/null
@@ -0,0 +1,197 @@
+/*
+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 "ConvexTriangleMeshShape.h"
+#include "CollisionShapes/CollisionMargin.h"
+
+#include "SimdQuaternion.h"
+#include "CollisionShapes/StridingMeshInterface.h"
+
+
+ConvexTriangleMeshShape ::ConvexTriangleMeshShape (StridingMeshInterface* meshInterface)
+:m_stridingMesh(meshInterface)
+{
+}
+
+
+
+
+///It's not nice to have all this virtual function overhead, so perhaps we can also gather the points once
+///but then we are duplicating
+class LocalSupportVertexCallback: public InternalTriangleIndexCallback
+{
+
+       SimdVector3 m_supportVertexLocal;
+public:
+
+       SimdScalar m_maxDot;
+       SimdVector3 m_supportVecLocal;
+
+       LocalSupportVertexCallback(const SimdVector3& supportVecLocal)
+               : m_supportVertexLocal(0.f,0.f,0.f),
+               m_supportVecLocal(supportVecLocal),
+               m_maxDot(-1e30f)
+       {
+       }
+
+       virtual void InternalProcessTriangleIndex(SimdVector3* triangle,int partId,int  triangleIndex)
+       {
+               for (int i=0;i<3;i++)
+               {
+                       SimdScalar dot = m_supportVecLocal.dot(triangle[i]);
+                       if (dot > m_maxDot)
+                       {
+                               m_maxDot = dot;
+                               m_supportVertexLocal = triangle[i];
+                       }
+               }
+       }
+       
+       SimdVector3     GetSupportVertexLocal()
+       {
+               return m_supportVertexLocal;
+       }
+
+};
+
+
+
+
+
+SimdVector3    ConvexTriangleMeshShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec0)const
+{
+       SimdVector3 supVec(0.f,0.f,0.f);
+       SimdScalar newDot,maxDot = -1e30f;
+
+       SimdVector3 vec = vec0;
+       SimdScalar lenSqr = vec.length2();
+       if (lenSqr < 0.0001f)
+       {
+               vec.setValue(1,0,0);
+       } else
+       {
+               float rlen = 1.f / SimdSqrt(lenSqr );
+               vec *= rlen;
+       }
+
+       LocalSupportVertexCallback      supportCallback(vec);
+       SimdVector3 aabbMax(1e30f,1e30f,1e30f);
+       m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
+       supVec = supportCallback.GetSupportVertexLocal();
+
+       return supVec;
+}
+
+void   ConvexTriangleMeshShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const
+{
+       SimdScalar newDot;
+       //use 'w' component of supportVerticesOut?
+       {
+               for (int i=0;i<numVectors;i++)
+               {
+                       supportVerticesOut[i][3] = -1e30f;
+               }
+       }
+       
+       //todo: could do the batch inside the callback!
+
+
+       for (int j=0;j<numVectors;j++)
+       {
+               const SimdVector3& vec = vectors[j];
+               LocalSupportVertexCallback      supportCallback(vec);
+               SimdVector3 aabbMax(1e30f,1e30f,1e30f);
+               m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
+               supportVerticesOut[j] = supportCallback.GetSupportVertexLocal();
+       }
+       
+}
+       
+
+
+SimdVector3    ConvexTriangleMeshShape::LocalGetSupportingVertex(const SimdVector3& vec)const
+{
+       SimdVector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec);
+
+       if ( GetMargin()!=0.f )
+       {
+               SimdVector3 vecnorm = vec;
+               if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+               {
+                       vecnorm.setValue(-1.f,-1.f,-1.f);
+               } 
+               vecnorm.normalize();
+               supVertex+= GetMargin() * vecnorm;
+       }
+       return supVertex;
+}
+
+
+
+
+
+
+
+
+
+//currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection
+//Please note that you can debug-draw ConvexTriangleMeshShape with the Raytracer Demo
+int    ConvexTriangleMeshShape::GetNumVertices() const
+{
+       //cache this?
+       assert(0);
+       return 0;
+       
+}
+
+int ConvexTriangleMeshShape::GetNumEdges() const
+{
+       assert(0);      
+       return 0;
+}
+
+void ConvexTriangleMeshShape::GetEdge(int i,SimdPoint3& pa,SimdPoint3& pb) const
+{
+       assert(0);      
+}
+
+void ConvexTriangleMeshShape::GetVertex(int i,SimdPoint3& vtx) const
+{
+       assert(0);
+}
+
+int    ConvexTriangleMeshShape::GetNumPlanes() const
+{
+       return 0;
+}
+
+void ConvexTriangleMeshShape::GetPlane(SimdVector3& planeNormal,SimdPoint3& planeSupport,int i ) const
+{
+       assert(0);
+}
+
+//not yet
+bool ConvexTriangleMeshShape::IsInside(const SimdPoint3& pt,SimdScalar tolerance) const
+{
+       assert(0);
+       return false;
+}
+
+
+
+void   ConvexTriangleMeshShape::setLocalScaling(const SimdVector3& scaling)
+{
+       m_stridingMesh->setScaling(scaling);
+}
+
diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleMeshShape.h b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleMeshShape.h
new file mode 100644 (file)
index 0000000..28bcb2e
--- /dev/null
@@ -0,0 +1,49 @@
+#ifndef CONVEX_TRIANGLEMESH_SHAPE_H
+#define CONVEX_TRIANGLEMESH_SHAPE_H
+
+
+#include "PolyhedralConvexShape.h"
+#include "BroadphaseCollision/BroadphaseProxy.h" // for the types
+
+#include <vector>
+
+/// ConvexTriangleMeshShape is a convex hull of a triangle mesh. If you just have a point cloud, you can use ConvexHullShape instead.
+/// It uses the StridingMeshInterface instead of a point cloud. This can avoid the duplication of the triangle mesh data.
+class ConvexTriangleMeshShape : public PolyhedralConvexShape
+{
+
+       class StridingMeshInterface*    m_stridingMesh;
+
+public:
+       ConvexTriangleMeshShape(StridingMeshInterface* meshInterface);
+
+       class StridingMeshInterface*    GetStridingMesh()
+       {
+               return m_stridingMesh;
+       }
+       
+       virtual SimdVector3     LocalGetSupportingVertex(const SimdVector3& vec)const;
+       virtual SimdVector3     LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const;
+       virtual void    BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const;
+       
+       virtual int     GetShapeType()const { return CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE; }
+
+       //debugging
+       virtual char*   GetName()const {return "ConvexTrimesh";}
+       
+       virtual int     GetNumVertices() const;
+       virtual int GetNumEdges() const;
+       virtual void GetEdge(int i,SimdPoint3& pa,SimdPoint3& pb) const;
+       virtual void GetVertex(int i,SimdPoint3& vtx) const;
+       virtual int     GetNumPlanes() const;
+       virtual void GetPlane(SimdVector3& planeNormal,SimdPoint3& planeSupport,int i ) const;
+       virtual bool IsInside(const SimdPoint3& pt,SimdScalar tolerance) const;
+
+       
+       void    setLocalScaling(const SimdVector3& scaling);
+
+};
+
+
+
+#endif //CONVEX_TRIANGLEMESH_SHAPE_H
\ No newline at end of file
index 49e3c0ab3ee31c496292576f950fb16f232ecabd..217ce70369a226cab226e177996066d0de448781 100644 (file)
@@ -66,6 +66,17 @@ public:
                return CYLINDER_SHAPE_PROXYTYPE;
        }
        
+       virtual int     GetUpAxis() const
+       {
+               return 1;
+       }
+
+       //debugging
+       virtual char*   GetName()const
+       {
+               return "CylinderY";
+       }
+
 
 
 };
@@ -77,6 +88,16 @@ public:
 
        virtual SimdVector3     LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const;
        virtual void    BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const;
+       virtual int     GetUpAxis() const
+       {
+               return 0;
+       }
+               //debugging
+       virtual char*   GetName()const
+       {
+               return "CylinderX";
+       }
+
 };
 
 class CylinderShapeZ : public CylinderShape
@@ -87,6 +108,16 @@ public:
        virtual SimdVector3     LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const;
        virtual void    BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const;
 
+       virtual int     GetUpAxis() const
+       {
+               return 2;
+       }
+               //debugging
+       virtual char*   GetName()const
+       {
+               return "CylinderZ";
+       }
+
 };
 
 
index d4b83663a947e589f94b7f99b7ae697d133d0345..4407a22acdfa5cb7cf6d2067f2bce37b8a3940fd 100644 (file)
@@ -16,7 +16,7 @@ subject to the following restrictions:
 #ifndef EMPTY_SHAPE_H
 #define EMPTY_SHAPE_H
 
-#include "CollisionShape.h"
+#include "ConcaveShape.h"
 
 #include "SimdVector3.h"
 #include "SimdTransform.h"
@@ -27,8 +27,9 @@ subject to the following restrictions:
 
 
 
-/// EmptyShape is a collision shape without actual collision detection. It can be replaced by another shape during runtime
-class EmptyShape       : public CollisionShape
+/// EmptyShape is a collision shape without actual collision detection. 
+///It can be replaced by another shape during runtime
+class EmptyShape       : public ConcaveShape
 {
 public:
        EmptyShape();
@@ -53,22 +54,13 @@ public:
        
        virtual int     GetShapeType() const { return EMPTY_SHAPE_PROXYTYPE;}
 
-       virtual void    SetMargin(float margin)
-       {
-               m_collisionMargin = margin;
-       }
-       virtual float   GetMargin() const
-       {
-               return m_collisionMargin;
-       }
+       
        virtual char*   GetName()const
        {
                return "Empty";
        }
 
 
-private:
-       SimdScalar      m_collisionMargin;
 protected:
        SimdVector3     m_localScaling;
 
index 0cd20ecd04874dcfc16d84e97293b9213a17276b..1b30bfbcb83054d8102eba53eb9afa75ea4f2604 100644 (file)
@@ -193,21 +193,14 @@ int       OptimizedBvh::CalcSplittingAxis(NodeArray&      leafNodes,int startIndex,int endI
 }
 
 
-       
+
 void   OptimizedBvh::ReportAabbOverlappingNodex(NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
 {
-       if (aabbMin.length() > 1000.f)
-       {
-               for (size_t i=0;i<m_leafNodes.size();i++)
-               {
-                       const OptimizedBvhNode& node = m_leafNodes[i];
-                       nodeCallback->ProcessNode(&node);
-               }
-       } else
-       {
-               //WalkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax);
-               WalkStacklessTree(m_rootNode1,nodeCallback,aabbMin,aabbMax);
-       }
+       //either choose recursive traversal (WalkTree) or stackless (WalkStacklessTree)
+
+       //WalkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax);
+
+       WalkStacklessTree(m_rootNode1,nodeCallback,aabbMin,aabbMax);
 }
 
 void   OptimizedBvh::WalkTree(OptimizedBvhNode* rootNode,NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
diff --git a/extern/bullet/Bullet/CollisionShapes/StaticPlaneShape.cpp b/extern/bullet/Bullet/CollisionShapes/StaticPlaneShape.cpp
new file mode 100644 (file)
index 0000000..48aecf5
--- /dev/null
@@ -0,0 +1,100 @@
+/*
+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 "StaticPlaneShape.h"
+
+#include "SimdTransformUtil.h"
+
+
+StaticPlaneShape::StaticPlaneShape(const SimdVector3& planeNormal,SimdScalar planeConstant)
+:m_planeNormal(planeNormal),
+m_planeConstant(planeConstant),
+m_localScaling(0.f,0.f,0.f)
+{
+}
+
+
+StaticPlaneShape::~StaticPlaneShape()
+{
+}
+
+
+
+void StaticPlaneShape::GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const
+{
+       SimdVector3 infvec (1e30f,1e30f,1e30f);
+
+       SimdVector3 center = m_planeNormal*m_planeConstant;
+       aabbMin = center + infvec*m_planeNormal;
+       aabbMax = aabbMin;
+       aabbMin.setMin(center - infvec*m_planeNormal);
+       aabbMax.setMax(center - infvec*m_planeNormal); 
+
+       aabbMin.setValue(-1e30f,-1e30f,-1e30f);
+       aabbMax.setValue(1e30f,1e30f,1e30f);
+
+}
+
+
+
+
+void   StaticPlaneShape::ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
+{
+
+       SimdVector3 halfExtents = (aabbMax - aabbMin) * 0.5f;
+       SimdScalar radius = halfExtents.length();
+       SimdVector3 center = (aabbMax + aabbMin) * 0.5f;
+       
+       //this is where the triangles are generated, given AABB and plane equation (normal/constant)
+
+       SimdVector3 tangentDir0,tangentDir1;
+
+       //tangentDir0/tangentDir1 can be precalculated
+       SimdPlaneSpace1(m_planeNormal,tangentDir0,tangentDir1);
+
+       SimdVector3 supVertex0,supVertex1;
+
+       SimdVector3 projectedCenter = center - (m_planeNormal.dot(center) - m_planeConstant)*m_planeNormal;
+       
+       SimdVector3 triangle[3];
+       triangle[0] = projectedCenter + tangentDir0*radius + tangentDir1*radius;
+       triangle[1] = projectedCenter + tangentDir0*radius - tangentDir1*radius;
+       triangle[2] = projectedCenter - tangentDir0*radius - tangentDir1*radius;
+
+       callback->ProcessTriangle(triangle,0,0);
+
+       triangle[0] = projectedCenter - tangentDir0*radius - tangentDir1*radius;
+       triangle[1] = projectedCenter - tangentDir0*radius + tangentDir1*radius;
+       triangle[2] = projectedCenter + tangentDir0*radius + tangentDir1*radius;
+
+       callback->ProcessTriangle(triangle,0,1);
+
+}
+
+void   StaticPlaneShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
+{
+       //moving concave objects not supported
+       
+       inertia.setValue(0.f,0.f,0.f);
+}
+
+void   StaticPlaneShape::setLocalScaling(const SimdVector3& scaling)
+{
+       m_localScaling = scaling;
+}
+const SimdVector3& StaticPlaneShape::getLocalScaling() const
+{
+       return m_localScaling;
+}
diff --git a/extern/bullet/Bullet/CollisionShapes/StaticPlaneShape.h b/extern/bullet/Bullet/CollisionShapes/StaticPlaneShape.h
new file mode 100644 (file)
index 0000000..eaf8761
--- /dev/null
@@ -0,0 +1,61 @@
+/*
+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 STATIC_PLANE_SHAPE_H
+#define STATIC_PLANE_SHAPE_H
+
+#include "CollisionShapes/ConcaveShape.h"
+
+
+///StaticPlaneShape simulates an 'infinite' plane by dynamically reporting triangles approximated by intersection of the plane with the AABB.
+///Assumed is that the other objects is not also infinite, so a reasonable sized AABB.
+class StaticPlaneShape : public ConcaveShape
+{
+protected:
+       SimdVector3     m_localAabbMin;
+       SimdVector3     m_localAabbMax;
+       
+       SimdVector3     m_planeNormal;
+       SimdVector3     m_localScaling;
+       SimdScalar m_planeConstant;
+
+public:
+       StaticPlaneShape(const SimdVector3& planeNormal,SimdScalar planeConstant);
+
+       virtual ~StaticPlaneShape();
+
+
+       virtual int     GetShapeType() const
+       {
+               return STATIC_PLANE_PROXYTYPE;
+       }
+
+       virtual void GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const;
+
+       virtual void    ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const;
+
+       virtual void    CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia);
+
+       virtual void    setLocalScaling(const SimdVector3& scaling);
+       virtual const SimdVector3& getLocalScaling() const;
+       
+
+       //debugging
+       virtual char*   GetName()const {return "STATICPLANE";}
+
+
+};
+
+#endif //STATIC_PLANE_SHAPE_H
index ebbb7c3e5710453c7759e2ea532d54d66c8db401..d01d87d3f5dd9e3344edd2c6d3c7313d07c076bd 100644 (file)
@@ -15,39 +15,51 @@ subject to the following restrictions:
 
 #include "TriangleIndexVertexArray.h"
 
-TriangleIndexVertexArray::TriangleIndexVertexArray(int numTriangleIndices,int* triangleIndexBase,int triangleIndexStride,int numVertices,float* vertexBase,int vertexStride)
-:m_numTriangleIndices(numTriangleIndices),
-m_triangleIndexBase(triangleIndexBase),
-m_triangleIndexStride(triangleIndexStride),
-m_numVertices(numVertices),
-m_vertexBase(vertexBase),
-m_vertexStride(vertexStride)
+TriangleIndexVertexArray::TriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,float* vertexBase,int vertexStride)
 {
+       IndexedMesh mesh;
+       
+       mesh.m_numTriangles = numTriangles;
+       mesh.m_triangleIndexBase = triangleIndexBase;
+       mesh.m_triangleIndexStride = triangleIndexStride;
+       mesh.m_numVertices = numVertices;
+       mesh.m_vertexBase = vertexBase;
+       mesh.m_vertexStride = vertexStride;
+       
+       AddIndexedMesh(mesh);
+
 }
 
 void   TriangleIndexVertexArray::getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart)
 {
-       numverts = m_numVertices;
-       (*vertexbase) = (unsigned char *)m_vertexBase;
+       ASSERT(subpart< getNumSubParts() );
+       
+       IndexedMesh& mesh = m_indexedMeshes[subpart];
+
+       numverts = mesh.m_numVertices;
+       (*vertexbase) = (unsigned char *) mesh.m_vertexBase;
        type = PHY_FLOAT;
-       vertexStride = m_vertexStride;
+       vertexStride = mesh.m_vertexStride;
 
-       numfaces = m_numTriangleIndices;
-       (*indexbase) = (unsigned char *)m_triangleIndexBase;
-       indexstride = m_triangleIndexStride;
+       numfaces = mesh.m_numTriangles;
+
+       (*indexbase) = (unsigned char *)mesh.m_triangleIndexBase;
+       indexstride = mesh.m_triangleIndexStride;
        indicestype = PHY_INTEGER;
 }
 
 void   TriangleIndexVertexArray::getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) const
 {
-       numverts = m_numVertices;
-       (*vertexbase) = (unsigned char *)m_vertexBase;
+       const IndexedMesh& mesh = m_indexedMeshes[subpart];
+
+       numverts = mesh.m_numVertices;
+       (*vertexbase) = (const unsigned char *)mesh.m_vertexBase;
        type = PHY_FLOAT;
-       vertexStride = m_vertexStride;
+       vertexStride = mesh.m_vertexStride;
 
-       numfaces = m_numTriangleIndices;
-       (*indexbase) = (unsigned char *)m_triangleIndexBase;
-       indexstride = m_triangleIndexStride;
+       numfaces = mesh.m_numTriangles;
+       (*indexbase) = (const unsigned char *)mesh.m_triangleIndexBase;
+       indexstride = mesh.m_triangleIndexStride;
        indicestype = PHY_INTEGER;
 }
 
index 1134521ad87763828fbca026e99db972cb53c031..a44ef4514d503e9ff7d8cdc51c43460e2ad03acf 100644 (file)
@@ -14,22 +14,47 @@ subject to the following restrictions:
 */
 
 #include "StridingMeshInterface.h"
+#include <vector>
 
+///IndexedMesh indexes into existing vertex and index arrays, in a similar way OpenGL glDrawElements
+///instead of the number of indices, we pass the number of triangles
+///todo: explain with pictures
+struct IndexedMesh
+       {
+               int                     m_numTriangles;
+               int*            m_triangleIndexBase;
+               int                     m_triangleIndexStride;
+               int                     m_numVertices;
+               float*          m_vertexBase;
+               int                     m_vertexStride;
+       };
 
+///TriangleIndexVertexArray allows to use multiple meshes, by indexing into existing triangle/index arrays.
+///Additional meshes can be added using AddIndexedMesh
+///No duplcate is made of the vertex/index data, it only indexes into external vertex/index arrays.
+///So keep those arrays around during the lifetime of this TriangleIndexVertexArray.
 class TriangleIndexVertexArray : public StridingMeshInterface
 {
+       std::vector<IndexedMesh>        m_indexedMeshes;
 
-       int                     m_numTriangleIndices;
-       int*            m_triangleIndexBase;
-       int                     m_triangleIndexStride;
-       int                     m_numVertices;
-       float*          m_vertexBase;
-       int                     m_vertexStride;
-       
+               
 public:
 
+       
+
+       TriangleIndexVertexArray()
+       {
+       }
+
+       //just to be backwards compatible
        TriangleIndexVertexArray(int numTriangleIndices,int* triangleIndexBase,int triangleIndexStride,int numVertices,float* vertexBase,int vertexStride);
        
+       void    AddIndexedMesh(const IndexedMesh& mesh)
+       {
+               m_indexedMeshes.push_back(mesh);
+       }
+       
+       
        virtual void    getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0);
 
        virtual void    getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0) const;
@@ -42,7 +67,9 @@ public:
 
        /// getNumSubParts returns the number of seperate subparts
        /// each subpart has a continuous array of vertices and indices
-       virtual int             getNumSubParts() const { return 1;}
+       virtual int             getNumSubParts() const { 
+               return (int)m_indexedMeshes.size();
+       }
        
        virtual void    preallocateVertices(int numverts){}
        virtual void    preallocateIndices(int numindices){}
index 73b53cd02666e2f88efcc57f02142e9db2364240..f00ee280f11848a187454110fc98013370536ecc 100644 (file)
@@ -23,8 +23,7 @@ subject to the following restrictions:
 #include "stdio.h"
 
 TriangleMeshShape::TriangleMeshShape(StridingMeshInterface* meshInterface)
-: m_meshInterface(meshInterface),
-m_collisionMargin(CONVEX_DISTANCE_MARGIN)
+: m_meshInterface(meshInterface)
 {
        RecalcLocalAabb();
 }
index ccbb4d75f6185203717ff054f1d538ab4eb6fcbd..a6ee965da0f2944c22401f3034011edb7d17d02a 100644 (file)
@@ -16,30 +16,24 @@ subject to the following restrictions:
 #ifndef TRIANGLE_MESH_SHAPE_H
 #define TRIANGLE_MESH_SHAPE_H
 
-#include "CollisionShapes/CollisionShape.h"
-#include "BroadphaseCollision/BroadphaseProxy.h" // for the types
-
-#include "StridingMeshInterface.h"
-#include "TriangleCallback.h"
-
+#include "CollisionShapes/ConcaveShape.h"
+#include "CollisionShapes/StridingMeshInterface.h"
 
 
 ///Concave triangle mesh. Uses an interface to access the triangles to allow for sharing graphics/physics triangles.
-class TriangleMeshShape : public CollisionShape
+class TriangleMeshShape : public ConcaveShape
 {
 protected:
        StridingMeshInterface* m_meshInterface;
        SimdVector3     m_localAabbMin;
        SimdVector3     m_localAabbMax;
-       float m_collisionMargin;
+       
 
 public:
        TriangleMeshShape(StridingMeshInterface* meshInterface);
 
        virtual ~TriangleMeshShape();
 
-
-       
        virtual SimdVector3 LocalGetSupportingVertex(const SimdVector3& vec) const;
 
        virtual SimdVector3     LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const
@@ -68,16 +62,6 @@ public:
        //debugging
        virtual char*   GetName()const {return "TRIANGLEMESH";}
 
-       
-       virtual float GetMargin() const {
-               return m_collisionMargin;
-       }
-       virtual void SetMargin(float collisionMargin)
-       {
-               m_collisionMargin = collisionMargin;
-       }
-
-
 
 };
 
index 3d6fdb2be7ca8bed6d8a32605ab4dd0dd8f0f4a8..da5ecc2d78ba0c093dd26a3e60dca850bf537412 100644 (file)
@@ -34,6 +34,9 @@ struct DiscreteCollisionDetectorInterface
                void operator delete(void* ptr) {};
        
                virtual ~Result(){}     
+
+               ///SetShapeIdentifiers provides experimental support for per-triangle material / custom material combiner
+               virtual void SetShapeIdentifiers(int partId0,int index0,        int partId1,int index1)=0;
                virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)=0;
        };
 
index f87e9ced8a74bad41be411fe9a26337d526cec44..2b494e4595402d40f54484d41f6c3e96da49a13d 100644 (file)
@@ -122,12 +122,13 @@ bool      GjkConvexCast::calcTimeOfImpact(
                        n = x - c;
                        SimdScalar nDotr = n.dot(r);
 
-                       if (nDotr >= 0.f)
+                       if (nDotr >= -(SIMD_EPSILON*SIMD_EPSILON))
                                return false;
-
+                       
                        lambda = lambda - n.dot(n) / nDotr;
                        if (lambda <= lastLambda)
                                break;
+
                        lastLambda = lambda;
 
                        x = s + lambda * r;
index dcb96c21f0fcac09a3e742a79417ce31e742eaa1..0887ba7190144c9b5cecca1405980dc024e15d0c 100644 (file)
@@ -18,14 +18,16 @@ subject to the following restrictions:
 #include "NarrowPhaseCollision/SimplexSolverInterface.h"
 #include "NarrowPhaseCollision/ConvexPenetrationDepthSolver.h"
 
+#if defined(DEBUG) || defined (_DEBUG)
+#include <stdio.h> //for debug printf
+#endif
+
 static const SimdScalar rel_error = SimdScalar(1.0e-5);
 SimdScalar rel_error2 = rel_error * rel_error;
 float maxdist2 = 1.e30f;
-#include <stdio.h>
 
 
 int gGjkMaxIter=1000;
-bool gIrregularCatch = true;
 
 GjkPairDetector::GjkPairDetector(ConvexShape* objectA,ConvexShape* objectB,SimplexSolverInterface* simplexSolver,ConvexPenetrationDepthSolver* penetrationDepthSolver)
 :m_cachedSeparatingAxis(0.f,0.f,1.f),
@@ -33,7 +35,11 @@ m_penetrationDepthSolver(penetrationDepthSolver),
 m_simplexSolver(simplexSolver),
 m_minkowskiA(objectA),
 m_minkowskiB(objectB),
-m_ignoreMargin(false)
+m_ignoreMargin(false),
+m_partId0(-1),
+m_index0(-1),
+m_partId1(-1),
+m_index1(-1)
 {
 }
 
@@ -71,7 +77,22 @@ int curIter = 0;
                
                while (true)
                {
-               
+                       //rare failure case, perhaps deferate shapes?
+                       if (curIter++ > gGjkMaxIter)
+                       {
+                               #if defined(DEBUG) || defined (_DEBUG)
+                                       printf("GjkPairDetector maxIter exceeded:%i\n",curIter);
+                                       printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
+                                       m_cachedSeparatingAxis.getX(),
+                                       m_cachedSeparatingAxis.getY(),
+                                       m_cachedSeparatingAxis.getZ(),
+                                       squaredDistance,
+                                       m_minkowskiA->GetShapeType(),
+                                       m_minkowskiB->GetShapeType());
+                               #endif
+                               break;
+
+                       }
 
                        SimdVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
                        SimdVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
@@ -134,39 +155,6 @@ int curIter = 0;
                                m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
                                break;
                        }
-
-                               //rare failure case, perhaps deferate shapes?
-                       if (curIter++ > gGjkMaxIter)
-                       {
-
-#define CATCH_ME 1
-#ifdef CATCH_ME
-//this should not happen, we need to catch it
-//#if defined(DEBUG) || defined (_DEBUG)
-                               if (gIrregularCatch)
-                               {
-                                       gIrregularCatch = false;
-
-                                       printf("Problem with collision geometry\n");
-                                               printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
-                                               m_cachedSeparatingAxis.getX(),
-                                               m_cachedSeparatingAxis.getY(),
-                                               m_cachedSeparatingAxis.getZ(),
-                                               squaredDistance,
-                                               m_minkowskiA->GetShapeType(),
-                                               m_minkowskiB->GetShapeType());
-
-                                       printf("If you can reproduce this, please email bugs@continuousphysics.com\n");
-                                       printf("Please include above information, your Platform, version of OS.\n");
-                                       printf("Thanks.\n");
-                               }
-//#endif
-#endif //CATCH_ME
-
-                               break;
-
-                       }
-
                }
 
                if (checkSimplex)
@@ -223,6 +211,8 @@ int curIter = 0;
 
        if (isValid)
        {
+               output.SetShapeIdentifiers(m_partId0,m_index0,m_partId1,m_index1);
+
                output.AddContactPoint(
                        normalInB,
                        pointOnB,
index c550728f845571188f54d5b46fc1ce06489dab9a..5eee81e82c690a832dc48d67610a60e4d80404dc 100644 (file)
@@ -39,9 +39,17 @@ class GjkPairDetector : public DiscreteCollisionDetectorInterface
        ConvexShape* m_minkowskiA;
        ConvexShape* m_minkowskiB;
        bool            m_ignoreMargin;
+       
 
 public:
 
+       //experimental feature information, per triangle, per convex etc.
+       //'material combiner' / contact added callback
+       int     m_partId0;
+       int     m_index0;
+       int     m_partId1;
+       int     m_index1;
+
        GjkPairDetector(ConvexShape* objectA,ConvexShape* objectB,SimplexSolverInterface* simplexSolver,ConvexPenetrationDepthSolver*   penetrationDepthSolver);
        virtual ~GjkPairDetector() {};
 
index 35ebc4a40c613be2983665fe75fea6692993b151..250d9a9a8b4ca168db57a18a7872be1c6bee31fe 100644 (file)
@@ -40,6 +40,8 @@ class ManifoldPoint
                                        m_localPointB( pointB ), 
                                        m_normalWorldOnB( normal ), 
                                        m_distance1( distance ),
+                                       m_combinedFriction(0.f),
+                                       m_combinedRestitution(0.f),
                                        m_userPersistentData(0),                                        
                                        m_lifeTime(0)
                        {
@@ -57,6 +59,8 @@ class ManifoldPoint
                        SimdVector3 m_normalWorldOnB;
                
                        float   m_distance1;
+                       float   m_combinedFriction;
+                       float   m_combinedRestitution;
 
                                
                        void*   m_userPersistentData;
index a5dabd4232d1ba4a997232eafbc6fd003be67dff..70b856c32a90c5a26be542d871d6179280811e39 100644 (file)
@@ -32,6 +32,9 @@ struct MyResult : public DiscreteCollisionDetectorInterface::Result
        float m_depth;
        bool    m_hasResult;
 
+       virtual void SetShapeIdentifiers(int partId0,int index0,        int partId1,int index1)
+       {
+       }
        void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
        {
                m_normalOnBInWorld = normalOnBInWorld;
index df72b9a0417180a53be87a0707896ae27b87ade3..457662aa550cf7b6c284787af6477601d9f55c06 100644 (file)
@@ -19,7 +19,8 @@ subject to the following restrictions:
 #include <assert.h>
 
 float                                          gContactBreakingTreshold = 0.02f;
-ContactDestroyedCallback       gContactCallback = 0;
+ContactDestroyedCallback       gContactDestroyedCallback = 0;
+
 
 
 PersistentManifold::PersistentManifold()
@@ -75,9 +76,9 @@ void PersistentManifold::ClearUserCache(ManifoldPoint& pt)
                assert(occurance<=0);
 #endif //DEBUG_PERSISTENCY
 
-               if (pt.m_userPersistentData && gContactCallback)
+               if (pt.m_userPersistentData && gContactDestroyedCallback)
                {
-                       (*gContactCallback)(pt.m_userPersistentData);
+                       (*gContactDestroyedCallback)(pt.m_userPersistentData);
                        pt.m_userPersistentData = 0;
                }
                
index 98bef621f60ab936b80182eed0bb7e2ebcd0463a..53895be9adebf8e30c45a32dfd8dcf2589a044c7 100644 (file)
@@ -27,14 +27,15 @@ struct CollisionResult;
 extern float gContactBreakingTreshold;
 
 typedef bool (*ContactDestroyedCallback)(void* userPersistentData);
+extern ContactDestroyedCallback        gContactDestroyedCallback;
 
-extern ContactDestroyedCallback        gContactCallback;
 
 
 
 #define MANIFOLD_CACHE_SIZE 4
 
-///PersistentManifold maintains contact points, and reduces them to 4
+///PersistentManifold maintains contact points, and reduces them to 4.
+///It does contact filtering/contact reduction.
 class PersistentManifold 
 {
 
index a8e6324600e7ae42bc8817fea78a5fd5c329ef37..9d1daf01c46fecbd36d80f74bab73deda09baf46 100644 (file)
@@ -35,6 +35,11 @@ struct PointCollector : public DiscreteCollisionDetectorInterface::Result
        {
        }
 
+       virtual void SetShapeIdentifiers(int partId0,int index0,        int partId1,int index1)
+       {
+               //??
+       }
+
        virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
        {
                if (depth< m_distance)
index 8cefc900d290453b50ced16c699970aa2d88cd70..6a15d8e4f0c2fd6697fd688e8d6824bacdef8975 100644 (file)
@@ -26,8 +26,9 @@ subject to the following restrictions:
 #include "VoronoiSimplexSolver.h"
 #define SimplexSolverInterface VoronoiSimplexSolver
 #else
-/// for simplices from 1 to 4 vertices
-/// for example Johnson-algorithm or alternative approaches based on
+
+/// SimplexSolverInterface can incrementally calculate distance between origin and up to 4 vertices
+/// Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on
 /// voronoi regions or barycentric coordinates
 class SimplexSolverInterface
 {
index 019b4c3bfcb0a15f3759f7adbf8b016d9f0c94f8..a913261988a741f43a157710e38bbfed6055fd66 100644 (file)
@@ -37,7 +37,7 @@ bool  SubsimplexConvexCast::calcTimeOfImpact(
                CastResult& result)
 {
 
-               MinkowskiSumShape combi(m_convexA,m_convexB);
+       MinkowskiSumShape combi(m_convexA,m_convexB);
        MinkowskiSumShape* convex = &combi;
 
        SimdTransform   rayFromLocalA;
@@ -92,7 +92,7 @@ bool  SubsimplexConvexCast::calcTimeOfImpact(
                {
                        VdotR = v.dot(r);
 
-                       if (VdotR >= 0.f)
+                       if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON))
                                return false;
                        else
                        {
index 9c05571a92732dcec92fc993ad29be456865a2eb..9fc58e133df009f72dd9ab06a8f622508f86ba60 100644 (file)
@@ -22,7 +22,9 @@ subject to the following restrictions:
 class ConvexShape;
 
 /// SubsimplexConvexCast implements Gino van den Bergens' paper
+///"Ray Casting against General Convex Objects with Application to Continuous Collision Detection"
 /// GJK based Ray Cast, optimized version
+/// Objects should not start in overlap, otherwise results are not defined.
 class SubsimplexConvexCast : public ConvexCast
 {
        SimplexSolverInterface* m_simplexSolver;
@@ -34,7 +36,8 @@ public:
        SubsimplexConvexCast (ConvexShape*      shapeA,ConvexShape*     shapeB,SimplexSolverInterface* simplexSolver);
 
        //virtual ~SubsimplexConvexCast();
-       
+       ///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
+       ///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using GjkPairDetector.
        virtual bool    calcTimeOfImpact(
                        const SimdTransform& fromA,
                        const SimdTransform& toA,
index 1b97934aa1769fc3c7211d3cdde3678c7f96da20..7b72784e721b4518e6230e59d06cbeef2dbe6137 100644 (file)
@@ -33,18 +33,7 @@ SimdScalar contactTau = .02f;//0.02f;//*0.02f;
 
 
 
-SimdScalar     calculateCombinedFriction(RigidBody& body0,RigidBody& body1)
-{
-       SimdScalar friction = body0.getFriction() * body1.getFriction();
-
-       const SimdScalar MAX_FRICTION  = 10.f;
-       if (friction < -MAX_FRICTION)
-               friction = -MAX_FRICTION;
-       if (friction > MAX_FRICTION)
-               friction = MAX_FRICTION;
-       return friction;
 
-}
 
 
 //bilateral constraint between two dynamic objects
@@ -128,26 +117,23 @@ float resolveSingleCollision(
        SimdScalar rel_vel;
        rel_vel = normal.dot(vel);
        
-       float combinedRestitution = body1.getRestitution() * body2.getRestitution();
-
-       
 
        SimdScalar Kfps = 1.f / solverInfo.m_timeStep ;
 
        float damping = solverInfo.m_damping ;
-       float tau = solverInfo.m_tau;
+       float Kerp = solverInfo.m_erp;
        
        if (useGlobalSettingContacts)
        {
                damping = contactDamping;
-               tau = contactTau;
+               Kerp = contactTau;
        } 
 
-       float Kcor = tau *Kfps;
+       float Kcor = Kerp *Kfps;
 
        //printf("dist=%f\n",distance);
 
-       ConstraintPersistentData* cpd = (ConstraintPersistentData*) contactPoint.m_userPersistentData;
+               ConstraintPersistentData* cpd = (ConstraintPersistentData*) contactPoint.m_userPersistentData;
        assert(cpd);
 
        SimdScalar distance = cpd->m_penetration;//contactPoint.GetDistance();
@@ -156,7 +142,7 @@ float resolveSingleCollision(
        //distance = 0.f;
        SimdScalar positionalError = Kcor *-distance;
        //jacDiagABInv;
-       SimdScalar velocityError = cpd->m_restitution - rel_vel * damping;
+       SimdScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
 
        
        SimdScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
@@ -187,17 +173,19 @@ float resolveSingleFriction(
 
                )
 {
+
        const SimdVector3& pos1 = contactPoint.GetPositionWorldOnA();
        const SimdVector3& pos2 = contactPoint.GetPositionWorldOnB();
        const SimdVector3& normal = contactPoint.m_normalWorldOnB;
 
        SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
        SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
-       float combinedFriction = calculateCombinedFriction(body1,body2);
-
+       
        ConstraintPersistentData* cpd = (ConstraintPersistentData*) contactPoint.m_userPersistentData;
        assert(cpd);
 
+       float combinedFriction = cpd->m_friction;
+       
        SimdScalar limit = cpd->m_appliedImpulse * combinedFriction;
        //if (contactPoint.m_appliedImpulse>0.f)
        //friction
index 847f2881f41128172d702385309771891d68049a..8304d412d6fa94bdf20af99f5abceeeec677128a 100644 (file)
@@ -34,6 +34,7 @@ struct ConstraintPersistentData
        m_jacDiagABInv(0.f),
        m_persistentLifeTime(0),
        m_restitution(0.f),
+       m_friction(0.f),
        m_penetration(0.f)
        {
        }
@@ -50,6 +51,7 @@ struct ConstraintPersistentData
                        float   m_jacDiagABInvTangent1;
                        int             m_persistentLifeTime;
                        float   m_restitution;
+                       float   m_friction;
                        float   m_penetration;
                        SimdVector3     m_frictionWorldTangential0;
                        SimdVector3     m_frictionWorldTangential1;
index 6c6325df8a61f26a0fb44cc2954beb58bc87bd0d..2ec9b7f6ca1dd625c5e814cb56c8db9bfaee0595 100644 (file)
@@ -22,7 +22,7 @@ struct ContactSolverInfo
 
        inline ContactSolverInfo()
        {
-               m_tau = 0.4f;
+               m_tau = 0.6f;
                m_damping = 1.0f;
                m_friction = 0.3f;
                m_restitution = 0.f;
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp
new file mode 100644 (file)
index 0000000..9597f27
--- /dev/null
@@ -0,0 +1,250 @@
+/*
+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 "Generic6DofConstraint.h"
+#include "Dynamics/RigidBody.h"
+#include "Dynamics/MassProps.h"
+#include "SimdTransformUtil.h"
+
+static const SimdScalar kSign[] = { 1.0f, -1.0f, 1.0f };
+static const int kAxisA[] = { 1, 0, 0 };
+static const int kAxisB[] = { 2, 2, 1 };
+
+Generic6DofConstraint::Generic6DofConstraint()
+{
+}
+
+Generic6DofConstraint::Generic6DofConstraint(RigidBody& rbA, RigidBody& rbB, const SimdTransform& frameInA, const SimdTransform& frameInB)
+: TypedConstraint(rbA, rbB)
+, m_frameInA(frameInA)
+, m_frameInB(frameInB)
+{
+       //free means upper < lower, 
+       //locked means upper == lower
+       //limited means upper > lower
+       //so start all locked
+       for (int i=0; i<6;++i)
+       {
+               m_lowerLimit[i] = 0.0f;
+               m_upperLimit[i] = 0.0f;
+               m_accumulatedImpulse[i] = 0.0f;
+       }
+
+}
+
+
+void Generic6DofConstraint::BuildJacobian()
+{
+       SimdVector3     normal(0,0,0);
+
+       const SimdVector3& pivotInA = m_frameInA.getOrigin();
+       const SimdVector3& pivotInB = m_frameInB.getOrigin();
+
+       SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin();
+       SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin();
+
+       SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+       SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+
+       int i;
+       //linear part
+       for (i=0;i<3;i++)
+       {
+               if (isLimited(i))
+               {
+                       normal[i] = 1;
+                       
+                       // Create linear atom
+                       new (&m_jacLinear[i]) JacobianEntry(
+                               m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+                               m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+                               m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(),
+                               m_rbB.getCenterOfMassTransform()*pivotInB - m_rbB.getCenterOfMassPosition(),
+                               normal,
+                               m_rbA.getInvInertiaDiagLocal(),
+                               m_rbA.getInvMass(),
+                               m_rbB.getInvInertiaDiagLocal(),
+                               m_rbB.getInvMass());
+
+                       // Apply accumulated impulse
+                       SimdVector3 impulse_vector = m_accumulatedImpulse[i] * normal;
+
+                       m_rbA.applyImpulse( impulse_vector, rel_pos1);
+                       m_rbB.applyImpulse(-impulse_vector, rel_pos2);
+
+                       normal[i] = 0;
+               }
+       }
+
+       // angular part
+       for (i=0;i<3;i++)
+       {
+               if (isLimited(i+3))
+               {
+                       SimdVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] );
+                       SimdVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] );
+
+                       // Dirk: This is IMO mathematically the correct way, but we should consider axisA and axisB being near parallel maybe
+                       SimdVector3 axis = kSign[i] * axisA.cross(axisB);
+
+                       // Create angular atom
+                       new (&m_jacAng[i])      JacobianEntry(axis,
+                               m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+                               m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+                               m_rbA.getInvInertiaDiagLocal(),
+                               m_rbB.getInvInertiaDiagLocal());
+
+                       // Apply accumulated impulse
+                       SimdVector3 impulse_vector = m_accumulatedImpulse[i + 3] * axis;
+
+                       m_rbA.applyTorqueImpulse( impulse_vector);
+                       m_rbB.applyTorqueImpulse(-impulse_vector);
+               }
+       }
+}
+
+void   Generic6DofConstraint::SolveConstraint(SimdScalar       timeStep)
+{
+       SimdScalar tau = 0.1f;
+       SimdScalar damping = 1.0f;
+
+       SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin();
+       SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin();
+
+       SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+       SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+       
+       SimdVector3 normal(0,0,0);
+       int i;
+
+       // linear
+       for (i=0;i<3;i++)
+       {               
+               if (isLimited(i))
+               {
+                       SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
+                       SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
+               
+
+                       normal[i] = 1;
+                       SimdScalar jacDiagABInv = 1.f / m_jacLinear[i].getDiagonal();
+
+                       //velocity error (first order error)
+                       SimdScalar rel_vel = m_jacLinear[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
+                                                                                                                                       m_rbB.getLinearVelocity(),angvelB);
+               
+                       //positional error (zeroth order error)
+                       SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); 
+                       
+                       SimdScalar impulse = (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv;
+                       m_accumulatedImpulse[i] += impulse;
+
+                       SimdVector3 impulse_vector = normal * impulse;
+                       m_rbA.applyImpulse( impulse_vector, rel_pos1);
+                       m_rbB.applyImpulse(-impulse_vector, rel_pos2);
+                       
+                       normal[i] = 0;
+               }
+       }
+
+       // angular
+       for (i=0;i<3;i++)
+       {
+               if (isLimited(i+3))
+               {
+                       SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
+                       SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
+               
+                       SimdScalar jacDiagABInv = 1.f / m_jacAng[i].getDiagonal();
+                       
+                       //velocity error (first order error)
+                       SimdScalar rel_vel = m_jacAng[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
+                                                                                                                                                       m_rbB.getLinearVelocity(),angvelB);
+
+                       //positional error (zeroth order error)
+                       SimdVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] );
+                       SimdVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] );
+
+                       SimdScalar rel_pos = kSign[i] * axisA.dot(axisB);
+
+                       //impulse
+                       SimdScalar impulse = -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv;
+                       m_accumulatedImpulse[i + 3] += impulse;
+                       
+                       // Dirk: Not needed - we could actually project onto Jacobian entry here (same as above)
+                       SimdVector3 axis = kSign[i] * axisA.cross(axisB);
+                       SimdVector3 impulse_vector = axis * impulse;
+
+                       m_rbA.applyTorqueImpulse( impulse_vector);
+                       m_rbB.applyTorqueImpulse(-impulse_vector);
+               }
+       }
+}
+
+void   Generic6DofConstraint::UpdateRHS(SimdScalar     timeStep)
+{
+
+}
+
+SimdScalar Generic6DofConstraint::ComputeAngle(int axis) const
+       {
+       SimdScalar angle;
+
+       switch (axis)
+               {
+               case 0:
+                       {
+                       SimdVector3 v1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(1);
+                       SimdVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1);
+                       SimdVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2);
+
+                       SimdScalar s = v1.dot(w2);
+                       SimdScalar c = v1.dot(v2);
+
+                       angle = SimdAtan2( s, c );
+                       }
+                       break;
+
+               case 1:
+                       {
+                       SimdVector3 w1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(2);
+                       SimdVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2);
+                       SimdVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0);
+
+                       SimdScalar s = w1.dot(u2);
+                       SimdScalar c = w1.dot(w2);
+
+                       angle = SimdAtan2( s, c );
+                       }
+                       break;
+
+               case 2:
+                       {
+                       SimdVector3 u1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(0);
+                       SimdVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0);
+                       SimdVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1);
+
+                       SimdScalar s = u1.dot(v2);
+                       SimdScalar c = u1.dot(u2);
+
+                       angle = SimdAtan2( s, c );
+                       }
+                       break;
+               }
+
+       return angle;
+       }
+
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/Generic6DofConstraint.h b/extern/bullet/BulletDynamics/ConstraintSolver/Generic6DofConstraint.h
new file mode 100644 (file)
index 0000000..7a77253
--- /dev/null
@@ -0,0 +1,114 @@
+/*
+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 GENERIC_6DOF_CONSTRAINT_H
+#define GENERIC_6DOF_CONSTRAINT_H
+
+#include "SimdVector3.h"
+
+#include "ConstraintSolver/JacobianEntry.h"
+#include "TypedConstraint.h"
+
+class RigidBody;
+
+
+
+/// Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
+/// Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'
+/// Work in progress (is still a Hinge actually)
+class Generic6DofConstraint : public TypedConstraint
+{
+       JacobianEntry   m_jacLinear[3];                 // 3 orthogonal linear constraints
+       JacobianEntry   m_jacAng[3];            // 3 orthogonal angular constraints
+
+       SimdTransform   m_frameInA;                     // the constraint space w.r.t body A
+       SimdTransform   m_frameInB;                     // the constraint space w.r.t body B
+
+       SimdScalar      m_lowerLimit[6];        // the constraint lower limits
+       SimdScalar      m_upperLimit[6];        // the constraint upper limits
+
+       SimdScalar              m_accumulatedImpulse[6];
+
+               
+public:
+       Generic6DofConstraint(RigidBody& rbA, RigidBody& rbB, const SimdTransform& frameInA, const SimdTransform& frameInB );
+
+       Generic6DofConstraint();
+
+       virtual void    BuildJacobian();
+
+       virtual void    SolveConstraint(SimdScalar      timeStep);
+
+       void    UpdateRHS(SimdScalar    timeStep);
+
+       SimdScalar ComputeAngle(int axis) const;
+
+       void    setLinearLowerLimit(const SimdVector3& linearLower)
+       {
+               m_lowerLimit[0] = linearLower.getX();
+               m_lowerLimit[1] = linearLower.getY();
+               m_lowerLimit[2] = linearLower.getZ();
+       }
+
+       void    setLinearUpperLimit(const SimdVector3& linearUpper)
+       {
+               m_upperLimit[0] = linearUpper.getX();
+               m_upperLimit[1] = linearUpper.getY();
+               m_upperLimit[2] = linearUpper.getZ();
+       }
+
+       void    setAngularLowerLimit(const SimdVector3& angularLower)
+       {
+               m_lowerLimit[3] = angularLower.getX();
+               m_lowerLimit[4] = angularLower.getY();
+               m_lowerLimit[5] = angularLower.getZ();
+       }
+
+       void    setAngularUpperLimit(const SimdVector3& angularUpper)
+       {
+               m_upperLimit[3] = angularUpper.getX();
+               m_upperLimit[4] = angularUpper.getY();
+               m_upperLimit[5] = angularUpper.getZ();
+       }
+
+       //first 3 are linear, next 3 are angular
+       void SetLimit(int axis, SimdScalar lo, SimdScalar hi)
+       {
+               m_lowerLimit[axis] = lo; 
+               m_upperLimit[axis] = hi; 
+       }
+
+       //free means upper < lower, 
+       //locked means upper == lower
+       //limited means upper > lower
+       //limitIndex: first 3 are linear, next 3 are angular
+       bool    isLimited(int limitIndex)
+       {
+               return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
+       }
+
+       const RigidBody& GetRigidBodyA() const
+       {
+               return m_rbA;
+       }
+       const RigidBody& GetRigidBodyB() const
+       {
+               return m_rbB;
+       }
+       
+
+};
+
+#endif //GENERIC_6DOF_CONSTRAINT_H
index 0e5c7cf5715b7876f3dc394da82e68ca8f3b8254..fb7f1f47cd7bea406002f7d16975fc1fee79f0a6 100644 (file)
@@ -28,7 +28,7 @@ HingeConstraint::HingeConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector
                                                                 SimdVector3& axisInA,SimdVector3& axisInB)
 :TypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
 m_axisInA(axisInA),
-m_axisInB(axisInB),
+m_axisInB(-axisInB),
 m_angularOnly(false)
 {
 
@@ -73,7 +73,7 @@ void  HingeConstraint::BuildJacobian()
        //calculate two perpendicular jointAxis, orthogonal to hingeAxis
        //these two jointAxis require equal angular velocities for both bodies
 
-       //this is ununsed for now, it's a todo
+       //this is unused for now, it's a todo
        SimdVector3 axisWorldA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
        SimdVector3 jointAxis0;
        SimdVector3 jointAxis1;
@@ -96,6 +96,102 @@ void        HingeConstraint::BuildJacobian()
 
 void   HingeConstraint::SolveConstraint(SimdScalar     timeStep)
 {
+//#define NEW_IMPLEMENTATION
+
+#ifdef NEW_IMPLEMENTATION
+       SimdScalar tau = 0.3f;
+       SimdScalar damping = 1.f;
+
+       SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
+       SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
+
+       // Dirk: Don't we need to update this after each applied impulse
+       SimdVector3 angvelA; // = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
+    SimdVector3 angvelB; // = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
+
+
+       if (!m_angularOnly)
+       {
+               SimdVector3 normal(0,0,0);
+
+               for (int i=0;i<3;i++)
+               {               
+                       normal[i] = 1;
+                       SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
+
+                       SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+                       SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+                       
+                       SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
+                       SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
+                       SimdVector3 vel = vel1 - vel2;
+
+                       // Dirk: Get new angular velocity since it changed after applying an impulse
+                       angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
+                       angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
+       
+                       //velocity error (first order error)
+                       SimdScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
+                                                                                                                                       m_rbB.getLinearVelocity(),angvelB);
+               
+                       //positional error (zeroth order error)
+                       SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); 
+                       
+                       SimdScalar impulse = tau*depth/timeStep * jacDiagABInv -  damping * rel_vel * jacDiagABInv;
+
+                       SimdVector3 impulse_vector = normal * impulse;
+                       m_rbA.applyImpulse( impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
+                       m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
+                       
+                       normal[i] = 0;
+               }
+       }
+
+       ///solve angular part
+
+       // get axes in world space
+       SimdVector3 axisA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
+       SimdVector3 axisB = GetRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
+
+       // constraint axes in world space
+       SimdVector3 jointAxis0;
+       SimdVector3 jointAxis1;
+       SimdPlaneSpace1(axisA,jointAxis0,jointAxis1);
+
+
+       // Dirk: Get new angular velocity since it changed after applying an impulse
+       angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
+    angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
+       
+       SimdScalar jacDiagABInv0 = 1.f / m_jacAng[0].getDiagonal();
+       SimdScalar rel_vel0 = m_jacAng[0].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
+                                                                                                                                       m_rbB.getLinearVelocity(),angvelB);
+       float tau1 = tau;//0.f;
+
+       SimdScalar impulse0 = (tau1 * axisB.dot(jointAxis1) / timeStep - damping * rel_vel0) * jacDiagABInv0;
+       SimdVector3 angular_impulse0 = jointAxis0 * impulse0;
+
+       m_rbA.applyTorqueImpulse( angular_impulse0);
+       m_rbB.applyTorqueImpulse(-angular_impulse0);
+
+
+
+       // Dirk: Get new angular velocity since it changed after applying an impulse    
+       angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
+    angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
+
+       SimdScalar jacDiagABInv1 = 1.f / m_jacAng[1].getDiagonal();
+       SimdScalar rel_vel1 = m_jacAng[1].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
+                                                                                                                                       m_rbB.getLinearVelocity(),angvelB);;
+
+       SimdScalar impulse1 = -(tau1 * axisB.dot(jointAxis0) / timeStep + damping * rel_vel1) * jacDiagABInv1;
+       SimdVector3 angular_impulse1 = jointAxis1 * impulse1;
+
+       m_rbA.applyTorqueImpulse( angular_impulse1);
+       m_rbB.applyTorqueImpulse(-angular_impulse1);
+
+#else
+
 
        SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
        SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
@@ -104,7 +200,7 @@ void        HingeConstraint::SolveConstraint(SimdScalar     timeStep)
        SimdScalar tau = 0.3f;
        SimdScalar damping = 1.f;
 
-       if (!m_angularOnly)
+//linear part
        {
                for (int i=0;i<3;i++)
                {               
@@ -169,6 +265,8 @@ void        HingeConstraint::SolveConstraint(SimdScalar     timeStep)
        m_rbA.applyTorqueImpulse(-velrel+angularError);
        m_rbB.applyTorqueImpulse(velrel-angularError);
 
+#endif
+
 }
 
 void   HingeConstraint::UpdateRHS(SimdScalar   timeStep)
index 821c681a7bdc8c15497a5f263b118ff555f5ec7d..d1dd367a01296a893cc846661d19938733f25a61 100644 (file)
@@ -42,28 +42,48 @@ public:
                const SimdScalar massInvA,
                const SimdVector3& inertiaInvB,
                const SimdScalar massInvB)
-               :m_jointAxis(jointAxis)
+               :m_linearJointAxis(jointAxis)
        {
-               m_aJ = world2A*(rel_pos1.cross(m_jointAxis));
-               m_bJ = world2B*(rel_pos2.cross(-m_jointAxis));
+               m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis));
+               m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis));
                m_0MinvJt       = inertiaInvA * m_aJ;
                m_1MinvJt = inertiaInvB * m_bJ;
                m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
+
+               ASSERT(m_Adiag > 0.0f);
        }
 
-               //angular constraint between two different rigidbodies
+       //angular constraint between two different rigidbodies
        JacobianEntry(const SimdVector3& jointAxis,
                const SimdMatrix3x3& world2A,
                const SimdMatrix3x3& world2B,
                const SimdVector3& inertiaInvA,
                const SimdVector3& inertiaInvB)
-               :m_jointAxis(m_jointAxis)
+               :m_linearJointAxis(SimdVector3(0.f,0.f,0.f))
+       {
+               m_aJ= world2A*jointAxis;
+               m_bJ = world2B*-jointAxis;
+               m_0MinvJt       = inertiaInvA * m_aJ;
+               m_1MinvJt = inertiaInvB * m_bJ;
+               m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+
+               ASSERT(m_Adiag > 0.0f);
+       }
+
+       //angular constraint between two different rigidbodies
+       JacobianEntry(const SimdVector3& axisInA,
+               const SimdVector3& axisInB,
+               const SimdVector3& inertiaInvA,
+               const SimdVector3& inertiaInvB)
+               : m_linearJointAxis(SimdVector3(0.f,0.f,0.f))
+               , m_aJ(axisInA)
+               , m_bJ(-axisInB)
        {
-               m_aJ= world2A*m_jointAxis;
-               m_bJ = world2B*-m_jointAxis;
                m_0MinvJt       = inertiaInvA * m_aJ;
                m_1MinvJt = inertiaInvB * m_bJ;
                m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+
+               ASSERT(m_Adiag > 0.0f);
        }
 
        //constraint on one rigidbody
@@ -73,13 +93,15 @@ public:
                const SimdVector3& jointAxis,
                const SimdVector3& inertiaInvA, 
                const SimdScalar massInvA)
-               :m_jointAxis(jointAxis)
+               :m_linearJointAxis(jointAxis)
        {
-               m_aJ= world2A*(rel_pos1.cross(m_jointAxis));
-               m_bJ = world2A*(rel_pos2.cross(-m_jointAxis));
+               m_aJ= world2A*(rel_pos1.cross(jointAxis));
+               m_bJ = world2A*(rel_pos2.cross(-jointAxis));
                m_0MinvJt       = inertiaInvA * m_aJ;
                m_1MinvJt = SimdVector3(0.f,0.f,0.f);
                m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
+
+               ASSERT(m_Adiag > 0.0f);
        }
 
        SimdScalar      getDiagonal() const { return m_Adiag; }
@@ -88,7 +110,7 @@ public:
        SimdScalar      getNonDiagonal(const JacobianEntry& jacB, const SimdScalar massInvA) const
        {
                const JacobianEntry& jacA = *this;
-               SimdScalar lin = massInvA * jacA.m_jointAxis.dot(jacB.m_jointAxis);
+               SimdScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
                SimdScalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
                return lin + ang;
        }
@@ -99,7 +121,7 @@ public:
        SimdScalar      getNonDiagonal(const JacobianEntry& jacB,const SimdScalar massInvA,const SimdScalar massInvB) const
        {
                const JacobianEntry& jacA = *this;
-               SimdVector3 lin = jacA.m_jointAxis* jacB.m_jointAxis;
+               SimdVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
                SimdVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
                SimdVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
                SimdVector3 lin0 = massInvA * lin ;
@@ -113,7 +135,7 @@ public:
                SimdVector3 linrel = linvelA - linvelB;
                SimdVector3 angvela  = angvelA * m_aJ;
                SimdVector3 angvelb  = angvelB * m_bJ;
-               linrel *= m_jointAxis;
+               linrel *= m_linearJointAxis;
                angvela += angvelb;
                angvela += linrel;
                SimdScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
@@ -121,7 +143,7 @@ public:
        }
 //private:
 
-       SimdVector3     m_jointAxis;
+       SimdVector3     m_linearJointAxis;
        SimdVector3     m_aJ;
        SimdVector3     m_bJ;
        SimdVector3     m_0MinvJt;
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp
deleted file mode 100644 (file)
index 2d91b4a..0000000
+++ /dev/null
@@ -1,265 +0,0 @@
-/*
-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 "OdeConstraintSolver.h"
-
-#include "NarrowPhaseCollision/PersistentManifold.h"
-#include "Dynamics/RigidBody.h"
-#include "ContactConstraint.h"
-#include "Solve2LinearConstraint.h"
-#include "ContactSolverInfo.h"
-#include "Dynamics/BU_Joint.h"
-#include "Dynamics/ContactJoint.h"
-
-#include "IDebugDraw.h"
-
-#define USE_SOR_SOLVER
-
-#include "SorLcp.h"
-
-#include <math.h>
-#include <float.h>//FLT_MAX
-#ifdef WIN32
-#include <memory.h>
-#endif
-#include <string.h>
-#include <stdio.h>
-
-#if defined (WIN32)
-#include <malloc.h>
-#else
-#if defined (__FreeBSD__) || defined (__OpenBSD__)
-#include <stdlib.h>
-#else
-#include <alloca.h>
-#endif
-#endif
-
-class BU_Joint;
-
-//see below
-
-OdeConstraintSolver::OdeConstraintSolver():
-m_cfm(0.f),//1e-5f),
-m_erp(0.4f)
-{
-}
-
-
-
-
-
-
-
-//iterative lcp and penalty method
-float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer)
-{
-       m_CurBody = 0;
-       m_CurJoint = 0;
-
-
-       RigidBody* bodies [MAX_RIGIDBODIES];
-
-       int numBodies = 0;
-       BU_Joint* joints [MAX_RIGIDBODIES*4];
-       int numJoints = 0;
-
-       for (int j=0;j<numManifolds;j++)
-       {
-
-               int body0=-1,body1=-1;
-
-               PersistentManifold* manifold = manifoldPtr[j];
-               if (manifold->GetNumContacts() > 0)
-               {
-                       body0 = ConvertBody((RigidBody*)manifold->GetBody0(),bodies,numBodies);
-                       body1 = ConvertBody((RigidBody*)manifold->GetBody1(),bodies,numBodies);
-                       ConvertConstraint(manifold,joints,numJoints,bodies,body0,body1,debugDrawer);
-               }
-       }
-
-       SolveInternal1(m_cfm,m_erp,bodies,numBodies,joints,numJoints,infoGlobal);
-
-       return 0.f;
-
-}
-
-/////////////////////////////////////////////////////////////////////////////////
-
-
-typedef SimdScalar dQuaternion[4];
-#define _R(i,j) R[(i)*4+(j)]
-
-void dRfromQ1 (dMatrix3 R, const dQuaternion q)
-{
-  // q = (s,vx,vy,vz)
-  SimdScalar qq1 = 2.f*q[1]*q[1];
-  SimdScalar qq2 = 2.f*q[2]*q[2];
-  SimdScalar qq3 = 2.f*q[3]*q[3];
-  _R(0,0) = 1.f - qq2 - qq3;
-  _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]);
-  _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]);
-  _R(0,3) = 0.f;
-       
-  _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]);
-  _R(1,1) = 1.f - qq1 - qq3;
-  _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]);
-  _R(1,3) = 0.f;
-
-  _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]);
-  _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]);
-  _R(2,2) = 1.f - qq1 - qq2;
-  _R(2,3) = 0.f;
-
-}
-
-
-
-int OdeConstraintSolver::ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies)
-{
-       if (!body || (body->getInvMass() == 0.f) )
-       {
-               return -1;
-       }
-       //first try to find
-       int i,j;
-       for (i=0;i<numBodies;i++)
-       {
-               if (bodies[i] == body)
-                       return i;
-       }
-       //if not found, create a new body
-       bodies[numBodies++] = body;
-       //convert data
-
-
-       body->m_facc.setValue(0,0,0,0);
-       body->m_tacc.setValue(0,0,0,0);
-       
-       //are the indices the same ?
-       for (i=0;i<4;i++)
-       {
-               for ( j=0;j<3;j++)
-               {
-                       body->m_invI[i+4*j] = 0.f;
-                       body->m_I[i+4*j] = 0.f;
-               }
-       }
-       body->m_invI[0+4*0] =   body->getInvInertiaDiagLocal()[0];
-       body->m_invI[1+4*1] =   body->getInvInertiaDiagLocal()[1];
-       body->m_invI[2+4*2] =   body->getInvInertiaDiagLocal()[2];
-
-       body->m_I[0+0*4] = 1.f/body->getInvInertiaDiagLocal()[0];
-       body->m_I[1+1*4] = 1.f/body->getInvInertiaDiagLocal()[1];
-       body->m_I[2+2*4] = 1.f/body->getInvInertiaDiagLocal()[2];
-       
-
-       
-       
-       dQuaternion q;
-
-       q[1] = body->getOrientation()[0];
-       q[2] = body->getOrientation()[1];
-       q[3] = body->getOrientation()[2];
-       q[0] = body->getOrientation()[3];
-       
-       dRfromQ1(body->m_R,q);
-       
-       return numBodies-1;
-}
-
-
-
-
-       
-#define MAX_JOINTS_1 8192
-
-static ContactJoint gJointArray[MAX_JOINTS_1];
-
-
-void OdeConstraintSolver::ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints,
-                                          RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer)
-{
-
-
-       manifold->RefreshContactPoints(((RigidBody*)manifold->GetBody0())->getCenterOfMassTransform(),
-               ((RigidBody*)manifold->GetBody1())->getCenterOfMassTransform());
-
-       int bodyId0 = _bodyId0,bodyId1 = _bodyId1;
-
-       int i,numContacts = manifold->GetNumContacts();
-       
-       bool swapBodies = (bodyId0 < 0);
-
-       
-       RigidBody* body0,*body1;
-
-       if (swapBodies)
-       {
-               bodyId0 = _bodyId1;
-               bodyId1 = _bodyId0;
-
-               body0 = (RigidBody*)manifold->GetBody1();
-               body1 = (RigidBody*)manifold->GetBody0();
-
-       } else
-       {
-               body0 = (RigidBody*)manifold->GetBody0();
-               body1 = (RigidBody*)manifold->GetBody1();
-       }
-
-       assert(bodyId0 >= 0);
-
-       SimdVector3 color(0,1,0);
-       for (i=0;i<numContacts;i++)
-       {
-
-               if (debugDrawer)
-               {
-                       const ManifoldPoint& cp = manifold->GetContactPoint(i);
-
-                       debugDrawer->DrawContactPoint(
-                               cp.m_positionWorldOnB,
-                               cp.m_normalWorldOnB,
-                               cp.GetDistance(),
-                               cp.GetLifeTime(),
-                               color);
-
-               }
-               assert (m_CurJoint < MAX_JOINTS_1);
-
-//             if (manifold->GetContactPoint(i).GetDistance() < 0.0f)
-               {
-                       ContactJoint* cont = new (&gJointArray[m_CurJoint++]) ContactJoint( manifold ,i, swapBodies,body0,body1);
-
-                       cont->node[0].joint = cont;
-                       cont->node[0].body = bodyId0 >= 0 ? bodies[bodyId0] : 0;
-                       
-                       cont->node[1].joint = cont;
-                       cont->node[1].body = bodyId1 >= 0 ? bodies[bodyId1] : 0;
-                       
-                       joints[numJoints++] = cont;
-                       for (int i=0;i<6;i++)
-                               cont->lambda[i] = 0.f;
-
-                       cont->flags = 0;
-               }
-       }
-
-       //create a new contact constraint
-};
-
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.h b/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.h
deleted file mode 100644 (file)
index 9984055..0000000
+++ /dev/null
@@ -1,66 +0,0 @@
-/*
-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 ODE_CONSTRAINT_SOLVER_H
-#define ODE_CONSTRAINT_SOLVER_H
-
-#include "ConstraintSolver.h"
-
-class RigidBody;
-class BU_Joint;
-
-/// OdeConstraintSolver is one of the available solvers for Bullet dynamics framework
-/// It uses the the unmodified version of quickstep solver from the open dynamics project
-class OdeConstraintSolver : public ConstraintSolver
-{
-private:
-
-       int m_CurBody;
-       int m_CurJoint;
-
-       float   m_cfm;
-       float   m_erp;
-       
-
-       int ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies);
-       void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints,
-                                          RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer);
-
-public:
-
-       OdeConstraintSolver();
-
-       virtual ~OdeConstraintSolver() {}
-       
-       virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info,IDebugDraw* debugDrawer = 0);
-
-       ///setConstraintForceMixing, the cfm adds some positive value to the main diagonal
-       ///This can improve convergence (make matrix positive semidefinite), but it can make the simulation look more 'springy'
-       void    setConstraintForceMixing(float cfm) { 
-               m_cfm  = cfm;
-       }
-
-       ///setErrorReductionParamter sets the maximum amount of error reduction
-       ///which limits energy addition during penetration depth recovery
-       void    setErrorReductionParamter(float erp)
-       {
-               m_erp = erp;
-       }
-};
-
-
-
-
-#endif //ODE_CONSTRAINT_SOLVER_H
similarity index 91%
rename from extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp
rename to extern/bullet/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cpp
index fe59a40683a44a104241fdd4df326042c91e6b16..26637bb9932dc11600c8e8827e305eb7dd513b12 100644 (file)
@@ -14,7 +14,7 @@ subject to the following restrictions:
 */
 
 
-#include "SimpleConstraintSolver.h"
+#include "SequentialImpulseConstraintSolver.h"
 #include "NarrowPhaseCollision/PersistentManifold.h"
 #include "Dynamics/RigidBody.h"
 #include "ContactConstraint.h"
@@ -45,14 +45,14 @@ bool  MyContactDestroyedCallback(void* userPersistentData)
 }
 
 
-SimpleConstraintSolver::SimpleConstraintSolver()
+SequentialImpulseConstraintSolver::SequentialImpulseConstraintSolver()
 {
-       gContactCallback = &MyContactDestroyedCallback;
+       gContactDestroyedCallback = &MyContactDestroyedCallback;
 }
 
 
-/// SimpleConstraintSolver Sequentially applies impulses
-float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer)
+/// SequentialImpulseConstraintSolver Sequentially applies impulses
+float SequentialImpulseConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer)
 {
        
        ContactSolverInfo info = infoGlobal;
@@ -113,7 +113,7 @@ SimdScalar restitutionCurve(SimdScalar rel_vel, SimdScalar restitution)
 
 
 
-float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer)
+float SequentialImpulseConstraintSolver::Solve(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer)
 {
 
        RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0();
@@ -188,10 +188,10 @@ float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const Conta
                                SimdScalar rel_vel;
                                rel_vel = cp.m_normalWorldOnB.dot(vel);
                                
-                               float combinedRestitution = body0->getRestitution() * body1->getRestitution();
-
+                               float combinedRestitution = cp.m_combinedRestitution;
+                               
                                cpd->m_penetration = cp.GetDistance();
-
+                               cpd->m_friction = cp.m_combinedFriction;
                                cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
                                if (cpd->m_restitution <= 0.) //0.f)
                                {
@@ -294,7 +294,7 @@ float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const Conta
        return maxImpulse;
 }
 
-float SimpleConstraintSolver::SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer)
+float SequentialImpulseConstraintSolver::SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer)
 {
        RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0();
        RigidBody* body1 = (RigidBody*)manifoldPtr->GetBody1();
similarity index 81%
rename from extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h
rename to extern/bullet/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.h
index 99b77623b4b621cc178429ab0673f0d53d6ebfbf..598a4cfa903e4f723411d6fda533b9e2b08f38a3 100644 (file)
@@ -13,17 +13,17 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-#ifndef SIMPLE_CONSTRAINT_SOLVER_H
-#define SIMPLE_CONSTRAINT_SOLVER_H
+#ifndef SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
+#define SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
 
 #include "ConstraintSolver.h"
 class IDebugDraw;
 
-/// SimpleConstraintSolver uses a Propagation Method and Sequentially applies impulses
+/// SequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses
 /// The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com
 /// Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP)
 /// Applies impulses for combined restitution and penetration recovery and to simulate friction
-class SimpleConstraintSolver : public ConstraintSolver
+class SequentialImpulseConstraintSolver : public ConstraintSolver
 {
        float Solve(PersistentManifold* manifold, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer);
        float SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer);
@@ -31,13 +31,13 @@ class SimpleConstraintSolver : public ConstraintSolver
 
 public:
 
-       SimpleConstraintSolver();
+       SequentialImpulseConstraintSolver();
 
-       virtual ~SimpleConstraintSolver() {}
+       virtual ~SequentialImpulseConstraintSolver() {}
        
        virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info, IDebugDraw* debugDrawer=0);
 
 };
 
-#endif //SIMPLE_CONSTRAINT_SOLVER_H
+#endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
 
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.cpp
deleted file mode 100644 (file)
index d7c374d..0000000
+++ /dev/null
@@ -1,849 +0,0 @@
-/*************************************************************************
- *                                                                       *
- * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
- * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
- *                                                                       *
- * This library is free software; you can redistribute it and/or         *
- * modify it under the terms of EITHER:                                  *
- *   (1) The GNU Lesser General 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      *
- *       General Public License is included with this library in the     *
- *       file LICENSE.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.TXT and LICENSE-BSD.TXT for more details.                     *
- *                                                                       *
- *************************************************************************/
-
-#include "SorLcp.h"
-
-#ifdef USE_SOR_SOLVER
-
-// SOR LCP taken from ode quickstep, 
-// todo: write own successive overrelaxation gauss-seidel, or jacobi iterative solver
-
-
-#include "SimdScalar.h"
-
-#include "Dynamics/RigidBody.h"
-#include <math.h>
-#include <float.h>//FLT_MAX
-#ifdef WIN32
-#include <memory.h>
-#endif
-#include <string.h>
-#include <stdio.h>
-
-#if defined (WIN32)
-#include <malloc.h>
-#else
-#if defined (__FreeBSD__) || defined (__OpenBSD__)
-#include <stdlib.h>
-#else
-#include <alloca.h>
-#endif
-#endif
-
-#include "Dynamics/BU_Joint.h"
-#include "ContactSolverInfo.h"
-
-////////////////////////////////////////////////////////////////////
-//math stuff
-
-typedef SimdScalar dVector4[4];
-typedef SimdScalar dMatrix3[4*3];
-#define dInfinity FLT_MAX
-
-
-
-#define dRecip(x) ((float)(1.0f/(x)))                          /* reciprocal */
-
-
-
-#define dMULTIPLY0_331NEW(A,op,B,C) \
-{\
-       float tmp[3];\
-       tmp[0] = C.getX();\
-       tmp[1] = C.getY();\
-       tmp[2] = C.getZ();\
-       dMULTIPLYOP0_331(A,op,B,tmp);\
-}
-
-#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C)
-#define dMULTIPLYOP0_331(A,op,B,C) \
-  (A)[0] op dDOT1((B),(C)); \
-  (A)[1] op dDOT1((B+4),(C)); \
-  (A)[2] op dDOT1((B+8),(C));
-
-#define dAASSERT ASSERT
-#define dIASSERT ASSERT
-
-#define REAL float
-#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)])
-SimdScalar dDOT1  (const SimdScalar *a, const SimdScalar *b) { return dDOTpq(a,b,1,1); }
-#define dDOT14(a,b) dDOTpq(a,b,1,4)
-
-#define dCROSS(a,op,b,c) \
-  (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \
-  (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \
-  (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]);
-
-
-#define dMULTIPLYOP2_333(A,op,B,C) \
-  (A)[0] op dDOT1((B),(C)); \
-  (A)[1] op dDOT1((B),(C+4)); \
-  (A)[2] op dDOT1((B),(C+8)); \
-  (A)[4] op dDOT1((B+4),(C)); \
-  (A)[5] op dDOT1((B+4),(C+4)); \
-  (A)[6] op dDOT1((B+4),(C+8)); \
-  (A)[8] op dDOT1((B+8),(C)); \
-  (A)[9] op dDOT1((B+8),(C+4)); \
-  (A)[10] op dDOT1((B+8),(C+8));
-#define dMULTIPLYOP0_333(A,op,B,C) \
-  (A)[0] op dDOT14((B),(C)); \
-  (A)[1] op dDOT14((B),(C+1)); \
-  (A)[2] op dDOT14((B),(C+2)); \
-  (A)[4] op dDOT14((B+4),(C)); \
-  (A)[5] op dDOT14((B+4),(C+1)); \
-  (A)[6] op dDOT14((B+4),(C+2)); \
-  (A)[8] op dDOT14((B+8),(C)); \
-  (A)[9] op dDOT14((B+8),(C+1)); \
-  (A)[10] op dDOT14((B+8),(C+2));
-
-#define dMULTIPLY2_333(A,B,C) dMULTIPLYOP2_333(A,=,B,C)
-#define dMULTIPLY0_333(A,B,C) dMULTIPLYOP0_333(A,=,B,C)
-#define dMULTIPLYADD0_331(A,B,C) dMULTIPLYOP0_331(A,+=,B,C)
-
-
-////////////////////////////////////////////////////////////////////
-#define EFFICIENT_ALIGNMENT 16
-#define dEFFICIENT_SIZE(x) ((((x)-1)|(EFFICIENT_ALIGNMENT-1))+1)
-/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste
- * up to 15 bytes per allocation, depending on what alloca() returns.
- */
-
-#define dALLOCA16(n) \
-  ((char*)dEFFICIENT_SIZE(((size_t)(alloca((n)+(EFFICIENT_ALIGNMENT-1))))))
-
-
-
-/////////////////////////////////////////////////////////////////////
-/////////////////////////////////////////////////////////////////////
-
-#ifdef DEBUG
-#define ANSI_FTOL 1
-
-extern "C" { 
-    __declspec(naked) void _ftol2() {
-        __asm    {
-#if ANSI_FTOL
-            fnstcw   WORD PTR [esp-2]
-            mov      ax, WORD PTR [esp-2]
-                       
-            OR AX,      0C00h
-                       
-            mov      WORD PTR [esp-4], ax
-            fldcw    WORD PTR [esp-4]
-            fistp    QWORD PTR [esp-12]
-            fldcw    WORD PTR [esp-2]
-            mov      eax, DWORD PTR [esp-12]
-            mov      edx, DWORD PTR [esp-8]
-#else
-            fistp    DWORD PTR [esp-12]
-            mov                 eax, DWORD PTR [esp-12]
-            mov                 ecx, DWORD PTR [esp-8]
-#endif
-            ret
-        }
-    }
-}
-#endif //DEBUG
-
-
-
-  
-
-
-#define ALLOCA dALLOCA16
-
-typedef const SimdScalar *dRealPtr;
-typedef SimdScalar *dRealMutablePtr;
-#define dRealArray(name,n) SimdScalar name[n];
-#define dRealAllocaArray(name,n) SimdScalar *name = (SimdScalar*) ALLOCA ((n)*sizeof(SimdScalar));
-
-void dSetZero1 (SimdScalar *a, int n)
-{
-  dAASSERT (a && n >= 0);
-  while (n > 0) {
-    *(a++) = 0;
-    n--;
-  }
-}
-
-void dSetValue1 (SimdScalar *a, int n, SimdScalar value)
-{
-  dAASSERT (a && n >= 0);
-  while (n > 0) {
-    *(a++) = value;
-    n--;
-  }
-}
-
-
-//***************************************************************************
-// configuration
-
-// for the SOR and CG methods:
-// uncomment the following line to use warm starting. this definitely
-// help for motor-driven joints. unfortunately it appears to hurt
-// with high-friction contacts using the SOR method. use with care
-
-//#define WARM_STARTING 1
-
-// for the SOR method:
-// uncomment the following line to randomly reorder constraint rows
-// during the solution. depending on the situation, this can help a lot
-// or hardly at all, but it doesn't seem to hurt.
-
-#define RANDOMLY_REORDER_CONSTRAINTS 1
-
-
-
-//***************************************************************************
-// various common computations involving the matrix J
-
-// compute iMJ = inv(M)*J'
-
-static void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
-       RigidBody * const *body, dRealPtr invI)
-{
-       int i,j;
-       dRealMutablePtr iMJ_ptr = iMJ;
-       dRealMutablePtr J_ptr = J;
-       for (i=0; i<m; i++) {
-               int b1 = jb[i*2];       
-               int b2 = jb[i*2+1];
-               SimdScalar k = body[b1]->getInvMass();
-               for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j];
-               dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3);
-               if (b2 >= 0) {
-                       k = body[b2]->getInvMass();
-                       for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6];
-                       dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9);
-               }
-               J_ptr += 12;
-               iMJ_ptr += 12;
-       }
-}
-
-#if 0
-static void multiply_invM_JTSpecial (int m, int nb, dRealMutablePtr iMJ, int *jb,
-       dRealMutablePtr in, dRealMutablePtr out,int onlyBody1,int onlyBody2)
-{
-       int i,j;
-
-               
-
-       dRealMutablePtr out_ptr1 = out + onlyBody1*6;
-       
-       for (j=0; j<6; j++) 
-               out_ptr1[j] = 0;
-
-       if (onlyBody2 >= 0)
-       {
-               out_ptr1 = out + onlyBody2*6;
-
-               for (j=0; j<6; j++) 
-                       out_ptr1[j] = 0;
-       }
-
-       dRealPtr iMJ_ptr = iMJ;
-       for (i=0; i<m; i++) {
-
-               int b1 = jb[i*2];       
-
-               dRealMutablePtr out_ptr = out + b1*6;
-               if ((b1 == onlyBody1) || (b1 == onlyBody2))
-               {
-                       for (j=0; j<6; j++) 
-                               out_ptr[j] += iMJ_ptr[j] * in[i] ;
-               }
-                       
-               iMJ_ptr += 6;
-
-               int b2 = jb[i*2+1];
-               if ((b2 == onlyBody1) || (b2 == onlyBody2))
-               {
-                       if (b2 >= 0) 
-                       {
-                                       out_ptr = out + b2*6;
-                                       for (j=0; j<6; j++) 
-                                               out_ptr[j] += iMJ_ptr[j] * in[i];
-                       }
-               }
-               
-               iMJ_ptr += 6;
-               
-       }
-}
-#endif
-
-
-// compute out = inv(M)*J'*in.
-
-#if 0
-static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb,
-       dRealMutablePtr in, dRealMutablePtr out)
-{
-       int i,j;
-       dSetZero1 (out,6*nb);
-       dRealPtr iMJ_ptr = iMJ;
-       for (i=0; i<m; i++) {
-               int b1 = jb[i*2];       
-               int b2 = jb[i*2+1];
-               dRealMutablePtr out_ptr = out + b1*6;
-               for (j=0; j<6; j++) 
-                       out_ptr[j] += iMJ_ptr[j] * in[i];
-               iMJ_ptr += 6;
-               if (b2 >= 0) {
-                       out_ptr = out + b2*6;
-                       for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i];
-               }
-               iMJ_ptr += 6;
-       }
-}
-#endif
-
-
-// compute out = J*in.
-
-static void multiply_J (int m, dRealMutablePtr J, int *jb,
-       dRealMutablePtr in, dRealMutablePtr out)
-{
-       int i,j;
-       dRealPtr J_ptr = J;
-       for (i=0; i<m; i++) {
-               int b1 = jb[i*2];       
-               int b2 = jb[i*2+1];
-               SimdScalar sum = 0;
-               dRealMutablePtr in_ptr = in + b1*6;
-               for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];                                
-               J_ptr += 6;
-               if (b2 >= 0) {
-                       in_ptr = in + b2*6;
-                       for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];                                
-               }
-               J_ptr += 6;
-               out[i] = sum;
-       }
-}
-
-//***************************************************************************
-// SOR-LCP method
-
-// nb is the number of bodies in the body array.
-// J is an m*12 matrix of constraint rows
-// jb is an array of first and second body numbers for each constraint row
-// invI is the global frame inverse inertia for each body (stacked 3x3 matrices)
-//
-// this returns lambda and fc (the constraint force).
-// note: fc is returned as inv(M)*J'*lambda, the constraint force is actually J'*lambda
-//
-// b, lo and hi are modified on exit
-
-
-struct IndexError {
-       SimdScalar error;               // error to sort on
-       int findex;
-       int index;              // row index
-};
-
-static unsigned long seed2 = 0;
-
-unsigned long dRand2()
-{
-  seed2 = (1664525L*seed2 + 1013904223L) & 0xffffffff;
-  return seed2;
-}
-
-int dRandInt2 (int n)
-{
-  float a = float(n) / 4294967296.0f;
-  return (int) (float(dRand2()) * a);
-}
-
-
-static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, RigidBody * const *body,
-       dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs,
-       dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
-       int numiter,float overRelax)
-{
-       const int num_iterations = numiter;
-       const float sor_w = overRelax;          // SOR over-relaxation parameter
-
-       int i,j;
-
-#ifdef WARM_STARTING
-       // for warm starting, this seems to be necessary to prevent
-       // jerkiness in motor-driven joints. i have no idea why this works.
-       for (i=0; i<m; i++) lambda[i] *= 0.9;
-#else
-       dSetZero1 (lambda,m);
-#endif
-
-       // the lambda computed at the previous iteration.
-       // this is used to measure error for when we are reordering the indexes.
-       dRealAllocaArray (last_lambda,m);
-
-       // a copy of the 'hi' vector in case findex[] is being used
-       dRealAllocaArray (hicopy,m);
-       memcpy (hicopy,hi,m*sizeof(float));
-
-       // precompute iMJ = inv(M)*J'
-       dRealAllocaArray (iMJ,m*12);
-       compute_invM_JT (m,J,iMJ,jb,body,invI);
-
-       // compute fc=(inv(M)*J')*lambda. we will incrementally maintain fc
-       // as we change lambda.
-#ifdef WARM_STARTING
-       multiply_invM_JT (m,nb,iMJ,jb,lambda,fc);
-#else
-       dSetZero1 (invMforce,nb*6);
-#endif
-
-       // precompute 1 / diagonals of A
-       dRealAllocaArray (Ad,m);
-       dRealPtr iMJ_ptr = iMJ;
-       dRealMutablePtr J_ptr = J;
-       for (i=0; i<m; i++) {
-               float sum = 0;
-               for (j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j];
-               if (jb[i*2+1] >= 0) {
-                       for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j];
-               }
-               iMJ_ptr += 12;
-               J_ptr += 12;
-               Ad[i] = sor_w / sum;//(sum + cfm[i]);
-       }
-
-       // scale J and b by Ad
-       J_ptr = J;
-       for (i=0; i<m; i++) {
-               for (j=0; j<12; j++) {
-                       J_ptr[0] *= Ad[i];
-                       J_ptr++;
-               }
-               rhs[i] *= Ad[i];
-       }
-
-       // scale Ad by CFM
-       for (i=0; i<m; i++) 
-               Ad[i] *= cfm[i];
-
-       // order to solve constraint rows in
-       IndexError *order = (IndexError*) alloca (m*sizeof(IndexError));
-
-#ifndef REORDER_CONSTRAINTS
-       // make sure constraints with findex < 0 come first.
-       j=0;
-       for (i=0; i<m; i++) 
-               if (findex[i] < 0) 
-                       order[j++].index = i;
-       for (i=0; i<m; i++) 
-               if (findex[i] >= 0) 
-                       order[j++].index = i;
-       dIASSERT (j==m);
-#endif
-
-       for (int iteration=0; iteration < num_iterations; iteration++) {
-
-#ifdef REORDER_CONSTRAINTS
-               // constraints with findex < 0 always come first.
-               if (iteration < 2) {
-                       // for the first two iterations, solve the constraints in
-                       // the given order
-                       for (i=0; i<m; i++) {
-                               order[i].error = i;
-                               order[i].findex = findex[i];
-                               order[i].index = i;
-                       }
-               }
-               else {
-                       // sort the constraints so that the ones converging slowest
-                       // get solved last. use the absolute (not relative) error.
-                       for (i=0; i<m; i++) {
-                               float v1 = dFabs (lambda[i]);
-                               float v2 = dFabs (last_lambda[i]);
-                               float max = (v1 > v2) ? v1 : v2;
-                               if (max > 0) {
-                                       //@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max;
-                                       order[i].error = dFabs(lambda[i]-last_lambda[i]);
-                               }
-                               else {
-                                       order[i].error = dInfinity;
-                               }
-                               order[i].findex = findex[i];
-                               order[i].index = i;
-                       }
-               }
-               qsort (order,m,sizeof(IndexError),&compare_index_error);
-#endif
-#ifdef RANDOMLY_REORDER_CONSTRAINTS
-                if ((iteration & 7) == 0) {
-                       for (i=1; i<m; ++i) {
-                               IndexError tmp = order[i];
-                               int swapi = dRandInt2(i+1);
-                               order[i] = order[swapi];
-                               order[swapi] = tmp;
-                       }
-                }
-#endif
-
-               //@@@ potential optimization: swap lambda and last_lambda pointers rather
-               //    than copying the data. we must make sure lambda is properly
-               //    returned to the caller
-               memcpy (last_lambda,lambda,m*sizeof(float));
-
-               for (int i=0; i<m; i++) {
-                       // @@@ potential optimization: we could pre-sort J and iMJ, thereby
-                       //     linearizing access to those arrays. hmmm, this does not seem
-                       //     like a win, but we should think carefully about our memory
-                       //     access pattern.
-               
-                       int index = order[i].index;
-                       J_ptr = J + index*12;
-                       iMJ_ptr = iMJ + index*12;
-               
-                       // set the limits for this constraint. note that 'hicopy' is used.
-                       // this is the place where the QuickStep method differs from the
-                       // direct LCP solving method, since that method only performs this
-                       // limit adjustment once per time step, whereas this method performs
-                       // once per iteration per constraint row.
-                       // the constraints are ordered so that all lambda[] values needed have
-                       // already been computed.
-                       if (findex[index] >= 0) {
-                               hi[index] = SimdFabs (hicopy[index] * lambda[findex[index]]);
-                               lo[index] = -hi[index];
-                       }
-
-                       int b1 = jb[index*2];
-                       int b2 = jb[index*2+1];
-                       float delta = rhs[index] - lambda[index]*Ad[index];
-                       dRealMutablePtr fc_ptr = invMforce + 6*b1;
-                       
-                       // @@@ potential optimization: SIMD-ize this and the b2 >= 0 case
-                       delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] +
-                               fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] +
-                               fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5];
-                       // @@@ potential optimization: handle 1-body constraints in a separate
-                       //     loop to avoid the cost of test & jump?
-                       if (b2 >= 0) {
-                               fc_ptr = invMforce + 6*b2;
-                               delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] +
-                                       fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] +
-                                       fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11];
-                       }
-
-                       // compute lambda and clamp it to [lo,hi].
-                       // @@@ potential optimization: does SSE have clamping instructions
-                       //     to save test+jump penalties here?
-                       float new_lambda = lambda[index] + delta;
-                       if (new_lambda < lo[index]) {
-                               delta = lo[index]-lambda[index];
-                               lambda[index] = lo[index];
-                       }
-                       else if (new_lambda > hi[index]) {
-                               delta = hi[index]-lambda[index];
-                               lambda[index] = hi[index];
-                       }
-                       else {
-                               lambda[index] = new_lambda;
-                       }
-
-                       //@@@ a trick that may or may not help
-                       //float ramp = (1-((float)(iteration+1)/(float)num_iterations));
-                       //delta *= ramp;
-               
-                       // update invMforce.
-                       // @@@ potential optimization: SIMD for this and the b2 >= 0 case
-                       fc_ptr = invMforce + 6*b1;
-                       fc_ptr[0] += delta * iMJ_ptr[0];
-                       fc_ptr[1] += delta * iMJ_ptr[1];
-                       fc_ptr[2] += delta * iMJ_ptr[2];
-                       fc_ptr[3] += delta * iMJ_ptr[3];
-                       fc_ptr[4] += delta * iMJ_ptr[4];
-                       fc_ptr[5] += delta * iMJ_ptr[5];
-                       // @@@ potential optimization: handle 1-body constraints in a separate
-                       //     loop to avoid the cost of test & jump?
-                       if (b2 >= 0) {
-                               fc_ptr = invMforce + 6*b2;
-                               fc_ptr[0] += delta * iMJ_ptr[6];
-                               fc_ptr[1] += delta * iMJ_ptr[7];
-                               fc_ptr[2] += delta * iMJ_ptr[8];
-                               fc_ptr[3] += delta * iMJ_ptr[9];
-                               fc_ptr[4] += delta * iMJ_ptr[10];
-                               fc_ptr[5] += delta * iMJ_ptr[11];
-                       }
-               }
-       }
-}
-
-
-
-void SolveInternal1 (float global_cfm,
-                                        float global_erp,
-                                        RigidBody * const *body, int nb,
-                                       BU_Joint * const *_joint, 
-                                       int nj, 
-                                       const ContactSolverInfo& solverInfo)
-{
-
-       int numIter = solverInfo.m_numIterations;
-       float sOr = solverInfo.m_sor;
-
-       int i,j;
-       
-       SimdScalar stepsize1 = dRecip(solverInfo.m_timeStep);
-
-       // number all bodies in the body list - set their tag values
-       for (i=0; i<nb; i++) 
-               body[i]->m_odeTag = i;
-       
-       // make a local copy of the joint array, because we might want to modify it.
-       // (the "BU_Joint *const*" declaration says we're allowed to modify the joints
-       // but not the joint array, because the caller might need it unchanged).
-       //@@@ do we really need to do this? we'll be sorting constraint rows individually, not joints
-       BU_Joint **joint = (BU_Joint**) alloca (nj * sizeof(BU_Joint*));
-       memcpy (joint,_joint,nj * sizeof(BU_Joint*));
-       
-       // for all bodies, compute the inertia tensor and its inverse in the global
-       // frame, and compute the rotational force and add it to the torque
-       // accumulator. I and invI are a vertical stack of 3x4 matrices, one per body.
-       dRealAllocaArray (I,3*4*nb);
-       dRealAllocaArray (invI,3*4*nb);
-/*     for (i=0; i<nb; i++) {
-               dMatrix3 tmp;
-               // compute inertia tensor in global frame
-               dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R);
-               // compute inverse inertia tensor in global frame
-               dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R);
-               dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp);
-               // compute rotational force
-               dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp);
-       }
-*/
-       for (i=0; i<nb; i++) {
-               dMatrix3 tmp;
-               // compute inertia tensor in global frame
-               dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R);
-               dMULTIPLY0_333 (I+i*12,body[i]->m_R,tmp);
-
-               // compute inverse inertia tensor in global frame
-               dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R);
-               dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp);
-               // compute rotational force
-               dMULTIPLY0_331 (tmp,I+i*12,body[i]->getAngularVelocity());
-               dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp);
-       }
-
-
-
-
-       // get joint information (m = total constraint dimension, nub = number of unbounded variables).
-       // joints with m=0 are inactive and are removed from the joints array
-       // entirely, so that the code that follows does not consider them.
-       //@@@ do we really need to save all the info1's
-       BU_Joint::Info1 *info = (BU_Joint::Info1*) alloca (nj*sizeof(BU_Joint::Info1));
-       for (i=0, j=0; j<nj; j++) {     // i=dest, j=src
-               joint[j]->GetInfo1 (info+i);
-               dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m);
-               if (info[i].m > 0) {
-                       joint[i] = joint[j];
-                       i++;
-               }
-       }
-       nj = i;
-
-       // create the row offset array
-       int m = 0;
-       int *ofs = (int*) alloca (nj*sizeof(int));
-       for (i=0; i<nj; i++) {
-               ofs[i] = m;
-               m += info[i].m;
-       }
-
-       // if there are constraints, compute the constraint force
-       dRealAllocaArray (J,m*12);
-       int *jb = (int*) alloca (m*2*sizeof(int));
-       if (m > 0) {
-               // create a constraint equation right hand side vector `c', a constraint
-               // force mixing vector `cfm', and LCP low and high bound vectors, and an
-               // 'findex' vector.
-               dRealAllocaArray (c,m);
-               dRealAllocaArray (cfm,m);
-               dRealAllocaArray (lo,m);
-               dRealAllocaArray (hi,m);
-               int *findex = (int*) alloca (m*sizeof(int));
-               dSetZero1 (c,m);
-               dSetValue1 (cfm,m,global_cfm);
-               dSetValue1 (lo,m,-dInfinity);
-               dSetValue1 (hi,m, dInfinity);
-               for (i=0; i<m; i++) findex[i] = -1;
-               
-               // get jacobian data from constraints. an m*12 matrix will be created
-               // to store the two jacobian blocks from each constraint. it has this
-               // format:
-               //
-               //   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 \    .
-               //   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2  }-- jacobian for joint 0, body 1 and body 2 (3 rows)
-               //   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 /
-               //   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }--- jacobian for joint 1, body 1 and body 2 (3 rows)
-               //   etc...
-               //
-               //   (lll) = linear jacobian data
-               //   (aaa) = angular jacobian data
-               //
-               dSetZero1 (J,m*12);
-               BU_Joint::Info2 Jinfo;
-               Jinfo.rowskip = 12;
-               Jinfo.fps = stepsize1;
-               Jinfo.erp = global_erp;
-               for (i=0; i<nj; i++) {
-                       Jinfo.J1l = J + ofs[i]*12;
-                       Jinfo.J1a = Jinfo.J1l + 3;
-                       Jinfo.J2l = Jinfo.J1l + 6;
-                       Jinfo.J2a = Jinfo.J1l + 9;
-                       Jinfo.c = c + ofs[i];
-                       Jinfo.cfm = cfm + ofs[i];
-                       Jinfo.lo = lo + ofs[i];
-                       Jinfo.hi = hi + ofs[i];
-                       Jinfo.findex = findex + ofs[i];
-                       joint[i]->GetInfo2 (&Jinfo);
-
-                       if (Jinfo.c[0] > solverInfo.m_maxErrorReduction)
-                               Jinfo.c[0] = solverInfo.m_maxErrorReduction;
-
-
-
-
-                       // adjust returned findex values for global index numbering
-                       for (j=0; j<info[i].m; j++) {
-                               if (findex[ofs[i] + j] >= 0) 
-                                       findex[ofs[i] + j] += ofs[i];
-                       }
-               }
-
-               // create an array of body numbers for each joint row
-               int *jb_ptr = jb;
-               for (i=0; i<nj; i++) {
-                       int b1 = (joint[i]->node[0].body) ? (joint[i]->node[0].body->m_odeTag) : -1;
-                       int b2 = (joint[i]->node[1].body) ? (joint[i]->node[1].body->m_odeTag) : -1;
-                       for (j=0; j<info[i].m; j++) {
-                               jb_ptr[0] = b1;
-                               jb_ptr[1] = b2;
-                               jb_ptr += 2;
-                       }
-               }
-               dIASSERT (jb_ptr == jb+2*m);
-
-               // compute the right hand side `rhs'
-               dRealAllocaArray (tmp1,nb*6);
-               // put v/h + invM*fe into tmp1
-               for (i=0; i<nb; i++) {
-                       SimdScalar body_invMass = body[i]->getInvMass();
-                       for (j=0; j<3; j++) 
-                               tmp1[i*6+j] = body[i]->m_facc[j] * body_invMass + body[i]->getLinearVelocity()[j] * stepsize1;
-                       dMULTIPLY0_331NEW (tmp1 + i*6 + 3,=,invI + i*12,body[i]->m_tacc);
-                       for (j=0; j<3; j++) 
-                               tmp1[i*6+3+j] += body[i]->getAngularVelocity()[j] * stepsize1;
-               }
-
-               // put J*tmp1 into rhs
-               dRealAllocaArray (rhs,m);
-               multiply_J (m,J,jb,tmp1,rhs);
-
-               // complete rhs
-               for (i=0; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i];
-
-               // scale CFM
-               for (i=0; i<m; i++) 
-                       cfm[i] *= stepsize1;
-
-               // load lambda from the value saved on the previous iteration
-               dRealAllocaArray (lambda,m);
-#ifdef WARM_STARTING
-               dSetZero1 (lambda,m);   //@@@ shouldn't be necessary
-               for (i=0; i<nj; i++) {
-                       memcpy (lambda+ofs[i],joint[i]->lambda,info[i].m * sizeof(SimdScalar));
-               }
-#endif
-
-               // solve the LCP problem and get lambda and invM*constraint_force
-               dRealAllocaArray (cforce,nb*6);
-
-               SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,numIter,sOr);
-
-#ifdef WARM_STARTING
-               // save lambda for the next iteration
-               //@@@ note that this doesn't work for contact joints yet, as they are
-               // recreated every iteration
-               for (i=0; i<nj; i++) {
-                       memcpy (joint[i]->lambda,lambda+ofs[i],info[i].m * sizeof(SimdScalar));
-               }
-#endif
-
-       
-               // note that the SOR method overwrites rhs and J at this point, so
-               // they should not be used again.
-               
-               // add stepsize * cforce to the body velocity
-               for (i=0; i<nb; i++) {
-                       SimdVector3 linvel = body[i]->getLinearVelocity();
-                       SimdVector3 angvel = body[i]->getAngularVelocity();
-                       
-                       for (j=0; j<3; j++) 
-                               linvel[j] += solverInfo.m_timeStep* cforce[i*6+j];
-                       for (j=0; j<3; j++) 
-                               angvel[j] += solverInfo.m_timeStep* cforce[i*6+3+j];
-               
-                       body[i]->setLinearVelocity(linvel);
-                       body[i]->setAngularVelocity(angvel);
-
-               }
-               
-       }
-
-
-
-       // compute the velocity update:
-       // add stepsize * invM * fe to the body velocity
-
-       for (i=0; i<nb; i++) {
-               SimdScalar body_invMass = body[i]->getInvMass();
-               SimdVector3 linvel = body[i]->getLinearVelocity();
-               SimdVector3 angvel = body[i]->getAngularVelocity();
-
-               for (j=0; j<3; j++) 
-               {
-                       linvel[j] += solverInfo.m_timeStep * body_invMass * body[i]->m_facc[j];
-               }
-               for (j=0; j<3; j++) 
-               {
-                       body[i]->m_tacc[j] *= solverInfo.m_timeStep;    
-               }
-               dMULTIPLY0_331NEW(angvel,+=,invI + i*12,body[i]->m_tacc);
-               body[i]->setAngularVelocity(angvel);
-
-       }
-
-
-
-}
-
-
-#endif //USE_SOR_SOLVER
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.h b/extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.h
deleted file mode 100644 (file)
index 37fe09c..0000000
+++ /dev/null
@@ -1,45 +0,0 @@
-/*************************************************************************
- *                                                                       *
- * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
- * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
- *                                                                       *
- * This library is free software; you can redistribute it and/or         *
- * modify it under the terms of EITHER:                                  *
- *   (1) The GNU Lesser General 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      *
- *       General Public License is included with this library in the     *
- *       file LICENSE.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.TXT and LICENSE-BSD.TXT for more details.                     *
- *                                                                       *
- *************************************************************************/
-
-#define USE_SOR_SOLVER
-#ifdef USE_SOR_SOLVER
-
-#ifndef SOR_LCP_H
-#define SOR_LCP_H
-class RigidBody;
-class BU_Joint;
-#include "SimdScalar.h"
-
-struct ContactSolverInfo;
-
-void SolveInternal1 (float global_cfm,
-                                        float global_erp,
-                                        RigidBody * const *body, int nb,
-                    BU_Joint * const *_joint, int nj, const ContactSolverInfo& info);
-
-int dRandInt2 (int n);
-
-
-#endif //SOR_LCP_H
-
-#endif //USE_SOR_SOLVER
-
index 9fd859e0f2d417291c6549ce898d45f6f2646056..cab71449f529372146235ffd60893fe2fe2f875c 100644 (file)
@@ -178,7 +178,7 @@ void ContactJoint::GetInfo2(Info2 *info)
        c2[1] = relativePositionB[1];
        c2[2] = relativePositionB[2];
        
-       
+       //combined friction is available in the contact point
        float friction = FRICTION_CONSTANT*m_body0->getFriction() * m_body1->getFriction();
        
        // first friction direction
index e4ccf83953a3c0f63a451f7eb33128098fd4415d..c268515e327df242364ed361e16518f2613cf7ae 100644 (file)
@@ -32,10 +32,13 @@ RigidBody::RigidBody( const MassProps& massProps,SimdScalar linearDamping,SimdSc
        m_angularVelocity(0.f,0.f,0.f),
        m_linearDamping(0.f),
        m_angularDamping(0.5f),
-       m_friction(friction),
-       m_restitution(restitution),
        m_kinematicTimeStep(0.f)
 {
+
+       //moved to CollisionObject
+       m_friction = friction;
+       m_restitution = restitution;
+
        m_debugBodyId = uniqueId++;
        
        setMassProps(massProps.m_mass, massProps.m_inertiaLocal);
index fd96dc99d60a86dbbb7c2d827e1838792cd9bc75..ba2399d2da4eb3361ebc06104829df437cfdaf5f 100644 (file)
@@ -31,7 +31,6 @@ typedef SimdScalar dMatrix3[4*3];
 extern float gLinearAirDamping;
 extern bool gUseEpa;
 
-#define MAX_RIGIDBODIES 8192
 
 
 /// RigidBody class for RigidBody Dynamics
@@ -173,22 +172,7 @@ public:
        void    getAabb(SimdVector3& aabbMin,SimdVector3& aabbMax) const;
 
 
-       void    setRestitution(float rest)
-       {
-               m_restitution = rest;
-       }
-       float   getRestitution() const
-       {
-               return m_restitution;
-       }
-       void    setFriction(float frict)
-       {
-               m_friction = frict;
-       }
-       float   getFriction() const
-       {
-               return m_friction;
-       }
+
 
        
        inline float ComputeImpulseDenominator(const SimdPoint3& pos, const SimdVector3& normal) const
@@ -228,8 +212,6 @@ private:
        SimdScalar              m_angularDamping;
        SimdScalar              m_inverseMass;
 
-       SimdScalar              m_friction;
-       SimdScalar              m_restitution;
 
        SimdScalar              m_kinematicTimeStep;
 
@@ -254,6 +236,7 @@ public:
                m_broadphaseProxy = broadphaseProxy;
        }
        
+       
 
        /// for ode solver-binding
        dMatrix3                m_R;//temp
@@ -261,10 +244,11 @@ public:
        dMatrix3                m_invI;
 
        int                             m_odeTag;
-       float           m_padding[1024];
+       
        SimdVector3             m_tacc;//temp
        SimdVector3             m_facc;
-       
+
+
 
        int     m_debugBodyId;
 };
index 60b8dbbf86271fea0500528e717a795ca513a16f..83bddc8ee1ef1047e1fd149bcdf34995fe842966 100644 (file)
@@ -34,7 +34,7 @@ class BP_Proxy;
 float  gDeactivationTime = 2.f;
 bool   gDisableDeactivation = false;
 
-float gLinearSleepingTreshold = 0.4f;
+float gLinearSleepingTreshold = 0.8f;
 float gAngularSleepingTreshold = 1.0f;
 
 #include "Dynamics/MassProps.h"
@@ -210,28 +210,6 @@ void               CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionsta
 
 }
 
-void CcdPhysicsController::SetMargin(float margin)
-{
-       if (m_body && m_body->GetCollisionShape())
-       {
-               m_body->GetCollisionShape()->SetMargin(margin);
-       }
-
-
-}
-
-float CcdPhysicsController::GetMargin() const
-{
-       if (m_body && m_body->GetCollisionShape())
-       {
-               return m_body->GetCollisionShape()->GetMargin();
-       }
-
-       return 0.f;
-
-}
-
-
                // kinematic methods
 void           CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
 {
@@ -557,6 +535,7 @@ PHY_IPhysicsController*     CcdPhysicsController::GetReplica()
 DefaultMotionState::DefaultMotionState()
 {
        m_worldTransform.setIdentity();
+       m_localScaling.setValue(1.f,1.f,1.f);
 }
 
 
@@ -574,9 +553,9 @@ void        DefaultMotionState::getWorldPosition(float& posX,float& posY,float& posZ)
 
 void   DefaultMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ)
 {
-       scaleX = 1.;
-       scaleY = 1.;
-       scaleZ = 1.;
+       scaleX = m_localScaling.getX();
+       scaleY = m_localScaling.getY();
+       scaleZ = m_localScaling.getZ();
 }
 
 void   DefaultMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal)
index 7488e74db0128ba8fb267dff023e8483f38b4015..632d5d776d22af16a9fc8d78d82c5e6340ff20d3 100644 (file)
@@ -39,20 +39,38 @@ extern bool gDisableDeactivation;
 class CcdPhysicsEnvironment;
 
 
+
+
 struct CcdConstructionInfo
 {
+
+       ///CollisionFilterGroups provides some optional usage of basic collision filtering
+       ///this is done during broadphase, so very early in the pipeline
+       ///more advanced collision filtering should be done in CollisionDispatcher::NeedsCollision
+       enum CollisionFilterGroups
+       {
+               DefaultFilter = 1,
+               StaticFilter = 2,
+               KinematicFilter = 4,
+               DebrisFilter = 8,
+               AllFilter = DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter,
+       };
+
+
        CcdConstructionInfo()
                : m_gravity(0,0,0),
+               m_scaling(1.f,1.f,1.f),
                m_mass(0.f),
                m_restitution(0.1f),
                m_friction(0.5f),
                m_linearDamping(0.1f),
                m_angularDamping(0.1f),
+               m_collisionFlags(0),
+               m_collisionFilterGroup(DefaultFilter),
+               m_collisionFilterMask(AllFilter),
                m_MotionState(0),
                m_physicsEnv(0),
-               m_inertiaFactor(1.f),
-               m_scaling(1.f,1.f,1.f),
-               m_collisionFlags(0)
+               m_inertiaFactor(1.f)
        {
        }
 
@@ -66,6 +84,15 @@ struct CcdConstructionInfo
        SimdScalar      m_angularDamping;
        int                     m_collisionFlags;
 
+       ///optional use of collision group/mask:
+       ///only collision with object goups that match the collision mask.
+       ///this is very basic early out. advanced collision filtering should be
+       ///done in the CollisionDispatcher::NeedsCollision and NeedsResponse
+       ///both values default to 1
+       short int       m_collisionFilterGroup;
+       short int       m_collisionFilterMask;
+
+
        CollisionShape*                 m_collisionShape;
        class   PHY_IMotionState*                       m_MotionState;
 
@@ -158,10 +185,21 @@ class CcdPhysicsController : public PHY_IPhysicsController
                virtual void                            setNewClientInfo(void* clientinfo);
                virtual PHY_IPhysicsController* GetReplica();
                
+               ///There should be no 'SetCollisionFilterGroup' method, as changing this during run-time is will result in errors
+               short int       GetCollisionFilterGroup() const
+               {
+                       return m_cci.m_collisionFilterGroup;
+               }
+               ///There should be no 'SetCollisionFilterGroup' method, as changing this during run-time is will result in errors
+               short int       GetCollisionFilterMask() const
+               {
+                       return m_cci.m_collisionFilterMask;
+               }
+
 
                virtual void    calcXform() {} ;
-               virtual void SetMargin(float margin);
-               virtual float GetMargin() const;
+               virtual void SetMargin(float margin) {};
+               virtual float GetMargin() const {return 0.f;};
 
 
                bool    wantsSleeping();
@@ -208,6 +246,7 @@ class       DefaultMotionState : public PHY_IMotionState
                virtual void    calculateWorldTransformations();
                
                SimdTransform   m_worldTransform;
+               SimdVector3             m_localScaling;
 
 };
 
index 2fee6d069631c07533da4a49ae0049852a0afa37..9362595170d216aebbf0b062294c848284738860 100644 (file)
@@ -30,14 +30,12 @@ subject to the following restrictions:
 
 #include "CollisionShapes/ConvexShape.h"
 #include "CollisionShapes/ConeShape.h"
-
-
+#include "CollisionDispatch/SimulationIslandManager.h"
 
 #include "BroadphaseCollision/Dispatcher.h"
 #include "NarrowPhaseCollision/PersistentManifold.h"
 #include "CollisionShapes/TriangleMeshShape.h"
-#include "ConstraintSolver/OdeConstraintSolver.h"
-#include "ConstraintSolver/SimpleConstraintSolver.h"
+#include "ConstraintSolver/SequentialImpulseConstraintSolver.h"