[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/geometry/Translation2d.h

@@ -0,0 +1,223 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <units/length.h>
+
+#include "Rotation2d.h"
+
+namespace wpi {
+class json;
+}  // namespace wpi
+
+namespace frc {
+
+/**
+ * Represents a translation in 2d space.
+ * This object can be used to represent a point or a vector.
+ *
+ * This assumes that you are using conventional mathematical axes.
+ * When the robot is placed on the origin, facing toward the X direction,
+ * moving forward increases the X, whereas moving to the left increases the Y.
+ */
+class Translation2d {
+ public:
+  /**
+   * Constructs a Translation2d with X and Y components equal to zero.
+   */
+  constexpr Translation2d() = default;
+
+  /**
+   * Constructs a Translation2d with the X and Y components equal to the
+   * provided values.
+   *
+   * @param x The x component of the translation.
+   * @param y The y component of the translation.
+   */
+  Translation2d(units::meter_t x, units::meter_t y);
+
+  /**
+   * Calculates the distance between two translations in 2d space.
+   *
+   * This function uses the pythagorean theorem to calculate the distance.
+   * distance = std::sqrt((x2 - x1)^2 + (y2 - y1)^2)
+   *
+   * @param other The translation to compute the distance to.
+   *
+   * @return The distance between the two translations.
+   */
+  units::meter_t Distance(const Translation2d& other) const;
+
+  /**
+   * Returns the X component of the translation.
+   *
+   * @return The x component of the translation.
+   */
+  units::meter_t X() const { return m_x; }
+
+  /**
+   * Returns the Y component of the translation.
+   *
+   * @return The y component of the translation.
+   */
+  units::meter_t Y() const { return m_y; }
+
+  /**
+   * Returns the norm, or distance from the origin to the translation.
+   *
+   * @return The norm of the translation.
+   */
+  units::meter_t Norm() const;
+
+  /**
+   * Applies a rotation to the translation in 2d space.
+   *
+   * This multiplies the translation vector by a counterclockwise rotation
+   * matrix of the given angle.
+   *
+   * [x_new]   [other.cos, -other.sin][x]
+   * [y_new] = [other.sin,  other.cos][y]
+   *
+   * For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
+   * Translation2d of {0, 2}.
+   *
+   * @param other The rotation to rotate the translation by.
+   *
+   * @return The new rotated translation.
+   */
+  Translation2d RotateBy(const Rotation2d& other) const;
+
+  /**
+   * Adds two translations in 2d space and returns the sum. This is similar to
+   * vector addition.
+   *
+   * For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
+   * Translation2d{3.0, 8.0}
+   *
+   * @param other The translation to add.
+   *
+   * @return The sum of the translations.
+   */
+  Translation2d operator+(const Translation2d& other) const;
+
+  /**
+   * Adds the new translation to the current translation.
+   *
+   * This is similar to the + operator, except that the current object is
+   * mutated.
+   *
+   * @param other The translation to add.
+   *
+   * @return The reference to the new mutated object.
+   */
+  Translation2d& operator+=(const Translation2d& other);
+
+  /**
+   * Subtracts the other translation from the other translation and returns the
+   * difference.
+   *
+   * For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
+   * Translation2d{4.0, 2.0}
+   *
+   * @param other The translation to subtract.
+   *
+   * @return The difference between the two translations.
+   */
+  Translation2d operator-(const Translation2d& other) const;
+
+  /**
+   * Subtracts the new translation from the current translation.
+   *
+   * This is similar to the - operator, except that the current object is
+   * mutated.
+   *
+   * @param other The translation to subtract.
+   *
+   * @return The reference to the new mutated object.
+   */
+  Translation2d& operator-=(const Translation2d& other);
+
+  /**
+   * Returns the inverse of the current translation. This is equivalent to
+   * rotating by 180 degrees, flipping the point over both axes, or simply
+   * negating both components of the translation.
+   *
+   * @return The inverse of the current translation.
+   */
+  Translation2d operator-() const;
+
+  /**
+   * Multiplies the translation by a scalar and returns the new translation.
+   *
+   * For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
+   *
+   * @param scalar The scalar to multiply by.
+   *
+   * @return The scaled translation.
+   */
+  Translation2d operator*(double scalar) const;
+
+  /**
+   * Multiplies the current translation by a scalar.
+   *
+   * This is similar to the * operator, except that current object is mutated.
+   *
+   * @param scalar The scalar to multiply by.
+   *
+   * @return The reference to the new mutated object.
+   */
+  Translation2d& operator*=(double scalar);
+
+  /**
+   * Divides the translation by a scalar and returns the new translation.
+   *
+   * For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
+   *
+   * @param scalar The scalar to divide by.
+   *
+   * @return The scaled translation.
+   */
+  Translation2d operator/(double scalar) const;
+
+  /**
+   * Checks equality between this Translation2d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const Translation2d& other) const;
+
+  /**
+   * Checks inequality between this Translation2d and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const Translation2d& other) const;
+
+  /*
+   * Divides the current translation by a scalar.
+   *
+   * This is similar to the / operator, except that current object is mutated.
+   *
+   * @param scalar The scalar to divide by.
+   *
+   * @return The reference to the new mutated object.
+   */
+  Translation2d& operator/=(double scalar);
+
+ private:
+  units::meter_t m_x = 0_m;
+  units::meter_t m_y = 0_m;
+};
+
+void to_json(wpi::json& json, const Translation2d& state);
+
+void from_json(const wpi::json& json, Translation2d& state);
+
+}  // namespace frc