From 4d0625d4c6a9c234a92226e5fccd1fa02495abb1 Mon Sep 17 00:00:00 2001 From: chenfufeng Date: Wed, 21 Sep 2022 18:25:33 +0800 Subject: [PATCH] =?UTF-8?q?[Feat]=E4=BC=98=E5=8C=96=E8=87=AA=E8=BD=A6?= =?UTF-8?q?=E5=88=B0=E5=81=9C=E6=AD=A2=E7=BA=BF=E7=9A=84=E8=B7=9D=E7=A6=BB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../v2x/redlightwarning/LocationUtils.java | 74 +++++++++++++++++++ .../redlightwarning/RedLightWarningManager.kt | 28 ++++++- .../core/MogoTrafficLightManager.kt | 4 + 3 files changed, 102 insertions(+), 4 deletions(-) create mode 100644 core/function-impl/mogo-core-function-v2x/src/main/java/com/mogo/eagle/core/function/v2x/redlightwarning/LocationUtils.java 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,