Pathfinding & Navigation

        
          
import heapq # Min-heap for priority queue

def dijkstra(graph, start_node):
  # Initialize distances: {node: distance}
  distances = {node: float('infinity') for node in graph}
  distances[start_node] = 0

  # Priority Queue: stores (distance, node) tuples
  priority_queue = [(0, start_node)] # (distance, node)

  # Path reconstruction: {node: previous_node_in_shortest_path}
  previous_nodes = {node: None for node in graph}

  while priority_queue:
      current_distance, current_node = heapq.heappop(priority_queue)

      # If we've found a shorter path to current_node already, skip
      if current_distance > distances[current_node]:
          continue

      for neighbor, weight in graph[current_node].items():
          distance = current_distance + weight

          # If a shorter path to neighbor is found
          if distance < distances[neighbor]:
              distances[neighbor] = distance
              previous_nodes[neighbor] = current_node # Record path
              heapq.heappush(priority_queue, (distance, neighbor))
  
  return distances, previous_nodes

# Helper to reconstruct path from start to target
def reconstruct_path(previous_nodes, start_node, target_node):
  path = []
  current = target_node
  while current is not None:
      path.append(current)
      if current == start_node:
          break
      current = previous_nodes[current]
  path.reverse() # Path is built backwards, so reverse it
  if path[0] == start_node:
      return path
  return [] # No path found

# Main for demonstration
# Example graph: Adjacency list representation {node: {neighbor: weight}}
graph = {
  'A': {'B': 1, 'C': 4},
  'B': {'A': 1, 'C': 2, 'D': 5},
  'C': {'A': 4, 'B': 2, 'D': 1},
  'D': {'B': 5, 'C': 1, 'E': 3},
  'E': {'D': 3}
}

start = 'A'
end = 'E'
all_distances, prev_nodes = dijkstra(graph, start)
print(f"Distances from {start}: {all_distances}")
# Expected: {'A': 0, 'B': 1, 'C': 3, 'D': 4, 'E': 7}

path_to_E = reconstruct_path(prev_nodes, start, end)
print(f"Shortest path from {start} to {end}: {path_to_E}")
# Expected: ['A', 'B', 'C', 'D', 'E']

        
      

        
          
// Simple Priority Queue implementation (for demonstration)
// In real-world, use a min-heap based PriorityQueue.
class PriorityQueue {
  constructor() {
      this.values = [];
  }
  enqueue(val, priority) {
      this.values.push({val, priority});
      this.sort(); // O(N log N) for naive sort
  }
  dequeue() {
      return this.values.shift(); // O(N)
  }
  sort() {
      this.values.sort((a, b) => a.priority - b.priority);
  }
  isEmpty() {
      return this.values.length === 0;
  }
}

function dijkstra(graph, startNode) {
  const distances = {};
  const previousNodes = {};
  const pq = new PriorityQueue();

  // Initialize distances and add all nodes to PQ
  for (const node in graph) {
      distances[node] = Infinity;
      previousNodes[node] = null;
  }
  distances[startNode] = 0;
  pq.enqueue(startNode, 0);

  while (!pq.isEmpty()) {
      let { val: currentNode, priority: currentDistance } = pq.dequeue();

      // If we've found a shorter path to currentNode already, skip
      if (currentDistance > distances[currentNode]) {
          continue;
      }

      for (const neighbor in graph[currentNode]) {
          const weight = graph[currentNode][neighbor];
          const distance = currentDistance + weight;

          if (distance < distances[neighbor]) {
              distances[neighbor] = distance;
              previousNodes[neighbor] = currentNode;
              pq.enqueue(neighbor, distance);
          }
      }
  }
  return { distances, previousNodes };
}

// Helper to reconstruct path from start to target
function reconstructPath(previousNodes, startNode, targetNode) {
  const path = [];
  let current = targetNode;
  while (current !== null) {
      path.push(current);
      if (current === startNode) break;
      current = previousNodes[current];
  }
  if (path.length > 0 && path[path.length - 1] === startNode) {
      return path.reverse();
  }
  return []; // No path found
}


