package com.xiaopeng.pushMessage.utils;
|
|
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.concurrent.*;
|
|
|
@Slf4j
|
public class MapMatchingUtil {
|
|
|
|
|
/**
|
* 获取剩余导航数据
|
* @param polyline
|
* @param longitude
|
* @param latitude
|
* @return
|
*/
|
public static List<LonLatDto> getRemainingNavigation(List<LonLatDto> polyline, Double longitude, Double latitude){
|
//导航数据只有一个点的情况
|
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){
|
return Collections.emptyList();
|
}
|
//大于5米的情况，将车子当前位置添加进结果中
|
lonLatDto = new LonLatDto(longitude, latitude);
|
polyline.add(0, lonLatDto);
|
return polyline;
|
}
|
//多个点位的情况，需要找出当前点位处于那两个点位之间，然后截取掉超出的部分轨迹
|
return getRecentlyPoint(polyline, longitude, latitude);
|
}
|
|
|
/**
|
* 判断点位在轨迹线路中的点位之间
|
* 返回截取后的剩余轨迹
|
* @param polyline
|
* @param longitude
|
* @param latitude
|
* @return
|
*/
|
public static List<LonLatDto> getRecentlyPoint(List<LonLatDto> polyline, Double longitude, Double latitude){
|
for (int i = 0; i < polyline.size(); i++) {
|
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){
|
//取距离最近的点位数据
|
if(distance1 < distance2){
|
return polyline.subList(i, polyline.size() - 1);
|
}
|
return polyline.subList(i + 1, polyline.size() - 1);
|
}
|
}
|
return Collections.emptyList();
|
}
|
|
|
|
|
|
|
|
/**
|
* 轨迹裁剪（线段版本）
|
* @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};
|
}
|
|
|
}
|
