Upgrade to latest Bullet trunk, that is in sync with Blender/extern/bullet2. (except...
authorErwin Coumans <blender@erwincoumans.com>
Sat, 13 Sep 2008 07:06:43 +0000 (07:06 +0000)
committerErwin Coumans <blender@erwincoumans.com>
Sat, 13 Sep 2008 07:06:43 +0000 (07:06 +0000)
In case someone reads those SVN logs: you can enable some extra broadphase SSE optimizations by replacing WIN32_AVOID_SSE_WHEN_EMBEDDED_INSIDE_BLENDER by WIN32 in extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h

Thanks to Benoit Bolsee for the upstream patch/contribution.
Removed some obsolete files, they were just intended for comparison/testing.

57 files changed:
extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h
extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp
extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h
extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
extern/bullet2/src/BulletCollision/CMakeLists.txt
extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h [new file with mode: 0644]
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h
extern/bullet2/src/BulletDynamics/CMakeLists.txt
extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.cpp [deleted file]
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.h [deleted file]
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.cpp [deleted file]
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.h [deleted file]
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeMacros.h [deleted file]
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.cpp [deleted file]
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.h [deleted file]
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeSolverBody.h [deleted file]
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp [deleted file]
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h [deleted file]
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.cpp [deleted file]
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.h [deleted file]
extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
extern/bullet2/src/BulletSoftBody/btSoftBody.cpp
extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp
extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h
extern/bullet2/src/LinearMath/CMakeLists.txt
extern/bullet2/src/LinearMath/btAabbUtil2.h
extern/bullet2/src/LinearMath/btMatrix3x3.h
extern/bullet2/src/LinearMath/btScalar.h
extern/bullet2/src/btBulletCollisionCommon.h
extern/bullet2/src/btBulletDynamicsCommon.h

index e7c5fb5..d0ad09a 100644 (file)
@@ -27,6 +27,7 @@
 #include "btOverlappingPairCallback.h"
 
 //#define DEBUG_BROADPHASE 1
+#define USE_OVERLAP_TEST_ON_REMOVES 1
 
 /// The internal templace class btAxisSweep3Internal implements the sweep and prune broadphase.
 /// It uses quantized integers to represent the begin and end points for each of the 3 axis.
@@ -52,9 +53,7 @@ public:
        };
 
 public:
-       //This breaks the Intel compiler, see http://softwarecommunity.intel.com/isn/Community/en-US/forums/thread/30253577.aspx
-       class Handle : public btBroadphaseProxy
-       //ATTRIBUTE_ALIGNED16(class) Handle : public btBroadphaseProxy
+       class   Handle : public btBroadphaseProxy
        {
        public:
        BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -80,7 +79,7 @@ protected:
        BP_FP_INT_TYPE m_numHandles;                                            // number of active handles
        BP_FP_INT_TYPE m_maxHandles;                                            // max number of handles
        Handle* m_pHandles;                                             // handles pool
-       void* m_pHandlesRawPtr;
+       
        BP_FP_INT_TYPE m_firstFreeHandle;               // free handles list
 
        Edge* m_pEdges[3];                                              // edge arrays for the 3 axes (each array has m_maxHandles * 2 + 2 sentinel entries)
@@ -100,7 +99,7 @@ protected:
        void freeHandle(BP_FP_INT_TYPE handle);
        
 
-       bool testOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB);
+       bool testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1);
 
 #ifdef DEBUG_BROADPHASE
        void debugPrintAxis(int axis,bool checkCardinality=true);
@@ -273,10 +272,9 @@ m_invalidPair(0)
 
        m_quantize = btVector3(btScalar(maxInt),btScalar(maxInt),btScalar(maxInt)) / aabbSize;
 
-       // allocate handles buffer and put all handles on free list
-       m_pHandlesRawPtr = btAlignedAlloc(sizeof(Handle)*maxHandles,16);
-       m_pHandles = new(m_pHandlesRawPtr) Handle[maxHandles];
-
+       // allocate handles buffer, using btAlignedAlloc, and put all handles on free list
+       m_pHandles = new Handle[maxHandles];
+       
        m_maxHandles = maxHandles;
        m_numHandles = 0;
 
@@ -327,7 +325,7 @@ btAxisSweep3Internal<BP_FP_INT_TYPE>::~btAxisSweep3Internal()
        {
                btAlignedFree(m_pEdgesRawPtr[i]);
        }
-       btAlignedFree(m_pHandlesRawPtr);
+       delete [] m_pHandles;
 
        if (m_ownsPairCache)
        {
@@ -603,34 +601,17 @@ bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testAabbOverlap(btBroadphaseProxy* pr
 }
 
 template <typename BP_FP_INT_TYPE>
-bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB)
+bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1)
 {
        //optimization 1: check the array index (memory address), instead of the m_pos
 
-       for (int axis = 0; axis < 3; axis++)
+       if (pHandleA->m_maxEdges[axis0] < pHandleB->m_minEdges[axis0] || 
+               pHandleB->m_maxEdges[axis0] < pHandleA->m_minEdges[axis0] ||
+               pHandleA->m_maxEdges[axis1] < pHandleB->m_minEdges[axis1] ||
+               pHandleB->m_maxEdges[axis1] < pHandleA->m_minEdges[axis1]) 
        { 
-               if (axis != ignoreAxis)
-               {
-                       if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] || 
-                               pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis]) 
-                       { 
-                               return false; 
-                       } 
-               }
+               return false; 
        } 
-
-       //optimization 2: only 2 axis need to be tested (conflicts with 'delayed removal' optimization)
-
-       /*for (int axis = 0; axis < 3; axis++)
-       {
-               if (m_pEdges[axis][pHandleA->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleB->m_minEdges[axis]].m_pos ||
-                       m_pEdges[axis][pHandleB->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleA->m_minEdges[axis]].m_pos)
-               {
-                       return false;
-               }
-       }
-       */
-
        return true;
 }
 
@@ -700,7 +681,9 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinDown(int axis, BP_FP_INT_TYPE
                if (pPrev->IsMax())
                {
                        // if previous edge is a maximum check the bounds and add an overlap if necessary
-                       if (updateOverlaps && testOverlap(axis,pHandleEdge, pHandlePrev))
+                       const int axis1 = (1  << axis) & 3;
+                       const int axis2 = (1  << axis1) & 3;
+                       if (updateOverlaps && testOverlap2D(pHandleEdge, pHandlePrev,axis1,axis2))
                        {
                                m_pairCache->addOverlappingPair(pHandleEdge,pHandlePrev);
                                if (m_userPairCallback)
@@ -748,12 +731,19 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE ed
 
                if (pNext->IsMax())
                {
-
+                       Handle* handle0 = getHandle(pEdge->m_handle);
+                       Handle* handle1 = getHandle(pNext->m_handle);
+                       const int axis1 = (1  << axis) & 3;
+                       const int axis2 = (1  << axis1) & 3;
+                       
                        // if next edge is maximum remove any overlap between the two handles
-                       if (updateOverlaps)
+                       if (updateOverlaps 
+#ifdef USE_OVERLAP_TEST_ON_REMOVES
+                               && testOverlap2D(handle0,handle1,axis1,axis2)
+#endif //USE_OVERLAP_TEST_ON_REMOVES
+                               )
                        {
-                               Handle* handle0 = getHandle(pEdge->m_handle);
-                               Handle* handle1 = getHandle(pNext->m_handle);
+                               
 
                                m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher); 
                                if (m_userPairCallback)
@@ -799,12 +789,20 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE
                if (!pPrev->IsMax())
                {
                        // if previous edge was a minimum remove any overlap between the two handles
-                       if (updateOverlaps)
+                       Handle* handle0 = getHandle(pEdge->m_handle);
+                       Handle* handle1 = getHandle(pPrev->m_handle);
+                       const int axis1 = (1  << axis) & 3;
+                       const int axis2 = (1  << axis1) & 3;
+
+                       if (updateOverlaps  
+#ifdef USE_OVERLAP_TEST_ON_REMOVES
+                               && testOverlap2D(handle0,handle1,axis1,axis2)
+#endif //USE_OVERLAP_TEST_ON_REMOVES
+                               )
                        {
                                //this is done during the overlappingpairarray iteration/narrowphase collision
 
-                               Handle* handle0 = getHandle(pEdge->m_handle);
-                               Handle* handle1 = getHandle(pPrev->m_handle);
+                               
                                m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher);
                                if (m_userPairCallback)
                                        m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher);
@@ -850,10 +848,13 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxUp(int axis, BP_FP_INT_TYPE ed
        {
                Handle* pHandleNext = getHandle(pNext->m_handle);
 
+               const int axis1 = (1  << axis) & 3;
+               const int axis2 = (1  << axis1) & 3;
+
                if (!pNext->IsMax())
                {
                        // if next edge is a minimum check the bounds and add an overlap if necessary
-                       if (updateOverlaps && testOverlap(axis, pHandleEdge, pHandleNext))
+                       if (updateOverlaps && testOverlap2D(pHandleEdge, pHandleNext,axis1,axis2))
                        {
                                Handle* handle0 = getHandle(pEdge->m_handle);
                                Handle* handle1 = getHandle(pNext->m_handle);
index fade711..7c41c8d 100644 (file)
@@ -663,24 +663,23 @@ Benchmarking dbvt...
         Leaves: 8192
         sizeof(btDbvtVolume): 32 bytes
         sizeof(btDbvtNode):   44 bytes
-[1] btDbvtVolume intersections: 3537 ms (0%)
-[2] btDbvtVolume merges: 1945 ms (0%)
-[3] btDbvt::collideTT: 6646 ms (0%)
-[4] btDbvt::collideTT self: 3389 ms (0%)
-[5] btDbvt::collideTT xform: 7505 ms (0%)
-[6] btDbvt::collideTT xform,self: 7480 ms (0%)
-[7] btDbvt::collideRAY: 6307 ms (0%),(332511 r/s)
-[8] insert/remove: 2105 ms (-3%),(996271 ir/s)
-[9] updates (teleport): 1943 ms (0%),(1079337 u/s)
-[10] updates (jitter): 1301 ms (0%),(1611953 u/s)
-[11] optimize (incremental): 2510 ms (0%),(1671000 o/s)
-[12] btDbvtVolume notequal: 3677 ms (0%)
-[13] culling(OCL+fullsort): 2231 ms (0%),(458 t/s)
-[14] culling(OCL+qsort): 3500 ms (0%),(2340 t/s)
-[15] culling(KDOP+qsort): 1151 ms (0%),(7117 t/s)
-[16] insert/remove batch(256): 5138 ms (0%),(816330 bir/s)
-[17] btDbvtVolume proximity: 2842 ms (0%)
-[18] btDbvtVolume select: 3390 ms (0%)
+[1] btDbvtVolume intersections: 3499 ms (-1%)
+[2] btDbvtVolume merges: 1934 ms (0%)
+[3] btDbvt::collideTT: 5485 ms (-21%)
+[4] btDbvt::collideTT self: 2814 ms (-20%)
+[5] btDbvt::collideTT xform: 7379 ms (-1%)
+[6] btDbvt::collideTT xform,self: 7270 ms (-2%)
+[7] btDbvt::collideRAY: 6314 ms (0%),(332143 r/s)
+[8] insert/remove: 2093 ms (0%),(1001983 ir/s)
+[9] updates (teleport): 1879 ms (-3%),(1116100 u/s)
+[10] updates (jitter): 1244 ms (-4%),(1685813 u/s)
+[11] optimize (incremental): 2514 ms (0%),(1668000 o/s)
+[12] btDbvtVolume notequal: 3659 ms (0%)
+[13] culling(OCL+fullsort): 2218 ms (0%),(461 t/s)
+[14] culling(OCL+qsort): 3688 ms (5%),(2221 t/s)
+[15] culling(KDOP+qsort): 1139 ms (-1%),(7192 t/s)
+[16] insert/remove batch(256): 5092 ms (0%),(823704 bir/s)
+[17] btDbvtVolume select: 3419 ms (0%)
 */
 
 struct btDbvtBenchmark
@@ -787,7 +786,7 @@ static const bool           cfgEnable                                       =       true;
 //[1] btDbvtVolume intersections
 bool                                   cfgBenchmark1_Enable            =       cfgEnable;
 static const int               cfgBenchmark1_Iterations        =       8;
-static const int               cfgBenchmark1_Reference         =       3537;
+static const int               cfgBenchmark1_Reference         =       3499;
 //[2] btDbvtVolume merges
 bool                                   cfgBenchmark2_Enable            =       cfgEnable;
 static const int               cfgBenchmark2_Iterations        =       4;
@@ -795,21 +794,21 @@ static const int          cfgBenchmark2_Reference         =       1945;
 //[3] btDbvt::collideTT
 bool                                   cfgBenchmark3_Enable            =       cfgEnable;
 static const int               cfgBenchmark3_Iterations        =       512;
-static const int               cfgBenchmark3_Reference         =       6646;
+static const int               cfgBenchmark3_Reference         =       5485;
 //[4] btDbvt::collideTT self
 bool                                   cfgBenchmark4_Enable            =       cfgEnable;
 static const int               cfgBenchmark4_Iterations        =       512;
-static const int               cfgBenchmark4_Reference         =       3389;
+static const int               cfgBenchmark4_Reference         =       2814;
 //[5] btDbvt::collideTT xform
 bool                                   cfgBenchmark5_Enable            =       cfgEnable;
 static const int               cfgBenchmark5_Iterations        =       512;
 static const btScalar  cfgBenchmark5_OffsetScale       =       2;
-static const int               cfgBenchmark5_Reference         =       7505;
+static const int               cfgBenchmark5_Reference         =       7379;
 //[6] btDbvt::collideTT xform,self
 bool                                   cfgBenchmark6_Enable            =       cfgEnable;
 static const int               cfgBenchmark6_Iterations        =       512;
 static const btScalar  cfgBenchmark6_OffsetScale       =       2;
-static const int               cfgBenchmark6_Reference         =       7480;
+static const int               cfgBenchmark6_Reference         =       7270;
 //[7] btDbvt::collideRAY
 bool                                   cfgBenchmark7_Enable            =       cfgEnable;
 static const int               cfgBenchmark7_Passes            =       32;
@@ -824,13 +823,13 @@ static const int          cfgBenchmark8_Reference         =       2105;
 bool                                   cfgBenchmark9_Enable            =       cfgEnable;
 static const int               cfgBenchmark9_Passes            =       32;
 static const int               cfgBenchmark9_Iterations        =       65536;
-static const int               cfgBenchmark9_Reference         =       1943;
+static const int               cfgBenchmark9_Reference         =       1879;
 //[10] updates (jitter)
 bool                                   cfgBenchmark10_Enable           =       cfgEnable;
 static const btScalar  cfgBenchmark10_Scale            =       cfgVolumeCenterScale/10000;
 static const int               cfgBenchmark10_Passes           =       32;
 static const int               cfgBenchmark10_Iterations       =       65536;
-static const int               cfgBenchmark10_Reference        =       1301;
+static const int               cfgBenchmark10_Reference        =       1244;
 //[11] optimize (incremental)
 bool                                   cfgBenchmark11_Enable           =       cfgEnable;
 static const int               cfgBenchmark11_Passes           =       64;
@@ -857,14 +856,10 @@ bool                                      cfgBenchmark16_Enable           =       cfgEnable;
 static const int               cfgBenchmark16_BatchCount       =       256;
 static const int               cfgBenchmark16_Passes           =       16384;
 static const int               cfgBenchmark16_Reference        =       5138;
-//[17] proximity
+//[17] select
 bool                                   cfgBenchmark17_Enable           =       cfgEnable;
-static const int               cfgBenchmark17_Iterations       =       8;
-static const int               cfgBenchmark17_Reference        =       2842;
-//[18] select
-bool                                   cfgBenchmark18_Enable           =       cfgEnable;
-static const int               cfgBenchmark18_Iterations       =       4;
-static const int               cfgBenchmark18_Reference        =       3390;
+static const int               cfgBenchmark17_Iterations       =       4;
+static const int               cfgBenchmark17_Reference        =       3390;
 
 btClock                                        wallclock;
 printf("Benchmarking dbvt...\r\n");
@@ -1259,32 +1254,6 @@ if(cfgBenchmark17_Enable)
        {// Benchmark 17
        srand(380843);
        btAlignedObjectArray<btDbvtVolume>      volumes;
-       btAlignedObjectArray<btScalar>          results;
-       volumes.resize(cfgLeaves);
-       results.resize(cfgLeaves);
-       for(int i=0;i<cfgLeaves;++i)
-               {
-               volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
-               }
-       printf("[17] btDbvtVolume proximity: ");
-       wallclock.reset();
-       for(int i=0;i<cfgBenchmark17_Iterations;++i)
-               {
-               for(int j=0;j<cfgLeaves;++j)
-                       {
-                       for(int k=0;k<cfgLeaves;++k)
-                               {
-                               results[k]=Proximity(volumes[j],volumes[k]);
-                               }
-                       }
-               }
-       const int time=(int)wallclock.getTimeMilliseconds();
-       printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark17_Reference)*100/time);
-       }
-if(cfgBenchmark18_Enable)
-       {// Benchmark 18
-       srand(380843);
-       btAlignedObjectArray<btDbvtVolume>      volumes;
        btAlignedObjectArray<int>                       results;
        btAlignedObjectArray<int>                       indices;
        volumes.resize(cfgLeaves);
@@ -1299,9 +1268,9 @@ if(cfgBenchmark18_Enable)
                {
                btSwap(indices[i],indices[rand()%cfgLeaves]);
                }
-       printf("[18] btDbvtVolume select: ");
+       printf("[17] btDbvtVolume select: ");
        wallclock.reset();
-       for(int i=0;i<cfgBenchmark18_Iterations;++i)
+       for(int i=0;i<cfgBenchmark17_Iterations;++i)
                {
                for(int j=0;j<cfgLeaves;++j)
                        {
@@ -1313,7 +1282,7 @@ if(cfgBenchmark18_Enable)
                        }
                }
        const int time=(int)wallclock.getTimeMilliseconds();
-       printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark18_Reference)*100/time);
+       printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark17_Reference)*100/time);
        }
 printf("\r\n\r\n");
 }
index 10f9462..da29644 100644 (file)
@@ -31,7 +31,7 @@ subject to the following restrictions:
 #define DBVT_IMPL_SSE                  1       // SSE
 
 // Template implementation of ICollide
-#ifdef WIN32_AVOID_WHEN_EMBEDDED_INSIDE_BLENDER
+#ifdef WIN32_AVOID_SSE_WHEN_EMBEDDED_INSIDE_BLENDER //there is always some weird compiler that breaks SSE builds
        #if (defined (_MSC_VER) && _MSC_VER >= 1400)
        #define DBVT_USE_TEMPLATE               1
        #else
@@ -41,6 +41,9 @@ subject to the following restrictions:
 #define        DBVT_USE_TEMPLATE               0
 #endif
 
+// Use only intrinsics instead of inline asm
+#define DBVT_USE_INTRINSIC_SSE 1
+
 // Using memmov for collideOCL
 #define DBVT_USE_MEMMOVE               1
 
@@ -57,14 +60,21 @@ subject to the following restrictions:
 #endif
 
 // Specific methods implementation
-#ifdef WIN32_AVOID_WHEN_EMBEDDED_INSIDE_BLENDER
-#define DBVT_PROXIMITY_IMPL            DBVT_IMPL_SSE
+
+#ifdef WIN32_AVOID_SSE_WHEN_EMBEDDED_INSIDE_BLENDER //there is always some weird compiler that breaks SSE builds
 #define DBVT_SELECT_IMPL               DBVT_IMPL_SSE
 #define DBVT_MERGE_IMPL                        DBVT_IMPL_SSE
+#define DBVT_INT0_IMPL                 DBVT_IMPL_SSE
 #else
-#define DBVT_PROXIMITY_IMPL            DBVT_IMPL_GENERIC
 #define DBVT_SELECT_IMPL               DBVT_IMPL_GENERIC
 #define DBVT_MERGE_IMPL                        DBVT_IMPL_GENERIC
+#define DBVT_INT0_IMPL                 DBVT_IMPL_GENERIC
+#endif
+
+#if    (DBVT_SELECT_IMPL==DBVT_IMPL_SSE)||     \
+       (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)||      \
+       (DBVT_INT0_IMPL==DBVT_IMPL_SSE)
+#include <emmintrin.h>
 #endif
 
 //
@@ -104,10 +114,6 @@ subject to the following restrictions:
 #error "DBVT_ENABLE_BENCHMARK undefined"
 #endif
 
-#ifndef DBVT_PROXIMITY_IMPL
-#error "DBVT_PROXIMITY_IMPL undefined"
-#endif
-
 #ifndef DBVT_SELECT_IMPL
 #error "DBVT_SELECT_IMPL undefined"
 #endif
@@ -116,6 +122,10 @@ subject to the following restrictions:
 #error "DBVT_MERGE_IMPL undefined"
 #endif
 
+#ifndef DBVT_INT0_IMPL
+#error "DBVT_INT0_IMPL undefined"
+#endif
+
 //
 // Defaults volumes
 //
@@ -133,8 +143,8 @@ static inline btDbvtAabbMm          FromCR(const btVector3& c,btScalar r);
 static inline btDbvtAabbMm             FromMM(const btVector3& mi,const btVector3& mx);
 static inline btDbvtAabbMm             FromPoints(const btVector3* pts,int n);
 static inline btDbvtAabbMm             FromPoints(const btVector3** ppts,int n);
-DBVT_INLINE void                               Expand(const btVector3 e);
-DBVT_INLINE void                               SignedExpand(const btVector3 e);
+DBVT_INLINE void                               Expand(const btVector3& e);
+DBVT_INLINE void                               SignedExpand(const btVector3& e);
 DBVT_INLINE bool                               Contain(const btDbvtAabbMm& a) const;
 DBVT_INLINE int                                        Classify(const btVector3& n,btScalar o,int s) const;
 DBVT_INLINE btScalar                   ProjectMinimum(const btVector3& v,unsigned signs) const;
@@ -173,12 +183,12 @@ struct    btDbvtNode
 {
        btDbvtVolume    volume;
        btDbvtNode*             parent;
-       bool    isleaf() const          { return(childs[1]==0); }
-       bool    isinternal() const      { return(!isleaf()); }
+       DBVT_INLINE bool        isleaf() const          { return(childs[1]==0); }
+       DBVT_INLINE bool        isinternal() const      { return(!isleaf()); }
        union   {
-               btDbvtNode*     childs[2];
-               void*   data;
-               };
+                       btDbvtNode*     childs[2];
+                       void*   data;
+                       };
 };
 
 ///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
