Grid template

The logic is the same, but the API changes because of the template, and some code has been moved from implementation to header files.
@RainCT Thanks for the comment.
It looks that later is used for:
* Initialize a `vector<double>`
* check for `isReachable`, where it's not clear for me when `distance` can be greater than `UNREACHABLE - 0.5`