GRASS Programmer's Manual 6.4.1(2011)
e_intersect.c
Go to the documentation of this file.
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
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Defines