Extras/PhysicsEffects/src/base_level/collision/pfx_intersect_common.h

   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 #ifndef _SCE_PFX_INTERSECT_COMMON_H\r
  18 #define _SCE_PFX_INTERSECT_COMMON_H\r
  19 \r
  20 #include "../../../include/physics_effects/base_level/collision/pfx_ray.h"\r
  21 \r
  22 namespace sce {\r
  23 namespace PhysicsEffects {\r
  24 \r
  25 #define SCE_PFX_INTERSECT_COMMON_EPSILON 0.00001f\r
  26 #define SCE_PFX_RAY_TRIANGLE_EPSILON 0.00001f\r
  27 \r
  28 // Internally used intersect functions\r
  29 \r
  30 struct PfxTriangle\r
  31 {\r
  32         PfxVector3 points[3];\r
  33         \r
  34         PfxTriangle(const PfxVector3 &p0,const PfxVector3 &p1,const PfxVector3 &p2)\r
  35         {\r
  36                 points[0] = p0;\r
  37                 points[1] = p1;\r
  38                 points[2] = p2;\r
  39         }\r
  40 };\r
  41 \r
  42 struct PfxPlane\r
  43 {\r
  44         PfxVector3 normal; // normal\r
  45         PfxVector3 point; // point on the plane\r
  46         \r
  47         PfxPlane(const PfxVector3 &n,const PfxVector3 &q)\r
  48         {\r
  49                 normal = n;\r
  50                 point = q;\r
  51         }\r
  52         \r
  53         PfxPlane(const PfxTriangle &triangle)\r
  54         {\r
  55                 normal = normalize(cross(triangle.points[1]-triangle.points[0],triangle.points[2]-triangle.points[0]));\r
  56                 point = triangle.points[0];\r
  57         }\r
  58         \r
  59         PfxFloat onPlane(const PfxVector3 &p) const\r
  60         {\r
  61                 return dot((p-point),normal);\r
  62         }\r
  63 };\r
  64 \r
  65 static SCE_PFX_FORCE_INLINE\r
  66 PfxBool pfxIntersectRayAABBFast(\r
  67         const PfxVector3 &rayStartPosition,\r
  68         const PfxVector3 &rayDirection,\r
  69         const PfxVector3 &AABBcenter,\r
  70         const PfxVector3 &AABBhalf,\r
  71         PfxFloat &variable)\r
  72 {\r
  73         PfxVector3 AABBmin = AABBcenter - AABBhalf;\r
  74         PfxVector3 AABBmax = AABBcenter + AABBhalf;\r
  75         \r
  76         PfxVector3 dir = rayDirection;\r
  77         PfxVector3 absDir = absPerElem(dir);\r
  78         PfxVector3 sign = copySignPerElem(PfxVector3(1.0),dir);\r
  79         \r
  80 if(absDir[0] < SCE_PFX_INTERSECT_COMMON_EPSILON) {\r
  81         if(rayStartPosition[0] < AABBmin[0] || rayStartPosition[0] > AABBmax[0]) {\r
  82                 return false;\r
  83         }\r
  84         dir[0] = sign[0] * SCE_PFX_INTERSECT_COMMON_EPSILON;\r
  85 }\r
  86 \r
  87 if(absDir[1] < SCE_PFX_INTERSECT_COMMON_EPSILON) {\r
  88         if(rayStartPosition[1] < AABBmin[1] || rayStartPosition[1] > AABBmax[1]) {\r
  89                 return false;\r
  90         }\r
  91         dir[1] = sign[1] * SCE_PFX_INTERSECT_COMMON_EPSILON;\r
  92 }\r
  93 \r
  94 if(absDir[2] < SCE_PFX_INTERSECT_COMMON_EPSILON) {\r
  95         if(rayStartPosition[2] < AABBmin[2] || rayStartPosition[2] > AABBmax[2]) {\r
  96                 return false;\r
  97         }\r
  98         dir[2] = sign[2] * SCE_PFX_INTERSECT_COMMON_EPSILON;\r
  99 }\r
 100 \r
 101         PfxVector3 t1 = divPerElem(AABBmin - rayStartPosition, dir);\r
 102         PfxVector3 t2 = divPerElem(AABBmax - rayStartPosition, dir);\r
 103         \r
 104         PfxVector3 tmin = minPerElem(t1,t2);\r
 105         PfxVector3 tmax = maxPerElem(t1,t2);\r
 106         \r
 107 if(maxElem(tmin) > minElem(tmax)) return false;\r
 108 \r
 109 if(tmin[0] > tmin[1]) {\r
 110         if(tmin[0] > tmin[2]) {\r
 111                 variable = tmin[0];\r
 112         }\r
 113         else {\r
 114                 variable = tmin[2];\r
 115         }\r
 116 }\r
 117 else {\r
 118         if(tmin[1] > tmin[2]) {\r
 119                 variable = tmin[1];\r
 120         }\r
 121         else {\r
 122                 variable = tmin[2];\r
 123         }\r
 124 }\r
 125 \r
 126         return true;\r
 127 }\r
 128 \r
 129 static SCE_PFX_FORCE_INLINE\r
 130 PfxBool pfxIntersectRayAABB(\r
 131         const PfxVector3 &rayStartPosition,\r
 132         const PfxVector3 &rayDirection,\r
 133         const PfxVector3 &AABBcenter,\r
 134         const PfxVector3 &AABBhalf,\r
 135         PfxFloat &variable,\r
 136         PfxVector3 &normal)\r
 137 {\r
 138         PfxVector3 AABBmin = AABBcenter - AABBhalf;\r
 139         PfxVector3 AABBmax = AABBcenter + AABBhalf;\r
 140         \r
 141         PfxVector3 dir = rayDirection;\r
 142         PfxVector3 absDir = absPerElem(dir);\r
 143         PfxVector3 sign = copySignPerElem(PfxVector3(1.0),dir);\r
 144 \r
 145         // 始点がBoxの内側にあるか判定\r
 146         if( AABBmin[0] < rayStartPosition[0] && rayStartPosition[0] < AABBmax[0] &&\r
 147                 AABBmin[1] < rayStartPosition[1] && rayStartPosition[1] < AABBmax[1] &&\r
 148                 AABBmin[2] < rayStartPosition[2] && rayStartPosition[2] < AABBmax[2]) {\r
 149                 return false;\r
 150         }\r
 151 \r
 152 if(absDir[0] < SCE_PFX_INTERSECT_COMMON_EPSILON) {\r
 153         if(rayStartPosition[0] < AABBmin[0] || rayStartPosition[0] > AABBmax[0]) {\r
 154                 return false;\r
 155         }\r
 156         dir[0] = sign[0] * SCE_PFX_INTERSECT_COMMON_EPSILON;\r
 157 }\r
 158 \r
 159 if(absDir[1] < SCE_PFX_INTERSECT_COMMON_EPSILON) {\r
 160         if(rayStartPosition[1] < AABBmin[1] || rayStartPosition[1] > AABBmax[1]) {\r
 161                 return false;\r
 162         }\r
 163         dir[1] = sign[1] * SCE_PFX_INTERSECT_COMMON_EPSILON;\r
 164 }\r
 165 \r
 166 if(absDir[2] < SCE_PFX_INTERSECT_COMMON_EPSILON) {\r
 167         if(rayStartPosition[2] < AABBmin[2] || rayStartPosition[2] > AABBmax[2]) {\r
 168                 return false;\r
 169         }\r
 170         dir[2] = sign[2] * SCE_PFX_INTERSECT_COMMON_EPSILON;\r
 171 }\r
 172 \r
 173         PfxVector3 t1 = divPerElem(AABBmin - rayStartPosition, dir);\r
 174         PfxVector3 t2 = divPerElem(AABBmax - rayStartPosition, dir);\r
 175         \r
 176         PfxVector3 tmin = minPerElem(t1,t2);\r
 177         PfxVector3 tmax = maxPerElem(t1,t2);\r
 178         \r
 179         normal = PfxVector3(0);\r
 180 \r
 181 if(maxElem(tmin) > minElem(tmax)) return false;\r
 182 \r
 183 if(tmin[0] > tmin[1]) {\r
 184         if(tmin[0] > tmin[2]) {\r
 185                 variable = tmin[0];\r
 186                 normal[0] = -sign[0];\r
 187         }\r
 188         else {\r
 189                 variable = tmin[2];\r
 190                 normal[2] = -sign[2];\r
 191         }\r
 192 }\r
 193 else {\r
 194         if(tmin[1] > tmin[2]) {\r
 195                 variable = tmin[1];\r
 196                 normal[1] = -sign[1];\r
 197         }\r
 198         else {\r
 199                 variable = tmin[2];\r
 200                 normal[2] = -sign[2];\r
 201         }\r
 202 }\r
 203 \r
 204         return true;\r
 205 }\r
 206 \r
 207 static SCE_PFX_FORCE_INLINE\r
 208 void pfxClosestTwoLines(\r
 209         const PfxVector3 &p1,const PfxVector3 &q1, // line1\r
 210         const PfxVector3 &p2,const PfxVector3 &q2, // line2\r
 211         PfxVector3 &s1,PfxVector3 &s2)\r
 212 {\r
 213         PfxVector3 v1 = q1-p1;\r
 214         PfxVector3 v2 = q2-p2;\r
 215         PfxVector3 r = p1 - p2;\r
 216 \r
 217         PfxFloat a = dot(v1,v1);\r
 218         PfxFloat e = dot(v2,v2);\r
 219         PfxFloat f = dot(v2,r);\r
 220         PfxFloat b = dot(v1,v2);\r
 221         PfxFloat c = dot(v1,r);\r
 222         PfxFloat den = a*e-b*b;\r
 223         \r
 224         PfxFloat s,t;\r
 225         \r
 226         if(den != 0.0f) {\r
 227                 s = SCE_PFX_CLAMP((b*f-c*e)/den,0.0f,1.0f);\r
 228         }\r
 229         else {\r
 230                 s = 0.0f;\r
 231         }\r
 232         \r
 233         t = (b*s+f)/e;\r
 234         \r
 235         if(t < 0.0f) {\r
 236                 t = 0.0f;\r
 237                 s = SCE_PFX_CLAMP(-c/a,0.0f,1.0f);\r
 238         }\r
 239         else if(t > 1.0f) {\r
 240                 t = 1.0f;\r
 241                 s = SCE_PFX_CLAMP((b-c)/a,0.0f,1.0f);\r
 242         }\r
 243         \r
 244         s1 = p1 + s * v1;\r
 245         s2 = p2 + t * v2;\r
 246 }\r
 247 \r
 248 static SCE_PFX_FORCE_INLINE\r
 249 void pfxClosestPointAABB(\r
 250         const PfxVector3 &point,\r
 251         const PfxVector3 &AABBhalf,\r
 252         PfxVector3 &s)\r
 253 {\r
 254         s = point;\r
 255         s = maxPerElem(s,-AABBhalf);\r
 256         s = minPerElem(s,AABBhalf);\r
 257 }\r
 258 \r
 259 static SCE_PFX_FORCE_INLINE\r
 260 void pfxClosestPointTriangle(\r
 261         const PfxVector3 &point,\r
 262         const PfxTriangle &triangle,\r
 263         PfxVector3 &s)\r
 264 {\r
 265         PfxVector3 a = triangle.points[0];\r
 266         PfxVector3 b = triangle.points[1];\r
 267         PfxVector3 c = triangle.points[2];\r
 268     PfxVector3 ab = b - a;\r
 269     PfxVector3 ac = c - a;\r
 270     PfxVector3 ap = point - a;\r
 271     PfxFloat d1 = dot(ab, ap);\r
 272     PfxFloat d2 = dot(ac, ap);\r
 273         if(d1 <= 0.0f && d2 <= 0.0f) {\r
 274                 s = a;\r
 275                 return;\r
 276         }\r
 277 \r
 278     PfxVector3 bp = point - b;\r
 279     PfxFloat d3 = dot(ab, bp);\r
 280     PfxFloat d4 = dot(ac, bp);\r
 281         if (d3 >= 0.0f && d4 <= d3) {\r
 282                 s = b;\r
 283                 return;\r
 284         }\r
 285 \r
 286     PfxFloat vc = d1*d4 - d3*d2;\r
 287     if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) {\r
 288         PfxFloat v = d1 / (d1 - d3);\r
 289         s = a + v * ab;\r
 290                 return;\r
 291     }\r
 292 \r
 293     PfxVector3 cp = point - c;\r
 294     PfxFloat d5 = dot(ab, cp);\r
 295     PfxFloat d6 = dot(ac, cp);\r
 296         if (d6 >= 0.0f && d5 <= d6) {\r
 297                 s = c;\r
 298                 return;\r
 299         }\r
 300 \r
 301     PfxFloat vb = d5*d2 - d1*d6;\r
 302     if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) {\r
 303         PfxFloat w = d2 / (d2 - d6);\r
 304         s = a + w * ac;\r
 305                 return;\r
 306     }\r
 307 \r
 308     PfxFloat va = d3*d6 - d5*d4;\r
 309     if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) {\r
 310         PfxFloat w = (d4 - d3) / ((d4 - d3) + (d5 - d6));\r
 311         s = b + w * (c - b);\r
 312                 return;\r
 313     }\r
 314 \r
 315     PfxFloat den = 1.0f / (va + vb + vc);\r
 316     PfxFloat v = vb * den;\r
 317     PfxFloat w = vc * den;\r
 318     s = a + ab * v + ac * w;\r
 319 }\r
 320 \r
 321 static SCE_PFX_FORCE_INLINE\r
 322 PfxBool pfxIntersectRayTriangle(\r
 323         const PfxVector3 &rayStartPosition,\r
 324         const PfxVector3 &rayDirection,\r
 325         const PfxTriangle &triangle,\r
 326         PfxFloat &variable)\r
 327 {\r
 328         PfxFloat v,w;\r
 329         PfxVector3 ab = triangle.points[1] - triangle.points[0];\r
 330         PfxVector3 ac = triangle.points[2] - triangle.points[0];\r
 331 \r
 332         PfxVector3 n = cross(ab,ac);\r
 333 \r
 334         PfxFloat d = dot(-rayDirection,n);\r
 335         \r
 336         if(fabsf(d) < 0.00001f) return false;\r
 337 \r
 338         PfxVector3 ap = rayStartPosition - triangle.points[0];\r
 339         PfxFloat t = dot(ap,n) / d;\r
 340 \r
 341         if(t <= 0.0f || t >= 1.0f) return false;\r
 342 \r
 343         variable = t;\r
 344 \r
 345         PfxVector3 e = cross(-rayDirection,ap);\r
 346         v = dot(ac,e) / d;\r
 347         if(v < -SCE_PFX_RAY_TRIANGLE_EPSILON || v > 1.0f+SCE_PFX_RAY_TRIANGLE_EPSILON) return false;\r
 348 \r
 349         w = -dot(ab,e) / d;\r
 350         if(w < -SCE_PFX_RAY_TRIANGLE_EPSILON || v+w > 1.0f+SCE_PFX_RAY_TRIANGLE_EPSILON) return false;\r
 351 \r
 352         return true;\r
 353 }\r
 354 \r
 355 static SCE_PFX_FORCE_INLINE\r
 356 PfxBool pfxIntersectRayTriangleWithoutFrontFace(\r
 357         const PfxVector3 &rayStartPosition,\r
 358         const PfxVector3 &rayDirection,\r
 359         const PfxTriangle &triangle,\r
 360         PfxFloat &variable)\r
 361 {\r
 362         PfxFloat v,w;\r
 363         PfxVector3 ab = triangle.points[1] - triangle.points[0];\r
 364         PfxVector3 ac = triangle.points[2] - triangle.points[0];\r
 365 \r
 366         PfxVector3 n = cross(ab,ac);\r
 367 \r
 368         PfxFloat d = dot(-rayDirection,n);\r
 369         \r
 370         if(d >= 0.0f) return false;\r
 371 \r
 372         PfxVector3 ap = rayStartPosition - triangle.points[0];\r
 373         PfxFloat t = dot(ap,n) / d;\r
 374 \r
 375         if(t <= 0.0f || t >= 1.0f) return false;\r
 376 \r
 377         variable = t;\r
 378 \r
 379         PfxVector3 e = cross(-rayDirection,ap);\r
 380         v = dot(ac,e) / d;\r
 381         if(v < -SCE_PFX_RAY_TRIANGLE_EPSILON || v > 1.0f+SCE_PFX_RAY_TRIANGLE_EPSILON) return false;\r
 382 \r
 383         w = -dot(ab,e) / d;\r
 384         if(w < -SCE_PFX_RAY_TRIANGLE_EPSILON || v+w > 1.0f+SCE_PFX_RAY_TRIANGLE_EPSILON) return false;\r
 385 \r
 386         return true;\r
 387 }\r
 388 \r
 389 static SCE_PFX_FORCE_INLINE\r
 390 PfxBool pfxIntersectRayTriangleWithoutBackFace(\r
 391         const PfxVector3 &rayStartPosition,\r
 392         const PfxVector3 &rayDirection,\r
 393         const PfxTriangle &triangle,\r
 394         PfxFloat &variable)\r
 395 {\r
 396         PfxFloat v,w;\r
 397         PfxVector3 ab = triangle.points[1] - triangle.points[0];\r
 398         PfxVector3 ac = triangle.points[2] - triangle.points[0];\r
 399 \r
 400         PfxVector3 n = cross(ab,ac);\r
 401 \r
 402         PfxFloat d = dot(-rayDirection,n);\r
 403         \r
 404         if(d <= 0.0f) return false;\r
 405 \r
 406         PfxVector3 ap = rayStartPosition - triangle.points[0];\r
 407         PfxFloat t = dot(ap,n) / d;\r
 408 \r
 409         if(t <= 0.0f || t >= 1.0f) return false;\r
 410 \r
 411         variable = t;\r
 412 \r
 413         PfxVector3 e = cross(-rayDirection,ap);\r
 414         v = dot(ac,e) / d;\r
 415         if(v < -SCE_PFX_RAY_TRIANGLE_EPSILON || v > 1.0f+SCE_PFX_RAY_TRIANGLE_EPSILON) return false;\r
 416 \r
 417         w = -dot(ab,e) / d;\r
 418         if(w < -SCE_PFX_RAY_TRIANGLE_EPSILON || v+w > 1.0f+SCE_PFX_RAY_TRIANGLE_EPSILON) return false;\r
 419 \r
 420         return true;\r
 421 }\r
 422 \r
 423 } //namespace PhysicsEffects\r
 424 } //namespace sce\r
 425 \r
 426 \r
 427 #endif // _SCE_PFX_INTERSECT_COMMON_H\r