OPCODE/OPC_TriBoxOverlap.h

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