remoteturtle/server/pathfinding/DStarLite.js

/**
 * 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,
    };
  }
}