Added path following and debug display

debug display contains velocity direction, force direction and past path