package com.xiaopeng.automaticPilot.util;
|
|
import java.util.ArrayList;
|
import java.util.List;
|
|
/**
|
* GPS路径生成工具
|
*/
|
public class GpsPathGenerator {
|
|
/**
|
* 地球半径（米）
|
*/
|
private static final double EARTH_RADIUS = 6371000;
|
|
/**
|
* 简单GPS坐标点
|
*/
|
public static class GpsPoint {
|
public final double longitude;
|
public final double latitude;
|
|
public GpsPoint(double longitude, double latitude) {
|
this.longitude = longitude;
|
this.latitude = latitude;
|
}
|
}
|
|
/**
|
* 生成从起点到终点的GPS路径
|
*
|
* @param startLongitude 起点经度
|
* @param startLatitude 起点纬度
|
* @param endLongitude 终点经度
|
* @param endLatitude 终点纬度
|
* @param intervalSeconds 间隔秒数（每隔多少秒一个点）
|
* @param speed 速度（米/秒），默认10.5米/秒
|
* @return GPS坐标点列表
|
*/
|
public static List<GpsPoint> generatePath(double startLongitude, double startLatitude,
|
double endLongitude, double endLatitude,
|
int intervalSeconds, double speed) {
|
List<GpsPoint> pathPoints = new ArrayList<>();
|
|
// 计算总距离（米）
|
double totalDistance = calculateDistance(startLatitude, startLongitude, endLatitude, endLongitude);
|
|
// 计算每个间隔移动的距离
|
double distancePerInterval = speed * intervalSeconds;
|
|
// 计算需要的点数
|
int pointCount = (int) Math.ceil(totalDistance / distancePerInterval) + 1;
|
|
// 生成路径点
|
for (int i = 0; i < pointCount; i++) {
|
double ratio = i * distancePerInterval / totalDistance;
|
if (ratio > 1.0) {
|
ratio = 1.0;
|
}
|
|
// 线性插值计算中间点
|
double longitude = startLongitude + (endLongitude - startLongitude) * ratio;
|
double latitude = startLatitude + (endLatitude - startLatitude) * ratio;
|
|
pathPoints.add(new GpsPoint(longitude, latitude));
|
}
|
|
return pathPoints;
|
}
|
|
|
/**
|
* 使用Haversine公式计算两个GPS坐标之间的距离（米）
|
*
|
* @param lat1 起点纬度
|
* @param lon1 起点经度
|
* @param lat2 终点纬度
|
* @param lon2 终点经度
|
* @return 距离（米）
|
*/
|
public static double calculateDistance(double lat1, double lon1, double lat2, double lon2) {
|
double lat1Rad = Math.toRadians(lat1);
|
double lat2Rad = Math.toRadians(lat2);
|
double deltaLat = Math.toRadians(lat2 - lat1);
|
double deltaLon = Math.toRadians(lon2 - lon1);
|
|
double a = Math.sin(deltaLat / 2) * Math.sin(deltaLat / 2) +
|
Math.cos(lat1Rad) * Math.cos(lat2Rad) *
|
Math.sin(deltaLon / 2) * Math.sin(deltaLon / 2);
|
double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
|
return EARTH_RADIUS * c;
|
}
|
|
/**
|
* 计算两个点之间的方位角（度）
|
*
|
* @param lat1 起点纬度
|
* @param lon1 起点经度
|
* @param lat2 终点纬度
|
* @param lon2 终点经度
|
* @return 方位角（0-360度）
|
*/
|
public static int calculateCourse(double lat1, double lon1, double lat2, double lon2) {
|
double lat1Rad = Math.toRadians(lat1);
|
double lat2Rad = Math.toRadians(lat2);
|
double deltaLon = Math.toRadians(lon2 - lon1);
|
|
double y = Math.sin(deltaLon) * Math.cos(lat2Rad);
|
double x = Math.cos(lat1Rad) * Math.sin(lat2Rad) -
|
Math.sin(lat1Rad) * Math.cos(lat2Rad) * Math.cos(deltaLon);
|
|
double bearing = Math.toDegrees(Math.atan2(y, x));
|
|
// 转换为0-360度
|
return (int) ((bearing + 360) % 360);
|
}
|
}
|
