[3.3.0]

[feature]
[精确计算最近的轨迹点]

OCH/mogo-och-charter-passenger/src/main/java/com/mogo/och/bus/passenger/model/CharterPassengerModel.kt

@@ -14,7 +14,6 @@ import com.mogo.eagle.core.data.autopilot.AutopilotStatusInfo
 import com.mogo.eagle.core.data.map.MogoLocation
 import com.mogo.eagle.core.function.api.autopilot.IMoGoAutopilotStatusListener
 import com.mogo.eagle.core.function.api.autopilot.IMoGoChassisLocationGCJ02Listener
-import com.mogo.eagle.core.function.api.autopilot.IMoGoChassisLocationWGS84Listener
 import com.mogo.eagle.core.function.api.autopilot.IMoGoPlanningRottingListener
 import com.mogo.eagle.core.function.call.autopilot.*
 import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotControlManager.startAutoPilot
@@ -28,11 +27,11 @@ import com.mogo.eagle.core.utilcode.mogo.logger.scene.SceneConstant
 import com.mogo.eagle.core.utilcode.mogo.logger.scene.SceneConstant.Companion.M_BUS_P
 import com.mogo.eagle.core.utilcode.util.*
 import com.mogo.och.bus.passenger.R
-import com.mogo.och.bus.passenger.bean.LoopInfo
+import com.mogo.och.common.module.manager.loopmanager.LoopInfo
 import com.mogo.och.bus.passenger.bean.response.*
 import com.mogo.och.bus.passenger.callback.*
 import com.mogo.och.bus.passenger.constant.CharterPassengerConst
-import com.mogo.och.bus.passenger.net.BusPassengerModelLoopManager
+import com.mogo.och.common.module.manager.loopmanager.BusPassengerModelLoopManager
 import com.mogo.och.bus.passenger.net.BusPassengerServiceManager
 import com.mogo.och.bus.passenger.utils.ToastCharterUtils
 import com.mogo.och.bus.passenger.utils.VoiceFocusManager
@@ -255,7 +254,7 @@ object CharterPassengerModel {
             }
         }
 
