package com.xiaopeng.pushMessage.utils;
|
|
import com.xiaopeng.api.dto.operation.LonLatDto;
|
import org.gavaghan.geodesy.Ellipsoid;
|
import org.gavaghan.geodesy.GeodeticCalculator;
|
import org.gavaghan.geodesy.GeodeticCurve;
|
import org.gavaghan.geodesy.GlobalCoordinates;
|
|
import java.time.Instant;
|
import java.util.ArrayList;
|
import java.util.HashMap;
|
import java.util.List;
|
import java.util.Map;
|
import java.util.concurrent.CountDownLatch;
|
import java.util.concurrent.LinkedBlockingQueue;
|
import java.util.concurrent.ThreadPoolExecutor;
|
import java.util.concurrent.TimeUnit;
|
|
public class TrajectoryDensifier {
|
|
private static final int THREAD_NUM = 5;
|
|
|
public static List<LonLatDto> densify(List<LonLatDto> originalPoints, int distance) {
|
int num = originalPoints.size() / THREAD_NUM;
|
if(num == 0){
|
num = 1;
|
}
|
ThreadPoolExecutor poolExecutor = new ThreadPoolExecutor(num, num, 0L, TimeUnit.MILLISECONDS, new LinkedBlockingQueue<>());
|
CountDownLatch latch = new CountDownLatch(num);
|
Map<Integer, List<LonLatDto>> map = new HashMap<>();
|
for (int i = 0; i < num; i++) {
|
List<LonLatDto> trackPoints = originalPoints.subList(i * THREAD_NUM, i == num - 1 ? originalPoints.size() : (i + 1) * THREAD_NUM);
|
int finalI = i;
|
poolExecutor.execute(()->{
|
List<LonLatDto> new_polyline = new ArrayList<>();
|
for (int j = 0; j < trackPoints.size(); j++) {
|
LonLatDto trackPoint = trackPoints.get(j);
|
if(j == trackPoints.size() - 1){
|
new_polyline.add(new LonLatDto(trackPoint.getLongitude(), trackPoint.getLatitude()));
|
break;
|
}
|
LonLatDto trackPoint1 = trackPoints.get(j+1);
|
|
//对两个点位进行坐标补全
|
List<TrackPoint> track = new ArrayList<>();
|
track.add(new TrackPoint(trackPoint.getLatitude(), trackPoint.getLongitude(), null));
|
track.add(new TrackPoint(trackPoint1.getLatitude(), trackPoint1.getLongitude(), null));
|
//10米补全一个点位
|
List<TrackPoint> densified = densifyByDistance(track, distance);
|
for (TrackPoint point : densified) {
|
new_polyline.add(new LonLatDto(point.getLongitude(), point.getLatitude()));
|
}
|
}
|
new_polyline.remove(new_polyline.size() - 1);
|
map.put(finalI, new_polyline);
|
latch.countDown();
|
});
|
}
|
try {
|
latch.await();
|
poolExecutor.shutdown();
|
} catch (InterruptedException e) {
|
e.printStackTrace();
|
throw new RuntimeException(e);
|
}
|
List<LonLatDto> result = new ArrayList<>();
|
for (int i = 0; i < num; i++) {
|
result.addAll(map.get(i));
|
}
|
//添加最后一个点
|
result.add(originalPoints.get(originalPoints.size() - 1));
|
return result;
|
}
|
|
|
|
/**
|
* 轨迹补密主方法
|
* @param originalPoints 原始轨迹点列表（必须按时间排序）
|
* @param distance 补密的间隔距离，例如每 5 米补一个点
|
* @return 补密后的轨迹列表
|
*/
|
public static List<TrackPoint> densifyByDistance(List<TrackPoint> originalPoints, int distance) {
|
List<TrackPoint> result = new ArrayList<>();
|
if (originalPoints == null || originalPoints.size() < 2) {
|
return originalPoints;
|
}
|
|
// 添加第一个点
|
result.add(originalPoints.get(0));
|
|
for (int i = 0; i < originalPoints.size() - 1; i++) {
|
TrackPoint start = originalPoints.get(i);
|
TrackPoint end = originalPoints.get(i + 1);
|
|
GlobalCoordinates source1 = new GlobalCoordinates(start.getLatitude(), start.getLongitude());
|
GlobalCoordinates target1 = new GlobalCoordinates(end.getLatitude(), end.getLongitude());
|
GeodeticCurve geoCurve1 = new GeodeticCalculator().calculateGeodeticCurve(Ellipsoid.WGS84, source1, target1);
|
double distance1 =geoCurve1.getEllipsoidalDistance();
|
|
// 如果两点间时间差小于间隔，说明不需要补密，直接添加终点（在循环结束后添加）
|
if (distance1 <= distance) {
|
// 这里不做处理，等待下一轮或直接添加终点
|
} else {
|
// 计算需要插入多少个点
|
long steps = (long) (distance1 / distance);
|
|
for (long j = 1; j <= steps; j++) {
|
|
// 计算插值比例 t (0.0 ~ 1.0)
|
double fraction = (double) (j * distance) / distance1;
|
|
// 线性插值计算经纬度
|
double newLat = start.getLatitude() + fraction * (end.getLatitude() - start.getLatitude());
|
double newLon = start.getLongitude() + fraction * (end.getLongitude() - start.getLongitude());
|
|
result.add(new TrackPoint(newLat, newLon, null));
|
}
|
}
|
|
// 确保每一段的终点被添加（除了最后一次循环，因为循环结束后会添加最后一个点）
|
// 为了避免重复添加，我们只在循环最后添加最后一个点，或者在这里判断
|
}
|
|
// 添加最后一个点
|
// result.add(originalPoints.get(originalPoints.size() - 1));
|
|
return result;
|
}
|
|
|
|
/**
|
* 轨迹补密主方法
|
* @param originalPoints 原始轨迹点列表（必须按时间排序）
|
* @param intervalSeconds 补密的时间间隔（秒），例如每 5 秒补一个点
|
* @return 补密后的轨迹列表
|
*/
|
public static List<TrackPoint> densifyByTime(List<TrackPoint> originalPoints, long intervalSeconds) {
|
List<TrackPoint> result = new ArrayList<>();
|
if (originalPoints == null || originalPoints.size() < 2) {
|
return originalPoints;
|
}
|
|
// 添加第一个点
|
result.add(originalPoints.get(0));
|
|
for (int i = 0; i < originalPoints.size() - 1; i++) {
|
TrackPoint start = originalPoints.get(i);
|
TrackPoint end = originalPoints.get(i + 1);
|
|
long timeDiffMillis = end.getTimestamp().toEpochMilli() - start.getTimestamp().toEpochMilli();
|
long intervalMillis = intervalSeconds * 1000;
|
|
// 如果两点间时间差小于间隔，说明不需要补密，直接添加终点（在循环结束后添加）
|
if (timeDiffMillis <= intervalMillis) {
|
// 这里不做处理，等待下一轮或直接添加终点
|
} else {
|
// 计算需要插入多少个点
|
long steps = timeDiffMillis / intervalMillis;
|
|
for (long j = 1; j <= steps; j++) {
|
// 计算当前插入点的时间
|
Instant newTime = start.getTimestamp().plusMillis(j * intervalMillis);
|
|
// 如果计算出的时间超过了终点时间，则停止（防止浮点数误差或边界问题）
|
if (newTime.isAfter(end.getTimestamp())) {
|
break;
|
}
|
|
// 计算插值比例 t (0.0 ~ 1.0)
|
double fraction = (double) (j * intervalMillis) / timeDiffMillis;
|
|
// 线性插值计算经纬度
|
double newLat = start.getLatitude() + fraction * (end.getLatitude() - start.getLatitude());
|
double newLon = start.getLongitude() + fraction * (end.getLongitude() - start.getLongitude());
|
|
result.add(new TrackPoint(newLat, newLon, newTime));
|
}
|
}
|
|
// 确保每一段的终点被添加（除了最后一次循环，因为循环结束后会添加最后一个点）
|
// 为了避免重复添加，我们只在循环最后添加最后一个点，或者在这里判断
|
}
|
|
// 添加最后一个点
|
result.add(originalPoints.get(originalPoints.size() - 1));
|
|
return result;
|
}
|
|
// 主函数测试
|
public static void main(String[] args) {
|
// List<TrackPoint> track = new ArrayList<>();
|
// // 模拟数据：两个点相差 20 秒
|
// track.add(new TrackPoint(39.9042, 116.4074, Instant.parse("2026-03-28T10:00:00Z"))); // 北京
|
// track.add(new TrackPoint(39.9050, 116.4080, Instant.parse("2026-03-28T10:00:20Z")));
|
//
|
// // 每 5 秒补一个点
|
// List<TrackPoint> densified = densifyByDistance(track, 10);
|
//
|
// System.out.println("原始点数: " + track.size());
|
// System.out.println("补密后点数: " + densified.size());
|
// System.out.println("轨迹详情:");
|
// for (TrackPoint p : densified) {
|
// System.out.println("[" + p.getLongitude() + ", " + p.getLatitude() + "]");
|
// }
|
|
List<LonLatDto> track = new ArrayList<>();
|
track.add(new LonLatDto(113.386036, 23.15308));
|
track.add(new LonLatDto(113.385671, 23.153252));
|
track.add(new LonLatDto(113.385402, 23.153397));
|
track.add(new LonLatDto(113.385211, 23.15352));
|
|
List<LonLatDto> densify = densify(track, 10);
|
for (LonLatDto p : densify) {
|
System.out.println("[" + p.getLongitude() + ", " + p.getLatitude() + "]");
|
}
|
}
|
|
}
|
