[wpimath] Refactor kinematics, odometry, and pose estimator (#5355)

wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h

@@ -0,0 +1,56 @@
+// 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.
+
+#pragma once
+
+#include <wpi/MathExtras.h>
+#include <wpi/SymbolExports.h>
+
+#include "units/length.h"
+
+namespace frc {
+/**
+ * Represents the wheel positions for a differential drive drivetrain.
+ */
+struct WPILIB_DLLEXPORT DifferentialDriveWheelPositions {
+  /**
+   * Distance driven by the left side.
+   */
+  units::meter_t left = 0_m;
+
+  /**
+   * Distance driven by the right side.
+   */
+  units::meter_t right = 0_m;
+
+  /**
+   * Checks equality between this DifferentialDriveWheelPositions and another
+   * object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const DifferentialDriveWheelPositions& other) const = default;
+
+  /**
+   * Checks inequality between this DifferentialDriveWheelPositions and another
+   * object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const DifferentialDriveWheelPositions& other) const = default;
+
+  DifferentialDriveWheelPositions operator-(
+      const DifferentialDriveWheelPositions& other) const {
+    return {left - other.left, right - other.right};
+  }
+
+  DifferentialDriveWheelPositions Interpolate(
+      const DifferentialDriveWheelPositions& endValue, double t) const {
+    return {wpi::Lerp(left, endValue.left, t),
+            wpi::Lerp(right, endValue.right, t)};
+  }
+};
+}  // namespace frc