Changeset 7983 for code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch

Timestamp:

Feb 27, 2011, 7:43:24 AM (14 years ago)

Author:

rgrieder

Message:

Updated Bullet Physics Engine from v2.74 to v2.77

Location:

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch

Files:

• SphereTriangleDetector.cpp (modified) (1 diff)
• SphereTriangleDetector.h (modified) (1 diff)
• btBox2dBox2dCollisionAlgorithm.cpp (added)
• btBox2dBox2dCollisionAlgorithm.h (added)
• btBoxBoxCollisionAlgorithm.cpp (modified) (1 diff)
• btBoxBoxDetector.cpp (modified) (9 diffs)
• btCollisionDispatcher.cpp (modified) (4 diffs)
• btCollisionDispatcher.h (modified) (3 diffs)
• btCollisionObject.cpp (modified) (4 diffs)
• btCollisionObject.h (modified) (10 diffs)
• btCollisionWorld.cpp (modified) (35 diffs)
• btCollisionWorld.h (modified) (22 diffs)
• btCompoundCollisionAlgorithm.cpp (modified) (7 diffs)
• btConvex2dConvex2dAlgorithm.cpp (added)
• btConvex2dConvex2dAlgorithm.h (added)
• btConvexConcaveCollisionAlgorithm.cpp (modified) (2 diffs)
• btConvexConvexAlgorithm.cpp (modified) (12 diffs)
• btConvexConvexAlgorithm.h (modified) (1 diff)
• btConvexPlaneCollisionAlgorithm.cpp (modified) (1 diff)
• btConvexPlaneCollisionAlgorithm.h (modified) (1 diff)
• btDefaultCollisionConfiguration.cpp (modified) (2 diffs)
• btDefaultCollisionConfiguration.h (modified) (3 diffs)
• btGhostObject.h (modified) (2 diffs)
• btInternalEdgeUtility.cpp (added)
• btInternalEdgeUtility.h (added)
• btManifoldResult.cpp (modified) (3 diffs)
• btManifoldResult.h (modified) (5 diffs)
• btSimulationIslandManager.cpp (modified) (6 diffs)
• btSphereTriangleCollisionAlgorithm.cpp (modified) (1 diff)
• btUnionFind.cpp (modified) (2 diffs)
• btUnionFind.h (modified) (2 diffs)

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp

@@ -38 +38 @@
         btScalar timeOfImpact = btScalar(1.);
         btScalar depth = btScalar(0.);
-//      output.m_distance = btScalar(1e30);
+//      output.m_distance = btScalar(BT_LARGE_FLOAT);
         //move sphere into triangle space
         btTransform     sphereInTr = transformB.inverseTimes(transformA);

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.h

@@ -35 +35 @@
         virtual ~SphereTriangleDetector() {};
 
+        bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold);
+
 private:
 
-        bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold);
+        
         bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p );
         bool facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal);

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp

@@ -62 +62 @@
 
         btDiscreteCollisionDetectorInterface::ClosestPointInput input;
-        input.m_maximumDistanceSquared = 1e30f;
+        input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
         input.m_transformA = body0->getWorldTransform();
         input.m_transformB = body1->getWorldTransform();

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp

@@ -1 @@
-
 /*
  * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith
@@ -213 +212 @@
         } else
         {
-                a=1e30f;
+                a=BT_LARGE_FLOAT;
         }
     cx = a*(cx + q*(p[n*2-2]+p[0]));
@@ -268 +267 @@
 {
   const btScalar fudge_factor = btScalar(1.05);
-  btVector3 p,pp,normalC;
+  btVector3 p,pp,normalC(0.f,0.f,0.f);
   const btScalar *normalR = 0;
   btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
@@ -334 +333 @@
 #define TST(expr1,expr2,n1,n2,n3,cc) \
   s2 = btFabs(expr1) - (expr2); \
-  if (s2 > 0) return 0; \
+  if (s2 > SIMD_EPSILON) return 0; \
   l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
-  if (l > 0) { \
+  if (l > SIMD_EPSILON) { \
     s2 /= l; \
     if (s2*fudge_factor > s) { \
@@ -346 +345 @@
     } \
   }
+
+  btScalar fudge2 (1.0e-5f);
+
+  Q11 += fudge2;
+  Q12 += fudge2;
+  Q13 += fudge2;
+
+  Q21 += fudge2;
+  Q22 += fudge2;
+  Q23 += fudge2;
+
+  Q31 += fudge2;
+  Q32 += fudge2;
+  Q33 += fudge2;
 
   // separating axis = u1 x (v1,v2,v3)
@@ -425 +438 @@
 #else
                 output.addContactPoint(-normal,pb,-*depth);
+
 #endif //
                 *return_code = code;
@@ -594 +608 @@
 
   if (cnum <= maxc) {
+
+          if (code<4) 
+          {
     // we have less contacts than we need, so we use them all
-    for (j=0; j < cnum; j++) {
-
-                //AddContactPoint...
-
-                //dContactGeom *con = CONTACT(contact,skip*j);
-      //for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
-      //con->depth = dep[j];
-
+    for (j=0; j < cnum; j++) 
+        {
                 btVector3 pointInWorld;
                 for (i=0; i<3; i++) 
@@ -609 +620 @@
 
     }
+          } else
+          {
+                  // we have less contacts than we need, so we use them all
+                for (j=0; j < cnum; j++) 
+                {
+                        btVector3 pointInWorld;
+                        for (i=0; i<3; i++) 
+                                pointInWorld[i] = point[j*3+i] + pa[i]-normal[i]*dep[j];
+                                //pointInWorld[i] = point[j*3+i] + pa[i];
+                        output.addContactPoint(-normal,pointInWorld,-dep[j]);
+                }
+          }
   }
   else {
@@ -633 +656 @@
                 for (i=0; i<3; i++) 
                         posInWorld[i] = point[iret[j]*3+i] + pa[i];
-                output.addContactPoint(-normal,posInWorld,-dep[iret[j]]);
+                if (code<4) 
+           {
+                        output.addContactPoint(-normal,posInWorld,-dep[iret[j]]);
+                } else
+                {
+                        output.addContactPoint(-normal,posInWorld-normal*dep[iret[j]],-dep[iret[j]]);
+                }
     }
     cnum = maxc;

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

@@ -35 +35 @@
 
 btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration): 
-        m_count(0),
-        m_useIslands(true),
-        m_staticWarningReported(false),
+m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD),
         m_collisionConfiguration(collisionConfiguration)
 {
@@ -80 +78 @@
         btCollisionObject* body1 = (btCollisionObject*)b1;
 
-        //test for Bullet 2.74: use a relative contact breaking threshold without clamping against 'gContactBreakingThreshold'
-        //btScalar contactBreakingThreshold = btMin(gContactBreakingThreshold,btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold()));
-        btScalar contactBreakingThreshold = btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold());
+        //optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance)
+        
+        btScalar contactBreakingThreshold =  (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? 
+                btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold) , body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold))
+                : gContactBreakingThreshold ;
 
         btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold());
@@ -170 +170 @@
 
 #ifdef BT_DEBUG
-        if (!m_staticWarningReported)
+        if (!(m_dispatcherFlags & btCollisionDispatcher::CD_STATIC_STATIC_REPORTED))
         {
                 //broadphase filtering already deals with this
@@ -176 +176 @@
                         (body1->isStaticObject() || body1->isKinematicObject()))
                 {
-                        m_staticWarningReported = true;
+                        m_dispatcherFlags |= btCollisionDispatcher::CD_STATIC_STATIC_REPORTED;
                         printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n");
                 }

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.h

@@ -43 +43 @@
 class btCollisionDispatcher : public btDispatcher
 {
-        int m_count;
+        int             m_dispatcherFlags;
         
         btAlignedObjectArray<btPersistentManifold*>     m_manifoldsPtr;
 
-        bool m_useIslands;
-
-        bool    m_staticWarningReported;
-        
         btManifoldResult        m_defaultManifoldResult;
 
@@ -60 +56 @@
 
         btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
-        
 
         btCollisionConfiguration*       m_collisionConfiguration;
@@ -66 +61 @@
 
 public:
+
+        enum DispatcherFlags
+        {
+                CD_STATIC_STATIC_REPORTED = 1,
+                CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD = 2
+        };
+
+        int     getDispatcherFlags() const
+        {
+                return m_dispatcherFlags;
+        }
+
+        void    setDispatcherFlags(int flags)
+        {
+        (void) flags;
+                m_dispatcherFlags = 0;
+        }
 
         ///registerCollisionCreateFunc allows registration of custom/alternative collision create functions

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -16 +16 @@
 
 #include "btCollisionObject.h"
+#include "LinearMath/btSerializer.h"
 
 btCollisionObject::btCollisionObject()
         :       m_anisotropicFriction(1.f,1.f,1.f),
         m_hasAnisotropicFriction(false),
-        m_contactProcessingThreshold(1e30f),
+        m_contactProcessingThreshold(BT_LARGE_FLOAT),
                 m_broadphaseHandle(0),
                 m_collisionShape(0),
+                m_extensionPointer(0),
                 m_rootCollisionShape(0),
                 m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT),
@@ -31 +33 @@
                 m_friction(btScalar(0.5)),
                 m_restitution(btScalar(0.)),
+                m_internalType(CO_COLLISION_OBJECT),
                 m_userObjectPointer(0),
-                m_internalType(CO_COLLISION_OBJECT),
                 m_hitFraction(btScalar(1.)),
                 m_ccdSweptSphereRadius(btScalar(0.)),
@@ -38 +40 @@
                 m_checkCollideWith(false)
 {
-        
+        m_worldTransform.setIdentity();
 }
 
@@ -65 +67 @@
 }
 
+const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+
+        btCollisionObjectData* dataOut = (btCollisionObjectData*)dataBuffer;
+
+        m_worldTransform.serialize(dataOut->m_worldTransform);
+        m_interpolationWorldTransform.serialize(dataOut->m_interpolationWorldTransform);
+        m_interpolationLinearVelocity.serialize(dataOut->m_interpolationLinearVelocity);
+        m_interpolationAngularVelocity.serialize(dataOut->m_interpolationAngularVelocity);
+        m_anisotropicFriction.serialize(dataOut->m_anisotropicFriction);
+        dataOut->m_hasAnisotropicFriction = m_hasAnisotropicFriction;
+        dataOut->m_contactProcessingThreshold = m_contactProcessingThreshold;
+        dataOut->m_broadphaseHandle = 0;
+        dataOut->m_collisionShape = serializer->getUniquePointer(m_collisionShape);
+        dataOut->m_rootCollisionShape = 0;//@todo
+        dataOut->m_collisionFlags = m_collisionFlags;
+        dataOut->m_islandTag1 = m_islandTag1;
+        dataOut->m_companionId = m_companionId;
+        dataOut->m_activationState1 = m_activationState1;
+        dataOut->m_activationState1 = m_activationState1;
+        dataOut->m_deactivationTime = m_deactivationTime;
+        dataOut->m_friction = m_friction;
+        dataOut->m_restitution = m_restitution;
+        dataOut->m_internalType = m_internalType;
+        
+        char* name = (char*) serializer->findNameForPointer(this);
+        dataOut->m_name = (char*)serializer->getUniquePointer(name);
+        if (dataOut->m_name)
+        {
+                serializer->serializeName(name);
+        }
+        dataOut->m_hitFraction = m_hitFraction;
+        dataOut->m_ccdSweptSphereRadius = m_ccdSweptSphereRadius;
+        dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold;
+        dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold;
+        dataOut->m_checkCollideWith = m_checkCollideWith;
+
+        return btCollisionObjectDataName;
+}
 
 
+void btCollisionObject::serializeSingleObject(class btSerializer* serializer) const
+{
+        int len = calculateSerializeBufferSize();
+        btChunk* chunk = serializer->allocate(len,1);
+        const char* structType = serialize(chunk->m_oldPtr, serializer);
+        serializer->finalizeChunk(chunk,structType,BT_COLLISIONOBJECT_CODE,(void*)this);
+}

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -28 +28 @@
 struct  btBroadphaseProxy;
 class   btCollisionShape;
+struct btCollisionShapeData;
 #include "LinearMath/btMotionState.h"
 #include "LinearMath/btAlignedAllocator.h"
 #include "LinearMath/btAlignedObjectArray.h"
 
-
 typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray;
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btCollisionObjectData btCollisionObjectDoubleData
+#define btCollisionObjectDataName "btCollisionObjectDoubleData"
+#else
+#define btCollisionObjectData btCollisionObjectFloatData
+#define btCollisionObjectDataName "btCollisionObjectFloatData"
+#endif
 
 
@@ -54 +62 @@
         btVector3       m_interpolationAngularVelocity;
         
-        btVector3               m_anisotropicFriction;
-        bool                            m_hasAnisotropicFriction;
-        btScalar                m_contactProcessingThreshold;   
+        btVector3       m_anisotropicFriction;
+        int                     m_hasAnisotropicFriction;
+        btScalar        m_contactProcessingThreshold;   
 
         btBroadphaseProxy*              m_broadphaseHandle;
         btCollisionShape*               m_collisionShape;
+        ///m_extensionPointer is used by some internal low-level Bullet extensions.
+        void*                                   m_extensionPointer;
         
         ///m_rootCollisionShape is temporarily used to store the original collision shape
@@ -77 +87 @@
         btScalar                m_restitution;
 
-        ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
-        void*                   m_userObjectPointer;
-
         ///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc.
         ///do not assign your own m_internalType unless you write a new dynamics object class.
         int                             m_internalType;
 
+        ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
+        void*                   m_userObjectPointer;
+
         ///time of impact calculation
         btScalar                m_hitFraction; 
@@ -94 +104 @@
         
         /// If some object should have elaborate collision filtering by sub-classes
-        bool                    m_checkCollideWith;
-
-        char    m_pad[7];
+        int                     m_checkCollideWith;
 
         virtual bool    checkCollideWithOverride(btCollisionObject* /* co */)
