pcb 4.1.1
An interactive printed circuit board layout editor.

tribox3.c

Go to the documentation of this file.
00001 
00006 /********************************************************/
00007 /* AABB-triangle overlap test code                      */
00008 /* by Tomas Akenine-Möller                              */
00009 /* Function: int triBoxOverlap(float boxcenter[3],      */
00010 /*          float boxhalfsize[3],float triverts[3][3]); */
00011 /* History:                                             */
00012 /*   2001-03-05: released the code in its first version */
00013 /*   2001-06-18: changed the order of the tests, faster */
00014 /*                                                      */
00015 /* Acknowledgement: Many thanks to Pierre Terdiman for  */
00016 /* suggestions and discussions on how to optimize code. */
00017 /* Thanks to David Hunt for finding a ">="-bug!         */
00018 /********************************************************/
00019 #include <math.h>
00020 #include <stdio.h>
00021 
00022 #define X 0
00023 #define Y 1
00024 #define Z 2
00025 
00026 #define CROSS(dest,v1,v2) \
00027           dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
00028           dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
00029           dest[2]=v1[0]*v2[1]-v1[1]*v2[0]; 
00030 
00031 #define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])
00032 
00033 #define SUB(dest,v1,v2) \
00034           dest[0]=v1[0]-v2[0]; \
00035           dest[1]=v1[1]-v2[1]; \
00036           dest[2]=v1[2]-v2[2]; 
00037 
00038 #define FINDMINMAX(x0,x1,x2,min,max) \
00039   min = max = x0;   \
00040   if(x1<min) min=x1;\
00041   if(x1>max) max=x1;\
00042   if(x2<min) min=x2;\
00043   if(x2>max) max=x2;
00044 
00045 int planeBoxOverlap(double normal[3], double vert[3], double maxbox[3]) // -NJMP-
00046 {
00047   int q;
00048   double vmin[3],vmax[3],v;
00049   for(q=X;q<=Z;q++)
00050   {
00051     v=vert[q];                                  // -NJMP-
00052     if(normal[q]>0.0f)
00053     {
00054       vmin[q]=-maxbox[q] - v;   // -NJMP-
00055       vmax[q]= maxbox[q] - v;   // -NJMP-
00056     }
00057     else
00058     {
00059       vmin[q]= maxbox[q] - v;   // -NJMP-
00060       vmax[q]=-maxbox[q] - v;   // -NJMP-
00061     }
00062   }
00063   if(DOT(normal,vmin)>0.0f) return 0;   // -NJMP-
00064   if(DOT(normal,vmax)>=0.0f) return 1;  // -NJMP-
00065   
00066   return 0;
00067 }
00068 
00069 
00070 /*======================== X-tests ========================*/
00071 #define AXISTEST_X01(a, b, fa, fb)                         \
00072         p0 = a*v0[Y] - b*v0[Z];                            \
00073         p2 = a*v2[Y] - b*v2[Z];                            \
00074         if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
00075         rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z];   \
00076         if(min>rad || max<-rad) return 0;
00077 
00078 #define AXISTEST_X2(a, b, fa, fb)                          \
00079         p0 = a*v0[Y] - b*v0[Z];                            \
00080         p1 = a*v1[Y] - b*v1[Z];                            \
00081         if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
00082         rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z];   \
00083         if(min>rad || max<-rad) return 0;
00084 
00085 /*======================== Y-tests ========================*/
00086 #define AXISTEST_Y02(a, b, fa, fb)                         \
00087         p0 = -a*v0[X] + b*v0[Z];                           \
00088         p2 = -a*v2[X] + b*v2[Z];                           \
00089         if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
00090         rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z];   \
00091         if(min>rad || max<-rad) return 0;
00092 
00093 #define AXISTEST_Y1(a, b, fa, fb)                          \
00094         p0 = -a*v0[X] + b*v0[Z];                           \
00095         p1 = -a*v1[X] + b*v1[Z];                           \
00096         if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
00097         rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z];   \
00098         if(min>rad || max<-rad) return 0;
00099 
00100 /*======================== Z-tests ========================*/
00101 
00102 #define AXISTEST_Z12(a, b, fa, fb)                         \
00103         p1 = a*v1[X] - b*v1[Y];                            \
00104         p2 = a*v2[X] - b*v2[Y];                            \
00105         if(p2<p1) {min=p2; max=p1;} else {min=p1; max=p2;} \
00106         rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y];   \
00107         if(min>rad || max<-rad) return 0;
00108 
00109 #define AXISTEST_Z0(a, b, fa, fb)                          \
00110         p0 = a*v0[X] - b*v0[Y];                            \
00111         p1 = a*v1[X] - b*v1[Y];                            \
00112         if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
00113         rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y];   \
00114         if(min>rad || max<-rad) return 0;
00115 
00116 int triBoxOverlap(double boxcenter[3],double boxhalfsize[3],double triverts[3][3])
00117 {
00118 
00119   /*    use separating axis theorem to test overlap between triangle and box */
00120   /*    need to test for overlap in these directions: */
00121   /*    1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */
00122   /*       we do not even need to test these) */
00123   /*    2) normal of the triangle */
00124   /*    3) crossproduct(edge from tri, {x,y,z}-directin) */
00125   /*       this gives 3x3=9 more tests */
00126    double v0[3],v1[3],v2[3];
00127 //   double axis[3];
00128    double min,max,p0,p1,p2,rad,fex,fey,fez;             // -NJMP- "d" local variable removed
00129    double normal[3],e0[3],e1[3],e2[3];
00130 
00131    /* This is the fastest branch on Sun */
00132    /* move everything so that the boxcenter is in (0,0,0) */
00133    SUB(v0,triverts[0],boxcenter);
00134    SUB(v1,triverts[1],boxcenter);
00135    SUB(v2,triverts[2],boxcenter);
00136 
00137    /* compute triangle edges */
00138    SUB(e0,v1,v0);      /* tri edge 0 */
00139    SUB(e1,v2,v1);      /* tri edge 1 */
00140    SUB(e2,v0,v2);      /* tri edge 2 */
00141 
00142    /* Bullet 3:  */
00143    /*  test the 9 tests first (this was faster) */
00144    fex = fabsf(e0[X]);
00145    fey = fabsf(e0[Y]);
00146    fez = fabsf(e0[Z]);
00147    AXISTEST_X01(e0[Z], e0[Y], fez, fey);
00148    AXISTEST_Y02(e0[Z], e0[X], fez, fex);
00149    AXISTEST_Z12(e0[Y], e0[X], fey, fex);
00150 
00151    fex = fabsf(e1[X]);
00152    fey = fabsf(e1[Y]);
00153    fez = fabsf(e1[Z]);
00154    AXISTEST_X01(e1[Z], e1[Y], fez, fey);
00155    AXISTEST_Y02(e1[Z], e1[X], fez, fex);
00156    AXISTEST_Z0(e1[Y], e1[X], fey, fex);
00157 
00158    fex = fabsf(e2[X]);
00159    fey = fabsf(e2[Y]);
00160    fez = fabsf(e2[Z]);
00161    AXISTEST_X2(e2[Z], e2[Y], fez, fey);
00162    AXISTEST_Y1(e2[Z], e2[X], fez, fex);
00163    AXISTEST_Z12(e2[Y], e2[X], fey, fex);
00164 
00165    /* Bullet 1: */
00166    /*  first test overlap in the {x,y,z}-directions */
00167    /*  find min, max of the triangle each direction, and test for overlap in */
00168    /*  that direction -- this is equivalent to testing a minimal AABB around */
00169    /*  the triangle against the AABB */
00170 
00171    /* test in X-direction */
00172    FINDMINMAX(v0[X],v1[X],v2[X],min,max);
00173    if(min>boxhalfsize[X] || max<-boxhalfsize[X]) return 0;
00174 
00175    /* test in Y-direction */
00176    FINDMINMAX(v0[Y],v1[Y],v2[Y],min,max);
00177    if(min>boxhalfsize[Y] || max<-boxhalfsize[Y]) return 0;
00178 
00179    /* test in Z-direction */
00180    FINDMINMAX(v0[Z],v1[Z],v2[Z],min,max);
00181    if(min>boxhalfsize[Z] || max<-boxhalfsize[Z]) return 0;
00182 
00183    /* Bullet 2: */
00184    /*  test if the box intersects the plane of the triangle */
00185    /*  compute plane equation of triangle: normal*x+d=0 */
00186    CROSS(normal,e0,e1);
00187    // -NJMP- (line removed here)
00188    if(!planeBoxOverlap(normal,v0,boxhalfsize)) return 0;        // -NJMP-
00189 
00190    return 1;   /* box and triangle overlaps */
00191 }
00192