[wpimath] Add simulated annealing (#5961)

Co-authored-by: Ashray._.g <ashray.gupta@gmail.com>

wpimath/src/main/native/include/frc/optimization/SimulatedAnnealing.h

@@ -0,0 +1,94 @@
+// 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.
+
+#pragma once
+
+#include <cmath>
+#include <functional>
+#include <limits>
+#include <random>
+
+namespace frc {
+
+/**
+ * An implementation of the Simulated Annealing stochastic nonlinear
+ * optimization method.
+ *
+ * @see <a
+ * href="https://en.wikipedia.org/wiki/Simulated_annealing">https://en.wikipedia.org/wiki/Simulated_annealing</a>
+ * @tparam State The type of the state to optimize.
+ */
+template <typename State>
+class SimulatedAnnealing {
+ public:
+  /**
+   * Constructor for Simulated Annealing that can be used for the same functions
+   * but with different initial states.
+   *
+   * @param initialTemperature The initial temperature. Higher temperatures make
+   *     it more likely a worse state will be accepted during iteration, helping
+   *     to avoid local minima. The temperature is decreased over time.
+   * @param neighbor Function that generates a random neighbor of the current
+   *     state.
+   * @param cost Function that returns the scalar cost of a state.
+   */
+  constexpr SimulatedAnnealing(double initialTemperature,
+                               std::function<State(const State&)> neighbor,
+                               std::function<double(const State&)> cost)
+      : m_initialTemperature{initialTemperature},
+        m_neighbor{neighbor},
+        m_cost{cost} {}
+
+  /**
+   * Runs the Simulated Annealing algorithm.
+   *
+   * @param initialGuess The initial state.
+   * @param iterations Number of iterations to run the solver.
+   * @return The optimized state.
+   */
+  State Solve(const State& initialGuess, int iterations) {
+    State minState = initialGuess;
+    double minCost = std::numeric_limits<double>::infinity();
+
+    std::random_device rd;
+    std::mt19937 gen{rd()};
+    std::uniform_real_distribution<> distr{0.0, 1.0};
+
+    State state = initialGuess;
+    double cost = m_cost(state);
+
+    for (int i = 0; i < iterations; ++i) {
+      double temperature = m_initialTemperature / i;
+
+      State proposedState = m_neighbor(state);
+      double proposedCost = m_cost(proposedState);
+      double deltaCost = proposedCost - cost;
+
+      double acceptanceProbability = std::exp(-deltaCost / temperature);
+
+      // If cost went down or random number exceeded acceptance probability,
+      // accept the proposed state
+      if (deltaCost < 0 || acceptanceProbability >= distr(gen)) {
+        state = proposedState;
+        cost = proposedCost;
+      }
+
+      // If proposed cost is less than minimum, the proposed state becomes the
+      // new minimum
+      if (proposedCost < minCost) {
+        minState = proposedState;
+        minCost = proposedCost;
+      }
+    }
+
+    return minState;
+  }
+
+ private:
+  double m_initialTemperature;
+  std::function<State(const State&)> m_neighbor;
+  std::function<double(const State&)> m_cost;
+};
+
+}  // namespace frc