Dstar path planning C program