intersect.c

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

Generated on Sun Apr 6 17:32:44 2008 for GRASS by  doxygen 1.5.5