@@ -186,8 +196,6 @@ struct      btDbvtNode
 ///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
 struct btDbvt
        {
-       
-       
        /* Stack element        */ 
        struct  sStkNN
                {
@@ -250,8 +258,8 @@ struct      btDbvt
                        };
                
        // Fields
-       btDbvtNode*                     m_root;
-       btDbvtNode*                     m_free;
+       btDbvtNode*             m_root;
+       btDbvtNode*             m_free;
        int                             m_lkhd;
        int                             m_leaves;
        unsigned                m_opath;
@@ -408,17 +416,17 @@ return(box);
 }
 
 //
-DBVT_INLINE void               btDbvtAabbMm::Expand(const btVector3 e)
+DBVT_INLINE void               btDbvtAabbMm::Expand(const btVector3& e)
 {
 mi-=e;mx+=e;
 }
        
 //
-DBVT_INLINE void               btDbvtAabbMm::SignedExpand(const btVector3 e)
+DBVT_INLINE void               btDbvtAabbMm::SignedExpand(const btVector3& e)
 {
-if(e.x()>0) mx.setX(mx.x()+e.x()); else mi.setX(mi.x()+e.x());
-if(e.y()>0) mx.setY(mx.y()+e.y()); else mi.setY(mi.y()+e.y());
-if(e.z()>0) mx.setZ(mx.z()+e.z()); else mi.setZ(mi.z()+e.z());
+if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]);
+if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]);
+if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]);
 }
        
 //
@@ -486,12 +494,19 @@ for(int i=0;i<3;++i)
 DBVT_INLINE bool               Intersect(      const btDbvtAabbMm& a,
                                                                        const btDbvtAabbMm& b)
 {
+#if    DBVT_INT0_IMPL == DBVT_IMPL_SSE
+const __m128   rt(_mm_or_ps(   _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)),
+                                                               _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi))));
+const __int32* pu((const __int32*)&rt);
+return((pu[0]|pu[1]|pu[2])==0);
+#else
 return(        (a.mi.x()<=b.mx.x())&&
                (a.mx.x()>=b.mi.x())&&
                (a.mi.y()<=b.mx.y())&&
                (a.mx.y()>=b.mi.y())&&
                (a.mi.z()<=b.mx.z())&&          
                (a.mx.z()>=b.mi.z()));
+#endif
 }
 
 //
@@ -558,32 +573,8 @@ return(txmax>0);
 DBVT_INLINE btScalar   Proximity(      const btDbvtAabbMm& a,
                                                                        const btDbvtAabbMm& b)
 {
-#if    DBVT_PROXIMITY_IMPL == DBVT_IMPL_SSE
-DBVT_ALIGN btScalar                                                    r[1];
-static DBVT_ALIGN const unsigned __int32       mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
-__asm
-       {
-       mov             eax,a
-       mov             ecx,b
-       movaps  xmm0,[eax]
-       movaps  xmm2,[ecx]
-       movaps  xmm1,[eax+16]
-       movaps  xmm3,[ecx+16]
-       addps   xmm0,xmm1
-       addps   xmm2,xmm3
-       subps   xmm0,xmm2
-       andps   xmm0,mask
-       movhlps xmm1,xmm0
-       addps   xmm0,xmm1
-       pshufd  xmm1,xmm0,1
-       addss   xmm0,xmm1
-       movss   r,xmm0
-       }
-return(r[0]);
-#else
 const btVector3        d=(a.mi+a.mx)-(b.mi+b.mx);
 return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
-#endif
 }
 
 //
@@ -592,36 +583,57 @@ DBVT_INLINE int                   Select( const btDbvtAabbMm& o,
                                                                const btDbvtAabbMm& b)
 {
 #if    DBVT_SELECT_IMPL == DBVT_IMPL_SSE
-DBVT_ALIGN __int32                                                     r[1];
 static DBVT_ALIGN const unsigned __int32       mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
-__asm
-       {
-       mov             eax,o
-       mov             ecx,a
-       mov             edx,b
-       movaps  xmm0,[eax]
-       movaps  xmm5,mask
-       addps   xmm0,[eax+16]   
-       movaps  xmm1,[ecx]
-       movaps  xmm2,[edx]
-       addps   xmm1,[ecx+16]
-       addps   xmm2,[edx+16]
-       subps   xmm1,xmm0
-       subps   xmm2,xmm0
-       andps   xmm1,xmm5
-       andps   xmm2,xmm5
-       movhlps xmm3,xmm1
-       movhlps xmm4,xmm2
-       addps   xmm1,xmm3
-       addps   xmm2,xmm4
-       pshufd  xmm3,xmm1,1
-       pshufd  xmm4,xmm2,1
-       addss   xmm1,xmm3
-       addss   xmm2,xmm4
-       cmpless xmm2,xmm1
-       movss   r,xmm2
-       }
-return(r[0]&1);
+       // TODO: the intrinsic version is 11% slower
+       #if DBVT_USE_INTRINSIC_SSE
+       __m128  omi(_mm_load_ps(o.mi));
+       omi=_mm_add_ps(omi,_mm_load_ps(o.mx));
+       __m128  ami(_mm_load_ps(a.mi));
+       ami=_mm_add_ps(ami,_mm_load_ps(a.mx));
+       ami=_mm_sub_ps(ami,omi);
+       ami=_mm_and_ps(ami,_mm_load_ps((const float*)mask));
+       __m128  bmi(_mm_load_ps(b.mi));
+       bmi=_mm_add_ps(bmi,_mm_load_ps(b.mx));
+       bmi=_mm_sub_ps(bmi,omi);
+       bmi=_mm_and_ps(bmi,_mm_load_ps((const float*)mask));
+       __m128  t0(_mm_movehl_ps(ami,ami));
+       ami=_mm_add_ps(ami,t0);
+       ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1));
+       __m128  t1(_mm_movehl_ps(bmi,bmi));
+       bmi=_mm_add_ps(bmi,t1);
+       bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1));
+       return(_mm_cmple_ss(bmi,ami).m128_u32[0]&1);
+       #else
+       DBVT_ALIGN __int32      r[1];
+       __asm
+               {
+               mov             eax,o
+               mov             ecx,a
+               mov             edx,b
+               movaps  xmm0,[eax]
+               movaps  xmm5,mask
+               addps   xmm0,[eax+16]   
+               movaps  xmm1,[ecx]
+               movaps  xmm2,[edx]
+               addps   xmm1,[ecx+16]
+               addps   xmm2,[edx+16]
+               subps   xmm1,xmm0
+               subps   xmm2,xmm0
+               andps   xmm1,xmm5
+               andps   xmm2,xmm5
+               movhlps xmm3,xmm1
+               movhlps xmm4,xmm2
+               addps   xmm1,xmm3
+               addps   xmm2,xmm4
+               pshufd  xmm3,xmm1,1
+               pshufd  xmm4,xmm2,1
+               addss   xmm1,xmm3
+               addss   xmm2,xmm4
+               cmpless xmm2,xmm1
+               movss   r,xmm2
+               }
+       return(r[0]&1);
+       #endif
 #else
 return(Proximity(o,a)<Proximity(o,b)?0:1);
 #endif
@@ -633,20 +645,14 @@ DBVT_INLINE void          Merge(  const btDbvtAabbMm& a,
                                                                btDbvtAabbMm& r)
 {
 #if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
-__asm
-       {
-       mov             eax,a
-       mov             edx,b
-       mov             ecx,r
-       movaps  xmm0,[eax+0]
-       movaps  xmm1,[edx+0]
-       movaps  xmm2,[eax+16]
-       movaps  xmm3,[edx+16]
-       minps   xmm0,xmm1
-       maxps   xmm2,xmm3
-       movaps  [ecx+0],xmm0
-       movaps  [ecx+16],xmm2
-       }
+__m128 ami(_mm_load_ps(a.mi));
+__m128 amx(_mm_load_ps(a.mx));
+__m128 bmi(_mm_load_ps(b.mi));
+__m128 bmx(_mm_load_ps(b.mx));
+ami=_mm_min_ps(ami,bmi);
+amx=_mm_max_ps(amx,bmx);
+_mm_store_ps(r.mi,ami);
+_mm_store_ps(r.mx,amx);
 #else
 for(int i=0;i<3;++i)
        {
@@ -717,7 +723,7 @@ if(root0&&root1)
        int                                                             treshold=DOUBLE_STACKSIZE-4;
        stack.resize(DOUBLE_STACKSIZE);
        stack[0]=sStkNN(root0,root1);
-       do      {
+       do      {               
                sStkNN  p=stack[--depth];
                if(depth>treshold)
                        {
@@ -838,12 +844,13 @@ collideTT(root0,root1,xform,policy);
 //
 DBVT_PREFIX
 inline void            btDbvt::collideTV(      const btDbvtNode* root,
-                                                                       const btDbvtVolume& volume,
+                                                                       const btDbvtVolume& vol,
                                                                        DBVT_IPOLICY)
 {
 DBVT_CHECKTYPE
 if(root)
        {
+       ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume(vol);
        btAlignedObjectArray<const btDbvtNode*> stack;
        stack.reserve(SIMPLE_STACKSIZE);
        stack.push_back(root);
@@ -1095,7 +1102,10 @@ if(root)
 #undef DBVT_IPOLICY
 #undef DBVT_CHECKTYPE
 #undef DBVT_IMPL_GENERIC
-#undef DBVT_IMPL_FPU0x86
 #undef DBVT_IMPL_SSE
+#undef DBVT_USE_INTRINSIC_SSE
+#undef DBVT_SELECT_IMPL
+#undef DBVT_MERGE_IMPL
+#undef DBVT_INT0_IMPL
 
 #endif
index 2d27f22..a57952f 100644 (file)
@@ -55,17 +55,15 @@ btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* o
        m_maxHandles = maxProxies;
        m_numHandles = 0;
        m_firstFreeHandle = 0;
-       m_firstAllocatedHandle = -1;
+       
 
        {
                for (int i = m_firstFreeHandle; i < maxProxies; i++)
                {
                        m_pHandles[i].SetNextFree(i + 1);
                        m_pHandles[i].m_uniqueId = i+2;//any UID will do, we just avoid too trivial values (0,1) for debugging purposes
-                       m_pHandles[i].SetNextAllocated(-1);
                }
                m_pHandles[maxProxies - 1].SetNextFree(0);
-               m_pHandles[maxProxies - 1].SetNextAllocated(-1);
        
        }
 
@@ -179,31 +177,29 @@ void      btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
        //first check for new overlapping pairs
        int i,j;
 
-       if (m_firstAllocatedHandle >= 0)
+       if (m_numHandles >= 0)
        {
 
-               btSimpleBroadphaseProxy* proxy0 = &m_pHandles[m_firstAllocatedHandle];
-
                for (i=0;i<m_numHandles;i++)
                {
-                       btSimpleBroadphaseProxy* proxy1 = &m_pHandles[m_firstAllocatedHandle];
+                       btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i];
 
-                       for (j=0;j<m_numHandles;j++)
+                       for (j=i+1;j<m_numHandles;j++)
                        {
-                               
-                               if (proxy0 != proxy1)
-                               {
-                                       btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
-                                       btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
+                               btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j];
+                               btAssert(proxy0 != proxy1);
 
-                                       if (aabbOverlap(p0,p1))
-                                       {
-                                               if ( !m_pairCache->findPair(proxy0,proxy1))
-                                               {
-                                                       m_pairCache->addOverlappingPair(proxy0,proxy1);
-                                               }
-                                       } else
+                               btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
+                               btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
+
+                               if (aabbOverlap(p0,p1))
+                               {
+                                       if ( !m_pairCache->findPair(proxy0,proxy1))
                                        {
+                                               m_pairCache->addOverlappingPair(proxy0,proxy1);
+                                       }
+                               } else
+                               {
                                        if (!m_pairCache->hasDeferredRemoval())
                                        {
                                                if ( m_pairCache->findPair(proxy0,proxy1))
@@ -211,19 +207,13 @@ void      btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
                                                        m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher);
                                                }
                                        }
-
-                                       }
                                }
-                               proxy1 = &m_pHandles[proxy1->GetNextAllocated()];
-
                        }
-                       proxy0 = &m_pHandles[proxy0->GetNextAllocated()];
-
                }
 
                if (m_ownsPairCache && m_pairCache->hasDeferredRemoval())
                {
-                       
+
                        btBroadphasePairArray&  overlappingPairArray = m_pairCache->getOverlappingPairArray();
 
                        //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
@@ -237,11 +227,11 @@ void      btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
                        previousPair.m_pProxy0 = 0;
                        previousPair.m_pProxy1 = 0;
                        previousPair.m_algorithm = 0;
-                       
-                       
+
+
                        for (i=0;i<overlappingPairArray.size();i++)
                        {
-                       
+
                                btBroadphasePair& pair = overlappingPairArray[i];
 
                                bool isDuplicate = (pair == previousPair);
@@ -268,31 +258,31 @@ void      btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
                                        //should have no algorithm
                                        btAssert(!pair.m_algorithm);
                                }
-                               
+
                                if (needsRemoval)
                                {
                                        m_pairCache->cleanOverlappingPair(pair,dispatcher);
 
-                       //              m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
-                       //              m_overlappingPairArray.pop_back();
+                                       //              m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
+                                       //              m_overlappingPairArray.pop_back();
                                        pair.m_pProxy0 = 0;
                                        pair.m_pProxy1 = 0;
                                        m_invalidPair++;
                                        gOverlappingPairs--;
                                } 
-                               
+
                        }
 
-               ///if you don't like to skip the invalid pairs in the array, execute following code:
-               #define CLEAN_INVALID_PAIRS 1
-               #ifdef CLEAN_INVALID_PAIRS
+                       ///if you don't like to skip the invalid pairs in the array, execute following code:
+#define CLEAN_INVALID_PAIRS 1
+#ifdef CLEAN_INVALID_PAIRS
 
                        //perform a sort, to sort 'invalid' pairs to the end
                        overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
 
                        overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
                        m_invalidPair = 0;
-               #endif//CLEAN_INVALID_PAIRS
+#endif//CLEAN_INVALID_PAIRS
 
                }
        }
index 49dfeb8..e2ebb82 100644 (file)
@@ -25,7 +25,7 @@ struct btSimpleBroadphaseProxy : public btBroadphaseProxy
        btVector3       m_min;
        btVector3       m_max;
        int                     m_nextFree;
-       int                     m_nextAllocated;
+       
 //     int                     m_handleId;
 
        
@@ -42,8 +42,7 @@ struct btSimpleBroadphaseProxy : public btBroadphaseProxy
        SIMD_FORCE_INLINE void SetNextFree(int next) {m_nextFree = next;}
        SIMD_FORCE_INLINE int GetNextFree() const {return m_nextFree;}
 
-       SIMD_FORCE_INLINE void SetNextAllocated(int next) {m_nextAllocated = next;}
-       SIMD_FORCE_INLINE int GetNextAllocated() const {return m_nextAllocated;}
+       
 
 
 };
@@ -57,22 +56,18 @@ protected:
 
        int             m_numHandles;                                           // number of active handles
        int             m_maxHandles;                                           // max number of handles
+       
        btSimpleBroadphaseProxy* m_pHandles;                                            // handles pool
+
        void* m_pHandlesRawPtr;
        int             m_firstFreeHandle;              // free handles list
-       int             m_firstAllocatedHandle;
-
+       
        int allocHandle()
        {
-
+               btAssert(m_numHandles < m_maxHandles);
                int freeHandle = m_firstFreeHandle;
                m_firstFreeHandle = m_pHandles[freeHandle].GetNextFree();
-               
-               m_pHandles[freeHandle].SetNextAllocated(m_firstAllocatedHandle);
-               m_firstAllocatedHandle = freeHandle;
-               
                m_numHandles++;
-
                return freeHandle;
        }
 
@@ -84,13 +79,9 @@ protected:
                proxy->SetNextFree(m_firstFreeHandle);
                m_firstFreeHandle = handle;
 
-               m_firstAllocatedHandle = proxy->GetNextAllocated();
-               proxy->SetNextAllocated(-1);
-
                m_numHandles--;
        }
 
-
        btOverlappingPairCache* m_pairCache;
        bool    m_ownsPairCache;
 
index e565bf7..d77ca64 100644 (file)
@@ -5,56 +5,149 @@ ${BULLET_PHYSICS_SOURCE_DIR}/src }
        
 ADD_LIBRARY(LibBulletCollision
                                BroadphaseCollision/btAxisSweep3.cpp
+                               BroadphaseCollision/btAxisSweep3.h
                                BroadphaseCollision/btBroadphaseProxy.cpp
+                               BroadphaseCollision/btBroadphaseProxy.h
                                BroadphaseCollision/btCollisionAlgorithm.cpp
+                               BroadphaseCollision/btCollisionAlgorithm.h
                                BroadphaseCollision/btDispatcher.cpp
+                               BroadphaseCollision/btDispatcher.h
+                               BroadphaseCollision/btDbvtBroadphase.cpp
+                               BroadphaseCollision/btDbvtBroadphase.h
+                               BroadphaseCollision/btDbvt.cpp
+                               BroadphaseCollision/btDbvt.h
+                               BroadphaseCollision/btMultiSapBroadphase.cpp
+                               BroadphaseCollision/btMultiSapBroadphase.h
                                BroadphaseCollision/btOverlappingPairCache.cpp
+                               BroadphaseCollision/btOverlappingPairCache.h
+                               BroadphaseCollision/btOverlappingPairCallback.h
+                               BroadphaseCollision/btQuantizedBvh.cpp
+                               BroadphaseCollision/btQuantizedBvh.h
                                BroadphaseCollision/btSimpleBroadphase.cpp
+                               BroadphaseCollision/btSimpleBroadphase.h
                                CollisionDispatch/btCollisionDispatcher.cpp
+                               CollisionDispatch/btCollisionDispatcher.h
                                CollisionDispatch/btCollisionObject.cpp
+                               CollisionDispatch/btCollisionObject.h
                                CollisionDispatch/btCollisionWorld.cpp
+                               CollisionDispatch/btCollisionWorld.h
                                CollisionDispatch/btCompoundCollisionAlgorithm.cpp
+                               CollisionDispatch/btCompoundCollisionAlgorithm.h
                                CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
+                               CollisionDispatch/btConvexConcaveCollisionAlgorithm.h
+                               CollisionDispatch/btDefaultCollisionConfiguration.cpp
+                               CollisionDispatch/btDefaultCollisionConfiguration.h
                                CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
+                               CollisionDispatch/btSphereSphereCollisionAlgorithm.h
+                               CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp
+                               CollisionDispatch/btBoxBoxCollisionAlgorithm.h
+                               CollisionDispatch/btBoxBoxDetector.cpp
+                               CollisionDispatch/btBoxBoxDetector.h
                                CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
+                               CollisionDispatch/btSphereBoxCollisionAlgorithm.h
+                               CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
+                               CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
+                               CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
+                               CollisionDispatch/btSphereTriangleCollisionAlgorithm.h
                                CollisionDispatch/btConvexConvexAlgorithm.cpp
+                               CollisionDispatch/btConvexConvexAlgorithm.h
                                CollisionDispatch/btEmptyCollisionAlgorithm.cpp
+                               CollisionDispatch/btEmptyCollisionAlgorithm.h
                                CollisionDispatch/btManifoldResult.cpp
+                               CollisionDispatch/btManifoldResult.h
                                CollisionDispatch/btSimulationIslandManager.cpp
+                               CollisionDispatch/btSimulationIslandManager.h
                                CollisionDispatch/btUnionFind.cpp
+                               CollisionDispatch/btUnionFind.h
+                               CollisionDispatch/SphereTriangleDetector.cpp
+                               CollisionDispatch/SphereTriangleDetector.h
                                CollisionShapes/btBoxShape.cpp
+                               CollisionShapes/btBoxShape.h
                                CollisionShapes/btBvhTriangleMeshShape.cpp
+                               CollisionShapes/btBvhTriangleMeshShape.h
+                               CollisionShapes/btCapsuleShape.cpp
+                               CollisionShapes/btCapsuleShape.h
                                CollisionShapes/btCollisionShape.cpp
+                               CollisionShapes/btCollisionShape.h
                                CollisionShapes/btCompoundShape.cpp
+                               CollisionShapes/btCompoundShape.h
                                CollisionShapes/btConcaveShape.cpp
+                               CollisionShapes/btConcaveShape.h
                                CollisionShapes/btConeShape.cpp
+                               CollisionShapes/btConeShape.h
                                CollisionShapes/btConvexHullShape.cpp
+                               CollisionShapes/btConvexHullShape.h
                                CollisionShapes/btConvexShape.cpp
+                               CollisionShapes/btConvexShape.h
+                               CollisionShapes/btConvexInternalShape.cpp
+                               CollisionShapes/btConvexInternalShape.h
                                CollisionShapes/btConvexTriangleMeshShape.cpp
+                               CollisionShapes/btConvexTriangleMeshShape.h
                                CollisionShapes/btCylinderShape.cpp
+                               CollisionShapes/btCylinderShape.h
                                CollisionShapes/btEmptyShape.cpp
+                               CollisionShapes/btEmptyShape.h
+                               CollisionShapes/btHeightfieldTerrainShape.cpp
+                               CollisionShapes/btHeightfieldTerrainShape.h
                                CollisionShapes/btMinkowskiSumShape.cpp
+                               CollisionShapes/btMinkowskiSumShape.h
+                               CollisionShapes/btMaterial.h
+                               CollisionShapes/btMultimaterialTriangleMeshShape.cpp
+                               CollisionShapes/btMultimaterialTriangleMeshShape.h
                                CollisionShapes/btMultiSphereShape.cpp
+                               CollisionShapes/btMultiSphereShape.h
                                CollisionShapes/btOptimizedBvh.cpp
+                               CollisionShapes/btOptimizedBvh.h
                                CollisionShapes/btPolyhedralConvexShape.cpp
+                               CollisionShapes/btPolyhedralConvexShape.h
+                               CollisionShapes/btScaledBvhTriangleMeshShape.cpp
+                               CollisionShapes/btScaledBvhTriangleMeshShape.h
                                CollisionShapes/btTetrahedronShape.cpp
+                               CollisionShapes/btTetrahedronShape.h
                                CollisionShapes/btSphereShape.cpp
+                               CollisionShapes/btSphereShape.h
+                               CollisionShapes/btShapeHull.h
+                               CollisionShapes/btShapeHull.cpp
                                CollisionShapes/btStaticPlaneShape.cpp
+                               CollisionShapes/btStaticPlaneShape.h
                                CollisionShapes/btStridingMeshInterface.cpp
+                               CollisionShapes/btStridingMeshInterface.h
                                CollisionShapes/btTriangleCallback.cpp
+                               CollisionShapes/btTriangleCallback.h
                                CollisionShapes/btTriangleBuffer.cpp
+                               CollisionShapes/btTriangleBuffer.h
                                CollisionShapes/btTriangleIndexVertexArray.cpp
+                               CollisionShapes/btTriangleIndexVertexArray.h
+                               CollisionShapes/btTriangleIndexVertexMaterialArray.h
+                               CollisionShapes/btTriangleIndexVertexMaterialArray.cpp
                                CollisionShapes/btTriangleMesh.cpp
+                               CollisionShapes/btTriangleMesh.h
                                CollisionShapes/btTriangleMeshShape.cpp
+                               CollisionShapes/btTriangleMeshShape.h
+                               CollisionShapes/btUniformScalingShape.cpp
+                               CollisionShapes/btUniformScalingShape.h
                                NarrowPhaseCollision/btContinuousConvexCollision.cpp
+                               NarrowPhaseCollision/btContinuousConvexCollision.h
                                NarrowPhaseCollision/btGjkEpa.cpp
+                               NarrowPhaseCollision/btGjkEpa.h
+                               NarrowPhaseCollision/btGjkEpa2.cpp
+                               NarrowPhaseCollision/btGjkEpa2.h
                                NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
+                               NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
                                NarrowPhaseCollision/btConvexCast.cpp
+                               NarrowPhaseCollision/btConvexCast.h
                                NarrowPhaseCollision/btGjkConvexCast.cpp
+                               NarrowPhaseCollision/btGjkConvexCast.h
                                NarrowPhaseCollision/btGjkPairDetector.cpp
+                               NarrowPhaseCollision/btGjkPairDetector.h
                                NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
+                               NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
                                NarrowPhaseCollision/btPersistentManifold.cpp
+                               NarrowPhaseCollision/btPersistentManifold.h
                                NarrowPhaseCollision/btRaycastCallback.cpp
+                               NarrowPhaseCollision/btRaycastCallback.h
                                NarrowPhaseCollision/btSubSimplexConvexCast.cpp
+                               NarrowPhaseCollision/btSubSimplexConvexCast.h
                                NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
+                               NarrowPhaseCollision/btVoronoiSimplexSolver.h
 )