-    fun updateRoutePoints(routePoints: List<MessagePad.Location?>?) {
+    fun updateRoutePoints(routePoints: List<MessagePad.Location>) {
         mRoutePoints = null
         val latLngModels = CoordinateCalculateRouteUtil
             .coordinateConverterWgsToGcjLocations(mContext, routePoints)

OCH/mogo-och-charter-passenger/src/main/java/com/mogo/och/bus/passenger/presenter/BusPassengerFunctionOrderPresenter.kt

@@ -13,7 +13,7 @@ import com.mogo.eagle.core.utilcode.mogo.logger.scene.SceneConstant
 import com.mogo.eagle.core.utilcode.mogo.logger.scene.SceneConstant.Companion.M_BUS_P
 import com.mogo.eagle.core.utilcode.util.GsonUtils
 import com.mogo.och.bus.passenger.utils.ToastCharterUtils
-import com.mogo.och.bus.passenger.bean.LoopInfo
+import com.mogo.och.common.module.manager.loopmanager.LoopInfo
 import com.mogo.och.bus.passenger.bean.event.EventLineSites
 import com.mogo.och.bus.passenger.bean.response.LineInfoListResponse
 import com.mogo.och.bus.passenger.bean.response.LineInfoResponse
@@ -21,7 +21,7 @@ import com.mogo.och.bus.passenger.bean.response.SiteInfoResponse
 import com.mogo.och.bus.passenger.model.CharterPassengerModel
 import com.mogo.och.bus.passenger.model.IOrderStatusChangeListener
 import com.mogo.och.bus.passenger.model.OrderStatusEnum
-import com.mogo.och.bus.passenger.net.BusPassengerModelLoopManager
+import com.mogo.och.common.module.manager.loopmanager.BusPassengerModelLoopManager
 import com.mogo.och.bus.passenger.net.BusPassengerServiceManager
 import com.mogo.och.bus.passenger.ui.dialogfragment.fragment.M1OrderLineFragment
 import com.mogo.och.common.module.bean.dpmsg.BaseDPMsg

OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/manager/loopmanager/BusPassengerModelLoopManager.kt

@@ -1,9 +1,7 @@
-package com.mogo.och.bus.passenger.net
+package com.mogo.och.common.module.manager.loopmanager
 
 import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger
 import com.mogo.eagle.core.utilcode.mogo.logger.scene.SceneConstant.Companion.M_BUS_P
-import com.mogo.och.bus.passenger.bean.LoopInfo
-import com.mogo.och.bus.passenger.constant.CharterPassengerConst
 import com.mogo.och.common.module.utils.RxUtils
 import io.reactivex.Observable
 import io.reactivex.disposables.Disposable
@@ -22,6 +20,9 @@ object BusPassengerModelLoopManager {
 
     private val mControllerStatusCallbackMap = ConcurrentHashMap<String, LoopInfo>()
 
+    const val LOOP_LINE_2S = 2 * 1000L
+    const val LOOP_LINE_1S = 1 * 1000L
+    const val LOOP_DELAY = 100L
 
     fun setLoopFunction(tag: String, function: LoopInfo) {
         if (tag.isBlank()) return
@@ -56,10 +57,7 @@ object BusPassengerModelLoopManager {
             return
         }
         CallerLogger.i(M_BUS_P + TAG, "startQueryDriverLineLoop()")
-        mQueryLineDisposable = Observable.interval(
-            CharterPassengerConst.LOOP_DELAY,
-            CharterPassengerConst.LOOP_LINE_1S, TimeUnit.MILLISECONDS
-        )
+        mQueryLineDisposable = Observable.interval(LOOP_DELAY, LOOP_LINE_1S, TimeUnit.MILLISECONDS)
             .map { aLong: Long -> aLong + 1 }
             .subscribeOn(Schedulers.io())
             .observeOn(Schedulers.io())

OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/manager/loopmanager/LoopInfo.kt

@@ -1,3 +1,3 @@
-package com.mogo.och.bus.passenger.bean
+package com.mogo.och.common.module.manager.loopmanager
 
 data class LoopInfo(val interval:Long,val function: () -> Unit,val immediately:Boolean=false)

OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/manager/trajectorymamager/DistanceDegree.kt

@@ -0,0 +1,13 @@
+package com.mogo.och.common.module.manager.trajectorymamager
+
+data class DistanceDegree(var distance:Float,var degree:Double?,var isNext:Boolean?):Comparable<DistanceDegree>{
+    override fun compareTo(other: DistanceDegree): Int {
+        // 对比面积
+        if(distance == other.distance){
+            return 0;
+        } else if(distance < other.distance){
+            return -1;
+        }
+        return 1;
+    }
+}

OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/manager/trajectorymamager/IDistanceListener.kt

@@ -0,0 +1,38 @@
+package com.mogo.och.common.module.manager.trajectorymamager
+
+import com.mogo.eagle.core.data.map.MogoLocation
+
+interface IDistanceListener {
+    /**
+     * @param distance 距离终点坐标的距离
+     */
+    fun distanceCallback(distance: Float)
+}
+
+interface ITrajectoryListener{
+    /**
+     * @param routeArrivied 已经走过的坐标
+     * @param routeArriving 没有走过的坐标
+     * @param location 车的坐标
+     * @return
+     */
+    fun trajectoryCallback(
+        routeArrivied: MutableList<MogoLocation>,
+        routeArriving: MutableList<MogoLocation>,
+        location: MogoLocation
+    )
+}
+
+interface ITrajectoryWithStationListener{
+    /**
+     * @param routeArrivied 已经走过的坐标 第一个是站点坐标
+     * @param routeArriving 没有走过的坐标 最后一个事站点坐标
+     * @param location 车的坐标
+     * @return
+     */
+    fun trajectoryWithStationCallback(
+        routeArrivied: MutableList<MogoLocation>,
+        routeArriving: MutableList<MogoLocation>,
+        location: MogoLocation
+    )
+}

OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/manager/trajectorymamager/StationAndIndex.kt

@@ -0,0 +1,10 @@
+package com.mogo.och.common.module.manager.trajectorymamager
+
+import com.mogo.eagle.core.data.map.MogoLocation
+
+data class StationAndIndex(
+    var stationPoint: MogoLocation?,// 站点坐标
+    var index: Int?,// 坐标对应轨迹中最近的点
+    var distance: Float?,//轨迹中最近的点
+    var isNext:Boolean?,// 最近的点在轨迹中是在站点的下一个还是上一个
+)

OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/manager/trajectorymamager/TrajectoryAndDistanceManager.kt

@@ -0,0 +1,325 @@
+package com.mogo.och.common.module.manager.trajectorymamager
+
+import com.mogo.commons.AbsMogoApplication
+import com.mogo.eagle.core.data.map.MogoLocation
+import com.mogo.eagle.core.function.api.autopilot.IMoGoPlanningRottingListener
+import com.mogo.eagle.core.function.call.autopilot.CallerChassisLocationGCJ02ListenerManager
+import com.mogo.eagle.core.function.call.autopilot.CallerPlanningRottingListenerManager
+import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger.d
+import com.mogo.eagle.core.utilcode.mogo.logger.scene.SceneConstant
+import com.mogo.eagle.core.utilcode.mogo.logger.scene.SceneConstant.Companion.M_OCHCOMMON
+import com.mogo.eagle.core.utilcode.util.CoordinateUtils
+import com.mogo.och.common.module.manager.loopmanager.BusPassengerModelLoopManager
+import com.mogo.och.common.module.manager.loopmanager.LoopInfo
+import com.mogo.och.common.module.utils.CoordinateCalculateRouteUtil
+import mogo.telematics.pad.MessagePad
+import java.util.ArrayList
+import java.util.concurrent.ConcurrentHashMap
+
+object TrajectoryAndDistanceManager: IMoGoPlanningRottingListener{
+
+    private val distanceListeners: ConcurrentHashMap<String, IDistanceListener> = ConcurrentHashMap()
+    private val trajectoryListeners: ConcurrentHashMap<String, ITrajectoryListener> = ConcurrentHashMap()
+    private val trajectoryWithStationListeners: ConcurrentHashMap<String, ITrajectoryWithStationListener> = ConcurrentHashMap()
+
+    const val TAG = "DistanceManager"
+    const val TAGDISTANCE = "BusPassengerModelDistance"
+
+
+    fun addListener(tag: String, listener: IDistanceListener) {
+        if (distanceListeners.containsKey(tag)) { return }
+        distanceListeners[tag] = listener
+    }
+    fun addListener(tag: String, listener: ITrajectoryListener) {
+        if (trajectoryListeners.containsKey(tag)) { return }
+        trajectoryListeners[tag] = listener
+    }
+    fun addListener(tag: String, listener: ITrajectoryWithStationListener) {
+        if (trajectoryWithStationListeners.containsKey(tag)) { return }
+        trajectoryWithStationListeners[tag] = listener
+    }
+
+    /**
+     * 删除监听
+     * @param tag 标记，用来注销监听使用
+     */
+    fun removeListener(tag: String) {
+        distanceListeners.remove(tag)
+        trajectoryListeners.remove(tag)
+        trajectoryWithStationListeners.remove(tag)
+    }
+
+
+
+    @Volatile
+    private var mRoutePoints: MutableList<MogoLocation>? = ArrayList()
+
+    @Volatile
+    private var endStationInfo: StationAndIndex = StationAndIndex(null, null, null, null)
+
+    @Volatile
+    private var startStationInfo: StationAndIndex = StationAndIndex(null, null, null, null)
+
+    //上一次计算最近点的缓存
+    private var preCarLocationIndexInTrajectory = 0
+
+    init {
+        CallerPlanningRottingListenerManager.addListener(TAG, this)
+    }
+
+    override fun onAutopilotRotting(globalPathResp: MessagePad.GlobalPathResp?) {
+        d(M_OCHCOMMON + TAG, "onAutopilotRotting: 收到轨迹")
+        globalPathResp?.wayPointsList?.let {
+            if (it.size > 0) {
+                d(M_OCHCOMMON + TAG, "收到轨迹:${it.size}第一个点${it[0]}最后一个点:${it.last()}")
+                endCalculateDistanceLoop()
+                updateRoutePoints(it)
+                startCalculateDistanceLoop()
+            }
+        }
+    }
+
+    private fun updateRoutePoints(routePoints: List<MessagePad.Location>?) {
+        mRoutePoints = null
+        val latLngModels = CoordinateCalculateRouteUtil
+            .coordinateConverterWgsToGcjLocations(AbsMogoApplication.getApp(), routePoints!!)
+        mRoutePoints = latLngModels
+    }
+
+    private fun removeTempData() {
+        this.endStationInfo.index = null
+        this.endStationInfo.distance = null
+        this.endStationInfo.isNext = null
+        this.startStationInfo.index = null
+        this.startStationInfo.distance = null
+        this.startStationInfo.isNext = null
+        preCarLocationIndexInTrajectory = 0
+    }
+
+    fun cleanRoutePoints() {
+        mRoutePoints = null
+    }
+
+    /**
+     * 设置或清理站点坐标
+     */
+    fun setStationPoint(startStationInfo: MogoLocation?,endStationInfo: MogoLocation?) {
+        this.endStationInfo.stationPoint = endStationInfo
+        this.startStationInfo.stationPoint = startStationInfo
+        if (this.endStationInfo.stationPoint == null) {
+            removeTempData()
+            endCalculateDistanceLoop()
+            cleanRoutePoints()
+        }else{
+            startCalculateDistanceLoop()
+        }
+    }
+
+    /**
+     * 根据两点距离和轨迹距离来计算真实距离
+     */
+    private fun calculateDistance() {
+        //mLocation gcj坐标
+        CallerChassisLocationGCJ02ListenerManager.getChassisLocationGCJ02().let {
+
+            if (mRoutePoints.isNullOrEmpty() || endStationInfo.stationPoint == null) {
+                d(M_OCHCOMMON + TAG, "没有轨迹或站点坐标停止计算")
+                //结束距离计算
+                endCalculateDistanceLoop()
+                return
+            }
+            d(M_OCHCOMMON + TAG, "站点坐标：${endStationInfo.stationPoint}")
+            calculateRouteSumLength(it)
+        }
+    }
+
+    /**
+     * 启动路距计算
+     */
+    private fun startCalculateDistanceLoop() {
+        BusPassengerModelLoopManager.setLoopFunction(TAGDISTANCE, LoopInfo(1, ::calculateDistance))
+        d(SceneConstant.M_BUS_P + TAG, "开始路距计算")
+    }
+
+    /**
+     * 结束启动路距计算
+     */
+    private fun endCalculateDistanceLoop() {
+        BusPassengerModelLoopManager.removeLoopFunction(TAGDISTANCE)
+        d(SceneConstant.M_BUS_P + TAG, "结束路距计算")
+    }
+
+
+    @Synchronized
+    private fun calculateRouteSumLength(
+        location: MogoLocation,
+    ) {
+        if (mRoutePoints.isNullOrEmpty()) return
+        // 计算起始站点在轨迹中的信息   这个是一个常量
+        if (startStationInfo.stationPoint != null
+            && startStationInfo.isNext == null
+            && startStationInfo.index == null
+            && startStationInfo.distance == null
+        ) {
+            //要前往的站在轨迹中对应的点的信息
+            val startStationInfoInTra = CoordinateCalculateRouteUtil.getNearestPointInfo(
+                preCarLocationIndexInTrajectory, mRoutePoints!!, startStationInfo.stationPoint!!
+            )
+            startStationInfo.isNext = startStationInfoInTra.second
+            startStationInfo.index = startStationInfoInTra.first
+            startStationInfo.distance = startStationInfoInTra.third
+            preCarLocationIndexInTrajectory = startStationInfoInTra.first
+        }
+
+        // 计算车的位置在轨迹中的信息  这个是一个变量可以缓存
+        val carLocationInfo = CoordinateCalculateRouteUtil.getNearestPointInfo(
+            preCarLocationIndexInTrajectory, mRoutePoints!!, location
+        )
+        preCarLocationIndexInTrajectory = carLocationInfo.first
+
+        // 计算结束站点在轨迹中的信息   这个是一个常量可以缓存
+        if (endStationInfo.stationPoint != null
+            && endStationInfo.isNext == null
+            && endStationInfo.index == null
+            && endStationInfo.distance == null
+        ) {
+            //要前往的站在轨迹中对应的点
+            val stationPointInRouteIndex = CoordinateCalculateRouteUtil.getNearestPointInfo(
+                preCarLocationIndexInTrajectory, mRoutePoints!!, endStationInfo.stationPoint!!
+            )
+            endStationInfo.isNext = stationPointInRouteIndex.second
+            endStationInfo.index = stationPointInRouteIndex.first
+            endStationInfo.distance = stationPointInRouteIndex.third
+        }
+        // 距离回调
+        if(distanceListeners.size>0) {
+            invokeDistance(carLocationInfo, location)
+        }
+        // 不带站点轨迹回调
+        if(trajectoryListeners.size>0) {
+            invokeTrajectory(carLocationInfo, location)
+        }
+        // 只展示站点之间轨迹
+        if(trajectoryWithStationListeners.size>0) {
+            invokeTrajectoryWithStation(carLocationInfo, location)
+        }
+    }
+
+
+    private fun invokeDistance(carLocationInfo:Triple<Int,Boolean?,Float>,location: MogoLocation){
+        var lastSumLength = 0f
+
+        val stationIndex = endStationInfo.index?:0
+        if (preCarLocationIndexInTrajectory < stationIndex) {
+            // subList 是[)  需要的是[]
+            val subList = mRoutePoints!!.subList(preCarLocationIndexInTrajectory, stationIndex + 1)
+            // 轨迹点所有的距离
+            lastSumLength = CoordinateCalculateRouteUtil.calculateRouteSumLength(subList)
+            val stationDistance = endStationInfo.distance ?: 0f
+            if (endStationInfo.isNext == true) {// isNext = true
+                lastSumLength -= stationDistance
+            } else {// isNext = false
+                lastSumLength += stationDistance
+            }
+
+            if (carLocationInfo.second == true) {
+                lastSumLength += carLocationInfo.third
+            } else {
+                lastSumLength -= carLocationInfo.third
+            }
+        } else {
+            val lastPoints = mRoutePoints!![endStationInfo.index!!]
+            lastSumLength = CoordinateUtils.calculateLineDistance(
+                lastPoints.longitude, lastPoints.latitude,
+                location.longitude, location.latitude
+            )
+        }
+        if(distanceListeners.size>0) {
+            distanceListeners.forEach {
+                //val tag = it.key
+                val listener = it.value
+                listener.distanceCallback(lastSumLength)
+            }
+        }
+    }
+
+    private fun invokeTrajectory(
+        carLocationInfo: Triple<Int, Boolean?, Float>,
+        location: MogoLocation
+    ) {
+        if (mRoutePoints.isNullOrEmpty()) return
+        val routeArrivied = mutableListOf<MogoLocation>()
+        val routeArriving = mutableListOf<MogoLocation>()
+        if(carLocationInfo.second==true){// isNext = true
+            routeArrivied.addAll(mRoutePoints!!.subList(0,carLocationInfo.first))
+            routeArriving.addAll(mRoutePoints!!.subList(carLocationInfo.first,mRoutePoints!!.size))
+        }else{// isNext = false
+            val indexNext = carLocationInfo.first+1
+            routeArrivied.addAll(mRoutePoints!!.subList(0,indexNext))
+            routeArriving.addAll(mRoutePoints!!.subList(indexNext,mRoutePoints!!.size))
+        }
+        if(trajectoryListeners.size>0) {
+            trajectoryListeners.forEach {
+                //val tag = it.key
+                val listener = it.value
+                listener.trajectoryCallback(routeArrivied, routeArriving, location)
+            }
+        }
+    }
+
+    private fun invokeTrajectoryWithStation(
+        carLocationInfo: Triple<Int, Boolean?, Float>,
+        location: MogoLocation
+    ) {
+        if (mRoutePoints.isNullOrEmpty()) return
+        var tempPoints = mutableListOf<MogoLocation>()
+        val routeArrivied = mutableListOf<MogoLocation>()
+        val routeArriving = mutableListOf<MogoLocation>()
+        if (startStationInfo.stationPoint != null
+            && startStationInfo.isNext != null
+            && startStationInfo.index != null
+            && startStationInfo.distance != null
+        ) {
+            if(startStationInfo.isNext==true){
+                tempPoints.addAll(mRoutePoints!!.subList(startStationInfo.index!!,mRoutePoints!!.size))
+            }else{
+                tempPoints.addAll(mRoutePoints!!.subList(startStationInfo.index!!+1,mRoutePoints!!.size))
+            }
+        }else{
+            tempPoints.addAll(mRoutePoints!!)
+        }
+
+        if (endStationInfo.stationPoint != null
+            && endStationInfo.isNext != null
+            && endStationInfo.index != null
+            && endStationInfo.distance != null
+        ) {
+            val index = endStationInfo.index!!
+            if(endStationInfo.isNext==true){
+                tempPoints = tempPoints.subList(0, index-1)
+            }else{
+                tempPoints = tempPoints.subList(0, index)
+            }
+        }
+
+        if(carLocationInfo.second==true){// isNext = true
+            routeArrivied.addAll(tempPoints.subList(0,carLocationInfo.first))
+            routeArriving.addAll(mRoutePoints!!.subList(carLocationInfo.first,mRoutePoints!!.size))
+        }else{// isNext = false
+            val indexNext = carLocationInfo.first+1
+            routeArrivied.addAll(tempPoints.subList(0,indexNext))
+            routeArriving.addAll(mRoutePoints!!.subList(indexNext,mRoutePoints!!.size))
+        }
+
+        routeArrivied.add(0, startStationInfo.stationPoint!!)
+        routeArriving.add(endStationInfo.stationPoint!!)
+        if(trajectoryWithStationListeners.size>0) {
+            trajectoryWithStationListeners.forEach {
+                //val tag = it.key
+                val listener = it.value
+                listener.trajectoryWithStationCallback(routeArrivied, routeArriving, location)
+            }
+        }
+    }
+
+}

OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/utils/CoordinateCalculateRouteUtil.java

@@ -1,504 +0,0 @@
-package com.mogo.och.common.module.utils;
-
-import android.content.Context;
-import android.location.Location;
-
-import com.amap.api.maps.CoordinateConverter;
-import com.amap.api.maps.model.LatLng;
-import com.mogo.eagle.core.data.map.MogoLocation;
-import com.mogo.eagle.core.utilcode.mogo.logger.Logger;
-import com.mogo.eagle.core.utilcode.util.CoordinateUtils;
-
-import java.util.ArrayList;
-import java.util.HashMap;
-import java.util.List;
-import java.util.Map;
-
-import kotlin.Triple;
-import mogo.telematics.pad.MessagePad;
-
-/**
- * @author: wangmingjun
- * @date: 2022/3/28
- */
-public class CoordinateCalculateRouteUtil {
-
-    public static <T> float calculateRouteSumLength(List<T> points){
-        if (null == points || points.size() == 0) return 0;
-
-        float sumLength = 0;
-
-        if (points.get(0) instanceof MogoLocation){
-            //计算全路径总距离
-            for (int i = 0;i + 1< points.size();i++){
-                MogoLocation locationPre = (MogoLocation) points.get(i);
-                MogoLocation location = (MogoLocation) points.get(i+1);
-                double preLat = locationPre.getLatitude();
-                double preLon = locationPre.getLongitude();
-                double laLat  = location.getLatitude();
-                double laLon = location.getLongitude();
-
-                float length = CoordinateUtils.calculateLineDistance(laLon,laLat,preLon,preLat);
-                sumLength += length;
-            }
-        }else if (points.get(0) instanceof Location){
-            //计算全路径总距离
-            for (int i = 0;i + 1< points.size();i++){
-                Location locationPre = (Location) points.get(i);
-                Location location = (Location) points.get(i+1);
-                double preLat = locationPre.getLatitude();
-                double preLon = locationPre.getLongitude();
-                double laLat  = location.getLatitude();
-                double laLon = location.getLongitude();
-
-                float length = CoordinateUtils.calculateLineDistance(laLon,laLat,preLon,preLat);
-                sumLength += length;
-            }
-        }else if (points.get(0) instanceof LatLng){
-            for (int i = 0;i + 1< points.size();i++){
-                LatLng locationPre = (LatLng) points.get(i);
-                LatLng location = (LatLng) points.get(i+1);
-                double preLat = locationPre.latitude;
-                double preLon = locationPre.longitude;
-                double laLat  = location.latitude;
-                double laLon = location.longitude;
-
-                float length = CoordinateUtils.calculateLineDistance(laLon,laLat,preLon,preLat);
-                sumLength += length;
-            }
-        }
-        return sumLength;
-    }
-
-    public static float calculateRouteSumLength(List<MogoLocation> mRoutePoints, MogoLocation location, MogoLocation station){
-        if (null == mRoutePoints || mRoutePoints.size() == 0) return 0;
-
-        float lastSumLength = 0f;
-
-        //当前位置距离轨迹中最近的点
-        int currentRouteIndex = getArrivedPointIndexNew(
-                0, mRoutePoints, location.getLongitude(), location.getLatitude()
-        );
-        // 距离当前位置轨迹中最近的轨迹点坐标
-        MogoLocation currentPoint = mRoutePoints.get(currentRouteIndex);
-        // 当前位置距离最近的点的距离
-        float calculateCurrentdex = CoordinateUtils.calculateLineDistance(
-                location.getLongitude(), location.getLatitude(),
-                currentPoint.getLongitude(), currentPoint.getLatitude()
-        );
-
-
-        //要前往的站在轨迹中对应的点
-        int stationPointInRouteIndex = getArrivedPointIndexNew(
-                currentRouteIndex, mRoutePoints,
-                station.getLongitude(),
-                station.getLatitude()
-        );
-        // 距离站点最近的轨迹点
-        MogoLocation stationPointInRoute = mRoutePoints.get(stationPointInRouteIndex);
-        // 站点距离轨迹中最近点的距离
-        float calculateLineDistance = CoordinateUtils.calculateLineDistance(
-                stationPointInRoute.getLongitude(), stationPointInRoute.getLatitude(),
-                station.getLongitude(), station.getLatitude()
-        );
-
-        if (currentRouteIndex < stationPointInRouteIndex) {
-            // subList 是[)  需要的是[]
-            List<MogoLocation> subList = mRoutePoints.subList(currentRouteIndex, stationPointInRouteIndex + 1);
-            // 轨迹点所有的距离
-            lastSumLength = calculateRouteSumLength(subList);
-            // region 站点坐标和 站点坐标对应轨迹点的坐标距离
-            // 需要加距离 和下一个轨迹点成钝角
-            if (stationPointInRouteIndex + 1 < mRoutePoints.size()) {
-                MogoLocation lastPointsNext = mRoutePoints.get(stationPointInRouteIndex + 1);
-                double degree = getDegree(
-                        station.getLongitude(),station.getLatitude(),
-                        stationPointInRoute.getLongitude(), stationPointInRoute.getLatitude(),
-                        lastPointsNext.getLongitude(), lastPointsNext.getLatitude());
-                if (degree > 90) {
-                    lastSumLength = lastSumLength + calculateLineDistance;
-                }
-            }
-            // 需要减距离 和上一个轨迹点成钝角
-            if (stationPointInRouteIndex - 1 >= 0) {
-                MogoLocation lastPointsPre = mRoutePoints.get(stationPointInRouteIndex - 1);
-                double degree = getDegree(
-                        station.getLongitude(),station.getLatitude(),
-                        stationPointInRoute.getLongitude(), stationPointInRoute.getLatitude(),
-                        lastPointsPre.getLongitude(), lastPointsPre.getLatitude());
-                if (degree > 90) {
-                    lastSumLength = lastSumLength - calculateLineDistance;
-                }
-            }
-            // endregion
-
-            // region 当前位置和 对应轨迹点的坐标距离
-            // 需要加距离 和下一个轨迹点成钝角
-            if (currentRouteIndex + 1 < stationPointInRouteIndex) {
-                MogoLocation currentPointsNext = mRoutePoints.get(currentRouteIndex + 1);
-                double degree = getDegree(
-                        location.getLongitude(),location.getLatitude(),
-                        currentPoint.getLongitude(), currentPoint.getLatitude(),
-                        currentPointsNext.getLongitude(), currentPointsNext.getLatitude());
-                if (degree > 90) {
-                    lastSumLength = lastSumLength - calculateCurrentdex;
-                }
-            }
-
-            // 需要减距离 和上一个轨迹点成钝角
-            if (currentRouteIndex - 1 >= 0) {
-                MogoLocation lastPointsPre = mRoutePoints.get(currentRouteIndex - 1);
-                double degree = getDegree(
-                        location.getLongitude(),location.getLatitude(),
-                        currentPoint.getLongitude(), currentPoint.getLatitude(),
-                        lastPointsPre.getLongitude(), lastPointsPre.getLatitude());
-                if (degree > 90) {
-                    lastSumLength = lastSumLength + calculateCurrentdex;
-                }
-            }
-            // endregion
-
-        } else {
-            MogoLocation lastPoints = mRoutePoints.get(stationPointInRouteIndex);
-            lastSumLength = CoordinateUtils.calculateLineDistance(
-                    lastPoints.getLongitude(), lastPoints.getLatitude(),
-                    location.getLongitude(), location.getLatitude()
-            );
-        }
-        return lastSumLength;
-    }
-
-    public static List<LatLng> coordinateConverterWgsToGcjListCommon(Context mContext, List<MessagePad.Location> models) {
-        //转成MogoLatLng集合
-        List<LatLng> list = new ArrayList<>();
-        for (MessagePad.Location m : models) {
-            LatLng mogoLatLng = coordinateConverterWgsToGcj(mContext, m);
-            list.add(mogoLatLng);
-        }
-        return list;
-    }
-
-    public static LatLng coordinateConverterWgsToGcj(Context mContext, MessagePad.Location mogoLatLng) {
-        CoordinateConverter mCoordinateConverter = new CoordinateConverter(mContext);
-        mCoordinateConverter.from(CoordinateConverter.CoordType.GPS);
-        mCoordinateConverter.coord(new LatLng(mogoLatLng.getLatitude(), mogoLatLng.getLongitude()));
-        LatLng latLng = mCoordinateConverter.convert();
-        return latLng;
-    }
-
-    public static LatLng coordinateConverterWgsToGcj(Context mContext, double lon, double lat) {
-        CoordinateConverter mCoordinateConverter = new CoordinateConverter(mContext);
-        mCoordinateConverter.from(CoordinateConverter.CoordType.GPS);
-        mCoordinateConverter.coord(new LatLng(lat,lon));
-        LatLng latLng = mCoordinateConverter.convert();
-        return latLng;
-    }
-
-    /**
-     * 简单粗暴 直接比较 todo 需要优化
-     * @param mRoutePoints
-     * @param realLon
-     * @param realLat
-     * @return
-     */
-    public static List<LatLng> getRemainPointListByCompare(List<LatLng> mRoutePoints,double realLon,double realLat) {
-        List<LatLng> latePoints = new ArrayList<>();
-        int currentIndex = 0; //记录疑似点
-        if (mRoutePoints.size() > 0){
-            //基础点
-            LatLng baseLatLng = mRoutePoints.get(0);
-            if (baseLatLng != null){
-                float baseDiffDis = CoordinateUtils.calculateLineDistance(realLon,realLat
-                        ,baseLatLng.longitude,baseLatLng.latitude);// lon,lat, prelon, prelat
-
-                for (int i= 1; i < mRoutePoints.size(); i++){
-                    LatLng latLng = mRoutePoints.get(i);
-                    float diff = CoordinateUtils.calculateLineDistance(realLon,realLat
-                            ,latLng.longitude,latLng.latitude);
-                    if (baseDiffDis > diff){
-//                    Logger.d(M_TAXI + "calculateRouteSumLength", "点："+i+"-------先记录点----- ");
-                        baseDiffDis = diff;
-                        currentIndex = i;
-                    }
-                }
-//            Logger.d(M_TAXI + "calculateRouteSumLength", "点："+currentIndex+"-------是最近的点------ ");
-                if (currentIndex == mRoutePoints.size()-1){
-                    latePoints.add(mRoutePoints.get(currentIndex));
-                }else if(currentIndex < mRoutePoints.size()-1){
-                    latePoints.addAll(mRoutePoints.subList(currentIndex,mRoutePoints.size()));
-                }
-                return latePoints;
-            }
-
-        }
-        return latePoints;
-    }
-
-    /**
-     * 简单粗暴 直接比较 todo 需要优化
-     * @param mRoutePoints
-     * @param realLon
-     * @param realLat
-     * @return 返回已经到达点的index
-     */
-    public static int getArrivedPointIndex(List<LatLng> mRoutePoints,double realLon,double realLat) {
-        int currentIndex = 0; //记录疑似点
-        if (mRoutePoints.size() > 0){
-            //基础点
-            LatLng baseLatLng = mRoutePoints.get(0);
-            if (baseLatLng != null){
-                float baseDiffDis = CoordinateUtils.calculateLineDistance(realLon,realLat
-                        ,baseLatLng.longitude,baseLatLng.latitude);// lon,lat, prelon, prelat
-
-                for (int i= 1; i < mRoutePoints.size(); i++){
-                    LatLng latLng = mRoutePoints.get(i);
-                    float diff = CoordinateUtils.calculateLineDistance(realLon,realLat
-                            ,latLng.longitude,latLng.latitude);
-                    if (baseDiffDis > diff){
-//                    Logger.d(M_TAXI + "calculateRouteSumLength", "点："+i+"-------先记录点----- ");
-                        baseDiffDis = diff;
-                        currentIndex = i;
-                    }
-                }
-                return currentIndex;
-            }
-        }
-        return currentIndex;
-    }
-
-    public static float calculateRouteSumLengthByLocation(List<Location> points){
-        if (null == points || points.size() == 0) return 0;
-
-        float sumLength = 0;
-
-        //计算全路径总距离
-        for (int i = 0;i + 1< points.size();i++){
-            double preLat = points.get(i).getLatitude();
-            double preLon = points.get(i).getLongitude();
-            double laLat  = points.get(i+1).getLatitude();
-            double laLon = points.get(i+1).getLongitude();
-
-            float length = CoordinateUtils.calculateLineDistance(laLon,laLat,preLon,preLat);
-            sumLength += length;
-        }
-        return sumLength;
-    }
-
-
-    public static List<MogoLocation> coordinateConverterWgsToGcjLocations(Context mContext, List<MessagePad.Location> models) {
-        //转成MogoLatLng集合
-        List<MogoLocation> list = new ArrayList<>();
-        for (MessagePad.Location m : models) {
-            LatLng mogoLatLng = coordinateConverterWgsToGcj(mContext, m);
-            MogoLocation location = new MogoLocation();
-            location.setHeading((float) m.getHeading());
-            location.setLatitude(mogoLatLng.latitude);
-            location.setLongitude(mogoLatLng.longitude);
-            list.add(location);
-        }
-        return list;
-    }
-
-    public static List<MogoLocation> coordinateConverterLatlngToLocation(List<LatLng> models) {
-        //转成MogoLatLng集合
-        List<MogoLocation> list = new ArrayList<>();
-        for (LatLng m : models) {
-            MogoLocation location = new MogoLocation();
-            location.setLatitude(m.latitude);
-            location.setLongitude(m.longitude);
-            list.add(location);
-        }
-        return list;
-    }
-
-    public static List<LatLng> coordinateConverterLocationToLatLng(Context mContext, List<MogoLocation> models) {
-        //转成MogoLatLng集合
-        List<LatLng> list = new ArrayList<>();
-        for (MogoLocation m : models) {
-            LatLng mogoLatLng = new LatLng(m.getLatitude(), m.getLongitude());
-            list.add(mogoLatLng);
-        }
-        return list;
-    }
-
-    /**
-     * 根据前一个index，经纬度航向角，确认剩余轨迹
-     * @param preIndex
-     * @param mRoutePoints
-     * @param realLocation
-     * @return
-     */
-    public static Map<Integer,List<MogoLocation>> getRemainPointListByCompareNew(int preIndex,
-                                                                           List<MogoLocation> mRoutePoints,
-                                                                                 MogoLocation  realLocation) {
-        Map<Integer,List<MogoLocation>> routePonits = new HashMap<>();
-        List<MogoLocation> latePoints = new ArrayList<>(); // 剩余轨迹集合
-        int currentIndex = 0; //记录疑似点
-        if (mRoutePoints.size() > 0){
-            //基础点
-            MogoLocation baseLatLng = mRoutePoints.get(0);
-            float baseDiffDis = CoordinateUtils.calculateLineDistance(realLocation.getLongitude(),
-                    realLocation.getLatitude()
-                    ,baseLatLng.getLongitude(),baseLatLng.getLongitude());// lon,lat, prelon, prelat
-
-            for (int i= 0; i < mRoutePoints.size(); i++){
-                MogoLocation latLng = mRoutePoints.get(i);
-                //todo 先看index对应点的方向和realLocation方向是否一致， 方向角度不能过90度
-                if (realLocation.getHeading() == realLocation.getHeading() - latLng.getHeading() ||
-                        Math.abs(realLocation.getHeading() - latLng.getHeading()) <= 90){
-                    float diff = CoordinateUtils.calculateLineDistance(realLocation.getLongitude(),
-                            realLocation.getLatitude(),
-                            latLng.getLongitude(),latLng.getLatitude());
-                    if (baseDiffDis > diff ){
-//                    Logger.d(M_TAXI + "calculateRouteSumLength", "点："+i+"-------先记录点----- ");
-                        baseDiffDis = diff;
-                        currentIndex = i;
-                    }
-                }
-            }
-            Logger.d( "calculateRouteSumLength", "点："+currentIndex+"-------是最近的点------ ");
-            if (currentIndex == mRoutePoints.size()-1){
-                MogoLocation location = mRoutePoints.get(currentIndex);
-//                LatLng latLng = new LatLng(location.getLatitude(), location.getLongitude());
-                latePoints.add(location);
-            }else {
-                List<MogoLocation> locations = mRoutePoints.subList(currentIndex,mRoutePoints.size());
-                for (MogoLocation location: locations) {
-//                    LatLng latLng = new LatLng(location.getLatitude(), location.getLongitude());
-                    latePoints.add(location);
-                }
-            }
-            routePonits.put(currentIndex,latePoints);
-            return routePonits;
-        }
-        return routePonits;
-    }
-
-    public static int getArrivedPointIndexNew(int preIndex, List<MogoLocation> mRoutePoints,
-                                              MogoLocation  realLocation) {
-        int currentIndex = 0; //记录疑似点         //基础点
-        MogoLocation baseLatLng = mRoutePoints.get(0);
-        float baseDiffDis = CoordinateUtils.calculateLineDistance(realLocation.getLongitude(),
-                realLocation.getLatitude()
-                , baseLatLng.getLongitude(), baseLatLng.getLongitude());// lon,lat, prelon, prelat
-
-        for (int i = 0; i < mRoutePoints.size(); i++) {
-            MogoLocation latLng = mRoutePoints.get(i);
-            if (realLocation.getHeading() == realLocation.getHeading() - latLng.getHeading() ||
-                    Math.abs(realLocation.getHeading() - latLng.getHeading()) <= 90){
-            float diff = CoordinateUtils.calculateLineDistance(realLocation.getLongitude(),
-                    realLocation.getLatitude(),
-                    latLng.getLongitude(), latLng.getLatitude());
-            if (baseDiffDis > diff && i>currentIndex) {
-//                    Logger.d(M_TAXI + "calculateRouteSumLength", "点："+i+"-------先记录点----- ");
-                baseDiffDis = diff;
-                currentIndex = i;
-            }
-            }
-        }
-        Logger.d("calculateRouteSumLength", "点：" + currentIndex + "-------是最近的点------ ");
-        return currentIndex;
-    }
-
-    public static int getArrivedPointIndexNew(int preIndex, List<MogoLocation> mRoutePoints,
-                                              double realLon,double realLat) {
-        int currentIndex = preIndex; //记录疑似点         //基础点
-        MogoLocation baseLatLng = mRoutePoints.get(0);
-        float baseDiffDis = CoordinateUtils.calculateLineDistance(realLon,
-                realLat
-                , baseLatLng.getLongitude(), baseLatLng.getLongitude());// lon,lat, prelon, prelat
-
-        for (int i = preIndex; i < mRoutePoints.size(); i++) {
-            MogoLocation latLng = mRoutePoints.get(i);
-//            if (realLocation.getBearing() == realLocation.getBearing() - latLng.getBearing() ||
-//                    Math.abs(realLocation.getBearing() - latLng.getBearing()) <= 90){
-                float diff = CoordinateUtils.calculateLineDistance(realLon,
-                        realLat,
-                        latLng.getLongitude(), latLng.getLatitude());
-                if (baseDiffDis > diff && i>currentIndex) {
-//                    Logger.d(M_TAXI + "calculateRouteSumLength", "点："+i+"-------先记录点----- ");
-                    baseDiffDis = diff;
-                    currentIndex = i;
-                }
-//            }
-        }
-        Logger.d("calculateRouteSumLength", "点：" + currentIndex + "-------是最近的点------ ");
-        return currentIndex;
-    }
-
-    /**
-     * https://blog.csdn.net/Jeanne_0523/article/details/106056255
-     * @param vertexPointX
-     * @param vertexPointY
-     * @param point0X  角
-     * @param point0Y  角
-     * @param point1X
-     * @param point1Y
-     * @return
-     */
-    public static int getDegree(double vertexPointX, double vertexPointY, double point0X, double point0Y, double point1X, double point1Y) {
-        //向量的点乘
-        double vector = (point0X - vertexPointX) * (point1X - vertexPointX) + (point0Y - vertexPointY) * (point1Y - vertexPointY);
-        //向量的模乘
-        double sqrt = Math.sqrt(
-                (Math.abs((point0X - vertexPointX) * (point0X - vertexPointX)) + Math.abs((point0Y - vertexPointY) * (point0Y - vertexPointY)))
-                        * (Math.abs((point1X - vertexPointX) * (point1X - vertexPointX)) + Math.abs((point1Y - vertexPointY) * (point1Y - vertexPointY)))
-        );
-        //反余弦计算弧度
-        double radian = Math.acos(vector / sqrt);
-        //弧度转角度制
-        return (int) (180 * radian / Math.PI);
-    }
-
-
-    private static Triple<Double,Double,Double> ball2xyz(Double thera,Double fie,Double r){
-        double x = r * Math.cos(thera) * Math.cos(fie);
-        double y = r * Math.cos(thera) * Math.sin(fie);
-        double z = r * Math.sin(thera);
-        return new Triple(x,y,z);
-    }
-
-    /**
-     * https://blog.csdn.net/reborn_lee/article/details/82497577
-     * 将地理经纬度转换成笛卡尔坐标系
-     */
-    private static Triple<Double,Double,Double>  geo2xyz(double lat,double lng){
-        double thera = (Math.PI * lat) / 180;
-        double fie = (Math.PI * lng) / 180;
-        return ball2xyz(thera, fie,6400.0);
-    }
-
-    /**
-     * 计算3个地理坐标点之间的夹角
-     * @param l1 顶点坐标
-     * @param l2
-     * @param l3
-     * @return l1为顶点的角度 精度没有angleOflocation高
-     */
-    public static double getDegree(LatLng l2,LatLng l1,LatLng l3) {
-        Triple<Double,Double,Double> p1 = geo2xyz(l1.latitude,l1.longitude);
-        Triple<Double,Double,Double> p2 = geo2xyz(l2.latitude,l2.longitude);
-        Triple<Double,Double,Double> p3 = geo2xyz(l3.latitude,l3.longitude);
-
-        double x1 = p1.getFirst();
-        double y1 = p1.getSecond();
-        double z1 = p1.getThird();
-
-        double x2 = p2.getFirst();
-        double y2 = p2.getSecond();
-        double z2 = p2.getThird();
-
-        double x3 = p3.getFirst();
-        double y3 = p3.getSecond();
-        double z3 = p3.getThird();
-
-        // 计算向量 P2P1 和 P2P3 的夹角 https://www.zybang.com/question/3379a30c0dd3041b3ef966803f0bf758.html
-        double p1P2 = Math.sqrt(Math.pow(x2 - x1,2.0) + Math.pow(y2 - y1,2.0) + Math.pow(z2 - z1,2.0));
-        double p2p3 = Math.sqrt(Math.pow(x3 - x2,2.0) + Math.pow(y3 - y2,2.0) + Math.pow(z3 - z2,2.0));
-
-        double p = (x1 - x2) * (x3 - x2) + (y1 - y2) * (y3 - y2) + (z1 - z2) * (z3 - z2); //P2P1*P2P3
-
-        return (Math.acos(p / (p1P2 * p2p3)) / Math.PI) * 180;
-    }
-
-}

OCH/mogo-och-common-module/src/main/java/com/mogo/och/common/module/utils/CoordinateCalculateRouteUtil.kt

@@ -0,0 +1,701 @@
+package com.mogo.och.common.module.utils
+
+import android.content.Context
+import android.location.Location
+import com.amap.api.maps.CoordinateConverter
+import com.amap.api.maps.model.LatLng
+import com.mogo.eagle.core.data.map.MogoLocation
+import com.mogo.eagle.core.utilcode.mogo.logger.Logger
+import com.mogo.eagle.core.utilcode.util.CoordinateUtils
+import com.mogo.eagle.core.utilcode.util.DrivingDirectionUtils
+import com.mogo.och.common.module.manager.trajectorymamager.DistanceDegree
+import mogo.telematics.pad.MessagePad
+import java.util.TreeMap
+import kotlin.math.acos
+import kotlin.math.cos
+import kotlin.math.pow
+import kotlin.math.sin
+import kotlin.math.sqrt
+
+/**
+ * @author: wangmingjun
+ * @date: 2022/3/28
+ */
+object CoordinateCalculateRouteUtil {
+    @JvmStatic
+    fun <T> calculateRouteSumLength(points: List<T>?): Float {
+        if (null == points || points.size == 0) return 0f
+        var sumLength = 0f
+        if (points[0] is MogoLocation) {
+            //计算全路径总距离
+            var i = 0
+            while (i + 1 < points.size) {
+                val locationPre = points[i] as MogoLocation
+                val location = points[i + 1] as MogoLocation
+                val preLat = locationPre.latitude
+                val preLon = locationPre.longitude
+                val laLat = location.latitude
+                val laLon = location.longitude
+                val length = CoordinateUtils.calculateLineDistance(laLon, laLat, preLon, preLat)
+                sumLength += length
+                i++
+            }
+        } else if (points[0] is Location) {
+            //计算全路径总距离
+            var i = 0
+            while (i + 1 < points.size) {
+                val locationPre = points[i] as Location
+                val location = points[i + 1] as Location
+                val preLat = locationPre.latitude
+                val preLon = locationPre.longitude
+                val laLat = location.latitude
+                val laLon = location.longitude
+                val length = CoordinateUtils.calculateLineDistance(laLon, laLat, preLon, preLat)
+                sumLength += length
+                i++
+            }
+        } else if (points[0] is LatLng) {
+            var i = 0
+            while (i + 1 < points.size) {
+                val locationPre = points[i] as LatLng
+                val location = points[i + 1] as LatLng
+                val preLat = locationPre.latitude
+                val preLon = locationPre.longitude
+                val laLat = location.latitude
+                val laLon = location.longitude
+                val length = CoordinateUtils.calculateLineDistance(laLon, laLat, preLon, preLat)
+                sumLength += length
+                i++
+            }
+        }
+        return sumLength
+    }
+
+    fun calculateRouteSumLength(
+        mRoutePoints: List<MogoLocation>?,
+        location: MogoLocation,
+        station: MogoLocation
+    ): Float {
+        if (null == mRoutePoints || mRoutePoints.size == 0) return 0f
+        var lastSumLength = 0f
+
+        //当前位置距离轨迹中最近的点
+        val currentRouteIndex = getArrivedPointIndexNew(
+            0, mRoutePoints, location.longitude, location.latitude
+        )
+        // 距离当前位置轨迹中最近的轨迹点坐标
+        val currentPoint = mRoutePoints[currentRouteIndex]
+        // 当前位置距离最近的点的距离
+        val calculateCurrentdex = CoordinateUtils.calculateLineDistance(
+            location.longitude, location.latitude,
+            currentPoint.longitude, currentPoint.latitude
+        )
+
+
+        //要前往的站在轨迹中对应的点
+        val stationPointInRouteIndex = getArrivedPointIndexNew(
+            currentRouteIndex, mRoutePoints,
+            station.longitude,
+            station.latitude
+        )
+        // 距离站点最近的轨迹点
+        val stationPointInRoute = mRoutePoints[stationPointInRouteIndex]
+        // 站点距离轨迹中最近点的距离
+        val calculateLineDistance = CoordinateUtils.calculateLineDistance(
+            stationPointInRoute.longitude, stationPointInRoute.latitude,
+            station.longitude, station.latitude
+        )
+        if (currentRouteIndex < stationPointInRouteIndex) {
+            // subList 是[)  需要的是[]
+            val subList = mRoutePoints.subList(currentRouteIndex, stationPointInRouteIndex + 1)
+            // 轨迹点所有的距离
+            lastSumLength = calculateRouteSumLength(subList)
+            // region 站点坐标和 站点坐标对应轨迹点的坐标距离
+            // 需要加距离 和下一个轨迹点成钝角
+            if (stationPointInRouteIndex + 1 < mRoutePoints.size) {
+                val lastPointsNext = mRoutePoints[stationPointInRouteIndex + 1]
+                val degree = getDegree(
+                    station.longitude, station.latitude,
+                    stationPointInRoute.longitude, stationPointInRoute.latitude,
+                    lastPointsNext.longitude, lastPointsNext.latitude
+                ).toDouble()
+                if (degree > 90) {
+                    lastSumLength = lastSumLength + calculateLineDistance
+                }
+            }
+            // 需要减距离 和上一个轨迹点成钝角
+            if (stationPointInRouteIndex - 1 >= 0) {
+                val lastPointsPre = mRoutePoints[stationPointInRouteIndex - 1]
+                val degree = getDegree(
+                    station.longitude, station.latitude,
+                    stationPointInRoute.longitude, stationPointInRoute.latitude,
+                    lastPointsPre.longitude, lastPointsPre.latitude
+                ).toDouble()
+                if (degree > 90) {
+                    lastSumLength = lastSumLength - calculateLineDistance
+                }
+            }
+            // endregion
+
+            // region 当前位置和 对应轨迹点的坐标距离
+            // 需要加距离 和下一个轨迹点成钝角
+            if (currentRouteIndex + 1 < stationPointInRouteIndex) {
+                val currentPointsNext = mRoutePoints[currentRouteIndex + 1]
+                val degree = getDegree(
+                    location.longitude, location.latitude,
+                    currentPoint.longitude, currentPoint.latitude,
+                    currentPointsNext.longitude, currentPointsNext.latitude
+                ).toDouble()
+                if (degree > 90) {
+                    lastSumLength = lastSumLength - calculateCurrentdex
+                }
+            }
+
+            // 需要减距离 和上一个轨迹点成钝角
+            if (currentRouteIndex - 1 >= 0) {
+                val lastPointsPre = mRoutePoints[currentRouteIndex - 1]
+                val degree = getDegree(
+                    location.longitude, location.latitude,
+                    currentPoint.longitude, currentPoint.latitude,
+                    lastPointsPre.longitude, lastPointsPre.latitude
+                ).toDouble()
+                if (degree > 90) {
+                    lastSumLength = lastSumLength + calculateCurrentdex
+                }
+            }
+            // endregion
+        } else {
+            val lastPoints = mRoutePoints[stationPointInRouteIndex]
+            lastSumLength = CoordinateUtils.calculateLineDistance(
+                lastPoints.longitude, lastPoints.latitude,
+                location.longitude, location.latitude
+            )
+        }
+        return lastSumLength
+    }
+
+    @JvmStatic
+    fun coordinateConverterWgsToGcjListCommon(
+        mContext: Context?,
+        models: List<MessagePad.Location>
+    ): List<LatLng> {
+        //转成MogoLatLng集合
+        val list: MutableList<LatLng> = ArrayList()
+        for (m in models) {
+            val mogoLatLng = coordinateConverterWgsToGcj(mContext, m)
+            list.add(mogoLatLng)
+        }
+        return list
+    }
+
+    @JvmStatic
+    fun coordinateConverterWgsToGcj(
+        mContext: Context?,
+        mogoLatLng: MessagePad.Location
+    ): LatLng {
+        val mCoordinateConverter =
+            CoordinateConverter(mContext)
+        mCoordinateConverter.from(CoordinateConverter.CoordType.GPS)
+        mCoordinateConverter.coord(
+            LatLng(
+                mogoLatLng.latitude,
+                mogoLatLng.longitude
+            )
+        )
+        return mCoordinateConverter.convert()
+    }
+
+    @JvmStatic
+    fun coordinateConverterWgsToGcj(
+        mContext: Context?,
+        lon: Double,
+        lat: Double
+    ): LatLng {
+        val mCoordinateConverter =
+            CoordinateConverter(mContext)
+        mCoordinateConverter.from(CoordinateConverter.CoordType.GPS)
+        mCoordinateConverter.coord(LatLng(lat, lon))
+        return mCoordinateConverter.convert()
+    }
+
+    /**
+     * 简单粗暴 直接比较 todo 需要优化
+     * @param mRoutePoints
+     * @param realLon
+     * @param realLat
+     * @return
+     */
+    fun getRemainPointListByCompare(
+        mRoutePoints: List<LatLng>,
+        realLon: Double,
+        realLat: Double
+    ): List<LatLng> {
+        val latePoints: MutableList<LatLng> = ArrayList()
+        var currentIndex = 0 //记录疑似点
+        if (mRoutePoints.size > 0) {
+            //基础点
+            val baseLatLng = mRoutePoints[0]
+            if (baseLatLng != null) {
+                var baseDiffDis = CoordinateUtils.calculateLineDistance(
+                    realLon, realLat, baseLatLng.longitude, baseLatLng.latitude
+                ) // lon,lat, prelon, prelat
+                for (i in 1 until mRoutePoints.size) {
+                    val latLng = mRoutePoints[i]
+                    val diff = CoordinateUtils.calculateLineDistance(
+                        realLon, realLat, latLng.longitude, latLng.latitude
+                    )
+                    if (baseDiffDis > diff) {
+//                    Logger.d(M_TAXI + "calculateRouteSumLength", "点："+i+"-------先记录点----- ");
+                        baseDiffDis = diff
+                        currentIndex = i
+                    }
+                }
+                //            Logger.d(M_TAXI + "calculateRouteSumLength", "点："+currentIndex+"-------是最近的点------ ");
+                if (currentIndex == mRoutePoints.size - 1) {
+                    latePoints.add(mRoutePoints[currentIndex])
+                } else if (currentIndex < mRoutePoints.size - 1) {
+                    latePoints.addAll(mRoutePoints.subList(currentIndex, mRoutePoints.size))
+                }
+                return latePoints
+            }
+        }
+        return latePoints
+    }
+
+    /**
+     * 简单粗暴 直接比较 todo 需要优化
+     * @param mRoutePoints
+     * @param realLon
+     * @param realLat
+     * @return 返回已经到达点的index
+     */
+    fun getArrivedPointIndex(mRoutePoints: List<LatLng>, realLon: Double, realLat: Double): Int {
+        var currentIndex = 0 //记录疑似点
+        if (mRoutePoints.size > 0) {
+            //基础点
+            val baseLatLng = mRoutePoints[0]
+            if (baseLatLng != null) {
+                var baseDiffDis = CoordinateUtils.calculateLineDistance(
+                    realLon, realLat, baseLatLng.longitude, baseLatLng.latitude
+                ) // lon,lat, prelon, prelat
+                for (i in 1 until mRoutePoints.size) {
+                    val latLng = mRoutePoints[i]
+                    val diff = CoordinateUtils.calculateLineDistance(
+                        realLon, realLat, latLng.longitude, latLng.latitude
+                    )
+                    if (baseDiffDis > diff) {
+//                    Logger.d(M_TAXI + "calculateRouteSumLength", "点："+i+"-------先记录点----- ");
+                        baseDiffDis = diff
+                        currentIndex = i
+                    }
+                }
+                return currentIndex
+            }
+        }
+        return currentIndex
+    }
+
+    fun calculateRouteSumLengthByLocation(points: List<Location>?): Float {
+        if (points.isNullOrEmpty()) return 0f
+        var sumLength = 0f
+
+        //计算全路径总距离
+        var i = 0
+        while (i + 1 < points.size) {
+            val preLat = points[i].latitude
+            val preLon = points[i].longitude
+            val laLat = points[i + 1].latitude
+            val laLon = points[i + 1].longitude
+            val length = CoordinateUtils.calculateLineDistance(laLon, laLat, preLon, preLat)
+            sumLength += length
+            i++
+        }
+        return sumLength
+    }
+
+    @JvmStatic
+    fun coordinateConverterWgsToGcjLocations(
+        mContext: Context?,
+        models: List<MessagePad.Location>
+    ): MutableList<MogoLocation> {
+        //转成MogoLatLng集合
+        val list = mutableListOf<MogoLocation>()
+        for (m in models) {
+            val mogoLatLng = coordinateConverterWgsToGcj(mContext, m)
+            val location = MogoLocation()
+            location.heading = m.heading.toFloat().toDouble()
+            location.latitude = mogoLatLng.latitude
+            location.longitude = mogoLatLng.longitude
+            list.add(location)
+        }
+        return list
+    }
+
+    @JvmStatic
+    fun coordinateConverterLatlngToLocation(models: List<LatLng>): List<MogoLocation> {
+        //转成MogoLatLng集合
+        val list: MutableList<MogoLocation> = ArrayList()
+        for (m in models) {
+            val location = MogoLocation()
+            location.latitude = m.latitude
+            location.longitude = m.longitude
+            list.add(location)
+        }
+        return list
+    }
+
+    @JvmStatic
+    fun coordinateConverterLocationToLatLng(
+        mContext: Context?,
+        models: List<MogoLocation>
+    ): List<LatLng> {
+        //转成MogoLatLng集合
+        val list: MutableList<LatLng> = ArrayList()
+        for (m in models) {
+            val mogoLatLng = LatLng(m.latitude, m.longitude)
+            list.add(mogoLatLng)
+        }
+        return list
+    }
+
+    /**
+     * 根据前一个index，经纬度航向角，确认剩余轨迹
+     * @param preIndex
+     * @param mRoutePoints
+     * @param realLocation
+     * @return
+     */
+    @JvmStatic
+    fun getRemainPointListByCompareNew(
+        preIndex: Int,
+        mRoutePoints: List<MogoLocation>,
+        realLocation: MogoLocation
+    ): Map<Int, List<MogoLocation>> {
+        val routePonits: MutableMap<Int, List<MogoLocation>> = HashMap()
+        val latePoints: MutableList<MogoLocation> = ArrayList() // 剩余轨迹集合
+        var currentIndex = 0 //记录疑似点
+        if (mRoutePoints.size > 0) {
+            //基础点
+            val baseLatLng = mRoutePoints[0]
+            var baseDiffDis = CoordinateUtils.calculateLineDistance(
+                realLocation.longitude,
+                realLocation.latitude, baseLatLng.longitude, baseLatLng.longitude
+            ) // lon,lat, prelon, prelat
+            for (i in mRoutePoints.indices) {
+                val latLng = mRoutePoints[i]
+                //todo 先看index对应点的方向和realLocation方向是否一致， 方向角度不能过90度
+                if (realLocation.heading == realLocation.heading - latLng.heading ||
+                    Math.abs(realLocation.heading - latLng.heading) <= 90
+                ) {
+                    val diff = CoordinateUtils.calculateLineDistance(
+                        realLocation.longitude,
+                        realLocation.latitude,
+                        latLng.longitude, latLng.latitude
+                    )
+                    if (baseDiffDis > diff) {
+                        baseDiffDis = diff
+                        currentIndex = i
+                    }
+                }
+            }
+            Logger.d("calculateRouteSumLength", "点：$currentIndex-------是最近的点------ ")
+            if (currentIndex == mRoutePoints.size - 1) {
+                val location = mRoutePoints[currentIndex]
+                //                LatLng latLng = new LatLng(location.getLatitude(), location.getLongitude());
+                latePoints.add(location)
+            } else {
+                val locations = mRoutePoints.subList(currentIndex, mRoutePoints.size)
+                for (location in locations) {
+//                    LatLng latLng = new LatLng(location.getLatitude(), location.getLongitude());
+                    latePoints.add(location)
+                }
+            }
+            routePonits[currentIndex] = latePoints
+            return routePonits
+        }
+        return routePonits
+    }
+
+    @JvmStatic
+    fun getArrivedPointIndexNew(
+        preIndex: Int, mRoutePoints: List<MogoLocation>,
+        realLocation: MogoLocation
+    ): Int {
+        var currentIndex = 0 //记录疑似点         //基础点
+        val baseLatLng = mRoutePoints[0]
+        var baseDiffDis = CoordinateUtils.calculateLineDistance(
+            realLocation.longitude,
+            realLocation.latitude, baseLatLng.longitude, baseLatLng.longitude
+        ) // lon,lat, prelon, prelat
+        for (i in mRoutePoints.indices) {
+            val latLng = mRoutePoints[i]
+            if (realLocation.heading == realLocation.heading - latLng.heading ||
+                Math.abs(realLocation.heading - latLng.heading) <= 90
+            ) {
+                val diff = CoordinateUtils.calculateLineDistance(
+                    realLocation.longitude,
+                    realLocation.latitude,
+                    latLng.longitude, latLng.latitude
+                )
+                if (baseDiffDis > diff && i > currentIndex) {
+                    baseDiffDis = diff
+                    currentIndex = i
+                }
+            }
+        }
+        Logger.d("calculateRouteSumLength", "点：$currentIndex-------是最近的点------ ")
+        return currentIndex
+    }
+
+    @JvmStatic
+    fun getArrivedPointIndexNew(
+        preIndex: Int, mRoutePoints: List<MogoLocation>,
+        realLon: Double, realLat: Double
+    ): Int {
+        var currentIndex = preIndex //记录疑似点         //基础点
+        val baseLatLng = mRoutePoints[0]
+        var baseDiffDis = CoordinateUtils.calculateLineDistance(
+            realLon,
+            realLat, baseLatLng.longitude, baseLatLng.longitude
+        ) // lon,lat, prelon, prelat
+        for (i in preIndex until mRoutePoints.size) {
+            val latLng = mRoutePoints[i]
+            //            if (realLocation.getBearing() == realLocation.getBearing() - latLng.getBearing() ||
+//                    Math.abs(realLocation.getBearing() - latLng.getBearing()) <= 90){
+            val diff = CoordinateUtils.calculateLineDistance(
+                realLon,
+                realLat,
+                latLng.longitude, latLng.latitude
+            )
+            if (baseDiffDis > diff && i > currentIndex) {
+//                    Logger.d(M_TAXI + "calculateRouteSumLength", "点："+i+"-------先记录点----- ");
+                baseDiffDis = diff
+                currentIndex = i
+            }
+            //            }
+        }
+        Logger.d("calculateRouteSumLength", "点：$currentIndex-------是最近的点------ ")
+        return currentIndex
+    }
+
+    /**
+     * https://blog.csdn.net/Jeanne_0523/article/details/106056255
+     * @param vertexPointX
+     * @param vertexPointY
+     * @param point0X  角
+     * @param point0Y  角
+     * @param point1X
+     * @param point1Y
+     * @return
+     */
+    fun getDegree(
+        vertexPointX: Double,
+        vertexPointY: Double,
+        point0X: Double,
+        point0Y: Double,
+        point1X: Double,
+        point1Y: Double
+    ): Int {
+        //向量的点乘
+        val vector =
+            (point0X - vertexPointX) * (point1X - vertexPointX) + (point0Y - vertexPointY) * (point1Y - vertexPointY)
+        //向量的模乘
+        val sqrt = Math.sqrt(
+            (Math.abs((point0X - vertexPointX) * (point0X - vertexPointX)) + Math.abs((point0Y - vertexPointY) * (point0Y - vertexPointY)))
+                    * (Math.abs((point1X - vertexPointX) * (point1X - vertexPointX)) + Math.abs((point1Y - vertexPointY) * (point1Y - vertexPointY)))
+        )
+        //反余弦计算弧度
+        val radian = Math.acos(vector / sqrt)
+        //弧度转角度制
+        return (180 * radian / Math.PI).toInt()
+    }
+
+    private fun ball2xyz(thera: Double, fie: Double, r: Double): Triple<Double, Double, Double> {
+        val x = r * cos(thera) * cos(fie)
+        val y = r * cos(thera) * sin(fie)
+        val z = r * sin(thera)
+        return Triple(x, y, z)
+    }
+
+    /**
+     * https://blog.csdn.net/reborn_lee/article/details/82497577
+     * 将地理经纬度转换成笛卡尔坐标系
+     */
+    private fun geo2xyz(lat: Double, lng: Double): Triple<Double, Double, Double> {
+        val thera = Math.PI * lat / 180
+        val fie = Math.PI * lng / 180
+        return ball2xyz(thera, fie, 6400.0)
+    }
+
+    /**
+     * 计算3个地理坐标点之间的夹角
+     * @param l1 顶点坐标
+     * @param l2
+     * @param l3
+     * @return l1为顶点的角度 精度没有angleOflocation高
+     */
+    fun getDegree(l2: MogoLocation, l1: MogoLocation, l3: MogoLocation): Double {
+        val (x1, y1, z1) = geo2xyz(l1.latitude, l1.longitude)
+        val (x2, y2, z2) = geo2xyz(l2.latitude, l2.longitude)
+        val (x3, y3, z3) = geo2xyz(l3.latitude, l3.longitude)
+
+        // 计算向量 P2P1 和 P2P3 的夹角 https://www.zybang.com/question/3379a30c0dd3041b3ef966803f0bf758.html
+        val p1P2 = sqrt((x2 - x1).pow(2.0) + (y2 - y1).pow(2.0) + (z2 - z1).pow(2.0))
+        val p2p3 = sqrt((x3 - x2).pow(2.0) + (y3 - y2).pow(2.0) + (z3 - z2).pow(2.0))
+        val p = (x1 - x2) * (x3 - x2) + (y1 - y2) * (y3 - y2) + (z1 - z2) * (z3 - z2) //P2P1*P2P3
+        return acos(p / (p1P2 * p2p3)) / Math.PI * 180
+    }
+
+    fun getHeadingAngle(location: MogoLocation, nextPoint: MogoLocation): Double {
+        val y = Math.sin(nextPoint.longitude - location.longitude) * Math.cos(nextPoint.latitude)
+        val x = Math.cos(location.latitude) * Math.sin(nextPoint.latitude) - Math.sin(
+            location.latitude
+        ) *
+                Math.cos(nextPoint.latitude) * Math.cos(nextPoint.longitude - location.longitude)
+        var bearing = Math.atan2(y, x)
+        bearing = Math.toDegrees(bearing)
+        if (bearing < 0) {
+            bearing += 360.0
+        }
+        return bearing
+    }
+
+    /**
+     * @param preIndex 上次计算缓存
+     * @param mRoutePoints 轨迹点
+     * @param location 目标坐标
+     * @return Triple<Int,Boolean?,Float>
+     *          距离目标坐标最近的轨迹点下标、
+     *          最近点坐标是目标坐标的上一个点还是下一个点
+     *          目标到最近轨迹点的距离
+     */
+    @JvmStatic
+    fun getNearestPointInfo(
+        preIndex: Int,
+        mRoutePoints: List<MogoLocation>,
+        location: MogoLocation,
+    ): Triple<Int,Boolean?,Float> {
+        var currentIndex:Int = preIndex //记录疑似点         //基础点
+        // 轨迹中的点和定位点的距离集合
+        val distanceMap: TreeMap<DistanceDegree, Int> = TreeMap()
+        for (index in preIndex until mRoutePoints.size) {
+            val latLngIndex = mRoutePoints[index]
+            val distance = CoordinateUtils.calculateLineDistance(
+                location.longitude,
+                location.latitude,
+                latLngIndex.longitude,
+                latLngIndex.latitude
+            )
+            distanceMap[DistanceDegree(distance, null,null)] = index
+            if (distanceMap.size > 4) {
+                distanceMap.pollLastEntry()
+            }
+        }
+        distanceMap.forEach {
+            val distanceDegree = it.key
+            val pointIndex = it.value
+            val currentPoint = mRoutePoints[pointIndex]// 疑似最近的点
+            var nextDegree = 0.0
+            var preDegree = 0.0
+
+            if (pointIndex + 1 < mRoutePoints.size) {
+                val nextPoint = mRoutePoints[pointIndex + 1]
+                nextDegree = CoordinateCalculateRouteUtil.getDegree(location, currentPoint, nextPoint)
+            }
+            // 需要减距离 和上一个轨迹点成钝角
+            if (pointIndex - 1 >= 0) {
+                val prePoint = mRoutePoints[pointIndex - 1]
+                preDegree = CoordinateCalculateRouteUtil.getDegree(
+                    location,
+                    currentPoint,
+                    prePoint
+                )
+            }
+            if(nextDegree>90){
+                if (pointIndex + 1 < mRoutePoints.size) {
+                    val nextPoint = mRoutePoints[pointIndex + 1]
+                    val headingAngle = CoordinateCalculateRouteUtil.getHeadingAngle(currentPoint, nextPoint)
+                    distanceDegree.degree = headingAngle
+                    distanceDegree.isNext = false
+                }
+            }
+            if(preDegree>90){
+                if (pointIndex - 1 >= 0) {
+                    val prePoint = mRoutePoints[pointIndex - 1]
+                    val headingAngle = CoordinateCalculateRouteUtil.getHeadingAngle(prePoint, currentPoint)
+                    distanceDegree.degree = headingAngle;
+                    distanceDegree.isNext = true
+                }
+            }
+            // 距离过近直接返回
+            if(distanceDegree.distance<1){
+                currentIndex = pointIndex
+                return Triple(currentIndex,distanceDegree.isNext,distanceDegree.distance)
+            }
+        }
+
+        val iterator = distanceMap.iterator()
+        // 有航向角比较航向角
+        if(location.heading!=0.0) {
+            while (iterator.hasNext()) {
+                val next = iterator.next()
+                val key = next.key
+                if (key.degree == null) {
+                    iterator.remove()
+                } else {
+                    key.degree?.let {
+                        val dexAngle = DrivingDirectionUtils.getAngleDiff(location.heading, it)
+                        if (dexAngle > 90) {
+                            iterator.remove()
+                        }
+                    }
+                }
+            }
+        }
+
+        // 上一次查询此次最近点包含上个点和下个点
+        if(distanceMap.containsValue(preIndex)&&distanceMap.containsValue(preIndex+1)){
+            var preIndexDistance:DistanceDegree?=null
+            var preIndexNextDistance:DistanceDegree?=null
+            distanceMap.iterator().forEach { en ->
+                val key = en.key
+                val value = en.value
+                if(value==preIndex){
+                    preIndexDistance = key
+                }else if(value==preIndex+1){
+                    preIndexNextDistance = key
+                }
+            }
+            if(preIndexDistance!=null&&preIndexNextDistance!=null){
+                if(preIndexDistance!!.distance<preIndexNextDistance!!.distance){
+                    currentIndex = preIndex
+                    return Triple(currentIndex,preIndexDistance?.isNext,preIndexDistance!!.distance)
+                }else{
+                    currentIndex = preIndex+1
+                    return  Triple(currentIndex,preIndexNextDistance?.isNext,preIndexNextDistance!!.distance)
+                }
+            }
+        }
+
+        // 最近的点 只有一个前面的点
+        var tempDistance = Float.MAX_VALUE
+        var isNext:Boolean? = null
+        distanceMap.iterator().forEach { en ->
+            val key = en.key
+            val value = en.value
+            // 排除没有第一个值0是
+            if(value==preIndex+1&&preIndex!=0){
+                currentIndex = value
+                return Triple(currentIndex,key.isNext,key.distance)
+            }
+            key.distance.let {
+                if (it < tempDistance) {
+                    tempDistance = it
+                    currentIndex = value
+                    isNext = key.isNext
+                }
+            }
+        }
+        return Triple(currentIndex,isNext,tempDistance)
+    }
+}

core/mogo-core-utils/src/main/java/com/mogo/eagle/core/utilcode/mogo/logger/scene/SceneConstant.kt

@@ -30,6 +30,9 @@ class SceneConstant {
         const val M_OLD_OTHER = "M_OLD_OTHER-"
         const val M_OTHER = "M_OTHER-"
 
+        // Och common
+        const val M_OCHCOMMON = "M_OCHCOMMON-"
+
         //小巴车
         const val M_BUS = "M_BUS-"
         //小巴车乘客屏