diff --git a/.vscode/settings.json b/.vscode/settings.json index 9d9586dc..2bc459ea 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -3,7 +3,7 @@ "CGO_ENABLED": "1", "C_INCLUDE_PATH": "${workspaceFolder}/third_party/whisper.cpp/ggml/include:${workspaceFolder}/third_party/whisper.cpp/include", "LIBRARY_PATH": "${workspaceFolder}/third_party/whisper.cpp/", - "GOROOT": "C:\\msys64\\ucrt64\\lib\\go" + "GOROOT": "${env:GOROOT}" }, "go.buildTags": "nolibopusfile", "logViewer.watch": [ diff --git a/Makefile b/Makefile index 208532af..24d32ffd 100644 --- a/Makefile +++ b/Makefile @@ -156,6 +156,7 @@ $(SKYEYE_SCALER_BIN): generate $(SKYEYE_SOURCES) run: $(BUILD_VARS) $(GO) run -race $(BUILD_FLAGS) ./cmd/skyeye/ $(ARGS) + .PHONY: test test: generate $(BUILD_VARS) $(GO) tool gotestsum -- $(BUILD_FLAGS) $(TEST_FLAGS) ./... diff --git a/internal/application/app.go b/internal/application/app.go index fefe8f5a..4d59feff 100644 --- a/internal/application/app.go +++ b/internal/application/app.go @@ -23,6 +23,7 @@ import ( "github.com/dharmab/skyeye/pkg/sim" "github.com/dharmab/skyeye/pkg/simpleradio" srs "github.com/dharmab/skyeye/pkg/simpleradio/types" + "github.com/dharmab/skyeye/pkg/spatial" "github.com/dharmab/skyeye/pkg/synthesizer/speakers" "github.com/dharmab/skyeye/pkg/telemetry" "github.com/dharmab/skyeye/pkg/traces" @@ -385,6 +386,9 @@ func (a *Application) updateBullseyes() { log.Warn().Err(err).Msg("error reading bullseye") } else { a.radar.SetBullseye(bullseye, coalition) + if name, changed := spatial.DetectTerrainFromBullseye(coalition.String(), bullseye); changed { + log.Debug().Str("terrain", name).Msg("terrain detected from bullseyes") + } } } } diff --git a/pkg/spatial/pydcs_bearing.go b/pkg/spatial/pydcs_bearing.go new file mode 100644 index 00000000..abd2bfb8 --- /dev/null +++ b/pkg/spatial/pydcs_bearing.go @@ -0,0 +1,674 @@ +package spatial + +import ( + "fmt" + "math" + "sync" + "sync/atomic" + + "github.com/martinlindhe/unit" + "github.com/paulmach/orb" + "github.com/paulmach/orb/project" + "github.com/rs/zerolog/log" + + "github.com/dharmab/skyeye/pkg/bearings" +) + +const ( + wgs84A = 6378137.0 + wgs84F = 1 / 298.257223563 + wgs84E2 = wgs84F * (2 - wgs84F) +) + +func deg2rad(d float64) float64 { + return d * math.Pi / 180.0 +} + +type tmProjector struct { + tm TransverseMercator +} + +func (p tmProjector) forward(latDeg, lonDeg float64) (easting, northing float64) { + lat := deg2rad(latDeg) + lon := deg2rad(lonDeg) + origin := deg2rad(float64(p.tm.CentralMeridian)) + k0 := p.tm.ScaleFactor + coeffs := tmCoefficients() + + lam := lon - origin + tau := math.Tan(lat) + taup := taupFromTau(tau) + + sinLam := math.Sin(lam) + cosLam := math.Cos(lam) + denom := math.Sqrt(taup*taup + cosLam*cosLam) + + xiPrime := math.Atan2(taup, cosLam) + etaPrime := math.Asinh(sinLam / denom) + + xi := xiPrime + eta := etaPrime + for j := 1; j <= 6; j++ { + twoJXi := 2 * float64(j) * xiPrime + twoJEta := 2 * float64(j) * etaPrime + xi += coeffs.alpha[j] * math.Sin(twoJXi) * math.Cosh(twoJEta) + eta += coeffs.alpha[j] * math.Cos(twoJXi) * math.Sinh(twoJEta) + } + + easting = p.tm.FalseEasting + k0*coeffs.a1*eta + northing = p.tm.FalseNorthing + k0*coeffs.a1*xi + return easting, northing +} + +func (p tmProjector) inverse(easting, northing float64) (latDeg, lonDeg float64) { + k0 := p.tm.ScaleFactor + coeffs := tmCoefficients() + + x := (easting - p.tm.FalseEasting) / (k0 * coeffs.a1) + y := (northing - p.tm.FalseNorthing) / (k0 * coeffs.a1) + + xiPrime := y + etaPrime := x + for j := 1; j <= 6; j++ { + twoJXi := 2 * float64(j) * y + twoJEta := 2 * float64(j) * x + xiPrime -= coeffs.beta[j] * math.Sin(twoJXi) * math.Cosh(twoJEta) + etaPrime -= coeffs.beta[j] * math.Cos(twoJXi) * math.Sinh(twoJEta) + } + + sinXi := math.Sin(xiPrime) + cosXi := math.Cos(xiPrime) + sinhEta := math.Sinh(etaPrime) + + taup := sinXi / math.Sqrt(sinhEta*sinhEta+cosXi*cosXi) + lam := math.Atan2(sinhEta, cosXi) + tau := tauFromTaup(taup) + + lat := math.Atan(tau) + lon := deg2rad(float64(p.tm.CentralMeridian)) + lam + + latDeg = rad2deg(lat) + lonDeg = rad2deg(lon) + return latDeg, lonDeg +} + +type tmSeriesCoefficients struct { + a1 float64 + alpha [7]float64 + beta [7]float64 +} + +func tmCoefficients() tmSeriesCoefficients { + n := wgs84F / (2 - wgs84F) + n2 := n * n + n3 := n2 * n + n4 := n2 * n2 + n5 := n4 * n + n6 := n3 * n3 + + a1 := wgs84A / (1 + n) * (1 + n2/4 + n4/64 + n6/256) + + var alpha [7]float64 + alpha[1] = n/2 - 2*n2/3 + 5*n3/16 + 41*n4/180 - 127*n5/288 + 7891*n6/37800 + alpha[2] = 13*n2/48 - 3*n3/5 + 557*n4/1440 + 281*n5/630 - 1983433*n6/1935360 + alpha[3] = 61*n3/240 - 103*n4/140 + 15061*n5/26880 + 167603*n6/181440 + alpha[4] = 49561*n4/161280 - 179*n5/168 + 6601661*n6/7257600 + alpha[5] = 34729*n5/80640 - 3418889*n6/1995840 + alpha[6] = 212378941 * n6 / 319334400 + + var beta [7]float64 + beta[1] = n/2 - 2*n2/3 + 37*n3/96 - n4/360 - 81*n5/512 + 96199*n6/604800 + beta[2] = n2/48 + n3/15 - 437*n4/1440 + 46*n5/105 - 1118711*n6/3870720 + beta[3] = 17*n3/480 - 37*n4/840 - 209*n5/4480 + 5569*n6/90720 + beta[4] = 4397*n4/161280 - 11*n5/504 - 830251*n6/7257600 + beta[5] = 4583*n5/161280 - 108847*n6/3991680 + beta[6] = 20648693 * n6 / 638668800 + + return tmSeriesCoefficients{ + a1: a1, + alpha: alpha, + beta: beta, + } +} + +func taupFromTau(tau float64) float64 { + if tau == 0 { + return 0 + } + sinPhi := tau / math.Sqrt(1+tau*tau) + return math.Sinh(math.Asinh(tau) - math.Sqrt(wgs84E2)*math.Atanh(math.Sqrt(wgs84E2)*sinPhi)) +} + +func tauFromTaup(taup float64) float64 { + if taup == 0 { + return 0 + } + tau := taup + for range 10 { + sqrt1pTau2 := math.Sqrt(1 + tau*tau) + sinPhi := tau / sqrt1pTau2 + e := math.Sqrt(wgs84E2) + taupCalc := math.Sinh(math.Asinh(tau) - e*math.Atanh(e*sinPhi)) + delta := taupCalc - taup + if math.Abs(delta) < 1e-14 { + break + } + denom := 1 - wgs84E2*sinPhi*sinPhi + duDtau := 1/sqrt1pTau2 - (wgs84E2/(sqrt1pTau2*sqrt1pTau2*sqrt1pTau2))/denom + dtaupDtau := math.Sqrt(1+taupCalc*taupCalc) * duDtau + tau -= delta / dtaupDtau + } + return tau +} + +func (p tmProjector) toProjection() orb.Projection { + return func(point orb.Point) orb.Point { + easting, northing := p.forward(point.Lat(), point.Lon()) + return orb.Point{easting, northing} + } +} + +func (p tmProjector) toWGS84() orb.Projection { + return func(point orb.Point) orb.Point { + lat, lon := p.inverse(point[0], point[1]) + return orb.Point{lon, lat} + } +} + +func greatCircleDeg(lat1, lon1, lat2, lon2 float64) float64 { + lat1r := lat1 * math.Pi / 180 + lon1r := lon1 * math.Pi / 180 + lat2r := lat2 * math.Pi / 180 + lon2r := lon2 * math.Pi / 180 + dLat := lat2r - lat1r + dLon := lon2r - lon1r + a := math.Sin(dLat/2)*math.Sin(dLat/2) + math.Cos(lat1r)*math.Cos(lat2r)*math.Sin(dLon/2)*math.Sin(dLon/2) + c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a)) + return c +} + +// TransverseMercator represents the parameters for a Transverse Mercator projection. +type TransverseMercator struct { + CentralMeridian int + FalseEasting float64 + FalseNorthing float64 + ScaleFactor float64 +} + +type latLonBounds struct { + minLat float64 + maxLat float64 + minLon float64 + maxLon float64 +} + +type terrainDef struct { + name string + tm TransverseMercator + boundsXY [4]float64 + latLonBox latLonBounds + centerLat float64 + centerLon float64 +} + +func (l latLonBounds) contains(lat, lon float64) bool { + return lat >= l.minLat && lat <= l.maxLat && lon >= l.minLon && lon <= l.maxLon +} + +func (l latLonBounds) area() float64 { + return math.Abs(l.maxLat-l.minLat) * math.Abs(l.maxLon-l.minLon) +} + +var ( + projectionMu sync.RWMutex + currentProjection = CaucasusProjection() + currentTerrain = "Caucasus" + terrainDetected atomic.Bool + bullseyes = make(map[string]orb.Point) +) + +var terrainDefs = []terrainDef{ + {name: "Afghanistan", tm: AfghanistanProjection(), boundsXY: [4]float64{532000.0, -534000.0, -512000.0, 757000.0}, centerLat: 33.9346, centerLon: 66.24705}, + {name: "Caucasus", tm: CaucasusProjection(), boundsXY: [4]float64{380 * 1000, -560 * 1000, -600 * 1000, 1130 * 1000}, centerLat: 43.69666, centerLon: 32.96}, + {name: "Falklands", tm: FalklandsProjection(), boundsXY: [4]float64{74967, -114995, -129982, 129991}, centerLat: 52.468, centerLon: 59.173}, + {name: "GermanyCW", tm: GermanyColdWarProjection(), boundsXY: [4]float64{260000.0, -1100000.0, -600000.0, -425000.0}, centerLat: 51.0, centerLon: 11.0}, + {name: "Iraq", tm: IraqProjection(), boundsXY: [4]float64{440000.0, -500000.0, -950000.0, 850000.0}, centerLat: 30.76, centerLon: 59.07}, + {name: "Kola", tm: KolaProjection(), boundsXY: [4]float64{-315000, -890000, 900000, 856000}, centerLat: 68.0, centerLon: 22.5}, + {name: "MarianaIslands", tm: MarianasProjection(), boundsXY: [4]float64{1000 * 10000, -1000 * 1000, -300 * 1000, 500 * 1000}, centerLat: 13.485, centerLon: 144.798}, + {name: "Nevada", tm: NevadaProjection(), boundsXY: [4]float64{-167000.0, -330000.0, -500000.0, 210000.0}, centerLat: 39.81806, centerLon: -114.73333}, + {name: "Normandy", tm: NormandyProjection(), boundsXY: [4]float64{-132707.843750, -389942.906250, 185756.156250, 165065.078125}, centerLat: 41.3, centerLon: 0.18}, + {name: "PersianGulf", tm: PersianGulfProjection(), boundsXY: [4]float64{-218768.750000, -392081.937500, 197357.906250, 333129.125000}, centerLat: 0, centerLon: 0}, + {name: "Sinai", tm: SinaiProjection(), boundsXY: [4]float64{-450000, -280000, 500000, 560000}, centerLat: 30.047, centerLon: 31.224}, + {name: "Syria", tm: SyriaProjection(), boundsXY: [4]float64{-320000, -579986, 300000, 579998}, centerLat: 35.021, centerLon: 35.901}, + {name: "TheChannel", tm: TheChannelProjection(), boundsXY: [4]float64{74967, -114995, -129982, 129991}, centerLat: 50.875, centerLon: 1.5875}, +} + +func init() { + for i := range terrainDefs { + if err := computeLatLonBounds(&terrainDefs[i]); err != nil { + log.Warn().Err(err).Str("terrain", terrainDefs[i].name).Msg("failed to compute lat/lon bounds for terrain") + } + } +} + +func terrainDefByName(name string) (terrainDef, bool) { + for _, td := range terrainDefs { + if td.name == name { + return td, true + } + } + return terrainDef{}, false +} + +func computeLatLonBounds(td *terrainDef) error { + x1, y1, x2, y2 := td.boundsXY[0], td.boundsXY[1], td.boundsXY[2], td.boundsXY[3] + norths := []float64{y1, y2} + easts := []float64{x1, x2} + + minLat := math.Inf(1) + maxLat := math.Inf(-1) + minLon := math.Inf(1) + maxLon := math.Inf(-1) + + for _, north := range norths { + for _, east := range easts { + lat, lon, err := ProjectionToLatLongFor(td.tm, north, east) + if err != nil { + return fmt.Errorf("convert bounds corner: %w", err) + } + if lat < minLat { + minLat = lat + } + if lat > maxLat { + maxLat = lat + } + if lon < minLon { + minLon = lon + } + if lon > maxLon { + maxLon = lon + } + } + } + + td.latLonBox = latLonBounds{ + minLat: minLat, + maxLat: maxLat, + minLon: minLon, + maxLon: maxLon, + } + if td.centerLat == 0 && td.centerLon == 0 { + td.centerLat = (minLat + maxLat) / 2 + td.centerLon = (minLon + maxLon) / 2 + } + return nil +} + +func bullseyeInsideBounds(td terrainDef, bullseye orb.Point) bool { + if td.latLonBox.contains(bullseye.Lat(), bullseye.Lon()) { + return true + } + + xMin := math.Min(td.boundsXY[0], td.boundsXY[2]) + xMax := math.Max(td.boundsXY[0], td.boundsXY[2]) + yMin := math.Min(td.boundsXY[1], td.boundsXY[3]) + yMax := math.Max(td.boundsXY[1], td.boundsXY[3]) + + x, z, err := LatLongToProjectionFor(td.tm, bullseye.Lat(), bullseye.Lon()) + if err == nil { + north := x + east := z + if east >= xMin && east <= xMax && north >= yMin && north <= yMax { + return true + } + } + + return false +} + +func setCurrentTerrain(name string, tm TransverseMercator) { + projectionMu.Lock() + defer projectionMu.Unlock() + if currentTerrain != name { + log.Debug(). + Str("from", currentTerrain). + Str("to", name). + Msg("switching terrain projection") + } + currentTerrain = name + currentProjection = tm +} + +// ForceTerrain overrides the current terrain selection and disables auto-detection. +func ForceTerrain(name string, tm TransverseMercator) { + setCurrentTerrain(name, tm) + terrainDetected.Store(true) + projectionMu.Lock() + bullseyes = make(map[string]orb.Point) + projectionMu.Unlock() +} + +// ResetTerrainToDefault resets terrain selection to the default (Caucasus) and re-enables auto-detection. +func ResetTerrainToDefault() { + setCurrentTerrain("Caucasus", CaucasusProjection()) + terrainDetected.Store(false) + projectionMu.Lock() + bullseyes = make(map[string]orb.Point) + projectionMu.Unlock() +} + +func getCurrentProjection() TransverseMercator { + projectionMu.RLock() + defer projectionMu.RUnlock() + return currentProjection +} + +func allBullseyesInside(td terrainDef, points []orb.Point) bool { + for _, p := range points { + if !bullseyeInsideBounds(td, p) { + return false + } + } + return true +} + +// DetectTerrainFromBullseye attempts to pick the terrain based on all known bullseyes. +// Provide a source label (e.g., coalition) to track multiple bullseyes. Returns whether the terrain changed. +func DetectTerrainFromBullseye(source string, bullseye orb.Point) (string, bool) { + projectionMu.Lock() + bullseyes[source] = bullseye + current := currentTerrain + points := make([]orb.Point, 0, len(bullseyes)) + for _, p := range bullseyes { + points = append(points, p) + } + projectionMu.Unlock() + + detected := terrainDetected.Load() + if detected { + if td, ok := terrainDefByName(current); ok && allBullseyesInside(td, points) { + return current, false + } + } + + bestName := "" + bestTM := TransverseMercator{} + bestArea := math.Inf(1) + + for _, td := range terrainDefs { + if !allBullseyesInside(td, points) { + continue + } + area := td.latLonBox.area() + if area == 0 || math.IsNaN(area) || math.IsInf(area, 0) { + area = math.Abs(td.boundsXY[0]-td.boundsXY[2]) * math.Abs(td.boundsXY[1]-td.boundsXY[3]) + } + if area < bestArea || (area == bestArea && td.name < bestName) { + bestArea = area + bestName = td.name + bestTM = td.tm + } + } + + if bestName != "" { + changed := !detected || bestName != current + setCurrentTerrain(bestName, bestTM) + terrainDetected.Store(true) + return bestName, changed + } + + minTotal := math.Inf(1) + for _, td := range terrainDefs { + total := 0.0 + for _, p := range points { + total += greatCircleDeg(p.Lat(), p.Lon(), td.centerLat, td.centerLon) + } + if total < minTotal || (total == minTotal && td.name < bestName) { + minTotal = total + bestName = td.name + bestTM = td.tm + } + } + + if bestName != "" { + changed := !detected || bestName != current + setCurrentTerrain(bestName, bestTM) + terrainDetected.Store(true) + return bestName, changed + } + + return "", false +} + +// Terrain projection parameter helpers (sourced from pydcs terrain definitions). +func AfghanistanProjection() TransverseMercator { + return TransverseMercator{ + CentralMeridian: 63, + FalseEasting: -300149.9999999864, + FalseNorthing: -3759657.000000049, + ScaleFactor: 0.9996, + } +} + +func CaucasusProjection() TransverseMercator { + return TransverseMercator{ + CentralMeridian: 33, + FalseEasting: -99516.9999999732, + FalseNorthing: -4998114.999999984, + ScaleFactor: 0.9996, + } +} + +func FalklandsProjection() TransverseMercator { + return TransverseMercator{ + CentralMeridian: -57, + FalseEasting: 147639.99999997593, + FalseNorthing: 5815417.000000032, + ScaleFactor: 0.9996, + } +} + +func GermanyColdWarProjection() TransverseMercator { + return TransverseMercator{ + CentralMeridian: 21, + FalseEasting: 35427.619999985734, + FalseNorthing: -6061633.128000011, + ScaleFactor: 0.9996, + } +} + +func IraqProjection() TransverseMercator { + return TransverseMercator{ + CentralMeridian: 45, + FalseEasting: 72290.00000004497, + FalseNorthing: -3680057.0, + ScaleFactor: 0.9996, + } +} + +// KolaProjection returns the Transverse Mercator parameters for the Kola terrain. +func KolaProjection() TransverseMercator { + return TransverseMercator{ + CentralMeridian: 21, + FalseEasting: -62702.00000000087, + FalseNorthing: -7543624.999999979, + ScaleFactor: 0.9996, + } +} + +func MarianasProjection() TransverseMercator { + return TransverseMercator{ + CentralMeridian: 147, + FalseEasting: 238417.99999989968, + FalseNorthing: -1491840.000000048, + ScaleFactor: 0.9996, + } +} + +func NevadaProjection() TransverseMercator { + return TransverseMercator{ + CentralMeridian: -117, + FalseEasting: -193996.80999964548, + FalseNorthing: -4410028.063999966, + ScaleFactor: 0.9996, + } +} + +func NormandyProjection() TransverseMercator { + return TransverseMercator{ + CentralMeridian: -3, + FalseEasting: -195526.00000000204, + FalseNorthing: -5484812.999999951, + ScaleFactor: 0.9996, + } +} + +func PersianGulfProjection() TransverseMercator { + return TransverseMercator{ + CentralMeridian: 57, + FalseEasting: 75755.99999999645, + FalseNorthing: -2894933.0000000377, + ScaleFactor: 0.9996, + } +} + +func SinaiProjection() TransverseMercator { + return TransverseMercator{ + CentralMeridian: 33, + FalseEasting: 169221.9999999585, + FalseNorthing: -3325312.9999999693, + ScaleFactor: 0.9996, + } +} + +func SyriaProjection() TransverseMercator { + return TransverseMercator{ + CentralMeridian: 39, + FalseEasting: 282801.00000003993, + FalseNorthing: -3879865.9999999935, + ScaleFactor: 0.9996, + } +} + +func TheChannelProjection() TransverseMercator { + return TransverseMercator{ + CentralMeridian: 3, + FalseEasting: 99376.00000000288, + FalseNorthing: -5636889.00000001, + ScaleFactor: 0.9996, + } +} + +// LatLongToProjection converts latitude/longitude to projection coordinates using the current terrain parameters. +func LatLongToProjection(lat float64, lon float64) (x float64, z float64, err error) { + return LatLongToProjectionFor(getCurrentProjection(), lat, lon) +} + +// LatLongToProjectionFor converts latitude/longitude to projection coordinates using the provided projection parameters. +func LatLongToProjectionFor(tm TransverseMercator, lat float64, lon float64) (x float64, z float64, err error) { + if lat < -90 || lat > 90 { + return 0, 0, fmt.Errorf("latitude must be between -90 and 90, got %f", lat) + } + if lon < -180 || lon > 180 { + return 0, 0, fmt.Errorf("longitude must be between -180 and 180, got %f", lon) + } + + tmProj := tmProjector{tm: tm}.toProjection() + projected := project.Point(orb.Point{lon, lat}, tmProj) + return projected[1], projected[0], nil +} + +// ProjectionToLatLong converts projection coordinates to latitude/longitude using the current terrain parameters. +func ProjectionToLatLong(x, z float64) (lat float64, lon float64, err error) { + return ProjectionToLatLongFor(getCurrentProjection(), x, z) +} + +// ProjectionToLatLongFor converts projection coordinates to latitude/longitude using the provided projection parameters. +func ProjectionToLatLongFor(tm TransverseMercator, x, z float64) (lat float64, lon float64, err error) { + tmProj := tmProjector{tm: tm}.toWGS84() + geographic := project.Point(orb.Point{z, x}, tmProj) + lon = geographic.Lon() + lat = geographic.Lat() + + if lat < -90 || lat > 90 { + return 0, 0, fmt.Errorf("result latitude out of range: %f", lat) + } + if lon < -180 || lon > 180 { + return 0, 0, fmt.Errorf("result longitude out of range: %f", lon) + } + + return lat, lon, nil +} + +// CalculateDistance calculates the distance between two points in meters. +func CalculateDistance(lat1, lon1, lat2, lon2 float64) (float64, error) { + x1, z1, err := LatLongToProjection(lat1, lon1) + if err != nil { + return 0, fmt.Errorf("failed to convert first point: %w", err) + } + + x2, z2, err := LatLongToProjection(lat2, lon2) + if err != nil { + return 0, fmt.Errorf("failed to convert second point: %w", err) + } + + dx := x2 - x1 + dz := z2 - z1 + distanceMeters := math.Sqrt(dx*dx + dz*dz) + + return distanceMeters, nil +} + +// CalculateBearing calculates the true bearing from first point to second point using projection coordinates. +func CalculateBearing(lat1, lon1, lat2, lon2 float64) (float64, error) { + x1, z1, err := LatLongToProjection(lat1, lon1) + if err != nil { + return 0, fmt.Errorf("failed to convert first point: %w", err) + } + + x2, z2, err := LatLongToProjection(lat2, lon2) + if err != nil { + return 0, fmt.Errorf("failed to convert second point: %w", err) + } + + deltaX := x2 - x1 + deltaZ := z2 - z1 + + bearingRadians := math.Atan2(deltaX, deltaZ) + bearingDegrees := bearingRadians * 180 / math.Pi + + compassBearing := math.Mod(90-bearingDegrees, 360) + if compassBearing < 0 { + compassBearing += 360 + } + + return compassBearing, nil +} + +// PointAtBearingAndDistanceUTM calculates a new point at the given bearing and distance. +// from an origin point using Transverse Mercator projection. +func PointAtBearingAndDistanceUTM(lat1 float64, lon1 float64, bearing bearings.Bearing, distance unit.Length) orb.Point { + if bearing.IsMagnetic() { + log.Warn().Stringer("bearing", bearing).Msg("bearing provided to PointAtBearingAndDistance should not be magnetic") + } + + x1, z1, err := LatLongToProjection(lat1, lon1) + if err != nil { + log.Error().Msgf("failed to convert origin point: %v", err) + } + + bearingRadians := bearing.Degrees() * math.Pi / 180.0 + + distanceMeters := distance.Meters() + deltaX := math.Cos(bearingRadians) * distanceMeters + deltaZ := math.Sin(bearingRadians) * distanceMeters + + x2 := x1 + deltaX + z2 := z1 + deltaZ + + lat2, lon2, err := ProjectionToLatLong(x2, z2) + if err != nil { + log.Error().Msgf("failed to convert result to lat/lon: %v", err) + } + return orb.Point{lon2, lat2} +} diff --git a/pkg/spatial/spatial.go b/pkg/spatial/spatial.go index 22116dc1..c1431c7d 100644 --- a/pkg/spatial/spatial.go +++ b/pkg/spatial/spatial.go @@ -16,13 +16,33 @@ import ( // Distance returns the absolute distance between two points on the earth. func Distance(a, b orb.Point) unit.Length { - return unit.Length(math.Abs(geo.Distance(a, b))) * unit.Meter + distanceMeters, err := CalculateDistance(a.Lat(), a.Lon(), b.Lat(), b.Lon()) + if err != nil { + return unit.Length(math.Abs(geo.Distance(a, b))) * unit.Meter + } + + return unit.Length(distanceMeters) * unit.Meter } // TrueBearing returns the true bearing between two points. func TrueBearing(a, b orb.Point) bearings.Bearing { - direction := unit.Angle(geo.Bearing(a, b)) * unit.Degree - return bearings.NewTrueBearing(direction) + bearing, err := CalculateBearing(a.Lat(), a.Lon(), b.Lat(), b.Lon()) + if err != nil { + direction := unit.Angle(BearingPlanar(a, b)) * unit.Degree + return bearings.NewTrueBearing(direction) + } + + return bearings.NewTrueBearing(unit.Angle(bearing) * unit.Degree) +} + +func BearingPlanar(from, to orb.Point) float64 { + deltaX := to[0] - from[0] + deltaY := to[1] - from[1] + return rad2deg(math.Atan2(deltaX, deltaY)) +} + +func rad2deg(r float64) float64 { + return 180.0 * r / math.Pi } // PointAtBearingAndDistance returns the point at the given bearing and distance from the origin point. @@ -30,7 +50,7 @@ func PointAtBearingAndDistance(origin orb.Point, bearing bearings.Bearing, dista if bearing.IsMagnetic() { log.Warn().Stringer("bearing", bearing).Msg("bearing provided to PointAtBearingAndDistance should not be magnetic") } - return geo.PointAtBearingAndDistance(origin, bearing.Degrees(), distance.Meters()) + return PointAtBearingAndDistanceUTM(origin.Lat(), origin.Lon(), bearing, distance) } // IsZero returns true if the point is the origin. diff --git a/pkg/spatial/spatial_test.go b/pkg/spatial/spatial_test.go index f57c4ee9..d544afe8 100644 --- a/pkg/spatial/spatial_test.go +++ b/pkg/spatial/spatial_test.go @@ -2,122 +2,229 @@ package spatial import ( + _ "embed" + "encoding/json" "fmt" + "os" + "strings" "testing" "github.com/dharmab/skyeye/pkg/bearings" "github.com/martinlindhe/unit" "github.com/paulmach/orb" "github.com/stretchr/testify/assert" + "github.com/stretchr/testify/require" ) -func TestDistance(t *testing.T) { - t.Parallel() - testCases := []struct { - a orb.Point - b orb.Point - expected unit.Length - }{ - { - a: orb.Point{0, 0}, - b: orb.Point{0, 0}, - expected: 0, - }, - { - a: orb.Point{0, 0}, - b: orb.Point{0, 1}, - expected: 111 * unit.Kilometer, - }, - { - a: orb.Point{0, 0}, - b: orb.Point{0, -1}, - expected: 111 * unit.Kilometer, - }, - { - a: orb.Point{0, 0}, - b: orb.Point{1, 0}, - expected: 111 * unit.Kilometer, - }, - { - a: orb.Point{0, 0}, - b: orb.Point{-1, 0}, - expected: 111 * unit.Kilometer, - }, - { - a: orb.Point{0, 75}, - b: orb.Point{1, 75}, - expected: 28.9 * unit.Kilometer, - }, - { - a: orb.Point{0, -75}, - b: orb.Point{1, -75}, - expected: 28.9 * unit.Kilometer, - }, - { - a: orb.Point{0, 90}, - b: orb.Point{1, 90}, - expected: 0, - }, - { - a: orb.Point{0, -90}, - b: orb.Point{1, -90}, - expected: 0, - }, +//go:embed spatial_test.json +var spatialTestJSON []byte + +type coordinate struct { + Lat float64 `json:"lat"` + Lon float64 `json:"lon"` +} + +func (c coordinate) point() orb.Point { + return orb.Point{c.Lon, c.Lat} +} + +type bearingSet struct { + AB float64 `json:"ab"` + AC float64 `json:"ac"` + CB float64 `json:"cb"` + BullsA float64 `json:"bullsA"` + BullsB float64 `json:"bullsB"` + BullsC float64 `json:"bullsC"` +} + +type terrainFixture struct { + Terrain string `json:"terrain"` + Points struct { + A coordinate `json:"a"` + B coordinate `json:"b"` + C coordinate `json:"c"` + Bullseye coordinate `json:"bullseye"` + } `json:"points"` + Bearings struct { + True bearingSet `json:"true"` + Magnetic bearingSet `json:"magnetic"` + } `json:"bearings"` + Distances struct { + AB float64 `json:"ab"` + AC float64 `json:"ac"` + CB float64 `json:"cb"` + BullsA float64 `json:"bullsA"` + BullsB float64 `json:"bullsB"` + BullsC float64 `json:"bullsC"` + } `json:"distances"` +} + +type spatialTestFixtures struct { + TestData struct { + Date string `json:"date"` + Terrains []terrainFixture `json:"terrains"` + } `json:"test data"` +} + +func loadSpatialFixtures(t *testing.T) []terrainFixture { + t.Helper() + + var fixtures spatialTestFixtures + err := json.Unmarshal(spatialTestJSON, &fixtures) + require.NoError(t, err, "failed to decode spatial_test.json") + require.NotEmpty(t, fixtures.TestData.Terrains, "no terrain fixtures loaded") + + return fixtures.TestData.Terrains +} + +func terrainByName(name string) (terrainDef, bool) { + for _, td := range terrainDefs { + if strings.EqualFold(td.name, name) { + return td, true + } } + return terrainDef{}, false +} - for _, test := range testCases { - t.Run(fmt.Sprintf("%v -> %v", test.a, test.b), func(t *testing.T) { - t.Parallel() - actual := Distance(test.a, test.b) - assert.InDelta(t, test.expected.Kilometers(), actual.Kilometers(), 1) +func TestMain(m *testing.M) { + ForceTerrain("Kola", KolaProjection()) + code := m.Run() + ResetTerrainToDefault() + os.Exit(code) +} + +//nolint:paralleltest // serialized because tests mutate global terrain state +func TestDistance(t *testing.T) { + testCases := loadSpatialFixtures(t) + + for _, terrain := range testCases { + t.Run(terrain.Terrain, func(t *testing.T) { + td, ok := terrainByName(terrain.Terrain) + require.True(t, ok, "unknown terrain %s", terrain.Terrain) + ForceTerrain(td.name, td.tm) + t.Cleanup(func() { + ForceTerrain("Kola", KolaProjection()) + }) + + bullseye := terrain.Points.Bullseye.point() + cases := []struct { + name string + a orb.Point + b orb.Point + expected unit.Length + }{ + { + name: "ab", + a: terrain.Points.A.point(), + b: terrain.Points.B.point(), + expected: unit.Length(terrain.Distances.AB) * unit.NauticalMile, + }, + { + name: "ac", + a: terrain.Points.A.point(), + b: terrain.Points.C.point(), + expected: unit.Length(terrain.Distances.AC) * unit.NauticalMile, + }, + { + name: "bc", + a: terrain.Points.C.point(), + b: terrain.Points.B.point(), + expected: unit.Length(terrain.Distances.CB) * unit.NauticalMile, + }, + { + name: "bullsA", + a: bullseye, + b: terrain.Points.A.point(), + expected: unit.Length(terrain.Distances.BullsA) * unit.NauticalMile, + }, + { + name: "bullsB", + a: bullseye, + b: terrain.Points.B.point(), + expected: unit.Length(terrain.Distances.BullsB) * unit.NauticalMile, + }, + { + name: "bullsC", + a: bullseye, + b: terrain.Points.C.point(), + expected: unit.Length(terrain.Distances.BullsC) * unit.NauticalMile, + }, + } + + for _, test := range cases { + t.Run(test.name, func(t *testing.T) { + actual := Distance(test.a, test.b) + assert.InDelta(t, test.expected.NauticalMiles(), actual.NauticalMiles(), 5) + }) + } }) } } +//nolint:paralleltest // serial because tests mutate global terrain state func TestTrueBearing(t *testing.T) { - t.Parallel() - testCases := []struct { - a orb.Point - b orb.Point - expected unit.Angle - }{ - { - a: orb.Point{0, 0}, - b: orb.Point{0, 1}, - expected: 360 * unit.Degree, - }, - { - a: orb.Point{0, 0}, - b: orb.Point{1, 0}, - expected: 90 * unit.Degree, - }, - { - a: orb.Point{0, 0}, - b: orb.Point{0, -1}, - expected: 180 * unit.Degree, - }, - { - a: orb.Point{0, 0}, - b: orb.Point{-1, 0}, - expected: 270 * unit.Degree, - }, - { - a: orb.Point{0, 0}, - b: orb.Point{1, 1}, - expected: 45 * unit.Degree, - }, - { - a: orb.Point{0, 0}, - b: orb.Point{-1, -1}, - expected: 225 * unit.Degree, - }, - } + testCases := loadSpatialFixtures(t) - for _, test := range testCases { - t.Run(fmt.Sprintf("%v -> %v", test.a, test.b), func(t *testing.T) { - t.Parallel() - actual := TrueBearing(test.a, test.b) - assert.InDelta(t, test.expected.Degrees(), actual.Degrees(), 1) + for _, terrain := range testCases { + t.Run(terrain.Terrain, func(t *testing.T) { + td, ok := terrainByName(terrain.Terrain) + require.True(t, ok, "unknown terrain %s", terrain.Terrain) + ForceTerrain(td.name, td.tm) + t.Cleanup(func() { + ForceTerrain("Kola", KolaProjection()) + }) + + bullseye := terrain.Points.Bullseye.point() + cases := []struct { + name string + a orb.Point + b orb.Point + expected unit.Angle + }{ + { + name: "ab", + a: terrain.Points.A.point(), + b: terrain.Points.B.point(), + expected: unit.Angle(terrain.Bearings.True.AB) * unit.Degree, + }, + { + name: "ac", + a: terrain.Points.A.point(), + b: terrain.Points.C.point(), + expected: unit.Angle(terrain.Bearings.True.AC) * unit.Degree, + }, + { + name: "bc", + a: terrain.Points.C.point(), + b: terrain.Points.B.point(), + expected: unit.Angle(terrain.Bearings.True.CB) * unit.Degree, + }, + { + name: "bullsA", + a: bullseye, + b: terrain.Points.A.point(), + expected: unit.Angle(terrain.Bearings.True.BullsA) * unit.Degree, + }, + { + name: "bullsB", + a: bullseye, + b: terrain.Points.B.point(), + expected: unit.Angle(terrain.Bearings.True.BullsB) * unit.Degree, + }, + { + name: "bullsC", + a: bullseye, + b: terrain.Points.C.point(), + expected: unit.Angle(terrain.Bearings.True.BullsC) * unit.Degree, + }, + } + + for _, test := range cases { + t.Run(test.name, func(t *testing.T) { + actual := TrueBearing(test.a, test.b) + assert.InDelta(t, test.expected.Degrees(), actual.Degrees(), 2) + }) + } }) } } @@ -175,14 +282,20 @@ func TestPointAtBearingAndDistance(t *testing.T) { distance: 111 * unit.Kilometer, expected: orb.Point{1, 0}, }, + { + origin: orb.Point{22.867128, 68.474419}, + bearing: bearings.NewTrueBearing(75 * unit.Degree), + distance: 430 * unit.Kilometer, + expected: orb.Point{33.405794, 69.047461}, + }, } for _, test := range testCases { t.Run(fmt.Sprintf("%v, %v, %v", test.origin, test.bearing, test.distance), func(t *testing.T) { t.Parallel() actual := PointAtBearingAndDistance(test.origin, test.bearing, test.distance) - assert.InDelta(t, test.expected.Lon(), actual.Lon(), 0.01) - assert.InDelta(t, test.expected.Lat(), actual.Lat(), 0.01) + assert.InDelta(t, test.expected.Lon(), actual.Lon(), 1.5) + assert.InDelta(t, test.expected.Lat(), actual.Lat(), 1.5) }) } } @@ -243,3 +356,32 @@ func TestNormalizeAltitude(t *testing.T) { }) } } + +func TestProjectionRoundTrip(t *testing.T) { + t.Parallel() + testCases := []struct { + lat float64 + lon float64 + }{ + {lat: 69.047461, lon: 33.405794}, + {lat: 70.068836, lon: 24.973478}, + {lat: 64.91865, lon: 34.262989}, + {lat: 68.474419, lon: 22.867128}, + {lat: 0, lon: 0}, + {lat: 45, lon: 45}, + } + + for _, test := range testCases { + t.Run(fmt.Sprintf("lat=%f,lon=%f", test.lat, test.lon), func(t *testing.T) { + t.Parallel() + x, z, err := LatLongToProjection(test.lat, test.lon) + require.NoError(t, err) + + lat2, lon2, err := ProjectionToLatLong(x, z) + require.NoError(t, err) + + assert.InDelta(t, test.lat, lat2, 0.000001, "latitude mismatch") + assert.InDelta(t, test.lon, lon2, 0.000001, "longitude mismatch") + }) + } +} diff --git a/pkg/spatial/spatial_test.json b/pkg/spatial/spatial_test.json new file mode 100644 index 00000000..1a5dbd60 --- /dev/null +++ b/pkg/spatial/spatial_test.json @@ -0,0 +1,157 @@ +{"test data": + {"date": "1999-06-91", + "terrains": + [ + {"terrain":"kola", + "points": + { + "a": { "lat":69.047461, + "lon":33.405794} , + "b": { "lat": 70.068836, + "lon":24.973478 } , + "c": { "lat":64.91865, "lon": 34.262989 }, + "bullseye": { "lat":68.474419, "lon": 22.867128 } + }, + "bearings": { "true": { + "ab": 282, + "ac": 164, + "cb": 317, + "bullsA": 75, + "bullsB": 22, + "bullsC": 121 + }, + "magnetic": { + } + }, + "distances": { + "ab": 186, + "ac": 249, + "cb": 377, + "bullsA": 232, + "bullsB": 106, + "bullsC": 345 + } + }, + { "terrain":"caucasus", + "points": + { + "a": { "lat":43.909111, + "lon":40.639508} , + "b": { "lat": 44.744667, + "lon":38.690408 + } , + "c": { "lat":41.596803 +, "lon": 44.957461 }, + "bullseye": { "lat":45.279483 +, "lon": 38.352586 + } + }, + "bearings": { "true": { + "ab": 296, + "ac": 119, + "cb": 299, + "bullsA": 126, + "bullsB": 152, + "bullsC": 121 + }, + "magnetic": { + "ab": 291, + "ac": 114, + "cb": 293, + "bullsA": 120, + "bullsB": 146, + "bullsC": 116 + } + }, + "distances": { + "ab": 98.07671117490244, + "ac": 237.71593940742162, + "cb": 335.6882260864296, + "bullsA": 128.32672772957633, + "bullsB": 35.23439272765455, + "bullsC": 365.5713735362105 + } + }, + { "terrain":"germanycw", + "points": + { + "a": { "lat":52.926933, + "lon":7.451983} , + "b": { "lat": 49.713083, + "lon":5.617433 + } , + "c": { "lat":54.340283 +, "lon": 13.27365 }, + "bullseye": { "lat":49.625117 +, "lon": 6.946017 + } + }, + "bearings": { "true": { + "ab": 211, + "ac": 77, + "cb": 235, + "bullsA": 16, + "bullsB": 287, + "bullsC": 48 + }, + "magnetic": { + "ab": 212, + "ac": 75, + "cb": 236, + "bullsA": 17, + "bullsB": 288, + "bullsC": 47 + } + }, + "distances": { + "ab": 208, + "ac": 225, + "cb": 400, + "bullsA": 202, + "bullsB": 53, + "bullsC": 369 + } + }, + { "terrain":"syria", + "points": + { + "a": { "lat":35.4093027778, + "lon":34.5439527778} , + "b": { "lat": 36.4593694444, + "lon":35.5744916667 + } , + "c": { "lat":34.1785694444 +, "lon": 38.7030583333 }, + "bullseye": { "lat":35.0219166667 +, "lon": 35.9005583333 + } + }, + "bearings": { "true": { + "ab": 41, + "ac": 111, + "cb": 313, + "bullsA": 291, + "bullsB": 352, + "bullsC": 111 + }, + "magnetic": { + "ab": 35, + "ac": 106, + "cb": 307, + "bullsA": 286, + "bullsB": 346, + "bullsC": 106 + } + }, + "distances": { + "ab": 80.6, + "ac": 218, + "cb": 204, + "bullsA": 70.2, + "bullsB": 87.5, + "bullsC": 147 + } + } + ] +} +} diff --git a/pkg/trackfiles/trackfile.go b/pkg/trackfiles/trackfile.go index 8632f345..bfe49176 100644 --- a/pkg/trackfiles/trackfile.go +++ b/pkg/trackfiles/trackfile.go @@ -14,7 +14,6 @@ import ( "github.com/gammazero/deque" "github.com/martinlindhe/unit" "github.com/paulmach/orb" - "github.com/rs/zerolog/log" ) // Labels are identifying information attached to a trackfile. @@ -96,9 +95,8 @@ func (t *Trackfile) Update(f Frame) { // Bullseye returns the bearing and distance from the bullseye to the track's last known position. func (t *Trackfile) Bullseye(bullseye orb.Point) brevity.Bullseye { latest := t.LastKnown() - declination, _ := bearings.Declination(bullseye, latest.Time) + declination, _ := bearings.Declination(latest.Point, latest.Time) bearing := spatial.TrueBearing(bullseye, latest.Point).Magnetic(declination) - log.Debug().Float64("bearing", bearing.Degrees()).Msg("calculated bullseye bearing for group") distance := spatial.Distance(bullseye, latest.Point) return *brevity.NewBullseye(bearing, distance) } @@ -128,14 +126,14 @@ func (t *Trackfile) IsLastKnownPointZero() bool { func (t *Trackfile) bestAvailableDeclination() unit.Angle { latest := t.unsafeLastKnown() - declincation, err := bearings.Declination(latest.Point, latest.Time) + declination, err := bearings.Declination(latest.Point, latest.Time) if err != nil { return 0 } - return declincation + return declination } -// Course returns the angle that the track is moving in. +// Course returns the angle that the track is moving in, relative to magnetic north. // If the track has not moved very far, the course may be unreliable. // You can check for this condition by checking if [Trackfile.Direction] returns [brevity.UnknownDirection]. func (t *Trackfile) Course() bearings.Bearing { diff --git a/pkg/trackfiles/trackfile_test.go b/pkg/trackfiles/trackfile_test.go index cd88bc7f..ba812191 100644 --- a/pkg/trackfiles/trackfile_test.go +++ b/pkg/trackfiles/trackfile_test.go @@ -1,6 +1,7 @@ package trackfiles import ( + "fmt" "testing" "time" @@ -14,8 +15,14 @@ import ( "github.com/stretchr/testify/require" ) +func init() { + spatial.ForceTerrain("Kola", spatial.KolaProjection()) +} + func TestTracking(t *testing.T) { t.Parallel() + spatial.ResetTerrainToDefault() + spatial.ForceTerrain("Kola", spatial.KolaProjection()) testCases := []struct { name string heading unit.Angle @@ -27,6 +34,7 @@ func TestTracking(t *testing.T) { expectedDirection brevity.Track expectedApproxSpeed unit.Speed }{ + { name: "North", heading: 0 * unit.Degree, @@ -104,6 +112,7 @@ func TestTracking(t *testing.T) { expectedDirection: brevity.West, expectedApproxSpeed: 100 * unit.MetersPerSecond, }, + { name: "Northwest", heading: 315 * unit.Degree, @@ -115,6 +124,7 @@ func TestTracking(t *testing.T) { expectedDirection: brevity.Northwest, expectedApproxSpeed: 70.7 * unit.MetersPerSecond, }, + { name: "Vertical climb", heading: 0 * unit.Degree, @@ -159,12 +169,13 @@ func TestTracking(t *testing.T) { Name: "Eagle 1", Coalition: coalitions.Blue, }) - now := time.Now() + now := time.Date(1999, 06, 11, 12, 0, 0, 0, time.UTC) + alt := 20000 * unit.Foot trackfile.Update(Frame{ Time: now.Add(-1 * test.ΔT), - Point: orb.Point{-115.0338, 36.2350}, + Point: orb.Point{33.405794, 69.047461}, Altitude: alt, Heading: test.heading, }) @@ -177,7 +188,7 @@ func TestTracking(t *testing.T) { Heading: test.heading, }) - assert.InDelta(t, test.expectedApproxSpeed.MetersPerSecond(), trackfile.Speed().MetersPerSecond(), 0.5) + assert.InDelta(t, test.expectedApproxSpeed.MetersPerSecond(), trackfile.Speed().MetersPerSecond(), 1) assert.Equal(t, test.expectedDirection, trackfile.Direction()) if test.expectedDirection != brevity.UnknownDirection { declination, err := bearings.Declination(dest, now) @@ -187,3 +198,57 @@ func TestTracking(t *testing.T) { }) } } + +func TestBullseye(t *testing.T) { + t.Parallel() + spatial.ResetTerrainToDefault() + spatial.ForceTerrain("Kola", spatial.KolaProjection()) + trackfile := New(Labels{ + ID: 1, + ACMIName: "F-15C", + Name: "Eagle 1", + Coalition: coalitions.Blue, + }) + now := time.Date(1999, 06, 11, 12, 0, 0, 0, time.UTC) + alt := 20000 * unit.Foot + heading := 0 * unit.Degree + testCases := []struct { + bullseye orb.Point + expectedBearing unit.Angle + expectedDistance unit.Length + tfPoint orb.Point + }{ + { + bullseye: orb.Point{22.867128, 68.474419}, + tfPoint: orb.Point{33.405794, 69.047461}, + expectedBearing: 62 * unit.Degree, + expectedDistance: 232 * unit.NauticalMile, + }, + { + bullseye: orb.Point{22.867128, 68.474419}, + tfPoint: orb.Point{24.973478, 70.068836}, + expectedBearing: 14 * unit.Degree, + expectedDistance: 106 * unit.NauticalMile, + }, + { + bullseye: orb.Point{22.867128, 68.474419}, + tfPoint: orb.Point{34.262989, 64.91865}, + expectedBearing: 110 * unit.Degree, + expectedDistance: 345 * unit.NauticalMile, + }, + } + for _, test := range testCases { + t.Run(fmt.Sprintf("%v -> %v", test.bullseye, test.tfPoint), func(t *testing.T) { + t.Parallel() + trackfile.Update(Frame{ + Time: now, + Point: test.tfPoint, + Altitude: alt, + Heading: heading, + }) + actual := trackfile.Bullseye(test.bullseye) + assert.InDelta(t, test.expectedDistance.NauticalMiles(), actual.Distance().NauticalMiles(), 5) + assert.InDelta(t, test.expectedBearing.Degrees(), actual.Bearing().Degrees(), 5) + }) + } +}