From 1bfbc2f712926c19ce75d5094609fb433eaa03ce Mon Sep 17 00:00:00 2001 From: Jesse Eichar Date: Mon, 4 Apr 2016 16:21:01 +0200 Subject: [PATCH] Added Robust Line intersector. Need more tests still --- .../big-double-cga.go => big_cga/big_cga.go} | 75 ++-- algorithm/big_cga/big_cga_test.go | 57 +++ algorithm/central_endpoint/intersector.go | 77 ++++ .../central_endpoint/intersector_test.go | 43 +++ algorithm/hcoords/hcoords.go | 43 +++ algorithm/hcoords/hcoords_test.go | 51 +++ algorithm/line_intersector_test_support.go | 71 ++-- algorithm/robust_line_intersector.go | 351 +++++++++++++++++- bounds.go | 13 + bounds_test.go | 21 ++ geom.go | 15 + geom_test.go | 69 ++++ 12 files changed, 825 insertions(+), 61 deletions(-) rename algorithm/{cga/big-double-cga.go => big_cga/big_cga.go} (50%) create mode 100644 algorithm/big_cga/big_cga_test.go create mode 100644 algorithm/central_endpoint/intersector.go create mode 100644 algorithm/central_endpoint/intersector_test.go create mode 100644 algorithm/hcoords/hcoords.go create mode 100644 algorithm/hcoords/hcoords_test.go diff --git a/algorithm/cga/big-double-cga.go b/algorithm/big_cga/big_cga.go similarity index 50% rename from algorithm/cga/big-double-cga.go rename to algorithm/big_cga/big_cga.go index 5aeb66bf..8072efdc 100644 --- a/algorithm/cga/big-double-cga.go +++ b/algorithm/big_cga/big_cga.go @@ -1,4 +1,4 @@ -package cga +package big_cga import ( "github.com/twpayne/go-geom" @@ -11,22 +11,36 @@ import ( */ var dp_safe_epsilon = 1e-15 +type Orientation int + +const ( + CLOCKWISE Orientation = iota - 1 + COLLINEAR + COUNTER_CLOCKWISE +) + +var orientationLabels = [3]string{"CLOCKWISE", "COLLINEAR", "COUNTER_CLOCKWISE"} + +func (o Orientation) String() string { + return orientationLabels[int(o+1)] +} + /** - * Returns the index of the direction of the point q relative to - * a vector specified by p1-p2. + * Returns the index of the direction of the point point relative to + * a vector specified by vectorOrigin-vectorEnd. * - * @param p1 the origin point of the vector - * @param p2 the final point of the vector - * @param q the point to compute the direction to + * @param vectorOrigin the origin point of the vector + * @param vectorEnd the final point of the vector + * @param point the point to compute the direction to * - * @return 1 if q is counter-clockwise (left) from p1-p2 - * @return -1 if q is clockwise (right) from p1-p2 - * @return 0 if q is collinear with p1-p2 + * @return COUNTER_CLOCKWISE if point is counter-clockwise (left) from vectorOrigin-vectorEnd + * @return CLOCKWISE if point is clockwise (right) from vectorOrigin-vectorEnd + * @return COLLINEAR if point is collinear with vectorOrigin-vectorEnd */ -func OrientationIndex(p1, p2, q geom.Coord) int { +func OrientationIndex(vectorOrigin, vectorEnd, point geom.Coord) Orientation { // fast filter for orientation index // avoids use of slow extended-precision arithmetic in many cases - index := OrientationIndexFilter(p1, p2, q) + index := orientationIndexFilter(vectorOrigin, vectorEnd, point) if index <= 1 { return index } @@ -34,15 +48,17 @@ func OrientationIndex(p1, p2, q geom.Coord) int { var dx1, dy1, dx2, dy2 big.Float // normalize coordinates - dx1.SetFloat64(p2.X()).Add(&dx1, big.NewFloat(-p1.X())) - dy1.SetFloat64(p2.Y()).Add(&dy1, big.NewFloat(-p1.Y())) - dx2.SetFloat64(q.X()).Add(&dx2, big.NewFloat(-p2.X())) - dy2.SetFloat64(q.Y()).Add(&dy2, big.NewFloat(-p2.Y())) + dx1.SetFloat64(vectorEnd.X()).Add(&dx1, big.NewFloat(-vectorOrigin.X())) + dy1.SetFloat64(vectorEnd.Y()).Add(&dy1, big.NewFloat(-vectorOrigin.Y())) + dx2.SetFloat64(point.X()).Add(&dx2, big.NewFloat(-vectorEnd.X())) + dy2.SetFloat64(point.Y()).Add(&dy2, big.NewFloat(-vectorEnd.Y())) - // calculate determinant. Calculation takes place in dx1 - dx1.Mul(&dx1, &dy2).Sub(&dx1, dy1.Mul(&dx1, &dx2)) + // calculate determinant. Calculation takes place in dx1 for performance + dx1.Mul(&dx1, &dy2) + dy1.Mul(&dy1, &dx2) + dx1.Sub(&dx1, &dy1) - return bigSignum(dx1) + return Orientation(bigSignum(dx1)) } /** @@ -65,7 +81,7 @@ func OrientationIndex(p1, p2, q geom.Coord) int { * @return the orientation index if it can be computed safely * @return i > 1 if the orientation index cannot be computed safely */ -func OrientationIndexFilter(pa, pb, pc geom.Coord) int { +func orientationIndexFilter(pa, pb, pc geom.Coord) Orientation { var detsum float64 detleft := (pa.X() - pc.X()) * (pb.Y() - pc.Y()) @@ -96,18 +112,25 @@ func OrientationIndexFilter(pa, pb, pc geom.Coord) int { return 2 } -func signum(x float64) int { +func signum(x float64) Orientation { if x > 0 { - return 1 + return COUNTER_CLOCKWISE } if x < 0 { - return -1 + return CLOCKWISE } - return 0 + return COLLINEAR } -func bigSignum(x big.Float) int { +func bigSignum(x big.Float) Orientation { if x.IsInf() { - return 0 + return COLLINEAR + } + switch x.Sign() { + case -1: + return CLOCKWISE + case 0: + return COLLINEAR + default: + return COUNTER_CLOCKWISE } - return x.Sign() } diff --git a/algorithm/big_cga/big_cga_test.go b/algorithm/big_cga/big_cga_test.go new file mode 100644 index 00000000..1a83f030 --- /dev/null +++ b/algorithm/big_cga/big_cga_test.go @@ -0,0 +1,57 @@ +package big_cga_test + +import ( + "github.com/twpayne/go-geom" + "github.com/twpayne/go-geom/algorithm/big_cga" + "testing" +) + +func TestOrientationIndex(t *testing.T) { + for i, testData := range []struct { + vectorOrigin, vectorEnd, point geom.Coord + result big_cga.Orientation + }{ + + { + vectorOrigin: geom.Coord{-1.0, -1.0}, + vectorEnd: geom.Coord{1.0, 1.0}, + point: geom.Coord{0, 0}, + result: big_cga.COLLINEAR, + }, + { + vectorOrigin: geom.Coord{1.0, 1.0}, + vectorEnd: geom.Coord{-1.0, -1.0}, + point: geom.Coord{0, 0}, + result: big_cga.COLLINEAR, + }, + { + vectorOrigin: geom.Coord{10.0, 10.0}, + vectorEnd: geom.Coord{20.0, 20.0}, + point: geom.Coord{10.0, 20.0}, + result: big_cga.COUNTER_CLOCKWISE, + }, + { + vectorOrigin: geom.Coord{10.0, 10.0}, + vectorEnd: geom.Coord{20.0, 20.0}, + point: geom.Coord{20.0, 10.0}, + result: big_cga.CLOCKWISE, + }, + { + vectorOrigin: geom.Coord{10.0, 20.0}, + vectorEnd: geom.Coord{20.0, 10.0}, + point: geom.Coord{10.0, 10.0}, + result: big_cga.CLOCKWISE, + }, + { + vectorOrigin: geom.Coord{10.0, 20.0}, + vectorEnd: geom.Coord{20.0, 10.0}, + point: geom.Coord{20.0, 20.00}, + result: big_cga.COUNTER_CLOCKWISE, + }, + } { + orientationIndex := big_cga.OrientationIndex(testData.vectorOrigin, testData.vectorEnd, testData.point) + if orientationIndex != testData.result { + t.Errorf("Test %v Failed. Expected: %v (%v) but was %v (%v) : TestData: %v", i+1, testData.result, int(testData.result), orientationIndex, int(orientationIndex), testData) + } + } +} diff --git a/algorithm/central_endpoint/intersector.go b/algorithm/central_endpoint/intersector.go new file mode 100644 index 00000000..0ea59772 --- /dev/null +++ b/algorithm/central_endpoint/intersector.go @@ -0,0 +1,77 @@ +package central_endpoint + +import ( + "github.com/twpayne/go-geom" + "math" +) + +/* + * Computes an approximate intersection of two line segments by taking the most central of the endpoints of the segments. + + * This is effective in cases where the segments are nearly parallel and should intersect at an endpoint. + * It is also a reasonable strategy for cases where the endpoint of one segment lies on or almost on the interior of another one. + * Taking the most central endpoint ensures that the computed intersection point lies in the envelope of the segments. + + * Also, by always returning one of the input points, this should result in reducing segment fragmentation. + * Intended to be used as a last resort for computing ill-conditioned intersection situations which cause other methods to fail. + * + */ +func GetIntersection(line1End1, line1End2, line2End1, line2End2 geom.Coord) geom.Coord { + intersector := centralEndpointIntersector{ + line1End1: line1End1, + line1End2: line1End2, + line2End1: line2End1, + line2End2: line2End2} + intersector.compute() + return intersector.intPt +} + +type centralEndpointIntersector struct { + line1End1, line1End2, line2End1, line2End2, intPt geom.Coord +} + +func (intersector *centralEndpointIntersector) compute() { + pts := [4]geom.Coord{intersector.line1End1, intersector.line1End2, intersector.line2End1, intersector.line2End2} + centroid := average(pts) + intersector.intPt = findNearestPoint(centroid, pts) +} + +func average(pts [4]geom.Coord) geom.Coord { + n := float64(len(pts)) + avg := geom.Coord{0, 0} + + for i := 0; i < len(pts); i++ { + avg[0] += pts[i][0] + avg[1] += pts[i][1] + } + if n > 0 { + avg[0] = avg[0] / n + avg[1] = avg[1] / n + } + return avg +} + +/** + * Determines a point closest to the given point. + * + * @param p the point to compare against + * @param p1 a potential result point + * @param p2 a potential result point + * @param q1 a potential result point + * @param q2 a potential result point + * @return the point closest to the input point p + */ +func findNearestPoint(p geom.Coord, pts [4]geom.Coord) geom.Coord { + minDist := math.MaxFloat64 + result := geom.Coord{} + for i := 0; i < len(pts); i++ { + dist := p.Distance2D(pts[i]) + + // always initialize the result + if i == 0 || dist < minDist { + minDist = dist + result = pts[i] + } + } + return result +} diff --git a/algorithm/central_endpoint/intersector_test.go b/algorithm/central_endpoint/intersector_test.go new file mode 100644 index 00000000..d819e17c --- /dev/null +++ b/algorithm/central_endpoint/intersector_test.go @@ -0,0 +1,43 @@ +package central_endpoint_test + +import ( + "github.com/twpayne/go-geom" + "github.com/twpayne/go-geom/algorithm/central_endpoint" + "testing" +) + +func TestGetIntersection(t *testing.T) { + for i, tc := range []struct { + p1, p2, p3, p4 geom.Coord + layout geom.Layout + result geom.Coord + }{ + { + p1: geom.Coord{-1, 0}, p2: geom.Coord{1, 0}, p3: geom.Coord{0, -1}, p4: geom.Coord{0, 1}, + layout: geom.XY, + result: geom.Coord{-1.0, 0.0}, + }, + { + p1: geom.Coord{10, 10}, p2: geom.Coord{20, 20}, p3: geom.Coord{10, 20}, p4: geom.Coord{20, 10}, + layout: geom.XY, + result: geom.Coord{10.0, 10.0}, + }, + { + p1: geom.Coord{10, 10}, p2: geom.Coord{20, 20}, p3: geom.Coord{20, 20}, p4: geom.Coord{10, 10}, + layout: geom.XY, + result: geom.Coord{10.0, 10.0}, + }, + { + p1: geom.Coord{10, 10}, p2: geom.Coord{20, 20}, p3: geom.Coord{30, 20}, p4: geom.Coord{20, 10}, + layout: geom.XY, + result: geom.Coord{20.0, 20.0}, + }, + } { + calculatedResult := central_endpoint.GetIntersection(tc.p1, tc.p2, tc.p3, tc.p4) + + if !calculatedResult.Equal(tc.layout, tc.result) { + t.Errorf("Test '%v' failed: expected \n%v but was \n%v", i+1, tc.result, calculatedResult) + } + + } +} diff --git a/algorithm/hcoords/hcoords.go b/algorithm/hcoords/hcoords.go new file mode 100644 index 00000000..aa67a374 --- /dev/null +++ b/algorithm/hcoords/hcoords.go @@ -0,0 +1,43 @@ +package hcoords + +import ( + "fmt" + "github.com/twpayne/go-geom" + "math" +) + +/** + * Computes the (approximate) intersection point between two line segments + * using homogeneous coordinates. + *

+ * Note that this algorithm is not numerically stable; i.e. it can produce intersection points which + * lie outside the envelope of the line segments themselves. In order to increase the precision of the calculation + * input points should be normalized before passing them to this routine. + */ +func GetIntersection(p1, p2, q1, q2 geom.Coord) (geom.Coord, error) { + // unrolled computation + px := p1[1] - p2[1] + py := p2[0] - p1[0] + pw := p1[0]*p2[1] - p2[0]*p1[1] + + qx := q1[1] - q2[1] + qy := q2[0] - q1[0] + qw := q1[0]*q2[1] - q2[0]*q1[1] + + x := py*qw - qy*pw + y := qx*pw - px*qw + w := px*qy - qx*py + + xInt := x / w + yInt := y / w + + if math.IsNaN(xInt) || math.IsNaN(xInt) || math.IsNaN(yInt) || math.IsNaN(yInt) { + return nil, fmt.Errorf("intersection cannot be calculated using the h-coords implementation") + } + + if math.IsInf(xInt, 0) || math.IsInf(xInt, 0) || math.IsInf(yInt, 0) || math.IsInf(yInt, 0) { + return nil, fmt.Errorf("intersection cannot be calculated using the h-coords implementation") + } + + return geom.Coord{xInt, yInt}, nil +} diff --git a/algorithm/hcoords/hcoords_test.go b/algorithm/hcoords/hcoords_test.go new file mode 100644 index 00000000..38b27476 --- /dev/null +++ b/algorithm/hcoords/hcoords_test.go @@ -0,0 +1,51 @@ +package hcoords_test + +import ( + "github.com/twpayne/go-geom" + "github.com/twpayne/go-geom/algorithm/hcoords" + "testing" +) + +func TestGetIntersection(t *testing.T) { + for i, tc := range []struct { + p1, p2, p3, p4 geom.Coord + layout geom.Layout + result geom.Coord + err bool + }{ + { + p1: geom.Coord{-1, 0}, p2: geom.Coord{1, 0}, p3: geom.Coord{0, -1}, p4: geom.Coord{0, 1}, + layout: geom.XY, + result: geom.Coord{0.0, 0.0}, + err: false, + }, + { + p1: geom.Coord{10, 10}, p2: geom.Coord{20, 20}, p3: geom.Coord{10, 20}, p4: geom.Coord{20, 10}, + layout: geom.XY, + result: geom.Coord{15.0, 15.0}, + err: false, + }, + { + p1: geom.Coord{10, 10}, p2: geom.Coord{20, 20}, p3: geom.Coord{20, 20}, p4: geom.Coord{10, 10}, + layout: geom.XY, + result: nil, + err: true, + }, + { + p1: geom.Coord{10, 10}, p2: geom.Coord{20, 20}, p3: geom.Coord{30, 20}, p4: geom.Coord{20, 10}, + layout: geom.XY, + result: nil, + err: true, + }, + } { + calculatedResult, err := hcoords.GetIntersection(tc.p1, tc.p2, tc.p3, tc.p4) + + if !calculatedResult.Equal(tc.layout, tc.result) { + t.Errorf("Test '%v' failed: expected \n%v but was \n%v", i+1, tc.result, calculatedResult) + } + + if tc.err != (err != nil) { + t.Errorf("Test '%v' failed: expected \n%v but was \n%v", i+1, tc.result, calculatedResult) + } + } +} diff --git a/algorithm/line_intersector_test_support.go b/algorithm/line_intersector_test_support.go index 782c0958..de54142c 100644 --- a/algorithm/line_intersector_test_support.go +++ b/algorithm/line_intersector_test_support.go @@ -3,6 +3,7 @@ package algorithm import ( "github.com/twpayne/go-geom" "reflect" + "runtime/debug" "testing" ) @@ -13,7 +14,8 @@ func exectuteLineIntersectionPointOnLineTest(t *testing.T, intersectionStrategy }{ { p: geom.Coord{0, 0}, lineEnd1: geom.Coord{-1, 0}, lineEnd2: geom.Coord{1, 0}, result: true, - }, { + }, + { p: geom.Coord{0, 1}, lineEnd1: geom.Coord{-1, 1}, lineEnd2: geom.Coord{1, 1}, result: true, }, { @@ -32,7 +34,7 @@ func exectuteLineIntersectionPointOnLineTest(t *testing.T, intersectionStrategy intersector := LineIntersector{Layout: geom.XY, Strategy: intersectionStrategy} calculatedResult := intersector.PointIntersectsLine(tc.p, tc.lineEnd1, tc.lineEnd2) if !reflect.DeepEqual(tc.result, calculatedResult) { - t.Errorf("Test '%v' failed: expected \n%v was \n%v", i, tc.result, calculatedResult) + t.Errorf("Test '%v' failed: expected \n%v was \n%v", i+1, tc.result, calculatedResult) } } } @@ -42,22 +44,22 @@ func executeLineIntersectionLinesTest(t *testing.T, intersectionStrategy LineInt p1, p2, p3, p4 geom.Coord result LineOnLineIntersection }{ - { - p1: geom.Coord{-1, 0}, p2: geom.Coord{1, 0}, p3: geom.Coord{0, -1}, p4: geom.Coord{0, 1}, - result: LineOnLineIntersection{ - HasIntersection: true, - IntersectionType: POINT_INTERSECTION, - Intersection: []geom.Coord{geom.Coord{0, 0}}, - }, - }, - { - p1: geom.Coord{10, 10}, p2: geom.Coord{20, 20}, p3: geom.Coord{10, 20}, p4: geom.Coord{20, 10}, - result: LineOnLineIntersection{ - HasIntersection: true, - IntersectionType: POINT_INTERSECTION, - Intersection: []geom.Coord{geom.Coord{15, 15}}, - }, - }, + //{ + // p1: geom.Coord{-1, 0}, p2: geom.Coord{1, 0}, p3: geom.Coord{0, -1}, p4: geom.Coord{0, 1}, + // result: LineOnLineIntersection{ + // HasIntersection: true, + // IntersectionType: POINT_INTERSECTION, + // Intersection: []geom.Coord{geom.Coord{0, 0}}, + // }, + //}, + //{ + // p1: geom.Coord{10, 10}, p2: geom.Coord{20, 20}, p3: geom.Coord{10, 20}, p4: geom.Coord{20, 10}, + // result: LineOnLineIntersection{ + // HasIntersection: true, + // IntersectionType: POINT_INTERSECTION, + // Intersection: []geom.Coord{geom.Coord{15, 15}}, + // }, + //}, { p1: geom.Coord{10, 10}, p2: geom.Coord{20, 20}, p3: geom.Coord{20, 20}, p4: geom.Coord{10, 10}, result: LineOnLineIntersection{ @@ -66,21 +68,28 @@ func executeLineIntersectionLinesTest(t *testing.T, intersectionStrategy LineInt Intersection: []geom.Coord{geom.Coord{10, 10}, geom.Coord{20, 20}}, }, }, - { - p1: geom.Coord{10, 10}, p2: geom.Coord{20, 20}, p3: geom.Coord{30, 20}, p4: geom.Coord{20, 10}, - result: LineOnLineIntersection{ - HasIntersection: false, - IntersectionType: NO_INTERSECTION, - Intersection: []geom.Coord{}, - }, - }, + //{ + // p1: geom.Coord{10, 10}, p2: geom.Coord{20, 20}, p3: geom.Coord{30, 20}, p4: geom.Coord{20, 10}, + // result: LineOnLineIntersection{ + // HasIntersection: false, + // IntersectionType: NO_INTERSECTION, + // Intersection: []geom.Coord{}, + // }, + //}, } { - intersector := LineIntersector{Layout: geom.XY, Strategy: intersectionStrategy} - calculatedResult := intersector.LineIntersectsLine(tc.p1, tc.p2, tc.p3, tc.p4) + func() { + defer func() { + if err := recover(); err != nil { + t.Errorf("An error occurred during Test '%v': %v\n%s", i+1, err, debug.Stack()) + } + }() + intersector := LineIntersector{Layout: geom.XY, Strategy: intersectionStrategy} + calculatedResult := intersector.LineIntersectsLine(tc.p1, tc.p2, tc.p3, tc.p4) - if !reflect.DeepEqual(calculatedResult, tc.result) { - t.Errorf("Test '%v' failed: expected \n%v but was \n%v", i, tc.result, calculatedResult) - } + if !reflect.DeepEqual(calculatedResult, tc.result) { + t.Errorf("Test '%v' failed: expected \n%v but was \n%v", i+1, tc.result, calculatedResult) + } + }() } } diff --git a/algorithm/robust_line_intersector.go b/algorithm/robust_line_intersector.go index ffc8ab8c..ba957b67 100644 --- a/algorithm/robust_line_intersector.go +++ b/algorithm/robust_line_intersector.go @@ -1,8 +1,12 @@ package algorithm import ( + "fmt" "github.com/twpayne/go-geom" - "github.com/twpayne/go-geom/algorithm/cga" + "github.com/twpayne/go-geom/algorithm/big_cga" + "github.com/twpayne/go-geom/algorithm/central_endpoint" + "github.com/twpayne/go-geom/algorithm/hcoords" + "math" ) type RobustLineIntersector struct { @@ -11,8 +15,8 @@ type RobustLineIntersector struct { func (intersector RobustLineIntersector) computePointOnLineIntersection(data *lineIntersectorData, p, lineEndpoint1, lineEndpoint2 geom.Coord) { data.isProper = false // do between check first, since it is faster than the orientation test - if geom.NewLineStringFlat(data.layout, []geom.Coord{lineEndpoint1, lineEndpoint2}).Bounds().ContainsPoint(p) { - if cga.OrientationIndex(lineEndpoint1, lineEndpoint2, p) == 0 && cga.OrientationIndex(lineEndpoint2, lineEndpoint1, p) == 0 { + if isPointWithinLineBounds(data.layout, p, lineEndpoint1, lineEndpoint2) { + if big_cga.OrientationIndex(lineEndpoint1, lineEndpoint2, p) == big_cga.COLLINEAR && big_cga.OrientationIndex(lineEndpoint2, lineEndpoint1, p) == big_cga.COLLINEAR { data.isProper = true if p.Equal(data.layout, lineEndpoint1) || p.Equal(data.layout, lineEndpoint2) { data.isProper = false @@ -24,6 +28,345 @@ func (intersector RobustLineIntersector) computePointOnLineIntersection(data *li data.intersectionType = NO_INTERSECTION } -func (intersector RobustLineIntersector) computeLineOnLineIntersection(data *lineIntersectorData, line1End1, line1End2, line2End1, line2End2 geom.Coord) { +func (intersector RobustLineIntersector) computeLineOnLineIntersection(data *lineIntersectorData, p1, p2, q1, q2 geom.Coord) { + data.isProper = false + + // first try a fast test to see if the envelopes of the lines intersect + if !doLinesOverlap(data.layout, p1, p2, q1, q2) { + data.intersectionType = NO_INTERSECTION + return + } + + // for each endpoint, compute which side of the other segment it lies + // if both endpoints lie on the same side of the other segment, + // the segments do not intersect + Pq1 := big_cga.OrientationIndex(p1, p2, q1) + Pq2 := big_cga.OrientationIndex(p1, p2, q2) + + if (Pq1 > 0 && Pq2 > 0) || (Pq1 < 0 && Pq2 < 0) { + data.intersectionType = NO_INTERSECTION + return + } + + Qp1 := big_cga.OrientationIndex(q1, q2, p1) + Qp2 := big_cga.OrientationIndex(q1, q2, p2) + + if (Qp1 > 0 && Qp2 > 0) || (Qp1 < 0 && Qp2 < 0) { + data.intersectionType = NO_INTERSECTION + return + } + + collinear := Pq1 == 0 && Pq2 == 0 && Qp1 == 0 && Qp2 == 0 + + if collinear { + data.intersectionType = computeCollinearIntersection(data, p1, p2, q1, q2) + return + } + + /* + * At this point we know that there is a single intersection point + * (since the lines are not collinear). + */ + + /* + * Check if the intersection is an endpoint. If it is, copy the endpoint as + * the intersection point. Copying the point rather than computing it + * ensures the point has the exact value, which is important for + * robustness. It is sufficient to simply check for an endpoint which is on + * the other line, since at this point we know that the inputLines must + * intersect. + */ + if Pq1 == 0 || Pq2 == 0 || Qp1 == 0 || Qp2 == 0 { + data.isProper = false + + /* + * Check for two equal endpoints. + * This is done explicitly rather than by the orientation tests + * below in order to improve robustness. + * + * [An example where the orientation tests fail to be consistent is + * the following (where the true intersection is at the shared endpoint + * POINT (19.850257749638203 46.29709338043669) + * + * LINESTRING ( 19.850257749638203 46.29709338043669, 20.31970698357233 46.76654261437082 ) + * and + * LINESTRING ( -48.51001596420236 -22.063180333403878, 19.850257749638203 46.29709338043669 ) + * + * which used to produce the INCORRECT result: (20.31970698357233, 46.76654261437082, NaN) + * + */ + if p1.Equal(data.layout, q1) || p1.Equal(data.layout, q2) { + data.intersectionPoints[0] = p1 + } else if p2.Equal(data.layout, q1) || p2.Equal(data.layout, q2) { + data.intersectionPoints[0] = p2 + } else if Pq1 == 0 { + // Now check to see if any endpoint lies on the interior of the other segment. + copy(q1, data.intersectionPoints[0]) + } else if Pq2 == 0 { + copy(q2, data.intersectionPoints[0]) + } else if Qp1 == 0 { + copy(p1, data.intersectionPoints[0]) + } else if Qp2 == 0 { + copy(p2, data.intersectionPoints[0]) + } + } else { + data.isProper = true + data.intersectionPoints[0] = intersection(data, p1, p2, q1, q2) + } + + data.intersectionType = POINT_INTERSECTION +} + +func computeCollinearIntersection(data *lineIntersectorData, line1End1, line1End2, line2End1, line2End2 geom.Coord) LineIntersectionType { + line2End1IntersectsLine1 := isPointWithinLineBounds(data.layout, line2End1, line1End1, line1End2) + line2End2IntersectsLine1 := isPointWithinLineBounds(data.layout, line2End2, line1End1, line1End2) + line1End1IntersectsLine2 := isPointWithinLineBounds(data.layout, line1End1, line2End1, line2End2) + line1End2IntersectsLine2 := isPointWithinLineBounds(data.layout, line1End2, line2End1, line2End2) + + if line1End1IntersectsLine2 && line1End2IntersectsLine2 { + data.intersectionPoints[0] = line1End1 + data.intersectionPoints[1] = line1End2 + return COLLINEAR_INTERSECTION + } + + if line2End1IntersectsLine1 && line2End2IntersectsLine1 { + data.intersectionPoints[0] = line2End1 + data.intersectionPoints[1] = line2End2 + return COLLINEAR_INTERSECTION + } + + if line2End1IntersectsLine1 && line1End1IntersectsLine2 { + data.intersectionPoints[0] = line2End1 + data.intersectionPoints[1] = line1End1 + + return isPointOrCollinearIntersection(data, line2End1, line1End1, line2End2IntersectsLine1, line1End2IntersectsLine2) + } + if line2End1IntersectsLine1 && line1End2IntersectsLine2 { + data.intersectionPoints[0] = line2End1 + data.intersectionPoints[1] = line1End2 + + return isPointOrCollinearIntersection(data, line2End1, line1End2, line2End2IntersectsLine1, line1End1IntersectsLine2) + } + + if line2End2IntersectsLine1 && line1End1IntersectsLine2 { + data.intersectionPoints[0] = line2End2 + data.intersectionPoints[1] = line1End1 + + return isPointOrCollinearIntersection(data, line2End2, line1End1, line2End1IntersectsLine1, line1End2IntersectsLine2) + } + + if line2End2IntersectsLine1 && line1End2IntersectsLine2 { + data.intersectionPoints[0] = line2End2 + data.intersectionPoints[1] = line1End2 + + return isPointOrCollinearIntersection(data, line2End2, line1End2, line2End1IntersectsLine1, line1End1IntersectsLine2) + } + + return NO_INTERSECTION +} + +func isPointOrCollinearIntersection(data *lineIntersectorData, p1, p2 geom.Coord, intersection1, intersection2 bool) LineIntersectionType { + if p1.Equal(data.layout, p2) && !intersection1 && !intersection2 { + return POINT_INTERSECTION + } else { + return COLLINEAR_INTERSECTION + } +} + +/** + * This method computes the actual value of the intersection point. + * To obtain the maximum precision from the intersection calculation, + * the coordinates are normalized by subtracting the minimum + * ordinate values (in absolute value). This has the effect of + * removing common significant digits from the calculation to + * maintain more bits of precision. + */ +func intersection(data *lineIntersectorData, p1, p2, q1, q2 geom.Coord) geom.Coord { + intPt := intersectionWithNormalization(p1, p2, q1, q2) + + /** + * Due to rounding it can happen that the computed intersection is + * outside the envelopes of the input segments. Clearly this + * is inconsistent. + * This code checks this condition and forces a more reasonable answer + * + * MD - May 4 2005 - This is still a problem. Here is a failure case: + * + * LINESTRING (2089426.5233462777 1180182.3877339689, 2085646.6891757075 1195618.7333999649) + * LINESTRING (1889281.8148903656 1997547.0560044837, 2259977.3672235999 483675.17050843034) + * int point = (2097408.2633752143,1144595.8008114607) + * + * MD - Dec 14 2006 - This does not seem to be a failure case any longer + */ + if !isInSegmentEnvelopes(data, intPt) { + // System.out.println("Intersection outside segment envelopes: " + intPt); + // System.out.println("Segments: " + this); + // compute a safer result + intPt = central_endpoint.GetIntersection(p1, p2, q1, q2) + // System.out.println("Snapped to " + intPt); + + fmt.Println(",,", intPt) + } + + // TODO Enable if we add a precision model + //if precisionModel != null { + // precisionModel.makePrecise(intPt); + //} + + return intPt +} + +func intersectionWithNormalization(line1End1, line1End2, line2End1, line2End2 geom.Coord) geom.Coord { + var n1, n2, n3, n4 geom.Coord = geom.Coord{0, 0}, geom.Coord{0, 0}, geom.Coord{0, 0}, geom.Coord{0, 0} + copy(n1, line1End1) + copy(n2, line1End2) + copy(n3, line2End1) + copy(n4, line2End2) + + normPt := geom.Coord{0, 0} + normalizeToEnvCentre(n1, n2, n3, n4, normPt) + + intPt := safeHCoordinateIntersection(n1, n2, n3, n4) + + intPt[0] += normPt[0] + intPt[1] += normPt[1] + + return intPt +} + +/** + * Computes a segment intersection using homogeneous coordinates. + * Round-off error can cause the raw computation to fail, + * (usually due to the segments being approximately parallel). + * If this happens, a reasonable approximation is computed instead. + * + * @param p1 a segment endpoint + * @param p2 a segment endpoint + * @param q1 a segment endpoint + * @param q2 a segment endpoint + * @return the computed intersection point + */ +func safeHCoordinateIntersection(p1, p2, q1, q2 geom.Coord) geom.Coord { + if intPt, err := hcoords.GetIntersection(p1, p2, q1, q2); err != nil { + return central_endpoint.GetIntersection(p1, p2, q1, q2) + } else { + return intPt + } +} + +/* + * Test whether a point lies in the envelopes of both input segments. + * A correctly computed intersection point should return true + * for this test. + * Since this test is for debugging purposes only, no attempt is + * made to optimize the envelope test. + * + * @return true if the input point lies within both input segment envelopes + */ +func isInSegmentEnvelopes(data *lineIntersectorData, intPt geom.Coord) bool { + intersection1 := isPointWithinLineBounds(data.layout, intPt, data.inputLines[0][0], data.inputLines[0][1]) + intersection2 := isPointWithinLineBounds(data.layout, intPt, data.inputLines[1][0], data.inputLines[1][1]) + + return intersection1 && intersection2 +} + +/** + * Normalize the supplied coordinates to + * so that the midpoint of their intersection envelope + * lies at the origin. + */ +func normalizeToEnvCentre(n00, n01, n10, n11, normPt geom.Coord) { + // Note: All these "max" checks are inlined for performance. + // It would be visually cleaner to do that but requires more function calls + + minX0 := n01[0] + if n00[0] < n01[0] { + minX0 = n00[0] + } + + minY0 := n01[1] + if n00[1] < n01[1] { + minY0 = n00[1] + } + maxX0 := n01[0] + if n00[0] > n01[0] { + maxX0 = n00[0] + } + maxY0 := n01[1] + if n00[1] > n01[1] { + maxY0 = n00[1] + } + + minX1 := n11[0] + if n10[0] < n11[0] { + minX1 = n10[0] + } + minY1 := n11[1] + if n10[1] < n11[1] { + minY1 = n10[1] + } + maxX1 := n11[0] + if n10[0] > n11[0] { + maxX1 = n10[0] + } + maxY1 := n11[1] + if n10[1] > n11[1] { + maxY1 = n10[1] + } + + intMinX := minX1 + if minX0 > minX1 { + intMinX = minX0 + } + intMaxX := maxX1 + if maxX0 < maxX1 { + intMaxX = maxX0 + } + intMinY := minY1 + if minY0 > minY1 { + intMinY = minY0 + } + intMaxY := maxY1 + if maxY0 < maxY1 { + intMaxY = maxY0 + } + + intMidX := (intMinX + intMaxX) / 2.0 + intMidY := (intMinY + intMaxY) / 2.0 + normPt[0] = intMidX + normPt[1] = intMidY + + n00[0] -= normPt[0] + n00[1] -= normPt[1] + n01[0] -= normPt[0] + n01[1] -= normPt[1] + n10[0] -= normPt[0] + n10[1] -= normPt[1] + n11[0] -= normPt[0] + n11[1] -= normPt[1] +} + +func isPointWithinLineBounds(layout geom.Layout, p, lineEndpoint1, lineEndpoint2 geom.Coord) bool { + minx := math.Min(lineEndpoint1[0], lineEndpoint2[0]) + maxx := math.Max(lineEndpoint1[0], lineEndpoint2[0]) + miny := math.Min(lineEndpoint1[1], lineEndpoint2[1]) + maxy := math.Max(lineEndpoint1[1], lineEndpoint2[1]) + return geom.NewBounds(layout).Set(minx, miny, maxx, maxy).OverlapsPoint(layout, p) +} + +func doLinesOverlap(layout geom.Layout, line1End1, line1End2, line2End1, line2End2 geom.Coord) bool { + + min1x := math.Min(line1End1[0], line1End2[0]) + max1x := math.Max(line1End1[0], line1End2[0]) + min1y := math.Min(line1End1[1], line1End2[1]) + max1y := math.Max(line1End1[1], line1End2[1]) + bounds1 := geom.NewBounds(layout).Set(min1x, min1y, max1x, max1y) + + min2x := math.Min(line2End1[0], line2End2[0]) + max2x := math.Max(line2End1[0], line2End2[0]) + min2y := math.Min(line2End1[1], line2End2[1]) + max2y := math.Max(line2End1[1], line2End2[1]) + bounds2 := geom.NewBounds(layout).Set(min2x, min2y, max2x, max2y) + return bounds1.Overlaps(layout, bounds2) } diff --git a/bounds.go b/bounds.go index aa329d05..d96f09e8 100644 --- a/bounds.go +++ b/bounds.go @@ -85,6 +85,19 @@ func (b *Bounds) Set(args ...float64) *Bounds { return b } +// Set sets the minimum and maximum values. +func (b *Bounds) SetCoords(min, max Coord) *Bounds { + b.min = Coord(make([]float64, b.layout.Stride())) + b.max = Coord(make([]float64, b.layout.Stride())) + + for i := 0; i < b.layout.Stride(); i++ { + b.min[i] = math.Min(min[i], max[i]) + b.max[i] = math.Max(min[i], max[i]) + } + + return b +} + func (b *Bounds) OverlapsPoint(layout Layout, point Coord) bool { for i, stride := 0, layout.Stride(); i < stride; i++ { if b.min[i] > point[i] || b.max[i] < point[i] { diff --git a/bounds_test.go b/bounds_test.go index fd031684..d1d2f382 100644 --- a/bounds_test.go +++ b/bounds_test.go @@ -193,3 +193,24 @@ func TestBoundsSet(t *testing.T) { bounds.Set(2, 2, 2, 2, 2) }() } + +func TestBoundsSetCoords(t *testing.T) { + bounds := &Bounds{layout: XY, min: Coord{0, 0}, max: Coord{10, 10}} + bounds.SetCoords(Coord{0, 0}, Coord{20, 20}) + expected := Bounds{layout: XY, min: Coord{0, 0}, max: Coord{20, 20}} + if !reflect.DeepEqual(expected, *bounds) { + t.Errorf("Expected %v but got %v", expected, *bounds) + } + + bounds = NewBounds(XY) + bounds.SetCoords(Coord{0, 0}, Coord{20, 20}) + if !reflect.DeepEqual(expected, *bounds) { + t.Errorf("Expected %v but got %v", expected, *bounds) + } + + bounds = NewBounds(XY) + bounds.SetCoords(Coord{20, 0}, Coord{0, 20}) // set coords should ensure valid min / max + if !reflect.DeepEqual(expected, *bounds) { + t.Errorf("Expected %v but got %v", expected, *bounds) + } +} diff --git a/geom.go b/geom.go index b2910a06..ae21d0ed 100644 --- a/geom.go +++ b/geom.go @@ -5,6 +5,7 @@ package geom import ( "errors" "fmt" + "math" ) // A Layout describes the meaning of an N-dimensional coordinate. Layout(N) for @@ -104,6 +105,20 @@ func (c Coord) Equal(layout Layout, other Coord) bool { return true } +/** + * Computes the 2-dimensional Euclidean distance to another location. + * All non x,z ordinates are ignored. + * + * @param other the point to compare to + * @return the 2-dimensional Euclidean distance between the locations + */ +func (c Coord) Distance2D(other Coord) float64 { + dx := c[0] - other[0] + dy := c[1] - other[1] + + return math.Sqrt(dx*dx + dy*dy) +} + // A T is a generic interface geomemented by all geometry types. type T interface { Layout() Layout diff --git a/geom_test.go b/geom_test.go index b868900f..1cac22d8 100644 --- a/geom_test.go +++ b/geom_test.go @@ -283,3 +283,72 @@ func TestSetCoord(t *testing.T) { } } } +func TestCoordDistance2D(t *testing.T) { + const diagOf1 = 1.4142135623730951 + for i, tc := range []struct { + src, other Coord + expected float64 + }{ + { + src: Coord{0, 0}, + other: Coord{1, 0}, + expected: 1, + }, + { + src: Coord{0, 0}, + other: Coord{0, 1}, + expected: 1, + }, + { + src: Coord{0, 0}, + other: Coord{-1, 0}, + expected: 1, + }, + { + src: Coord{0, 0}, + other: Coord{0, -1}, + expected: 1, + }, + { + src: Coord{0, 0}, + other: Coord{1, 1}, + expected: diagOf1, + }, + { + src: Coord{0, 0}, + other: Coord{1, -1}, + expected: diagOf1, + }, + { + src: Coord{0, 0}, + other: Coord{-1, -1}, + expected: diagOf1, + }, + { + src: Coord{0, 0}, + other: Coord{-1, 1}, + expected: diagOf1, + }, + { + src: Coord{0, 0}, + other: Coord{1, -1}, + expected: diagOf1, + }, + { + src: Coord{0, 0}, + other: Coord{0, 0}, + expected: 0, + }, + { + src: Coord{-100, 23}, + other: Coord{1, 2}, + expected: 103.16006979447037, + }, + } { + + distance := tc.src.Distance2D(tc.other) + if distance != tc.expected { + t.Errorf("Test %v failed: expected %v but got %v. Test Data: %v", i+1, tc.expected, distance, tc) + } + } +}