.. _algorithms:

================================================================================
Algorithms
================================================================================

.. note::
  - This page contains the pseudo code outlining the algorithms implemented.
  - An effort has been made to implement the algorithm in a way that allows easy correspondence with the pseudo code, facilitating learning.

.. important::
  - The pseudo code is written based on the original papers from which the algorithms are derived; slight modifications might be made, while retaining the essence of the algorithm.
  - Minor modifications have been made to the implementation of Dijkstra to highlight it's similarity of A*.
  - Using the grid on which the algorithm runs to maintain cost would make the speed up the  algorithms significantly for both A* and Dijkstra, and remove the necessity for using the closed list, maintaining cost in the open list.

.. todo::
  - Highlight changes between pseudocode and implementation

.. contents::
  :backlinks: none

Dijkstra
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
| **procedure main()**
|  OL = 0 //Initialize the open list. Priority queue sorted by cost
|  CL = 0 //Initialize the closed list
|  OL <--- s\ :sub:`start`\
|  while(!OL.empty())
|    q <--- OL.pop()
|    for all s in Succ(q)
|      if(s=s\ :sub:`goal`\) return path found
|      if(s is obstacle) continue
|      s.parent <--- q
|      s.cost = q.cost + distance between successor and q
|      if(s` = s in OL && s`.cost < s.cost)
|        continue
|      if(s` = s in OL && s`.cost < s.cost)
|        continue
|      OL <--- s
|    CL <--- q
|  return no path

A*
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
| **procedure main()**
|  OL = 0 //Initialize the open list. Priority queue sorted by cost + heuristic cost value
|  CL = 0 //Initialize the closed list
|  OL <--- s\ :sub:`start`\
|  while(!OL.empty())
|    q <--- OL.pop()
|    for all s in Succ(q)
|      if(s=s\ :sub:`goal`\) return path found
|      if(s is obstacle) continue
|      s.parent <--- q
|      s.cost = q.cost + distance between successor and q
|      s.h_cost = heuristic(s, s\ :sub:`goal`\)
|      if(s` = s in OL && s`.cost + s`.h_cost < s.cost + s.h_cost)
|        continue
|      if(s` = s in CL && s`.cost + s`.h_cost < s.cost + s.h_cost)
|        continue
|      OL <--- s
|    CL <--- q
|  return no path

LPA*:
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
| **procedure CalculateKey(s)**
|  return [min(g(s), rhs(s)) + h(s\ :sub:`start`\ , s); min(g(s),rhs(s))];
| **procedure Initialize()**
|  U = 0;
|  for all s in S rhs(s) = g(s) = inf
|  rhs(s\ :sub:`start`\ ) = 0;
|  U.insert(s\ :sub:`start`\ , CalculateKey(s\ :sub:`start`\ ));
| **procedure UpdateVertex()**
|  if(u!=s\ :sub:`start`\ ) rhs(s) = min\ for all :sub:`s' in Succ(u)`\ c(u, s') + g(s')
|  if(u in U) U.remove(u)
|  if(g(u)!=rhs(u)) U.insert(u, CalculateKey(u));
| **procedure ComputeShortestPath()**
|  while(U.TopKey() < CalculateKey(s\ :sub:`goal`\ ) OR rhs(s\ :sub:`goal`\) != g(s\ :sub:`goal`\ ))
|   u = U.Pop();
|   if(g(u) > rhs(u))
|     g(u) = rhs(u);
|     for all s in Succ(u) UpdateVertex(s);
|   else
|     g(u) = inf;
|     for all s in Succ(u) (union) {u} UpdateVertex(s)
| **procedure Main()**
|  Initialize();
|  forever: // This runs for n times by default in current implementation
|   ComputeShortestPath();
|   Scan graph for changed edge costs;
|   if any edge cost changed // this occurs with probability 1/n for every iteration
|   for all directed edges (u,v) with changed edge costs
|    Update the edge cost c(u,v);
|    UpdateVertex(u)

