Merge OptimalTrajectoryGenerator and SwerveDiscreteOptimal classes

The geometry classes now overload operator== to produce equality
constraints.

build.rs

@@ -23,6 +23,7 @@ fn main() {
         .file("src/trajoptlibrust.cpp")
         .include("src")
         .include(format!("{}/include", cmake_dest.display()))
+        .include(format!("{}/include/eigen3", cmake_dest.display()))
         .std("c++20");
 
     bridge_build.compile("trajoptrust");

examples/Swerve/src/Main.cpp

@@ -1,6 +1,6 @@
 // Copyright (c) TrajoptLib contributors
 
-#include <trajopt/OptimalTrajectoryGenerator.hpp>
+#include <trajopt/SwerveTrajectoryGenerator.hpp>
 
 int main() {
   trajopt::SwerveDrivetrain swerveDrivetrain{
@@ -23,7 +23,9 @@ int main() {
   path.WptZeroVelocity(1);
   path.ControlIntervalCounts({4});
 
+  trajopt::SwerveTrajectoryGenerator generator{path};
+
   // SOLVE
   [[maybe_unused]]
-  auto solution = trajopt::OptimalTrajectoryGenerator::Generate(path, true);
+  auto solution = generator.Generate(true);
 }