[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/test/native/cpp/geometry/Pose2dTest.cpp

@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include "frc/geometry/Pose2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Pose2dTest, TransformBy) {
+  const Pose2d initial{1_m, 2_m, Rotation2d(45.0_deg)};
+  const Transform2d transform{Translation2d{5.0_m, 0.0_m}, Rotation2d(5.0_deg)};
+
+  const auto transformed = initial + transform;
+
+  EXPECT_NEAR(transformed.X().to<double>(), 1 + 5 / std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(transformed.Y().to<double>(), 2 + 5 / std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(transformed.Rotation().Degrees().to<double>(), 50.0, kEpsilon);
+}
+
+TEST(Pose2dTest, RelativeTo) {
+  const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
+  const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
+
+  const auto finalRelativeToInitial = final.RelativeTo(initial);
+
+  EXPECT_NEAR(finalRelativeToInitial.X().to<double>(), 5.0 * std::sqrt(2.0),
+              kEpsilon);
+  EXPECT_NEAR(finalRelativeToInitial.Y().to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().to<double>(), 0.0,
+              kEpsilon);
+}
+
+TEST(Pose2dTest, Equality) {
+  const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
+  const Pose2d b{0_m, 5_m, Rotation2d(43_deg)};
+  EXPECT_TRUE(a == b);
+}
+
+TEST(Pose2dTest, Inequality) {
+  const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
+  const Pose2d b{0_m, 5_ft, Rotation2d(43_deg)};
+  EXPECT_TRUE(a != b);
+}
+
+TEST(Pose2dTest, Minus) {
+  const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
+  const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
+
+  const auto transform = final - initial;
+
+  EXPECT_NEAR(transform.X().to<double>(), 5.0 * std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(transform.Y().to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(transform.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
+}

wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp

@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include <wpi/math>
+
+#include "frc/geometry/Rotation2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Rotation2dTest, RadiansToDegrees) {
+  const Rotation2d one{units::radian_t(wpi::math::pi / 3)};
+  const Rotation2d two{units::radian_t(wpi::math::pi / 4)};
+
+  EXPECT_NEAR(one.Degrees().to<double>(), 60.0, kEpsilon);
+  EXPECT_NEAR(two.Degrees().to<double>(), 45.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, DegreesToRadians) {
+  const auto one = Rotation2d(45.0_deg);
+  const auto two = Rotation2d(30.0_deg);
+
+  EXPECT_NEAR(one.Radians().to<double>(), wpi::math::pi / 4.0, kEpsilon);
+  EXPECT_NEAR(two.Radians().to<double>(), wpi::math::pi / 6.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, RotateByFromZero) {
+  const Rotation2d zero;
+  auto sum = zero + Rotation2d(90.0_deg);
+
+  EXPECT_NEAR(sum.Radians().to<double>(), wpi::math::pi / 2.0, kEpsilon);
+  EXPECT_NEAR(sum.Degrees().to<double>(), 90.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, RotateByNonZero) {
+  auto rot = Rotation2d(90.0_deg);
+  rot += Rotation2d(30.0_deg);
+
+  EXPECT_NEAR(rot.Degrees().to<double>(), 120.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, Minus) {
+  const auto one = Rotation2d(70.0_deg);
+  const auto two = Rotation2d(30.0_deg);
+
+  EXPECT_NEAR((one - two).Degrees().to<double>(), 40.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, Equality) {
+  const auto one = Rotation2d(43_deg);
+  const auto two = Rotation2d(43_deg);
+  EXPECT_TRUE(one == two);
+}
+
+TEST(Rotation2dTest, Inequality) {
+  const auto one = Rotation2d(43_deg);
+  const auto two = Rotation2d(43.5_deg);
+  EXPECT_TRUE(one != two);
+}

wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp

@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Transform2d.h"
+#include "frc/geometry/Translation2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Transform2dTest, Inverse) {
+  const Pose2d initial{1_m, 2_m, Rotation2d(45.0_deg)};
+  const Transform2d transform{Translation2d{5.0_m, 0.0_m}, Rotation2d(5.0_deg)};
+
+  auto transformed = initial + transform;
+  auto untransformed = transformed + transform.Inverse();
+
+  EXPECT_NEAR(initial.X().to<double>(), untransformed.X().to<double>(),
+              kEpsilon);
+  EXPECT_NEAR(initial.Y().to<double>(), untransformed.Y().to<double>(),
+              kEpsilon);
+  EXPECT_NEAR(initial.Rotation().Degrees().to<double>(),
+              untransformed.Rotation().Degrees().to<double>(), kEpsilon);
+}

wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp

@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include "frc/geometry/Translation2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Translation2dTest, Sum) {
+  const Translation2d one{1.0_m, 3.0_m};
+  const Translation2d two{2.0_m, 5.0_m};
+
+  const auto sum = one + two;
+
+  EXPECT_NEAR(sum.X().to<double>(), 3.0, kEpsilon);
+  EXPECT_NEAR(sum.Y().to<double>(), 8.0, kEpsilon);
+}
+
+TEST(Translation2dTest, Difference) {
+  const Translation2d one{1.0_m, 3.0_m};
+  const Translation2d two{2.0_m, 5.0_m};
+
+  const auto difference = one - two;
+
+  EXPECT_NEAR(difference.X().to<double>(), -1.0, kEpsilon);
+  EXPECT_NEAR(difference.Y().to<double>(), -2.0, kEpsilon);
+}
+
+TEST(Translation2dTest, RotateBy) {
+  const Translation2d another{3.0_m, 0.0_m};
+  const auto rotated = another.RotateBy(Rotation2d(90.0_deg));
+
+  EXPECT_NEAR(rotated.X().to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(rotated.Y().to<double>(), 3.0, kEpsilon);
+}
+
+TEST(Translation2dTest, Multiplication) {
+  const Translation2d original{3.0_m, 5.0_m};
+  const auto mult = original * 3;
+
+  EXPECT_NEAR(mult.X().to<double>(), 9.0, kEpsilon);
+  EXPECT_NEAR(mult.Y().to<double>(), 15.0, kEpsilon);
+}
+
+TEST(Translation2d, Division) {
+  const Translation2d original{3.0_m, 5.0_m};
+  const auto div = original / 2;
+
+  EXPECT_NEAR(div.X().to<double>(), 1.5, kEpsilon);
+  EXPECT_NEAR(div.Y().to<double>(), 2.5, kEpsilon);
+}
+
+TEST(Translation2dTest, Norm) {
+  const Translation2d one{3.0_m, 5.0_m};
+  EXPECT_NEAR(one.Norm().to<double>(), std::hypot(3, 5), kEpsilon);
+}
+
+TEST(Translation2dTest, Distance) {
+  const Translation2d one{1_m, 1_m};
+  const Translation2d two{6_m, 6_m};
+  EXPECT_NEAR(one.Distance(two).to<double>(), 5 * std::sqrt(2), kEpsilon);
+}
+
+TEST(Translation2dTest, UnaryMinus) {
+  const Translation2d original{-4.5_m, 7_m};
+  const auto inverted = -original;
+
+  EXPECT_NEAR(inverted.X().to<double>(), 4.5, kEpsilon);
+  EXPECT_NEAR(inverted.Y().to<double>(), -7, kEpsilon);
+}
+
+TEST(Translation2dTest, Equality) {
+  const Translation2d one{9_m, 5.5_m};
+  const Translation2d two{9_m, 5.5_m};
+  EXPECT_TRUE(one == two);
+}
+
+TEST(Translation2dTest, Inequality) {
+  const Translation2d one{9_m, 5.5_m};
+  const Translation2d two{9_m, 5.7_m};
+  EXPECT_TRUE(one != two);
+}

wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp

@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include <wpi/math>
+
+#include "frc/geometry/Pose2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Twist2dTest, Straight) {
+  const Twist2d straight{5.0_m, 0.0_m, 0.0_rad};
+  const auto straightPose = Pose2d().Exp(straight);
+
+  EXPECT_NEAR(straightPose.X().to<double>(), 5.0, kEpsilon);
+  EXPECT_NEAR(straightPose.Y().to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(straightPose.Rotation().Radians().to<double>(), 0.0, kEpsilon);
+}
+
+TEST(Twist2dTest, QuarterCircle) {
+  const Twist2d quarterCircle{5.0_m / 2.0 * wpi::math::pi, 0_m,
+                              units::radian_t(wpi::math::pi / 2.0)};
+  const auto quarterCirclePose = Pose2d().Exp(quarterCircle);
+
+  EXPECT_NEAR(quarterCirclePose.X().to<double>(), 5.0, kEpsilon);
+  EXPECT_NEAR(quarterCirclePose.Y().to<double>(), 5.0, kEpsilon);
+  EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().to<double>(), 90.0,
+              kEpsilon);
+}
+
+TEST(Twist2dTest, DiagonalNoDtheta) {
+  const Twist2d diagonal{2.0_m, 2.0_m, 0.0_deg};
+  const auto diagonalPose = Pose2d().Exp(diagonal);
+
+  EXPECT_NEAR(diagonalPose.X().to<double>(), 2.0, kEpsilon);
+  EXPECT_NEAR(diagonalPose.Y().to<double>(), 2.0, kEpsilon);
+  EXPECT_NEAR(diagonalPose.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
+}
+
+TEST(Twist2dTest, Equality) {
+  const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
+  const Twist2d two{5.0_m, 1.0_m, 3.0_rad};
+  EXPECT_TRUE(one == two);
+}
+
+TEST(Twist2dTest, Inequality) {
+  const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
+  const Twist2d two{5.0_m, 1.2_m, 3.0_rad};
+  EXPECT_TRUE(one != two);
+}
+
+TEST(Twist2dTest, Pose2dLog) {
+  const Pose2d end{5_m, 5_m, Rotation2d(90_deg)};
+  const Pose2d start{};
+
+  const auto twist = start.Log(end);
+
+  EXPECT_NEAR(twist.dx.to<double>(), 5 / 2.0 * wpi::math::pi, kEpsilon);
+  EXPECT_NEAR(twist.dy.to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(twist.dtheta.to<double>(), wpi::math::pi / 2.0, kEpsilon);
+}