SCRIPT Move cc files

wpimath/src/main/native/cpp/util/MathShared.cpp

@@ -0,0 +1,44 @@
+// 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 "wpimath/MathShared.h"
+
+#include <memory>
+#include <utility>
+
+#include <wpi/mutex.h>
+#include <wpi/timestamp.h>
+
+#include "units/time.h"
+
+using namespace wpi::math;
+
+namespace {
+class DefaultMathShared : public MathShared {
+ public:
+  void ReportErrorV(fmt::string_view format, fmt::format_args args) override {}
+  void ReportWarningV(fmt::string_view format, fmt::format_args args) override {
+  }
+  void ReportUsage(std::string_view resource, std::string_view data) override {}
+  units::second_t GetTimestamp() override {
+    return units::second_t{wpi::Now() * 1.0e-6};
+  }
+};
+}  // namespace
+
+static std::unique_ptr<MathShared> mathShared;
+static wpi::mutex setLock;
+
+MathShared& MathSharedStore::GetMathShared() {
+  std::scoped_lock lock(setLock);
+  if (!mathShared) {
+    mathShared = std::make_unique<DefaultMathShared>();
+  }
+  return *mathShared;
+}
+
+void MathSharedStore::SetMathShared(std::unique_ptr<MathShared> shared) {
+  std::scoped_lock lock(setLock);
+  mathShared = std::move(shared);
+}

wpimath/src/main/native/cpp/util/StateSpaceUtil.cpp

@@ -0,0 +1,71 @@
+// 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 "frc/StateSpaceUtil.h"
+
+#include <limits>
+
+namespace frc {
+
+template bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A,
+                                   const Matrixd<1, 1>& B);
+template bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A,
+                                   const Matrixd<2, 1>& B);
+template bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(
+    const Eigen::MatrixXd& A, const Eigen::MatrixXd& B);
+
+template bool IsDetectable<Eigen::Dynamic, Eigen::Dynamic>(
+    const Eigen::MatrixXd& A, const Eigen::MatrixXd& C);
+
+template Eigen::VectorXd ClampInputMaxMagnitude<Eigen::Dynamic>(
+    const Eigen::VectorXd& u, const Eigen::VectorXd& umin,
+    const Eigen::VectorXd& umax);
+
+template Eigen::VectorXd DesaturateInputVector<Eigen::Dynamic>(
+    const Eigen::VectorXd& u, double maxMagnitude);
+
+Eigen::MatrixXd MakeCostMatrix(const std::span<const double> costs) {
+  Eigen::MatrixXd result{costs.size(), costs.size()};
+  result.setZero();
+
+  for (size_t i = 0; i < costs.size(); ++i) {
+    if (costs[i] == std::numeric_limits<double>::infinity()) {
+      result(i, i) = 0.0;
+    } else {
+      result(i, i) = 1.0 / (std::pow(costs[i], 2));
+    }
+  }
+  return result;
+}
+
+Eigen::VectorXd MakeWhiteNoiseVector(const std::span<const double> stdDevs) {
+  std::random_device rd;
+  std::mt19937 gen{rd()};
+
+  Eigen::VectorXd result{stdDevs.size()};
+  for (size_t i = 0; i < stdDevs.size(); ++i) {
+    // Passing a standard deviation of 0.0 to std::normal_distribution is
+    // undefined behavior
+    if (stdDevs[i] == 0.0) {
+      result(i) = 0.0;
+    } else {
+      std::normal_distribution distr{0.0, stdDevs[i]};
+      result(i) = distr(gen);
+    }
+  }
+  return result;
+}
+
+Eigen::MatrixXd MakeCovMatrix(const std::span<const double> stdDevs) {
+  Eigen::MatrixXd result{stdDevs.size(), stdDevs.size()};
+  result.setZero();
+
+  for (size_t i = 0; i < stdDevs.size(); ++i) {
+    result(i, i) = std::pow(stdDevs[i], 2);
+  }
+
+  return result;
+}
+
+}  // namespace frc