Added DiscreteAgle

DiscreteAngle.cpp

@@ -0,0 +1,26 @@
+// This Source Code Form is subject to the terms of the Mozilla Public
+// License, v. 2.0.If a copy of the MPL was not distributed with this
+// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
+
+#include "DiscreteAngle.h"
+
+#include <math.h>
+
+DiscreteAngle16::DiscreteAngle16(float angle) : Range16() {
+  if (!isfinite(angle)) {
+    range = 0;
+    return;
+  }
+
+  // clamp the float range to -1..1
+  while (angle <= -180.0F)
+    angle += 360.0F;
+  while (angle > 180.0F)
+    angle -= 360.0F;
+
+  // map (-180..180] to 1..65535
+  // This means that range value 0 is not used
+  this->range = (unsigned short)((angle + 180.0F) * 65535.0F);
+}
+
+DiscreteAngle16 &DiscreteAngle16::operator-() { -this->range; }

DiscreteAngle.h

@@ -0,0 +1,14 @@
+#ifndef DISCRETEANGLE_H
+#define DISCRETEANGLE_H
+
+#include "Range.h"
+
+// A fixed point 16-bit signed angle between (-180..180]
+class DiscreteAngle16 : public Range16 {
+public:
+  DiscreteAngle16(float angle);
+
+  inline DiscreteAngle16 &operator-() override;
+};
+
+#endif

Range.cpp

@@ -0,0 +1,39 @@
+#include "Range.h"
+
+Range16::Range16() { this->range = 0; }
+
+Range16::Range16(float f) {
+  // clamp the float range to -1..1
+  if (f < -1.0F)
+    f = -1.0F;
+  else if (f > 1.0F)
+    f = 1.0F;
+
+  // map -1..1 to 0..65535
+  this->range = (unsigned short)((f + 1.0F) * 65535.0F);
+}
+
+Range16::Range16(short s) {
+  // clamp the range to -32767..32767
+  // This is needed because the representation can support +/-32768 (like 2s
+  // complement)
+  if (s < -32767)
+    s = -32767;
+  else if (s > 32767)
+    s = 32767;
+
+  this->range = (unsigned short)(s + 32767);
+}
+
+Range16 Range16::operator-(Range16 a) { this->range - a.range; };
+Range16 Range16::operator+(Range16 a) { this->range + a.range; };
+
+Range16 &Range16::operator-() { -this->range; }
+
+bool Range16::operator==(Range16 a) { this->range == a.range; }
+bool Range16::operator!=(Range16 a) { return (this->range != a.range); }
+
+bool Range16::operator<(Range16 a) { return (this->range < a.range); }
+bool Range16::operator>(Range16 a) { return (this->range > a.range); }
+bool Range16::operator<=(Range16 a) { return (this->range <= a.range); }
+bool Range16::operator>=(Range16 a) { return (this->range >= a.range); }

Range.h

@@ -0,0 +1,29 @@
+#ifndef RANGE_H
+#define RANGE_H
+
+/// @brief Signed range. May be renamed to SignedRange later
+class Range16 {
+public:
+  Range16();
+  Range16(float range); // range -1..1
+  Range16(short range); // range -32768..32767
+
+  inline Range16 operator-(Range16 a);
+  inline Range16 operator+(Range16 a);
+  inline virtual Range16 &operator-();
+
+  inline bool operator==(Range16 a);
+  inline bool operator!=(Range16 a);
+
+  inline bool operator<(Range16 a);
+  inline bool operator>(Range16 a);
+  inline bool operator<=(Range16 a);
+  inline bool operator>=(Range16 a);
+
+protected:
+  // How do we make sure we have 16 bit range on every platform?
+  // uint16_t range; // 16-bit range
+  unsigned short range; // 16-bit range
+};
+
+#endif