diff --git a/core/function-impl/mogo-core-function-v2x/src/main/java/com/mogo/eagle/core/function/v2x/redlightwarning/LocationUtils.java b/core/function-impl/mogo-core-function-v2x/src/main/java/com/mogo/eagle/core/function/v2x/redlightwarning/LocationUtils.java
new file mode 100644
index 0000000000..fc062a3f45
--- /dev/null
+++ b/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 )
+ * ( 单位:米 )
+ * @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;
+ }
+}
diff --git a/core/function-impl/mogo-core-function-v2x/src/main/java/com/mogo/eagle/core/function/v2x/redlightwarning/RedLightWarningManager.kt b/core/function-impl/mogo-core-function-v2x/src/main/java/com/mogo/eagle/core/function/v2x/redlightwarning/RedLightWarningManager.kt
index 66acaf96d6..fa0c9d62ae 100644
--- a/core/function-impl/mogo-core-function-v2x/src/main/java/com/mogo/eagle/core/function/v2x/redlightwarning/RedLightWarningManager.kt
+++ b/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, 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()
+ }
+
/**
* 闯红灯预警
*/
diff --git a/core/function-impl/mogo-core-function-v2x/src/main/java/com/mogo/eagle/core/function/v2x/trafficlight/core/MogoTrafficLightManager.kt b/core/function-impl/mogo-core-function-v2x/src/main/java/com/mogo/eagle/core/function/v2x/trafficlight/core/MogoTrafficLightManager.kt
index 96f39e2222..332fe3ba85 100644
--- a/core/function-impl/mogo-core-function-v2x/src/main/java/com/mogo/eagle/core/function/v2x/trafficlight/core/MogoTrafficLightManager.kt
+++ b/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,