Este algoritmo es mortal, ¿algún experto que me eche una ayudita con el D*...?

Sigue sin funcionar: Se mete en bucles y se queda colgado en ocasiones.