feat(decart): interp

internal/numerics/spherical_test.go

@@ -0,0 +1,230 @@
+package numerics
+
+import (
+	"math"
+	"testing"
+)
+
+// Closed-form checks for great-circle stepping. Each expectation is an exact
+// analytic result, not a golden value copied from a previous run.
+
+const tolDeg = 1e-9
+
+func TestGeoStep(t *testing.T) {
+	t.Parallel()
+
+	// Angular distance covered by `speed` for `dt` at sea level, in degrees.
+	arcDeg := func(speed, dt float64) float64 {
+		return speed * dt / EarthRadius * 180 / math.Pi
+	}
+
+	tests := []struct {
+		name             string
+		start            GeoVec
+		rate             Rate
+		dt               float64
+		wantLat, wantLng float64
+		wantAlt          float64
+	}{
+		{
+			name:    "eastward at the equator advances longitude only",
+			start:   GeoVec{Lat: 0, Lng: 0},
+			rate:    Rate{East: 10},
+			dt:      100,
+			wantLat: 0,
+			wantLng: arcDeg(10, 100),
+		},
+		{
+			name:    "northward at the equator advances latitude only",
+			start:   GeoVec{Lat: 0, Lng: 0},
+			rate:    Rate{North: 10},
+			dt:      100,
+			wantLat: arcDeg(10, 100),
+			wantLng: 0,
+		},
+		{
+			name:    "zero horizontal rate leaves the position alone",
+			start:   GeoVec{Lat: 51.5, Lng: 359.9, Altitude: 1000},
+			rate:    Rate{Vertical: 5},
+			dt:      10,
+			wantLat: 51.5,
+			wantLng: 359.9,
+			wantAlt: 1050,
+		},
+		{
+			// The whole point of the change: 111 m from the pole, a due-north
+			// step must pass over the pole and come down the far meridian.
+			// Start 0.001 deg from the pole, travel 600 m (0.005396 deg), so it
+			// overshoots by 0.004396 deg on longitude 30+180.
+			name:    "due north over the pole flips longitude by 180",
+			start:   GeoVec{Lat: 89.999, Lng: 30},
+			rate:    Rate{North: 10},
+			dt:      60,
+			wantLat: 90 - (arcDeg(10, 60) - 0.001),
+			wantLng: 210,
+		},
+		{
+			name:    "due south over the south pole flips longitude by 180",
+			start:   GeoVec{Lat: -89.999, Lng: 200},
+			rate:    Rate{North: -10},
+			dt:      60,
+			wantLat: -90 + (arcDeg(10, 60) - 0.001),
+			wantLng: 20,
+		},
+		{
+			name:    "longitude stays wrapped into [0,360)",
+			start:   GeoVec{Lat: 0, Lng: 359.999},
+			rate:    Rate{East: 100},
+			dt:      100,
+			wantLat: 0,
+			wantLng: math.Mod(359.999+arcDeg(100, 100), 360),
+		},
+	}
+
+	for _, tt := range tests {
+		t.Run(tt.name, func(t *testing.T) {
+			t.Parallel()
+			got := GeoStep(tt.start, tt.rate, tt.dt)
+			if math.Abs(got.Lat-tt.wantLat) > tolDeg {
+				t.Errorf("Lat = %.12f, want %.12f", got.Lat, tt.wantLat)
+			}
+			if math.Abs(got.Lng-tt.wantLng) > tolDeg {
+				t.Errorf("Lng = %.12f, want %.12f", got.Lng, tt.wantLng)
+			}
+			if math.Abs(got.Altitude-tt.wantAlt) > 1e-9 {
+				t.Errorf("Altitude = %v, want %v", got.Altitude, tt.wantAlt)
+			}
+		})
+	}
+}
+
+// The defect this replaces: dLng went as 1/cos(lat), reaching 1.46e12 deg/s at
+// the pole. Ground displacement must instead stay equal to speed*dt at every
+// latitude, because that is what physically happens.
+func TestGeoStepGroundDistanceIndependentOfLatitude(t *testing.T) {
+	t.Parallel()
+
+	const speed, dt = 10.0, 60.0
+	want := speed * dt // 600 m
+
+	for _, lat := range []float64{0, 45, 60, 85.051129, 89, 89.9, 89.99, 89.999, 90} {
+		start := GeoVec{Lat: lat, Lng: 17}
+		got := GeoStep(start, Rate{East: speed}, dt)
+		d := GreatCircleMetres(start, got)
+		// 0.1 mm. The point is to catch a divergence — the old code was wrong by
+		// a factor of 1e12 here — not to police the last bit: near the pole
+		// cos(lat) is ~1e-5, so double precision limits this to a few microns.
+		if math.Abs(d-want) > 1e-4 {
+			t.Errorf("lat %g: ground distance = %.9f m, want %.9f m", lat, d, want)
+		}
+	}
+}
+
+func TestGeoStepReverseGoesBackAlongTheSameCircle(t *testing.T) {
+	t.Parallel()
+
+	// A negative dt must travel the same arc in the opposite direction. Note it
+	// is NOT true that GeoStep(GeoStep(y, r, dt), r, -dt) == y: the local frame
+	// rotates during the step, so the same east/north pair means a different
+	// physical direction at the arrival point. The invariant that does hold is
+	// that the two endpoints straddle the start on one great circle.
+	const speed, dt = 12.0, 60.0
+	for _, lat := range []float64{0, 60, 89.99} {
+		y := GeoVec{Lat: lat, Lng: 100, Altitude: 5000}
+		r := Rate{East: speed, Vertical: 3}
+		fwd := GeoStep(y, r, dt)
+		back := GeoStep(y, r, -dt)
+
+		// The arc is flown at altitude, so its projection onto the surface —
+		// which is what GreatCircleMetres reports — is shorter by R/(R+alt).
+		wantGround := speed * dt * EarthRadius / (EarthRadius + y.Altitude)
+		if d := GreatCircleMetres(y, back); math.Abs(d-wantGround) > 1e-4 {
+			t.Errorf("lat %g: reverse arc = %.9f m, want %.9f m", lat, d, wantGround)
+		}
+		if d := GreatCircleMetres(fwd, back); math.Abs(d-2*wantGround) > 1e-4 {
+			t.Errorf("lat %g: forward/reverse separation = %.9f m, want %.9f m",
+				lat, d, 2*wantGround)
+		}
+		if math.Abs(back.Altitude-(y.Altitude-3*dt)) > 1e-9 {
+			t.Errorf("lat %g: reverse altitude = %v, want %v", lat, back.Altitude, y.Altitude-3*dt)
+		}
+	}
+}
+
+// Great-circle motion is rotation about a fixed axis, so a field returning the
+// local east/north components of `omega x r` has an exact solution: a single
+// rotation. RK4 must reproduce it, including next to the pole where the local
+// frame spins fastest between stages.
+//
+// A field with *constant* east/north would be a rhumb line, not a great circle,
+// so it cannot be used for this comparison.
+func TestRK4StepMatchesGreatCircle(t *testing.T) {
+	t.Parallel()
+
+	for _, lat := range []float64{0, 60, 89.9, 89.999} {
+		start := GeoVec{Lat: lat, Lng: 40, Altitude: 20000}
+		const speed, dt, n = 35.0, 60.0, 10
+
+		// Rate at the start point defines the great circle; GeoStep over the
+		// whole interval is then the exact answer.
+		initial := Rate{East: speed * 0.8, North: speed * 0.6}
+		exact := GeoStep(start, initial, dt*n)
+
+		// Pointwise field for that same great circle: rotation about the axis
+		// r0 x t0 at constant angular rate.
+		field := greatCircleField(start, initial)
+
+		stepped := start
+		for range n {
+			stepped = RK4Step(0, stepped, dt, field)
+		}
+
+		if d := GreatCircleMetres(stepped, exact); d > 0.01 {
+			t.Errorf("lat %g: RK4 drifted %.6f m from the exact great circle", lat, d)
+		}
+	}
+}
+
+func TestAddRate(t *testing.T) {
+	t.Parallel()
+
+	got := AddRate(Rate{East: 1, North: 2, Vertical: 3}, Rate{East: 10, North: 20, Vertical: 30})
+	want := Rate{East: 11, North: 22, Vertical: 33}
+	if got != want {
+		t.Errorf("AddRate = %+v, want %+v", got, want)
+	}
+}
+
+// greatCircleField builds a rate field whose exact solution is the great circle
+// through `start` with initial rate `initial`: rotation about the fixed axis
+// r0 x t0. At any point it returns the local east/north components of that
+// rotation's velocity, so the field is defined pointwise without needing to
+// know how far along the arc we are.
+func greatCircleField(start GeoVec, initial Rate) RateField {
+	speed := math.Hypot(initial.East, initial.North)
+	r0, e0, n0 := basis(start.Lat, start.Lng)
+	var t0 [3]float64
+	for i := range t0 {
+		t0[i] = (initial.East*e0[i] + initial.North*n0[i]) / speed
+	}
+	axis := cross(r0, t0)
+
+	return func(_ float64, y GeoVec) Rate {
+		r, e, n := basis(y.Lat, y.Lng)
+		v := cross(axis, r)
+		return Rate{
+			East:  speed * dot(v, e),
+			North: speed * dot(v, n),
+		}
+	}
+}
+
+func cross(a, b [3]float64) [3]float64 {
+	return [3]float64{
+		a[1]*b[2] - a[2]*b[1],
+		a[2]*b[0] - a[0]*b[2],
+		a[0]*b[1] - a[1]*b[0],
+	}
+}
+
+func dot(a, b [3]float64) float64 { return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] }