D* Lite:
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
| **procedure CalculateKey(s)**
|  return [min(g(s), rhs(s)) + h(s\ :sub:`start`\ , s) + k\ :sub:`m`\ ; min(g(s),rhs(s))];
| **procedure Initialize()**
|  U = 0;
|  k\ :sub:`m`\ = 0;
|  for all s in S rhs(s) = g(s) = inf
|  rhs(s\ :sub:`goal`\ ) = 0;
|  U.insert(s\ :sub:`goal`\ , CalculateKey(s\ :sub:`goal`\ ));
| **procedure UpdateVertex()**
|  if(u!=s\ :sub:`goal`\ ) rhs(s) = min\ for all :sub:`s' in Succ(u)`\ c(u, s') + g(s')
|  if(u in U) U.remove(u)
|  if(g(u)!=rhs(u)) U.insert(u, CalculateKey(u));
| **procedure ComputeShortestPath()**
|  while(U.TopKey() < CalculateKey(s\ :sub:`start`\ ) OR rhs(s\ :sub:`start`\) != g(s\ :sub:`start`\ ))
|   k\ :sub:`old`\ = U.TopKey();
|   u = U.Pop();
|   if(k\ :sub:`old`\ < CalculateKey(u))
|     U.insert(u, CalculateKey(u))
|   else if(g(u) > rhs(u))
|     g(u) = rhs(u);
|     for all s in Pred(u) UpdateVertex(s);
|   else
|     g(u) = inf;
|     for all s in Pred(u) (union) {u} UpdateVertex(s)
| **procedure Main()**
|  s\ :sub:`last`\ = s\ :sub:`start`\
|  Initialize();
|  ComputeShortestPath();
|  while(s\ :sub:`start`\!=s\ :sub:`goal`\)
|   / if(g(s\ :sub:`start`\)=inf) there is no known path /
|   s\ :sub:`start`\ = arg min\ :sub:`for all s' in Succ(s\ :sub:`start`\)`\(c(s\ :sub:`start`\,s')+g(s')
|   Move to start
|   Scan graph for changed edge costs;
|   if any edge cost changed
|   k\ :sub:`m`\ = k\ :sub:`m`\ + h(s\ :sub:`last`\,s\ :sub:`start`\);
|   s\ :sub:`last`\ = s\ :sub:`start`\
|   for all directed edges (u,v) with changed edge costs
|    Update the edge cost c(u,v);
|    UpdateVertex(u)
|  ComputeShortestPath()

RRT
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
| **procedure main()**
|  T = 0; // Initialize list/tree
|  for k = 1 to K
|   q\ :sub:`rand`\ ← GenerateRandomNode()
|   q\ :sub:`parent`\ ← FindNearestPoint(q\ :sub:`rand`\, T, threshold) // Returns closest node based on distance
|   if(q\ :sub:`parent`\ exists)
|     q\ :sub:`parent`\ <--- q\ :sub:`parent`\
|     if(q\ :sub:`rand`\ = goal) return path found
|  return no path

RRTStar
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
| **procedure rewire()**
|  for all node in close_node_list != q\ :sub:`parent`\
|   if(q\ :sub:`rand`\.cost + dist(node ,q\ :sub:`rand`\) < node.cost)
|     node.parent <--- q\ :sub:`rand`\
|     node.cost <--- q\ :sub:`rand`\.cost + distance(node, q\ :sub:`rand`\)
| **procedure main()**
|  T = 0; // Initialize list/tree
|  for k = 1 to K
|   q\ :sub:`rand`\ ← GenerateRandomNode()
|   q\ :sub:`parent`\, close_node_list ← FindNearestPoint(q\ :sub:`rand`\, T, threshold) // Returns node that results in lowest total cost and list of all close nodes                                                  // Updates q\ :sub:`rand`\.cost to q\ :sub:`parent`\.cost + distance(q\ :sub:`rand`\, q\ :sub:`parent`\)
|   if(q\ :sub:`parent`\ exists)
|     q\ :sub:`parent`\ <--- q\ :sub:`parent`\
|     if(q\ :sub:`rand`\ = goal) return path found
|     rewire()
|  return no path

Ant Colony Optimization
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
| **procedure main()**
|  Initialize ant colony
|  for each iterations
|    initialize ants
|    for each ant
|      while ant position != goal and number of steps < max steps
|        calculate probabilities of possible next edges to traverse
|        choose next position based on probability
|        if not next positions possible, break
|        if reached revisited points, remove loop and reduce number of steps taken
|        update pheromone trail
|      update ant in ant colony
|    update pheromone trail by evaporation equation
|    update pheromone trails based on ants that reached the goal
|  if path found return last best path
|  else return no path

Genetic Algorithm
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
| **procedure main()**
|  Initialize paths
|  for each generation
|    choose paths based on criteria
|    mutate paths (using CrossoverMutation()) to find new paths if number of paths less than set number
|    for each path
|      calculate cost and validity of path
|      if path valid add to paths list
|  if path found return last best path
|  else return no path
| **procedure CrossoverMutation()**
|  randomly choose 2 chromosomes (paths) in path list
|  initialize new chromosome
|  for each gene in chromosome
|    choose either the gene from the first chromosome, form the second chromosome or a random gene based on probabilities set
|    add this gene to new chromosome
|  return new chromosome