[wpimath] Increase constexpr support in geometry data types (#4231)

This uses std::is_constant_evaluated() to conditionally use the gcem library for constexpr calculations.

wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp

@@ -53,3 +53,22 @@ TEST(Pose2dTest, Minus) {
   EXPECT_NEAR(0.0, transform.Y().value(), 1e-9);
   EXPECT_DOUBLE_EQ(0.0, transform.Rotation().Degrees().value());
 }
+
+TEST(Pose2dTest, Constexpr) {
+  constexpr Pose2d defaultConstructed;
+  constexpr Pose2d translationRotation{Translation2d{0_m, 1_m},
+                                       Rotation2d{0_deg}};
+  constexpr Pose2d coordRotation{0_m, 0_m, Rotation2d{45_deg}};
+
+  constexpr auto added =
+      translationRotation + Transform2d{Translation2d{}, Rotation2d{45_deg}};
+  constexpr auto multiplied = coordRotation * 2;
+
+  static_assert(defaultConstructed.X() == 0_m);
+  static_assert(translationRotation.Y() == 1_m);
+  static_assert(coordRotation.Rotation().Degrees() == 45_deg);
+  static_assert(added.X() == 0_m);
+  static_assert(added.Y() == 1_m);
+  static_assert(added.Rotation().Degrees() == 45_deg);
+  static_assert(multiplied.Rotation().Degrees() == 90_deg);
+}

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

@@ -63,3 +63,22 @@ TEST(Rotation2dTest, Inequality) {
   const auto rot2 = Rotation2d{43.5_deg};
   EXPECT_NE(rot1, rot2);
 }
+
+TEST(Rotation2dTest, Constexpr) {
+  constexpr Rotation2d defaultCtor;
+  constexpr Rotation2d radianCtor{5_rad};
+  constexpr Rotation2d degreeCtor{270_deg};
+  constexpr Rotation2d rotation45{45_deg};
+  constexpr Rotation2d cartesianCtor{3.5, -3.5};
+
+  constexpr auto negated = -radianCtor;
+  constexpr auto multiplied = radianCtor * 2;
+  constexpr auto subtracted = cartesianCtor - degreeCtor;
+
+  static_assert(defaultCtor.Radians() == 0_rad);
+  static_assert(degreeCtor.Degrees() == 270_deg);
+  static_assert(negated.Radians() == (-5_rad));
+  static_assert(multiplied.Radians() == 10_rad);
+  static_assert(subtracted == rotation45);
+  static_assert(radianCtor != degreeCtor);
+}

wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp

@@ -40,3 +40,21 @@ TEST(Transform2dTest, Composition) {
   EXPECT_DOUBLE_EQ(transformedSeparate.Rotation().Degrees().value(),
                    transformedCombined.Rotation().Degrees().value());
 }
+
+TEST(Transform2dTest, Constexpr) {
+  constexpr Transform2d defaultCtor;
+  constexpr Transform2d translationRotationCtor{Translation2d{},
+                                                Rotation2d{10_deg}};
+  constexpr auto multiplied = translationRotationCtor * 5;
+  constexpr auto divided = translationRotationCtor / 2;
+
+  static_assert(defaultCtor.Translation().X() == 0_m);
+  static_assert(translationRotationCtor.X() == 0_m);
+  static_assert(translationRotationCtor.Y() == 0_m);
+  static_assert(multiplied.Rotation().Degrees() == 50_deg);
+  static_assert(translationRotationCtor.Inverse().Rotation().Degrees() ==
+                (-10_deg));
+  static_assert(translationRotationCtor.Inverse().X() == 0_m);
+  static_assert(translationRotationCtor.Inverse().Y() == 0_m);
+  static_assert(divided.Rotation().Degrees() == 5_deg);
+}

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

@@ -93,3 +93,21 @@ TEST(Translation2dTest, PolarConstructor) {
   EXPECT_DOUBLE_EQ(1.0, two.X().value());
   EXPECT_DOUBLE_EQ(std::sqrt(3.0), two.Y().value());
 }
+
+TEST(Translation2dTest, Constexpr) {
+  constexpr Translation2d defaultCtor;
+  constexpr Translation2d componentCtor{1_m, 2_m};
+  constexpr auto added = defaultCtor + componentCtor;
+  constexpr auto subtracted = defaultCtor - componentCtor;
+  constexpr auto negated = -componentCtor;
+  constexpr auto multiplied = componentCtor * 2;
+  constexpr auto divided = componentCtor / 2;
+
+  static_assert(defaultCtor.X() == 0_m);
+  static_assert(componentCtor.Y() == 2_m);
+  static_assert(added.X() == 1_m);
+  static_assert(subtracted.Y() == (-2_m));
+  static_assert(negated.X() == (-1_m));
+  static_assert(multiplied.X() == 2_m);
+  static_assert(divided.Y() == 1_m);
+}

wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp

@@ -126,3 +126,24 @@ TEST(Translation3dTest, PolarConstructor) {
   EXPECT_NEAR(two.Y().value(), std::sqrt(3.0), kEpsilon);
   EXPECT_NEAR(two.Z().value(), 0.0, kEpsilon);
 }
+
+TEST(Translation3dTest, Constexpr) {
+  constexpr Translation3d defaultCtor;
+  constexpr Translation3d componentCtor{1_m, 2_m, 3_m};
+  constexpr auto added = defaultCtor + componentCtor;
+  constexpr auto subtracted = defaultCtor - componentCtor;
+  constexpr auto negated = -componentCtor;
+  constexpr auto multiplied = componentCtor * 2;
+  constexpr auto divided = componentCtor / 2;
+  constexpr Translation2d projected = componentCtor.ToTranslation2d();
+
+  static_assert(defaultCtor.X() == 0_m);
+  static_assert(componentCtor.Y() == 2_m);
+  static_assert(added.Z() == 3_m);
+  static_assert(subtracted.X() == (-1_m));
+  static_assert(negated.Y() == (-2_m));
+  static_assert(multiplied.Z() == 6_m);
+  static_assert(divided.Y() == 1_m);
+  static_assert(projected.X() == 1_m);
+  static_assert(projected.Y() == 2_m);
+}

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

@@ -64,3 +64,13 @@ TEST(Twist2dTest, Pose2dLog) {
   const auto reapplied = start.Exp(twist);
   EXPECT_EQ(end, reapplied);
 }
+
+TEST(Twist2dTest, Constexpr) {
+  constexpr Twist2d defaultCtor;
+  constexpr Twist2d componentCtor{1_m, 2_m, 3_rad};
+  constexpr auto multiplied = componentCtor * 2;
+
+  static_assert(defaultCtor.dx == 0_m);
+  static_assert(componentCtor.dy == 2_m);
+  static_assert(multiplied.dtheta == 6_rad);
+}

wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp

@@ -115,3 +115,17 @@ TEST(Twist3dTest, Pose3dLogZ) {
   const auto reapplied = start.Exp(twist);
   EXPECT_EQ(end, reapplied);
 }
+
+TEST(Twist3dTest, Constexpr) {
+  constexpr Twist3d defaultCtor;
+  constexpr Twist3d componentCtor{1_m, 2_m, 3_m, 4_rad, 5_rad, 6_rad};
+  constexpr auto multiplied = componentCtor * 2;
+
+  static_assert(defaultCtor.dx == 0_m);
+  static_assert(componentCtor.dy == 2_m);
+  static_assert(componentCtor.dz == 3_m);
+  static_assert(multiplied.dx == 2_m);
+  static_assert(multiplied.rx == 8_rad);
+  static_assert(multiplied.ry == 10_rad);
+  static_assert(multiplied.rz == 12_rad);
+}