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
00052
00053
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
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
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
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
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
00164
00165
00166
00167
00168
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 {
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
00199
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
00216 det22(dda, bx1, ax1, bx1, bx2, by1, ay1, by1, by2);
00217 sgn_da = mpf_sgn(dda);
00218 if (sgn_da != 0) {
00219
00220 G_debug(3, " parallel segments");
00221 return 0;
00222 }
00223
00224
00225
00226
00227
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
00259 G_debug(3, " overlap");
00260
00261
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
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
00298 G_debug(3, " partial overlap");
00299 if ((bx1 > ax1) && (bx1 < ax2)) {
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)) {
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
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
00343
00344
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
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
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
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;
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
00444 G_debug(3, " -> parallel/collinear");
00445
00446 if ((!FZERO(d1, tol)) || (!FZERO(d2, tol))) {
00447 G_debug(2, " -> parallel");
00448 return 0;
00449 }
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460
00461
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 }
00470 if (by1 > by2) {
00471 t = by1;
00472 by1 = by2;
00473 by2 = t;
00474 }
00475 if (ay1 > by2 || ay2 < by1) {
00476 G_debug(2, " -> no intersection");
00477 return 0;
00478 }
00479
00480
00481 if (FEQUAL(ay1, by2, tol)) {
00482 *x1 = ax1;
00483 *y1 = ay1;
00484 G_debug(2, " -> connected by end points");
00485 return 1;
00486 }
00487 if (FEQUAL(ay2, by1, tol)) {
00488 *x1 = ax2;
00489 *y1 = ay2;
00490 G_debug(2, " -> connected by end points");
00491 return 1;
00492 }
00493
00494
00495 G_debug(3, " -> vertical overlap");
00496
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
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
00522 G_debug(2, " -> partial overlap");
00523 if (by1 > ay1 && by1 < ay2) {
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) {
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
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
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
00575 G_debug(2, " -> overlap/connected end points");
00576
00577
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 }
00599 if (bx1 > bx2) {
00600 t = bx1;
00601 bx1 = bx2;
00602 bx2 = t;
00603 t = by1;
00604 by1 = by2;
00605 by2 = t;
00606 }
00607
00608
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
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
00634 G_debug(2, " -> partial overlap");
00635 if (bx1 > ax1 && bx1 < ax2) {
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) {
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
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
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
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
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
00742
00743
00744
00745
00746
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 {
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
00775
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
00785 da = DA;
00786 db = DB;
00787 if ((da != 0) || (db != 0)) {
00788
00789 G_debug(DLEVEL, " parallel segments");
00790 return 0;
00791 }
00792
00793
00794
00795
00796
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
00828 G_debug(DLEVEL, " overlap");
00829
00830
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
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
00867 G_debug(DLEVEL, " partial overlap");
00868 if ((bx1 > ax1) && (bx1 < ax2)) {
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)) {
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
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
00911
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
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
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
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
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 {
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