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.*;
|
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", new ArrayList<>());
|
map.put("served", new ArrayList<>());
|
return map;
|
}
|
//大于5米的情况，将车子当前位置添加进结果中
|
map.put("unserved", polyline);
|
map.put("served", new ArrayList<>());
|
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 && distance1 > distance3){
|
//继续向前找，找车辆处于那一段点位中间
|
continue;
|
}
|
//车辆位置在两点之间的情况
|
//车辆位置到上一个点的距离+车辆位置到下一个点的距离 = 上一个点到下一个点的距离
|
//车辆位置到上一个点的距离+车辆位置到下一个点的距离 <= 上一个点到下一个点的距离 + 5米的漂移量
|
if ((distance1 + distance2) == distance3 || (distance1 + distance2) <= distance3 + 5) {
|
//剩余未服务的点位
|
List<LonLatDto> unserved = new ArrayList<>(polyline.subList(i + 1, polyline.size()));
|
List<LonLatDto> served = new ArrayList<>(polyline.subList(0, i + 1));
|
map.put("unserved", unserved);
|
map.put("served", served);
|
|
System.err.println("车辆在两点之间，从第二个开始截取 : " + i);
|
return map;
|
}
|
}
|
//所有数据遍历完后任然没有合适的数据则返回原始数据
|
map.put("unserved", polyline);
|
map.put("served", new ArrayList<>());
|
return map;
|
}
|
|
|
|
|
|
/**
|
* 轨迹裁剪（线段版本）
|
* @param polyline
|
* @param currentPoint
|
* @return
|
*/
|
public static List<LonLatDto> clipBySegmentDistance(List<LonLatDto> polyline,LonLatDto currentPoint) {
|
|
if (polyline == null || polyline.size() < 2) {
|
return Collections.emptyList();
|
}
|
|
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) {
|
break;
|
}
|
}
|
|
// 从最优线段的后一个点开始截取
|
return new ArrayList<>(
|
polyline.subList(minSegmentIndex + 1, polyline.size())
|
);
|
}
|
|
|
/**
|
* 点到线段距离
|
* @param p
|
* @param a
|
* @param b
|
* @return
|
*/
|
public static double pointToSegmentDistance(
|
LonLatDto p,
|
LonLatDto a,
|
LonLatDto b) {
|
|
double refLat = p.getLatitude();
|
|
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 x = pXY[0];
|
double y = pXY[1];
|
|
double x1 = aXY[0];
|
double y1 = aXY[1];
|
|
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);
|
}
|
|
/**
|
* 经纬度转平面坐标（局部近似）
|
* @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) {
|
String json = "[{\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" +
|
"}]";
|
List<LonLatDto> polyline = JSON.parseArray(json, LonLatDto.class);
|
String xcgj = "[{\"longitude\":113.38368194444445,\"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;
|
|
}
|
|
|
|
|
// 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}]");
|
// String str = "[";
|
// for (int i = 0; i < objects.size(); i++) {
|
// JSONObject jsonObject = objects.getJSONObject(i);
|
// str += "[" + jsonObject.getDouble("longitude") + "," + jsonObject.getDouble("latitude") + "],";
|
// }
|
// System.err.println(str + "]");
|
}
|
|
}
|
