GRASS Programmer's Manual
6.4.1(2011)
|
00001 #include <stdlib.h> 00002 #include <math.h> 00003 #include <grass/gis.h> 00004 #include "e_intersect.h" 00005 00006 #define SWAP(a,b) {t = a; a = b; b = t;} 00007 #ifndef MIN 00008 #define MIN(X,Y) ((X<Y)?X:Y) 00009 #endif 00010 #ifndef MAX 00011 #define MAX(X,Y) ((X>Y)?X:Y) 00012 #endif 00013 #define D (ax2-ax1)*(by1-by2) - (ay2-ay1)*(bx1-bx2) 00014 #define DA (bx1-ax1)*(by1-by2) - (by1-ay1)*(bx1-bx2) 00015 #define DB (ax2-ax1)*(by1-ay1) - (ay2-ay1)*(bx1-ax1) 00016 00017 00018 #ifdef ASDASDASFDSAFFDAS 00019 mpf_t p11, p12, p21, p22, t1, t2; 00020 00021 mpf_t dd, dda, ddb, delta; 00022 00023 mpf_t rra, rrb; 00024 00025 int initialized = 0; 00026 00027 void initialize_mpf_vars() 00028 { 00029 mpf_set_default_prec(2048); 00030 00031 mpf_init(p11); 00032 mpf_init(p12); 00033 mpf_init(p21); 00034 mpf_init(p22); 00035 00036 mpf_init(t1); 00037 mpf_init(t2); 00038 00039 mpf_init(dd); 00040 mpf_init(dda); 00041 mpf_init(ddb); 00042 mpf_init(delta); 00043 00044 mpf_init(rra); 00045 mpf_init(rrb); 00046 00047 initialized = 1; 00048 } 00049 00050 /* 00051 Caclulates: 00052 |a11-b11 a12-b12| 00053 |a21-b21 a22-b22| 00054 */ 00055 void det22(mpf_t rop, double a11, double b11, double a12, double b12, 00056 double a21, double b21, double a22, double b22) 00057 { 00058 mpf_set_d(t1, a11); 00059 mpf_set_d(t2, b11); 00060 mpf_sub(p11, t1, t2); 00061 mpf_set_d(t1, a12); 00062 mpf_set_d(t2, b12); 00063 mpf_sub(p12, t1, t2); 00064 mpf_set_d(t1, a21); 00065 mpf_set_d(t2, b21); 00066 mpf_sub(p21, t1, t2); 00067 mpf_set_d(t1, a22); 00068 mpf_set_d(t2, b22); 00069 mpf_sub(p22, t1, t2); 00070 00071 mpf_mul(t1, p11, p22); 00072 mpf_mul(t2, p12, p21); 00073 mpf_sub(rop, t1, t2); 00074 00075 return; 00076 } 00077 00078 void swap(double *a, double *b) 00079 { 00080 double t = *a; 00081 00082 *a = *b; 00083 *b = t; 00084 return; 00085 } 00086 00087 /* multi-precision version */ 00088 int segment_intersection_2d_e(double ax1, double ay1, double ax2, double ay2, 00089 double bx1, double by1, double bx2, double by2, 00090 double *x1, double *y1, double *x2, double *y2) 00091 { 00092 double t; 00093 00094 double max_ax, min_ax, max_ay, min_ay; 00095 00096 double max_bx, min_bx, max_by, min_by; 00097 00098 int sgn_d, sgn_da, sgn_db; 00099 00100 int vertical; 00101 00102 int f11, f12, f21, f22; 00103 00104 mp_exp_t exp; 00105 00106 char *s; 00107 00108 if (!initialized) 00109 initialize_mpf_vars(); 00110 00111 /* TODO: Works for points ? */ 00112 G_debug(3, "segment_intersection_2d_e()"); 00113 G_debug(4, " ax1 = %.18f, ay1 = %.18f", ax1, ay1); 00114 G_debug(4, " ax2 = %.18f, ay2 = %.18f", ax2, ay2); 00115 G_debug(4, " bx1 = %.18f, by1 = %.18f", bx1, by1); 00116 G_debug(4, " bx2 = %.18f, by2 = %.18f", bx2, by2); 00117 00118 f11 = ((ax1 == bx1) && (ay1 == by1)); 00119 f12 = ((ax1 == bx2) && (ay1 == by2)); 00120 f21 = ((ax2 == bx1) && (ay2 == by1)); 00121 f22 = ((ax2 == bx2) && (ay2 == by2)); 00122 00123 /* Check for identical segments */ 00124 if ((f11 && f22) || (f12 && f21)) { 00125 G_debug(3, " identical segments"); 00126 *x1 = ax1; 00127 *y1 = ay1; 00128 *x2 = ax2; 00129 *y2 = ay2; 00130 return 5; 00131 } 00132 /* Check for identical endpoints */ 00133 if (f11 || f12) { 00134 G_debug(3, " connected by endpoints"); 00135 *x1 = ax1; 00136 *y1 = ay1; 00137 return 1; 00138 } 00139 if (f21 || f22) { 00140 G_debug(3, " connected by endpoints"); 00141 *x1 = ax2; 00142 *y1 = ay2; 00143 return 1; 00144 } 00145 00146 if ((MAX(ax1, ax2) < MIN(bx1, bx2)) || (MAX(bx1, bx2) < MIN(ax1, ax2))) { 00147 G_debug(3, " no intersection (disjoint bounding boxes)"); 00148 return 0; 00149 } 00150 if ((MAX(ay1, ay2) < MIN(by1, by2)) || (MAX(by1, by2) < MIN(ay1, ay2))) { 00151 G_debug(3, " no intersection (disjoint bounding boxes)"); 00152 return 0; 00153 } 00154 00155 det22(dd, ax2, ax1, bx1, bx2, ay2, ay1, by1, by2); 00156 sgn_d = mpf_sgn(dd); 00157 if (sgn_d != 0) { 00158 G_debug(3, " general position"); 00159 00160 det22(dda, bx1, ax1, bx1, bx2, by1, ay1, by1, by2); 00161 sgn_da = mpf_sgn(dda); 00162 00163 /*mpf_div(rra, dda, dd); 00164 mpf_div(rrb, ddb, dd); 00165 s = mpf_get_str(NULL, &exp, 10, 40, rra); 00166 G_debug(4, " ra = %sE%d", (s[0]==0)?"0":s, exp); 00167 s = mpf_get_str(NULL, &exp, 10, 24, rrb); 00168 G_debug(4, " rb = %sE%d", (s[0]==0)?"0":s, exp); 00169 */ 00170 00171 if (sgn_d > 0) { 00172 if ((sgn_da < 0) || (mpf_cmp(dda, dd) > 0)) { 00173 G_debug(3, " no intersection"); 00174 return 0; 00175 } 00176 00177 det22(ddb, ax2, ax1, bx1, ax1, ay2, ay1, by1, ay1); 00178 sgn_db = mpf_sgn(ddb); 00179 if ((sgn_db < 0) || (mpf_cmp(ddb, dd) > 0)) { 00180 G_debug(3, " no intersection"); 00181 return 0; 00182 } 00183 } 00184 else { /* if sgn_d < 0 */ 00185 if ((sgn_da > 0) || (mpf_cmp(dda, dd) < 0)) { 00186 G_debug(3, " no intersection"); 00187 return 0; 00188 } 00189 00190 det22(ddb, ax2, ax1, bx1, ax1, ay2, ay1, by1, ay1); 00191 sgn_db = mpf_sgn(ddb); 00192 if ((sgn_db > 0) || (mpf_cmp(ddb, dd) < 0)) { 00193 G_debug(3, " no intersection"); 00194 return 0; 00195 } 00196 } 00197 00198 /*G_debug(3, " ra=%.17g rb=%.17g", mpf_get_d(dda)/mpf_get_d(dd), mpf_get_d(ddb)/mpf_get_d(dd)); */ 00199 /*G_debug(3, " sgn_d=%d sgn_da=%d sgn_db=%d cmp(dda,dd)=%d cmp(ddb,dd)=%d", sgn_d, sgn_da, sgn_db, mpf_cmp(dda, dd), mpf_cmp(ddb, dd)); */ 00200 00201 mpf_set_d(delta, ax2 - ax1); 00202 mpf_mul(t1, dda, delta); 00203 mpf_div(t2, t1, dd); 00204 *x1 = ax1 + mpf_get_d(t2); 00205 00206 mpf_set_d(delta, ay2 - ay1); 00207 mpf_mul(t1, dda, delta); 00208 mpf_div(t2, t1, dd); 00209 *y1 = ay1 + mpf_get_d(t2); 00210 00211 G_debug(3, " intersection %.16g, %.16g", *x1, *y1); 00212 return 1; 00213 } 00214 00215 /* segments are parallel or collinear */ 00216 det22(dda, bx1, ax1, bx1, bx2, by1, ay1, by1, by2); 00217 sgn_da = mpf_sgn(dda); 00218 if (sgn_da != 0) { 00219 /* segments are parallel */ 00220 G_debug(3, " parallel segments"); 00221 return 0; 00222 } 00223 00224 /* segments are colinear. check for overlap */ 00225 00226 /* swap endpoints if needed */ 00227 /* if segments are vertical, we swap x-coords with y-coords */ 00228 vertical = 0; 00229 if (ax1 > ax2) { 00230 SWAP(ax1, ax2); 00231 SWAP(ay1, ay2); 00232 } 00233 else if (ax1 == ax2) { 00234 vertical = 1; 00235 if (ay1 > ay2) 00236 SWAP(ay1, ay2); 00237 SWAP(ax1, ay1); 00238 SWAP(ax2, ay2); 00239 } 00240 if (bx1 > bx2) { 00241 SWAP(bx1, bx2); 00242 SWAP(by1, by2); 00243 } 00244 else if (bx1 == bx2) { 00245 if (by1 > by2) 00246 SWAP(by1, by2); 00247 SWAP(bx1, by1); 00248 SWAP(bx2, by2); 00249 } 00250 00251 G_debug(3, " collinear segments"); 00252 00253 if ((bx2 < ax1) || (bx1 > ax2)) { 00254 G_debug(3, " no intersection"); 00255 return 0; 00256 } 00257 00258 /* there is overlap or connected end points */ 00259 G_debug(3, " overlap"); 00260 00261 /* a contains b */ 00262 if ((ax1 < bx1) && (ax2 > bx2)) { 00263 G_debug(3, " a contains b"); 00264 if (!vertical) { 00265 *x1 = bx1; 00266 *y1 = by1; 00267 *x2 = bx2; 00268 *y2 = by2; 00269 } 00270 else { 00271 *x1 = by1; 00272 *y1 = bx1; 00273 *x2 = by2; 00274 *y2 = bx2; 00275 } 00276 return 3; 00277 } 00278 00279 /* b contains a */ 00280 if ((ax1 > bx1) && (ax2 < bx2)) { 00281 G_debug(3, " b contains a"); 00282 if (!vertical) { 00283 *x1 = bx1; 00284 *y1 = by1; 00285 *x2 = bx2; 00286 *y2 = by2; 00287 } 00288 else { 00289 *x1 = by1; 00290 *y1 = bx1; 00291 *x2 = by2; 00292 *y2 = bx2; 00293 } 00294 return 4; 00295 } 00296 00297 /* general overlap, 2 intersection points */ 00298 G_debug(3, " partial overlap"); 00299 if ((bx1 > ax1) && (bx1 < ax2)) { /* b1 is in a */ 00300 if (!vertical) { 00301 *x1 = bx1; 00302 *y1 = by1; 00303 *x2 = ax2; 00304 *y2 = ay2; 00305 } 00306 else { 00307 *x1 = by1; 00308 *y1 = bx1; 00309 *x2 = ay2; 00310 *y2 = ax2; 00311 } 00312 return 2; 00313 } 00314 if ((bx2 > ax1) && (bx2 < ax2)) { /* b2 is in a */ 00315 if (!vertical) { 00316 *x1 = bx2; 00317 *y1 = by2; 00318 *x2 = ax1; 00319 *y2 = ay1; 00320 } 00321 else { 00322 *x1 = by2; 00323 *y1 = bx2; 00324 *x2 = ay1; 00325 *y2 = ax1; 00326 } 00327 return 2; 00328 } 00329 00330 /* should not be reached */ 00331 G_warning(("segment_intersection_2d() ERROR (should not be reached)")); 00332 G_warning("%.16g %.16g", ax1, ay1); 00333 G_warning("%.16g %.16g", ax2, ay2); 00334 G_warning("x"); 00335 G_warning("%.16g %.16g", bx1, by1); 00336 G_warning("%.16g %.16g", bx2, by2); 00337 00338 return 0; 00339 } 00340 #endif 00341 00342 /* OLD */ 00343 /* tollerance aware version */ 00344 /* TODO: fix all ==s left */ 00345 int segment_intersection_2d_tol(double ax1, double ay1, double ax2, 00346 double ay2, double bx1, double by1, 00347 double bx2, double by2, double *x1, 00348 double *y1, double *x2, double *y2, 00349 double tol) 00350 { 00351 double tola, tolb; 00352 00353 double d, d1, d2, ra, rb, t; 00354 00355 int switched = 0; 00356 00357 /* TODO: Works for points ? */ 00358 G_debug(4, "segment_intersection_2d()"); 00359 G_debug(4, " ax1 = %.18f, ay1 = %.18f", ax1, ay1); 00360 G_debug(4, " ax2 = %.18f, ay2 = %.18f", ax2, ay2); 00361 G_debug(4, " bx1 = %.18f, by1 = %.18f", bx1, by1); 00362 G_debug(4, " bx2 = %.18f, by2 = %.18f", bx2, by2); 00363 00364 00365 /* Check identical lines */ 00366 if ((FEQUAL(ax1, bx1, tol) && FEQUAL(ay1, by1, tol) && 00367 FEQUAL(ax2, bx2, tol) && FEQUAL(ay2, by2, tol)) || 00368 (FEQUAL(ax1, bx2, tol) && FEQUAL(ay1, by2, tol) && 00369 FEQUAL(ax2, bx1, tol) && FEQUAL(ay2, by1, tol))) { 00370 G_debug(2, " -> identical segments"); 00371 *x1 = ax1; 00372 *y1 = ay1; 00373 *x2 = ax2; 00374 *y2 = ay2; 00375 return 5; 00376 } 00377 00378 /* 'Sort' lines by x1, x2, y1, y2 */ 00379 if (bx1 < ax1) 00380 switched = 1; 00381 else if (bx1 == ax1) { 00382 if (bx2 < ax2) 00383 switched = 1; 00384 else if (bx2 == ax2) { 00385 if (by1 < ay1) 00386 switched = 1; 00387 else if (by1 == ay1) { 00388 if (by2 < ay2) 00389 switched = 1; /* by2 != ay2 (would be identical */ 00390 } 00391 } 00392 } 00393 00394 if (switched) { 00395 t = ax1; 00396 ax1 = bx1; 00397 bx1 = t; 00398 t = ay1; 00399 ay1 = by1; 00400 by1 = t; 00401 t = ax2; 00402 ax2 = bx2; 00403 bx2 = t; 00404 t = ay2; 00405 ay2 = by2; 00406 by2 = t; 00407 } 00408 00409 d = (ax2 - ax1) * (by1 - by2) - (ay2 - ay1) * (bx1 - bx2); 00410 d1 = (bx1 - ax1) * (by1 - by2) - (by1 - ay1) * (bx1 - bx2); 00411 d2 = (ax2 - ax1) * (by1 - ay1) - (ay2 - ay1) * (bx1 - ax1); 00412 00413 G_debug(2, " d = %.18g", d); 00414 G_debug(2, " d1 = %.18g", d1); 00415 G_debug(2, " d2 = %.18g", d2); 00416 00417 tola = tol / MAX(fabs(ax2 - ax1), fabs(ay2 - ay1)); 00418 tolb = tol / MAX(fabs(bx2 - bx1), fabs(by2 - by1)); 00419 G_debug(2, " tol = %.18g", tol); 00420 G_debug(2, " tola = %.18g", tola); 00421 G_debug(2, " tolb = %.18g", tolb); 00422 if (!FZERO(d, tol)) { 00423 ra = d1 / d; 00424 rb = d2 / d; 00425 00426 G_debug(2, " not parallel/collinear: ra = %.18g", ra); 00427 G_debug(2, " rb = %.18g", rb); 00428 00429 if ((ra <= -tola) || (ra >= 1 + tola) || (rb <= -tolb) || 00430 (rb >= 1 + tolb)) { 00431 G_debug(2, " no intersection"); 00432 return 0; 00433 } 00434 00435 ra = MIN(MAX(ra, 0), 1); 00436 *x1 = ax1 + ra * (ax2 - ax1); 00437 *y1 = ay1 + ra * (ay2 - ay1); 00438 00439 G_debug(2, " intersection %.18f, %.18f", *x1, *y1); 00440 return 1; 00441 } 00442 00443 /* segments are parallel or collinear */ 00444 G_debug(3, " -> parallel/collinear"); 00445 00446 if ((!FZERO(d1, tol)) || (!FZERO(d2, tol))) { /* lines are parallel */ 00447 G_debug(2, " -> parallel"); 00448 return 0; 00449 } 00450 00451 /* segments are colinear. check for overlap */ 00452 00453 /* aa = adx*adx + ady*ady; 00454 bb = bdx*bdx + bdy*bdy; 00455 00456 t = (ax1-bx1)*bdx + (ay1-by1)*bdy; */ 00457 00458 00459 /* Collinear vertical */ 00460 /* original code assumed lines were not both vertical 00461 * so there is a special case if they are */ 00462 if (FEQUAL(ax1, ax2, tol) && FEQUAL(bx1, bx2, tol) && 00463 FEQUAL(ax1, bx1, tol)) { 00464 G_debug(2, " -> collinear vertical"); 00465 if (ay1 > ay2) { 00466 t = ay1; 00467 ay1 = ay2; 00468 ay2 = t; 00469 } /* to be sure that ay1 < ay2 */ 00470 if (by1 > by2) { 00471 t = by1; 00472 by1 = by2; 00473 by2 = t; 00474 } /* to be sure that by1 < by2 */ 00475 if (ay1 > by2 || ay2 < by1) { 00476 G_debug(2, " -> no intersection"); 00477 return 0; 00478 } 00479 00480 /* end points */ 00481 if (FEQUAL(ay1, by2, tol)) { 00482 *x1 = ax1; 00483 *y1 = ay1; 00484 G_debug(2, " -> connected by end points"); 00485 return 1; /* endpoints only */ 00486 } 00487 if (FEQUAL(ay2, by1, tol)) { 00488 *x1 = ax2; 00489 *y1 = ay2; 00490 G_debug(2, " -> connected by end points"); 00491 return 1; /* endpoints only */ 00492 } 00493 00494 /* general overlap */ 00495 G_debug(3, " -> vertical overlap"); 00496 /* a contains b */ 00497 if (ay1 <= by1 && ay2 >= by2) { 00498 G_debug(2, " -> a contains b"); 00499 *x1 = bx1; 00500 *y1 = by1; 00501 *x2 = bx2; 00502 *y2 = by2; 00503 if (!switched) 00504 return 3; 00505 else 00506 return 4; 00507 } 00508 /* b contains a */ 00509 if (ay1 >= by1 && ay2 <= by2) { 00510 G_debug(2, " -> b contains a"); 00511 *x1 = ax1; 00512 *y1 = ay1; 00513 *x2 = ax2; 00514 *y2 = ay2; 00515 if (!switched) 00516 return 4; 00517 else 00518 return 3; 00519 } 00520 00521 /* general overlap, 2 intersection points */ 00522 G_debug(2, " -> partial overlap"); 00523 if (by1 > ay1 && by1 < ay2) { /* b1 in a */ 00524 if (!switched) { 00525 *x1 = bx1; 00526 *y1 = by1; 00527 *x2 = ax2; 00528 *y2 = ay2; 00529 } 00530 else { 00531 *x1 = ax2; 00532 *y1 = ay2; 00533 *x2 = bx1; 00534 *y2 = by1; 00535 } 00536 return 2; 00537 } 00538 00539 if (by2 > ay1 && by2 < ay2) { /* b2 in a */ 00540 if (!switched) { 00541 *x1 = bx2; 00542 *y1 = by2; 00543 *x2 = ax1; 00544 *y2 = ay1; 00545 } 00546 else { 00547 *x1 = ax1; 00548 *y1 = ay1; 00549 *x2 = bx2; 00550 *y2 = by2; 00551 } 00552 return 2; 00553 } 00554 00555 /* should not be reached */ 00556 G_warning(("Vect_segment_intersection() ERROR (collinear vertical segments)")); 00557 G_warning("%.15g %.15g", ax1, ay1); 00558 G_warning("%.15g %.15g", ax2, ay2); 00559 G_warning("x"); 00560 G_warning("%.15g %.15g", bx1, by1); 00561 G_warning("%.15g %.15g", bx2, by2); 00562 return 0; 00563 } 00564 00565 G_debug(2, " -> collinear non vertical"); 00566 00567 /* Collinear non vertical */ 00568 if ((bx1 > ax1 && bx2 > ax1 && bx1 > ax2 && bx2 > ax2) || 00569 (bx1 < ax1 && bx2 < ax1 && bx1 < ax2 && bx2 < ax2)) { 00570 G_debug(2, " -> no intersection"); 00571 return 0; 00572 } 00573 00574 /* there is overlap or connected end points */ 00575 G_debug(2, " -> overlap/connected end points"); 00576 00577 /* end points */ 00578 if ((ax1 == bx1 && ay1 == by1) || (ax1 == bx2 && ay1 == by2)) { 00579 *x1 = ax1; 00580 *y1 = ay1; 00581 G_debug(2, " -> connected by end points"); 00582 return 1; 00583 } 00584 if ((ax2 == bx1 && ay2 == by1) || (ax2 == bx2 && ay2 == by2)) { 00585 *x1 = ax2; 00586 *y1 = ay2; 00587 G_debug(2, " -> connected by end points"); 00588 return 1; 00589 } 00590 00591 if (ax1 > ax2) { 00592 t = ax1; 00593 ax1 = ax2; 00594 ax2 = t; 00595 t = ay1; 00596 ay1 = ay2; 00597 ay2 = t; 00598 } /* to be sure that ax1 < ax2 */ 00599 if (bx1 > bx2) { 00600 t = bx1; 00601 bx1 = bx2; 00602 bx2 = t; 00603 t = by1; 00604 by1 = by2; 00605 by2 = t; 00606 } /* to be sure that bx1 < bx2 */ 00607 00608 /* a contains b */ 00609 if (ax1 <= bx1 && ax2 >= bx2) { 00610 G_debug(2, " -> a contains b"); 00611 *x1 = bx1; 00612 *y1 = by1; 00613 *x2 = bx2; 00614 *y2 = by2; 00615 if (!switched) 00616 return 3; 00617 else 00618 return 4; 00619 } 00620 /* b contains a */ 00621 if (ax1 >= bx1 && ax2 <= bx2) { 00622 G_debug(2, " -> b contains a"); 00623 *x1 = ax1; 00624 *y1 = ay1; 00625 *x2 = ax2; 00626 *y2 = ay2; 00627 if (!switched) 00628 return 4; 00629 else 00630 return 3; 00631 } 00632 00633 /* general overlap, 2 intersection points (lines are not vertical) */ 00634 G_debug(2, " -> partial overlap"); 00635 if (bx1 > ax1 && bx1 < ax2) { /* b1 is in a */ 00636 if (!switched) { 00637 *x1 = bx1; 00638 *y1 = by1; 00639 *x2 = ax2; 00640 *y2 = ay2; 00641 } 00642 else { 00643 *x1 = ax2; 00644 *y1 = ay2; 00645 *x2 = bx1; 00646 *y2 = by1; 00647 } 00648 return 2; 00649 } 00650 if (bx2 > ax1 && bx2 < ax2) { /* b2 is in a */ 00651 if (!switched) { 00652 *x1 = bx2; 00653 *y1 = by2; 00654 *x2 = ax1; 00655 *y2 = ay1; 00656 } 00657 else { 00658 *x1 = ax1; 00659 *y1 = ay1; 00660 *x2 = bx2; 00661 *y2 = by2; 00662 } 00663 return 2; 00664 } 00665 00666 /* should not be reached */ 00667 G_warning(("segment_intersection_2d() ERROR (collinear non vertical segments)")); 00668 G_warning("%.15g %.15g", ax1, ay1); 00669 G_warning("%.15g %.15g", ax2, ay2); 00670 G_warning("x"); 00671 G_warning("%.15g %.15g", bx1, by1); 00672 G_warning("%.15g %.15g", bx2, by2); 00673 00674 return 0; 00675 } 00676 00677 int segment_intersection_2d(double ax1, double ay1, double ax2, double ay2, 00678 double bx1, double by1, double bx2, double by2, 00679 double *x1, double *y1, double *x2, double *y2) 00680 { 00681 const int DLEVEL = 4; 00682 00683 double t; 00684 00685 int vertical; 00686 00687 int f11, f12, f21, f22; 00688 00689 double d, da, db; 00690 00691 /* TODO: Works for points ? */ 00692 G_debug(DLEVEL, "segment_intersection_2d()"); 00693 G_debug(4, " ax1 = %.18f, ay1 = %.18f", ax1, ay1); 00694 G_debug(4, " ax2 = %.18f, ay2 = %.18f", ax2, ay2); 00695 G_debug(4, " bx1 = %.18f, by1 = %.18f", bx1, by1); 00696 G_debug(4, " bx2 = %.18f, by2 = %.18f", bx2, by2); 00697 00698 f11 = ((ax1 == bx1) && (ay1 == by1)); 00699 f12 = ((ax1 == bx2) && (ay1 == by2)); 00700 f21 = ((ax2 == bx1) && (ay2 == by1)); 00701 f22 = ((ax2 == bx2) && (ay2 == by2)); 00702 00703 /* Check for identical segments */ 00704 if ((f11 && f22) || (f12 && f21)) { 00705 G_debug(DLEVEL, " identical segments"); 00706 *x1 = ax1; 00707 *y1 = ay1; 00708 *x2 = ax2; 00709 *y2 = ay2; 00710 return 5; 00711 } 00712 /* Check for identical endpoints */ 00713 if (f11 || f12) { 00714 G_debug(DLEVEL, " connected by endpoints"); 00715 *x1 = ax1; 00716 *y1 = ay1; 00717 return 1; 00718 } 00719 if (f21 || f22) { 00720 G_debug(DLEVEL, " connected by endpoints"); 00721 *x1 = ax2; 00722 *y1 = ay2; 00723 return 1; 00724 } 00725 00726 if ((MAX(ax1, ax2) < MIN(bx1, bx2)) || (MAX(bx1, bx2) < MIN(ax1, ax2))) { 00727 G_debug(DLEVEL, " no intersection (disjoint bounding boxes)"); 00728 return 0; 00729 } 00730 if ((MAX(ay1, ay2) < MIN(by1, by2)) || (MAX(by1, by2) < MIN(ay1, ay2))) { 00731 G_debug(DLEVEL, " no intersection (disjoint bounding boxes)"); 00732 return 0; 00733 } 00734 00735 d = D; 00736 if (d != 0) { 00737 G_debug(DLEVEL, " general position"); 00738 00739 da = DA; 00740 00741 /*mpf_div(rra, dda, dd); 00742 mpf_div(rrb, ddb, dd); 00743 s = mpf_get_str(NULL, &exp, 10, 40, rra); 00744 G_debug(4, " ra = %sE%d", (s[0]==0)?"0":s, exp); 00745 s = mpf_get_str(NULL, &exp, 10, 24, rrb); 00746 G_debug(4, " rb = %sE%d", (s[0]==0)?"0":s, exp); 00747 */ 00748 00749 if (d > 0) { 00750 if ((da < 0) || (da > d)) { 00751 G_debug(DLEVEL, " no intersection"); 00752 return 0; 00753 } 00754 00755 db = DB; 00756 if ((db < 0) || (db > d)) { 00757 G_debug(DLEVEL, " no intersection"); 00758 return 0; 00759 } 00760 } 00761 else { /* if d < 0 */ 00762 if ((da > 0) || (da < d)) { 00763 G_debug(DLEVEL, " no intersection"); 00764 return 0; 00765 } 00766 00767 db = DB; 00768 if ((db > 0) || (db < d)) { 00769 G_debug(DLEVEL, " no intersection"); 00770 return 0; 00771 } 00772 } 00773 00774 /*G_debug(DLEVEL, " ra=%.17g rb=%.17g", mpf_get_d(dda)/mpf_get_d(dd), mpf_get_d(ddb)/mpf_get_d(dd)); */ 00775 /*G_debug(DLEVEL, " sgn_d=%d sgn_da=%d sgn_db=%d cmp(dda,dd)=%d cmp(ddb,dd)=%d", sgn_d, sgn_da, sgn_db, mpf_cmp(dda, dd), mpf_cmp(ddb, dd)); */ 00776 00777 *x1 = ax1 + (ax2 - ax1) * da / d; 00778 *y1 = ay1 + (ay2 - ay1) * da / d; 00779 00780 G_debug(DLEVEL, " intersection %.16g, %.16g", *x1, *y1); 00781 return 1; 00782 } 00783 00784 /* segments are parallel or collinear */ 00785 da = DA; 00786 db = DB; 00787 if ((da != 0) || (db != 0)) { 00788 /* segments are parallel */ 00789 G_debug(DLEVEL, " parallel segments"); 00790 return 0; 00791 } 00792 00793 /* segments are colinear. check for overlap */ 00794 00795 /* swap endpoints if needed */ 00796 /* if segments are vertical, we swap x-coords with y-coords */ 00797 vertical = 0; 00798 if (ax1 > ax2) { 00799 SWAP(ax1, ax2); 00800 SWAP(ay1, ay2); 00801 } 00802 else if (ax1 == ax2) { 00803 vertical = 1; 00804 if (ay1 > ay2) 00805 SWAP(ay1, ay2); 00806 SWAP(ax1, ay1); 00807 SWAP(ax2, ay2); 00808 } 00809 if (bx1 > bx2) { 00810 SWAP(bx1, bx2); 00811 SWAP(by1, by2); 00812 } 00813 else if (bx1 == bx2) { 00814 if (by1 > by2) 00815 SWAP(by1, by2); 00816 SWAP(bx1, by1); 00817 SWAP(bx2, by2); 00818 } 00819 00820 G_debug(DLEVEL, " collinear segments"); 00821 00822 if ((bx2 < ax1) || (bx1 > ax2)) { 00823 G_debug(DLEVEL, " no intersection"); 00824 return 0; 00825 } 00826 00827 /* there is overlap or connected end points */ 00828 G_debug(DLEVEL, " overlap"); 00829 00830 /* a contains b */ 00831 if ((ax1 < bx1) && (ax2 > bx2)) { 00832 G_debug(DLEVEL, " a contains b"); 00833 if (!vertical) { 00834 *x1 = bx1; 00835 *y1 = by1; 00836 *x2 = bx2; 00837 *y2 = by2; 00838 } 00839 else { 00840 *x1 = by1; 00841 *y1 = bx1; 00842 *x2 = by2; 00843 *y2 = bx2; 00844 } 00845 return 3; 00846 } 00847 00848 /* b contains a */ 00849 if ((ax1 > bx1) && (ax2 < bx2)) { 00850 G_debug(DLEVEL, " b contains a"); 00851 if (!vertical) { 00852 *x1 = bx1; 00853 *y1 = by1; 00854 *x2 = bx2; 00855 *y2 = by2; 00856 } 00857 else { 00858 *x1 = by1; 00859 *y1 = bx1; 00860 *x2 = by2; 00861 *y2 = bx2; 00862 } 00863 return 4; 00864 } 00865 00866 /* general overlap, 2 intersection points */ 00867 G_debug(DLEVEL, " partial overlap"); 00868 if ((bx1 > ax1) && (bx1 < ax2)) { /* b1 is in a */ 00869 if (!vertical) { 00870 *x1 = bx1; 00871 *y1 = by1; 00872 *x2 = ax2; 00873 *y2 = ay2; 00874 } 00875 else { 00876 *x1 = by1; 00877 *y1 = bx1; 00878 *x2 = ay2; 00879 *y2 = ax2; 00880 } 00881 return 2; 00882 } 00883 if ((bx2 > ax1) && (bx2 < ax2)) { /* b2 is in a */ 00884 if (!vertical) { 00885 *x1 = bx2; 00886 *y1 = by2; 00887 *x2 = ax1; 00888 *y2 = ay1; 00889 } 00890 else { 00891 *x1 = by2; 00892 *y1 = bx2; 00893 *x2 = ay1; 00894 *y2 = ax1; 00895 } 00896 return 2; 00897 } 00898 00899 /* should not be reached */ 00900 G_warning(("segment_intersection_2d() ERROR (should not be reached)")); 00901 G_warning("%.16g %.16g", ax1, ay1); 00902 G_warning("%.16g %.16g", ax2, ay2); 00903 G_warning("x"); 00904 G_warning("%.16g %.16g", bx1, by1); 00905 G_warning("%.16g %.16g", bx2, by2); 00906 00907 return 0; 00908 } 00909 00910 #define N 52 /* double's mantisa size in bits */ 00911 /* a and b are different in at most <bits> significant digits */ 00912 int almost_equal(double a, double b, int bits) 00913 { 00914 int ea, eb, e; 00915 00916 if (a == b) 00917 return 1; 00918 00919 if (a == 0 || b == 0) { 00920 /* return (0 < -N+bits); */ 00921 return (bits > N); 00922 } 00923 00924 frexp(a, &ea); 00925 frexp(b, &eb); 00926 if (ea != eb) 00927 return (bits > N + abs(ea - eb)); 00928 frexp(a - b, &e); 00929 return (e < ea - N + bits); 00930 } 00931 00932 #ifdef ASDASDFASFEAS 00933 int segment_intersection_2d_test(double ax1, double ay1, double ax2, 00934 double ay2, double bx1, double by1, 00935 double bx2, double by2, double *x1, 00936 double *y1, double *x2, double *y2) 00937 { 00938 double t; 00939 00940 double max_ax, min_ax, max_ay, min_ay; 00941 00942 double max_bx, min_bx, max_by, min_by; 00943 00944 int sgn_d, sgn_da, sgn_db; 00945 00946 int vertical; 00947 00948 int f11, f12, f21, f22; 00949 00950 mp_exp_t exp; 00951 00952 char *s; 00953 00954 double d, da, db, ra, rb; 00955 00956 if (!initialized) 00957 initialize_mpf_vars(); 00958 00959 /* TODO: Works for points ? */ 00960 G_debug(3, "segment_intersection_2d_test()"); 00961 G_debug(3, " ax1 = %.18e, ay1 = %.18e", ax1, ay1); 00962 G_debug(3, " ax2 = %.18e, ay2 = %.18e", ax2, ay2); 00963 G_debug(3, " bx1 = %.18e, by1 = %.18e", bx1, by1); 00964 G_debug(3, " bx2 = %.18e, by2 = %.18e", bx2, by2); 00965 00966 f11 = ((ax1 == bx1) && (ay1 == by1)); 00967 f12 = ((ax1 == bx2) && (ay1 == by2)); 00968 f21 = ((ax2 == bx1) && (ay2 == by1)); 00969 f22 = ((ax2 == bx2) && (ay2 == by2)); 00970 00971 /* Check for identical segments */ 00972 if ((f11 && f22) || (f12 && f21)) { 00973 G_debug(4, " identical segments"); 00974 *x1 = ax1; 00975 *y1 = ay1; 00976 *x2 = ax2; 00977 *y2 = ay2; 00978 return 5; 00979 } 00980 /* Check for identical endpoints */ 00981 if (f11 || f12) { 00982 G_debug(4, " connected by endpoints"); 00983 *x1 = ax1; 00984 *y1 = ay1; 00985 return 1; 00986 } 00987 if (f21 || f22) { 00988 G_debug(4, " connected by endpoints"); 00989 *x1 = ax2; 00990 *y1 = ay2; 00991 return 1; 00992 } 00993 00994 if ((MAX(ax1, ax2) < MIN(bx1, bx2)) || (MAX(bx1, bx2) < MIN(ax1, ax2))) { 00995 G_debug(4, " no intersection (disjoint bounding boxes)"); 00996 return 0; 00997 } 00998 if ((MAX(ay1, ay2) < MIN(by1, by2)) || (MAX(by1, by2) < MIN(ay1, ay2))) { 00999 G_debug(4, " no intersection (disjoint bounding boxes)"); 01000 return 0; 01001 } 01002 01003 d = (ax2 - ax1) * (by1 - by2) - (ay2 - ay1) * (bx1 - bx2); 01004 da = (bx1 - ax1) * (by1 - by2) - (by1 - ay1) * (bx1 - bx2); 01005 db = (ax2 - ax1) * (by1 - ay1) - (ay2 - ay1) * (bx1 - ax1); 01006 01007 det22(dd, ax2, ax1, bx1, bx2, ay2, ay1, by1, by2); 01008 sgn_d = mpf_sgn(dd); 01009 s = mpf_get_str(NULL, &exp, 10, 40, dd); 01010 G_debug(3, " dd = %sE%d", (s[0] == 0) ? "0" : s, exp); 01011 G_debug(3, " d = %.18E", d); 01012 01013 if (sgn_d != 0) { 01014 G_debug(3, " general position"); 01015 01016 det22(dda, bx1, ax1, bx1, bx2, by1, ay1, by1, by2); 01017 det22(ddb, ax2, ax1, bx1, ax1, ay2, ay1, by1, ay1); 01018 sgn_da = mpf_sgn(dda); 01019 sgn_db = mpf_sgn(ddb); 01020 01021 ra = da / d; 01022 rb = db / d; 01023 mpf_div(rra, dda, dd); 01024 mpf_div(rrb, ddb, dd); 01025 01026 s = mpf_get_str(NULL, &exp, 10, 40, rra); 01027 G_debug(4, " rra = %sE%d", (s[0] == 0) ? "0" : s, exp); 01028 G_debug(4, " ra = %.18E", ra); 01029 s = mpf_get_str(NULL, &exp, 10, 40, rrb); 01030 G_debug(4, " rrb = %sE%d", (s[0] == 0) ? "0" : s, exp); 01031 G_debug(4, " rb = %.18E", rb); 01032 01033 if (sgn_d > 0) { 01034 if ((sgn_da < 0) || (mpf_cmp(dda, dd) > 0)) { 01035 G_debug(DLEVEL, " no intersection"); 01036 return 0; 01037 } 01038 01039 if ((sgn_db < 0) || (mpf_cmp(ddb, dd) > 0)) { 01040 G_debug(DLEVEL, " no intersection"); 01041 return 0; 01042 } 01043 } 01044 else { /* if sgn_d < 0 */ 01045 if ((sgn_da > 0) || (mpf_cmp(dda, dd) < 0)) { 01046 G_debug(DLEVEL, " no intersection"); 01047 return 0; 01048 } 01049 01050 if ((sgn_db > 0) || (mpf_cmp(ddb, dd) < 0)) { 01051 G_debug(DLEVEL, " no intersection"); 01052 return 0; 01053 } 01054 } 01055 01056 mpf_set_d(delta, ax2 - ax1); 01057 mpf_mul(t1, dda, delta); 01058 mpf_div(t2, t1, dd); 01059 *x1 = ax1 + mpf_get_d(t2); 01060 01061 mpf_set_d(delta, ay2 - ay1); 01062 mpf_mul(t1, dda, delta); 01063 mpf_div(t2, t1, dd); 01064 *y1 = ay1 + mpf_get_d(t2); 01065 01066 G_debug(2, " intersection at:"); 01067 G_debug(2, " xx = %.18e", *x1); 01068 G_debug(2, " x = %.18e", ax1 + ra * (ax2 - ax1)); 01069 G_debug(2, " yy = %.18e", *y1); 01070 G_debug(2, " y = %.18e", ay1 + ra * (ay2 - ay1)); 01071 return 1; 01072 } 01073 01074 G_debug(3, " parallel/collinear..."); 01075 return -1; 01076 } 01077 #endif