From afc4c1f97a1b1a9a9d1b5ffc13b7abe274545f5d Mon Sep 17 00:00:00 2001 From: MayaTheShy Date: Fri, 20 Feb 2026 01:40:48 -0500 Subject: [PATCH] feat: Implement D* Lite pathfinding algorithm with node management and dynamic replanning --- server/pathfinding/DStarLite.js | 361 ++++++++++++++++++++++++++++++++ 1 file changed, 361 insertions(+) create mode 100644 server/pathfinding/DStarLite.js diff --git a/server/pathfinding/DStarLite.js b/server/pathfinding/DStarLite.js new file mode 100644 index 0000000..afa3e94 --- /dev/null +++ b/server/pathfinding/DStarLite.js @@ -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, + }; + } +}