Finding shortest path in a time/distance map

Question

I get a distance map output after using a Fast Marching Method. The PDE involved is the Eikonal equation which take the form : $$\begin{cases} c(x).|\nabla u| =1\\ u(x) =\phi(x) \end{cases} $$ where c is a speed function and u the unknown distance/time we compute. Below an example of a 2D distance map (in blue the inital points and in red the further points).

Given 2 points in the map I would like to show a simple/shortest path between them. I have tried using a naive algorithm which is not very good since I can have complicated problems with obstacles, multiple sources, different speeds.

find_path(point A, point B)
{
  point current_point = B;   // B is ending point
  while(current_point is not A)  
  {
    new_current_point is the the neighbor of current_point with the minimum value   // I use a 5 point stencil
    current_point = new_current_point
    tag every current_point found to show the path
  }
}

My data structure is an uni dimensional row major array (I am coding in C) representing distance values in a 2D regular grid. I think gradient descent might be a good solution but I have difficulties finding a simple algorithm. Thank you.

Edit I join an article Application of the fast marching method for outdoor motion planning in robotics (starting page 12) which use a gradient descent in order to compute that. I have already seen that other papers but I cannot see how it works.

Update I am trying to add a backtracking algorithm to my naïve one but I have troubles putting it in applications. I feel like I am doing a brute force and this is not what I want to do. If someone has an idea on how to change my naive algorithm.

Answer 1

If you already have a distance map $T(x)$ that describes the distance (with respect to a given metric $d$) from a given point $x_0$ to any other point $x$ in your domain (i.e., $d(x_0,x) = T(x)$), then the shortest path from $x_0$ to any point $y$ -- called the geodesic between $x_0$ and $y$ -- can be computed by backtracking: Set $X(0) = y$ and then solve (e.g., by explicit Euler) the gradient flow $$ \dot X(s) = -\nabla T(X(s)), \qquad s>0,$$ where $\nabla T$ is the gradient of $T$, until for some $S>0$ you have $X(S) = x_0$.

Your algorithm can be seen as a space-discrete variant of this, but it will only ever compute the shortest path from B to the "bluest" point in your distance map -- if that is not A, it will not terminate in general.

(Note that $T$ depends on you initial point $x_0$, and has to be recomputed for each initial point. There are more efficient methods for computing all possible pairs of geodesics at once; this is sometimes referred to all-to-all geodesics, see, e.g., http://www.ias-iss.org/ojs/IAS/article/viewFile/891/789.)

Answer 2

This can be solved using some variant of Dijkstra's algorithm, such as the A* algorithm.