[wpimath] Move math functionality into new wpimath library (#2629)

The wpimath library is a new library designed to separate the reusable math functionality
from the common utility library (wpiutil) and the hardware-dependent library (wpilibc/j).

Package names / include file names were NOT changed to minimize breakage.  In a future year
it would be good to revamp these for a more uniform user experience and to reduce the risk
of accidental naming conflicts.

While theoretically all of this functionality could be placed into wpiutil, several pieces
of this library (e.g. DARE) are very time-consuming to compile, so it's nice to avoid this
expense for users who only want cscore or ntcore.  It also allows for easy future separation
of build tasks vs number of workers on memory-constrained machines.

This moves the following functionality from wpiutil into wpimath:
- Eigen
- ejml
- Drake
- DARE
- wpiutil.math package (Matrix etc)
- units

And the following functionality from wpilibc/j into wpimath:
- Geometry
- Kinematics
- Spline
- Trajectory
- LinearFilter
- MedianFilter
- Feed-forward controllers

wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h

@@ -0,0 +1,85 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+
+#include <Eigen/Core>
+
+#include "frc/spline/Spline.h"
+
+namespace frc {
+/**
+ * Represents a hermite spline of degree 3.
+ */
+class CubicHermiteSpline : public Spline<3> {
+ public:
+  /**
+   * Constructs a cubic hermite spline with the specified control vectors. Each
+   * control vector contains info about the location of the point and its first
+   * derivative.
+   *
+   * @param xInitialControlVector The control vector for the initial point in
+   * the x dimension.
+   * @param xFinalControlVector The control vector for the final point in
+   * the x dimension.
+   * @param yInitialControlVector The control vector for the initial point in
+   * the y dimension.
+   * @param yFinalControlVector The control vector for the final point in
+   * the y dimension.
+   */
+  CubicHermiteSpline(std::array<double, 2> xInitialControlVector,
+                     std::array<double, 2> xFinalControlVector,
+                     std::array<double, 2> yInitialControlVector,
+                     std::array<double, 2> yFinalControlVector);
+
+ protected:
+  /**
+   * Returns the coefficients matrix.
+   * @return The coefficients matrix.
+   */
+  Eigen::Matrix<double, 6, 3 + 1> Coefficients() const override {
+    return m_coefficients;
+  }
+
+ private:
+  Eigen::Matrix<double, 6, 4> m_coefficients =
+      Eigen::Matrix<double, 6, 4>::Zero();
+
+  /**
+   * Returns the hermite basis matrix for cubic hermite spline interpolation.
+   * @return The hermite basis matrix for cubic hermite spline interpolation.
+   */
+  static Eigen::Matrix<double, 4, 4> MakeHermiteBasis() {
+    // clang-format off
+    static auto basis = (Eigen::Matrix<double, 4, 4>() <<
+     +2.0, +1.0, -2.0, +1.0,
+     -3.0, -2.0, +3.0, -1.0,
+     +0.0, +1.0, +0.0, +0.0,
+     +1.0, +0.0, +0.0, +0.0).finished();
+    // clang-format on
+    return basis;
+  }
+
+  /**
+   * Returns the control vector for each dimension as a matrix from the
+   * user-provided arrays in the constructor.
+   *
+   * @param initialVector The control vector for the initial point.
+   * @param finalVector The control vector for the final point.
+   *
+   * @return The control vector matrix for a dimension.
+   */
+  static Eigen::Vector4d ControlVectorFromArrays(
+      std::array<double, 2> initialVector, std::array<double, 2> finalVector) {
+    return (Eigen::Vector4d() << initialVector[0], initialVector[1],
+            finalVector[0], finalVector[1])
+        .finished();
+  }
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h

@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+
+#include <Eigen/Core>
+
+#include "frc/spline/Spline.h"
+
+namespace frc {
+/**
+ * Represents a hermite spline of degree 5.
+ */
+class QuinticHermiteSpline : public Spline<5> {
+ public:
+  /**
+   * Constructs a quintic hermite spline with the specified control vectors.
+   * Each control vector contains into about the location of the point, its
+   * first derivative, and its second derivative.
+   *
+   * @param xInitialControlVector The control vector for the initial point in
+   * the x dimension.
+   * @param xFinalControlVector The control vector for the final point in
+   * the x dimension.
+   * @param yInitialControlVector The control vector for the initial point in
+   * the y dimension.
+   * @param yFinalControlVector The control vector for the final point in
+   * the y dimension.
+   */
+  QuinticHermiteSpline(std::array<double, 3> xInitialControlVector,
+                       std::array<double, 3> xFinalControlVector,
+                       std::array<double, 3> yInitialControlVector,
+                       std::array<double, 3> yFinalControlVector);
+
+ protected:
+  /**
+   * Returns the coefficients matrix.
+   * @return The coefficients matrix.
+   */
+  Eigen::Matrix<double, 6, 6> Coefficients() const override {
+    return m_coefficients;
+  }
+
+ private:
+  Eigen::Matrix<double, 6, 6> m_coefficients =
+      Eigen::Matrix<double, 6, 6>::Zero();
+
+  /**
+   * Returns the hermite basis matrix for quintic hermite spline interpolation.
+   * @return The hermite basis matrix for quintic hermite spline interpolation.
+   */
+  static Eigen::Matrix<double, 6, 6> MakeHermiteBasis() {
+    // clang-format off
+    static const auto basis = (Eigen::Matrix<double, 6, 6>() <<
+      -06.0, -03.0, -00.5, +06.0, -03.0, +00.5,
+      +15.0, +08.0, +01.5, -15.0, +07.0, +01.0,
+      -10.0, -06.0, -01.5, +10.0, -04.0, +00.5,
+      +00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
+      +00.0, +01.0, +00.0, +00.0, +00.0, +00.0,
+      +01.0, +00.0, +00.0, +00.0, +00.0, +00.0).finished();
+    // clang-format on
+    return basis;
+  }
+
+  /**
+   * Returns the control vector for each dimension as a matrix from the
+   * user-provided arrays in the constructor.
+   *
+   * @param initialVector The control vector for the initial point.
+   * @param finalVector The control vector for the final point.
+   *
+   * @return The control vector matrix for a dimension.
+   */
+  static Eigen::Matrix<double, 6, 1> ControlVectorFromArrays(
+      std::array<double, 3> initialVector, std::array<double, 3> finalVector) {
+    return (Eigen::Matrix<double, 6, 1>() << initialVector[0], initialVector[1],
+            initialVector[2], finalVector[0], finalVector[1], finalVector[2])
+        .finished();
+  }
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/spline/Spline.h

@@ -0,0 +1,130 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+#include <utility>
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "frc/geometry/Pose2d.h"
+#include "units/curvature.h"
+#include "units/length.h"
+
+namespace frc {
+/**
+ * Represents a two-dimensional parametric spline that interpolates between two
+ * points.
+ *
+ * @tparam Degree The degree of the spline.
+ */
+template <int Degree>
+class Spline {
+ public:
+  using PoseWithCurvature = std::pair<Pose2d, units::curvature_t>;
+
+  Spline() = default;
+
+  Spline(const Spline&) = default;
+  Spline& operator=(const Spline&) = default;
+
+  Spline(Spline&&) = default;
+  Spline& operator=(Spline&&) = default;
+
+  virtual ~Spline() = default;
+
+  /**
+   * Represents a control vector for a spline.
+   *
+   * Each element in each array represents the value of the derivative at the
+   * index. For example, the value of x[2] is the second derivative in the x
+   * dimension.
+   */
+  struct ControlVector {
+    std::array<double, (Degree + 1) / 2> x;
+    std::array<double, (Degree + 1) / 2> y;
+  };
+
+  /**
+   * Gets the pose and curvature at some point t on the spline.
+   *
+   * @param t The point t
+   * @return The pose and curvature at that point.
+   */
+  PoseWithCurvature GetPoint(double t) const {
+    Eigen::Matrix<double, Degree + 1, 1> polynomialBases;
+
+    // Populate the polynomial bases
+    for (int i = 0; i <= Degree; i++) {
+      polynomialBases(i) = std::pow(t, Degree - i);
+    }
+
+    // This simply multiplies by the coefficients. We need to divide out t some
+    // n number of times where n is the derivative we want to take.
+    Eigen::Matrix<double, 6, 1> combined = Coefficients() * polynomialBases;
+
+    double dx, dy, ddx, ddy;
+
+    // If t = 0, all other terms in the equation cancel out to zero. We can use
+    // the last x^0 term in the equation.
+    if (t == 0.0) {
+      dx = Coefficients()(2, Degree - 1);
+      dy = Coefficients()(3, Degree - 1);
+      ddx = Coefficients()(4, Degree - 2);
+      ddy = Coefficients()(5, Degree - 2);
+    } else {
+      // Divide out t for first derivative.
+      dx = combined(2) / t;
+      dy = combined(3) / t;
+
+      // Divide out t for second derivative.
+      ddx = combined(4) / t / t;
+      ddy = combined(5) / t / t;
+    }
+
+    // Find the curvature.
+    const auto curvature =
+        (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * std::hypot(dx, dy));
+
+    return {
+        {FromVector(combined.template block<2, 1>(0, 0)), Rotation2d(dx, dy)},
+        units::curvature_t(curvature)};
+  }
+
+ protected:
+  /**
+   * Returns the coefficients of the spline.
+   *
+   * @return The coefficients of the spline.
+   */
+  virtual Eigen::Matrix<double, 6, Degree + 1> Coefficients() const = 0;
+
+  /**
+   * Converts a Translation2d into a vector that is compatible with Eigen.
+   *
+   * @param translation The Translation2d to convert.
+   * @return The vector.
+   */
+  static Eigen::Vector2d ToVector(const Translation2d& translation) {
+    return (Eigen::Vector2d() << translation.X().to<double>(),
+            translation.Y().to<double>())
+        .finished();
+  }
+
+  /**
+   * Converts an Eigen vector into a Translation2d.
+   *
+   * @param vector The vector to convert.
+   * @return The Translation2d.
+   */
+  static Translation2d FromVector(const Eigen::Vector2d& vector) {
+    return Translation2d(units::meter_t(vector(0)), units::meter_t(vector(1)));
+  }
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/spline/SplineHelper.h

@@ -0,0 +1,109 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+#include <utility>
+#include <vector>
+
+#include "frc/spline/CubicHermiteSpline.h"
+#include "frc/spline/QuinticHermiteSpline.h"
+
+namespace frc {
+/**
+ * Helper class that is used to generate cubic and quintic splines from user
+ * provided waypoints.
+ */
+class SplineHelper {
+ public:
+  /**
+   * Returns 2 cubic control vectors from a set of exterior waypoints and
+   * interior translations.
+   *
+   * @param start             The starting pose.
+   * @param interiorWaypoints The interior waypoints.
+   * @param end               The ending pose.
+   * @return 2 cubic control vectors.
+   */
+  static std::array<Spline<3>::ControlVector, 2>
+  CubicControlVectorsFromWaypoints(
+      const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
+      const Pose2d& end);
+
+  /**
+   * Returns quintic control vectors from a set of waypoints.
+   *
+   * @param waypoints The waypoints
+   * @return List of control vectors
+   */
+  static std::vector<Spline<5>::ControlVector>
+  QuinticControlVectorsFromWaypoints(const std::vector<Pose2d>& waypoints);
+
+  /**
+   * Returns a set of cubic splines corresponding to the provided control
+   * vectors. The user is free to set the direction of the start and end
+   * point. The directions for the middle waypoints are determined
+   * automatically to ensure continuous curvature throughout the path.
+   *
+   * The derivation for the algorithm used can be found here:
+   * <https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08/undervisningsmateriale/chap7alecture.pdf>
+   *
+   * @param start The starting control vector.
+   * @param waypoints The middle waypoints. This can be left blank if you
+   * only wish to create a path with two waypoints.
+   * @param end The ending control vector.
+   *
+   * @return A vector of cubic hermite splines that interpolate through the
+   * provided waypoints.
+   */
+  static std::vector<CubicHermiteSpline> CubicSplinesFromControlVectors(
+      const Spline<3>::ControlVector& start,
+      std::vector<Translation2d> waypoints,
+      const Spline<3>::ControlVector& end);
+
+  /**
+   * Returns a set of quintic splines corresponding to the provided control
+   * vectors. The user is free to set the direction of all waypoints. Continuous
+   * curvature is guaranteed throughout the path.
+   *
+   * @param controlVectors The control vectors.
+   * @return A vector of quintic hermite splines that interpolate through the
+   * provided waypoints.
+   */
+  static std::vector<QuinticHermiteSpline> QuinticSplinesFromControlVectors(
+      const std::vector<Spline<5>::ControlVector>& controlVectors);
+
+ private:
+  static Spline<3>::ControlVector CubicControlVector(double scalar,
+                                                     const Pose2d& point) {
+    return {{point.X().to<double>(), scalar * point.Rotation().Cos()},
+            {point.Y().to<double>(), scalar * point.Rotation().Sin()}};
+  }
+
+  static Spline<5>::ControlVector QuinticControlVector(double scalar,
+                                                       const Pose2d& point) {
+    return {{point.X().to<double>(), scalar * point.Rotation().Cos(), 0.0},
+            {point.Y().to<double>(), scalar * point.Rotation().Sin(), 0.0}};
+  }
+
+  /**
+   * Thomas algorithm for solving tridiagonal systems Af = d.
+   *
+   * @param a the values of A above the diagonal
+   * @param b the values of A on the diagonal
+   * @param c the values of A below the diagonal
+   * @param d the vector on the rhs
+   * @param solutionVector the unknown (solution) vector, modified in-place
+   */
+  static void ThomasAlgorithm(const std::vector<double>& a,
+                              const std::vector<double>& b,
+                              const std::vector<double>& c,
+                              const std::vector<double>& d,
+                              std::vector<double>* solutionVector);
+};
+}  // namespace frc

wpimath/src/main/native/include/frc/spline/SplineParameterizer.h

@@ -0,0 +1,145 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+/*
+ * MIT License
+ *
+ * Copyright (c) 2018 Team 254
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <stack>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <wpi/Twine.h>
+
+#include "frc/spline/Spline.h"
+#include "units/angle.h"
+#include "units/curvature.h"
+#include "units/length.h"
+#include "units/math.h"
+
+namespace frc {
+
+/**
+ * Class used to parameterize a spline by its arc length.
+ */
+class SplineParameterizer {
+ public:
+  using PoseWithCurvature = std::pair<Pose2d, units::curvature_t>;
+
+  struct MalformedSplineException : public std::runtime_error {
+    explicit MalformedSplineException(const char* what_arg)
+        : runtime_error(what_arg) {}
+  };
+
+  /**
+   * Parameterizes the spline. This method breaks up the spline into various
+   * arcs until their dx, dy, and dtheta are within specific tolerances.
+   *
+   * @param spline The spline to parameterize.
+   * @param t0 Starting internal spline parameter. It is recommended to leave
+   * this as default.
+   * @param t1 Ending internal spline parameter. It is recommended to leave this
+   * as default.
+   *
+   * @return A vector of poses and curvatures that represents various points on
+   * the spline.
+   */
+  template <int Dim>
+  static std::vector<PoseWithCurvature> Parameterize(const Spline<Dim>& spline,
+                                                     double t0 = 0.0,
+                                                     double t1 = 1.0) {
+    std::vector<PoseWithCurvature> splinePoints;
+
+    // The parameterization does not add the initial point. Let's add that.
+    splinePoints.push_back(spline.GetPoint(t0));
+
+    // We use an "explicit stack" to simulate recursion, instead of a recursive
+    // function call This give us greater control, instead of a stack overflow
+    std::stack<StackContents> stack;
+    stack.emplace(StackContents{t0, t1});
+
+    StackContents current;
+    PoseWithCurvature start;
+    PoseWithCurvature end;
+    int iterations = 0;
+
+    while (!stack.empty()) {
+      current = stack.top();
+      stack.pop();
+      start = spline.GetPoint(current.t0);
+      end = spline.GetPoint(current.t1);
+
+      const auto twist = start.first.Log(end.first);
+
+      if (units::math::abs(twist.dy) > kMaxDy ||
+          units::math::abs(twist.dx) > kMaxDx ||
+          units::math::abs(twist.dtheta) > kMaxDtheta) {
+        stack.emplace(StackContents{(current.t0 + current.t1) / 2, current.t1});
+        stack.emplace(StackContents{current.t0, (current.t0 + current.t1) / 2});
+      } else {
+        splinePoints.push_back(spline.GetPoint(current.t1));
+      }
+
+      if (iterations++ >= kMaxIterations) {
+        throw MalformedSplineException(
+            "Could not parameterize a malformed spline. "
+            "This means that you probably had two or more adjacent "
+            "waypoints that were very close together with headings "
+            "in opposing directions.");
+      }
+    }
+
+    return splinePoints;
+  }
+
+ private:
+  // Constraints for spline parameterization.
+  static constexpr units::meter_t kMaxDx = 5_in;
+  static constexpr units::meter_t kMaxDy = 0.05_in;
+  static constexpr units::radian_t kMaxDtheta = 0.0872_rad;
+
+  struct StackContents {
+    double t0;
+    double t1;
+  };
+
+  /**
+   * A malformed spline does not actually explode the LIFO stack size. Instead,
+   * the stack size stays at a relatively small number (e.g. 30) and never
+   * decreases. Because of this, we must count iterations. Even long, complex
+   * paths don't usually go over 300 iterations, so hitting this maximum should
+   * definitely indicate something has gone wrong.
+   */
+  static constexpr int kMaxIterations = 5000;
+
+  friend class CubicHermiteSplineTest;
+  friend class QuinticHermiteSplineTest;
+};
+}  // namespace frc