bullet: Update to current svn, r2636
[blender.git] / extern / bullet2 / src / BulletCollision / CollisionDispatch / btManifoldResult.cpp
index f8dfa5b101f76c2330e324249a8b5496a7dc9176..4b2986a008737c800958274a4db004aaecd22fe0 100644 (file)
@@ -17,13 +17,30 @@ subject to the following restrictions:
 #include "btManifoldResult.h"
 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
-
+#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
 
 ///This is to allow MaterialCombiner/Custom Friction/Restitution values
 ContactAddedCallback           gContactAddedCallback=0;
 
+
+
 ///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
-inline btScalar        calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1)
+inline btScalar        calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+       btScalar friction = body0->getRollingFriction() * body1->getRollingFriction();
+
+       const btScalar MAX_FRICTION  = btScalar(10.);
+       if (friction < -MAX_FRICTION)
+               friction = -MAX_FRICTION;
+       if (friction > MAX_FRICTION)
+               friction = MAX_FRICTION;
+       return friction;
+
+}
+
+
+///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
+btScalar       btManifoldResult::calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1)
 {
        btScalar friction = body0->getFriction() * body1->getFriction();
 
@@ -36,20 +53,24 @@ inline btScalar     calculateCombinedFriction(const btCollisionObject* body0,const b
 
 }
 
-inline btScalar        calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1)
+btScalar       btManifoldResult::calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1)
 {
        return body0->getRestitution() * body1->getRestitution();
 }
 
 
 
-btManifoldResult::btManifoldResult(btCollisionObject* body0,btCollisionObject* body1)
+btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
                :m_manifoldPtr(0),
-               m_body0(body0),
-               m_body1(body1)
+               m_body0Wrap(body0Wrap),
+               m_body1Wrap(body1Wrap)
+#ifdef DEBUG_PART_INDEX
+               ,m_partId0(-1),
+       m_partId1(-1),
+       m_index0(-1),
+       m_index1(-1)
+#endif //DEBUG_PART_INDEX
 {
-       m_rootTransA = body0->getWorldTransform();
-       m_rootTransB = body1->getWorldTransform();
 }
 
 
@@ -57,11 +78,12 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
 {
        btAssert(m_manifoldPtr);
        //order in manifold needs to match
-       
+
        if (depth > m_manifoldPtr->getContactBreakingThreshold())
+//     if (depth > m_manifoldPtr->getContactProcessingThreshold())
                return;
 
-       bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
+       bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
 
        btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
 
@@ -70,12 +92,12 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
        
        if (isSwapped)
        {
-               localA = m_rootTransB.invXform(pointA );
-               localB = m_rootTransA.invXform(pointInWorld);
+               localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
+               localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
        } else
        {
-               localA = m_rootTransA.invXform(pointA );
-               localB = m_rootTransB.invXform(pointInWorld);
+               localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA );
+               localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
        }
 
        btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
@@ -84,14 +106,27 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
        
        int insertIndex = m_manifoldPtr->getCacheEntry(newPt);
 
-       newPt.m_combinedFriction = calculateCombinedFriction(m_body0,m_body1);
-       newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0,m_body1);
+       newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+       newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+       newPt.m_combinedRollingFriction = calculateCombinedRollingFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+       btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2);
+       
 
+       
    //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
        if (insertIndex >= 0)
@@ -106,13 +141,13 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
        //User can override friction and/or restitution
        if (gContactAddedCallback &&
                //and if either of the two bodies requires custom material
-                ((m_body0->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
-                  (m_body1->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)))
+                ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
+                  (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)))
        {
                //experimental feature info, for per-triangle material etc.
-               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);
+               const btCollisionObjectWrapper* obj0Wrap = isSwapped? m_body1Wrap : m_body0Wrap;
+               const btCollisionObjectWrapper* obj1Wrap = isSwapped? m_body0Wrap : m_body1Wrap;
+               (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0Wrap,newPt.m_partId0,newPt.m_index0,obj1Wrap,newPt.m_partId1,newPt.m_index1);
        }
 
 }