index e556046..45ebff5 100644 (file)
@@ -226,7 +226,7 @@ void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[])
     a = btScalar(j)*(2*M__PI/m) + A[i0];
     if (a > M__PI) a -= 2*M__PI;
     btScalar maxdiff=1e9,diff;
-#ifndef dNODEBUG
+#if defined(DEBUG) || defined (_DEBUG)
     *iret = i0;                        // iret is not allowed to keep this value
 #endif
     for (i=0; i<n; i++) {
@@ -239,7 +239,7 @@ void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[])
        }
       }
     }
-#ifndef dNODEBUG
+#if defined(DEBUG) || defined (_DEBUG)
     btAssert (*iret != i0);    // ensure iret got set
 #endif
     avail[*iret] = 0;
index d164124..eebd0c9 100644 (file)
@@ -31,7 +31,7 @@ btCollisionObject::btCollisionObject()
                m_internalType(CO_COLLISION_OBJECT),
                m_hitFraction(btScalar(1.)),
                m_ccdSweptSphereRadius(btScalar(0.)),
-               m_ccdSquareMotionThreshold(btScalar(0.)),
+               m_ccdMotionThreshold(btScalar(0.)),
                m_checkCollideWith(false)
 {
        
index 96cbd1c..eb297ef 100644 (file)
@@ -81,8 +81,8 @@ protected:
        ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
        btScalar                m_ccdSweptSphereRadius;
 
-       /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold
-       btScalar                m_ccdSquareMotionThreshold;
+       /// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold
+       btScalar                m_ccdMotionThreshold;
        
        /// If some object should have elaborate collision filtering by sub-classes
        bool                    m_checkCollideWith;
@@ -332,16 +332,22 @@ public:
                m_ccdSweptSphereRadius = radius;
        }
 
+       btScalar        getCcdMotionThreshold() const
+       {
+               return m_ccdMotionThreshold;
+       }
+
        btScalar        getCcdSquareMotionThreshold() const
        {
-               return m_ccdSquareMotionThreshold;
+               return m_ccdMotionThreshold*m_ccdMotionThreshold;
        }
 
 
-       /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold
-       void    setCcdSquareMotionThreshold(btScalar ccdSquareMotionThreshold)
+
+       /// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold
+       void    setCcdMotionThreshold(btScalar ccdMotionThreshold)
        {
-               m_ccdSquareMotionThreshold = ccdSquareMotionThreshold;
+               m_ccdMotionThreshold = ccdMotionThreshold*ccdMotionThreshold;
        }
 
        ///users can point to their objects, userPointer is not used by Bullet
index 2fd972a..d8674a3 100644 (file)
@@ -127,6 +127,11 @@ void       btCollisionWorld::updateAabbs()
                {
                        btPoint3 minAabb,maxAabb;
                        colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
+                       //need to increase the aabb for contact thresholds
+                       btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
+                       minAabb -= contactThreshold;
+                       maxAabb += contactThreshold;
+
                        btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
 
                        //moving objects should be moderately sized, probably something wrong if not
@@ -381,14 +386,14 @@ void      btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
                                        btTransform childWorldTrans = colObjWorldTransform * childTrans;
                                        // replace collision shape so that callback can determine the triangle
                                        btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
-                                       collisionObject->setCollisionShape((btCollisionShape*)childCollisionShape);
+                                       collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
                                        rayTestSingle(rayFromTrans,rayToTrans,
                                                collisionObject,
                                                childCollisionShape,
                                                childWorldTrans,
                                                resultCallback);
                                        // restore
-                                       collisionObject->setCollisionShape(saveCollisionShape);
+                                       collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
                                }
                        }
                }
@@ -577,14 +582,14 @@ void      btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt
                                        btTransform childWorldTrans = colObjWorldTransform * childTrans;
                                        // replace collision shape so that callback can determine the triangle
                                        btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
-                                       collisionObject->setCollisionShape((btCollisionShape*)childCollisionShape);
+                                       collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
                                        objectQuerySingle(castShape, convexFromTrans,convexToTrans,
                                                collisionObject,
                                                childCollisionShape,
                                                childWorldTrans,
                                                resultCallback, allowedPenetration);
                                        // restore
-                                       collisionObject->setCollisionShape(saveCollisionShape);
+                                       collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
                                }
                        }
                }
index a29a6d6..da9336c 100644 (file)
@@ -16,12 +16,17 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
-
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
+#include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btAabbUtil2.h"
 
 btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
 :btCollisionAlgorithm(ci),
-m_isSwapped(isSwapped)
+m_isSwapped(isSwapped),
+m_sharedManifold(ci.m_manifold)
 {
+       m_ownsManifold = false;
+
        btCollisionObject* colObj = m_isSwapped? body1 : body0;
        btCollisionObject* otherObj = m_isSwapped? body0 : body1;
        assert (colObj->getCollisionShape()->isCompound());
@@ -33,11 +38,17 @@ m_isSwapped(isSwapped)
        m_childCollisionAlgorithms.resize(numChildren);
        for (i=0;i<numChildren;i++)
        {
-               btCollisionShape* tmpShape = colObj->getCollisionShape();
-               btCollisionShape* childShape = compoundShape->getChildShape(i);
-               colObj->internalSetTemporaryCollisionShape( childShape );
-               m_childCollisionAlgorithms[i] = ci.m_dispatcher1->findAlgorithm(colObj,otherObj);
-               colObj->internalSetTemporaryCollisionShape( tmpShape );
+               if (compoundShape->getDynamicAabbTree())
+               {
+                       m_childCollisionAlgorithms[i] = 0;
+               } else
+               {
+                       btCollisionShape* tmpShape = colObj->getCollisionShape();
+                       btCollisionShape* childShape = compoundShape->getChildShape(i);
+                       colObj->internalSetTemporaryCollisionShape( childShape );
+                       m_childCollisionAlgorithms[i] = ci.m_dispatcher1->findAlgorithm(colObj,otherObj,m_sharedManifold);
+                       colObj->internalSetTemporaryCollisionShape( tmpShape );
+               }
        }
 }
 
@@ -48,11 +59,109 @@ btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
        int i;
        for (i=0;i<numChildren;i++)
        {
-               m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
-               m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
+               if (m_childCollisionAlgorithms[i])
+               {
+                       m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
+                       m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
+               }
        }
 }
 
+
+
+
+struct btCompoundLeafCallback : btDbvt::ICollide
+{
+
+public:
+
+       btCollisionObject* m_compoundColObj;
+       btCollisionObject* m_otherObj;
+       btDispatcher* m_dispatcher;
+       const btDispatcherInfo& m_dispatchInfo;
+       btManifoldResult*       m_resultOut;
+       btCollisionAlgorithm**  m_childCollisionAlgorithms;
+       btPersistentManifold*   m_sharedManifold;
+
+
+
+
+       btCompoundLeafCallback (btCollisionObject* compoundObj,btCollisionObject* otherObj,btDispatcher* dispatcher,const btDispatcherInfo& dispatchInfo,btManifoldResult*      resultOut,btCollisionAlgorithm**        childCollisionAlgorithms,btPersistentManifold*  sharedManifold)
+               :m_compoundColObj(compoundObj),m_otherObj(otherObj),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
+               m_childCollisionAlgorithms(childCollisionAlgorithms),
+               m_sharedManifold(sharedManifold)
+       {
+
+       }
+
+
+       void    ProcessChildShape(btCollisionShape* childShape,int index)
+       {
+               
+               btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape());
+
+
+               //backup
+               btTransform     orgTrans = m_compoundColObj->getWorldTransform();
+               btTransform     orgInterpolationTrans = m_compoundColObj->getInterpolationWorldTransform();
+               const btTransform& childTrans = compoundShape->getChildTransform(index);
+               btTransform     newChildWorldTrans = orgTrans*childTrans ;
+
+               //perform an AABB check first
+               btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
+               childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
+               m_otherObj->getCollisionShape()->getAabb(m_otherObj->getWorldTransform(),aabbMin1,aabbMax1);
+
+               if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+               {
+
+                       m_compoundColObj->setWorldTransform( newChildWorldTrans);
+                       m_compoundColObj->setInterpolationWorldTransform(newChildWorldTrans);
+
+                       //the contactpoint is still projected back using the original inverted worldtrans
+                       btCollisionShape* tmpShape = m_compoundColObj->getCollisionShape();
+                       m_compoundColObj->internalSetTemporaryCollisionShape( childShape );
+
+                       if (!m_childCollisionAlgorithms[index])
+                               m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(m_compoundColObj,m_otherObj,m_sharedManifold);
+
+                       m_childCollisionAlgorithms[index]->processCollision(m_compoundColObj,m_otherObj,m_dispatchInfo,m_resultOut);
+                       if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+                       {
+                               btVector3 worldAabbMin,worldAabbMax;
+                               m_dispatchInfo.m_debugDraw->drawAabb(aabbMin0,aabbMax0,btVector3(1,1,1));
+                               m_dispatchInfo.m_debugDraw->drawAabb(aabbMin1,aabbMax1,btVector3(1,1,1));
+                       }
+                       
+                       //revert back transform
+                       m_compoundColObj->internalSetTemporaryCollisionShape( tmpShape);
+                       m_compoundColObj->setWorldTransform(  orgTrans );
+                       m_compoundColObj->setInterpolationWorldTransform(orgInterpolationTrans);
+               }
+       }
+       void            Process(const btDbvtNode* leaf)
+       {
+               int index = int(leaf->data);
+
+               btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape());
+               btCollisionShape* childShape = compoundShape->getChildShape(index);
+               if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+               {
+                       btVector3 worldAabbMin,worldAabbMax;
+                       btTransform     orgTrans = m_compoundColObj->getWorldTransform();
+                       btTransformAabb(leaf->volume.Mins(),leaf->volume.Maxs(),0.,orgTrans,worldAabbMin,worldAabbMax);
+                       m_dispatchInfo.m_debugDraw->drawAabb(worldAabbMin,worldAabbMax,btVector3(1,0,0));
+               }
+               ProcessChildShape(childShape,index);
+
+       }
+};
+
+
+
+
+
+
 void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 {
        btCollisionObject* colObj = m_isSwapped? body1 : body0;
@@ -61,37 +170,69 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt
        assert (colObj->getCollisionShape()->isCompound());
        btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
 
-       //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
-       //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
-       //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.
+       btDbvt* tree = compoundShape->getDynamicAabbTree();
+       //use a dynamic aabb tree to cull potential child-overlaps
+       btCompoundLeafCallback  callback(colObj,otherObj,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
 
-       int numChildren = m_childCollisionAlgorithms.size();
-       int i;
-       for (i=0;i<numChildren;i++)
+
+       if (tree)
        {
-               //temporarily exchange parent btCollisionShape with childShape, and recurse
-               btCollisionShape* childShape = compoundShape->getChildShape(i);
 
-               //backup
-               btTransform     orgTrans = colObj->getWorldTransform();
-               btTransform     orgInterpolationTrans = colObj->getInterpolationWorldTransform();
+               btVector3 localAabbMin,localAabbMax;
+               btTransform otherInCompoundSpace;
+               otherInCompoundSpace = colObj->getWorldTransform().inverse() * otherObj->getWorldTransform();
+               otherObj->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax);
 
-               const btTransform& childTrans = compoundShape->getChildTransform(i);
-               btTransform     newChildWorldTrans = orgTrans*childTrans ;
-               colObj->setWorldTransform( newChildWorldTrans);
-               colObj->setInterpolationWorldTransform(newChildWorldTrans);
+               const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
+               //process all children, that overlap with  the given AABB bounds
+               tree->collideTV(tree->m_root,bounds,callback);
+
+       } else
+       {
+               //iterate over all children, perform an AABB check inside ProcessChildShape
+               int numChildren = m_childCollisionAlgorithms.size();
+               int i;
+               for (i=0;i<numChildren;i++)
+               {
+                       callback.ProcessChildShape(compoundShape->getChildShape(i),i);
+               }
+       }
+
+       {
+                               //iterate over all children, perform an AABB check inside ProcessChildShape
+               int numChildren = m_childCollisionAlgorithms.size();
+               int i;
+               btManifoldArray manifoldArray;
+
+               for (i=0;i<numChildren;i++)
+               {
+                       if (m_childCollisionAlgorithms[i])
+                       {
+                               btCollisionShape* childShape = compoundShape->getChildShape(i);
+                       //if not longer overlapping, remove the algorithm
+                               btTransform     orgTrans = colObj->getWorldTransform();
+                               btTransform     orgInterpolationTrans = colObj->getInterpolationWorldTransform();
+                               const btTransform& childTrans = compoundShape->getChildTransform(i);
+                               btTransform     newChildWorldTrans = orgTrans*childTrans ;
+
+                               //perform an AABB check first
+                               btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
+                               childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
+                               otherObj->getCollisionShape()->getAabb(otherObj->getWorldTransform(),aabbMin1,aabbMax1);
+
+                               if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+                               {
+                                       m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
+                                       m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
+                                       m_childCollisionAlgorithms[i] = 0;
+                               }
+
+                       }
+                       
+               }
+
+               
 
-               //the contactpoint is still projected back using the original inverted worldtrans
-               btCollisionShape* tmpShape = colObj->getCollisionShape();
-               colObj->internalSetTemporaryCollisionShape( childShape );
-               m_childCollisionAlgorithms[i]->processCollision(colObj,otherObj,dispatchInfo,resultOut);
-               //revert back
-               colObj->internalSetTemporaryCollisionShape( tmpShape);
-               colObj->setWorldTransform(  orgTrans );
-               colObj->setInterpolationWorldTransform(orgInterpolationTrans);
        }
 }
 
index 1682c67..624a3cf 100644 (file)
@@ -28,11 +28,13 @@ class btDispatcher;
 class btDispatcher;
 
 /// btCompoundCollisionAlgorithm  supports collision between CompoundCollisionShapes and other collision shapes
-/// Place holder, not fully implemented yet
 class btCompoundCollisionAlgorithm  : public btCollisionAlgorithm
 {
        btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms;
        bool m_isSwapped;
+
+       class btPersistentManifold*     m_sharedManifold;
+       bool                                    m_ownsManifold;
        
 public:
 
@@ -49,7 +51,8 @@ public:
                int i;
                for (i=0;i<m_childCollisionAlgorithms.size();i++)
                {
-                       m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
+                       if (m_childCollisionAlgorithms[i])
+                               m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
                }
        }
 
index 3f94f4d..1c31708 100644 (file)
@@ -22,7 +22,9 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
 #include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h"
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
 #include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
@@ -70,11 +72,14 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault
        
        mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc),16);
        m_sphereSphereCF = new(mem) btSphereSphereCollisionAlgorithm::CreateFunc;
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
        mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16);
        m_sphereBoxCF = new(mem) btSphereBoxCollisionAlgorithm::CreateFunc;
        mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16);
        m_boxSphereCF = new (mem)btSphereBoxCollisionAlgorithm::CreateFunc;
        m_boxSphereCF->m_swapped = true;
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+
        mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16);
        m_sphereTriangleCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc;
        mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16);
@@ -176,10 +181,13 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration()
        m_sphereSphereCF->~btCollisionAlgorithmCreateFunc();
        btAlignedFree( m_sphereSphereCF);
 
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
        m_sphereBoxCF->~btCollisionAlgorithmCreateFunc();
        btAlignedFree( m_sphereBoxCF);
        m_boxSphereCF->~btCollisionAlgorithmCreateFunc();
        btAlignedFree( m_boxSphereCF);
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+
        m_sphereTriangleCF->~btCollisionAlgorithmCreateFunc();
        btAlignedFree( m_sphereTriangleCF);
        m_triangleSphereCF->~btCollisionAlgorithmCreateFunc();
@@ -212,7 +220,7 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg
        {
                return  m_sphereSphereCF;
        }
-
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
        if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==BOX_SHAPE_PROXYTYPE))
        {
                return  m_sphereBoxCF;
@@ -222,6 +230,8 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg
        {
                return  m_boxSphereCF;
        }
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+
 
        if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE ) && (proxyType1==TRIANGLE_SHAPE_PROXYTYPE))
        {
index 0b274d3..4f5af5f 100644 (file)
@@ -71,8 +71,11 @@ class        btDefaultCollisionConfiguration : public btCollisionConfiguration
        btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc;
        btCollisionAlgorithmCreateFunc* m_emptyCreateFunc;
        btCollisionAlgorithmCreateFunc* m_sphereSphereCF;
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
        btCollisionAlgorithmCreateFunc* m_sphereBoxCF;
        btCollisionAlgorithmCreateFunc* m_boxSphereCF;
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+
        btCollisionAlgorithmCreateFunc* m_boxBoxCF;
        btCollisionAlgorithmCreateFunc* m_sphereTriangleCF;
        btCollisionAlgorithmCreateFunc* m_triangleSphereCF;
index 46961f5..fdbd4ab 100644 (file)
@@ -63,7 +63,7 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co
        input.m_transformA = sphereObj->getWorldTransform();
        input.m_transformB = triObj->getWorldTransform();
 
-       bool swapResults = m_swapped && !m_ownManifold;
+       bool swapResults = m_swapped;
 
        detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw,swapResults);
 
index adac455..8bc2cfa 100644 (file)
@@ -21,19 +21,7 @@ subject to the following restrictions:
 
 void btBoxShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
 {
-       const btVector3& halfExtents = getHalfExtentsWithoutMargin();
-
-       btMatrix3x3 abs_b = t.getBasis().absolute();  
-       btPoint3 center = t.getOrigin();
-       btVector3 extent = btVector3(abs_b[0].dot(halfExtents),
-                  abs_b[1].dot(halfExtents),
-                 abs_b[2].dot(halfExtents));
-       extent += btVector3(getMargin(),getMargin(),getMargin());
-
-       aabbMin = center - extent;
-       aabbMax = center + extent;
-
-
+       btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax);
 }
 
 
index 92bcce5..d4b046d 100644 (file)
@@ -49,10 +49,11 @@ public:
        {
                        btVector3 halfExtents(getRadius(),getRadius(),getRadius());
                        halfExtents[m_upAxis] = getRadius() + getHalfHeight();
+                       halfExtents += btVector3(getMargin(),getMargin(),getMargin());
                        btMatrix3x3 abs_b = t.getBasis().absolute();  
                        btPoint3 center = t.getOrigin();
                        btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));              
-                       extent += btVector3(getMargin(),getMargin(),getMargin());
+                       
                        aabbMin = center - extent;
                        aabbMax = center + extent;
        }
index 740f178..f08b810 100644 (file)
@@ -14,23 +14,29 @@ subject to the following restrictions:
 */
 
 #include "btCompoundShape.h"
-
-
 #include "btCollisionShape.h"
-
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
 
 btCompoundShape::btCompoundShape()
 :m_localAabbMin(btScalar(1e30),btScalar(1e30),btScalar(1e30)),
 m_localAabbMax(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)),
-m_aabbTree(0),
 m_collisionMargin(btScalar(0.)),
-m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.))
+m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)),
+m_dynamicAabbTree(0)
 {
+       void* mem = btAlignedAlloc(sizeof(btDbvt),16);
+       m_dynamicAabbTree = new(mem) btDbvt();
+       btAssert(mem==m_dynamicAabbTree);
 }
 
 
 btCompoundShape::~btCompoundShape()
 {
+       if (m_dynamicAabbTree)
+       {
+               m_dynamicAabbTree->~btDbvt();
+               btAlignedFree(m_dynamicAabbTree);
+       }
 }
 
 void   btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape)
@@ -60,71 +66,88 @@ void        btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio
                }
 
        }
