Add linear referencing methods to LineString

The first method is `InterpolatePoint`, which interpolates a single `Point` given a fraction along the `LineString`. The second method is `InterpolateEvenlySpacedPoints`, which interpolates many `Point`s along a `LineString` at once.
peterstace · Apr 1, 2022 · 623210f · 623210f
1 parent 25bf547
commit 623210f
Show file tree

Hide file tree

Showing 5 changed files with 245 additions and 0 deletions.
diff --git a/geom/alg_linear_interpolation.go b/geom/alg_linear_interpolation.go
@@ -0,0 +1,57 @@
+package geom
+
+import (
+	"math"
+	"sort"
+)
+
+type linearInterpolator struct {
+	seq        Sequence
+	cumulative []float64
+	total      float64
+}
+
+func newLinearInterpolator(seq Sequence) linearInterpolator {
+	n := seq.Length()
+	if n == 0 {
+		panic("empty seq in newLinearInterpolator")
+	}
+	var total float64
+	cumulative := make([]float64, n-1)
+	for i := 0; i < n-1; i++ {
+		total += seq.GetXY(i).distanceTo(seq.GetXY(i + 1))
+		cumulative[i] = total
+	}
+	return linearInterpolator{seq, cumulative, total}
+}
+
+func (l linearInterpolator) interpolate(frac float64) Point {
+	frac = math.Max(0, math.Min(1, frac))
+	idx := sort.SearchFloat64s(l.cumulative, frac*l.total)
+	if idx == l.seq.Length() {
+		return l.seq.Get(idx - 1).asUncheckedPoint()
+	}
+
+	p0 := l.seq.Get(idx)
+	p1 := l.seq.Get(idx + 1)
+
+	partial := frac * l.total
+	if idx-1 >= 0 {
+		partial -= l.cumulative[idx-1]
+	}
+	partial /= p0.XY.distanceTo(p1.XY)
+
+	return Coordinates{
+		XY: XY{
+			X: lerp(p0.X, p1.X, partial),
+			Y: lerp(p0.Y, p1.Y, partial),
+		},
+		Z:    lerp(p0.Z, p1.Z, partial),
+		M:    lerp(p0.M, p1.M, partial),
+		Type: l.seq.CoordinatesType(),
+	}.asUncheckedPoint()
+}
+
+func lerp(a, b, ratio float64) float64 {
+	return a*(1-ratio) + b*ratio
+}
diff --git a/geom/alg_linear_interpolation_test.go b/geom/alg_linear_interpolation_test.go
@@ -0,0 +1,129 @@
+package geom_test
+
+import (
+	"fmt"
+	"strconv"
+	"testing"
+)
+
+func TestInterpolatePointEmpty(t *testing.T) {
+	for _, variant := range []string{"", "Z", "M", "ZM"} {
+		for _, ratio := range []float64{-0.5, 0, 0.5, 1, 1.5} {
+			t.Run(fmt.Sprintf("%v_%v", variant, ratio), func(t *testing.T) {
+				inputWKT := "LINESTRING " + variant + " EMPTY"
+				input := geomFromWKT(t, inputWKT).MustAsLineString()
+				wantWKT := "POINT " + variant + " EMPTY"
+				got := input.InterpolatePoint(ratio).AsGeometry()
+				expectGeomEqWKT(t, got, wantWKT)
+			})
+		}
+	}
+}
+
+func TestInterpolatePoint(t *testing.T) {
+	for i, tc := range []struct {
+		lsWKT   string
+		frac    float64
+		wantWKT string
+	}{
+		// Single segment XY
+		{"LINESTRING(2 1,1 3)", -0.5, "POINT(2.00 1.0)"},
+		{"LINESTRING(2 1,1 3)", 0.00, "POINT(2.00 1.0)"},
+		{"LINESTRING(2 1,1 3)", 0.25, "POINT(1.75 1.5)"},
+		{"LINESTRING(2 1,1 3)", 0.50, "POINT(1.50 2.0)"},
+		{"LINESTRING(2 1,1 3)", 0.75, "POINT(1.25 2.5)"},
+		{"LINESTRING(2 1,1 3)", 1.00, "POINT(1.00 3.0)"},
+		{"LINESTRING(2 1,1 3)", 1.50, "POINT(1.00 3.0)"},
+
+		// Single segment Z
+		{"LINESTRING Z (2 1 5,1 3 7)", -0.5, "POINT Z (2.00 1.0 5.0)"},
+		{"LINESTRING Z (2 1 5,1 3 7)", 0.00, "POINT Z (2.00 1.0 5.0)"},
+		{"LINESTRING Z (2 1 5,1 3 7)", 0.25, "POINT Z (1.75 1.5 5.5)"},
+		{"LINESTRING Z (2 1 5,1 3 7)", 0.50, "POINT Z (1.50 2.0 6.0)"},
+		{"LINESTRING Z (2 1 5,1 3 7)", 0.75, "POINT Z (1.25 2.5 6.5)"},
+		{"LINESTRING Z (2 1 5,1 3 7)", 1.00, "POINT Z (1.00 3.0 7.0)"},
+		{"LINESTRING Z (2 1 5,1 3 7)", 1.50, "POINT Z (1.00 3.0 7.0)"},
+
+		// Single segment M
+		{"LINESTRING M (2 1 5,1 3 7)", -0.5, "POINT M (2.00 1.0 5.0)"},
+		{"LINESTRING M (2 1 5,1 3 7)", 0.00, "POINT M (2.00 1.0 5.0)"},
+		{"LINESTRING M (2 1 5,1 3 7)", 0.25, "POINT M (1.75 1.5 5.5)"},
+		{"LINESTRING M (2 1 5,1 3 7)", 0.50, "POINT M (1.50 2.0 6.0)"},
+		{"LINESTRING M (2 1 5,1 3 7)", 0.75, "POINT M (1.25 2.5 6.5)"},
+		{"LINESTRING M (2 1 5,1 3 7)", 1.00, "POINT M (1.00 3.0 7.0)"},
+		{"LINESTRING M (2 1 5,1 3 7)", 1.50, "POINT M (1.00 3.0 7.0)"},
+
+		// Single segment ZM
+		{"LINESTRING ZM (2 1 5 7,1 3 7 5)", -0.5, "POINT ZM (2.00 1.0 5.0 7.0)"},
+		{"LINESTRING ZM (2 1 5 7,1 3 7 5)", 0.00, "POINT ZM (2.00 1.0 5.0 7.0)"},
+		{"LINESTRING ZM (2 1 5 7,1 3 7 5)", 0.25, "POINT ZM (1.75 1.5 5.5 6.5)"},
+		{"LINESTRING ZM (2 1 5 7,1 3 7 5)", 0.50, "POINT ZM (1.50 2.0 6.0 6.0)"},
+		{"LINESTRING ZM (2 1 5 7,1 3 7 5)", 0.75, "POINT ZM (1.25 2.5 6.5 5.5)"},
+		{"LINESTRING ZM (2 1 5 7,1 3 7 5)", 1.00, "POINT ZM (1.00 3.0 7.0 5.0)"},
+		{"LINESTRING ZM (2 1 5 7,1 3 7 5)", 1.50, "POINT ZM (1.00 3.0 7.0 5.0)"},
+
+		// Multiple Segments (all equal length)
+		{"LINESTRING(0 0,1 0,2 0,3 0,3 1)", 0.000, "POINT(0.0 0.0)"},
+		{"LINESTRING(0 0,1 0,2 0,3 0,3 1)", 0.125, "POINT(0.5 0.0)"},
+		{"LINESTRING(0 0,1 0,2 0,3 0,3 1)", 0.250, "POINT(1.0 0.0)"},
+		{"LINESTRING(0 0,1 0,2 0,3 0,3 1)", 0.375, "POINT(1.5 0.0)"},
+		{"LINESTRING(0 0,1 0,2 0,3 0,3 1)", 0.875, "POINT(3.0 0.5)"},
+
+		// Multiple Segments (different lengths)
+		{"LINESTRING(0 0,3 0,3 1)", 0.000, "POINT(0.0 0.0)"},
+		{"LINESTRING(0 0,3 0,3 1)", 0.125, "POINT(0.5 0.0)"},
+		{"LINESTRING(0 0,3 0,3 1)", 0.250, "POINT(1.0 0.0)"},
+		{"LINESTRING(0 0,3 0,3 1)", 0.375, "POINT(1.5 0.0)"},
+		{"LINESTRING(0 0,3 0,3 1)", 0.875, "POINT(3.0 0.5)"},
+	} {
+		t.Run(strconv.Itoa(i), func(t *testing.T) {
+			ls := geomFromWKT(t, tc.lsWKT).MustAsLineString()
+			t.Logf("ls:   %v", ls.AsText())
+			t.Logf("frac: %v", tc.frac)
+			got := ls.InterpolatePoint(tc.frac).AsGeometry()
+			expectGeomEqWKT(t, got, tc.wantWKT)
+		})
+	}
+}
+
+func TestInterpolateEvenlySpacedPointsEmpty(t *testing.T) {
+	for _, variant := range []string{"", "Z", "M", "ZM"} {
+		for n, want := range map[int]string{
+			-1: "MULTIPOINT " + variant + " EMPTY",
+			0:  "MULTIPOINT " + variant + " EMPTY",
+			1:  "MULTIPOINT " + variant + " (EMPTY)",
+			2:  "MULTIPOINT " + variant + " (EMPTY,EMPTY)",
+		} {
+			t.Run(fmt.Sprintf("%v_%v", variant, n), func(t *testing.T) {
+				inputWKT := "LINESTRING " + variant + " EMPTY"
+				input := geomFromWKT(t, inputWKT).MustAsLineString()
+				got := input.InterpolateEvenlySpacedPoints(n).AsGeometry()
+				expectGeomEqWKT(t, got, want)
+			})
+		}
+	}
+}
+
+func TestInterpolateEvenlySpacedPoints(t *testing.T) {
+	for i, tc := range []struct {
+		lsWKT   string
+		n       int
+		wantWKT string
+	}{
+		{"LINESTRING(1 1,2 3)", 0, "MULTIPOINT EMPTY"},
+		{"LINESTRING(1 1,2 3)", 1, "MULTIPOINT((1.5 2))"},
+		{"LINESTRING(1 1,2 3)", 2, "MULTIPOINT((1 1),(2 3))"},
+		{"LINESTRING(1 1,2 3)", 3, "MULTIPOINT((1 1),(1.5 2),(2 3))"},
+		{"LINESTRING(1 1,2 3)", 5, "MULTIPOINT((1 1),(1.25 1.5),(1.5 2),(1.75 2.5),(2 3))"},
+
+		{"LINESTRING(0 0,1 0,2 0,3 0,3 1)", 5, "MULTIPOINT(0 0,1 0,2 0,3 0,3 1)"},
+		{"LINESTRING(0 0,        3 0,3 1)", 5, "MULTIPOINT(0 0,1 0,2 0,3 0,3 1)"},
+		{"LINESTRING(0 0,        3 0,3 1)", 1, "MULTIPOINT(2 0)"},
+	} {
+		t.Run(strconv.Itoa(i), func(t *testing.T) {
+			ls := geomFromWKT(t, tc.lsWKT).MustAsLineString()
+			got := ls.InterpolateEvenlySpacedPoints(tc.n).AsGeometry()
+			expectGeomEqWKT(t, got, tc.wantWKT)
+		})
+	}
+}
diff --git a/geom/type_coordinates.go b/geom/type_coordinates.go
@@ -62,3 +62,9 @@ func (c Coordinates) appendFloat64s(dst []float64) []float64 {
 		panic(c.Type.String())
 	}
 }
