dali-physics/third-party/bullet3/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.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
  17 #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h"
  18 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
  19 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
  20 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
  21 #include "LinearMath/btIDebugDraw.h"
  22 #include "LinearMath/btAabbUtil2.h"
  23 #include "btManifoldResult.h"
  24 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
  25
  26 btShapePairCallback gCompoundChildShapePairCallback = 0;
  27
  28 btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped)
  29         : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
  30           m_isSwapped(isSwapped),
  31           m_sharedManifold(ci.m_manifold)
  32 {
  33         m_ownsManifold = false;
  34
  35         const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
  36         btAssert(colObjWrap->getCollisionShape()->isCompound());
  37
  38         const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
  39         m_compoundShapeRevision = compoundShape->getUpdateRevision();
  40
  41         preallocateChildAlgorithms(body0Wrap, body1Wrap);
  42 }
  43
  44 void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
  45 {
  46         const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
  47         const btCollisionObjectWrapper* otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
  48         btAssert(colObjWrap->getCollisionShape()->isCompound());
  49
  50         const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
  51
  52         int numChildren = compoundShape->getNumChildShapes();
  53         int i;
  54
  55         m_childCollisionAlgorithms.resize(numChildren);
  56         for (i = 0; i < numChildren; i++)
  57         {
  58                 if (compoundShape->getDynamicAabbTree())
  59                 {
  60                         m_childCollisionAlgorithms[i] = 0;
  61                 }
  62                 else
  63                 {
  64                         const btCollisionShape* childShape = compoundShape->getChildShape(i);
  65
  66                         btCollisionObjectWrapper childWrap(colObjWrap, childShape, colObjWrap->getCollisionObject(), colObjWrap->getWorldTransform(), -1, i);  //wrong child trans, but unused (hopefully)
  67                         m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap, otherObjWrap, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
  68
  69                         btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsContact;
  70                         btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsClosestPoints;
  71                 }
  72         }
  73 }
  74
  75 void btCompoundCollisionAlgorithm::removeChildAlgorithms()
  76 {
  77         int numChildren = m_childCollisionAlgorithms.size();
  78         int i;
  79         for (i = 0; i < numChildren; i++)
  80         {
  81                 if (m_childCollisionAlgorithms[i])
  82                 {
  83                         m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
  84                         m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
  85                 }
  86         }
  87 }
  88
  89 btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
  90 {
  91         removeChildAlgorithms();
  92 }
  93
  94 struct btCompoundLeafCallback : btDbvt::ICollide
  95 {
  96 public:
  97         const btCollisionObjectWrapper* m_compoundColObjWrap;
  98         const btCollisionObjectWrapper* m_otherObjWrap;
  99         btDispatcher* m_dispatcher;
 100         const btDispatcherInfo& m_dispatchInfo;
 101         btManifoldResult* m_resultOut;
 102         btCollisionAlgorithm** m_childCollisionAlgorithms;
 103         btPersistentManifold* m_sharedManifold;
 104
 105         btCompoundLeafCallback(const btCollisionObjectWrapper* compoundObjWrap, const btCollisionObjectWrapper* otherObjWrap, btDispatcher* dispatcher, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut, btCollisionAlgorithm** childCollisionAlgorithms, btPersistentManifold* sharedManifold)
 106                 : m_compoundColObjWrap(compoundObjWrap), m_otherObjWrap(otherObjWrap), m_dispatcher(dispatcher), m_dispatchInfo(dispatchInfo), m_resultOut(resultOut), m_childCollisionAlgorithms(childCollisionAlgorithms), m_sharedManifold(sharedManifold)
 107         {
 108         }
 109
 110         void ProcessChildShape(const btCollisionShape* childShape, int index)
 111         {
 112                 btAssert(index >= 0);
 113                 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(m_compoundColObjWrap->getCollisionShape());
 114                 btAssert(index < compoundShape->getNumChildShapes());
 115
 116                 if (gCompoundChildShapePairCallback)
 117                 {
 118                         if (!gCompoundChildShapePairCallback(m_otherObjWrap->getCollisionShape(), childShape))
 119                                 return;
 120                 }
 121
 122                 //backup
 123                 btTransform orgTrans = m_compoundColObjWrap->getWorldTransform();
 124
 125                 const btTransform& childTrans = compoundShape->getChildTransform(index);
 126                 btTransform newChildWorldTrans = orgTrans * childTrans;
 127
 128                 //perform an AABB check first
 129                 btVector3 aabbMin0, aabbMax0;
 130                 childShape->getAabb(newChildWorldTrans, aabbMin0, aabbMax0);
 131
 132                 btVector3 extendAabb(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold);
 133                 aabbMin0 -= extendAabb;
 134                 aabbMax0 += extendAabb;
 135
 136                 btVector3 aabbMin1, aabbMax1;
 137                 m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(), aabbMin1, aabbMax1);
 138
 139
 140                 if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
 141                 {
 142                         btTransform preTransform = childTrans;
 143                         if (this->m_compoundColObjWrap->m_preTransform)
 144                         {
 145                                 preTransform = preTransform *(*(this->m_compoundColObjWrap->m_preTransform));
 146                         }
 147                         btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, preTransform, -1, index);
 148
 149                         btCollisionAlgorithm* algo = 0;
 150                         bool allocatedAlgorithm = false;
 151
 152                         if (m_resultOut->m_closestPointDistanceThreshold > 0)
 153                         {
 154                                 algo = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, 0, BT_CLOSEST_POINT_ALGORITHMS);
 155                                 allocatedAlgorithm = true;
 156                         }
 157                         else
 158                         {
 159                                 //the contactpoint is still projected back using the original inverted worldtrans
 160                                 if (!m_childCollisionAlgorithms[index])
 161                                 {
 162                                         m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
 163                                 }
 164                                 algo = m_childCollisionAlgorithms[index];
 165                         }
 166
 167                         const btCollisionObjectWrapper* tmpWrap = 0;
 168
 169                         ///detect swapping case
 170                         if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject())
 171                         {
 172                                 tmpWrap = m_resultOut->getBody0Wrap();
 173                                 m_resultOut->setBody0Wrap(&compoundWrap);
 174                                 m_resultOut->setShapeIdentifiersA(-1, index);
 175                         }
 176                         else
 177                         {
 178                                 tmpWrap = m_resultOut->getBody1Wrap();
 179                                 m_resultOut->setBody1Wrap(&compoundWrap);
 180                                 m_resultOut->setShapeIdentifiersB(-1, index);
 181                         }
 182
 183                         algo->processCollision(&compoundWrap, m_otherObjWrap, m_dispatchInfo, m_resultOut);
 184
 185 #if 0
 186                         if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
 187                         {
 188                                 btVector3 worldAabbMin,worldAabbMax;
 189                                 m_dispatchInfo.m_debugDraw->drawAabb(aabbMin0,aabbMax0,btVector3(1,1,1));
 190                                 m_dispatchInfo.m_debugDraw->drawAabb(aabbMin1,aabbMax1,btVector3(1,1,1));
 191                         }
 192 #endif
 193
 194                         if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject())
 195                         {
 196                                 m_resultOut->setBody0Wrap(tmpWrap);
 197                         }
 198                         else
 199                         {
 200                                 m_resultOut->setBody1Wrap(tmpWrap);
 201                         }
 202                         if (allocatedAlgorithm)
 203                         {
 204                                 algo->~btCollisionAlgorithm();
 205                                 m_dispatcher->freeCollisionAlgorithm(algo);
 206                         }
 207                 }
 208         }
 209         void Process(const btDbvtNode* leaf)
 210         {
 211                 int index = leaf->dataAsInt;
 212
 213                 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(m_compoundColObjWrap->getCollisionShape());
 214                 const btCollisionShape* childShape = compoundShape->getChildShape(index);
 215
 216 #if 0
 217                 if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
 218                 {
 219                         btVector3 worldAabbMin,worldAabbMax;
 220                         btTransform     orgTrans = m_compoundColObjWrap->getWorldTransform();
 221                         btTransformAabb(leaf->volume.Mins(),leaf->volume.Maxs(),0.,orgTrans,worldAabbMin,worldAabbMax);
 222                         m_dispatchInfo.m_debugDraw->drawAabb(worldAabbMin,worldAabbMax,btVector3(1,0,0));
 223                 }
 224 #endif
 225
 226                 ProcessChildShape(childShape, index);
 227         }
 228 };
 229
 230 void btCompoundCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 231 {
 232         const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
 233         const btCollisionObjectWrapper* otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
 234
 235         btAssert(colObjWrap->getCollisionShape()->isCompound());
 236         const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
 237
 238         ///btCompoundShape might have changed:
 239         ////make sure the internal child collision algorithm caches are still valid
 240         if (compoundShape->getUpdateRevision() != m_compoundShapeRevision)
 241         {
 242                 ///clear and update all
 243                 removeChildAlgorithms();
 244
 245                 preallocateChildAlgorithms(body0Wrap, body1Wrap);
 246                 m_compoundShapeRevision = compoundShape->getUpdateRevision();
 247         }
 248
 249         if (m_childCollisionAlgorithms.size() == 0)
 250                 return;
 251
 252         const btDbvt* tree = compoundShape->getDynamicAabbTree();
 253         //use a dynamic aabb tree to cull potential child-overlaps
 254         btCompoundLeafCallback callback(colObjWrap, otherObjWrap, m_dispatcher, dispatchInfo, resultOut, &m_childCollisionAlgorithms[0], m_sharedManifold);
 255
 256         ///we need to refresh all contact manifolds
 257         ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
 258         ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
 259         {
 260                 int i;
 261                 manifoldArray.resize(0);
 262                 for (i = 0; i < m_childCollisionAlgorithms.size(); i++)
 263                 {
 264                         if (m_childCollisionAlgorithms[i])
 265                         {
 266                                 m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
 267                                 for (int m = 0; m < manifoldArray.size(); m++)
 268                                 {
 269                                         if (manifoldArray[m]->getNumContacts())
 270                                         {
 271                                                 resultOut->setPersistentManifold(manifoldArray[m]);
 272                                                 resultOut->refreshContactPoints();
 273                                                 resultOut->setPersistentManifold(0);  //??necessary?
 274                                         }
 275                                 }
 276                                 manifoldArray.resize(0);
 277                         }
 278                 }
 279         }
 280
 281         if (tree)
 282         {
 283                 btVector3 localAabbMin, localAabbMax;
 284                 btTransform otherInCompoundSpace;
 285                 otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform();
 286                 otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace, localAabbMin, localAabbMax);
 287                 btVector3 extraExtends(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold);
 288                 localAabbMin -= extraExtends;
 289                 localAabbMax += extraExtends;
 290
 291                 const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax);
 292                 //process all children, that overlap with  the given AABB bounds
 293                 tree->collideTVNoStackAlloc(tree->m_root, bounds, stack2, callback);
 294         }
 295         else
 296         {
 297                 //iterate over all children, perform an AABB check inside ProcessChildShape
 298                 int numChildren = m_childCollisionAlgorithms.size();
 299                 int i;
 300                 for (i = 0; i < numChildren; i++)
 301                 {
 302                         callback.ProcessChildShape(compoundShape->getChildShape(i), i);
 303                 }
 304         }
 305
 306         {
 307                 //iterate over all children, perform an AABB check inside ProcessChildShape
 308                 int numChildren = m_childCollisionAlgorithms.size();
 309                 int i;
 310                 manifoldArray.resize(0);
 311                 const btCollisionShape* childShape = 0;
 312                 btTransform orgTrans;
 313
 314                 btTransform newChildWorldTrans;
 315                 btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1;
 316
 317                 for (i = 0; i < numChildren; i++)
 318                 {
 319                         if (m_childCollisionAlgorithms[i])
 320                         {
 321                                 childShape = compoundShape->getChildShape(i);
 322                                 //if not longer overlapping, remove the algorithm
 323                                 orgTrans = colObjWrap->getWorldTransform();
 324
 325                                 const btTransform& childTrans = compoundShape->getChildTransform(i);
 326                                 newChildWorldTrans = orgTrans * childTrans;
 327
 328                                 //perform an AABB check first
 329                                 childShape->getAabb(newChildWorldTrans, aabbMin0, aabbMax0);
 330                                 otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->getWorldTransform(), aabbMin1, aabbMax1);
 331
 332                                 if (!TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
 333                                 {
 334                                         m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
 335                                         m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
 336                                         m_childCollisionAlgorithms[i] = 0;
 337                                 }
 338                         }
 339                 }
 340         }
 341 }
 342
 343 btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 344 {
 345         btAssert(0);
 346         //needs to be fixed, using btCollisionObjectWrapper and NOT modifying internal data structures
 347         btCollisionObject* colObj = m_isSwapped ? body1 : body0;
 348         btCollisionObject* otherObj = m_isSwapped ? body0 : body1;
 349
 350         btAssert(colObj->getCollisionShape()->isCompound());
 351
 352         btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
 353
 354         //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
 355         //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
 356         //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
 357         //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
 358         //then use each overlapping node AABB against Tree0
 359         //and vise versa.
 360
 361         btScalar hitFraction = btScalar(1.);
 362
 363         int numChildren = m_childCollisionAlgorithms.size();
 364         int i;
 365         btTransform orgTrans;
 366         btScalar frac;
 367         for (i = 0; i < numChildren; i++)
 368         {
 369                 //btCollisionShape* childShape = compoundShape->getChildShape(i);
 370
 371                 //backup
 372                 orgTrans = colObj->getWorldTransform();
 373
 374                 const btTransform& childTrans = compoundShape->getChildTransform(i);
 375                 //btTransform   newChildWorldTrans = orgTrans*childTrans ;
 376                 colObj->setWorldTransform(orgTrans * childTrans);
 377
 378                 //btCollisionShape* tmpShape = colObj->getCollisionShape();
 379                 //colObj->internalSetTemporaryCollisionShape( childShape );
 380                 frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj, otherObj, dispatchInfo, resultOut);
 381                 if (frac < hitFraction)
 382                 {
 383                         hitFraction = frac;
 384                 }
 385                 //revert back
 386                 //colObj->internalSetTemporaryCollisionShape( tmpShape);
 387                 colObj->setWorldTransform(orgTrans);
 388         }
 389         return hitFraction;
 390 }