Path linearization and smoothing implemented, things looks pretty nao