s2: Add Polyline.Interpolate.

Signed-off-by: David Symonds <dsymonds@golang.org>

s2/polyline.go

@@ -508,10 +508,56 @@ func (p *Polyline) Intersects(o *Polyline) bool {
 	return false
 }
 
+// Interpolate returns the point whose distance from vertex 0 along the polyline is
+// the given fraction of the polyline's total length, and the index of
+// the next vertex after the interpolated point P. Fractions less than zero
+// or greater than one are clamped. The return value is unit length. The cost of
+// this function is currently linear in the number of vertices.
+//
+// This method allows the caller to easily construct a given suffix of the
+// polyline by concatenating P with the polyline vertices starting at that next
+// vertex. Note that P is guaranteed to be different than the point at the next
+// vertex, so this will never result in a duplicate vertex.
+//
+// The polyline must not be empty. Note that if fraction >= 1.0, then the next
+// vertex will be set to len(p) (indicating that no vertices from the polyline
+// need to be appended). The value of the next vertex is always between 1 and
+// len(p).
+//
+// This method can also be used to construct a prefix of the polyline, by
+// taking the polyline vertices up to next vertex-1 and appending the
+// returned point P if it is different from the last vertex (since in this
+// case there is no guarantee of distinctness).
+func (p *Polyline) Interpolate(fraction float64) (Point, int) {
+	// We intentionally let the (fraction >= 1) case fall through, since
+	// we need to handle it in the loop below in any case because of
+	// possible roundoff errors.
+	if fraction <= 0 {
+		return (*p)[0], 1
+	}
+	target := s1.Angle(fraction) * p.Length()
+
+	for i := 1; i < len(*p); i++ {
+		length := (*p)[i-1].Distance((*p)[i])
+		if target < length {
+			// This interpolates with respect to arc length rather than
+			// straight-line distance, and produces a unit-length result.
+			result := InterpolateAtDistance(target, (*p)[i-1], (*p)[i])
+
+			// It is possible that (result == vertex(i)) due to rounding errors.
+			if result == (*p)[i] {
+				return result, i + 1
+			}
+			return result, i
+		}
+		target -= length
+	}
+
+	return (*p)[len(*p)-1], len(*p)
+}
+
 // TODO(roberts): Differences from C++.
-// Suffix
-// Interpolate/UnInterpolate
-// ApproxEqual
+// UnInterpolate
 // NearlyCoversPolyline
 // InitToSnapped
 // InitToSimplified

s2/polyline_test.go

@@ -43,9 +43,10 @@ func TestPolylineBasics(t *testing.T) {
 	}
 
 	semiEquator := PolylineFromLatLngs(latlngs)
-	//if got, want := semiEquator.Interpolate(0.5), Point{r3.Vector{0, 1, 0}}; !got.ApproxEqual(want) {
-	//	t.Errorf("semiEquator.Interpolate(0.5) = %v, want %v", got, want)
-	//}
+	want := PointFromCoords(0, 1, 0)
+	if got, _ := semiEquator.Interpolate(0.5); !got.ApproxEqual(want) {
+		t.Errorf("semiEquator.Interpolate(0.5) = %v, want %v", got, want)
+	}
 	semiEquator.Reverse()
 	if got, want := (*semiEquator)[2], (Point{r3.Vector{1, 0, 0}}); !got.ApproxEqual(want) {
 		t.Errorf("semiEquator[2] = %v, want %v", got, want)
@@ -517,8 +518,73 @@ func TestPolylineApproxEqual(t *testing.T) {
 	}
 }
 
+func TestPolylineInterpolate(t *testing.T) {
+	vertices := []Point{PointFromCoords(1, 0, 0),
+		PointFromCoords(0, 1, 0),
+		PointFromCoords(0, 1, 1),
+		PointFromCoords(0, 0, 1),
+	}
+	line := Polyline(vertices)
+
+	want := vertices[0]
+	point, next := line.Interpolate(-0.1)
+	if point != vertices[0] {
+		t.Errorf("%v.Interpolate(%v) = %v, want %v", line, -0.1, point, vertices[0])
+	}
+	if next != 1 {
+		t.Errorf("%v.Interpolate(%v) = %v, want %v", line, -0.1, next, 1)
+	}
+
+	want = PointFromCoords(1, math.Tan(0.2*math.Pi/2.0), 0)
+	if got, _ := line.Interpolate(0.1); !got.ApproxEqual(want) {
+		t.Errorf("%v.Interpolate(%v) = %v, want %v", line, 0.1, got, want)
+	}
+
+	want = PointFromCoords(1, 1, 0)
+	if got, _ := line.Interpolate(0.25); !got.ApproxEqual(want) {
+		t.Errorf("%v.Interpolate(%v) = %v, want %v", line, 0.25, got, want)
+	}
+
+	want = vertices[1]
+	if got, _ := line.Interpolate(0.5); got != want {
+		t.Errorf("%v.Interpolate(%v) = %v, want %v", line, 0.5, got, want)
+	}
+
+	want = vertices[2]
+	point, next = line.Interpolate(0.75)
+	if !point.ApproxEqual(want) {
+		t.Errorf("%v.Interpolate(%v) = %v, want %v", line, 0.75, point, want)
+	}
+	if next != 3 {
+		t.Errorf("%v.Interpolate(%v) = %v, want %v", line, 0.75, next, 3)
+	}
+
+	point, next = line.Interpolate(1.1)
+	if point != vertices[3] {
+		t.Errorf("%v.Interpolate(%v) = %v, want %v", line, 1.1, point, vertices[3])
+	}
+	if next != 4 {
+		t.Errorf("%v.Interpolate(%v) = %v, want %v", line, 1.1, next, 4)
+	}
+
+	// Check the case where the interpolation fraction is so close to 1 that
+	// the interpolated point is identical to the last vertex.
+	vertices2 := []Point{PointFromCoords(1, 1, 1),
+		PointFromCoords(1, 1, 1+1e-15),
+		PointFromCoords(1, 1, 1+2e-15),
+	}
+	shortLine := Polyline(vertices2)
+
+	point, next = shortLine.Interpolate(1.0 - 2e-16)
+	if point != vertices2[2] {
+		t.Errorf("%v.Interpolate(%v) = %v, want %v", shortLine, 1.0-2e-16, point, vertices2[2])
+	}
+	if next != 3 {
+		t.Errorf("%v.Interpolate(%v) = %v, want %v", shortLine, 1.0-2e-16, next, 3)
+	}
+}
+
 // TODO(roberts): Test differences from C++:
-// Interpolate
 // UnInterpolate
 //
 // PolylineCoveringTest