Path smoothness is incorrect

[Nabushika]

For 2 dimensions it works fine, but for more than 2, (for example) there are multiple ways to have a path segment a certain angle from the x-axis.
So if it ignores x and just moves in y and z, it'll get 0 (best) path smoothness cause it's always moving perpendicular (90 degrees) to the x axis.

Solution: rather than taking the angle from the x-axis and keeping track of the differences, just take the angle from the last direction. (i.e. arccos(norm(x2-x1) . norm(x1-x0)))