feat: Implement PriorityQueue class for D* Lite pathfinding with min-heap functionality

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,
+    };
+  }
+}

server/pathfinding/Node.js

@@ -0,0 +1,61 @@
+/**
+ * Node - Represents a pathfinding node in the D* Lite algorithm
+ * Each node wraps a Point and stores pathfinding metadata
+ */
+import { Point } from './Point.js';
+
+export class Node {
+  constructor(point) {
+    this.point = point;
+    this.key = point.toKey();
+
+    // D* Lite values
+    this.g = Infinity;   // Cost from start to this node
+    this.rhs = Infinity; // One-step lookahead cost
+    
+    // Whether this node is blocked (wall, unbreakable block, etc.)
+    this.blocked = false;
+    
+    // Whether this node contains a mineable block (costs more to traverse but possible)
+    this.mineable = false;
+    
+    // The block data at this position (if known)
+    this.blockData = null;
+  }
+
+  /**
+   * Calculate the D* Lite key pair for priority queue ordering
+   * @param {Point} start - The current start position
+   * @param {number} km - The key modifier (updated on robot movement)
+   */
+  calculateKey(start, km = 0) {
+    const minVal = Math.min(this.g, this.rhs);
+    return [
+      minVal + start.euclideanDistanceTo(this.point) + km,
+      minVal
+    ];
+  }
+
+  /**
+   * Check if this node is consistent (g === rhs)
+   */
+  isConsistent() {
+    return this.g === this.rhs;
+  }
+
+  /**
+   * Get the traversal cost to move to this node
+   * - Blocked nodes: Infinity
+   * - Mineable blocks: 2 (higher cost to prefer open paths)
+   * - Open space: 1
+   */
+  getTraversalCost() {
+    if (this.blocked) return Infinity;
+    if (this.mineable) return 2;
+    return 1;
+  }
+
+  toString() {
+    return `Node(${this.point}, g=${this.g}, rhs=${this.rhs}, blocked=${this.blocked})`;
+  }
+}

server/pathfinding/Point.js

@@ -0,0 +1,100 @@
+/**
+ * Point - Represents a 3D coordinate in the Minecraft world
+ * Inspired by runi95/turtle-control-panel Point class
+ */
+export class Point {
+  constructor(x, y, z) {
+    this.x = x;
+    this.y = y;
+    this.z = z;
+  }
+
+  /**
+   * Returns the Manhattan distance to another point
+   */
+  distanceTo(other) {
+    return Math.abs(this.x - other.x) + Math.abs(this.y - other.y) + Math.abs(this.z - other.z);
+  }
+
+  /**
+   * Returns the Euclidean distance to another point (used for D* Lite heuristic)
+   */
+  euclideanDistanceTo(other) {
+    const dx = this.x - other.x;
+    const dy = this.y - other.y;
+    const dz = this.z - other.z;
+    return Math.sqrt(dx * dx + dy * dy + dz * dz);
+  }
+
+  /**
+   * Check equality with another point
+   */
+  equals(other) {
+    if (!other) return false;
+    return this.x === other.x && this.y === other.y && this.z === other.z;
+  }
+
+  /**
+   * Get all 6 neighboring points (up, down, north, south, east, west)
+   */
+  getNeighbors() {
+    return [
+      new Point(this.x + 1, this.y, this.z), // East
+      new Point(this.x - 1, this.y, this.z), // West
+      new Point(this.x, this.y + 1, this.z), // Up
+      new Point(this.x, this.y - 1, this.z), // Down
+      new Point(this.x, this.y, this.z + 1), // South
+      new Point(this.x, this.y, this.z - 1), // North
+    ];
+  }
+
+  /**
+   * Get the cardinal direction needed to move from this point to another adjacent point
+   * Returns: 'forward', 'back', 'up', 'down', or facing adjustment needed
+   */
+  directionTo(other) {
+    const dx = other.x - this.x;
+    const dy = other.y - this.y;
+    const dz = other.z - this.z;
+
+    if (dy === 1) return 'up';
+    if (dy === -1) return 'down';
+    if (dx === 1) return { facing: 1 }; // East
+    if (dx === -1) return { facing: 3 }; // West
+    if (dz === 1) return { facing: 2 }; // South
+    if (dz === -1) return { facing: 0 }; // North
+
+    return null;
+  }
+
+  /**
+   * Unique string key for maps
+   */
+  toKey() {
+    return `${this.x},${this.y},${this.z}`;
+  }
+
+  /**
+   * Create Point from key string
+   */
+  static fromKey(key) {
+    const [x, y, z] = key.split(',').map(Number);
+    return new Point(x, y, z);
+  }
+
+  /**
+   * Create Point from object with x,y,z properties
+   */
+  static from(obj) {
+    if (!obj) return null;
+    return new Point(obj.x, obj.y, obj.z);
+  }
+
+  toString() {
+    return `(${this.x}, ${this.y}, ${this.z})`;
+  }
+
+  toJSON() {
+    return { x: this.x, y: this.y, z: this.z };
+  }
+}