+
+// asUncheckedPoint shadows the asUncheckedPoint method on XY so that it's not
+// accidentally called.
+func (c Coordinates) asUncheckedPoint() Point {
+	return newUncheckedPoint(c)
+}
diff --git a/geom/type_line_string.go b/geom/type_line_string.go
@@ -442,3 +442,52 @@ func (s LineString) Simplify(threshold float64) LineString {
 	seq = NewSequence(floats, seq.CoordinatesType())
 	return newLineStringWithOmitInvalid(seq)
 }
+
+// InterpolatePoint returns a Point interpolated along the LineString at the
+// given fraction. The fraction should be between 0 and 1, and will be clipped
+// to that range if outside of it. Z and M coordinates are also interpolated if
+// applicable.
+func (s LineString) InterpolatePoint(fraction float64) Point {
+	if s.IsEmpty() {
+		return Point{}.ForceCoordinatesType(s.CoordinatesType())
+	}
+	interp := newLinearInterpolator(s.Coordinates())
+	return interp.interpolate(fraction)
+}
+
+// InterpolateEvenlySpacedPoints returns a MultiPoint consisting of n Points
+// evenly spaced along the LineString. If n is negative or 0, then an empty
+// MultiPoint is returned. If n is 1, then a MultiPoint containing the
+// LineString midpoint is returned. If n is 2 or greater, then the returned
+// MultiPoint contains the LineString start point, n - 2 evenly spaced
+// intermediate Points, and the LineString end point (in that order).
+func (s LineString) InterpolateEvenlySpacedPoints(n int) MultiPoint {
+	if n < 0 {
+		n = 0
+	}
+	if n == 0 {
+		return MultiPoint{}.ForceCoordinatesType(s.CoordinatesType())
+	}
+
+	seq := s.Coordinates()
+	if seq.Length() == 0 {
+		pts := make([]Point, n)
+		for i := 0; i < n; i++ {
+			pts[i] = Point{}.ForceCoordinatesType(s.CoordinatesType())
+		}
+		return NewMultiPoint(pts)
+	}
+
+	if n == 1 {
+		interp := newLinearInterpolator(s.Coordinates())
+		return interp.interpolate(0.5).AsMultiPoint()
+	}
+
+	interp := newLinearInterpolator(s.Coordinates())
+	pts := make([]Point, n)
+	for i := 0; i < n; i++ {
+		frac := float64(i) / float64(n-1)
+		pts[i] = interp.interpolate(frac)
+	}
+	return NewMultiPoint(pts)
+}
diff --git a/geom/xy.go b/geom/xy.go
@@ -112,6 +112,10 @@ func (w XY) Less(o XY) bool {
 	return w.Y < o.Y
 }
 
+func (w XY) distanceTo(o XY) float64 {
+	return math.Sqrt(w.distanceSquaredTo(o))
+}
+
 func (w XY) distanceSquaredTo(o XY) float64 {
 	delta := o.Sub(w)
 	return delta.Dot(delta)