@@ -113 +121 @@
                 CF_NO_CONTACT_RESPONSE = 4,
                 CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution)
-                CF_CHARACTER_OBJECT = 16
+                CF_CHARACTER_OBJECT = 16,
+                CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing
+                CF_DISABLE_SPU_COLLISION_PROCESSING = 64//disable parallel/SPU processing
         };
 
@@ -119 +129 @@
         {
                 CO_COLLISION_OBJECT =1,
-                CO_RIGID_BODY,
+                CO_RIGID_BODY=2,
                 ///CO_GHOST_OBJECT keeps track of all objects overlapping its AABB and that pass its collision filter
                 ///It is useful for collision sensors, explosion objects, character controller etc.
-                CO_GHOST_OBJECT,
-                CO_SOFT_BODY,
-                CO_HF_FLUID
+                CO_GHOST_OBJECT=4,
+                CO_SOFT_BODY=8,
+                CO_HF_FLUID=16,
+                CO_USER_TYPE=32
         };
 
@@ -144 +155 @@
         bool    hasAnisotropicFriction() const
         {
-                return m_hasAnisotropicFriction;
+                return m_hasAnisotropicFriction!=0;
         }
 
@@ -214 +225 @@
         }
 
+        ///Avoid using this internal API call, the extension pointer is used by some Bullet extensions. 
+        ///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead.
+        void*           internalGetExtensionPointer() const
+        {
+                return m_extensionPointer;
+        }
+        ///Avoid using this internal API call, the extension pointer is used by some Bullet extensions
+        ///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead.
+        void    internalSetExtensionPointer(void* pointer)
+        {
+                m_extensionPointer = pointer;
+        }
+
         SIMD_FORCE_INLINE       int     getActivationState() const { return m_activationState1;}
         
@@ -394 +418 @@
         void    setCcdMotionThreshold(btScalar ccdMotionThreshold)
         {
-                m_ccdMotionThreshold = ccdMotionThreshold*ccdMotionThreshold;
+                m_ccdMotionThreshold = ccdMotionThreshold;
         }
 
@@ -417 +441 @@
                 return true;
         }
+
+        virtual int     calculateSerializeBufferSize()  const;
+
+        ///fills the dataBuffer and returns the struct name (and 0 on failure)
+        virtual const char*     serialize(void* dataBuffer, class btSerializer* serializer) const;
+
+        virtual void serializeSingleObject(class btSerializer* serializer) const;
+
 };
 
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btCollisionObjectDoubleData
+{
+        void                                    *m_broadphaseHandle;
+        void                                    *m_collisionShape;
+        btCollisionShapeData    *m_rootCollisionShape;
+        char                                    *m_name;
+
+        btTransformDoubleData   m_worldTransform;
+        btTransformDoubleData   m_interpolationWorldTransform;
+        btVector3DoubleData             m_interpolationLinearVelocity;
+        btVector3DoubleData             m_interpolationAngularVelocity;
+        btVector3DoubleData             m_anisotropicFriction;
+        double                                  m_contactProcessingThreshold;   
+        double                                  m_deactivationTime;
+        double                                  m_friction;
+        double                                  m_restitution;
+        double                                  m_hitFraction; 
+        double                                  m_ccdSweptSphereRadius;
+        double                                  m_ccdMotionThreshold;
+
+        int                                             m_hasAnisotropicFriction;
+        int                                             m_collisionFlags;
+        int                                             m_islandTag1;
+        int                                             m_companionId;
+        int                                             m_activationState1;
+        int                                             m_internalType;
+        int                                             m_checkCollideWith;
+
+        char    m_padding[4];
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct  btCollisionObjectFloatData
+{
+        void                                    *m_broadphaseHandle;
+        void                                    *m_collisionShape;
+        btCollisionShapeData    *m_rootCollisionShape;
+        char                                    *m_name;
+
+        btTransformFloatData    m_worldTransform;
+        btTransformFloatData    m_interpolationWorldTransform;
+        btVector3FloatData              m_interpolationLinearVelocity;
+        btVector3FloatData              m_interpolationAngularVelocity;
+        btVector3FloatData              m_anisotropicFriction;
+        float                                   m_contactProcessingThreshold;   
+        float                                   m_deactivationTime;
+        float                                   m_friction;
+        float                                   m_restitution;
+        float                                   m_hitFraction; 
+        float                                   m_ccdSweptSphereRadius;
+        float                                   m_ccdMotionThreshold;
+
+        int                                             m_hasAnisotropicFriction;
+        int                                             m_collisionFlags;
+        int                                             m_islandTag1;
+        int                                             m_companionId;
+        int                                             m_activationState1;
+        int                                             m_internalType;
+        int                                             m_checkCollideWith;
+};
+
+
+
+SIMD_FORCE_INLINE       int     btCollisionObject::calculateSerializeBufferSize() const
+{
+        return sizeof(btCollisionObjectData);
+}
+
+
+
 #endif //COLLISION_OBJECT_H

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -27 +27 @@
 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
-
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
 #include "LinearMath/btAabbUtil2.h"
 #include "LinearMath/btQuickprof.h"
 #include "LinearMath/btStackAlloc.h"
+#include "LinearMath/btSerializer.h"
 
 //#define USE_BRUTEFORCE_RAYBROADPHASE 1
@@ -43 +44 @@
 
 
+///for debug drawing
+
+//for debug rendering
+#include "BulletCollision/CollisionShapes/btBoxShape.h"
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
+#include "BulletCollision/CollisionShapes/btCompoundShape.h"
+#include "BulletCollision/CollisionShapes/btConeShape.h"
+#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btCylinderShape.h"
+#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
+#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
+#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
+
+
+
 btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration)
 :m_dispatcher1(dispatcher),
 m_broadphasePairCache(pairCache),
-m_debugDrawer(0)
+m_debugDrawer(0),
+m_forceUpdateAllAabbs(true)
 {
         m_stackAlloc = collisionConfiguration->getStackAllocator();
@@ -89 +109 @@
 {
 
+        btAssert(collisionObject);
+
         //check that the object isn't already added
-                btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());
-
-                m_collisionObjects.push_back(collisionObject);
-
-                //calculate new AABB
-                btTransform trans = collisionObject->getWorldTransform();
-
-                btVector3       minAabb;
-                btVector3       maxAabb;
-                collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
-
-                int type = collisionObject->getCollisionShape()->getShapeType();
-                collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
-                        minAabb,
-                        maxAabb,
-                        type,
-                        collisionObject,
-                        collisionFilterGroup,
-                        collisionFilterMask,
-                        m_dispatcher1,0
-                        ))      ;
+        btAssert( m_collisionObjects.findLinearSearch(collisionObject)  == m_collisionObjects.size());
+
+        m_collisionObjects.push_back(collisionObject);
+
+        //calculate new AABB
+        btTransform trans = collisionObject->getWorldTransform();
+
+        btVector3       minAabb;
+        btVector3       maxAabb;
+        collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
+
+        int type = collisionObject->getCollisionShape()->getShapeType();
+        collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
+                minAabb,
+                maxAabb,
+                type,
+                collisionObject,
+                collisionFilterGroup,
+                collisionFilterMask,
+                m_dispatcher1,0
+                ))      ;
 
 
@@ -163 +185 @@
 
                 //only update aabb of active objects
