vectorlib: enhance numerical stability for segment intersections

git-svn-id: https://svn.osgeo.org/grass/grass/trunk@70780 15284696-431f-4ddb-bdfa-cd5b030d7da7

lib/vector/Vlib/intersect.c

@@ -114,7 +114,6 @@ int Vect_segment_intersection(double ax1, double ay1, double az1, double ax2,
 {
     static int first_3d = 1;
     double d, d1, d2, r1, dtol, t;
-    int switched;
 
     /* TODO: Works for points ? */
 
@@ -128,30 +127,21 @@ int Vect_segment_intersection(double ax1, double ay1, double az1, double ax2,
 	first_3d = 0;
     }
 
-    /* Check identical segments */
-    if ((ax1 == bx1 && ay1 == by1 && ax2 == bx2 && ay2 == by2) ||
-	(ax1 == bx2 && ay1 == by2 && ax2 == bx1 && ay2 == by1)) {
-	G_debug(2, " -> identical segments");
-	*x1 = ax1;
-	*y1 = ay1;
-	*z1 = az1;
-	*x2 = ax2;
-	*y2 = ay2;
-	*z2 = az2;
-	return 5;
-    }
-
-    /*  'Sort' lines by x, y 
+    /*  'Sort' each segment by x, y 
      *   MUST happen before D, D1, D2 are calculated */
-    switched = 0;
-    if (bx2 < bx1)
-	switched = 1;
-    else if (bx2 == bx1) {
-	if (by2 < by1)
-	    switched = 1;
+    if (ax2 < ax1 || (ax2 == ax1 && ay2 < ay1)) {
+	t = ax1;
+	ax1 = ax2;
+	ax2 = t;
+	t = ay1;
+	ay1 = ay2;
+	ay2 = t;
+	t = az1;
+	az1 = az2;
+	az2 = t;
     }
 
-    if (switched) {
+    if (bx2 < bx1 || (bx2 == bx1 && by2 < by1)) {
 	t = bx1;
 	bx1 = bx2;
 	bx2 = t;
@@ -163,24 +153,57 @@ int Vect_segment_intersection(double ax1, double ay1, double az1, double ax2,
 	bz2 = t;
     }
 
+    /* Check for identical segments */
+    if (ax1 == bx1 && ay1 == by1 && ax2 == bx2 && ay2 == by2) {
+	G_debug(2, " -> identical segments");
+	*x1 = ax1;
+	*y1 = ay1;
+	*z1 = az1;
+	*x2 = ax2;
+	*y2 = ay2;
+	*z2 = az2;
+	return 5;
+    }
+
+    /*  'Sort' segments by x, y: make sure a <= b
+     *   MUST happen before D, D1, D2 are calculated */
     switched = 0;
-    if (ax2 < ax1)
+    if (bx1 < ax1)
 	switched = 1;
-    else if (ax2 == ax1) {
-	if (ay2 < ay1)
+    else if (bx1 == ax1) {
+	if (bx2 < ax2)
 	    switched = 1;
+	else if (bx2 == ax2) {
+	    if (by1 < ay1)
+		switched = 1;
+	    else if (by1 == ay1) {
+		if (by2 < ay2)
+		    switched = 1;
+	    }
+	}
     }
 
     if (switched) {
 	t = ax1;
-	ax1 = ax2;
-	ax2 = t;
+	ax1 = bx1;
+	bx1 = t;
+	t = ax2;
+	ax2 = bx2;
+	bx2 = t;
+
 	t = ay1;
-	ay1 = ay2;
-	ay2 = t;
+	ay1 = by1;
+	by1 = t;
+	t = ay2;
+	ay2 = by2;
+	by2 = t;
+
 	t = az1;
-	az1 = az2;
-	az2 = t;
+	az1 = bz1;
+	bz1 = t;
+	t = az2;
+	az2 = bz2;
+	bz2 = t;
     }
 
     d = D;
@@ -1145,9 +1168,6 @@ static int find_cross(int id, const struct RTree_Rect *rect, int *arg)
 				    BPnts->y[j + 1], BPnts->z[j + 1], &x1,
 				    &y1, &z1, &x2, &y2, &z2, 0);
 
-    if (!IPnts)
-	IPnts = Vect_new_line_struct();
-
     switch (ret) {
     case 0:
     case 5:
@@ -1208,6 +1228,7 @@ Vect_line_check_intersection(struct line_pnts *APoints,
 
     if (!IPnts)
 	IPnts = Vect_new_line_struct();
+    Vect_reset_line(IPnts);
 
     /* If one or both are point (Points->n_points == 1) */
     if (APoints->n_points == 1 && BPoints->n_points == 1) {

lib/vector/diglib/linecros.c

@@ -61,6 +61,7 @@ dig_test_for_intersection(double ax1, double ay1,
 {
     register double d, d1, d2;
     double t;
+    int switched;
 
     if (ax1 > ax2 || (ax1 == ax2 && ay1 > ay2)) {
 	t = ax1;
@@ -82,6 +83,38 @@ dig_test_for_intersection(double ax1, double ay1,
 	by2 = t;
     }
 
+    switched = 0;
+    if (bx1 < ax1)
+	switched = 1;
+    else if (bx1 == ax1) {
+	if (bx2 < ax2)
+	    switched = 1;
+	else if (bx2 == ax2) {
+	    if (by1 < ay1)
+		switched = 1;
+	    else if (by1 == ay1) {
+		if (by2 < ay2)
+		    switched = 1;
+	    }
+	}
+    }
+
+    if (switched) {
+	t = ax1;
+	ax1 = bx1;
+	bx1 = t;
+	t = ax2;
+	ax2 = bx2;
+	bx2 = t;
+
+	t = ay1;
+	ay1 = by1;
+	by1 = t;
+	t = ay2;
+	ay2 = by2;
+	by2 = t;
+    }
+
     d = D;
     d1 = D1;
     d2 = D2;
@@ -156,6 +189,7 @@ dig_find_intersection(double ax1, double ay1,
 {
     register double d, r1, r2;
     double t;
+    int switched;
 
     if (ax1 > ax2 || (ax1 == ax2 && ay1 > ay2)) {
 	t = ax1;
@@ -177,6 +211,38 @@ dig_find_intersection(double ax1, double ay1,
 	by2 = t;
     }
 
+    switched = 0;
+    if (bx1 < ax1)
+	switched = 1;
+    else if (bx1 == ax1) {
+	if (bx2 < ax2)
+	    switched = 1;
+	else if (bx2 == ax2) {
+	    if (by1 < ay1)
+		switched = 1;
+	    else if (by1 == ay1) {
+		if (by2 < ay2)
+		    switched = 1;
+	    }
+	}
+    }
+
+    if (switched) {
+	t = ax1;
+	ax1 = bx1;
+	bx1 = t;
+	t = ax2;
+	ax2 = bx2;
+	bx2 = t;
+
+	t = ay1;
+	ay1 = by1;
+	by1 = t;
+	t = ay2;
+	ay2 = by2;
+	by2 = t;
+    }
+
     d = D;
 
     if (d) {