// Main for demonstration
// Example graph: Adjacency list representation {node: {neighbor: weight}}
const graph = {
  'A': {'B': 1, 'C': 4},
  'B': {'A': 1, 'C': 2, 'D': 5},
  'C': {'A': 4, 'B': 2, 'D': 1},
  'D': {'B': 5, 'C': 1, 'E': 3},
  'E': {'D': 3}
};

const start = 'A';
const end = 'E';
const { distances: allDistances, previousNodes: prevNodes } = dijkstra(graph, start);
// Expected: { A: 0, B: 1, C: 3, D: 4, E: 7 }

const pathToE = reconstructPath(prevNodes, start, end);
// Expected: ['A', 'B', 'C', 'D', 'E']

        
      

        
          
import heapq

def a_star(graph, start, goal, heuristic_func):
  # g_score: cost from start to current node
  g_score = {node: float('infinity') for node in graph}
  g_score[start] = 0

  # f_score: estimated total cost from start to goal through current node (g + h)
  f_score = {node: float('infinity') for node in graph}
  f_score[start] = heuristic_func(start, goal)

  # Priority queue: stores (f_score, node)
  open_set = [(f_score[start], start)] # (f_score, node)

  # Path reconstruction
  previous_nodes = {node: None for node in graph}

  while open_set:
      current_f, current_node = heapq.heappop(open_set)

      # Optimization: If we've already processed this node with a better path, skip
      if current_f > f_score[current_node]:
          continue

      if current_node == goal:
          return reconstruct_path(previous_nodes, start, goal), g_score[goal]

      for neighbor, weight in graph[current_node].items():
          tentative_g_score = g_score[current_node] + weight

          if tentative_g_score < g_score[neighbor]:
              previous_nodes[neighbor] = current_node
              g_score[neighbor] = tentative_g_score
              f_score[neighbor] = tentative_g_score + heuristic_func(neighbor, goal)
              heapq.heappush(open_set, (f_score[neighbor], neighbor))

  return None, float('infinity') # No path found

# Example heuristic: Manhattan distance (for grid-like graph)
# (Assumes node names are (x,y) tuples or can be parsed)
def manhattan_distance(node1, node2):
  # This is a dummy for a non-grid graph, replace with actual distance func
  # For simplicity, let's say it's just a placeholder constant if not a grid.
  if isinstance(node1, str) and isinstance(node2, str):
    return 1 # Or some other simple estimate
  # For a real grid:
  # x1, y1 = node1
  # x2, y2 = node2
  # return abs(x1 - x2) + abs(y1 - y2)
  return abs(ord(node1[0]) - ord(node2[0])) + abs(int(node1[1:]) - int(node2[1:]))


# Reconstruct path helper (same as Dijkstra's)
def reconstruct_path(previous_nodes, start_node, target_node):
  path = []
  current = target_node
  while current is not None:
      path.append(current)
      if current == start_node:
          break
      current = previous_nodes[current]
  path.reverse()
  if path and path[0] == start_node:
      return path
  return []

# Main for demonstration (using same graph as Dijkstra's for comparison)
graph_astar = {
  'A': {'B': 1, 'C': 4},
  'B': {'A': 1, 'C': 2, 'D': 5},
  'C': {'A': 4, 'B': 2, 'D': 1},
  'D': {'B': 5, 'C': 1, 'E': 3},
  'E': {'D': 3}
}

# Heuristic function for this simple graph (conceptual)
# Need to provide some heuristic values that guide A*
# For a conceptual example without actual coordinates, this can be tricky.
# Let's use simplified 'straight-line distance' values if available.
# A realistic h() needs actual coordinates or problem-specific estimates.
# For demo, let's just make one that somewhat prioritizes directness.
# In this small graph, a simple heuristic might not show huge gains over Dijkstra.
def conceptual_heuristic(node, goal):
  if node == goal: return 0
  if node == 'A':
      if goal == 'B': return 1
      if goal == 'C': return 2
      if goal == 'D': return 4
      if goal == 'E': return 6
  if node == 'B':
      if goal == 'C': return 1
      if goal == 'D': return 3
      if goal == 'E': return 5
  if node == 'C':
      if goal == 'D': return 1
      if goal == 'E': return 3
  if node == 'D':
      if goal == 'E': return 2 # true distance is 3, slightly underestimate
  return 100 # Large value for un-estimated nodes

