Added path smoothing function and minor changes to line of sight function