package com.xiaopeng.pushMessage.utils;
|
|
import com.xiaopeng.api.dto.operation.LonLatDto;
|
import lombok.extern.slf4j.Slf4j;
|
|
import java.util.ArrayList;
|
import java.util.Collections;
|
import java.util.List;
|
|
@Slf4j
|
public class MapMatchingUtil {
|
|
/**
|
* 轨迹裁剪（线段版本）
|
* @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};
|
}
|
|
|
}
|
