Updating explanation a bit and resizing waypoints so that they are column vectors.
This could be somewhat handy.  Obviously, it still needs some work.