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

@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#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.0, 2.0, Rotation2d::FromDegrees(45.0)};
+  const Transform2d transform{Translation2d{5.0, 0.0},
+                              Rotation2d::FromDegrees(5.0)};
+
+  const auto transformed = initial + transform;
+
+  EXPECT_NEAR(transformed.Translation().X(), 1 + 5 / std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(transformed.Translation().Y(), 2 + 5 / std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(transformed.Rotation().Degrees(), 50.0, kEpsilon);
+}
+
+TEST(Pose2dTest, RelativeTo) {
+  const Pose2d initial{0.0, 0.0, Rotation2d::FromDegrees(45.0)};
+  const Pose2d final{5.0, 5.0, Rotation2d::FromDegrees(45.0)};
+
+  const auto finalRelativeToInitial = final.RelativeTo(initial);
+
+  EXPECT_NEAR(finalRelativeToInitial.Translation().X(), 5.0 * std::sqrt(2.0),
+              kEpsilon);
+  EXPECT_NEAR(finalRelativeToInitial.Translation().Y(), 0.0, kEpsilon);
+  EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees(), 0.0, kEpsilon);
+}

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

@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include "frc/geometry/Rotation2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+static constexpr double kPi = 3.14159265358979323846;
+
+TEST(Rotation2dTest, RadiansToDegrees) {
+  const Rotation2d one{kPi / 3};
+  const Rotation2d two{kPi / 4};
+
+  EXPECT_NEAR(one.Degrees(), 60.0, kEpsilon);
+  EXPECT_NEAR(two.Degrees(), 45.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, DegreesToRadians) {
+  const auto one = Rotation2d::FromDegrees(45.0);
+  const auto two = Rotation2d::FromDegrees(30.0);
+
+  EXPECT_NEAR(one.Radians(), kPi / 4.0, kEpsilon);
+  EXPECT_NEAR(two.Radians(), kPi / 6.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, RotateByFromZero) {
+  const Rotation2d zero;
+  auto sum = zero + Rotation2d::FromDegrees(90.0);
+
+  EXPECT_NEAR(sum.Radians(), kPi / 2.0, kEpsilon);
+  EXPECT_NEAR(sum.Degrees(), 90.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, RotateByNonZero) {
+  auto rot = Rotation2d::FromDegrees(90.0);
+  rot += Rotation2d::FromDegrees(30.0);
+
+  EXPECT_NEAR(rot.Degrees(), 120.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, Minus) {
+  const auto one = Rotation2d::FromDegrees(70.0);
+  const auto two = Rotation2d::FromDegrees(30.0);
+
+  EXPECT_NEAR((one - two).Degrees(), 40.0, kEpsilon);
+}

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

@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#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, 3.0};
+  const Translation2d two{2.0, 5.0};
+
+  const auto sum = one + two;
+
+  EXPECT_NEAR(sum.X(), 3.0, kEpsilon);
+  EXPECT_NEAR(sum.Y(), 8.0, kEpsilon);
+}
+
+TEST(Translation2dTest, Difference) {
+  const Translation2d one{1.0, 3.0};
+  const Translation2d two{2.0, 5.0};
+
+  const auto difference = one - two;
+
+  EXPECT_NEAR(difference.X(), -1.0, kEpsilon);
+  EXPECT_NEAR(difference.Y(), -2.0, kEpsilon);
+}
+
+TEST(Translation2dTest, RotateBy) {
+  const Translation2d another{3.0, 0.0};
+  const auto rotated = another.RotateBy(Rotation2d::FromDegrees(90.0));
+
+  EXPECT_NEAR(rotated.X(), 0.0, kEpsilon);
+  EXPECT_NEAR(rotated.Y(), 3.0, kEpsilon);
+}
+
+TEST(Translation2dTest, Multiplication) {
+  const Translation2d original{3.0, 5.0};
+  const auto mult = original * 3;
+
+  EXPECT_NEAR(mult.X(), 9.0, kEpsilon);
+  EXPECT_NEAR(mult.Y(), 15.0, kEpsilon);
+}
+
+TEST(Translation2d, Division) {
+  const Translation2d original{3.0, 5.0};
+  const auto div = original / 2;
+
+  EXPECT_NEAR(div.X(), 1.5, kEpsilon);
+  EXPECT_NEAR(div.Y(), 2.5, kEpsilon);
+}
+
+TEST(Translation2dTest, Norm) {
+  const Translation2d one{3.0, 5.0};
+  EXPECT_NEAR(one.Norm(), std::hypot(3, 5), kEpsilon);
+}
+
+TEST(Translation2dTest, Distance) {
+  const Translation2d one{1, 1};
+  const Translation2d two{6, 6};
+  EXPECT_NEAR(one.Distance(two), 5 * std::sqrt(2), kEpsilon);
+}
+
+TEST(Translation2dTest, UnaryMinus) {
+  const Translation2d original{-4.5, 7};
+  const auto inverted = -original;
+
+  EXPECT_NEAR(inverted.X(), 4.5, kEpsilon);
+  EXPECT_NEAR(inverted.Y(), -7, kEpsilon);
+}

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

@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#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, 0.0, 0.0};
+  const auto straightPose = Pose2d().Exp(straight);
+
+  EXPECT_NEAR(straightPose.Translation().X(), 5.0, kEpsilon);
+  EXPECT_NEAR(straightPose.Translation().Y(), 0.0, kEpsilon);
+  EXPECT_NEAR(straightPose.Rotation().Radians(), 0.0, kEpsilon);
+}
+
+TEST(Twist2dTest, QuarterCircle) {
+  const Twist2d quarterCircle{5.0 / 2.0 * 3.14159265358979323846, 0,
+                              3.14159265358979323846 / 2.0};
+  const auto quarterCirclePose = Pose2d().Exp(quarterCircle);
+
+  EXPECT_NEAR(quarterCirclePose.Translation().X(), 5.0, kEpsilon);
+  EXPECT_NEAR(quarterCirclePose.Translation().Y(), 5.0, kEpsilon);
+  EXPECT_NEAR(quarterCirclePose.Rotation().Degrees(), 90.0, kEpsilon);
+}
+
+TEST(Twist2dTest, DiagonalNoDtheta) {
+  const Twist2d diagonal{2.0, 2.0, 0.0};
+  const auto diagonalPose = Pose2d().Exp(diagonal);
+
+  EXPECT_NEAR(diagonalPose.Translation().X(), 2.0, kEpsilon);
+  EXPECT_NEAR(diagonalPose.Translation().Y(), 2.0, kEpsilon);
+  EXPECT_NEAR(diagonalPose.Rotation().Degrees(), 0.0, kEpsilon);
+}