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

   1 /*\r
   2 Physics Effects Copyright(C) 2010 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 "../../../include/physics_effects/base_level/base/pfx_perf_counter.h"\r
  18 #include "../../../include/physics_effects/base_level/collision/pfx_shape_iterator.h"\r
  19 #include "../../../include/physics_effects/low_level/collision/pfx_collision_detection.h"\r
  20 #include "../../base_level/broadphase/pfx_check_collidable.h"\r
  21 #include "../../base_level/collision/pfx_contact_cache.h"\r
  22 #include "pfx_detect_collision_func.h"\r
  23 \r
  24 namespace sce {\r
  25 namespace PhysicsEffects {\r
  26 \r
  27 ///////////////////////////////////////////////////////////////////////////////\r
  28 \r
  29 int pfxCheckParamOfDetectCollision(PfxDetectCollisionParam &param)\r
  30 {\r
  31         if(!param.contactPairs || !param.offsetContactManifolds || !param.offsetRigidStates|| !param.offsetCollidables ) return SCE_PFX_ERR_INVALID_VALUE;\r
  32         if(!SCE_PFX_PTR_IS_ALIGNED16(param.contactPairs) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetContactManifolds) || \r
  33                 !SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates) || !SCE_PFX_PTR_IS_ALIGNED16(param.offsetCollidables)) return SCE_PFX_ERR_INVALID_ALIGN;\r
  34         return SCE_PFX_OK;\r
  35 }\r
  36 \r
  37 ///////////////////////////////////////////////////////////////////////////////\r
  38 // SINGLE THREAD\r
  39 \r
  40 #define SCE_PFX_CONTACT_THRESHOLD 0.0f\r
  41 \r
  42 PfxInt32 pfxDetectCollision(PfxDetectCollisionParam &param)\r
  43 {\r
  44         PfxInt32 ret = pfxCheckParamOfDetectCollision(param);\r
  45         if(ret != SCE_PFX_OK) \r
  46                 return ret;\r
  47 \r
  48         SCE_PFX_PUSH_MARKER("pfxDetectCollision");\r
  49 \r
  50         PfxConstraintPair *contactPairs = param.contactPairs;\r
  51         PfxUInt32 numContactPairs = param.numContactPairs;\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=0;i<numContactPairs;i++) {\r
  58                 const PfxBroadphasePair &pair = contactPairs[i];\r
  59                 if(!pfxCheckCollidableInCollision(pair)) {\r
  60                         continue;\r
  61                 }\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                         const PfxShape &shapeA = *itrShapeA;\r
  84                         PfxTransform3 offsetTrA = shapeA.getOffsetTransform();\r
  85                         PfxTransform3 worldTrA = tA0 * offsetTrA;\r
  86 \r
  87                         PfxShapeIterator itrShapeB(collB);\r
  88                         for(PfxUInt32 k=0;k<collB.getNumShapes();k++,++itrShapeB) {\r
  89                                 const PfxShape &shapeB = *itrShapeB;\r
  90                                 PfxTransform3 offsetTrB = shapeB.getOffsetTransform();\r
  91                                 PfxTransform3 worldTrB = tB0 * offsetTrB;\r
  92 \r
  93                                 if( (shapeA.getContactFilterSelf()&shapeB.getContactFilterTarget()) && \r
  94                                     (shapeA.getContactFilterTarget()&shapeB.getContactFilterSelf()) ) {\r
  95                                         pfxGetDetectCollisionFunc(shapeA.getType(),shapeB.getType())(\r
  96                                                 contactCache,\r
  97                                                 shapeA,offsetTrA,worldTrA,j,\r
  98                                                 shapeB,offsetTrB,worldTrB,k,\r
  99                                                 SCE_PFX_CONTACT_THRESHOLD);\r
 100                                 }\r
 101                         }\r
 102                 }\r
 103                 \r
 104                 for(int j=0;j<contactCache.getNumContacts();j++) {\r
 105                         const PfxCachedContactPoint &cp = contactCache.getContactPoint(j);\r
 106 \r
 107                         contact.addContactPoint(\r
 108                                 cp.m_distance,\r
 109                                 cp.m_normal,\r
 110                                 cp.m_localPointA,\r
 111                                 cp.m_localPointB,\r
 112                                 cp.m_subData\r
 113                                 );\r
 114                 }\r
 115         }\r
 116 \r
 117         SCE_PFX_POP_MARKER();\r
 118         \r
 119         (void) numRigidBodies;\r
 120 \r
 121         return SCE_PFX_OK;\r
 122 }\r
 123 \r
 124 } //namespace PhysicsEffects\r
 125 } //namespace sce\r