+       if (m_dynamicAabbTree)
+       {
+               const btDbvtVolume      bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
+               int index = m_children.size()-1;
+               child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
+       }
+
+}
+
+void btCompoundShape::removeChildShapeByIndex(int childShapeIndex)
+{
+       btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size());
+       if (m_dynamicAabbTree)
+       {
+               m_dynamicAabbTree->remove(m_children[childShapeIndex].m_node);
+       }
+       m_children.swap(childShapeIndex,m_children.size()-1);
+       m_children.pop_back();
+
 }
 
 void btCompoundShape::removeChildShape(btCollisionShape* shape)
 {
-   bool done_removing;
-   
-   // Find the children containing the shape specified, and remove those children.
-   do
-   {
-      done_removing = true;
-      
-      for(int i = 0; i < m_children.size(); i++)
-      {
-         if(m_children[i].m_childShape == shape)
-         {
-            m_children.remove(m_children[i]);
-            done_removing = false;  // Do another iteration pass after removing from the vector
-            break;
-         }
-      }
-   }
-   while (!done_removing);
-   
-   recalculateLocalAabb();
+       // Find the children containing the shape specified, and remove those children.
+       //note: there might be multiple children using the same shape!
+       for(int i = m_children.size()-1; i >= 0 ; i--)
+       {
+               if(m_children[i].m_childShape == shape)
+               {
+                       m_children.swap(i,m_children.size()-1);
+                       m_children.pop_back();
+                       //remove it from the m_dynamicAabbTree too
+                       //m_dynamicAabbTree->remove(m_aabbProxies[i]);
+                       //m_aabbProxies.swap(i,m_children.size()-1);
+                       //m_aabbProxies.pop_back();
+               }
+       }
+
+
+
+       recalculateLocalAabb();
 }
 
 void btCompoundShape::recalculateLocalAabb()
 {
-   // Recalculate the local aabb
-   // Brute force, it iterates over all the shapes left.
-   m_localAabbMin = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30));
-   m_localAabbMax = btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
-   
-   //extend the local aabbMin/aabbMax
-   for (int j = 0; j < m_children.size(); j++)
-   {
-      btVector3 localAabbMin,localAabbMax;
-      m_children[j].m_childShape->getAabb(m_children[j].m_transform, 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];
-      }
-   }
+       // Recalculate the local aabb
+       // Brute force, it iterates over all the shapes left.
+       m_localAabbMin = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+       m_localAabbMax = btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
+
+       //extend the local aabbMin/aabbMax
+       for (int j = 0; j < m_children.size(); j++)
+       {
+               btVector3 localAabbMin,localAabbMax;
+               m_children[j].m_childShape->getAabb(m_children[j].m_transform, 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
+
+///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
 void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
 {
        btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin);
+       localHalfExtents += btVector3(getMargin(),getMargin(),getMargin());
        btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin);
-       
+
        btMatrix3x3 abs_b = trans.getBasis().absolute();  
 
        btPoint3 center = trans(localCenter);
 
        btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
-                  abs_b[1].dot(localHalfExtents),
-                 abs_b[2].dot(localHalfExtents));
-       extent += btVector3(getMargin(),getMargin(),getMargin());
+               abs_b[1].dot(localHalfExtents),
+               abs_b[2].dot(localHalfExtents));
+       aabbMin = center-extent;
+       aabbMax = center+extent;
 
-       aabbMin = center - extent;
-       aabbMax = center + extent;
 }
 
 void   btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
@@ -134,9 +157,9 @@ void        btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) co
        ident.setIdentity();
        btVector3 aabbMin,aabbMax;
        getAabb(ident,aabbMin,aabbMax);
-       
+
        btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
-       
+
        btScalar lx=btScalar(2.)*(halfExtents.x());
        btScalar ly=btScalar(2.)*(halfExtents.y());
        btScalar lz=btScalar(2.)*(halfExtents.z());
@@ -147,5 +170,62 @@ void       btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) co
 
 }
 
-       
-       
+
+
+
+void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const
+{
+       int n = m_children.size();
+
+       btScalar totalMass = 0;
+       btVector3 center(0, 0, 0);
+       for (int k = 0; k < n; k++)
+       {
+               center += m_children[k].m_transform.getOrigin() * masses[k];
+               totalMass += masses[k];
+       }
+       center /= totalMass;
+       principal.setOrigin(center);
+
+       btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
+       for (int k = 0; k < n; k++)
+       {
+               btVector3 i;
+               m_children[k].m_childShape->calculateLocalInertia(masses[k], i);
+
+               const btTransform& t = m_children[k].m_transform;
+               btVector3 o = t.getOrigin() - center;
+
+               //compute inertia tensor in coordinate system of compound shape
+               btMatrix3x3 j = t.getBasis().transpose();
+               j[0] *= i[0];
+               j[1] *= i[1];
+               j[2] *= i[2];
+               j = t.getBasis() * j;
+
+               //add inertia tensor
+               tensor[0] += j[0];
+               tensor[1] += j[1];
+               tensor[2] += j[2];
+
+               //compute inertia tensor of pointmass at o
+               btScalar o2 = o.length2();
+               j[0].setValue(o2, 0, 0);
+               j[1].setValue(0, o2, 0);
+               j[2].setValue(0, 0, o2);
+               j[0] += o * -o.x(); 
+               j[1] += o * -o.y(); 
+               j[2] += o * -o.z();
+
+               //add inertia tensor of pointmass
+               tensor[0] += masses[k] * j[0];
+               tensor[1] += masses[k] * j[1];
+               tensor[2] += masses[k] * j[2];
+       }
+
+       tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
+       inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
+}
+
+
+
index f77e4b5..3624749 100644 (file)
@@ -24,8 +24,8 @@ subject to the following restrictions:
 #include "btCollisionMargin.h"
 #include "LinearMath/btAlignedObjectArray.h"
 
-class btOptimizedBvh;
-
+//class btOptimizedBvh;
+struct btDbvt;
 
 ATTRIBUTE_ALIGNED16(struct) btCompoundShapeChild
 {
@@ -35,14 +35,15 @@ ATTRIBUTE_ALIGNED16(struct) btCompoundShapeChild
        btCollisionShape*       m_childShape;
        int                                     m_childShapeType;
        btScalar                        m_childMargin;
+       struct btDbvtNode*      m_node;
 };
 
 SIMD_FORCE_INLINE bool operator==(const btCompoundShapeChild& c1, const btCompoundShapeChild& c2)
 {
-   return  ( c1.m_transform      == c2.m_transform &&
-             c1.m_childShape     == c2.m_childShape &&
-             c1.m_childShapeType == c2.m_childShapeType &&
-             c1.m_childMargin    == c2.m_childMargin );
+       return  ( c1.m_transform      == c2.m_transform &&
+               c1.m_childShape     == c2.m_childShape &&
+               c1.m_childShapeType == c2.m_childShapeType &&
+               c1.m_childMargin    == c2.m_childMargin );
 }
 
 /// btCompoundShape allows to store multiple other btCollisionShapes
@@ -55,7 +56,8 @@ ATTRIBUTE_ALIGNED16(class) btCompoundShape    : public btCollisionShape
        btVector3                                               m_localAabbMin;
        btVector3                                               m_localAabbMax;
 
-       btOptimizedBvh*                                 m_aabbTree;
+       //btOptimizedBvh*                                       m_aabbTree;
+       btDbvt*                                                 m_dynamicAabbTree;
 
 public:
        BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -66,11 +68,11 @@ public:
 
        void    addChildShape(const btTransform& localTransform,btCollisionShape* shape);
 
-   /** Remove all children shapes that contain the specified shape. */
+       /// Remove all children shapes that contain the specified shape
        virtual void removeChildShape(btCollisionShape* shape);
 
-         
-       
+       void removeChildShapeByIndex(int childShapeindex);
+
 
        int             getNumChildShapes() const
        {
@@ -103,9 +105,9 @@ public:
 
        ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
        virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
-       
-   /** Re-calculate the local Aabb. Is called at the end of removeChildShapes. 
-       Use this yourself if you modify the children or their transforms. */
+
+       /** Re-calculate the local Aabb. Is called at the end of removeChildShapes. 
+       Use this yourself if you modify the children or their transforms. */
        virtual void recalculateLocalAabb(); 
 
        virtual void    setLocalScaling(const btVector3& scaling)
@@ -118,7 +120,7 @@ public:
        }
 
        virtual void    calculateLocalInertia(btScalar mass,btVector3& inertia) const;
-       
+
        virtual int     getShapeType() const { return COMPOUND_SHAPE_PROXYTYPE;}
 
        virtual void    setMargin(btScalar margin)
@@ -137,11 +139,19 @@ public:
        //this is optional, but should make collision queries faster, by culling non-overlapping nodes
        void    createAabbTreeFromChildren();
 
-       const btOptimizedBvh*                                   getAabbTree() const
+       btDbvt*                                                 getDynamicAabbTree()
        {
-               return m_aabbTree;
+               return m_dynamicAabbTree;
        }
 
+       ///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia
+       ///and the center of mass to the current coordinate system. "masses" points to an array of masses of the children. The resulting transform
+       ///"principal" has to be applied inversely to all children transforms in order for the local coordinate system of the compound
+       ///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform
+       ///of the collision object by the principal transform.
+       void calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const;
+
+
 private:
        btScalar        m_collisionMargin;
 protected:
index f1682aa..02a293e 100644 (file)
@@ -204,3 +204,113 @@ const btVector3& btConvexTriangleMeshShape::getLocalScaling() const
 {
        return m_stridingMesh->getScaling();
 }
+
+void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const
+{
+   class CenterCallback: public btInternalTriangleIndexCallback
+   {
+      bool first;
+      btVector3 ref;
+      btVector3 sum;
+      btScalar volume;
+
+   public:
+
+      CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0)
+      {
+      }
+
+      virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
+      {
+         (void) triangleIndex;
+         (void) partId;
+         if (first)
+         {
+            ref = triangle[0];
+            first = false;
+         }
+         else
+         {
+            btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref));
+            sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref));
+            volume += vol;
+         }
+      }
+      
+      btVector3 getCenter()
+      {
+         return (volume > 0) ? sum / volume : ref;
+      }
+
+      btScalar getVolume()
+      {
+         return volume * btScalar(1. / 6);
+      }
+
+   };
+
+   class InertiaCallback: public btInternalTriangleIndexCallback
+   {
+      btMatrix3x3 sum;
+      btVector3 center;
+
+   public:
+
+      InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center)
+      {
+      }
+
+      virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
+      {
+         (void) triangleIndex;
+         (void) partId;
+         btMatrix3x3 i;
+         btVector3 a = triangle[0] - center;
+         btVector3 b = triangle[1] - center;
+         btVector3 c = triangle[2] - center;
+         btVector3 abc = a + b + c;
+         btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6);
+         for (int j = 0; j < 3; j++)
+         {
+            for (int k = 0; k <= j; k++)
+            {
+               i[j][k] = i[k][j] = volNeg * (center[j] * center[k]
+                 + btScalar(0.25) * (center[j] * abc[k] + center[k] * abc[j])
+                  + btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k])
+                  + btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j]));
+            }
+         }
+         btScalar i00 = -i[0][0];
+         btScalar i11 = -i[1][1];
+         btScalar i22 = -i[2][2];
+         i[0][0] = i11 + i22; 
+         i[1][1] = i22 + i00; 
+         i[2][2] = i00 + i11;
+         sum[0] += i[0];
+         sum[1] += i[1];
+         sum[2] += i[2];
+      }
+      
+      btMatrix3x3& getInertia()
+      {
+         return sum;
+      }
+
+   };
+
+   CenterCallback centerCallback;
+   btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+   m_stridingMesh->InternalProcessAllTriangles(&centerCallback, -aabbMax, aabbMax);
+   btVector3 center = centerCallback.getCenter();
+   principal.setOrigin(center);
+   volume = centerCallback.getVolume();
+
+   InertiaCallback inertiaCallback(center);
+   m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax);
+
+   btMatrix3x3& i = inertiaCallback.getInertia();
+   i.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
+   inertia.setValue(i[0][0], i[1][1], i[2][2]);
+   inertia /= volume;
+}
+
index 01eecf2..6ff0bf5 100644 (file)
@@ -46,6 +46,13 @@ public:
        virtual void    setLocalScaling(const btVector3& scaling);
        virtual const btVector3& getLocalScaling() const;
 
+       ///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia
+       ///and the center of mass to the current coordinate system. A mass of 1 is assumed, for other masses just multiply the computed "inertia"
+       ///by the mass. The resulting transform "principal" has to be applied inversely to the mesh in order for the local coordinate system of the
+       ///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform
+       ///of the collision object by the principal transform. This method also computes the volume of the convex mesh.
+       void calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const;
+
 };
 
 
index 9e4800a..a291d6b 100644 (file)
@@ -91,19 +91,15 @@ btHeightfieldTerrainShape::~btHeightfieldTerrainShape()
 
 void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
 {
-/*
-       aabbMin.setValue(-1e30f,-1e30f,-1e30f);
-       aabbMax.setValue(1e30f,1e30f,1e30f);
-*/
-
        btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5);
+       halfExtents += btVector3(getMargin(),getMargin(),getMargin());
 
        btMatrix3x3 abs_b = t.getBasis().absolute();  
        btPoint3 center = t.getOrigin();
        btVector3 extent = btVector3(abs_b[0].dot(halfExtents),
                   abs_b[1].dot(halfExtents),
                  abs_b[2].dot(halfExtents));
-       extent += btVector3(getMargin(),getMargin(),getMargin());
+       
 
        aabbMin = center - extent;
        aabbMax = center + extent;
index 9b78fc0..a248c55 100644 (file)
@@ -306,6 +306,8 @@ void        btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int f
                                        if (curNodeSubPart >= 0)
                                                meshInterface->unLockReadOnlyVertexBase(curNodeSubPart);
                                        meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,   type,stride,&indexbase,indexstride,numfaces,indicestype,nodeSubPart);
+
+                                       curNodeSubPart = nodeSubPart;
                                        btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT);
                                }
                                //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts,
index 0d55e4f..4c4ce7f 100644 (file)
@@ -18,6 +18,7 @@ subject to the following restrictions:
 
 #include "LinearMath/btPoint3.h"
 #include "LinearMath/btMatrix3x3.h"
+#include "LinearMath/btAabbUtil2.h"
 #include "btConvexInternalShape.h"
 
 
@@ -46,28 +47,7 @@ public:
 
                //lazy evaluation of local aabb
                btAssert(m_isLocalAabbValid);
-
-               btAssert(m_localAabbMin.getX() <= m_localAabbMax.getX());
-               btAssert(m_localAabbMin.getY() <= m_localAabbMax.getY());
-               btAssert(m_localAabbMin.getZ() <= m_localAabbMax.getZ());
-
-
-               btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin);
-               btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin);
-               
-               btMatrix3x3 abs_b = trans.getBasis().absolute();  
-
-               btPoint3 center = trans(localCenter);
-
-               btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
-                          abs_b[1].dot(localHalfExtents),
-                         abs_b[2].dot(localHalfExtents));
-               extent += btVector3(margin,margin,margin);
-
-               aabbMin = center - extent;
-               aabbMax = center + extent;
-
-               
+               btTransformAabb(m_localAabbMin,m_localAabbMax,margin,trans,aabbMin,aabbMax);
        }
 
        
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp
new file mode 100644 (file)
index 0000000..5a17b4e
--- /dev/null
@@ -0,0 +1,103 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 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 "btScaledBvhTriangleMeshShape.h"
+
+btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,btVector3 localScaling)
+:m_bvhTriMeshShape(childShape),
+m_localScaling(localScaling)
+{
+
+}
+
+btScaledBvhTriangleMeshShape::~btScaledBvhTriangleMeshShape()
+{
+}
+
+
+class btScaledTriangleCallback : public btTriangleCallback
+{
+       btTriangleCallback* m_originalCallback;
+
+       btVector3       m_localScaling;
+
+public:
+
+       btScaledTriangleCallback(btTriangleCallback* originalCallback,btVector3 localScaling)
+               :m_originalCallback(originalCallback),
+               m_localScaling(localScaling)
+       {
+       }
+
+       virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
+       {
+               btVector3 newTriangle[3];
+               newTriangle[0] = triangle[0]*m_localScaling;
+               newTriangle[1] = triangle[1]*m_localScaling;
+               newTriangle[2] = triangle[2]*m_localScaling;
+               m_originalCallback->processTriangle(&newTriangle[0],partId,triangleIndex);
+       }
+};
+
+void   btScaledBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
+{
+       btScaledTriangleCallback scaledCallback(callback,m_localScaling);
+       
+       btVector3 invLocalScaling(1.f/m_localScaling.getX(),1.f/m_localScaling.getY(),1.f/m_localScaling.getZ());
+       btVector3 scaledAabbMin = aabbMin * invLocalScaling;
+       btVector3 scaledAabbMax = aabbMax * invLocalScaling;
+       m_bvhTriMeshShape->processAllTriangles(&scaledCallback,scaledAabbMin,scaledAabbMax);
+}
+
+
+void   btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
+{
+       btVector3 localAabbMin = m_bvhTriMeshShape->getLocalAabbMin();
+       btVector3 localAabbMax = m_bvhTriMeshShape->getLocalAabbMax();
+       localAabbMin *= m_localScaling;
+       localAabbMax *= m_localScaling;
+
+       btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin);
+       btScalar margin = m_bvhTriMeshShape->getMargin();
+       localHalfExtents += btVector3(margin,margin,margin);
+       btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin);
+       
+       btMatrix3x3 abs_b = trans.getBasis().absolute();  
+
+       btPoint3 center = trans(localCenter);
+
+       btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
+                  abs_b[1].dot(localHalfExtents),
+                 abs_b[2].dot(localHalfExtents));
+       aabbMin = center - extent;
+       aabbMax = center + extent;
+
+}
+
+void   btScaledBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling)
+{
+       m_localScaling = scaling;
+}
+
+const btVector3& btScaledBvhTriangleMeshShape::getLocalScaling() const
+{
+       return m_localScaling;
+}
+
+void   btScaledBvhTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
+{
+       ///don't make this a movable object!
+       btAssert(0);
+}
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h
new file mode 100644 (file)
index 0000000..8e1d29f
--- /dev/null
@@ -0,0 +1,57 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2008 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 SCALED_BVH_TRIANGLE_MESH_SHAPE_H
+#define SCALED_BVH_TRIANGLE_MESH_SHAPE_H
+
+#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
+
+
+///The btScaledBvhTriangleMeshShape allows to instance a scaled version of an existing btBvhTriangleMeshShape.
+///Note that each btBvhTriangleMeshShape still can have its own local scaling, independent from this btScaledBvhTriangleMeshShape 'localScaling'
+ATTRIBUTE_ALIGNED16(class) btScaledBvhTriangleMeshShape : public btConcaveShape
+{
+       
+       
+       btVector3       m_localScaling;
+
+       btBvhTriangleMeshShape* m_bvhTriMeshShape;
+
+public:
+
+
+       btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,btVector3 localScaling);
+
+       virtual ~btScaledBvhTriangleMeshShape();
+
+       virtual int     getShapeType() const
+       {
+               //use un-used 'FAST_CONCAVE_MESH_PROXYTYPE' for now, later add SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE to btBroadphaseProxy.h
+               return FAST_CONCAVE_MESH_PROXYTYPE;
+       }
+
+       virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
+       virtual void    setLocalScaling(const btVector3& scaling);
+       virtual const btVector3& getLocalScaling() const;
+       virtual void    calculateLocalInertia(btScalar mass,btVector3& inertia) const;
+
+       virtual void    processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
+
+       //debugging
+       virtual const char*     getName()const {return "SCALEDBVHTRIANGLEMESH";}
+
+};
+
+#endif //BVH_TRIANGLE_MESH_SHAPE_H
index cd0a00b..0a2c770 100644 (file)
@@ -47,6 +47,7 @@ void btTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,bt
 {
 
        btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin);
+       localHalfExtents += btVector3(getMargin(),getMargin(),getMargin());
        btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin);
        
        btMatrix3x3 abs_b = trans.getBasis().absolute();  
@@ -56,12 +57,10 @@ void btTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,bt
        btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
                   abs_b[1].dot(localHalfExtents),
                  abs_b[2].dot(localHalfExtents));
-       extent += btVector3(getMargin(),getMargin(),getMargin());
-
        aabbMin = center - extent;
        aabbMax = center + extent;
 
-       
+
 }
 
 void   btTriangleMeshShape::recalcLocalAabb()
index 9fba77c..c9eabaf 100644 (file)
@@ -65,6 +65,16 @@ public:
                return m_meshInterface;
        }
 
+       const btVector3& getLocalAabbMin() const
+       {
+               return m_localAabbMin;
+       }
+       const btVector3& getLocalAabbMax() const
+       {
+               return m_localAabbMax;
+       }
+
+
 
        //debugging
        virtual const char*     getName()const {return "TRIANGLEMESH";}
index 8598575..58b023e 100644 (file)
@@ -5,16 +5,32 @@ ${BULLET_PHYSICS_SOURCE_DIR}/src }
 ADD_LIBRARY(LibBulletDynamics
 
        ConstraintSolver/btContactConstraint.cpp
+       ConstraintSolver/btContactConstraint.h
+       ConstraintSolver/btConeTwistConstraint.cpp
+       ConstraintSolver/btConeTwistConstraint.h
        ConstraintSolver/btGeneric6DofConstraint.cpp
+       ConstraintSolver/btGeneric6DofConstraint.h
        ConstraintSolver/btHingeConstraint.cpp
+       ConstraintSolver/btHingeConstraint.h
        ConstraintSolver/btPoint2PointConstraint.cpp
+       ConstraintSolver/btPoint2PointConstraint.h
        ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+       ConstraintSolver/btSequentialImpulseConstraintSolver.h
+       ConstraintSolver/btSliderConstraint.cpp
+       ConstraintSolver/btSliderConstraint.h
        ConstraintSolver/btSolve2LinearConstraint.cpp
+       ConstraintSolver/btSolve2LinearConstraint.h
        ConstraintSolver/btTypedConstraint.cpp
+       ConstraintSolver/btTypedConstraint.h
+       Dynamics/Bullet-C-API.cpp
        Dynamics/btDiscreteDynamicsWorld.cpp
+       Dynamics/btDiscreteDynamicsWorld.h
        Dynamics/btSimpleDynamicsWorld.cpp
-       Dynamics/Bullet-C-API.cpp
+       Dynamics/btSimpleDynamicsWorld.h
        Dynamics/btRigidBody.cpp
+       Dynamics/btRigidBody.h
        Vehicle/btRaycastVehicle.cpp
+       Vehicle/btRaycastVehicle.h
        Vehicle/btWheelInfo.cpp
+       Vehicle/btWheelInfo.h
 )
index e11b49d..61dad52 100644 (file)
@@ -114,17 +114,34 @@ void      btConeTwistConstraint::buildJacobian()
 
        btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
 
+       btScalar swx=btScalar(0.),swy = btScalar(0.);
+       btScalar thresh = btScalar(10.);
+       btScalar fact;
+
        // Get Frame into world space
        if (m_swingSpan1 >= btScalar(0.05f))
        {
                b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
-               swing1  = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
+//             swing1  = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
+               swx = b2Axis1.dot(b1Axis1);
+               swy = b2Axis1.dot(b1Axis2);
+               swing1  = btAtan2Fast(swy, swx);
+               fact = (swy*swy + swx*swx) * thresh * thresh;
+               fact = fact / (fact + btScalar(1.0));
+               swing1 *= fact; 
+
        }
 
        if (m_swingSpan2 >= btScalar(0.05f))
        {
                b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);                     
