From 446d588f6983d2f9e7bb18546a961fa4c14c99a0 Mon Sep 17 00:00:00 2001
From: puzhibing <393733352@qq.com>
Date: 星期二, 19 五月 2026 03:58:31 +0800
Subject: [PATCH] 合并代码
---
xp-robotaxi-push-message-boot/src/main/java/com/xiaopeng/pushMessage/utils/MapMatchingUtil.java | 407 ++++++++++++++++++++++++++++++++++++++++++++-------------
1 files changed, 312 insertions(+), 95 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 7248886..7b7f575 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
@@ -9,6 +9,7 @@
import org.gavaghan.geodesy.GeodeticCalculator;
import org.gavaghan.geodesy.GeodeticCurve;
import org.gavaghan.geodesy.GlobalCoordinates;
+import org.springframework.data.geo.Point;
import java.util.*;
import java.util.concurrent.*;
@@ -115,117 +116,139 @@
-
-
- /**
- * 轨迹裁剪（线段版本）
- * @param polyline
- * @param currentPoint
- * @return
- */
- public static List<LonLatDto> clipBySegmentDistance(List<LonLatDto> polyline,LonLatDto currentPoint) {
-
- if (polyline == null || polyline.size() < 2) {
- return Collections.emptyList();
+ public static Map<String, List<LonLatDto>> getRemainingNavigationByAngle(List<LonLatDto> polyline, Double longitude, Double latitude){
+ //导航数据只有一个点的情况
+ Map<String, List<LonLatDto>> map = new HashMap<>();
+ if(polyline.size() < 3){
+ map.put("unserved", new ArrayList<>());
+ map.put("served", polyline);
+ return map;
}
- double minDistance = Double.MAX_VALUE;
- int minSegmentIndex = 0;
-
- int increaseCount = 0;
- final int threshold = 3;
- final double tolerance = 1.0;
-
- for (int i = 0; i < polyline.size() - 1; i++) {
-
- double d = pointToSegmentDistance(
- currentPoint,
- polyline.get(i),
- polyline.get(i + 1)
- );
-
- if (d < minDistance) {
- minDistance = d;
- minSegmentIndex = i;
- increaseCount = 0;
- } else if (d > minDistance + tolerance) {
- increaseCount++;
- }
-
- if (increaseCount >= threshold) {
+ for (int i = 0; i < polyline.size(); i++) {
+ if(i+1 == polyline.size()){
break;
}
- }
+ LonLatDto centerPoint = polyline.get(i);
+ LonLatDto nextPoint = polyline.get(i+1);
- // 从最优线段的后一个点开始截取
- return new ArrayList<>(
- polyline.subList(minSegmentIndex + 1, polyline.size())
- );
+
+ // 定义三个点
+ // Point A
+ double[] pointA = {nextPoint.getLongitude(), nextPoint.getLatitude()};
+ // Point B: 中间点
+ double[] pointB = {centerPoint.getLongitude(), centerPoint.getLatitude()};
+ // Point C
+ double[] pointC = {longitude, latitude};
+ //正向夹角
+ double angle = calculateAngle(pointA, pointB, pointC);
+ // 定义三个点
+ // Point A
+ double[] pointA1 = {centerPoint.getLongitude(), centerPoint.getLatitude()};
+ // Point B: 中间点
+ double[] pointB1 = {nextPoint.getLongitude(), nextPoint.getLatitude()};
+ // Point C
+ double[] pointC1 = {longitude, latitude};
+ //反向夹角
+ double angle1 = calculateAngle(pointA1, pointB1, pointC1);
+ //正向夹角和反向夹角都小于90度，则认为车辆在两点之间
+ if(90 >= angle && 90 >= angle1){
+ double lon = centerPoint.getLongitude();
+ double lat = centerPoint.getLatitude();
+ if(angle < angle1){
+ lon = nextPoint.getLongitude();
+ lat = nextPoint.getLatitude();
+ }
+ //计算距离，定位偏移10米内
+ GlobalCoordinates source3 = new GlobalCoordinates(lat, lon);
+ GlobalCoordinates target3 = new GlobalCoordinates(latitude, longitude);
+ GeodeticCurve geoCurve3 = new GeodeticCalculator().calculateGeodeticCurve(Ellipsoid.WGS84, source3, target3);
+ double distance3 =geoCurve3.getEllipsoidalDistance();
+ if(distance3 > 10){
+ continue;
+ }
+ map.put("unserved", polyline.subList(i, polyline.size()));
+ map.put("served", polyline.subList(0, i));
+ System.err.println("车辆在两点之间，点位夹角 : " + angle + " " + i);
+ return map;
+ }
+ }
+ map.put("unserved", polyline);
+ map.put("served", new ArrayList<>());
+ return map;
+
}
/**
- * 点到线段距离
- * @param p
- * @param a
- * @param b
+ * 计算两个点位之间的夹角
+ * @param a 起始点
+ * @param m 中点
+ * @param b 终点
* @return
*/
- public static double pointToSegmentDistance(
- LonLatDto p,
- LonLatDto a,
- LonLatDto b) {
+ public static double calculateAngle(double a_x, double a_y, double m_x, double m_y, double b_x, double b_y) {
+ // 获取从A到M和从B到M的向量
+ double[] vectorAM = new double[]{a_x - m_x, a_y - m_y};
+ double[] vectorBM = new double[]{b_x - m_x, b_y - m_y};
- double refLat = p.getLatitude();
+ // 计算两个向量的点积
+ double dotProduct = vectorAM[0] * vectorBM[0] + vectorAM[1] * vectorBM[1];
- double[] pXY = toXY(p.getLatitude(), p.getLongitude(), refLat);
- double[] aXY = toXY(a.getLatitude(), a.getLongitude(), refLat);
- double[] bXY = toXY(b.getLatitude(), b.getLongitude(), refLat);
+ // 计算两个向量的模（长度）
+ double magnitudeAM = Math.sqrt(vectorAM[0] * vectorAM[0] + vectorAM[1] * vectorAM[1]);
+ double magnitudeBM = Math.sqrt(vectorBM[0] * vectorBM[0] + vectorBM[1] * vectorBM[1]);
- double x = pXY[0];
- double y = pXY[1];
+ // 计算夹角的余弦值
+ double cosTheta = dotProduct / (magnitudeAM * magnitudeBM);
- double x1 = aXY[0];
- double y1 = aXY[1];
+ // 计算夹角（弧度）并转换为度
+ double angleInRadians = Math.acos(cosTheta);
+ double angleInDegrees = Math.toDegrees(angleInRadians);
- double x2 = bXY[0];
- double y2 = bXY[1];
-
- double dx = x2 - x1;
- double dy = y2 - y1;
-
- if (dx == 0 && dy == 0) {
- return Math.hypot(x - x1, y - y1);
- }
-
- double t = ((x - x1) * dx + (y - y1) * dy) / (dx * dx + dy * dy);
-
- if (t < 0) {
- return Math.hypot(x - x1, y - y1);
- } else if (t > 1) {
- return Math.hypot(x - x2, y - y2);
- }
-
- double projX = x1 + t * dx;
- double projY = y1 + t * dy;
-
- return Math.hypot(x - projX, y - projY);
+ return angleInDegrees;
}
- /**
- * 经纬度转平面坐标（局部近似）
- * @param lat
- * @param lon
- * @param refLat
- * @return
- */
- private static double[] toXY(double lat, double lon, double refLat) {
- double R = 6378137; // 地球半径
- double x = Math.toRadians(lon) * R * Math.cos(Math.toRadians(refLat));
- double y = Math.toRadians(lat) * R;
- return new double[]{x, y};
- }
-
+// public static void main(String[] args) {
+// // 计算坐标差
+// double x1 = 113.3843844686235;
+//
+// double y1 = 23.155130575839845;
+//
+// double x2 = 113.3836848841087;
+//
+// double y2 = 23.155180919552134;
+//
+// double x3 = 113.38368194444445;
+//
+// double y3 = 23.15508361111111;
+//
+// //计算三条边
+//
+// double a = Math.sqrt((x2 - x3) * (x2 - x3) + (y2 - y3) * (y2- y3));
+//
+// double b = Math.sqrt((x1 - x3) * (x1 - x3) + (y1 - y3) * (y1 - y3));
+//
+// double c = Math.sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
+//
+// //计算三个角
+//
+// double A = Math.toDegrees(Math.acos((a * a - b * b - c * c) / (-2 * b * c)));
+//
+// double B = Math.toDegrees(Math.acos((b * b - a * a - c * c) / (-2 * a * c)));
+//
+// double C = Math.toDegrees(Math.acos((c * c - a * a - b * b) / (-2 * a * b)));
+//
+// //显示结果，保留小数点后两位
+//
+// System.out.println("The three angles are " +
+//
+// Math.round(A * 100) / 100.0 + " " +
+//
+// Math.round(B * 100) / 100.0 + " " +
+//
+// Math.round(C * 100) / 100.0);
+// }
public static void main(String[] args) {
String json = "[{\n" +
@@ -335,10 +358,21 @@
"\t\"longitude\": 113.386128\n" +
"}]";
List<LonLatDto> polyline = JSON.parseArray(json, LonLatDto.class);
- String xcgj = "[{\"longitude\":113.38368194444445,\"latitude\":23.15508361111111}]";
+ String xcgj = "[{\"longitude\":113.38358194444445,\"latitude\":23.15508361111111}]";
List<LonLatDto> lonLatDtos = JSON.parseArray(xcgj, LonLatDto.class);
for (LonLatDto lonLatDto : lonLatDtos) {
Map<String, List<LonLatDto>> remainingNavigation = getRemainingNavigation(polyline, lonLatDto.getLongitude(), lonLatDto.getLatitude());
+ List<LonLatDto> unserved = remainingNavigation.get("unserved");
+ List<LonLatDto> served = remainingNavigation.get("served");
+ System.err.println(JSON.toJSONString(served));
+ polyline = unserved;
+
+ }
+ System.err.println("------------------------------------");
+
+ polyline = JSON.parseArray(json, LonLatDto.class);
+ for (LonLatDto lonLatDto : lonLatDtos) {
+ Map<String, List<LonLatDto>> remainingNavigation = getRemainingNavigationByAngle(polyline, lonLatDto.getLongitude(), lonLatDto.getLatitude());
List<LonLatDto> unserved = remainingNavigation.get("unserved");
List<LonLatDto> served = remainingNavigation.get("served");
System.err.println(JSON.toJSONString(served));
@@ -349,7 +383,112 @@
-// JSONArray objects = JSON.parseArray("[{\"latitude\":23.154856,\"longitude\":113.383825},{\"latitude\":23.154741004509642,\"longitude\":113.38390239240225},{\"latitude\":23.154626009019285,\"longitude\":113.38397978480451},{\"latitude\":23.154593,\"longitude\":113.384002},{\"latitude\":23.154488868635756,\"longitude\":113.3840956684042},{\"latitude\":23.15438473727151,\"longitude\":113.3841893368084},{\"latitude\":23.153869,\"longitude\":113.384732},{\"latitude\":23.153783434325412,\"longitude\":113.38484554532484},{\"latitude\":23.153697868650823,\"longitude\":113.38495909064967},{\"latitude\":23.153612302976235,\"longitude\":113.3850726359745},{\"latitude\":23.153606,\"longitude\":113.385081},{\"latitude\":23.15352802191651,\"longitude\":113.38520076538181},{\"latitude\":23.153450043833022,\"longitude\":113.38532053076364},{\"latitude\":23.153397,\"longitude\":113.385402},{\"latitude\":23.15332878835048,\"longitude\":113.38552854437049},{\"latitude\":23.15326057670096,\"longitude\":113.38565508874098},{\"latitude\":23.153252,\"longitude\":113.385671},{\"latitude\":23.153190501345524,\"longitude\":113.3858015058656},{\"latitude\":23.15312900269105,\"longitude\":113.38593201173121},{\"latitude\":23.15308,\"longitude\":113.386036},{\"latitude\":23.15306,\"longitude\":113.386128}]");
+// JSONArray objects = JSON.parseArray("[{\n" +
+// "\t\"latitude\": 23.155130575839845,\n" +
+// "\t\"longitude\": 113.3843844686235\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.155210151679693,\n" +
+// "\t\"longitude\": 113.38426593724698\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.155289727519538,\n" +
+// "\t\"longitude\": 113.38414740587048\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.15529,\n" +
+// "\t\"longitude\": 113.384147\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.155362958389055,\n" +
+// "\t\"longitude\": 113.38402358903004\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.155435916778114,\n" +
+// "\t\"longitude\": 113.38390017806007\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.15550887516717,\n" +
+// "\t\"longitude\": 113.3837767670901\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.155581833556226,\n" +
+// "\t\"longitude\": 113.38365335612013\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.155585,\n" +
+// "\t\"longitude\": 113.383648\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.155542,\n" +
+// "\t\"longitude\": 113.383621\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.155312,\n" +
+// "\t\"longitude\": 113.383648\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.155180919552134,\n" +
+// "\t\"longitude\": 113.3836848841087\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.155081,\n" +
+// "\t\"longitude\": 113.383713\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.15495796304221,\n" +
+// "\t\"longitude\": 113.38377424506344\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.154856,\n" +
+// "\t\"longitude\": 113.383825\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.154741004509642,\n" +
+// "\t\"longitude\": 113.38390239240225\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.154626009019285,\n" +
+// "\t\"longitude\": 113.38397978480451\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.154593,\n" +
+// "\t\"longitude\": 113.384002\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.154488868635756,\n" +
+// "\t\"longitude\": 113.3840956684042\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.15438473727151,\n" +
+// "\t\"longitude\": 113.3841893368084\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.153869,\n" +
+// "\t\"longitude\": 113.384732\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.153783434325412,\n" +
+// "\t\"longitude\": 113.38484554532484\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.153697868650823,\n" +
+// "\t\"longitude\": 113.38495909064967\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.153612302976235,\n" +
+// "\t\"longitude\": 113.3850726359745\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.153606,\n" +
+// "\t\"longitude\": 113.385081\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.15352802191651,\n" +
+// "\t\"longitude\": 113.38520076538181\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.153450043833022,\n" +
+// "\t\"longitude\": 113.38532053076364\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.153397,\n" +
+// "\t\"longitude\": 113.385402\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.15332878835048,\n" +
+// "\t\"longitude\": 113.38552854437049\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.15326057670096,\n" +
+// "\t\"longitude\": 113.38565508874098\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.153252,\n" +
+// "\t\"longitude\": 113.385671\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.153190501345524,\n" +
+// "\t\"longitude\": 113.3858015058656\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.15312900269105,\n" +
+// "\t\"longitude\": 113.38593201173121\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.15308,\n" +
+// "\t\"longitude\": 113.386036\n" +
+// "}, {\n" +
+// "\t\"latitude\": 23.15306,\n" +
+// "\t\"longitude\": 113.386128\n" +
+// "}]");
// String str = "[";
// for (int i = 0; i < objects.size(); i++) {
// JSONObject jsonObject = objects.getJSONObject(i);
@@ -358,4 +497,82 @@
// System.err.println(str + "]");
}
+
+
+ private static final double EARTH_RADIUS_KM = 6371.0; // 地球平均半径 (公里)
+
+ /**
+ * 计算由三个经纬度点构成的夹角
+ *
+ * @param pointA 经纬度点 A (格式: {经度, 纬度})
+ * @param pointB 经纬度点 B (格式: {经度, 纬度}) - 此点为夹角的顶点
+ * @param pointC 经纬度点 C (格式: {经度, 纬度})
+ * @return 夹角 ABC 的大小，单位为度
+ */
+ public static double calculateAngle(double[] pointA, double[] pointB, double[] pointC) {
+ // 1. 将经纬度（度）转换为弧度
+ double lat1 = Math.toRadians(pointA[1]);
+ double lon1 = Math.toRadians(pointA[0]);
+ double lat2 = Math.toRadians(pointB[1]);
+ double lon2 = Math.toRadians(pointB[0]);
+ double lat3 = Math.toRadians(pointC[1]);
+ double lon3 = Math.toRadians(pointC[0]);
+
+ // 2. 计算各点对之间的中心角 (angular distance)
+ double angularDist_AB = haversineDistanceInRadians(lat1, lon1, lat2, lon2);
+ double angularDist_BC = haversineDistanceInRadians(lat2, lon2, lat3, lon3);
+ double angularDist_AC = haversineDistanceInRadians(lat1, lon1, lat3, lon3);
+
+ // 3. 使用球面余弦定律计算夹角 B (angle at point B)
+ // cos(B) = (cos(b) - cos(c) * cos(a)) / (sin(c) * sin(a))
+ // 其中 a 是 BC 的中心角, b 是 AC 的中心角, c 是 AB 的中心角
+ double cos_angle_B = (
+ Math.cos(angularDist_AC) -
+ Math.cos(angularDist_AB) * Math.cos(angularDist_BC)
+ ) / (
+ Math.sin(angularDist_AB) * Math.sin(angularDist_BC)
+ );
+
+ // 4. 防止由于浮点数精度问题导致的无效输入 (例如 cos_angle_B > 1 或 < -1)
+ cos_angle_B = Math.max(-1.0, Math.min(1.0, cos_angle_B));
+
+ // 5. 计算夹角 B 并转换为度
+ double angle_B_rad = Math.acos(cos_angle_B);
+ double angle_B_deg = Math.toDegrees(angle_B_rad);
+
+ return angle_B_deg;
+ }
+
+ /**
+ * 使用 Haversine 公式计算两个经纬度点之间的中心角 (弧度)
+ * 此函数计算的是球心角，不是地面距离
+ */
+ private static double haversineDistanceInRadians(double lat1, double lon1, double lat2, double lon2) {
+ double delta_lat = lat2 - lat1;
+ double delta_lon = lon2 - lon1;
+
+ double a = Math.sin(delta_lat / 2) * Math.sin(delta_lat / 2) +
+ Math.cos(lat1) * Math.cos(lat2) *
+ Math.sin(delta_lon / 2) * Math.sin(delta_lon / 2);
+
+ double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
+
+ return c; // 返回中心角 (弧度)
+ }
+
+ // --- 示例 ---
+// public static void main(String[] args) {
+// // 定义三个点
+// // Point A: 北京天安门广场附近
+// double[] pointA = {113.3836848841087,23.155180919552134};
+// // Point B: 北京某个中间点
+// double[] pointB = {113.383713,23.155081};
+// // Point C: 北京另一个方向的点
+// double[] pointC = {113.38368194444445,23.15508361111111};
+//
+// double angle = calculateAngle(pointA, pointB, pointC);
+//
+// System.out.printf("由点 A(%f, %f), B(%f, %f), C(%f, %f) 构成的夹角 ABC 为: %.2f 度%n",
+// pointA[0], pointA[1], pointB[0], pointB[1], pointC[0], pointC[1], angle);
+// }
}
--
Gitblit v1.7.1