CMakeLists.txt

@@ -5,9 +5,8 @@ if(ESP_PLATFORM)
         INCLUDE_DIRS "."
     )
 else() 
-    project(RoboidControl)
+    project(RoboidCOntrol)
     add_subdirectory(LinearAlgebra)
-    add_subdirectory(Examples)
 
     set(CMAKE_CXX_STANDARD 17)      # Enable c++11 standard
     set(CMAKE_POSITION_INDEPENDENT_CODE ON)
@@ -30,7 +29,7 @@ else()
     )
     file(GLOB srcs 
         *.cpp
-        Things/*.cpp
+        Sensors/*.cpp
         Messages/*.cpp
         Arduino/*.cpp
         Posix/*.cpp
@@ -52,6 +51,13 @@ else()
         LinearAlgebra
     )
 
+#     if(MSVC)
+#     target_compile_options(RoboidControlTest PRIVATE /W4 /WX)
+# #   else()
+# #     target_compile_options(RoboidControlTest PRIVATE -Wall -Wextra -Wpedantic -Werror)
+#   endif()
+  
+
     include(GoogleTest)
     gtest_discover_tests(RoboidControlTest)
 endif()

Examples/BB2B.cpp

@@ -1,38 +0,0 @@
-#include <iostream>
-
-#include "Thing.h"
-#include "Things/DifferentialDrive.h"
-#include "Things/TouchSensor.h"
-
-using namespace RoboidControl;
-
-void CollisionSteering();
-
-DifferentialDrive* bb2b = nullptr;
-TouchSensor* touchLeft = nullptr;
-TouchSensor* touchRight = nullptr;
-
-int main() {
-  bb2b = new DifferentialDrive();
-
-  touchLeft = new TouchSensor(bb2b);
-  touchRight = new TouchSensor(bb2b);
-
-  while (true) {
-    CollisionSteering();
-  }
-
-  return 0;
-}
-
-void CollisionSteering() {
-  if (touchLeft->touchedSomething)
-    bb2b->SetRightWheelVelocity(-0.5f);
-  else
-    bb2b->SetRightWheelVelocity(0.5f);
-
-  if (touchRight->touchedSomething)
-    bb2b->SetLeftWheelVelocity(-0.5f);
-  else
-    bb2b->SetLeftWheelVelocity(0.5f);
-}

Examples/CMakeLists.txt

@@ -1,25 +0,0 @@
-# examples/CMakeLists.txt
-
-# Specify the minimum CMake version
-cmake_minimum_required(VERSION 3.10)
-
-# Specify the path to the main project directory
-set(MAIN_PROJECT_DIR "${CMAKE_SOURCE_DIR}/..")
-
-# Set the project name
-project(Examples)
-
-include_directories(..)
-
-# Add the executable for the main project
-#add_executable(MainExecutable ${SOURCES})
-# Find the main project library (assuming it's defined in the root CMakeLists.txt)
-#find_package(RoboidControl REQUIRED)  # Replace MyLibrary with your actual library name
-
-# Add example executables
-add_executable(BB2B BB2B.cpp)
-target_link_libraries(
-    BB2B
-    RoboidControl
-    LinearAlgebra
-)

LinearAlgebra/Angle.cpp

@@ -3,15 +3,15 @@
 // file, You can obtain one at https ://mozilla.org/MPL/2.0/.
 
 #include "Angle.h"
-#include <math.h>
 #include "FloatSingle.h"
+#include <math.h>
 
-namespace LinearAlgebra {
+const float Rad2Deg = 57.29578F;
+const float Deg2Rad = 0.0174532924F;
 
 //===== AngleSingle, AngleOf<float>
 
-template <>
+template <> AngleOf<float> Passer::LinearAlgebra::AngleOf<float>::Degrees(float degrees) {
-AngleOf<float> AngleOf<float>::Degrees(float degrees) {
   if (isfinite(degrees)) {
     while (degrees < -180)
       degrees += 360;
@@ -22,8 +22,7 @@ AngleOf<float> AngleOf<float>::Degrees(float degrees) {
   return AngleOf<float>(degrees);
 }
 
-template <>
+template <> AngleOf<float> AngleOf<float>::Radians(float radians) {
-AngleOf<float> AngleOf<float>::Radians(float radians) {
   if (isfinite(radians)) {
     while (radians <= -pi)
       radians += 2 * pi;
@@ -34,13 +33,9 @@ AngleOf<float> AngleOf<float>::Radians(float radians) {
   return Binary(radians * Rad2Deg);
 }
 
-template <>
+template <> float AngleOf<float>::InDegrees() const { return this->value; }
-float AngleOf<float>::InDegrees() const {
-  return this->value;
-}
 
-template <>
+template <> float AngleOf<float>::InRadians() const {
-float AngleOf<float>::InRadians() const {
   return this->value * Deg2Rad;
 }
 
@@ -63,29 +58,25 @@ AngleOf<signed short> AngleOf<signed short>::Radians(float radians) {
   return Binary(value);
 }
 
-template <>
+template <> float AngleOf<signed short>::InDegrees() const {
-float AngleOf<signed short>::InDegrees() const {
   float degrees = this->value / 65536.0f * 360.0f;
   return degrees;
 }
 
-template <>
+template <> float AngleOf<signed short>::InRadians() const {
-float AngleOf<signed short>::InRadians() const {
   float radians = this->value / 65536.0f * (2 * pi);
   return radians;
 }
 
 //===== Angle8, AngleOf<signed char>
 
-template <>
+template <> AngleOf<signed char> AngleOf<signed char>::Degrees(float degrees) {
-AngleOf<signed char> AngleOf<signed char>::Degrees(float degrees) {
   // map float [-180..180) to integer [-128..127)
   signed char value = (signed char)roundf(degrees / 360.0F * 256.0F);
   return Binary(value);
 }
 
-template <>
+template <> AngleOf<signed char> AngleOf<signed char>::Radians(float radians) {
-AngleOf<signed char> AngleOf<signed char>::Radians(float radians) {
   if (!isfinite(radians))
     return AngleOf<signed char>::zero;
 
@@ -94,42 +85,32 @@ AngleOf<signed char> AngleOf<signed char>::Radians(float radians) {
   return Binary(value);
 }
 
-template <>
+template <> float AngleOf<signed char>::InDegrees() const {
-float AngleOf<signed char>::InDegrees() const {
   float degrees = this->value / 256.0f * 360.0f;
   return degrees;
 }
 
-template <>
+template <> float AngleOf<signed char>::InRadians() const {
-float AngleOf<signed char>::InRadians() const {
   float radians = this->value / 128.0f * pi;
   return radians;
 }
 
 //===== Generic
 
-template <typename T>
+template <typename T> AngleOf<T>::AngleOf() : value(0) {}
-AngleOf<T>::AngleOf() : value(0) {}
 
-template <typename T>
+template <typename T> AngleOf<T>::AngleOf(T rawValue) : value(rawValue) {}
-AngleOf<T>::AngleOf(T rawValue) : value(rawValue) {}
 
-template <typename T>
+template <typename T> const AngleOf<T> AngleOf<T>::zero = AngleOf<T>();
-const AngleOf<T> AngleOf<T>::zero = AngleOf<T>();
 
-template <typename T>
+template <typename T> AngleOf<T> AngleOf<T>::Binary(T rawValue) {
-AngleOf<T> AngleOf<T>::Binary(T rawValue) {
   AngleOf<T> angle = AngleOf<T>();
   angle.SetBinary(rawValue);
   return angle;
 }
 
-template <typename T>
+template <typename T> T AngleOf<T>::GetBinary() const { return this->value; }
-T AngleOf<T>::GetBinary() const {
+template <typename T> void AngleOf<T>::SetBinary(T rawValue) {
-  return this->value;
-}
-template <typename T>
-void AngleOf<T>::SetBinary(T rawValue) {
   this->value = rawValue;
 }
 
@@ -138,28 +119,24 @@ bool AngleOf<T>::operator==(const AngleOf<T> angle) const {
   return this->value == angle.value;
 }
 
-template <typename T>
+template <typename T> bool AngleOf<T>::operator>(AngleOf<T> angle) const {
-bool AngleOf<T>::operator>(AngleOf<T> angle) const {
   return this->value > angle.value;
 }
 
-template <typename T>
+template <typename T> bool AngleOf<T>::operator>=(AngleOf<T> angle) const {
-bool AngleOf<T>::operator>=(AngleOf<T> angle) const {
   return this->value >= angle.value;
 }
 
-template <typename T>
+template <typename T> bool AngleOf<T>::operator<(AngleOf<T> angle) const {
-bool AngleOf<T>::operator<(AngleOf<T> angle) const {
   return this->value < angle.value;
 }
 
-template <typename T>
+template <typename T> bool AngleOf<T>::operator<=(AngleOf<T> angle) const {
-bool AngleOf<T>::operator<=(AngleOf<T> angle) const {
   return this->value <= angle.value;
 }
 
 template <typename T>
-signed int AngleOf<T>::Sign(AngleOf<T> angle) {
+signed int Passer::LinearAlgebra::AngleOf<T>::Sign(AngleOf<T> angle) {
   if (angle.value < 0)
     return -1;
   if (angle.value > 0)
@@ -168,15 +145,14 @@ signed int AngleOf<T>::Sign(AngleOf<T> angle) {
 }
 
 template <typename T>
-AngleOf<T> AngleOf<T>::Abs(AngleOf<T> angle) {
+AngleOf<T> Passer::LinearAlgebra::AngleOf<T>::Abs(AngleOf<T> angle) {
   if (Sign(angle) < 0)
     return -angle;
   else
     return angle;
 }
 
-template <typename T>
+template <typename T> AngleOf<T> AngleOf<T>::operator-() const {
-AngleOf<T> AngleOf<T>::operator-() const {
   AngleOf<T> angle = Binary(-this->value);
   return angle;
 }
@@ -230,8 +206,7 @@ AngleOf<T> AngleOf<T>::operator+=(const AngleOf<T>& angle) {
 //   return AngleOf::Degrees((float)factor * angle.InDegrees());
 // }
 
-template <typename T>
+template <typename T> void AngleOf<T>::Normalize() {
-void AngleOf<T>::Normalize() {
   float angleValue = this->InDegrees();
   if (!isfinite(angleValue))
     return;
@@ -243,8 +218,7 @@ void AngleOf<T>::Normalize() {
   *this = AngleOf::Degrees(angleValue);
 }
 
-template <typename T>
+template <typename T> AngleOf<T> AngleOf<T>::Normalize(AngleOf<T> angle) {
-AngleOf<T> AngleOf<T>::Normalize(AngleOf<T> angle) {
   float angleValue = angle.InDegrees();
   if (!isfinite(angleValue))
     return angle;
@@ -263,8 +237,7 @@ AngleOf<T> AngleOf<T>::Clamp(AngleOf<T> angle, AngleOf<T> min, AngleOf<T> max) {
 }
 
 template <typename T>
-AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle,
+AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle, AngleOf<T> toAngle,
-                                   AngleOf<T> toAngle,
                                    float maxDegrees) {
   maxDegrees = fmaxf(0, maxDegrees); // filter out negative distances
   AngleOf<T> d = toAngle - fromAngle;
@@ -276,34 +249,28 @@ AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle,
   return fromAngle + d;
 }
 
-template <typename T>
+template <typename T> float AngleOf<T>::Cos(AngleOf<T> angle) {
-float AngleOf<T>::Cos(AngleOf<T> angle) {
   return cosf(angle.InRadians());
 }
-template <typename T>
+template <typename T> float AngleOf<T>::Sin(AngleOf<T> angle) {
-float AngleOf<T>::Sin(AngleOf<T> angle) {
   return sinf(angle.InRadians());
 }
-template <typename T>
+template <typename T> float AngleOf<T>::Tan(AngleOf<T> angle) {
-float AngleOf<T>::Tan(AngleOf<T> angle) {
   return tanf(angle.InRadians());
 }
 
-template <typename T>
+template <typename T> AngleOf<T> AngleOf<T>::Acos(float f) {
-AngleOf<T> AngleOf<T>::Acos(float f) {
   return AngleOf<T>::Radians(acosf(f));
 }
-template <typename T>
+template <typename T> AngleOf<T> AngleOf<T>::Asin(float f) {
-AngleOf<T> AngleOf<T>::Asin(float f) {
   return AngleOf<T>::Radians(asinf(f));
 }
-template <typename T>
+template <typename T> AngleOf<T> AngleOf<T>::Atan(float f) {
-AngleOf<T> AngleOf<T>::Atan(float f) {
   return AngleOf<T>::Radians(atanf(f));
 }
 
 template <typename T>
-AngleOf<T> AngleOf<T>::Atan2(float y, float x) {
+AngleOf<T> Passer::LinearAlgebra::AngleOf<T>::Atan2(float y, float x) {
   return AngleOf<T>::Radians(atan2f(y, x));
 }
 
@@ -387,8 +354,6 @@ AngleOf<T> AngleOf<T>::SineRuleAngle(float a, AngleOf<T> beta, float b) {
   return alpha;
 }
 
-template class AngleOf<float>;
+template class Passer::LinearAlgebra::AngleOf<float>;
-template class AngleOf<signed char>;
+template class Passer::LinearAlgebra::AngleOf<signed char>;
-template class AngleOf<signed short>;
+template class Passer::LinearAlgebra::AngleOf<signed short>;
-
-}  // namespace LinearAlgebra

LinearAlgebra/Angle.h

@@ -5,6 +5,7 @@
 #ifndef ANGLE_H
 #define ANGLE_H
 
+namespace Passer {
 namespace LinearAlgebra {
 
 static float pi = 3.1415927410125732421875F;
@@ -17,8 +18,7 @@ static float Deg2Rad = (pi * 2) / 360.0f;
 /// The angle is internally limited to (-180..180] degrees or (-PI...PI]
 /// radians. When an angle exceeds this range, it is normalized to a value
 /// within the range.
-template <typename T>
+template <typename T> class AngleOf {
-class AngleOf {
 public:
   /// @brief Create a new angle with a zero value
   AngleOf<T>();
@@ -150,8 +150,7 @@ class AngleOf {
   /// @param toAngle The angle to rotate towards
   /// @param maxAngle The maximum angle to rotate
   /// @return The rotated angle
-  static AngleOf<T> MoveTowards(AngleOf<T> fromAngle,
+  static AngleOf<T> MoveTowards(AngleOf<T> fromAngle, AngleOf<T> toAngle,
-                                AngleOf<T> toAngle,
                                 float maxAngle);
 
   /// @brief Calculates the cosine of an angle
@@ -216,12 +215,8 @@ using AngleSingle = AngleOf<float>;
 using Angle16 = AngleOf<signed short>;
 using Angle8 = AngleOf<signed char>;
 
-#if defined(ARDUINO)
-using Angle = Angle16;
-#else
-using Angle = AngleSingle;
-#endif
-
 } // namespace LinearAlgebra
+} // namespace Passer
+using namespace Passer::LinearAlgebra;
 
 #endif

LinearAlgebra/Direction.cpp

@@ -9,8 +9,7 @@
 
 #include <math.h>
 
-template <typename T>
+template <typename T> DirectionOf<T>::DirectionOf() {
-DirectionOf<T>::DirectionOf() {
   this->horizontal = AngleOf<T>();
   this->vertical = AngleOf<T>();
 }
@@ -42,7 +41,7 @@ const DirectionOf<T> DirectionOf<T>::right =
     DirectionOf<T>(AngleOf<T>::Degrees(90), AngleOf<T>());
 
 template <typename T>
-Vector3 DirectionOf<T>::ToVector3() const {
+Vector3 Passer::LinearAlgebra::DirectionOf<T>::ToVector3() const {
   Quaternion q = Quaternion::Euler(-this->vertical.InDegrees(),
                                    this->horizontal.InDegrees(), 0);
   Vector3 v = q * Vector3::forward;
@@ -50,12 +49,12 @@ Vector3 DirectionOf<T>::ToVector3() const {
 }
 
 template <typename T>
-DirectionOf<T> DirectionOf<T>::FromVector3(Vector3 vector) {
+DirectionOf<T>
+Passer::LinearAlgebra::DirectionOf<T>::FromVector3(Vector3 vector) {
   DirectionOf<T> d;
   d.horizontal = AngleOf<T>::Atan2(
       vector.Right(),
-      vector
+      vector.Forward()); // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
-          .Forward());  // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
   d.vertical =
       AngleOf<T>::Degrees(-90) -
       AngleOf<T>::Acos(
@@ -65,32 +64,34 @@ DirectionOf<T> DirectionOf<T>::FromVector3(Vector3 vector) {
 }
 
 template <typename T>
-DirectionOf<T> DirectionOf<T>::Degrees(float horizontal, float vertical) {
+DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::Degrees(float horizontal,
+                                                              float vertical) {
   return DirectionOf<T>(AngleOf<T>::Degrees(horizontal),
                         AngleOf<T>::Degrees(vertical));
 }
 
 template <typename T>
-DirectionOf<T> DirectionOf<T>::Radians(float horizontal, float vertical) {
+DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::Radians(float horizontal,
+                                                              float vertical) {
   return DirectionOf<T>(AngleOf<T>::Radians(horizontal),
                         AngleOf<T>::Radians(vertical));
 }
 
 template <typename T>
-bool DirectionOf<T>::operator==(const DirectionOf<T> direction) const {
+bool Passer::LinearAlgebra::DirectionOf<T>::operator==(
+    const DirectionOf<T> direction) const {
   return (this->horizontal == direction.horizontal) &&
          (this->vertical == direction.vertical);
 }
 
 template <typename T>
-DirectionOf<T> DirectionOf<T>::operator-() const {
+DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::operator-() const {
   DirectionOf<T> r = DirectionOf<T>(this->horizontal + AngleOf<T>::Degrees(180),
                                     -this->vertical);
   return r;
 }
 
-template <typename T>
+template <typename T> void DirectionOf<T>::Normalize() {
-void DirectionOf<T>::Normalize() {
   if (this->vertical > AngleOf<T>::Degrees(90) ||
       this->vertical < AngleOf<T>::Degrees(-90)) {
     this->horizontal += AngleOf<T>::Degrees(180);
@@ -98,5 +99,5 @@ void DirectionOf<T>::Normalize() {
   }
 }
 
-template class DirectionOf<float>;
+template class Passer::LinearAlgebra::DirectionOf<float>;
-template class DirectionOf<signed short>;
+template class Passer::LinearAlgebra::DirectionOf<signed short>;

LinearAlgebra/Direction.h

@@ -7,6 +7,7 @@
 
 #include "Angle.h"
 
+namespace Passer {
 namespace LinearAlgebra {
 
 struct Vector3;
@@ -21,8 +22,7 @@ struct Vector3;
 /// rotation has been applied.
 /// The angles are automatically normalized to stay within the abovenmentioned
 /// ranges.
-template <typename T>
+template <typename T> class DirectionOf {
-class DirectionOf {
 public:
   /// @brief horizontal angle, range= (-180..180]
   AngleOf<T> horizontal;
@@ -98,7 +98,7 @@ using Direction = DirectionSingle;
 #endif
 
 } // namespace LinearAlgebra
-
+} // namespace Passer
-using namespace LinearAlgebra;
+using namespace Passer::LinearAlgebra;
 
 #endif

LinearAlgebra/FloatSingle.h

@@ -5,6 +5,7 @@
 #ifndef FLOAT_H
 #define FLOAT_H
 
+namespace Passer {
 namespace LinearAlgebra {
 
 class Float {
@@ -16,7 +17,7 @@ class Float {
 };
 
 } // namespace LinearAlgebra
-
+} // namespace Passer
-using namespace LinearAlgebra;
+using namespace Passer::LinearAlgebra;
 
 #endif

LinearAlgebra/Matrix.h

@@ -3,11 +3,11 @@
 
 #include "Vector3.h"
 
+namespace Passer {
 namespace LinearAlgebra {
 
 /// @brief Single precision float matrix
-template <typename T>
+template <typename T> class MatrixOf {
-class MatrixOf {
 public:
   MatrixOf(unsigned int rows, unsigned int cols);
   MatrixOf(unsigned int rows, unsigned int cols, const T *source)
@@ -54,8 +54,7 @@ class MatrixOf {
     }
   }
 
-  static void Multiply(const MatrixOf<T>* m1,
+  static void Multiply(const MatrixOf<T> *m1, const MatrixOf<T> *m2,
-                       const MatrixOf<T>* m2,
                        MatrixOf<T> *r);
   void Multiply(const MatrixOf<T> *m, MatrixOf<T> *r) const {
     Multiply(this, m, r);
@@ -116,6 +115,7 @@ class MatrixOf {
 };
 
 } // namespace LinearAlgebra
-using namespace LinearAlgebra;
+} // namespace Passer
+using namespace Passer::LinearAlgebra;
 
 #endif

LinearAlgebra/Polar.cpp

@@ -3,13 +3,11 @@
 #include "Polar.h"
 #include "Vector2.h"
 
-template <typename T>
+template <typename T> PolarOf<T>::PolarOf() {
-PolarOf<T>::PolarOf() {
   this->distance = 0.0f;
   this->angle = AngleOf<T>();
 }
-template <typename T>
+template <typename T> PolarOf<T>::PolarOf(float distance, AngleOf<T> angle) {
-PolarOf<T>::PolarOf(float distance, AngleOf<T> angle) {
   // distance should always be 0 or greater
   if (distance < 0.0f) {
     this->distance = -distance;
@@ -36,18 +34,16 @@ PolarOf<T> PolarOf<T>::Radians(float distance, float radians) {
   return PolarOf<T>(distance, AngleOf<T>::Radians(radians));
 }
 
-template <typename T>
+template <typename T> PolarOf<T> PolarOf<T>::FromVector2(Vector2 v) {
-PolarOf<T> PolarOf<T>::FromVector2(Vector2 v) {
   float distance = v.magnitude();
   AngleOf<T> angle =
       AngleOf<T>::Degrees(Vector2::SignedAngle(Vector2::forward, v));
   PolarOf<T> p = PolarOf(distance, angle);
   return p;
 }
-template <typename T>
+template <typename T> PolarOf<T> PolarOf<T>::FromSpherical(SphericalOf<T> v) {
-PolarOf<T> PolarOf<T>::FromSpherical(SphericalOf<T> v) {
+  float distance = v.distance * cosf(v.direction.vertical.InDegrees() *
-  float distance =
+                                     Passer::LinearAlgebra::Deg2Rad);
-      v.distance * cosf(v.direction.vertical.InDegrees() * Deg2Rad);
   AngleOf<T> angle = v.direction.horizontal;
   PolarOf<T> p = PolarOf(distance, angle);
   return p;
@@ -64,37 +60,31 @@ const PolarOf<T> PolarOf<T>::right = PolarOf(1.0, AngleOf<T>::Degrees(90));
 template <typename T>
 const PolarOf<T> PolarOf<T>::left = PolarOf(1.0, AngleOf<T>::Degrees(-90));
 
-template <typename T>
+template <typename T> bool PolarOf<T>::operator==(const PolarOf &v) const {
-bool PolarOf<T>::operator==(const PolarOf& v) const {
   return (this->distance == v.distance &&
           this->angle.InDegrees() == v.angle.InDegrees());
 }
 
-template <typename T>
+template <typename T> PolarOf<T> PolarOf<T>::Normalize(const PolarOf &v) {
-PolarOf<T> PolarOf<T>::Normalize(const PolarOf& v) {
   PolarOf<T> r = PolarOf(1, v.angle);
   return r;
 }
-template <typename T>
+template <typename T> PolarOf<T> PolarOf<T>::normalized() const {
-PolarOf<T> PolarOf<T>::normalized() const {
   PolarOf<T> r = PolarOf(1, this->angle);
   return r;
 }
 
-template <typename T>
+template <typename T> PolarOf<T> PolarOf<T>::operator-() const {
-PolarOf<T> PolarOf<T>::operator-() const {
   PolarOf<T> v =
       PolarOf(this->distance, this->angle + AngleOf<T>::Degrees(180));
   return v;
 }
 
-template <typename T>
+template <typename T> PolarOf<T> PolarOf<T>::operator-(const PolarOf &v) const {
-PolarOf<T> PolarOf<T>::operator-(const PolarOf& v) const {
   PolarOf<T> r = -v;
   return *this + r;
 }
-template <typename T>
+template <typename T> PolarOf<T> PolarOf<T>::operator-=(const PolarOf &v) {
-PolarOf<T> PolarOf<T>::operator-=(const PolarOf& v) {
   *this = *this - v;
   return *this;
   // angle = AngleOf<T>::Normalize(newAngle);
@@ -115,8 +105,7 @@ PolarOf<T> PolarOf<T>::operator-=(const PolarOf& v) {
 //   return d;
 // }
 
-template <typename T>
+template <typename T> PolarOf<T> PolarOf<T>::operator+(const PolarOf &v) const {
-PolarOf<T> PolarOf<T>::operator+(const PolarOf& v) const {
   if (v.distance == 0)
     return PolarOf(this->distance, this->angle);
   if (this->distance == 0.0f)
@@ -144,19 +133,16 @@ PolarOf<T> PolarOf<T>::operator+(const PolarOf& v) const {
   PolarOf vector = PolarOf(newDistance, newAngleA);
   return vector;
 }
-template <typename T>
+template <typename T> PolarOf<T> PolarOf<T>::operator+=(const PolarOf &v) {
-PolarOf<T> PolarOf<T>::operator+=(const PolarOf& v) {
   *this = *this + v;
   return *this;
 }
 
-template <typename T>
+template <typename T> PolarOf<T> PolarOf<T>::operator*=(float f) {
-PolarOf<T> PolarOf<T>::operator*=(float f) {
   this->distance *= f;
   return *this;
 }
-template <typename T>
+template <typename T> PolarOf<T> PolarOf<T>::operator/=(float f) {
-PolarOf<T> PolarOf<T>::operator/=(float f) {
   this->distance /= f;
   return *this;
 }
@@ -175,5 +161,5 @@ PolarOf<T> PolarOf<T>::Rotate(const PolarOf& v, AngleOf<T> angle) {
   return r;
 }
 
-template class PolarOf<float>;
+template class Passer::LinearAlgebra::PolarOf<float>;
-template class PolarOf<signed short>;
+template class Passer::LinearAlgebra::PolarOf<signed short>;

LinearAlgebra/Polar.h

@@ -7,16 +7,15 @@
 
 #include "Angle.h"
 
+namespace Passer {
 namespace LinearAlgebra {
 
 struct Vector2;
-template <typename T>
+template <typename T> class SphericalOf;
-class SphericalOf;
 
 /// @brief A polar vector using an angle in various representations
 /// @tparam T The implementation type used for the representation of the angle
-template <typename T>
+template <typename T> class PolarOf {
-class PolarOf {
 public:
   /// @brief The distance in meters
   /// @remark The distance shall never be negative
@@ -154,7 +153,8 @@ using Polar16 = PolarOf<signed short>;
 // using Polar = PolarSingle;
 
 } // namespace LinearAlgebra
-using namespace LinearAlgebra;
+} // namespace Passer
+using namespace Passer::LinearAlgebra;
 
 #include "Spherical.h"
 #include "Vector2.h"

LinearAlgebra/Quaternion.h

@@ -32,6 +32,7 @@ typedef struct Quat {
 } Quat;
 }
 
+namespace Passer {
 namespace LinearAlgebra {
 
 /// <summary>
@@ -156,8 +157,7 @@ struct Quaternion : Quat {
   /// <param name="to">The destination rotation</param>
   /// <param name="maxDegreesDelta">The maximum amount of degrees to
   /// rotate</param> <returns>The possibly limited rotation</returns>
-  static Quaternion RotateTowards(const Quaternion& from,
+  static Quaternion RotateTowards(const Quaternion &from, const Quaternion &to,
-                                  const Quaternion& to,
                                   float maxDegreesDelta);
 
   /// <summary>
@@ -191,8 +191,7 @@ struct Quaternion : Quat {
   /// <returns>The resulting rotation</returns>
   /// A factor 0 returns rotation1, factor1 returns rotation2.
   static Quaternion Slerp(const Quaternion &rotation1,
-                          const Quaternion& rotation2,
+                          const Quaternion &rotation2, float factor);
-                          float factor);
   /// <summary>
   /// Unclamped sherical lerp between two rotations
   /// </summary>
@@ -203,8 +202,7 @@ struct Quaternion : Quat {
   /// A factor 0 returns rotation1, factor1 returns rotation2.
   /// Values outside the 0..1 range will result in extrapolated rotations
   static Quaternion SlerpUnclamped(const Quaternion &rotation1,
-                                   const Quaternion& rotation2,
+                                   const Quaternion &rotation2, float factor);
-                                   float factor);
 
   /// <summary>
   /// Create a rotation from euler angles
@@ -262,10 +260,8 @@ struct Quaternion : Quat {
   /// <param name="swing">A pointer to the quaternion for the swing
   /// result</param> <param name="twist">A pointer to the quaternion for the
   /// twist result</param>
-  static void GetSwingTwist(Vector3 axis,
+  static void GetSwingTwist(Vector3 axis, Quaternion rotation,
-                            Quaternion rotation,
+                            Quaternion *swing, Quaternion *twist);
-                            Quaternion* swing,
-                            Quaternion* twist);
 
   /// <summary>
   /// Calculate the dot product of two quaternions
@@ -288,6 +284,7 @@ struct Quaternion : Quat {
 };
 
 } // namespace LinearAlgebra
-using namespace LinearAlgebra;
+} // namespace Passer
+using namespace Passer::LinearAlgebra;
 
 #endif

LinearAlgebra/Spherical.cpp

@@ -5,15 +5,13 @@
 
 #include <math.h>
 
-template <typename T>
+template <typename T> SphericalOf<T>::SphericalOf() {
-SphericalOf<T>::SphericalOf() {
   this->distance = 0.0f;
   this->direction = DirectionOf<T>();
 }
 
 template <typename T>
-SphericalOf<T>::SphericalOf(float distance,
+SphericalOf<T>::SphericalOf(float distance, AngleOf<T> horizontal,
-                            AngleOf<T> horizontal,
                             AngleOf<T> vertical) {
   if (distance < 0) {
     this->distance = -distance;
@@ -36,8 +34,7 @@ SphericalOf<T>::SphericalOf(float distance, DirectionOf<T> direction) {
 }
 
 template <typename T>
-SphericalOf<T> SphericalOf<T>::Degrees(float distance,
+SphericalOf<T> SphericalOf<T>::Degrees(float distance, float horizontal,
-                                       float horizontal,
                                        float vertical) {
   AngleOf<T> horizontalAngle = AngleOf<T>::Degrees(horizontal);
   AngleOf<T> verticalAngle = AngleOf<T>::Degrees(vertical);
@@ -46,8 +43,7 @@ SphericalOf<T> SphericalOf<T>::Degrees(float distance,
 }
 
 template <typename T>
-SphericalOf<T> SphericalOf<T>::Radians(float distance,
+SphericalOf<T> SphericalOf<T>::Radians(float distance, float horizontal,
-                                       float horizontal,
                                        float vertical) {
   return SphericalOf<T>(distance, AngleOf<T>::Radians(horizontal),
                         AngleOf<T>::Radians(vertical));
@@ -61,8 +57,7 @@ SphericalOf<T> SphericalOf<T>::FromPolar(PolarOf<T> polar) {
   return r;
 }
 
-template <typename T>
+template <typename T> SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
-SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
   float distance = v.magnitude();
   if (distance == 0.0f) {
     return SphericalOf(distance, AngleOf<T>(), AngleOf<T>());
@@ -86,8 +81,7 @@ SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
  * @tparam T The type of the distance and direction values.
  * @return Vector3 The 3D vector representation of the spherical coordinates.
  */
-template <typename T>
+template <typename T> Vector3 SphericalOf<T>::ToVector3() const {
-Vector3 SphericalOf<T>::ToVector3() const {
   float verticalRad = (pi / 2) - this->direction.vertical.InRadians();
   float horizontalRad = this->direction.horizontal.InRadians();
 
@@ -132,8 +126,7 @@ SphericalOf<T> SphericalOf<T>::WithDistance(float distance) {
   return v;
 }
 
-template <typename T>
+template <typename T> SphericalOf<T> SphericalOf<T>::operator-() const {
-SphericalOf<T> SphericalOf<T>::operator-() const {
   SphericalOf<T> v = SphericalOf<T>(
       this->distance, this->direction.horizontal + AngleOf<T>::Degrees(180),
       this->direction.vertical + AngleOf<T>::Degrees(180));
@@ -216,14 +209,12 @@ SphericalOf<T> SphericalOf<T>::operator+=(const SphericalOf<T>& v) {
   return *this;
 }
 
-template <typename T>
+template <typename T> SphericalOf<T> SphericalOf<T>::operator*=(float f) {
-SphericalOf<T> SphericalOf<T>::operator*=(float f) {
   this->distance *= f;
   return *this;
 }
 
-template <typename T>
+template <typename T> SphericalOf<T> SphericalOf<T>::operator/=(float f) {
-SphericalOf<T> SphericalOf<T>::operator/=(float f) {
   this->distance /= f;
   return *this;
 }
@@ -265,8 +256,8 @@ AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf& v1,
 }
 
 template <typename T>
-AngleOf<T> SphericalOf<T>::SignedAngleBetween(const SphericalOf<T>& v1,
+AngleOf<T> Passer::LinearAlgebra::SphericalOf<T>::SignedAngleBetween(
-                                              const SphericalOf<T>& v2,
+    const SphericalOf<T> &v1, const SphericalOf<T> &v2,
     const SphericalOf<T> &axis) {
   Vector3 v1_vector = v1.ToVector3();
   Vector3 v2_vector = v2.ToVector3();
@@ -299,5 +290,5 @@ SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T>& v,
   return r;
 }
 
-template class SphericalOf<float>;
+template class Passer::LinearAlgebra::SphericalOf<float>;
-template class SphericalOf<signed short>;
+template class Passer::LinearAlgebra::SphericalOf<signed short>;

LinearAlgebra/Spherical.h

@@ -7,16 +7,15 @@
 
 #include "Direction.h"
 
+namespace Passer {
 namespace LinearAlgebra {
 
 struct Vector3;
-template <typename T>
+template <typename T> class PolarOf;
-class PolarOf;
 
 /// @brief A spherical vector using angles in various representations
 /// @tparam T The implementation type used for the representations of the agles
-template <typename T>
+template <typename T> class SphericalOf {
-class SphericalOf {
 public:
   /// @brief The distance in meters
   /// @remark The distance should never be negative
@@ -39,8 +38,7 @@ class SphericalOf {
   /// @param horizontal The horizontal angle in degrees
   /// @param vertical The vertical angle in degrees
   /// @return The spherical vector
-  static SphericalOf<T> Degrees(float distance,
+  static SphericalOf<T> Degrees(float distance, float horizontal,
-                                float horizontal,
                                 float vertical);
   /// @brief Short-hand Deg alias for the Degrees function
   constexpr static auto Deg = Degrees;
@@ -50,8 +48,7 @@ class SphericalOf {
   /// @param horizontal The horizontal angle in radians
   /// @param vertical The vertical angle in radians
   /// @return The spherical vectpr
-  static SphericalOf<T> Radians(float distance,
+  static SphericalOf<T> Radians(float distance, float horizontal,
-                                float horizontal,
                                 float vertical);
   // Short-hand Rad alias for the Radians function
   constexpr static auto Rad = Radians;
@@ -157,8 +154,7 @@ class SphericalOf {
   /// @param horizontalAngle The horizontal rotation angle in local space
   /// @param verticalAngle The vertical rotation angle in local space
   /// @return The rotated vector
-  static SphericalOf<T> Rotate(const SphericalOf& v,
+  static SphericalOf<T> Rotate(const SphericalOf &v, AngleOf<T> horizontalAngle,
-                               AngleOf<T> horizontalAngle,
                                AngleOf<T> verticalAngle);
   /// @brief Rotate a spherical vector horizontally
   /// @param v The vector to rotate
@@ -191,7 +187,8 @@ using Spherical = SphericalSingle;
 #endif
 
 } // namespace LinearAlgebra
-using namespace LinearAlgebra;
+} // namespace Passer
+using namespace Passer::LinearAlgebra;
 
 #include "Polar.h"
 #include "Vector3.h"

LinearAlgebra/SwingTwist.cpp

@@ -164,5 +164,5 @@ void SwingTwistOf<T>::Normalize() {
   }
 }
 
-template class SwingTwistOf<float>;
+template class Passer::LinearAlgebra::SwingTwistOf<float>;
-template class SwingTwistOf<signed short>;
+template class Passer::LinearAlgebra::SwingTwistOf<signed short>;

LinearAlgebra/SwingTwist.h

@@ -10,13 +10,13 @@
 #include "Quaternion.h"
 #include "Spherical.h"
 
+namespace Passer {
 namespace LinearAlgebra {
 
 /// @brief An orientation using swing and twist angles in various
 /// representations
 /// @tparam T The implmentation type used for the representation of the angles
-template <typename T>
+template <typename T> class SwingTwistOf {
-class SwingTwistOf {
 public:
   DirectionOf<T> swing;
   AngleOf<T> twist;
@@ -25,8 +25,7 @@ class SwingTwistOf {
   SwingTwistOf<T>(DirectionOf<T> swing, AngleOf<T> twist);
   SwingTwistOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical, AngleOf<T> twist);
 
-  static SwingTwistOf<T> Degrees(float horizontal,
+  static SwingTwistOf<T> Degrees(float horizontal, float vertical = 0,
-                                 float vertical = 0,
                                  float twist = 0);
 
   Quaternion ToQuaternion() const;
@@ -73,13 +72,8 @@ class SwingTwistOf {
 using SwingTwistSingle = SwingTwistOf<float>;
 using SwingTwist16 = SwingTwistOf<signed short>;
 
-#if defined(ARDUINO)
-using SwingTwist = SwingTwist16;
-#else
-using SwingTwist = SwingTwistSingle;
-#endif
-
 } // namespace LinearAlgebra
-using namespace LinearAlgebra;
+} // namespace Passer
+using namespace Passer::LinearAlgebra;
 
 #endif

LinearAlgebra/Vector2.cpp

@@ -30,7 +30,7 @@ Vector2::Vector2(Vector3 v) {
   y = v.Forward(); // z;
 }
 Vector2::Vector2(PolarSingle p) {
-  float horizontalRad = p.angle.InDegrees() * Deg2Rad;
+  float horizontalRad = p.angle.InDegrees() * Passer::LinearAlgebra::Deg2Rad;
   float cosHorizontal = cosf(horizontalRad);
   float sinHorizontal = sinf(horizontalRad);
 
@@ -56,15 +56,9 @@ bool Vector2::operator==(const Vector2& v) {
 float Vector2::Magnitude(const Vector2 &v) {
   return sqrtf(v.x * v.x + v.y * v.y);
 }
-float Vector2::magnitude() const {
+float Vector2::magnitude() const { return (float)sqrtf(x * x + y * y); }
-  return (float)sqrtf(x * x + y * y);
+float Vector2::SqrMagnitude(const Vector2 &v) { return v.x * v.x + v.y * v.y; }
-}
+float Vector2::sqrMagnitude() const { return (x * x + y * y); }
-float Vector2::SqrMagnitude(const Vector2& v) {
-  return v.x * v.x + v.y * v.y;
-}
-float Vector2::sqrMagnitude() const {
-  return (x * x + y * y);
-}
 
 Vector2 Vector2::Normalize(const Vector2 &v) {
   float num = Vector2::Magnitude(v);
@@ -83,9 +77,7 @@ Vector2 Vector2::normalized() const {
   return result;
 }
 
-Vector2 Vector2::operator-() {
+Vector2 Vector2::operator-() { return Vector2(-this->x, -this->y); }
-  return Vector2(-this->x, -this->y);
-}
 
 Vector2 Vector2::operator-(const Vector2 &v) const {
   return Vector2(this->x - v.x, this->y - v.y);
@@ -156,11 +148,12 @@ float Vector2::SignedAngle(const Vector2& v1, const Vector2& v2) {
 
   float angleFrom = atan2f(v1.y, v1.x);
   float angleTo = atan2f(v2.y, v2.x);
-  return -(angleTo - angleFrom) * Rad2Deg;
+  return -(angleTo - angleFrom) * Passer::LinearAlgebra::Rad2Deg;
 }
 
-Vector2 Vector2::Rotate(const Vector2& v, AngleSingle a) {
+Vector2 Vector2::Rotate(const Vector2 &v,
-  float angleRad = a.InDegrees() * Deg2Rad;
+                        Passer::LinearAlgebra::AngleSingle a) {
+  float angleRad = a.InDegrees() * Passer::LinearAlgebra::Deg2Rad;
 #if defined(AVR)
   float sinValue = sin(angleRad);
   float cosValue = cos(angleRad); // * Angle::Deg2Rad);

LinearAlgebra/Vector2.h

@@ -26,11 +26,11 @@ typedef struct Vec2 {
 } Vec2;
 }
 
+namespace Passer {
 namespace LinearAlgebra {
 
 struct Vector3;
-template <typename T>
+template <typename T> class PolarOf;
-class PolarOf;
 
 /// @brief A 2-dimensional vector
 /// @remark This uses the right=handed carthesian coordinate system.
@@ -188,7 +188,7 @@ struct Vector2 : Vec2 {
   /// @param v The vector to rotate
   /// @param a The angle in degrees to rotate
   /// @return The rotated vector
-  static Vector2 Rotate(const Vector2& v, AngleSingle a);
+  static Vector2 Rotate(const Vector2 &v, Passer::LinearAlgebra::AngleSingle a);
 
   /// @brief Lerp (linear interpolation) between two vectors
   /// @param v1 The starting vector
@@ -202,7 +202,8 @@ struct Vector2 : Vec2 {
 };
 
 } // namespace LinearAlgebra
-using namespace LinearAlgebra;
+} // namespace Passer
+using namespace Passer::LinearAlgebra;
 
 #include "Polar.h"
 

LinearAlgebra/Vector3.cpp

@@ -31,8 +31,10 @@ Vector3::Vector3(Vector2 v) {
 }
 
 Vector3::Vector3(SphericalOf<float> s) {
-  float verticalRad = (90.0f - s.direction.vertical.InDegrees()) * Deg2Rad;
+  float verticalRad = (90.0f - s.direction.vertical.InDegrees()) *
-  float horizontalRad = s.direction.horizontal.InDegrees() * Deg2Rad;
+                      Passer::LinearAlgebra::Deg2Rad;
+  float horizontalRad =
+      s.direction.horizontal.InDegrees() * Passer::LinearAlgebra::Deg2Rad;
   float cosVertical = cosf(verticalRad);
   float sinVertical = sinf(verticalRad);
   float cosHorizontal = cosf(horizontalRad);
@@ -68,16 +70,12 @@ const Vector3 Vector3::back = Vector3(0, 0, -1);
 float Vector3::Magnitude(const Vector3 &v) {
   return sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
 }
-float Vector3::magnitude() const {
+float Vector3::magnitude() const { return (float)sqrtf(x * x + y * y + z * z); }
-  return (float)sqrtf(x * x + y * y + z * z);
-}
 
 float Vector3::SqrMagnitude(const Vector3 &v) {
   return v.x * v.x + v.y * v.y + v.z * v.z;
 }
-float Vector3::sqrMagnitude() const {
+float Vector3::sqrMagnitude() const { return (x * x + y * y + z * z); }
-  return (x * x + y * y + z * z);
-}
 
 Vector3 Vector3::Normalize(const Vector3 &v) {
   float num = Vector3::Magnitude(v);
@@ -202,8 +200,7 @@ AngleOf<float> Vector3::Angle(const Vector3& v1, const Vector3& v2) {
   return AngleOf<float>::Radians(r);
 }
 
-AngleOf<float> Vector3::SignedAngle(const Vector3& v1,
+AngleOf<float> Vector3::SignedAngle(const Vector3 &v1, const Vector3 &v2,
-                                    const Vector3& v2,
                                     const Vector3 &axis) {
   // angle in [0,180]
   AngleOf<float> angle = Vector3::Angle(v1, v2);

LinearAlgebra/Vector3.h

@@ -31,10 +31,10 @@ typedef struct Vec3 {
 } Vec3;
 }
 
+namespace Passer {
 namespace LinearAlgebra {
 
-template <typename T>
+template <typename T> class SphericalOf;
-class SphericalOf;
 
 /// @brief A 3-dimensional vector
 /// @remark This uses a right-handed carthesian coordinate system.
@@ -210,8 +210,7 @@ struct Vector3 : Vec3 {
   /// @param v2 The ending vector
   /// @param axis The axis to rotate around
   /// @return The signed angle between the two vectors
-  static AngleOf<float> SignedAngle(const Vector3& v1,
+  static AngleOf<float> SignedAngle(const Vector3 &v1, const Vector3 &v2,
-                                    const Vector3& v2,
                                     const Vector3 &axis);
 
   /// @brief Lerp (linear interpolation) between two vectors
@@ -226,7 +225,8 @@ struct Vector3 : Vec3 {
 };
 
 } // namespace LinearAlgebra
-using namespace LinearAlgebra;
+} // namespace Passer
+using namespace Passer::LinearAlgebra;
 
 #include "Spherical.h"
 

LinearAlgebra/test/Angle16_test.cc

@@ -1,13 +1,11 @@
 #if GTEST
 #include "gtest/gtest.h"
 
-#include <math.h>
 #include <limits>
+#include <math.h>
 
 #include "Angle.h"
 
-using namespace LinearAlgebra;
-
 #define FLOAT_INFINITY std::numeric_limits<float>::infinity()
 
 TEST(Angle16, Construct) {

LinearAlgebra/test/Angle8_test.cc

@@ -1,13 +1,11 @@
 #if GTEST
 #include <gtest/gtest.h>
 
-#include <math.h>
 #include <limits>
+#include <math.h>
 
 #include "Angle.h"
 
-using namespace LinearAlgebra;
-
 #define FLOAT_INFINITY std::numeric_limits<float>::infinity()
 
 TEST(Angle8, Construct) {

LinearAlgebra/test/AngleSingle_test.cc

@@ -1,13 +1,11 @@
 #if GTEST
 #include <gtest/gtest.h>
 
-#include <math.h>
 #include <limits>
+#include <math.h>
 
 #include "Angle.h"
 
-using namespace LinearAlgebra;
-
 #define FLOAT_INFINITY std::numeric_limits<float>::infinity()
 
 TEST(AngleSingle, Construct) {

Messages/LowLevelMessages.cpp

@@ -1,18 +1,15 @@
 #include "LowLevelMessages.h"
 
-#include <iostream>
 #include "LinearAlgebra/float16.h"
+#include <iostream>
 
 namespace RoboidControl {
 
-void LowLevelMessages::SendAngle8(char* buffer,
+void LowLevelMessages::SendAngle8(char* buffer, unsigned char* ix, const float angle) {
-                                  unsigned char* ix,
-                                  const float angle) {
   Angle8 packedAngle2 = Angle8::Degrees(angle);
   buffer[(*ix)++] = packedAngle2.GetBinary();
 }
-Angle8 LowLevelMessages::ReceiveAngle8(const char* buffer,
+Angle8 LowLevelMessages::ReceiveAngle8(const char* buffer, unsigned char* startIndex) {
-                                       unsigned char* startIndex) {
   unsigned char binary = buffer[(*startIndex)++];
 
   Angle8 angle = Angle8::Binary(binary);
@@ -20,17 +17,14 @@ Angle8 LowLevelMessages::ReceiveAngle8(const char* buffer,
   return angle;
 }
 
-void LowLevelMessages::SendFloat16(char* buffer,
+void LowLevelMessages::SendFloat16(char* buffer, unsigned char* ix, float value) {
-                                   unsigned char* ix,
-                                   float value) {
   float16 value16 = float16(value);
   short binary = value16.getBinary();
 
   buffer[(*ix)++] = (binary >> 8) & 0xFF;
   buffer[(*ix)++] = binary & 0xFF;
 }
-float LowLevelMessages::ReceiveFloat16(const char* buffer,
+float LowLevelMessages::ReceiveFloat16(const char* buffer, unsigned char* startIndex) {
-                                       unsigned char* startIndex) {
   unsigned char ix = *startIndex;
   unsigned char msb = buffer[ix++];
   unsigned char lsb = buffer[ix++];
@@ -42,30 +36,25 @@ float LowLevelMessages::ReceiveFloat16(const char* buffer,
   return (float)f.toFloat();
 }
 
-void LowLevelMessages::SendSpherical(char* buffer,
+void LowLevelMessages::SendSpherical16(char* buffer, unsigned char* ix, Spherical16 s) {
-                                     unsigned char* ix,
-                                     Spherical s) {
   SendFloat16(buffer, ix, s.distance);
   SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
   SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
 }
-Spherical LowLevelMessages::ReceiveSpherical(const char* buffer,
+Spherical16 LowLevelMessages::ReceiveSpherical16(const char* buffer, unsigned char* startIndex) {
-                                             unsigned char* startIndex) {
   float distance = ReceiveFloat16(buffer, startIndex);
 
   Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
-  Angle horizontal = Angle::Radians(horizontal8.InRadians());
+  Angle16 horizontal = Angle16::Binary(horizontal8.GetBinary() * 256);
 
   Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
-  Angle vertical = Angle::Radians(vertical8.InRadians());
+  Angle16 vertical = Angle16::Binary(vertical8.GetBinary() * 256);
 
-  Spherical s = Spherical(distance, horizontal, vertical);
+  Spherical16 s = Spherical16(distance, horizontal, vertical);
   return s;
 }
 
-void LowLevelMessages::SendQuat32(char* buffer,
+void LowLevelMessages::SendQuat32(char* buffer, unsigned char* ix, SwingTwist16 rotation) {
-                                  unsigned char* ix,
-                                  SwingTwist rotation) {
   Quaternion q = rotation.ToQuaternion();
   unsigned char qx = (char)(q.x * 127 + 128);
   unsigned char qy = (char)(q.y * 127 + 128);
@@ -77,22 +66,20 @@ void LowLevelMessages::SendQuat32(char* buffer,
     qz = -qz;
     qw = -qw;
   }
-  // std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw
+  // std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw << "\n";
-  // << "\n";
   buffer[(*ix)++] = qx;
   buffer[(*ix)++] = qy;
   buffer[(*ix)++] = qz;
   buffer[(*ix)++] = qw;
 }
 
-SwingTwist LowLevelMessages::ReceiveQuat32(const char* buffer,
+SwingTwist16 LowLevelMessages::ReceiveQuat32(const char* buffer, unsigned char* ix) {
-                                           unsigned char* ix) {
   float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
   float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
   float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;
   float qw = buffer[(*ix)++] / 255.0F;
   Quaternion q = Quaternion(qx, qy, qz, qw);
-  SwingTwist s = SwingTwist::FromQuaternion(q);
+  SwingTwist16 s = SwingTwist16::FromQuaternion(q);
   return s;
 }
 

Messages/LowLevelMessages.h

@@ -11,12 +11,11 @@ class LowLevelMessages {
   static void SendFloat16(char* buffer, unsigned char* ix, float value);
   static float ReceiveFloat16(const char* buffer, unsigned char* startIndex);
 
-  static void SendSpherical(char* buffer, unsigned char* ix, Spherical s);
+  static void SendSpherical16(char* buffer, unsigned char* ix, Spherical16 s);
-  static Spherical ReceiveSpherical(const char* buffer,
+  static Spherical16 ReceiveSpherical16(const char* buffer, unsigned char* startIndex);
-                                    unsigned char* startIndex);
 
-  static void SendQuat32(char* buffer, unsigned char* ix, SwingTwist q);
+  static void SendQuat32(char* buffer, unsigned char* ix, SwingTwist16 q);
-  static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix);
+  static SwingTwist16 ReceiveQuat32(const char* buffer, unsigned char* ix);
 };
 
 }  // namespace RoboidControl

Messages/PoseMsg.cpp

@@ -3,6 +3,22 @@
 
 namespace RoboidControl {
 
+// PoseMsg::PoseMsg(unsigned char networkId,
+//                  unsigned char thingId,
+//                  unsigned char poseType,
+//                  Spherical16 position,
+//                  SwingTwist16 orientation,
+//                  Spherical16 linearVelocity,
+//                  Spherical16 angularVelocity) {
+//   this->networkId = networkId;
+//   this->thingId = thingId;
+
+//   this->poseType = poseType;
+//   this->position = position;
+//   this->orientation = orientation;
+//   this->linearVelocity = linearVelocity;
+//   this->angularVelocity = angularVelocity;
+// }
 PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) {
   this->networkId = networkId;
   this->thingId = thing->id;
@@ -35,7 +51,7 @@ PoseMsg::PoseMsg(const char* buffer) {
   this->networkId = buffer[ix++];
   this->thingId = buffer[ix++];
   this->poseType = buffer[ix++];
-  this->position = LowLevelMessages::ReceiveSpherical(buffer, &ix);
+  this->position = LowLevelMessages::ReceiveSpherical16(buffer, &ix);
   this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
   // linearVelocity
   // angularVelocity
@@ -53,13 +69,13 @@ unsigned char PoseMsg::Serialize(char* buffer) {
   buffer[ix++] = this->thingId;
   buffer[ix++] = this->poseType;
   if ((this->poseType & Pose_Position) != 0)
-    LowLevelMessages::SendSpherical(buffer, &ix, this->position);
+    LowLevelMessages::SendSpherical16(buffer, &ix, this->position);
   if ((this->poseType & Pose_Orientation) != 0)
     LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
   if ((this->poseType & Pose_LinearVelocity) != 0)
-    LowLevelMessages::SendSpherical(buffer, &ix, this->linearVelocity);
+    LowLevelMessages::SendSpherical16(buffer, &ix, this->linearVelocity);
   if ((this->poseType & Pose_AngularVelocity) != 0)
-    LowLevelMessages::SendSpherical(buffer, &ix, this->angularVelocity);
+    LowLevelMessages::SendSpherical16(buffer, &ix, this->angularVelocity);
   return ix;
 }
 

Messages/PoseMsg.h

@@ -3,8 +3,8 @@
 namespace RoboidControl {
 
 /// @brief Message to communicate the pose of the thing
-/// The pose is in local space relative to the parent. If there is not parent
+/// The pose is in local space relative to the parent. If there is not parent (the thing is a root thing), the pose will
-/// (the thing is a root thing), the pose will be in world space.
+/// be in world space.
 class PoseMsg : public IMessage {
  public:
   /// @brief The message ID
@@ -29,14 +29,29 @@ class PoseMsg : public IMessage {
   static const unsigned char Pose_AngularVelocity = 0x08;
 
   /// @brief The position of the thing in local space in meters
-  Spherical position;
+  Spherical16 position;
   /// @brief The orientation of the thing in local space
-  SwingTwist orientation;
+  SwingTwist16 orientation;
-  /// @brief The linear velocity of the thing in local space in meters per
+  /// @brief The linear velocity of the thing in local space in meters per second
-  /// second
+  Spherical16 linearVelocity;
-  Spherical linearVelocity;
   /// @brief The angular velocity of the thing in local space
-  Spherical angularVelocity;
+  Spherical16 angularVelocity;
+
+  /// @brief Create a new message for sending
+  /// @param networkId The network ID of the thing
+  /// @param thingId The ID of the thing
+  /// @param poseType Bit pattern stating which pose components are available
+  /// @param position The position of the thing in local space in meters
+  /// @param orientation The orientation of the thing in local space
+  /// @param linearVelocity The linear velocity of the thing in local space in meters per second
+  /// @param angularVelocity The angular velocity of the thing in local space
+//   PoseMsg(unsigned char networkId,
+//           unsigned char thingId,
+//           unsigned char poseType,
+//           Spherical16 position,
+//           SwingTwist16 orientation,
+//           Spherical16 linearVelocity = Spherical16(),
+//           Spherical16 angularVelocity = Spherical16());
 
   /// @brief Create a new message for sending
   /// @param networkId he network ID of the thing

Participant.cpp

@@ -8,12 +8,11 @@ Participant::Participant() {}
 
 Participant::Participant(const char* ipAddress, int port) {
   // make a copy of the ip address string
-  int addressLength = (int)strlen(ipAddress);
+  int addressLength = strlen(ipAddress);
   int stringLength = addressLength + 1;
   char* addressString = new char[stringLength];
 #if defined(_WIN32) || defined(_WIN64)
-  strncpy_s(addressString, stringLength, ipAddress,
+  strncpy_s(addressString, stringLength, ipAddress, addressLength);  // Leave space for null terminator
-            addressLength);  // Leave space for null terminator
 #else
   strncpy(addressString, ipAddress, addressLength);
 #endif
@@ -33,8 +32,7 @@ Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
     if (thing->id == thingId)
       return thing;
   }
-  //   std::cout << "Could not find thing " << this->ipAddress << ":" <<
+  //   std::cout << "Could not find thing " << this->ipAddress << ":" << this->port
-  //   this->port
   //             << "[" << (int)networkId << "/" << (int)thingId << "]\n";
   return nullptr;
 }
@@ -44,36 +42,32 @@ void Participant::Add(Thing* thing, bool checkId) {
     // allocate a new thing ID
     thing->id = (unsigned char)this->things.size() + 1;
     this->things.push_back(thing);
-    // std::cout << "Add thing with generated ID " << this->ipAddress << ":" <<
+    // std::cout << "Add thing with generated ID " << this->ipAddress << ":" << this->port << "[" <<
-    // this->port << "[" << (int)thing->networkId << "/"
+    // (int)thing->networkId << "/"
     //           << (int)thing->id << "]\n";
   } else {
     Thing* foundThing = Get(thing->networkId, thing->id);
     if (foundThing == nullptr) {
       this->things.push_back(thing);
-      // std::cout << "Add thing " << this->ipAddress << ":" << this->port <<
+      // std::cout << "Add thing " << this->ipAddress << ":" << this->port << "[" << (int)thing->networkId << "/"
-      // "[" << (int)thing->networkId << "/"
       //           << (int)thing->id << "]\n";
     }
     // else
-    //   std::cout << "Did not add, existing thing " << this->ipAddress << ":"
+    //   std::cout << "Did not add, existing thing " << this->ipAddress << ":" << this->port << "["
-    //   << this->port << "["
     //             << (int)thing->networkId << "/" << (int)thing->id << "]\n";
   }
 }
 
 void Participant::Remove(Thing* thing) {
   this->things.remove_if([thing](Thing* obj) { return obj == thing; });
-  std::cout << "Removing " << thing->networkId << "/" << thing->id
+  std::cout << "Removing " << thing->networkId << "/" << thing->id << " list size = " << this->things.size() << "\n";
-            << " list size = " << this->things.size() << "\n";
 }
 
 // void Participant::UpdateAll(unsigned long currentTimeMs) {
 //   // Not very efficient, but it works for now.
 
 //   for (Thing* thing : this->things) {
-//     if (thing != nullptr && thing->GetParent() == nullptr) {  // update all
+//     if (thing != nullptr && thing->GetParent() == nullptr) {  // update all root things
-//     root things
 //       // std::cout << " update " << (int)ix << " thingid " << (int)thing->id
 //       //           << "\n";
 //       thing->Update(currentTimeMs);

Thing.cpp

@@ -2,24 +2,25 @@
 
 #include <string.h>
 #include <algorithm>
-#include <chrono>
 #include <iostream>
 #include <list>
+#include <chrono>
 
 #include "LocalParticipant.h"
 
 namespace RoboidControl {
 
+
 // LocalParticipant* Thing::CheckHiddenParticipant() {
 //   if (isolatedParticipant == nullptr)
 //     isolatedParticipant = new LocalParticipant(0);
 //   return isolatedParticipant;
 // }
 
-Thing::Thing(int thingType) : Thing(LocalParticipant::Isolated(), thingType) {}
+Thing::Thing(int thingType) : Thing(LocalParticipant::Isolated(), thingType) {
+}
 
-Thing::Thing(Participant* owner, Type thingType)
+Thing::Thing(Participant* owner, Type thingType) : Thing(owner, (unsigned char)thingType) {}
-    : Thing(owner, (unsigned char)thingType) {}
 
 Thing::Thing(Participant* owner, int thingType) {
   this->owner = owner;
@@ -27,30 +28,26 @@ Thing::Thing(Participant* owner, int thingType) {
   this->type = thingType;
   this->networkId = 0;
 
-  this->position = Spherical::zero;
+  this->position = Spherical16::zero;
-  this->orientation = SwingTwist::identity;
+  this->orientation = SwingTwist16::identity;
 
-  this->linearVelocity = Spherical::zero;
+  this->linearVelocity = Spherical16::zero;
-  this->angularVelocity = Spherical::zero;
+  this->angularVelocity = Spherical16::zero;
 
   // std::cout << "add thing to participant\n";
   owner->Add(this);
 }
 
-Thing::Thing(Participant* owner,
+Thing::Thing(Participant* owner, unsigned char networkId, unsigned char thingId, Type thingType) {
-             unsigned char networkId,
-             unsigned char thingId,
-             Type thingType) {
   // no participant reference yet..
   this->owner = owner;
   this->networkId = networkId;
   this->id = thingId;
   this->type = (unsigned char)thingType;
 
-  this->linearVelocity = Spherical::zero;
+  this->linearVelocity = Spherical16::zero;
-  this->angularVelocity = Spherical::zero;
+  this->angularVelocity = Spherical16::zero;
-  // std::cout << "Created thing " << (int)this->networkId << "/" <<
+  // std::cout << "Created thing " << (int)this->networkId << "/" << (int)this->id
-  // (int)this->id
   //           << "\n";
   owner->Add(this, false);
 }
@@ -189,6 +186,7 @@ void Thing::Update(unsigned long currentTimeMs) {
   // PoseMsg* poseMsg = new PoseMsg(this->networkId, this);
   // participant->Send(remoteParticipant, poseMsg);
   // delete poseMsg;
+
 }
 
 void Thing::UpdateThings(unsigned long currentTimeMs) {
@@ -203,38 +201,38 @@ void Thing::ProcessBinary(char* bytes) {
   (void)bytes;
 };
 
-void Thing::SetPosition(Spherical position) {
+void Thing::SetPosition(Spherical16 position) {
   this->position = position;
   this->positionUpdated = true;
 }
-Spherical Thing::GetPosition() {
+Spherical16 Thing::GetPosition() {
   return this->position;
 }
 
-void Thing::SetOrientation(SwingTwist orientation) {
+void Thing::SetOrientation(SwingTwist16 orientation) {
   this->orientation = orientation;
   this->orientationUpdated = true;
 }
 
-SwingTwist Thing::GetOrientation() {
+SwingTwist16 Thing::GetOrientation() {
   return this->orientation;
 }
 
-void Thing::SetLinearVelocity(Spherical linearVelocity) {
+void Thing::SetLinearVelocity(Spherical16 linearVelocity) {
   this->linearVelocity = linearVelocity;
   this->linearVelocityUpdated = true;
 }
 
-Spherical Thing::GetLinearVelocity() {
+Spherical16 Thing::GetLinearVelocity() {
   return this->linearVelocity;
 }
 
-void Thing::SetAngularVelocity(Spherical angularVelocity) {
+void Thing::SetAngularVelocity(Spherical16 angularVelocity) {
   this->angularVelocity = angularVelocity;
   this->angularVelocityUpdated = true;
 }
 
-Spherical Thing::GetAngularVelocity() {
+Spherical16 Thing::GetAngularVelocity() {
   return this->angularVelocity;
 }
 

Thing.h

@@ -49,10 +49,7 @@ class Thing {
   /// @param networkId The network ID of the thing
   /// @param thingId The ID of the thing
   /// @param thingType The type of thing
-  Thing(Participant* participant,
+  Thing(Participant* participant, unsigned char networkId, unsigned char thingId, Type thingType = Type::Undetermined);
-        unsigned char networkId,
-        unsigned char thingId,
-        Type thingType = Type::Undetermined);
 
   /// @brief The participant managing this thing
   Participant* owner;
@@ -108,24 +105,23 @@ class Thing {
  public:
   /// @brief The name of the thing
   const char* name = nullptr;
-  /// @brief An URL pointing to the location where a model of the thing can be
+  /// @brief An URL pointing to the location where a model of the thing can be found
-  /// found
   const char* modelUrl = nullptr;
   /// @brief The scale of the model (deprecated I think)
   float modelScale = 1;
 
   /// @brief Set the position of the thing
   /// @param position The new position in local space, in meters
-  void SetPosition(Spherical position);
+  void SetPosition(Spherical16 position);
   /// @brief Get the position of the thing
   /// @return The position in local space, in meters
-  Spherical GetPosition();
+  Spherical16 GetPosition();
   /// @brief Set the orientation of the thing
   /// @param orientation The new orientation in local space
-  void SetOrientation(SwingTwist orientation);
+  void SetOrientation(SwingTwist16 orientation);
   /// @brief Get the orientation of the thing
   /// @return The orienation in local space
-  SwingTwist GetOrientation();
+  SwingTwist16 GetOrientation();
   /// @brief The scale of the thing (deprecated I think)
   //float scale = 1;  // assuming uniform scale
 
@@ -135,18 +131,17 @@ class Thing {
   bool orientationUpdated = false;
 
   /// @brief Set the linear velocity of the thing
-  /// @param linearVelocity The new linear velocity in local space, in meters
+  /// @param linearVelocity The new linear velocity in local space, in meters per second
-  /// per second
+  void SetLinearVelocity(Spherical16 linearVelocity);
-  void SetLinearVelocity(Spherical linearVelocity);
   /// @brief Get the linear velocity of the thing
   /// @return The linear velocity in local space, in meters per second
-  virtual Spherical GetLinearVelocity();
+  virtual Spherical16 GetLinearVelocity();
   /// @brief Set the angular velocity of the thing
   /// @param angularVelocity the new angular velocity in local space
-  virtual void SetAngularVelocity(Spherical angularVelocity);
+  virtual void SetAngularVelocity(Spherical16 angularVelocity);
   /// @brief Get the angular velocity of the thing
   /// @return The angular velocity in local space
-  virtual Spherical GetAngularVelocity();
+  virtual Spherical16 GetAngularVelocity();
   bool linearVelocityUpdated = false;
   bool angularVelocityUpdated = false;
 
@@ -154,16 +149,16 @@ class Thing {
   /// @brief The position in local space
   /// @remark When this Thing has a parent, the position is relative to the
   /// parent's position and orientation
-  Spherical position;
+  Spherical16 position;
   /// @brief The orientation in local space
   /// @remark When this Thing has a parent, the orientation is relative to the
   /// parent's orientation
-  SwingTwist orientation;
+  SwingTwist16 orientation;
 
   /// @brief The linear velocity in local space
-  Spherical linearVelocity;
+  Spherical16 linearVelocity;
   /// @brief The angular velocity in local spze
-  Spherical angularVelocity;
+  Spherical16 angularVelocity;
 
  public:
   /// @brief Terminated things are no longer updated
@@ -182,8 +177,7 @@ class Thing {
 
   /// @brief Updates the state of the thing
   /// @param currentTimeMs The current clock time in milliseconds
-  virtual void Update(
+  virtual void Update(unsigned long currentTimeMs);  // { (void)currentTimeMs; };
-      unsigned long currentTimeMs);  // { (void)currentTimeMs; };
 
   static void UpdateThings(unsigned long currentTimeMs);
 

Things/DifferentialDrive.cpp

@@ -1,21 +1,16 @@
 #include "DifferentialDrive.h"
 
 namespace RoboidControl {
-DifferentialDrive::DifferentialDrive() : Thing() {}
 
-RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant)
+RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant) : Thing(participant) {
-    : Thing(participant) {
   // this->leftWheel = new Thing(participant);
   // this->rightWheel = new Thing(participant);
 }
 
-void DifferentialDrive::SetDimensions(float wheelDiameter,
+void DifferentialDrive::SetDimensions(float wheelDiameter, float wheelSeparation) {
-                                      float wheelSeparation) {
+  this->wheelRadius = wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2;
-  this->wheelRadius =
+  this->wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation;
-      wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2;
+  this->rpsToMs = wheelDiameter * Passer::LinearAlgebra::pi;
-  this->wheelSeparation =
-      wheelSeparation > 0 ? wheelSeparation : -wheelSeparation;
-  this->rpsToMs = wheelDiameter * LinearAlgebra::pi;
 
   float distance = this->wheelSeparation / 2;
   if (leftWheel != nullptr)
@@ -35,10 +30,6 @@ void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) {
     this->rightWheel->SetPosition(Spherical(distance, Direction::right));
 }
 
-void DifferentialDrive::SetLeftWheelVelocity(float angularSpeed) {}
-
-void DifferentialDrive::SetRightWheelVelocity(float angularSpeed) {}
-
 void DifferentialDrive::Update(unsigned long currentMs) {
   // this assumes forward velocity only....
   float linearVelocity = this->GetLinearVelocity().distance;
@@ -50,21 +41,15 @@ void DifferentialDrive::Update(unsigned long currentMs) {
     angularSpeed = -angularSpeed;
 
   // wheel separation can be replaced by this->leftwheel->position->distance
-  float speedLeft =
+  float speedLeft = (linearVelocity + angularSpeed * this->wheelSeparation / 2) / this->wheelRadius * Rad2Deg;
-      (linearVelocity + angularSpeed * this->wheelSeparation / 2) /
-      this->wheelRadius * Rad2Deg;
   if (this->leftWheel != nullptr)
     this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left));
 
-  float speedRight =
+  float speedRight = (linearVelocity - angularSpeed * this->wheelSeparation / 2) / this->wheelRadius * Rad2Deg;
-      (linearVelocity - angularSpeed * this->wheelSeparation / 2) /
-      this->wheelRadius * Rad2Deg;
   if (this->rightWheel != nullptr)
-    this->rightWheel->SetAngularVelocity(
+    this->rightWheel->SetAngularVelocity(Spherical(speedRight, Direction::right));
-        Spherical(speedRight, Direction::right));
 
-  // std::cout << "lin. speed " << linearVelocity << "   ang. speed " <<
+    // std::cout << "lin. speed " << linearVelocity << "   ang. speed " << angularVelocity.distance << "   left wheel  "
-  // angularVelocity.distance << "   left wheel  "
     //         << speedLeft << "   right wheel " << speedRight << "\n";
 }
 

Things/DifferentialDrive.h

@@ -7,15 +7,11 @@ namespace RoboidControl {
 /// @brief A thing which can move itself using a differential drive system
 class DifferentialDrive : public Thing {
  public:
-  DifferentialDrive();
   DifferentialDrive(Participant* participant);
 
   void SetDimensions(float wheelDiameter, float wheelSeparation);
   void SetMotors(Thing* leftWheel, Thing* rightWheel);
 
-  void SetLeftWheelVelocity(float angularSpeed);
-  void SetRightWheelVelocity(float angularSpeed);
-
   virtual void Update(unsigned long currentMs) override;
 
  protected:

Things/TouchSensor.cpp

@@ -1,13 +1,10 @@
 #include "TouchSensor.h"
 
 namespace RoboidControl {
-TouchSensor::TouchSensor() : Thing(Thing::Type::TouchSensor) {}
 
-TouchSensor::TouchSensor(Participant* participant)
+TouchSensor::TouchSensor(Participant* participant) : Thing(participant) {
-    : Thing(participant, Thing::Type::TouchSensor) {}
+  this->touchedSomething = false;
-
+  this->type = (unsigned char)Thing::Type::TouchSensor;
-TouchSensor::TouchSensor(Thing* parent) : Thing(parent->owner) {
-  this->SetParent(parent);
 }
 
 void TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {}

Things/TouchSensor.h

@@ -7,20 +7,15 @@ namespace RoboidControl {
 /// @brief A sensor which can detect touches
 class TouchSensor : public Thing {
  public:
-  /// @brief Create a touch sensor with isolated participant
+  /// @brief Value which is true when the sensor is touching something, false otherwise
-  TouchSensor();
+  bool touchedSomething = false;
+
   /// @brief Create a touch sensor
   TouchSensor(Participant* participant);
   /// @brief Create a temperature sensor with the given ID
   /// @param networkId The network ID of the sensor
   /// @param thingId The ID of the thing
-  TouchSensor(Thing* parent);
+  // TouchSensor(RemoteParticipant* participant, unsigned char networkId, unsigned char thingId);
-  // TouchSensor(RemoteParticipant* participant, unsigned char networkId,
-  // unsigned char thingId);
-
-  /// @brief Value which is true when the sensor is touching something, false
-  /// otherwise
-  bool touchedSomething = false;
 
   /// @brief Function to create a binary message with the temperature
   /// @param buffer The byte array for thw binary data