Fixed route interpolation

1 files changed, 41 insertions, 24 deletions

diff --git a/fakelocation/src/main/java/foundation/e/advancedprivacy/fakelocation/services/FakeLocationService.kt b/fakelocation/src/main/java/foundation/e/advancedprivacy/fakelocation/services/FakeLocationService.kt
index c388afc..d13e263 100644
--- a/fakelocation/src/main/java/foundation/e/advancedprivacy/fakelocation/services/FakeLocationService.kt
+++ b/fakelocation/src/main/java/foundation/e/advancedprivacy/fakelocation/services/FakeLocationService.kt
@@ -154,31 +154,49 @@ class FakeLocationService : Service() {
         }.start()
     }
 
-    private fun calculateRouteSegment(route: List<FakeLocationCoordinate>, routeTime: Float): Pair<FakeLocationCoordinate,Pair<Float,Float>>? {
-        if(route.size < 2)
+    private fun calculateRouteSegment(): Pair<FakeLocationCoordinate,Pair<Float,Float>>? {
+        var route_buf: List<FakeLocationCoordinate> = route ?: return null
+        if(route_buf.size < 2)
             return null
-        var prev = route.first()
-        var timeCurrent: Float = 0f
-        do {
-            var route_current = if(routeReversed) route.reversed() else route
-            for(coord in route_current) {
-                var direction = Pair<Float,Float>((coord.latitude - prev.latitude) * 111139.0f, (coord.longitude - prev.longitude) * 111139.0f)
-                if(!(coord.latitude == prev.latitude && coord.longitude == prev.longitude)) {
-                    var distance_target = sqrt((direction.first * direction.first) + (direction.second * direction.second))
-                    var direction_unit = Pair<Float,Float>(direction.first / distance_target, direction.second / distance_target)
-                    var location_meters = Pair<Float,Float>(direction_unit.first * (routeTime - timeCurrent) * prev.speed,
-                        direction_unit.second * (routeTime - timeCurrent) * prev.speed)
-                    var distance_current = sqrt((location_meters.first * location_meters.first) + (location_meters.second - location_meters.second))
-                    var location = Pair<Float,Float>(prev.latitude + (location_meters.first / 111139.0f), prev.longitude + (location_meters.second / 111139.0f))
-                    if(distance_current < distance_target)
-                        return Pair<FakeLocationCoordinate,Pair<Float,Float>>(prev, location)
-                    timeCurrent += distance_target / prev.speed
-                    prev = coord
+        var segment: Pair<FakeLocationCoordinate,FakeLocationCoordinate>? = null
+        var factor: Float? = null
+        var iter = route_buf.iterator()
+        while(iter.hasNext()) {
+            segment = Pair<FakeLocationCoordinate,FakeLocationCoordinate>(
+                if(segment == null) iter.next() else segment.second,
+                iter.next()
+            )
+            if(routeTime >= segment.first.timestamp && routeTime < segment.second.timestamp) {
+                factor = (routeTime - segment.first.timestamp) / (segment.second.timestamp - segment.first.timestamp)
+                break
+            }
+        }
+        if(factor == null && loopRoute) {
+            iter = route_buf.reversed().iterator()
+            while(iter.hasNext()) {
+                segment = Pair<FakeLocationCoordinate,FakeLocationCoordinate>(
+                    if(segment == null) iter.next() else segment.second,
+                    iter.next()
+                )
+                if(routeTime >= ((2 * route_buf.last().timestamp) - segment.first.timestamp) && routeTime < ((2 * route_buf.last().timestamp) - segment.second.timestamp)) {
+                    factor = (routeTime - ((2 * route_buf.last().timestamp) - segment.first.timestamp)) / (((2 * route_buf.last().timestamp) - segment.second.timestamp) - ((2 * route_buf.last().timestamp) - segment.first.timestamp))
+                    break
                 }
             }
-            if(loopRoute)
-                routeReversed = !routeReversed
-        } while(loopRoute)
+            if(factor == null) {
+                routeTime -= (2 * route_buf.last().timestamp)
+                return calculateRouteSegment()
+            }
+        }
+        if(segment != null && factor != null) {
+            return Pair<FakeLocationCoordinate,Pair<Float,Float>>(
+                segment.first,
+                Pair<Float,Float>(
+                    segment.first.latitude + ((segment.second.latitude - segment.first.latitude) * factor),
+                    segment.first.longitude + ((segment.second.longitude - segment.first.longitude) * factor)
+                )
+            )
+        }
         return null
     }
 
@@ -187,8 +205,7 @@ class FakeLocationService : Service() {
         routeTime = 0f
         cdtRoute = object : CountDownTimer(PERIOD_UPDATES_SERIE, PERIOD_LOCATION_UPDATE) {
             override fun onTick(millisUntilFinished: Long) {
-                var route_buf: List<FakeLocationCoordinate> = route ?: return
-                var coord = calculateRouteSegment(route_buf, routeTime)
+                var coord = calculateRouteSegment()
                 if(coord == null) {
                     // done with route
                     return