00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include <math.h>
00016
00017
00018
00019
00020
00021
00022 #define USE_EPSILON_TEST TRUE
00023 #define EPSILON 0.000001
00024
00025
00026
00027 #define CROSS(dest,v1,v2) \
00028 dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
00029 dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
00030 dest[2]=v1[0]*v2[1]-v1[1]*v2[0];
00031
00032 #define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])
00033
00034 #define SUB(dest,v1,v2) \
00035 dest[0]=v1[0]-v2[0]; \
00036 dest[1]=v1[1]-v2[1]; \
00037 dest[2]=v1[2]-v2[2];
00038
00039
00040 #define SORT(a,b) \
00041 if(a>b) \
00042 { \
00043 float c; \
00044 c=a; \
00045 a=b; \
00046 b=c; \
00047 }
00048
00049 #define ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1) \
00050 isect0=VV0+(VV1-VV0)*D0/(D0-D1); \
00051 isect1=VV0+(VV2-VV0)*D0/(D0-D2);
00052
00053
00054 #define COMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,isect0,isect1) \
00055 if(D0D1>0.0f) \
00056 { \
00057 \
00058 \
00059 ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1); \
00060 } \
00061 else if(D0D2>0.0f) \
00062 { \
00063 \
00064 ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1); \
00065 } \
00066 else if(D1*D2>0.0f || D0!=0.0f) \
00067 { \
00068 \
00069 ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1); \
00070 } \
00071 else if(D1!=0.0f) \
00072 { \
00073 ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1); \
00074 } \
00075 else if(D2!=0.0f) \
00076 { \
00077 ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1); \
00078 } \
00079 else \
00080 { \
00081 \
00082 return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \
00083 }
00084
00085
00086
00087
00088
00089
00090 #define EDGE_EDGE_TEST(V0,U0,U1) \
00091 Bx=U0[i0]-U1[i0]; \
00092 By=U0[i1]-U1[i1]; \
00093 Cx=V0[i0]-U0[i0]; \
00094 Cy=V0[i1]-U0[i1]; \
00095 f=Ay*Bx-Ax*By; \
00096 d=By*Cx-Bx*Cy; \
00097 if((f>0 && d>=0 && d<=f) || (f<0 && d<=0 && d>=f)) \
00098 { \
00099 e=Ax*Cy-Ay*Cx; \
00100 if(f>0) \
00101 { \
00102 if(e>=0 && e<=f) return 1; \
00103 } \
00104 else \
00105 { \
00106 if(e<=0 && e>=f) return 1; \
00107 } \
00108 }
00109
00110 #define EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2) \
00111 { \
00112 float Ax,Ay,Bx,By,Cx,Cy,e,d,f; \
00113 Ax=V1[i0]-V0[i0]; \
00114 Ay=V1[i1]-V0[i1]; \
00115 \
00116 EDGE_EDGE_TEST(V0,U0,U1); \
00117 \
00118 EDGE_EDGE_TEST(V0,U1,U2); \
00119 \
00120 EDGE_EDGE_TEST(V0,U2,U0); \
00121 }
00122
00123 #define POINT_IN_TRI(V0,U0,U1,U2) \
00124 { \
00125 float a,b,c,d0,d1,d2; \
00126 \
00127 \
00128 a=U1[i1]-U0[i1]; \
00129 b=-(U1[i0]-U0[i0]); \
00130 c=-a*U0[i0]-b*U0[i1]; \
00131 d0=a*V0[i0]+b*V0[i1]+c; \
00132 \
00133 a=U2[i1]-U1[i1]; \
00134 b=-(U2[i0]-U1[i0]); \
00135 c=-a*U1[i0]-b*U1[i1]; \
00136 d1=a*V0[i0]+b*V0[i1]+c; \
00137 \
00138 a=U0[i1]-U2[i1]; \
00139 b=-(U0[i0]-U2[i0]); \
00140 c=-a*U2[i0]-b*U2[i1]; \
00141 d2=a*V0[i0]+b*V0[i1]+c; \
00142 if(d0*d1>0.0) \
00143 { \
00144 if(d0*d2>0.0) return 1; \
00145 } \
00146 }
00147
00148 int coplanar_tri_tri(float N[3],float V0[3],float V1[3],float V2[3],
00149 float U0[3],float U1[3],float U2[3])
00150 {
00151 float A[3];
00152 short i0,i1;
00153
00154
00155 A[0]=fabs(N[0]);
00156 A[1]=fabs(N[1]);
00157 A[2]=fabs(N[2]);
00158 if(A[0]>A[1])
00159 {
00160 if(A[0]>A[2])
00161 {
00162 i0=1;
00163 i1=2;
00164 }
00165 else
00166 {
00167 i0=0;
00168 i1=1;
00169 }
00170 }
00171 else
00172 {
00173 if(A[2]>A[1])
00174 {
00175 i0=0;
00176 i1=1;
00177 }
00178 else
00179 {
00180 i0=0;
00181 i1=2;
00182 }
00183 }
00184
00185
00186 EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2);
00187 EDGE_AGAINST_TRI_EDGES(V1,V2,U0,U1,U2);
00188 EDGE_AGAINST_TRI_EDGES(V2,V0,U0,U1,U2);
00189
00190
00191 POINT_IN_TRI(V0,U0,U1,U2);
00192 POINT_IN_TRI(U0,V0,V1,V2);
00193
00194 return 0;
00195 }
00196
00197
00198 int tri_tri_intersect(float V0[3],float V1[3],float V2[3],
00199 float U0[3],float U1[3],float U2[3])
00200 {
00201 float E1[3],E2[3];
00202 float N1[3],N2[3],d1,d2;
00203 float du0,du1,du2,dv0,dv1,dv2;
00204 float D[3];
00205 float isect1[2], isect2[2];
00206 float du0du1,du0du2,dv0dv1,dv0dv2;
00207 short index;
00208 float vp0,vp1,vp2;
00209 float up0,up1,up2;
00210 float b,c,max;
00211
00212
00213 SUB(E1,V1,V0);
00214 SUB(E2,V2,V0);
00215 CROSS(N1,E1,E2);
00216 d1=-DOT(N1,V0);
00217
00218
00219
00220 du0=DOT(N1,U0)+d1;
00221 du1=DOT(N1,U1)+d1;
00222 du2=DOT(N1,U2)+d1;
00223
00224
00225 #if USE_EPSILON_TEST==TRUE
00226 if(fabs(du0)<EPSILON) du0=0.0;
00227 if(fabs(du1)<EPSILON) du1=0.0;
00228 if(fabs(du2)<EPSILON) du2=0.0;
00229 #endif
00230 du0du1=du0*du1;
00231 du0du2=du0*du2;
00232
00233 if(du0du1>0.0f && du0du2>0.0f)
00234 return 0;
00235
00236
00237 SUB(E1,U1,U0);
00238 SUB(E2,U2,U0);
00239 CROSS(N2,E1,E2);
00240 d2=-DOT(N2,U0);
00241
00242
00243
00244 dv0=DOT(N2,V0)+d2;
00245 dv1=DOT(N2,V1)+d2;
00246 dv2=DOT(N2,V2)+d2;
00247
00248 #if USE_EPSILON_TEST==TRUE
00249 if(fabs(dv0)<EPSILON) dv0=0.0;
00250 if(fabs(dv1)<EPSILON) dv1=0.0;
00251 if(fabs(dv2)<EPSILON) dv2=0.0;
00252 #endif
00253
00254 dv0dv1=dv0*dv1;
00255 dv0dv2=dv0*dv2;
00256
00257 if(dv0dv1>0.0f && dv0dv2>0.0f)
00258 return 0;
00259
00260
00261 CROSS(D,N1,N2);
00262
00263
00264 max=fabs(D[0]);
00265 index=0;
00266 b=fabs(D[1]);
00267 c=fabs(D[2]);
00268 if(b>max) max=b,index=1;
00269 if(c>max) max=c,index=2;
00270
00271
00272 vp0=V0[index];
00273 vp1=V1[index];
00274 vp2=V2[index];
00275
00276 up0=U0[index];
00277 up1=U1[index];
00278 up2=U2[index];
00279
00280
00281 COMPUTE_INTERVALS(vp0,vp1,vp2,dv0,dv1,dv2,dv0dv1,dv0dv2,isect1[0],isect1[1]);
00282
00283
00284 COMPUTE_INTERVALS(up0,up1,up2,du0,du1,du2,du0du1,du0du2,isect2[0],isect2[1]);
00285
00286 SORT(isect1[0],isect1[1]);
00287 SORT(isect2[0],isect2[1]);
00288
00289 if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return 0;
00290 return 1;
00291 }
00292