-               swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
+//             swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
+               swx = b2Axis1.dot(b1Axis1);
+               swy = b2Axis1.dot(b1Axis3);
+               swing2  = btAtan2Fast(swy, swx);
+               fact = (swy*swy + swx*swx) * thresh * thresh;
+               fact = fact / (fact + btScalar(1.0));
+               swing2 *= fact; 
        }
 
        btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);             
index 114abce..a0523a8 100644 (file)
@@ -241,15 +241,18 @@ void      btHingeConstraint::buildJacobian()
        m_solveLimit = false;
        m_accLimitImpulse = btScalar(0.);
 
-       if (m_lowerLimit < m_upperLimit)
+//     if (m_lowerLimit < m_upperLimit)
+       if (m_lowerLimit <= m_upperLimit)
        {
-               if (hingeAngle <= m_lowerLimit*m_limitSoftness)
+//             if (hingeAngle <= m_lowerLimit*m_limitSoftness)
+               if (hingeAngle <= m_lowerLimit)
                {
                        m_correction = (m_lowerLimit - hingeAngle);
                        m_limitSign = 1.0f;
                        m_solveLimit = true;
                } 
-               else if (hingeAngle >= m_upperLimit*m_limitSoftness)
+//             else if (hingeAngle >= m_upperLimit*m_limitSoftness)
+               else if (hingeAngle >= m_upperLimit)
                {
                        m_correction = m_upperLimit - hingeAngle;
                        m_limitSign = -1.0f;
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.cpp
deleted file mode 100644 (file)
index 7d2f199..0000000
+++ /dev/null
@@ -1,278 +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 "btOdeContactJoint.h"
-#include "btOdeSolverBody.h"
-#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
-
-
-//this constant needs to be set up so different solvers give 'similar' results
-#define FRICTION_CONSTANT 120.f
-
-
-btOdeContactJoint::btOdeContactJoint(btPersistentManifold* manifold,int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1)
-:m_manifold(manifold),
-m_index(index),
-m_swapBodies(swap),
-m_body0(body0),
-m_body1(body1)
-{
-}
-
-int m_numRows = 3;
-
-
-void btOdeContactJoint::GetInfo1(Info1 *info)
-{
-       info->m = m_numRows;
-       //friction adds another 2...
-       
-       info->nub = 0;
-}
-
-#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 M_SQRT12 btScalar(0.7071067811865475244008443621048490)
-
-#define dRecipSqrt(x) ((float)(1.0f/btSqrt(float(x))))         /* reciprocal square root */
-
-
-
-void dPlaneSpace1 (const dVector3 n, dVector3 p, dVector3 q);
-void dPlaneSpace1 (const dVector3 n, dVector3 p, dVector3 q)
-{
-  if (btFabs(n[2]) > M_SQRT12) {
-    // choose p in y-z plane
-    btScalar a = n[1]*n[1] + n[2]*n[2];
-    btScalar k = dRecipSqrt (a);
-    p[0] = 0;
-    p[1] = -n[2]*k;
-    p[2] = n[1]*k;
-    // set q = n x p
-    q[0] = a*k;
-    q[1] = -n[0]*p[2];
-    q[2] = n[0]*p[1];
-  }
-  else {
-    // choose p in x-y plane
-    btScalar a = n[0]*n[0] + n[1]*n[1];
-    btScalar k = dRecipSqrt (a);
-    p[0] = -n[1]*k;
-    p[1] = n[0]*k;
-    p[2] = 0;
-    // set q = n x p
-    q[0] = -n[2]*p[1];
-    q[1] = n[2]*p[0];
-    q[2] = a*k;
-  }
-}
-
-
-
-void btOdeContactJoint::GetInfo2(Info2 *info)
-{
-       
-       int s = info->rowskip;
-       int s2 = 2*s;
-       
-       float swapFactor = m_swapBodies ? -1.f : 1.f;
-       
-       // get normal, with sign adjusted for body1/body2 polarity
-       dVector3 normal;
-       
-       
-       btManifoldPoint& point = m_manifold->getContactPoint(m_index);
-       
-       normal[0] = swapFactor*point.m_normalWorldOnB.x();
-       normal[1] = swapFactor*point.m_normalWorldOnB.y();
-       normal[2] = swapFactor*point.m_normalWorldOnB.z();
-       normal[3] = 0;  // @@@ hmmm
-       
-       assert(m_body0);
-       //      if (GetBody0())
-       btVector3 relativePositionA;
-       {
-               relativePositionA = point.getPositionWorldOnA() - m_body0->m_centerOfMassPosition;
-               dVector3 c1;
-               c1[0] = relativePositionA.x();
-               c1[1] = relativePositionA.y();
-               c1[2] = relativePositionA.z();
-               
-               // set jacobian for normal
-               info->J1l[0] = normal[0];
-               info->J1l[1] = normal[1];
-               info->J1l[2] = normal[2];
-               dCROSS (info->J1a,=,c1,normal);
-               
-       }
-
-       btVector3 relativePositionB(0,0,0);
-       if (m_body1)
-       {
-               //              if (GetBody1())
-               
-               {
-                       dVector3 c2;
-                       btVector3       posBody1 = m_body1 ? m_body1->m_centerOfMassPosition : btVector3(0,0,0);
-                       relativePositionB = point.getPositionWorldOnB() - posBody1;
-                       
-                       //                      for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] -
-                       //                                        j->node[1].body->pos[i];
-                       c2[0] = relativePositionB.x();
-                       c2[1] = relativePositionB.y();
-                       c2[2] = relativePositionB.z();
-                       
-                       info->J2l[0] = -normal[0];
-                       info->J2l[1] = -normal[1];
-                       info->J2l[2] = -normal[2];
-                       dCROSS (info->J2a,= -,c2,normal);
-               }
-       }
-       
-       btScalar k = info->fps * info->erp;
-       
-       float depth = -point.getDistance();
-//     if (depth < 0.f)
-//             depth = 0.f;
-       
-       info->c[0] = k * depth;
-       //float maxvel = .2f;
-
-//     if (info->c[0] > maxvel)
-//             info->c[0] = maxvel;
-
-
-       //can override it, not necessary
-//     info->cfm[0] = 0.f;
-//     info->cfm[1] = 0.f;
-//     info->cfm[2] = 0.f;
-       
-       
-       
-       // set LCP limits for normal
-       info->lo[0] = 0;
-       info->hi[0] = 1e30f;//dInfinity;
-       info->lo[1] = 0;
-       info->hi[1] = 0.f;
-       info->lo[2] = 0.f;
-       info->hi[2] = 0.f;
-
-#define DO_THE_FRICTION_2
-#ifdef DO_THE_FRICTION_2
-       // now do jacobian for tangential forces
-       dVector3 t1,t2; // two vectors tangential to normal
-       
-       dVector3 c1;
-       c1[0] = relativePositionA.x();
-       c1[1] = relativePositionA.y();
-       c1[2] = relativePositionA.z();
-       
-       dVector3 c2;
-       c2[0] = relativePositionB.x();
-       c2[1] = relativePositionB.y();
-       c2[2] = relativePositionB.z();
-       
-       //combined friction is available in the contact point
-       float friction = 0.25;//FRICTION_CONSTANT ;//* m_body0->m_friction * m_body1->m_friction;
-       
-       // first friction direction
-       if (m_numRows >= 2) 
-       {
-               
-               
-               
-               dPlaneSpace1 (normal,t1,t2);
-               
-               info->J1l[s+0] = t1[0];
-               info->J1l[s+1] = t1[1];
-               info->J1l[s+2] = t1[2];
-               dCROSS (info->J1a+s,=,c1,t1);
-//             if (1) { //j->node[1].body) {
-                       info->J2l[s+0] = -t1[0];
-                       info->J2l[s+1] = -t1[1];
-                       info->J2l[s+2] = -t1[2];
-                       dCROSS (info->J2a+s,= -,c2,t1);
-//             }
-               // set right hand side
-//             if (0) {//j->contact.surface.mode & dContactMotion1) {
-                       //info->c[1] = j->contact.surface.motion1;
-//             }
-               // set LCP bounds and friction index. this depends on the approximation
-               // mode
-               //1e30f
-               
-               
-               info->lo[1] = -friction;//-j->contact.surface.mu;
-               info->hi[1] = friction;//j->contact.surface.mu;
-//             if (1)//j->contact.surface.mode & dContactApprox1_1) 
-                       info->findex[1] = 0;
-               
-               // set slip (constraint force mixing)
-//             if (0)//j->contact.surface.mode & dContactSlip1)
-//             {
-//                     //      info->cfm[1] = j->contact.surface.slip1;
-//             } else
-//             {
-//                     //info->cfm[1] = 0.f;
-//             }
-       }
-       
-       // second friction direction
-       if (m_numRows >= 3) {
-               info->J1l[s2+0] = t2[0];
-               info->J1l[s2+1] = t2[1];
-               info->J1l[s2+2] = t2[2];
-               dCROSS (info->J1a+s2,=,c1,t2);
-//             if (1) { //j->node[1].body) {
-                       info->J2l[s2+0] = -t2[0];
-                       info->J2l[s2+1] = -t2[1];
-                       info->J2l[s2+2] = -t2[2];
-                       dCROSS (info->J2a+s2,= -,c2,t2);
-//             }
-
-               // set right hand side
-//             if (0){//j->contact.surface.mode & dContactMotion2) {
-                       //info->c[2] = j->contact.surface.motion2;
-//             }
-               // set LCP bounds and friction index. this depends on the approximation
-               // mode
-//             if (0){//j->contact.surface.mode & dContactMu2) {
-                       //info->lo[2] = -j->contact.surface.mu2;
-                       //info->hi[2] = j->contact.surface.mu2;
-//             }
-//             else {
-                       info->lo[2] = -friction;
-                       info->hi[2] = friction;
-//             }
-//             if (0)//j->contact.surface.mode & dContactApprox1_2) 
-                       
-//             {
-//                     info->findex[2] = 0;
-//             }
-               // set slip (constraint force mixing)
-//             if (0) //j->contact.surface.mode & dContactSlip2)
-                       
-//             {
-                       //info->cfm[2] = j->contact.surface.slip2;
-                       
-//             }
-       }
-       
-#endif //DO_THE_FRICTION_2
-       
-}
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.h
deleted file mode 100644 (file)
index 8dd0282..0000000
+++ /dev/null
@@ -1,50 +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 CONTACT_JOINT_H
-#define CONTACT_JOINT_H
-
-#include "btOdeJoint.h"
-struct btOdeSolverBody;
-class btPersistentManifold;
-
-class btOdeContactJoint : public btOdeJoint
-{
-       btPersistentManifold* m_manifold;
-       int             m_index;
-       bool    m_swapBodies;
-       btOdeSolverBody* m_body0;
-       btOdeSolverBody* m_body1;
-
-
-public:
-
-       btOdeContactJoint() {};
-
-       btOdeContactJoint(btPersistentManifold* manifold,int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1);
-
-       //btOdeJoint interface for solver
-
-       virtual void GetInfo1(Info1 *info);
-
-       virtual void GetInfo2(Info2 *info);
-
-
-       
-
-};
-
-#endif //CONTACT_JOINT_H
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.cpp
deleted file mode 100644 (file)
index 46c3783..0000000
+++ /dev/null
@@ -1,25 +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 "btOdeJoint.h"
-
-btOdeJoint::btOdeJoint()
-{
-
-}
-btOdeJoint::~btOdeJoint()
-{
-
-}
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.h
deleted file mode 100644 (file)
index 50733d1..0000000
+++ /dev/null
@@ -1,94 +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 btOdeJoint_H
-#define btOdeJoint_H
-
-struct btOdeSolverBody;
-class btOdeJoint;
-
-#include "LinearMath/btScalar.h"
-
-struct BU_ContactJointNode {
-  btOdeJoint *joint;           // pointer to enclosing btOdeJoint object
-  btOdeSolverBody* body;                       // *other* body this joint is connected to
-};
-typedef btScalar dVector3[4];
-
-
-class btOdeJoint  {
-  
-public:
-       // naming convention: the "first" body this is connected to is node[0].body,
-  // and the "second" body is node[1].body. if this joint is only connected
-  // to one body then the second body is 0.
-
-  // info returned by getInfo1 function. the constraint dimension is m (<=6).
-  // i.e. that is the total number of rows in the jacobian. `nub' is the
-  // number of unbounded variables (which have lo,hi = -/+ infinity).
-
-  btOdeJoint();
-  virtual ~btOdeJoint();
-  
-
-  struct Info1 {
-    int m,nub;
-  };
-
-  // info returned by getInfo2 function
-
-  struct Info2 {
-    // integrator parameters: frames per second (1/stepsize), default error
-    // reduction parameter (0..1).
-    btScalar fps,erp;
-
-    // for the first and second body, pointers to two (linear and angular)
-    // n*3 jacobian sub matrices, stored by rows. these matrices will have
-    // been initialized to 0 on entry. if the second body is zero then the
-    // J2xx pointers may be 0.
-    btScalar *J1l,*J1a,*J2l,*J2a;
-
-    // elements to jump from one row to the next in J's
-    int rowskip;
-
-    // right hand sides of the equation J*v = c + cfm * lambda. cfm is the
-    // "constraint force mixing" vector. c is set to zero on entry, cfm is
-    // set to a constant value (typically very small or zero) value on entry.
-    btScalar *c,*cfm;
-
-    // lo and hi limits for variables (set to -/+ infinity on entry).
-    btScalar *lo,*hi;
-
-    // findex vector for variables. see the LCP solver interface for a
-    // description of what this does. this is set to -1 on entry.
-    // note that the returned indexes are relative to the first index of
-    // the constraint.
-    int *findex;
-  };
-
-  // virtual function table: size of the joint structure, function pointers.
-  // we do it this way instead of using C++ virtual functions because
-  // sometimes we need to allocate joints ourself within a memory pool.
-
-  virtual void GetInfo1 (Info1 *info)=0;
-  virtual void GetInfo2 (Info2 *info)=0;
-
-  int flags;                   // dJOINT_xxx flags
-  BU_ContactJointNode node[2];         // connections to bodies. node[1].body can be 0
-   btScalar lambda[6];         // lambda generated by last step
-};
-
-
-#endif //btOdeJoint_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeMacros.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeMacros.h
deleted file mode 100644 (file)
index e4bc262..0000000
+++ /dev/null
@@ -1,212 +0,0 @@
-/*
- * Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith
- * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
- * All rights reserved.  Email: russ@q12.org   Web: www.q12.org
- Bullet Continuous Collision Detection and Physics Library
- Bullet is 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.
-*/
-
-#define ODE_MACROS
-#ifdef ODE_MACROS
-
-#include "LinearMath/btScalar.h"
-
-typedef btScalar dVector4[4];
-typedef btScalar dMatrix3[4*3];
-#define dInfinity FLT_MAX
-
-
-
-#define dRecip(x) ((float)(1.0f/(x)))                          /* reciprocal */
-
-
-
-#define dMULTIPLY0_331NEW(A,op,B,C) \
-{\
-       btScalar 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 btAssert
-#define dIASSERT btAssert
-
-#define REAL float
-#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)])
-inline btScalar dDOT1 (const btScalar *a, const btScalar *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]);
-
-/*
- * set a 3x3 submatrix of A to a matrix such that submatrix(A)*b = a x b.
- * A is stored by rows, and has `skip' elements per row. the matrix is
- * assumed to be already zero, so this does not write zero elements!
- * if (plus,minus) is (+,-) then a positive version will be written.
- * if (plus,minus) is (-,+) then a negative version will be written.
- */
-
-#define dCROSSMAT(A,a,skip,plus,minus) \
-{ \
-  (A)[1] = minus (a)[2]; \
-  (A)[2] = plus (a)[1]; \
-  (A)[(skip)+0] = plus (a)[2]; \
-  (A)[(skip)+2] = minus (a)[0]; \
-  (A)[2*(skip)+0] = minus (a)[1]; \
-  (A)[2*(skip)+1] = plus (a)[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))))))
-
-//#define ALLOCA dALLOCA16
-
-typedef const btScalar *dRealPtr;
-typedef btScalar *dRealMutablePtr;
-//#define dRealArray(name,n) btScalar name[n];
-//#define dRealAllocaArray(name,n) btScalar *name = (btScalar*) ALLOCA ((n)*sizeof(btScalar));
-
-///////////////////////////////////////////////////////////////////////////////
-
- //Remotion: 10.10.2007
-#define ALLOCA(size) stackAlloc->allocate( dEFFICIENT_SIZE(size) );
-
-//#define dRealAllocaArray(name,size) btScalar *name = (btScalar*) stackAlloc->allocate(dEFFICIENT_SIZE(size)*sizeof(btScalar));
-#define dRealAllocaArray(name,size) btScalar *name = NULL; \
-       unsigned int  memNeeded_##name = dEFFICIENT_SIZE(size)*sizeof(btScalar); \
-       if (memNeeded_##name < static_cast<size_t>(stackAlloc->getAvailableMemory())) name = (btScalar*) stackAlloc->allocate(memNeeded_##name); \
-       else{ btAssert(memNeeded_##name < static_cast<size_t>(stackAlloc->getAvailableMemory())); name = (btScalar*) alloca(memNeeded_##name); } 
-
-
-
-
-
-///////////////////////////////////////////////////////////////////////////////
-#if 0
-inline void dSetZero1 (btScalar *a, int n)
-{
-  dAASSERT (a && n >= 0);
-  while (n > 0) {
-    *(a++) = 0;
-    n--;
-  }
-}
-
-inline void dSetValue1 (btScalar *a, int n, btScalar value)
-{
-  dAASSERT (a && n >= 0);
-  while (n > 0) {
-    *(a++) = value;
-    n--;
-  }
-}
-#else
-
-/// This macros are for MSVC and XCode compilers. Remotion.
-
-
-#include <string.h> //for memset
-
-//Remotion: 10.10.2007
-//------------------------------------------------------------------------------
-#define IS_ALIGNED_16(x)       ((size_t(x)&15)==0)
-//------------------------------------------------------------------------------
-inline void dSetZero1 (btScalar *dest, int size)
-{
-       dAASSERT (dest && size >= 0);
-       memset(dest, 0, size * sizeof(btScalar));
-}
-//------------------------------------------------------------------------------
-inline void dSetValue1 (btScalar *dest, int size, btScalar val)
-{
-       dAASSERT (dest && size >= 0);
-       int n_mod4 = size & 3;          
-       int n4 = size - n_mod4;
-/*#ifdef __USE_SSE__
-//it is not supported on double precision, todo...
-       if(IS_ALIGNED_16(dest)){
-               __m128 xmm0 = _mm_set_ps1(val);
-               for (int i=0; i<n4; i+=4)
-               {
-                       _mm_store_ps(&dest[i],xmm0);
-               }
-       }else
-#endif
-       */
-
-       {
-               for (int i=0; i<n4; i+=4) // Unrolled Loop
-               {
-                       dest[i  ] = val;
-                       dest[i+1] = val;
-                       dest[i+2] = val;
-                       dest[i+3] = val;
-               }
-       }
-       for (int  i=n4; i<size; i++){
-               dest[i] = val;
-       }
-}
-#endif
-/////////////////////////////////////////////////////////////////////
-
-
-#endif //USE_SOR_SOLVER
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.cpp
deleted file mode 100644 (file)
index ab90c92..0000000
+++ /dev/null
@@ -1,393 +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 "btOdeQuickstepConstraintSolver.h"
-
-#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
-#include "BulletDynamics/Dynamics/btRigidBody.h"
-#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
-#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h"
-#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
-#include "btOdeJoint.h"
-#include "btOdeContactJoint.h"
-#include "btOdeTypedJoint.h"
-#include "btOdeSolverBody.h"
-#include <new>
-#include "LinearMath/btQuickprof.h"
-
-#include "LinearMath/btIDebugDraw.h"
-
-#define USE_SOR_SOLVER
-
-#include "btSorLcp.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__)
-#include <stdlib.h>
-#else
-#include <alloca.h>
-#endif
-#endif
-
-class btOdeJoint;
-
-//see below
-//to bridge with ODE quickstep, we make a temp copy of the rigidbodies in each simultion island
-
-
-btOdeQuickstepConstraintSolver::btOdeQuickstepConstraintSolver():
-        m_cfm(0.f),//1e-5f),
-        m_erp(0.4f)
-{
-}
-
-
-//iterative lcp and penalty method
-btScalar btOdeQuickstepConstraintSolver::solveGroup(btCollisionObject** /*bodies*/,int numBulletBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
-{
-
-    m_CurBody = 0;
-    m_CurJoint = 0;
-    m_CurTypedJoint = 0;
-       int j;
-
-       int max_contacts = 0; /// should be 4 //Remotion
-       for ( j=0;j<numManifolds;j++){
-               btPersistentManifold* manifold = manifoldPtr[j];
-               if (manifold->getNumContacts() > max_contacts)  max_contacts = manifold->getNumContacts();
-       }
-       //if(max_contacts > 4)
-       //      printf(" max_contacts > 4");
-
-       int numBodies = 0;
-       m_odeBodies.clear();
-       m_odeBodies.reserve(numBulletBodies + 1); //???
-       // btOdeSolverBody* odeBodies [ODE_MAX_SOLVER_BODIES];
-
-    int numJoints = 0;
-       m_joints.clear();
-       m_joints.reserve(numManifolds * max_contacts + 4 + numConstraints + 1); //???
-   // btOdeJoint* joints [ODE_MAX_SOLVER_JOINTS*2];
-
-       m_SolverBodyArray.resize(numBulletBodies + 1);
-       m_JointArray.resize(numManifolds * max_contacts + 4);
-       m_TypedJointArray.resize(numConstraints + 1);
-
-
-       //capture contacts
-       int body0=-1,body1=-1;
-    for (j=0;j<numManifolds;j++)
-    {
-        btPersistentManifold* manifold = manifoldPtr[j];
-        if (manifold->getNumContacts() > 0)
-        {
-            body0 = ConvertBody((btRigidBody*)manifold->getBody0(),m_odeBodies,numBodies);
-            body1 = ConvertBody((btRigidBody*)manifold->getBody1(),m_odeBodies,numBodies);
-            ConvertConstraint(manifold,m_joints,numJoints,m_odeBodies,body0,body1,debugDrawer);
-        }
-    }
-
-    //capture constraints
-    for (j=0;j<numConstraints;j++)
-    {
-       btTypedConstraint * typedconstraint = constraints[j];
-               body0 = ConvertBody((btRigidBody*)&typedconstraint->getRigidBodyA(),m_odeBodies,numBodies);
-               body1 = ConvertBody((btRigidBody*)&typedconstraint->getRigidBodyB(),m_odeBodies,numBodies);
-               ConvertTypedConstraint(typedconstraint,m_joints,numJoints,m_odeBodies,body0,body1,debugDrawer);
-    }
-       //if(numBodies > numBulletBodies) 
-       //      printf(" numBodies > numBulletBodies");
-       //if(numJoints > numManifolds * 4 + numConstraints) 
-       //      printf(" numJoints > numManifolds * 4 + numConstraints");
-
-
-     m_SorLcpSolver.SolveInternal1(m_cfm,m_erp,m_odeBodies,numBodies,m_joints,numJoints,infoGlobal,stackAlloc); ///do
-
-    //write back resulting velocities
-    for (int i=0;i<numBodies;i++)
-    {
-        if (m_odeBodies[i]->m_invMass)
-        {
-            m_odeBodies[i]->m_originalBody->setLinearVelocity(m_odeBodies[i]->m_linearVelocity);
-            m_odeBodies[i]->m_originalBody->setAngularVelocity(m_odeBodies[i]->m_angularVelocity);
-        }
-    }
-
-       /// Remotion, just free all this here
-       m_odeBodies.clear();
-       m_joints.clear();
-
-       m_SolverBodyArray.clear();
-       m_JointArray.clear();
-       m_TypedJointArray.clear();
-
-    return 0.f;
-
-}
-
-/////////////////////////////////////////////////////////////////////////////////
-
-
-typedef btScalar dQuaternion[4];
-#define _R(i,j) R[(i)*4+(j)]
-
-void dRfromQ1 (dMatrix3 R, const dQuaternion q);
-void dRfromQ1 (dMatrix3 R, const dQuaternion q)
-{
-    // q = (s,vx,vy,vz)
-    btScalar qq1 = 2.f*q[1]*q[1];
-    btScalar qq2 = 2.f*q[2]*q[2];
-    btScalar 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 btOdeQuickstepConstraintSolver::ConvertBody(btRigidBody* orgBody,btOdeSolverBody** bodies,int& numBodies)
-int btOdeQuickstepConstraintSolver::ConvertBody(btRigidBody* orgBody,btAlignedObjectArray< btOdeSolverBody*> &bodies,int& numBodies)
-{
-    assert(orgBody);
-    if (!orgBody || (orgBody->getInvMass() == 0.f) )
-    {
-        return -1;
-    }
-
-    if (orgBody->getCompanionId()>=0)
-    {
-        return orgBody->getCompanionId();
-    }
-    //first try to find
-    int i,j;
-
-    //if not found, create a new body
-   // btOdeSolverBody* body = bodies[numBodies] = &gSolverBodyArray[numBodies];
-       btOdeSolverBody* body = &m_SolverBodyArray[numBodies];
-       bodies.push_back(body); // Remotion 10.10.07: 
-
-    orgBody->setCompanionId(numBodies);
-
-    numBodies++;
-
-    body->m_originalBody = orgBody;
-
-    body->m_facc.setValue(0,0,0,0);
-    body->m_tacc.setValue(0,0,0,0);
-
-    body->m_linearVelocity = orgBody->getLinearVelocity();
-    body->m_angularVelocity = orgBody->getAngularVelocity();
-    body->m_invMass = orgBody->getInvMass();
-    body->m_centerOfMassPosition = orgBody->getCenterOfMassPosition();
-    body->m_friction = orgBody->getFriction();
-
-    //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] =      orgBody->getInvInertiaDiagLocal().x();
-    body->m_invI[1+4*1] =      orgBody->getInvInertiaDiagLocal().y();
-    body->m_invI[2+4*2] =      orgBody->getInvInertiaDiagLocal().z();
-
-    body->m_I[0+0*4] = 1.f/orgBody->getInvInertiaDiagLocal().x();
-    body->m_I[1+1*4] = 1.f/orgBody->getInvInertiaDiagLocal().y();
-    body->m_I[2+2*4] = 1.f/orgBody->getInvInertiaDiagLocal().z();
-
-
-
-
-    dQuaternion q;
-
-    q[1] = orgBody->getOrientation().x();
-    q[2] = orgBody->getOrientation().y();
-    q[3] = orgBody->getOrientation().z();
-    q[0] = orgBody->getOrientation().w();
-
-    dRfromQ1(body->m_R,q);
-
-    return numBodies-1;
-}
-
-
-
-
-
-
-
-
-
-void btOdeQuickstepConstraintSolver::ConvertConstraint(btPersistentManifold* manifold,
-                                                                                       btAlignedObjectArray<btOdeJoint*> &joints,int& numJoints,
-                                                                                       const btAlignedObjectArray< btOdeSolverBody*> &bodies,
-                                                                                       int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer)
-{
-
-
-    manifold->refreshContactPoints(((btRigidBody*)manifold->getBody0())->getCenterOfMassTransform(),
-                                   ((btRigidBody*)manifold->getBody1())->getCenterOfMassTransform());
-
-    int bodyId0 = _bodyId0,bodyId1 = _bodyId1;
-
-    int i,numContacts = manifold->getNumContacts();
-
-    bool swapBodies = (bodyId0 < 0);
-
-
-    btOdeSolverBody* body0,*body1;
-
-    if (swapBodies)
-    {
-        bodyId0 = _bodyId1;
-        bodyId1 = _bodyId0;
-
-        body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody1();
-        body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody0();
-
-    }
-    else
-    {
-        body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody0();
-        body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody1();
-    }
-
-    assert(bodyId0 >= 0);
-
-    btVector3 color(0,1,0);
-    for (i=0;i<numContacts;i++)
-    {
-
-        //assert (m_CurJoint < ODE_MAX_SOLVER_JOINTS);
-
-//             if (manifold->getContactPoint(i).getDistance() < 0.0f)
-        {
-
-            btOdeContactJoint* cont = new (&m_JointArray[m_CurJoint++]) btOdeContactJoint( manifold ,i, swapBodies,body0,body1);
-            //btOdeContactJoint* cont = new (&gJointArray[m_CurJoint++]) btOdeContactJoint( 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;
-                       joints.push_back(cont); // Remotion 10.10.07: 
-                       numJoints++;
-
-            for (int i=0;i<6;i++)
-                cont->lambda[i] = 0.f;
-
-            cont->flags = 0;
-        }
-    }
-
-    //create a new contact constraint
-}
-
-void btOdeQuickstepConstraintSolver::ConvertTypedConstraint(
-                                       btTypedConstraint * constraint,
-                                       btAlignedObjectArray<btOdeJoint*> &joints,int& numJoints,
-                                       const btAlignedObjectArray< btOdeSolverBody*> &bodies,int _bodyId0,int _bodyId1,btIDebugDraw* /*debugDrawer*/)
-{
-
-    int bodyId0 = _bodyId0,bodyId1 = _bodyId1;
-    bool swapBodies = (bodyId0 < 0);
-
-
-    btOdeSolverBody* body0,*body1;
-
-    if (swapBodies)
-    {
-        bodyId0 = _bodyId1;
-        bodyId1 = _bodyId0;
-
-        body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody1();
-        body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody0();
-
-    }
-    else
-    {
-        body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody0();
-        body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody1();
-    }
-
-    assert(bodyId0 >= 0);
-
-
-       //assert (m_CurTypedJoint < ODE_MAX_SOLVER_JOINTS);
-
-
-       btOdeTypedJoint * cont = NULL;
-
-       // Determine constraint type
-       int joint_type = constraint->getConstraintType();
-       switch(joint_type)
-       {
-       case POINT2POINT_CONSTRAINT_TYPE:
-       case D6_CONSTRAINT_TYPE:
-               cont = new (&m_TypedJointArray[m_CurTypedJoint ++]) btOdeTypedJoint(constraint,0, swapBodies,body0,body1);
-               //cont = new (&gTypedJointArray[m_CurTypedJoint ++]) btOdeTypedJoint(constraint,0, swapBodies,body0,body1);
-               break;
-
-       };
-
-       if(cont)
-       {
-               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;
-               joints.push_back(cont); // Remotion 10.10.07: 
-               numJoints++;
-
-               for (int i=0;i<6;i++)
-                       cont->lambda[i] = 0.f;
-
-               cont->flags = 0;
-       }
-
-}
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.h
deleted file mode 100644 (file)
index e548ea6..0000000
+++ /dev/null
@@ -1,109 +0,0 @@
-/*
- * Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith
- * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
- * All rights reserved.  Email: russ@q12.org   Web: www.q12.org
- Bullet Continuous Collision Detection and Physics Library
- Bullet is 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 "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
-
-#include "LinearMath/btAlignedObjectArray.h"
-#include "btOdeContactJoint.h"
-#include "btOdeTypedJoint.h"
-#include "btOdeSolverBody.h"
-#include "btSorLcp.h"
-
-class btRigidBody;
-struct btOdeSolverBody;
-class btOdeJoint;
-
-/// btOdeQuickstepConstraintSolver is one of the available solvers for Bullet dynamics framework
-/// It uses an adapted version quickstep solver from the Open Dynamics Engine project
-class btOdeQuickstepConstraintSolver : public btConstraintSolver
-{
-private:
-       int             m_CurBody;
-       int             m_CurJoint;
-       int             m_CurTypedJoint;
-
-       float   m_cfm;
-       float   m_erp;
-
-       btSorLcpSolver  m_SorLcpSolver;
-
-       btAlignedObjectArray<btOdeSolverBody*> m_odeBodies;
-       btAlignedObjectArray<btOdeJoint*>                m_joints;
-
-       btAlignedObjectArray<btOdeSolverBody>  m_SolverBodyArray;
-       btAlignedObjectArray<btOdeContactJoint>   m_JointArray;
-       btAlignedObjectArray<btOdeTypedJoint>  m_TypedJointArray;
-
-
-private:
-       int  ConvertBody(btRigidBody* body,btAlignedObjectArray< btOdeSolverBody*> &bodies,int& numBodies);
-       void ConvertConstraint(btPersistentManifold* manifold,
-                                                       btAlignedObjectArray<btOdeJoint*> &joints,int& numJoints,
-                                                       const btAlignedObjectArray< btOdeSolverBody*> &bodies,
-                                                       int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer);
-
-       void ConvertTypedConstraint(
-                                                       btTypedConstraint * constraint,
-                                                       btAlignedObjectArray<btOdeJoint*> &joints,int& numJoints,
-                                                       const btAlignedObjectArray< btOdeSolverBody*> &bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer);
-
-
-public:
-
-       btOdeQuickstepConstraintSolver();
-
-       virtual ~btOdeQuickstepConstraintSolver() {}
-
-       virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher);
-
-       ///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;
-       }
-
-       ///clear internal cached data and reset random seed
-       void reset()
-       {
-               m_SorLcpSolver.dRand2_seed = 0;
-       }
-
-       void    setRandSeed(unsigned long seed)
-       {
-               m_SorLcpSolver.dRand2_seed = seed;
-       }
-       unsigned long   getRandSeed() const
-       {
-               return m_SorLcpSolver.dRand2_seed;
-       }
-};
-
-
-
-
-#endif //ODE_CONSTRAINT_SOLVER_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeSolverBody.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeSolverBody.h
deleted file mode 100644 (file)
index 0c93697..0000000
+++ /dev/null
@@ -1,48 +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_SOLVER_BODY_H
-#define ODE_SOLVER_BODY_H
-
-class  btRigidBody;
-#include "LinearMath/btVector3.h"
-typedef btScalar dMatrix3[4*3];
-
-///ODE's quickstep needs just a subset of the rigidbody data in its own layout, so make a temp copy
-struct btOdeSolverBody 
-{
-       btRigidBody*    m_originalBody;
-
-       btVector3               m_centerOfMassPosition;
-       /// for ode solver-binding
-       dMatrix3                m_R;//temp
-       dMatrix3                m_I;
-       dMatrix3                m_invI;
-
-       int                             m_odeTag;
-       float                   m_invMass;
-       float                   m_friction;
-
-       btVector3               m_tacc;//temp
-       btVector3               m_facc;
-
-       btVector3               m_linearVelocity;
-       btVector3               m_angularVelocity;
-               
-};
-
-
-#endif //#ifndef ODE_SOLVER_BODY_H
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp
deleted file mode 100644 (file)
index f683bf7..0000000
+++ /dev/null
@@ -1,880 +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 "btOdeTypedJoint.h"
-#include "btOdeSolverBody.h"
-#include "btOdeMacros.h"
-#include <stdio.h>
-
-void btOdeTypedJoint::GetInfo1(Info1 *info)
-{
-    int joint_type = m_constraint->getConstraintType();
-    switch (joint_type)
-    {
-    case POINT2POINT_CONSTRAINT_TYPE:
-    {
-        OdeP2PJoint p2pjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1);
-        p2pjoint.GetInfo1(info);
-    }
-    break;
-    case D6_CONSTRAINT_TYPE:
-    {
-        OdeD6Joint d6joint(m_constraint,m_index,m_swapBodies,m_body0,m_body1);
-        d6joint.GetInfo1(info);
-    }
-    break;
-    case SLIDER_CONSTRAINT_TYPE:
-    {
-        OdeSliderJoint sliderjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1);
-        sliderjoint.GetInfo1(info);
-    }
-    break;
-    };
-}
-
-void btOdeTypedJoint::GetInfo2(Info2 *info)
-{
-    int joint_type = m_constraint->getConstraintType();
-    switch (joint_type)
-    {
-    case POINT2POINT_CONSTRAINT_TYPE:
-    {
-        OdeP2PJoint p2pjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1);
-        p2pjoint.GetInfo2(info);
-    }
-    break;
-    case D6_CONSTRAINT_TYPE:
-    {
-        OdeD6Joint d6joint(m_constraint,m_index,m_swapBodies,m_body0,m_body1);
-        d6joint.GetInfo2(info);
-    }
-    break;
-    case SLIDER_CONSTRAINT_TYPE:
-    {
-        OdeSliderJoint sliderjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1);
-        sliderjoint.GetInfo2(info);
-    }
-    break;
-    };
-}
-
-
-OdeP2PJoint::OdeP2PJoint(
-    btTypedConstraint * constraint,
-    int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1):
-        btOdeTypedJoint(constraint,index,swap,body0,body1)
-{
-}
-
-
-void OdeP2PJoint::GetInfo1(Info1 *info)
-{
-    info->m = 3;
-    info->nub = 3;
-}
-
-
-void OdeP2PJoint::GetInfo2(Info2 *info)
-{
-
-    btPoint2PointConstraint * p2pconstraint = this->getP2PConstraint();
-
-    //retrieve matrices
-    btTransform body0_trans;
-    if (m_body0)
-    {
-        body0_trans = m_body0->m_originalBody->getCenterOfMassTransform();
-    }
-//    btScalar body0_mat[12];
-//    body0_mat[0] = body0_trans.getBasis()[0][0];
-//    body0_mat[1] = body0_trans.getBasis()[0][1];
-//    body0_mat[2] = body0_trans.getBasis()[0][2];
-//    body0_mat[4] = body0_trans.getBasis()[1][0];
-//    body0_mat[5] = body0_trans.getBasis()[1][1];
-//    body0_mat[6] = body0_trans.getBasis()[1][2];
-//    body0_mat[8] = body0_trans.getBasis()[2][0];
-//    body0_mat[9] = body0_trans.getBasis()[2][1];
-//    body0_mat[10] = body0_trans.getBasis()[2][2];
-
-    btTransform body1_trans;
-
-    if (m_body1)
-    {
-        body1_trans = m_body1->m_originalBody->getCenterOfMassTransform();
-    }
-//    btScalar body1_mat[12];
-//    body1_mat[0] = body1_trans.getBasis()[0][0];
-//    body1_mat[1] = body1_trans.getBasis()[0][1];
-//    body1_mat[2] = body1_trans.getBasis()[0][2];
-//    body1_mat[4] = body1_trans.getBasis()[1][0];
-//    body1_mat[5] = body1_trans.getBasis()[1][1];
-//    body1_mat[6] = body1_trans.getBasis()[1][2];
-//    body1_mat[8] = body1_trans.getBasis()[2][0];
-//    body1_mat[9] = body1_trans.getBasis()[2][1];
-//    body1_mat[10] = body1_trans.getBasis()[2][2];
-
-
-
-
-    // anchor points in global coordinates with respect to body PORs.
-
-
-    int s = info->rowskip;
-
-    // set jacobian
-    info->J1l[0] = 1;
-    info->J1l[s+1] = 1;
-    info->J1l[2*s+2] = 1;
-
-
-    btVector3 a1,a2;
-
-    a1 = body0_trans.getBasis()*p2pconstraint->getPivotInA();
-    //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA);
-    dCROSSMAT (info->J1a,a1,s,-,+);
-    if (m_body1)
-    {
-        info->J2l[0] = -1;
-        info->J2l[s+1] = -1;
-        info->J2l[2*s+2] = -1;
-        a2 = body1_trans.getBasis()*p2pconstraint->getPivotInB();
-        //dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB);
-        dCROSSMAT (info->J2a,a2,s,+,-);
-    }
-
-
-    // set right hand side
-    btScalar k = info->fps * info->erp;
-    if (m_body1)
-    {
-        for (int j=0; j<3; j++)
-        {
-            info->c[j] = k * (a2[j] + body1_trans.getOrigin()[j] -
-                              a1[j] - body0_trans.getOrigin()[j]);
-        }
-    }
-    else
-    {
-        for (int j=0; j<3; j++)
-        {
-            info->c[j] = k * (p2pconstraint->getPivotInB()[j] - a1[j] -
-                              body0_trans.getOrigin()[j]);
-        }
-    }
-}
-
-
-///////////////////limit motor support
-
-/*! \pre testLimitValue must be called on limot*/
-int bt_get_limit_motor_info2(
-       btRotationalLimitMotor * limot,
-       btRigidBody * body0, btRigidBody * body1,
-       btOdeJoint::Info2 *info, int row, btVector3& ax1, int rotational)
-{
-
-
-    int srow = row * info->rowskip;
-
-    // if the joint is powered, or has joint limits, add in the extra row
-    int powered = limot->m_enableMotor;
-    int limit = limot->m_currentLimit;
-
-    if (powered || limit)
-    {
-        btScalar *J1 = rotational ? info->J1a : info->J1l;
-        btScalar *J2 = rotational ? info->J2a : info->J2l;
-
-        J1[srow+0] = ax1[0];
-        J1[srow+1] = ax1[1];
-        J1[srow+2] = ax1[2];
-        if (body1)
-        {
-            J2[srow+0] = -ax1[0];
-            J2[srow+1] = -ax1[1];
-            J2[srow+2] = -ax1[2];
-        }
-
-        // linear limot torque decoupling step:
-        //
-        // if this is a linear limot (e.g. from a slider), we have to be careful
-        // that the linear constraint forces (+/- ax1) applied to the two bodies
-        // do not create a torque couple. in other words, the points that the
-        // constraint force is applied at must lie along the same ax1 axis.
-        // a torque couple will result in powered or limited slider-jointed free
-        // bodies from gaining angular momentum.
-        // the solution used here is to apply the constraint forces at the point
-        // halfway between the body centers. there is no penalty (other than an
-        // extra tiny bit of computation) in doing this adjustment. note that we
-        // only need to do this if the constraint connects two bodies.
-
-        btVector3 ltd; // Linear Torque Decoupling vector (a torque)
-        if (!rotational && body1)
-        {
-            btVector3 c;
-            c[0]=btScalar(0.5)*(body1->getCenterOfMassPosition()[0]
-                                       -body0->getCenterOfMassPosition()[0]);
-            c[1]=btScalar(0.5)*(body1->getCenterOfMassPosition()[1]
-                                       -body0->getCenterOfMassPosition()[1]);
-            c[2]=btScalar(0.5)*(body1->getCenterOfMassPosition()[2]
-                                       -body0->getCenterOfMassPosition()[2]);
-
-                       ltd = c.cross(ax1);
-
-            info->J1a[srow+0] = ltd[0];
-            info->J1a[srow+1] = ltd[1];
-            info->J1a[srow+2] = ltd[2];
-            info->J2a[srow+0] = ltd[0];
-            info->J2a[srow+1] = ltd[1];
-            info->J2a[srow+2] = ltd[2];
-        }
-
-        // if we're limited low and high simultaneously, the joint motor is
-        // ineffective
-
-        if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
-
-        if (powered)
-        {
-            info->cfm[row] = 0.0f;//limot->m_normalCFM;
-            if (! limit)
-            {
-                info->c[row] = limot->m_targetVelocity;
-                info->lo[row] = -limot->m_maxMotorForce;
-                info->hi[row] = limot->m_maxMotorForce;
-            }
-        }
-
-        if (limit)
-        {
-            btScalar k = info->fps * limot->m_ERP;
-            info->c[row] = -k * limot->m_currentLimitError;
-            info->cfm[row] = 0.0f;//limot->m_stopCFM;
-
-            if (limot->m_loLimit == limot->m_hiLimit)
-            {
-                // limited low and high simultaneously
-                info->lo[row] = -dInfinity;
-                info->hi[row] = dInfinity;
-            }
-            else
-            {
-                if (limit == 1)
-                {
-                    // low limit
-                    info->lo[row] = 0;
-                    info->hi[row] = SIMD_INFINITY;
-                }
-                else
-                {
-                    // high limit
-                    info->lo[row] = -SIMD_INFINITY;
-                    info->hi[row] = 0;
-                }
-
-                // deal with bounce
-                if (limot->m_bounce > 0)
-                {
-                    // calculate joint velocity
-                    btScalar vel;
-                    if (rotational)
-                    {
-                        vel = body0->getAngularVelocity().dot(ax1);
-                        if (body1)
-                            vel -= body1->getAngularVelocity().dot(ax1);
-                    }
-                    else
-                    {
-                        vel = body0->getLinearVelocity().dot(ax1);
-                        if (body1)
-                            vel -= body1->getLinearVelocity().dot(ax1);
-                    }
-
-                    // only apply bounce if the velocity is incoming, and if the
-                    // resulting c[] exceeds what we already have.
-                    if (limit == 1)
-                    {
-                        // low limit
-                        if (vel < 0)
-                        {
-                            btScalar newc = -limot->m_bounce* vel;
-                            if (newc > info->c[row]) info->c[row] = newc;
-                        }
-                    }
-                    else
-                    {
-                        // high limit - all those computations are reversed
-                        if (vel > 0)
-                        {
-                            btScalar newc = -limot->m_bounce * vel;
-                            if (newc < info->c[row]) info->c[row] = newc;
-                        }
-                    }
-                }
-            }
-        }
-        return 1;
-    }
-    else return 0;
-}
-
-
-///////////////////OdeD6Joint
-
-
-
-
-
-OdeD6Joint::OdeD6Joint(
-    btTypedConstraint * constraint,
-    int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1):
-        btOdeTypedJoint(constraint,index,swap,body0,body1)
-{
-}
-
-
-void OdeD6Joint::GetInfo1(Info1 *info)
-{
-       btGeneric6DofConstraint * d6constraint = this->getD6Constraint();
-       //prepare constraint
-       d6constraint->calculateTransforms();
-    info->m = 3;
-    info->nub = 3;
-
-    //test angular limits
-    for (int i=0;i<3 ;i++ )
-    {
-       //if(i==2) continue;
-               if(d6constraint->testAngularLimitMotor(i))
-               {
-                       info->m++;
-               }
-    }
-
-
-}
-
-
-int OdeD6Joint::setLinearLimits(Info2 *info)
-{
-
-    btGeneric6DofConstraint * d6constraint = this->getD6Constraint();
-
-    //retrieve matrices
-    btTransform body0_trans;
-    if (m_body0)
-    {
-        body0_trans = m_body0->m_originalBody->getCenterOfMassTransform();
-    }
-
-    btTransform body1_trans;
-
-    if (m_body1)
-    {
-        body1_trans = m_body1->m_originalBody->getCenterOfMassTransform();
-    }
-
-    // anchor points in global coordinates with respect to body PORs.
-
-    int s = info->rowskip;
-
-    // set jacobian
-    info->J1l[0] = 1;
-    info->J1l[s+1] = 1;
-    info->J1l[2*s+2] = 1;
-
-
-    btVector3 a1,a2;
-
-    a1 = body0_trans.getBasis()*d6constraint->getFrameOffsetA().getOrigin();
-    //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA);
-    dCROSSMAT (info->J1a,a1,s,-,+);
-    if (m_body1)
-    {
-        info->J2l[0] = -1;
-        info->J2l[s+1] = -1;
-        info->J2l[2*s+2] = -1;
-        a2 = body1_trans.getBasis()*d6constraint->getFrameOffsetB().getOrigin();
-
-        //dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB);
-        dCROSSMAT (info->J2a,a2,s,+,-);
-    }
-
-
-    // set right hand side
-    btScalar k = info->fps * info->erp;
-    if (m_body1)
-    {
-        for (int j=0; j<3; j++)
-        {
-            info->c[j] = k * (a2[j] + body1_trans.getOrigin()[j] -
-                              a1[j] - body0_trans.getOrigin()[j]);
-        }
-    }
-    else
-    {
-        for (int j=0; j<3; j++)
-        {
-            info->c[j] = k * (d6constraint->getCalculatedTransformB().getOrigin()[j] - a1[j] -
-                              body0_trans.getOrigin()[j]);
-        }
-    }
-
-    return 3;
-
-}
-
-int OdeD6Joint::setAngularLimits(Info2 *info, int row_offset)
-{
-       btGeneric6DofConstraint * d6constraint = this->getD6Constraint();
-       int row = row_offset;
-       //solve angular limits
-    for (int i=0;i<3 ;i++ )
-    {
-       //if(i==2) continue;
-               if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
-               {
-                       btVector3 axis = d6constraint->getAxis(i);
-                       row += bt_get_limit_motor_info2(
-                               d6constraint->getRotationalLimitMotor(i),
-                               m_body0->m_originalBody,
-                               m_body1 ? m_body1->m_originalBody : NULL,
-                               info,row,axis,1);
-               }
-    }
-
-    return row;
-}
-
-void OdeD6Joint::GetInfo2(Info2 *info)
-{
-    int row = setLinearLimits(info);
-    setAngularLimits(info, row);
-}
-
-//----------------------------------------------------------------------------------
-//----------------------------------------------------------------------------------
-//----------------------------------------------------------------------------------
-//----------------------------------------------------------------------------------
-/*
-OdeSliderJoint
-Ported from ODE by Roman Ponomarev (rponom@gmail.com)
-April 24, 2008
-*/
-
-OdeSliderJoint::OdeSliderJoint(
-    btTypedConstraint * constraint,
-    int index,bool swap, btOdeSolverBody* body0, btOdeSolverBody* body1):
-        btOdeTypedJoint(constraint,index,swap,body0,body1)
-{
-} // OdeSliderJoint::OdeSliderJoint()
-
-//----------------------------------------------------------------------------------
-
-void OdeSliderJoint::GetInfo1(Info1* info)
-{
-       info->nub = 4; 
-       info->m = 4; // Fixed 2 linear + 2 angular
-       btSliderConstraint * slider = this->getSliderConstraint();
-       //prepare constraint
-       slider->calculateTransforms();
-       slider->testLinLimits();
-       if(slider->getSolveLinLimit() || slider->getPoweredLinMotor())
-       {
-               info->m++; // limit 3rd linear as well
-       }
-       slider->testAngLimits();
-       if(slider->getSolveAngLimit() || slider->getPoweredAngMotor())
-       {
-               info->m++; // limit 3rd angular as well
-       }
-} // OdeSliderJoint::GetInfo1()
-
-//----------------------------------------------------------------------------------
-
-void OdeSliderJoint::GetInfo2(Info2 *info)
-{
-       int i, s = info->rowskip;
-       btSliderConstraint * slider = this->getSliderConstraint();
-       const btTransform& trA = slider->getCalculatedTransformA();
-       const btTransform& trB = slider->getCalculatedTransformB();
-       // make rotations around Y and Z equal
-       // the slider axis should be the only unconstrained
-       // rotational axis, the angular velocity of the two bodies perpendicular to
-       // the slider axis should be equal. thus the constraint equations are
-       //    p*w1 - p*w2 = 0
-       //    q*w1 - q*w2 = 0
-       // where p and q are unit vectors normal to the slider axis, and w1 and w2
-       // are the angular velocity vectors of the two bodies.
-       // get slider axis (X)
-       btVector3 ax1 = trA.getBasis().getColumn(0);
-       // get 2 orthos to slider axis (Y, Z)
-       btVector3 p = trA.getBasis().getColumn(1);
-       btVector3 q = trA.getBasis().getColumn(2);
-       // set the two slider rows 
-       info->J1a[0] = p[0];
-       info->J1a[1] = p[1];
-       info->J1a[2] = p[2];
-       info->J1a[s+0] = q[0];
-       info->J1a[s+1] = q[1];
-       info->J1a[s+2] = q[2];
-       if(m_body1) 
-       {
-               info->J2a[0] = -p[0];
-               info->J2a[1] = -p[1];
-               info->J2a[2] = -p[2];
-               info->J2a[s+0] = -q[0];
-               info->J2a[s+1] = -q[1];
-               info->J2a[s+2] = -q[2];
-       }
-       // compute the right hand side of the constraint equation. set relative
-       // body velocities along p and q to bring the slider back into alignment.
-       // if ax1,ax2 are the unit length slider axes as computed from body1 and
-       // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
-       // if "theta" is the angle between ax1 and ax2, we need an angular velocity
-       // along u to cover angle erp*theta in one step :
-       //   |angular_velocity| = angle/time = erp*theta / stepsize
-       //                      = (erp*fps) * theta
-       //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
-       //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
-       // ...as ax1 and ax2 are unit length. if theta is smallish,
-       // theta ~= sin(theta), so
-       //    angular_velocity  = (erp*fps) * (ax1 x ax2)
-       // ax1 x ax2 is in the plane space of ax1, so we project the angular
-       // velocity to p and q to find the right hand side.
-       btScalar k = info->fps * info->erp * slider->getSoftnessOrthoAng();
-    btVector3 ax2 = trB.getBasis().getColumn(0);
-       btVector3 u;
-       if(m_body1)
-       {
-               u = ax1.cross(ax2);
-       }
-       else
-       {
-               u = ax2.cross(ax1);
-       }
-       info->c[0] = k * u.dot(p);
-       info->c[1] = k * u.dot(q);
-       // pull out pos and R for both bodies. also get the connection
-       // vector c = pos2-pos1.
-       // next two rows. we want: vel2 = vel1 + w1 x c ... but this would
-       // result in three equations, so we project along the planespace vectors
-       // so that sliding along the slider axis is disregarded. for symmetry we
-       // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.
-       btTransform bodyA_trans = m_body0->m_originalBody->getCenterOfMassTransform();
-       btTransform bodyB_trans;
-       if(m_body1)
-       {
-               bodyB_trans = m_body1->m_originalBody->getCenterOfMassTransform();
-       }
-       int s2 = 2 * s, s3 = 3 * s;
-       btVector3 c;
-       if(m_body1)
-       {
-               c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
-               btVector3 tmp = btScalar(0.5) * c.cross(p);
-
-               for (i=0; i<3; i++) info->J1a[s2+i] = tmp[i];
-               for (i=0; i<3; i++) info->J2a[s2+i] = tmp[i];
-
-               tmp = btScalar(0.5) * c.cross(q);
-
-               for (i=0; i<3; i++) info->J1a[s3+i] = tmp[i];
-               for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i];
-
-               for (i=0; i<3; i++) info->J2l[s2+i] = -p[i];
-               for (i=0; i<3; i++) info->J2l[s3+i] = -q[i];
-       }
-       for (i=0; i<3; i++) info->J1l[s2+i] = p[i];
-       for (i=0; i<3; i++) info->J1l[s3+i] = q[i];
-       // compute two elements of right hand side. we want to align the offset
-       // point (in body 2's frame) with the center of body 1.
-       btVector3 ofs; // offset point in global coordinates
-       if(m_body1)
-       {
-               ofs = trB.getOrigin() - trA.getOrigin();
-       }
-       else
-       {
-               ofs = trA.getOrigin() - trB.getOrigin();
-       }
-       k = info->fps * info->erp * slider->getSoftnessOrthoLin();
-       info->c[2] = k * p.dot(ofs);
-       info->c[3] = k * q.dot(ofs);
-       int nrow = 3; // last filled row
-       int srow;
-       // check linear limits linear
-       btScalar limit_err = btScalar(0.0);
-       int limit = 0;
-       if(slider->getSolveLinLimit())
-       {
-               limit_err = slider->getLinDepth();
-               if(m_body1) 
-               {
-                       limit = (limit_err > btScalar(0.0)) ? 1 : 2;
-               }
-               else
-               {
-                       limit = (limit_err > btScalar(0.0)) ? 2 : 1;
-               }
-       }
-       int powered = 0;
-       if(slider->getPoweredLinMotor())
-       {
-               powered = 1;
-       }
-       // if the slider has joint limits, add in the extra row
-       if (limit || powered) 
-       {
-               nrow++;
-               srow = nrow * info->rowskip;
-               info->J1l[srow+0] = ax1[0];
-               info->J1l[srow+1] = ax1[1];
-               info->J1l[srow+2] = ax1[2];
-               if(m_body1)
-               {
-                       info->J2l[srow+0] = -ax1[0];
-                       info->J2l[srow+1] = -ax1[1];
-                       info->J2l[srow+2] = -ax1[2];
-               }
-               // linear torque decoupling step:
-               //
-               // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
-               // do not create a torque couple. in other words, the points that the
-               // constraint force is applied at must lie along the same ax1 axis.
-               // a torque couple will result in limited slider-jointed free
-               // bodies from gaining angular momentum.
-               // the solution used here is to apply the constraint forces at the point
-               // halfway between the body centers. there is no penalty (other than an
-               // extra tiny bit of computation) in doing this adjustment. note that we
-               // only need to do this if the constraint connects two bodies.
-           if (m_body1) 
-               {
-                       dVector3 ltd;   // Linear Torque Decoupling vector (a torque)
-                       c = btScalar(0.5) * c;
-                       dCROSS (ltd,=,c,ax1);
-                       info->J1a[srow+0] = ltd[0];
-                       info->J1a[srow+1] = ltd[1];
-                       info->J1a[srow+2] = ltd[2];
-                       info->J2a[srow+0] = ltd[0];
-                       info->J2a[srow+1] = ltd[1];
-                       info->J2a[srow+2] = ltd[2];
-               }
-               // right-hand part
-               btScalar lostop = slider->getLowerLinLimit();
-               btScalar histop = slider->getUpperLinLimit();
-               if(limit && (lostop == histop))
-               {  // the joint motor is ineffective
-                       powered = 0;
-               }
-               if(powered)
-               {
-            info->cfm[nrow] = btScalar(0.0); 
-            if(!limit)
-            {
-                               info->c[nrow] = slider->getTargetLinMotorVelocity();
-                               info->lo[nrow] = -slider->getMaxLinMotorForce() * info->fps;
-                               info->hi[nrow] = slider->getMaxLinMotorForce() * info->fps;
-            }
-               }
-               if(limit)
-               {
-                       k = info->fps * info->erp;
-                       if(m_body1) 
-                       {
-                               info->c[nrow] = k * limit_err;
-                       }
-                       else
-                       {
-                               info->c[nrow] = - k * limit_err;
-                       }
-                       info->cfm[nrow] = btScalar(0.0); // stop_cfm;
-                       if(lostop == histop) 
-                       {
-                               // limited low and high simultaneously
-                               info->lo[nrow] = -SIMD_INFINITY;
-                               info->hi[nrow] = SIMD_INFINITY;
-                       }
-                       else 
-                       {
-                               if(limit == 1) 
-                               {
-                                       // low limit
-                                       info->lo[nrow] = 0;
-                                       info->hi[nrow] = SIMD_INFINITY;
-                               }
-                               else 
-                               {
-                                       // high limit
-                                       info->lo[nrow] = -SIMD_INFINITY;
-                                       info->hi[nrow] = 0;
-                               }
-                       }
-                       // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
-                       btScalar bounce = btFabs(btScalar(1.0) - slider->getDampingLimLin());
-                       if(bounce > btScalar(0.0))
-                       {
-                               btScalar vel = m_body0->m_originalBody->getLinearVelocity().dot(ax1);
-                               if(m_body1)
-                               {
-                                       vel -= m_body1->m_originalBody->getLinearVelocity().dot(ax1);
-                               }
-                               // only apply bounce if the velocity is incoming, and if the
-                               // resulting c[] exceeds what we already have.
-                               if(limit == 1)
-                               {
-                                       // low limit
-                                       if(vel < 0)
-                                       {
-                                               btScalar newc = -bounce * vel;
-                                               if (newc > info->c[nrow]) info->c[nrow] = newc;
-                                       }
-                               }
-                               else
-                               {
-                                       // high limit - all those computations are reversed
-                                       if(vel > 0)
-                                       {
-                                               btScalar newc = -bounce * vel;
-                                               if(newc < info->c[nrow]) info->c[nrow] = newc;
-                                       }
-                               }
-                       }
-                       info->c[nrow] *= slider->getSoftnessLimLin();
-               } // if(limit)
-       } // if linear limit
-       // check angular limits
-       limit_err = btScalar(0.0);
-       limit = 0;
-       if(slider->getSolveAngLimit())
-       {
-               limit_err = slider->getAngDepth();
-               if(m_body1) 
-               {
-                       limit = (limit_err > btScalar(0.0)) ? 1 : 2;
-               }
-               else
-               {
-                       limit = (limit_err > btScalar(0.0)) ? 2 : 1;
-               }
-       }
-       // if the slider has joint limits, add in the extra row
-       powered = 0;
-       if(slider->getPoweredAngMotor())
-       {
-               powered = 1;
-       }
-       if(limit || powered) 
-       {
-               nrow++;
-               srow = nrow * info->rowskip;
-               info->J1a[srow+0] = ax1[0];
-               info->J1a[srow+1] = ax1[1];
-               info->J1a[srow+2] = ax1[2];
-               if(m_body1)
-               {
-                       info->J2a[srow+0] = -ax1[0];
-                       info->J2a[srow+1] = -ax1[1];
-                       info->J2a[srow+2] = -ax1[2];
-               }
-               btScalar lostop = slider->getLowerAngLimit();
-               btScalar histop = slider->getUpperAngLimit();
-               if(limit && (lostop == histop))
-               {  // the joint motor is ineffective
-                       powered = 0;
-               }
-               if(powered)
-               {
-            info->cfm[nrow] = btScalar(0.0); 
-            if(!limit)
-            {
-                               info->c[nrow] = slider->getTargetAngMotorVelocity();
-                               info->lo[nrow] = -slider->getMaxAngMotorForce() * info->fps;
-                               info->hi[nrow] = slider->getMaxAngMotorForce() * info->fps;
-            }
-               }
-               if(limit)
-               {
-                       k = info->fps * info->erp;
-                       if (m_body1) 
-                       {
-                               info->c[nrow] = k * limit_err;
-                       }
-                       else
-                       {
-                               info->c[nrow] = -k * limit_err;
-                       }
-                       info->cfm[nrow] = btScalar(0.0); // stop_cfm;
-                       if(lostop == histop) 
-                       {
-                               // limited low and high simultaneously
-                               info->lo[nrow] = -SIMD_INFINITY;
-                               info->hi[nrow] = SIMD_INFINITY;
-                       }
-                       else 
-                       {
-                               if (limit == 1) 
-                               {
-                                       // low limit
-                                       info->lo[nrow] = 0;
-                                       info->hi[nrow] = SIMD_INFINITY;
-                               }
-                               else 
-                               {
-                                       // high limit
-                                       info->lo[nrow] = -SIMD_INFINITY;
-                                       info->hi[nrow] = 0;
-                               }
-                       }
-                       // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
-                       btScalar bounce = btFabs(btScalar(1.0) - slider->getDampingLimAng());
-                       if(bounce > btScalar(0.0))
-                       {
-                               btScalar vel = m_body0->m_originalBody->getAngularVelocity().dot(ax1);
-                               if(m_body1)
-                               {
-                                       vel -= m_body1->m_originalBody->getAngularVelocity().dot(ax1);
-                               }
-                               // only apply bounce if the velocity is incoming, and if the
-                               // resulting c[] exceeds what we already have.
-                               if(limit == 1)
-                               {
-                                       // low limit
-                                       if(vel < 0)
-                                       {
-                                               btScalar newc = -bounce * vel;
-                                               if (newc > info->c[nrow]) info->c[nrow] = newc;
-                                       }
-                               }
-                               else
-                               {
-                                       // high limit - all those computations are reversed
-                                       if(vel > 0)
-                                       {
-                                               btScalar newc = -bounce * vel;
-                                               if(newc < info->c[nrow]) info->c[nrow] = newc;
-                                       }
-                               }
-                       }
-                       info->c[nrow] *= slider->getSoftnessLimAng();
-               } // if(limit)
-       } // if angular limit or powered
-} // OdeSliderJoint::GetInfo2()
-
-//----------------------------------------------------------------------------------
-//----------------------------------------------------------------------------------
-
-
-
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h
deleted file mode 100644 (file)
index a2affda..0000000
+++ /dev/null
@@ -1,142 +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.
-*/
-/*
-2007-09-09
-Added support for typed joints by Francisco Le?n
-email: projectileman@yahoo.com
-http://gimpact.sf.net
-*/
-
-#ifndef TYPED_JOINT_H
-#define TYPED_JOINT_H
-
-#include "btOdeJoint.h"
-#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
-#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
-#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
-
-struct btOdeSolverBody;
-
-class btOdeTypedJoint : public btOdeJoint
-{
-public:
-       btTypedConstraint * m_constraint;
-       int             m_index;
-       bool    m_swapBodies;
-       btOdeSolverBody* m_body0;
-       btOdeSolverBody* m_body1;
-
-       btOdeTypedJoint(){}
-       btOdeTypedJoint(
-               btTypedConstraint * constraint,
-               int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1):
-                       m_constraint(constraint),
-                       m_index(index),
-                       m_swapBodies(swap),
-                       m_body0(body0),
-                       m_body1(body1)
-       {
-       }
-
-       virtual void GetInfo1(Info1 *info);
-       virtual void GetInfo2(Info2 *info);
-};
-
-
-
-class OdeP2PJoint : public btOdeTypedJoint
-{
-protected:
-       inline btPoint2PointConstraint * getP2PConstraint()
-       {
-               return static_cast<btPoint2PointConstraint * >(m_constraint);
-       }
-public:
-
-       OdeP2PJoint() {};
-
-       OdeP2PJoint(btTypedConstraint* constraint,int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1);
-
-       //btOdeJoint interface for solver
-
-       virtual void GetInfo1(Info1 *info);
-
-       virtual void GetInfo2(Info2 *info);
-};
-
-
-class OdeD6Joint : public btOdeTypedJoint
-{
-protected:
-       inline btGeneric6DofConstraint * getD6Constraint()
-       {
-               return static_cast<btGeneric6DofConstraint * >(m_constraint);
-       }
-
-       int setLinearLimits(Info2 *info);
-       int setAngularLimits(Info2 *info, int row_offset);
-
-public:
-
-       OdeD6Joint() {};
-
-       OdeD6Joint(btTypedConstraint* constraint,int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1);
-
-       //btOdeJoint interface for solver
-
-       virtual void GetInfo1(Info1 *info);
-
-       virtual void GetInfo2(Info2 *info);
-};
-
-//! retrieves the constraint info from a btRotationalLimitMotor object
-/*! \pre testLimitValue must be called on limot*/
-int bt_get_limit_motor_info2(
-       btRotationalLimitMotor * limot,
-       btRigidBody * body0, btRigidBody * body1,
-       btOdeJoint::Info2 *info, int row, btVector3& ax1, int rotational);
-
-/*
-OdeSliderJoint
-Ported from ODE by Roman Ponomarev (rponom@gmail.com)
-April 24, 2008
-*/
-class OdeSliderJoint : public btOdeTypedJoint
-{
-protected:
-       inline btSliderConstraint * getSliderConstraint()
-       {
-               return static_cast<btSliderConstraint * >(m_constraint);
-       }
-public:
-
-       OdeSliderJoint() {};
-
-       OdeSliderJoint(btTypedConstraint* constraint,int index,bool swap, btOdeSolverBody* body0, btOdeSolverBody* body1);
-
-       //BU_Joint interface for solver
-
-       virtual void GetInfo1(Info1 *info);
-
-       virtual void GetInfo2(Info2 *info);
-};
-
-
-
-
-#endif //CONTACT_JOINT_H
-
-
-
index e7f07a4..4128f50 100644 (file)
@@ -342,6 +342,7 @@ void btSliderConstraint::calculateTransforms(void){
 void btSliderConstraint::testLinLimits(void)
 {
        m_solveLinLim = false;
+       m_linPos = m_depth[0];
        if(m_lowerLinLimit <= m_upperLinLimit)
        {
                if(m_depth[0] > m_upperLinLimit)
index f69dfcf..580dfa1 100644 (file)
@@ -103,6 +103,8 @@ protected:
        btVector3 m_relPosA;
        btVector3 m_relPosB;
 
+       btScalar m_linPos;
+
        btScalar m_angDepth;
        btScalar m_kAngle;
 
@@ -191,6 +193,7 @@ public:
        btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; }
        void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; }
        btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; }
+       btScalar getLinearPos() { return m_linPos; }
 
        // access for ODE solver
        bool getSolveLinLimit() { return m_solveLinLim; }
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.cpp
deleted file mode 100644 (file)
index 175d15d..0000000
+++ /dev/null
@@ -1,683 +0,0 @@
-/*
- * Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith
- * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
- * All rights reserved.  Email: russ@q12.org   Web: www.q12.org
- Bullet Continuous Collision Detection and Physics Library
- Bullet is 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 "btSorLcp.h"
-#include "btOdeSolverBody.h"
-
-#ifdef USE_SOR_SOLVER
-
-// SOR LCP taken from ode quickstep, for comparisons to Bullet sequential impulse solver.
-#include "LinearMath/btScalar.h"
-
-#include "BulletDynamics/Dynamics/btRigidBody.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__)
-#include <stdlib.h>
-#else
-#include <alloca.h>
-#endif
-#endif
-
-#include "btOdeJoint.h"
-#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
-////////////////////////////////////////////////////////////////////
-//math stuff
-#include "btOdeMacros.h"
-
-//***************************************************************************
-// 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'
-inline void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
-       //OdeSolverBody* const *body,
-        const btAlignedObjectArray<btOdeSolverBody*> &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];
-               btScalar k = body[b1]->m_invMass;
-               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]->m_invMass;
-                       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.
-inline 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];
-               btScalar 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
-
-//------------------------------------------------------------------------------
-ATTRIBUTE_ALIGNED16(struct) IndexError {
-       btScalar error;         // error to sort on
-       int findex;
-       int index;              // row index
-};
-
-//------------------------------------------------------------------------------
-void btSorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb, 
-       const btAlignedObjectArray<btOdeSolverBody*> &body,
-       dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs,
-       dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
-       int numiter,float overRelax,
-       btStackAlloc* stackAlloc
-       )
-{
-       //btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007
-       AutoBlockSa asaBlock(stackAlloc);
-
-       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));
-       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] = btFabs (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];
-                       }
-               }
-       }
-       //stackAlloc->endBlock(saBlock);//Remo: 10.10.2007
-}
-
-//------------------------------------------------------------------------------
-void btSorLcpSolver::SolveInternal1 (
-                       float global_cfm,
-                       float global_erp,
-                       const btAlignedObjectArray<btOdeSolverBody*> &body, int nb,
-                       btAlignedObjectArray<btOdeJoint*> &joint, 
-                       int nj, const btContactSolverInfo& solverInfo,
-                       btStackAlloc* stackAlloc)
-{
-       //btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007
-       AutoBlockSa asaBlock(stackAlloc);
-
-       int numIter = solverInfo.m_numIterations;
-       float sOr = solverInfo.m_sor;
-
-       int i,j;
-
-       btScalar 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 "btOdeJoint *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
-       //btOdeJoint **joint = (btOdeJoint**) alloca (nj * sizeof(btOdeJoint*));
-       //memcpy (joint,_joint,nj * sizeof(btOdeJoint*));
-
-       // 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]->m_angularVelocity);
-//             dCROSS (body[i]->m_tacc,-=,body[i]->m_angularVelocity,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
-       btOdeJoint::Info1 *info = (btOdeJoint::Info1*) ALLOCA (nj*sizeof(btOdeJoint::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);
-               btOdeJoint::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++) {
-                       btScalar body_invMass = body[i]->m_invMass;
-                       for (j=0; j<3; j++)
-                               tmp1[i*6+j] = body[i]->m_facc[j] * body_invMass + body[i]->m_linearVelocity[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]->m_angularVelocity[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(btScalar));
-               }
-#endif
-
-               // solve the LCP problem and get lambda and invM*constraint_force
-               dRealAllocaArray (cforce,nb*6);
-
-               /// SOR_LCP
-               SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,numIter,sOr,stackAlloc);
-
-#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(btScalar));
-               }
-#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++) {
-                       for (j=0; j<3; j++)
-                               body[i]->m_linearVelocity[j] += solverInfo.m_timeStep* cforce[i*6+j];
-                       for (j=0; j<3; j++)
-                               body[i]->m_angularVelocity[j] += solverInfo.m_timeStep* cforce[i*6+3+j];
-
-               }
-       }
-
-       // compute the velocity update:
-       // add stepsize * invM * fe to the body velocity
-       for (i=0; i<nb; i++) {
-               btScalar body_invMass = body[i]->m_invMass;
-               btVector3 linvel = body[i]->m_linearVelocity;
-               btVector3 angvel = body[i]->m_angularVelocity;
-
-               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]->m_angularVelocity = angvel;
-       }
-       //stackAlloc->endBlock(saBlock);//Remo: 10.10.2007
-}
-
-
-#endif //USE_SOR_SOLVER
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.h
deleted file mode 100644 (file)
index 0d3583d..0000000
+++ /dev/null
@@ -1,112 +0,0 @@
-/*
- * Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith
- * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
- * All rights reserved.  Email: russ@q12.org   Web: www.q12.org
- Bullet Continuous Collision Detection and Physics Library
- Bullet is 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.
-*/
-
-#define USE_SOR_SOLVER
-#ifdef USE_SOR_SOLVER
-
-#ifndef SOR_LCP_H
-#define SOR_LCP_H
-struct btOdeSolverBody;
-class btOdeJoint;
-#include "LinearMath/btScalar.h"
-#include "LinearMath/btAlignedObjectArray.h"
-#include "LinearMath/btStackAlloc.h"
-
-struct btContactSolverInfo;
-
-
-//=============================================================================
-class btSorLcpSolver //Remotion: 11.10.2007
-{
-public:
-       btSorLcpSolver()
-       {
-               dRand2_seed = 0;
-       }
-
-       void SolveInternal1 (float global_cfm,
-               float global_erp,
-               const btAlignedObjectArray<btOdeSolverBody*> &body, int nb,
-               btAlignedObjectArray<btOdeJoint*> &joint, 
-               int nj, const btContactSolverInfo& solverInfo,
-               btStackAlloc* stackAlloc
-               );
-
-public: //data
-       unsigned long dRand2_seed;
-
-protected: //typedef
-       typedef const btScalar *dRealPtr;
-       typedef btScalar *dRealMutablePtr;
-
-protected: //members
-       //------------------------------------------------------------------------------
-       SIMD_FORCE_INLINE unsigned long dRand2()
-       {
-               dRand2_seed = (1664525L*dRand2_seed + 1013904223L) & 0xffffffff;
-               return dRand2_seed;
-       }
-       //------------------------------------------------------------------------------
-       SIMD_FORCE_INLINE int dRandInt2 (int n)
-       {
-               float a = float(n) / 4294967296.0f;
-               return (int) (float(dRand2()) * a);
-       }
-       //------------------------------------------------------------------------------
-       void SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb, 
-               const btAlignedObjectArray<btOdeSolverBody*> &body,
-               dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs,
-               dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
-               int numiter,float overRelax,
-               btStackAlloc* stackAlloc
-               );
-};
-
-
-//=============================================================================
-class AutoBlockSa //Remotion: 10.10.2007
-{
-       btStackAlloc* stackAlloc;
-       btBlock*          saBlock;
-public:
-       AutoBlockSa(btStackAlloc* stackAlloc_)
-       {
-               stackAlloc = stackAlloc_;
-               saBlock = stackAlloc->beginBlock();
-       }
-       ~AutoBlockSa()
-       {
-               stackAlloc->endBlock(saBlock);
-       }
-       //operator btBlock* () { return saBlock; }
-};
-// //Usage
-//void function(btStackAlloc* stackAlloc)
-//{
-//     AutoBlockSa(stackAlloc);
-//   ...
-//     if(...) return;
-//     return;
-//}
-//------------------------------------------------------------------------------
-
-
-#endif //SOR_LCP_H
-
-#endif //USE_SOR_SOLVER
-
index c7b1af2..e46c4e6 100644 (file)
@@ -269,7 +269,7 @@ void        btDiscreteDynamicsWorld::synchronizeMotionStates()
                                {
                                        btTransform interpolatedTransform;
                                        btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
-                                               body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime,interpolatedTransform);
+                                               body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform);
                                        body->getMotionState()->setWorldTransform(interpolatedTransform);
                                }
                        }
