三角形与立方体相交

 /********************************************************/ /************************************************* *******/ 
 /* AABB-triangle overlap test code                      */ /* AABB-triangle overlap test code */ 
 /* by Tomas Akenine-Möller                              */ /* by Tomas Akenine-Möller */ 
 /* Function: int triBoxOverlap(float boxcenter[3],      */ /* Function: int triBoxOverlap(float boxcenter[3], */ 
 /*          float boxhalfsize[3],float triverts[3][3]); */ /* float boxhalfsize[3],float triverts[3][3]); */ 
 /* History:                                             */ /* History: */ 
 /*   2001-03-05: released the code in its first version */ /* 2001-03-05: released the code in its first version */ 
 /*   2001-06-18: changed the order of the tests, faster */ /* 2001-06-18: changed the order of the tests, faster */ 
 /*                                                      */ /* */ 
 /* Acknowledgement: Many thanks to Pierre Terdiman for  */ /* Acknowledgement: Many thanks to Pierre Terdiman for */ 
 /* suggestions and discussions on how to optimize code. */ /* suggestions and discussions on how to optimize code. */ 
 /* Thanks to David Hunt for finding a ">="-bug!         */ /* Thanks to David Hunt for finding a ">="-bug! */ 
 /********************************************************/ /************************************************* *******/ 
 #include <math.h> #include <math.h> 
 #include <stdio.h> #include <stdio.h> 

 #define X 0 #define X 0 
 #define Y 1 #define Y 1 
 #define Z 2 #define Z 2 
 #define CROSS(dest,v1,v2) / #define CROSS(dest,v1,v2) / 
           dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; / dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; / 
           dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; / dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; / 
           dest[2]=v1[0]*v2[1]-v1[1]*v2[0]; dest[2]=v1[0]*v2[1]-v1[1]*v2[0];  

 #define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2]) #define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2]) 

 #define SUB(dest,v1,v2) / #define SUB(dest,v1,v2) / 
           dest[0]=v1[0]-v2[0]; / dest[0]=v1[0]-v2[0]; / 
           dest[1]=v1[1]-v2[1]; / dest[1]=v1[1]-v2[1]; / 
           dest[2]=v1[2]-v2[2]; dest[2]=v1[2]-v2[2];  

 #define FINDMINMAX(x0,x1,x2,min,max) / #define FINDMINMAX(x0,x1,x2,min,max) / 
   min = max = x0;   / min = max = x0; / 
   if(x1<min) min=x1;/ if(x1<min) min=x1;/ 
   if(x1>max) max=x1;/ if(x1>max) max=x1;/ 
   if(x2<min) min=x2;/ if(x2<min) min=x2;/ 
   if(x2>max) max=x2; if(x2>max) max=x2; 

 int planeBoxOverlap(float normal[3],float d, float maxbox[3]) int planeBoxOverlap(float normal[3],float d, float maxbox[3]) 
 { { 
   int q; int q; 
   float vmin[3],vmax[3]; float vmin[3],vmax[3]; 
   for(q=X;q<=Z;q++) for(q=X;q<=Z;q++) 
   { { 
     if(normal[q]>0.0f) if(normal[q]>0.0f) 
     { { 
       vmin[q]=-maxbox[q]; vmin[q]=-maxbox[q]; 
       vmax[q]=maxbox[q]; vmax[q]=maxbox[q]; 
     } } 
     else else 
     { { 
       vmin[q]=maxbox[q]; vmin[q]=maxbox[q]; 
       vmax[q]=-maxbox[q]; vmax[q]=-maxbox[q]; 
     } } 
   } } 
   if(DOT(normal,vmin)+d>0.0f) return 0; if(DOT(normal,vmin)+d>0.0f) return 0; 
   if(DOT(normal,vmax)+d>=0.0f) return 1; if(DOT(normal,vmax)+d>=0.0f) return 1; 
  
   return 0; return 0; 
 } } 


 /*======================== X-tests ========================*/ /*======================== X-tests ===================== ===*/ 
 #define AXISTEST_X01(a, b, fa, fb)      / #define AXISTEST_X01(a, b, fa, fb) / 
	 p0 = a*v0[Y] - b*v0[Z];              / p0 = a*v0[Y] - b*v0[Z]; / 
	 p2 = a*v2[Y] - b*v2[Z];              / p2 = a*v2[Y] - b*v2[Z]; / 
         if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} / if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} / 
	 rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z];   / rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z]; / 
	 if(min>rad || max<-rad) return 0; if(min>rad || max<-rad) return 0; 

 #define AXISTEST_X2(a, b, fa, fb)      / #define AXISTEST_X2(a, b, fa, fb) / 
	 p0 = a*v0[Y] - b*v0[Z];              / p0 = a*v0[Y] - b*v0[Z]; / 
	 p1 = a*v1[Y] - b*v1[Z];              / p1 = a*v1[Y] - b*v1[Z]; / 
         if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} / if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} / 
	 rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z];   / rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z]; / 
	 if(min>rad || max<-rad) return 0; if(min>rad || max<-rad) return 0; 

 /*======================== Y-tests ========================*/ /*======================== Y-tests ===================== ===*/ 
 #define AXISTEST_Y02(a, b, fa, fb)      / #define AXISTEST_Y02(a, b, fa, fb) / 
	 p0 = -a*v0[X] + b*v0[Z];            / p0 = -a*v0[X] + b*v0[Z]; / 
	 p2 = -a*v2[X] + b*v2[Z];                    / p2 = -a*v2[X] + b*v2[Z]; / 
         if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} / if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} / 
	 rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z];   / rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z]; / 
	 if(min>rad || max<-rad) return 0; if(min>rad || max<-rad) return 0; 

 #define AXISTEST_Y1(a, b, fa, fb)      / #define AXISTEST_Y1(a, b, fa, fb) / 
	 p0 = -a*v0[X] + b*v0[Z];            / p0 = -a*v0[X] + b*v0[Z]; / 
	 p1 = -a*v1[X] + b*v1[Z];                  / p1 = -a*v1[X] + b*v1[Z]; / 
         if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} / if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} / 
	 rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z];   / rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z]; / 
	 if(min>rad || max<-rad) return 0; if(min>rad || max<-rad) return 0; 

 /*======================== Z-tests ========================*/ /*======================== Z-tests ===================== ===*/ 

 #define AXISTEST_Z12(a, b, fa, fb)      / #define AXISTEST_Z12(a, b, fa, fb) / 
	 p1 = a*v1[X] - b*v1[Y];              / p1 = a*v1[X] - b*v1[Y]; / 
	 p2 = a*v2[X] - b*v2[Y];              / p2 = a*v2[X] - b*v2[Y]; / 
         if(p2<p1) {min=p2; max=p1;} else {min=p1; max=p2;} / if(p2<p1) {min=p2; max=p1;} else {min=p1; max=p2;} / 
	 rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y];   / rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y]; / 
	 if(min>rad || max<-rad) return 0; if(min>rad || max<-rad) return 0; 

 #define AXISTEST_Z0(a, b, fa, fb)      / #define AXISTEST_Z0(a, b, fa, fb) / 
	 p0 = a*v0[X] - b*v0[Y];       / p0 = a*v0[X] - b*v0[Y]; / 
	 p1 = a*v1[X] - b*v1[Y];              / p1 = a*v1[X] - b*v1[Y]; / 
         if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} / if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} / 
	 rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y];   / rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y]; / 
	 if(min>rad || max<-rad) return 0; if(min>rad || max<-rad) return 0; 

 int triBoxOverlap(float boxcenter[3],float boxhalfsize[3],float triverts[3][3]) int triBoxOverlap(float boxcenter[3],float boxhalfsize[3],float triverts[3][3]) 
 { { 

   /*    use separating axis theorem to test overlap between triangle and box */ /* use separating axis theorem to test overlap between triangle and box */ 
   /*    need to test for overlap in these directions: */ /* need to test for overlap in these directions: */ 
   /*    1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */ /* 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */ 
   /*       we do not even need to test these) */ /* we do not even need to test these) */ 
   /*    2) normal of the triangle */ /* 2) normal of the triangle */ 
   /*    3) crossproduct(edge from tri, {x,y,z}-directin) */ /* 3) crossproduct(edge from tri, {x,y,z}-directin) */ 
   /*       this gives 3x3=9 more tests */ /* this gives 3x3=9 more tests */ 
    float v0[3],v1[3],v2[3]; float v0[3],v1[3],v2[3]; 
    float axis[3]; float axis[3]; 
    float min,max,d,p0,p1,p2,rad,fex,fey,fez; float min,max,d,p0,p1,p2,rad,fex,fey,fez;   
    float normal[3],e0[3],e1[3],e2[3]; float normal[3],e0[3],e1[3],e2[3]; 

    /* This is the fastest branch on Sun */ /* This is the fastest branch on Sun */ 
    /* move everything so that the boxcenter is in (0,0,0) */ /* move everything so that the boxcenter is in (0,0,0) */ 
    SUB(v0,triverts[0],boxcenter); SUB(v0,triverts[0],boxcenter); 
    SUB(v1,triverts[1],boxcenter); SUB(v1,triverts[1],boxcenter); 
    SUB(v2,triverts[2],boxcenter); SUB(v2,triverts[2],boxcenter); 

    /* compute triangle edges */ /* compute triangle edges */ 
    SUB(e0,v1,v0); /* tri edge 0 */ SUB(e0,v1,v0); /* tri edge 0 */ 
    SUB(e1,v2,v1); /* tri edge 1 */ SUB(e1,v2,v1); /* tri edge 1 */ 
    SUB(e2,v0,v2); /* tri edge 2 */ SUB(e2,v0,v2); /* tri edge 2 */ 

    /* Bullet 3:  */ /* Bullet 3: */ 
    /*  test the 9 tests first (this was faster) */ /* test the 9 tests first (this was faster) */ 
    fex = fabs(e0[X]); fex = fabs(e0[X]); 
    fey = fabs(e0[Y]); fey = fabs(e0[Y]); 
    fez = fabs(e0[Z]); fez = fabs(e0[Z]); 
    AXISTEST_X01(e0[Z], e0[Y], fez, fey); AXISTEST_X01(e0[Z], e0[Y], fez, fey); 
    AXISTEST_Y02(e0[Z], e0[X], fez, fex); AXISTEST_Y02(e0[Z], e0[X], fez, fex); 
    AXISTEST_Z12(e0[Y], e0[X], fey, fex); AXISTEST_Z12(e0[Y], e0[X], fey, fex); 

    fex = fabs(e1[X]); fex = fabs(e1[X]); 
    fey = fabs(e1[Y]); fey = fabs(e1[Y]); 
    fez = fabs(e1[Z]); fez = fabs(e1[Z]); 
    AXISTEST_X01(e1[Z], e1[Y], fez, fey); AXISTEST_X01(e1[Z], e1[Y], fez, fey); 
    AXISTEST_Y02(e1[Z], e1[X], fez, fex); AXISTEST_Y02(e1[Z], e1[X], fez, fex); 
    AXISTEST_Z0(e1[Y], e1[X], fey, fex); AXISTEST_Z0(e1[Y], e1[X], fey, fex); 

    fex = fabs(e2[X]); fex = fabs(e2[X]); 
    fey = fabs(e2[Y]); fey = fabs(e2[Y]); 
    fez = fabs(e2[Z]); fez = fabs(e2[Z]); 
    AXISTEST_X2(e2[Z], e2[Y], fez, fey); AXISTEST_X2(e2[Z], e2[Y], fez, fey); 
    AXISTEST_Y1(e2[Z], e2[X], fez, fex); AXISTEST_Y1(e2[Z], e2[X], fez, fex); 
    AXISTEST_Z12(e2[Y], e2[X], fey, fex); AXISTEST_Z12(e2[Y], e2[X], fey, fex); 

    /* Bullet 1: */ /* Bullet 1: */ 
    /*  first test overlap in the {x,y,z}-directions */ /* first test overlap in the {x,y,z}-directions */ 
    /*  find min, max of the triangle each direction, and test for overlap in */ /* find min, max of the triangle each direction, and test for overlap in */ 
    /*  that direction -- this is equivalent to testing a minimal AABB around */ /* that direction -- this is equivalent to testing a minimal AABB around */ 
    /*  the triangle against the AABB */ /* the triangle against the AABB */ 

    /* test in X-direction */ /* test in X-direction */ 
    FINDMINMAX(v0[X],v1[X],v2[X],min,max); FINDMINMAX(v0[X],v1[X],v2[X],min,max); 
    if(min>boxhalfsize[X] || max<-boxhalfsize[X]) return 0; if(min>boxhalfsize[X] || max<-boxhalfsize[X]) return 0; 

    /* test in Y-direction */ /* test in Y-direction */ 
    FINDMINMAX(v0[Y],v1[Y],v2[Y],min,max); FINDMINMAX(v0[Y],v1[Y],v2[Y],min,max); 
    if(min>boxhalfsize[Y] || max<-boxhalfsize[Y]) return 0; if(min>boxhalfsize[Y] || max<-boxhalfsize[Y]) return 0; 

    /* test in Z-direction */ /* test in Z-direction */ 
    FINDMINMAX(v0[Z],v1[Z],v2[Z],min,max); FINDMINMAX(v0[Z],v1[Z],v2[Z],min,max); 
    if(min>boxhalfsize[Z] || max<-boxhalfsize[Z]) return 0; if(min>boxhalfsize[Z] || max<-boxhalfsize[Z]) return 0; 

    /* Bullet 2: */ /* Bullet 2: */ 
    /*  test if the box intersects the plane of the triangle */ /* test if the box intersects the plane of the triangle */ 
    /*  compute plane equation of triangle: normal*x+d=0 */ /* compute plane equation of triangle: normal*x+d=0 */ 
    CROSS(normal,e0,e1); CROSS(normal,e0,e1); 
    d=-DOT(normal,v0); /* plane eq: normal.x+d=0 */ d=-DOT(normal,v0); /* plane eq: normal.x+d=0 */ 
    if(!planeBoxOverlap(normal,d,boxhalfsize)) return 0; if(!planeBoxOverlap(normal,d,boxhalfsize)) return 0; 

    return 1; /* box and triangle overlaps */ return 1; /* box and triangle overlaps */ 
 } }