Extras/PhysicsEffects/src/low_level/collision/pfx_collision_detection_parallel.cpp

   1 /*\r
   2 Applied Research Associates Inc. (c)2011\r
   3 \r
   4 Physics Effects Copyright(C) 2010 Sony Computer Entertainment Inc.\r
   5 All rights reserved.\r
   6 \r
   7 Physics Effects is open software; you can redistribute it and/or\r
   8 modify it under the terms of the BSD License.\r
   9 \r
  10 Physics Effects is distributed in the hope that it will be useful,\r
  11 but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.\r
  13 See the BSD License for more details.\r
  14 \r
  15 A copy of the BSD License is distributed with\r
  16 Physics Effects under the filename: physics_effects_license.txt\r
  17 */\r
  18 \r
  19 #include "../../../include/physics_effects/base_level/base/pfx_perf_counter.h"\r
  20 #include "../../../include/physics_effects/base_level/collision/pfx_shape_iterator.h"\r
  21 #include "../../../include/physics_effects/low_level/collision/pfx_collision_detection.h"\r
  22 #include "../../base_level/broadphase/pfx_check_collidable.h"\r
  23 #include "../../base_level/collision/pfx_contact_cache.h"\r
  24 #include "pfx_detect_collision_func.h"\r
  25 \r
  26 namespace sce {\r
  27 namespace PhysicsEffects {\r
  28 \r
  29 ///////////////////////////////////////////////////////////////////////////////\r
  30 // This function is implemented in pfx_collision_detection_single.cpp\r
  31 extern int pfxCheckParamOfDetectCollision(PfxDetectCollisionParam &param);\r
  32 \r
  33 ///////////////////////////////////////////////////////////////////////////////\r
  34 // MULTIPLE THREADS\r
  35 \r
  36 #define SCE_PFX_CONTACT_THRESHOLD 0.0f\r
  37 \r
  38 //----------------------------------------------------------------------------\r
  39 //  pfxDetectCollisionTaskEntry\r
  40 //\r
  41 /// The thread PfxTaskEntry function used to perform narrow phase collision\r
  42 /// detection in parallel.\r
  43 //----------------------------------------------------------------------------\r
  44 void pfxDetectCollisionTaskEntry(PfxTaskArg *arg)\r
  45 {\r
  46         PfxDetectCollisionParam &param = *((PfxDetectCollisionParam*)arg->io);\r
  47 \r
  48         PfxUInt32 iFirstContactPair = arg->data[0];\r
  49         PfxUInt32 iEndContactPair = arg->data[1];\r
  50 \r
  51         PfxConstraintPair *contactPairs = param.contactPairs;\r
  52         PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;\r
  53         PfxRigidState *offsetRigidStates = param.offsetRigidStates;\r
  54         PfxCollidable *offsetCollidables = param.offsetCollidables;\r
  55         PfxUInt32 numRigidBodies = param.numRigidBodies;\r
  56 \r
  57         for(PfxUInt32 i = iFirstContactPair; i < iEndContactPair; i++)\r
  58         {\r
  59                 const PfxBroadphasePair &pair = contactPairs[i];\r
  60                 if(!pfxCheckCollidableInCollision(pair))\r
  61                         continue;\r
  62 \r
  63                 PfxUInt32 iContact = pfxGetContactId(pair);\r
  64                 PfxUInt32 iA = pfxGetObjectIdA(pair);\r
  65                 PfxUInt32 iB = pfxGetObjectIdB(pair);\r
  66 \r
  67                 PfxContactManifold &contact = offsetContactManifolds[iContact];\r
  68 \r
  69                 SCE_PFX_ALWAYS_ASSERT(iA==contact.getRigidBodyIdA());\r
  70                 SCE_PFX_ALWAYS_ASSERT(iB==contact.getRigidBodyIdB());\r
  71 \r
  72                 PfxRigidState &stateA = offsetRigidStates[iA];\r
  73                 PfxRigidState &stateB = offsetRigidStates[iB];\r
  74                 PfxCollidable &collA = offsetCollidables[iA];\r
  75                 PfxCollidable &collB = offsetCollidables[iB];\r
  76                 PfxTransform3 tA0(stateA.getOrientation(), stateA.getPosition());\r
  77                 PfxTransform3 tB0(stateB.getOrientation(), stateB.getPosition());\r
  78                 \r
  79                 PfxContactCache contactCache;\r
  80                 \r
  81                 PfxShapeIterator itrShapeA(collA);\r
  82                 for(PfxUInt32 j=0;j<collA.getNumShapes();j++,++itrShapeA)\r
  83                 {\r
  84                         const PfxShape &shapeA = *itrShapeA;\r
  85                         PfxTransform3 offsetTrA = shapeA.getOffsetTransform();\r
  86                         PfxTransform3 worldTrA = tA0 * offsetTrA;\r
  87 \r
  88                         PfxShapeIterator itrShapeB(collB);\r
  89                         for(PfxUInt32 k=0;k<collB.getNumShapes();k++,++itrShapeB)\r
  90                         {\r
  91                                 const PfxShape &shapeB = *itrShapeB;\r
  92                                 PfxTransform3 offsetTrB = shapeB.getOffsetTransform();\r
  93                                 PfxTransform3 worldTrB = tB0 * offsetTrB;\r
  94 \r
  95                                 if( (shapeA.getContactFilterSelf()&shapeB.getContactFilterTarget()) && \r
  96                                     (shapeA.getContactFilterTarget()&shapeB.getContactFilterSelf()) )\r
  97                                 {\r
  98                                         pfxGetDetectCollisionFunc(shapeA.getType(),shapeB.getType())(\r
  99                                                 contactCache,\r
 100                                                 shapeA,offsetTrA,worldTrA,j,\r
 101                                                 shapeB,offsetTrB,worldTrB,k,\r
 102                                                 SCE_PFX_CONTACT_THRESHOLD);\r
 103                                 }\r
 104                         }\r
 105                 }\r
 106                 \r
 107                 for(int j=0;j<contactCache.getNumContacts();j++)\r
 108                 {\r
 109                         const PfxCachedContactPoint &cp = contactCache.getContactPoint(j);\r
 110 \r
 111                         contact.addContactPoint(\r
 112                                 cp.m_distance,\r
 113                                 cp.m_normal,\r
 114                                 cp.m_localPointA,\r
 115                                 cp.m_localPointB,\r
 116                                 cp.m_subData\r
 117                                 );\r
 118                 }\r
 119         }\r
 120 }\r
 121 \r
 122 //----------------------------------------------------------------------------\r
 123 //  pfxDetectCollision\r
 124 //\r
 125 /// Perform narrow phase collision detection in parallel using a task\r
 126 /// manager.\r
 127 ///\r
 128 /// @param param        Information about pairs that may be colliding\r
 129 /// @param taskManager  Pointer to the thread task manager to use\r
 130 ///\r
 131 /// @return SCE_PFX_OK if successful, otherwise, returns an error code.\r
 132 //----------------------------------------------------------------------------\r
 133 PfxInt32 pfxDetectCollision(PfxDetectCollisionParam &param, PfxTaskManager *taskManager)\r
 134 {\r
 135         PfxInt32 ret = pfxCheckParamOfDetectCollision(param);\r
 136         if(ret != SCE_PFX_OK) \r
 137                 return ret;\r
 138 \r
 139         SCE_PFX_PUSH_MARKER("pfxDetectCollision");\r
 140 \r
 141         PfxUInt32 maxBatchSize = param.numContactPairs / (PfxUInt32)(taskManager->getNumTasks());\r
 142         PfxUInt32 iEnd = maxBatchSize, iStart = 0;\r
 143         int task = 0;\r
 144         taskManager->setTaskEntry((void*)pfxDetectCollisionTaskEntry);\r
 145 \r
 146         for (task = 0; task < taskManager->getNumTasks() - 1; task++, iStart += maxBatchSize, iEnd += maxBatchSize)\r
 147         {\r
 148                 taskManager->startTask(task, static_cast<void*>(&param), iStart, iEnd, 0, 0);\r
 149         }\r
 150 \r
 151         // send final task\r
 152         iEnd = param.numContactPairs;\r
 153         taskManager->startTask(taskManager->getNumTasks() - 1, static_cast<void*>(&param), iStart, iEnd, 0, 0);\r
 154 \r
 155         // wait for tasks to complete\r
 156         PfxUInt32 data1, data2, data3, data4;\r
 157         for (PfxUInt32 i = 0; i < taskManager->getNumTasks(); i++)\r
 158                 taskManager->waitTask(task, data1, data2, data3, data4);\r
 159 \r
 160         SCE_PFX_POP_MARKER();\r
 161         \r
 162         return SCE_PFX_OK;\r
 163 }\r
 164 \r
 165 } //namespace PhysicsEffects\r
 166 } //namespace sce\r