server/pathfinding/PriorityQueue.js

@@ -0,0 +1,175 @@
+/**
+ * PriorityQueue - Min-heap priority queue for D* Lite
+ * Uses two-element key pairs [k1, k2] for ordering
+ */
+export class PriorityQueue {
+  constructor() {
+    this.heap = [];
+    this.keyMap = new Map(); // key string -> index in heap
+  }
+
+  get size() {
+    return this.heap.length;
+  }
+
+  isEmpty() {
+    return this.heap.length === 0;
+  }
+
+  /**
+   * Compare two key pairs
+   * Returns negative if a < b, positive if a > b, 0 if equal
+   */
+  static compareKeys(a, b) {
+    if (a[0] !== b[0]) return a[0] - b[0];
+    return a[1] - b[1];
+  }
+
+  /**
+   * Get the minimum key without removing
+   */
+  topKey() {
+    if (this.heap.length === 0) return [Infinity, Infinity];
+    return this.heap[0].priority;
+  }
+
+  /**
+   * Insert or update a node with a priority
+   */
+  insertOrUpdate(node, priority) {
+    const key = node.key;
+
+    if (this.keyMap.has(key)) {
+      // Update existing entry
+      const index = this.keyMap.get(key);
+      const oldPriority = this.heap[index].priority;
+      this.heap[index].priority = priority;
+      this.heap[index].node = node;
+
+      // Determine whether to bubble up or sift down
+      if (PriorityQueue.compareKeys(priority, oldPriority) < 0) {
+        this._bubbleUp(index);
+      } else {
+        this._siftDown(index);
+      }
+    } else {
+      // Insert new entry
+      const entry = { node, priority };
+      this.heap.push(entry);
+      const index = this.heap.length - 1;
+      this.keyMap.set(key, index);
+      this._bubbleUp(index);
+    }
+  }
+
+  /**
+   * Remove and return the minimum node
+   */
+  pop() {
+    if (this.heap.length === 0) return null;
+
+    const min = this.heap[0];
+    this.keyMap.delete(min.node.key);
+
+    const last = this.heap.pop();
+    if (this.heap.length > 0) {
+      this.heap[0] = last;
+      this.keyMap.set(last.node.key, 0);
+      this._siftDown(0);
+    }
+
+    return min.node;
+  }
+
+  /**
+   * Remove a specific node by key
+   */
+  remove(node) {
+    const key = node.key;
+    if (!this.keyMap.has(key)) return;
+
+    const index = this.keyMap.get(key);
+    this.keyMap.delete(key);
+
+    const last = this.heap.pop();
+    if (index < this.heap.length) {
+      this.heap[index] = last;
+      this.keyMap.set(last.node.key, index);
+      this._bubbleUp(index);
+      this._siftDown(index);
+    }
+  }
+
+  /**
+   * Check if a node is in the queue
+   */
+  contains(node) {
+    return this.keyMap.has(node.key);
+  }
+
+  /**
+   * Clear the queue
+   */
+  clear() {
+    this.heap = [];
+    this.keyMap.clear();
+  }
+
+  // --- Internal heap operations ---
+
+  _parent(i) {
+    return Math.floor((i - 1) / 2);
+  }
+
+  _leftChild(i) {
+    return 2 * i + 1;
+  }
+
+  _rightChild(i) {
+    return 2 * i + 2;
+  }
+
+  _swap(i, j) {
+    const temp = this.heap[i];
+    this.heap[i] = this.heap[j];
+    this.heap[j] = temp;
+
+    this.keyMap.set(this.heap[i].node.key, i);
+    this.keyMap.set(this.heap[j].node.key, j);
+  }
+
+  _bubbleUp(i) {
+    while (i > 0) {
+      const parent = this._parent(i);
+      if (PriorityQueue.compareKeys(this.heap[i].priority, this.heap[parent].priority) < 0) {
+        this._swap(i, parent);
+        i = parent;
+      } else {
+        break;
+      }
+    }
+  }
+
+  _siftDown(i) {
+    const n = this.heap.length;
+    while (true) {
+      let smallest = i;
+      const left = this._leftChild(i);
+      const right = this._rightChild(i);
+
+      if (left < n && PriorityQueue.compareKeys(this.heap[left].priority, this.heap[smallest].priority) < 0) {
+        smallest = left;
+      }
+      if (right < n && PriorityQueue.compareKeys(this.heap[right].priority, this.heap[smallest].priority) < 0) {
+        smallest = right;
+      }
+
+      if (smallest !== i) {
+        this._swap(i, smallest);
+        i = smallest;
+      } else {
+        break;
+      }
+    }
+  }
+}

server/pathfinding/index.js

@@ -0,0 +1,4 @@
+export { Point } from './Point.js';
+export { Node } from './Node.js';
+export { PriorityQueue } from './PriorityQueue.js';
+export { DStarLite } from './DStarLite.js';