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
00812 isect2[0]=0;isect2[1]=0;
00813
00814 Point3 isectpointA1;
00815 Point3 isectpointA2;
00816
00817
00818 typename Point3::value_type du0du1, du0du2, dv0dv1, dv0dv2;
00819 short index;
00820 typename Point3::value_type vp0, vp1, vp2;
00821 typename Point3::value_type up0, up1, up2;
00822 typename Point3::value_type b, c, max;
00823
00824 int smallest1, smallest2;
00825
00826
00827 E1[0] = V1[0] - V0[0];
00828 E1[1] = V1[1] - V0[1];
00829 E1[2] = V1[2] - V0[2];;
00830 E2[0] = V2[0] - V0[0];
00831 E2[1] = V2[1] - V0[1];
00832 E2[2] = V2[2] - V0[2];;
00833 N1[0] = E1[1] * E2[2] - E1[2] * E2[1];
00834 N1[1] = E1[2] * E2[0] - E1[0] * E2[2];
00835 N1[2] = E1[0] * E2[1] - E1[1] * E2[0];;
00836 d1 = -(N1[0] * V0[0] + N1[1] * V0[1] + N1[2] * V0[2]);
00837
00838
00839
00840 du0 = (N1[0] * U0[0] + N1[1] * U0[1] + N1[2] * U0[2]) + d1;
00841 du1 = (N1[0] * U1[0] + N1[1] * U1[1] + N1[2] * U1[2]) + d1;
00842 du2 = (N1[0] * U2[0] + N1[1] * U2[1] + N1[2] * U2[2]) + d1;
00843
00844
00845 using namespace std;
00846 if (std::abs (du0) < prec)
00847 du0 = 0.0;
00848 if (std::abs (du1) < prec)
00849 du1 = 0.0;
00850 if (std::abs (du2) < prec)
00851 du2 = 0.0;
00852
00853 du0du1 = du0 * du1;
00854 du0du2 = du0 * du2;
00855
00856 if (du0du1 > 0.0f && du0du2 > 0.0f)
00857 return false;
00858
00859
00860 E1[0] = U1[0] - U0[0];
00861 E1[1] = U1[1] - U0[1];
00862 E1[2] = U1[2] - U0[2];;
00863 E2[0] = U2[0] - U0[0];
00864 E2[1] = U2[1] - U0[1];
00865 E2[2] = U2[2] - U0[2];;
00866 N2[0] = E1[1] * E2[2] - E1[2] * E2[1];
00867 N2[1] = E1[2] * E2[0] - E1[0] * E2[2];
00868 N2[2] = E1[0] * E2[1] - E1[1] * E2[0];;
00869 d2 = -(N2[0] * U0[0] + N2[1] * U0[1] + N2[2] * U0[2]);
00870
00871
00872
00873 dv0 = (N2[0] * V0[0] + N2[1] * V0[1] + N2[2] * V0[2]) + d2;
00874 dv1 = (N2[0] * V1[0] + N2[1] * V1[1] + N2[2] * V1[2]) + d2;
00875 dv2 = (N2[0] * V2[0] + N2[1] * V2[1] + N2[2] * V2[2]) + d2;
00876
00877
00878 if (std::abs (dv0) < prec)
00879 dv0 = 0.0;
00880 if (std::abs (dv1) < prec)
00881 dv1 = 0.0;
00882 if (std::abs (dv2) < prec)
00883 dv2 = 0.0;
00884
00885
00886 dv0dv1 = dv0 * dv1;
00887 dv0dv2 = dv0 * dv2;
00888
00889 if (dv0dv1 > 0.0f && dv0dv2 > 0.0f)
00890 return false;
00891
00892
00893 D[0] = N1[1] * N2[2] - N1[2] * N2[1];
00894 D[1] = N1[2] * N2[0] - N1[0] * N2[2];
00895 D[2] = N1[0] * N2[1] - N1[1] * N2[0];;
00896
00897
00898 max = std::abs (D[0]);
00899 index = 0;
00900 b = std::abs (D[1]);
00901 c = std::abs (D[2]);
00902 if (b > max)
00903 max = b, index = 1;
00904 if (c > max)
00905 max = c, index = 2;
00906
00907
00908 vp0 = V0[index];
00909 vp1 = V1[index];
00910 vp2 = V2[index];
00911
00912 up0 = U0[index];
00913 up1 = U1[index];
00914 up2 = U2[index];
00915
00916
00917 coplanar =
00918 compute_intervals_isectline (V0, V1, V2, vp0, vp1, vp2, dv0, dv1, dv2,
00919 dv0dv1, dv0dv2, isect1[0], isect1[1],
00920 isectpointA1, isectpointA2);
00921 if (coplanar)
00922 return intersectp_coplanar_triangles (N1, V0, V1, V2, U0, U1, U2);
00923
00924
00925
00926 Point3 isectpointB1;isectpointB1[0]=0;isectpointB1[1]=0;isectpointB1[2]=0;
00927 Point3 isectpointB2;isectpointB2[0]=0;isectpointB2[1]=0;isectpointB2[2]=0;
00928
00929 compute_intervals_isectline (U0, U1, U2, up0, up1, up2, du0, du1, du2,
00930 du0du1, du0du2, isect2[0], isect2[1],
00931 isectpointB1, isectpointB2);
00932
00933 if (isect1[0] > isect1[1])
00934 {
00935 typename Point3::value_type c;
00936 c = isect1[0];
00937 isect1[0] = isect1[1];
00938 isect1[1] = c;
00939 smallest1 = 1;
00940 }
00941 else
00942 smallest1 = 0;;
00943 if (isect2[0] > isect2[1])
00944 {
00945 typename Point3::value_type c;
00946 c = isect2[0];
00947 isect2[0] = isect2[1];
00948 isect2[1] = c;
00949 smallest2 = 1;
00950 }
00951 else
00952 smallest2 = 0;;
00953
00954 if (isect1[1] < isect2[0] || isect2[1] < isect1[0])
00955 return false;
00956
00957
00958
00959 if (isect2[0] < isect1[0])
00960 {
00961 if (smallest1 == 0)
00962 {
00963 isectpt1[0] = isectpointA1[0];
00964 isectpt1[1] = isectpointA1[1];
00965 isectpt1[2] = isectpointA1[2];;
00966 }
00967 else
00968 {
00969 isectpt1[0] = isectpointA2[0];
00970 isectpt1[1] = isectpointA2[1];
00971 isectpt1[2] = isectpointA2[2];;
00972 }
00973
00974 if (isect2[1] < isect1[1])
00975 {
00976 if (smallest2 == 0)
00977 {
00978 isectpt2[0] = isectpointB2[0];
00979 isectpt2[1] = isectpointB2[1];
00980 isectpt2[2] = isectpointB2[2];;
00981 }
00982 else
00983 {
00984 isectpt2[0] = isectpointB1[0];
00985 isectpt2[1] = isectpointB1[1];
00986 isectpt2[2] = isectpointB1[2];;
00987 }
00988 }
00989 else
00990 {
00991 if (smallest1 == 0)
00992 {
00993 isectpt2[0] = isectpointA2[0];
00994 isectpt2[1] = isectpointA2[1];
00995 isectpt2[2] = isectpointA2[2];;
00996 }
00997 else
00998 {
00999 isectpt2[0] = isectpointA1[0];
01000 isectpt2[1] = isectpointA1[1];
01001 isectpt2[2] = isectpointA1[2];;
01002 }
01003 }
01004 }
01005 else
01006 {
01007 if (smallest2 == 0)
01008 {
01009 isectpt1[0] = isectpointB1[0];
01010 isectpt1[1] = isectpointB1[1];
01011 isectpt1[2] = isectpointB1[2];;
01012 }
01013 else
01014 {
01015 isectpt1[0] = isectpointB2[0];
01016 isectpt1[1] = isectpointB2[1];
01017 isectpt1[2] = isectpointB2[2];;
01018 }
01019
01020 if (isect2[1] > isect1[1])
01021 {
01022 if (smallest1 == 0)
01023 {
01024 isectpt2[0] = isectpointA2[0];
01025 isectpt2[1] = isectpointA2[1];
01026 isectpt2[2] = isectpointA2[2];;
01027 }
01028 else
01029 {
01030 isectpt2[0] = isectpointA1[0];
01031 isectpt2[1] = isectpointA1[1];
01032 isectpt2[2] = isectpointA1[2];;
01033 }
01034 }
01035 else
01036 {
01037 if (smallest2 == 0)
01038 {
01039 isectpt2[0] = isectpointB2[0];
01040 isectpt2[1] = isectpointB2[1];
01041 isectpt2[2] = isectpointB2[2];;
01042 }
01043 else
01044 {
01045 isectpt2[0] = isectpointB1[0];
01046 isectpt2[1] = isectpointB1[1];
01047 isectpt2[2] = isectpointB1[2];;
01048 }
01049 }
01050 }
01051 return true;
01052 }
01053
