94 lines
3 KiB
Go
94 lines
3 KiB
Go
package numerics
|
|
|
|
// Field returns the time derivative of a geographic state at (t, y).
|
|
// The derivative is direction-independent; the integrator applies the sign
|
|
// of dt for reverse-time integration.
|
|
type Field func(t float64, y GeoVec) GeoVec
|
|
|
|
// Crossed reports whether a termination condition holds at (t, y).
|
|
type Crossed func(t float64, y GeoVec) bool
|
|
|
|
// RK4Step performs one classical Runge-Kutta-4 step from (t, y) with step dt.
|
|
// dt may be negative to integrate backwards in time. Longitude wrapping is
|
|
// applied at every intermediate add via GeoAdd, matching the reference
|
|
// integrator. The function performs no heap allocation.
|
|
func RK4Step(t float64, y GeoVec, dt float64, f Field) GeoVec {
|
|
half := dt / 2
|
|
k1 := f(t, y)
|
|
k2 := f(t+half, GeoAdd(y, half, k1))
|
|
k3 := f(t+half, GeoAdd(y, half, k2))
|
|
k4 := f(t+dt, GeoAdd(y, dt, k3))
|
|
|
|
y2 := GeoAdd(y, dt/6, k1)
|
|
y2 = GeoAdd(y2, dt/3, k2)
|
|
y2 = GeoAdd(y2, dt/3, k3)
|
|
y2 = GeoAdd(y2, dt/6, k4)
|
|
return y2
|
|
}
|
|
|
|
// RefineCrossing locates a crossing between (t1, y1) (not crossed) and
|
|
// (t2, y2) (crossed) by binary search in the linear-interpolation parameter
|
|
// space, stopping when the parameter interval is narrower than tol.
|
|
//
|
|
// It returns the final midpoint sampled, matching Tawhiri's solver.pyx: the
|
|
// returned point is not guaranteed to satisfy the predicate, but for tol << 1
|
|
// it is within one tolerance-width of the true crossing.
|
|
func RefineCrossing(t1 float64, y1 GeoVec, t2 float64, y2 GeoVec, crossed Crossed, tol float64) (float64, GeoVec) {
|
|
left, right := 0.0, 1.0
|
|
t3, y3 := t2, y2
|
|
for right-left > tol {
|
|
mid := (left + right) / 2
|
|
t3 = Lerp(t1, t2, mid)
|
|
y3 = GeoLerp(y1, y2, mid)
|
|
if crossed(t3, y3) {
|
|
right = mid
|
|
} else {
|
|
left = mid
|
|
}
|
|
}
|
|
return t3, y3
|
|
}
|
|
|
|
// Path is a struct-of-arrays trajectory: parallel slices of time and the
|
|
// three state components. SoA layout keeps each component contiguous, which
|
|
// is friendlier to cache and to vectorised post-processing than a slice of
|
|
// point structs, and lets the integrator append with a single bounds check
|
|
// per component.
|
|
type Path struct {
|
|
T []float64
|
|
Lat []float64
|
|
Lng []float64
|
|
Altitude []float64
|
|
}
|
|
|
|
// NewPath returns a Path with capacity reserved for n points.
|
|
func NewPath(n int) Path {
|
|
return Path{
|
|
T: make([]float64, 0, n),
|
|
Lat: make([]float64, 0, n),
|
|
Lng: make([]float64, 0, n),
|
|
Altitude: make([]float64, 0, n),
|
|
}
|
|
}
|
|
|
|
// Len returns the number of points in the path.
|
|
func (p *Path) Len() int { return len(p.T) }
|
|
|
|
// Append adds one point to the path.
|
|
func (p *Path) Append(t float64, y GeoVec) {
|
|
p.T = append(p.T, t)
|
|
p.Lat = append(p.Lat, y.Lat)
|
|
p.Lng = append(p.Lng, y.Lng)
|
|
p.Altitude = append(p.Altitude, y.Altitude)
|
|
}
|
|
|
|
// Last returns the final (t, state) of the path. It panics on an empty path.
|
|
func (p *Path) Last() (float64, GeoVec) {
|
|
i := len(p.T) - 1
|
|
return p.T[i], GeoVec{Lat: p.Lat[i], Lng: p.Lng[i], Altitude: p.Altitude[i]}
|
|
}
|
|
|
|
// At returns the point at index i.
|
|
func (p *Path) At(i int) (float64, GeoVec) {
|
|
return p.T[i], GeoVec{Lat: p.Lat[i], Lng: p.Lng[i], Altitude: p.Altitude[i]}
|
|
}
|