@@ -708,7 +708,78 @@ void       btDiscreteDynamicsWorld::calculateSimulationIslands()
 }
 
 
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
 
+class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
+{
+       btCollisionObject* m_me;
+       btScalar m_allowedPenetration;
+       btOverlappingPairCache* m_pairCache;
+
+
+public:
+       btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache) : 
+         btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
+               m_allowedPenetration(0.0f),
+               m_me(me),
+               m_pairCache(pairCache)
+       {
+       }
+
+       virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
+       {
+               if (convexResult.m_hitCollisionObject == m_me)
+                       return 1.0;
+
+               btVector3 linVelA,linVelB;
+               linVelA = m_convexToWorld-m_convexFromWorld;
+               linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin();
+
+               btVector3 relativeVelocity = (linVelA-linVelB);
+               //don't report time of impact for motion away from the contact normal (or causes minor penetration)
+               if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration)
+                       return 1.f;
+
+               return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
+       }
+
+       virtual bool needsCollision(btBroadphaseProxy* proxy0) const
+       {
+               //don't collide with itself
+               if (proxy0->m_clientObject == m_me)
+                       return false;
+
+               ///don't do CCD when the collision filters are not matching
+               if (!btCollisionWorld::ClosestConvexResultCallback::needsCollision(proxy0))
+                       return false;
+
+               ///don't do CCD when there are already contact points (touching contact/penetration)
+               btAlignedObjectArray<btPersistentManifold*> manifoldArray;
+               btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
+               if (collisionPair)
+               {
+                       if (collisionPair->m_algorithm)
+                       {
+                               manifoldArray.resize(0);
+                               collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
+                               for (int j=0;j<manifoldArray.size();j++)
+                               {
+                                       btPersistentManifold* manifold = manifoldArray[j];
+                                       if (manifold->getNumContacts()>0)
+                                               return false;
+                               }
+                       }
+               }
+               return true;
+       }
+
+
+};
+
+///internal debugging variable. this value shouldn't be too high
+int gNumClampedCcdMotions=0;
+
+//#include "stdio.h"
 void   btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 {
        BT_PROFILE("integrateTransforms");
@@ -719,9 +790,34 @@ void       btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
                btRigidBody* body = btRigidBody::upcast(colObj);
                if (body)
                {
+                       body->setHitFraction(1.f);
+
                        if (body->isActive() && (!body->isStaticOrKinematicObject()))
                        {
                                body->predictIntegratedTransform(timeStep, predictedTrans);
+                               btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
+
+                               if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
+                               {
+                                       BT_PROFILE("CCD motion clamping");
+                                       if (body->getCollisionShape()->isConvex())
+                                       {
+                                               gNumClampedCcdMotions++;
+                                               
+                                               btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache());
+                                               btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
+                                               btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
+                                               convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults);
+                                               if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
+                                               {
+                                                       body->setHitFraction(sweepResults.m_closestHitFraction);
+                                                       body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
+                                                       body->setHitFraction(0.f);
+//                                                     printf("clamped integration to hit fraction = %f\n",fraction);
+                                               }
+                                       }
+                               }
+                               
                                body->proceedToTransform( predictedTrans);
                        }
                }
