Ich habe den Algorithmus soweit verstanden, denke ich jedenfalls.
Allerdings sehe ich zwei Probleme:

1. Es gibt nicht EINEN Knoten mit minimaler Distanz zu einem vorigen Knoten. Die Distanz zwischen allen benachbarten Knoten ist identisch (quadratisch angeordnet)
2. Die Umsetzung in lite-c (habe keine Erfahrung mit Structs usw.)