Sorry to revive an rather old thread, but since this is a continuation of @notincontrol's work, I felt it needed to be tied with it.
Here is my attempt at converting his code to work with swerve:
https://github.com/faust1706/Smooth-Swerve
I based the calculations off Ether's materials, but that doesn't mean I miss typed or something.
How to use: define set of waypoints, x and y, and then a set of angles you want the robot to be facing at each way point.
A known thing that is broken is the output velocity per motor.