Extras/PhysicsEffects/src/low_level/collision/pfx_ray_cast.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_vec_utils.h"\r
  18 #include "../../../include/physics_effects/low_level/collision/pfx_ray_cast.h"\r
  19 #include "../../../include/physics_effects/base_level/collision/pfx_shape_iterator.h"\r
  20 #include "pfx_intersect_ray_func.h"\r
  21 #include "../../base_level/collision/pfx_intersect_common.h"\r
  22 \r
  23 \r
  24 namespace sce {\r
  25 namespace PhysicsEffects {\r
  26 \r
  27 \r
  28 void pfxRayTraverseForward(\r
  29         const PfxRayInput &ray,PfxRayOutput &out,const PfxAabb16 &rayAABB,\r
  30         PfxBroadphaseProxy *proxies,int numProxies,\r
  31         PfxRigidState *offsetRigidStates,\r
  32         PfxCollidable *offsetCollidables,\r
  33         int axis,const PfxVector3 &center,const PfxVector3 &half)\r
  34 {\r
  35 #ifdef SCE_PFX_USE_GEOMETRY\r
  36         PfxGeomSegment segment((PfxPoint3)ray.m_startPosition,ray.m_direction);\r
  37 #endif\r
  38 \r
  39         for(int i=0;i<numProxies;i++) {\r
  40                 PfxBroadphaseProxy &proxy = proxies[i];\r
  41 \r
  42                 // 終了条件のチェック\r
  43                 if(pfxGetXYZMax(rayAABB,axis) < pfxGetXYZMin(proxy,axis)) {\r
  44                         return;\r
  45                 }\r
  46 \r
  47                 PfxVector3 boundOnRay = ray.m_startPosition + out.m_variable * ray.m_direction;\r
  48                 PfxVector3 AABBmin = pfxConvertCoordLocalToWorld(PfxVecInt3((PfxInt32)pfxGetXMin(proxy),(PfxInt32)pfxGetYMin(proxy),(PfxInt32)pfxGetZMin(proxy)),center,half);\r
  49                 PfxVector3 AABBmax = pfxConvertCoordLocalToWorld(PfxVecInt3((PfxInt32)pfxGetXMax(proxy),(PfxInt32)pfxGetYMax(proxy),(PfxInt32)pfxGetZMax(proxy)),center,half);\r
  50 \r
  51                 if(boundOnRay[axis] < AABBmin[axis]) {\r
  52                         return;\r
  53                 }\r
  54 \r
  55                 // スキップ\r
  56                 if(pfxGetXYZMax(proxy,axis) < pfxGetXYZMin(rayAABB,axis)) {\r
  57                         continue;\r
  58                 }\r
  59 \r
  60                 PfxUInt16 rigidbodyId = pfxGetObjectId(proxy);\r
  61                 PfxUInt32 contactFilterSelf = pfxGetSelf(proxy);\r
  62                 PfxUInt32 contactFilterTarget = pfxGetTarget(proxy);\r
  63 \r
  64 #ifdef SCE_PFX_USE_GEOMETRY\r
  65                 PfxFloatInVec t_(1.0f);\r
  66                 PfxGeomAabb aabb((PfxPoint3)AABBmin,(PfxPoint3)AABBmax);\r
  67                 if( (ray.m_contactFilterSelf&contactFilterTarget) && (ray.m_contactFilterTarget&contactFilterSelf) && pfxTestAabb(rayAABB,proxy) &&\r
  68                         intersectionPoint(segment,aabb,&t_) && t_ < out.m_variable ) {\r
  69 #else\r
  70                 float t_=1.0f;\r
  71                 if( (ray.m_contactFilterSelf&contactFilterTarget) && (ray.m_contactFilterTarget&contactFilterSelf) && pfxTestAabb(rayAABB,proxy) &&\r
  72                         pfxIntersectRayAABBFast(ray.m_startPosition,ray.m_direction,(AABBmax+AABBmin)*0.5f,(AABBmax-AABBmin)*0.5f,t_) && t_ < out.m_variable ) {\r
  73 #endif\r
  74 \r
  75                         PfxRigidState &state = offsetRigidStates[rigidbodyId];\r
  76                         PfxCollidable &coll = offsetCollidables[rigidbodyId];\r
  77                         PfxTransform3 transform(state.getOrientation(), state.getPosition());\r
  78                         \r
  79                         PfxRayOutput tout = out;\r
  80                         \r
  81                         PfxShapeIterator itrShape(coll);\r
  82                         for(PfxUInt32 j=0;j<coll.getNumShapes();j++,++itrShape) {\r
  83                                 const PfxShape &shape = *itrShape;\r
  84                                 PfxTransform3 shapeTr = transform * shape.getOffsetTransform();\r
  85                                 \r
  86                                 if(pfxGetIntersectRayFunc(shape.getType())(ray,tout,shape,shapeTr) && tout.m_variable < out.m_variable) {\r
  87                                         out = tout;\r
  88                                         out.m_shapeId = j;\r
  89                                         out.m_objectId = rigidbodyId;\r
  90                                 }\r
  91                         }\r
  92                 }\r
  93         }\r
  94 }\r
  95 \r
  96 void pfxRayTraverseBackward(\r
  97         const PfxRayInput &ray,PfxRayOutput &out,const PfxAabb16 &rayAABB,\r
  98         PfxBroadphaseProxy *proxies,int numProxies,\r
  99         PfxRigidState *offsetRigidStates,\r
 100         PfxCollidable *offsetCollidables,\r
 101         int axis,const PfxVector3 &center,const PfxVector3 &half)\r
 102 {\r
 103 #ifdef SCE_PFX_USE_GEOMETRY\r
 104         PfxGeomSegment segment((PfxPoint3)ray.m_startPosition,ray.m_direction);\r
 105 #endif\r
 106 \r
 107         for(int i=numProxies-1;i>=0;i--) {\r
 108                 PfxBroadphaseProxy &proxy = proxies[i];\r
 109 \r
 110                 // 終了条件のチェック\r
 111                 if(pfxGetXYZMax(proxy,axis) < pfxGetXYZMin(rayAABB,axis)) {\r
 112                         return;\r
 113                 }\r
 114                 \r
 115                 PfxVector3 boundOnRay = ray.m_startPosition + out.m_variable * ray.m_direction;\r
 116                 PfxVector3 AABBmin = pfxConvertCoordLocalToWorld(PfxVecInt3((PfxInt32)pfxGetXMin(proxy),(PfxInt32)pfxGetYMin(proxy),(PfxInt32)pfxGetZMin(proxy)),center,half);\r
 117                 PfxVector3 AABBmax = pfxConvertCoordLocalToWorld(PfxVecInt3((PfxInt32)pfxGetXMax(proxy),(PfxInt32)pfxGetYMax(proxy),(PfxInt32)pfxGetZMax(proxy)),center,half);\r
 118                 \r
 119                 if(AABBmax[axis] < boundOnRay[axis]) {\r
 120                         return;\r
 121                 }\r
 122                 \r
 123                 // スキップ\r
 124                 if(pfxGetXYZMax(rayAABB,axis) < pfxGetXYZMin(proxy,axis)) {\r
 125                         continue;\r
 126                 }\r
 127                 \r
 128                 PfxUInt16 rigidbodyId = pfxGetObjectId(proxy);\r
 129                 PfxUInt32 contactFilterSelf = pfxGetSelf(proxy);\r
 130                 PfxUInt32 contactFilterTarget = pfxGetTarget(proxy);\r
 131                 \r
 132 #ifdef SCE_PFX_USE_GEOMETRY\r
 133                 PfxFloatInVec t_(1.0f);\r
 134                 PfxGeomAabb aabb((PfxPoint3)AABBmin,(PfxPoint3)AABBmax);\r
 135                 if( (ray.m_contactFilterSelf&contactFilterTarget) && (ray.m_contactFilterTarget&contactFilterSelf) && pfxTestAabb(rayAABB,proxy) &&\r
 136                         intersectionPoint(segment,aabb,&t_) && t_ < out.m_variable ) {\r
 137 #else\r
 138                 float t_=1.0f;\r
 139                 if( (ray.m_contactFilterSelf&contactFilterTarget) && (ray.m_contactFilterTarget&contactFilterSelf) && pfxTestAabb(rayAABB,proxy) &&\r
 140                         pfxIntersectRayAABBFast(ray.m_startPosition,ray.m_direction,(AABBmax+AABBmin)*0.5f,(AABBmax-AABBmin)*0.5f,t_) && t_ < out.m_variable ) {\r
 141 #endif\r
 142 \r
 143                         PfxRigidState &state = offsetRigidStates[rigidbodyId];\r
 144                         PfxCollidable &coll = offsetCollidables[rigidbodyId];\r
 145                         PfxTransform3 transform(state.getOrientation(), state.getPosition());\r
 146                         \r
 147                         PfxRayOutput tout = out;\r
 148                         \r
 149                         PfxShapeIterator itrShape(coll);\r
 150                         for(PfxUInt32 j=0;j<coll.getNumShapes();j++,++itrShape) {\r
 151                                 const PfxShape &shape = *itrShape;\r
 152                                 PfxTransform3 shapeTr = transform * shape.getOffsetTransform();\r
 153                                 \r
 154                                 if(pfxGetIntersectRayFunc(shape.getType())(ray,tout,shape,shapeTr) && tout.m_variable < out.m_variable) {\r
 155                                         out = tout;\r
 156                                         out.m_shapeId = j;\r
 157                                         out.m_objectId = rigidbodyId;\r
 158                                 }\r
 159                         }\r
 160                 }\r
 161         }\r
 162 }\r
 163 \r
 164 \r
 165 void pfxCastSingleRay(const PfxRayInput &ray,PfxRayOutput &out,const PfxRayCastParam &param)\r
 166 {\r
 167         SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesX));\r
 168         SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesY));\r
 169         SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesZ));\r
 170         SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesXb));\r
 171         SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesYb));\r
 172         SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.proxiesZb));\r
 173         SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.offsetRigidStates));\r
 174         SCE_PFX_ASSERT(SCE_PFX_PTR_IS_ALIGNED16(param.offsetCollidables));\r
 175 \r
 176         PfxBroadphaseProxy *proxies[] = {\r
 177                 param.proxiesX,\r
 178                 param.proxiesY,\r
 179                 param.proxiesZ,\r
 180                 param.proxiesXb,\r
 181                 param.proxiesYb,\r
 182                 param.proxiesZb,\r
 183         };\r
 184         \r
 185         out.m_variable = 1.0f;\r
 186         out.m_contactFlag = false;\r
 187         \r
 188         // 探索軸\r
 189         PfxVector3 chkAxisVec = absPerElem(ray.m_direction);\r
 190         int axis = 0;\r
 191         if(chkAxisVec[1] < chkAxisVec[0]) axis = 1;\r
 192         if(chkAxisVec[2] < chkAxisVec[axis]) axis = 2;\r
 193 \r
 194         // レイのAABB作成\r
 195         PfxVector3 p1 = ray.m_startPosition;\r
 196         PfxVector3 p2 = ray.m_startPosition + ray.m_direction;\r
 197         PfxVecInt3 rayMin,rayMax;\r
 198         pfxConvertCoordWorldToLocal(param.rangeCenter,param.rangeExtent,minPerElem(p1,p2),maxPerElem(p1,p2),rayMin,rayMax);\r
 199 \r
 200         PfxAabb16 rayAABB;\r
 201         pfxSetXMin(rayAABB,rayMin.getX());\r
 202         pfxSetXMax(rayAABB,rayMax.getX());\r
 203         pfxSetYMin(rayAABB,rayMin.getY());\r
 204         pfxSetYMax(rayAABB,rayMax.getY());\r
 205         pfxSetZMin(rayAABB,rayMin.getZ());\r
 206         pfxSetZMax(rayAABB,rayMax.getZ());\r
 207         \r
 208         // AABB探索開始\r
 209         int sign = ray.m_direction[axis] < 0.0f ? -1 : 1; // 探索方向\r
 210 \r
 211         if(sign > 0) {\r
 212                 pfxRayTraverseForward(\r
 213                         ray,out,rayAABB,\r
 214                         proxies[axis],param.numProxies,\r
 215                         param.offsetRigidStates,param.offsetCollidables,\r
 216                         axis,param.rangeCenter,param.rangeExtent);\r
 217         }\r
 218         else {\r
 219                 pfxRayTraverseBackward(\r
 220                         ray,out,rayAABB,\r
 221                         proxies[axis+3],param.numProxies,\r
 222                         param.offsetRigidStates,param.offsetCollidables,\r
 223                         axis,param.rangeCenter,param.rangeExtent);\r
 224         }\r
 225 }\r
 226 \r
 227 } //namespace PhysicsEffects\r
 228 } //namespace sce\r