Add geometry classes (#1766)

These classes introduce ways to represent poses and provide easy ways to transform, rotate, and translate poses across 2d space. This classes will be especially useful for a planned odometry and kinematics suite.

Furthermore, these classes can also be used to simply represent waypoints on a field, do superstructure motion planning, etc.

wpilibc/src/main/native/include/frc/geometry/Translation2d.h

@@ -0,0 +1,196 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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 "Rotation2d.h"
+
+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(double x, double 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.
+   */
+  double Distance(const Translation2d& other) const;
+
+  /**
+   * Returns the X component of the translation.
+   *
+   * @return The x component of the translation.
+   */
+  double X() const { return m_x; }
+
+  /**
+   * Returns the Y component of the translation.
+   *
+   * @return The y component of the translation.
+   */
+  double Y() const { return m_y; }
+
+  /**
+   * Returns the norm, or distance from the origin to the translation.
+   *
+   * @return The norm of the translation.
+   */
+  double 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;
+
+  /*
+   * 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:
+  double m_x = 0;
+  double m_y = 0;
+};
+}  // namespace frc