diff --git a/internal/sweatbox/airport.go b/internal/sweatbox/airport.go index 8b1fb00..1572001 100644 --- a/internal/sweatbox/airport.go +++ b/internal/sweatbox/airport.go @@ -126,10 +126,14 @@ func (g *Graph) Intersect(nameA, nameB string) bool { return ok } -// FindIntersection finds the first pair of waypoints (scan order of A then B) -// within tol and returns the point on surface A (TWRTrainer FindNextWaypoint -// semantics), plus the point indices on A and B. ok is false if either surface -// is missing or no pair is within tolerance. +// FindIntersection finds waypoints of A and B within tol and returns the +// closest pair (ties broken by scan order of A then B). The returned point is +// on surface A. ok is false if either surface is missing or no pair is within +// tolerance. +// +// Closest-within-tol (rather than first-within-tol) keeps junctions correct on +// dense polylines where several vertices of A lie within snap of a shared +// vertex on B. // // When nameA and nameB refer to the same surface, returns the first waypoint. func (g *Graph) FindIntersection(nameA, nameB string) (pt Point, idxA, idxB int, ok bool) { @@ -149,18 +153,24 @@ func (g *Graph) FindIntersection(nameA, nameB string) (pt Point, idxA, idxB int, if ia == ib { return sa.Points[0], 0, 0, true } - tol := g.tolM - tolSq := tol * tol + tolSq := g.tolM * g.tolM + bestD := tolSq + 1 + bestA, bestB := -1, -1 for a := range sa.Points { pa := sa.Points[a] for b := range sb.Points { pb := sb.Points[b] - if geo.DistanceSq(pa.Lat, pa.Lon, pb.Lat, pb.Lon) <= tolSq { - return pa, a, b, true + d := geo.DistanceSq(pa.Lat, pa.Lon, pb.Lat, pb.Lon) + if d <= tolSq && d < bestD { + bestD = d + bestA, bestB = a, b } } } - return Point{}, -1, -1, false + if bestA < 0 { + return Point{}, -1, -1, false + } + return sa.Points[bestA], bestA, bestB, true } // Neighbors returns names of surfaces that intersect name (excluding itself), diff --git a/internal/sweatbox/taxi.go b/internal/sweatbox/taxi.go index b2b94e6..0c956d3 100644 --- a/internal/sweatbox/taxi.go +++ b/internal/sweatbox/taxi.go @@ -7,6 +7,10 @@ import ( "github.com/renorris/openfsd/internal/geo" ) +// pathStitchEpsM is the only distance used when collapsing adjacent path samples +// at leg joints. Intersection snap (~100 ft) must not drop real polyline vertices. +const pathStitchEpsM = 1.0 + // TaxiPlan is a validated taxi route: ordered surface steps, hold-shorts, an // optional final parking, and an expanded waypoint polyline for the engine. // @@ -23,6 +27,7 @@ type TaxiPlan struct { // Waypoints is the ordered ground path (lat/lon) along surface polylines. Waypoints []Point // HoldAt lists hold-short positions resolved along the route. + // Every entry has WaypointIndex >= 0 (off-route holds are rejected). HoldAt []TaxiHold } @@ -32,8 +37,7 @@ type TaxiHold struct { Name string // Point is where the aircraft should stop. Point Point - // WaypointIndex is the index into TaxiPlan.Waypoints of Point (−1 if the - // hold could not be placed on the expanded path but validation passed). + // WaypointIndex is the index into TaxiPlan.Waypoints of Point. WaypointIndex int } @@ -120,7 +124,7 @@ func (g *Graph) PlanTaxi(current string, steps, holds []string) (TaxiPlan, strin } } - // Normalize / validate holds. + // Normalize / validate holds (existence + kind only; on-route check after path). normHolds := make([]string, 0, len(holds)) for _, raw := range holds { tok := strings.TrimSpace(raw) @@ -137,7 +141,6 @@ func (g *Graph) PlanTaxi(current string, steps, holds []string) (TaxiPlan, strin } if s.Kind == SurfaceRunway { // Keep the designator the instructor typed when it is an end name. - // Prefer the token if it matches an end; else combined name. up := strings.ToUpper(tok) if up == s.RwyA || up == s.RwyB { name = up @@ -166,64 +169,44 @@ func (g *Graph) PlanTaxi(current string, steps, holds []string) (TaxiPlan, strin // First step must intersect current taxiway/runway (when current is set). if curName != "" { - first := firstRouteSurface(normSteps, parking, g) - if first == "" { - return zero, "First step of taxi route must intersect with current taxiway/runway." - } - if !g.Intersect(curName, first) && !sameSurface(g, curName, first) { + first := firstRouteSurface(normSteps, parking) + if first == "" || (!g.Intersect(curName, first) && !sameSurface(g, curName, first)) { return zero, "First step of taxi route must intersect with current taxiway/runway." } } // Consecutive surface steps must intersect. - routeSurfaces := make([]string, 0, len(normSteps)+1) - if curName != "" && (len(normSteps) == 0 || !sameSurface(g, curName, normSteps[0])) { - // Include current only for intersection chaining into first step; - // not as a plan step. - } - routeSurfaces = append(routeSurfaces, normSteps...) - for i := 1; i < len(routeSurfaces); i++ { - a, b := routeSurfaces[i-1], routeSurfaces[i] + for i := 1; i < len(normSteps); i++ { + a, b := normSteps[i-1], normSteps[i] if !g.Intersect(a, b) { return zero, fmt.Sprintf("%s and %s do not intersect.", displayName(a), displayName(b)) } } - // When current is set and differs from first step, current must intersect first - // (already checked). Parking-only from an intersecting surface is ok. - if len(normSteps) >= 1 && curName != "" && !sameSurface(g, curName, normSteps[0]) { - if !g.Intersect(curName, normSteps[0]) { - return zero, "First step of taxi route must intersect with current taxiway/runway." - } - } - - // Parking exit: last surface (or current) must be able to leave toward parking. - // We do not require a 100 ft snap between last surface and parking; TWRTrainer - // taxis to the closest waypoint on the last surface then direct to parking. + // Parking-only: need a current surface to leave from. if parking != "" && len(normSteps) == 0 { - // Taxi direct from current surface to parking — current required. - if curName == "" { - return zero, "First step of taxi route must intersect with current taxiway/runway." - } - if g.Surface(curName) == nil { + if curName == "" || g.Surface(curName) == nil { return zero, "First step of taxi route must intersect with current taxiway/runway." } } - plan := TaxiPlan{ - Steps: append([]string(nil), normSteps...), - Holds: append([]string(nil), normHolds...), - Parking: parking, + wps := g.buildTaxiPath(curName, normSteps, parking) + wps, holdAt, errMsg := g.resolveHolds(normSteps, curName, normHolds, wps) + if errMsg != "" { + return zero, errMsg } - wps, holdAt := g.buildTaxiPath(curName, normSteps, parking, normHolds) - plan.Waypoints = wps - plan.HoldAt = holdAt - return plan, "" + return TaxiPlan{ + Steps: append([]string(nil), normSteps...), + Holds: append([]string(nil), normHolds...), + Parking: parking, + Waypoints: wps, + HoldAt: holdAt, + }, "" } // firstRouteSurface is the first taxiway/runway on the route, or parking name. -func firstRouteSurface(normSteps []string, parking string, g *Graph) string { +func firstRouteSurface(normSteps []string, parking string) string { if len(normSteps) > 0 { return normSteps[0] } @@ -243,15 +226,13 @@ func displayName(name string) string { return strings.ToUpper(name) } -// buildTaxiPath expands surface polylines into a waypoint list and places holds. -func (g *Graph) buildTaxiPath(current string, steps []string, parking string, holds []string) ([]Point, []TaxiHold) { +// buildTaxiPath expands surface polylines into a waypoint list. +func (g *Graph) buildTaxiPath(current string, steps []string, parking string) []Point { var wps []Point if len(steps) == 0 && parking == "" { - return nil, nil + return nil } - // Chain of surfaces we walk (excluding parking). - // When current differs from first step, entry is their intersection. type leg struct { surface string from Point @@ -262,21 +243,21 @@ func (g *Graph) buildTaxiPath(current string, steps []string, parking string, ho var legs []leg - // Determine starting surface and entry point. - startSurface := "" + // Entry onto first step from a different current surface (point on first step). var entry Point entrySet := false if len(steps) > 0 { - startSurface = steps[0] - if current != "" && !sameSurface(g, current, startSurface) { - if p, _, _, ok := g.FindIntersection(current, startSurface); ok { - entry = p + if current != "" && !sameSurface(g, current, steps[0]) { + if p, _, idxB, ok := g.FindIntersection(current, steps[0]); ok { + sb := g.Surface(steps[0]) + if sb != nil && idxB >= 0 && idxB < len(sb.Points) { + entry = sb.Points[idxB] + } else { + entry = p + } entrySet = true } } - } else { - // Parking-only: walk on current toward parking. - startSurface = current } // Build legs between consecutive steps. @@ -291,7 +272,6 @@ func (g *Graph) buildTaxiPath(current string, steps []string, parking string, ho } else { // From previous intersection (point on this surface). if p, _, idxB, ok := g.FindIntersection(steps[i-1], sName); ok { - // Prefer the point on this surface (B). sb := g.Surface(sName) if sb != nil && idxB >= 0 && idxB < len(sb.Points) { lg.from = sb.Points[idxB] @@ -321,11 +301,8 @@ func (g *Graph) buildTaxiPath(current string, steps []string, parking string, ho } } } else { - // Destination is this surface (typically a runway): to = entry onto it - // from previous, already as from; keep to unset so we still include from. - // For a multi-step route ending on a runway, the end point is the - // intersection of the previous surface and this runway — already "from". - // Single-step runway from another current: "from" is entry; path is [entry]. + // Destination is this surface (typically a runway). Path ends at entry + // from the previous surface (or from current when single-step). if !lg.fromSet && current != "" && !sameSurface(g, current, sName) { if p, _, idxB, ok := g.FindIntersection(current, sName); ok { sb := g.Surface(sName) @@ -341,7 +318,7 @@ func (g *Graph) buildTaxiPath(current string, steps []string, parking string, ho legs = append(legs, lg) } - // Parking-only leg: from closest on current to parking point. + // Parking-only leg: leave current at closest waypoint toward parking. if len(steps) == 0 && parking != "" && current != "" { ps := g.Surface(parking) lg := leg{surface: current} @@ -356,7 +333,7 @@ func (g *Graph) buildTaxiPath(current string, steps []string, parking string, ho legs = append(legs, lg) } - // Expand legs into waypoints. + // Expand legs into waypoints (stitch with tiny epsilon only). for _, lg := range legs { s := g.Surface(lg.surface) if s == nil || len(s.Points) == 0 { @@ -364,16 +341,16 @@ func (g *Graph) buildTaxiPath(current string, steps []string, parking string, ho } switch { case lg.fromSet && lg.toSet: - seg := walkPolyline(s, lg.from, lg.to, g.tolM) - wps = appendUniquePoints(wps, seg, g.tolM) + seg := walkPolyline(s, lg.from, lg.to) + wps = appendUniquePoints(wps, seg, pathStitchEpsM) case lg.fromSet: - wps = appendUniquePoints(wps, []Point{lg.from}, g.tolM) + wps = appendUniquePoints(wps, []Point{lg.from}, pathStitchEpsM) case lg.toSet: - wps = appendUniquePoints(wps, []Point{lg.to}, g.tolM) + wps = appendUniquePoints(wps, []Point{lg.to}, pathStitchEpsM) } } - // Append parking coordinate (always, even if within snap of last taxiway point). + // Append parking coordinate (always, even if near last taxiway point). if parking != "" { if ps := g.Surface(parking); ps != nil && len(ps.Points) > 0 { p := ps.Points[0] @@ -383,75 +360,208 @@ func (g *Graph) buildTaxiPath(current string, steps []string, parking string, ho } } - // Resolve hold-short positions against the route surfaces. - holdAt := g.resolveHolds(steps, current, holds, wps) - return wps, holdAt + return wps } -// resolveHolds places each hold at the intersection of the hold target with a -// route surface (first match in route order). Destination runway is not auto- -// added here — the engine holds short of the final runway by status. -func (g *Graph) resolveHolds(steps []string, current string, holds []string, wps []Point) []TaxiHold { +// resolveHolds places each hold on the route path. Holds must map onto Waypoints +// (within intersection snap). Off-route holds are rejected with an instructor string. +// May insert HOLD vertices into wps when they lie on a route surface but were +// outside the expanded turn-to-turn segment (e.g. already on first surface). +// +// Placement prefers the latest path position that intersects the hold target: +// for each route step (reverse order), the step∩hold point is a candidate; when +// the hold is the step itself (e.g. destination runway), the entry from the +// previous surface (or current) is used. HOLD surfaces use their single point. +func (g *Graph) resolveHolds(steps []string, current string, holds []string, wps []Point) ([]Point, []TaxiHold, string) { if len(holds) == 0 { - return nil + return wps, nil, "" } - // Surfaces along the taxi (current + steps) for intersection search. - chain := make([]string, 0, len(steps)+1) - if current != "" { - chain = append(chain, current) - } - chain = append(chain, steps...) out := make([]TaxiHold, 0, len(holds)) for _, h := range holds { hs := g.Surface(h) - var hp Point - found := false + var bestPoint Point + bestIdx := -1 + if hs != nil && hs.Kind == SurfaceHold && len(hs.Points) == 1 { - hp = hs.Points[0] - found = true - } else { - // Intersection of hold target with any route surface. - for _, sName := range chain { - if sameSurface(g, sName, h) { - // Holding short of a surface we're taxiing on: use first - // intersection of that surface with a neighboring chain member. - continue - } - if p, _, _, ok := g.FindIntersection(sName, h); ok { - hp = p - found = true - break - } + hp := hs.Points[0] + // HOLD is on-route if it snaps to any route-step (or current) surface vertex. + if !g.holdNearRouteSurface(hp, steps, current) { + return wps, nil, fmt.Sprintf("%s is not on the taxi route.", displayName(h)) } - // If hold is the destination runway itself, use entry onto it. - if !found && len(steps) > 0 && sameSurface(g, steps[len(steps)-1], h) { - if len(steps) >= 2 { - if p, _, _, ok := g.FindIntersection(steps[len(steps)-2], steps[len(steps)-1]); ok { - hp = p - found = true - } - } else if current != "" { - if p, _, _, ok := g.FindIntersection(current, steps[0]); ok { - hp = p - found = true - } + idx := findWaypointIndex(wps, hp, g.tolM) + if idx < 0 { + // Inject so the engine sees the hold on the path (common when + // already on the first surface: expansion starts at the next turn). + wps, idx = insertWaypoint(wps, hp, pathStitchEpsM) + } + out = append(out, TaxiHold{Name: h, Point: wps[idx], WaypointIndex: idx}) + continue + } + + // Collect candidate intersections with route steps (reverse: prefer last crossing). + for i := len(steps) - 1; i >= 0; i-- { + sName := steps[i] + var cand Point + var ok bool + if sameSurface(g, sName, h) { + // Holding short of a surface that is itself a step (e.g. dest runway): + // stop at the entry onto that surface. + if i > 0 { + cand, ok = intersectionOnSurface(g, steps[i-1], sName) + } else if current != "" && !sameSurface(g, current, sName) { + cand, ok = intersectionOnSurface(g, current, sName) + } else { + // Already on the hold surface as first step — use first path point + // that lies on this surface. + cand, ok = firstPathPointOnSurface(g, wps, h) + } + } else { + cand, ok = intersectionOnSurface(g, sName, h) + } + if !ok { + continue + } + idx := findWaypointIndex(wps, cand, g.tolM) + if idx < 0 { + continue + } + // Keep the candidate furthest along the path. + if idx > bestIdx { + bestIdx = idx + bestPoint = wps[idx] + } + } + + // Fallback: hold ∩ current only when no step candidate mapped onto the path + // (avoids Issue 1: A∩19 beating B∩19 when A is current but route leaves via B). + if bestIdx < 0 && current != "" && !sameSurface(g, current, h) { + if cand, ok := intersectionOnSurface(g, current, h); ok { + if idx := findWaypointIndex(wps, cand, g.tolM); idx >= 0 { + bestIdx = idx + bestPoint = wps[idx] } } } - th := TaxiHold{Name: h, WaypointIndex: -1} - if found { - th.Point = hp - th.WaypointIndex = findWaypointIndex(wps, hp, g.tolM) + + if bestIdx < 0 { + return wps, nil, fmt.Sprintf("%s is not on the taxi route.", displayName(h)) } - out = append(out, th) + out = append(out, TaxiHold{Name: h, Point: bestPoint, WaypointIndex: bestIdx}) } - return out + // Re-snap indices after possible HOLD insertions shifted the path. + for i := range out { + idx := findWaypointIndex(wps, out[i].Point, g.tolM) + if idx < 0 { + return wps, nil, fmt.Sprintf("%s is not on the taxi route.", displayName(out[i].Name)) + } + out[i].WaypointIndex = idx + out[i].Point = wps[idx] + } + return wps, out, "" } -// walkPolyline returns points along s from near "from" to near "to" (inclusive), -// walking the shorter direction along the polyline when ambiguous. -func walkPolyline(s *Surface, from, to Point, tolM float64) []Point { +// holdNearRouteSurface reports whether p is within intersection snap of any +// vertex on a route step (or current) surface. +func (g *Graph) holdNearRouteSurface(p Point, steps []string, current string) bool { + if current != "" && g.PointIndexWithin(current, p) >= 0 { + return true + } + for _, sName := range steps { + if g.PointIndexWithin(sName, p) >= 0 { + return true + } + } + // Also allow closest-within-tol when the hold is not exactly a stored vertex. + tolSq := g.tolM * g.tolM + check := func(name string) bool { + s := g.Surface(name) + if s == nil { + return false + } + for _, q := range s.Points { + if geo.DistanceSq(p.Lat, p.Lon, q.Lat, q.Lon) <= tolSq { + return true + } + } + return false + } + if current != "" && check(current) { + return true + } + for _, sName := range steps { + if check(sName) { + return true + } + } + return false +} + +// insertWaypoint inserts p into wps if not already present within epsM. +// Insertion is before the closest existing waypoint (or append if empty). +// Returns the updated slice and the index of p. +func insertWaypoint(wps []Point, p Point, epsM float64) ([]Point, int) { + if idx := findWaypointIndex(wps, p, epsM); idx >= 0 { + return wps, idx + } + if len(wps) == 0 { + return []Point{p}, 0 + } + // Insert before the closest path vertex so holds ahead of the first turn + // appear earlier on the path. + best := 0 + bestD := geo.DistanceSq(p.Lat, p.Lon, wps[0].Lat, wps[0].Lon) + for i := 1; i < len(wps); i++ { + d := geo.DistanceSq(p.Lat, p.Lon, wps[i].Lat, wps[i].Lon) + if d < bestD { + bestD = d + best = i + } + } + out := make([]Point, 0, len(wps)+1) + out = append(out, wps[:best]...) + out = append(out, p) + out = append(out, wps[best:]...) + return out, best +} + +// intersectionOnSurface returns the intersection point preferred on surface B +// (the second name), falling back to the point on A. +func intersectionOnSurface(g *Graph, nameA, nameB string) (Point, bool) { + p, idxA, idxB, ok := g.FindIntersection(nameA, nameB) + if !ok { + return Point{}, false + } + if sb := g.Surface(nameB); sb != nil && idxB >= 0 && idxB < len(sb.Points) { + return sb.Points[idxB], true + } + if sa := g.Surface(nameA); sa != nil && idxA >= 0 && idxA < len(sa.Points) { + return sa.Points[idxA], true + } + return p, true +} + +// firstPathPointOnSurface finds the first waypoint within snap of any point on surface h. +func firstPathPointOnSurface(g *Graph, wps []Point, h string) (Point, bool) { + s := g.Surface(h) + if s == nil || len(s.Points) == 0 || len(wps) == 0 { + return Point{}, false + } + tolSq := g.tolM * g.tolM + for _, wp := range wps { + for _, sp := range s.Points { + if geo.DistanceSq(wp.Lat, wp.Lon, sp.Lat, sp.Lon) <= tolSq { + return wp, true + } + } + } + return Point{}, false +} + +// walkPolyline returns points along s from the vertex nearest "from" to the +// vertex nearest "to" (inclusive), walking index-forward or index-reverse along +// the open polyline (no closed-loop shorter-arc logic). +func walkPolyline(s *Surface, from, to Point) []Point { if s == nil || len(s.Points) == 0 { return nil } @@ -463,11 +573,9 @@ func walkPolyline(s *Surface, from, to Point, tolM float64) []Point { if iFrom == iTo { return []Point{s.Points[iFrom]} } - // Forward path length vs reverse. if iFrom < iTo { return append([]Point(nil), s.Points[iFrom:iTo+1]...) } - // Reverse. out := make([]Point, 0, iFrom-iTo+1) for i := iFrom; i >= iTo; i-- { out = append(out, s.Points[i]) @@ -491,12 +599,14 @@ func closestIndex(pts []Point, p Point) int { return best } -func appendUniquePoints(dst []Point, src []Point, tolM float64) []Point { - tolSq := tolM * tolM +// appendUniquePoints appends src onto dst, skipping a point only when it is +// within epsM of the current last point (stitch joint dedup — not intersection snap). +func appendUniquePoints(dst []Point, src []Point, epsM float64) []Point { + epsSq := epsM * epsM for _, p := range src { if len(dst) > 0 { last := dst[len(dst)-1] - if geo.DistanceSq(last.Lat, last.Lon, p.Lat, p.Lon) <= tolSq { + if geo.DistanceSq(last.Lat, last.Lon, p.Lat, p.Lon) <= epsSq { continue } } @@ -507,10 +617,15 @@ func appendUniquePoints(dst []Point, src []Point, tolM float64) []Point { func findWaypointIndex(wps []Point, p Point, tolM float64) int { tolSq := tolM * tolM + // Prefer closest within tol so a hold snaps to the best path vertex. + best := -1 + bestD := tolSq + 1 for i, q := range wps { - if geo.DistanceSq(q.Lat, q.Lon, p.Lat, p.Lon) <= tolSq { - return i + d := geo.DistanceSq(q.Lat, q.Lon, p.Lat, p.Lon) + if d <= tolSq && d < bestD { + bestD = d + best = i } } - return -1 + return best } diff --git a/internal/sweatbox/taxi_test.go b/internal/sweatbox/taxi_test.go index fa3dfde..916b4a1 100644 --- a/internal/sweatbox/taxi_test.go +++ b/internal/sweatbox/taxi_test.go @@ -1,8 +1,11 @@ package sweatbox import ( + "math" "strings" "testing" + + "github.com/renorris/openfsd/internal/geo" ) func TestPlanTaxi_KBTVHappyPaths(t *testing.T) { @@ -26,24 +29,6 @@ func TestPlanTaxi_KBTVHappyPaths(t *testing.T) { t.Fatalf("waypoints too short: %v", plan.Waypoints) } - // Current on A, taxi B 19 — first step B intersects A. - plan, errMsg = g.PlanTaxi("A", []string{"B", "19"}, []string{"19"}) - if errMsg != "" { - t.Fatalf("A→B→19: %s", errMsg) - } - if len(plan.Holds) != 1 || plan.Holds[0] != "19" { - t.Errorf("holds = %v", plan.Holds) - } - if len(plan.HoldAt) != 1 { - t.Fatalf("HoldAt len = %d", len(plan.HoldAt)) - } - if plan.HoldAt[0].Name != "19" { - t.Errorf("HoldAt name = %q", plan.HoldAt[0].Name) - } - if plan.HoldAt[0].Point.Lat == 0 { - t.Error("HoldAt point unset") - } - // Via C K J to 33 plan, errMsg = g.PlanTaxi("C", []string{"C", "K", "J", "33"}, nil) if errMsg != "" { @@ -53,8 +38,7 @@ func TestPlanTaxi_KBTVHappyPaths(t *testing.T) { t.Errorf("expected multi-point path, got %d", len(plan.Waypoints)) } - // Parking destination @G1 via A (A is near gates; G1 may be far — still allowed) - // Use GA9 which snaps to J. + // Parking destination @GA9 (snaps near J). plan, errMsg = g.PlanTaxi("J", []string{"J", "@GA9"}, nil) if errMsg != "" { t.Fatalf("J @GA9: %s", errMsg) @@ -65,11 +49,14 @@ func TestPlanTaxi_KBTVHappyPaths(t *testing.T) { if len(plan.Steps) != 1 || plan.Steps[0] != "J" { t.Errorf("Steps = %v", plan.Steps) } - // Last waypoint should be parking. + // Last waypoint should be parking (even if within snap of last taxiway point). last := plan.Waypoints[len(plan.Waypoints)-1] ps := g.Surface("GA9") - if ps == nil || last.Lat != ps.Points[0].Lat { - t.Errorf("last wp = %v, parking = %v", last, ps) + if ps == nil || last != ps.Points[0] { + t.Errorf("last wp = %v, parking = %v", last, ps.Points[0]) + } + if len(plan.Waypoints) < 2 { + t.Fatal("parking within snap of taxiway must still append parking as final point") } // Bare parking name as last step. @@ -82,6 +69,52 @@ func TestPlanTaxi_KBTVHappyPaths(t *testing.T) { } } +// Issue 1 regression: hold short of 19 on A→B→19 must be B∩19, not A∩19. +func TestPlanTaxi_HoldAtCorrectCrossing_KBTV(t *testing.T) { + g := NewGraph(loadKBTV(t)) + + bX19, _, _, ok := g.FindIntersection("B", "19") + if !ok { + t.Fatal("B and 19 must intersect on KBTV") + } + aX19, _, _, ok := g.FindIntersection("A", "19") + if !ok { + t.Fatal("A and 19 must intersect on KBTV") + } + // Sanity: the two crossings are far apart (~560 m). + if geo.Distance(bX19.Lat, bX19.Lon, aX19.Lat, aX19.Lon) < 200 { + t.Fatalf("test setup: A∩19 and B∩19 too close") + } + + plan, errMsg := g.PlanTaxi("A", []string{"B", "19"}, []string{"19"}) + if errMsg != "" { + t.Fatalf("A→B→19 hs 19: %s", errMsg) + } + if len(plan.HoldAt) != 1 { + t.Fatalf("HoldAt len = %d", len(plan.HoldAt)) + } + h := plan.HoldAt[0] + if h.Name != "19" { + t.Errorf("HoldAt name = %q", h.Name) + } + if h.WaypointIndex < 0 || h.WaypointIndex >= len(plan.Waypoints) { + t.Fatalf("WaypointIndex = %d, path len %d", h.WaypointIndex, len(plan.Waypoints)) + } + if plan.Waypoints[h.WaypointIndex] != h.Point { + t.Errorf("Waypoints[i]=%v HoldAt.Point=%v", plan.Waypoints[h.WaypointIndex], h.Point) + } + // Hold must be near B∩19, not A∩19. + dB := geo.Distance(h.Point.Lat, h.Point.Lon, bX19.Lat, bX19.Lon) + dA := geo.Distance(h.Point.Lat, h.Point.Lon, aX19.Lat, aX19.Lon) + if dB > DefaultIntersectionTolM { + t.Errorf("hold is %.1fm from B∩19 (want ≤%.1f); A∩19 dist=%.1fm point=%v", + dB, DefaultIntersectionTolM, dA, h.Point) + } + if dA < dB { + t.Errorf("hold closer to A∩19 (%.1fm) than B∩19 (%.1fm)", dA, dB) + } +} + func TestPlanTaxi_ValidationErrors(t *testing.T) { g := NewGraph(loadKBTV(t)) @@ -107,6 +140,8 @@ func TestPlanTaxi_ValidationErrors(t *testing.T) { {"pair no intersect", "A", []string{"A", "D"}, nil, "do not intersect"}, {"duplicate consecutive", "A", []string{"A", "A", "B"}, nil, "occurs twice in a row"}, {"off surface empty current ok if steps ok", "", []string{"A", "B"}, nil, ""}, + // Issue 5: hold of 19 is not on C–K–J–33 path (even though C∩19 exists). + {"off-route hold", "C", []string{"C", "K", "J", "33"}, []string{"19"}, "not on the taxi route"}, } for _, tc := range tests { t.Run(tc.name, func(t *testing.T) { @@ -125,11 +160,6 @@ func TestPlanTaxi_ValidationErrors(t *testing.T) { } func manySteps(n int) []string { - // Alternate A/B would fail consecutive check; use unique invalid for count test — - // count is checked before surface lookup... actually count is first, then normalize. - // For max steps we only need len > 100; unknown steps come after. - // Build 101 "A" would fail consecutive after count check order: - // count is checked first — good. out := make([]string, n) for i := range out { out[i] = "A" @@ -180,16 +210,30 @@ func TestPlanTaxi_HoldShortNamedAndRunway(t *testing.T) { if len(plan.HoldAt) != 2 { t.Fatalf("HoldAt = %+v", plan.HoldAt) } - // BHP uses its single point. + // BHP must land on path. if plan.HoldAt[0].Name != "BHP" { t.Errorf("hold0 = %q", plan.HoldAt[0].Name) } - if plan.HoldAt[0].Point.Lat != 10.001 { + if plan.HoldAt[0].WaypointIndex < 0 { + t.Errorf("BHP WaypointIndex = %d", plan.HoldAt[0].WaypointIndex) + } + if math.Abs(plan.HoldAt[0].Point.Lat-10.001) > 1e-9 { t.Errorf("BHP point = %v", plan.HoldAt[0].Point) } + // Runway 9 hold at B∩9 (entry onto runway), on path. if plan.HoldAt[1].Name != "9" { t.Errorf("hold1 = %q", plan.HoldAt[1].Name) } + if plan.HoldAt[1].WaypointIndex < 0 || plan.HoldAt[1].WaypointIndex >= len(plan.Waypoints) { + t.Fatalf("rwy WaypointIndex = %d", plan.HoldAt[1].WaypointIndex) + } + bX9, ok := intersectionOnSurface(g, "B", "9") + if !ok { + t.Fatal("B∩9 missing") + } + if geo.Distance(plan.HoldAt[1].Point.Lat, plan.HoldAt[1].Point.Lon, bX9.Lat, bX9.Lon) > DefaultIntersectionTolM { + t.Errorf("rwy hold %v far from B∩9 %v", plan.HoldAt[1].Point, bX9) + } // HOLD cannot be a taxi step. _, errMsg = g.PlanTaxi("A", []string{"BHP"}, nil) @@ -208,6 +252,9 @@ func TestPlanTaxi_HoldShortNamedAndRunway(t *testing.T) { if len(plan.Waypoints) == 0 { t.Error("expected waypoints to parking") } + if plan.Waypoints[len(plan.Waypoints)-1] != g.Surface("G1").Points[0] { + t.Error("final waypoint must be parking") + } } func TestPlanTaxi_NilGraph(t *testing.T) { @@ -228,24 +275,114 @@ func TestPlanTaxi_EmptyHoldToken(t *testing.T) { func TestPlanTaxi_PathWalksIntermediate(t *testing.T) { g := NewGraph(loadKBTV(t)) - // A has many points; A→E should walk along A between shared points. - plan, errMsg := g.PlanTaxi("A", []string{"A", "E"}, nil) + aSurf := g.Surface("A") + if aSurf == nil { + t.Fatal("taxiway A missing") + } + + // FindIntersection(nameA,nameB) → (pt on A, idxA, idxB, ok). + // C∩A: index on A is idxB; A∩B: index on A is idxA. + _, _, iEntry, ok := g.FindIntersection("C", "A") + if !ok { + t.Fatal("C∩A") + } + _, iExit, _, ok := g.FindIntersection("A", "B") + if !ok { + t.Fatal("A∩B") + } + lo, hi := iEntry, iExit + if lo > hi { + lo, hi = hi, lo + } + wantSpan := hi - lo + 1 + if wantSpan < 2 { + t.Fatalf("unexpected A index span %d..%d", lo, hi) + } + + // Current on C: taxi A B — walk A from C∩A to A∩B inclusive of intermediate A vertices. + plan, errMsg := g.PlanTaxi("C", []string{"A", "B"}, nil) if errMsg != "" { t.Fatal(errMsg) } - // Intersection A∩E near north end; from start of A (if current=A, from unset) - // path should at least include the intersection point. - if len(plan.Waypoints) < 1 { - t.Fatal("no waypoints") + for i := lo; i <= hi; i++ { + found := false + for _, wp := range plan.Waypoints { + if wp == aSurf.Points[i] { + found = true + break + } + } + if !found { + t.Errorf("missing A[%d]=%v in path %v", i, aSurf.Points[i], plan.Waypoints) + } } - // Current off A: from C onto A then to B - plan, errMsg = g.PlanTaxi("C", []string{"A", "B"}, nil) + if len(plan.Waypoints) < wantSpan { + t.Errorf("path len %d < A span %d", len(plan.Waypoints), wantSpan) + } +} + +// Issue 2: dense polyline vertices (~10 m spacing) must not collapse under 100 ft snap. +func TestPlanTaxi_DensePolylineRetainsVertices(t *testing.T) { + // Build a N–S taxiway with 20 points ~10 m apart, then turn onto short east taxiway. + const n = 20 + pts := make([]Point, n) + // ~10 m in latitude ≈ 10/111320 degrees. + dLat := 10.0 / 111320.0 + for i := 0; i < n; i++ { + pts[i] = Point{Lat: 40.0 + float64(i)*dLat, Lon: -75.0} + } + // End of A shares last point with start of B. + bPts := []Point{ + pts[n-1], + {Lat: pts[n-1].Lat, Lon: -75.0 + dLat}, // ~10 m east at mid-lat (approx) + } + apt := Airport{ + Surfaces: []Surface{ + {Kind: SurfaceTaxiway, Name: "A", Points: pts}, + {Kind: SurfaceTaxiway, Name: "B", Points: bPts}, + }, + } + g := NewGraph(&apt) + + // Verify consecutive spacing is well under 100 ft. + d := geo.Distance(pts[0].Lat, pts[0].Lon, pts[1].Lat, pts[1].Lon) + if d > 15 || d < 5 { + t.Fatalf("setup spacing = %.1fm, want ~10m", d) + } + + plan, errMsg := g.PlanTaxi("A", []string{"A", "B"}, nil) if errMsg != "" { t.Fatal(errMsg) } - // Should include C∩A and A∩B and intermediates on A between them. - if len(plan.Waypoints) < 2 { - t.Fatalf("want path along A, got %d pts: %v", len(plan.Waypoints), plan.Waypoints) + // When already on A, path starts at A∩B (last point) only for the A leg + // (from unset, to = last). That yields a single point on A then B. + // Use current off A so we walk full A from start: place a parking as current + // that intersects first point of A. + apt2 := apt + apt2.Surfaces = append(apt2.Surfaces, Surface{ + Kind: SurfaceParking, Name: "P1", + Points: []Point{pts[0]}, + }) + g2 := NewGraph(&apt2) + // Taxi from P1 (intersects A[0]) along A to B — first step A must ∩ current P1. + plan, errMsg = g2.PlanTaxi("P1", []string{"A", "B"}, nil) + if errMsg != "" { + t.Fatal(errMsg) + } + // Full walk of A from index 0 to n-1 inclusive = n vertices, then B may add one more. + // With 100 ft stitch dedup this would collapse to ~5; with 1 m stitch we keep all n. + countOnA := 0 + for _, wp := range plan.Waypoints { + for _, ap := range pts { + if wp == ap { + countOnA++ + break + } + } + } + if countOnA < n { + t.Fatalf("retained %d/%d A vertices (100ft collapse bug if ~5); path=%v", + countOnA, n, plan.Waypoints) } } @@ -261,28 +398,39 @@ func TestWalkPolyline_Directions(t *testing.T) { s := &Surface{Points: []Point{ {0, 0}, {1, 0}, {2, 0}, {3, 0}, }} - fwd := walkPolyline(s, Point{0, 0}, Point{2, 0}, 30) + fwd := walkPolyline(s, Point{0, 0}, Point{2, 0}) if len(fwd) != 3 { t.Fatalf("fwd len = %d", len(fwd)) } - rev := walkPolyline(s, Point{3, 0}, Point{1, 0}, 30) + rev := walkPolyline(s, Point{3, 0}, Point{1, 0}) if len(rev) != 3 || rev[0].Lat != 3 || rev[2].Lat != 1 { t.Fatalf("rev = %v", rev) } - same := walkPolyline(s, Point{1, 0}, Point{1, 0}, 30) + same := walkPolyline(s, Point{1, 0}, Point{1, 0}) if len(same) != 1 { t.Fatalf("same = %v", same) } - if walkPolyline(nil, Point{}, Point{}, 30) != nil { + if walkPolyline(nil, Point{}, Point{}) != nil { t.Error("nil surface") } } -func TestAppendUniquePoints(t *testing.T) { - var dst []Point - dst = appendUniquePoints(dst, []Point{{1, 2}, {1, 2}, {3, 4}}, 30.48) +func TestAppendUniquePoints_StitchEpsNotSnap(t *testing.T) { + // Points ~10 m apart must all be kept with pathStitchEpsM. + dLat := 10.0 / 111320.0 + src := []Point{ + {40.0, -75.0}, + {40.0 + dLat, -75.0}, + {40.0 + 2*dLat, -75.0}, + } + dst := appendUniquePoints(nil, src, pathStitchEpsM) + if len(dst) != 3 { + t.Fatalf("stitch eps kept %d, want 3 (would be 1 under 100ft snap)", len(dst)) + } + // Exact duplicate still collapses. + dst = appendUniquePoints(nil, []Point{{1, 2}, {1, 2}, {3, 4}}, pathStitchEpsM) if len(dst) != 2 { - t.Fatalf("dst = %v", dst) + t.Fatalf("exact dup: %v", dst) } } @@ -295,11 +443,14 @@ func TestPlanTaxi_CaseInsensitive(t *testing.T) { if plan.Steps[0] != "B" { t.Errorf("steps[0]=%q", plan.Steps[0]) } + if plan.HoldAt[0].WaypointIndex < 0 { + t.Error("hold index") + } } func TestPlanTaxi_MaxHoldsBoundary(t *testing.T) { g := NewGraph(loadKBTV(t)) - // 10 holds of A is ok (count only); 11 fails. + // 10 holds of B is ok (B is on A→B→19); 11 fails count before path. holds := make([]string, 10) for i := range holds { holds[i] = "B" @@ -314,3 +465,91 @@ func TestPlanTaxi_MaxHoldsBoundary(t *testing.T) { t.Errorf("11 holds: %q", errMsg) } } + +func TestPlanTaxi_HOLDOffRouteRejected(t *testing.T) { + apt := Airport{ + Surfaces: []Surface{ + {Kind: SurfaceTaxiway, Name: "A", Points: []Point{{10, 20}, {10.001, 20}}}, + {Kind: SurfaceTaxiway, Name: "B", Points: []Point{{10.001, 20}, {10.002, 20}}}, + // HOLD far from path. + {Kind: SurfaceHold, Name: "FAR", Points: []Point{{11, 21}}}, + }, + } + g := NewGraph(&apt) + _, errMsg := g.PlanTaxi("A", []string{"A", "B"}, []string{"FAR"}) + if !strings.Contains(errMsg, "not on the taxi route") { + t.Errorf("got %q", errMsg) + } +} + +func TestPlanTaxi_HoldOnCurrentDestinationSurface(t *testing.T) { + // Already on runway 19, taxi via B (off then... 19→B): hold short of 19 uses + // firstPathPointOnSurface when hold is the first step and current. + g := NewGraph(loadKBTV(t)) + // Route must leave 19 onto a connected surface then... actually hold of 19 + // while steps start with 19: PlanTaxi("19", []string{"19", "B"}, []string{"19"}) + // sameSurface first step and current — not Already there (multi-step). + plan, errMsg := g.PlanTaxi("19", []string{"19", "B"}, []string{"19"}) + if errMsg != "" { + t.Fatalf("%s", errMsg) + } + if len(plan.HoldAt) != 1 || plan.HoldAt[0].WaypointIndex < 0 { + t.Fatalf("HoldAt=%+v", plan.HoldAt) + } +} + +func TestInsertWaypoint(t *testing.T) { + wps, idx := insertWaypoint(nil, Point{1, 2}, pathStitchEpsM) + if len(wps) != 1 || idx != 0 { + t.Fatalf("empty: %v idx=%d", wps, idx) + } + // Already present within eps + wps2, idx2 := insertWaypoint(wps, Point{1, 2}, pathStitchEpsM) + if len(wps2) != 1 || idx2 != 0 { + t.Fatalf("dup: %v idx=%d", wps2, idx2) + } + // Insert before closer of two distant points (~5 m from first). + dLat := 5.0 / 111320.0 + base := []Point{{40.0, -75.0}, {40.1, -75.0}} + wps3, idx3 := insertWaypoint(base, Point{40.0 + dLat, -75.0}, pathStitchEpsM) + if idx3 != 0 || len(wps3) != 3 { + t.Fatalf("insert: %v idx=%d", wps3, idx3) + } +} + +func TestHoldNearRouteSurface_CurrentOnly(t *testing.T) { + apt := Airport{Surfaces: []Surface{ + {Kind: SurfaceTaxiway, Name: "A", Points: []Point{{10, 20}, {10.001, 20}}}, + {Kind: SurfaceHold, Name: "H1", Points: []Point{{10, 20}}}, + }} + g := NewGraph(&apt) + if !g.holdNearRouteSurface(Point{10, 20}, nil, "A") { + t.Error("should be near current A") + } + if g.holdNearRouteSurface(Point{0, 0}, []string{"A"}, "") { + t.Error("far point") + } +} + +func TestIntersectionOnSurface_PrefersB(t *testing.T) { + g := NewGraph(loadKBTV(t)) + p, ok := intersectionOnSurface(g, "A", "B") + if !ok { + t.Fatal("A∩B") + } + // Point should be on B (exact shared vertex on both). + b := g.Surface("B") + found := false + for _, q := range b.Points { + if q == p { + found = true + break + } + } + if !found { + // Within snap of a B point is also fine. + if g.PointIndexWithin("B", p) < 0 { + t.Errorf("point %v not on B", p) + } + } +}