step one
This commit is contained in:
parent
7a8d5d13fa
commit
9e663db9dc
68 changed files with 5647 additions and 2958 deletions
47
internal/engine/constraints.go
Normal file
47
internal/engine/constraints.go
Normal file
|
|
@ -0,0 +1,47 @@
|
|||
package engine
|
||||
|
||||
// MaxAltitude triggers when altitude rises above Limit (in metres).
|
||||
// Used as the burst condition for ascent stages.
|
||||
type MaxAltitude struct {
|
||||
Limit float64
|
||||
On Action
|
||||
}
|
||||
|
||||
func (c MaxAltitude) Name() string { return "max_altitude" }
|
||||
func (c MaxAltitude) Violated(_ float64, s State) bool { return s.Altitude >= c.Limit }
|
||||
func (c MaxAltitude) Action() Action { return c.On }
|
||||
|
||||
// MinAltitude triggers when altitude falls at or below Limit (in metres).
|
||||
// With Limit=0 this is the "sea level" terminator.
|
||||
type MinAltitude struct {
|
||||
Limit float64
|
||||
On Action
|
||||
}
|
||||
|
||||
func (c MinAltitude) Name() string { return "min_altitude" }
|
||||
func (c MinAltitude) Violated(_ float64, s State) bool { return s.Altitude <= c.Limit }
|
||||
func (c MinAltitude) Action() Action { return c.On }
|
||||
|
||||
// MaxTime triggers when t exceeds Limit (UNIX seconds). Used as a stop
|
||||
// condition for float profiles.
|
||||
type MaxTime struct {
|
||||
Limit float64
|
||||
On Action
|
||||
}
|
||||
|
||||
func (c MaxTime) Name() string { return "max_time" }
|
||||
func (c MaxTime) Violated(t float64, _ State) bool { return t > c.Limit }
|
||||
func (c MaxTime) Action() Action { return c.On }
|
||||
|
||||
// TerrainContact triggers when altitude has dropped at or below ground level.
|
||||
// Equivalent to Tawhiri's elevation termination.
|
||||
type TerrainContact struct {
|
||||
Provider TerrainProvider
|
||||
On Action
|
||||
}
|
||||
|
||||
func (c TerrainContact) Name() string { return "terrain_contact" }
|
||||
func (c TerrainContact) Violated(_ float64, s State) bool {
|
||||
return c.Provider.Elevation(s.Lat, s.Lng) > s.Altitude
|
||||
}
|
||||
func (c TerrainContact) Action() Action { return c.On }
|
||||
176
internal/engine/engine_test.go
Normal file
176
internal/engine/engine_test.go
Normal file
|
|
@ -0,0 +1,176 @@
|
|||
package engine
|
||||
|
||||
import (
|
||||
"math"
|
||||
"testing"
|
||||
"time"
|
||||
|
||||
"predictor-refactored/internal/weather"
|
||||
)
|
||||
|
||||
// noWind is a WindField that always returns zero wind. Lets us test
|
||||
// integration of vertical-only profiles deterministically.
|
||||
type noWind struct{ epoch time.Time }
|
||||
|
||||
func (n noWind) Wind(_ float64, _, _, _ float64) (weather.Sample, error) {
|
||||
return weather.Sample{}, nil
|
||||
}
|
||||
func (n noWind) Epoch() time.Time { return n.epoch }
|
||||
func (n noWind) Source() string { return "test" }
|
||||
|
||||
// flatGround returns 0 metres everywhere.
|
||||
type flatGround struct{}
|
||||
|
||||
func (flatGround) Elevation(_, _ float64) float64 { return 0 }
|
||||
|
||||
func TestConstantAscentToBurst(t *testing.T) {
|
||||
burst := 30000.0
|
||||
rate := 5.0
|
||||
|
||||
ascend := &Propagator{
|
||||
Name: "ascent",
|
||||
Step: 60,
|
||||
Model: Sum(ConstantRate(rate), WindTransport(noWind{}, nil)),
|
||||
Constraints: []Constraint{MaxAltitude{Limit: burst, On: ActionStop}},
|
||||
}
|
||||
|
||||
prof := Profile{Stages: []*Propagator{ascend}, Direction: Forward}
|
||||
results := prof.Run(0, State{Lat: 0, Lng: 0, Altitude: 0})
|
||||
|
||||
if len(results) != 1 || results[0].Outcome != OutcomeStopped {
|
||||
t.Fatalf("expected one stopped stage, got %+v", results)
|
||||
}
|
||||
|
||||
last := results[0].Points[len(results[0].Points)-1]
|
||||
// Refinement tolerance is 0.01 in parameter space over a 60s step, so the
|
||||
// returned point sits within ±0.6s × rate ≈ ±3m of the boundary.
|
||||
if math.Abs(last.Altitude-burst) > 5 {
|
||||
t.Errorf("burst altitude = %v, want within 5m of %v", last.Altitude, burst)
|
||||
}
|
||||
wantTime := burst / rate
|
||||
if math.Abs(last.Time-wantTime) > 1 {
|
||||
t.Errorf("burst time = %v, want within 1s of %v", last.Time, wantTime)
|
||||
}
|
||||
}
|
||||
|
||||
func TestProfileWithFallback(t *testing.T) {
|
||||
burst := 1000.0
|
||||
rate := 5.0
|
||||
|
||||
descent := &Propagator{
|
||||
Name: "descent",
|
||||
Step: 60,
|
||||
Model: ParachuteDescent(rate),
|
||||
Constraints: []Constraint{TerrainContact{Provider: flatGround{}, On: ActionStop}},
|
||||
}
|
||||
ascend := &Propagator{
|
||||
Name: "ascent",
|
||||
Step: 60,
|
||||
Model: ConstantRate(rate),
|
||||
Constraints: []Constraint{MaxAltitude{Limit: burst, On: ActionFallback}},
|
||||
Fallback: descent,
|
||||
}
|
||||
|
||||
prof := Profile{Stages: []*Propagator{ascend}, Direction: Forward}
|
||||
results := prof.Run(0, State{Altitude: 0})
|
||||
|
||||
if len(results) != 2 {
|
||||
t.Fatalf("expected 2 results (ascent then descent fallback), got %d", len(results))
|
||||
}
|
||||
if results[0].Outcome != OutcomeFallback {
|
||||
t.Errorf("first outcome = %v, want OutcomeFallback", results[0].Outcome)
|
||||
}
|
||||
if results[1].Outcome != OutcomeStopped {
|
||||
t.Errorf("second outcome = %v, want OutcomeStopped", results[1].Outcome)
|
||||
}
|
||||
|
||||
last := results[1].Points[len(results[1].Points)-1]
|
||||
if math.Abs(last.Altitude) > 5 {
|
||||
t.Errorf("final altitude = %v, want within 5m of 0", last.Altitude)
|
||||
}
|
||||
}
|
||||
|
||||
func TestReverseDirection(t *testing.T) {
|
||||
// Start at altitude 100m with downward rate; integrating reverse should
|
||||
// give increasing altitude.
|
||||
desc := &Propagator{
|
||||
Name: "rewind",
|
||||
Step: 1,
|
||||
Model: ConstantRate(-1), // forward: alt decreases at 1 m/s
|
||||
Constraints: []Constraint{MaxAltitude{Limit: 200, On: ActionStop}},
|
||||
}
|
||||
prof := Profile{Stages: []*Propagator{desc}, Direction: Reverse}
|
||||
results := prof.Run(0, State{Altitude: 100})
|
||||
|
||||
last := results[0].Points[len(results[0].Points)-1]
|
||||
if math.Abs(last.Altitude-200) > 1 {
|
||||
t.Errorf("reverse final altitude = %v, want ~200", last.Altitude)
|
||||
}
|
||||
if last.Time >= 0 {
|
||||
t.Errorf("reverse final time = %v, want < 0", last.Time)
|
||||
}
|
||||
}
|
||||
|
||||
func TestPiecewiseRate(t *testing.T) {
|
||||
m := Piecewise([]RateSegment{
|
||||
{Until: 100, Rate: 5},
|
||||
{Until: 200, Rate: 3},
|
||||
{Until: math.Inf(1), Rate: 0},
|
||||
})
|
||||
|
||||
if r := m(50, State{}); r.Altitude != 5 {
|
||||
t.Errorf("rate at t=50 = %v, want 5", r.Altitude)
|
||||
}
|
||||
if r := m(150, State{}); r.Altitude != 3 {
|
||||
t.Errorf("rate at t=150 = %v, want 3", r.Altitude)
|
||||
}
|
||||
if r := m(300, State{}); r.Altitude != 0 {
|
||||
t.Errorf("rate at t=300 = %v, want 0", r.Altitude)
|
||||
}
|
||||
}
|
||||
|
||||
// fixedWind returns a constant wind sample.
|
||||
type fixedWind struct{ u, v float64 }
|
||||
|
||||
func (w fixedWind) Wind(_ float64, _, _, _ float64) (weather.Sample, error) {
|
||||
return weather.Sample{U: w.u, V: w.v}, nil
|
||||
}
|
||||
func (fixedWind) Epoch() time.Time { return time.Unix(0, 0) }
|
||||
func (fixedWind) Source() string { return "test-fixed" }
|
||||
|
||||
func TestWindTransportUnitConversion(t *testing.T) {
|
||||
// Pure eastward wind of 10 m/s at the equator at sea level.
|
||||
// Expected dlng/dt = (180/pi) * 10 / (6371009 * cos(0)) ≈ 0.00008991 deg/s.
|
||||
// Expected dlat/dt = 0.
|
||||
wind := WindTransport(fixedWind{u: 10, v: 0}, nil)
|
||||
d := wind(0, State{Lat: 0, Lng: 0, Altitude: 0})
|
||||
|
||||
wantLng := (180.0 / math.Pi) * 10.0 / 6371009.0
|
||||
if math.Abs(d.Lng-wantLng) > 1e-12 {
|
||||
t.Errorf("dlng = %v, want %v", d.Lng, wantLng)
|
||||
}
|
||||
if math.Abs(d.Lat) > 1e-12 {
|
||||
t.Errorf("dlat = %v, want 0 for u=10 v=0", d.Lat)
|
||||
}
|
||||
|
||||
// Pure northward at 60° latitude: dlat = (180/pi) * v / R, dlng = 0.
|
||||
wind2 := WindTransport(fixedWind{u: 0, v: 5}, nil)
|
||||
d = wind2(0, State{Lat: 60, Lng: 0, Altitude: 0})
|
||||
wantLat := (180.0 / math.Pi) * 5.0 / 6371009.0
|
||||
if math.Abs(d.Lat-wantLat) > 1e-12 {
|
||||
t.Errorf("dlat at lat=60 = %v, want %v", d.Lat, wantLat)
|
||||
}
|
||||
}
|
||||
|
||||
func TestStateAddWrapsLongitude(t *testing.T) {
|
||||
// Demonstrates state algebra used by the integrator and refinement.
|
||||
s := stateAdd(State{Lat: 0, Lng: 350, Altitude: 0}, 1, State{Lng: 20})
|
||||
if math.Abs(s.Lng-10) > 1e-9 {
|
||||
t.Errorf("addState wrap: lng = %v, want 10", s.Lng)
|
||||
}
|
||||
|
||||
mid := stateLerp(State{Lng: 350}, State{Lng: 10}, 0.5)
|
||||
if math.Abs(mid.Lng-0) > 1e-9 && math.Abs(mid.Lng-360) > 1e-9 {
|
||||
t.Errorf("lerpState lng wrap: %v, want 0 or 360", mid.Lng)
|
||||
}
|
||||
}
|
||||
151
internal/engine/models.go
Normal file
151
internal/engine/models.go
Normal file
|
|
@ -0,0 +1,151 @@
|
|||
package engine
|
||||
|
||||
import (
|
||||
"math"
|
||||
"sort"
|
||||
"sync/atomic"
|
||||
|
||||
"predictor-refactored/internal/weather"
|
||||
)
|
||||
|
||||
// Sum composes models by summing their derivatives at each evaluation point.
|
||||
//
|
||||
// Useful for combining e.g. a vertical-rate model with a horizontal wind model
|
||||
// into a single propagator. Equivalent to Tawhiri's LinearModel.
|
||||
func Sum(models ...Model) Model {
|
||||
if len(models) == 1 {
|
||||
return models[0]
|
||||
}
|
||||
return func(t float64, s State) State {
|
||||
var sum State
|
||||
for _, m := range models {
|
||||
d := m(t, s)
|
||||
sum.Lat += d.Lat
|
||||
sum.Lng += d.Lng
|
||||
sum.Altitude += d.Altitude
|
||||
}
|
||||
return sum
|
||||
}
|
||||
}
|
||||
|
||||
// ConstantRate returns a model with a constant vertical velocity (m/s).
|
||||
// A positive rate is upward (ascent); a negative rate is downward.
|
||||
func ConstantRate(rate float64) Model {
|
||||
return func(_ float64, _ State) State {
|
||||
return State{Altitude: rate}
|
||||
}
|
||||
}
|
||||
|
||||
// ParachuteDescent returns a model where vertical velocity grows with altitude
|
||||
// because thinner air provides less drag.
|
||||
//
|
||||
// seaLevelRate is the descent speed at sea level (m/s, positive number).
|
||||
// The terminal velocity at altitude is computed as
|
||||
//
|
||||
// v = -k / sqrt(rho(alt)), k = seaLevelRate * 1.1045,
|
||||
//
|
||||
// using the NASA atmosphere model for rho. Equivalent to Tawhiri's drag_descent.
|
||||
func ParachuteDescent(seaLevelRate float64) Model {
|
||||
k := seaLevelRate * 1.1045
|
||||
return func(_ float64, s State) State {
|
||||
return State{Altitude: -k / math.Sqrt(nasaDensity(s.Altitude))}
|
||||
}
|
||||
}
|
||||
|
||||
// nasaDensity returns air density (kg/m^3) for the given altitude in metres,
|
||||
// using the NASA simple atmosphere model. See
|
||||
// https://www.grc.nasa.gov/WWW/K-12/airplane/atmosmet.html.
|
||||
func nasaDensity(alt float64) float64 {
|
||||
var temp, pressure float64
|
||||
switch {
|
||||
case alt > 25000:
|
||||
temp = -131.21 + 0.00299*alt
|
||||
pressure = 2.488 * math.Pow((temp+273.1)/216.6, -11.388)
|
||||
case alt > 11000:
|
||||
temp = -56.46
|
||||
pressure = 22.65 * math.Exp(1.73-0.000157*alt)
|
||||
default:
|
||||
temp = 15.04 - 0.00649*alt
|
||||
pressure = 101.29 * math.Pow((temp+273.1)/288.08, 5.256)
|
||||
}
|
||||
return pressure / (0.2869 * (temp + 273.1))
|
||||
}
|
||||
|
||||
// RateSegment is one entry in a Piecewise rate schedule.
|
||||
type RateSegment struct {
|
||||
// Until is the UNIX timestamp at which this segment ends.
|
||||
// The model applies the segment's Rate for all t < Until.
|
||||
Until float64
|
||||
// Rate is the vertical velocity (m/s) during the segment. Positive is up.
|
||||
Rate float64
|
||||
}
|
||||
|
||||
// Piecewise returns a model that produces a piecewise-constant vertical rate
|
||||
// over a sequence of time intervals.
|
||||
//
|
||||
// Segments are searched by their Until field; the first segment whose Until
|
||||
// exceeds t supplies the active rate. For t at or after the last Until, the
|
||||
// final segment's Rate is held indefinitely. Input is sorted ascending by
|
||||
// Until on construction.
|
||||
func Piecewise(segments []RateSegment) Model {
|
||||
if len(segments) == 0 {
|
||||
return ConstantRate(0)
|
||||
}
|
||||
sorted := append([]RateSegment(nil), segments...)
|
||||
sort.Slice(sorted, func(i, j int) bool { return sorted[i].Until < sorted[j].Until })
|
||||
finalRate := sorted[len(sorted)-1].Rate
|
||||
|
||||
return func(t float64, _ State) State {
|
||||
idx := sort.Search(len(sorted), func(i int) bool { return sorted[i].Until > t })
|
||||
if idx == len(sorted) {
|
||||
return State{Altitude: finalRate}
|
||||
}
|
||||
return State{Altitude: sorted[idx].Rate}
|
||||
}
|
||||
}
|
||||
|
||||
// Warnings aggregates non-fatal conditions encountered during integration.
|
||||
type Warnings struct {
|
||||
// AltitudeTooHigh counts evaluations where the wind sampler reported
|
||||
// that altitude was above the highest pressure level of the dataset.
|
||||
AltitudeTooHigh atomic.Int64
|
||||
}
|
||||
|
||||
// ToMap returns warnings as a map suitable for JSON output. Only counters
|
||||
// that have fired are included.
|
||||
func (w *Warnings) ToMap() map[string]any {
|
||||
out := make(map[string]any)
|
||||
if n := w.AltitudeTooHigh.Load(); n > 0 {
|
||||
out["altitude_too_high"] = map[string]any{
|
||||
"count": n,
|
||||
"description": "altitude exceeded the highest pressure level of the wind dataset; samples were extrapolated",
|
||||
}
|
||||
}
|
||||
return out
|
||||
}
|
||||
|
||||
// WindTransport returns a model that moves laterally at the wind velocity
|
||||
// sampled from field. The vertical component of the returned derivative is
|
||||
// zero. Wind units are converted from m/s to deg/s on Earth's surface.
|
||||
//
|
||||
// If warnings is non-nil, the AltitudeTooHigh counter is incremented for any
|
||||
// sample where the wind field reported altitude above the model top.
|
||||
func WindTransport(field weather.WindField, warnings *Warnings) Model {
|
||||
const earthR = 6371009.0
|
||||
const piOver180 = math.Pi / 180.0
|
||||
const degPerRad = 180.0 / math.Pi
|
||||
return func(t float64, s State) State {
|
||||
sample, err := field.Wind(t, s.Lat, s.Lng, s.Altitude)
|
||||
if err != nil {
|
||||
return State{}
|
||||
}
|
||||
if sample.AboveModel && warnings != nil {
|
||||
warnings.AltitudeTooHigh.Add(1)
|
||||
}
|
||||
r := earthR + s.Altitude
|
||||
return State{
|
||||
Lat: degPerRad * sample.V / r,
|
||||
Lng: degPerRad * sample.U / (r * math.Cos(s.Lat*piOver180)),
|
||||
}
|
||||
}
|
||||
}
|
||||
55
internal/engine/profile.go
Normal file
55
internal/engine/profile.go
Normal file
|
|
@ -0,0 +1,55 @@
|
|||
package engine
|
||||
|
||||
// Profile is an ordered chain of propagators executed sequentially. Each
|
||||
// propagator picks up where the previous one finished.
|
||||
type Profile struct {
|
||||
// Stages are run in order. For Direction=Reverse they are still iterated
|
||||
// from index 0 onwards, but each propagator integrates with negative dt.
|
||||
Stages []*Propagator
|
||||
|
||||
// Direction controls the sign of dt across the whole profile.
|
||||
Direction Direction
|
||||
|
||||
// Globals are constraints evaluated alongside each stage's local Constraints.
|
||||
// Useful for profile-wide bounds like "stop after N hours total".
|
||||
Globals []Constraint
|
||||
}
|
||||
|
||||
// Run executes the profile from the given launch point. Returns one Result
|
||||
// per executed stage, including any Fallback chains that were activated.
|
||||
func (p *Profile) Run(t0 float64, launch State) []Result {
|
||||
if p.Direction == 0 {
|
||||
p.Direction = Forward
|
||||
}
|
||||
|
||||
results := make([]Result, 0, len(p.Stages))
|
||||
t, s := t0, launch
|
||||
|
||||
for i := 0; i < len(p.Stages); i++ {
|
||||
stage := p.Stages[i]
|
||||
res := stage.run(t, s, p.Direction, p.Globals)
|
||||
results = append(results, res)
|
||||
|
||||
last := res.Points[len(res.Points)-1]
|
||||
t = last.Time
|
||||
s = State{Lat: last.Lat, Lng: last.Lng, Altitude: last.Altitude}
|
||||
|
||||
// Follow Fallback chains until none remains. Each fallback consumes
|
||||
// from the same point the previous stage stopped at.
|
||||
for res.Outcome == OutcomeFallback && stage.Fallback != nil {
|
||||
stage = stage.Fallback
|
||||
res = stage.run(t, s, p.Direction, p.Globals)
|
||||
results = append(results, res)
|
||||
last = res.Points[len(res.Points)-1]
|
||||
t = last.Time
|
||||
s = State{Lat: last.Lat, Lng: last.Lng, Altitude: last.Altitude}
|
||||
}
|
||||
|
||||
// If a propagator's stop fired (not a fallback), end the profile.
|
||||
if res.Outcome == OutcomeStopped {
|
||||
continue
|
||||
}
|
||||
}
|
||||
|
||||
return results
|
||||
}
|
||||
156
internal/engine/propagator.go
Normal file
156
internal/engine/propagator.go
Normal file
|
|
@ -0,0 +1,156 @@
|
|||
package engine
|
||||
|
||||
import (
|
||||
"predictor-refactored/internal/numerics"
|
||||
)
|
||||
|
||||
// Propagator advances state under one Model, checking a set of Constraints
|
||||
// after every integration step.
|
||||
//
|
||||
// When a constraint fires, the propagator binary-search refines the violation
|
||||
// point and emits it as its final trajectory point. The Action of the
|
||||
// triggering constraint controls what the surrounding Profile does next:
|
||||
// stop the profile, transfer to Fallback, or clip and continue.
|
||||
type Propagator struct {
|
||||
// Name identifies the propagator in trajectory metadata.
|
||||
Name string
|
||||
|
||||
// Step is the magnitude of the integration step in seconds (always positive).
|
||||
// The Profile flips its sign for Reverse direction.
|
||||
Step float64
|
||||
|
||||
// Model produces the per-second time derivative of state.
|
||||
Model Model
|
||||
|
||||
// Constraints are evaluated after each step. Any fired constraint stops
|
||||
// the propagator at the refined point; the first one in this slice wins
|
||||
// on ties.
|
||||
Constraints []Constraint
|
||||
|
||||
// Fallback is the propagator to switch to when a constraint with
|
||||
// ActionFallback fires. Optional.
|
||||
Fallback *Propagator
|
||||
|
||||
// Tolerance is the binary-search refinement tolerance in parameter space
|
||||
// (default 0.01, matching Tawhiri).
|
||||
Tolerance float64
|
||||
}
|
||||
|
||||
// Outcome describes how a propagator's run ended.
|
||||
type Outcome int
|
||||
|
||||
const (
|
||||
// OutcomeStopped means a Constraint with ActionStop fired and the profile
|
||||
// should end here.
|
||||
OutcomeStopped Outcome = iota
|
||||
// OutcomeFallback means a Constraint with ActionFallback fired and the
|
||||
// profile should transfer to the propagator's Fallback chain.
|
||||
OutcomeFallback
|
||||
// OutcomeContinued means no constraint fired before the time horizon was
|
||||
// reached. In practice this is only seen when a propagator runs unbounded,
|
||||
// which means the profile is misconfigured.
|
||||
OutcomeContinued
|
||||
)
|
||||
|
||||
// Result is the output of running one propagator.
|
||||
type Result struct {
|
||||
Propagator string
|
||||
Points []TrajectoryPoint
|
||||
Outcome Outcome
|
||||
// Constraint is the constraint that fired, or nil if Outcome == OutcomeContinued.
|
||||
Constraint Constraint
|
||||
}
|
||||
|
||||
// run integrates the model from (t0, s0) in direction dir, returning a Result.
|
||||
// globals are constraints injected by the Profile and checked alongside the
|
||||
// propagator's local Constraints.
|
||||
func (p *Propagator) run(t0 float64, s0 State, dir Direction, globals []Constraint) Result {
|
||||
dt := p.Step * float64(dir)
|
||||
tol := p.Tolerance
|
||||
if tol == 0 {
|
||||
tol = 0.01
|
||||
}
|
||||
|
||||
deriv := numerics.Deriv[State](func(t float64, s State) State { return p.Model(t, s) })
|
||||
add := numerics.VecAdd[State](stateAdd)
|
||||
lerp := numerics.VecLerp[State](stateLerp)
|
||||
|
||||
out := Result{
|
||||
Propagator: p.Name,
|
||||
Outcome: OutcomeContinued,
|
||||
Points: []TrajectoryPoint{{
|
||||
Time: t0, Lat: s0.Lat, Lng: s0.Lng, Altitude: s0.Altitude,
|
||||
}},
|
||||
}
|
||||
|
||||
t := t0
|
||||
s := s0
|
||||
|
||||
for {
|
||||
s2 := numerics.RK4Step(t, s, dt, deriv, add)
|
||||
t2 := t + dt
|
||||
|
||||
if c, fired := firstFiring(p.Constraints, globals, t2, s2); fired {
|
||||
trig := numerics.Trigger[State](func(tt float64, ss State) bool { return c.Violated(tt, ss) })
|
||||
t3, s3 := numerics.RefineTrigger(t, s, t2, s2, trig, lerp, tol)
|
||||
|
||||
switch c.Action() {
|
||||
case ActionClip:
|
||||
s3 = clipToConstraint(c, s3)
|
||||
out.Points = append(out.Points, TrajectoryPoint{
|
||||
Time: t3, Lat: s3.Lat, Lng: s3.Lng, Altitude: s3.Altitude,
|
||||
})
|
||||
t, s = t3, s3
|
||||
continue
|
||||
case ActionFallback:
|
||||
out.Points = append(out.Points, TrajectoryPoint{
|
||||
Time: t3, Lat: s3.Lat, Lng: s3.Lng, Altitude: s3.Altitude,
|
||||
})
|
||||
out.Outcome = OutcomeFallback
|
||||
out.Constraint = c
|
||||
return out
|
||||
default: // ActionStop
|
||||
out.Points = append(out.Points, TrajectoryPoint{
|
||||
Time: t3, Lat: s3.Lat, Lng: s3.Lng, Altitude: s3.Altitude,
|
||||
})
|
||||
out.Outcome = OutcomeStopped
|
||||
out.Constraint = c
|
||||
return out
|
||||
}
|
||||
}
|
||||
|
||||
t, s = t2, s2
|
||||
out.Points = append(out.Points, TrajectoryPoint{
|
||||
Time: t, Lat: s.Lat, Lng: s.Lng, Altitude: s.Altitude,
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
// firstFiring scans local then global constraints for the first one whose
|
||||
// Violated returns true at (t, s).
|
||||
func firstFiring(local, globals []Constraint, t float64, s State) (Constraint, bool) {
|
||||
for _, c := range local {
|
||||
if c.Violated(t, s) {
|
||||
return c, true
|
||||
}
|
||||
}
|
||||
for _, c := range globals {
|
||||
if c.Violated(t, s) {
|
||||
return c, true
|
||||
}
|
||||
}
|
||||
return nil, false
|
||||
}
|
||||
|
||||
// clipToConstraint adjusts s so that the given constraint is exactly satisfied
|
||||
// (not violated). Implemented for constraints with a well-defined boundary;
|
||||
// others fall through unchanged.
|
||||
func clipToConstraint(c Constraint, s State) State {
|
||||
switch v := c.(type) {
|
||||
case MaxAltitude:
|
||||
s.Altitude = v.Limit
|
||||
case MinAltitude:
|
||||
s.Altitude = v.Limit
|
||||
}
|
||||
return s
|
||||
}
|
||||
50
internal/engine/state.go
Normal file
50
internal/engine/state.go
Normal file
|
|
@ -0,0 +1,50 @@
|
|||
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)
|
||||
}
|
||||
80
internal/engine/types.go
Normal file
80
internal/engine/types.go
Normal file
|
|
@ -0,0 +1,80 @@
|
|||
// Package engine is the trajectory calculation engine. It composes
|
||||
// propagators (model-driven integrators) into profiles (ordered chains) and
|
||||
// runs them over a wind field.
|
||||
//
|
||||
// The engine has no direct dependency on any specific data source: wind data
|
||||
// is consumed through weather.WindField and terrain data through any type
|
||||
// satisfying TerrainProvider.
|
||||
package engine
|
||||
|
||||
// State holds the spatial state of the balloon. When returned by a Model
|
||||
// the same struct is interpreted as the per-second time derivative of state.
|
||||
type State struct {
|
||||
// Lat is degrees latitude in [-90, 90] (or deg/s when returned as a derivative).
|
||||
Lat float64
|
||||
// Lng is degrees longitude in [0, 360) (or deg/s as a derivative).
|
||||
Lng float64
|
||||
// Altitude is metres above mean sea level (or m/s as a derivative).
|
||||
Altitude float64
|
||||
}
|
||||
|
||||
// Model returns the time derivative of state at (t, s).
|
||||
//
|
||||
// The derivative is direction-independent; the integrator applies the sign
|
||||
// of dt for reverse propagation.
|
||||
type Model func(t float64, s State) State
|
||||
|
||||
// TrajectoryPoint is one sampled point of an integration result.
|
||||
type TrajectoryPoint struct {
|
||||
Time float64 // UNIX seconds
|
||||
Lat float64
|
||||
Lng float64
|
||||
Altitude float64
|
||||
}
|
||||
|
||||
// Direction is the time direction of integration. Forward (+1) integrates
|
||||
// from launch to landing; Reverse (-1) integrates from a known landing back
|
||||
// to a candidate launch point.
|
||||
type Direction int8
|
||||
|
||||
const (
|
||||
Forward Direction = +1
|
||||
Reverse Direction = -1
|
||||
)
|
||||
|
||||
// Action describes what the profile runner should do when a Constraint
|
||||
// reports a violation.
|
||||
type Action int
|
||||
|
||||
const (
|
||||
// ActionStop ends the current propagator at the (refined) violation point.
|
||||
// This matches the only behaviour available in the reference Tawhiri solver.
|
||||
ActionStop Action = iota
|
||||
// ActionFallback ends the current propagator and starts its Fallback
|
||||
// propagator from the violation point. Useful for "if max altitude is
|
||||
// reached during ascent, switch to descent" profiles.
|
||||
ActionFallback
|
||||
// ActionClip clips the violated coordinate to the boundary and continues
|
||||
// integration. Useful for soft constraints such as "max altitude floor".
|
||||
ActionClip
|
||||
)
|
||||
|
||||
// Constraint reports when integration should stop, branch, or clip.
|
||||
//
|
||||
// A constraint is direction-agnostic: it reads state and decides. The profile
|
||||
// runner is responsible for refining the trigger point via binary search and
|
||||
// dispatching the configured Action.
|
||||
type Constraint interface {
|
||||
// Name identifies the constraint in logs and result metadata.
|
||||
Name() string
|
||||
// Violated reports whether the constraint is breached at (t, s).
|
||||
Violated(t float64, s State) bool
|
||||
// Action is the behaviour to take on violation.
|
||||
Action() Action
|
||||
}
|
||||
|
||||
// TerrainProvider returns ground elevation in metres at a coordinate.
|
||||
// Implementations must be safe for concurrent use.
|
||||
type TerrainProvider interface {
|
||||
Elevation(lat, lng float64) float64
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue