feat: Implement D* Lite pathfinding algorithm with node management and dynamic replanning
This commit is contained in:
MayaTheShy
361
server/pathfinding/DStarLite.js
Normal file
361
server/pathfinding/DStarLite.js
Normal file
@@ -0,0 +1,361 @@
|
||||
/**
|
||||
* D* Lite Pathfinding Algorithm
|
||||
*
|
||||
* An incremental heuristic search algorithm that efficiently replans
|
||||
* when the environment changes (new blocks discovered, blocks mined, etc.)
|
||||
*
|
||||
* Based on Koenig & Likhachev (2002) and adapted from
|
||||
* runi95/turtle-control-panel implementation.
|
||||
*/
|
||||
import { Point } from './Point.js';
|
||||
import { Node } from './Node.js';
|
||||
import { PriorityQueue } from './PriorityQueue.js';
|
||||
|
||||
// Unbreakable blocks that cannot be mined
|
||||
const UNBREAKABLE_BLOCKS = new Set([
|
||||
'minecraft:bedrock',
|
||||
'minecraft:barrier',
|
||||
'minecraft:command_block',
|
||||
'minecraft:chain_command_block',
|
||||
'minecraft:repeating_command_block',
|
||||
'minecraft:structure_block',
|
||||
'minecraft:jigsaw',
|
||||
'minecraft:end_portal_frame',
|
||||
'minecraft:end_portal',
|
||||
'minecraft:nether_portal',
|
||||
'minecraft:spawner',
|
||||
'minecraft:reinforced_deepslate',
|
||||
]);
|
||||
|
||||
// Blocks that are liquid/hazardous - avoid unless necessary
|
||||
const HAZARDOUS_BLOCKS = new Set([
|
||||
'minecraft:lava',
|
||||
'minecraft:water',
|
||||
'minecraft:flowing_lava',
|
||||
'minecraft:flowing_water',
|
||||
]);
|
||||
|
||||
export class DStarLite {
|
||||
/**
|
||||
* @param {Point} start - Starting position
|
||||
* @param {Point} goal - Goal position
|
||||
* @param {Map} worldBlocks - Map of "x,y,z" -> blockData from server
|
||||
* @param {Object} options - Configuration options
|
||||
*/
|
||||
constructor(start, goal, worldBlocks, options = {}) {
|
||||
this.start = start;
|
||||
this.goal = goal;
|
||||
this.worldBlocks = worldBlocks;
|
||||
this.km = 0;
|
||||
this.nodes = new Map(); // key -> Node
|
||||
this.queue = new PriorityQueue();
|
||||
|
||||
// Options
|
||||
this.maxSteps = options.maxSteps || 50000;
|
||||
this.canMine = options.canMine !== false; // Default: true - can mine through blocks
|
||||
this.boundaryMin = options.boundaryMin || null; // Point - min boundary
|
||||
this.boundaryMax = options.boundaryMax || null; // Point - max boundary
|
||||
this.avoidHazards = options.avoidHazards !== false; // Default: true
|
||||
|
||||
// Initialize
|
||||
this._initialize();
|
||||
}
|
||||
|
||||
/**
|
||||
* Get or create a node for a point
|
||||
*/
|
||||
getNode(point) {
|
||||
const key = point.toKey();
|
||||
if (!this.nodes.has(key)) {
|
||||
const node = new Node(point);
|
||||
|
||||
// Check world blocks for this position
|
||||
const blockData = this.worldBlocks.get(key);
|
||||
if (blockData) {
|
||||
node.blockData = blockData;
|
||||
|
||||
if (UNBREAKABLE_BLOCKS.has(blockData.name)) {
|
||||
node.blocked = true;
|
||||
} else if (this.avoidHazards && HAZARDOUS_BLOCKS.has(blockData.name)) {
|
||||
node.blocked = true;
|
||||
} else if (blockData.name && blockData.name !== 'minecraft:air') {
|
||||
// There's a block here - it's mineable
|
||||
node.mineable = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Check boundary constraints
|
||||
if (this.boundaryMin && this.boundaryMax) {
|
||||
if (point.x < this.boundaryMin.x || point.x > this.boundaryMax.x ||
|
||||
point.y < this.boundaryMin.y || point.y > this.boundaryMax.y ||
|
||||
point.z < this.boundaryMin.z || point.z > this.boundaryMax.z) {
|
||||
node.blocked = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Don't go below bedrock
|
||||
if (point.y < -64) node.blocked = true;
|
||||
if (point.y > 320) node.blocked = true;
|
||||
|
||||
this.nodes.set(key, node);
|
||||
}
|
||||
return this.nodes.get(key);
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize the D* Lite algorithm
|
||||
*/
|
||||
_initialize() {
|
||||
const goalNode = this.getNode(this.goal);
|
||||
goalNode.rhs = 0;
|
||||
|
||||
const key = goalNode.calculateKey(this.start, this.km);
|
||||
this.queue.insertOrUpdate(goalNode, key);
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute the shortest path
|
||||
* Returns true if a path exists, false otherwise
|
||||
*/
|
||||
computeShortestPath() {
|
||||
const startNode = this.getNode(this.start);
|
||||
let steps = 0;
|
||||
|
||||
while (
|
||||
!this.queue.isEmpty() &&
|
||||
(PriorityQueue.compareKeys(this.queue.topKey(), startNode.calculateKey(this.start, this.km)) < 0 ||
|
||||
startNode.rhs !== startNode.g)
|
||||
) {
|
||||
steps++;
|
||||
if (steps > this.maxSteps) {
|
||||
console.warn(`D* Lite: Max steps (${this.maxSteps}) exceeded`);
|
||||
return false;
|
||||
}
|
||||
|
||||
const oldKey = this.queue.topKey();
|
||||
const u = this.queue.pop();
|
||||
if (!u) break;
|
||||
|
||||
const newKey = u.calculateKey(this.start, this.km);
|
||||
|
||||
if (PriorityQueue.compareKeys(oldKey, newKey) < 0) {
|
||||
// Key has changed, reinsert with new key
|
||||
this.queue.insertOrUpdate(u, newKey);
|
||||
} else if (u.g > u.rhs) {
|
||||
// Overconsistent - decrease g
|
||||
u.g = u.rhs;
|
||||
|
||||
// Update predecessors
|
||||
const neighbors = u.point.getNeighbors();
|
||||
for (const neighborPoint of neighbors) {
|
||||
const neighborNode = this.getNode(neighborPoint);
|
||||
this._updateNode(neighborNode);
|
||||
}
|
||||
} else {
|
||||
// Underconsistent - increase g
|
||||
u.g = Infinity;
|
||||
|
||||
// Update u and its predecessors
|
||||
this._updateNode(u);
|
||||
const neighbors = u.point.getNeighbors();
|
||||
for (const neighborPoint of neighbors) {
|
||||
const neighborNode = this.getNode(neighborPoint);
|
||||
this._updateNode(neighborNode);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return startNode.g !== Infinity;
|
||||
}
|
||||
|
||||
/**
|
||||
* Update a node's rhs value and queue status
|
||||
*/
|
||||
_updateNode(node) {
|
||||
if (!node.point.equals(this.goal)) {
|
||||
// rhs = min over all successors of (cost(node, succ) + succ.g)
|
||||
let minRhs = Infinity;
|
||||
const neighbors = node.point.getNeighbors();
|
||||
|
||||
for (const neighborPoint of neighbors) {
|
||||
const neighborNode = this.getNode(neighborPoint);
|
||||
const cost = this._cost(node, neighborNode);
|
||||
const val = cost + neighborNode.g;
|
||||
if (val < minRhs) {
|
||||
minRhs = val;
|
||||
}
|
||||
}
|
||||
|
||||
node.rhs = minRhs;
|
||||
}
|
||||
|
||||
// Remove from queue if present
|
||||
if (this.queue.contains(node)) {
|
||||
this.queue.remove(node);
|
||||
}
|
||||
|
||||
// Re-insert if inconsistent
|
||||
if (node.g !== node.rhs) {
|
||||
const key = node.calculateKey(this.start, this.km);
|
||||
this.queue.insertOrUpdate(node, key);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the cost to traverse from one node to an adjacent node
|
||||
*/
|
||||
_cost(from, to) {
|
||||
return to.getTraversalCost();
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the computed path from start to goal
|
||||
* Returns array of Points, or null if no path exists
|
||||
*/
|
||||
getPath() {
|
||||
if (!this.computeShortestPath()) {
|
||||
return null;
|
||||
}
|
||||
|
||||
const path = [this.start];
|
||||
let current = this.start;
|
||||
let maxPathLength = 10000;
|
||||
|
||||
while (!current.equals(this.goal) && maxPathLength > 0) {
|
||||
maxPathLength--;
|
||||
|
||||
const currentNode = this.getNode(current);
|
||||
if (currentNode.g === Infinity) {
|
||||
return null; // No path
|
||||
}
|
||||
|
||||
// Find the best neighbor to move to
|
||||
let bestNeighbor = null;
|
||||
let bestCost = Infinity;
|
||||
const neighbors = current.getNeighbors();
|
||||
|
||||
for (const neighborPoint of neighbors) {
|
||||
const neighborNode = this.getNode(neighborPoint);
|
||||
const cost = this._cost(currentNode, neighborNode) + neighborNode.g;
|
||||
if (cost < bestCost) {
|
||||
bestCost = cost;
|
||||
bestNeighbor = neighborPoint;
|
||||
}
|
||||
}
|
||||
|
||||
if (!bestNeighbor || bestCost === Infinity) {
|
||||
return null; // No path
|
||||
}
|
||||
|
||||
path.push(bestNeighbor);
|
||||
current = bestNeighbor;
|
||||
}
|
||||
|
||||
return path;
|
||||
}
|
||||
|
||||
/**
|
||||
* Update the algorithm when the turtle moves to a new position
|
||||
* This is the key D* Lite optimization - it adjusts km to avoid full recomputation
|
||||
*/
|
||||
updateStart(newStart) {
|
||||
this.km += this.start.euclideanDistanceTo(newStart);
|
||||
this.start = newStart;
|
||||
}
|
||||
|
||||
/**
|
||||
* Notify the algorithm that a block has changed at a position
|
||||
* This triggers efficient replanning
|
||||
* @param {Point} point - The position that changed
|
||||
* @param {Object|null} blockData - New block data, or null if block was removed
|
||||
*/
|
||||
updateBlock(point, blockData) {
|
||||
const node = this.getNode(point);
|
||||
const oldBlocked = node.blocked;
|
||||
const oldMineable = node.mineable;
|
||||
|
||||
// Update node state
|
||||
node.blockData = blockData;
|
||||
|
||||
if (!blockData || blockData.name === 'minecraft:air') {
|
||||
node.blocked = false;
|
||||
node.mineable = false;
|
||||
} else if (UNBREAKABLE_BLOCKS.has(blockData.name)) {
|
||||
node.blocked = true;
|
||||
node.mineable = false;
|
||||
} else if (this.avoidHazards && HAZARDOUS_BLOCKS.has(blockData.name)) {
|
||||
node.blocked = true;
|
||||
node.mineable = false;
|
||||
} else {
|
||||
node.blocked = false;
|
||||
node.mineable = true;
|
||||
}
|
||||
|
||||
// Only replan if the traversability changed
|
||||
if (node.blocked !== oldBlocked || node.mineable !== oldMineable) {
|
||||
// Update this node and all its neighbors
|
||||
this._updateNode(node);
|
||||
const neighbors = point.getNeighbors();
|
||||
for (const neighborPoint of neighbors) {
|
||||
if (this.nodes.has(neighborPoint.toKey())) {
|
||||
const neighborNode = this.getNode(neighborPoint);
|
||||
this._updateNode(neighborNode);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the next step the turtle should take from current position
|
||||
* Returns the next Point to move to, or null if at goal or no path
|
||||
*/
|
||||
getNextStep() {
|
||||
if (this.start.equals(this.goal)) {
|
||||
return null; // Already at goal
|
||||
}
|
||||
|
||||
if (!this.computeShortestPath()) {
|
||||
return null; // No path
|
||||
}
|
||||
|
||||
const startNode = this.getNode(this.start);
|
||||
if (startNode.g === Infinity) {
|
||||
return null;
|
||||
}
|
||||
|
||||
let bestNeighbor = null;
|
||||
let bestCost = Infinity;
|
||||
const neighbors = this.start.getNeighbors();
|
||||
|
||||
for (const neighborPoint of neighbors) {
|
||||
const neighborNode = this.getNode(neighborPoint);
|
||||
const cost = this._cost(startNode, neighborNode) + neighborNode.g;
|
||||
if (cost < bestCost) {
|
||||
bestCost = cost;
|
||||
bestNeighbor = neighborPoint;
|
||||
}
|
||||
}
|
||||
|
||||
return bestNeighbor;
|
||||
}
|
||||
|
||||
/**
|
||||
* Replan the path (called after block updates)
|
||||
*/
|
||||
replan() {
|
||||
return this.computeShortestPath();
|
||||
}
|
||||
|
||||
/**
|
||||
* Get statistics about the pathfinding
|
||||
*/
|
||||
getStats() {
|
||||
return {
|
||||
nodesExplored: this.nodes.size,
|
||||
queueSize: this.queue.size,
|
||||
startG: this.getNode(this.start).g,
|
||||
goalRhs: this.getNode(this.goal).rhs,
|
||||
km: this.km,
|
||||
};
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user