Theta Star: Grid Version

anyAngle.js

// Theta* Algorithm Implementation in JavaScript

class Node {
    constructor(x, y, g = Infinity, h = Infinity, parent = null) {
      this.x = x;
      this.y = y;
      this.g = g; // Cost from start node
      this.h = h; // Heuristic cost to goal node
      this.parent = parent;
    }
  
    get f() {
      return this.g + this.h;
    }
  }
  
  function heuristic(node, goal) {
    return Math.sqrt((node.x - goal.x) ** 2 + (node.y - goal.y) ** 2);
  }
  
  function isInBounds(node, width, height) {
    return node.x >= 0 && node.x < width && node.y >= 0 && node.y < height;
  }
  
  function lineOfSight(grid, node1, node2) {
    let x0 = node1.x;
    let y0 = node1.y;
    let x1 = node2.x;
    let y1 = node2.y;
  
    let dx = Math.abs(x1 - x0);
    let dy = Math.abs(y1 - y0);
    let sx = x0 < x1 ? 1 : -1;
    let sy = y0 < y1 ? 1 : -1;
    let err = dx - dy;
  
    while (x0 !== x1 || y0 !== y1) {
      if (!isInBounds({ x: x0, y: y0 }, grid[0].length, grid.length) || grid[y0][x0] === 1) {
        return false;
      }
      let e2 = 2 * err;
      if (e2 > -dy) {
        err -= dy;
        x0 += sx;
      }
      if (e2 < dx) {
        err += dx;
        y0 += sy;
      }
    }
    return true;
  }
  
  function thetaStar(grid, start, goal) {
    const openSet = [];
    const closedSet = new Set();
    const width = grid[0].length;
    const height = grid.length;
  
    const startNode = new Node(start.x, start.y, 0, heuristic(start, goal));
    openSet.push(startNode);
  
    while (openSet.length > 0) {
      // Sort openSet by f value
      openSet.sort((a, b) => a.f - b.f);
      const current = openSet.shift();
  
      // Goal reached
      if (current.x === goal.x && current.y === goal.y) {
        const path = [];
        let node = current;
        while (node) {
          path.push({ x: node.x, y: node.y });
          node = node.parent;
        }
        return path.reverse();
      }
  
      closedSet.add(`${current.x},${current.y}`);
  
      // Generate neighbors
      const neighbors = [
        { x: current.x + 1, y: current.y },
        { x: current.x - 1, y: current.y },
        { x: current.x, y: current.y + 1 },
        { x: current.x, y: current.y - 1 },
        { x: current.x + 1, y: current.y + 1 },
        { x: current.x - 1, y: current.y - 1 },
        { x: current.x + 1, y: current.y - 1 },
        { x: current.x - 1, y: current.y + 1 },
      ];
  
      for (const neighbor of neighbors) {
        if (!isInBounds(neighbor, width, height) || grid[neighbor.y][neighbor.x] === 1) {
          continue;
        }
  
        const neighborKey = `${neighbor.x},${neighbor.y}`;
        if (closedSet.has(neighborKey)) {
          continue;
        }
  
        let neighborNode = openSet.find((node) => node.x === neighbor.x && node.y === neighbor.y);
        let tentativeG;
  
        if (current.parent && lineOfSight(grid, current.parent, neighbor)) {
          tentativeG = current.parent.g + heuristic(current.parent, neighbor);
          if (!neighborNode) {
            neighborNode = new Node(neighbor.x, neighbor.y, tentativeG, heuristic(neighbor, goal), current.parent);
            openSet.push(neighborNode);
          } else if (tentativeG < neighborNode.g) {
            neighborNode.g = tentativeG;
            neighborNode.parent = current.parent;
          }
        } else {
          tentativeG = current.g + heuristic(current, neighbor);
          if (!neighborNode) {
            neighborNode = new Node(neighbor.x, neighbor.y, tentativeG, heuristic(neighbor, goal), current);
            openSet.push(neighborNode);
          } else if (tentativeG < neighborNode.g) {
            neighborNode.g = tentativeG;
            neighborNode.parent = current;
          }
        }
      }
    }
    return null; // No path found
  }
  
  // Example Usage:
  const grid = [
    [0, 0, 0, 0, 0],
    [1, 1, 0, 1, 0],
    [0, 0, 0, 0, 0],
    [0, 1, 1, 1, 0],
    [0, 0, 0, 0, 0],
  ];
  
  const start = { x: 0, y: 0 };
  const goal = { x: 4, y: 4 };
  
  const path = thetaStar(grid, start, goal);
  console.log(path);

[ { x: 0, y: 0 }, { x: 2, y: 1 }, { x: 4, y: 3 }, { x: 4, y: 4 } ]