start_astar = 'A'
goal_astar = 'E'
path_astar, total_cost_astar = a_star(graph_astar, start_astar, goal_astar, conceptual_heuristic)

print(f"A* Path from {start_astar} to {goal_astar}: {path_astar}")
print(f"A* Total cost: {total_cost_astar}")
# Expected: Path: ['A', 'B', 'C', 'D', 'E'], Cost: 7

        
      

        
          
// For simplicity, we'll use a basic Array.sort based Priority Queue.
// For real applications, use a min-heap based PriorityQueue for O(log N) operations.
class PriorityQueueAStar {
  constructor() {
      this.values = []; // Stores { val: node, priority: f_score }
  }
  enqueue(val, priority) {
      this.values.push({ val, priority });
      this.sort(); // O(N log N)
  }
  dequeue() {
      return this.values.shift(); // O(N)
  }
  sort() {
      this.values.sort((a, b) => a.priority - b.priority);
  }
  isEmpty() {
      return this.values.length === 0;
  }
}

// Conceptual heuristic function
function conceptualHeuristic(node, goal) {
  if (node === goal) return 0;
  // This is a simplified example. In a real scenario, this would use
  // actual geometric distance, or problem-specific estimates.
  // For this small graph, let's create some 'straight-line distance' estimates.
  const heuristicValues = {
      'A': { 'B': 1, 'C': 2, 'D': 4, 'E': 6 },
      'B': { 'C': 1, 'D': 3, 'E': 5 },
      'C': { 'D': 1, 'E': 3 },
      'D': { 'E': 2 } // Slightly underestimate true cost to 'E' (which is 3)
  };
  return heuristicValues[node]?.[goal] || Infinity;
}

function aStar(graph, start, goal, heuristicFunc) {
  const gScore = {}; // Cost from start to current node
  const fScore = {}; // Estimated total cost from start to goal through current node (g + h)
  const previousNodes = {};
  const openSet = new PriorityQueueAStar();

  for (const node in graph) {
      gScore[node] = Infinity;
      fScore[node] = Infinity;
      previousNodes[node] = null;
  }

  gScore[start] = 0;
  fScore[start] = heuristicFunc(start, goal);
  openSet.enqueue(start, fScore[start]);

  while (!openSet.isEmpty()) {
      const { val: currentNode, priority: currentFScore } = openSet.dequeue();

      if (currentNode === goal) {
          return {
              path: reconstructPath(previousNodes, start, goal),
              cost: gScore[goal]
          };
      }

      // Optimization: If we've already processed this node with a better path, skip
      if (currentFScore > fScore[currentNode]) {
           continue;
      }


      for (const neighbor in graph[currentNode]) {
          const weight = graph[currentNode][neighbor];
          const tentativeGScore = gScore[currentNode] + weight;

          if (tentativeGScore < gScore[neighbor]) {
              previousNodes[neighbor] = currentNode;
              gScore[neighbor] = tentativeGScore;
              fScore[neighbor] = tentativeGScore + heuristicFunc(neighbor, goal);
              openSet.enqueue(neighbor, fScore[neighbor]);
          }
      }
  }
  return { path: [], cost: Infinity }; // No path found
}

// Reconstruct path helper (same as Dijkstra's)
function reconstructPath(previousNodes, startNode, targetNode) {
  const path = [];
  let current = targetNode;
  while (current !== null) {
      path.push(current);
      if (current === startNode) break;
      current = previousNodes[current];
  }
  if (path.length > 0 && path[path.length - 1] === startNode) {
      return path.reverse();
  }
  return [];
}

// Main for demonstration
const graphAstar = {
  'A': {'B': 1, 'C': 4},
  'B': {'A': 1, 'C': 2, 'D': 5},
  'C': {'A': 4, 'B': 2, 'D': 1},
  'D': {'B': 5, 'C': 1, 'E': 3},
  'E': {'D': 3}
};

const startAstar = 'A';
const goalAstar = 'E';
const { path: pathAstar, cost: totalCostAstar } = aStar(graphAstar, startAstar, goalAstar, conceptualHeuristic);

// Expected: Path: ['A', 'B', 'C', 'D', 'E'], Cost: 7