-                if (colObj->isActive())
+                if (m_forceUpdateAllAabbs || colObj->isActive())
                 {
                         updateSingleAabb(colObj);
@@ -226 +248 @@
 
 void    btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
-                                          btCollisionObject* collisionObject,
-                                          const btCollisionShape* collisionShape,
-                                          const btTransform& colObjWorldTransform,
-                                          RayResultCallback& resultCallback)
+                                                                                btCollisionObject* collisionObject,
+                                                                                const btCollisionShape* collisionShape,
+                                                                                const btTransform& colObjWorldTransform,
+                                                                                RayResultCallback& resultCallback)
 {
         btSphereShape pointShape(btScalar(0.0));
@@ -237 +259 @@
         if (collisionShape->isConvex())
         {
-//              BT_PROFILE("rayTestConvex");
+                //              BT_PROFILE("rayTestConvex");
                 btConvexCast::CastResult castResult;
                 castResult.m_fraction = resultCallback.m_closestHitFraction;
@@ -266 +288 @@
                                         btCollisionWorld::LocalRayResult localRayResult
                                                 (
-                                                        collisionObject,
-                                                        0,
-                                                        castResult.m_normal,
-                                                        castResult.m_fraction
+                                                collisionObject,
+                                                0,
+                                                castResult.m_normal,
+                                                castResult.m_fraction
                                                 );
 
@@ -281 +303 @@
                 if (collisionShape->isConcave())
                 {
-//                      BT_PROFILE("rayTestConcave");
+                        //                      BT_PROFILE("rayTestConcave");
                         if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
                         {
@@ -297 +319 @@
                                         btTriangleMeshShape*    m_triangleMesh;
 
+                                        btTransform m_colObjWorldTransform;
+
                                         BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
-                                                btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*    triangleMesh):
-                  //@BP Mod
-                                                btTriangleRaycastCallback(from,to, resultCallback->m_flags),
-                                                        m_resultCallback(resultCallback),
-                                                        m_collisionObject(collisionObject),
-                                                        m_triangleMesh(triangleMesh)
-                                                {
-                                                }
+                                                btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape*    triangleMesh,const btTransform& colObjWorldTransform):
+                                        //@BP Mod
+                                        btTriangleRaycastCallback(from,to, resultCallback->m_flags),
+                                                m_resultCallback(resultCallback),
+                                                m_collisionObject(collisionObject),
+                                                m_triangleMesh(triangleMesh),
+                                                m_colObjWorldTransform(colObjWorldTransform)
+                                        {
+                                        }
 
 
@@ -314 +339 @@
                                                 shapeInfo.m_triangleIndex = triangleIndex;
 
+                                                btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
+
                                                 btCollisionWorld::LocalRayResult rayResult
-                                                (m_collisionObject,
+                                                        (m_collisionObject,
                                                         &shapeInfo,
-                                                        hitNormalLocal,
+                                                        hitNormalWorld,
                                                         hitFraction);
 
-                                                bool    normalInWorldSpace = false;
+                                                bool    normalInWorldSpace = true;
                                                 return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
                                         }
@@ -326 +353 @@
                                 };
 
-                                BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh);
+                                BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh,colObjWorldTransform);
                                 rcb.m_hitFraction = resultCallback.m_closestHitFraction;
                                 triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal);
@@ -347 +374 @@
                                         btConcaveShape* m_triangleMesh;
 
+                                        btTransform m_colObjWorldTransform;
+
                                         BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
-                                                btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh):
-                  //@BP Mod
-                  btTriangleRaycastCallback(from,to, resultCallback->m_flags),
-                                                        m_resultCallback(resultCallback),
-                                                        m_collisionObject(collisionObject),
-                                                        m_triangleMesh(triangleMesh)
-                                                {
-                                                }
+                                                btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform):
+                                        //@BP Mod
+                                        btTriangleRaycastCallback(from,to, resultCallback->m_flags),
+                                                m_resultCallback(resultCallback),
+                                                m_collisionObject(collisionObject),
+                                                m_triangleMesh(triangleMesh),
+                                                m_colObjWorldTransform(colObjWorldTransform)
+                                        {
+                                        }
 
 
@@ -364 +394 @@
                                                 shapeInfo.m_triangleIndex = triangleIndex;
 
+                                                btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
+
                                                 btCollisionWorld::LocalRayResult rayResult
-                                                (m_collisionObject,
+                                                        (m_collisionObject,
                                                         &shapeInfo,
-                                                        hitNormalLocal,
+                                                        hitNormalWorld,
                                                         hitFraction);
 
-                                                bool    normalInWorldSpace = false;
+                                                bool    normalInWorldSpace = true;
                                                 return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
-
-
                                         }
 
@@ -379 +409 @@
 
 
-                                BridgeTriangleRaycastCallback   rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape);
+                                BridgeTriangleRaycastCallback   rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
                                 rcb.m_hitFraction = resultCallback.m_closestHitFraction;
 
@@ -390 +420 @@
                         }
                 } else {
-//                      BT_PROFILE("rayTestCompound");
+                        //                      BT_PROFILE("rayTestCompound");
                         ///@todo: use AABB tree or other BVH acceleration structure, see btDbvt
                         if (collisionShape->isCompound())
@@ -404 +434 @@
                                         btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
                                         collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
+                    struct LocalInfoAdder2 : public RayResultCallback {
+                                                RayResultCallback* m_userCallback;
+                                                int m_i;
+                        LocalInfoAdder2 (int i, RayResultCallback *user)
+                                                        : m_userCallback(user),
+                                                        m_i(i)
+                                                { 
+                                                        m_closestHitFraction = m_userCallback->m_closestHitFraction;
+                                                }
+                                                virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b)
+                            {
+                                    btCollisionWorld::LocalShapeInfo    shapeInfo;
+                                    shapeInfo.m_shapePart = -1;
+                                    shapeInfo.m_triangleIndex = m_i;
+                                    if (r.m_localShapeInfo == NULL)
+                                        r.m_localShapeInfo = &shapeInfo;
+
+                                                                        const btScalar result = m_userCallback->addSingleResult(r, b);
+                                                                        m_closestHitFraction = m_userCallback->m_closestHitFraction;
+                                                                        return result;
+                            }
+                    };
+
+                    LocalInfoAdder2 my_cb(i, &resultCallback);
+
                                         rayTestSingle(rayFromTrans,rayToTrans,
                                                 collisionObject,
                                                 childCollisionShape,
                                                 childWorldTrans,
-                                                resultCallback);
+                                                my_cb);
                                         // restore
                                         collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
@@ -418 +473 @@
 
 void    btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
-                                          btCollisionObject* collisionObject,
-                                          const btCollisionShape* collisionShape,
-                                          const btTransform& colObjWorldTransform,
-                                          ConvexResultCallback& resultCallback, btScalar allowedPenetration)
+                                                                                        btCollisionObject* collisionObject,
+                                                                                        const btCollisionShape* collisionShape,
+                                                                                        const btTransform& colObjWorldTransform,
+                                                                                        ConvexResultCallback& resultCallback, btScalar allowedPenetration)
 {
         if (collisionShape->isConvex())
@@ -433 +488 @@
                 btVoronoiSimplexSolver  simplexSolver;
                 btGjkEpaPenetrationDepthSolver  gjkEpaPenetrationSolver;
-                
+
                 btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
                 //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
@@ -439 +494 @@
 
                 btConvexCast* castPtr = &convexCaster1;
-        
-        
-                
+
+
+
                 if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
                 {
@@ -451 +506 @@
                                         castResult.m_normal.normalize();
                                         btCollisionWorld::LocalConvexResult localConvexResult
-                                                                (
-                                                                        collisionObject,
-                                                                        0,
-                                                                        castResult.m_normal,
-                                                                        castResult.m_hitPoint,
-                                                                        castResult.m_fraction
-                                                                );
+                                                (
+                                                collisionObject,
+                                                0,
+                                                castResult.m_normal,
+                                                castResult.m_hitPoint,
+                                                castResult.m_fraction
+                                                );
 
                                         bool normalInWorldSpace = true;
@@ -487 +542 @@
                                         BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
                                                 btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld):
-                                                btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
-                                                        m_resultCallback(resultCallback),
-                                                        m_collisionObject(collisionObject),
-                                                        m_triangleMesh(triangleMesh)
-                                                {
-                                                }
+                                        btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
+                                                m_resultCallback(resultCallback),
+                                                m_collisionObject(collisionObject),
+                                                m_triangleMesh(triangleMesh)
+                                        {
+                                        }
 
 
@@ -504 +559 @@
 
                                                         btCollisionWorld::LocalConvexResult convexResult
-                                                        (m_collisionObject,
+                                                                (m_collisionObject,
                                                                 &shapeInfo,
                                                                 hitNormalLocal,
@@ -544 +599 @@
                                         BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
                                                 btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape*      triangleMesh, const btTransform& triangleToWorld):
-                                                btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
-                                                        m_resultCallback(resultCallback),
-                                                        m_collisionObject(collisionObject),
-                                                        m_triangleMesh(triangleMesh)
-                                                {
-                                                }
+                                        btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
+                                                m_resultCallback(resultCallback),
+                                                m_collisionObject(collisionObject),
+                                                m_triangleMesh(triangleMesh)
+                                        {
+                                        }
 
 
@@ -561 +616 @@
 
                                                         btCollisionWorld::LocalConvexResult convexResult
-                                                        (m_collisionObject,
+                                                                (m_collisionObject,
                                                                 &shapeInfo,
                                                                 hitNormalLocal,
@@ -604 +659 @@
                                         btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
                                         collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
+                    struct      LocalInfoAdder : public ConvexResultCallback {
+                            ConvexResultCallback* m_userCallback;
+                                                        int m_i;
+
+                            LocalInfoAdder (int i, ConvexResultCallback *user)
+                                                                : m_userCallback(user), m_i(i)
+                                                        {
+                                                                m_closestHitFraction = m_userCallback->m_closestHitFraction;
+                                                        }
+                            virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult&      r,      bool b)
+                            {
+                                    btCollisionWorld::LocalShapeInfo    shapeInfo;
+                                    shapeInfo.m_shapePart = -1;
+                                    shapeInfo.m_triangleIndex = m_i;
+                                    if (r.m_localShapeInfo == NULL)
+                                        r.m_localShapeInfo = &shapeInfo;
+                                                                        const btScalar result = m_userCallback->addSingleResult(r, b);
+                                                                        m_closestHitFraction = m_userCallback->m_closestHitFraction;
+                                                                        return result;
+                                    
+                            }
+                    };
+
+                    LocalInfoAdder my_cb(i, &resultCallback);
+                                        
+
                                         objectQuerySingle(castShape, convexFromTrans,convexToTrans,
                                                 collisionObject,
                                                 childCollisionShape,
                                                 childWorldTrans,
-                                                resultCallback, allowedPenetration);
+                                                my_cb, allowedPenetration);
                                         // restore
                                         collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
@@ -631 +712 @@
 
         btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
-        :m_rayFromWorld(rayFromWorld),
-        m_rayToWorld(rayToWorld),
-        m_world(world),
-        m_resultCallback(resultCallback)
+                :m_rayFromWorld(rayFromWorld),
+                m_rayToWorld(rayToWorld),
+                m_world(world),
+                m_resultCallback(resultCallback)
         {
                 m_rayFromTrans.setIdentity();
@@ -644 +725 @@
 
                 rayDir.normalize ();
-                ///what about division by zero? --> just set rayDirection[i] to INF/1e30
-                m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
-                m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
-                m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
+                ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
+                m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
+                m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
+                m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
                 m_signs[0] = m_rayDirectionInverse[0] < 0.0;
                 m_signs[1] = m_rayDirectionInverse[1] < 0.0;
@@ -656 +737 @@
         }
 
-        
+
 
         virtual bool    process(const btBroadphaseProxy* proxy)
@@ -687 +768 @@
                                 m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
                                         collisionObject,
-                                                collisionObject->getCollisionShape(),
-                                                collisionObject->getWorldTransform(),
-                                                m_resultCallback);
+                                        collisionObject->getCollisionShape(),
+                                        collisionObject->getWorldTransform(),
+                                        m_resultCallback);
                         }
                 }
