A star when using any diagonal heuristic causes weird trajectories in cavities. Fix is provided