Extras/PhysicsEffects/src/base_level/collision/pfx_intersect_ray_capsule.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_capsule.h"\r
  19 \r
  20 namespace sce {\r
  21 namespace PhysicsEffects {\r
  22 PfxBool pfxIntersectRayCapsule(const PfxRayInput &ray,PfxRayOutput &out,const PfxCapsule &capsule,const PfxTransform3 &transform)\r
  23 {\r
  24         // レイをCapsuleのローカル座標へ変換\r
  25         PfxTransform3 transformCapsule = orthoInverse(transform);\r
  26         PfxVector3 startPosL = transformCapsule.getUpper3x3() * ray.m_startPosition + transformCapsule.getTranslation();\r
  27         PfxVector3 rayDirL = transformCapsule.getUpper3x3() * ray.m_direction;\r
  28         \r
  29         PfxFloat radSqr = capsule.m_radius * capsule.m_radius;\r
  30 \r
  31         // 始点がカプセルの内側にあるか判定\r
  32         {\r
  33                 PfxFloat h = fabsf(startPosL[0]);\r
  34                 if(h > capsule.m_halfLen) h = capsule.m_halfLen;\r
  35                 PfxVector3 Px(out.m_variable,0,0);\r
  36                 PfxFloat sqrLen = lengthSqr(startPosL-Px);\r
  37                 if(sqrLen <= radSqr) return false;\r
  38         }\r
  39 \r
  40         // カプセルの胴体との交差判定\r
  41         do {\r
  42                 PfxVector3 P(startPosL);\r
  43                 PfxVector3 D(rayDirL);\r
  44                 \r
  45                 P[0] = 0.0f;\r
  46                 D[0] = 0.0f;\r
  47                 \r
  48                 PfxFloat a = dot(D,D);\r
  49                 PfxFloat b = dot(P,D);\r
  50                 PfxFloat c = dot(P,P) - radSqr;\r
  51                 \r
  52                 PfxFloat d = b * b - a * c;\r
  53                 \r
  54                 if(d < 0.0f || fabs(a) < 0.00001f) return false;\r
  55                 \r
  56                 PfxFloat tt = ( -b - sqrtf(d) ) / a;\r
  57                 \r
  58                 if(tt < 0.0f)\r
  59                         break;\r
  60                 else if(tt > 1.0f)\r
  61                         return false;\r
  62                 \r
  63                 if(tt < out.m_variable) {\r
  64                         PfxVector3 cp = startPosL + tt * rayDirL;\r
  65                         \r
  66                         if(fabsf(cp[0]) <= capsule.m_halfLen) {\r
  67                                 out.m_contactFlag = true;\r
  68                                 out.m_variable = tt;\r
  69                                 out.m_contactPoint = PfxVector3(transform * PfxPoint3(cp));\r
  70                                 out.m_contactNormal = transform.getUpper3x3() * normalize(cp);\r
  71                                 out.m_subData.m_type = PfxSubData::NONE;\r
  72                                 return true;\r
  73                         }\r
  74                 }\r
  75         } while(0);\r
  76         \r
  77         // カプセルの両端にある球体との交差判定\r
  78         PfxFloat a = dot(rayDirL,rayDirL);\r
  79         if(fabs(a) < 0.00001f) return false;\r
  80         \r
  81         do {\r
  82                 PfxVector3 center(capsule.m_halfLen,0.0f,0.0f);\r
  83                 PfxVector3 v = startPosL - center;\r
  84 \r
  85                 PfxFloat b = dot(v,rayDirL);\r
  86                 PfxFloat c = dot(v,v) - radSqr;\r
  87 \r
  88                 PfxFloat d = b * b - a * c;\r
  89                 \r
  90                 if(d < 0.0f) break;\r
  91                 \r
  92                 PfxFloat tt = ( -b - sqrtf(d) ) / a;\r
  93                 \r
  94                 if(tt < 0.0f || tt > 1.0f) break;\r
  95                 \r
  96                 if(tt < out.m_variable) {\r
  97                         PfxVector3 cp = startPosL + tt * rayDirL;\r
  98                         out.m_contactFlag = true;\r
  99                         out.m_variable = tt;\r
 100                         out.m_contactPoint = ray.m_startPosition + tt * ray.m_direction;\r
 101                         out.m_contactNormal = transform.getUpper3x3() * normalize(cp-center);\r
 102                         out.m_subData.m_type = PfxSubData::NONE;\r
 103                         return true;\r
 104                 }\r
 105         } while(0);\r
 106         \r
 107         {\r
 108                 PfxVector3 center(-capsule.m_halfLen,0.0f,0.0f);\r
 109                 PfxVector3 v = startPosL - center;\r
 110 \r
 111                 PfxFloat b = dot(v,rayDirL);\r
 112                 PfxFloat c = dot(v,v) - radSqr;\r
 113 \r
 114                 PfxFloat d = b * b - a * c;\r
 115                 \r
 116                 if(d < 0.0f) return false;\r
 117                 \r
 118                 PfxFloat tt = ( -b - sqrtf(d) ) / a;\r
 119                 \r
 120                 if(tt < 0.0f || tt > 1.0f) return false;\r
 121                 \r
 122                 if(tt < out.m_variable) {\r
 123                         PfxVector3 cp = startPosL + out.m_variable * rayDirL;\r
 124                         out.m_contactFlag = true;\r
 125                         out.m_variable = tt;\r
 126                         out.m_contactPoint = ray.m_startPosition + tt * ray.m_direction;\r
 127                         out.m_contactNormal = transform.getUpper3x3() * normalize(cp-center);\r
 128                         out.m_subData.m_type = PfxSubData::NONE;\r
 129                         return true;\r
 130                 }\r
 131         }\r
 132         \r
 133         return false;\r
 134 }\r
 135 } //namespace PhysicsEffects\r
 136 } //namespace sce\r