diff options
Diffstat (limited to 'android-app')
6 files changed, 462 insertions, 0 deletions
diff --git a/android-app/app/src/main/kotlin/com/example/androidapp/routing/IsochroneResult.kt b/android-app/app/src/main/kotlin/com/example/androidapp/routing/IsochroneResult.kt new file mode 100644 index 0000000..60a5918 --- /dev/null +++ b/android-app/app/src/main/kotlin/com/example/androidapp/routing/IsochroneResult.kt @@ -0,0 +1,12 @@ +package com.example.androidapp.routing + +/** + * The result of an isochrone weather routing computation. + * + * @param path Ordered list of [RoutePoint]s from the start to the destination. + * @param etaMs Estimated Time of Arrival as a UNIX timestamp in milliseconds. + */ +data class IsochroneResult( + val path: List<RoutePoint>, + val etaMs: Long +) diff --git a/android-app/app/src/main/kotlin/com/example/androidapp/routing/IsochroneRouter.kt b/android-app/app/src/main/kotlin/com/example/androidapp/routing/IsochroneRouter.kt new file mode 100644 index 0000000..25055a8 --- /dev/null +++ b/android-app/app/src/main/kotlin/com/example/androidapp/routing/IsochroneRouter.kt @@ -0,0 +1,178 @@ +package com.example.androidapp.routing + +import com.example.androidapp.data.model.BoatPolars +import com.example.androidapp.data.model.WindForecast +import kotlin.math.asin +import kotlin.math.atan2 +import kotlin.math.cos +import kotlin.math.pow +import kotlin.math.sin +import kotlin.math.sqrt + +/** + * Isochrone-based weather routing engine (Section 3.4). + * + * Algorithm: + * 1. Start from a single point; expand a fan of headings at each time step. + * 2. For each candidate heading, compute BSP from [BoatPolars] at the local forecast wind. + * 3. Advance position by BSP × Δt using the spherical-Earth destination-point formula. + * 4. Check whether the destination has been reached (within [arrivalRadiusM]). + * 5. Prune candidates: for each angular sector around the start, keep only the point that + * advanced furthest (removes dominated points). + * 6. Repeat until the destination is reached or [maxSteps] is exhausted. + * 7. Backtrace parent pointers to produce the optimal path. + */ +object IsochroneRouter { + + private const val EARTH_RADIUS_M = 6_371_000.0 + internal const val NM_TO_M = 1_852.0 + private const val KT_TO_M_PER_S = NM_TO_M / 3600.0 + + const val DEFAULT_HEADING_STEP_DEG = 5.0 + const val DEFAULT_ARRIVAL_RADIUS_M = 1_852.0 // 1 NM + const val DEFAULT_PRUNE_SECTORS = 72 // 5° sectors + const val DEFAULT_MAX_STEPS = 200 + + /** + * Compute an optimised route from start to destination. + * + * @param startLat Start latitude (decimal degrees). + * @param startLon Start longitude (decimal degrees). + * @param destLat Destination latitude (decimal degrees). + * @param destLon Destination longitude (decimal degrees). + * @param startTimeMs Departure time as UNIX timestamp (ms). + * @param stepMs Time increment per isochrone step (ms). Typical: 1–3 hours. + * @param polars Boat polar table. + * @param windAt Function returning [WindForecast] for a given position and time. + * @param headingStepDeg Angular resolution of the heading fan (degrees). Default 5°. + * @param arrivalRadiusM Distance threshold to consider destination reached (metres). + * @param maxSteps Maximum number of isochrone expansions before giving up. + * @return [IsochroneResult] with the optimal path and ETA, or null if unreachable. + */ + fun route( + startLat: Double, + startLon: Double, + destLat: Double, + destLon: Double, + startTimeMs: Long, + stepMs: Long, + polars: BoatPolars, + windAt: (lat: Double, lon: Double, timeMs: Long) -> WindForecast, + headingStepDeg: Double = DEFAULT_HEADING_STEP_DEG, + arrivalRadiusM: Double = DEFAULT_ARRIVAL_RADIUS_M, + maxSteps: Int = DEFAULT_MAX_STEPS + ): IsochroneResult? { + val start = RoutePoint(startLat, startLon, startTimeMs) + var isochrone = listOf(start) + + repeat(maxSteps) { step -> + val nextTimeMs = startTimeMs + (step + 1).toLong() * stepMs + val candidates = mutableListOf<RoutePoint>() + + for (point in isochrone) { + var heading = 0.0 + while (heading < 360.0) { + val wind = windAt(point.lat, point.lon, point.timestampMs) + val twa = ((heading - wind.twdDeg + 360.0) % 360.0) + val bspKt = polars.bsp(twa, wind.twsKt) + if (bspKt > 0.0) { + val distM = bspKt * KT_TO_M_PER_S * (stepMs / 1000.0) + val (newLat, newLon) = destinationPoint(point.lat, point.lon, heading, distM) + val newPoint = RoutePoint(newLat, newLon, nextTimeMs, parent = point) + + if (haversineM(newLat, newLon, destLat, destLon) <= arrivalRadiusM) { + return IsochroneResult( + path = backtrace(newPoint), + etaMs = nextTimeMs + ) + } + candidates.add(newPoint) + } + heading += headingStepDeg + } + } + + if (candidates.isEmpty()) return null + isochrone = prune(candidates, startLat, startLon, DEFAULT_PRUNE_SECTORS) + } + + return null + } + + /** Walk parent pointers from destination back to start, then reverse. */ + internal fun backtrace(dest: RoutePoint): List<RoutePoint> { + val path = mutableListOf<RoutePoint>() + var current: RoutePoint? = dest + while (current != null) { + path.add(current) + current = current.parent + } + path.reverse() + return path + } + + /** + * Angular-sector pruning: divide the plane into [sectors] equal angular sectors around the + * start. Within each sector keep only the candidate that is furthest from the start. + */ + internal fun prune( + candidates: List<RoutePoint>, + startLat: Double, + startLon: Double, + sectors: Int + ): List<RoutePoint> { + val sectorSize = 360.0 / sectors + val best = mutableMapOf<Int, RoutePoint>() + + for (point in candidates) { + val bearing = bearingDeg(startLat, startLon, point.lat, point.lon) + val sector = (bearing / sectorSize).toInt().coerceIn(0, sectors - 1) + val existing = best[sector] + if (existing == null || + haversineM(startLat, startLon, point.lat, point.lon) > + haversineM(startLat, startLon, existing.lat, existing.lon) + ) { + best[sector] = point + } + } + + return best.values.toList() + } + + /** Haversine great-circle distance in metres. */ + internal fun haversineM(lat1: Double, lon1: Double, lat2: Double, lon2: Double): Double { + val dLat = Math.toRadians(lat2 - lat1) + val dLon = Math.toRadians(lon2 - lon1) + val a = sin(dLat / 2).pow(2) + + cos(Math.toRadians(lat1)) * cos(Math.toRadians(lat2)) * sin(dLon / 2).pow(2) + return 2.0 * EARTH_RADIUS_M * asin(sqrt(a)) + } + + /** Initial bearing from point 1 to point 2 (degrees, 0 = North, clockwise). */ + internal fun bearingDeg(lat1Deg: Double, lon1Deg: Double, lat2Deg: Double, lon2Deg: Double): Double { + val lat1 = Math.toRadians(lat1Deg) + val lat2 = Math.toRadians(lat2Deg) + val dLon = Math.toRadians(lon2Deg - lon1Deg) + val y = sin(dLon) * cos(lat2) + val x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon) + return (Math.toDegrees(atan2(y, x)) + 360.0) % 360.0 + } + + /** Spherical-Earth destination-point given start, bearing, and distance. */ + internal fun destinationPoint( + lat1Deg: Double, + lon1Deg: Double, + bearingDeg: Double, + distM: Double + ): Pair<Double, Double> { + val lat1 = Math.toRadians(lat1Deg) + val lon1 = Math.toRadians(lon1Deg) + val brng = Math.toRadians(bearingDeg) + val d = distM / EARTH_RADIUS_M + + val lat2 = asin(sin(lat1) * cos(d) + cos(lat1) * sin(d) * cos(brng)) + val lon2 = lon1 + atan2(sin(brng) * sin(d) * cos(lat1), cos(d) - sin(lat1) * sin(lat2)) + + return Pair(Math.toDegrees(lat2), Math.toDegrees(lon2)) + } +} diff --git a/android-app/app/src/main/kotlin/com/example/androidapp/routing/RoutePoint.kt b/android-app/app/src/main/kotlin/com/example/androidapp/routing/RoutePoint.kt new file mode 100644 index 0000000..02988d1 --- /dev/null +++ b/android-app/app/src/main/kotlin/com/example/androidapp/routing/RoutePoint.kt @@ -0,0 +1,16 @@ +package com.example.androidapp.routing + +/** + * A single point in the isochrone routing tree. + * + * @param lat Latitude (decimal degrees). + * @param lon Longitude (decimal degrees). + * @param timestampMs UNIX time in milliseconds when this position is reached. + * @param parent The previous [RoutePoint] (null for the start point). + */ +data class RoutePoint( + val lat: Double, + val lon: Double, + val timestampMs: Long, + val parent: RoutePoint? = null +) diff --git a/android-app/app/src/main/kotlin/org/terst/nav/data/model/BoatPolars.kt b/android-app/app/src/main/kotlin/org/terst/nav/data/model/BoatPolars.kt new file mode 100644 index 0000000..0286ea8 --- /dev/null +++ b/android-app/app/src/main/kotlin/org/terst/nav/data/model/BoatPolars.kt @@ -0,0 +1,69 @@ +package org.terst.nav.data.model + +import kotlin.math.pow +import kotlin.math.sqrt + +/** + * Boat polar speed table: maps (TWA, TWS) → BSP (boat speed through water, knots). + * + * Interpolation is bilinear — linear on TWA within a given TWS, then linear on TWS. + * Port-tack mirror: TWA > 180° is folded to 360° - TWA before lookup. + */ +data class BoatPolars( + /** Outer key: TWS in knots. Inner key: TWA in degrees [0, 180]. Value: BSP in knots. */ + val table: Map<Double, Map<Double, Double>> +) { + /** + * Returns boat speed (knots) for the given True Wind Angle and True Wind Speed. + * TWA outside [0, 360] is clamped; port/starboard symmetry is applied (>180° mirrored). + */ + fun bsp(twaDeg: Double, twsKt: Double): Double { + val twa = if (twaDeg > 180.0) 360.0 - twaDeg else twaDeg.coerceIn(0.0, 180.0) + + val twsKeys = table.keys.sorted() + if (twsKeys.isEmpty()) return 0.0 + + val twsClamped = twsKt.coerceIn(twsKeys.first(), twsKeys.last()) + val twsLow = twsKeys.lastOrNull { it <= twsClamped } ?: twsKeys.first() + val twsHigh = twsKeys.firstOrNull { it >= twsClamped } ?: twsKeys.last() + + val bspLow = bspAtTws(twa, table[twsLow] ?: return 0.0) + val bspHigh = bspAtTws(twa, table[twsHigh] ?: return 0.0) + + return if (twsHigh == twsLow) bspLow + else { + val t = (twsClamped - twsLow) / (twsHigh - twsLow) + bspLow + t * (bspHigh - bspLow) + } + } + + private fun bspAtTws(twaDeg: Double, twaMap: Map<Double, Double>): Double { + val twaKeys = twaMap.keys.sorted() + if (twaKeys.isEmpty()) return 0.0 + + val twaClamped = twaDeg.coerceIn(twaKeys.first(), twaKeys.last()) + val twaLow = twaKeys.lastOrNull { it <= twaClamped } ?: twaKeys.first() + val twaHigh = twaKeys.firstOrNull { it >= twaClamped } ?: twaKeys.last() + + val bspLow = twaMap[twaLow] ?: 0.0 + val bspHigh = twaMap[twaHigh] ?: 0.0 + + return if (twaHigh == twaLow) bspLow + else { + val t = (twaClamped - twaLow) / (twaHigh - twaLow) + bspLow + t * (bspHigh - bspLow) + } + } + + companion object { + /** Default polar for a typical 35-foot cruising sloop. */ + val DEFAULT: BoatPolars = BoatPolars( + mapOf( + 5.0 to mapOf(45.0 to 3.5, 60.0 to 4.2, 90.0 to 4.8, 120.0 to 5.0, 150.0 to 4.5, 180.0 to 4.0), + 10.0 to mapOf(45.0 to 5.5, 60.0 to 6.5, 90.0 to 7.0, 120.0 to 7.2, 150.0 to 6.8, 180.0 to 6.0), + 15.0 to mapOf(45.0 to 6.5, 60.0 to 7.5, 90.0 to 8.0, 120.0 to 8.5, 150.0 to 8.0, 180.0 to 7.0), + 20.0 to mapOf(45.0 to 7.0, 60.0 to 8.0, 90.0 to 8.5, 120.0 to 9.0, 150.0 to 8.5, 180.0 to 7.5) + ) + ) + } +} diff --git a/android-app/app/src/main/kotlin/org/terst/nav/data/model/WindForecast.kt b/android-app/app/src/main/kotlin/org/terst/nav/data/model/WindForecast.kt new file mode 100644 index 0000000..f009da8 --- /dev/null +++ b/android-app/app/src/main/kotlin/org/terst/nav/data/model/WindForecast.kt @@ -0,0 +1,18 @@ +package org.terst.nav.data.model + +/** + * Wind conditions at a specific location and time. + * + * @param lat Latitude (decimal degrees). + * @param lon Longitude (decimal degrees). + * @param timestampMs UNIX time in milliseconds. + * @param twsKt True Wind Speed in knots. + * @param twdDeg True Wind Direction in degrees (the direction FROM which the wind blows, 0–360). + */ +data class WindForecast( + val lat: Double, + val lon: Double, + val timestampMs: Long, + val twsKt: Double, + val twdDeg: Double +) diff --git a/android-app/app/src/test/kotlin/com/example/androidapp/routing/IsochroneRouterTest.kt b/android-app/app/src/test/kotlin/com/example/androidapp/routing/IsochroneRouterTest.kt new file mode 100644 index 0000000..e5615e9 --- /dev/null +++ b/android-app/app/src/test/kotlin/com/example/androidapp/routing/IsochroneRouterTest.kt @@ -0,0 +1,169 @@ +package com.example.androidapp.routing + +import com.example.androidapp.data.model.BoatPolars +import com.example.androidapp.data.model.WindForecast +import org.junit.Assert.* +import org.junit.Test + +class IsochroneRouterTest { + + private val startTimeMs = 1_000_000_000L + private val oneHourMs = 3_600_000L + + // ── BoatPolars ──────────────────────────────────────────────────────────── + + @Test + fun `bsp returns exact value for exact twa and tws entry`() { + val polars = BoatPolars.DEFAULT + // At TWS=10, TWA=90 the table has 7.0 kt + assertEquals(7.0, polars.bsp(90.0, 10.0), 1e-9) + } + + @Test + fun `bsp interpolates between twa entries`() { + val polars = BoatPolars.DEFAULT + // At TWS=10: TWA=60 → 6.5, TWA=90 → 7.0; midpoint TWA=75 → 6.75 + assertEquals(6.75, polars.bsp(75.0, 10.0), 1e-9) + } + + @Test + fun `bsp interpolates between tws entries`() { + val polars = BoatPolars.DEFAULT + // At TWA=90: TWS=10 → 7.0, TWS=15 → 8.0; midpoint TWS=12.5 → 7.5 + assertEquals(7.5, polars.bsp(90.0, 12.5), 1e-9) + } + + @Test + fun `bsp mirrors port tack twa to starboard`() { + val polars = BoatPolars.DEFAULT + // TWA=270 should mirror to 360-270=90, giving same as TWA=90 + assertEquals(polars.bsp(90.0, 10.0), polars.bsp(270.0, 10.0), 1e-9) + } + + @Test + fun `bsp clamps tws below table minimum`() { + val polars = BoatPolars.DEFAULT + // TWS=0 clamps to minimum TWS=5 + assertEquals(polars.bsp(90.0, 5.0), polars.bsp(90.0, 0.0), 1e-9) + } + + @Test + fun `bsp clamps tws above table maximum`() { + val polars = BoatPolars.DEFAULT + // TWS=100 clamps to maximum TWS=20 + assertEquals(polars.bsp(90.0, 20.0), polars.bsp(90.0, 100.0), 1e-9) + } + + // ── IsochroneRouter geometry helpers ───────────────────────────────────── + + @Test + fun `haversineM returns zero for same point`() { + assertEquals(0.0, IsochroneRouter.haversineM(10.0, 20.0, 10.0, 20.0), 1e-3) + } + + @Test + fun `haversineM one degree of latitude is approximately 111_195 m`() { + val dist = IsochroneRouter.haversineM(0.0, 0.0, 1.0, 0.0) + assertEquals(111_195.0, dist, 50.0) + } + + @Test + fun `bearingDeg returns 0 for due north`() { + val bearing = IsochroneRouter.bearingDeg(0.0, 0.0, 1.0, 0.0) + assertEquals(0.0, bearing, 1e-6) + } + + @Test + fun `bearingDeg returns 90 for due east`() { + val bearing = IsochroneRouter.bearingDeg(0.0, 0.0, 0.0, 1.0) + assertEquals(90.0, bearing, 1e-4) + } + + @Test + fun `destinationPoint due north by 1 NM moves latitude by expected amount`() { + val (lat, lon) = IsochroneRouter.destinationPoint(0.0, 0.0, 0.0, IsochroneRouter.NM_TO_M) + assertTrue("latitude should increase", lat > 0.0) + assertEquals(0.0, lon, 1e-9) + // 1 NM ≈ 1/60 degree of latitude + assertEquals(1.0 / 60.0, lat, 1e-4) + } + + // ── Pruning ─────────────────────────────────────────────────────────────── + + @Test + fun `prune keeps only furthest point per sector`() { + // Two points both due north of origin at different distances + val close = RoutePoint(1.0, 0.0, startTimeMs) + val far = RoutePoint(2.0, 0.0, startTimeMs) + val result = IsochroneRouter.prune(listOf(close, far), 0.0, 0.0, 72) + assertEquals(1, result.size) + assertEquals(far, result[0]) + } + + @Test + fun `prune keeps points in different sectors separately`() { + // One point north, one point east — different sectors + val north = RoutePoint(1.0, 0.0, startTimeMs) + val east = RoutePoint(0.0, 1.0, startTimeMs) + val result = IsochroneRouter.prune(listOf(north, east), 0.0, 0.0, 72) + assertEquals(2, result.size) + } + + // ── Full routing ────────────────────────────────────────────────────────── + + @Test + fun `route finds path to destination with constant wind`() { + // Destination is ~5 NM due east of start; constant 10kt easterly (FROM east = 90°) + // A 10kt boat sailing downwind (TWA=180) = 6.0 kt; ~5 NM / 6 kt ≈ 50 min → 1 step + val destLat = 0.0 + val destLon = 0.0 + (5.0 / 60.0) // ~5 NM east + val constantWind = { _: Double, _: Double, _: Long -> + WindForecast(0.0, 0.0, startTimeMs, twsKt = 10.0, twdDeg = 90.0) + } + val result = IsochroneRouter.route( + startLat = 0.0, + startLon = 0.0, + destLat = destLat, + destLon = destLon, + startTimeMs = startTimeMs, + stepMs = oneHourMs, + polars = BoatPolars.DEFAULT, + windAt = constantWind, + arrivalRadiusM = 2_000.0 // 2 km arrival radius + ) + assertNotNull("Should find a route", result) + result!! + assertTrue("Path should have at least 2 points (start + arrival)", result.path.size >= 2) + assertEquals("Path should start at origin", 0.0, result.path.first().lat, 1e-6) + assertEquals("ETA should be after start", startTimeMs, result.etaMs - oneHourMs) + } + + @Test + fun `route returns null when polars produce zero speed`() { + val zeroPolar = BoatPolars(emptyMap()) + val result = IsochroneRouter.route( + startLat = 0.0, + startLon = 0.0, + destLat = 1.0, + destLon = 0.0, + startTimeMs = startTimeMs, + stepMs = oneHourMs, + polars = zeroPolar, + windAt = { _, _, _ -> WindForecast(0.0, 0.0, startTimeMs, 10.0, 0.0) }, + maxSteps = 3 + ) + assertNull("Should return null when no progress is possible", result) + } + + @Test + fun `backtrace returns path from start to arrival in order`() { + val p0 = RoutePoint(0.0, 0.0, 0L) + val p1 = RoutePoint(1.0, 0.0, 1L, parent = p0) + val p2 = RoutePoint(2.0, 0.0, 2L, parent = p1) + val path = IsochroneRouter.backtrace(p2) + assertEquals(3, path.size) + assertEquals(p0, path[0]) + assertEquals(p1, path[1]) + assertEquals(p2, path[2]) + } +} |