Extras/CDTestFramework/Opcode/OPC_TriBoxOverlap.h

   1 /*
   2  *      OPCODE - Optimized Collision Detection
   3  * http://www.codercorner.com/Opcode.htm
   4  *
   5  * Copyright (c) 2001-2008 Pierre Terdiman,  pierre@codercorner.com
   6
   7 This software is provided 'as-is', without any express or implied warranty.
   8 In no event will the authors be held liable for any damages arising from the use of this software.
   9 Permission is granted to anyone to use this software for any purpose,
  10 including commercial applications, and to alter it and redistribute it freely,
  11 subject to the following restrictions:
  12
  13 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  14 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  15 3. This notice may not be removed or altered from any source distribution.
  16 */
  17
  18
  19 //! This macro quickly finds the min & max values among 3 variables
  20 #define FINDMINMAX(x0, x1, x2, min, max)        \
  21         min = max = x0;                                                 \
  22         if(x1<min) min=x1;                                              \
  23         if(x1>max) max=x1;                                              \
  24         if(x2<min) min=x2;                                              \
  25         if(x2>max) max=x2;
  26
  27 //! TO BE DOCUMENTED
  28 inline_ BOOL planeBoxOverlap(const Point& normal, const float d, const Point& maxbox)
  29 {
  30         Point vmin, vmax;
  31         for(udword q=0;q<=2;q++)
  32         {
  33                 if(normal[q]>0.0f)      { vmin[q]=-maxbox[q]; vmax[q]=maxbox[q]; }
  34                 else                            { vmin[q]=maxbox[q]; vmax[q]=-maxbox[q]; }
  35         }
  36         if((normal|vmin)+d>0.0f) return FALSE;
  37         if((normal|vmax)+d>=0.0f) return TRUE;
  38
  39         return FALSE;
  40 }
  41
  42 //! TO BE DOCUMENTED
  43 #define AXISTEST_X01(a, b, fa, fb)                                                      \
  44         min = a*v0.y - b*v0.z;                                                                  \
  45         max = a*v2.y - b*v2.z;                                                                  \
  46         if(min>max) {const float tmp=max; max=min; min=tmp;     }       \
  47         rad = fa * extents.y + fb * extents.z;                                  \
  48         if(min>rad || max<-rad) return FALSE;
  49
  50 //! TO BE DOCUMENTED
  51 #define AXISTEST_X2(a, b, fa, fb)                                                       \
  52         min = a*v0.y - b*v0.z;                                                                  \
  53         max = a*v1.y - b*v1.z;                                                                  \
  54         if(min>max) {const float tmp=max; max=min; min=tmp;     }       \
  55         rad = fa * extents.y + fb * extents.z;                                  \
  56         if(min>rad || max<-rad) return FALSE;
  57
  58 //! TO BE DOCUMENTED
  59 #define AXISTEST_Y02(a, b, fa, fb)                                                      \
  60         min = b*v0.z - a*v0.x;                                                                  \
  61         max = b*v2.z - a*v2.x;                                                                  \
  62         if(min>max) {const float tmp=max; max=min; min=tmp;     }       \
  63         rad = fa * extents.x + fb * extents.z;                                  \
  64         if(min>rad || max<-rad) return FALSE;
  65
  66 //! TO BE DOCUMENTED
  67 #define AXISTEST_Y1(a, b, fa, fb)                                                       \
  68         min = b*v0.z - a*v0.x;                                                                  \
  69         max = b*v1.z - a*v1.x;                                                                  \
  70         if(min>max) {const float tmp=max; max=min; min=tmp;     }       \
  71         rad = fa * extents.x + fb * extents.z;                                  \
  72         if(min>rad || max<-rad) return FALSE;
  73
  74 //! TO BE DOCUMENTED
  75 #define AXISTEST_Z12(a, b, fa, fb)                                                      \
  76         min = a*v1.x - b*v1.y;                                                                  \
  77         max = a*v2.x - b*v2.y;                                                                  \
  78         if(min>max) {const float tmp=max; max=min; min=tmp;     }       \
  79         rad = fa * extents.x + fb * extents.y;                                  \
  80         if(min>rad || max<-rad) return FALSE;
  81
  82 //! TO BE DOCUMENTED
  83 #define AXISTEST_Z0(a, b, fa, fb)                                                       \
  84         min = a*v0.x - b*v0.y;                                                                  \
  85         max = a*v1.x - b*v1.y;                                                                  \
  86         if(min>max) {const float tmp=max; max=min; min=tmp;     }       \
  87         rad = fa * extents.x + fb * extents.y;                                  \
  88         if(min>rad || max<-rad) return FALSE;
  89
  90 // compute triangle edges
  91 // - edges lazy evaluated to take advantage of early exits
  92 // - fabs precomputed (half less work, possible since extents are always >0)
  93 // - customized macros to take advantage of the null component
  94 // - axis vector discarded, possibly saves useless movs
  95 #define IMPLEMENT_CLASS3_TESTS                                          \
  96         float rad;                                                                              \
  97         float min, max;                                                                 \
  98                                                                                                         \
  99         const float fey0 = fabsf(e0.y);                                 \
 100         const float fez0 = fabsf(e0.z);                                 \
 101         AXISTEST_X01(e0.z, e0.y, fez0, fey0);                   \
 102         const float fex0 = fabsf(e0.x);                                 \
 103         AXISTEST_Y02(e0.z, e0.x, fez0, fex0);                   \
 104         AXISTEST_Z12(e0.y, e0.x, fey0, fex0);                   \
 105                                                                                                         \
 106         const float fey1 = fabsf(e1.y);                                 \
 107         const float fez1 = fabsf(e1.z);                                 \
 108         AXISTEST_X01(e1.z, e1.y, fez1, fey1);                   \
 109         const float fex1 = fabsf(e1.x);                                 \
 110         AXISTEST_Y02(e1.z, e1.x, fez1, fex1);                   \
 111         AXISTEST_Z0(e1.y, e1.x, fey1, fex1);                    \
 112                                                                                                         \
 113         const Point e2 = mLeafVerts[0] - mLeafVerts[2]; \
 114         const float fey2 = fabsf(e2.y);                                 \
 115         const float fez2 = fabsf(e2.z);                                 \
 116         AXISTEST_X2(e2.z, e2.y, fez2, fey2);                    \
 117         const float fex2 = fabsf(e2.x);                                 \
 118         AXISTEST_Y1(e2.z, e2.x, fez2, fex2);                    \
 119         AXISTEST_Z12(e2.y, e2.x, fey2, fex2);
 120
 121 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 122 /**
 123  *      Triangle-Box overlap test using the separating axis theorem.
 124  *      This is the code from Tomas Möller, a bit optimized:
 125  *      - with some more lazy evaluation (faster path on PC)
 126  *      - with a tiny bit of assembly
 127  *      - with "SAT-lite" applied if needed
 128  *      - and perhaps with some more minor modifs...
 129  *
 130  *      \param          center          [in] box center
 131  *      \param          extents         [in] box extents
 132  *      \return         true if triangle & box overlap
 133  */
 134 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 135 inline_ BOOL AABBTreeCollider::TriBoxOverlap(const Point& center, const Point& extents)
 136 {
 137         // Stats
 138         mNbBVPrimTests++;
 139
 140         // use separating axis theorem to test overlap between triangle and box
 141         // need to test for overlap in these directions:
 142         // 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
 143         //    we do not even need to test these)
 144         // 2) normal of the triangle
 145         // 3) crossproduct(edge from tri, {x,y,z}-directin)
 146         //    this gives 3x3=9 more tests
 147
 148         // move everything so that the boxcenter is in (0,0,0)
 149         Point v0, v1, v2;
 150         v0.x = mLeafVerts[0].x - center.x;
 151         v1.x = mLeafVerts[1].x - center.x;
 152         v2.x = mLeafVerts[2].x - center.x;
 153
 154         // First, test overlap in the {x,y,z}-directions
 155 #ifdef OPC_USE_FCOMI
 156         // find min, max of the triangle in x-direction, and test for overlap in X
 157         if(FCMin3(v0.x, v1.x, v2.x)>extents.x)  return FALSE;
 158         if(FCMax3(v0.x, v1.x, v2.x)<-extents.x) return FALSE;
 159
 160         // same for Y
 161         v0.y = mLeafVerts[0].y - center.y;
 162         v1.y = mLeafVerts[1].y - center.y;
 163         v2.y = mLeafVerts[2].y - center.y;
 164
 165         if(FCMin3(v0.y, v1.y, v2.y)>extents.y)  return FALSE;
 166         if(FCMax3(v0.y, v1.y, v2.y)<-extents.y) return FALSE;
 167
 168         // same for Z
 169         v0.z = mLeafVerts[0].z - center.z;
 170         v1.z = mLeafVerts[1].z - center.z;
 171         v2.z = mLeafVerts[2].z - center.z;
 172
 173         if(FCMin3(v0.z, v1.z, v2.z)>extents.z)  return FALSE;
 174         if(FCMax3(v0.z, v1.z, v2.z)<-extents.z) return FALSE;
 175 #else
 176         float min,max;
 177         // Find min, max of the triangle in x-direction, and test for overlap in X
 178         FINDMINMAX(v0.x, v1.x, v2.x, min, max);
 179         if(min>extents.x || max<-extents.x) return FALSE;
 180
 181         // Same for Y
 182         v0.y = mLeafVerts[0].y - center.y;
 183         v1.y = mLeafVerts[1].y - center.y;
 184         v2.y = mLeafVerts[2].y - center.y;
 185
 186         FINDMINMAX(v0.y, v1.y, v2.y, min, max);
 187         if(min>extents.y || max<-extents.y) return FALSE;
 188
 189         // Same for Z
 190         v0.z = mLeafVerts[0].z - center.z;
 191         v1.z = mLeafVerts[1].z - center.z;
 192         v2.z = mLeafVerts[2].z - center.z;
 193
 194         FINDMINMAX(v0.z, v1.z, v2.z, min, max);
 195         if(min>extents.z || max<-extents.z) return FALSE;
 196 #endif
 197         // 2) Test if the box intersects the plane of the triangle
 198         // compute plane equation of triangle: normal*x+d=0
 199         // ### could be precomputed since we use the same leaf triangle several times
 200         const Point e0 = v1 - v0;
 201         const Point e1 = v2 - v1;
 202         const Point normal = e0 ^ e1;
 203         const float d = -normal|v0;
 204         if(!planeBoxOverlap(normal, d, extents)) return FALSE;
 205
 206         // 3) "Class III" tests
 207         if(mFullPrimBoxTest)
 208         {
 209                 IMPLEMENT_CLASS3_TESTS
 210         }
 211         return TRUE;
 212 }
 213
 214 //! A dedicated version where the box is constant
 215 inline_ BOOL OBBCollider::TriBoxOverlap()
 216 {
 217         // Stats
 218         mNbVolumePrimTests++;
 219
 220         // Hook
 221         const Point& extents = mBoxExtents;
 222         const Point& v0 = mLeafVerts[0];
 223         const Point& v1 = mLeafVerts[1];
 224         const Point& v2 = mLeafVerts[2];
 225
 226         // use separating axis theorem to test overlap between triangle and box
 227         // need to test for overlap in these directions:
 228         // 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
 229         //    we do not even need to test these)
 230         // 2) normal of the triangle
 231         // 3) crossproduct(edge from tri, {x,y,z}-directin)
 232         //    this gives 3x3=9 more tests
 233
 234         // Box center is already in (0,0,0)
 235
 236         // First, test overlap in the {x,y,z}-directions
 237 #ifdef OPC_USE_FCOMI
 238         // find min, max of the triangle in x-direction, and test for overlap in X
 239         if(FCMin3(v0.x, v1.x, v2.x)>mBoxExtents.x)      return FALSE;
 240         if(FCMax3(v0.x, v1.x, v2.x)<-mBoxExtents.x)     return FALSE;
 241
 242         if(FCMin3(v0.y, v1.y, v2.y)>mBoxExtents.y)      return FALSE;
 243         if(FCMax3(v0.y, v1.y, v2.y)<-mBoxExtents.y)     return FALSE;
 244
 245         if(FCMin3(v0.z, v1.z, v2.z)>mBoxExtents.z)      return FALSE;
 246         if(FCMax3(v0.z, v1.z, v2.z)<-mBoxExtents.z)     return FALSE;
 247 #else
 248         float min,max;
 249         // Find min, max of the triangle in x-direction, and test for overlap in X
 250         FINDMINMAX(v0.x, v1.x, v2.x, min, max);
 251         if(min>mBoxExtents.x || max<-mBoxExtents.x) return FALSE;
 252
 253         FINDMINMAX(v0.y, v1.y, v2.y, min, max);
 254         if(min>mBoxExtents.y || max<-mBoxExtents.y) return FALSE;
 255
 256         FINDMINMAX(v0.z, v1.z, v2.z, min, max);
 257         if(min>mBoxExtents.z || max<-mBoxExtents.z) return FALSE;
 258 #endif
 259         // 2) Test if the box intersects the plane of the triangle
 260         // compute plane equation of triangle: normal*x+d=0
 261         // ### could be precomputed since we use the same leaf triangle several times
 262         const Point e0 = v1 - v0;
 263         const Point e1 = v2 - v1;
 264         const Point normal = e0 ^ e1;
 265         const float d = -normal|v0;
 266         if(!planeBoxOverlap(normal, d, mBoxExtents)) return FALSE;
 267
 268         // 3) "Class III" tests - here we always do full tests since the box is a primitive (not a BV)
 269         {
 270                 IMPLEMENT_CLASS3_TESTS
 271         }
 272         return TRUE;
 273 }
 274
 275 //! ...and another one, jeez
 276 inline_ BOOL AABBCollider::TriBoxOverlap()
 277 {
 278         // Stats
 279         mNbVolumePrimTests++;
 280
 281         // Hook
 282         const Point& center             = mBox.mCenter;
 283         const Point& extents    = mBox.mExtents;
 284
 285         // use separating axis theorem to test overlap between triangle and box
 286         // need to test for overlap in these directions:
 287         // 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
 288         //    we do not even need to test these)
 289         // 2) normal of the triangle
 290         // 3) crossproduct(edge from tri, {x,y,z}-directin)
 291         //    this gives 3x3=9 more tests
 292
 293         // move everything so that the boxcenter is in (0,0,0)
 294         Point v0, v1, v2;
 295         v0.x = mLeafVerts[0].x - center.x;
 296         v1.x = mLeafVerts[1].x - center.x;
 297         v2.x = mLeafVerts[2].x - center.x;
 298
 299         // First, test overlap in the {x,y,z}-directions
 300 #ifdef OPC_USE_FCOMI
 301         // find min, max of the triangle in x-direction, and test for overlap in X
 302         if(FCMin3(v0.x, v1.x, v2.x)>extents.x)  return FALSE;
 303         if(FCMax3(v0.x, v1.x, v2.x)<-extents.x) return FALSE;
 304
 305         // same for Y
 306         v0.y = mLeafVerts[0].y - center.y;
 307         v1.y = mLeafVerts[1].y - center.y;
 308         v2.y = mLeafVerts[2].y - center.y;
 309
 310         if(FCMin3(v0.y, v1.y, v2.y)>extents.y)  return FALSE;
 311         if(FCMax3(v0.y, v1.y, v2.y)<-extents.y) return FALSE;
 312
 313         // same for Z
 314         v0.z = mLeafVerts[0].z - center.z;
 315         v1.z = mLeafVerts[1].z - center.z;
 316         v2.z = mLeafVerts[2].z - center.z;
 317
 318         if(FCMin3(v0.z, v1.z, v2.z)>extents.z)  return FALSE;
 319         if(FCMax3(v0.z, v1.z, v2.z)<-extents.z) return FALSE;
 320 #else
 321         float min,max;
 322         // Find min, max of the triangle in x-direction, and test for overlap in X
 323         FINDMINMAX(v0.x, v1.x, v2.x, min, max);
 324         if(min>extents.x || max<-extents.x) return FALSE;
 325
 326         // Same for Y
 327         v0.y = mLeafVerts[0].y - center.y;
 328         v1.y = mLeafVerts[1].y - center.y;
 329         v2.y = mLeafVerts[2].y - center.y;
 330
 331         FINDMINMAX(v0.y, v1.y, v2.y, min, max);
 332         if(min>extents.y || max<-extents.y) return FALSE;
 333
 334         // Same for Z
 335         v0.z = mLeafVerts[0].z - center.z;
 336         v1.z = mLeafVerts[1].z - center.z;
 337         v2.z = mLeafVerts[2].z - center.z;
 338
 339         FINDMINMAX(v0.z, v1.z, v2.z, min, max);
 340         if(min>extents.z || max<-extents.z) return FALSE;
 341 #endif
 342         // 2) Test if the box intersects the plane of the triangle
 343         // compute plane equation of triangle: normal*x+d=0
 344         // ### could be precomputed since we use the same leaf triangle several times
 345         const Point e0 = v1 - v0;
 346         const Point e1 = v2 - v1;
 347         const Point normal = e0 ^ e1;
 348         const float d = -normal|v0;
 349         if(!planeBoxOverlap(normal, d, extents)) return FALSE;
 350
 351         // 3) "Class III" tests - here we always do full tests since the box is a primitive (not a BV)
 352         {
 353                 IMPLEMENT_CLASS3_TESTS
 354         }
 355         return TRUE;
 356 }