[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/kinematics/SwerveDriveOdometry.h

@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <chrono>
+#include <cstddef>
+#include <ctime>
+
+#include <wpi/timestamp.h>
+
+#include "SwerveDriveKinematics.h"
+#include "SwerveModuleState.h"
+#include "frc/geometry/Pose2d.h"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * Class for swerve drive odometry. Odometry allows you to track the robot's
+ * position on the field over a course of a match using readings from your
+ * swerve drive encoders and swerve azimuth encoders.
+ *
+ * Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ */
+template <size_t NumModules>
+class SwerveDriveOdometry {
+ public:
+  /**
+   * Constructs a SwerveDriveOdometry object.
+   *
+   * @param kinematics The swerve drive kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param initialPose The starting position of the robot on the field.
+   */
+  SwerveDriveOdometry(SwerveDriveKinematics<NumModules> kinematics,
+                      const Rotation2d& gyroAngle,
+                      const Pose2d& initialPose = Pose2d());
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * The gyroscope angle does not need to be reset here on the user's robot
+   * code. The library automatically takes care of offsetting the gyro angle.
+   *
+   * @param pose The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
+    m_pose = pose;
+    m_previousAngle = pose.Rotation();
+    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   * @return The pose of the robot.
+   */
+  const Pose2d& GetPose() const { return m_pose; }
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and
+   * integration of the pose over time. This method takes in the current time as
+   * a parameter to calculate period (difference between two timestamps). The
+   * period is used to calculate the change in distance from a velocity. This
+   * also takes in an angle parameter which is used instead of the
+   * angular rate that is calculated from forward kinematics.
+   *
+   * @param currentTime The current time.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param moduleStates The current state of all swerve modules. Please provide
+   *                     the states in the same order in which you instantiated
+   *                     your SwerveDriveKinematics.
+   *
+   * @return The new pose of the robot.
+   */
+  template <typename... ModuleStates>
+  const Pose2d& UpdateWithTime(units::second_t currentTime,
+                               const Rotation2d& gyroAngle,
+                               ModuleStates&&... moduleStates);
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and
+   * integration of the pose over time. This method automatically calculates
+   * the current time to calculate period (difference between two timestamps).
+   * The period is used to calculate the change in distance from a velocity.
+   * This also takes in an angle parameter which is used instead of the
+   * angular rate that is calculated from forward kinematics.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param moduleStates The current state of all swerve modules. Please provide
+   *                     the states in the same order in which you instantiated
+   *                     your SwerveDriveKinematics.
+   *
+   * @return The new pose of the robot.
+   */
+  template <typename... ModuleStates>
+  const Pose2d& Update(const Rotation2d& gyroAngle,
+                       ModuleStates&&... moduleStates) {
+    return UpdateWithTime(wpi::Now() * 1.0e-6_s, gyroAngle, moduleStates...);
+  }
+
+ private:
+  SwerveDriveKinematics<NumModules> m_kinematics;
+  Pose2d m_pose;
+
+  units::second_t m_previousTime = -1_s;
+  Rotation2d m_previousAngle;
+  Rotation2d m_gyroOffset;
+};
+
+}  // namespace frc
+
+#include "SwerveDriveOdometry.inc"