50 lines
1.2 KiB
Go
50 lines
1.2 KiB
Go
package engine
|
|
|
|
import "math"
|
|
|
|
// pymod returns a % b with Python semantics: the result has the sign of b,
|
|
// so for b > 0 the result is always in [0, b).
|
|
func pymod(a, b float64) float64 {
|
|
r := math.Mod(a, b)
|
|
if r < 0 {
|
|
r += b
|
|
}
|
|
return r
|
|
}
|
|
|
|
// stateAdd is the RK4 integrator's update operation y + k*dy, with longitude
|
|
// kept wrapped to [0, 360).
|
|
//
|
|
// Time is not stored in State — it is tracked separately by the integrator
|
|
// and passed to Model.
|
|
func stateAdd(y State, k float64, dy State) State {
|
|
return State{
|
|
Lat: y.Lat + k*dy.Lat,
|
|
Lng: pymod(y.Lng+k*dy.Lng, 360),
|
|
Altitude: y.Altitude + k*dy.Altitude,
|
|
}
|
|
}
|
|
|
|
// stateLerp computes the linear interpolation of two states by parameter l
|
|
// in [0, 1]. Longitude uses lngLerp so that wrap-around is handled.
|
|
func stateLerp(a, b State, l float64) State {
|
|
return State{
|
|
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 great-circle arc.
|
|
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)
|
|
}
|