OPC_TriBoxOverlap.h
Go to the documentation of this file.
00001 
00003 #define FINDMINMAX(x0, x1, x2, min, max)        \
00004         min = max = x0;                                                 \
00005         if(x1<min) min=x1;                                              \
00006         if(x1>max) max=x1;                                              \
00007         if(x2<min) min=x2;                                              \
00008         if(x2>max) max=x2;
00009 
00011 inline_ BOOL planeBoxOverlap(const Point& normal, const float d, const Point& maxbox)
00012 {
00013         Point vmin, vmax;
00014         for(udword q=0;q<=2;q++)
00015         {
00016                 if(normal[q]>0.0f)      { vmin[q]=-maxbox[q]; vmax[q]=maxbox[q]; }
00017                 else                            { vmin[q]=maxbox[q]; vmax[q]=-maxbox[q]; }
00018         }
00019         if((normal|vmin)+d>0.0f) return FALSE;
00020         if((normal|vmax)+d>=0.0f) return TRUE;
00021 
00022         return FALSE;
00023 }
00024 
00026 #define AXISTEST_X01(a, b, fa, fb)                                                      \
00027         min = a*v0.y - b*v0.z;                                                                  \
00028         max = a*v2.y - b*v2.z;                                                                  \
00029         if(min>max) {const float tmp=max; max=min; min=tmp;     }       \
00030         rad = fa * extents.y + fb * extents.z;                                  \
00031         if(min>rad || max<-rad) return FALSE;
00032 
00034 #define AXISTEST_X2(a, b, fa, fb)                                                       \
00035         min = a*v0.y - b*v0.z;                                                                  \
00036         max = a*v1.y - b*v1.z;                                                                  \
00037         if(min>max) {const float tmp=max; max=min; min=tmp;     }       \
00038         rad = fa * extents.y + fb * extents.z;                                  \
00039         if(min>rad || max<-rad) return FALSE;
00040 
00042 #define AXISTEST_Y02(a, b, fa, fb)                                                      \
00043         min = b*v0.z - a*v0.x;                                                                  \
00044         max = b*v2.z - a*v2.x;                                                                  \
00045         if(min>max) {const float tmp=max; max=min; min=tmp;     }       \
00046         rad = fa * extents.x + fb * extents.z;                                  \
00047         if(min>rad || max<-rad) return FALSE;
00048 
00050 #define AXISTEST_Y1(a, b, fa, fb)                                                       \
00051         min = b*v0.z - a*v0.x;                                                                  \
00052         max = b*v1.z - a*v1.x;                                                                  \
00053         if(min>max) {const float tmp=max; max=min; min=tmp;     }       \
00054         rad = fa * extents.x + fb * extents.z;                                  \
00055         if(min>rad || max<-rad) return FALSE;
00056 
00058 #define AXISTEST_Z12(a, b, fa, fb)                                                      \
00059         min = a*v1.x - b*v1.y;                                                                  \
00060         max = a*v2.x - b*v2.y;                                                                  \
00061         if(min>max) {const float tmp=max; max=min; min=tmp;     }       \
00062         rad = fa * extents.x + fb * extents.y;                                  \
00063         if(min>rad || max<-rad) return FALSE;
00064 
00066 #define AXISTEST_Z0(a, b, fa, fb)                                                       \
00067         min = a*v0.x - b*v0.y;                                                                  \
00068         max = a*v1.x - b*v1.y;                                                                  \
00069         if(min>max) {const float tmp=max; max=min; min=tmp;     }       \
00070         rad = fa * extents.x + fb * extents.y;                                  \
00071         if(min>rad || max<-rad) return FALSE;
00072 
00073 // compute triangle edges
00074 // - edges lazy evaluated to take advantage of early exits
00075 // - fabs precomputed (half less work, possible since extents are always >0)
00076 // - customized macros to take advantage of the null component
00077 // - axis vector discarded, possibly saves useless movs
00078 #define IMPLEMENT_CLASS3_TESTS                                          \
00079         float rad;                                                                              \
00080         float min, max;                                                                 \
00081                                                                                                         \
00082         const float fey0 = fabsf(e0.y);                                 \
00083         const float fez0 = fabsf(e0.z);                                 \
00084         AXISTEST_X01(e0.z, e0.y, fez0, fey0);                   \
00085         const float fex0 = fabsf(e0.x);                                 \
00086         AXISTEST_Y02(e0.z, e0.x, fez0, fex0);                   \
00087         AXISTEST_Z12(e0.y, e0.x, fey0, fex0);                   \
00088                                                                                                         \
00089         const float fey1 = fabsf(e1.y);                                 \
00090         const float fez1 = fabsf(e1.z);                                 \
00091         AXISTEST_X01(e1.z, e1.y, fez1, fey1);                   \
00092         const float fex1 = fabsf(e1.x);                                 \
00093         AXISTEST_Y02(e1.z, e1.x, fez1, fex1);                   \
00094         AXISTEST_Z0(e1.y, e1.x, fey1, fex1);                    \
00095                                                                                                         \
00096         const Point e2 = mLeafVerts[0] - mLeafVerts[2]; \
00097         const float fey2 = fabsf(e2.y);                                 \
00098         const float fez2 = fabsf(e2.z);                                 \
00099         AXISTEST_X2(e2.z, e2.y, fez2, fey2);                    \
00100         const float fex2 = fabsf(e2.x);                                 \
00101         AXISTEST_Y1(e2.z, e2.x, fez2, fex2);                    \
00102         AXISTEST_Z12(e2.y, e2.x, fey2, fex2);
00103 
00105 
00117 
00118 inline_ BOOL AABBTreeCollider::TriBoxOverlap(const Point& center, const Point& extents)
00119 {
00120         // Stats
00121         mNbBVPrimTests++;
00122 
00123         // use separating axis theorem to test overlap between triangle and box 
00124         // need to test for overlap in these directions: 
00125         // 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle 
00126         //    we do not even need to test these) 
00127         // 2) normal of the triangle 
00128         // 3) crossproduct(edge from tri, {x,y,z}-directin) 
00129         //    this gives 3x3=9 more tests 
00130 
00131         // move everything so that the boxcenter is in (0,0,0) 
00132         Point v0, v1, v2;
00133         v0.x = mLeafVerts[0].x - center.x;
00134         v1.x = mLeafVerts[1].x - center.x;
00135         v2.x = mLeafVerts[2].x - center.x;
00136 
00137         // First, test overlap in the {x,y,z}-directions
00138 #ifdef OPC_USE_FCOMI
00139         // find min, max of the triangle in x-direction, and test for overlap in X
00140         if(FCMin3(v0.x, v1.x, v2.x)>extents.x)  return FALSE;
00141         if(FCMax3(v0.x, v1.x, v2.x)<-extents.x) return FALSE;
00142 
00143         // same for Y
00144         v0.y = mLeafVerts[0].y - center.y;
00145         v1.y = mLeafVerts[1].y - center.y;
00146         v2.y = mLeafVerts[2].y - center.y;
00147 
00148         if(FCMin3(v0.y, v1.y, v2.y)>extents.y)  return FALSE;
00149         if(FCMax3(v0.y, v1.y, v2.y)<-extents.y) return FALSE;
00150 
00151         // same for Z
00152         v0.z = mLeafVerts[0].z - center.z;
00153         v1.z = mLeafVerts[1].z - center.z;
00154         v2.z = mLeafVerts[2].z - center.z;
00155 
00156         if(FCMin3(v0.z, v1.z, v2.z)>extents.z)  return FALSE;
00157         if(FCMax3(v0.z, v1.z, v2.z)<-extents.z) return FALSE;
00158 #else
00159         float min,max;
00160         // Find min, max of the triangle in x-direction, and test for overlap in X
00161         FINDMINMAX(v0.x, v1.x, v2.x, min, max);
00162         if(min>extents.x || max<-extents.x) return FALSE;
00163 
00164         // Same for Y
00165         v0.y = mLeafVerts[0].y - center.y;
00166         v1.y = mLeafVerts[1].y - center.y;
00167         v2.y = mLeafVerts[2].y - center.y;
00168 
00169         FINDMINMAX(v0.y, v1.y, v2.y, min, max);
00170         if(min>extents.y || max<-extents.y) return FALSE;
00171 
00172         // Same for Z
00173         v0.z = mLeafVerts[0].z - center.z;
00174         v1.z = mLeafVerts[1].z - center.z;
00175         v2.z = mLeafVerts[2].z - center.z;
00176 
00177         FINDMINMAX(v0.z, v1.z, v2.z, min, max);
00178         if(min>extents.z || max<-extents.z) return FALSE;
00179 #endif
00180         // 2) Test if the box intersects the plane of the triangle
00181         // compute plane equation of triangle: normal*x+d=0
00182         // ### could be precomputed since we use the same leaf triangle several times
00183         const Point e0 = v1 - v0;
00184         const Point e1 = v2 - v1;
00185         const Point normal = e0 ^ e1;
00186         const float d = -normal|v0;
00187         if(!planeBoxOverlap(normal, d, extents)) return FALSE;
00188 
00189         // 3) "Class III" tests
00190         if(mFullPrimBoxTest)
00191         {
00192                 IMPLEMENT_CLASS3_TESTS
00193         }
00194         return TRUE;
00195 }
00196 
00198 inline_ BOOL OBBCollider::TriBoxOverlap()
00199 {
00200         // Stats
00201         mNbVolumePrimTests++;
00202 
00203         // Hook
00204         const Point& extents = mBoxExtents;
00205         const Point& v0 = mLeafVerts[0];
00206         const Point& v1 = mLeafVerts[1];
00207         const Point& v2 = mLeafVerts[2];
00208 
00209         // use separating axis theorem to test overlap between triangle and box 
00210         // need to test for overlap in these directions: 
00211         // 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle 
00212         //    we do not even need to test these) 
00213         // 2) normal of the triangle 
00214         // 3) crossproduct(edge from tri, {x,y,z}-directin) 
00215         //    this gives 3x3=9 more tests 
00216 
00217         // Box center is already in (0,0,0)
00218 
00219         // First, test overlap in the {x,y,z}-directions
00220 #ifdef OPC_USE_FCOMI
00221         // find min, max of the triangle in x-direction, and test for overlap in X
00222         if(FCMin3(v0.x, v1.x, v2.x)>mBoxExtents.x)      return FALSE;
00223         if(FCMax3(v0.x, v1.x, v2.x)<-mBoxExtents.x)     return FALSE;
00224 
00225         if(FCMin3(v0.y, v1.y, v2.y)>mBoxExtents.y)      return FALSE;
00226         if(FCMax3(v0.y, v1.y, v2.y)<-mBoxExtents.y)     return FALSE;
00227 
00228         if(FCMin3(v0.z, v1.z, v2.z)>mBoxExtents.z)      return FALSE;
00229         if(FCMax3(v0.z, v1.z, v2.z)<-mBoxExtents.z)     return FALSE;
00230 #else
00231         float min,max;
00232         // Find min, max of the triangle in x-direction, and test for overlap in X
00233         FINDMINMAX(v0.x, v1.x, v2.x, min, max);
00234         if(min>mBoxExtents.x || max<-mBoxExtents.x) return FALSE;
00235 
00236         FINDMINMAX(v0.y, v1.y, v2.y, min, max);
00237         if(min>mBoxExtents.y || max<-mBoxExtents.y) return FALSE;
00238 
00239         FINDMINMAX(v0.z, v1.z, v2.z, min, max);
00240         if(min>mBoxExtents.z || max<-mBoxExtents.z) return FALSE;
00241 #endif
00242         // 2) Test if the box intersects the plane of the triangle
00243         // compute plane equation of triangle: normal*x+d=0
00244         // ### could be precomputed since we use the same leaf triangle several times
00245         const Point e0 = v1 - v0;
00246         const Point e1 = v2 - v1;
00247         const Point normal = e0 ^ e1;
00248         const float d = -normal|v0;
00249         if(!planeBoxOverlap(normal, d, mBoxExtents)) return FALSE;
00250 
00251         // 3) "Class III" tests - here we always do full tests since the box is a primitive (not a BV)
00252         {
00253                 IMPLEMENT_CLASS3_TESTS
00254         }
00255         return TRUE;
00256 }
00257 
00259 inline_ BOOL AABBCollider::TriBoxOverlap()
00260 {
00261         // Stats
00262         mNbVolumePrimTests++;
00263 
00264         // Hook
00265         const Point& center             = mBox.mCenter;
00266         const Point& extents    = mBox.mExtents;
00267 
00268         // use separating axis theorem to test overlap between triangle and box 
00269         // need to test for overlap in these directions: 
00270         // 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle 
00271         //    we do not even need to test these) 
00272         // 2) normal of the triangle 
00273         // 3) crossproduct(edge from tri, {x,y,z}-directin) 
00274         //    this gives 3x3=9 more tests 
00275 
00276         // move everything so that the boxcenter is in (0,0,0) 
00277         Point v0, v1, v2;
00278         v0.x = mLeafVerts[0].x - center.x;
00279         v1.x = mLeafVerts[1].x - center.x;
00280         v2.x = mLeafVerts[2].x - center.x;
00281 
00282         // First, test overlap in the {x,y,z}-directions
00283 #ifdef OPC_USE_FCOMI
00284         // find min, max of the triangle in x-direction, and test for overlap in X
00285         if(FCMin3(v0.x, v1.x, v2.x)>extents.x)  return FALSE;
00286         if(FCMax3(v0.x, v1.x, v2.x)<-extents.x) return FALSE;
00287 
00288         // same for Y
00289         v0.y = mLeafVerts[0].y - center.y;
00290         v1.y = mLeafVerts[1].y - center.y;
00291         v2.y = mLeafVerts[2].y - center.y;
00292 
00293         if(FCMin3(v0.y, v1.y, v2.y)>extents.y)  return FALSE;
00294         if(FCMax3(v0.y, v1.y, v2.y)<-extents.y) return FALSE;
00295 
00296         // same for Z
00297         v0.z = mLeafVerts[0].z - center.z;
00298         v1.z = mLeafVerts[1].z - center.z;
00299         v2.z = mLeafVerts[2].z - center.z;
00300 
00301         if(FCMin3(v0.z, v1.z, v2.z)>extents.z)  return FALSE;
00302         if(FCMax3(v0.z, v1.z, v2.z)<-extents.z) return FALSE;
00303 #else
00304         float min,max;
00305         // Find min, max of the triangle in x-direction, and test for overlap in X
00306         FINDMINMAX(v0.x, v1.x, v2.x, min, max);
00307         if(min>extents.x || max<-extents.x) return FALSE;
00308 
00309         // Same for Y
00310         v0.y = mLeafVerts[0].y - center.y;
00311         v1.y = mLeafVerts[1].y - center.y;
00312         v2.y = mLeafVerts[2].y - center.y;
00313 
00314         FINDMINMAX(v0.y, v1.y, v2.y, min, max);
00315         if(min>extents.y || max<-extents.y) return FALSE;
00316 
00317         // Same for Z
00318         v0.z = mLeafVerts[0].z - center.z;
00319         v1.z = mLeafVerts[1].z - center.z;
00320         v2.z = mLeafVerts[2].z - center.z;
00321 
00322         FINDMINMAX(v0.z, v1.z, v2.z, min, max);
00323         if(min>extents.z || max<-extents.z) return FALSE;
00324 #endif
00325         // 2) Test if the box intersects the plane of the triangle
00326         // compute plane equation of triangle: normal*x+d=0
00327         // ### could be precomputed since we use the same leaf triangle several times
00328         const Point e0 = v1 - v0;
00329         const Point e1 = v2 - v1;
00330         const Point normal = e0 ^ e1;
00331         const float d = -normal|v0;
00332         if(!planeBoxOverlap(normal, d, extents)) return FALSE;
00333 
00334         // 3) "Class III" tests - here we always do full tests since the box is a primitive (not a BV)
00335         {
00336                 IMPLEMENT_CLASS3_TESTS
00337         }
00338         return TRUE;
00339 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:56