@@ -698 +779 @@
 void    btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
 {
-        BT_PROFILE("rayTest");
+        //BT_PROFILE("rayTest");
         /// use the broadphase to accelerate the search for objects, based on their aabb
         /// and for each object with ray-aabb overlap, perform an exact ray test
@@ -737 +818 @@
                 btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin());
                 btVector3 rayDir = unnormalizedRayDir.normalized();
-                ///what about division by zero? --> just set rayDirection[i] to INF/1e30
-                m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
-                m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
-                m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
+                ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
+                m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
+                m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
+                m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
                 m_signs[0] = m_rayDirectionInverse[0] < 0.0;
                 m_signs[1] = m_rayDirectionInverse[1] < 0.0;
@@ -761 +842 @@
                         //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
                         m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans,
-                                        collisionObject,
-                                                collisionObject->getCollisionShape(),
-                                                collisionObject->getWorldTransform(),
-                                                m_resultCallback,
-                                                m_allowedCcdPenetration);
-                }
-                
+                                collisionObject,
+                                collisionObject->getCollisionShape(),
+                                collisionObject->getWorldTransform(),
+                                m_resultCallback,
+                                m_allowedCcdPenetration);
+                }
+
                 return true;
         }
@@ -782 +863 @@
         /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
 
-        
+
 
         btTransform     convexFromTrans,convexToTrans;
@@ -825 +906 @@
                                 objectQuerySingle(castShape, convexFromTrans,convexToTrans,
                                         collisionObject,
-                                                collisionObject->getCollisionShape(),
-                                                collisionObject->getWorldTransform(),
-                                                resultCallback,
-                                                allowedCcdPenetration);
+                                        collisionObject->getCollisionShape(),
+                                        collisionObject->getWorldTransform(),
+                                        resultCallback,
+                                        allowedCcdPenetration);
                         }
                 }
@@ -834 +915 @@
 #endif //USE_BRUTEFORCE_RAYBROADPHASE
 }
