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