From 70f50f9f00a70397a336a75787ea4255f3b9ccf6 Mon Sep 17 00:00:00 2001
From: puzhibing <393733352@qq.com>
Date: 星期六, 28 三月 2026 22:10:02 +0800
Subject: [PATCH] 修改bug
---
xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/MapMatchingUtil.java | 208 +++++++++++++++++++++++++++++++++++++++++++++++++++
1 files changed, 205 insertions(+), 3 deletions(-)
diff --git a/xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/MapMatchingUtil.java b/xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/MapMatchingUtil.java
index 5784d42..422ed98 100644
--- a/xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/MapMatchingUtil.java
+++ b/xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/MapMatchingUtil.java
@@ -1,15 +1,217 @@
package com.xiaopeng.pushMessage.utils;
+import com.alibaba.fastjson2.JSON;
+import com.alibaba.fastjson2.JSONArray;
+import com.alibaba.fastjson2.JSONObject;
import com.xiaopeng.api.dto.operation.LonLatDto;
import lombok.extern.slf4j.Slf4j;
+import org.gavaghan.geodesy.Ellipsoid;
+import org.gavaghan.geodesy.GeodeticCalculator;
+import org.gavaghan.geodesy.GeodeticCurve;
+import org.gavaghan.geodesy.GlobalCoordinates;
-import java.util.ArrayList;
-import java.util.Collections;
-import java.util.List;
+import java.util.*;
+import java.util.concurrent.*;
+
@Slf4j
public class MapMatchingUtil {
+
+
+
+ /**
+ * 获取剩余导航数据
+ * @param polyline
+ * @param longitude
+ * @param latitude
+ * @return
+ */
+ public static Map<String, List<LonLatDto>> getRemainingNavigation(List<LonLatDto> polyline, Double longitude, Double latitude){
+ //导航数据只有一个点的情况
+ Map<String, List<LonLatDto>> map = new HashMap<>();
+ if(polyline.size() == 1){
+ LonLatDto lonLatDto = polyline.get(0);
+ GlobalCoordinates source1 = new GlobalCoordinates(lonLatDto.getLatitude(), lonLatDto.getLongitude());
+ GlobalCoordinates target1 = new GlobalCoordinates(latitude, longitude);
+ GeodeticCurve geoCurve1 = new GeodeticCalculator().calculateGeodeticCurve(Ellipsoid.WGS84, source1, target1);
+ double distance =geoCurve1.getEllipsoidalDistance();
+ //判断距离小于5米，则可以返回空数据
+ if(5 >= distance){
+ map.put("unserved", Collections.emptyList());
+ map.put("served", Collections.emptyList());
+ return map;
+ }
+ //大于5米的情况，将车子当前位置添加进结果中
+ map.put("unserved", polyline);
+ map.put("served", Collections.emptyList());
+ return map;
+ }
+ //多个点位的情况，需要找出当前点位处于那两个点位之间，然后截取掉超出的部分轨迹
+ return getRecentlyPoint(polyline, longitude, latitude);
+ }
+
+
+ /**
+ * 判断点位在轨迹线路中的点位之间
+ * 返回截取后的剩余轨迹
+ * @param polyline
+ * @param longitude
+ * @param latitude
+ * @return
+ */
+ public static Map<String, List<LonLatDto>> getRecentlyPoint(List<LonLatDto> polyline, Double longitude, Double latitude){
+ Map<String, List<LonLatDto>> map = new HashMap<>();
+ for (int i = 0; i < polyline.size(); i++) {
+ if(i == polyline.size() - 1){
+ break;
+ }
+ LonLatDto lonLatDto1 = polyline.get(i);
+
+ GlobalCoordinates source1 = new GlobalCoordinates(lonLatDto1.getLatitude(), lonLatDto1.getLongitude());
+ GlobalCoordinates target1 = new GlobalCoordinates(latitude, longitude);
+ GeodeticCurve geoCurve1 = new GeodeticCalculator().calculateGeodeticCurve(Ellipsoid.WGS84, source1, target1);
+ double distance1 =geoCurve1.getEllipsoidalDistance();
+
+ LonLatDto lonLatDto2 = polyline.get(i + 1);
+ GlobalCoordinates source2 = new GlobalCoordinates(lonLatDto2.getLatitude(), lonLatDto2.getLongitude());
+ GlobalCoordinates target2 = new GlobalCoordinates(latitude, longitude);
+ GeodeticCurve geoCurve2 = new GeodeticCalculator().calculateGeodeticCurve(Ellipsoid.WGS84, source2, target2);
+ double distance2 =geoCurve2.getEllipsoidalDistance();
+
+
+ GlobalCoordinates source3 = new GlobalCoordinates(lonLatDto1.getLatitude(), lonLatDto1.getLongitude());
+ GlobalCoordinates target3 = new GlobalCoordinates(lonLatDto2.getLatitude(), lonLatDto2.getLongitude());
+ GeodeticCurve geoCurve3 = new GeodeticCalculator().calculateGeodeticCurve(Ellipsoid.WGS84, source3, target3);
+ double distance3 =geoCurve3.getEllipsoidalDistance();
+
+ //判断当前位置是否处于两个点位之间
+ if((distance1 + distance2) == distance3 || (distance1 + distance2) <= distance3 + 5){
+ //剩余未服务的点位（创建新副本避免修改原列表）
+ List<LonLatDto> lonLatDtos = new ArrayList<>(polyline.subList(i + 1, polyline.size()));
+ //已经服务的点位，给到前端处理（创建新副本避免修改原列表）
+ List<LonLatDto> lonLatDtos1 = new ArrayList<>(polyline.subList(0, i + 1));
+ map.put("unserved", lonLatDtos);
+ map.put("served", lonLatDtos1);
+ return map;
+ }
+ }
+ map.put("unserved", polyline);
+ map.put("served", Collections.emptyList());
+ return map;
+ }
+
+
+// public static void main(String[] args) {
+// JSONArray objects = JSON.parseArray("[{\n" +
+// "\t\t\"latitude\": 30.58549,\n" +
+// "\t\t\"longitude\": 104.054501\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.585938,\n" +
+// "\t\t\"longitude\": 104.054501\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.586522,\n" +
+// "\t\t\"longitude\": 104.054501\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.587026,\n" +
+// "\t\t\"longitude\": 104.054507\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.587922,\n" +
+// "\t\t\"longitude\": 104.054518\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.588062,\n" +
+// "\t\t\"longitude\": 104.054518\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.588292,\n" +
+// "\t\t\"longitude\": 104.054528\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.588292,\n" +
+// "\t\t\"longitude\": 104.0544\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.588292,\n" +
+// "\t\t\"longitude\": 104.0544\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.587949,\n" +
+// "\t\t\"longitude\": 104.054384\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.587311,\n" +
+// "\t\t\"longitude\": 104.054389\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.587118,\n" +
+// "\t\t\"longitude\": 104.054394\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.586704,\n" +
+// "\t\t\"longitude\": 104.054394\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.586125,\n" +
+// "\t\t\"longitude\": 104.0544\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.585938,\n" +
+// "\t\t\"longitude\": 104.054405\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.585476,\n" +
+// "\t\t\"longitude\": 104.054405\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.584693,\n" +
+// "\t\t\"longitude\": 104.054405\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.584403,\n" +
+// "\t\t\"longitude\": 104.054416\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.584312,\n" +
+// "\t\t\"longitude\": 104.054416\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.583593,\n" +
+// "\t\t\"longitude\": 104.054416\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.583486,\n" +
+// "\t\t\"longitude\": 104.054416\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.583116,\n" +
+// "\t\t\"longitude\": 104.054421\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.582391,\n" +
+// "\t\t\"longitude\": 104.054426\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.582397,\n" +
+// "\t\t\"longitude\": 104.054577\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.582397,\n" +
+// "\t\t\"longitude\": 104.054577\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.582815,\n" +
+// "\t\t\"longitude\": 104.054561\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.58311,\n" +
+// "\t\t\"longitude\": 104.05455\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.58311,\n" +
+// "\t\t\"longitude\": 104.05455\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.58311,\n" +
+// "\t\t\"longitude\": 104.054899\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.583143,\n" +
+// "\t\t\"longitude\": 104.054931\n" +
+// "\t}, {\n" +
+// "\t\t\"latitude\": 30.583148,\n" +
+// "\t\t\"longitude\": 104.054973\n" +
+// "\t}]");
+// List<LonLatDto> lonLatDtoList = new ArrayList<>();
+// for (int i = 0; i < objects.size(); i++) {
+// JSONObject object = objects.getJSONObject(i);
+// LonLatDto lonLatDto = new LonLatDto(object.getDouble("longitude"), object.getDouble("latitude"));
+// lonLatDtoList.add(lonLatDto);
+// }
+// for (int i = 0; i < 20; i++) {
+// List<LonLatDto> remainingNavigation = getRemainingNavigation(lonLatDtoList, 104.054503, 30.58549);
+// lonLatDtoList = remainingNavigation;
+// }
+//
+//
+// }
+
+
/**
* 轨迹裁剪（线段版本）
* @param polyline
--
Gitblit v1.7.1