index d206a60..d9e2652 100644 (file)
@@ -59,7 +59,7 @@ protected:
 
        virtual void    predictUnconstraintMotion(btScalar timeStep);
        
-       void    integrateTransforms(btScalar timeStep);
+       virtual void    integrateTransforms(btScalar timeStep);
                
        void    calculateSimulationIslands();
 
index dd9dfa7..929e24d 100644 (file)
@@ -60,7 +60,7 @@ public:
                ///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'.
                ///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'.
                ///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant.
-               virtual int             stepSimulation( btScalar timeStep,int maxSubSteps=10, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0;
+               virtual int             stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0;
                        
                virtual void    debugDrawWorld() = 0;
                                
index dfb48e2..2553009 100644 (file)
@@ -2002,7 +2002,7 @@ for(i=0;i<m_clusters.size();++i)
                                mi.setMin(c.m_nodes[j]->m_x);
                                mx.setMax(c.m_nodes[j]->m_x);
                                }                       
-                       const btDbvtVolume      bounds=btDbvtVolume::FromMM(mi,mx);
+                       const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(mi,mx);
                        if(c.m_leaf)
                                m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg);
                                else
@@ -2532,7 +2532,7 @@ switch(m_cfg.collisions&fCollision::RVSmask)
                const btScalar          basemargin=getCollisionShape()->getMargin();
                btVector3                       mins;
                btVector3                       maxs;
