package com.ard.tianyu.controller.wayline.terrain;
|
|
import com.ard.tianyu.domain.dto.wayline.ArdActionDTO;
|
import com.ard.tianyu.domain.dto.wayline.ArdPositionDTO;
|
import com.ard.tianyu.util.GeoTools;
|
import com.ard.tianyu.util.Vertex;
|
import com.ard.tianyu.domain.dto.wayline.ArdUavPositionDTO;
|
import com.ard.tianyu.domain.dto.wayline.ArdWaylineDTO;
|
import com.fasterxml.jackson.databind.ObjectMapper;
|
import org.springframework.stereotype.Component;
|
import javax.annotation.Resource;
|
import java.io.IOException;
|
import java.util.ArrayList;
|
import java.util.List;
|
|
/**
|
* 仿地飞行规划器
|
*
|
* @author lsy
|
* @date 2025/6/18
|
*/
|
@Component
|
public class TerrainFollowingPlanner {
|
@Resource
|
private GeoUtils geoUtils;
|
|
private final double intervalMeter = 100.0; // 插值距离
|
private final double smoothThreshold = 3.0; // 平滑阈值
|
private static final ObjectMapper mapper = new ObjectMapper();
|
|
// 算法主函数
|
public List<ArdUavPositionDTO> interpolateWithTerrain(ArdWaylineDTO ardWaylineDTO) throws Exception {
|
// 0. 验证源航线数据
|
validateWaylineData(ardWaylineDTO);
|
|
// 1. 先进行纯几何插值（不考虑高度）
|
List<ArdUavPositionDTO> interpolatedPoints = pureInterpolation(ardWaylineDTO.getUavPositions());
|
|
// 2. 然后统一处理地形高度
|
return adjustAltitudeForTerrain(interpolatedPoints, ardWaylineDTO.getGlobalFlyHeight());
|
}
|
|
// 第一阶段：纯几何插值
|
private List<ArdUavPositionDTO> pureInterpolation(List<ArdUavPositionDTO> originalPoints) {
|
List<ArdUavPositionDTO> result = new ArrayList<>();
|
|
for (int i = 0; i < originalPoints.size() - 1; i++) {
|
ArdUavPositionDTO start = originalPoints.get(i);
|
ArdUavPositionDTO end = originalPoints.get(i + 1);
|
|
// 添加原始起点
|
result.add(deepCopy(start, ArdUavPositionDTO.class));
|
|
// 插值中间点
|
result.addAll(interpolateBetween(start, end));
|
}
|
|
// 添加最后一个点
|
result.add(deepCopy(originalPoints.get(originalPoints.size() - 1), ArdUavPositionDTO.class));
|
|
return result;
|
}
|
|
// 第二阶段：地形高度调整
|
private List<ArdUavPositionDTO> adjustAltitudeForTerrain(List<ArdUavPositionDTO> points, double flyHeight) throws IOException {
|
|
double homeTerrainHeight = 0; // 记录起飞点地形高度作为基准海拔
|
double lastFlightAltitude = flyHeight; // 记录上一个飞行高度
|
|
for (int i = 0; i < points.size(); i++) {
|
ArdUavPositionDTO point = points.get(i);
|
double currentTerrainHeight = geoUtils.getHeight(
|
point.getUavPosition().getLongitude(),
|
point.getUavPosition().getLatitude()
|
);
|
if (i == 0) {
|
homeTerrainHeight = currentTerrainHeight;
|
}
|
// 计算相对于起飞点的地形高度差
|
double terrainHeightDiff = currentTerrainHeight - homeTerrainHeight;
|
|
// 理论目标高度 = 设定高度 + 地形高度变化量
|
double targetAltitude = flyHeight + terrainHeightDiff;
|
|
// 平滑处理：如果高度变化不大，沿用上一个飞行高度
|
if (i > 0 && Math.abs(targetAltitude - lastFlightAltitude) < smoothThreshold) {
|
targetAltitude = lastFlightAltitude;
|
}
|
|
// 设置高度
|
point.getUavPosition().setAltitude(targetAltitude);
|
updateActionAltitude(point, currentTerrainHeight);
|
|
lastFlightAltitude = targetAltitude; // 更新最后飞行高度
|
}
|
|
renumberWaypoints(points);
|
return points;
|
}
|
|
// 辅助方法：两点间插值
|
private List<ArdUavPositionDTO> interpolateBetween(ArdUavPositionDTO start, ArdUavPositionDTO end) {
|
List<ArdUavPositionDTO> interpolated = new ArrayList<>();
|
// 计算两点间的距离
|
Vertex startV=new Vertex( start.getUavPosition().getLatitude(), start.getUavPosition().getLongitude());
|
Vertex endV=new Vertex( end.getUavPosition().getLatitude(), end.getUavPosition().getLongitude());
|
double distance = GeoTools.DistanceSphere(startV,endV);
|
// 如果距离小于插值间隔，则直接返回
|
if (distance <= intervalMeter) return interpolated;
|
// 计算插值步数
|
int steps = (int) (distance / intervalMeter);
|
for (int i = 1; i < steps; i++) {
|
double ratio = i * intervalMeter / distance;
|
double lat = interpolate(
|
start.getUavPosition().getLatitude(),
|
end.getUavPosition().getLatitude(),
|
ratio
|
);
|
double lon = interpolate(
|
start.getUavPosition().getLongitude(),
|
end.getUavPosition().getLongitude(),
|
ratio
|
);
|
|
ArdUavPositionDTO newPoint = deepCopy(start, ArdUavPositionDTO.class);
|
newPoint.setType(1);
|
newPoint.getUavPosition().setLatitude(lat);
|
newPoint.getUavPosition().setLongitude(lon);
|
interpolated.add(newPoint);
|
}
|
|
return interpolated;
|
}
|
|
/**
|
* 更新航点动作中的地面高度
|
*
|
* @param point 航点对象
|
* @param terrainHeight 当前地形高度
|
*/
|
private void updateActionAltitude(ArdUavPositionDTO point, double terrainHeight) {
|
if (point.getActionGroup() == null || point.getActionGroup().isEmpty()) {
|
return;
|
}
|
|
for (ArdActionDTO action : point.getActionGroup()) {
|
// 如果已有地面位置，则更新高度
|
if (action.getGroundPosition() != null) {
|
action.getGroundPosition().setLongitude(point.getUavPosition().getLongitude());
|
action.getGroundPosition().setLatitude(point.getUavPosition().getLatitude());
|
action.getGroundPosition().setAltitude(terrainHeight);
|
}
|
// 如果没有地面位置，则创建新的地面位置
|
else {
|
action.setGroundPosition(ArdPositionDTO.builder()
|
.longitude(point.getUavPosition().getLongitude())
|
.latitude(point.getUavPosition().getLatitude())
|
.altitude(terrainHeight)
|
.build());
|
}
|
}
|
}
|
|
/**
|
* 重新编号航点和动作
|
*
|
* @param points 航点列表
|
*/
|
private void renumberWaypoints(List<ArdUavPositionDTO> points) {
|
// 航点重新编号
|
for (int i = 0; i < points.size(); i++) {
|
ArdUavPositionDTO point = points.get(i);
|
point.setIndex(i + 1); // 通常从1开始编号
|
|
// 动作重新编号
|
if (point.getActionGroup() != null && !point.getActionGroup().isEmpty()) {
|
int actionIndex = 1;
|
for (ArdActionDTO action : point.getActionGroup()) {
|
action.setActionIndex(actionIndex++);
|
}
|
}
|
}
|
}
|
|
|
/**
|
* 校验航线数据有效性
|
*
|
* @param ardWaylineDTO 航线数据
|
* @throws IllegalArgumentException 如果数据无效
|
*/
|
private void validateWaylineData(ArdWaylineDTO ardWaylineDTO) {
|
if (ardWaylineDTO == null) {
|
throw new IllegalArgumentException("航线数据不能为null");
|
}
|
if (ardWaylineDTO.getUavPositions() == null || ardWaylineDTO.getUavPositions().size() < 2) {
|
throw new IllegalArgumentException("航点数量必须大于等于2");
|
}
|
if (ardWaylineDTO.getGlobalFlyHeight() <= 0) {
|
throw new IllegalArgumentException("全局飞行高度必须大于0");
|
}
|
}
|
|
// 通用深拷贝
|
public static <T> T deepCopy(T original, Class<T> clazz) {
|
if (original == null) {
|
return null;
|
}
|
try {
|
byte[] bytes = mapper.writeValueAsBytes(original);
|
return mapper.readValue(bytes, clazz);
|
} catch (Exception e) {
|
e.printStackTrace();
|
return null;
|
}
|
}
|
|
// 线性插值
|
public double interpolate(double start, double end, double ratio) {
|
return start + (end - start) * ratio;
|
}
|
}
|
