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