Extras/PhysicsEffects/include/BulletPhysicsEffects/btLowLevelBroadphase.cpp

   1 /*\r
   2 Physics Effects Copyright(C) 2011 Sony Computer Entertainment Inc.\r
   3 All rights reserved.\r
   4 \r
   5 Physics Effects is open software; you can redistribute it and/or\r
   6 modify it under the terms of the BSD License.\r
   7 \r
   8 Physics Effects is distributed in the hope that it will be useful,\r
   9 but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  10 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.\r
  11 See the BSD License for more details.\r
  12 \r
  13 A copy of the BSD License is distributed with\r
  14 Physics Effects under the filename: physics_effects_license.txt\r
  15 */\r
  16 \r
  17 #include "btLowLevelBroadphase.h"\r
  18 \r
  19 \r
  20 #include "BulletMultiThreaded/PlatformDefinitions.h"\r
  21 #include "BulletCollision/NarrowphaseCollision/btPersistentManifold.h"\r
  22 \r
  23 // Include base level headers\r
  24 #include "physics_effects/base_level/pfx_base_level_include.h"\r
  25 \r
  26 // Include low level headers\r
  27 #include "physics_effects/low_level/broadphase/pfx_broadphase.h"\r
  28 \r
  29 #include "physics_effects/low_level/sort/pfx_parallel_sort.h"\r
  30 \r
  31 #include "BulletMultiThreaded/vectormath2bullet.h"\r
  32 \r
  33 #include "physics_effects/base_level/base/pfx_vec_utils.h"\r
  34 #include "physics_effects/base_level/collision/pfx_aabb.h"\r
  35 #include "physics_effects/base_level/rigidbody/pfx_rigid_state.h"\r
  36 #include "btLowLevelData.h"\r
  37 \r
  38 using namespace sce::PhysicsEffects;\r
  39 \r
  40 //E Temporary buffers\r
  41 #define POOL_BYTES (5*1024*1024)\r
  42 unsigned char SCE_PFX_ALIGNED(128) poolBuff[POOL_BYTES];\r
  43 \r
  44 //E Stack allocator for temporary buffers\r
  45 PfxHeapManager pool(poolBuff,POOL_BYTES);\r
  46 \r
  47 \r
  48 ///////////////////////////////////////////////////////////////////////////////\r
  49 // Broadphase\r
  50 \r
  51 \r
  52 \r
  53 \r
  54 \r
  55 //\r
  56 btLowLevelBroadphase::btLowLevelBroadphase(btLowLevelData* lowLevelData, btOverlappingPairCache* paircache, int maxProxies)\r
  57 :m_lowLevelData(lowLevelData)\r
  58 {\r
  59         \r
  60         m_guidGenerator = 1;\r
  61         m_releasepaircache      =       (paircache!=0)?false:true;\r
  62         m_paircache                     =       paircache? paircache    : new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();\r
  63         m_clientData.m_bp = this;\r
  64         m_clientData.m_dispatcher = 0;\r
  65 \r
  66 \r
  67         m_broadphaseAabbMin.setValue(1e30,1e30,1e30);\r
  68         m_broadphaseAabbMax.setValue(-1e30,-1e30,-1e30);\r
  69 \r
  70                 // allocate handles buffer and put all handles on free list\r
  71         m_pHandlesRawPtr = btAlignedAlloc(sizeof(btLowLevelBroadphaseProxy)*maxProxies,16);\r
  72         m_pHandles = new(m_pHandlesRawPtr) btLowLevelBroadphaseProxy[maxProxies];\r
  73         m_maxHandles = maxProxies;\r
  74         m_numHandles = 0;\r
  75         m_firstFreeHandle = 0;\r
  76         m_LastHandleIndex = -1;\r
  77         \r
  78 \r
  79         {\r
  80                 for (int i = m_firstFreeHandle; i < maxProxies; i++)\r
  81                 {\r
  82                         m_pHandles[i].SetNextFree(i + 1);\r
  83                         m_pHandles[i].m_uniqueId = i;//start from zero, so we can re-use the uid for body ID\r
  84                 }\r
  85                 m_pHandles[maxProxies - 1].SetNextFree(0);\r
  86         \r
  87         }\r
  88 \r
  89 }\r
  90 \r
  91 //\r
  92 btLowLevelBroadphase::~btLowLevelBroadphase()\r
  93 {\r
  94         if(m_releasepaircache) \r
  95         {\r
  96                 m_paircache->~btOverlappingPairCache();\r
  97                 btAlignedFree(m_paircache);\r
  98         }\r
  99 }\r
 100 \r
 101 \r
 102 btBroadphaseProxy*      btLowLevelBroadphase::createProxy(  const btVector3& aabbMin,  const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* /*dispatcher*/,void* multiSapProxy)\r
 103 {\r
 104         if (m_numHandles >= m_maxHandles)\r
 105         {\r
 106                 btAssert(0);\r
 107                 return 0; //should never happen, but don't let the game crash ;-)\r
 108         }\r
 109         btAssert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]);\r
 110 \r
 111         int newHandleIndex = allocHandle();\r
 112         btLowLevelBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btLowLevelBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy);\r
 113         m_uid2ptr.insert(proxy->m_uniqueId,proxy);\r
 114         return proxy;\r
 115 }\r
 116 \r
 117 void    btLowLevelBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher* dispatcher)\r
 118 {\r
 119                 m_uid2ptr.remove(proxyOrg->m_uniqueId);\r
 120 \r
 121                 btLowLevelBroadphaseProxy* proxy0 = static_cast<btLowLevelBroadphaseProxy*>(proxyOrg);\r
 122                 freeHandle(proxy0);\r
 123 \r
 124                 m_paircache->removeOverlappingPairsContainingProxy(proxyOrg,dispatcher);\r
 125 \r
 126                 //validate();\r
 127                 \r
 128 }\r
 129 \r
 130 void    btLowLevelBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const\r
 131 {\r
 132         const btLowLevelBroadphaseProxy* sbp = getLowLevelProxyFromProxy(proxy);\r
 133         aabbMin = sbp->m_aabbMin;\r
 134         aabbMax = sbp->m_aabbMax;\r
 135 }\r
 136 \r
 137 void    btLowLevelBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* /*dispatcher*/)\r
 138 {\r
 139         btLowLevelBroadphaseProxy* sbp = getLowLevelProxyFromProxy(proxy);\r
 140         sbp->m_aabbMin = aabbMin;\r
 141         sbp->m_aabbMax = aabbMax;\r
 142 }\r
 143 \r
 144 \r
 145 bool    btLowLevelBroadphase::aabbOverlap(btLowLevelBroadphaseProxy* proxy0,btLowLevelBroadphaseProxy* proxy1)\r
 146 {\r
 147         return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] && \r
 148                    proxy0->m_aabbMin[1] <= proxy1->m_aabbMax[1] && proxy1->m_aabbMin[1] <= proxy0->m_aabbMax[1] &&\r
 149                    proxy0->m_aabbMin[2] <= proxy1->m_aabbMax[2] && proxy1->m_aabbMin[2] <= proxy0->m_aabbMax[2];\r
 150 \r
 151 }\r
 152 \r
 153 \r
 154 PfxBroadphasePair*              btLowLevelBroadphase::getCurrentPairs()\r
 155 {\r
 156         return &m_lowLevelData->m_pairsBuff[m_lowLevelData->m_pairSwap][0];\r
 157 }\r
 158 \r
 159 const PfxBroadphasePair*                btLowLevelBroadphase::getCurrentPairs() const\r
 160 {\r
 161         return &m_lowLevelData->m_pairsBuff[m_lowLevelData->m_pairSwap][0];\r
 162 }\r
 163 \r
 164 int     btLowLevelBroadphase::getNumCurrentPairs() const\r
 165 {\r
 166         return m_lowLevelData->m_numPairs[m_lowLevelData->m_pairSwap];\r
 167 }\r
 168 \r
 169 void btLowLevelBroadphase::broadphase(PfxSortData32* proxies, int numRigidBodies, int axis, btDispatcher* dispatcher)\r
 170 {\r
 171         m_lowLevelData->m_pairSwap = 1-m_lowLevelData->m_pairSwap;\r
 172 \r
 173         unsigned int &numPreviousPairs = m_lowLevelData->m_numPairs[1-m_lowLevelData->m_pairSwap];\r
 174         unsigned int &numCurrentPairs = m_lowLevelData->m_numPairs[m_lowLevelData->m_pairSwap];\r
 175         PfxBroadphasePair *previousPairs = &m_lowLevelData->m_pairsBuff[1-m_lowLevelData->m_pairSwap][0];\r
 176         PfxBroadphasePair *currentPairs = &m_lowLevelData->m_pairsBuff[m_lowLevelData->m_pairSwap][0];\r
 177 \r
 178 \r
 179         //E Create broadpahse proxies\r
 180         {\r
 181 //              for(int i=0;i<numRigidBodies;i++) {\r
 182 //                      pfxUpdateBroadphaseProxy(proxies[i],states[i],collidables[i],worldCenter,worldExtent,axis);\r
 183 //              }\r
 184 \r
 185                 int workBytes = sizeof(PfxBroadphaseProxy) * numRigidBodies;\r
 186                 void *workBuff = pool.allocate(workBytes);\r
 187                                 \r
 188                 pfxParallelSort(proxies,numRigidBodies,workBuff,workBytes);\r
 189 \r
 190                 pool.deallocate(workBuff);\r
 191         }\r
 192 \r
 193         //E Find overlapped pairs\r
 194         {\r
 195                 PfxFindPairsParam findPairsParam;\r
 196                 findPairsParam.pairBytes = pfxGetPairBytesOfFindPairs(m_lowLevelData->m_maxPairs);\r
 197                 findPairsParam.pairBuff = pool.allocate(findPairsParam.pairBytes);\r
 198                 findPairsParam.workBytes = pfxGetWorkBytesOfFindPairs(m_lowLevelData->m_maxPairs);\r
 199                 findPairsParam.workBuff = pool.allocate(findPairsParam.workBytes);\r
 200                 findPairsParam.proxies = proxies;\r
 201                 findPairsParam.numProxies = numRigidBodies;\r
 202                 findPairsParam.maxPairs = m_lowLevelData->m_maxPairs;\r
 203                 findPairsParam.axis = axis;\r
 204 \r
 205                 PfxFindPairsResult findPairsResult;\r
 206 \r
 207 \r
 208                 int ret = pfxFindPairs(findPairsParam,findPairsResult);\r
 209                 if(ret != SCE_PFX_OK) \r
 210                         SCE_PFX_PRINTF("pfxFindPairs failed %d\n",ret);\r
 211                 \r
 212                 pool.deallocate(findPairsParam.workBuff);\r
 213 \r
 214                 //E Decompose overlapped pairs into 3 arrays\r
 215                 PfxDecomposePairsParam decomposePairsParam;\r
 216                 decomposePairsParam.pairBytes = pfxGetPairBytesOfDecomposePairs(numPreviousPairs,findPairsResult.numPairs);\r
 217                 decomposePairsParam.pairBuff = pool.allocate(decomposePairsParam.pairBytes);\r
 218                 decomposePairsParam.workBytes = pfxGetWorkBytesOfDecomposePairs(numPreviousPairs,findPairsResult.numPairs);\r
 219                 decomposePairsParam.workBuff = pool.allocate(decomposePairsParam.workBytes);\r
 220                 decomposePairsParam.previousPairs = previousPairs;\r
 221                 decomposePairsParam.numPreviousPairs = numPreviousPairs;\r
 222                 decomposePairsParam.currentPairs = findPairsResult.pairs; // Set pairs from pfxFindPairs()\r
 223                 decomposePairsParam.numCurrentPairs = findPairsResult.numPairs; // Set the number of pairs from pfxFindPairs()\r
 224 \r
 225                 PfxDecomposePairsResult decomposePairsResult;\r
 226 \r
 227                 ret = pfxDecomposePairs(decomposePairsParam,decomposePairsResult);\r
 228                 if(ret != SCE_PFX_OK) \r
 229                         SCE_PFX_PRINTF("pfxDecomposePairs failed %d\n",ret);\r
 230 \r
 231                 pool.deallocate(decomposePairsParam.workBuff);\r
 232 \r
 233                 PfxBroadphasePair *outNewPairs = decomposePairsResult.outNewPairs;\r
 234                 PfxBroadphasePair *outKeepPairs = decomposePairsResult.outKeepPairs;\r
 235                 PfxBroadphasePair *outRemovePairs = decomposePairsResult.outRemovePairs;\r
 236                 PfxUInt32 numOutNewPairs = decomposePairsResult.numOutNewPairs;\r
 237                 PfxUInt32 numOutKeepPairs = decomposePairsResult.numOutKeepPairs;\r
 238                 PfxUInt32 numOutRemovePairs = decomposePairsResult.numOutRemovePairs;\r
 239 \r
 240                 for (int i=0;i<numOutRemovePairs;i++)\r
 241                 {\r
 242                         int idA = pfxGetObjectIdA(outRemovePairs[i]);\r
 243                         int idB = pfxGetObjectIdB(outRemovePairs[i]);\r
 244                         //use m_uid2ptr to get pointer\r
 245                         \r
 246                         btBroadphaseProxy* proxyA = (btBroadphaseProxy*)*m_uid2ptr[idA];\r
 247                         btBroadphaseProxy* proxyB = (btBroadphaseProxy*)*m_uid2ptr[idB];\r
 248                         m_paircache->removeOverlappingPair(proxyA,proxyB,dispatcher);\r
 249 \r
 250                         //free low level contacts\r
 251                         m_lowLevelData->m_contactIdPool[m_lowLevelData->m_numContactIdPool++] = pfxGetContactId(outRemovePairs[i]);\r
 252 \r
 253                 }\r
 254 \r
 255                 for (int i=0;i<numOutNewPairs;i++)\r
 256                 {\r
 257                         int idA = pfxGetObjectIdA(outNewPairs[i]);\r
 258                         int idB = pfxGetObjectIdB(outNewPairs[i]);\r
 259                         //use m_uid2ptr to get pointer\r
 260                         \r
 261                         btBroadphaseProxy* proxyA = (btBroadphaseProxy*)*m_uid2ptr[idA];\r
 262                         btBroadphaseProxy* proxyB = (btBroadphaseProxy*)*m_uid2ptr[idB];\r
 263                         btBroadphasePair* btpair = m_paircache->addOverlappingPair(proxyA,proxyB);\r
 264 \r
 265                         //initialize low level contacts\r
 266                         int cId = 0;\r
 267                         if(m_lowLevelData->m_numContactIdPool > 0) {\r
 268                                 cId = m_lowLevelData->m_contactIdPool[--m_lowLevelData->m_numContactIdPool];\r
 269                         }\r
 270                         else {\r
 271                                 cId = m_lowLevelData->m_numContacts++;\r
 272                         }\r
 273                         if(cId >= m_lowLevelData->m_maxContacts) {\r
 274                                 cId = 0;\r
 275                         }\r
 276                         SCE_PFX_ASSERT(cId < m_lowLevelData->m_maxContacts);\r
 277                         pfxSetContactId(outNewPairs[i],cId);\r
 278                         PfxContactManifold &contact = m_lowLevelData->m_contacts[cId];\r
 279                         int sz = sizeof(PfxContactManifold);\r
 280                         int sz2 = sizeof(btPersistentManifold);\r
 281                         int sz3 = 4*3*sizeof(btConstraintRow);\r
 282                         contact.reset(pfxGetObjectIdA(outNewPairs[i]),pfxGetObjectIdB(outNewPairs[i]));\r
 283                         contact.setCompositeFriction(0.1f);\r
 284                         btpair->m_internalTmpValue = cId;\r
 285                 }\r
 286                 \r
 287 \r
 288                 //E Merge 'new' and 'keep' pairs\r
 289                 numCurrentPairs = 0;\r
 290                 for(PfxUInt32 i=0;i<numOutKeepPairs;i++) {\r
 291                         currentPairs[numCurrentPairs++] = outKeepPairs[i];\r
 292                 }\r
 293                 for(PfxUInt32 i=0;i<numOutNewPairs;i++) {\r
 294                         currentPairs[numCurrentPairs++] = outNewPairs[i];\r
 295                 }\r
 296                 \r
 297                 pool.deallocate(decomposePairsParam.pairBuff);\r
 298                 pool.deallocate(findPairsParam.pairBuff);\r
 299         }\r
 300         \r
 301         {\r
 302                 int workBytes = sizeof(PfxBroadphasePair) * numCurrentPairs;\r
 303                 void *workBuff = pool.allocate(workBytes);\r
 304                 \r
 305                 pfxParallelSort(currentPairs,numCurrentPairs,workBuff,workBytes);\r
 306                 \r
 307                 pool.deallocate(workBuff);\r
 308         }\r
 309 }\r
 310 \r
 311 \r
 312 PfxInt32 MyUpdateBroadphaseProxy(PfxBroadphaseProxy& proxy,int rigidbodyId,const btBroadphaseProxy* bulletProxy, const PfxVector3& worldCenter,const PfxVector3& worldExtent,PfxUInt32 axis)\r
 313 {\r
 314         SCE_PFX_ALWAYS_ASSERT(axis<3);\r
 315         \r
 316         PfxInt32 ret = SCE_PFX_OK;\r
 317         \r
 318         PfxVector3 minRig = getVmVector3(bulletProxy->m_aabbMin);\r
 319         PfxVector3 maxRig = getVmVector3(bulletProxy->m_aabbMax);\r
 320         \r
 321         PfxVecInt3 aabbMin,aabbMax;\r
 322         pfxConvertCoordWorldToLocal(worldCenter,worldExtent,minRig,maxRig,aabbMin,aabbMax);\r
 323         \r
 324         pfxSetXMin(proxy,aabbMin.getX());\r
 325         pfxSetXMax(proxy,aabbMax.getX());\r
 326         pfxSetYMin(proxy,aabbMin.getY());\r
 327         pfxSetYMax(proxy,aabbMax.getY());\r
 328         pfxSetZMin(proxy,aabbMin.getZ());\r
 329         pfxSetZMax(proxy,aabbMax.getZ());\r
 330         pfxSetKey(proxy,aabbMin.get(axis));\r
 331         pfxSetObjectId(proxy,rigidbodyId);\r
 332         pfxSetMotionMask(proxy, kPfxMotionTypeActive);\r
 333         pfxSetSelf(proxy,bulletProxy->m_collisionFilterGroup);\r
 334         pfxSetTarget(proxy,bulletProxy->m_collisionFilterMask);\r
 335         \r
 336         return ret;\r
 337 }\r
 338 \r
 339 void    btLowLevelBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)\r
 340 {\r
 341 \r
 342         //set the broadphase proxies\r
 343 \r
 344         btAlignedObjectArray<PfxBroadphaseProxy> proxies;\r
 345         proxies.reserve(m_LastHandleIndex);\r
 346 \r
 347         //E Find the axis along which all rigid bodies are most widely positioned\r
 348         int axis = 0;\r
 349         int i;\r
 350         PfxVector3 s(0.0f),s2(0.0f);\r
 351 \r
 352         PfxVector3 worldMin(-1000);//PFX_FLT_MAX);\r
 353         PfxVector3 worldMax(1000);//-PFX_FLT_MAX);\r
 354         \r
 355         int numRigidBodies = 0;\r
 356         for (i=0; i <= m_LastHandleIndex; i++)\r
 357         {\r
 358                 btLowLevelBroadphaseProxy* proxy0 = &m_pHandles[i];\r
 359                 if(!proxy0->m_clientObject)\r
 360                 {\r
 361                         continue;\r
 362                 }\r
 363                 \r
 364                 PfxVector3 pe_pos = getVmVector3(0.5f*(proxy0->m_aabbMax+proxy0->m_aabbMin));\r
 365                 PfxVector3 pe_min = getVmVector3(proxy0->m_aabbMin);\r
 366                 PfxVector3 pe_max = getVmVector3(proxy0->m_aabbMax);\r
 367                 numRigidBodies++;\r
 368                 //worldMin = minPerElem(worldMin,pe_min);\r
 369                 //worldMax = maxPerElem(worldMax,pe_max);\r
 370 \r
 371                 s += pe_pos;\r
 372                 s2 += mulPerElem(pe_pos,pe_pos);\r
 373         }\r
 374 \r
 375         \r
 376         if (numRigidBodies)\r
 377         {\r
 378                 PfxVector3 v = s2 - mulPerElem(s,s) / (float)numRigidBodies;\r
 379                 if(v[1] > v[0]) \r
 380                         axis = 1;\r
 381                 if(v[2] > v[axis]) \r
 382                         axis = 2;\r
 383         }\r
 384 \r
 385         PfxVector3 worldCenter = 0.5f*(worldMax+worldMin);\r
 386         PfxVector3 worldExtent = 0.5f*(worldMax-worldMin);\r
 387 \r
 388         for (i=0; i <= m_LastHandleIndex; i++)\r
 389         {\r
 390                 btLowLevelBroadphaseProxy* proxy0 = &m_pHandles[i];\r
 391                 if(!proxy0->m_clientObject)\r
 392                 {\r
 393                         continue;\r
 394                 }\r
 395 \r
 396                 PfxBroadphaseProxy& proxy = proxies.expandNonInitializing();\r
 397                 MyUpdateBroadphaseProxy(proxy,proxy0->m_uniqueId,proxy0,worldCenter,worldExtent,axis);\r
 398 \r
 399         }\r
 400 \r
 401         //find pairs, and call 'addOverlappingPair' for new pairs and 'removeOverlappingPair' for removed pairs\r
 402         broadphase(&proxies[0],proxies.size(),axis, dispatcher);\r
 403 }\r
 404 \r
 405 \r
 406 //\r
 407 btOverlappingPairCache*                 btLowLevelBroadphase::getOverlappingPairCache()\r
 408 {\r
 409         return(m_paircache);\r
 410 }\r
 411 \r
 412 //\r
 413 const btOverlappingPairCache*   btLowLevelBroadphase::getOverlappingPairCache() const\r
 414 {\r
 415         return(m_paircache);\r
 416 }\r
 417 \r
 418 \r
 419 void                                                    btLowLevelBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const\r
 420 {\r
 421         aabbMin = m_broadphaseAabbMin;\r
 422         aabbMax = m_broadphaseAabbMax;\r
 423 }\r
 424 \r
 425 void    btLowLevelBroadphase::printStats()\r
 426 {\r
 427 }\r
 428 \r
 429 void    btLowLevelBroadphase::setNumTasks(int numTasks)\r
 430 {\r
 431 }\r