## Pseudocode of the 3D DFM algorithm

The pseudo code of the 3D DFM algorithm is given in table 4.

Procedure CalculateKey(x)

{01} return [0.5*min(v(x), u(x)) + 0.5*He(x, xgoal); min(v(x), u(x))]; Procedure Initialize() {02} Q = 0;

{03} for all x e Q, v(x) = u(x) = œ; {04} v(xstart) = 0

{05} Q.Insert(xstart, [0.5*He(xstart, xgoal); 0]); Procedure FMComputeV(x)

{06} Select configurations A1, B1, C1 using the computation procedure of table 2; {07} Apply case 1 or case 2 using the computation procedure 3.4.2.

Procedure Update(x)

{08} if x # xstart then v(x) = FMComputeV(x); {09} if x e Q then Q.Remove(x);

{10} if |v(x) - u(x)| > s then Q.Insert(x, CalculateKey(x)); Procedure RunDFM()

{11} while Q.TopKey() < CalculateKey(xgoal) OR |v(xgoal) - u(xgoal)| > s

Procedure Main()

{20} forever

{22} Wait for changes in t;

{23} for all configurations {x} with changed cost

Table 4. Pseudo code of the 3D DFM algorithm. Main functions are:

• Q.Insert(x, key(x)): insert configuration x in the priority queue Q with priority key(x) CalculateKey(x);

• Q.Remove(x): remove configuration x from the priority queue Q;

• Q.Pop(): remove xtop from the priority queue Q and return it; Main procedures are:

• Initialize(), lines {02-05}. Estimate v and distance function u are initialized at <x>, except for the start configuration xstart for which v(xstart) = 0. Then, start configuration is inconsistent and it is inserted in the priority queue Q described farther.

• FMComputeV(x), lines {06-07}. The estimate v(x) is computed using the procedure described in table 2 similarly to the static 3D Fast Marching algorithm.

• Update(x), lines {08-10}. First, v(x) is computed using FMComputeV(x). Second, x is removed from Q and, if x is still inconsistent, then it is re-inserted in Q.

• RunDFM(), lines {11-18}. The inconsistent configurations in Q are processed until their priorities become higher than the priority of the goal configuration xgoal AND xgoal becomes consistent (line {11}). 