Replace iterative solver in SwerveSetpointGenerator with direct solutions

[rmheuer]

This replaces the iterative root finder in SwerveSetpoint generator, used for findDriveMaxS and findSteeringMaxS, with direct solutions for each of them.
This is currently a draft because:

• I derived these about 6 months ago so I don't remember the exact details, I will try to re-derive them and add explanations later.
• It is currently untested as I am writing this on a Chromebook.

[rmheuer]

I have now added the derivations alongside the implementations, and verified that PathPlannerLib still builds. I have not yet tested this implementation with a robot project, although the solutions themselves have been tested and seem correct in my original implementation here.

[rmheuer]

I've noticed an oversight in my findSteeringMaxS implementation due to the domain of tan(x). That one may need some rethinking.

[mjansen4857]

Just out of curiosity, do you have any data on how this affects setpoints (if at all) and the performance?

Also, this will need to be added to the python version (maybe the c++ version as well depending on which gets merged first). I can help do the translation if you need it.

[rmheuer]

I wrote a small test that runs each implementation 1 million times with random inputs (source here), the results on my laptop are:

Steering:
Avg. time for iterative method: 813.218286 ns
Avg. time for direct method: 84.401104 ns
Drive:
Avg. time for iterative method: 514.946105 ns
Avg. time for direct method: 66.917233 ns

I don't currently have access to a RoboRIO to test on-robot performance, but I assume the results would be similar.