[Opt3.0]优化自车是否走过轨迹点的逻辑

core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/smp/MarkerDrawerManager.kt

@@ -1,10 +1,10 @@
 package com.mogo.eagle.core.function.smp
 
 import android.content.Context
-import android.util.Log
 import com.amap.api.maps.CoordinateConverter
 import com.amap.api.maps.model.LatLng
 import com.mogo.eagle.core.utilcode.util.CoordinateUtils
+import com.mogo.eagle.core.utilcode.util.DrivingDirectionUtils
 import io.reactivex.Observable
 import io.reactivex.ObservableOnSubscribe
 import io.reactivex.android.schedulers.AndroidSchedulers
@@ -35,7 +35,7 @@ object MarkerDrawerManager {
     var lastArrivedIndex: Int = -1
 
     @Volatile
-    var lonLat = Pair(0.0, 0.0)
+    var lonLatHeading = Triple(0.0, 0.0, 0.0)
 
     var callback: Callback? = null
 
@@ -59,16 +59,17 @@ object MarkerDrawerManager {
     private fun getLoopCalCarObservable(): Observable<Int> {
         return Observable.create(ObservableOnSubscribe { emitter ->
             if (emitter.isDisposed) return@ObservableOnSubscribe
-            loopRouteAndWipe(planningPoints, lonLat.first, lonLat.second)
+            loopRouteAndWipe(planningPoints, lonLatHeading.first, lonLatHeading.second, lonLatHeading.third)
             emitter.onComplete()
         })
     }
 
-    private fun loopRouteAndWipe(routePoints: List<LatLng>?, lon: Double, lat: Double) {
+    private fun loopRouteAndWipe(routePoints: List<LatLng>?, lon: Double, lat: Double, heading: Double) {
         if (routePoints != null && routePoints.isNotEmpty()) {
-            val arrivedIndex: Int = getArrivedPointIndex(routePoints, lon, lat)
+            val newPointList = ArrayList<LatLng>()
+            val arrivedIndex: Int = getArrivedPointIndex(routePoints, newPointList, lon, lat, heading)
             if (arrivedIndex != -1 && lastArrivedIndex != arrivedIndex) {
-                callback?.onLocationChanged(routePoints, arrivedIndex)
+                callback?.onLocationChanged(newPointList, arrivedIndex)
                 lastArrivedIndex = arrivedIndex
             }
         }
@@ -90,8 +91,10 @@ object MarkerDrawerManager {
      */
     private fun getArrivedPointIndex(
         routePoints: List<LatLng>,
+        newPoints: MutableList<LatLng>,
         realLon: Double,
-        realLat: Double
+        realLat: Double,
+        heading: Double
     ): Int {
         var currentIndex = 0 //记录疑似点
         if (routePoints.isNotEmpty()) {
@@ -100,6 +103,7 @@ object MarkerDrawerManager {
             var baseDiffDis = CoordinateUtils.calculateLineDistance(
                 realLon, realLat, baseLatLng.longitude, baseLatLng.latitude
             ) // lon,lat, prelon, prelat
+//            val lastIndex = if (lastArrivedIndex >= 1 && lastArrivedIndex <= routePoints.size - 1) lastArrivedIndex else 1
             for (i in 1 until routePoints.size) {
                 val latLng = routePoints[i]
                 val diff = CoordinateUtils.calculateLineDistance(
@@ -107,12 +111,17 @@ object MarkerDrawerManager {
                 )
                 if (baseDiffDis > diff) {
                     baseDiffDis = diff
-                    currentIndex = i
+                    // 距离最近的时候判断是否走过
+                    if (DrivingDirectionUtils.getDegreeOfCar2Poi2(realLon, realLat, latLng.longitude, latLng.latitude, heading) >= 90) {
+                        currentIndex = i
+                    }
                 }
             }
-            return currentIndex
+            newPoints.addAll(routePoints)
+            newPoints.add(currentIndex + 1, LatLng(realLat, realLon))
+            return currentIndex + 1
         }
-        return currentIndex
+        return 0
     }
 
     /**

core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/view/OverMapView.kt

@@ -36,7 +36,7 @@ import com.mogo.eagle.core.function.smp.MakerWithCount
 import com.mogo.eagle.core.function.smp.MarkerDrawerManager
 import com.mogo.eagle.core.function.smp.MarkerDrawerManager.callback
 import com.mogo.eagle.core.function.smp.MarkerDrawerManager.coordinateConverterWgsToGcj
-import com.mogo.eagle.core.function.smp.MarkerDrawerManager.lonLat
+import com.mogo.eagle.core.function.smp.MarkerDrawerManager.lonLatHeading
 import com.mogo.eagle.core.function.smp.MarkerDrawerManager.planningPoints
 import com.mogo.eagle.core.function.smp.MarkerDrawerManager.startLoopCalCarLocation
 import com.mogo.eagle.core.function.smp.MarkerDrawerManager.updateRoutePoints
@@ -636,6 +636,8 @@ class OverMapView @JvmOverloads constructor(
             texIndexList.clear()
         }
         for (i in coordinates.indices) {
+            // 线段数比点数少一个
+            if (i == 0) continue
             if (i <= locIndex) {
                 if (isClearArrived) {
                     textureList.add(transparentBitmap)
@@ -647,7 +649,7 @@ class OverMapView @JvmOverloads constructor(
                 // 未走过的纹理
                 textureList.add(unArrivedBitmap)
             }
-            texIndexList.add(i)
+            texIndexList.add(i - 1)
         }
         if (mAMap != null && coordinates.size > 2) {
             //设置线段纹理
@@ -670,7 +672,7 @@ class OverMapView @JvmOverloads constructor(
     override fun onChassisLocationGCJ02(gnssInfo: MogoLocation?) {
         gnssInfo?.let {
             mLocation = it
-            lonLat = Pair(it.longitude, it.latitude)
+            lonLatHeading = Triple(it.longitude, it.latitude, it.heading)
             drawCarMarker(it)
             if (isFirstLocation) {
                 displayCustomOverView()