RouteCalculator
This commit is contained in:
Dimitris
@@ -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<Place>,
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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) {
|
||||
|
||||
Reference in New Issue
Block a user