From 72b31852809ff829888f11db565e1a4e84b7534c Mon Sep 17 00:00:00 2001 From: Dimitris Date: Thu, 7 May 2026 10:34:27 +0200 Subject: [PATCH] RouteCalculator --- app/build.gradle.kts | 4 +- .../navigation/car/DeviceLocationManager.kt | 4 +- .../navigation/car/NavigationSession.kt | 1 + .../navigation/car/navigation/Simulation.kt | 11 +-- .../java/com/kouros/navigation/data/Data.kt | 5 +- .../navigation/model/RouteCalculator.kt | 92 ++++++++++++------- .../navigation/model/RouteCalculatorTest.kt | 12 ++- .../kouros/navigation/model/RouteModelTest.kt | 3 +- 8 files changed, 79 insertions(+), 53 deletions(-) diff --git a/app/build.gradle.kts b/app/build.gradle.kts index 8b13f92..832c4b5 100644 --- a/app/build.gradle.kts +++ b/app/build.gradle.kts @@ -17,8 +17,8 @@ android { applicationId = "com.kouros.navigation" minSdk = 33 targetSdk = 37 - versionCode = 108 - versionName = "0.3.0.108" + versionCode = 109 + versionName = "0.3.0.109" base.archivesName = "navi-$versionName" testInstrumentationRunner = "androidx.test.runner.AndroidJUnitRunner" } diff --git a/common/car/src/main/java/com/kouros/navigation/car/DeviceLocationManager.kt b/common/car/src/main/java/com/kouros/navigation/car/DeviceLocationManager.kt index 1d4d3ea..6cfd317 100644 --- a/common/car/src/main/java/com/kouros/navigation/car/DeviceLocationManager.kt +++ b/common/car/src/main/java/com/kouros/navigation/car/DeviceLocationManager.kt @@ -107,8 +107,8 @@ class DeviceLocationManager( val lastLocation = locationManager.getLastKnownLocation(LocationManager.GPS_PROVIDER) if (lastLocation != null) { if (setIndividualLocation) { - onInitialLocation(lastLocation) - onLocationUpdate(lastLocation) + onInitialLocation(homeVogelhart) + onLocationUpdate(homeVogelhart) } else { onInitialLocation(lastLocation) onLocationUpdate(lastLocation) diff --git a/common/car/src/main/java/com/kouros/navigation/car/NavigationSession.kt b/common/car/src/main/java/com/kouros/navigation/car/NavigationSession.kt index 5fbd75f..8f0ec90 100644 --- a/common/car/src/main/java/com/kouros/navigation/car/NavigationSession.kt +++ b/common/car/src/main/java/com/kouros/navigation/car/NavigationSession.kt @@ -485,6 +485,7 @@ class NavigationSession : CarSession(), NavigationListener, NavigationObserverCa val distance = location.distanceTo(snappedLocation) when { distance > MAXIMAL_ROUTE_DEVIATION -> { + Log.d(TAG, "Maximal route deviation") stopNavigation() navigationScreen.calculateNewRoute(routeModel.navState.destination) return false diff --git a/common/car/src/main/java/com/kouros/navigation/car/navigation/Simulation.kt b/common/car/src/main/java/com/kouros/navigation/car/navigation/Simulation.kt index cf65dc5..1a54cea 100644 --- a/common/car/src/main/java/com/kouros/navigation/car/navigation/Simulation.kt +++ b/common/car/src/main/java/com/kouros/navigation/car/navigation/Simulation.kt @@ -28,8 +28,8 @@ class Simulation { ) { if (routeModel.navState.route.isRouteValid()) { if (BuildConfig.DEBUG) { - gpxSimulation(routeModel, lifecycleScope, updateLocation) - //currentSimulation(routeModel, lifecycleScope, updateLocation) + //gpxSimulation(routeModel, lifecycleScope, updateLocation) + currentSimulation(routeModel, lifecycleScope, updateLocation) } else { currentSimulation(routeModel, lifecycleScope, updateLocation) } @@ -46,7 +46,7 @@ class Simulation { if (points.isEmpty()) return simulationJob?.cancel() var lastLocation = Location(LocationManager.FUSED_PROVIDER) - var curBearing = 0f + var curBearing: Float simulationJob = lifecycleScope.launch { for ((index, point) in points.withIndex()) { if (index >= 0) { @@ -90,7 +90,7 @@ class Simulation { simulationJob?.cancel() simulationJob =lifecycleScope.launch() { var lastLocation = Location(LocationManager.FUSED_PROVIDER) - var curBearing = 0f + val curBearing = 0f val parser = GPXParser() val parsedGpx: Gpx? = parser.parse(route.byteInputStream()) @@ -118,9 +118,6 @@ class Simulation { // Update your app's state as if a real GPS update occurred updateLocation(fakeLocation) // Wait before moving to the next point (e.g., every 1 second) - if (duration > 100) { - // delay(duration / 4) - } delay(200) lastTime = p.time lastLocation = fakeLocation diff --git a/common/data/src/main/java/com/kouros/navigation/data/Data.kt b/common/data/src/main/java/com/kouros/navigation/data/Data.kt index e0c3bfd..fe7f97f 100644 --- a/common/data/src/main/java/com/kouros/navigation/data/Data.kt +++ b/common/data/src/main/java/com/kouros/navigation/data/Data.kt @@ -16,6 +16,7 @@ package com.kouros.navigation.data +import android.location.Location import android.net.Uri import com.google.gson.annotations.Expose import com.kouros.navigation.data.route.Lane @@ -28,7 +29,7 @@ data class Category( val name: String, ) - +data class StepMatch(val stepIndex: Int, val waypointIndex: Int, val location: Location) data class Places( val places: List, @@ -144,7 +145,7 @@ object Constants { const val NEAREST_LOCATION_DISTANCE = 10F - const val MAXIMUM_LOCATION_DISTANCE = 100000F + const val MAXIMUM_LOCATION_DISTANCE = Float.MAX_VALUE const val TRAFFIC_UPDATE = 300 diff --git a/common/data/src/main/java/com/kouros/navigation/model/RouteCalculator.kt b/common/data/src/main/java/com/kouros/navigation/model/RouteCalculator.kt index 5882f9a..4106790 100644 --- a/common/data/src/main/java/com/kouros/navigation/model/RouteCalculator.kt +++ b/common/data/src/main/java/com/kouros/navigation/model/RouteCalculator.kt @@ -3,60 +3,84 @@ package com.kouros.navigation.model import android.location.Location import android.util.Log import androidx.car.app.navigation.model.Step -import com.kouros.navigation.data.ApplicationConfig import com.kouros.navigation.data.Constants.MAXIMUM_LOCATION_DISTANCE import com.kouros.navigation.data.Constants.NEAREST_LOCATION_DISTANCE import com.kouros.navigation.data.Constants.SPEED_UPDATE_DISTANCE import com.kouros.navigation.data.Constants.TAG -import com.kouros.navigation.utils.bearingPositive +import com.kouros.navigation.data.StepMatch +import com.kouros.navigation.data.osrm.Waypoints import com.kouros.navigation.utils.location import java.util.concurrent.TimeUnit import kotlin.math.absoluteValue class RouteCalculator(var routeModel: RouteModel) { + var bestMatch: StepMatch? = null var lastSpeedLocation: Location = location(0.0, 0.0) var lastSpeedIndex: Int = 0 fun findStep(location: Location) { + val route = routeModel.navState.route + val steps = routeModel.curLeg.steps + val startIndex = route.currentStepIndex + + // Windowed search for performance (current + 2 steps) + //val endIndex = (startIndex + 2).coerceAtMost(steps.size - 1) + val endIndex = steps.size - 1 + var nearestDistance = MAXIMUM_LOCATION_DISTANCE - var count = 0 - var lastDistance = 0F - var increaseDistance = 0 - for ((index, step) in routeModel.curLeg.steps.withIndex()) { - count++ - var distance = 0F - if (index >= routeModel.navState.route.currentStepIndex) { - for ((wayIndex, waypoint) in step.maneuver.waypoints.withIndex()) { - count++ - if (wayIndex >= step.waypointIndex) { - distance = location.distanceTo(waypoint) - if (distance < nearestDistance ) { - nearestDistance = distance - routeModel.navState.route.currentStepIndex = step.index - step.waypointIndex = wayIndex - step.wayPointLocation = waypoint - } - } - if (stopSearch(nearestDistance, distance) || increaseDistance > 20) { - break - } - if (distance > lastDistance) { - increaseDistance++ - } - lastDistance = distance + var searchCount = 0 + + for (i in startIndex..endIndex) { + val step = steps[i] + val waypoints = step.maneuver.waypoints + val startWayIndex = if (i == startIndex) step.waypointIndex else 0 + + var lastDistance = Float.MAX_VALUE + var increaseCount = 0 + + for (j in startWayIndex until waypoints.size) { + searchCount++ + val waypoint = waypoints[j] + val distance = location.distanceTo(waypoint) + + if (distance < nearestDistance) { + nearestDistance = distance + bestMatch = StepMatch(step.index, j, waypoint) } - } - if (stopSearch(nearestDistance, distance) || increaseDistance > 20) { - break + + // Track if we are getting further away + if (distance > lastDistance) { + increaseCount++ + } else { + increaseCount = 0 + } + // Absolute early exit: found a close point and now moved far away + if (stopSearch(nearestDistance, distance, increaseCount)) { + bestMatch?.let { match -> + route.currentStepIndex = match.stepIndex + steps[match.stepIndex].waypointIndex = match.waypointIndex + steps[match.stepIndex].wayPointLocation = match.location + } + Log.d(TAG, "FindStep optimized: count=$searchCount, dist=$nearestDistance") + return + } + lastDistance = distance } } - Log.d(TAG, "FindStep $nearestDistance $count $increaseDistance") + bestMatch?.let { match -> + route.currentStepIndex = match.stepIndex + steps[match.stepIndex].waypointIndex = match.waypointIndex + steps[match.stepIndex].wayPointLocation = match.location + + } + Log.d(TAG, "FindStep not optimized: count=$searchCount, dist=$nearestDistance") } - private fun stopSearch(nearestDistance: Float, distance: Float): Boolean { - return nearestDistance < NEAREST_LOCATION_DISTANCE && distance > NEAREST_LOCATION_DISTANCE * 10 + private fun stopSearch(nearestDistance: Float, distance: Float, increaseCount: Int): Boolean { + return (nearestDistance < NEAREST_LOCATION_DISTANCE && distance > NEAREST_LOCATION_DISTANCE * 10) + || increaseCount > 10 } fun travelLeftTime(): Double { @@ -92,7 +116,7 @@ class RouteCalculator(var routeModel: RouteModel) { fun leftStepDistance(): Double { val step = routeModel.route.currentStep() var leftDistance = 0F - if (step.waypointIndex < step.maneuver.waypoints.size -1) { + if (step.waypointIndex < step.maneuver.waypoints.size - 1) { leftDistance = step.maneuver.leftDistance[step.waypointIndex] val waypointLocation = step.maneuver.waypoints[step.waypointIndex] if (routeModel.navState.lastLocation.latitude != 0.0) { diff --git a/common/data/src/test/java/com/kouros/navigation/model/RouteCalculatorTest.kt b/common/data/src/test/java/com/kouros/navigation/model/RouteCalculatorTest.kt index a4b4103..b0881c1 100644 --- a/common/data/src/test/java/com/kouros/navigation/model/RouteCalculatorTest.kt +++ b/common/data/src/test/java/com/kouros/navigation/model/RouteCalculatorTest.kt @@ -7,6 +7,7 @@ import com.kouros.navigation.data.route.Maneuver import com.kouros.navigation.data.route.Routes import com.kouros.navigation.data.route.Step import com.kouros.navigation.data.route.Summary +import com.kouros.navigation.utils.location import org.junit.Assert.assertEquals import org.junit.Assert.assertTrue import org.junit.Before @@ -44,7 +45,7 @@ class RouteCalculatorTest { waypointIndex: Int = 0, ): Step { val waypoints = (0 until numWaypoints).map { i -> - listOf(11.0 + index * 0.01 + i * 0.001, 48.0) + location(11.0 + index * 0.01 + i * 0.001, 48.0) } return Step( index = index, @@ -127,13 +128,14 @@ class RouteCalculatorTest { ) val mockLocation: Location = mock() - // step0/wp0: 500F, step0/wp1: 5F — 5F < NEAREST_LOCATION_DISTANCE (10F) → break - whenever(mockLocation.distanceTo(any())).thenReturn(500F, 5F) + // step0/wp0: 500F, step0/wp1: 5F, step1/wp0: 200F + // 5F < NEAREST_LOCATION_DISTANCE (10F) AND 200F > 10 * 10F → break + whenever(mockLocation.distanceTo(any())).thenReturn(500F, 5F, 200F) routeCalculator.findStep(mockLocation) - // step1 and step2 are never evaluated - verify(mockLocation, times(2)).distanceTo(any()) + // should stop after the 3rd call (step1/wp0 triggers stopSearch) + verify(mockLocation, times(3)).distanceTo(any()) assertEquals(0, routeModel.navState.route.currentStepIndex) } diff --git a/common/data/src/test/java/com/kouros/navigation/model/RouteModelTest.kt b/common/data/src/test/java/com/kouros/navigation/model/RouteModelTest.kt index cc23c02..54818a1 100644 --- a/common/data/src/test/java/com/kouros/navigation/model/RouteModelTest.kt +++ b/common/data/src/test/java/com/kouros/navigation/model/RouteModelTest.kt @@ -9,6 +9,7 @@ import com.kouros.navigation.data.route.Routes import com.kouros.navigation.data.route.Step import com.kouros.navigation.data.route.Summary import com.kouros.navigation.utils.GeoUtils.createPointCollection +import com.kouros.navigation.utils.location import org.junit.Before import org.junit.Test import org.maplibre.geojson.FeatureCollection @@ -39,7 +40,7 @@ class RouteModelTest { waypointIndex: Int = 0, ): Step { val waypoints = (0 until numWaypoints).map { i -> - listOf(11.0 + index * 0.01 + i * 0.001, 48.0) + location(11.0 + index * 0.01 + i * 0.001, 48.0) } return Step( index = index,