add + and - operators for Angle

LemLib · Dec 29, 2024 · 2a20ff5 · 2a20ff5
1 parent 0acf513
commit 2a20ff5
Show file tree

Hide file tree

Showing 2 changed files with 47 additions and 6 deletions.
diff --git a/include/units/Angle.hpp b/include/units/Angle.hpp
@@ -24,8 +24,23 @@ template <> struct LookupName<Quantity<std::ratio<0>, std::ratio<0>, std::ratio<
 // -15_cDeg == 105_stDeg
 // 15_cDeg == 75_stDeg
 
-// in order to do conversions properly between angles in standard position and compass angles, we need this helper class
-// it can only be constructed through string literals
+/**
+ * @brief DO NOT USE
+ *
+ * this class prevents conversion errors from compass angles to angles in standard position.
+ *
+ * consider the following:
+ * 0_cDeg gets converted to standard position angles internally, so it's converted to 90
+ * -0_cDeg is converted to standard position angles internally, and only afterwards is the
+ * negative applied, so now it equals -90 internally
+ *
+ * This class solves this problem by introducing the CAngle type. You can do things like
+ * negate it, multiply it, etc. without messing up the angle. However, this class can
+ * only be created through string literals, you can't do something like
+ * CAngle angle = 2_cDeg
+ * because the constructor is private. However, you can do
+ * Angle angle = 2_cDeg
+ */
 class CAngle : public Quantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>, std::ratio<0>,
                                std::ratio<0>, std::ratio<0>> {
         // make string literals friends, so they have access to the constructor
@@ -43,8 +58,12 @@ class CAngle : public Quantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std:
         // make CAngle able to be implicitly converted to Angle
         constexpr operator Angle() const { return Angle(M_PI_2 - this->value); }
 
+        constexpr Angle operator-(Angle other) const { return Angle(*this) - other; }
+
         constexpr CAngle operator-() const { return CAngle(-this->value); }
 
+        constexpr Angle operator+(Angle other) const { return Angle(*this) + other; }
+
         constexpr CAngle operator+() const { return CAngle(this->value); }
     private:
         // only allow construction through literals
@@ -64,12 +83,27 @@ inline std::ostream& operator<<(std::ostream& os, const Angle& quantity) {
     return os;
 }
 
+constexpr Angle operator+(Angle lhs, CAngle rhs) { return lhs + Angle(rhs); }
+
+constexpr Angle operator-(Angle lhs, CAngle rhs) { return lhs - Angle(rhs); }
+
 constexpr CAngle operator*(double multiple, CAngle quantity) { return CAngle(multiple * quantity.internal()); }
 
 constexpr CAngle operator*(CAngle quantity, double multiple) { return CAngle(multiple * quantity.internal()); }
 
 constexpr CAngle operator/(CAngle quantity, double multiple) { return CAngle(quantity.internal() / multiple); }
 
+namespace units {
+template <typename T>
+concept isAngle = std::same_as<T, CAngle> || std::same_as<T, Angle> ||
+                  std::same_as<T, Quantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>,
+                                           std::ratio<0>, std::ratio<0>, std::ratio<0>>>;
+
+template <isAngle Q, isAngle R, isAngle S> constexpr Angle clamp(Q lhs, R lo, S hi) {
+    return Angle(std::clamp(lhs.internal(), lo.internal(), hi.internal()));
+}
+} // namespace units
+
 constexpr Angle rad = Angle(1.0);
 constexpr Angle deg = Angle(M_PI / 180);
 constexpr Angle rot = Angle(M_TWOPI);

diff --git a/src/main.cpp b/src/main.cpp
@@ -21,11 +21,9 @@ void initialize() {
                              std::ratio<0>, std::ratio<0>>(1.0);
     a.orientation += 2_rpm2;
     2_rpm2 -= a.orientation;
-    /*
     to_cDeg(Quantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>, std::ratio<0>,
                      std::ratio<0>, std::ratio<0>>(5.0) -
             a.theta() + 5_cDeg);
-    */
     Quantity<std::ratio<0>, std::ratio<0>, std::ratio<1>, std::ratio<0>, std::ratio<1>, std::ratio<0>, std::ratio<0>,
              std::ratio<0>>
         c = Multiplied<Angle, Time>();
@@ -34,11 +32,9 @@ void initialize() {
     Length z = toLinear<Angle>(y, 2_cm);
     static_assert(Angle(5.1) >= Quantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>,
                                          std::ratio<0>, std::ratio<0>, std::ratio<0>>(5.0));
-    /*
     units::clamp(2_cDeg, a.theta(),
                  Quantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>, std::ratio<0>,
                           std::ratio<0>, std::ratio<0>>(5.0));
-    */
     units::max(10_celsius, Quantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<0>,
                                     std::ratio<1>, std::ratio<0>, std::ratio<0>>(1.0));
     // check Vector3D overloads
@@ -61,4 +57,15 @@ void angleTests() {
     static_assert(r2i(to_stDeg(2 * 15_cDeg)) == r2i(to_stDeg(60_stDeg)));
     static_assert(r2i(to_stDeg(+0_cDeg)) == r2i(to_stDeg(90_stDeg)));
     static_assert(90_stDeg == +0_cDeg);
+    Angle a = 2_cDeg;
+    Angle b = 2_cDeg + 2_stDeg;
+    Angle c = 2_stDeg - 2_cDeg;
+    Angle d = 2_cDeg + Quantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>,
+                                std::ratio<0>, std::ratio<0>, std::ratio<0>>(5.0);
+    Angle e = Quantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>, std::ratio<0>,
+                       std::ratio<0>, std::ratio<0>>(5.0) +
+              2_cDeg;
+    units::clamp(2_cDeg, 2_stDeg,
+                 Quantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>, std::ratio<0>,
+                          std::ratio<0>, std::ratio<0>>(5.0));
 }