+
+
+
+struct btBridgedManifoldResult : public btManifoldResult
+{
+
+        btCollisionWorld::ContactResultCallback&        m_resultCallback;
+
+        btBridgedManifoldResult( btCollisionObject* obj0,btCollisionObject* obj1,btCollisionWorld::ContactResultCallback& resultCallback )
+                :btManifoldResult(obj0,obj1),
+                m_resultCallback(resultCallback)
+        {
+        }
+
+        virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+        {
+                bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
+                btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
+                btVector3 localA;
+                btVector3 localB;
+                if (isSwapped)
+                {
+                        localA = m_rootTransB.invXform(pointA );
+                        localB = m_rootTransA.invXform(pointInWorld);
+                } else
+                {
+                        localA = m_rootTransA.invXform(pointA );
+                        localB = m_rootTransB.invXform(pointInWorld);
+                }
+                
+                btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
+                newPt.m_positionWorldOnA = pointA;
+                newPt.m_positionWorldOnB = pointInWorld;
+                
+           //BP mod, store contact triangles.
+                if (isSwapped)
+                {
+                        newPt.m_partId0 = m_partId1;
+                        newPt.m_partId1 = m_partId0;
+                        newPt.m_index0  = m_index1;
+                        newPt.m_index1  = m_index0;
+                } else
+                {
+                        newPt.m_partId0 = m_partId0;
+                        newPt.m_partId1 = m_partId1;
+                        newPt.m_index0  = m_index0;
+                        newPt.m_index1  = m_index1;
+                }
+
+                //experimental feature info, for per-triangle material etc.
+                btCollisionObject* obj0 = isSwapped? m_body1 : m_body0;
+                btCollisionObject* obj1 = isSwapped? m_body0 : m_body1;
+                m_resultCallback.addSingleResult(newPt,obj0,newPt.m_partId0,newPt.m_index0,obj1,newPt.m_partId1,newPt.m_index1);
+
+        }
+        
+};
+
+
+
+struct btSingleContactCallback : public btBroadphaseAabbCallback
+{
+
+        btCollisionObject* m_collisionObject;
+        btCollisionWorld*       m_world;
+        btCollisionWorld::ContactResultCallback&        m_resultCallback;
+        
+        
+        btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world,btCollisionWorld::ContactResultCallback& resultCallback)
+                :m_collisionObject(collisionObject),
+                m_world(world),
+                m_resultCallback(resultCallback)
+        {
+        }
+
+        virtual bool    process(const btBroadphaseProxy* proxy)
+        {
+                btCollisionObject*      collisionObject = (btCollisionObject*)proxy->m_clientObject;
+                if (collisionObject == m_collisionObject)
+                        return true;
+
+                //only perform raycast if filterMask matches
+                if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
+                {
+                        btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(m_collisionObject,collisionObject);
+                        if (algorithm)
+                        {
+                                btBridgedManifoldResult contactPointResult(m_collisionObject,collisionObject, m_resultCallback);
+                                //discrete collision detection query
+                                algorithm->processCollision(m_collisionObject,collisionObject, m_world->getDispatchInfo(),&contactPointResult);
+
+                                algorithm->~btCollisionAlgorithm();
+                                m_world->getDispatcher()->freeCollisionAlgorithm(algorithm);
+                        }
+                }
+                return true;
+        }
+};
+
+
+///contactTest performs a discrete collision test against all objects in the btCollisionWorld, and calls the resultCallback.
+///it reports one or more contact points for every overlapping object (including the one with deepest penetration)
+void    btCollisionWorld::contactTest( btCollisionObject* colObj, ContactResultCallback& resultCallback)
+{
+        btVector3 aabbMin,aabbMax;
+        colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(),aabbMin,aabbMax);
+        btSingleContactCallback contactCB(colObj,this,resultCallback);
+        
+        m_broadphasePairCache->aabbTest(aabbMin,aabbMax,contactCB);
+}
+
+
+///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected.
+///it reports one or more contact points (including the one with deepest penetration)
+void    btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback)
+{
+        btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(colObjA,colObjB);
+        if (algorithm)
+        {
+                btBridgedManifoldResult contactPointResult(colObjA,colObjB, resultCallback);
+                //discrete collision detection query
+                algorithm->processCollision(colObjA,colObjB, getDispatchInfo(),&contactPointResult);
+
+                algorithm->~btCollisionAlgorithm();
+                getDispatcher()->freeCollisionAlgorithm(algorithm);
+        }
+
+}
+
+
+
+
+class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
+{
+        btIDebugDraw*   m_debugDrawer;
+        btVector3       m_color;
+        btTransform     m_worldTrans;
+
+public:
+
+        DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
+          m_debugDrawer(debugDrawer),
+                  m_color(color),
+                  m_worldTrans(worldTrans)
+          {
+          }
+
+          virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
+          {
+                  processTriangle(triangle,partId,triangleIndex);
+          }
+
+          virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
+          {
+                  (void)partId;
+                  (void)triangleIndex;
+
+                  btVector3 wv0,wv1,wv2;
+                  wv0 = m_worldTrans*triangle[0];
+                  wv1 = m_worldTrans*triangle[1];
+                  wv2 = m_worldTrans*triangle[2];
+                  btVector3 center = (wv0+wv1+wv2)*btScalar(1./3.);
+
+                  btVector3 normal = (wv1-wv0).cross(wv2-wv0);
+                  normal.normalize();
+                  btVector3 normalColor(1,1,0);
+                  m_debugDrawer->drawLine(center,center+normal,normalColor);
+
+
+
+                 
+                  m_debugDrawer->drawLine(wv0,wv1,m_color);
+                  m_debugDrawer->drawLine(wv1,wv2,m_color);
+                  m_debugDrawer->drawLine(wv2,wv0,m_color);
+          }
+};
+
+
+void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
+{
+        // Draw a small simplex at the center of the object
+        getDebugDrawer()->drawTransform(worldTransform,1);
+
+        if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
+        {
+                const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
+                for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
+                {
+                        btTransform childTrans = compoundShape->getChildTransform(i);
+                        const btCollisionShape* colShape = compoundShape->getChildShape(i);
+                        debugDrawObject(worldTransform*childTrans,colShape,color);
+                }
+
+        } else
+        {
+                switch (shape->getShapeType())
+                {
+
+                case BOX_SHAPE_PROXYTYPE:
+                        {
+                                const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
+                                btVector3 halfExtents = boxShape->getHalfExtentsWithMargin();
+                                getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color);
+                                break;
+                        }
+
+                case SPHERE_SHAPE_PROXYTYPE:
+                        {
+                                const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
+                                btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
+
+                                getDebugDrawer()->drawSphere(radius, worldTransform, color);
+                                break;
+                        }
+                case MULTI_SPHERE_SHAPE_PROXYTYPE:
+                        {
+                                const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
+
+                                btTransform childTransform;
+                                childTransform.setIdentity();
+
+                                for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
+                                {
+                                        childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
+                                        getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
+                                }
+
+                                break;
+                        }
+                case CAPSULE_SHAPE_PROXYTYPE:
+                        {
+                                const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
+
+                                btScalar radius = capsuleShape->getRadius();
+                                btScalar halfHeight = capsuleShape->getHalfHeight();
+
+                                int upAxis = capsuleShape->getUpAxis();
+
+
+                                btVector3 capStart(0.f,0.f,0.f);
+                                capStart[upAxis] = -halfHeight;
+
+                                btVector3 capEnd(0.f,0.f,0.f);
+                                capEnd[upAxis] = halfHeight;
+
+                                // Draw the ends
+                                {
+
+                                        btTransform childTransform = worldTransform;
+                                        childTransform.getOrigin() = worldTransform * capStart;
+                                        getDebugDrawer()->drawSphere(radius, childTransform, color);
+                                }
+
+                                {
+                                        btTransform childTransform = worldTransform;
+                                        childTransform.getOrigin() = worldTransform * capEnd;
+                                        getDebugDrawer()->drawSphere(radius, childTransform, color);
+                                }
+
+                                // Draw some additional lines
+                                btVector3 start = worldTransform.getOrigin();
+
+
+                                capStart[(upAxis+1)%3] = radius;
+                                capEnd[(upAxis+1)%3] = radius;
+                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+                                capStart[(upAxis+1)%3] = -radius;
+                                capEnd[(upAxis+1)%3] = -radius;
+                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+
+                                capStart[(upAxis+1)%3] = 0.f;
+                                capEnd[(upAxis+1)%3] = 0.f;
+
+                                capStart[(upAxis+2)%3] = radius;
+                                capEnd[(upAxis+2)%3] = radius;
+                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+                                capStart[(upAxis+2)%3] = -radius;
+                                capEnd[(upAxis+2)%3] = -radius;
+                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+
+
+                                break;
+                        }
+                case CONE_SHAPE_PROXYTYPE:
+                        {
+                                const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
+                                btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
+                                btScalar height = coneShape->getHeight();//+coneShape->getMargin();
+                                btVector3 start = worldTransform.getOrigin();
+
+                                int upAxis= coneShape->getConeUpIndex();
+
+
+                                btVector3       offsetHeight(0,0,0);
+                                offsetHeight[upAxis] = height * btScalar(0.5);
+                                btVector3       offsetRadius(0,0,0);
+                                offsetRadius[(upAxis+1)%3] = radius;
+                                btVector3       offset2Radius(0,0,0);
+                                offset2Radius[(upAxis+2)%3] = radius;
+
+                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
+                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
+                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
+                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
+
+                                // Drawing the base of the cone
+                                btVector3 yaxis(0,0,0);
+                                yaxis[upAxis] = btScalar(1.0);
+                                btVector3 xaxis(0,0,0);
+                                xaxis[(upAxis+1)%3] = btScalar(1.0);
+                                getDebugDrawer()->drawArc(start-worldTransform.getBasis()*(offsetHeight),worldTransform.getBasis()*yaxis,worldTransform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,10.0);
+                break;
+
+                        }
+                case CYLINDER_SHAPE_PROXYTYPE:
+                        {
+                                const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
+                                int upAxis = cylinder->getUpAxis();
+                                btScalar radius = cylinder->getRadius();
+                                btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
+                                btVector3 start = worldTransform.getOrigin();
+                                btVector3       offsetHeight(0,0,0);
+                                offsetHeight[upAxis] = halfHeight;
+                                btVector3       offsetRadius(0,0,0);
+                                offsetRadius[(upAxis+1)%3] = radius;
+                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
+                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
+
+                                // Drawing top and bottom caps of the cylinder
+                                btVector3 yaxis(0,0,0);
+                                yaxis[upAxis] = btScalar(1.0);
+                                btVector3 xaxis(0,0,0);
+                                xaxis[(upAxis+1)%3] = btScalar(1.0);
+                                getDebugDrawer()->drawArc(start-worldTransform.getBasis()*(offsetHeight),worldTransform.getBasis()*yaxis,worldTransform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0));
+                                getDebugDrawer()->drawArc(start+worldTransform.getBasis()*(offsetHeight),worldTransform.getBasis()*yaxis,worldTransform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0));
+                                break;
+                        }
+
+                case STATIC_PLANE_PROXYTYPE:
+                        {
+                                const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
+                                btScalar planeConst = staticPlaneShape->getPlaneConstant();
+                                const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
+                                btVector3 planeOrigin = planeNormal * planeConst;
+                                btVector3 vec0,vec1;
+                                btPlaneSpace1(planeNormal,vec0,vec1);
+                                btScalar vecLen = 100.f;
+                                btVector3 pt0 = planeOrigin + vec0*vecLen;
+                                btVector3 pt1 = planeOrigin - vec0*vecLen;
+                                btVector3 pt2 = planeOrigin + vec1*vecLen;
+                                btVector3 pt3 = planeOrigin - vec1*vecLen;
+                                getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color);
+                                getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color);
+                                break;
+
+                        }
+                default:
+                        {
+
+                                if (shape->isConcave())
+                                {
+                                        btConcaveShape* concaveMesh = (btConcaveShape*) shape;
+
+                                        ///@todo pass camera, for some culling? no -> we are not a graphics lib
+                                        btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+                                        btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+
+                                        DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
+                                        concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
+
+                                }
+
+                                if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
+                                {
+                                        btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
+                                        //todo: pass camera for some culling                    
+                                        btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
+                                        btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
+                                        //DebugDrawcallback drawCallback;
+                                        DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
+                                        convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
+                                }
+
+
+                                /// for polyhedral shapes
+                                if (shape->isPolyhedral())
+                                {
+                                        btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
+
+                                        int i;
+                                        for (i=0;i<polyshape->getNumEdges();i++)
+                                        {
+                                                btVector3 a,b;
+                                                polyshape->getEdge(i,a,b);
+                                                btVector3 wa = worldTransform * a;
+                                                btVector3 wb = worldTransform * b;
+                                                getDebugDrawer()->drawLine(wa,wb,color);
+
+                                        }
+
+
+                                }
+                        }
+                }
+        }
+}
+
+
+void    btCollisionWorld::debugDrawWorld()
+{
+        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
+        {
+                int numManifolds = getDispatcher()->getNumManifolds();
+                btVector3 color(0,0,0);
+                for (int i=0;i<numManifolds;i++)
+                {
+                        btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
+                        //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
+                        //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
+
+                        int numContacts = contactManifold->getNumContacts();
+                        for (int j=0;j<numContacts;j++)
+                        {
+                                btManifoldPoint& cp = contactManifold->getContactPoint(j);
+                                getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
+                        }
+                }
+        }
+
+        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
+        {
+                int i;
+
+                for (  i=0;i<m_collisionObjects.size();i++)
+                {
+                        btCollisionObject* colObj = m_collisionObjects[i];
+                        if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0)
+                        {
+                                if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
+                                {
+                                        btVector3 color(btScalar(1.),btScalar(1.),btScalar(1.));
+                                        switch(colObj->getActivationState())
+                                        {
+                                        case  ACTIVE_TAG:
+                                                color = btVector3(btScalar(1.),btScalar(1.),btScalar(1.)); break;
+                                        case ISLAND_SLEEPING:
+                                                color =  btVector3(btScalar(0.),btScalar(1.),btScalar(0.));break;
+                                        case WANTS_DEACTIVATION:
+                                                color = btVector3(btScalar(0.),btScalar(1.),btScalar(1.));break;
+                                        case DISABLE_DEACTIVATION:
+                                                color = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));break;
+                                        case DISABLE_SIMULATION:
+                                                color = btVector3(btScalar(1.),btScalar(1.),btScalar(0.));break;
+                                        default:
+                                                {
+                                                        color = btVector3(btScalar(1),btScalar(0.),btScalar(0.));
+                                                }
+                                        };
+
+                                        debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
+                                }
+                                if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+                                {
+                                        btVector3 minAabb,maxAabb;
+                                        btVector3 colorvec(1,0,0);
+                                        colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
+                                        m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
+                                }
+                        }
+
+                }
+        }
+}
+
+
+void    btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
+{
+        int i;
+        //serialize all collision objects
+        for (i=0;i<m_collisionObjects.size();i++)
+        {
+                btCollisionObject* colObj = m_collisionObjects[i];
+                if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT)
+                {
+                        colObj->serializeSingleObject(serializer);
+                }
+        }
+
+        ///keep track of shapes already serialized
+        btHashMap<btHashPtr,btCollisionShape*>  serializedShapes;
+
+        for (i=0;i<m_collisionObjects.size();i++)
+        {
+                btCollisionObject* colObj = m_collisionObjects[i];
+                btCollisionShape* shape = colObj->getCollisionShape();
+
+                if (!serializedShapes.find(shape))
+                {
+                        serializedShapes.insert(shape,shape);
+                        shape->serializeSingleShape(serializer);
+                }
+        }
+
+}
+
+
+void    btCollisionWorld::serialize(btSerializer* serializer)
+{
+
+        serializer->startSerialization();
+        
+        serializeCollisionObjects(serializer);
+        
+        serializer->finishSerialization();
+}
+

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h

@@ -23 +23 @@
  * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ).
  *
+ * The main documentation is Bullet_User_Manual.pdf, included in the source code distribution.
  * There is the Physics Forum for feedback and general Collision Detection and Physics discussions.
  * Please visit http://www.bulletphysics.com
