[Feat]优化自车到停止线的距离

core/function-impl/mogo-core-function-v2x/src/main/java/com/mogo/eagle/core/function/v2x/redlightwarning/LocationUtils.java

@@ -0,0 +1,74 @@
+package com.mogo.eagle.core.function.v2x.redlightwarning;
+
+public class LocationUtils {
+    /**
+     * 地球半径
+     */
+    private static double EARTH_RADIUS = 6378.137;
+
+    private static double rad( double d ) {
+        return d * Math.PI / 180.0;
+    }
+
+    /**
+     * 计算两点间距离( 单位：米 )
+     * @param lat1
+     * @param lng1
+     * @param lat2
+     * @param lng2
+     * @return
+     */
+    public static double getDistance( double lat1, double lng1, double lat2, double lng2 ) {
+        double radLat1 = rad( lat1 );
+        double radLat2 = rad(lat2);
+        double a = radLat1 - radLat2;
+        double b = rad(lng1) - rad(lng2);
+        double s = 2 * Math.asin(Math.sqrt(Math.pow(Math.sin(a / 2), 2)
+                + Math.cos(radLat1) * Math.cos(radLat2)
+                * Math.pow(Math.sin(b / 2), 2)));
+        s = s * EARTH_RADIUS;
+        s = Math.round(s * 10000d) / 10000d;
+        s = s * 1000;
+        return s;
+    }
+
+
+    /**
+     * 点到直线的最短距离的判断 点（x0,y0） 到由两点组成的线段（x1,y1） ,( x2,y2 )    <br>
+     *     ( 单位：米 )
+     * @param x1
+     * @param y1
+     * @param x2
+     * @param y2
+     * @param x0
+     * @param y0
+     * @return
+     */
+    public static double pointToLine( double x1, double y1, double x2, double y2, double x0, double y0 ) {
+        double space;
+        double a, b, c;
+        a = getDistance(y1, x1, y2, x2);// 线段的长度
+        b = getDistance(y1, x1, y0, x0);// (x1,y1)到点的距离
+        c = getDistance(y2, x2, y0, x0);// (x2,y2)到点的距离
+        if (c <= 0.000001 || b <= 0.000001) {
+            space = 0;
+            return space;
+        }
+        if (a <= 0.000001) {
+            space = b;
+            return space;
+        }
+        if (c * c >= a * a + b * b) {
+            space = b;
+            return space;
+        }
+        if (b * b >= a * a + c * c) {
+            space = c;
+            return space;
+        }
+        double p = (a + b + c) / 2;// 半周长
+        double s = Math.sqrt(p * (p - a) * (p - b) * (p - c));// 海伦公式求面积
+        space = 2 * s / a;// 返回点到线的距离（利用三角形面积公式求高）
+        return space;
+    }
+}

core/function-impl/mogo-core-function-v2x/src/main/java/com/mogo/eagle/core/function/v2x/redlightwarning/RedLightWarningManager.kt

@@ -15,6 +15,7 @@ import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger
 import com.mogo.eagle.core.function.call.map.CallerMapUIServiceManager
 import com.mogo.eagle.core.function.call.trafficlight.CallerTrafficLightListenerManager
 import com.mogo.eagle.core.function.call.vip.CallVipSetListenerManager
+import com.mogo.eagle.core.function.v2x.trafficlight.core.MogoTrafficLightManager
 import com.mogo.eagle.core.utilcode.util.ThreadUtils
 import com.mogo.map.navi.IMogoCarLocationChangedListener2
 import com.mogo.module.common.MogoApisHandler
@@ -23,6 +24,7 @@ import com.zhjt.service_biz.BizConfig
 import kotlin.math.abs
 import kotlin.math.ceil
 import kotlin.math.floor
+import kotlin.math.min
 
 
 class RedLightWarningManager : IMoGoTrafficLightListener, IMoGoVipSetListener,
@@ -98,10 +100,15 @@ class RedLightWarningManager : IMoGoTrafficLightListener, IMoGoVipSetListener,
             CallerLogger.d("$M_V2X$TAG", "speed is:$speed")
             if (speed <= 2.5f) return// 小于等于9km/h不处理
             // 由于到路口100m时回调不准，手动计算直线距离
-            val distance = CallerMapUIServiceManager.getMapUIController()?.calculateLineDistance(
-                MogoLatLng(it.latitude, it.longitude),
-                MogoLatLng(trafficLightResult.lat, trafficLightResult.lon)
-            ) ?: 0f
+            val roadResult = MogoTrafficLightManager.INSTANCE.getRoadResult()
+            val distance = if (roadResult != null && roadResult.rectLatLngs.size >= 2) {
+                getMinDistance(roadResult.rectLatLngs, it.latitude, it.longitude)
+            } else {
+                CallerMapUIServiceManager.getMapUIController()?.calculateLineDistance(
+                    MogoLatLng(it.latitude, it.longitude),
+                    MogoLatLng(trafficLightResult.lat, trafficLightResult.lon)
+                ) ?: 0f
+            }
             CallerLogger.d(
                 "$M_V2X$TAG",
                 "路口经度为：${trafficLightResult.lon}，纬度为：${trafficLightResult.lat}；车的经度为：${it.longitude}，纬度为：${it.latitude}；两点距离为：${distance}"
@@ -163,6 +170,19 @@ class RedLightWarningManager : IMoGoTrafficLightListener, IMoGoVipSetListener,
         }
     }
 
+    private fun getMinDistance(points: List<MogoLatLng>, lat: Double, lon: Double): Float {
+        // 到路口100m时才计算，赋值一个较大值
+        var minValue = 9999.9
+        val size = points.size
+        for (i in 0..size step 2) {
+            if (i < size) {
+                // 自车到0-1、2-3、4-5、6-7组成的线段的最小距离
+                minValue = min(minValue, LocationUtils.pointToLine(points[i].lon, points[i].lat, points[i+1].lon, points[i+1].lat, lon, lat))
+            }
+        }
+        return if (minValue > 9999) 0f else minValue.toFloat()
+    }
+
     /**
      * 闯红灯预警
      */

core/function-impl/mogo-core-function-v2x/src/main/java/com/mogo/eagle/core/function/v2x/trafficlight/core/MogoTrafficLightManager.kt

@@ -151,6 +151,10 @@ class MogoTrafficLightManager : IMogoCarLocationChangedListener2 {
         return null
     }
 
+    fun getRoadResult(): RoadIDResult? {
+        return roadIDResult
+    }
+
     fun turnLightToGreen(
         lightId: Int,
         crossingNo: String,