Extras/PhysicsEffects/src/base_level/collision/pfx_intersect_ray_cylinder.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 "pfx_intersect_common.h"\r
  18 #include "pfx_intersect_ray_cylinder.h"\r
  19 \r
  20 namespace sce {\r
  21 namespace PhysicsEffects {\r
  22 \r
  23 PfxBool pfxIntersectRayCylinder(const PfxRayInput &ray,PfxRayOutput &out,const PfxCylinder &cylinder,const PfxTransform3 &transform)\r
  24 {\r
  25         // レイを円柱のローカル座標へ変換\r
  26         PfxTransform3 transformCapsule = orthoInverse(transform);\r
  27         PfxVector3 startPosL = transformCapsule.getUpper3x3() * ray.m_startPosition + transformCapsule.getTranslation();\r
  28         PfxVector3 rayDirL = transformCapsule.getUpper3x3() * ray.m_direction;\r
  29         \r
  30         PfxFloat radSqr = cylinder.m_radius * cylinder.m_radius;\r
  31 \r
  32         // 始点が円柱の内側にあるか判定\r
  33         {\r
  34                 PfxFloat h = fabsf(startPosL[0]);\r
  35                 if(h > cylinder.m_halfLen) h = cylinder.m_halfLen;\r
  36                 PfxVector3 Px(out.m_variable,0,0);\r
  37                 PfxFloat sqrLen = lengthSqr(startPosL-Px);\r
  38                 if(sqrLen <= radSqr) return false;\r
  39         }\r
  40 \r
  41         // 円柱の胴体との交差判定\r
  42         do {\r
  43                 PfxVector3 P(startPosL);\r
  44                 PfxVector3 D(rayDirL);\r
  45                 \r
  46                 P[0] = 0.0f;\r
  47                 D[0] = 0.0f;\r
  48                 \r
  49                 PfxFloat a = dot(D,D);\r
  50                 PfxFloat b = dot(P,D);\r
  51                 PfxFloat c = dot(P,P) - radSqr;\r
  52                 \r
  53                 PfxFloat d = b * b - a * c;\r
  54                 \r
  55                 if(d < 0.0f || fabs(a) < 0.00001f) return false;\r
  56                 \r
  57                 PfxFloat tt = ( -b - sqrtf(d) ) / a;\r
  58                 \r
  59                 if(tt < 0.0f)\r
  60                         break;\r
  61                 else if(tt > 1.0f)\r
  62                         return false;\r
  63                 \r
  64                 if(tt < out.m_variable) {\r
  65                         PfxVector3 cp = startPosL + tt * rayDirL;\r
  66                         \r
  67                         if(fabsf(cp[0]) <= cylinder.m_halfLen) {\r
  68                                 out.m_contactFlag = true;\r
  69                                 out.m_variable = tt;\r
  70                                 out.m_contactPoint = PfxVector3(transform * PfxPoint3(cp));\r
  71                                 out.m_contactNormal = transform.getUpper3x3() * normalize(cp);\r
  72                                 out.m_subData.m_type = PfxSubData::NONE;\r
  73                                 return true;\r
  74                         }\r
  75                 }\r
  76         } while(0);\r
  77         \r
  78         // 円柱の両端にある平面との交差判定\r
  79         {\r
  80                 if(fabsf(rayDirL[0]) < 0.00001f) return false;\r
  81                 \r
  82                 PfxFloat t1 = ( cylinder.m_halfLen - startPosL[0] ) / rayDirL[0];\r
  83                 PfxFloat t2 = ( - cylinder.m_halfLen - startPosL[0] ) / rayDirL[0];\r
  84 \r
  85                 PfxFloat tt = SCE_PFX_MIN(t1,t2);\r
  86                 \r
  87                 if(tt < 0.0f || tt > 1.0f) return false;\r
  88 \r
  89                 PfxVector3 p = startPosL + tt * rayDirL;\r
  90                 p[0] = 0.0f;\r
  91 \r
  92                 if(lengthSqr(p) < radSqr && tt < out.m_variable) {\r
  93                         PfxVector3 cp = startPosL + tt * rayDirL;\r
  94                         out.m_contactFlag = true;\r
  95                         out.m_variable = tt;\r
  96                         out.m_contactPoint = ray.m_startPosition + tt * ray.m_direction;\r
  97                         out.m_contactNormal = transform.getUpper3x3() * ((cp[0]>0.0f)?PfxVector3(1.0,0.0,0.0):PfxVector3(-1.0,0.0,0.0));\r
  98                         out.m_subData.m_type = PfxSubData::NONE;\r
  99                         return true;\r
 100                 }\r
 101         }\r
 102         \r
 103         return false;\r
 104 }\r
 105 } //namespace PhysicsEffects\r
 106 } //namespace sce\r