@@ -30 +31 @@
  * @subsection step1 Step 1: Download
  * You can download the Bullet Physics Library from the Google Code repository: http://code.google.com/p/bullet/downloads/list
+ *
  * @subsection step2 Step 2: Building
- * Bullet comes with autogenerated Project Files for Microsoft Visual Studio 6, 7, 7.1 and 8.
- * The main Workspace/Solution is located in Bullet/msvc/8/wksbullet.sln (replace 8 with your version).
- * 
- * Under other platforms, like Linux or Mac OS-X, Bullet can be build using either using make, cmake, http://www.cmake.org , or jam, http://www.perforce.com/jam/jam.html . cmake can autogenerate Xcode, KDevelop, MSVC and other build systems. just run cmake . in the root of Bullet.
- * So if you are not using MSVC or cmake, you can run ./autogen.sh ./configure to create both Makefile and Jamfile and then run make or jam.
- * Jam is a build system that can build the library, demos and also autogenerate the MSVC Project Files.
- * If you don't have jam installed, you can make jam from the included jam-2.5 sources, or download jam from ftp://ftp.perforce.com/jam
+ * Bullet main build system for all platforms is cmake, you can download http://www.cmake.org
+ * cmake can autogenerate projectfiles for Microsoft Visual Studio, Apple Xcode, KDevelop and Unix Makefiles.
+ * The easiest is to run the CMake cmake-gui graphical user interface and choose the options and generate projectfiles.
+ * You can also use cmake in the command-line. Here are some examples for various platforms:
+ * cmake . -G "Visual Studio 9 2008"
+ * cmake . -G Xcode
+ * cmake . -G "Unix Makefiles"
+ * Although cmake is recommended, you can also use autotools for UNIX: ./autogen.sh ./configure to create a Makefile and then run make.
  * 
  * @subsection step3 Step 3: Testing demos
@@ -54 +57 @@
  *
  * @section copyright Copyright
- * Copyright (C) 2005-2008 Erwin Coumans, some contributions Copyright Gino van den Bergen, Christer Ericson, Simon Hobbs, Ricardo Padrela, F Richter(res), Stephane Redon
- * Special thanks to all visitors of the Bullet Physics forum, and in particular above contributors, John McCutchan, Nathanael Presson, Dave Eberle, Dirk Gregorius, Erin Catto, Dave Eberle, Adam Moravanszky,
- * Pierre Terdiman, Kenny Erleben, Russell Smith, Oliver Strunk, Jan Paul van Waveren, Marten Svanfeldt.
+ * For up-to-data information and copyright and contributors list check out the Bullet_User_Manual.pdf
  * 
  */
-
-
  
  
@@ -71 +70 @@
 class btConvexShape;
 class btBroadphaseInterface;
+class btSerializer;
+
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btTransform.h"
@@ -82 +83 @@
 {
 
-
         
 protected:
 
         btAlignedObjectArray<btCollisionObject*>        m_collisionObjects;
-
         
         btDispatcher*   m_dispatcher1;
@@ -99 +98 @@
         btIDebugDraw*   m_debugDrawer;
 
-
-        
+        ///m_forceUpdateAllAabbs can be set to false as an optimization to only update active object AABBs
+        ///it is true by default, because it is error-prone (setting the position of static objects wouldn't update their AABB)
+        bool m_forceUpdateAllAabbs;
+
+        void    serializeCollisionObjects(btSerializer* serializer);
+
 public:
 
@@ -142 +145 @@
 
         virtual void    updateAabbs();
-
         
         virtual void    setDebugDrawer(btIDebugDraw*    debugDrawer)
@@ -153 +155 @@
                 return m_debugDrawer;
         }
+
+        virtual void    debugDrawWorld();
+
+        virtual void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
 
 
@@ -161 +167 @@
                 int     m_shapePart;
                 int     m_triangleIndex;
-
                 
                 //const btCollisionShape*       m_shapeTemp;
@@ -239 +244 @@
                 btVector3       m_hitNormalWorld;
                 btVector3       m_hitPointWorld;
-
                         
                 virtual btScalar        addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace)
@@ -245 +249 @@
                         //caller already does the filter on the m_closestHitFraction
                         btAssert(rayResult.m_hitFraction <= m_closestHitFraction);
-
                         
                         m_closestHitFraction = rayResult.m_hitFraction;
@@ -262 +265 @@
         };
 
+        struct  AllHitsRayResultCallback : public RayResultCallback
+        {
+                AllHitsRayResultCallback(const btVector3&       rayFromWorld,const btVector3&   rayToWorld)
+                :m_rayFromWorld(rayFromWorld),
+                m_rayToWorld(rayToWorld)
+                {
+                }
+
+                btAlignedObjectArray<btCollisionObject*>                m_collisionObjects;
+
+                btVector3       m_rayFromWorld;//used to calculate hitPointWorld from hitFraction
+                btVector3       m_rayToWorld;
+
+                btAlignedObjectArray<btVector3> m_hitNormalWorld;
+                btAlignedObjectArray<btVector3> m_hitPointWorld;
+                btAlignedObjectArray<btScalar> m_hitFractions;
+                        
+                virtual btScalar        addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace)
+                {
+                        m_collisionObject = rayResult.m_collisionObject;
+                        m_collisionObjects.push_back(rayResult.m_collisionObject);
+                        btVector3 hitNormalWorld;
+                        if (normalInWorldSpace)
+                        {
+                                hitNormalWorld = rayResult.m_hitNormalLocal;
+                        } else
+                        {
+                                ///need to transform normal into worldspace
+                                hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal;
+                        }
+                        m_hitNormalWorld.push_back(hitNormalWorld);
+                        btVector3 hitPointWorld;
+                        hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction);
+                        m_hitPointWorld.push_back(hitPointWorld);
+                        m_hitFractions.push_back(rayResult.m_hitFraction);
+                        return m_closestHitFraction;
+                }
+        };
+
 
         struct LocalConvexResult
@@ -292 +334 @@
                 short int       m_collisionFilterGroup;
                 short int       m_collisionFilterMask;
-
                 
                 ConvexResultCallback()
@@ -304 +345 @@
                 {
                 }
-
                 
                 bool    hasHit() const
@@ -310 +350 @@
                         return (m_closestHitFraction < btScalar(1.));
                 }
-
 
                 
@@ -339 +378 @@
                 btVector3       m_hitPointWorld;
                 btCollisionObject*      m_hitCollisionObject;
-
                 
                 virtual btScalar        addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace)
@@ -345 +383 @@
 //caller already does the filter on the m_closestHitFraction
                         btAssert(convexResult.m_hitFraction <= m_closestHitFraction);
-
                                                 
                         m_closestHitFraction = convexResult.m_hitFraction;
@@ -362 +399 @@
         };
 
+        ///ContactResultCallback is used to report contact points
+        struct  ContactResultCallback
+        {
+                short int       m_collisionFilterGroup;
+                short int       m_collisionFilterMask;
+                
+                ContactResultCallback()
+                        :m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
+                        m_collisionFilterMask(btBroadphaseProxy::AllFilter)
+                {
+                }
+
+                virtual ~ContactResultCallback()
+                {
+                }
+                
+                virtual bool needsCollision(btBroadphaseProxy* proxy0) const
+                {
+                        bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0;
+                        collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+                        return collides;
+                }
+
+                virtual btScalar        addSingleResult(btManifoldPoint& cp,    const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1) = 0;
+        };
+
+
+
         int     getNumCollisionObjects() const
         {
@@ -369 +434 @@
         /// rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback
         /// This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback.
-        void    rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; 
-
-        // convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback
-        // This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback.
+        virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; 
+
+        /// convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback
+        /// This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback.
         void    convexSweepTest (const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback,  btScalar allowedCcdPenetration = btScalar(0.)) const;
+
+        ///contactTest performs a discrete collision test between colObj against all objects in the btCollisionWorld, and calls the resultCallback.
+        ///it reports one or more contact points for every overlapping object (including the one with deepest penetration)
+        void    contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback);
+
+        ///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected.
+        ///it reports one or more contact points (including the one with deepest penetration)
+        void    contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback);
 
 
@@ -392 +465 @@
                                           ConvexResultCallback& resultCallback, btScalar        allowedPenetration);
 
-        void    addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter);
+        virtual void    addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter);
 
         btCollisionObjectArray& getCollisionObjectArray()
@@ -405 +478 @@
 
 
-        void    removeCollisionObject(btCollisionObject* collisionObject);
+        virtual void    removeCollisionObject(btCollisionObject* collisionObject);
 
         virtual void    performDiscreteCollisionDetection();
@@ -418 +491 @@
                 return m_dispatchInfo;
         }
+        
+        bool    getForceUpdateAllAabbs() const
+        {
+                return m_forceUpdateAllAabbs;
+        }
+        void setForceUpdateAllAabbs( bool forceUpdateAllAabbs)
+        {
+                m_forceUpdateAllAabbs = forceUpdateAllAabbs;
+        }
+
+        ///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (Bullet/Demos/SerializeDemo)
+        virtual void    serialize(btSerializer* serializer);
 
 };

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp

@@ -115 +115 @@
         void    ProcessChildShape(btCollisionShape* childShape,int index)
         {
-                
+                btAssert(index>=0);
                 btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape());
+                btAssert(index<compoundShape->getNumChildShapes());
 
 
@@ -142 +143 @@
                         if (!m_childCollisionAlgorithms[index])
                                 m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(m_compoundColObj,m_otherObj,m_sharedManifold);
+
+                        ///detect swapping case
+                        if (m_resultOut->getBody0Internal() == m_compoundColObj)
+                        {
+                                m_resultOut->setShapeIdentifiersA(-1,index);
+                        } else
+                        {
+                                m_resultOut->setShapeIdentifiersB(-1,index);
+                        }
 
                         m_childCollisionAlgorithms[index]->processCollision(m_compoundColObj,m_otherObj,m_dispatchInfo,m_resultOut);
@@ -258 +268 @@
                 int i;
                 btManifoldArray manifoldArray;
