Code for swerve kinematics

I need help with swerve drive kinematics. I’ve just read docs in this link
Paper: 4 wheel independent drive & independent steering ("swerve") - CD-Media: Papers - Chief Delphi. Any ready code for those?

If you don’t need to write it all from scratch, WPILib have swerve kinematics in their library ready for use: