89 lines
2.8 KiB
Go
89 lines
2.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
|
|
}
|
|
|
|
// AddGeo returns the component-wise sum a+b without longitude wrapping. Use it
|
|
// to combine derivative (rate) vectors — rates accumulate linearly, unlike
|
|
// positions, which wrap via GeoAdd.
|
|
func AddGeo(a, b GeoVec) GeoVec {
|
|
return GeoVec{Lat: a.Lat + b.Lat, Lng: a.Lng + b.Lng, Altitude: a.Altitude + b.Altitude}
|
|
}
|
|
|
|
// EarthRadius is the spherical Earth radius (metres) used for horizontal
|
|
// motion, matching the reference Tawhiri implementation.
|
|
const EarthRadius = 6371009.0
|
|
|
|
// WindToGeoRate converts eastward (u) and northward (v) wind in m/s at the
|
|
// given latitude (deg) and altitude (m) into the geographic rate in deg/s on a
|
|
// spherical Earth. The returned dLng diverges near the poles as cos(lat) → 0.
|
|
func WindToGeoRate(u, v, lat, alt float64) (dLat, dLng float64) {
|
|
const degPerRad = 180.0 / math.Pi
|
|
const piOver180 = math.Pi / 180.0
|
|
r := EarthRadius + alt
|
|
dLat = degPerRad * v / r
|
|
dLng = degPerRad * u / (r * math.Cos(lat*piOver180))
|
|
return dLat, dLng
|
|
}
|