-
+        btCollisionShape* childShape = 0;
+        btTransform     orgTrans;
+        btTransform     orgInterpolationTrans;
+        btTransform     newChildWorldTrans;
+        btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;        
+        
                 for (i=0;i<numChildren;i++)
                 {
                         if (m_childCollisionAlgorithms[i])
                         {
-                                btCollisionShape* childShape = compoundShape->getChildShape(i);
+                                childShape = compoundShape->getChildShape(i);
                         //if not longer overlapping, remove the algorithm
-                                btTransform     orgTrans = colObj->getWorldTransform();
-                                btTransform     orgInterpolationTrans = colObj->getInterpolationWorldTransform();
+                orgTrans = colObj->getWorldTransform();
+                orgInterpolationTrans = colObj->getInterpolationWorldTransform();
                                 const btTransform& childTrans = compoundShape->getChildTransform(i);
-                                btTransform     newChildWorldTrans = orgTrans*childTrans ;
+                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);
@@ -281 +295 @@
                                         m_childCollisionAlgorithms[i] = 0;
                                 }
-
                         }
-                        
-                }
-
-                
-
+                }
         }
 }
@@ -312 +321 @@
         int numChildren = m_childCollisionAlgorithms.size();
         int i;
+    btTransform orgTrans;
+    btScalar frac;
         for (i=0;i<numChildren;i++)
         {
@@ -318 +329 @@
 
                 //backup
-                btTransform     orgTrans = colObj->getWorldTransform();
+        orgTrans = colObj->getWorldTransform();
         
                 const btTransform& childTrans = compoundShape->getChildTransform(i);
@@ -326 +337 @@
                 btCollisionShape* tmpShape = colObj->getCollisionShape();
                 colObj->internalSetTemporaryCollisionShape( childShape );
-                btScalar frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut);
+        frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut);
                 if (frac<hitFraction)
                 {

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp

@@ -96 +96 @@
         if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe ))
         {
-                btVector3 color(255,255,0);
+                btVector3 color(1,1,0);
                 btTransform& tr = ob->getWorldTransform();
                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
@@ -122 +122 @@
                 
                 btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr);
-                ///this should use the btDispatcher, so the actual registered algorithm is used
-                //              btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody);
-
-                m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex);
-        //      cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
-//              cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
+
+                if (m_resultOut->getBody0Internal() == m_triBody)
+                {
+                        m_resultOut->setShapeIdentifiersA(partId,triangleIndex);
+                }
+                else
+                {
+                        m_resultOut->setShapeIdentifiersB(partId,triangleIndex);
+                }
+        
                 colAlgo->processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
                 colAlgo->~btCollisionAlgorithm();

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

@@ -14 +14 @@
 */
 
+///Specialized capsule-capsule collision algorithm has been added for Bullet 2.75 release to increase ragdoll performance
+///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums
+///with reproduction case
+//define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1
+
 #include "btConvexConvexAlgorithm.h"
 
@@ -21 +26 @@
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
+
+
 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
@@ -44 +52 @@
 
 
-
+///////////
+
+
+
+static SIMD_FORCE_INLINE void segmentsClosestPoints(
+        btVector3& ptsVector,
+        btVector3& offsetA,
+        btVector3& offsetB,
+        btScalar& tA, btScalar& tB,
+        const btVector3& translation,
+        const btVector3& dirA, btScalar hlenA,
+        const btVector3& dirB, btScalar hlenB )
+{
+        // compute the parameters of the closest points on each line segment
+
+        btScalar dirA_dot_dirB = btDot(dirA,dirB);
+        btScalar dirA_dot_trans = btDot(dirA,translation);
+        btScalar dirB_dot_trans = btDot(dirB,translation);
+
+        btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
+
+        if ( denom == 0.0f ) {
+                tA = 0.0f;
+        } else {
+                tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
+                if ( tA < -hlenA )
+                        tA = -hlenA;
+                else if ( tA > hlenA )
+                        tA = hlenA;
+        }
+
+        tB = tA * dirA_dot_dirB - dirB_dot_trans;
+
+        if ( tB < -hlenB ) {
+                tB = -hlenB;
+                tA = tB * dirA_dot_dirB + dirA_dot_trans;
+
+                if ( tA < -hlenA )
+                        tA = -hlenA;
+                else if ( tA > hlenA )
+                        tA = hlenA;
+        } else if ( tB > hlenB ) {
+                tB = hlenB;
+                tA = tB * dirA_dot_dirB + dirA_dot_trans;
+
+                if ( tA < -hlenA )
+                        tA = -hlenA;
+                else if ( tA > hlenA )
+                        tA = hlenA;
+        }
+
+        // compute the closest points relative to segment centers.
+
+        offsetA = dirA * tA;
+        offsetB = dirB * tB;
+
+        ptsVector = translation - offsetA + offsetB;
+}
+
+
+static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance(
+        btVector3& normalOnB,
+        btVector3& pointOnB,
+        btScalar capsuleLengthA,
+        btScalar        capsuleRadiusA,
+        btScalar capsuleLengthB,
+        btScalar        capsuleRadiusB,
+        int capsuleAxisA,
+        int capsuleAxisB,
+        const btTransform& transformA,
+        const btTransform& transformB,
+        btScalar distanceThreshold )
+{
+        btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA);
+        btVector3 translationA = transformA.getOrigin();
+        btVector3 directionB = transformB.getBasis().getColumn(capsuleAxisB);
+        btVector3 translationB = transformB.getOrigin();
+
+        // translation between centers
+
+        btVector3 translation = translationB - translationA;
+
+        // compute the closest points of the capsule line segments
+
+        btVector3 ptsVector;           // the vector between the closest points
+        
+        btVector3 offsetA, offsetB;    // offsets from segment centers to their closest points
+        btScalar tA, tB;              // parameters on line segment
+
+        segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation,
+                                                   directionA, capsuleLengthA, directionB, capsuleLengthB );
+
+        btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB;
+
+        if ( distance > distanceThreshold )
+                return distance;
+
+        btScalar lenSqr = ptsVector.length2();
+        if (lenSqr<= (SIMD_EPSILON*SIMD_EPSILON))
+        {
+                //degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA'
+                btVector3 q;
+                btPlaneSpace1(directionA,normalOnB,q);
+        } else
+        {
+                // compute the contact normal
+                normalOnB = ptsVector*-btRecipSqrt(lenSqr);
+        }
+        pointOnB = transformB.getOrigin()+offsetB + normalOnB * capsuleRadiusB;
+
+        return distance;
+}
+
+
+
+
+
+
+
+//////////
 
 
@@ -70 +197 @@
 m_lowLevelOfDetail(false),
 #ifdef USE_SEPDISTANCE_UTIL2
-,m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
+m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
                           (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
 #endif
@@ -112 +239 @@
                 m_transformA(transformA),
                 m_transformB(transformB),
+                m_unPerturbedTransform(unPerturbedTransform),
                 m_perturbA(perturbA),
-                m_unPerturbedTransform(unPerturbedTransform),
                 m_debugDrawer(debugDrawer)
         {
@@ -156 +283 @@
 extern btScalar gContactBreakingThreshold;
 
+
 //
 // Convex-Convex collision algorithm
@@ -177 +305 @@
         btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
 
+        btVector3  normalOnB;
+                btVector3  pointOnBWorld;
+#ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
+        if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
+        {
+                btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
+                btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
+                btVector3 localScalingA = capsuleA->getLocalScaling();
+                btVector3 localScalingB = capsuleB->getLocalScaling();
+                
+                btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
+
+                btScalar dist = capsuleCapsuleDistance(normalOnB,       pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(),
+                        capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(),
+                        body0->getWorldTransform(),body1->getWorldTransform(),threshold);
+
+                if (dist<threshold)
+                {
+                        btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
+                        resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);       
+                }
+                resultOut->refreshContactPoints();
+                return;
+        }
+#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
+
+
 #ifdef USE_SEPDISTANCE_UTIL2
-        m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
+        if (dispatchInfo.m_useConvexConservativeDistanceUtil)
+        {
+                m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
+        }
+
         if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
 #endif //USE_SEPDISTANCE_UTIL2
@@ -195 +354 @@
         if (dispatchInfo.m_useConvexConservativeDistanceUtil)
         {
-                input.m_maximumDistanceSquared = 1e30f;
+                input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
         } else
 #endif //USE_SEPDISTANCE_UTIL2
         {
-                input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
+                if (dispatchInfo.m_convexMaxDistanceUseCPT)
+                {
+                        input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
+                } else
+                {
+                        input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
+                }
                 input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
         }
@@ -208 +373 @@
 
         gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
-        btScalar sepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold;
-
-        //now perturbe directions to get multiple contact points
-        btVector3 v0,v1;
-        btVector3 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
-        btPlaneSpace1(sepNormalWorldSpace,v0,v1);
+
+        
+
+#ifdef USE_SEPDISTANCE_UTIL2
+        btScalar sepDist = 0.f;
+        if (dispatchInfo.m_useConvexConservativeDistanceUtil)
+        {
+                sepDist = gjkPairDetector.getCachedSeparatingDistance();
+                if (sepDist>SIMD_EPSILON)
+                {
+                        sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
+                        //now perturbe directions to get multiple contact points
+                        
+                }
+        }
+#endif //USE_SEPDISTANCE_UTIL2
+
         //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
         
         //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
-        if (resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
+        if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
         {
                 
                 int i;
+                btVector3 v0,v1;
+                btVector3 sepNormalWorldSpace;
+        
+                sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
+                btPlaneSpace1(sepNormalWorldSpace,v0,v1);
+
 
                 bool perturbeA = true;
@@ -250 +432 @@
                 for ( i=0;i<m_numPerturbationIterations;i++)
                 {
+                        if (v0.length2()>SIMD_EPSILON)
+                        {
                         btQuaternion perturbeRot(v0,perturbeAngle);
                         btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
@@ -273 +457 @@
                         btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw);
                         gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw);
+                        }
                         
-                        
                 }
         }
@@ -281 +465 @@
 
 #ifdef USE_SEPDISTANCE_UTIL2
