Added updateVelocity-Function in FindPath