package com.xiaopeng.pushMessage.utils;
|
|
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.List;
|
|
public class TrajectoryDensifier {
|
|
|
/**
|
* 轨迹补密主方法
|
* @param originalPoints 原始轨迹点列表（必须按时间排序）
|
* @param distance 补密的间隔距离，例如每 5 米补一个点
|
* @return 补密后的轨迹列表
|
*/
|
public static List<TrackPoint> densifyByTime(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 = densifyByTime(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() + "]");
|
}
|
}
|
|
}
|