-        if (dispatchInfo.m_useConvexConservativeDistanceUtil)
+        if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
         {
                 m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h

@@ -32 +32 @@
 ///Either improve GJK for large size ratios (testing a 100 units versus a 0.1 unit object) or only enable the util
 ///for certain pairs that have a small size ratio
-///#define USE_SEPDISTANCE_UTIL2 1
+
+//#define USE_SEPDISTANCE_UTIL2 1
 
 ///The convexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations between two convex objects.

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp

@@ -103 +103 @@
         btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape();
 
-    bool hasCollision = false;
+    
         const btVector3& planeNormal = planeShape->getPlaneNormal();
-        const btScalar& planeConstant = planeShape->getPlaneConstant();
+        //const btScalar& planeConstant = planeShape->getPlaneConstant();
 
         //first perform a collision query with the non-perturbated collision objects

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h

@@ -61 +61 @@
                         
                 CreateFunc() 
-                        : m_numPerturbationIterations(3),
-                        m_minimumPointsPerturbationThreshold(3)
+                        : m_numPerturbationIterations(1),
+                        m_minimumPointsPerturbationThreshold(1)
                 {
                 }

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp

@@ -46 +46 @@
         void* mem = btAlignedAlloc(sizeof(btVoronoiSimplexSolver),16);
         m_simplexSolver = new (mem)btVoronoiSimplexSolver();
-        
-#define USE_EPA 1
-#ifdef USE_EPA
-        mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16);
-        m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver;
-#else
-        mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16);
-        m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver;
-#endif//USE_EPA 
-        
-
+
+        if (constructionInfo.m_useEpaPenetrationAlgorithm)
+        {
+                mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16);
+                m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver;
+        }else
+        {
+                mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16);
+                m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver;
+        }
+        
         //default CreationFunctions, filling the m_doubleDispatch table
         mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc),16);
@@ -103 +103 @@
         int sl = sizeof(btConvexSeparatingDistanceUtil);
         sl = sizeof(btGjkPairDetector);
-        int     collisionAlgorithmMaxElementSize = btMax(maxSize,maxSize2);
+        int     collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize);
+        collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2);
         collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3);
 

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h

@@ -28 +28 @@
         int                                     m_defaultMaxPersistentManifoldPoolSize;
         int                                     m_defaultMaxCollisionAlgorithmPoolSize;
+        int                                     m_customCollisionAlgorithmMaxElementSize;
         int                                     m_defaultStackAllocatorSize;
+        int                                     m_useEpaPenetrationAlgorithm;
 
         btDefaultCollisionConstructionInfo()
@@ -36 +38 @@
                 m_defaultMaxPersistentManifoldPoolSize(4096),
                 m_defaultMaxCollisionAlgorithmPoolSize(4096),
-                m_defaultStackAllocatorSize(0)
+                m_customCollisionAlgorithmMaxElementSize(0),
+                m_defaultStackAllocatorSize(0),
+                m_useEpaPenetrationAlgorithm(true)
         {
         }
@@ -109 +113 @@
         }
 
+        virtual btVoronoiSimplexSolver* getSimplexSolver()
+        {
+                return m_simplexSolver;
+        }
+
 
         virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1);

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btGhostObject.h

@@ -161 +161 @@
         }
 
-        virtual void    removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0,btDispatcher* dispatcher)
+        virtual void    removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/)
         {
                 btAssert(0);
@@ -173 +173 @@
 
 #endif
+

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp

@@ -48 +48 @@
                 m_body0(body0),
                 m_body1(body1)
+#ifdef DEBUG_PART_INDEX
+                ,m_partId0(-1),
+        m_partId1(-1),
+        m_index0(-1),
+        m_index1(-1)
+#endif //DEBUG_PART_INDEX
 {
         m_rootTransA = body0->getWorldTransform();
@@ -89 +95 @@
 
    //BP mod, store contact triangles.
-   newPt.m_partId0 = m_partId0;
-   newPt.m_partId1 = m_partId1;
-   newPt.m_index0  = m_index0;
-   newPt.m_index1  = m_index1;
+        if (isSwapped)
+        {
+                newPt.m_partId0 = m_partId1;
+                newPt.m_partId1 = m_partId0;
+                newPt.m_index0  = m_index1;
+                newPt.m_index1  = m_index0;
+        } else
+        {
+                newPt.m_partId0 = m_partId0;
+                newPt.m_partId1 = m_partId1;
+                newPt.m_index0  = m_index0;
+                newPt.m_index1  = m_index1;
+        }
         //printf("depth=%f\n",depth);
         ///@todo, check this for any side effects
@@ -113 +128 @@
                 btCollisionObject* obj0 = isSwapped? m_body1 : m_body0;
                 btCollisionObject* obj1 = isSwapped? m_body0 : m_body1;
-                (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0,m_partId0,m_index0,obj1,m_partId1,m_index1);
+                (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0,newPt.m_partId0,newPt.m_index0,obj1,newPt.m_partId1,newPt.m_index1);
         }
 

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h

@@ -29 +29 @@
 extern ContactAddedCallback             gContactAddedCallback;
 
+//#define DEBUG_PART_INDEX 1
 
 
@@ -34 +35 @@
 class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result
 {
+protected:
+
         btPersistentManifold* m_manifoldPtr;
 
@@ -51 +54 @@
 
         btManifoldResult()
+#ifdef DEBUG_PART_INDEX
+                :
+        m_partId0(-1),
+        m_partId1(-1),
+        m_index0(-1),
+        m_index1(-1)
+#endif //DEBUG_PART_INDEX
         {
         }
@@ -72 +82 @@
         }
 
-        virtual void setShapeIdentifiers(int partId0,int index0,        int partId1,int index1)
+        virtual void setShapeIdentifiersA(int partId0,int index0)
         {
-                        m_partId0=partId0;
-                        m_partId1=partId1;
-                        m_index0=index0;
-                        m_index1=index1;                
+                m_partId0=partId0;
+                m_index0=index0;
+        }
+
+        virtual void setShapeIdentifiersB(      int partId1,int index1)
+        {
+                m_partId1=partId1;
+                m_index1=index1;
         }
 
@@ -100 +114 @@
         }
 
+        const btCollisionObject* getBody0Internal() const
+        {
+                return m_body0;
+        }
 
+        const btCollisionObject* getBody1Internal() const
+        {
+                return m_body1;
+        }
+        
 };
 

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp

@@ +1 @@
+
 /*
 Bullet Continuous Collision Detection and Physics Library
@@ -45 +46 @@
         
         {
+                btOverlappingPairCache* pairCachePtr = colWorld->getPairCache();
+                const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs();
+                btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr();
                 
-                for (int i=0;i<colWorld->getPairCache()->getNumOverlappingPairs();i++)
-                {
-                        btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr();
+                for (int i=0;i<numOverlappingPairs;i++)
+                {
                         const btBroadphasePair& collisionPair = pairPtr[i];
                         btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
@@ -64 +67 @@
 }
 
-
+#ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION
+void   btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
+{
+
+        // put the index into m_controllers into m_tag   
+        int index = 0;
+        {
+
+                int i;
+                for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
+                {
+                        btCollisionObject*   collisionObject= colWorld->getCollisionObjectArray()[i];
+                        //Adding filtering here
+                        if (!collisionObject->isStaticOrKinematicObject())
+                        {
+                                collisionObject->setIslandTag(index++);
+                        }
+                        collisionObject->setCompanionId(-1);
+                        collisionObject->setHitFraction(btScalar(1.));
+                }
+        }
+        // do the union find
+
+        initUnionFind( index );
+
+        findUnions(dispatcher,colWorld);
+}
+
+void   btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
+{
+        // put the islandId ('find' value) into m_tag   
+        {
+                int index = 0;
+                int i;
+                for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
+                {
+                        btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
+                        if (!collisionObject->isStaticOrKinematicObject())
+                        {
+                                collisionObject->setIslandTag( m_unionFind.find(index) );
+                                //Set the correct object offset in Collision Object Array
+                                m_unionFind.getElement(index).m_sz = i;
+                                collisionObject->setCompanionId(-1);
+                                index++;
+                        } else
+                        {
+                                collisionObject->setIslandTag(-1);
+                                collisionObject->setCompanionId(-2);
+                        }
+                }
+        }
+}
+
+
+#else //STATIC_SIMULATION_ISLAND_OPTIMIZATION
 void    btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
 {
-        
+
         initUnionFind( int (colWorld->getCollisionObjectArray().size()));
-        
+
         // put the index into m_controllers into m_tag  
         {
-                
+
                 int index = 0;
                 int i;
@@ -82 +139 @@
                         collisionObject->setHitFraction(btScalar(1.));
                         index++;
-                        
+
                 }
         }
         // do the union find
-        
+
         findUnions(dispatcher,colWorld);
-        
-
-        
-}
-
-
-
+}
 
 void    btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
@@ -100 +151 @@
         // put the islandId ('find' value) into m_tag   
         {
-                
-                
+
+
                 int index = 0;
                 int i;
@@ -120 +171 @@
         }
 }
+
+#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
 
 inline  int     getIslandId(const btPersistentManifold* lhs)

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp

@@ -60 +60 @@
         
         btDiscreteCollisionDetectorInterface::ClosestPointInput input;
-        input.m_maximumDistanceSquared = btScalar(1e30);///@todo: tighter bounds
+        input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds
         input.m_transformA = sphereObj->getWorldTransform();
         input.m_transformB = triObj->getWorldTransform();

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp

@@ -71 +71 @@
         {
                 m_elements[i].m_id = find(i);
+#ifndef STATIC_SIMULATION_ISLAND_OPTIMIZATION
                 m_elements[i].m_sz = i;
+#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
         }
         
@@ -79 +81 @@
 
 }
-

code/branches/kicklib/src/external/bullet/BulletCollision/CollisionDispatch/btUnionFind.h

@@ -19 +19 @@
 #include "LinearMath/btAlignedObjectArray.h"
 
-        #define USE_PATH_COMPRESSION 1
+#define USE_PATH_COMPRESSION 1
+
+///see for discussion of static island optimizations by Vroonsh here: http://code.google.com/p/bullet/issues/detail?id=406
+#define STATIC_SIMULATION_ISLAND_OPTIMIZATION 1
 
 struct  btElement
@@ -107 +110 @@
         
                 #ifdef USE_PATH_COMPRESSION
-                                //
-                                m_elements[x].m_id = m_elements[m_elements[x].m_id].m_id;
-                #endif //
+                                const btElement* elementPtr = &m_elements[m_elements[x].m_id];
+                                m_elements[x].m_id = elementPtr->m_id;
+                                x = elementPtr->m_id;                   
+                #else//
                                 x = m_elements[x].m_id;
+                #endif          
                                 //btAssert(x < m_N);
                                 //btAssert(x >= 0);