-               btDbvtVolume            volume;
+               ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume;
                pco->getCollisionShape()->getAabb(      pco->getInterpolationWorldTransform(),
                                                                                        mins,
                                                                                        maxs);
index e9ec81f..d991996 100644 (file)
@@ -373,7 +373,6 @@ void                        btSoftBodyHelpers::DrawInfos(           btSoftBody* psb,
                                                  bool areas,
                                                  bool /*stress*/)
 {
-       /*
        for(int i=0;i<psb->m_nodes.size();++i)
        {
                const btSoftBody::Node& n=psb->m_nodes[i];
@@ -391,8 +390,6 @@ void                        btSoftBodyHelpers::DrawInfos(           btSoftBody* psb,
                }
                if(text[0]) idraw->draw3dText(n.m_x,text);
        }
-       */
-
 }
 
 //
index 0c87770..1a7be2c 100644 (file)
@@ -750,7 +750,8 @@ struct btSoftColliders
                friction        =       btMin(psb->m_cfg.kDF,prb->getFriction());
                btVector3                       mins;
                btVector3                       maxs;
-               btDbvtVolume            volume;
+               
+               ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume;
                pr->getCollisionShape()->getAabb(pr->getInterpolationWorldTransform(),mins,maxs);
                volume=btDbvtVolume::FromMM(mins,maxs);
                volume.Expand(btVector3(1,1,1)*margin);
index 207eed9..02ffaad 100644 (file)
@@ -4,7 +4,32 @@ ${BULLET_PHYSICS_SOURCE_DIR}/src }
 )
 
 ADD_LIBRARY(LibLinearMath
-btQuickprof.cpp
-btGeometryUtil.cpp
+               btAlignedObjectArray.h
+               btList.h
+               btPoolAllocator.h
+               btRandom.h
+               btVector3.h
+               btDefaultMotionState.h
+               btMatrix3x3.h
+               btQuadWord.h
+               btHashMap.h
+               btScalar.h
+               btAabbUtil2.h
+               btConvexHull.h
+               btConvexHull.cpp
+               btMinMax.h
+               btQuaternion.h
+               btStackAlloc.h
+               btGeometryUtil.h
+               btMotionState.h
+               btTransform.h
+               btAlignedAllocator.h
+               btIDebugDraw.h
+               btPoint3.h
+               btQuickprof.h
+               btTransformUtil.h
+               btQuickprof.cpp
+               btGeometryUtil.cpp
+               btAlignedAllocator.cpp
 )
 
index 8bb6b3a..275c491 100644 (file)
@@ -17,6 +17,7 @@ subject to the following restrictions:
 #ifndef AABB_UTIL2
 #define AABB_UTIL2
 
+#include "btTransform.h"
 #include "btVector3.h"
 #include "btMinMax.h"
 
@@ -163,6 +164,39 @@ SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom,
 }
 
 
+
+SIMD_FORCE_INLINE      void btTransformAabb(const btVector3& halfExtents, btScalar margin,const btTransform& t,btVector3& aabbMinOut,btVector3& aabbMaxOut)
+{
+       btVector3 halfExtentsWithMargin = halfExtents+btVector3(margin,margin,margin);
+       btMatrix3x3 abs_b = t.getBasis().absolute();  
+       btVector3 center = t.getOrigin();
+       btVector3 extent = btVector3(abs_b[0].dot(halfExtentsWithMargin),
+                  abs_b[1].dot(halfExtentsWithMargin),
+                 abs_b[2].dot(halfExtentsWithMargin));
+       aabbMinOut = center - extent;
+       aabbMaxOut = center + extent;
+}
+
+
+SIMD_FORCE_INLINE      void btTransformAabb(const btVector3& localAabbMin,const btVector3& localAabbMax, btScalar margin,const btTransform& trans,btVector3& aabbMinOut,btVector3& aabbMaxOut)
+{
+               btAssert(localAabbMin.getX() <= localAabbMax.getX());
+               btAssert(localAabbMin.getY() <= localAabbMax.getY());
+               btAssert(localAabbMin.getZ() <= localAabbMax.getZ());
+               btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin);
+               localHalfExtents+=btVector3(margin,margin,margin);
+
+               btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin);
+               btMatrix3x3 abs_b = trans.getBasis().absolute();  
+               btVector3 center = trans(localCenter);
+               btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
+                          abs_b[1].dot(localHalfExtents),
+                         abs_b[2].dot(localHalfExtents));
+               aabbMinOut = center-extent;
+               aabbMaxOut = center+extent;
+}
+
+
 #endif
 
 
index ca1a801..14aa4ae 100644 (file)
@@ -284,6 +284,91 @@ class btMatrix3x3 {
                }
                
 
+               ///diagonalizes this matrix by the Jacobi method. rot stores the rotation
+               ///from the coordinate system in which the matrix is diagonal to the original
+               ///coordinate system, i.e., old_this = rot * new_this * rot^T. The iteration
+               ///stops when all off-diagonal elements are less than the threshold multiplied
+               ///by the sum of the absolute values of the diagonal, or when maxSteps have
+               ///been executed. Note that this matrix is assumed to be symmetric.
+               void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
+               {
+                rot.setIdentity();
+                for (int step = maxSteps; step > 0; step--)
+                {
+                       // find off-diagonal element [p][q] with largest magnitude
+                       int p = 0;
+                       int q = 1;
+                       int r = 2;
+                       btScalar max = btFabs(m_el[0][1]);
+                       btScalar v = btFabs(m_el[0][2]);
+                       if (v > max)
+                       {
+                          q = 2;
+                          r = 1;
+                          max = v;
+                       }
+                       v = btFabs(m_el[1][2]);
+                       if (v > max)
+                       {
+                          p = 1;
+                          q = 2;
+                          r = 0;
+                          max = v;
+                       }
+
+                       btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2]));
+                       if (max <= t)
+                       {
+                          if (max <= SIMD_EPSILON * t)
+                          {
+                                 return;
+                          }
+                          step = 1;
+                       }
+
+                       // compute Jacobi rotation J which leads to a zero for element [p][q] 
+                       btScalar mpq = m_el[p][q];
+                       btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
+                       btScalar theta2 = theta * theta;
+                       btScalar cos;
+                       btScalar sin;
+                       if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON))
+                       {
+                          t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
+                                                                               : 1 / (theta - btSqrt(1 + theta2));
+                          cos = 1 / btSqrt(1 + t * t);
+                          sin = cos * t;
+                       }
+                       else
+                       {
+                          // approximation for large theta-value, i.e., a nearly diagonal matrix
+                          t = 1 / (theta * (2 + btScalar(0.5) / theta2));
+                          cos = 1 - btScalar(0.5) * t * t;
+                          sin = cos * t;
+                       }
+
+                       // apply rotation to matrix (this = J^T * this * J)
+                       m_el[p][q] = m_el[q][p] = 0;
+                       m_el[p][p] -= t * mpq;
+                       m_el[q][q] += t * mpq;
+                       btScalar mrp = m_el[r][p];
+                       btScalar mrq = m_el[r][q];
+                       m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
+                       m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
+
+                       // apply rotation to rot (rot = rot * J)
+                       for (int i = 0; i < 3; i++)
+                       {
+                          btVector3& row = rot[i];
+                          mrp = row[p];
+                          mrq = row[q];
+                          row[p] = cos * mrp - sin * mrq;
+                          row[q] = cos * mrq + sin * mrp;
+                       }
+                }
+               }
+
+
                
        protected:
                btScalar cofac(int r1, int c1, int r2, int c2) const 
index 1fee626..3c96857 100644 (file)
@@ -24,7 +24,7 @@ subject to the following restrictions:
 #include <cfloat>
 #include <float.h>
 
-#define BT_BULLET_VERSION 271
+#define BT_BULLET_VERSION 272
 
 inline int     btGetVersion()
 {
@@ -65,7 +65,11 @@ inline int   btGetVersion()
                #endif //__MINGW32__
 
                #include <assert.h>
+#if defined(DEBUG) || defined (_DEBUG)
                #define btAssert assert
+#else
+               #define btAssert(x)
+#endif
                //btFullAssert is optional, slows down a lot
                #define btFullAssert(x)
 
@@ -116,7 +120,13 @@ inline int btGetVersion()
                #ifndef assert
                #include <assert.h>
                #endif
+
+#if defined(DEBUG) || defined (_DEBUG)
                #define btAssert assert
+#else
+               #define btAssert(x)
+#endif
+
                //btFullAssert is optional, slows down a lot
                #define btFullAssert(x)
                #define btLikely(_c)  _c
@@ -145,11 +155,16 @@ typedef float btScalar;
 #endif
 
 
+
 #define BT_DECLARE_ALIGNED_ALLOCATOR() \
-       SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes)        { return btAlignedAlloc(sizeInBytes,16); }      \
-       SIMD_FORCE_INLINE void  operator delete(void* ptr)                      { btAlignedFree(ptr); } \
-       SIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \
-       SIMD_FORCE_INLINE void  operator delete(void*, void*)           { }     \
+   SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes)   { return btAlignedAlloc(sizeInBytes,16); }   \
+   SIMD_FORCE_INLINE void  operator delete(void* ptr)         { btAlignedFree(ptr); }   \
+   SIMD_FORCE_INLINE void* operator new(size_t, void* ptr)   { return ptr; }   \
+   SIMD_FORCE_INLINE void  operator delete(void*, void*)      { }   \
+   SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes)   { return btAlignedAlloc(sizeInBytes,16); }   \
+   SIMD_FORCE_INLINE void  operator delete[](void* ptr)         { btAlignedFree(ptr); }   \
+   SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr)   { return ptr; }   \
+   SIMD_FORCE_INLINE void  operator delete[](void*, void*)      { }   \
 
 
 
index b21d8b4..4b14f6d 100644 (file)
@@ -43,7 +43,9 @@ subject to the following restrictions:
 
 ///Narrowphase Collision Detector
 #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
-#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h"
+
+//btSphereBoxCollisionAlgorithm is broken, use gjk for now
+//#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h"
 
 ///Dispatching and generation of collision pairs (broadphase)
index ce9bb1d..b95972c 100644 (file)
@@ -32,9 +32,7 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
 
 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
-///Optional ODE quickstep constraint solver, redistributed under ZLib license
-#include "BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.h"
-#include "BulletDynamics/ConstraintSolver/btOdeTypedJoint.h"
+
 
 ///Vehicle simulation, with wheel contact simulated by raycasts
 #include "BulletDynamics/Vehicle/btRaycastVehicle.h"