find_shortest_path.ts

type NodeId = number;

interface NodePath {
  id: NodeId;
  g: number; // Cost from start to this node
  f: number; // Estimated cost from start to goal through this node
  parent: NodeId | null;
}

export function findShortestPath(
  startNode: NodeId,
  goalNode: NodeId,
  getNeighbors: (id: NodeId) => Array<NodeId>,
  getCost: (from: NodeId, to: NodeId) => number | false,
  heuristic: (node: NodeId, goal: NodeId) => number
): NodeId[] {
  const openSet: Map<NodeId, NodePath> = new Map();
  const closedSet: Map<NodeId, NodePath> = new Map();

  openSet.set(startNode, {
    id: startNode,
    g: 0,
    f: heuristic(startNode, goalNode),
    parent: null,
  });

  while (openSet.size > 0) {
    // Get the node with the lowest f score
    let currentNode: NodePath = Array.from(openSet.values()).reduce((a, b) => (a.f < b.f ? a : b));

    if (currentNode.id === goalNode) {
      // Reconstruct path
      const path: NodeId[] = [];
      let node: NodePath | undefined = currentNode;
      while (node) {
        path.push(node.id);
        node = node.parent !== null ? closedSet.get(node.parent) : undefined;
      }
      return path.reverse();
    }

    openSet.delete(currentNode.id);
    closedSet.set(currentNode.id, currentNode);

    const neighbors = getNeighbors(currentNode.id);
    for (const neighborId of neighbors) {
      if (closedSet.has(neighborId)) {
        continue;
      }

      const cost = getCost(currentNode.id, neighborId);
      if (cost === false) {
        continue;
      }

      const tentativeGScore = currentNode.g + cost;
      const neighborNode = openSet.get(neighborId);

      if (!neighborNode || tentativeGScore < neighborNode.g) {
        const h = heuristic(neighborId, goalNode);
        const newNode: NodePath = {
          id: neighborId,
          g: tentativeGScore,
          f: tentativeGScore + h,
          parent: currentNode.id,
        };
        openSet.set(neighborId, newNode);
      }
    }
  }

  // Return empty array if no path is found
  return [];
}
const edges: { [key: number]: NodeId[] } = {
  1: [2, 5],
  2: [1, 3],
  3: [2, 4],
  4: [3],
  5: [1],
  // 他のエッジ
};


const getNeighbors = (id: number): NodeId[] => edges[id] || [];

const getCost = (from: NodeId, to: NodeId): number | false => {
  return 1;
  // // ノード間の距離を返す。道がなければfalseを返す
  // const distanceMap: { [key: string]: number | false } = {
  //   '1-2': 1,
  //   '2-1': 1,
  //   '2-3': 1,
  //   '3-2': 1,
  //   // 他のコスト
  // };
  // return distanceMap[`${from}-${to}`] || false;
};

const startNode = 1;
const goalNode = 4;

try {
  const path = findShortestPath(startNode, goalNode, getNeighbors, getCost, (from, to) => Math.abs(to - from));
  console.log('Path:', path);
} catch (error) {
  console.error(error.message);
}