[benchmark] Add benchmark subproject (#8042)

benchmark/src/main/native/cpp/Main.cpp

@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <benchmark/benchmark.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/path/TravelingSalesman.h>
+
+#include <units/angle.h>
+#include <units/length.h>
+#include <wpi/array.h>
+
+static constexpr wpi::array<frc::Pose2d, 6> poses{
+    frc::Pose2d{-1_m, 1_m, -90_deg}, frc::Pose2d{-1_m, 2_m, 90_deg},
+    frc::Pose2d{0_m, 0_m, 0_deg},    frc::Pose2d{0_m, 3_m, -90_deg},
+    frc::Pose2d{1_m, 1_m, 90_deg},   frc::Pose2d{1_m, 2_m, 90_deg},
+};
+static constexpr int iterations = 100;
+
+void BM_Transform(benchmark::State& state) {
+  frc::TravelingSalesman traveler{[](auto pose1, auto pose2) {
+    auto transform = pose2 - pose1;
+    return units::math::hypot(transform.X(), transform.Y()).value();
+  }};
+  for (auto _ : state) {
+    traveler.Solve(poses, iterations);
+  }
+}
+BENCHMARK(BM_Transform);
+
+void BM_Twist(benchmark::State& state) {
+  frc::TravelingSalesman traveler{[](auto pose1, auto pose2) {
+    auto twist = pose1.Log(pose2);
+    return units::math::hypot(twist.dx, twist.dy).value();
+  }};
+  for (auto _ : state) {
+    traveler.Solve(poses, iterations);
+  }
+}
+BENCHMARK(BM_Twist);
+
+BENCHMARK_MAIN();