Fixed more errors
Revert "Fixed more errors"

This reverts commit d42189cfd78da6a267da0e77d9cda5d3dfc5d49c.
Fixed more errors in D*, ComputeShortestPath and UpdateVertex