shape_doc 0.1
/Users/mourrain/Devel/mmx/shape/include/shape/ssiqts_tri_tri.hpp
Go to the documentation of this file.
00001 
00002 template<class Point3>
00003   bool intersectp_coplanar_triangles 
00004   (const Point3& N, 
00005    const Point3& V0, const Point3& V1, const Point3& V2, 
00006    const Point3& U0, const Point3& U1, const Point3& U2 )
00007   {    
00008 
00009     Point3 A;
00010     short i0, i1;
00011 
00012     A[0] = std::abs(N[0]); 
00013     A[1] = std::abs(N[1]); 
00014     A[2] = std::abs(N[2]);
00015     
00016     if (A[0] > A[1])
00017       {
00018         if (A[0] > A[2])
00019           {
00020             i0 = 1;
00021             i1 = 2;
00022           }
00023         else
00024           {
00025             i0 = 0;
00026             i1 = 1;
00027           }
00028       }
00029     else
00030       {
00031         if (A[2] > A[1])
00032           {
00033             i0 = 0;
00034             i1 = 1;
00035           }
00036         else
00037           {
00038             i0 = 0;
00039             i1 = 2;
00040           }
00041       }
00042 
00043     {
00044       typename Point3::value_type Ax, Ay, Bx, By, Cx, Cy, e, d, f;
00045       Ax = V1[i0] - V0[i0];
00046       Ay = V1[i1] - V0[i1];
00047       Bx = U0[i0] - U1[i0];
00048       By = U0[i1] - U1[i1];
00049       Cx = V0[i0] - U0[i0];
00050       Cy = V0[i1] - U0[i1];
00051       f = Ay * Bx - Ax * By;
00052       d = By * Cx - Bx * Cy;
00053       if ((f > 0 && d >= 0 && d <= f) || (f < 0 && d <= 0 && d >= f))
00054         {
00055           e = Ax * Cy - Ay * Cx;
00056           if (f > 0)
00057             {
00058               if (e >= 0 && e <= f)
00059                 return true;
00060             }
00061           else
00062             {
00063               if (e <= 0 && e >= f)
00064                 return true;
00065             }
00066         };
00067       Bx = U1[i0] - U2[i0];
00068       By = U1[i1] - U2[i1];
00069       Cx = V0[i0] - U1[i0];
00070       Cy = V0[i1] - U1[i1];
00071       f = Ay * Bx - Ax * By;
00072       d = By * Cx - Bx * Cy;
00073       if ((f > 0 && d >= 0 && d <= f) || (f < 0 && d <= 0 && d >= f))
00074         {
00075           e = Ax * Cy - Ay * Cx;
00076           if (f > 0)
00077             {
00078               if (e >= 0 && e <= f)
00079                 return true;
00080             }
00081           else
00082             {
00083               if (e <= 0 && e >= f)
00084                 return true;
00085             }
00086         };
00087       Bx = U2[i0] - U0[i0];
00088       By = U2[i1] - U0[i1];
00089       Cx = V0[i0] - U2[i0];
00090       Cy = V0[i1] - U2[i1];
00091       f = Ay * Bx - Ax * By;
00092       d = By * Cx - Bx * Cy;
00093       if ((f > 0 && d >= 0 && d <= f) || (f < 0 && d <= 0 && d >= f))
00094         {
00095           e = Ax * Cy - Ay * Cx;
00096           if (f > 0)
00097             {
00098               if (e >= 0 && e <= f)
00099                 return true;
00100             }
00101           else
00102             {
00103               if (e <= 0 && e >= f)
00104                 return true;
00105             }
00106         };
00107     };
00108     {
00109       typename Point3::value_type Ax, Ay, Bx, By, Cx, Cy, e, d, f;
00110       Ax = V2[i0] - V1[i0];
00111       Ay = V2[i1] - V1[i1];
00112       Bx = U0[i0] - U1[i0];
00113       By = U0[i1] - U1[i1];
00114       Cx = V1[i0] - U0[i0];
00115       Cy = V1[i1] - U0[i1];
00116       f = Ay * Bx - Ax * By;
00117       d = By * Cx - Bx * Cy;
00118       if ((f > 0 && d >= 0 && d <= f) || (f < 0 && d <= 0 && d >= f))
00119         {
00120           e = Ax * Cy - Ay * Cx;
00121           if (f > 0)
00122             {
00123               if (e >= 0 && e <= f)
00124                 return true;
00125             }
00126           else
00127             {
00128               if (e <= 0 && e >= f)
00129                 return true;
00130             }
00131         };
00132       Bx = U1[i0] - U2[i0];
00133       By = U1[i1] - U2[i1];
00134       Cx = V1[i0] - U1[i0];
00135       Cy = V1[i1] - U1[i1];
00136       f = Ay * Bx - Ax * By;
00137       d = By * Cx - Bx * Cy;
00138       if ((f > 0 && d >= 0 && d <= f) || (f < 0 && d <= 0 && d >= f))
00139         {
00140           e = Ax * Cy - Ay * Cx;
00141           if (f > 0)
00142             {
00143               if (e >= 0 && e <= f)
00144                 return true;
00145             }
00146           else
00147             {
00148               if (e <= 0 && e >= f)
00149                 return true;
00150             }
00151         };
00152       Bx = U2[i0] - U0[i0];
00153       By = U2[i1] - U0[i1];
00154       Cx = V1[i0] - U2[i0];
00155       Cy = V1[i1] - U2[i1];
00156       f = Ay * Bx - Ax * By;
00157       d = By * Cx - Bx * Cy;
00158       if ((f > 0 && d >= 0 && d <= f) || (f < 0 && d <= 0 && d >= f))
00159         {
00160           e = Ax * Cy - Ay * Cx;
00161           if (f > 0)
00162             {
00163               if (e >= 0 && e <= f)
00164                 return true;
00165             }
00166           else
00167             {
00168               if (e <= 0 && e >= f)
00169                 return true;
00170             }
00171         };
00172     };
00173     {
00174       typename Point3::value_type Ax, Ay, Bx, By, Cx, Cy, e, d, f;
00175       Ax = V0[i0] - V2[i0];
00176       Ay = V0[i1] - V2[i1];
00177       Bx = U0[i0] - U1[i0];
00178       By = U0[i1] - U1[i1];
00179       Cx = V2[i0] - U0[i0];
00180       Cy = V2[i1] - U0[i1];
00181       f = Ay * Bx - Ax * By;
00182       d = By * Cx - Bx * Cy;
00183       if ((f > 0 && d >= 0 && d <= f) || (f < 0 && d <= 0 && d >= f))
00184         {
00185           e = Ax * Cy - Ay * Cx;
00186           if (f > 0)
00187             {
00188               if (e >= 0 && e <= f)
00189                 return true;
00190             }
00191           else
00192             {
00193               if (e <= 0 && e >= f)
00194                 return true;
00195             }
00196         };
00197       Bx = U1[i0] - U2[i0];
00198       By = U1[i1] - U2[i1];
00199       Cx = V2[i0] - U1[i0];
00200       Cy = V2[i1] - U1[i1];
00201       f = Ay * Bx - Ax * By;
00202       d = By * Cx - Bx * Cy;
00203       if ((f > 0 && d >= 0 && d <= f) || (f < 0 && d <= 0 && d >= f))
00204         {
00205           e = Ax * Cy - Ay * Cx;
00206           if (f > 0)
00207             {
00208               if (e >= 0 && e <= f)
00209                 return true;
00210             }
00211           else
00212             {
00213               if (e <= 0 && e >= f)
00214                 return true;
00215             }
00216         };
00217       Bx = U2[i0] - U0[i0];
00218       By = U2[i1] - U0[i1];
00219       Cx = V2[i0] - U2[i0];
00220       Cy = V2[i1] - U2[i1];
00221       f = Ay * Bx - Ax * By;
00222       d = By * Cx - Bx * Cy;
00223       if ((f > 0 && d >= 0 && d <= f) || (f < 0 && d <= 0 && d >= f))
00224         {
00225           e = Ax * Cy - Ay * Cx;
00226           if (f > 0)
00227             {
00228               if (e >= 0 && e <= f)
00229                 return true;
00230             }
00231           else
00232             {
00233               if (e <= 0 && e >= f)
00234                 return true;
00235             }
00236         };
00237     };
00238 
00239 
00240     {
00241       typename Point3::value_type a, b, c, d0, d1, d2;
00242       a = U1[i1] - U0[i1];
00243       b = -(U1[i0] - U0[i0]);
00244       c = -a * U0[i0] - b * U0[i1];
00245       d0 = a * V0[i0] + b * V0[i1] + c;
00246       a = U2[i1] - U1[i1];
00247       b = -(U2[i0] - U1[i0]);
00248       c = -a * U1[i0] - b * U1[i1];
00249       d1 = a * V0[i0] + b * V0[i1] + c;
00250       a = U0[i1] - U2[i1];
00251       b = -(U0[i0] - U2[i0]);
00252       c = -a * U2[i0] - b * U2[i1];
00253       d2 = a * V0[i0] + b * V0[i1] + c;
00254       if (d0 * d1 > 0.0)
00255         {
00256           if (d0 * d2 > 0.0)
00257             return true;
00258         }
00259     };
00260     {
00261       typename Point3::value_type a, b, c, d0, d1, d2;
00262       a = V1[i1] - V0[i1];
00263       b = -(V1[i0] - V0[i0]);
00264       c = -a * V0[i0] - b * V0[i1];
00265       d0 = a * U0[i0] + b * U0[i1] + c;
00266       a = V2[i1] - V1[i1];
00267       b = -(V2[i0] - V1[i0]);
00268       c = -a * V1[i0] - b * V1[i1];
00269       d1 = a * U0[i0] + b * U0[i1] + c;
00270       a = V0[i1] - V2[i1];
00271       b = -(V0[i0] - V2[i0]);
00272       c = -a * V2[i0] - b * V2[i1];
00273       d2 = a * U0[i0] + b * U0[i1] + c;
00274       if (d0 * d1 > 0.0)
00275         {
00276           if (d0 * d2 > 0.0)
00277             return true;
00278         }
00279     };
00280 
00281     return false;
00282   }
00283 
00284   template<class Point3>
00285   bool intersectp_triangles3_div 
00286   (const Point3& V0, const Point3& V1, const Point3& V2,
00287    const Point3& U0, const Point3& U1, const Point3& U2, 
00288    const typename Point3::value_type& prec)
00289   {
00290     Point3 E1, E2;
00291     Point3 N1, N2;
00292     typename Point3::value_type d1, d2;
00293     typename Point3::value_type du0, du1, du2, dv0, dv1, dv2;
00294     Point3 D;
00295     typename Point3::value_type isect1[2], isect2[2];
00296     typename Point3::value_type du0du1, du0du2, dv0dv1, dv0dv2;
00297     short index;
00298     typename Point3::value_type vp0, vp1, vp2;
00299     typename Point3::value_type up0, up1, up2;
00300     typename Point3::value_type b, c, max;
00301 
00302 
00303     E1[0] = V1[0] - V0[0];
00304     E1[1] = V1[1] - V0[1];
00305     E1[2] = V1[2] - V0[2];;
00306     E2[0] = V2[0] - V0[0];
00307     E2[1] = V2[1] - V0[1];
00308     E2[2] = V2[2] - V0[2];;
00309     N1[0] = E1[1] * E2[2] - E1[2] * E2[1];
00310     N1[1] = E1[2] * E2[0] - E1[0] * E2[2];
00311     N1[2] = E1[0] * E2[1] - E1[1] * E2[0];;
00312     d1 = -(N1[0] * V0[0] + N1[1] * V0[1] + N1[2] * V0[2]);
00313 
00314 
00315 
00316     du0 = (N1[0] * U0[0] + N1[1] * U0[1] + N1[2] * U0[2]) + d1;
00317     du1 = (N1[0] * U1[0] + N1[1] * U1[1] + N1[2] * U1[2]) + d1;
00318     du2 = (N1[0] * U2[0] + N1[1] * U2[1] + N1[2] * U2[2]) + d1;
00319 
00320 
00321 
00322     if (std::abs (du0) < prec)
00323       du0 = 0.0;
00324     if (std::abs (du1) < prec)
00325       du1 = 0.0;
00326     if (std::abs (du2) < prec)
00327       du2 = 0.0;
00328 
00329     du0du1 = du0 * du1;
00330     du0du2 = du0 * du2;
00331 
00332     if (du0du1 > 0.0f && du0du2 > 0.0f)
00333       return false;
00334 
00335 
00336     E1[0] = U1[0] - U0[0];
00337     E1[1] = U1[1] - U0[1];
00338     E1[2] = U1[2] - U0[2];;
00339     E2[0] = U2[0] - U0[0];
00340     E2[1] = U2[1] - U0[1];
00341     E2[2] = U2[2] - U0[2];;
00342     N2[0] = E1[1] * E2[2] - E1[2] * E2[1];
00343     N2[1] = E1[2] * E2[0] - E1[0] * E2[2];
00344     N2[2] = E1[0] * E2[1] - E1[1] * E2[0];;
00345     d2 = -(N2[0] * U0[0] + N2[1] * U0[1] + N2[2] * U0[2]);
00346 
00347 
00348 
00349     dv0 = (N2[0] * V0[0] + N2[1] * V0[1] + N2[2] * V0[2]) + d2;
00350     dv1 = (N2[0] * V1[0] + N2[1] * V1[1] + N2[2] * V1[2]) + d2;
00351     dv2 = (N2[0] * V2[0] + N2[1] * V2[1] + N2[2] * V2[2]) + d2;
00352 
00353 
00354     if (std::abs (dv0) < prec)
00355       dv0 = 0.0;
00356     if (std::abs (dv1) < prec)
00357       dv1 = 0.0;
00358     if (std::abs (dv2) < prec)
00359       dv2 = 0.0;
00360 
00361 
00362     dv0dv1 = dv0 * dv1;
00363     dv0dv2 = dv0 * dv2;
00364 
00365     if (dv0dv1 > 0.0f && dv0dv2 > 0.0f)
00366       return false;
00367 
00368 
00369     D[0] = N1[1] * N2[2] - N1[2] * N2[1];
00370     D[1] = N1[2] * N2[0] - N1[0] * N2[2];
00371     D[2] = N1[0] * N2[1] - N1[1] * N2[0];;
00372 
00373 
00374     max = std::abs (D[0]);
00375     index = 0;
00376     b = std::abs (D[1]);
00377     c = std::abs (D[2]);
00378     if (b > max)
00379       max = b, index = 1;
00380     if (c > max)
00381       max = c, index = 2;
00382 
00383 
00384     vp0 = V0[index];
00385     vp1 = V1[index];
00386     vp2 = V2[index];
00387 
00388     up0 = U0[index];
00389     up1 = U1[index];
00390     up2 = U2[index];
00391 
00392 
00393     if (dv0dv1 > 0.0f)
00394       {
00395         isect1[0] = vp2 + (vp0 - vp2) * dv2 / (dv2 - dv0);
00396         isect1[1] = vp2 + (vp1 - vp2) * dv2 / (dv2 - dv1);;
00397       }
00398     else if (dv0dv2 > 0.0f)
00399       {
00400         isect1[0] = vp1 + (vp0 - vp1) * dv1 / (dv1 - dv0);
00401         isect1[1] = vp1 + (vp2 - vp1) * dv1 / (dv1 - dv2);;
00402       }
00403     else if (dv1 * dv2 > 0.0f || dv0 != 0.0f)
00404       {
00405         isect1[0] = vp0 + (vp1 - vp0) * dv0 / (dv0 - dv1);
00406         isect1[1] = vp0 + (vp2 - vp0) * dv0 / (dv0 - dv2);;
00407       }
00408     else if (dv1 != 0.0f)
00409       {
00410         isect1[0] = vp1 + (vp0 - vp1) * dv1 / (dv1 - dv0);
00411         isect1[1] = vp1 + (vp2 - vp1) * dv1 / (dv1 - dv2);;
00412       }
00413     else if (dv2 != 0.0f)
00414       {
00415         isect1[0] = vp2 + (vp0 - vp2) * dv2 / (dv2 - dv0);
00416         isect1[1] = vp2 + (vp1 - vp2) * dv2 / (dv2 - dv1);;
00417       }
00418     else
00419       {
00420         return intersectp_coplanar_triangles (N1, V0, V1, V2, U0, U1, U2);
00421       };
00422 
00423 
00424     if (du0du1 > 0.0f)
00425       {
00426         isect2[0] = up2 + (up0 - up2) * du2 / (du2 - du0);
00427         isect2[1] = up2 + (up1 - up2) * du2 / (du2 - du1);;
00428       }
00429     else if (du0du2 > 0.0f)
00430       {
00431         isect2[0] = up1 + (up0 - up1) * du1 / (du1 - du0);
00432         isect2[1] = up1 + (up2 - up1) * du1 / (du1 - du2);;
00433       }
00434     else if (du1 * du2 > 0.0f || du0 != 0.0f)
00435       {
00436         isect2[0] = up0 + (up1 - up0) * du0 / (du0 - du1);
00437         isect2[1] = up0 + (up2 - up0) * du0 / (du0 - du2);;
00438       }
00439     else if (du1 != 0.0f)
00440       {
00441         isect2[0] = up1 + (up0 - up1) * du1 / (du1 - du0);
00442         isect2[1] = up1 + (up2 - up1) * du1 / (du1 - du2);;
00443       }
00444     else if (du2 != 0.0f)
00445       {
00446         isect2[0] = up2 + (up0 - up2) * du2 / (du2 - du0);
00447         isect2[1] = up2 + (up1 - up2) * du2 / (du2 - du1);;
00448       }
00449     else
00450       {
00451         return intersectp_coplanar_triangles (N1, V0, V1, V2, U0, U1, U2);
00452       };
00453 
00454     if (isect1[0] > isect1[1])
00455       {
00456         typename Point3::value_type c;
00457         c = isect1[0];
00458         isect1[0] = isect1[1];
00459         isect1[1] = c;
00460       };
00461     if (isect2[0] > isect2[1])
00462       {
00463         typename Point3::value_type c;
00464         c = isect2[0];
00465         isect2[0] = isect2[1];
00466         isect2[1] = c;
00467       };
00468 
00469     if (isect1[1] < isect2[0] || isect2[1] < isect1[0])
00470       return false;
00471     return true;
00472   }
00473 
00474 
00475 
00476 
00477   template<class Point3>
00478   bool intersectp_triangle3 
00479   ( const Point3& V0, const Point3& V1, const Point3& V2,
00480     const Point3& U0, const Point3& U1, const Point3& U2,
00481     const typename Point3::value_type& prec )
00482   {
00483     Point3 E1, E2;
00484     Point3 N1, N2;
00485     typename Point3::value_type d1, d2;
00486     typename Point3::value_type du0, du1, du2, dv0, dv1, dv2;
00487     Point3 D;
00488     typename Point3::value_type isect1[2], isect2[2];
00489     typename Point3::value_type du0du1, du0du2, dv0dv1, dv0dv2;
00490     short index;
00491     typename Point3::value_type vp0, vp1, vp2;
00492     typename Point3::value_type up0, up1, up2;
00493     typename Point3::value_type bb, cc, max;
00494     typename Point3::value_type a, b, c, x0, x1;
00495     typename Point3::value_type d, e, f, y0, y1;
00496     typename Point3::value_type xx, yy, xxyy, tmp;
00497 
00498 
00499     E1[0] = V1[0] - V0[0];
00500     E1[1] = V1[1] - V0[1];
00501     E1[2] = V1[2] - V0[2];;
00502     E2[0] = V2[0] - V0[0];
00503     E2[1] = V2[1] - V0[1];
00504     E2[2] = V2[2] - V0[2];;
00505     N1[0] = E1[1] * E2[2] - E1[2] * E2[1];
00506     N1[1] = E1[2] * E2[0] - E1[0] * E2[2];
00507     N1[2] = E1[0] * E2[1] - E1[1] * E2[0];;
00508     d1 = -(N1[0] * V0[0] + N1[1] * V0[1] + N1[2] * V0[2]);
00509 
00510 
00511 
00512     du0 = (N1[0] * U0[0] + N1[1] * U0[1] + N1[2] * U0[2]) + d1;
00513     du1 = (N1[0] * U1[0] + N1[1] * U1[1] + N1[2] * U1[2]) + d1;
00514     du2 = (N1[0] * U2[0] + N1[1] * U2[1] + N1[2] * U2[2]) + d1;
00515 
00516 
00517 
00518     if (((typename Point3::value_type) std::abs (du0)) < prec)
00519       du0 = 0.0;
00520     if (((typename Point3::value_type) std::abs (du1)) < prec)
00521       du1 = 0.0;
00522     if (((typename Point3::value_type) std::abs (du2)) < prec)
00523       du2 = 0.0;
00524 
00525     du0du1 = du0 * du1;
00526     du0du2 = du0 * du2;
00527 
00528     if (du0du1 > 0.0f && du0du2 > 0.0f)
00529       return false;
00530 
00531 
00532     E1[0] = U1[0] - U0[0];
00533     E1[1] = U1[1] - U0[1];
00534     E1[2] = U1[2] - U0[2];;
00535     E2[0] = U2[0] - U0[0];
00536     E2[1] = U2[1] - U0[1];
00537     E2[2] = U2[2] - U0[2];;
00538     N2[0] = E1[1] * E2[2] - E1[2] * E2[1];
00539     N2[1] = E1[2] * E2[0] - E1[0] * E2[2];
00540     N2[2] = E1[0] * E2[1] - E1[1] * E2[0];;
00541     d2 = -(N2[0] * U0[0] + N2[1] * U0[1] + N2[2] * U0[2]);
00542 
00543 
00544 
00545     dv0 = (N2[0] * V0[0] + N2[1] * V0[1] + N2[2] * V0[2]) + d2;
00546     dv1 = (N2[0] * V1[0] + N2[1] * V1[1] + N2[2] * V1[2]) + d2;
00547     dv2 = (N2[0] * V2[0] + N2[1] * V2[1] + N2[2] * V2[2]) + d2;
00548 
00549 
00550     if (((typename Point3::value_type) std::abs (dv0)) < prec)
00551       dv0 = 0.0;
00552     if (((typename Point3::value_type) std::abs (dv1)) < prec)
00553       dv1 = 0.0;
00554     if (((typename Point3::value_type) std::abs (dv2)) < prec)
00555       dv2 = 0.0;
00556 
00557 
00558     dv0dv1 = dv0 * dv1;
00559     dv0dv2 = dv0 * dv2;
00560 
00561     if (dv0dv1 > 0.0f && dv0dv2 > 0.0f)
00562       return false;
00563 
00564 
00565     D[0] = N1[1] * N2[2] - N1[2] * N2[1];
00566     D[1] = N1[2] * N2[0] - N1[0] * N2[2];
00567     D[2] = N1[0] * N2[1] - N1[1] * N2[0];;
00568 
00569 
00570     max = (typename Point3::value_type) ((typename Point3::value_type) std::abs (D[0]));
00571     index = 0;
00572     bb = (typename Point3::value_type) ((typename Point3::value_type) std::abs (D[1]));
00573     cc = (typename Point3::value_type) ((typename Point3::value_type) std::abs (D[2]));
00574     if (bb > max)
00575       max = bb, index = 1;
00576     if (cc > max)
00577       max = cc, index = 2;
00578 
00579 
00580     vp0 = V0[index];
00581     vp1 = V1[index];
00582     vp2 = V2[index];
00583 
00584     up0 = U0[index];
00585     up1 = U1[index];
00586     up2 = U2[index];
00587 
00588 
00589     {
00590       if (dv0dv1 > 0.0f)
00591         {
00592           a = vp2;
00593           b = (vp0 - vp2) * dv2;
00594           c = (vp1 - vp2) * dv2;
00595           x0 = dv2 - dv0;
00596           x1 = dv2 - dv1;
00597         }
00598       else if (dv0dv2 > 0.0f)
00599         {
00600           a = vp1;
00601           b = (vp0 - vp1) * dv1;
00602           c = (vp2 - vp1) * dv1;
00603           x0 = dv1 - dv0;
00604           x1 = dv1 - dv2;
00605         }
00606       else if (dv1 * dv2 > 0.0f || dv0 != 0.0f)
00607         {
00608           a = vp0;
00609           b = (vp1 - vp0) * dv0;
00610           c = (vp2 - vp0) * dv0;
00611           x0 = dv0 - dv1;
00612           x1 = dv0 - dv2;
00613         }
00614       else if (dv1 != 0.0f)
00615         {
00616           a = vp1;
00617           b = (vp0 - vp1) * dv1;
00618           c = (vp2 - vp1) * dv1;
00619           x0 = dv1 - dv0;
00620           x1 = dv1 - dv2;
00621         }
00622       else if (dv2 != 0.0f)
00623         {
00624           a = vp2;
00625           b = (vp0 - vp2) * dv2;
00626           c = (vp1 - vp2) * dv2;
00627           x0 = dv2 - dv0;
00628           x1 = dv2 - dv1;
00629         }
00630       else
00631         {
00632           return intersectp_coplanar_triangles (N1, V0, V1, V2, U0, U1, U2);
00633         }
00634     };
00635 
00636 
00637     {
00638       if (du0du1 > 0.0f)
00639         {
00640           d = up2;
00641           e = (up0 - up2) * du2;
00642           f = (up1 - up2) * du2;
00643           y0 = du2 - du0;
00644           y1 = du2 - du1;
00645         }
00646       else if (du0du2 > 0.0f)
00647         {
00648           d = up1;
00649           e = (up0 - up1) * du1;
00650           f = (up2 - up1) * du1;
00651           y0 = du1 - du0;
00652           y1 = du1 - du2;
00653         }
00654       else if (du1 * du2 > 0.0f || du0 != 0.0f)
00655         {
00656           d = up0;
00657           e = (up1 - up0) * du0;
00658           f = (up2 - up0) * du0;
00659           y0 = du0 - du1;
00660           y1 = du0 - du2;
00661         }
00662       else if (du1 != 0.0f)
00663         {
00664           d = up1;
00665           e = (up0 - up1) * du1;
00666           f = (up2 - up1) * du1;
00667           y0 = du1 - du0;
00668           y1 = du1 - du2;
00669         }
00670       else if (du2 != 0.0f)
00671         {
00672           d = up2;
00673           e = (up0 - up2) * du2;
00674           f = (up1 - up2) * du2;
00675           y0 = du2 - du0;
00676           y1 = du2 - du1;
00677         }
00678       else
00679         {
00680           return intersectp_coplanar_triangles (N1, V0, V1, V2, U0, U1, U2);
00681         }
00682     };
00683 
00684     xx = x0 * x1;
00685     yy = y0 * y1;
00686     xxyy = xx * yy;
00687 
00688     tmp = a * xxyy;
00689     isect1[0] = tmp + b * x1 * yy;
00690     isect1[1] = tmp + c * x0 * yy;
00691 
00692     tmp = d * xxyy;
00693     isect2[0] = tmp + e * xx * y1;
00694     isect2[1] = tmp + f * xx * y0;
00695 
00696     if (isect1[0] > isect1[1])
00697       {
00698         typename Point3::value_type c;
00699         c = isect1[0];
00700         isect1[0] = isect1[1];
00701         isect1[1] = c;
00702       };
00703     if (isect2[0] > isect2[1])
00704       {
00705         typename Point3::value_type c;
00706         c = isect2[0];
00707         isect2[0] = isect2[1];
00708         isect2[1] = c;
00709       };
00710 
00711     if (isect1[1] < isect2[0] || isect2[1] < isect1[0])
00712       return false;
00713     return true;
00714   }
00715 
00716   template<class Point3>
00717   inline void
00718   isect2 (const Point3& VTX0, const Point3& VTX1, const Point3& VTX2, 
00719           typename Point3::value_type VV0, typename Point3::value_type VV1, typename Point3::value_type VV2, 
00720           typename Point3::value_type D0, typename Point3::value_type D1, typename Point3::value_type D2, typename Point3::value_type& isect0, typename Point3::value_type& isect1,
00721           Point3& isectpoint0, Point3& isectpoint1)
00722   {
00723     typename Point3::value_type tmp = D0 / (D0 - D1);
00724     Point3 diff;
00725     isect0 = VV0 + (VV1 - VV0) * tmp;
00726     diff[0] = VTX1[0] - VTX0[0];
00727     diff[1] = VTX1[1] - VTX0[1];
00728     diff[2] = VTX1[2] - VTX0[2];;
00729     diff[0] = tmp * diff[0];
00730     diff[1] = tmp * diff[1];
00731     diff[2] = tmp * diff[2];;
00732     isectpoint0[0] = diff[0] + VTX0[0];
00733     isectpoint0[1] = diff[1] + VTX0[1];
00734     isectpoint0[2] = diff[2] + VTX0[2];;
00735     tmp = D0 / (D0 - D2);
00736     isect1 = VV0 + (VV2 - VV0) * tmp;
00737     diff[0] = VTX2[0] - VTX0[0];
00738     diff[1] = VTX2[1] - VTX0[1];
00739     diff[2] = VTX2[2] - VTX0[2];;
00740     diff[0] = tmp * diff[0];
00741     diff[1] = tmp * diff[1];
00742     diff[2] = tmp * diff[2];;
00743     isectpoint1[0] = VTX0[0] + diff[0];
00744     isectpoint1[1] = VTX0[1] + diff[1];
00745     isectpoint1[2] = VTX0[2] + diff[2];;
00746   }
00747 
00748   template<class Point3>
00749   inline bool
00750   compute_intervals_isectline 
00751   (const Point3& VERT0, const Point3& VERT1, const Point3& VERT2,
00752    const typename Point3::value_type& VV0, const typename Point3::value_type& VV1, const typename Point3::value_type& VV2, const typename Point3::value_type& D0,
00753    const typename Point3::value_type& D1, const typename Point3::value_type& D2, const typename Point3::value_type& D0D1, const typename Point3::value_type& D0D2,
00754    typename Point3::value_type& isect0, typename Point3::value_type& isect1,
00755    Point3& isectpoint0, Point3& isectpoint1)
00756   {
00757     if (D0D1 > 0.0f)
00758       {
00759         isect2 (VERT2, VERT0, VERT1, VV2, VV0, VV1, D2, D0, D1, isect0, isect1,
00760                 isectpoint0, isectpoint1);
00761       }
00762     else if (D0D2 > 0.0f)
00763       {
00764 
00765         isect2 (VERT1, VERT0, VERT2, VV1, VV0, VV2, D1, D0, D2, isect0, isect1,
00766                 isectpoint0, isectpoint1);
00767       }
00768     else if (D1 * D2 > 0.0f || D0 != 0.0f)
00769       {
00770 
00771         isect2 (VERT0, VERT1, VERT2, VV0, VV1, VV2, D0, D1, D2, isect0, isect1,
00772                 isectpoint0, isectpoint1);
00773       }
00774     else if (D1 != 0.0f)
00775       {
00776         isect2 (VERT1, VERT0, VERT2, VV1, VV0, VV2, D1, D0, D2, isect0, isect1,
00777                 isectpoint0, isectpoint1);
00778       }
00779     else if (D2 != 0.0f)
00780       {
00781         isect2 (VERT2, VERT0, VERT1, VV2, VV0, VV1, D2, D0, D1, isect0, isect1,
00782                 isectpoint0, isectpoint1);
00783       }
00784     else
00785       {
00786 
00787         return true;
00788       }
00789     return false;
00790   }
00791 
00792 
00793 
00794 
00795   template<class Point3>
00796   bool 
00797   intersectp_triangles3_isegment 
00798   (bool& coplanar, 
00799    Point3& isectpt1, Point3& isectpt2,
00800    const Point3& V0, const Point3& V1, const Point3& V2,
00801    const Point3& U0, const Point3& U1, const Point3& U2,
00802    const typename Point3::value_type& prec )
00803   {
00804     coplanar = false;
00805     Point3 E1, E2;
00806     Point3 N1, N2;
00807     typename Point3::value_type d1, d2;
00808     typename Point3::value_type du0, du1, du2, dv0, dv1, dv2;
00809     Point3 D;
00810     typename Point3::value_type isect1[2], isect2[2];
00811     Point3 isectpointA1; 
00812     Point3 isectpointA2;
00813     Point3 isectpointB1;
00814     Point3 isectpointB2;
00815     typename Point3::value_type du0du1, du0du2, dv0dv1, dv0dv2;
00816     short index;
00817     typename Point3::value_type vp0, vp1, vp2;
00818     typename Point3::value_type up0, up1, up2;
00819     typename Point3::value_type b, c, max;
00820     //typename Point3::value_type tmp, diff[3];
00821     int smallest1, smallest2;
00822 
00823 
00824     E1[0] = V1[0] - V0[0];
00825     E1[1] = V1[1] - V0[1];
00826     E1[2] = V1[2] - V0[2];;
00827     E2[0] = V2[0] - V0[0];
00828     E2[1] = V2[1] - V0[1];
00829     E2[2] = V2[2] - V0[2];;
00830     N1[0] = E1[1] * E2[2] - E1[2] * E2[1];
00831     N1[1] = E1[2] * E2[0] - E1[0] * E2[2];
00832     N1[2] = E1[0] * E2[1] - E1[1] * E2[0];;
00833     d1 = -(N1[0] * V0[0] + N1[1] * V0[1] + N1[2] * V0[2]);
00834 
00835 
00836 
00837     du0 = (N1[0] * U0[0] + N1[1] * U0[1] + N1[2] * U0[2]) + d1;
00838     du1 = (N1[0] * U1[0] + N1[1] * U1[1] + N1[2] * U1[2]) + d1;
00839     du2 = (N1[0] * U2[0] + N1[1] * U2[1] + N1[2] * U2[2]) + d1;
00840 
00841     
00842     using namespace std;
00843     if (std::abs (du0) < prec)
00844       du0 = 0.0;
00845     if (std::abs (du1) < prec)
00846       du1 = 0.0;
00847     if (std::abs (du2) < prec)
00848       du2 = 0.0;
00849 
00850     du0du1 = du0 * du1;
00851     du0du2 = du0 * du2;
00852 
00853     if (du0du1 > 0.0f && du0du2 > 0.0f)
00854       return false;
00855 
00856 
00857     E1[0] = U1[0] - U0[0];
00858     E1[1] = U1[1] - U0[1];
00859     E1[2] = U1[2] - U0[2];;
00860     E2[0] = U2[0] - U0[0];
00861     E2[1] = U2[1] - U0[1];
00862     E2[2] = U2[2] - U0[2];;
00863     N2[0] = E1[1] * E2[2] - E1[2] * E2[1];
00864     N2[1] = E1[2] * E2[0] - E1[0] * E2[2];
00865     N2[2] = E1[0] * E2[1] - E1[1] * E2[0];;
00866     d2 = -(N2[0] * U0[0] + N2[1] * U0[1] + N2[2] * U0[2]);
00867 
00868 
00869 
00870     dv0 = (N2[0] * V0[0] + N2[1] * V0[1] + N2[2] * V0[2]) + d2;
00871     dv1 = (N2[0] * V1[0] + N2[1] * V1[1] + N2[2] * V1[2]) + d2;
00872     dv2 = (N2[0] * V2[0] + N2[1] * V2[1] + N2[2] * V2[2]) + d2;
00873 
00874 
00875     if (std::abs (dv0) < prec)
00876       dv0 = 0.0;
00877     if (std::abs (dv1) < prec)
00878       dv1 = 0.0;
00879     if (std::abs (dv2) < prec)
00880       dv2 = 0.0;
00881 
00882 
00883     dv0dv1 = dv0 * dv1;
00884     dv0dv2 = dv0 * dv2;
00885 
00886     if (dv0dv1 > 0.0f && dv0dv2 > 0.0f)
00887       return false;
00888 
00889 
00890     D[0] = N1[1] * N2[2] - N1[2] * N2[1];
00891     D[1] = N1[2] * N2[0] - N1[0] * N2[2];
00892     D[2] = N1[0] * N2[1] - N1[1] * N2[0];;
00893 
00894 
00895     max = std::abs (D[0]);
00896     index = 0;
00897     b = std::abs (D[1]);
00898     c = std::abs (D[2]);
00899     if (b > max)
00900       max = b, index = 1;
00901     if (c > max)
00902       max = c, index = 2;
00903 
00904 
00905     vp0 = V0[index];
00906     vp1 = V1[index];
00907     vp2 = V2[index];
00908 
00909     up0 = U0[index];
00910     up1 = U1[index];
00911     up2 = U2[index];
00912 
00913 
00914     coplanar =
00915       compute_intervals_isectline (V0, V1, V2, vp0, vp1, vp2, dv0, dv1, dv2,
00916                                    dv0dv1, dv0dv2, isect1[0], isect1[1],
00917                                    isectpointA1, isectpointA2);
00918     if (coplanar)
00919       return intersectp_coplanar_triangles (N1, V0, V1, V2, U0, U1, U2);
00920 
00921     
00922     
00923     compute_intervals_isectline (U0, U1, U2, up0, up1, up2, du0, du1, du2,
00924                                  du0du1, du0du2, isect2[0], isect2[1],
00925                                  isectpointB1, isectpointB2);
00926     
00927     if (isect1[0] > isect1[1])
00928       {
00929         typename Point3::value_type c;
00930         c = isect1[0];
00931         isect1[0] = isect1[1];
00932         isect1[1] = c;
00933         smallest1 = 1;
00934       }
00935     else
00936       smallest1 = 0;;
00937     if (isect2[0] > isect2[1])
00938       {
00939         typename Point3::value_type c;
00940         c = isect2[0];
00941         isect2[0] = isect2[1];
00942         isect2[1] = c;
00943         smallest2 = 1;
00944       }
00945     else
00946       smallest2 = 0;;
00947 
00948     if (isect1[1] < isect2[0] || isect2[1] < isect1[0])
00949       return false;
00950 
00951 
00952 
00953     if (isect2[0] < isect1[0])
00954       {
00955         if (smallest1 == 0)
00956           {
00957             isectpt1[0] = isectpointA1[0];
00958             isectpt1[1] = isectpointA1[1];
00959             isectpt1[2] = isectpointA1[2];;
00960           }
00961         else
00962           {
00963             isectpt1[0] = isectpointA2[0];
00964             isectpt1[1] = isectpointA2[1];
00965             isectpt1[2] = isectpointA2[2];;
00966           }
00967 
00968         if (isect2[1] < isect1[1])
00969           {
00970             if (smallest2 == 0)
00971               {
00972                 isectpt2[0] = isectpointB2[0];
00973                 isectpt2[1] = isectpointB2[1];
00974                 isectpt2[2] = isectpointB2[2];;
00975               }
00976             else
00977               {
00978                 isectpt2[0] = isectpointB1[0];
00979                 isectpt2[1] = isectpointB1[1];
00980                 isectpt2[2] = isectpointB1[2];;
00981               }
00982           }
00983         else
00984           {
00985             if (smallest1 == 0)
00986               {
00987                 isectpt2[0] = isectpointA2[0];
00988                 isectpt2[1] = isectpointA2[1];
00989                 isectpt2[2] = isectpointA2[2];;
00990               }
00991             else
00992               {
00993                 isectpt2[0] = isectpointA1[0];
00994                 isectpt2[1] = isectpointA1[1];
00995                 isectpt2[2] = isectpointA1[2];;
00996               }
00997           }
00998       }
00999     else
01000       {
01001         if (smallest2 == 0)
01002           {
01003             isectpt1[0] = isectpointB1[0];
01004             isectpt1[1] = isectpointB1[1];
01005             isectpt1[2] = isectpointB1[2];;
01006           }
01007         else
01008           {
01009             isectpt1[0] = isectpointB2[0];
01010             isectpt1[1] = isectpointB2[1];
01011             isectpt1[2] = isectpointB2[2];;
01012           }
01013 
01014         if (isect2[1] > isect1[1])
01015           {
01016             if (smallest1 == 0)
01017               {
01018                 isectpt2[0] = isectpointA2[0];
01019                 isectpt2[1] = isectpointA2[1];
01020                 isectpt2[2] = isectpointA2[2];;
01021               }
01022             else
01023               {
01024                 isectpt2[0] = isectpointA1[0];
01025                 isectpt2[1] = isectpointA1[1];
01026                 isectpt2[2] = isectpointA1[2];;
01027               }
01028           }
01029         else
01030           {
01031             if (smallest2 == 0)
01032               {
01033                 isectpt2[0] = isectpointB2[0];
01034                 isectpt2[1] = isectpointB2[1];
01035                 isectpt2[2] = isectpointB2[2];;
01036               }
01037             else
01038               {
01039                 isectpt2[0] = isectpointB1[0];
01040                 isectpt2[1] = isectpointB1[1];
01041                 isectpt2[2] = isectpointB1[2];;
01042               }
01043           }
01044       }
01045     return true;
01046   }
01047