Formatting
Fixed corner case with identical start and goal.
Creep logic transformed into behavior trees.