# Speed Optimization for Incremental Updating of Grid-Based Distance Maps
## 4 The Methodology
### Algorithm Pseudocode
#### 4.2.2 Update the DM  
Table 2 presents pseudocode for updating the DM. The function **update0** orderly pops the next unprocessed cell \(s\) with the lowest \(dist_{s}\) until the _OPEN_ queue is empty (lines 14 and 15). If \(s\) is cleared and is not yet propagated by a raise wavefront, the function **raise0** is called (lines 16 and 17). However, if \(s\) has a valid closest obstacle cell, the function **lower0** is called (lines 18 to 20).  
Newly inserted obstacle cells call function **lower0** to launch a lower wavefront to propagate the reduction of \(dist\) and \(obst\) values from the currently popped cell \(s\) to its affected cells (lines 26 to 29). The lower wavefront continues when the distance value (denoted as \(d\) which is computed in line 24) between \(obst_{s}\) and the newly expanded cell \(n\) holds the trend of distance reduction (being determined in line 25). Simultaneously, newly freed cells call function **raise0** to launch a raise wavefront, resetting the cells whose closest obstacle cell was the freed one (line 33 to 36). The raise wavefront terminates when it reaches those cells whose closest obstacle is valid; thus, a lower wavefront launched by \(n\) is generated (line 37). During the interwoven of these two wavefronts, inconsistent neighbors affected by the processed cell are again put on _OPEN_ (line 29 and 38), thus the propagation continues.  
Instead of propagating a wavefront along with full directions, we introduce a function **findSuccessors(s)** to filter cells by employing the rule of canonical ordering (line 21 and 30). As illustrated in Figure 6b, only the successors of a cell in the direction of the arrows are chosen as the  
\begin{table}
\begin{tabular}{l l l l} \hline \hline
**initialize0** & & & \\
1. & _OPEN_\(\leftarrow\varnothing\) & & & \\
2. & **for each cell \(s\) in the grid** & & **setObstacleCell (Cell \(o\))** & \\  & **map do** & 8. & \(dist_{o}\gets 0\) & \\
3. & \(dist_{s}\leftarrow\infty\) & 9. & \(obst_{o}\gets o\) & \\
4. & \(obst_{s}\leftarrow\varnothing\) & 10. & **insert (OPEN,\(o\),0)** & \\
5. & \(voro_{s}\leftarrow\false\) & & & \\
6. & \(raise_{s}\leftarrow false\) & & & \\
7. & \(dir_{s}\gets 0\) & & & \\ \hline \hline \end{tabular}
\end{table}
Table 1: The pseudocode for initialization.  
\begin{table}
\begin{tabular}{l l l l} \hline \hline
**update0** & & **lower (cell \(s\))** & **raise (cell \(o\))** \\
14. & **while**_OPEN_\(\
eq\varnothing\)** & 21. & \(N\leftarrow\text{findSuccessors}(s)\) & 30. & \(N\leftarrow\text{findSuccessors}(s)\) \\
15 & \(s\leftarrow\text{pop}(OPEN)\) & 22. & **for each**\(n\in N\)**do** & 31. & **for each**\(n\in N\)**do** \\
16. & **if**_raise_**then** & 23. & **if**\(\
eg raise_{n}\)**then** & 32. & **if**\((obst_{n}\
eq\varnothing\bigwedge\
eg raise_{n})\) \\
17. & raise(s) & 24. & \(d\leftarrow\text{distance }(obst_{s}\),n) & **then** \\
18. & **else if**isOcc(\(obst_{s}\))**then** & 25. & **if**\(d<dist_{n}\); **then** & 33. & **if**\(\
eg\) isOcc(\(obst_{n}\))**then** \\
19. & \(voro_{s}\leftarrow false\) & 26. & \(dist_{n}\gets d\) & 34. & \(\text{clearCell}(n)\) \\
20. & lower(s) & 27. & \(obst_{n}\gets obst_{s}\) & 35. & \(raise_{n}\gets true\) \\  & 28. & \(dir_{n}\leftarrow\text{direction(}s\),\(n\)) & 36. & \(dir_{n}\leftarrow\text{direction(}s\),\(n\)) \\  & 29. & insert(\(OPEN\),\(n\),\(d\)) & 37. & **else**\(dir_{n}\leftarrow\text{full-dir}\) \\  & & 38. & insert (\(OPEN\),\(n\),\(dist_{n}\)) \\  & & 39. & \(raise_{s}\leftarrow false\) \\ \hline \hline \end{tabular}
\end{table}
Table 2: The pseudocode for updating the Euclidean distance map.  
candidates of the next round of propagation, while the others are ignored. For a cell \(n\) conducting a wavefront from one of its adjacent cell \(s\), function **direction(\(s\),\(n\))** determines the direction from \(s\) to \(n\) and accordingly returns the value of \(dir_{n}\) (line 28 and 36).