From 8b612d5b808f64c8f68df752cd828496994c47ab Mon Sep 17 00:00:00 2001 From: Dimitris Date: Wed, 6 May 2026 19:49:43 +0200 Subject: [PATCH] RouteCalculator --- .../kouros/navigation/car/RouteModelTest.kt | 384 +++++++++++++++++- .../navigation/car/navigation/Simulation.kt | 8 +- .../navigation/car/screen/CategoriesScreen.kt | 52 ++- .../navigation/car/screen/NavigationScreen.kt | 5 +- .../navigation/car/screen/PlaceListScreen.kt | 5 +- .../data/tomtom/TomTomRepository.kt | 2 +- .../navigation/data/tomtom/TomTomRoute.kt | 11 +- .../navigation/model/RouteCalculator.kt | 21 +- gradle/libs.versions.toml | 4 +- 9 files changed, 451 insertions(+), 41 deletions(-) diff --git a/common/car/src/androidTest/java/com/kouros/navigation/car/RouteModelTest.kt b/common/car/src/androidTest/java/com/kouros/navigation/car/RouteModelTest.kt index 16a158d..e091af5 100644 --- a/common/car/src/androidTest/java/com/kouros/navigation/car/RouteModelTest.kt +++ b/common/car/src/androidTest/java/com/kouros/navigation/car/RouteModelTest.kt @@ -4,6 +4,7 @@ import android.location.Location import android.location.LocationManager import androidx.test.platform.app.InstrumentationRegistry import androidx.test.ext.junit.runners.AndroidJUnit4 +import com.kouros.navigation.car.navigation.Simulation import com.kouros.navigation.data.Constants.homeHohenwaldeck import com.kouros.navigation.data.RouteEngine import com.kouros.navigation.data.overpass.Overpass @@ -11,6 +12,7 @@ import com.kouros.navigation.data.route.ManeuverType import com.kouros.navigation.data.tomtom.TomTomRepository import com.kouros.navigation.model.NavigationViewModel import com.kouros.navigation.model.RouteModel +import com.kouros.navigation.utils.GeoUtils.snapLocation import com.kouros.navigation.utils.getSettingsRepository import com.kouros.navigation.utils.location import kotlinx.coroutines.delay @@ -21,6 +23,8 @@ import org.junit.runner.RunWith import org.junit.Assert.* import org.junit.Before +import org.maplibre.compose.expressions.dsl.step +import org.maplibre.geojson.Point import kotlin.collections.forEach /** @@ -33,6 +37,348 @@ class RouteModelTest { val routeModel = RouteModel() val location = Location(LocationManager.GPS_PROVIDER) + + val leftDistance = listOf( + 64.0, + 0.0, + 143.7469482421875, + 111.36382293701172, + 101.35636901855469, + 0.0, + 356.0272216796875, + 250.2962646484375, + 245.09136962890625, + 213.86209106445312, + 173.69473266601562, + 90.41661834716797, + 40.58605194091797, + 22.74074363708496, + 12.33098030090332, + 0.0, + 1046.3043212890625, + 1026.2342529296875, + 1012.8703002929688, + 979.4789428710938, + 972.7660522460938, + 971.6541137695312, + 914.8672485351562, + 873.6181030273438, + 831.3056030273438, + 781.1799926757812, + 719.982666015625, + 653.1173095703125, + 578.4839477539062, + 566.2300415039062, + 490.4866943359375, + 482.66766357421875, + 451.45355224609375, + 443.6345520019531, + 391.28887939453125, + 346.71197509765625, + 316.65283203125, + 237.5792236328125, + 141.71861267089844, + 78.22917175292969, + 55.94073486328125, + 41.31438064575195, + 31.061260223388672, + 20.943891525268555, + 16.43439483642578, + 0.0, + 346.5630798339844, + 339.0434875488281, + 329.3117370605469, + 321.055908203125, + 304.0730285644531, + 285.6786193847656, + 272.10870361328125, + 233.37289428710938, + 213.9094696044922, + 198.87030029296875, + 164.74563598632812, + 136.4317626953125, + 91.64434051513672, + 82.11692810058594, + 60.832786560058594, + 33.411705017089844, + 0.0, + 338.5975341796875, + 318.1346435546875, + 211.02232360839844, + 201.35411071777344, + 187.75144958496094, + 179.0858154296875, + 170.42019653320312, + 123.4335708618164, + 90.03872680664062, + 74.55345916748047, + 60.95069122314453, + 39.9212532043457, + 0.0, + 314.19586181640625, + 306.1593017578125, + 289.4617004394531, + 223.2646484375, + 129.7374725341797, + 76.47386932373047, + 56.635841369628906, + 52.30287170410156, + 33.61341857910156, + 20.613277435302734, + 0.0, + 5621.87744140625, + 5612.591796875, + 5587.8486328125, + 5483.76806640625, + 5447.275390625, + 5422.7744140625, + 5398.21875, + 5373.927734375, + 5358.28955078125, + 5347.1455078125, + 5339.36181640625, + 5314.6181640625, + 5303.107421875, + 5278.109375, + 5253.859375, + 5244.88720703125, + 5200.0400390625, + 5151.9326171875, + 5138.63916015625, + 5085.87548828125, + 5054.32080078125, + 5012.51220703125, + 4962.390625, + 4938.140625, + 4898.97607421875, + 4862.1416015625, + 4788.78564453125, + 4767.880859375, + 4761.89013671875, + 4756.5390625, + 4749.23388671875, + 4724.98388671875, + 4645.017578125, + 4596.826171875, + 4573.98583984375, + 4554.85009765625, + 4521.48388671875, + 4499.232421875, + 4484.6064453125, + 4471.07763671875, + 4457.54931640625, + 4415.10986328125, + 4389.8359375, + 4359.6376953125, + 4347.59716796875, + 4342.62109375, + 4334.59375, + 4320.33447265625, + 4303.408203125, + 4289.998046875, + 4261.86962890625, + 4215.78515625, + 4207.08251953125, + 4192.81201171875, + 4080.384521484375, + 4065.3955078125, + 4009.8798828125, + 3975.99853515625, + 3971.5283203125, + 3953.9345703125, + 3915.152587890625, + 3877.74365234375, + 3865.702880859375, + 3857.67578125, + 3850.37060546875, + 3845.019287109375, + 3843.161865234375, + 3819.26904296875, + 3803.01171875, + 3686.892578125, + 3661.30908203125, + 3629.2001953125, + 3613.607177734375, + 3591.255859375, + 3580.988525390625, + 3573.37060546875, + 3563.377685546875, + 3552.08349609375, + 3537.639404296875, + 3502.25146484375, + 3484.319580078125, + 3445.313720703125, + 3437.883544921875, + 3431.69873046875, + 3413.72021484375, + 3405.05224609375, + 3340.725830078125, + 3312.782470703125, + 3302.888671875, + 3292.994873046875, + 3240.986328125, + 3229.830322265625, + 3185.277099609375, + 3096.74755859375, + 2981.00146484375, + 2907.40869140625, + 2865.943115234375, + 2859.75830078125, + 2851.719482421875, + 2813.998779296875, + 2778.027587890625, + 2767.468505859375, + 2729.052001953125, + 2694.80712890625, + 2651.47265625, + 2613.131591796875, + 2602.36376953125, + 2588.665771484375, + 2549.015380859375, + 2530.110107421875, + 2521.1689453125, + 2510.401123046875, + 2478.26953125, + 2437.71337890625, + 2400.48681640625, + 2350.667236328125, + 2320.4267578125, + 2294.857666015625, + 2268.75830078125, + 2260.833740234375, + 2249.615234375, + 2229.476318359375, + 2210.573486328125, + 2192.78271484375, + 2184.963623046875, + 2148.23974609375, + 2114.8818359375, + 2105.955078125, + 2052.49951171875, + 2013.5179443359375, + 2006.804931640625, + 1959.955810546875, + 1869.44775390625, + 1787.787109375, + 1740.873046875, + 1719.6939697265625, + 1554.079833984375, + 1395.2606201171875, + 1349.453369140625, + 1296.93359375, + 1144.57080078125, + 1030.08203125, + 972.0895385742188, + 926.4032592773438, + 921.95556640625, + 911.9205322265625, + 902.9940185546875, + 887.3560180664062, + 803.21728515625, + 774.1542358398438, + 767.318603515625, + 762.62841796875, + 754.703857421875, + 751.3680419921875, + 738.9346923828125, + 725.406005859375, + 710.6475219726562, + 668.3836669921875, + 615.11279296875, + 605.7323608398438, + 572.6571044921875, + 552.6024780273438, + 545.567138671875, + 474.7272644042969, + 459.36187744140625, + 445.29119873046875, + 427.3172607421875, + 406.21124267578125, + 378.6969909667969, + 371.8613586425781, + 362.934814453125, + 347.29681396484375, + 329.3663330078125, + 320.4398193359375, + 301.3039855957031, + 237.61085510253906, + 112.25553131103516, + 94.07954406738281, + 75.73666381835938, + 0.0, + 1023.4342651367188, + 1003.37939453125, + 973.33203125, + 901.591796875, + 805.4495849609375, + 776.5214233398438, + 723.6364135742188, + 713.1954956054688, + 693.5875854492188, + 646.0399169921875, + 565.5601196289062, + 509.7200622558594, + 484.1024169921875, + 422.78350830078125, + 276.693603515625, + 265.35223388671875, + 187.42837524414062, + 160.74212646484375, + 97.36228942871094, + 29.534751892089844, + 18.390586853027344, + 0.0, + 462.20318603515625, + 450.08526611328125, + 439.66217041015625, + 397.6007080078125, + 346.669921875, + 290.0765380859375, + 270.7193603515625, + 208.91537475585938, + 191.0126495361328, + 177.56549072265625, + 165.6016082763672, + 117.09382629394531, + 108.90425109863281, + 75.23577117919922, + 37.74562072753906, + 0.0, + 940.9969482421875, + 921.5716552734375, + 891.173583984375, + 876.2035522460938, + 865.4982299804688, + 851.6742553710938, + 808.6068725585938, + 793.0664672851562, + 757.3865356445312, + 716.390380859375, + 701.561767578125, + 696.2328491210938, + 657.3816528320312, + 641.1264038085938, + 584.9859619140625, + 581.8070678710938, + 552.8590087890625, + 473.6039733886719, + 401.5708923339844, + 369.44659423828125, + 360.9601745605469, + 353.1898498535156, + 309.3813781738281, + 269.4887390136719, + 226.76416015625, + 141.3342742919922, + 135.6678466796875, + 126.8246078491211, + 94.33628845214844, + 68.04391479492188, + 0.0, + 149.6364288330078, + 118.28071594238281, + 0.0, + ) val distance = listOf( 1046.5, 1026.5, @@ -203,30 +549,20 @@ class RouteModelTest { @Test fun simulate() { - val viewModel = NavigationViewModel(TomTomRepository()) for ((index, waypoint) in routeModel.curRoute.waypoints.withIndex()) { if (routeModel.isNavigating()) { val curLocation = location(waypoint[0], waypoint[1]) if (index in 0..routeModel.curRoute.waypoints.size) { - //runBlocking { delay(1000) } - val start = System.currentTimeMillis() routeModel.updateLocation(curLocation, NavigationViewModel(TomTomRepository())) val stepData = routeModel.currentStep() - //println("${stepData.instruction} ${System.currentTimeMillis() - start}") - if (stepData.lane.isNotEmpty()) { - // println(stepData.street) - stepData.lane.forEach { - // println("${it.indications} ${it.valid}") - } - } - // val nextData = routeModel.nextStep() + assertEquals(stepData.leftStepDistance, leftDistance[index], 1.0) } } } } @Test - fun `leftStepDistance Inglolstädter `() { + fun `leftStepDistance Ingolstädter `() { var location = location(11.584352, 48.186771) routeModel.updateLocation(location, NavigationViewModel(TomTomRepository())) var step = routeModel.currentStep() @@ -264,5 +600,29 @@ class RouteModelTest { val end = System.currentTimeMillis() - start println("Time $end") } + + @Test + fun `check leftStepDistance Vogelhart Gpx `() { + routeModel.navState = routeModel.navState.copy(routeBearing = 270F) + var location = location(11.57927955, 48.18554854) + routeModel.updateLocation(location, NavigationViewModel(TomTomRepository())) + var stepData = routeModel.currentStep() + assertEquals(stepData.leftStepDistance, 62.0, 1.0) + + location = location(11.57919950, 48.18556317) + routeModel.updateLocation(location, NavigationViewModel(TomTomRepository())) + stepData = routeModel.currentStep() + assertEquals(stepData.leftStepDistance, 58.0, 1.0) + + location = location(11.57906832, 48.18556405) + routeModel.updateLocation(location, NavigationViewModel(TomTomRepository())) + stepData = routeModel.currentStep() + assertEquals(stepData.leftStepDistance, 48.0, 1.0) + + routeModel.navState = routeModel.navState.copy(routeBearing = 270F) + location = location(11.57875945, 48.18558161) + routeModel.updateLocation(location, NavigationViewModel(TomTomRepository())) + //assertEquals(routeModel.currentStep().leftStepDistance, 0.0, 1.0) + } } 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 c3c7e6e..cf65dc5 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) } @@ -81,7 +81,7 @@ class Simulation { runBlocking { simulationJob = launch(Dispatchers.IO) { route = TomTomRepository().fetchUrl( - "https://kouros-online.de/vh.gpx", + "https://kouros-online.de/VH.gpx", false ) } @@ -121,7 +121,7 @@ class Simulation { if (duration > 100) { // delay(duration / 4) } - delay(500) + delay(200) lastTime = p.time lastLocation = fakeLocation } diff --git a/common/car/src/main/java/com/kouros/navigation/car/screen/CategoriesScreen.kt b/common/car/src/main/java/com/kouros/navigation/car/screen/CategoriesScreen.kt index 6d53751..339a906 100644 --- a/common/car/src/main/java/com/kouros/navigation/car/screen/CategoriesScreen.kt +++ b/common/car/src/main/java/com/kouros/navigation/car/screen/CategoriesScreen.kt @@ -3,15 +3,16 @@ package com.kouros.navigation.car.screen import androidx.car.app.CarContext import androidx.car.app.Screen import androidx.car.app.model.Action +import androidx.car.app.model.CarColor import androidx.car.app.model.CarIcon import androidx.car.app.model.GridItem import androidx.car.app.model.GridTemplate import androidx.car.app.model.Header import androidx.car.app.model.ItemList -import androidx.car.app.model.ListTemplate -import androidx.car.app.model.Row import androidx.car.app.model.Template +import androidx.compose.ui.graphics.Color import androidx.core.graphics.drawable.IconCompat +import androidx.core.graphics.toColorInt import com.kouros.data.R import com.kouros.navigation.car.SurfaceRenderer import com.kouros.navigation.data.Category @@ -80,15 +81,50 @@ class CategoriesScreen( } } + fun carIcon(context: CarContext, category: String, index: Int): CarIcon { + + val customCarColor = + CarColor.createCustom(android.graphics.Color.MAGENTA, android.graphics.Color.MAGENTA) + if (index == -1) { - val resId = when (category) { - CHARGING_STATION -> R.drawable.ev_station_24px - FUEL_STATION -> R.drawable.local_gas_station_24 - PHARMACY -> R.drawable.local_pharmacy_24px - else -> R.drawable.restaurant_24px + val icon = when (category) { + CHARGING_STATION -> CarIcon.Builder( + IconCompat.createWithResource( + context, + R.drawable.ev_station_24px + ) + ).setTint(CarColor.GREEN) + + FUEL_STATION -> CarIcon.Builder( + IconCompat.createWithResource( + context, + R.drawable.local_gas_station_24 + ) + ).setTint(CarColor.BLUE) + + PHARMACY -> CarIcon.Builder( + IconCompat.createWithResource( + context, + R.drawable.local_pharmacy_24px + ) + ).setTint(CarColor.RED) + + RESTAURANT -> CarIcon.Builder( + IconCompat.createWithResource( + context, + R.drawable.restaurant_24px + ) + ).setTint(customCarColor) + + else -> CarIcon.Builder( + IconCompat.createWithResource( + context, + R.drawable.traffic_jam_48px + ) + ).setTint(CarColor.YELLOW) } - return CarIcon.Builder(IconCompat.createWithResource(context, resId)).build() + return icon.build() } else { return CarIcon.Builder( createNumberIcon( diff --git a/common/car/src/main/java/com/kouros/navigation/car/screen/NavigationScreen.kt b/common/car/src/main/java/com/kouros/navigation/car/screen/NavigationScreen.kt index c6f024d..7c053c6 100644 --- a/common/car/src/main/java/com/kouros/navigation/car/screen/NavigationScreen.kt +++ b/common/car/src/main/java/com/kouros/navigation/car/screen/NavigationScreen.kt @@ -222,10 +222,9 @@ open class NavigationScreen( return NavigationTemplate.Builder() .setNavigationInfo( MessageInfo.Builder( - message - //carContext.getString(R.string.arrived_exclamation_msg) + carContext.getString(R.string.arrived_exclamation_msg) ) - // .setText(street) + .setText(message) .setImage( CarIcon.Builder( IconCompat.createWithResource( diff --git a/common/car/src/main/java/com/kouros/navigation/car/screen/PlaceListScreen.kt b/common/car/src/main/java/com/kouros/navigation/car/screen/PlaceListScreen.kt index 3ce96dc..6e73f99 100644 --- a/common/car/src/main/java/com/kouros/navigation/car/screen/PlaceListScreen.kt +++ b/common/car/src/main/java/com/kouros/navigation/car/screen/PlaceListScreen.kt @@ -76,7 +76,10 @@ class PlaceListScreen( val itemListBuilder = ItemList.Builder() .setNoItemsMessage(carContext.getString(R.string.no_places)) recentPlaces.filter { it.category == category }.forEach { - val street = it.street + val street = it.street.ifEmpty { + it.name + } + val row = Row.Builder() .setImage(contactIcon(null, it.category)) .setTitle("$street ${it.city}") diff --git a/common/data/src/main/java/com/kouros/navigation/data/tomtom/TomTomRepository.kt b/common/data/src/main/java/com/kouros/navigation/data/tomtom/TomTomRepository.kt index 03e34a4..dfc2266 100644 --- a/common/data/src/main/java/com/kouros/navigation/data/tomtom/TomTomRepository.kt +++ b/common/data/src/main/java/com/kouros/navigation/data/tomtom/TomTomRepository.kt @@ -21,7 +21,7 @@ const val tomtomTrafficUrl = "https://api.tomtom.com/traffic/services/5/incident private const val tomtomFields = "{incidents{type,geometry{type,coordinates},properties{iconCategory,events{description}}}}" -val useLocal = BuildConfig.DEBUG +val useLocal = BuildConfig.DEBUG val useLocalTraffic = BuildConfig.DEBUG diff --git a/common/data/src/main/java/com/kouros/navigation/data/tomtom/TomTomRoute.kt b/common/data/src/main/java/com/kouros/navigation/data/tomtom/TomTomRoute.kt index e8b144e..b24a821 100644 --- a/common/data/src/main/java/com/kouros/navigation/data/tomtom/TomTomRoute.kt +++ b/common/data/src/main/java/com/kouros/navigation/data/tomtom/TomTomRoute.kt @@ -64,10 +64,10 @@ class TomTomRoute { // calculate left step distance for each point var leftStepDistance = stepDistance leftDistance.add(leftStepDistance.toFloat()) - points.subList( + val subPoints = points.subList( lastPointIndex, - instruction.pointIndex + 1, - ).forEach { + instruction.pointIndex + 1) + subPoints.forEach { if (lastLocation.latitude != 0.0) { val curLocation = location(it[0], it[1]) val dist = curLocation.distanceTo(lastLocation).absoluteValue @@ -80,10 +80,7 @@ class TomTomRoute { bearingBefore = 0, bearingAfter = 0, type = convertType(instruction.maneuver), - waypoints = points.subList( - lastPointIndex, - instruction.pointIndex + 1, - ).map { location(it[0], it[1]) }, + waypoints = subPoints.map { location(it[0], it[1]) }, exit = exitNumber(instruction), location = location( instruction.point.longitude, instruction.point.latitude 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 d84c122..5882f9a 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 @@ -8,6 +8,7 @@ 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.utils.location import java.util.concurrent.TimeUnit import kotlin.math.absoluteValue @@ -20,28 +21,42 @@ class RouteCalculator(var routeModel: RouteModel) { fun findStep(location: Location) { 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) { + if (distance < nearestDistance ) { nearestDistance = distance routeModel.navState.route.currentStepIndex = step.index step.waypointIndex = wayIndex step.wayPointLocation = waypoint } } - if (nearestDistance < NEAREST_LOCATION_DISTANCE && distance > NEAREST_LOCATION_DISTANCE * 5) { + if (stopSearch(nearestDistance, distance) || increaseDistance > 20) { break } + if (distance > lastDistance) { + increaseDistance++ + } + lastDistance = distance } } - if (nearestDistance < NEAREST_LOCATION_DISTANCE && distance > NEAREST_LOCATION_DISTANCE * 5) { + if (stopSearch(nearestDistance, distance) || increaseDistance > 20) { break } } + Log.d(TAG, "FindStep $nearestDistance $count $increaseDistance") + } + + private fun stopSearch(nearestDistance: Float, distance: Float): Boolean { + return nearestDistance < NEAREST_LOCATION_DISTANCE && distance > NEAREST_LOCATION_DISTANCE * 10 } fun travelLeftTime(): Double { diff --git a/gradle/libs.versions.toml b/gradle/libs.versions.toml index ef444ef..a2c7829 100644 --- a/gradle/libs.versions.toml +++ b/gradle/libs.versions.toml @@ -1,9 +1,9 @@ [versions] -agp = "9.2.0" +agp = "9.2.1" androidGpxParser = "2.3.1" androidSdkTurf = "6.0.1" datastore = "1.2.1" -gradle = "9.2.0" +gradle = "9.2.1" koinAndroid = "4.2.1" koinAndroidxCompose = "4.2.1" koinComposeViewmodel = "4.2.1"