dali-physics/third-party/bullet3/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #include "btManifoldResult.h"
  17 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
  18 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
  19 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
  20
  21 ///This is to allow MaterialCombiner/Custom Friction/Restitution values
  22 ContactAddedCallback gContactAddedCallback = 0;
  23
  24 CalculateCombinedCallback gCalculateCombinedRestitutionCallback = &btManifoldResult::calculateCombinedRestitution;
  25 CalculateCombinedCallback gCalculateCombinedFrictionCallback = &btManifoldResult::calculateCombinedFriction;
  26 CalculateCombinedCallback gCalculateCombinedRollingFrictionCallback = &btManifoldResult::calculateCombinedRollingFriction;
  27 CalculateCombinedCallback gCalculateCombinedSpinningFrictionCallback = &btManifoldResult::calculateCombinedSpinningFriction;
  28 CalculateCombinedCallback gCalculateCombinedContactDampingCallback = &btManifoldResult::calculateCombinedContactDamping;
  29 CalculateCombinedCallback gCalculateCombinedContactStiffnessCallback = &btManifoldResult::calculateCombinedContactStiffness;
  30
  31 btScalar btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0, const btCollisionObject* body1)
  32 {
  33         btScalar friction = body0->getRollingFriction() * body1->getFriction() + body1->getRollingFriction() * body0->getFriction();
  34
  35         const btScalar MAX_FRICTION = btScalar(10.);
  36         if (friction < -MAX_FRICTION)
  37                 friction = -MAX_FRICTION;
  38         if (friction > MAX_FRICTION)
  39                 friction = MAX_FRICTION;
  40         return friction;
  41 }
  42
  43 btScalar btManifoldResult::calculateCombinedSpinningFriction(const btCollisionObject* body0, const btCollisionObject* body1)
  44 {
  45         btScalar friction = body0->getSpinningFriction() * body1->getFriction() + body1->getSpinningFriction() * body0->getFriction();
  46
  47         const btScalar MAX_FRICTION = btScalar(10.);
  48         if (friction < -MAX_FRICTION)
  49                 friction = -MAX_FRICTION;
  50         if (friction > MAX_FRICTION)
  51                 friction = MAX_FRICTION;
  52         return friction;
  53 }
  54
  55 ///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
  56 btScalar btManifoldResult::calculateCombinedFriction(const btCollisionObject* body0, const btCollisionObject* body1)
  57 {
  58         btScalar friction = body0->getFriction() * body1->getFriction();
  59
  60         const btScalar MAX_FRICTION = btScalar(10.);
  61         if (friction < -MAX_FRICTION)
  62                 friction = -MAX_FRICTION;
  63         if (friction > MAX_FRICTION)
  64                 friction = MAX_FRICTION;
  65         return friction;
  66 }
  67
  68 btScalar btManifoldResult::calculateCombinedRestitution(const btCollisionObject* body0, const btCollisionObject* body1)
  69 {
  70         return body0->getRestitution() * body1->getRestitution();
  71 }
  72
  73 btScalar btManifoldResult::calculateCombinedContactDamping(const btCollisionObject* body0, const btCollisionObject* body1)
  74 {
  75         return body0->getContactDamping() + body1->getContactDamping();
  76 }
  77
  78 btScalar btManifoldResult::calculateCombinedContactStiffness(const btCollisionObject* body0, const btCollisionObject* body1)
  79 {
  80         btScalar s0 = body0->getContactStiffness();
  81         btScalar s1 = body1->getContactStiffness();
  82
  83         btScalar tmp0 = btScalar(1) / s0;
  84         btScalar tmp1 = btScalar(1) / s1;
  85         btScalar combinedStiffness = btScalar(1) / (tmp0 + tmp1);
  86         return combinedStiffness;
  87 }
  88
  89 btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
  90         : m_manifoldPtr(0),
  91           m_body0Wrap(body0Wrap),
  92           m_body1Wrap(body1Wrap)
  93           ,
  94           m_partId0(-1),
  95           m_partId1(-1),
  96           m_index0(-1),
  97           m_index1(-1)
  98           ,
  99           m_closestPointDistanceThreshold(0)
 100 {
 101 }
 102
 103 void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth)
 104 {
 105         btAssert(m_manifoldPtr);
 106         //order in manifold needs to match
 107
 108         if (depth > m_manifoldPtr->getContactBreakingThreshold())
 109                 //      if (depth > m_manifoldPtr->getContactProcessingThreshold())
 110                 return;
 111
 112         bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject();
 113         bool isNewCollision = m_manifoldPtr->getNumContacts() == 0;
 114
 115         btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
 116
 117         btVector3 localA;
 118         btVector3 localB;
 119
 120         if (isSwapped)
 121         {
 122                 localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA);
 123                 localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
 124         }
 125         else
 126         {
 127                 localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA);
 128                 localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld);
 129         }
 130
 131         btManifoldPoint newPt(localA, localB, normalOnBInWorld, depth);
 132         newPt.m_positionWorldOnA = pointA;
 133         newPt.m_positionWorldOnB = pointInWorld;
 134
 135         int insertIndex = m_manifoldPtr->getCacheEntry(newPt);
 136
 137         newPt.m_combinedFriction = gCalculateCombinedFrictionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
 138         newPt.m_combinedRestitution = gCalculateCombinedRestitutionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
 139         newPt.m_combinedRollingFriction = gCalculateCombinedRollingFrictionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
 140         newPt.m_combinedSpinningFriction = gCalculateCombinedSpinningFrictionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
 141
 142         if ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING) ||
 143                 (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING))
 144         {
 145                 newPt.m_combinedContactDamping1 = gCalculateCombinedContactDampingCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
 146                 newPt.m_combinedContactStiffness1 = gCalculateCombinedContactStiffnessCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject());
 147                 newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING;
 148         }
 149
 150         if ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_FRICTION_ANCHOR) ||
 151                 (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_FRICTION_ANCHOR))
 152         {
 153                 newPt.m_contactPointFlags |= BT_CONTACT_FLAG_FRICTION_ANCHOR;
 154         }
 155
 156         btPlaneSpace1(newPt.m_normalWorldOnB, newPt.m_lateralFrictionDir1, newPt.m_lateralFrictionDir2);
 157
 158         //BP mod, store contact triangles.
 159         if (isSwapped)
 160         {
 161                 newPt.m_partId0 = m_partId1;
 162                 newPt.m_partId1 = m_partId0;
 163                 newPt.m_index0 = m_index1;
 164                 newPt.m_index1 = m_index0;
 165         }
 166         else
 167         {
 168                 newPt.m_partId0 = m_partId0;
 169                 newPt.m_partId1 = m_partId1;
 170                 newPt.m_index0 = m_index0;
 171                 newPt.m_index1 = m_index1;
 172         }
 173         //printf("depth=%f\n",depth);
 174         ///@todo, check this for any side effects
 175         if (insertIndex >= 0)
 176         {
 177                 //const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex);
 178                 m_manifoldPtr->replaceContactPoint(newPt, insertIndex);
 179         }
 180         else
 181         {
 182                 insertIndex = m_manifoldPtr->addManifoldPoint(newPt);
 183         }
 184
 185         //User can override friction and/or restitution
 186         if (gContactAddedCallback &&
 187                 //and if either of the two bodies requires custom material
 188                 ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
 189                  (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)))
 190         {
 191                 //experimental feature info, for per-triangle material etc.
 192                 const btCollisionObjectWrapper* obj0Wrap = isSwapped ? m_body1Wrap : m_body0Wrap;
 193                 const btCollisionObjectWrapper* obj1Wrap = isSwapped ? m_body0Wrap : m_body1Wrap;
 194                 (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex), obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1);
 195         }
 196
 197         if (gContactStartedCallback && isNewCollision)
 198         {
 199                 gContactStartedCallback(m_manifoldPtr);
 200         }
 201 }