intersect.c

Go to the documentation of this file.
00001 
00021 #include <stdlib.h>
00022 #include <math.h>
00023 #include <grass/gis.h>
00024 #include <grass/Vect.h>
00025 #include <grass/glocale.h>
00026 
00027 /* function prototypes */
00028 static int cmp_cross(const void *pa, const void *pb);
00029 static void add_cross(int asegment, double adistance, int bsegment,
00030                       double bdistance, double x, double y);
00031 static double dist2(double x1, double y1, double x2, double y2);
00032 
00033 #if 0
00034 static int ident(double x1, double y1, double x2, double y2, double thresh);
00035 #endif
00036 static int cross_seg(int id, int *arg);
00037 static int find_cross(int id, int *arg);
00038 
00039 
00040 /* Some parts of code taken from grass50 v.spag/linecros.c
00041  * 
00042  * Based on the following:
00043  * 
00044  *     (ax2-ax1)r1 - (bx2-bx1)r2 = ax2 - ax1
00045  *     (ay2-ay1)r1 - (by2-by1)r2 = ay2 - ay1
00046  * 
00047  * Solving for r1 and r2, if r1 and r2 are between 0 and 1,
00048  * then line segments (ax1,ay1)(ax2,ay2) and (bx1,by1)(bx2,by2)
00049  * intersect
00050  * ****************************************************************/
00051 
00052 #define D  ((ax2-ax1)*(by1-by2) - (ay2-ay1)*(bx1-bx2))
00053 #define D1 ((bx1-ax1)*(by1-by2) - (by1-ay1)*(bx1-bx2))
00054 #define D2 ((ax2-ax1)*(by1-ay1) - (ay2-ay1)*(bx1-ax1))
00055 
00056 /* Intersect 2 line segments.
00057  *
00058  *  Returns: 0 - do not intersect
00059  *           1 - intersect at one point
00060  *                 \  /    \  /  \  /
00061  *                  \/      \/    \/
00062  *                  /\             \
00063  *                 /  \             \
00064  *           2 - partial overlap         ( \/                      )
00065  *                ------      a          (    distance < threshold )
00066  *                   ------   b          (                         )
00067  *           3 - a contains b            ( /\                      )
00068  *                ----------  a    ----------- a
00069  *                   ----     b          ----- b
00070  *           4 - b contains a
00071  *                   ----     a          ----- a
00072  *                ----------  b    ----------- b
00073  *           5 - identical
00074  *                ----------  a
00075  *                ----------  b
00076  *
00077  *  Intersection points: 
00078  *  return  point1 breakes: point2 breaks:    distance1 on:   distance2 on:
00079  *     0        -              -                  -              -  
00080  *     1        a,b            -                  a              b
00081  *     2        a              b                  a              b
00082  *     3        a              a                  a              a
00083  *     4        b              b                  b              b
00084  *     5        -              -                  -              -
00085  *     
00086  *  Sometimes (often) is important to get the same coordinates for a x b and b x a.
00087  *  To reach this, the segments a,b are 'sorted' at the beginning, so that for the same switched segments,
00088  *  results are identical. (reason is that double values are always rounded because of limited number
00089  *  of decimal places and for different order of coordinates, the results would be different)
00090  *     
00091  */
00092 
00109 int Vect_segment_intersection(double ax1, double ay1, double az1, double ax2,
00110                               double ay2, double az2, double bx1, double by1,
00111                               double bz1, double bx2, double by2, double bz2,
00112                               double *x1, double *y1, double *z1, double *x2,
00113                               double *y2, double *z2, int with_z)
00114 {
00115     static int first_3d = 1;
00116     double d, d1, d2, r1, r2, dtol, t;
00117     int switched = 0;
00118 
00119     /* TODO: Works for points ? */
00120 
00121     G_debug(4, "Vect_segment_intersection()");
00122     G_debug(4, "    %.15g , %.15g  - %.15g , %.15g", ax1, ay1, ax2, ay2);
00123     G_debug(4, "    %.15g , %.15g  - %.15g , %.15g", bx1, by1, bx2, by2);
00124 
00125     /* TODO 3D */
00126     if (with_z && first_3d) {
00127         G_warning(_("3D not supported by Vect_segment_intersection()"));
00128         first_3d = 0;
00129     }
00130 
00131     /* Check identical lines */
00132     if ((ax1 == bx1 && ay1 == by1 && ax2 == bx2 && ay2 == by2) ||
00133         (ax1 == bx2 && ay1 == by2 && ax2 == bx1 && ay2 == by1)) {
00134         G_debug(2, " -> identical segments");
00135         *x1 = ax1;
00136         *y1 = ay1;
00137         *z1 = az1;
00138         *x2 = ax2;
00139         *y2 = ay2;
00140         *z2 = az2;
00141         return 5;
00142     }
00143 
00144     /*  'Sort' lines by x1, x2, y1, y2 */
00145     if (bx1 < ax1)
00146         switched = 1;
00147     else if (bx1 == ax1) {
00148         if (bx2 < ax2)
00149             switched = 1;
00150         else if (bx2 == ax2) {
00151             if (by1 < ay1)
00152                 switched = 1;
00153             else if (by1 == ay1) {
00154                 if (by2 < ay2)
00155                     switched = 1;       /* by2 != ay2 (would be identical */
00156             }
00157         }
00158     }
00159     if (switched) {
00160         t = ax1;
00161         ax1 = bx1;
00162         bx1 = t;
00163         t = ay1;
00164         ay1 = by1;
00165         by1 = t;
00166         t = ax2;
00167         ax2 = bx2;
00168         bx2 = t;
00169         t = ay2;
00170         ay2 = by2;
00171         by2 = t;
00172     }
00173 
00174     d = D;
00175     d1 = D1;
00176     d2 = D2;
00177 
00178     G_debug(2, "Vect_segment_intersection(): d = %f, d1 = %f, d2 = %f", d, d1,
00179             d2);
00180 
00181     /* TODO: dtol was originaly set to 1.0e-10, which was usualy working but not always. 
00182      *       Can it be a problem to set the tolerance to 0.0 ? */
00183     dtol = 0.0;
00184     if (fabs(d) > dtol) {
00185         r1 = D1 / d;
00186         r2 = D2 / d;
00187 
00188         G_debug(2, " -> not parallel/collinear: r1 = %f, r2 = %f", r1, r2);
00189 
00190         if (r1 < 0 || r1 > 1 || r2 < 0 || r2 > 1) {
00191             G_debug(2, "  -> no intersection");
00192             return 0;
00193         }
00194 
00195         *x1 = ax1 + r1 * (ax2 - ax1);
00196         *y1 = ay1 + r1 * (ay2 - ay1);
00197         *z1 = 0;
00198 
00199         G_debug(2, "  -> intersection %f, %f", *x1, *y1);
00200         return 1;
00201     }
00202 
00203     /* segments are parallel or collinear */
00204     G_debug(3, " -> parallel/collinear");
00205 
00206     if (D1 || D2) {             /* lines are parallel */
00207         G_debug(2, "  -> parallel");
00208         return 0;
00209     }
00210 
00211     /* segments are colinear. check for overlap */
00212 
00213     /* Collinear vertical */
00214     /* original code assumed lines were not both vertical
00215      *  so there is a special case if they are */
00216     if (ax1 == ax2 && bx1 == bx2 && ax1 == bx1) {
00217         G_debug(2, "  -> collinear vertical");
00218         if (ay1 > ay2) {
00219             t = ay1;
00220             ay1 = ay2;
00221             ay2 = t;
00222         }                       /* to be sure that ay1 < ay2 */
00223         if (by1 > by2) {
00224             t = by1;
00225             by1 = by2;
00226             by2 = t;
00227         }                       /* to be sure that by1 < by2 */
00228         if (ay1 > by2 || ay2 < by1) {
00229             G_debug(2, "   -> no intersection");
00230             return 0;
00231         }
00232 
00233         /* end points */
00234         if (ay1 == by2) {
00235             *x1 = ax1;
00236             *y1 = ay1;
00237             *z1 = 0;
00238             G_debug(2, "   -> connected by end points");
00239             return 1;           /* endpoints only */
00240         }
00241         if (ay2 == by1) {
00242             *x1 = ax2;
00243             *y1 = ay2;
00244             *z1 = 0;
00245             G_debug(2, "    -> connected by end points");
00246             return 1;           /* endpoints only */
00247         }
00248 
00249         /* heneral overlap */
00250         G_debug(3, "   -> vertical overlap");
00251         /* a contains b */
00252         if (ay1 <= by1 && ay2 >= by2) {
00253             G_debug(2, "    -> a contains b");
00254             *x1 = bx1;
00255             *y1 = by1;
00256             *z1 = 0;
00257             *x2 = bx2;
00258             *y2 = by2;
00259             *z2 = 0;
00260             if (!switched)
00261                 return 3;
00262             else
00263                 return 4;
00264         }
00265         /* b contains a */
00266         if (ay1 >= by1 && ay2 <= by2) {
00267             G_debug(2, "    -> b contains a");
00268             *x1 = ax1;
00269             *y1 = ay1;
00270             *z1 = 0;
00271             *x2 = ax2;
00272             *y2 = ay2;
00273             *z2 = 0;
00274             if (!switched)
00275                 return 4;
00276             else
00277                 return 3;
00278         }
00279 
00280         /* general overlap, 2 intersection points */
00281         G_debug(2, "    -> partial overlap");
00282         if (by1 > ay1 && by1 < ay2) {   /* b1 in a */
00283             if (!switched) {
00284                 *x1 = bx1;
00285                 *y1 = by1;
00286                 *z1 = 0;
00287                 *x2 = ax2;
00288                 *y2 = ay2;
00289                 *z2 = 0;
00290             }
00291             else {
00292                 *x1 = ax2;
00293                 *y1 = ay2;
00294                 *z1 = 0;
00295                 *x2 = bx1;
00296                 *y2 = by1;
00297                 *z2 = 0;
00298             }
00299             return 2;
00300         }
00301         if (by2 > ay1 && by2 < ay2) {   /* b2 in a */
00302             if (!switched) {
00303                 *x1 = bx2;
00304                 *y1 = by2;
00305                 *z1 = 0;
00306                 *x2 = ax1;
00307                 *y2 = ay1;
00308                 *z2 = 0;
00309             }
00310             else {
00311                 *x1 = ax1;
00312                 *y1 = ay1;
00313                 *z1 = 0;
00314                 *x2 = bx2;
00315                 *y2 = by2;
00316                 *z2 = 0;
00317             }
00318             return 2;
00319         }
00320 
00321         /* should not be reached */
00322         G_warning(_("Vect_segment_intersection() ERROR (collinear vertical segments)"));
00323         G_warning("%.15g %.15g", ax1, ay1);
00324         G_warning("%.15g %.15g", ax2, ay2);
00325         G_warning("x");
00326         G_warning("%.15g %.15g", bx1, by1);
00327         G_warning("%.15g %.15g", bx2, by2);
00328 
00329         return 0;
00330     }
00331 
00332     G_debug(2, "   -> collinear non vertical");
00333 
00334     /* Collinear non vertical */
00335     if ((bx1 > ax1 && bx2 > ax1 && bx1 > ax2 && bx2 > ax2) ||
00336         (bx1 < ax1 && bx2 < ax1 && bx1 < ax2 && bx2 < ax2)) {
00337         G_debug(2, "   -> no intersection");
00338         return 0;
00339     }
00340 
00341     /* there is overlap or connected end points */
00342     G_debug(2, "   -> overlap/connected end points");
00343 
00344     /* end points */
00345     if ((ax1 == bx1 && ay1 == by1) || (ax1 == bx2 && ay1 == by2)) {
00346         *x1 = ax1;
00347         *y1 = ay1;
00348         *z1 = 0;
00349         G_debug(2, "    -> connected by end points");
00350         return 1;
00351     }
00352     if ((ax2 == bx1 && ay2 == by1) || (ax2 == bx2 && ay2 == by2)) {
00353         *x1 = ax2;
00354         *y1 = ay2;
00355         *z1 = 0;
00356         G_debug(2, "    -> connected by end points");
00357         return 1;
00358     }
00359 
00360     if (ax1 > ax2) {
00361         t = ax1;
00362         ax1 = ax2;
00363         ax2 = t;
00364         t = ay1;
00365         ay1 = ay2;
00366         ay2 = t;
00367     }                           /* to be sure that ax1 < ax2 */
00368     if (bx1 > bx2) {
00369         t = bx1;
00370         bx1 = bx2;
00371         bx2 = t;
00372         t = by1;
00373         by1 = by2;
00374         by2 = t;
00375     }                           /* to be sure that bx1 < bx2 */
00376 
00377     /* a contains b */
00378     if (ax1 <= bx1 && ax2 >= bx2) {
00379         G_debug(2, "    -> a contains b");
00380         *x1 = bx1;
00381         *y1 = by1;
00382         *z1 = 0;
00383         *x2 = bx2;
00384         *y2 = by2;
00385         *z2 = 0;
00386         if (!switched)
00387             return 3;
00388         else
00389             return 4;
00390     }
00391     /* b contains a */
00392     if (ax1 >= bx1 && ax2 <= bx2) {
00393         G_debug(2, "    -> b contains a");
00394         *x1 = ax1;
00395         *y1 = ay1;
00396         *z1 = 0;
00397         *x2 = ax2;
00398         *y2 = ay2;
00399         *z2 = 0;
00400         if (!switched)
00401             return 4;
00402         else
00403             return 3;
00404     }
00405 
00406     /* general overlap, 2 intersection points (lines are not vertical) */
00407     G_debug(2, "    -> partial overlap");
00408     if (bx1 > ax1 && bx1 < ax2) {       /* b1 is in a */
00409         if (!switched) {
00410             *x1 = bx1;
00411             *y1 = by1;
00412             *z1 = 0;
00413             *x2 = ax2;
00414             *y2 = ay2;
00415             *z2 = 0;
00416         }
00417         else {
00418             *x1 = ax2;
00419             *y1 = ay2;
00420             *z1 = 0;
00421             *x2 = bx1;
00422             *y2 = by1;
00423             *z2 = 0;
00424         }
00425         return 2;
00426     }
00427     if (bx2 > ax1 && bx2 < ax2) {       /* b2 is in a */
00428         if (!switched) {
00429             *x1 = bx2;
00430             *y1 = by2;
00431             *z1 = 0;
00432             *x2 = ax1;
00433             *y2 = ay1;
00434             *z2 = 0;
00435         }
00436         else {
00437             *x1 = ax1;
00438             *y1 = ay1;
00439             *z1 = 0;
00440             *x2 = bx2;
00441             *y2 = by2;
00442             *z2 = 0;
00443         }
00444         return 2;
00445     }
00446 
00447     /* should not be reached */
00448     G_warning(_("Vect_segment_intersection() ERROR (collinear non vertical segments)"));
00449     G_warning("%.15g %.15g", ax1, ay1);
00450     G_warning("%.15g %.15g", ax2, ay2);
00451     G_warning("x");
00452     G_warning("%.15g %.15g", bx1, by1);
00453     G_warning("%.15g %.15g", bx2, by2);
00454 
00455     return 0;
00456 }
00457 
00458 typedef struct
00459 {                               /* in arrays 0 - A line , 1 - B line */
00460     int segment[2];             /* segment number, start from 0 for first */
00461     double distance[2];
00462     double x, y, z;
00463 } CROSS;
00464 
00465 /* Current line in arrays is for some functions like cmp() set by: */
00466 static int current;
00467 static int second;              /* line whic is not current */
00468 
00469 static int a_cross = 0;
00470 static int n_cross;
00471 static CROSS *cross = NULL;
00472 static int *use_cross = NULL;
00473 
00474 static void add_cross(int asegment, double adistance, int bsegment,
00475                       double bdistance, double x, double y)
00476 {
00477     if (n_cross == a_cross) {
00478         /* Must be space + 1, used later for last line point, do it better */
00479         cross =
00480             (CROSS *) G_realloc((void *)cross,
00481                                 (a_cross + 101) * sizeof(CROSS));
00482         use_cross =
00483             (int *)G_realloc((void *)use_cross,
00484                              (a_cross + 101) * sizeof(int));
00485         a_cross += 100;
00486     }
00487 
00488     G_debug(5,
00489             "  add new cross: aseg/dist = %d/%f bseg/dist = %d/%f, x = %f y = %f",
00490             asegment, adistance, bsegment, bdistance, x, y);
00491     cross[n_cross].segment[0] = asegment;
00492     cross[n_cross].distance[0] = adistance;
00493     cross[n_cross].segment[1] = bsegment;
00494     cross[n_cross].distance[1] = bdistance;
00495     cross[n_cross].x = x;
00496     cross[n_cross].y = y;
00497     n_cross++;
00498 }
00499 
00500 static int cmp_cross(const void *pa, const void *pb)
00501 {
00502     CROSS *p1 = (CROSS *) pa;
00503     CROSS *p2 = (CROSS *) pb;
00504 
00505     if (p1->segment[current] < p2->segment[current])
00506         return -1;
00507     if (p1->segment[current] > p2->segment[current])
00508         return 1;
00509     /* the same segment */
00510     if (p1->distance[current] < p2->distance[current])
00511         return -1;
00512     if (p1->distance[current] > p2->distance[current])
00513         return 1;
00514     return 0;
00515 }
00516 
00517 static double dist2(double x1, double y1, double x2, double y2)
00518 {
00519     double dx, dy;
00520 
00521     dx = x2 - x1;
00522     dy = y2 - y1;
00523     return (dx * dx + dy * dy);
00524 }
00525 
00526 #if 0
00527 /* returns 1 if points are identical */
00528 static int ident(double x1, double y1, double x2, double y2, double thresh)
00529 {
00530     double dx, dy;
00531 
00532     dx = x2 - x1;
00533     dy = y2 - y1;
00534     if ((dx * dx + dy * dy) <= thresh * thresh)
00535         return 1;
00536 
00537     return 0;
00538 }
00539 #endif
00540 
00541 /* shared by Vect_line_intersection, Vect_line_check_intersection, cross_seg, find_cross */
00542 static struct line_pnts *APnts, *BPnts;
00543 
00544 /* break segments (called by rtree search) */
00545 static int cross_seg(int id, int *arg)
00546 {
00547     double x1, y1, z1, x2, y2, z2;
00548     int i, j, ret;
00549 
00550     /* !!! segment number for B lines is returned as +1 */
00551     i = *arg;
00552     j = id - 1;
00553     /* Note: -1 to make up for the +1 when data was inserted */
00554 
00555     ret = Vect_segment_intersection(APnts->x[i], APnts->y[i], APnts->z[i],
00556                                     APnts->x[i + 1], APnts->y[i + 1],
00557                                     APnts->z[i + 1], BPnts->x[j], BPnts->y[j],
00558                                     BPnts->z[j], BPnts->x[j + 1],
00559                                     BPnts->y[j + 1], BPnts->z[j + 1], &x1,
00560                                     &y1, &z1, &x2, &y2, &z2, 0);
00561 
00562     /* add ALL (including end points and duplicates), clean later */
00563     if (ret > 0) {
00564         G_debug(2, "  -> %d x %d: intersection type = %d", i, j, ret);
00565         if (ret == 1) {         /* one intersection on segment A */
00566             G_debug(3, "    in %f, %f ", x1, y1);
00567             add_cross(i, 0.0, j, 0.0, x1, y1);
00568         }
00569         else if (ret == 2 || ret == 3 || ret == 4 || ret == 5) {
00570             /*  partial overlap; a broken in one, b broken in one
00571              *  or a contains b; a is broken in 2 points (but 1 may be end)
00572              *  or b contains a; b is broken in 2 points (but 1 may be end) 
00573              *  or identical */
00574             G_debug(3, "    in %f, %f; %f, %f", x1, y1, x2, y2);
00575             add_cross(i, 0.0, j, 0.0, x1, y1);
00576             add_cross(i, 0.0, j, 0.0, x2, y2);
00577         }
00578     }
00579     return 1;                   /* keep going */
00580 }
00581 
00600 int
00601 Vect_line_intersection(struct line_pnts *APoints,
00602                        struct line_pnts *BPoints,
00603                        struct line_pnts ***ALines,
00604                        struct line_pnts ***BLines,
00605                        int *nalines, int *nblines, int with_z)
00606 {
00607     int i, j, k, l, last_seg, seg, last;
00608     int n_alive_cross;
00609     double dist, curdist, last_dist, last_x, last_y, last_z;
00610     double x, y, rethresh;
00611     struct Rect rect;
00612     struct line_pnts **XLines, *Points;
00613     struct Node *RTree;
00614     struct line_pnts *Points1, *Points2;        /* first, second points */
00615     int seg1, seg2, vert1, vert2;
00616 
00617     n_cross = 0;
00618     rethresh = 0.000001;        /* TODO */
00619     APnts = APoints;
00620     BPnts = BPoints;
00621 
00622     /* RE (representation error).
00623      *  RE thresh above is nonsense of course, the RE threshold should be based on
00624      *  number of significant digits for double (IEEE-754) which is 15 or 16 and exponent. 
00625      *  The number above is in fact not required threshold, and will not work
00626      *  for example: equator length is 40.075,695 km (8 digits), units are m (+3) 
00627      *  and we want precision in mm (+ 3) = 14 -> minimum rethresh may be around 0.001
00628      *  ?Maybe all nonsense? */
00629 
00630     /* Warning: This function is also used to intersect the line by itself i.e. APoints and
00631      * BPoints are identical. I am not sure if it is clever, but it seems to work, but
00632      * we have to keep this in mind and handle some special cases (maybe) */
00633 
00634     /* TODO: 3D, RE threshold, GV_POINTS (line x point) */
00635 
00636     /* Take each segment from A and intersect by each segment from B.
00637      *  
00638      *  All intersections are found first and saved to array, then sorted by a distance along the line,
00639      *  and then the line is split to pieces.
00640      *
00641      *  Note: If segments are collinear, check if previous/next segments are also collinear, 
00642      *  in that case do not break:
00643      *  +----------+  
00644      *  +----+-----+  etc.
00645      *  doesn't need to be broken 
00646      *
00647      *  Note: If 2 adjacent segments of line B have common vertex exactly (or within thresh) on line A,
00648      *  intersection points for these B segments may differ due to RE:
00649      *  ------------ a       ----+--+----            ----+--+----
00650      *      /\         =>       /    \     or maybe       \/
00651      *  b0 /  \ b1             /      \      even:        /\     
00652      *
00653      *  -> solution: snap all breaks to nearest vertices first within RE threshold
00654      *  
00655      *  Question: Snap all breaks to each other within RE threshold?
00656      *
00657      *  Note: If a break is snapped to end point or two breaks are snapped to the same vertex
00658      *        resulting new line is degenerated => before line is added to array, it must be checked
00659      *        if line is not degenerated
00660      *
00661      *  Note: to snap to vertices is important for cases where line A is broken by B and C line
00662      *  at the same point:
00663      *   \  /  b   no snap     \    /
00664      *    \/       could    ----+--+----
00665      *  ------ a   result   
00666      *    /\       in ?:         /\
00667      *   /  \  c                /  \
00668      * 
00669      *  Note: once we snap breaks to vertices, we have to do that for both lines A and B in the same way
00670      *  and because we cannot be sure that A childrens will not change a bit by break(s) 
00671      *  we have to break both A and B  at once i.e. in one Vect_line_intersection () call.
00672      */
00673 
00674     /* Spatial index: lines may be very long (thousands of segments) and check each segment 
00675      *  with each from second line takes a long time (n*m). Because of that, spatial index
00676      *  is build first for the second line and segments from the first line are broken by segments
00677      *  in bound box */
00678 
00679     /* Create rtree for B line */
00680     RTree = RTreeNewIndex();
00681     for (i = 0; i < BPoints->n_points - 1; i++) {
00682         if (BPoints->x[i] <= BPoints->x[i + 1]) {
00683             rect.boundary[0] = BPoints->x[i];
00684             rect.boundary[3] = BPoints->x[i + 1];
00685         }
00686         else {
00687             rect.boundary[0] = BPoints->x[i + 1];
00688             rect.boundary[3] = BPoints->x[i];
00689         }
00690 
00691         if (BPoints->y[i] <= BPoints->y[i + 1]) {
00692             rect.boundary[1] = BPoints->y[i];
00693             rect.boundary[4] = BPoints->y[i + 1];
00694         }
00695         else {
00696             rect.boundary[1] = BPoints->y[i + 1];
00697             rect.boundary[4] = BPoints->y[i];
00698         }
00699 
00700         if (BPoints->z[i] <= BPoints->z[i + 1]) {
00701             rect.boundary[2] = BPoints->z[i];
00702             rect.boundary[5] = BPoints->z[i + 1];
00703         }
00704         else {
00705             rect.boundary[2] = BPoints->z[i + 1];
00706             rect.boundary[5] = BPoints->z[i];
00707         }
00708 
00709         RTreeInsertRect(&rect, i + 1, &RTree, 0);       /* B line segment numbers in rtree start from 1 */
00710     }
00711 
00712     /* Break segments in A by segments in B */
00713     for (i = 0; i < APoints->n_points - 1; i++) {
00714         if (APoints->x[i] <= APoints->x[i + 1]) {
00715             rect.boundary[0] = APoints->x[i];
00716             rect.boundary[3] = APoints->x[i + 1];
00717         }
00718         else {
00719             rect.boundary[0] = APoints->x[i + 1];
00720             rect.boundary[3] = APoints->x[i];
00721         }
00722 
00723         if (APoints->y[i] <= APoints->y[i + 1]) {
00724             rect.boundary[1] = APoints->y[i];
00725             rect.boundary[4] = APoints->y[i + 1];
00726         }
00727         else {
00728             rect.boundary[1] = APoints->y[i + 1];
00729             rect.boundary[4] = APoints->y[i];
00730         }
00731         if (APoints->z[i] <= APoints->z[i + 1]) {
00732             rect.boundary[2] = APoints->z[i];
00733             rect.boundary[5] = APoints->z[i + 1];
00734         }
00735         else {
00736             rect.boundary[2] = APoints->z[i + 1];
00737             rect.boundary[5] = APoints->z[i];
00738         }
00739 
00740         j = RTreeSearch(RTree, &rect, (void *)cross_seg, &i);   /* A segment number from 0 */
00741     }
00742 
00743     /* Free RTree */
00744     RTreeDestroyNode(RTree);
00745 
00746     G_debug(2, "n_cross = %d", n_cross);
00747     /* Lines do not cross each other */
00748     if (n_cross == 0) {
00749         *nalines = 0;
00750         *nblines = 0;
00751         return 0;
00752     }
00753 
00754     /* Snap breaks to nearest vertices within RE threshold */
00755     for (i = 0; i < n_cross; i++) {
00756         /* 1. of A seg */
00757         seg = cross[i].segment[0];
00758         curdist =
00759             dist2(cross[i].x, cross[i].y, APoints->x[seg], APoints->y[seg]);
00760         x = APoints->x[seg];
00761         y = APoints->y[seg];
00762 
00763         /* 2. of A seg */
00764         dist =
00765             dist2(cross[i].x, cross[i].y, APoints->x[seg + 1],
00766                   APoints->y[seg + 1]);
00767         if (dist < curdist) {
00768             curdist = dist;
00769             x = APoints->x[seg + 1];
00770             y = APoints->y[seg + 1];
00771         }
00772 
00773         /* 1. of B seg */
00774         seg = cross[i].segment[1];
00775         dist =
00776             dist2(cross[i].x, cross[i].y, BPoints->x[seg], BPoints->y[seg]);
00777         if (dist < curdist) {
00778             curdist = dist;
00779             x = BPoints->x[seg];
00780             y = BPoints->y[seg];
00781         }
00782         dist = dist2(cross[i].x, cross[i].y, BPoints->x[seg + 1], BPoints->y[seg + 1]); /* 2. of B seg */
00783         if (dist < curdist) {
00784             curdist = dist;
00785             x = BPoints->x[seg + 1];
00786             y = BPoints->y[seg + 1];
00787         }
00788         if (curdist < rethresh * rethresh) {
00789             cross[i].x = x;
00790             cross[i].y = y;
00791         }
00792     }
00793 
00794     /* Calculate distances along segments */
00795     for (i = 0; i < n_cross; i++) {
00796         seg = cross[i].segment[0];
00797         cross[i].distance[0] =
00798             dist2(APoints->x[seg], APoints->y[seg], cross[i].x, cross[i].y);
00799         seg = cross[i].segment[1];
00800         cross[i].distance[1] =
00801             dist2(BPoints->x[seg], BPoints->y[seg], cross[i].x, cross[i].y);
00802     }
00803 
00804     /* l = 1 ~ line A, l = 2 ~ line B */
00805     for (l = 1; l < 3; l++) {
00806         for (i = 0; i < n_cross; i++)
00807             use_cross[i] = 1;
00808 
00809         /* Create array of lines */
00810         XLines = G_malloc((n_cross + 1) * sizeof(struct line_pnts *));
00811 
00812         if (l == 1) {
00813             G_debug(2, "Clean and create array for line A");
00814             Points = APoints;
00815             Points1 = APoints;
00816             Points2 = BPoints;
00817             current = 0;
00818             second = 1;
00819         }
00820         else {
00821             G_debug(2, "Clean and create array for line B");
00822             Points = BPoints;
00823             Points1 = BPoints;
00824             Points2 = APoints;
00825             current = 1;
00826             second = 0;
00827         }
00828 
00829         /* Sort points along lines */
00830         qsort((void *)cross, sizeof(char) * n_cross, sizeof(CROSS),
00831               cmp_cross);
00832 
00833         /* Print all (raw) breaks */
00834         for (i = 0; i < n_cross; i++) {
00835             G_debug(3,
00836                     "  cross = %d seg1/dist1 = %d/%f seg2/dist2 = %d/%f x = %f y = %f",
00837                     i, cross[i].segment[current],
00838                     sqrt(cross[i].distance[current]),
00839                     cross[i].segment[second], sqrt(cross[i].distance[second]),
00840                     cross[i].x, cross[i].y);
00841         }
00842 
00843         /* Remove breaks on first/last line vertices */
00844         for (i = 0; i < n_cross; i++) {
00845             if (use_cross[i] == 1) {
00846                 j = Points1->n_points - 1;
00847 
00848                 /* Note: */
00849                 if ((cross[i].segment[current] == 0 &&
00850                      cross[i].x == Points1->x[0] &&
00851                      cross[i].y == Points1->y[0]) ||
00852                     (cross[i].segment[current] == j - 1 &&
00853                      cross[i].x == Points1->x[j] &&
00854                      cross[i].y == Points1->y[j])) {
00855                     use_cross[i] = 0;   /* first/last */
00856                     G_debug(3, "cross %d deleted (first/last point)", i);
00857                 }
00858             }
00859         }
00860 
00861         /* Remove breaks with collinear previous and next segments on 1 and 2 */
00862         /* Note: breaks with collinear previous and nex must be remove duplicates,
00863          *        otherwise some cross may be lost. Example (+ is vertex):
00864          *             B          first cross intersections: A/B  segment:
00865          *             |               0/0, 0/1, 1/0, 1/1 - collinear previous and next
00866          *     AB -----+----+--- A     0/4, 0/5, 1/4, 1/5 - OK        
00867          *              \___|                   
00868          *                B                    
00869          *  This should not inluence that break is always on first segment, see below (I hope)
00870          */
00871         /* TODO: this doesn't find identical with breaks on revious/next */
00872         for (i = 0; i < n_cross; i++) {
00873             if (use_cross[i] == 0)
00874                 continue;
00875             G_debug(3, "  is %d between colinear?", i);
00876 
00877             seg1 = cross[i].segment[current];
00878             seg2 = cross[i].segment[second];
00879 
00880             /* Is it vertex on 1, which? */
00881             if (cross[i].x == Points1->x[seg1] &&
00882                 cross[i].y == Points1->y[seg1]) {
00883                 vert1 = seg1;
00884             }
00885             else if (cross[i].x == Points1->x[seg1 + 1] &&
00886                      cross[i].y == Points1->y[seg1 + 1]) {
00887                 vert1 = seg1 + 1;
00888             }
00889             else {
00890                 G_debug(3, "  -> is not vertex on 1. line");
00891                 continue;
00892             }
00893 
00894             /* Is it vertex on 2, which? */
00895             /* For 1. line it is easy, because breaks on vertex are always at end vertex
00896              *  for 2. line we need to find which vertex is on break if any (vert2 starts from 0) */
00897             if (cross[i].x == Points2->x[seg2] &&
00898                 cross[i].y == Points2->y[seg2]) {
00899                 vert2 = seg2;
00900             }
00901             else if (cross[i].x == Points2->x[seg2 + 1] &&
00902                      cross[i].y == Points2->y[seg2 + 1]) {
00903                 vert2 = seg2 + 1;
00904             }
00905             else {
00906                 G_debug(3, "  -> is not vertex on 2. line");
00907                 continue;
00908             }
00909             G_debug(3, "    seg1/vert1 = %d/%d  seg2/ver2 = %d/%d", seg1,
00910                     vert1, seg2, vert2);
00911 
00912             /* Check if the second vertex is not first/last */
00913             if (vert2 == 0 || vert2 == Points2->n_points - 1) {
00914                 G_debug(3, "  -> vertex 2 (%d) is first/last", vert2);
00915                 continue;
00916             }
00917 
00918             /* Are there first vertices of this segment identical */
00919             if (!((Points1->x[vert1 - 1] == Points2->x[vert2 - 1] &&
00920                    Points1->y[vert1 - 1] == Points2->y[vert2 - 1] &&
00921                    Points1->x[vert1 + 1] == Points2->x[vert2 + 1] &&
00922                    Points1->y[vert1 + 1] == Points2->y[vert2 + 1]) ||
00923                   (Points1->x[vert1 - 1] == Points2->x[vert2 + 1] &&
00924                    Points1->y[vert1 - 1] == Points2->y[vert2 + 1] &&
00925                    Points1->x[vert1 + 1] == Points2->x[vert2 - 1] &&
00926                    Points1->y[vert1 + 1] == Points2->y[vert2 - 1])
00927                 )
00928                 ) {
00929                 G_debug(3, "  -> previous/next are not identical");
00930                 continue;
00931             }
00932 
00933             use_cross[i] = 0;
00934 
00935             G_debug(3, "    -> collinear -> remove");
00936         }
00937 
00938         /* Remove duplicates, i.e. merge all identical breaks to one.
00939          *  We must be careful because two points with identical coordinates may be distant if measured along
00940          *  the line:
00941          *       |         Segments b0 and b1 overlap, b0 runs up, b1 down.
00942          *       |         Two inersections may be merged for a, because they are identical,
00943          *  -----+---- a   but cannot be merged for b, because both b0 and b1 must be broken. 
00944          *       |         I.e. Breaks on b have identical coordinates, but there are not identical
00945          *       b0 | b1      if measured along line b.
00946          *                 
00947          *      -> Breaks may be merged as identical if lay on the same segment, or on vertex connecting
00948          *      2 adjacent segments the points lay on
00949          *      
00950          *  Note: if duplicate is on a vertex, the break is removed from next segment =>
00951          *        break on vertex is always on first segment of this vertex (used below) 
00952          */
00953         last = -1;
00954         for (i = 1; i < n_cross; i++) {
00955             if (use_cross[i] == 0)
00956                 continue;
00957             if (last == -1) {   /* set first alive */
00958                 last = i;
00959                 continue;
00960             }
00961             seg = cross[i].segment[current];
00962             /* compare with last */
00963             G_debug(3, "  duplicate ?: cross = %d seg = %d dist = %f", i,
00964                     cross[i].segment[current], cross[i].distance[current]);
00965             if ((cross[i].segment[current] == cross[last].segment[current] &&
00966                  cross[i].distance[current] == cross[last].distance[current])
00967                 || (cross[i].segment[current] ==
00968                     cross[last].segment[current] + 1 &&
00969                     cross[i].distance[current] == 0 &&
00970                     cross[i].x == cross[last].x &&
00971                     cross[i].y == cross[last].y)) {
00972                 G_debug(3, "  cross %d identical to last -> removed", i);
00973                 use_cross[i] = 0;       /* identical */
00974             }
00975             else {
00976                 last = i;
00977             }
00978         }
00979 
00980         /* Create array of new lines */
00981         /* Count alive crosses */
00982         n_alive_cross = 0;
00983         G_debug(3, "  alive crosses:");
00984         for (i = 0; i < n_cross; i++) {
00985             if (use_cross[i] == 1) {
00986                 G_debug(3, "  %d", i);
00987                 n_alive_cross++;
00988             }
00989         }
00990 
00991         k = 0;
00992         if (n_alive_cross > 0) {
00993             /* Add last line point at the end of cross array (cross alley) */
00994             use_cross[n_cross] = 1;
00995             j = Points->n_points - 1;
00996             cross[n_cross].x = Points->x[j];
00997             cross[n_cross].y = Points->y[j];
00998             cross[n_cross].segment[current] = Points->n_points - 2;
00999 
01000             last_seg = 0;
01001             last_dist = 0;
01002             last_x = Points->x[0];
01003             last_y = Points->y[0];
01004             last_z = Points->z[0];
01005             /* Go through all cross (+last line point) and create for each new line 
01006              *  starting at last_* and ending at cross (last point) */
01007             for (i = 0; i <= n_cross; i++) {    /* i.e. n_cross + 1 new lines */
01008                 seg = cross[i].segment[current];
01009                 G_debug(2, "%d seg = %d dist = %f", i, seg,
01010                         cross[i].distance[current]);
01011                 if (use_cross[i] == 0) {
01012                     G_debug(3, "   removed -> next");
01013                     continue;
01014                 }
01015 
01016                 G_debug(2, " New line:");
01017                 XLines[k] = Vect_new_line_struct();
01018                 /* add last intersection or first point first */
01019                 Vect_append_point(XLines[k], last_x, last_y, last_z);
01020                 G_debug(2, "   append last vert: %f %f", last_x, last_y);
01021 
01022                 /* add first points of segments between last and current seg */
01023                 for (j = last_seg + 1; j <= seg; j++) {
01024                     G_debug(2, "  segment j = %d", j);
01025                     /* skipp vertex identical to last break */
01026                     if ((j == last_seg + 1) && Points->x[j] == last_x &&
01027                         Points->y[j] == last_y) {
01028                         G_debug(2, "   -> skip (identical to last break)");
01029                         continue;
01030                     }
01031                     Vect_append_point(XLines[k], Points->x[j], Points->y[j],
01032                                       Points->z[j]);
01033                     G_debug(2, "   append first of seg: %f %f", Points->x[j],
01034                             Points->y[j]);
01035                 }
01036 
01037                 /* add current cross or end point */
01038                 Vect_append_point(XLines[k], cross[i].x, cross[i].y, 0.0);
01039                 G_debug(2, "   append cross / last point: %f %f", cross[i].x,
01040                         cross[i].y);
01041                 last_seg = seg;
01042                 last_x = cross[i].x;
01043                 last_y = cross[i].y, last_z = 0;
01044 
01045                 /* Check if line is degenerate */
01046                 if (dig_line_degenerate(XLines[k]) > 0) {
01047                     G_debug(2, "   line is degenerate -> skipped");
01048                     Vect_destroy_line_struct(XLines[k]);
01049                 }
01050                 else {
01051                     k++;
01052                 }
01053             }
01054         }
01055         if (l == 1) {
01056             *nalines = k;
01057             *ALines = XLines;
01058         }
01059         else {
01060             *nblines = k;
01061             *BLines = XLines;
01062         }
01063     }
01064 
01065     return 1;
01066 }
01067 
01068 static struct line_pnts *APnts, *BPnts, *IPnts;
01069 
01070 static int cross_found;         /* set by find_cross() */
01071 
01072 /* break segments (called by rtree search) */
01073 static int find_cross(int id, int *arg)
01074 {
01075     double x1, y1, z1, x2, y2, z2;
01076     int i, j, ret;
01077 
01078     /* !!! segment number for B lines is returned as +1 */
01079     i = *arg;
01080     j = id - 1;
01081     /* Note: -1 to make up for the +1 when data was inserted */
01082 
01083     ret = Vect_segment_intersection(APnts->x[i], APnts->y[i], APnts->z[i],
01084                                     APnts->x[i + 1], APnts->y[i + 1],
01085                                     APnts->z[i + 1], BPnts->x[j], BPnts->y[j],
01086                                     BPnts->z[j], BPnts->x[j + 1],
01087                                     BPnts->y[j + 1], BPnts->z[j + 1], &x1,
01088                                     &y1, &z1, &x2, &y2, &z2, 0);
01089 
01090     if (!IPnts)
01091         IPnts = Vect_new_line_struct();
01092     
01093     switch (ret) {
01094     case 0:
01095     case 5:
01096         break;
01097     case 1:
01098         if (0 > Vect_copy_xyz_to_pnts(IPnts, &x1, &y1, &z1, 1))
01099             G_warning(_("Error while adding point to array. Out of memory"));
01100         break;
01101     case 2:
01102     case 3:
01103     case 4:
01104         if (0 > Vect_copy_xyz_to_pnts(IPnts, &x1, &y1, &z1, 1))
01105             G_warning(_("Error while adding point to array. Out of memory"));
01106         if (0 > Vect_copy_xyz_to_pnts(IPnts, &x2, &y2, &z2, 1))
01107             G_warning(_("Error while adding point to array. Out of memory"));
01108         break;
01109     }
01110     /* add ALL (including end points and duplicates), clean later */
01111     if (ret > 0) {
01112         cross_found = 1;
01113         return 0;
01114     }
01115     return 1;                   /* keep going */
01116 }
01117 
01130 int
01131 Vect_line_check_intersection(struct line_pnts *APoints,
01132                              struct line_pnts *BPoints, int with_z)
01133 {
01134     int i;
01135     double dist, rethresh;
01136     struct Rect rect;
01137     struct Node *RTree;
01138 
01139     rethresh = 0.000001;        /* TODO */
01140     APnts = APoints;
01141     BPnts = BPoints;
01142 
01143     /* TODO: 3D, RE (representation error) threshold, GV_POINTS (line x point) */
01144 
01145     if (!IPnts)
01146         IPnts = Vect_new_line_struct();
01147 
01148     /* If one or both are point (Points->n_points == 1) */
01149     if (APoints->n_points == 1 && BPoints->n_points == 1) {
01150         if (APoints->x[0] == BPoints->x[0] && APoints->y[0] == BPoints->y[0]) {
01151             if (!with_z) {
01152                 if (0 >
01153                     Vect_copy_xyz_to_pnts(IPnts, &APoints->x[0],
01154                                           &APoints->y[0], NULL, 1))
01155                     G_warning(_("Error while adding point to array. Out of memory"));
01156                 return 1;
01157             }
01158             else {
01159                 if (APoints->z[0] == BPoints->z[0]) {
01160                     if (0 >
01161                         Vect_copy_xyz_to_pnts(IPnts, &APoints->x[0],
01162                                               &APoints->y[0], &APoints->z[0],
01163                                               1))
01164                         G_warning(_("Error while adding point to array. Out of memory"));
01165                     return 1;
01166                 }
01167                 else
01168                     return 0;
01169             }
01170         }
01171         else {
01172             return 0;
01173         }
01174     }
01175 
01176     if (APoints->n_points == 1) {
01177         Vect_line_distance(BPoints, APoints->x[0], APoints->y[0],
01178                            APoints->z[0], with_z, NULL, NULL, NULL, &dist,
01179                            NULL, NULL);
01180 
01181         if (dist <= rethresh) {
01182             if (0 >
01183                 Vect_copy_xyz_to_pnts(IPnts, &APoints->x[0], &APoints->y[0],
01184                                       &APoints->z[0], 1))
01185                 G_warning(_("Error while adding point to array. Out of memory"));
01186             return 1;
01187         }
01188         else {
01189             return 0;
01190         }
01191     }
01192 
01193     if (BPoints->n_points == 1) {
01194         Vect_line_distance(APoints, BPoints->x[0], BPoints->y[0],
01195                            BPoints->z[0], with_z, NULL, NULL, NULL, &dist,
01196                            NULL, NULL);
01197 
01198         if (dist <= rethresh) {
01199             if (0 >
01200                 Vect_copy_xyz_to_pnts(IPnts, &BPoints->x[0], &BPoints->y[0],
01201                                       &BPoints->z[0], 1))
01202                 G_warning(_("Error while adding point to array. Out of memory"));
01203             return 1;
01204         }
01205         else
01206             return 0;
01207     }
01208 
01209     /* Take each segment from A and find if intersects any segment from B. */
01210 
01211     /* Spatial index: lines may be very long (thousands of segments) and check each segment 
01212      *  with each from second line takes a long time (n*m). Because of that, spatial index
01213      *  is build first for the second line and segments from the first line are broken by segments
01214      *  in bound box */
01215 
01216     /* Create rtree for B line */
01217     RTree = RTreeNewIndex();
01218     for (i = 0; i < BPoints->n_points - 1; i++) {
01219         if (BPoints->x[i] <= BPoints->x[i + 1]) {
01220             rect.boundary[0] = BPoints->x[i];
01221             rect.boundary[3] = BPoints->x[i + 1];
01222         }
01223         else {
01224             rect.boundary[0] = BPoints->x[i + 1];
01225             rect.boundary[3] = BPoints->x[i];
01226         }
01227 
01228         if (BPoints->y[i] <= BPoints->y[i + 1]) {
01229             rect.boundary[1] = BPoints->y[i];
01230             rect.boundary[4] = BPoints->y[i + 1];
01231         }
01232         else {
01233             rect.boundary[1] = BPoints->y[i + 1];
01234             rect.boundary[4] = BPoints->y[i];
01235         }
01236 
01237         if (BPoints->z[i] <= BPoints->z[i + 1]) {
01238             rect.boundary[2] = BPoints->z[i];
01239             rect.boundary[5] = BPoints->z[i + 1];
01240         }
01241         else {
01242             rect.boundary[2] = BPoints->z[i + 1];
01243             rect.boundary[5] = BPoints->z[i];
01244         }
01245 
01246         RTreeInsertRect(&rect, i + 1, &RTree, 0);       /* B line segment numbers in rtree start from 1 */
01247     }
01248 
01249     /* Find intersection */
01250     cross_found = 0;
01251 
01252     for (i = 0; i < APoints->n_points - 1; i++) {
01253         if (APoints->x[i] <= APoints->x[i + 1]) {
01254             rect.boundary[0] = APoints->x[i];
01255             rect.boundary[3] = APoints->x[i + 1];
01256         }
01257         else {
01258             rect.boundary[0] = APoints->x[i + 1];
01259             rect.boundary[3] = APoints->x[i];
01260         }
01261 
01262         if (APoints->y[i] <= APoints->y[i + 1]) {
01263             rect.boundary[1] = APoints->y[i];
01264             rect.boundary[4] = APoints->y[i + 1];
01265         }
01266         else {
01267             rect.boundary[1] = APoints->y[i + 1];
01268             rect.boundary[4] = APoints->y[i];
01269         }
01270         if (APoints->z[i] <= APoints->z[i + 1]) {
01271             rect.boundary[2] = APoints->z[i];
01272             rect.boundary[5] = APoints->z[i + 1];
01273         }
01274         else {
01275             rect.boundary[2] = APoints->z[i + 1];
01276             rect.boundary[5] = APoints->z[i];
01277         }
01278 
01279         RTreeSearch(RTree, &rect, (void *)find_cross, &i);      /* A segment number from 0 */
01280 
01281         if (cross_found) {
01282             break;
01283         }
01284     }
01285 
01286     /* Free RTree */
01287     RTreeDestroyNode(RTree);
01288 
01289     return cross_found;
01290 }
01291 
01305 int
01306 Vect_line_get_intersections(struct line_pnts *APoints,
01307                             struct line_pnts *BPoints,
01308                             struct line_pnts *IPoints, int with_z)
01309 {
01310     int ret;
01311 
01312     IPnts = IPoints;
01313     ret = Vect_line_check_intersection(APoints, BPoints, with_z);
01314 
01315     return ret;
01316 }