// ================================
|
// A*航线规划模块 - 基于3D栅格索引
|
// ================================
|
import * as Cesium from 'cesium'
|
|
class PathPlanning {
|
constructor(viewer, occupancyGrid) {
|
this.viewer = viewer;
|
this.occupancyGrid = occupancyGrid;
|
this.pathEntities = [];
|
this.isEnabled = false;
|
|
// 路径配置
|
this.config = {
|
pathColor: Cesium.Color.YELLOW.withAlpha(0.8), // 路径颜色（黄色）
|
pathOutlineColor: Cesium.Color.ORANGE, // 路径边框颜色
|
startPointColor: Cesium.Color.BLUE.withAlpha(0.9), // 起点颜色（蓝色）
|
endPointColor: Cesium.Color.BLUE.withAlpha(0.9), // 终点颜色（蓝色）
|
startEndOutlineColor: Cesium.Color.DARKBLUE, // 起点终点边框颜色
|
outlineWidth: 2, // 边框宽度
|
enableDiagonal: true, // 是否允许对角线移动（3D中为26连通性）
|
verticalWeight: 1.2 // 垂直移动权重（略高于水平移动）
|
};
|
|
// 显示控制
|
this.showOnlyPath = false; // 是否只显示路径单元
|
this.hasActivePath = false; // 是否有活跃的路径
|
|
// 3D栅格数据结构
|
this.grid3D = null; // 3维数组存储占用状态
|
this.gridDimensions = { width: 0, height: 0, depth: 0 }; // 栅格尺寸
|
this.gridEntityMap = new Map(); // 栅格索引(字符串)到实体的映射
|
this.indexToEntityMap = new Map(); // 栅格索引对象到实体的映射
|
|
// A*算法相关
|
this.openList = []; // 开放列表
|
this.closedList = []; // 关闭列表
|
}
|
|
// ================================
|
// 启用/禁用路径规划功能
|
// ================================
|
setEnabled(enabled) {
|
this.isEnabled = enabled;
|
if (!enabled) {
|
this.clearPath();
|
}
|
}
|
|
// ================================
|
// A*路径规划主函数 - 完全基于3D栅格索引
|
// ================================
|
async planPath() {
|
if (!this.isEnabled || !this.occupancyGrid.gridEntities.length) {
|
console.warn('路径规划未启用或网格未生成');
|
return;
|
}
|
|
try {
|
console.log('开始基于3D栅格索引的A*路径规划...');
|
|
// 1. 构建3D栅格索引结构和占用状态数组
|
this.build3DGridStructure();
|
|
// 2. 通过最近邻查找获取航线起点和终点对应的3D栅格索引
|
const { startIndex, endIndex } = await this.findWaypointGridIndices();
|
|
if (!startIndex || !endIndex) {
|
console.warn('无法找到航线起点或终点对应的栅格索引');
|
alert('无法找到航线起点或终点对应的栅格索引！');
|
return;
|
}
|
|
console.log('起点栅格索引:', startIndex);
|
console.log('终点栅格索引:', endIndex);
|
console.log('3D栅格维度:', this.gridDimensions);
|
|
// 3. 首先显示起点和终点（蓝色）
|
console.log('首先显示起点和终点...');
|
this.visualizeStartEndPoints(startIndex, endIndex);
|
|
// 等待一小段时间让用户看到起点和终点
|
await new Promise(resolve => setTimeout(resolve, 1000));
|
|
// 4. 在三维数组上执行A*路径规划
|
console.log('开始执行A*路径规划算法...');
|
const pathIndices = this.aStarOnGrid3D(startIndex, endIndex);
|
|
if (pathIndices && pathIndices.length > 0) {
|
// 5. 通过索引找到对应的3D占用网格单元，复制并修改颜色（包含起点终点的蓝色）
|
this.visualizePathByGridCopy(pathIndices, startIndex, endIndex);
|
console.log(`路径规划成功，路径长度：${pathIndices.length}个栅格索引`);
|
|
// 更新UI
|
this.updatePathInfo(pathIndices);
|
} else {
|
console.warn('未找到可行路径');
|
alert('未找到从起点到终点的可行路径！');
|
// 即使没找到路径，也保持起点终点的显示
|
}
|
|
} catch (error) {
|
console.error('路径规划错误：', error);
|
alert('路径规划过程中发生错误，请查看控制台日志。');
|
}
|
}
|
|
// ================================
|
// 构建3D栅格索引结构和占用状态数组 - 直接使用网格实体properties数据
|
// ================================
|
build3DGridStructure() {
|
console.log('开始构建3D栅格索引结构，直接使用网格实体properties数据...');
|
|
if (!this.occupancyGrid.gridEntities.length) {
|
throw new Error('没有可用的占用网格实体');
|
}
|
|
// 第一步：分析所有网格实体，从properties中提取gridIndex，确定栅格维度
|
let minX = Infinity, minY = Infinity, minZ = Infinity;
|
let maxX = -Infinity, maxY = -Infinity, maxZ = -Infinity;
|
|
const validEntities = [];
|
|
// 遍历所有网格实体，提取gridIndex信息
|
for (const entity of this.occupancyGrid.gridEntities) {
|
// 直接从properties中获取gridIndex
|
if (entity.properties && entity.properties.gridIndex) {
|
const gridIndex = entity.properties.gridIndex.getValue();
|
|
if (gridIndex && typeof gridIndex.x === 'number' &&
|
typeof gridIndex.y === 'number' && typeof gridIndex.z === 'number') {
|
|
// 更新维度范围
|
minX = Math.min(minX, gridIndex.x);
|
maxX = Math.max(maxX, gridIndex.x);
|
minY = Math.min(minY, gridIndex.y);
|
maxY = Math.max(maxY, gridIndex.y);
|
minZ = Math.min(minZ, gridIndex.z);
|
maxZ = Math.max(maxZ, gridIndex.z);
|
|
validEntities.push(entity);
|
} else {
|
console.warn('网格实体properties中的gridIndex格式不正确:', gridIndex);
|
}
|
} else {
|
console.warn('网格实体缺少gridIndex属性');
|
}
|
}
|
|
if (validEntities.length === 0) {
|
throw new Error('没有找到包含有效gridIndex的网格实体');
|
}
|
|
// 计算栅格维度（基于实际的索引范围）
|
this.gridDimensions = {
|
width: maxX - minX + 1,
|
height: maxY - minY + 1,
|
depth: maxZ - minZ + 1
|
};
|
|
console.log(`从properties提取的3D栅格维度: ${this.gridDimensions.width} x ${this.gridDimensions.height} x ${this.gridDimensions.depth}`);
|
console.log(`索引范围: X[${minX}, ${maxX}], Y[${minY}, ${maxY}], Z[${minZ}, ${maxZ}]`);
|
|
// 第二步：初始化3D占用状态数组
|
this.grid3D = new Array(this.gridDimensions.width);
|
for (let x = 0; x < this.gridDimensions.width; x++) {
|
this.grid3D[x] = new Array(this.gridDimensions.height);
|
for (let y = 0; y < this.gridDimensions.height; y++) {
|
this.grid3D[x][y] = new Array(this.gridDimensions.depth);
|
for (let z = 0; z < this.gridDimensions.depth; z++) {
|
this.grid3D[x][y][z] = {
|
occupied: false,
|
entity: null,
|
exists: false
|
};
|
}
|
}
|
}
|
|
// 第三步：直接使用实体properties数据填充3D数组
|
this.gridEntityMap.clear();
|
this.indexToEntityMap.clear();
|
|
let mappedCount = 0;
|
let occupiedCount = 0;
|
let freeCount = 0;
|
|
// 记录索引偏移量，将原始索引映射到数组索引
|
this.indexOffset = { x: minX, y: minY, z: minZ };
|
|
for (const entity of validEntities) {
|
const gridIndex = entity.properties.gridIndex.getValue();
|
const isOccupied = entity.properties.isOccupied ? entity.properties.isOccupied.getValue() : false;
|
|
// 转换为数组索引（相对于最小值的偏移）
|
const arrayX = gridIndex.x - minX;
|
const arrayY = gridIndex.y - minY;
|
const arrayZ = gridIndex.z - minZ;
|
|
// 确保索引在有效范围内
|
if (arrayX >= 0 && arrayX < this.gridDimensions.width &&
|
arrayY >= 0 && arrayY < this.gridDimensions.height &&
|
arrayZ >= 0 && arrayZ < this.gridDimensions.depth) {
|
|
// 存储到3D数组
|
this.grid3D[arrayX][arrayY][arrayZ] = {
|
occupied: isOccupied,
|
entity: entity,
|
exists: true,
|
originalIndex: { x: gridIndex.x, y: gridIndex.y, z: gridIndex.z } // 保存原始索引
|
};
|
|
// 建立映射关系
|
const gridKey = `${arrayX},${arrayY},${arrayZ}`;
|
this.gridEntityMap.set(gridKey, entity);
|
this.indexToEntityMap.set(`${arrayX}_${arrayY}_${arrayZ}`, entity);
|
|
mappedCount++;
|
if (isOccupied) {
|
occupiedCount++;
|
} else {
|
freeCount++;
|
}
|
} else {
|
console.warn(`网格索引超出范围: 原始(${gridIndex.x}, ${gridIndex.y}, ${gridIndex.z}) -> 数组(${arrayX}, ${arrayY}, ${arrayZ})`);
|
}
|
}
|
|
console.log(`3D栅格结构构建完成:`);
|
console.log(`- 栅格维度: ${this.gridDimensions.width} x ${this.gridDimensions.height} x ${this.gridDimensions.depth}`);
|
console.log(`- 索引偏移: (${this.indexOffset.x}, ${this.indexOffset.y}, ${this.indexOffset.z})`);
|
console.log(`- 映射实体数: ${mappedCount}`);
|
console.log(`- 占用栅格: ${occupiedCount}`);
|
console.log(`- 空闲栅格: ${freeCount}`);
|
console.log(`- 占用率: ${mappedCount > 0 ? ((occupiedCount / mappedCount) * 100).toFixed(1) : 0}%`);
|
}
|
|
// ================================
|
// 通过最近邻查找获取航线起点和终点对应的3D栅格索引
|
// ================================
|
async findWaypointGridIndices() {
|
console.log('开始通过最近邻查找航线起点和终点对应的栅格索引...');
|
|
// 获取航线航点
|
const waypoints = this.getFlightWaypoints();
|
|
if (!waypoints || waypoints.length < 2) {
|
console.warn('航线航点不足，无法确定起点和终点');
|
return { startIndex: null, endIndex: null };
|
}
|
|
const startWaypoint = waypoints[0];
|
const endWaypoint = waypoints[waypoints.length - 1];
|
|
console.log('航线起点位置:', startWaypoint.position);
|
console.log('航线终点位置:', endWaypoint.position);
|
|
// 使用最近邻搜索找到对应的栅格索引
|
const startIndex = this.findNearestGridIndex(startWaypoint.position);
|
const endIndex = this.findNearestGridIndex(endWaypoint.position);
|
|
if (!startIndex) {
|
console.error('无法找到航线起点对应的栅格索引');
|
return { startIndex: null, endIndex: null };
|
}
|
|
if (!endIndex) {
|
console.error('无法找到航线终点对应的栅格索引');
|
return { startIndex: null, endIndex: null };
|
}
|
|
// 验证找到的栅格索引是否可通行
|
if (this.grid3D[startIndex.x][startIndex.y][startIndex.z].occupied) {
|
console.warn('起点栅格被占用，尝试寻找附近的空闲栅格...');
|
const freeStartIndex = this.findNearestFreeGrid(startIndex);
|
if (freeStartIndex) {
|
console.log(`找到起点附近的空闲栅格: (${freeStartIndex.x}, ${freeStartIndex.y}, ${freeStartIndex.z})`);
|
startIndex.x = freeStartIndex.x;
|
startIndex.y = freeStartIndex.y;
|
startIndex.z = freeStartIndex.z;
|
} else {
|
console.error('起点附近没有可通行的栅格');
|
return { startIndex: null, endIndex: null };
|
}
|
}
|
|
if (this.grid3D[endIndex.x][endIndex.y][endIndex.z].occupied) {
|
console.warn('终点栅格被占用，尝试寻找附近的空闲栅格...');
|
const freeEndIndex = this.findNearestFreeGrid(endIndex);
|
if (freeEndIndex) {
|
console.log(`找到终点附近的空闲栅格: (${freeEndIndex.x}, ${freeEndIndex.y}, ${freeEndIndex.z})`);
|
endIndex.x = freeEndIndex.x;
|
endIndex.y = freeEndIndex.y;
|
endIndex.z = freeEndIndex.z;
|
} else {
|
console.error('终点附近没有可通行的栅格');
|
return { startIndex: null, endIndex: null };
|
}
|
}
|
|
console.log(`成功找到起点栅格索引: (${startIndex.x}, ${startIndex.y}, ${startIndex.z})`);
|
console.log(`成功找到终点栅格索引: (${endIndex.x}, ${endIndex.y}, ${endIndex.z})`);
|
|
return { startIndex, endIndex };
|
}
|
|
// ================================
|
// 最近邻搜索：找到距离指定世界坐标最近的栅格索引 - 直接使用properties数据
|
// ================================
|
findNearestGridIndex(targetPosition) {
|
let nearestIndex = null;
|
let minDistance = Infinity;
|
let nearestEntity = null;
|
|
// 直接遍历所有网格实体，使用properties中的gridIndex
|
for (const entity of this.occupancyGrid.gridEntities) {
|
if (entity.properties && entity.properties.gridIndex && entity.position) {
|
try {
|
// 获取栅格实体的位置
|
const entityPosition = entity.position.getValue(this.viewer.clock.currentTime);
|
|
// 计算距离
|
const distance = Cesium.Cartesian3.distance(targetPosition, entityPosition);
|
|
if (distance < minDistance) {
|
minDistance = distance;
|
nearestEntity = entity;
|
|
// 直接从properties获取gridIndex，并转换为数组索引
|
const originalIndex = entity.properties.gridIndex.getValue();
|
nearestIndex = {
|
x: originalIndex.x - this.indexOffset.x,
|
y: originalIndex.y - this.indexOffset.y,
|
z: originalIndex.z - this.indexOffset.z,
|
original: originalIndex // 保存原始索引以便调试
|
};
|
}
|
} catch (error) {
|
console.warn('处理网格实体时出错:', error);
|
}
|
}
|
}
|
|
if (nearestIndex) {
|
console.log(`找到最近栅格索引: 数组索引(${nearestIndex.x}, ${nearestIndex.y}, ${nearestIndex.z}), 原始索引(${nearestIndex.original.x}, ${nearestIndex.original.y}, ${nearestIndex.original.z}), 距离: ${minDistance.toFixed(2)}m`);
|
|
// 验证找到的索引是否有效
|
if (!this.isValidGridIndex(nearestIndex)) {
|
console.error('找到的栅格索引无效');
|
return null;
|
}
|
|
// 移除original属性，只返回数组索引
|
return { x: nearestIndex.x, y: nearestIndex.y, z: nearestIndex.z };
|
} else {
|
console.warn('未找到最近的栅格索引');
|
return null;
|
}
|
}
|
|
// ================================
|
// 寻找指定栅格索引附近的空闲（可通行）栅格
|
// ================================
|
findNearestFreeGrid(centerIndex) {
|
const searchRadius = 3; // 搜索半径
|
|
for (let radius = 1; radius <= searchRadius; radius++) {
|
for (let dx = -radius; dx <= radius; dx++) {
|
for (let dy = -radius; dy <= radius; dy++) {
|
for (let dz = -radius; dz <= radius; dz++) {
|
const x = centerIndex.x + dx;
|
const y = centerIndex.y + dy;
|
const z = centerIndex.z + dz;
|
|
if (this.isValidGridIndex({ x, y, z })) {
|
const cell = this.grid3D[x][y][z];
|
if (cell.exists && !cell.occupied) {
|
return { x, y, z };
|
}
|
}
|
}
|
}
|
}
|
}
|
|
return null; // 未找到空闲栅格
|
}
|
|
// ================================
|
// 获取航线航点
|
// ================================
|
getFlightWaypoints() {
|
// 尝试从UAVApp获取航线航点
|
if (window.uavApp && window.uavApp.waypoints && window.uavApp.waypoints.length > 0) {
|
return window.uavApp.waypoints;
|
}
|
|
console.warn('未找到航线航点，将无法进行基于航线的路径规划');
|
return [];
|
}
|
|
// ================================
|
// 在三维数组上执行A*路径规划算法
|
// ================================
|
aStarOnGrid3D(startIndex, endIndex) {
|
console.log('开始在三维数组上执行A*路径规划算法...');
|
console.log(`起点: (${startIndex.x}, ${startIndex.y}, ${startIndex.z})`);
|
console.log(`终点: (${endIndex.x}, ${endIndex.y}, ${endIndex.z})`);
|
|
// 清理算法数据结构
|
this.openList = [];
|
this.closedList = [];
|
|
// 创建起始节点
|
const startNode = {
|
x: startIndex.x,
|
y: startIndex.y,
|
z: startIndex.z,
|
g: 0,
|
h: this.calculateHeuristic(startIndex, endIndex),
|
f: 0,
|
parent: null
|
};
|
startNode.f = startNode.g + startNode.h;
|
|
this.openList.push(startNode);
|
|
let iterations = 0;
|
const maxIterations = 50000;
|
let verticalMoves = 0;
|
|
while (this.openList.length > 0 && iterations < maxIterations) {
|
iterations++;
|
|
// 找到f值最小的节点
|
this.openList.sort((a, b) => a.f - b.f);
|
const currentNode = this.openList.shift();
|
|
// 如果到达终点
|
if (currentNode.x === endIndex.x &&
|
currentNode.y === endIndex.y &&
|
currentNode.z === endIndex.z) {
|
console.log(`A*算法完成，迭代次数：${iterations}`);
|
console.log(`垂直移动次数：${verticalMoves}`);
|
return this.reconstructPath(currentNode);
|
}
|
|
// 将当前节点移到关闭列表
|
this.closedList.push(currentNode);
|
|
// 获取邻居节点
|
const neighbors = this.getNeighbors(currentNode);
|
|
for (const neighbor of neighbors) {
|
// 跳过已在关闭列表中的节点
|
if (this.isInClosedList(neighbor)) {
|
continue;
|
}
|
|
// 检查是否可通行
|
if (!this.isTraversable(neighbor.x, neighbor.y, neighbor.z)) {
|
continue;
|
}
|
|
// 计算移动成本
|
const moveCost = this.getMoveCost(currentNode, neighbor);
|
const tentativeG = currentNode.g + moveCost;
|
|
// 统计垂直移动
|
if (neighbor.z !== currentNode.z) {
|
verticalMoves++;
|
}
|
|
// 检查是否已在开放列表中
|
const existingNode = this.findInOpenList(neighbor);
|
if (existingNode) {
|
if (tentativeG < existingNode.g) {
|
existingNode.g = tentativeG;
|
existingNode.h = this.calculateHeuristic(neighbor, endIndex);
|
existingNode.f = existingNode.g + existingNode.h;
|
existingNode.parent = currentNode;
|
}
|
} else {
|
const newNode = {
|
x: neighbor.x,
|
y: neighbor.y,
|
z: neighbor.z,
|
g: tentativeG,
|
h: this.calculateHeuristic(neighbor, endIndex),
|
f: 0,
|
parent: currentNode
|
};
|
newNode.f = newNode.g + newNode.h;
|
this.openList.push(newNode);
|
}
|
}
|
|
// 每1000次迭代输出一次进度
|
if (iterations % 1000 === 0) {
|
const openCount = this.openList.length;
|
const closedCount = this.closedList.length;
|
console.log(`A*算法进度: ${iterations}次迭代, 开放列表: ${openCount}, 关闭列表: ${closedCount}`);
|
}
|
}
|
|
console.warn(`A*算法未找到路径，迭代次数：${iterations}`);
|
return null;
|
}
|
|
// ================================
|
// 重构路径
|
// ================================
|
reconstructPath(endNode) {
|
const path = [];
|
let currentNode = endNode;
|
|
while (currentNode) {
|
path.unshift({
|
x: currentNode.x,
|
y: currentNode.y,
|
z: currentNode.z
|
});
|
currentNode = currentNode.parent;
|
}
|
|
console.log(`路径重构完成，路径长度: ${path.length}个节点`);
|
return path;
|
}
|
|
// ================================
|
// 计算启发式函数（3D曼哈顿距离）
|
// ================================
|
calculateHeuristic(index1, index2) {
|
const dx = Math.abs(index1.x - index2.x);
|
const dy = Math.abs(index1.y - index2.y);
|
const dz = Math.abs(index1.z - index2.z);
|
|
// 使用3D曼哈顿距离，给垂直移动适当权重
|
return dx + dy + (dz * this.config.verticalWeight);
|
}
|
|
// ================================
|
// 获取邻居节点（3D：26连通性）
|
// ================================
|
getNeighbors(currentNode) {
|
const neighbors = [];
|
|
// 3D搜索：包括所有26个方向的邻居
|
for (let dx = -1; dx <= 1; dx++) {
|
for (let dy = -1; dy <= 1; dy++) {
|
for (let dz = -1; dz <= 1; dz++) {
|
if (dx === 0 && dy === 0 && dz === 0) continue;
|
|
// 如果不允许对角线移动，只使用6连通性
|
if (!this.config.enableDiagonal) {
|
const isDirectional = (Math.abs(dx) + Math.abs(dy) + Math.abs(dz)) === 1;
|
if (!isDirectional) continue;
|
}
|
|
const newX = currentNode.x + dx;
|
const newY = currentNode.y + dy;
|
const newZ = currentNode.z + dz;
|
|
// 检查边界
|
if (this.isValidGridIndex({ x: newX, y: newY, z: newZ })) {
|
neighbors.push({ x: newX, y: newY, z: newZ });
|
}
|
}
|
}
|
}
|
|
return neighbors;
|
}
|
|
// ================================
|
// A*算法辅助方法
|
// ================================
|
isInClosedList(node) {
|
return this.closedList.some(n =>
|
n.x === node.x && n.y === node.y && n.z === node.z
|
);
|
}
|
|
findInOpenList(node) {
|
return this.openList.find(n =>
|
n.x === node.x && n.y === node.y && n.z === node.z
|
);
|
}
|
|
// ================================
|
// 检查位置是否可通行
|
// ================================
|
isTraversable(x, y, z) {
|
if (!this.isValidGridIndex({ x, y, z })) {
|
return false;
|
}
|
|
const cell = this.grid3D[x][y][z];
|
|
// 如果该位置不存在网格实体，则不可通行
|
if (!cell.exists) {
|
return false;
|
}
|
|
// 如果有实体但未被占用，可通行
|
return !cell.occupied;
|
}
|
|
// ================================
|
// 计算移动成本
|
// ================================
|
getMoveCost(fromNode, toNode) {
|
const dx = Math.abs(toNode.x - fromNode.x);
|
const dy = Math.abs(toNode.y - fromNode.y);
|
const dz = Math.abs(toNode.z - fromNode.z);
|
|
// 计算基础移动成本
|
let baseCost;
|
if (dx && dy && dz) {
|
baseCost = Math.sqrt(3); // 3D对角线移动
|
} else if ((dx && dy) || (dx && dz) || (dy && dz)) {
|
baseCost = Math.sqrt(2); // 2D对角线移动
|
} else {
|
baseCost = 1; // 直线移动
|
}
|
|
// 如果是垂直移动，应用垂直权重
|
if (dz > 0) {
|
baseCost *= this.config.verticalWeight;
|
}
|
|
return baseCost;
|
}
|
|
// ================================
|
// 通过索引找到对应的3D占用网格单元，复制并修改为黄色 - 使用properties数据
|
// ================================
|
visualizePathByGridCopy(pathIndices, startIndex, endIndex) {
|
console.log('开始通过复制网格单元可视化路径...');
|
console.log('路径索引序列:', pathIndices.map(p => `(${p.x},${p.y},${p.z})`).join(' -> '));
|
console.log(`路径总长度: ${pathIndices.length}个栅格索引`);
|
|
// 清除之前的路径
|
this.clearPath();
|
|
let copiedCount = 0;
|
let missingCount = 0;
|
|
for (let i = 0; i < pathIndices.length; i++) {
|
const gridIndex = pathIndices[i];
|
|
// 从3D数组中获取对应的网格单元
|
const cell = this.grid3D[gridIndex.x][gridIndex.y][gridIndex.z];
|
|
if (cell.exists && cell.entity) {
|
// 获取原始网格实体的信息
|
const sourceEntity = cell.entity;
|
const sourcePosition = sourceEntity.position.getValue(this.viewer.clock.currentTime);
|
const sourceDimensions = sourceEntity.box.dimensions.getValue();
|
|
// 获取原始索引（用于显示和命名）
|
const originalIndex = cell.originalIndex || {
|
x: gridIndex.x + this.indexOffset.x,
|
y: gridIndex.y + this.indexOffset.y,
|
z: gridIndex.z + this.indexOffset.z
|
};
|
|
// 判断是否为起点或终点
|
const isStartPoint = (gridIndex.x === startIndex.x &&
|
gridIndex.y === startIndex.y &&
|
gridIndex.z === startIndex.z);
|
const isEndPoint = (gridIndex.x === endIndex.x &&
|
gridIndex.y === endIndex.y &&
|
gridIndex.z === endIndex.z);
|
|
// 根据点的类型选择颜色和名称
|
let entityColor, outlineColor, entityName, pointType;
|
|
if (isStartPoint) {
|
entityColor = this.config.startPointColor;
|
outlineColor = this.config.startEndOutlineColor;
|
entityName = `StartPoint_${originalIndex.x}_${originalIndex.y}_${originalIndex.z}`;
|
pointType = '起点';
|
} else if (isEndPoint) {
|
entityColor = this.config.endPointColor;
|
outlineColor = this.config.startEndOutlineColor;
|
entityName = `EndPoint_${originalIndex.x}_${originalIndex.y}_${originalIndex.z}`;
|
pointType = '终点';
|
} else {
|
entityColor = this.config.pathColor;
|
outlineColor = this.config.pathOutlineColor;
|
entityName = `PathGrid_${originalIndex.x}_${originalIndex.y}_${originalIndex.z}`;
|
pointType = '路径';
|
}
|
|
// 复制网格单元，修改颜色
|
const pathEntity = this.viewer.entities.add({
|
name: entityName,
|
position: sourcePosition,
|
box: {
|
dimensions: sourceDimensions,
|
material: entityColor,
|
outline: true,
|
outlineColor: outlineColor,
|
outlineWidth: this.config.outlineWidth
|
},
|
properties: {
|
isPathGrid: true,
|
gridIndex: originalIndex, // 使用原始索引
|
arrayIndex: { x: gridIndex.x, y: gridIndex.y, z: gridIndex.z }, // 保存数组索引
|
sourceEntityId: sourceEntity.id,
|
pathStep: copiedCount,
|
pointType: pointType
|
}
|
});
|
|
this.pathEntities.push(pathEntity);
|
copiedCount++;
|
|
console.log(`复制网格单元 数组索引[${gridIndex.x}, ${gridIndex.y}, ${gridIndex.z}] 原始索引[${originalIndex.x}, ${originalIndex.y}, ${originalIndex.z}] -> ${pointType}网格`);
|
|
} else {
|
missingCount++;
|
console.warn(`网格索引 [${gridIndex.x}, ${gridIndex.y}, ${gridIndex.z}] 对应的网格单元不存在`);
|
}
|
}
|
|
// 设置路径状态
|
this.hasActivePath = copiedCount > 0;
|
|
// 如果启用了只显示路径模式，隐藏其他网格
|
if (this.showOnlyPath && this.hasActivePath) {
|
this.updateGridVisibility();
|
}
|
|
console.log(`路径可视化完成:`);
|
console.log(`- 成功复制网格: ${copiedCount}个`);
|
console.log(`- 缺失网格: ${missingCount}个`);
|
|
// 显示起点和终点的原始索引
|
const startOriginalIndex = this.grid3D[startIndex.x][startIndex.y][startIndex.z].originalIndex ||
|
{ x: startIndex.x + this.indexOffset.x, y: startIndex.y + this.indexOffset.y, z: startIndex.z + this.indexOffset.z };
|
const endOriginalIndex = this.grid3D[endIndex.x][endIndex.y][endIndex.z].originalIndex ||
|
{ x: endIndex.x + this.indexOffset.x, y: endIndex.y + this.indexOffset.y, z: endIndex.z + this.indexOffset.z };
|
|
console.log(`- 起点: 数组索引(${startIndex.x}, ${startIndex.y}, ${startIndex.z}) 原始索引(${startOriginalIndex.x}, ${startOriginalIndex.y}, ${startOriginalIndex.z}) -> 蓝色`);
|
console.log(`- 终点: 数组索引(${endIndex.x}, ${endIndex.y}, ${endIndex.z}) 原始索引(${endOriginalIndex.x}, ${endOriginalIndex.y}, ${endOriginalIndex.z}) -> 蓝色`);
|
|
if (missingCount > 0) {
|
console.warn(`⚠️ 有${missingCount}个路径索引对应的网格单元缺失`);
|
}
|
|
// 统计路径的3D特征
|
this.analyzePathCharacteristics(pathIndices);
|
}
|
|
// ================================
|
// 分析路径特征
|
// ================================
|
analyzePathCharacteristics(pathIndices) {
|
if (!pathIndices || pathIndices.length === 0) {
|
return;
|
}
|
|
console.log('=== 路径特征分析 ===');
|
|
// 计算路径的空间范围
|
const xValues = pathIndices.map(p => p.x);
|
const yValues = pathIndices.map(p => p.y);
|
const zValues = pathIndices.map(p => p.z);
|
|
const xRange = [Math.min(...xValues), Math.max(...xValues)];
|
const yRange = [Math.min(...yValues), Math.max(...yValues)];
|
const zRange = [Math.min(...zValues), Math.max(...zValues)];
|
|
console.log(`X轴范围: [${xRange[0]}, ${xRange[1]}], 跨度: ${xRange[1] - xRange[0]}`);
|
console.log(`Y轴范围: [${yRange[0]}, ${yRange[1]}], 跨度: ${yRange[1] - yRange[0]}`);
|
console.log(`Z轴范围: [${zRange[0]}, ${zRange[1]}], 跨度: ${zRange[1] - zRange[0]}`);
|
|
// 统计移动类型
|
let verticalMoves = 0;
|
let horizontalMoves = 0;
|
let diagonalMoves = 0;
|
|
for (let i = 1; i < pathIndices.length; i++) {
|
const prev = pathIndices[i - 1];
|
const curr = pathIndices[i];
|
|
const dx = Math.abs(curr.x - prev.x);
|
const dy = Math.abs(curr.y - prev.y);
|
const dz = Math.abs(curr.z - prev.z);
|
|
if (dz > 0) verticalMoves++;
|
if (dx > 0 || dy > 0) horizontalMoves++;
|
if ((dx && dy) || (dx && dz) || (dy && dz)) diagonalMoves++;
|
}
|
|
const totalMoves = pathIndices.length - 1;
|
console.log(`总移动步数: ${totalMoves}`);
|
console.log(`垂直移动: ${verticalMoves} (${(verticalMoves/totalMoves*100).toFixed(1)}%)`);
|
console.log(`水平移动: ${horizontalMoves} (${(horizontalMoves/totalMoves*100).toFixed(1)}%)`);
|
console.log(`对角移动: ${diagonalMoves} (${(diagonalMoves/totalMoves*100).toFixed(1)}%)`);
|
|
// 检查3D路径特征
|
const hasVerticalVariation = (zRange[1] - zRange[0]) > 0;
|
if (hasVerticalVariation) {
|
console.log('✅ 路径包含3D垂直变化，成功进行了立体路径规划');
|
} else {
|
console.log('ℹ️ 路径没有垂直变化，为平面路径');
|
}
|
|
console.log('=== 分析完成 ===');
|
}
|
|
// ================================
|
// 清除路径
|
// ================================
|
clearPath() {
|
// 从场景中移除路径网格实体
|
for (const pathEntity of this.pathEntities) {
|
if (this.viewer.entities.contains(pathEntity)) {
|
this.viewer.entities.remove(pathEntity);
|
}
|
}
|
|
this.pathEntities = [];
|
this.hasActivePath = false; // 重置路径状态
|
|
// 恢复所有网格的显示
|
// this.updateGridVisibility();
|
|
console.log('路径已清除，所有路径网格（包括起点、终点）已从场景中移除');
|
}
|
|
// ================================
|
// 更新路径信息UI
|
// ================================
|
updatePathInfo(pathIndices) {
|
const pathLengthElement = document.getElementById('pathLength');
|
const pathStatusElement = document.getElementById('pathStatus');
|
|
if (pathLengthElement) {
|
pathLengthElement.textContent = pathIndices.length;
|
}
|
|
if (pathStatusElement) {
|
pathStatusElement.textContent = '规划完成';
|
}
|
}
|
|
// ================================
|
// 设置只显示路径模式
|
// ================================
|
setShowOnlyPath(showOnly) {
|
this.showOnlyPath = showOnly;
|
this.updateGridVisibility();
|
console.log(`只显示路径模式: ${showOnly ? '启用' : '禁用'}`);
|
}
|
|
// ================================
|
// 更新网格显示状态
|
// ================================
|
updateGridVisibility() {
|
if (!this.occupancyGrid.gridEntities.length) {
|
return;
|
}
|
|
// 控制原始占用网格的显示
|
for (const entity of this.occupancyGrid.gridEntities) {
|
if (entity.show !== undefined) {
|
if (this.showOnlyPath && this.hasActivePath) {
|
// 只显示路径模式：隐藏原始网格
|
entity.show = false;
|
} else {
|
// 正常显示模式：显示所有原始网格
|
entity.show = true;
|
}
|
}
|
}
|
|
// 控制路径网格的显示（黄色路径网格始终显示，除非被清除）
|
for (const pathEntity of this.pathEntities) {
|
if (pathEntity.show !== undefined) {
|
pathEntity.show = true; // 路径网格始终显示
|
}
|
}
|
|
const pathGridCount = this.pathEntities.length;
|
const originalGridCount = this.occupancyGrid.gridEntities.length;
|
|
console.log(`网格显示状态已更新: ${this.showOnlyPath ? '只显示路径' : '显示全部'}`);
|
console.log(`当前显示: 原始网格 ${this.showOnlyPath && this.hasActivePath ? 0 : originalGridCount} 个, 路径网格 ${pathGridCount} 个`);
|
}
|
|
// ================================
|
// 验证栅格索引是否有效
|
// ================================
|
isValidGridIndex(index) {
|
return index &&
|
typeof index.x === 'number' &&
|
typeof index.y === 'number' &&
|
typeof index.z === 'number' &&
|
index.x >= 0 && index.x < this.gridDimensions.width &&
|
index.y >= 0 && index.y < this.gridDimensions.height &&
|
index.z >= 0 && index.z < this.gridDimensions.depth;
|
}
|
|
// ================================
|
// 首先显示起点和终点（蓝色）
|
// ================================
|
visualizeStartEndPoints(startIndex, endIndex) {
|
console.log('开始显示起点和终点...');
|
|
// 清除之前的路径（如果有）
|
this.clearPath();
|
|
// 显示起点（蓝色）
|
this.createPointVisualization(startIndex, 'start');
|
|
// 显示终点（蓝色）
|
this.createPointVisualization(endIndex, 'end');
|
|
console.log(`起点和终点已显示为蓝色:`);
|
console.log(`- 起点: (${startIndex.x}, ${startIndex.y}, ${startIndex.z})`);
|
console.log(`- 终点: (${endIndex.x}, ${endIndex.y}, ${endIndex.z})`);
|
}
|
|
// ================================
|
// 创建单个点的可视化（起点或终点） - 使用properties数据
|
// ================================
|
createPointVisualization(gridIndex, pointType) {
|
// 从3D数组中获取对应的网格单元
|
const cell = this.grid3D[gridIndex.x][gridIndex.y][gridIndex.z];
|
|
if (cell.exists && cell.entity) {
|
// 获取原始网格实体的信息
|
const sourceEntity = cell.entity;
|
const sourcePosition = sourceEntity.position.getValue(this.viewer.clock.currentTime);
|
const sourceDimensions = sourceEntity.box.dimensions.getValue();
|
|
// 获取原始索引（用于显示和命名）
|
const originalIndex = cell.originalIndex || {
|
x: gridIndex.x + this.indexOffset.x,
|
y: gridIndex.y + this.indexOffset.y,
|
z: gridIndex.z + this.indexOffset.z
|
};
|
|
// 根据点类型设置属性
|
let entityColor, outlineColor, entityName, pointTypeChinese;
|
|
if (pointType === 'start') {
|
entityColor = this.config.startPointColor;
|
outlineColor = this.config.startEndOutlineColor;
|
entityName = `StartPoint_${originalIndex.x}_${originalIndex.y}_${originalIndex.z}`;
|
pointTypeChinese = '起点';
|
} else if (pointType === 'end') {
|
entityColor = this.config.endPointColor;
|
outlineColor = this.config.startEndOutlineColor;
|
entityName = `EndPoint_${originalIndex.x}_${originalIndex.y}_${originalIndex.z}`;
|
pointTypeChinese = '终点';
|
}
|
|
// 创建点的可视化实体
|
const pointEntity = this.viewer.entities.add({
|
name: entityName,
|
position: sourcePosition,
|
box: {
|
dimensions: sourceDimensions,
|
material: entityColor,
|
outline: true,
|
outlineColor: outlineColor,
|
outlineWidth: this.config.outlineWidth
|
},
|
properties: {
|
isPathGrid: true,
|
gridIndex: originalIndex, // 使用原始索引
|
arrayIndex: { x: gridIndex.x, y: gridIndex.y, z: gridIndex.z }, // 保存数组索引
|
sourceEntityId: sourceEntity.id,
|
pointType: pointTypeChinese
|
}
|
});
|
|
this.pathEntities.push(pointEntity);
|
console.log(`创建${pointTypeChinese}可视化: 数组索引[${gridIndex.x}, ${gridIndex.y}, ${gridIndex.z}] 原始索引[${originalIndex.x}, ${originalIndex.y}, ${originalIndex.z}]`);
|
|
} else {
|
console.warn(`网格索引 [${gridIndex.x}, ${gridIndex.y}, ${gridIndex.z}] 对应的网格单元不存在，无法创建${pointType}可视化`);
|
}
|
}
|
|
// ================================
|
// 设置路径实体透明度
|
// ================================
|
setPathAlpha(alpha) {
|
if (alpha < 0.1 || alpha > 1.0) {
|
console.warn('透明度值应该在0.1到1.0之间');
|
return;
|
}
|
|
console.log(`设置路径透明度为: ${alpha}`);
|
|
// 更新配置中的颜色透明度
|
this.config.pathColor = Cesium.Color.YELLOW.withAlpha(alpha);
|
this.config.startPointColor = Cesium.Color.BLUE.withAlpha(alpha);
|
this.config.endPointColor = Cesium.Color.BLUE.withAlpha(alpha);
|
|
// 更新所有现有的路径实体透明度
|
let updatedCount = 0;
|
|
for (const pathEntity of this.pathEntities) {
|
if (pathEntity.box && pathEntity.box.material) {
|
try {
|
// 获取实体的类型
|
const pointType = pathEntity.properties && pathEntity.properties.pointType ?
|
pathEntity.properties.pointType.getValue() : '路径';
|
|
// 根据实体类型设置对应的颜色
|
let newColor;
|
if (pointType === '起点' || pointType === '终点') {
|
newColor = Cesium.Color.BLUE.withAlpha(alpha);
|
} else {
|
newColor = Cesium.Color.YELLOW.withAlpha(alpha);
|
}
|
|
// 更新实体的材质颜色
|
pathEntity.box.material = newColor;
|
updatedCount++;
|
|
} catch (error) {
|
console.warn('更新路径实体透明度时出错:', error);
|
}
|
}
|
}
|
|
console.log(`成功更新${updatedCount}个路径实体的透明度`);
|
}
|
|
// ================================
|
// 获取当前路径透明度
|
// ================================
|
getPathAlpha() {
|
// 从配置中获取当前透明度
|
if (this.config.pathColor && this.config.pathColor.alpha !== undefined) {
|
return this.config.pathColor.alpha;
|
}
|
return 0.8; // 默认透明度
|
}
|
}
|
|
// ================================
|
// 导出模块（如果使用模块系统）
|
// ================================
|
if (typeof module !== 'undefined' && module.exports) {
|
module.exports = PathPlanning;
|
}
|
|
|
export default PathPlanning
|
