66 lines
1.8 KiB
Go
66 lines
1.8 KiB
Go
package numerics
|
|
|
|
import "math"
|
|
|
|
// GeoVec is a geographic state vector: latitude and longitude in degrees and
|
|
// altitude in metres. The same struct represents a per-second derivative,
|
|
// in which case the fields are deg/s and m/s.
|
|
//
|
|
// GeoVec is the hot-path state type for the integrator. It is a small value
|
|
// type (three float64) and is passed by value to stay allocation-free; a
|
|
// future SIMD/SoA batch integrator can lift these fields into parallel
|
|
// slices (see Path).
|
|
type GeoVec struct {
|
|
Lat float64 `json:"lat"`
|
|
Lng float64 `json:"lng"`
|
|
Altitude float64 `json:"altitude"`
|
|
}
|
|
|
|
// PyMod returns a mod b with Python semantics: the result carries the sign of
|
|
// b, so for b > 0 it always lies in [0, b).
|
|
func PyMod(a, b float64) float64 {
|
|
r := math.Mod(a, b)
|
|
if r < 0 {
|
|
r += b
|
|
}
|
|
return r
|
|
}
|
|
|
|
// GeoAdd returns y + k*dy with longitude wrapped to [0, 360). Latitude and
|
|
// altitude accumulate linearly. This is the integrator's state-update step.
|
|
func GeoAdd(y GeoVec, k float64, dy GeoVec) GeoVec {
|
|
return GeoVec{
|
|
Lat: y.Lat + k*dy.Lat,
|
|
Lng: PyMod(y.Lng+k*dy.Lng, 360),
|
|
Altitude: y.Altitude + k*dy.Altitude,
|
|
}
|
|
}
|
|
|
|
// GeoLerp linearly interpolates two geographic states by parameter l in
|
|
// [0, 1]. Longitude takes the shorter great-circle arc.
|
|
func GeoLerp(a, b GeoVec, l float64) GeoVec {
|
|
return GeoVec{
|
|
Lat: (1-l)*a.Lat + l*b.Lat,
|
|
Lng: LngLerp(a.Lng, b.Lng, l),
|
|
Altitude: (1-l)*a.Altitude + l*b.Altitude,
|
|
}
|
|
}
|
|
|
|
// LngLerp interpolates between two longitudes in [0, 360), choosing the
|
|
// shorter arc and wrapping the result back into range.
|
|
func LngLerp(a, b, l float64) float64 {
|
|
l2 := 1 - l
|
|
if a > b {
|
|
a, b = b, a
|
|
l, l2 = l2, l
|
|
}
|
|
if b-a < 180 {
|
|
return l2*a + l*b
|
|
}
|
|
return PyMod(l2*(a+360)+l*b, 360)
|
|
}
|
|
|
|
// Lerp returns (1-l)*a + l*b.
|
|
func Lerp(a, b, l float64) float64 {
|
|
return (1-l)*a + l*b
|
|
}
|