For several decades, the optimal path planning problem in robotics was solved using one common approach: discretize the domain using a reachability graph and search for the shortest path on this graph using a fast graph search algorithm. The advantage of this approach lies in a vast number of graph search algorithms available to solve the discrete problem efficiently. These include Dijkstra's algorithm, A* algorithm, D* algorithm, and many others. Commonly, graph search algorithms utilize various heuristics to accelerate the shortest path computations. This approach leads to extremely efficient algorithms that can be utilized for a real-time motion planning of mobile robots. In this case, however, the resulting trajectory may not converge to the optimum as the step-size tends to zero. For example, the approximate path computed on a regular 2D grid, in which each point is connected to four of its neighbors, is a "staircase" solution. This solution does not converge to an optimal path that is not aligned with the grid. Thus, fast and numerically accurate algorithms are in demand for robot optimal path planning. In our work, we employed a heuristic similar to A* algorithm into the Fast Marching Method (FMM). This approach reduces the computational demand of the traditional FMM by focusing the computations in the vicinity of the optimal trajectory, and it guarantees the convergence to an exact solution. Moreover, our algorithm computes a feedback plan, rather than a single trajectory, provided by many graph search strategies. The feedback optimal control eliminates the need to use special stabilizing controllers to follow the optimal path. In this work, we also explore the possibility to implement heuristic-driven wavefront propagation algorithms for systems with complex dynamics and cost functions.