00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #ifndef __VCGLIB_INTERSECTIONTRITRI3
00035 #define __VCGLIB_INTERSECTIONTRITRI3
00036
00037 #include <vcg/space/point3.h>
00038 #include <math.h>
00039
00040
00041 namespace vcg {
00042
00049 #ifndef FABS
00050 #define FABS(x) (T(fabs(x)))
00051 #endif
00052 #define USE_EPSILON_TEST
00053 #define TRI_TRI_INT_EPSILON 0.000001
00054
00055
00056
00057 #define CROSS(dest,v1,v2){ \
00058 dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
00059 dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
00060 dest[2]=v1[0]*v2[1]-v1[1]*v2[0];}
00061
00062 #define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])
00063
00064 #define SUB(dest,v1,v2){ \
00065 dest[0]=v1[0]-v2[0]; \
00066 dest[1]=v1[1]-v2[1]; \
00067 dest[2]=v1[2]-v2[2];}
00068
00069
00070 #define SORT(a,b) \
00071 if(a>b) \
00072 { \
00073 T c; \
00074 c=a; \
00075 a=b; \
00076 b=c; \
00077 }
00078
00079
00080 #define EDGE_EDGE_TEST(V0,U0,U1) \
00081 Bx=U0[i0]-U1[i0]; \
00082 By=U0[i1]-U1[i1]; \
00083 Cx=V0[i0]-U0[i0]; \
00084 Cy=V0[i1]-U0[i1]; \
00085 f=Ay*Bx-Ax*By; \
00086 d=By*Cx-Bx*Cy; \
00087 if((f>0 && d>=0 && d<=f) || (f<0 && d<=0 && d>=f)) \
00088 { \
00089 e=Ax*Cy-Ay*Cx; \
00090 if(f>0) \
00091 { \
00092 if(e>=0 && e<=f) return 1; \
00093 } \
00094 else \
00095 { \
00096 if(e<=0 && e>=f) return 1; \
00097 } \
00098 }
00099
00100 #define EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2) \
00101 { \
00102 T Ax,Ay,Bx,By,Cx,Cy,e,d,f; \
00103 Ax=V1[i0]-V0[i0]; \
00104 Ay=V1[i1]-V0[i1]; \
00105 \
00106 EDGE_EDGE_TEST(V0,U0,U1); \
00107 \
00108 EDGE_EDGE_TEST(V0,U1,U2); \
00109 \
00110 EDGE_EDGE_TEST(V0,U2,U0); \
00111 }
00112
00113 #define POINT_IN_TRI(V0,U0,U1,U2) \
00114 { \
00115 T a,b,c,d0,d1,d2; \
00116 \
00117 \
00118 a=U1[i1]-U0[i1]; \
00119 b=-(U1[i0]-U0[i0]); \
00120 c=-a*U0[i0]-b*U0[i1]; \
00121 d0=a*V0[i0]+b*V0[i1]+c; \
00122 \
00123 a=U2[i1]-U1[i1]; \
00124 b=-(U2[i0]-U1[i0]); \
00125 c=-a*U1[i0]-b*U1[i1]; \
00126 d1=a*V0[i0]+b*V0[i1]+c; \
00127 \
00128 a=U0[i1]-U2[i1]; \
00129 b=-(U0[i0]-U2[i0]); \
00130 c=-a*U2[i0]-b*U2[i1]; \
00131 d2=a*V0[i0]+b*V0[i1]+c; \
00132 if(d0*d1>0.0) \
00133 { \
00134 if(d0*d2>0.0) return 1; \
00135 } \
00136 }
00137
00138 template<class T>
00150 bool coplanar_tri_tri(const Point3<T> N, const Point3<T> V0, const Point3<T> V1,const Point3<T> V2,
00151 const Point3<T> U0, const Point3<T> U1,const Point3<T> U2)
00152 {
00153 T A[3];
00154 short i0,i1;
00155
00156
00157 A[0]=FABS(N[0]);
00158 A[1]=FABS(N[1]);
00159 A[2]=FABS(N[2]);
00160 if(A[0]>A[1])
00161 {
00162 if(A[0]>A[2])
00163 {
00164 i0=1;
00165 i1=2;
00166 }
00167 else
00168 {
00169 i0=0;
00170 i1=1;
00171 }
00172 }
00173 else
00174 {
00175 if(A[2]>A[1])
00176 {
00177 i0=0;
00178 i1=1;
00179 }
00180 else
00181 {
00182 i0=0;
00183 i1=2;
00184 }
00185 }
00186
00187
00188 EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2);
00189 EDGE_AGAINST_TRI_EDGES(V1,V2,U0,U1,U2);
00190 EDGE_AGAINST_TRI_EDGES(V2,V0,U0,U1,U2);
00191
00192
00193 POINT_IN_TRI(V0,U0,U1,U2);
00194 POINT_IN_TRI(U0,V0,V1,V2);
00195
00196 return 0;
00197 }
00198
00199
00200
00201 #define NEWCOMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,A,B,C,X0,X1) \
00202 { \
00203 if(D0D1>0.0f) \
00204 { \
00205 \
00206 \
00207 A=VV2; B=(VV0-VV2)*D2; C=(VV1-VV2)*D2; X0=D2-D0; X1=D2-D1; \
00208 } \
00209 else if(D0D2>0.0f)\
00210 { \
00211 \
00212 A=VV1; B=(VV0-VV1)*D1; C=(VV2-VV1)*D1; X0=D1-D0; X1=D1-D2; \
00213 } \
00214 else if(D1*D2>0.0f || D0!=0.0f) \
00215 { \
00216 \
00217 A=VV0; B=(VV1-VV0)*D0; C=(VV2-VV0)*D0; X0=D0-D1; X1=D0-D2; \
00218 } \
00219 else if(D1!=0.0f) \
00220 { \
00221 A=VV1; B=(VV0-VV1)*D1; C=(VV2-VV1)*D1; X0=D1-D0; X1=D1-D2; \
00222 } \
00223 else if(D2!=0.0f) \
00224 { \
00225 A=VV2; B=(VV0-VV2)*D2; C=(VV1-VV2)*D2; X0=D2-D0; X1=D2-D1; \
00226 } \
00227 else \
00228 { \
00229 \
00230 return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \
00231 } \
00232 }
00233
00234
00235 template<class T>
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245 bool NoDivTriTriIsect(const Point3<T> V0,const Point3<T> V1,const Point3<T> V2,
00246 const Point3<T> U0,const Point3<T> U1,const Point3<T> U2)
00247 {
00248 Point3<T> E1,E2;
00249 Point3<T> N1,N2;
00250 T d1,d2;
00251 T du0,du1,du2,dv0,dv1,dv2;
00252 Point3<T> D;
00253 T isect1[2], isect2[2];
00254 T du0du1,du0du2,dv0dv1,dv0dv2;
00255 short index;
00256 T vp0,vp1,vp2;
00257 T up0,up1,up2;
00258 T bb,cc,max;
00259
00260
00261 SUB(E1,V1,V0);
00262 SUB(E2,V2,V0);
00263 CROSS(N1,E1,E2);
00264 N1.Normalize();
00265 d1=-DOT(N1,V0);
00266
00267
00268
00269 du0=DOT(N1,U0)+d1;
00270 du1=DOT(N1,U1)+d1;
00271 du2=DOT(N1,U2)+d1;
00272
00273
00274 #ifdef USE_TRI_TRI_INT_EPSILON_TEST
00275 if(FABS(du0)<TRI_TRI_INT_EPSILON) du0=0.0;
00276 if(FABS(du1)<TRI_TRI_INT_EPSILON) du1=0.0;
00277 if(FABS(du2)<TRI_TRI_INT_EPSILON) du2=0.0;
00278 #endif
00279 du0du1=du0*du1;
00280 du0du2=du0*du2;
00281
00282 if(du0du1>0.0f && du0du2>0.0f)
00283 return 0;
00284
00285
00286 SUB(E1,U1,U0);
00287 SUB(E2,U2,U0);
00288 CROSS(N2,E1,E2);
00289 d2=-DOT(N2,U0);
00290
00291
00292
00293 dv0=DOT(N2,V0)+d2;
00294 dv1=DOT(N2,V1)+d2;
00295 dv2=DOT(N2,V2)+d2;
00296
00297 #ifdef USE_TRI_TRI_INT_EPSILON_TEST
00298 if(FABS(dv0)<TRI_TRI_INT_EPSILON) dv0=0.0;
00299 if(FABS(dv1)<TRI_TRI_INT_EPSILON) dv1=0.0;
00300 if(FABS(dv2)<TRI_TRI_INT_EPSILON) dv2=0.0;
00301 #endif
00302
00303 dv0dv1=dv0*dv1;
00304 dv0dv2=dv0*dv2;
00305
00306 if(dv0dv1>0.0f && dv0dv2>0.0f)
00307 return 0;
00308
00309
00310 CROSS(D,N1,N2);
00311
00312
00313 max=(T)FABS(D[0]);
00314 index=0;
00315 bb=(T)FABS(D[1]);
00316 cc=(T)FABS(D[2]);
00317 if(bb>max) max=bb,index=1;
00318 if(cc>max) max=cc,index=2;
00319
00320
00321 vp0=V0[index];
00322 vp1=V1[index];
00323 vp2=V2[index];
00324
00325 up0=U0[index];
00326 up1=U1[index];
00327 up2=U2[index];
00328
00329
00330 T a,b,c,x0,x1;
00331 NEWCOMPUTE_INTERVALS(vp0,vp1,vp2,dv0,dv1,dv2,dv0dv1,dv0dv2,a,b,c,x0,x1);
00332
00333
00334 T d,e,f,y0,y1;
00335 NEWCOMPUTE_INTERVALS(up0,up1,up2,du0,du1,du2,du0du1,du0du2,d,e,f,y0,y1);
00336
00337 T xx,yy,xxyy,tmp;
00338 xx=x0*x1;
00339 yy=y0*y1;
00340 xxyy=xx*yy;
00341
00342 tmp=a*xxyy;
00343 isect1[0]=tmp+b*x1*yy;
00344 isect1[1]=tmp+c*x0*yy;
00345
00346 tmp=d*xxyy;
00347 isect2[0]=tmp+e*xx*y1;
00348 isect2[1]=tmp+f*xx*y0;
00349
00350 SORT(isect1[0],isect1[1]);
00351 SORT(isect2[0],isect2[1]);
00352
00353 if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return 0;
00354 return 1;
00355 }
00356
00357
00358
00359 #define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])
00360 #define ADD(dest,v1,v2) dest[0]=v1[0]+v2[0]; dest[1]=v1[1]+v2[1]; dest[2]=v1[2]+v2[2];
00361 #define MULT(dest,v,factor) dest[0]=factor*v[0]; dest[1]=factor*v[1]; dest[2]=factor*v[2];
00362 #define SET(dest,src) dest[0]=src[0]; dest[1]=src[1]; dest[2]=src[2];
00363
00364 #define SORT2(a,b,smallest) \
00365 if(a>b) \
00366 { \
00367 float c; \
00368 c=a; \
00369 a=b; \
00370 b=c; \
00371 smallest=1; \
00372 } \
00373 else smallest=0;
00374
00375 template <class T>
00376 inline void isect2(Point3<T> VTX0,Point3<T> VTX1,Point3<T> VTX2,float VV0,float VV1,float VV2,
00377 float D0,float D1,float D2,float *isect0,float *isect1,Point3<T> &isectpoint0,Point3<T> &isectpoint1)
00378 {
00379 float tmp=D0/(D0-D1);
00380 float diff[3];
00381 *isect0=VV0+(VV1-VV0)*tmp;
00382 SUB(diff,VTX1,VTX0);
00383 MULT(diff,diff,tmp);
00384 ADD(isectpoint0,diff,VTX0);
00385 tmp=D0/(D0-D2);
00386 *isect1=VV0+(VV2-VV0)*tmp;
00387 SUB(diff,VTX2,VTX0);
00388 MULT(diff,diff,tmp);
00389 ADD(isectpoint1,VTX0,diff);
00390 }
00391
00392 template <class T>
00393 inline int compute_intervals_isectline(Point3<T> VERT0,Point3<T> VERT1,Point3<T> VERT2,
00394 float VV0,float VV1,float VV2,float D0,float D1,float D2,
00395 float D0D1,float D0D2,float *isect0,float *isect1,
00396 Point3<T> & isectpoint0, Point3<T> & isectpoint1)
00397 {
00398 if(D0D1>0.0f)
00399 {
00400
00401
00402 isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,isect0,isect1,isectpoint0,isectpoint1);
00403 }
00404 else if(D0D2>0.0f)
00405 {
00406
00407 isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,isect0,isect1,isectpoint0,isectpoint1);
00408 }
00409 else if(D1*D2>0.0f || D0!=0.0f)
00410 {
00411
00412 isect2(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,isect0,isect1,isectpoint0,isectpoint1);
00413 }
00414 else if(D1!=0.0f)
00415 {
00416 isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,isect0,isect1,isectpoint0,isectpoint1);
00417 }
00418 else if(D2!=0.0f)
00419 {
00420 isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,isect0,isect1,isectpoint0,isectpoint1);
00421 }
00422 else
00423 {
00424
00425 return 1;
00426 }
00427 return 0;
00428 }
00429
00430 #define COMPUTE_INTERVALS_ISECTLINE(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,isect0,isect1,isectpoint0,isectpoint1) \
00431 if(D0D1>0.0f) \
00432 { \
00433 \
00434 \
00435 isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,&isect0,&isect1,isectpoint0,isectpoint1); \
00436 }
00437 #if 0
00438 else if(D0D2>0.0f) \
00439 { \
00440 \
00441 isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,&isect0,&isect1,isectpoint0,isectpoint1); \
00442 } \
00443 else if(D1*D2>0.0f || D0!=0.0f) \
00444 { \
00445 \
00446 isect2(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,&isect0,&isect1,isectpoint0,isectpoint1); \
00447 } \
00448 else if(D1!=0.0f) \
00449 { \
00450 isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,&isect0,&isect1,isectpoint0,isectpoint1); \
00451 } \
00452 else if(D2!=0.0f) \
00453 { \
00454 isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,&isect0,&isect1,isectpoint0,isectpoint1); \
00455 } \
00456 else \
00457 { \
00458 \
00459 coplanar=1; \
00460 return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \
00461 }
00462 #endif
00463
00464 template <class T>
00465 bool tri_tri_intersect_with_isectline( Point3<T> V0,Point3<T> V1,Point3<T> V2,
00466 Point3<T> U0,Point3<T> U1,Point3<T> U2,bool &coplanar,
00467 Point3<T> &isectpt1,Point3<T> &isectpt2)
00468 {
00469 Point3<T> E1,E2;
00470 Point3<T> N1,N2;
00471 T d1,d2;
00472 float du0,du1,du2,dv0,dv1,dv2;
00473 Point3<T> D;
00474 float isect1[2], isect2[2];
00475 Point3<T> isectpointA1,isectpointA2;
00476 Point3<T> isectpointB1,isectpointB2;
00477 float du0du1,du0du2,dv0dv1,dv0dv2;
00478 short index;
00479 float vp0,vp1,vp2;
00480 float up0,up1,up2;
00481 float b,c,max;
00482
00483 Point3<T> diff;
00484 int smallest1,smallest2;
00485
00486
00487 SUB(E1,V1,V0);
00488 SUB(E2,V2,V0);
00489 CROSS(N1,E1,E2);
00490 d1=-DOT(N1,V0);
00491
00492
00493
00494 du0=DOT(N1,U0)+d1;
00495 du1=DOT(N1,U1)+d1;
00496 du2=DOT(N1,U2)+d1;
00497
00498
00499 #ifdef USE_EPSILON_TEST
00500 if(fabs(du0)<TRI_TRI_INT_EPSILON) du0=0.0;
00501 if(fabs(du1)<TRI_TRI_INT_EPSILON) du1=0.0;
00502 if(fabs(du2)<TRI_TRI_INT_EPSILON) du2=0.0;
00503 #endif
00504 du0du1=du0*du1;
00505 du0du2=du0*du2;
00506
00507 if(du0du1>0.0f && du0du2>0.0f)
00508 return 0;
00509
00510
00511 SUB(E1,U1,U0);
00512 SUB(E2,U2,U0);
00513 CROSS(N2,E1,E2);
00514 d2=-DOT(N2,U0);
00515
00516
00517
00518 dv0=DOT(N2,V0)+d2;
00519 dv1=DOT(N2,V1)+d2;
00520 dv2=DOT(N2,V2)+d2;
00521
00522 #ifdef USE_EPSILON_TEST
00523 if(fabs(dv0)<TRI_TRI_INT_EPSILON) dv0=0.0;
00524 if(fabs(dv1)<TRI_TRI_INT_EPSILON) dv1=0.0;
00525 if(fabs(dv2)<TRI_TRI_INT_EPSILON) dv2=0.0;
00526 #endif
00527
00528 dv0dv1=dv0*dv1;
00529 dv0dv2=dv0*dv2;
00530
00531 if(dv0dv1>0.0f && dv0dv2>0.0f)
00532 return 0;
00533
00534
00535 CROSS(D,N1,N2);
00536
00537
00538 max=fabs(D[0]);
00539 index=0;
00540 b=fabs(D[1]);
00541 c=fabs(D[2]);
00542 if(b>max) max=b,index=1;
00543 if(c>max) max=c,index=2;
00544
00545
00546 vp0=V0[index];
00547 vp1=V1[index];
00548 vp2=V2[index];
00549
00550 up0=U0[index];
00551 up1=U1[index];
00552 up2=U2[index];
00553
00554
00555 coplanar=compute_intervals_isectline(V0,V1,V2,vp0,vp1,vp2,dv0,dv1,dv2,
00556 dv0dv1,dv0dv2,&isect1[0],&isect1[1],isectpointA1,isectpointA2);
00557 if(coplanar) return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2);
00558
00559
00560
00561 compute_intervals_isectline(U0,U1,U2,up0,up1,up2,du0,du1,du2,
00562 du0du1,du0du2,&isect2[0],&isect2[1],isectpointB1,isectpointB2);
00563
00564 SORT2(isect1[0],isect1[1],smallest1);
00565 SORT2(isect2[0],isect2[1],smallest2);
00566
00567 if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return 0;
00568
00569
00570
00571 if(isect2[0]<isect1[0])
00572 {
00573 if(smallest1==0) { SET(isectpt1,isectpointA1); }
00574 else { SET(isectpt1,isectpointA2); }
00575
00576 if(isect2[1]<isect1[1])
00577 {
00578 if(smallest2==0) { SET(isectpt2,isectpointB2); }
00579 else { SET(isectpt2,isectpointB1); }
00580 }
00581 else
00582 {
00583 if(smallest1==0) { SET(isectpt2,isectpointA2); }
00584 else { SET(isectpt2,isectpointA1); }
00585 }
00586 }
00587 else
00588 {
00589 if(smallest2==0) { SET(isectpt1,isectpointB1); }
00590 else { SET(isectpt1,isectpointB2); }
00591
00592 if(isect2[1]>isect1[1])
00593 {
00594 if(smallest1==0) { SET(isectpt2,isectpointA2); }
00595 else { SET(isectpt2,isectpointA1); }
00596 }
00597 else
00598 {
00599 if(smallest2==0) { SET(isectpt2,isectpointB2); }
00600 else { SET(isectpt2,isectpointB1); }
00601 }
00602 }
00603 return 1;
00604 }
00605
00606
00607 }
00608
00609 #undef FABS
00610 #undef USE_EPSILON_TEST
00611 #undef TRI_TRI_INT_EPSILON
00612 #undef CROSS
00613 #undef DOT
00614 #undef SUB
00615 #undef SORT
00616 #undef SORT2
00617 #undef ADD
00618 #undef MULT
00619 #undef SET
00620 #undef EDGE_EDGE_TEST
00621 #undef EDGE_AGAINST_TRI_EDGE
00622 #undef POINT_IN_TRI
00623 #undef COMPUTE_INTERVALS_ISECTLINE
00624 #undef NEWCOMPUTE_INTERVALS
00625 #endif