Arduino/ArduinoParticipant.cpp

@@ -1,9 +1,5 @@
 #include "ArduinoParticipant.h"
 
-#if !defined(NO_STD)
-#include <iostream>
-#endif
-
 #if defined(ARDUINO)
 #if defined(ARDUINO_ARCH_ESP8266)
 #include <ESP8266WiFi.h>
@@ -23,19 +19,15 @@
 namespace RoboidControl {
 namespace Arduino {
 
-#if defined(ARDUINO) && defined(HAS_WIFI)
+void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
-WiFiUDP* udp;
+#if defined(ARDUINO)
-#endif
+  this->remoteIpAddress = remoteIpAddress;
-
+  this->remotePort = remotePort;
-void ParticipantUDP::Setup() {
-#if defined(ARDUINO) && defined(HAS_WIFI)
   GetBroadcastAddress();
 
 #if defined(UNO_R4)
   if (WiFi.status() == WL_NO_MODULE) {
-#if !defined(NO_STD)
     std::cout << "No network available!\n";
-#endif
     return;
   }
 #else
@@ -44,86 +36,80 @@ void ParticipantUDP::Setup() {
     return;
   }
 #endif
+  udp.begin(localPort);
 
-  udp = new WiFiUDP();
+  std::cout << "Wifi sync started to port " << this->remotePort << "\n";
-  udp->begin(this->port);
-
-#if !defined(NO_STD)
-  std::cout << "Wifi sync started local " << this->port;
-  if (this->remoteSite != nullptr)
-    std::cout << ", remote " << this->remoteSite->ipAddress << ":"
-              << this->remoteSite->port << "\n";
-#endif
 #endif
 }
 
-void ParticipantUDP::GetBroadcastAddress() {
+void LocalParticipant::GetBroadcastAddress() {
-#if defined(ARDUINO) && defined(HAS_WIFI)
+#if defined(ARDUINO)
   IPAddress broadcastAddress = WiFi.localIP();
   broadcastAddress[3] = 255;
   String broadcastIpString = broadcastAddress.toString();
   this->broadcastIpAddress = new char[broadcastIpString.length() + 1];
-  broadcastIpString.toCharArray(this->broadcastIpAddress,
+  broadcastIpString.toCharArray(this->broadcastIpAddress, broadcastIpString.length() + 1);
-                                broadcastIpString.length() + 1);
-#if !defined(NO_STD)
   std::cout << "Broadcast address: " << broadcastIpAddress << "\n";
 #endif
-#endif
 }
 
-void ParticipantUDP::Receive() {
+void LocalParticipant::Receive() {
-#if defined(ARDUINO) && defined(HAS_WIFI)
+#if defined(ARDUINO)
-  int packetSize = udp->parsePacket();
+  int packetSize = udp.parsePacket();
   while (packetSize > 0) {
-    udp->read(buffer, packetSize);
+    udp.read(buffer, packetSize);
 
-    String senderAddress = udp->remoteIP().toString();
+    String senderAddress = udp.remoteIP().toString();
     char sender_ipAddress[16];
     senderAddress.toCharArray(sender_ipAddress, 16);
-    unsigned int sender_port = udp->remotePort();
+    unsigned int sender_port = udp.remotePort();
 
+    // Participant* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port);
+    // if (remoteParticipant == nullptr) {
+    //   remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port);
+    //   // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
+    //   //           << "\n";
+    //   // std::cout << "New remote participant " << remoteParticipant->ipAddress
+    //   //           << ":" << remoteParticipant->port << " "
+    //   //           << (int)remoteParticipant->networkId << "\n";
+    // }
+
+    // ReceiveData(packetSize, remoteParticipant);
     ReceiveData(packetSize, sender_ipAddress, sender_port);
-    packetSize = udp->parsePacket();
+    packetSize = udp.parsePacket();
   }
 #endif
 }
 
-bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
+bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
-#if defined(ARDUINO) && defined(HAS_WIFI)
+#if defined(ARDUINO)
   // std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":"
   // << remoteParticipant->port << "\n";
 
   int n = 0;
-  int r = 0;
   do {
     if (n > 0) {
-#if !defined(NO_STD)
       std::cout << "Retry sending\n";
-#endif
       delay(10);
     }
     n++;
-
+    udp.beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
-    udp->beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
+    udp.write((unsigned char*)buffer, bufferSize);
-    udp->write((unsigned char*)buffer, bufferSize);
+  } while (udp.endPacket() == 0 && n < 10);
-    r = udp->endPacket();
-    // On an Uno R4 WiFi, endPacket blocks for 10 seconds the first time
-    // It is not cleary yet why
-  } while (r == 0 && n < 10);
 
 #endif
   return true;
 }
 
-bool ParticipantUDP::Publish(IMessage* msg) {
+bool LocalParticipant::Publish(IMessage* msg) {
-#if defined(ARDUINO) && defined(HAS_WIFI)
+#ifdef ARDUINO
   int bufferSize = msg->Serialize((char*)this->buffer);
   if (bufferSize <= 0)
     return true;
 
-  udp->beginPacket(this->broadcastIpAddress, this->port);
+  udp.beginPacket(this->broadcastIpAddress, this->remotePort);
-  udp->write((unsigned char*)buffer, bufferSize);
+  udp.write((unsigned char*)buffer, bufferSize);
-  udp->endPacket();
+  udp.endPacket();
 
   //   std::cout << "Publish to " << this->broadcastIpAddress << ":"
   //             << this->remotePort << "\n";

Arduino/ArduinoParticipant.h

@@ -1,20 +1,18 @@
 #pragma once
 
-#include "Participants/ParticipantUDP.h"
+#include "../LocalParticipant.h"
 
 namespace RoboidControl {
 namespace Arduino {
 
-class ParticipantUDP : public RoboidControl::ParticipantUDP {
+class LocalParticipant : public RoboidControl::LocalParticipant {
  public:
-  void Setup();
+  void Setup(int localPort, const char* remoteIpAddress, int remotePort);
   void Receive();
   bool Send(Participant* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);
 
  protected:
-  char* broadcastIpAddress = nullptr;
-
   void GetBroadcastAddress();
 };
 

Arduino/ArduinoUtils.cpp

@@ -42,13 +42,8 @@ struct NssServer {
 } nssServer;
 #endif
 
-bool StartWifi(const char* wifiSsid,
+bool StartWifi(const char* wifiSsid, const char* wifiPassword, bool hotspotFallback) {
-               const char* wifiPassword,
+#if UNO_R4 || ARDUINO_ARCH_RP2040
-               bool hotspotFallback) {
-#if !defined(HAS_WIFI)
-  return false;
-#else
-#if defined(UNO_R4) || defined(ARDUINO_ARCH_RP2040)
   if (WiFi.status() == WL_NO_MODULE) {
     Serial.println("WiFi not present, WiFiSync is disabled");
     return false;
@@ -125,10 +120,8 @@ bool StartWifi(const char* wifiSsid,
 #if ESP32
     printf("Checking credentials in flash\n");
     wifiPreferences.begin(PREFERENCES_NAMESPACE);
-    wifiPreferences.getBytes(STORAGE_KEY_WIFI, &credentials,
+    wifiPreferences.getBytes(STORAGE_KEY_WIFI, &credentials, sizeof(credentials));
-                             sizeof(credentials));
+    if (strcmp(wifiSsid, credentials.ssid) != 0 || strcmp(wifiPassword, credentials.password) != 0) {
-    if (strcmp(wifiSsid, credentials.ssid) != 0 ||
-        strcmp(wifiPassword, credentials.password) != 0) {
       printf("Updating credentials in flash...");
       const int ssidLen = strlen(wifiSsid);
       if (ssidLen < 32) {
@@ -141,8 +134,7 @@ bool StartWifi(const char* wifiSsid,
         memcpy(credentials.password, wifiPassword, pwdLen);
         credentials.password[pwdLen] = '\0';
       }
-      wifiPreferences.putBytes(STORAGE_KEY_WIFI, &credentials,
+      wifiPreferences.putBytes(STORAGE_KEY_WIFI, &credentials, sizeof(credentials));
-                               sizeof(credentials));
       printf(" completed.\n");
     }
     wifiPreferences.end();
@@ -150,15 +142,10 @@ bool StartWifi(const char* wifiSsid,
   }
 
   return (!hotSpotEnabled);
-#endif
 }
 
 void CheckFirmware(String url, String FIRMWARE_NAME, int FIRMWARE_VERSION) {
-#if !defined(HAS_WIFI)
+#if defined(UNO_R4)  // Uno R4 Wifi does not support this kind of firmware update (as far as I know)
-  return;
-#else
-#if defined(UNO_R4)  // Uno R4 Wifi does not support this kind of firmware
-                     // update (as far as I know)
   return;
 #else
   Serial.println("Checking for firmware updates.");
@@ -190,12 +177,10 @@ void CheckFirmware(String url, String FIRMWARE_NAME, int FIRMWARE_VERSION) {
       switch (ret) {
         case HTTP_UPDATE_FAILED:
 #if defined(ESP32)
-          Serial.printf("HTTP_UPDATE_FAILED Error (%d): %s",
+          Serial.printf("HTTP_UPDATE_FAILED Error (%d): %s", httpUpdate.getLastError(),
-                        httpUpdate.getLastError(),
                         httpUpdate.getLastErrorString().c_str());
 #else
-          Serial.printf("HTTP_UPDATE_FAILED Error (%d): %s",
+          Serial.printf("HTTP_UPDATE_FAILED Error (%d): %s", ESPhttpUpdate.getLastError(),
-                        ESPhttpUpdate.getLastError(),
                         ESPhttpUpdate.getLastErrorString().c_str());
 #endif
           break;
@@ -213,6 +198,5 @@ void CheckFirmware(String url, String FIRMWARE_NAME, int FIRMWARE_VERSION) {
     Serial.println(httpCode);
   }
 #endif
-#endif
 }
 #endif

Arduino/Things/DRV8833.cpp

@@ -3,83 +3,53 @@
 #include <Arduino.h>
 
 namespace RoboidControl {
-namespace Arduino {
 
-#pragma region DRV8833
+DRV8833Motor::DRV8833Motor(Participant* participant,
-
-DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(Type::Undetermined, parent) {
-  this->pinStandby = config.standby;
-  if (pinStandby != 255)
-    pinMode(pinStandby, OUTPUT);
-
-  this->motorA = new DRV8833Motor(this, config.AIn1, config.AIn2);
-  this->motorA->SetName("Motor A");
-  this->motorB = new DRV8833Motor(this, config.BIn1, config.BIn2);
-  this->motorB->SetName("Motor B");
-}
-
-#pragma endregion DRV8833
-
-#pragma region Differential drive
-
-DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
-                                              Thing* parent)
-    : RoboidControl::DifferentialDrive(this->drv8833.motorA,
-                                       this->drv8833.motorB,
-                                       parent),
-      drv8833(config, this) {}
-
-void DRV8833::DifferentialDrive::Update(bool recurse) {
-  RoboidControl::DifferentialDrive::Update(recurse);
-  this->drv8833.Update(false);
-}
-
-#pragma endregion Differential drive
-
-#pragma region Motor
-
-#if (ESP32)
-uint8_t DRV8833Motor::nextAvailablePwmChannel = 0;
-#endif
-
-DRV8833Motor::DRV8833Motor(DRV8833* driver,
                            unsigned char pinIn1,
                            unsigned char pinIn2,
                            bool reverse)
-    : Motor() {
+    : Thing(participant) {
-  this->SetParent(driver);
-
   this->pinIn1 = pinIn1;
   this->pinIn2 = pinIn2;
 
 #if (ESP32)
-  in1Ch = DRV8833Motor::nextAvailablePwmChannel++;
+  in1Ch = nextAvailablePwmChannel++;
   ledcSetup(in1Ch, 500, 8);
   ledcAttachPin(pinIn1, in1Ch);
-
+  in2Ch = nextAvailablePwmChannel++;
-  in2Ch = DRV8833Motor::nextAvailablePwmChannel++;
   ledcSetup(in2Ch, 500, 8);
   ledcAttachPin(pinIn2, in2Ch);
-
 #else
   pinMode(pinIn1, OUTPUT);  // configure the in1 pin to output mode
   pinMode(pinIn2, OUTPUT);  // configure the in1 pin to output mode
 #endif
 
-  // this->reverse = reverse;
+  this->reverse = reverse;
 }
 
-// void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
+void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
-//   this->maxRpm = rpm;
+  this->maxRpm = rpm;
-// }
+}
 
-void DRV8833Motor::SetTargetVelocity(float motorSpeed) {
+void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
-  Motor::SetTargetVelocity(motorSpeed);
+  Thing::SetAngularVelocity(velocity);
+  // ignoring rotation axis for now.
+  // Spherical angularVelocity = this->GetAngularVelocity();
+  float angularSpeed = velocity.distance;  // in degrees/sec
 
-  uint8_t motorSignal =
+  float rpm = angularSpeed / 360 * 60;
-      (uint8_t)(motorSpeed > 0 ? motorSpeed * 255 : -motorSpeed * 255);
+  float motorSpeed = rpm / this->maxRpm;
 
-  // std::cout << "moto speed " << this->name << " = " << motorSpeed
+  uint8_t motorSignal = (uint8_t)(motorSpeed * 255);
+  // if (direction == RotationDirection::CounterClockwise)
+  //   speed = -speed;
+  // Determine the rotation direction
+  if (velocity.direction.horizontal.InDegrees() < 0)
+    motorSpeed = -motorSpeed;
+  if (this->reverse)
+    motorSpeed =-motorSpeed;
+
+  // std::cout << "ang speed " << this->name << " = " << angularSpeed << "   rpm " << rpm
   //           << ", motor signal = " << (int)motorSignal << "\n";
 
 #if (ESP32)
@@ -129,7 +99,23 @@ void DRV8833Motor::SetTargetVelocity(float motorSpeed) {
 #endif
 }
 
-#pragma endregion Motor
+DRV8833::DRV8833(Participant* participant,
+                 unsigned char pinAIn1,
+                 unsigned char pinAIn2,
+                 unsigned char pinBIn1,
+                 unsigned char pinBIn2,
+                 unsigned char pinStandby,
+                 bool reverseA,
+                 bool reverseB)
+    : Thing(participant) {
+  this->pinStandby = pinStandby;
+  if (pinStandby != 255)
+    pinMode(pinStandby, OUTPUT);
+
+  this->motorA = new DRV8833Motor(participant, pinAIn1, pinAIn2, reverseA);
+  this->motorA->name = "Motor A";
+  this->motorB = new DRV8833Motor(participant, pinBIn1, pinBIn2, reverseB);
+  this->motorB->name = "Motor B";
+}
 
-}  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/DRV8833.h

@@ -1,85 +1,60 @@
 #pragma once
 
-#include <Arduino.h>
-#include "Participants/IsolatedParticipant.h"
 #include "Thing.h"
 #include "Things/DifferentialDrive.h"
-#include "Things/Motor.h"
 
 namespace RoboidControl {
 namespace Arduino {
 
-class DRV8833Motor;
+/// @brief Support for a DRV8833 motor controller
+class DRV8833Motor : public Thing {
+ public:
+  /// @brief Motor turning direction
+  enum class RotationDirection { Clockwise = 1, CounterClockwise = -1 };
+
+  /// @brief Setup the DC motor
+  /// @param pinIn1 the pin number for the in1 signal
+  /// @param pinIn2 the pin number for the in2 signal
+  /// @param direction the forward turning direction of the motor
+  DRV8833Motor(Participant* participant, unsigned char pinIn1, unsigned char pinIn2, bool reverse = false);
+  void SetMaxRPM(unsigned int rpm);
+
+  virtual void SetAngularVelocity(Spherical velocity) override;
+
+  bool reverse = false;
+
+ protected:
+  unsigned char pinIn1 = 255;
+  unsigned char pinIn2 = 255;
+  unsigned int maxRpm = 200;
+};
 
 class DRV8833 : public Thing {
  public:
-  struct Configuration {
-    int AIn1;
-    int AIn2;
-    int BIn1;
-    int BIn2;
-    int standby = 255;
-  };
-
   /// @brief Setup a DRV8833 motor controller
-  DRV8833(Configuration config, Thing* parent = Thing::LocalRoot());
+  /// @param pinAIn1 The pin number connected to the AIn1 port
+  /// @param pinAIn2 The pin number connected to the AIn2 port
+  /// @param pinBIn1 The pin number connected to the BIn1 port
+  /// @param pinBIn2 The pin number connceted to the BIn2 port
+  /// @param pinStandby The pin number connected to the standby port, 255
+  /// indicated that the port is not connected
+  /// @param reverseA The forward turning direction of motor A
+  /// @param reverseB The forward turning direction of motor B
+  DRV8833(Participant* participant,
+          unsigned char pinAIn1,
+          unsigned char pinAIn2,
+          unsigned char pinBIn1,
+          unsigned char pinBIn2,
+          unsigned char pinStandby = 255,
+          bool reverseA = false,
+          bool reverseB = false);
 
   DRV8833Motor* motorA = nullptr;
   DRV8833Motor* motorB = nullptr;
 
  protected:
   unsigned char pinStandby = 255;
-
- public:
-  class DifferentialDrive;
 };
 
-#pragma region Differential drive
-
-class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive {
- public:
-  DifferentialDrive(DRV8833::Configuration config, Thing* parent = Thing::LocalRoot());
-
-  virtual void Update(bool recurse = false) override;
-
- protected:
-  DRV8833 drv8833;
-};
-
-#pragma endregion Differential drive
-
-#pragma region Motor
-
-/// @brief Support for a DRV8833 motor controller
-class DRV8833Motor : public Motor {
- public:
-  /// @brief Setup the DC motor
-  /// @param pinIn1 the pin number for the in1 signal
-  /// @param pinIn2 the pin number for the in2 signal
-  /// @param direction the forward turning direction of the motor
-  DRV8833Motor(DRV8833* driver,
-               unsigned char pinIn1,
-               unsigned char pinIn2,
-               bool reverse = false);
-  // void SetMaxRPM(unsigned int rpm);
-
-  // virtual void SetAngularVelocity(Spherical velocity) override;
-  virtual void SetTargetVelocity(float targetSpeed) override;
-  // bool reverse = false;
-
- protected:
-  unsigned char pinIn1 = 255;
-  unsigned char pinIn2 = 255;
-  // unsigned int maxRpm = 200;
-
-#if (ESP32)
-  uint8_t in1Ch;
-  uint8_t in2Ch;
-  static uint8_t nextAvailablePwmChannel;
-#endif
-};
-
-#pragma endregion Motor
-
 }  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/DigitalInput.cpp

@@ -5,125 +5,17 @@
 namespace RoboidControl {
 namespace Arduino {
 
-#pragma region Digital input
+DigitalInput::DigitalInput(Participant* participant, unsigned char pin) : TouchSensor(participant) {
-
-DigitalInput::DigitalInput(unsigned char pin, Thing* parent)
-    : Thing(Type::Undetermined, parent) {
     this->pin = pin;
-  pinMode(this->pin, INPUT);
+
-  std::cout << "digital input start\n";
+    pinMode(pin, INPUT);
 }
 
-void DigitalInput::Update(bool recursive) {
+void DigitalInput::Update(unsigned long currentTimeMs) {
-  this->isHigh = digitalRead(this->pin);
+    this->touchedSomething = digitalRead(pin) == LOW;
-  this->isLow = !this->isHigh;
+
-  Thing::Update(recursive);
+    // std::cout << "DigitalINput pin " << (int)this->pin << ": " << this->touchedSomething << "\n";
 }
 
-#pragma endregion Digital input
-
-#pragma region Touch sensor
-
-DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing* parent)
-    : RoboidControl::TouchSensor(parent), digitalInput(pin, parent) {}
-
-void DigitalInput::TouchSensor::Update(bool recursive) {
-  this->touchedSomething = digitalInput.isLow;
-}
-
-#pragma endregion Touch sensor
-
-#pragma region Relative encoder
-
-int DigitalInput::RelativeEncoder::interruptCount = 0;
-volatile int DigitalInput::RelativeEncoder::pulseCount0 = 0;
-volatile int DigitalInput::RelativeEncoder::pulseCount1 = 0;
-
-DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config,
-                                               Thing* parent)
-    : RoboidControl::RelativeEncoder(parent),
-      digitalInput(config.pin, parent),
-      pulsesPerRevolution(config.pulsesPerRevolution) {}
-
-void DigitalInput::RelativeEncoder::Start() {
-  // We support up to 2 pulse counters
-  if (interruptCount == 0) {
-    std::cout << "pin interrupt 1 activated" << std::endl;
-    attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt0,
-                    RISING);
-  } else if (interruptCount == 1) {
-    std::cout << "pin interrupt 2 activated" << std::endl;
-    attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt1,
-                    RISING);
-  } else {
-    // maximum interrupt count reached
-    std::cout << "DigitalInput::RelativeEncoder: max. # counters of 2 reached"
-              << std::endl;
-    return;
-  }
-  interruptIx = interruptCount;
-  interruptCount++;
-  std::cout << "pin ints. " << interruptCount << std::endl;
-}
-
-int DigitalInput::RelativeEncoder::GetPulseCount() {
-  if (interruptIx == 0) {
-    int pulseCount = pulseCount0;
-    pulseCount0 = 0;
-    return pulseCount;
-  } else if (interruptIx == 1) {
-    int pulseCount = pulseCount1;
-    pulseCount1 = 0;
-    return pulseCount;
-  }
-  return 0;
-}
-
-void DigitalInput::RelativeEncoder::Update(bool recursive) {
-  unsigned long currentTimeMs = GetTimeMs();
-  if (this->lastUpdateTime == 0) {
-    this->Start();
-    this->lastUpdateTime = currentTimeMs;
-    return;
-  }
-
-  float timeStep = (float)(currentTimeMs - this->lastUpdateTime) / 1000.0f;
-  if (timeStep <= 0)
-    return;
-
-  int pulseCount = GetPulseCount();
-  netPulseCount += pulseCount;
-
-  this->pulseFrequency = pulseCount / timeStep;
-  this->rotationSpeed = pulseFrequency / pulsesPerRevolution;
-
-  std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency
-            << " timestep " << timeStep << std::endl;
-
-  this->lastUpdateTime = currentTimeMs;
-}
-
-#if defined(ESP8266) || defined(ESP32)
-void ICACHE_RAM_ATTR DigitalInput::RelativeEncoder::PulseInterrupt0() {
-  pulseCount0++;
-}
-#else
-void DigitalInput::RelativeEncoder::PulseInterrupt0() {
-  pulseCount0++;
-}
-#endif
-
-#if defined(ESP8266) || defined(ESP32)
-void IRAM_ATTR DigitalInput::RelativeEncoder::PulseInterrupt1() {
-  pulseCount1++;
-}
-#else
-void DigitalInput::RelativeEncoder::PulseInterrupt1() {
-  pulseCount1++;
-}
-#endif
-
-#pragma endregion Relative encoder
-
 }  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/DigitalInput.h

@@ -1,92 +1,25 @@
 #pragma once
 
-#include "Things/RelativeEncoder.h"
 #include "Things/TouchSensor.h"
 
 namespace RoboidControl {
 namespace Arduino {
 
 /// @brief A digital input represents the stat of a digital GPIO pin
-class DigitalInput : public Thing {
+class DigitalInput : public TouchSensor {
  public:
   /// @brief Create a new digital input
   /// @param participant The participant to use
   /// @param pin The digital pin
-  //DigitalInput(Participant* participant, unsigned char pin);
+  DigitalInput(Participant* participant, unsigned char pin);
-  // DigitalInput(Thing* parent, unsigned char pin);
-  DigitalInput(unsigned char pin, Thing* parent);
-
-  bool isHigh = false;
-  bool isLow = false;
 
     /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
-  virtual void Update(bool recursive = false) override;
+  virtual void Update(unsigned long currentTimeMs) override;
 
  protected:
   /// @brief The pin used for digital input
   unsigned char pin = 0;
-
- public:
-  class TouchSensor;
-  class RelativeEncoder;
 };
 
-#pragma region Touch sensor
-
-class DigitalInput::TouchSensor : public RoboidControl::TouchSensor {
- public:
-  TouchSensor(unsigned char pin, Thing* parent);
-
-  /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
-  virtual void Update(bool recurse = false) override;
-
- protected:
-  DigitalInput digitalInput;
-};
-
-#pragma endregion Touch sensor
-
-#pragma region Incremental encoder
-
-class DigitalInput::RelativeEncoder : public RoboidControl::RelativeEncoder {
- public:
-  struct Configuration {
-    unsigned char pin;
-    unsigned char pulsesPerRevolution;
-  };
-
-  RelativeEncoder(Configuration config, Thing* parent = Thing::LocalRoot());
-
-  unsigned char pulsesPerRevolution;
-
-  /// @brief The current pulse frequency in Hz
-  float pulseFrequency = 0;
-
-  /// @copydoc RoboidControl::Thing::Update()
-  virtual void Update(bool recurse = false) override;
-
- protected:
-  DigitalInput digitalInput;
-
-  int interruptIx = 0;
-
-  static int interruptCount;
-
-  volatile static int pulseCount0;
-  static void PulseInterrupt0();
-
-  volatile static int pulseCount1;
-  static void PulseInterrupt1();
-
-  int GetPulseCount();
-  long netPulseCount = 0;
-  unsigned long lastUpdateTime = 0;
-
- private:
-  void Start();
-};
-
-#pragma endregion Incremental encoder
-
 }  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/UltrasonicSensor.cpp

@@ -1,16 +1,14 @@
 #include "UltrasonicSensor.h"
 
 #include <Arduino.h>
-#include <iostream>
 
 namespace RoboidControl {
 namespace Arduino {
 
-UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
+UltrasonicSensor::UltrasonicSensor(Participant* participant, unsigned char pinTrigger, unsigned char pinEcho)
-    : Thing(Type::Undetermined, parent) {
+    : TouchSensor(participant) {
-  this->name = "Ultrasonic sensor";
+  this->pinTrigger = pinTrigger;
-  this->pinTrigger = config.trigger;
+  this->pinEcho = pinEcho;
-  this->pinEcho = config.echo;
 
   pinMode(pinTrigger, OUTPUT);  // configure the trigger pin to output mode
   pinMode(pinEcho, INPUT);      // configure the echo pin to input mode
@@ -19,14 +17,13 @@ UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
 float UltrasonicSensor::GetDistance() {
   // Start the ultrasonic 'ping'
   digitalWrite(pinTrigger, LOW);
-  delayMicroseconds(2);
+  delayMicroseconds(5);
   digitalWrite(pinTrigger, HIGH);
   delayMicroseconds(10);
   digitalWrite(pinTrigger, LOW);
 
   // Measure the duration of the pulse on the echo pin
-  unsigned long duration_us =
+  float duration_us = pulseIn(pinEcho, HIGH, 100000);  // the result is in microseconds
-      pulseIn(pinEcho, HIGH, 10000);  // the result is in microseconds
 
   // Calculate the distance:
   // * Duration should be divided by 2, because the ping goes to the object
@@ -37,38 +34,30 @@ float UltrasonicSensor::GetDistance() {
   // * Now we calculate the distance based on the speed of sound (340 m/s):
   // distance = duration_sec * 340;
   // * The result calculation is therefore:
-  this->distance = (float)duration_us / 2 / 1000000 * 340;
+  this->distance = duration_us / 2 / 1000000 * 340;
-
-  // Serial.println(this->distance);
-
-  // std::cout << "US distance " << this->distance << std::endl;
 
   // Filter faulty measurements. The sensor can often give values > 30 m which
   // are not correct
   // if (distance > 30)
   //   distance = 0;
 
-  return this->distance;
+  this->touchedSomething |= (this->distance <= this->touchDistance);
+
+//   std::cout << "Ultrasonic " << this->distance << " " << this->touchedSomething << "\n";
+
+  return distance;
 }
 
-void UltrasonicSensor::Update(bool recursive) {
+void UltrasonicSensor::Update(unsigned long currentTimeMs) {
+  this->touchedSomething = false;
   GetDistance();
-  Thing::Update(recursive);
 }
 
-#pragma region Touch sensor
+// void UltrasonicSensor::ProcessBinary(char* bytes) {
-
+//   this->touchedSomething = (bytes[0] == 1);
-UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent)
+//   if (this->touchedSomething)
-    : RoboidControl::TouchSensor(parent), ultrasonic(config, this) {}
+//     std::cout << "Touching something!\n";
-
+// }
-void UltrasonicSensor::TouchSensor::Update(bool recursive) {
-  RoboidControl::TouchSensor::Update(recursive);
-  this->ultrasonic.Update(false);
-  this->touchedSomething |= (this->ultrasonic.distance > 0 &&
-                            this->ultrasonic.distance <= this->touchDistance);
-}
-
-#pragma region Touch sensor
 
 }  // namespace Arduino
 }  // namespace RoboidControl

Arduino/Things/UltrasonicSensor.h

@@ -6,59 +6,36 @@ namespace RoboidControl {
 namespace Arduino {
 
 /// @brief An HC-SR04 ultrasonic distance sensor
-class UltrasonicSensor : Thing {
+class UltrasonicSensor : public TouchSensor {
  public:
-  struct Configuration {
+  /// @brief  Setup an ultrasonic sensor
-    int trigger;
+  /// @param participant The participant to use
-    int echo;
+  /// @param pinTrigger The pin number of the trigger signal
-  };
+  /// @param pinEcho The pin number of the echo signal
-
+  UltrasonicSensor(Participant* participant, unsigned char pinTrigger, unsigned char pinEcho);
-  UltrasonicSensor(Configuration config, Thing* parent = Thing::LocalRoot());
 
   // parameters
 
   /// @brief The distance at which the object is considered to be touched
-  // float touchDistance = 0.2f;
+  float touchDistance = 0.2f;
 
   // state
 
   /// @brief The last read distance
   float distance = 0;
-  /// @brief erform an ultrasonic 'ping' to determine the distance to the
+  /// @brief erform an ultrasonic 'ping' to determine the distance to the nearest object
-  /// nearest object
   /// @return the measured distance in meters to the nearest object
   float GetDistance();
 
   /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
-  virtual void Update(bool recursive = false) override;
+  virtual void Update(unsigned long currentTimeMs) override;
 
  protected:
   /// @brief The pin number of the trigger signal
   unsigned char pinTrigger = 0;
   /// @brief The pin number of the echo signal
   unsigned char pinEcho = 0;
-
- public:
-  class TouchSensor;
 };
 
-#pragma region Touch sensor
-
-class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
- public:
-  TouchSensor(UltrasonicSensor::Configuration config,
-              Thing* parent = Thing::LocalRoot());
-
-  float touchDistance = 0.2f;
-
-  /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
-  virtual void Update(bool recursive = false) override;
-
- protected:
-  UltrasonicSensor ultrasonic;
-};
-
-#pragma region Touch sensor
-
 }  // namespace Arduino
 }  // namespace RoboidControl

CMakeLists.txt

@@ -1,31 +1,16 @@
 cmake_minimum_required(VERSION 3.13)  # CMake version check
-
-file(GLOB srcs 
-    *.cpp
-    Things/*.cpp
-    Messages/*.cpp
-    Arduino/*.cpp
-    Posix/*.cpp
-    Windows/*.cpp
-    EspIdf/*.cpp
-    LinearAlgebra/*.cpp
-    Participants/*.cpp
-)
-
 if(ESP_PLATFORM)
     idf_component_register(
-        SRCS ${srcs}
+        SRC_DIRS "."
-        INCLUDE_DIRS "." "LinearAlgebra"
+        INCLUDE_DIRS "."
-        REQUIRES esp_netif esp_wifi
     )
 else() 
+    project(RoboidCOntrol)
+    add_subdirectory(LinearAlgebra)
+
     set(CMAKE_CXX_STANDARD 17)      # Enable c++11 standard
     set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
-    project(RoboidControl)
-    add_subdirectory(LinearAlgebra)
-    add_subdirectory(Examples)
-
     add_compile_definitions(GTEST)
     include(FetchContent)
     FetchContent_Declare(
@@ -42,6 +27,14 @@ else()
         .
         LinearAlgebra
     )
+    file(GLOB srcs 
+        *.cpp
+        Sensors/*.cpp
+        Messages/*.cpp
+        Arduino/*.cpp
+        Posix/*.cpp
+        Windows/*.cpp
+    )
     add_library(RoboidControl STATIC ${srcs})
     
     enable_testing()
@@ -58,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()

EspIdf/EspIdfParticipant.cpp

@@ -1,166 +0,0 @@
-#include "EspIdfParticipant.h"
-
-#if defined(IDF_VER)
-#include "esp_wifi.h"
-#endif
-
-namespace RoboidControl {
-namespace EspIdf {
-
-void ParticipantUDP::Setup(int localPort,
-                           const char* remoteIpAddress,
-                           int remotePort) {
-#if defined(IDF_VER)
-  std::cout << "Set up UDP\n";
-  GetBroadcastAddress();
-
-  wifi_ap_record_t ap_info;
-  esp_err_t result = esp_wifi_sta_get_ap_info(&ap_info);
-  if (result != ESP_OK) {
-    std::cout << "No network available!\n";
-    return;
-  }
-
-  // Create a UDP socket
-  this->sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
-  if (this->sockfd < 0) {
-    std::cout << "Unable to create UDP socket: errno " << errno << "\n";
-    vTaskDelete(NULL);
-    return;
-  }
-
-  // Set up the server address structure
-  struct sockaddr_in local_addr;
-  memset(&local_addr, 0, sizeof(local_addr));
-  local_addr.sin_family = AF_INET;
-  local_addr.sin_port = htons(this->port);
-  local_addr.sin_addr.s_addr =
-      htonl(INADDR_ANY);  // Listen on all available network interfaces
-
-  // Bind the socket to the address and port
-  if (bind(this->sockfd, (struct sockaddr*)&local_addr, sizeof(local_addr)) <
-      0) {
-    std::cout << "Unable to bind UDP socket: errno " << errno << "\n";
-    close(sockfd);
-    vTaskDelete(NULL);
-    return;
-  }
-
-  // Initialize the dest_addr structure
-  memset(&this->dest_addr, 0,
-         sizeof(this->dest_addr));  // Clear the entire structure
-  this->dest_addr.sin_family = AF_INET;
-  this->dest_addr.sin_port = htons(this->remoteSite->port);
-  inet_pton(AF_INET, this->remoteSite->ipAddress,
-            &this->dest_addr.sin_addr.s_addr);
-
-  std::cout << "Wifi sync started local " << this->port << ", remote "
-            << this->remoteSite->ipAddress << ":" << this->remoteSite->port
-            << "\n";
-#endif  // IDF_VER
-}
-
-void ParticipantUDP::GetBroadcastAddress() {
-#if defined(IDF_VER)
-  // SOMEHOW, THIS FUNCTION RESULTS IN MEMORY CORRUPION...
-
-  esp_netif_ip_info_t ip_info;
-  esp_netif_t* esp_netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
-
-  // Get IP information (IP address, netmask, gateway)
-  if (esp_netif_get_ip_info(esp_netif, &ip_info) != ESP_OK) {
-    std::cout << "Failed to get IP info\n";
-    return;
-  }
-
-  ip_addr_t broadcast_addr = {};
-  broadcast_addr.u_addr.ip4.addr =
-      (ip_info.ip.addr & ip_info.netmask.addr) | ~ip_info.netmask.addr;
-
-  snprintf(this->broadcastIpAddress, INET_ADDRSTRLEN, IPSTR,
-           IP2STR(&broadcast_addr.u_addr.ip4));
-  std::cout << "Broadcast address: " << this->broadcastIpAddress << "\n";
-#endif  // IDF_VER
-}
-
-void ParticipantUDP::Receive() {
-#if defined(IDF_VER)
-  struct pollfd fds[1];
-  fds[0].fd = sockfd;
-  fds[0].events = POLLIN;  // We're looking for data available to read
-
-  // Use poll() with a timeout of 0 to return immediately
-  int ret = poll(fds, 1, 0);
-  if (ret == -1) {
-    std::cout << "poll() error\n";
-    return;
-  }
-
-  // char buffer[1024];
-  struct sockaddr_in source_addr;
-
-  socklen_t addr_len = sizeof(source_addr);
-  char sender_ipAddress[INET_ADDRSTRLEN];
-
-  while (ret > 0 && fds[0].revents & POLLIN) {
-    int packetSize = recvfrom(this->sockfd, buffer, sizeof(buffer) - 1, 0,
-                              (struct sockaddr*)&source_addr, &addr_len);
-    if (packetSize < 0) {
-      std::cout << "recvfrom() error\n";
-      return;
-    } else if (packetSize == 0) {
-      break;
-    }
-
-    // std::cout << "receiving " << packetSize << " bytes, msgId " <<
-    // (int)this->buffer[0] << "\n";
-    inet_ntoa_r(source_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN);
-    unsigned int sender_port = ntohs(source_addr.sin_port);
-
-    ReceiveData(packetSize, sender_ipAddress, sender_port);
-
-    ret = poll(fds, 1, 0);
-    if (ret == -1) {
-      std::cout << "poll() error\n";
-      return;
-    }
-  }
-  // std::cout << "no more messages\n";
-
-#endif  // IDF_VER
-}
-
-bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
-#if defined(IDF_VER)
-  // std::cout << "Sending to " << remoteParticipant->ipAddress << ":"
-  //           << remoteParticipant->port << "\n";
-
-  int err = sendto(this->sockfd, buffer, bufferSize, 0,
-                   (struct sockaddr*)&dest_addr, sizeof(dest_addr));
-  if (errno != 0)
-    std::cout << "Send error " << err << " or " << errno << "\n";
-
-#endif
-  return true;
-}
-
-bool ParticipantUDP::Publish(IMessage* msg) {
-#if defined(IDF_VER)
-  int bufferSize = msg->Serialize((char*)this->buffer);
-  if (bufferSize <= 0)
-    return true;
-
-  struct sockaddr_in dest_addr;
-  dest_addr.sin_family = AF_INET;
-  dest_addr.sin_port = htons(this->port);
-  inet_pton(AF_INET, this->broadcastIpAddress, &dest_addr.sin_addr.s_addr);
-  int err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr,
-                   sizeof(dest_addr));
-  if (err != 0)
-    std::cout << "Publish error\n";
-#endif
-  return true;
-};
-
-}  // namespace EspIdf
-}  // namespace RoboidControl

EspIdf/EspIdfParticipant.h

@@ -1,32 +0,0 @@
-#pragma once
-
-#include "Participants/ParticipantUDP.h"
-
-#if defined(IDF_VER)
-#include "lwip/sockets.h"
-#endif
-
-namespace RoboidControl {
-namespace EspIdf {
-
-class ParticipantUDP : public RoboidControl::ParticipantUDP {
- public:
-  void Setup(int localPort, const char* remoteIpAddress, int remotePort);
-  void Receive();
-  bool Send(Participant* remoteParticipant, int bufferSize);
-  bool Publish(IMessage* msg);
-
- protected:
-#if defined(IDF_VER)
-  char broadcastIpAddress[INET_ADDRSTRLEN];
-
-  int sockfd;
-  struct sockaddr_in dest_addr;
-  // struct sockaddr_in src_addr;
-#endif
-
-  void GetBroadcastAddress();
-};
-
-}  // namespace EspIdf
-}  // namespace RoboidControl

EspIdf/EspIdfUtils.cpp

@@ -1,100 +0,0 @@
-#include "EspIdfUtils.h"
-
-#if defined(IDF_VER)
-#include <iostream>
-// #include "esp_event.h"
-// #include "esp_log.h"
-#include "esp_netif.h"
-#include "esp_wifi.h"
-// #include "lwip/inet.h"
-// #include "lwip/ip_addr.h"
-#include "string.h"
-
-const char* hotspotSSID = "Roboid";
-const char* hotspotPassword = "alchemy7000";
-
-esp_netif_t* wifi_netif = nullptr;
-// Semaphore to signal Wi-Fi connection status
-// SemaphoreHandle_t wifi_semaphore;
-static bool wifi_connected = false;
-
-static void wifi_event_handler(void* arg,
-                               esp_event_base_t event_base,
-                               int32_t event_id,
-                               void* event_data) {
-  if (event_base == WIFI_EVENT) {
-    if (event_id == WIFI_EVENT_STA_START)
-      esp_wifi_connect();
-    else if (event_id == WIFI_EVENT_STA_DISCONNECTED)
-      esp_wifi_connect();
-  } else if (event_base == IP_EVENT) {
-    if (event_id == IP_EVENT_STA_GOT_IP) {
-      // ip_event_got_ip_t* event = (ip_event_got_ip_t*)event_data;
-      // const char* ipaddr = IP2STR(&event->ip_info.ip);
-      wifi_connected = true;
-      // xSemaphoreGive(wifi_semaphore);  // Signal that connection is
-      // established
-    }
-  }
-}
-
-bool StartWifi(const char* wifiSsid, const char* wifiPassword) {
-  std::cout << "Connecting to WiFi " << wifiSsid << "\n";
-
-  esp_netif_init();
-  esp_event_loop_create_default();
-
-  wifi_netif = esp_netif_create_default_wifi_sta();
-  wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
-  esp_wifi_init(&cfg);
-
-  esp_event_handler_register(WIFI_EVENT, ESP_EVENT_ANY_ID, &wifi_event_handler,
-                             NULL);
-  esp_event_handler_register(IP_EVENT, IP_EVENT_STA_GOT_IP, &wifi_event_handler,
-                             NULL);
-
-  wifi_config_t wifi_config = {};
-  strncpy((char*)wifi_config.sta.ssid, wifiSsid, strlen(wifiSsid) + 1);
-  strncpy((char*)wifi_config.sta.password, wifiPassword,
-          strlen(wifiPassword) + 1);
-
-  esp_wifi_set_mode(WIFI_MODE_STA);
-  esp_wifi_set_config(WIFI_IF_STA, &wifi_config);
-  esp_wifi_start();
-
-  // Wait for connection with a timeout of 10 seconds
-  TickType_t xLastWakeTime = xTaskGetTickCount();
-  bool success = false;
-  for (int i = 0; i < 20; i++) {  // 20 iterations, each 500ms
-    if (wifi_connected) {
-      success = true;
-      std::cout << " Connected.\n";
-      break;
-    }
-    std::cout << ".";
-    fflush(stdout);  // Ensure output is printed immediately
-    vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(500));  // Wait 500ms
-  }
-
-  if (wifi_connected) {
-    esp_netif_ip_info_t ip_info = {};
-    esp_netif_t* esp_netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
-
-    // Get IP information (IP address, netmask, gateway)
-    if (esp_netif_get_ip_info(esp_netif, &ip_info) != ESP_OK) {
-      std::cout << "Failed to get IP info\n";
-      return false;
-    }
-
-    // Convert the IP address to string format using inet_ntoa
-    char ip_str[16];  // IPv4 address can have a max of 15 characters + null
-                      // terminator
-    snprintf(ip_str, sizeof(ip_str), IPSTR, IP2STR(&ip_info.ip));
-    std::cout << "IP address = " << ip_str << "\n";
-  } else
-    std::cout << "\nCould not connect to home network.\n";
-
-  return success;
-}
-
-#endif

EspIdf/EspIdfUtils.h

@@ -1,6 +0,0 @@
-#pragma once
-#if defined(IDF_VER)
-
-bool StartWifi(const char *wifiSsid, const char *wifiPassword);
-
-#endif

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,52 +145,51 @@ 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;
 }
 
 template <>
-AngleOf<float> AngleOf<float>::operator-(const AngleOf<float>& angle) const {
+AngleOf<float> AngleOf<float>::operator-(const AngleOf<float> &angle) const {
   AngleOf<float> r = Binary(this->value - angle.value);
   r = Normalize(r);
   return r;
 }
 template <typename T>
-AngleOf<T> AngleOf<T>::operator-(const AngleOf<T>& angle) const {
+AngleOf<T> AngleOf<T>::operator-(const AngleOf<T> &angle) const {
   AngleOf<T> r = Binary(this->value - angle.value);
   return r;
 }
 
 template <>
-AngleOf<float> AngleOf<float>::operator+(const AngleOf<float>& angle) const {
+AngleOf<float> AngleOf<float>::operator+(const AngleOf<float> &angle) const {
   AngleOf<float> r = Binary(this->value + angle.value);
   r = Normalize(r);
   return r;
 }
 template <typename T>
-AngleOf<T> AngleOf<T>::operator+(const AngleOf<T>& angle) const {
+AngleOf<T> AngleOf<T>::operator+(const AngleOf<T> &angle) const {
   AngleOf<T> r = Binary(this->value + angle.value);
   return r;
 }
 
 template <>
-AngleOf<float> AngleOf<float>::operator+=(const AngleOf<float>& angle) {
+AngleOf<float> AngleOf<float>::operator+=(const AngleOf<float> &angle) {
   this->value += angle.value;
   this->Normalize();
   return *this;
 }
 
 template <typename T>
-AngleOf<T> AngleOf<T>::operator+=(const AngleOf<T>& angle) {
+AngleOf<T> AngleOf<T>::operator+=(const AngleOf<T> &angle) {
   this->value += angle.value;
   return *this;
 }
@@ -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,11 +18,10 @@ 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:
- public:
   /// @brief Create a new angle with a zero value
-  AngleOf();
+  AngleOf<T>();
 
   /// @brief An zero value angle
   const static AngleOf<T> zero;
@@ -100,28 +100,28 @@ class AngleOf {
   /// @brief Substract another angle from this angle
   /// @param angle The angle to subtract from this angle
   /// @return The result of the subtraction
-  AngleOf<T> operator-(const AngleOf<T>& angle) const;
+  AngleOf<T> operator-(const AngleOf<T> &angle) const;
   /// @brief Add another angle from this angle
   /// @param angle The angle to add to this angle
   /// @return The result of the addition
-  AngleOf<T> operator+(const AngleOf<T>& angle) const;
+  AngleOf<T> operator+(const AngleOf<T> &angle) const;
   /// @brief Add another angle to this angle
   /// @param angle The angle to add to this angle
   /// @return The result of the addition
-  AngleOf<T> operator+=(const AngleOf<T>& angle);
+  AngleOf<T> operator+=(const AngleOf<T> &angle);
 
   /// @brief Mutliplies the angle
   /// @param angle The angle to multiply
   /// @param factor The factor by which the angle is multiplied
   /// @return The multiplied angle
-  friend AngleOf<T> operator*(const AngleOf<T>& angle, float factor) {
+  friend AngleOf<T> operator*(const AngleOf<T> &angle, float factor) {
     return AngleOf::Degrees((float)angle.InDegrees() * factor);
   }
   /// @brief Multiplies the angle
   /// @param factor The factor by which the angle is multiplies
   /// @param angle The angle to multiply
   /// @return The multiplied angle
-  friend AngleOf<T> operator*(float factor, const AngleOf<T>& angle) {
+  friend AngleOf<T> operator*(float factor, const AngleOf<T> &angle) {
     return AngleOf::Degrees((float)factor * angle.InDegrees());
   }
 
@@ -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
@@ -206,22 +205,18 @@ class AngleOf {
   /// @return The angle of the corner opposing side A
   static AngleOf<T> SineRuleAngle(float a, AngleOf<T> beta, float c);
 
- private:
+private:
   T value;
 
-  AngleOf(T rawValue);
+  AngleOf<T>(T rawValue);
 };
 
 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,9 +9,7 @@
 
 #include <math.h>
 
-namespace LinearAlgebra {
+template <typename T> DirectionOf<T>::DirectionOf() {
-template <typename T>
-DirectionOf<T>::DirectionOf() {
   this->horizontal = AngleOf<T>();
   this->vertical = AngleOf<T>();
 }
@@ -43,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;
@@ -51,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(
@@ -66,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);
@@ -99,6 +99,5 @@ void DirectionOf<T>::Normalize() {
   }
 }
 
-template class LinearAlgebra::DirectionOf<float>;
+template class Passer::LinearAlgebra::DirectionOf<float>;
-template class LinearAlgebra::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,20 +22,19 @@ 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:
- public:
   /// @brief horizontal angle, range= (-180..180]
   AngleOf<T> horizontal;
   /// @brief vertical angle, range in degrees = (-90..90]
   AngleOf<T> vertical;
 
   /// @brief Create a new direction with zero angles
-  DirectionOf();
+  DirectionOf<T>();
   /// @brief Create a new direction
   /// @param horizontal The horizontal angle
   /// @param vertical The vertical angle.
-  DirectionOf(AngleOf<T> horizontal, AngleOf<T> vertical);
+  DirectionOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical);
 
   /// @brief Convert the direction into a carthesian vector
   /// @return The carthesian vector corresponding to this direction.
@@ -83,7 +83,7 @@ class DirectionOf {
   /// @return The reversed direction.
   DirectionOf<T> operator-() const;
 
- protected:
+protected:
   /// @brief Normalize this vector to the specified ranges
   void Normalize();
 };
@@ -98,5 +98,7 @@ using Direction = DirectionSingle;
 #endif
 
 } // namespace LinearAlgebra
+} // namespace Passer
+using namespace Passer::LinearAlgebra;
 
 #endif

LinearAlgebra/FloatSingle.h

@@ -5,10 +5,11 @@
 #ifndef FLOAT_H
 #define FLOAT_H
 
+namespace Passer {
 namespace LinearAlgebra {
 
 class Float {
- public:
+public:
   static const float epsilon;
   static const float sqrEpsilon;
 
@@ -16,7 +17,7 @@ class Float {
 };
 
 } // namespace LinearAlgebra
-
+} // namespace Passer
-using namespace LinearAlgebra;
+using namespace Passer::LinearAlgebra;
 
 #endif

LinearAlgebra/Matrix.cpp

@@ -1,290 +1,6 @@
 #include "Matrix.h"
-#if !defined(NO_STD) 
-#include <iostream>
-#endif
 
-namespace LinearAlgebra {
+template <> MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
-
-#pragma region Matrix1
-
-Matrix1::Matrix1(int size) : size(size) {
-  if (this->size == 0)
-    data = nullptr;
-  else {
-    this->data = new float[size]();
-    this->externalData = false;
-  }
-}
-
-Matrix1::Matrix1(float* data, int size) : data(data), size(size) {
-  this->externalData = true;
-}
-
-Matrix1 LinearAlgebra::Matrix1::FromQuaternion(Quaternion q) {
-  Matrix1 r = Matrix1(4);
-  float* data = r.data;
-  data[0] = q.x;
-  data[1] = q.y;
-  data[2] = q.z;
-  data[3] = q.w;
-  return r;
-}
-
-Quaternion LinearAlgebra::Matrix1::ToQuaternion() {
-  return Quaternion(this->data[0], this->data[1], this->data[2], this->data[3]);
-}
-
-// Matrix1
-#pragma endregion
-
-#pragma region Matrix2
-
-Matrix2::Matrix2() {}
-
-Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) {
-  this->nValues = nRows * nCols;
-  if (this->nValues == 0)
-    this->data = nullptr;
-  else {
-    this->data = new float[this->nValues];
-    this->externalData = false;
-  }
-}
-
-Matrix2::Matrix2(float* data, int nRows, int nCols)
-    : nRows(nRows), nCols(nCols), data(data) {
-  this->nValues = nRows * nCols;
-  this->externalData = true;
-}
-
-Matrix2::Matrix2(const Matrix2& m)
-    : nRows(m.nRows), nCols(m.nCols), nValues(m.nValues) {
-  if (this->nValues == 0)
-    this->data = nullptr;
-  else {
-    this->data = new float[this->nValues];
-
-    for (int ix = 0; ix < this->nValues; ++ix)
-      this->data[ix] = m.data[ix];
-  }
-}
-
-Matrix2& Matrix2::operator=(const Matrix2& m) {
-  if (this != &m) {
-    delete[] this->data;  // Free the current memory
-
-    this->nRows = m.nRows;
-    this->nCols = m.nCols;
-    this->nValues = m.nValues;
-    if (this->nValues == 0)
-      this->data = nullptr;
-    else {
-      this->data = new float[this->nValues];
-      for (int ix = 0; ix < this->nValues; ++ix)
-        this->data[ix] = m.data[ix];
-    }
-  }
-  return *this;
-}
-
-Matrix2::~Matrix2() {
-  if (!this->externalData)
-    delete[] data;
-}
-
-Matrix2 Matrix2::Clone() const {
-  Matrix2 r = Matrix2(this->nRows, this->nCols);
-  for (int ix = 0; ix < this->nValues; ++ix)
-    r.data[ix] = this->data[ix];
-  return r;
-}
-
-// Move constructor
-Matrix2::Matrix2(Matrix2&& other) noexcept
-    : nRows(other.nRows),
-      nCols(other.nCols),
-      nValues(other.nValues),
-      data(other.data) {
-  other.data = nullptr;  // Set the other object's pointer to nullptr to avoid
-                         // double deletion
-}
-
-// Move assignment operator
-Matrix2& Matrix2::operator=(Matrix2&& other) noexcept {
-  if (this != &other) {
-    delete[] data;  // Clean up current data
-    nRows = other.nRows;
-    nCols = other.nCols;
-    nValues = other.nValues;
-    data = other.data;
-    other.data = nullptr;  // Avoid double deletion
-  }
-  return *this;
-}
-
-Matrix2 Matrix2::Zero(int nRows, int nCols) {
-  Matrix2 r = Matrix2(nRows, nCols);
-  for (int ix = 0; ix < r.nValues; ix++)
-    r.data[ix] = 0;
-  return r;
-}
-
-void Matrix2::Clear() {
-  for (int ix = 0; ix < this->nValues; ix++)
-    this->data[ix] = 0;
-}
-
-Matrix2 Matrix2::Identity(int size) {
-  return Diagonal(1, size);
-}
-
-Matrix2 Matrix2::Diagonal(float f, int size) {
-  Matrix2 r = Matrix2::Zero(size, size);
-  float* data = r.data;
-  int valueIx = 0;
-  for (int ix = 0; ix < size; ix++) {
-    data[valueIx] = f;
-    valueIx += size + 1;
-  }
-  return r;
-}
-
-Matrix2 Matrix2::SkewMatrix(const Vector3& v) {
-  Matrix2 r = Matrix2(3, 3);
-  float* data = r.data;
-  data[0 * 3 + 1] = -v.z;  // result(0, 1)
-  data[0 * 3 + 2] = v.y;   // result(0, 2)
-  data[1 * 3 + 0] = v.z;   // result(1, 0)
-  data[1 * 3 + 2] = -v.x;  // result(1, 2)
-  data[2 * 3 + 0] = -v.y;  // result(2, 0)
-  data[2 * 3 + 1] = v.x;   // result(2, 1)
-  return r;
-}
-
-Matrix2 Matrix2::Transpose() const {
-  Matrix2 r = Matrix2(this->nCols, this->nRows);
-
-  for (int rowIx = 0; rowIx < this->nRows; rowIx++) {
-    for (int colIx = 0; colIx < this->nCols; colIx++)
-      r.data[colIx * this->nCols + rowIx] =
-          this->data[rowIx * this->nCols + colIx];
-  }
-  return r;
-}
-
-Matrix2 LinearAlgebra::Matrix2::operator-() const {
-  Matrix2 r = Matrix2(this->nRows, this->nCols);
-  for (int ix = 0; ix < r.nValues; ix++)
-    r.data[ix] = -this->data[ix];
-  return r;
-}
-
-Matrix2 LinearAlgebra::Matrix2::operator+(const Matrix2& v) const {
-  Matrix2 r = Matrix2(this->nRows, this->nCols);
-  for (int ix = 0; ix < r.nValues; ix++)
-    r.data[ix] = this->data[ix] + v.data[ix];
-  return r;
-}
-
-Matrix2 Matrix2::operator+=(const Matrix2& v) {
-  for (int ix = 0; ix < this->nValues; ix++)
-    this->data[ix] += v.data[ix];
-  return *this;
-}
-
-Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const {
-  Matrix2 r = Matrix2(this->nRows, B.nCols);
-
-  int ACols = this->nCols;
-  int BCols = B.nCols;
-  int ARows = this->nRows;
-  // int BRows = B.nRows;
-
-  for (int i = 0; i < ARows; ++i) {
-    // Pre-compute row offsets
-    int ARowOffset = i * ACols;  // ARowOffset is constant for each row of A
-    int BColOffset = i * BCols;  // BColOffset is constant for each row of B
-    for (int j = 0; j < BCols; ++j) {
-      float sum = 0;
-      std::cout << " 0";
-      int BIndex = j;
-      for (int k = 0; k < ACols; ++k) {
-        std::cout << " + " << this->data[ARowOffset + k] << " * "
-                  << B.data[BIndex];
-        sum += this->data[ARowOffset + k] * B.data[BIndex];
-        BIndex += BCols;
-      }
-      r.data[BColOffset + j] = sum;
-      std::cout << " = " << sum << " ix: " << BColOffset + j << "\n";
-    }
-  }
-  return r;
-}
-
-Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) {
-  Matrix2 r = Matrix2(rowStop - rowStart, colStop - colStart);
-
-  int resultRowIx = 0;
-  int resultColIx = 0;
-  for (int i = rowStart; i < rowStop; i++) {
-    for (int j = colStart; j < colStop; j++)
-      r.data[resultRowIx * r.nCols + resultColIx] =
-          this->data[i * this->nCols + j];
-  }
-  return r;
-}
-
-void Matrix2::UpdateSlice(int rowStart,
-                          int rowStop,
-                          int colStart,
-                          int colStop,
-                          const Matrix2& m) const {
-  // for (int i = rowStart; i < rowStop; i++) {
-  //   for (int j = colStart; j < colStop; j++)
-  //     this->data[i * this->nCols + j] =
-  //         m.data[(i - rowStart) * m.nCols + (j - colStart)];
-  // }
-
-  int rRowDataIx = rowStart * this->nCols;
-  int mRowDataIx = 0;
-  for (int rowIx = rowStart; rowIx < rowStop; rowIx++) {
-    rRowDataIx = rowIx * this->nCols;
-    // rRowDataIx += this->nCols;
-    mRowDataIx += m.nCols;
-    for (int colIx = colStart; colIx < colStop; colIx++) {
-      this->data[rRowDataIx + colIx] = m.data[mRowDataIx + (colIx - colStart)];
-    }
-  }
-}
-
-/// @brief Compute the Omega matrix of a 3D vector
-/// @param v The vector
-/// @return 4x4 Omega matrix
-Matrix2 LinearAlgebra::Matrix2::Omega(const Vector3& v) {
-  Matrix2 r = Matrix2::Zero(4, 4);
-  r.UpdateSlice(0, 3, 0, 3, -Matrix2::SkewMatrix(v));
-
-  // set last row to -v
-  int ix = 3 * 4;
-  r.data[ix++] = -v.x;
-  r.data[ix++] = -v.y;
-  r.data[ix] = -v.z;
-
-  // Set last column to v
-  ix = 3;
-  r.data[ix += 4] = v.x;
-  r.data[ix += 4] = v.y;
-  r.data[ix] = v.z;
-  return r;
-}
-
-// Matrix2
-#pragma endregion
-
-}  // namespace LinearAlgebra
-
-template <>
-MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
   if (rows <= 0 || cols <= 0) {
     this->rows = 0;
     this->cols = 0;
@@ -298,17 +14,15 @@ MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
   this->data = new float[matrixSize]{0.0f};
 }
 
-template <>
+template <> MatrixOf<float>::MatrixOf(Vector3 v) : MatrixOf(3, 1) {
-MatrixOf<float>::MatrixOf(Vector3 v) : MatrixOf(3, 1) {
   Set(0, 0, v.Right());
   Set(1, 0, v.Up());
   Set(2, 0, v.Forward());
 }
 
 template <>
-void MatrixOf<float>::Multiply(const MatrixOf<float>* m1,
+void MatrixOf<float>::Multiply(const MatrixOf<float> *m1,
-                               const MatrixOf<float>* m2,
+                               const MatrixOf<float> *m2, MatrixOf<float> *r) {
-                               MatrixOf<float>* r) {
   for (unsigned int rowIx1 = 0; rowIx1 < m1->rows; rowIx1++) {
     for (unsigned int colIx2 = 0; colIx2 < m2->cols; colIx2++) {
       unsigned int rDataIx = colIx2 * m2->cols + rowIx1;
@@ -323,7 +37,7 @@ void MatrixOf<float>::Multiply(const MatrixOf<float>* m1,
 }
 
 template <>
-Vector3 MatrixOf<float>::Multiply(const MatrixOf<float>* m, Vector3 v) {
+Vector3 MatrixOf<float>::Multiply(const MatrixOf<float> *m, Vector3 v) {
   MatrixOf<float> v_m = MatrixOf<float>(v);
   MatrixOf<float> r_m = MatrixOf<float>(3, 1);
 
@@ -333,11 +47,10 @@ Vector3 MatrixOf<float>::Multiply(const MatrixOf<float>* m, Vector3 v) {
   return r;
 }
 
-template <typename T>
+template <typename T> Vector3 MatrixOf<T>::operator*(const Vector3 v) const {
-Vector3 MatrixOf<T>::operator*(const Vector3 v) const {
+  float *vData = new float[3]{v.Right(), v.Up(), v.Forward()};
-  float* vData = new float[3]{v.Right(), v.Up(), v.Forward()};
   MatrixOf<float> v_m = MatrixOf<float>(3, 1, vData);
-  float* rData = new float[3]{};
+  float *rData = new float[3]{};
   MatrixOf<float> r_m = MatrixOf<float>(3, 1, rData);
 
   Multiply(this, &v_m, &r_m);

LinearAlgebra/Matrix.h

@@ -1,125 +1,16 @@
 #ifndef MATRIX_H
 #define MATRIX_H
 
-#include "Quaternion.h"
 #include "Vector3.h"
 
+namespace Passer {
 namespace LinearAlgebra {
 
-/// @brief A 1-dimensional matrix or vector of arbitrary size
-class Matrix1 {
- public:
-  float* data = nullptr;
-  int size = 0;
-
-  Matrix1(int size);
-  Matrix1(float* data, int size);
-
-  static Matrix1 FromQuaternion(Quaternion q);
-  Quaternion ToQuaternion();
-
- private:
-  bool externalData = true;
-};
-
-/// @brief A 2-dimensional matrix of arbitrary size
-class Matrix2 {
- public:
-  int nRows = 0;
-  int nCols = 0;
-  int nValues = 0;
-  float* data = nullptr;
-
-  Matrix2();
-  Matrix2(int nRows, int nCols);
-  Matrix2(float* data, int nRows, int nCols);
-  Matrix2(const Matrix2& m);
-  Matrix2& operator=(const Matrix2& other);
-
-  ~Matrix2();
-
-  Matrix2 Clone() const;
-
-  static Matrix2 Zero(int nRows, int nCols);
-  void Clear();
-
-  static Matrix2 Identity(int size);
-
-  static Matrix2 Diagonal(float f, int size);
-
-  static Matrix2 SkewMatrix(const Vector3& v);
-
-  Matrix2 Transpose() const;
-
-  Matrix2 operator-() const;
-
-  /// @brief Add a matrix to this matrix
-  /// @param m The matrix to add to this matrix
-  /// @return The result of the addition
-  Matrix2 operator+(const Matrix2& v) const;
-  Matrix2 operator+=(const Matrix2& v);
-
-  Matrix2 operator*(const Matrix2& m) const;
-  friend Matrix2 operator*(const Matrix2& m, float f) {
-    Matrix2 r = Matrix2(m.nRows, m.nCols);
-    for (int ix = 0; ix < r.nValues; ix++)
-      r.data[ix] = m.data[ix] * f;
-    return r;
-  }
-  friend Matrix2 operator*(float f, const Matrix2& m) {
-    Matrix2 r = Matrix2(m.nRows, m.nCols);
-    for (int ix = 0; ix < r.nValues; ix++)
-      r.data[ix] = f * m.data[ix];
-    return r;
-  }
-
-  friend Matrix1 operator*(const Matrix2& m, const Matrix1& v) {
-    Matrix1 r = Matrix1(m.nRows);
-    for (int rowIx = 0; rowIx < m.nRows; rowIx++) {
-      int mRowIx = rowIx * m.nCols;
-      for (int colIx = 0; colIx < m.nCols; colIx++)
-        r.data[rowIx] += m.data[mRowIx + colIx] * v.data[rowIx];
-    }
-    return r;
-  }
-
-  friend Matrix2 operator/(const Matrix2& m, float f) {
-    Matrix2 r = Matrix2(m.nRows, m.nCols);
-    for (int ix = 0; ix < r.nValues; ix++)
-      r.data[ix] = m.data[ix] / f;
-    return r;
-  }
-  friend Matrix2 operator/(float f, const Matrix2& m) {
-    Matrix2 r = Matrix2(m.nRows, m.nCols);
-    for (int ix = 0; ix < r.nValues; ix++)
-      r.data[ix] = f / m.data[ix];
-    return r;
-  }
-
-  Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop);
-
-  void UpdateSlice(int rowStart,
-                int rowStop,
-                int colStart,
-                int colStop,
-                const Matrix2& m) const;
-  // private:
-  //  move constructor and move assignment operator
-  Matrix2(Matrix2&& other) noexcept;
-  Matrix2& operator=(Matrix2&& other) noexcept;
-
-  static Matrix2 Omega(const Vector3& v);
-
- private:
-  bool externalData = true;
-};
-
 /// @brief Single precision float matrix
-template <typename T>
+template <typename T> class MatrixOf {
-class MatrixOf {
+public:
- public:
   MatrixOf(unsigned int rows, unsigned int cols);
-  MatrixOf(unsigned int rows, unsigned int cols, const T* source)
+  MatrixOf(unsigned int rows, unsigned int cols, const T *source)
       : MatrixOf(rows, cols) {
     Set(source);
   }
@@ -134,7 +25,7 @@ class MatrixOf {
 
   /// @brief Transpose with result in matrix m
   /// @param r The matrix in which the transposed matrix is stored
-  void Transpose(MatrixOf<T>* r) const {
+  void Transpose(MatrixOf<T> *r) const {
     // Check dimensions first
     // We dont care about the rows and cols (we overwrite them)
     // but the data size should be equal to avoid problems
@@ -163,14 +54,13 @@ 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);
-                       MatrixOf<T>* r);
+  void Multiply(const MatrixOf<T> *m, MatrixOf<T> *r) const {
-  void Multiply(const MatrixOf<T>* m, MatrixOf<T>* r) const {
     Multiply(this, m, r);
   }
 
-  static Vector3 Multiply(const MatrixOf<T>* m, Vector3 v);
+  static Vector3 Multiply(const MatrixOf<T> *m, Vector3 v);
   Vector3 operator*(const Vector3 v) const;
 
   T Get(unsigned int rowIx, unsigned int colIx) const {
@@ -184,28 +74,28 @@ class MatrixOf {
   }
 
   // This function does not check on source size!
-  void Set(const T* source) {
+  void Set(const T *source) {
     unsigned int matrixSize = this->cols * this->rows;
     for (unsigned int dataIx = 0; dataIx < matrixSize; dataIx++)
       this->data[dataIx] = source[dataIx];
   }
 
   // This function does not check on source size!
-  void SetRow(unsigned int rowIx, const T* source) {
+  void SetRow(unsigned int rowIx, const T *source) {
     unsigned int dataIx = rowIx * this->cols;
     for (unsigned int sourceIx = 0; sourceIx < this->cols; dataIx++, sourceIx++)
       this->data[dataIx] = source[sourceIx];
   }
 
   // This function does not check on source size!
-  void SetCol(unsigned int colIx, const T* source) {
+  void SetCol(unsigned int colIx, const T *source) {
     unsigned int dataIx = colIx;
     for (unsigned int sourceIx = 0; sourceIx < this->cols;
          dataIx += this->cols, sourceIx++)
       this->data[dataIx] = source[sourceIx];
   }
 
-  void CopyFrom(const MatrixOf<T>* m) {
+  void CopyFrom(const MatrixOf<T> *m) {
     unsigned int thisMatrixSize = this->cols * this->rows;
     unsigned int mMatrixSize = m->cols * m->rows;
     if (mMatrixSize != thisMatrixSize)
@@ -218,13 +108,14 @@ class MatrixOf {
   unsigned int RowCount() const { return rows; }
   unsigned int ColCount() const { return cols; }
 
- private:
+private:
   unsigned int rows;
   unsigned int cols;
-  T* data;
+  T *data;
 };
 
 } // 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,36 +133,33 @@ 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;
 }
 
 template <typename T>
-float PolarOf<T>::Distance(const PolarOf& v1, const PolarOf& v2) {
+float PolarOf<T>::Distance(const PolarOf &v1, const PolarOf &v2) {
   float d =
       AngleOf<T>::CosineRuleSide(v1.distance, v2.distance, v2.angle - v1.angle);
   return d;
 }
 
 template <typename T>
-PolarOf<T> PolarOf<T>::Rotate(const PolarOf& v, AngleOf<T> angle) {
+PolarOf<T> PolarOf<T>::Rotate(const PolarOf &v, AngleOf<T> angle) {
   AngleOf<T> a = AngleOf<T>::Normalize(v.angle + angle);
   PolarOf<T> r = PolarOf(v.distance, a);
   return r;
 }
 
-template class LinearAlgebra::PolarOf<float>;
+template class Passer::LinearAlgebra::PolarOf<float>;
-template class LinearAlgebra::PolarOf<signed short>;
+template class Passer::LinearAlgebra::PolarOf<signed short>;

LinearAlgebra/Polar.h

@@ -7,17 +7,16 @@
 
 #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:
- public:
   /// @brief The distance in meters
   /// @remark The distance shall never be negative
   float distance;
@@ -77,12 +76,12 @@ class PolarOf {
   /// @return true: if it is identical to the given vector
   /// @note This uses float comparison to check equality which may have
   /// strange effects. Equality on floats should be avoided.
-  bool operator==(const PolarOf& v) const;
+  bool operator==(const PolarOf &v) const;
 
   /// @brief The vector length
   /// @param v The vector for which you need the length
   /// @return The vector length;
-  inline static float Magnitude(const PolarOf& v) { return v.distance; }
+  inline static float Magnitude(const PolarOf &v) { return v.distance; }
   /// @brief The vector length
   /// @return The vector length
   inline float magnitude() const { return this->distance; }
@@ -90,7 +89,7 @@ class PolarOf {
   /// @brief Convert the vector to a length of 1
   /// @param v The vector to convert
   /// @return The vector normalized to a length of 1
-  static PolarOf Normalize(const PolarOf& v);
+  static PolarOf Normalize(const PolarOf &v);
   /// @brief Convert the vector to a length of a
   /// @return The vector normalized to a length of 1
   PolarOf normalized() const;
@@ -103,23 +102,23 @@ class PolarOf {
   /// @brief Subtract a polar vector from this vector
   /// @param v The vector to subtract
   /// @return The result of the subtraction
-  PolarOf operator-(const PolarOf& v) const;
+  PolarOf operator-(const PolarOf &v) const;
-  PolarOf operator-=(const PolarOf& v);
+  PolarOf operator-=(const PolarOf &v);
   /// @brief Add a polar vector to this vector
   /// @param v The vector to add
   /// @return The result of the addition
-  PolarOf operator+(const PolarOf& v) const;
+  PolarOf operator+(const PolarOf &v) const;
-  PolarOf operator+=(const PolarOf& v);
+  PolarOf operator+=(const PolarOf &v);
 
   /// @brief Scale the vector uniformly up
   /// @param f The scaling factor
   /// @return The scaled vector
   /// @remark This operation will scale the distance of the vector. The angle
   /// will be unaffected.
-  friend PolarOf operator*(const PolarOf& v, float f) {
+  friend PolarOf operator*(const PolarOf &v, float f) {
     return PolarOf(v.distance * f, v.angle);
   }
-  friend PolarOf operator*(float f, const PolarOf& v) {
+  friend PolarOf operator*(float f, const PolarOf &v) {
     return PolarOf(f * v.distance, v.angle);
   }
   PolarOf operator*=(float f);
@@ -128,10 +127,10 @@ class PolarOf {
   /// @return The scaled factor
   /// @remark This operation will scale the distance of the vector. The angle
   /// will be unaffected.
-  friend PolarOf operator/(const PolarOf& v, float f) {
+  friend PolarOf operator/(const PolarOf &v, float f) {
     return PolarOf(v.distance / f, v.angle);
   }
-  friend PolarOf operator/(float f, const PolarOf& v) {
+  friend PolarOf operator/(float f, const PolarOf &v) {
     return PolarOf(f / v.distance, v.angle);
   }
   PolarOf operator/=(float f);
@@ -140,13 +139,13 @@ class PolarOf {
   /// @param v1 The first vector
   /// @param v2 The second vector
   /// @return The distance between the two vectors
-  static float Distance(const PolarOf& v1, const PolarOf& v2);
+  static float Distance(const PolarOf &v1, const PolarOf &v2);
 
   /// @brief Rotate a vector
   /// @param v The vector to rotate
   /// @param a The angle in degreesto rotate
   /// @return The rotated vector
-  static PolarOf Rotate(const PolarOf& v, AngleOf<T> a);
+  static PolarOf Rotate(const PolarOf &v, AngleOf<T> a);
 };
 
 using PolarSingle = PolarOf<float>;
@@ -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.cpp

@@ -6,7 +6,6 @@
 #include <float.h>
 #include <math.h>
 #include "Angle.h"
-#include "Matrix.h"
 #include "Vector3.h"
 
 void CopyQuat(const Quat& q1, Quat& q2) {
@@ -98,28 +97,6 @@ Vector3 Quaternion::ToAngles(const Quaternion& q1) {
   }
 }
 
-Matrix2 LinearAlgebra::Quaternion::ToRotationMatrix() {
-  Matrix2 r = Matrix2(3, 3);
-
-  float x = this->x;
-  float y = this->y;
-  float z = this->z;
-  float w = this->w;
-
-  float* data = r.data;
-  data[0 * 3 + 0] = 1 - 2 * (y * y + z * z);
-  data[0 * 3 + 1] = 2 * (x * y - w * z);
-  data[0 * 3 + 2] = 2 * (x * z + w * y);
-  data[1 * 3 + 0] = 2 * (x * y + w * z);
-  data[1 * 3 + 1] = 1 - 2 * (x * x + z * z);
-  data[1 * 3 + 2] = 2 * (y * z - w * x);
-  data[2 * 3 + 0] = 2 * (x * z - w * y);
-  data[2 * 3 + 1] = 2 * (y * z + w * x);
-  data[2 * 3 + 2] = 1 - 2 * (x * x + y * y);
-
-  return r;
-}
-
 Quaternion Quaternion::operator*(const Quaternion& r2) const {
   return Quaternion(
       this->x * r2.w + this->y * r2.z - this->z * r2.y + this->w * r2.x,

LinearAlgebra/Quaternion.h

@@ -32,15 +32,14 @@ typedef struct Quat {
 } Quat;
 }
 
+namespace Passer {
 namespace LinearAlgebra {
 
-class Matrix2;
-
 /// <summary>
 /// A quaternion
 /// </summary>
 struct Quaternion : Quat {
- public:
+public:
   /// <summary>
   /// Create a new identity quaternion
   /// </summary>
@@ -81,7 +80,7 @@ struct Quaternion : Quat {
   /// <returns>A unit quaternion</returns>
   /// This will preserve the orientation,
   /// but ensures that it is a unit quaternion.
-  static Quaternion Normalize(const Quaternion& q);
+  static Quaternion Normalize(const Quaternion &q);
 
   /// <summary>
   /// Convert to euler angles
@@ -89,16 +88,14 @@ struct Quaternion : Quat {
   /// <param name="q">The quaternion to convert</param>
   /// <returns>A vector containing euler angles</returns>
   /// The euler angles performed in the order: Z, X, Y
-  static Vector3 ToAngles(const Quaternion& q);
+  static Vector3 ToAngles(const Quaternion &q);
-
-  Matrix2 ToRotationMatrix();
 
   /// <summary>
   /// Rotate a vector using this quaterion
   /// </summary>
   /// <param name="vector">The vector to rotate</param>
   /// <returns>The rotated vector</returns>
-  Vector3 operator*(const Vector3& vector) const;
+  Vector3 operator*(const Vector3 &vector) const;
   /// <summary>
   /// Multiply this quaternion with another quaternion
   /// </summary>
@@ -106,7 +103,7 @@ struct Quaternion : Quat {
   /// <returns>The resulting rotation</returns>
   /// The result will be this quaternion rotated according to
   /// the give rotation.
-  Quaternion operator*(const Quaternion& rotation) const;
+  Quaternion operator*(const Quaternion &rotation) const;
 
   /// <summary>
   /// Check the equality of two quaternions
@@ -117,7 +114,7 @@ struct Quaternion : Quat {
   /// themselves. Two quaternions with the same rotational effect may have
   /// different components. Use Quaternion::Angle to check if the rotations are
   /// the same.
-  bool operator==(const Quaternion& quaternion) const;
+  bool operator==(const Quaternion &quaternion) const;
 
   /// <summary>
   /// The inverse of quaterion
@@ -132,8 +129,8 @@ struct Quaternion : Quat {
   /// <param name="forward">The look direction</param>
   /// <param name="upwards">The up direction</param>
   /// <returns>The look rotation</returns>
-  static Quaternion LookRotation(const Vector3& forward,
+  static Quaternion LookRotation(const Vector3 &forward,
-                                 const Vector3& upwards);
+                                 const Vector3 &upwards);
   /// <summary>
   /// Creates a quaternion with the given forward direction with up =
   /// Vector3::up
@@ -143,7 +140,7 @@ struct Quaternion : Quat {
   /// For the rotation, Vector::up is used for the up direction.
   /// Note: if the forward direction == Vector3::up, the result is
   /// Quaternion::identity
-  static Quaternion LookRotation(const Vector3& forward);
+  static Quaternion LookRotation(const Vector3 &forward);
 
   /// <summary>
   /// Calculat the rotation from on vector to another
@@ -160,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>
@@ -170,13 +166,13 @@ struct Quaternion : Quat {
   /// <param name="angle">The angle</param>
   /// <param name="axis">The axis</param>
   /// <returns>The resulting quaternion</returns>
-  static Quaternion AngleAxis(float angle, const Vector3& axis);
+  static Quaternion AngleAxis(float angle, const Vector3 &axis);
   /// <summary>
   /// Convert this quaternion to angle/axis representation
   /// </summary>
   /// <param name="angle">A pointer to the angle for the result</param>
   /// <param name="axis">A pointer to the axis for the result</param>
-  void ToAngleAxis(float* angle, Vector3* axis);
+  void ToAngleAxis(float *angle, Vector3 *axis);
 
   /// <summary>
   /// Get the angle between two orientations
@@ -194,9 +190,8 @@ struct Quaternion : Quat {
   /// <param name="factor">The factor between 0 and 1.</param>
   /// <returns>The resulting rotation</returns>
   /// A factor 0 returns rotation1, factor1 returns rotation2.
-  static Quaternion Slerp(const Quaternion& rotation1,
+  static Quaternion Slerp(const Quaternion &rotation1,
-                          const Quaternion& rotation2,
+                          const Quaternion &rotation2, float factor);
-                          float factor);
   /// <summary>
   /// Unclamped sherical lerp between two rotations
   /// </summary>
@@ -206,9 +201,8 @@ struct Quaternion : Quat {
   /// <returns>The resulting rotation</returns>
   /// 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,
+  static Quaternion SlerpUnclamped(const Quaternion &rotation1,
-                                   const Quaternion& rotation2,
+                                   const Quaternion &rotation2, float factor);
-                                   float factor);
 
   /// <summary>
   /// Create a rotation from euler angles
@@ -266,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
@@ -279,12 +271,12 @@ struct Quaternion : Quat {
   /// <returns></returns>
   static float Dot(Quaternion rotation1, Quaternion rotation2);
 
- private:
+private:
   float GetLength() const;
   float GetLengthSquared() const;
-  static float GetLengthSquared(const Quaternion& q);
+  static float GetLengthSquared(const Quaternion &q);
 
-  void ToAxisAngleRad(const Quaternion& q, Vector3* const axis, float* angle);
+  void ToAxisAngleRad(const Quaternion &q, Vector3 *const axis, float *angle);
   static Quaternion FromEulerRad(Vector3 euler);
   static Quaternion FromEulerRadXYZ(Vector3 euler);
 
@@ -292,6 +284,7 @@ struct Quaternion : Quat {
 };
 
 } // namespace LinearAlgebra
-using namespace LinearAlgebra;
+} // namespace Passer
+using namespace Passer::LinearAlgebra;
 
 #endif

LinearAlgebra/Spherical.cpp

@@ -5,17 +5,13 @@
 
 #include <math.h>
 
-namespace LinearAlgebra {
+template <typename T> SphericalOf<T>::SphericalOf() {
-
-template <typename T>
-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;
@@ -38,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);
@@ -48,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));
@@ -63,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>());
@@ -88,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();
 
@@ -134,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));
@@ -143,7 +134,7 @@ SphericalOf<T> SphericalOf<T>::operator-() const {
 }
 
 template <typename T>
-SphericalOf<T> SphericalOf<T>::operator-(const SphericalOf<T>& s2) const {
+SphericalOf<T> SphericalOf<T>::operator-(const SphericalOf<T> &s2) const {
   // let's do it the easy way...
   Vector3 v1 = this->ToVector3();
   Vector3 v2 = s2.ToVector3();
@@ -152,13 +143,13 @@ SphericalOf<T> SphericalOf<T>::operator-(const SphericalOf<T>& s2) const {
   return r;
 }
 template <typename T>
-SphericalOf<T> SphericalOf<T>::operator-=(const SphericalOf<T>& v) {
+SphericalOf<T> SphericalOf<T>::operator-=(const SphericalOf<T> &v) {
   *this = *this - v;
   return *this;
 }
 
 template <typename T>
-SphericalOf<T> SphericalOf<T>::operator+(const SphericalOf<T>& s2) const {
+SphericalOf<T> SphericalOf<T>::operator+(const SphericalOf<T> &s2) const {
   // let's do it the easy way...
   Vector3 v1 = this->ToVector3();
   Vector3 v2 = s2.ToVector3();
@@ -213,19 +204,17 @@ SphericalOf<T> SphericalOf<T>::operator+(const SphericalOf<T>& s2) const {
   */
 }
 template <typename T>
-SphericalOf<T> SphericalOf<T>::operator+=(const SphericalOf<T>& v) {
+SphericalOf<T> SphericalOf<T>::operator+=(const SphericalOf<T> &v) {
   *this = *this + 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;
 }
@@ -236,8 +225,8 @@ SphericalOf<T> SphericalOf<T>::operator/=(float f) {
 const float epsilon = 1E-05f;
 
 template <typename T>
-float SphericalOf<T>::DistanceBetween(const SphericalOf<T>& v1,
+float SphericalOf<T>::DistanceBetween(const SphericalOf<T> &v1,
-                                      const SphericalOf<T>& v2) {
+                                      const SphericalOf<T> &v2) {
   // SphericalOf<T> difference = v1 - v2;
   // return difference.distance;
   Vector3 vec1 = v1.ToVector3();
@@ -247,8 +236,8 @@ float SphericalOf<T>::DistanceBetween(const SphericalOf<T>& v1,
 }
 
 template <typename T>
-AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf& v1,
+AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf &v1,
-                                        const SphericalOf& v2) {
+                                        const SphericalOf &v2) {
   // float denominator = v1.distance * v2.distance;
   // if (denominator < epsilon)
   //   return 0.0f;
@@ -267,9 +256,9 @@ 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) {
+    const SphericalOf<T> &axis) {
   Vector3 v1_vector = v1.ToVector3();
   Vector3 v2_vector = v2.ToVector3();
   Vector3 axis_vector = axis.ToVector3();
@@ -278,7 +267,7 @@ AngleOf<T> SphericalOf<T>::SignedAngleBetween(const SphericalOf<T>& v1,
 }
 
 template <typename T>
-SphericalOf<T> SphericalOf<T>::Rotate(const SphericalOf<T>& v,
+SphericalOf<T> SphericalOf<T>::Rotate(const SphericalOf<T> &v,
                                       AngleOf<T> horizontalAngle,
                                       AngleOf<T> verticalAngle) {
   SphericalOf<T> r =
@@ -287,21 +276,19 @@ SphericalOf<T> SphericalOf<T>::Rotate(const SphericalOf<T>& v,
   return r;
 }
 template <typename T>
-SphericalOf<T> SphericalOf<T>::RotateHorizontal(const SphericalOf<T>& v,
+SphericalOf<T> SphericalOf<T>::RotateHorizontal(const SphericalOf<T> &v,
                                                 AngleOf<T> a) {
   SphericalOf<T> r =
       SphericalOf(v.distance, v.direction.horizontal + a, v.direction.vertical);
   return r;
 }
 template <typename T>
-SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T>& v,
+SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T> &v,
                                               AngleOf<T> a) {
   SphericalOf<T> r =
       SphericalOf(v.distance, v.direction.horizontal, v.direction.vertical + a);
   return r;
 }
 
-template class SphericalOf<float>;
+template class Passer::LinearAlgebra::SphericalOf<float>;
-template class SphericalOf<signed short>;
+template class Passer::LinearAlgebra::SphericalOf<signed short>;
-
-}  // namespace LinearAlgebra

LinearAlgebra/Spherical.h

@@ -7,26 +7,30 @@
 
 #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:
- public:
   /// @brief The distance in meters
   /// @remark The distance should never be negative
   float distance;
-  /// @brief The direction of the vector
+  /// @brief The angle in the horizontal plane in degrees, clockwise rotation
+  /// @details The angle is automatically normalized to -180 .. 180
+  // AngleOf<T> horizontal;
+  /// @brief The angle in the vertical plane in degrees. Positive is upward.
+  /// @details The angle is automatically normalized to -180 .. 180
+  // AngleOf<T> vertical;
   DirectionOf<T> direction;
 
-  SphericalOf();
+  SphericalOf<T>();
-  SphericalOf(float distance, AngleOf<T> horizontal, AngleOf<T> vertical);
+  SphericalOf<T>(float distance, AngleOf<T> horizontal, AngleOf<T> vertical);
-  SphericalOf(float distance, DirectionOf<T> direction);
+  SphericalOf<T>(float distance, DirectionOf<T> direction);
 
   /// @brief Create spherical vector without using AngleOf type. All given
   /// angles are in degrees
@@ -34,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;
@@ -45,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;
@@ -93,23 +95,23 @@ class SphericalOf {
   /// @brief Subtract a spherical vector from this vector
   /// @param v The vector to subtract
   /// @return The result of the subtraction
-  SphericalOf<T> operator-(const SphericalOf<T>& v) const;
+  SphericalOf<T> operator-(const SphericalOf<T> &v) const;
-  SphericalOf<T> operator-=(const SphericalOf<T>& v);
+  SphericalOf<T> operator-=(const SphericalOf<T> &v);
   /// @brief Add a spherical vector to this vector
   /// @param v The vector to add
   /// @return The result of the addition
-  SphericalOf<T> operator+(const SphericalOf<T>& v) const;
+  SphericalOf<T> operator+(const SphericalOf<T> &v) const;
-  SphericalOf<T> operator+=(const SphericalOf<T>& v);
+  SphericalOf<T> operator+=(const SphericalOf<T> &v);
 
   /// @brief Scale the vector uniformly up
   /// @param f The scaling factor
   /// @return The scaled vector
   /// @remark This operation will scale the distance of the vector. The angle
   /// will be unaffected.
-  friend SphericalOf<T> operator*(const SphericalOf<T>& v, float f) {
+  friend SphericalOf<T> operator*(const SphericalOf<T> &v, float f) {
     return SphericalOf<T>(v.distance * f, v.direction);
   }
-  friend SphericalOf<T> operator*(float f, const SphericalOf<T>& v) {
+  friend SphericalOf<T> operator*(float f, const SphericalOf<T> &v) {
     return SphericalOf<T>(f * v.distance, v.direction);
   }
   SphericalOf<T> operator*=(float f);
@@ -118,10 +120,10 @@ class SphericalOf {
   /// @return The scaled factor
   /// @remark This operation will scale the distance of the vector. The angle
   /// will be unaffected.
-  friend SphericalOf<T> operator/(const SphericalOf<T>& v, float f) {
+  friend SphericalOf<T> operator/(const SphericalOf<T> &v, float f) {
     return SphericalOf<T>(v.distance / f, v.direction);
   }
-  friend SphericalOf<T> operator/(float f, const SphericalOf<T>& v) {
+  friend SphericalOf<T> operator/(float f, const SphericalOf<T> &v) {
     return SphericalOf<T>(f / v.distance, v.direction);
   }
   SphericalOf<T> operator/=(float f);
@@ -130,42 +132,41 @@ class SphericalOf {
   /// @param v1 The first coordinate
   /// @param v2 The second coordinate
   /// @return The distance between the coordinates in meters
-  static float DistanceBetween(const SphericalOf<T>& v1,
+  static float DistanceBetween(const SphericalOf<T> &v1,
-                               const SphericalOf<T>& v2);
+                               const SphericalOf<T> &v2);
   /// @brief Calculate the unsigned angle between two spherical vectors
   /// @param v1 The first vector
   /// @param v2 The second vector
   /// @return The unsigned angle between the vectors
-  static AngleOf<T> AngleBetween(const SphericalOf<T>& v1,
+  static AngleOf<T> AngleBetween(const SphericalOf<T> &v1,
-                                 const SphericalOf<T>& v2);
+                                 const SphericalOf<T> &v2);
   /// @brief Calculate the signed angle between two spherical vectors
   /// @param v1 The first vector
   /// @param v2 The second vector
   /// @param axis The axis are which the angle is calculated
   /// @return The signed angle between the vectors
-  static AngleOf<T> SignedAngleBetween(const SphericalOf<T>& v1,
+  static AngleOf<T> SignedAngleBetween(const SphericalOf<T> &v1,
-                                       const SphericalOf<T>& v2,
+                                       const SphericalOf<T> &v2,
-                                       const SphericalOf<T>& axis);
+                                       const SphericalOf<T> &axis);
 
   /// @brief Rotate a spherical vector
   /// @param v The vector to rotate
   /// @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
   /// @param angle The horizontal rotation angle in local space
   /// @return The rotated vector
-  static SphericalOf<T> RotateHorizontal(const SphericalOf<T>& v,
+  static SphericalOf<T> RotateHorizontal(const SphericalOf<T> &v,
                                          AngleOf<T> angle);
   /// @brief Rotate a spherical vector vertically
   /// @param v The vector to rotate
   /// @param angle The vertical rotation angle in local space
   /// @return The rotated vector
-  static SphericalOf<T> RotateVertical(const SphericalOf<T>& v,
+  static SphericalOf<T> RotateVertical(const SphericalOf<T> &v,
                                        AngleOf<T> angle);
 };
 
@@ -186,6 +187,8 @@ using Spherical = SphericalSingle;
 #endif
 
 } // namespace LinearAlgebra
+} // namespace Passer
+using namespace Passer::LinearAlgebra;
 
 #include "Polar.h"
 #include "Vector3.h"

LinearAlgebra/SwingTwist.cpp

@@ -4,8 +4,6 @@
 
 #include "SwingTwist.h"
 
-namespace LinearAlgebra {
-
 template <typename T>
 SwingTwistOf<T>::SwingTwistOf() {
   this->swing = DirectionOf<T>(AngleOf<T>(), AngleOf<T>());
@@ -166,7 +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,23 +10,22 @@
 #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:
- public:
   DirectionOf<T> swing;
   AngleOf<T> twist;
 
-  SwingTwistOf();
+  SwingTwistOf<T>();
-  SwingTwistOf(DirectionOf<T> swing, AngleOf<T> twist);
+  SwingTwistOf<T>(DirectionOf<T> swing, AngleOf<T> twist);
-  SwingTwistOf(AngleOf<T> horizontal, AngleOf<T> vertical, 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;
@@ -44,7 +43,7 @@ class SwingTwistOf {
   /// </summary>
   /// <param name="vector">The vector to rotate</param>
   /// <returns>The rotated vector</returns>
-  SphericalOf<T> operator*(const SphericalOf<T>& vector) const;
+  SphericalOf<T> operator*(const SphericalOf<T> &vector) const;
   /// <summary>
   /// Multiply this rotation with another rotation
   /// </summary>
@@ -52,8 +51,8 @@ class SwingTwistOf {
   /// <returns>The resulting swing/twist rotation</returns>
   /// The result will be this rotation rotated according to
   /// the give rotation.
-  SwingTwistOf<T> operator*(const SwingTwistOf<T>& rotation) const;
+  SwingTwistOf<T> operator*(const SwingTwistOf<T> &rotation) const;
-  SwingTwistOf<T> operator*=(const SwingTwistOf<T>& rotation);
+  SwingTwistOf<T> operator*=(const SwingTwistOf<T> &rotation);
 
   static SwingTwistOf<T> Inverse(SwingTwistOf<T> rotation);
 
@@ -63,9 +62,9 @@ class SwingTwistOf {
   /// <param name="angle">The angle</param>
   /// <param name="axis">The axis</param>
   /// <returns>The resulting quaternion</returns>
-  static SwingTwistOf<T> AngleAxis(float angle, const DirectionOf<T>& axis);
+  static SwingTwistOf<T> AngleAxis(float angle, const DirectionOf<T> &axis);
 
-  static AngleOf<T> Angle(const SwingTwistOf<T>& r1, const SwingTwistOf<T>& r2);
+  static AngleOf<T> Angle(const SwingTwistOf<T> &r1, const SwingTwistOf<T> &r2);
 
   void Normalize();
 };
@@ -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);
 
@@ -49,24 +49,18 @@ const Vector2 Vector2::down = Vector2(0, -1);
 const Vector2 Vector2::forward = Vector2(0, 1);
 const Vector2 Vector2::back = Vector2(0, -1);
 
-bool Vector2::operator==(const Vector2& v) {
+bool Vector2::operator==(const Vector2 &v) {
   return (this->x == v.x && this->y == v.y);
 }
 
-float Vector2::Magnitude(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) {
+Vector2 Vector2::Normalize(const Vector2 &v) {
   float num = Vector2::Magnitude(v);
   Vector2 result = Vector2::zero;
   if (num > Float::epsilon) {
@@ -83,28 +77,26 @@ 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 {
+Vector2 Vector2::operator-(const Vector2 &v) const {
   return Vector2(this->x - v.x, this->y - v.y);
 }
-Vector2 Vector2::operator-=(const Vector2& v) {
+Vector2 Vector2::operator-=(const Vector2 &v) {
   this->x -= v.x;
   this->y -= v.y;
   return *this;
 }
-Vector2 Vector2::operator+(const Vector2& v) const {
+Vector2 Vector2::operator+(const Vector2 &v) const {
   return Vector2(this->x + v.x, this->y + v.y);
 }
-Vector2 Vector2::operator+=(const Vector2& v) {
+Vector2 Vector2::operator+=(const Vector2 &v) {
   this->x += v.x;
   this->y += v.y;
   return *this;
 }
 
-Vector2 Vector2::Scale(const Vector2& v1, const Vector2& v2) {
+Vector2 Vector2::Scale(const Vector2 &v1, const Vector2 &v2) {
   return Vector2(v1.x * v2.x, v1.y * v2.y);
 }
 // Vector2 Passer::LinearAlgebra::operator*(const Vector2 &v, float f) {
@@ -130,18 +122,18 @@ Vector2 Vector2::operator/=(float f) {
   return *this;
 }
 
-float Vector2::Dot(const Vector2& v1, const Vector2& v2) {
+float Vector2::Dot(const Vector2 &v1, const Vector2 &v2) {
   return v1.x * v2.x + v1.y * v2.y;
 }
 
-float Vector2::Distance(const Vector2& v1, const Vector2& v2) {
+float Vector2::Distance(const Vector2 &v1, const Vector2 &v2) {
   return Magnitude(v1 - v2);
 }
 
-float Vector2::Angle(const Vector2& v1, const Vector2& v2) {
+float Vector2::Angle(const Vector2 &v1, const Vector2 &v2) {
   return (float)fabs(SignedAngle(v1, v2));
 }
-float Vector2::SignedAngle(const Vector2& v1, const Vector2& v2) {
+float Vector2::SignedAngle(const Vector2 &v1, const Vector2 &v2) {
   float sqrMagFrom = v1.sqrMagnitude();
   float sqrMagTo = v2.sqrMagnitude();
 
@@ -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);
@@ -176,7 +169,7 @@ Vector2 Vector2::Rotate(const Vector2& v, AngleSingle a) {
   return r;
 }
 
-Vector2 Vector2::Lerp(const Vector2& v1, const Vector2& v2, float f) {
+Vector2 Vector2::Lerp(const Vector2 &v1, const Vector2 &v2, float f) {
   Vector2 v = v1 + (v2 - v1) * f;
   return v;
 }

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.
@@ -38,7 +38,7 @@ class PolarOf;
 struct Vector2 : Vec2 {
   friend struct Vec2;
 
- public:
+public:
   /// @brief A new 2-dimensional zero vector
   Vector2();
   /// @brief A new 2-dimensional vector
@@ -80,12 +80,12 @@ struct Vector2 : Vec2 {
   /// @return true if it is identical to the given vector
   /// @note This uses float comparison to check equality which may have strange
   /// effects. Equality on floats should be avoided.
-  bool operator==(const Vector2& v);
+  bool operator==(const Vector2 &v);
 
   /// @brief The vector length
   /// @param v The vector for which you need the length
   /// @return The vector length
-  static float Magnitude(const Vector2& v);
+  static float Magnitude(const Vector2 &v);
   /// @brief The vector length
   /// @return The vector length
   float magnitude() const;
@@ -95,7 +95,7 @@ struct Vector2 : Vec2 {
   /// @remark The squared length is computationally simpler than the real
   /// length. Think of Pythagoras A^2 + B^2 = C^2. This prevents the calculation
   /// of the squared root of C.
-  static float SqrMagnitude(const Vector2& v);
+  static float SqrMagnitude(const Vector2 &v);
   /// @brief The squared vector length
   /// @return The squared vector length
   /// @remark The squared length is computationally simpler than the real
@@ -106,7 +106,7 @@ struct Vector2 : Vec2 {
   /// @brief Convert the vector to a length of 1
   /// @param v The vector to convert
   /// @return The vector normalized to a length of 1
-  static Vector2 Normalize(const Vector2& v);
+  static Vector2 Normalize(const Vector2 &v);
   /// @brief Convert the vector to a length 1
   /// @return The vector normalized to a length of 1
   Vector2 normalized() const;
@@ -118,13 +118,13 @@ struct Vector2 : Vec2 {
   /// @brief Subtract a vector from this vector
   /// @param v The vector to subtract from this vector
   /// @return The result of the subtraction
-  Vector2 operator-(const Vector2& v) const;
+  Vector2 operator-(const Vector2 &v) const;
-  Vector2 operator-=(const Vector2& v);
+  Vector2 operator-=(const Vector2 &v);
   /// @brief Add a vector to this vector
   /// @param v The vector to add to this vector
   /// @return The result of the addition
-  Vector2 operator+(const Vector2& v) const;
+  Vector2 operator+(const Vector2 &v) const;
-  Vector2 operator+=(const Vector2& v);
+  Vector2 operator+=(const Vector2 &v);
 
   /// @brief Scale the vector using another vector
   /// @param v1 The vector to scale
@@ -132,16 +132,16 @@ struct Vector2 : Vec2 {
   /// @return The scaled vector
   /// @remark Each component of the vector v1 will be multiplied with the
   /// matching component from the scaling vector v2.
-  static Vector2 Scale(const Vector2& v1, const Vector2& v2);
+  static Vector2 Scale(const Vector2 &v1, const Vector2 &v2);
   /// @brief Scale the vector uniformly up
   /// @param f The scaling factor
   /// @return The scaled vector
   /// @remark Each component of the vector will be multipled with the same
   /// factor f.
-  friend Vector2 operator*(const Vector2& v, float f) {
+  friend Vector2 operator*(const Vector2 &v, float f) {
     return Vector2(v.x * f, v.y * f);
   }
-  friend Vector2 operator*(float f, const Vector2& v) {
+  friend Vector2 operator*(float f, const Vector2 &v) {
     return Vector2(v.x * f, v.y * f);
     // return Vector2(f * v.x, f * v.y);
   }
@@ -150,10 +150,10 @@ struct Vector2 : Vec2 {
   /// @param f The scaling factor
   /// @return The scaled vector
   /// @remark Each componet of the vector will be divided by the same factor.
-  friend Vector2 operator/(const Vector2& v, float f) {
+  friend Vector2 operator/(const Vector2 &v, float f) {
     return Vector2(v.x / f, v.y / f);
   }
-  friend Vector2 operator/(float f, const Vector2& v) {
+  friend Vector2 operator/(float f, const Vector2 &v) {
     return Vector2(f / v.x, f / v.y);
   }
   Vector2 operator/=(float f);
@@ -162,13 +162,13 @@ struct Vector2 : Vec2 {
   /// @param v1 The first vector
   /// @param v2 The second vector
   /// @return The dot product of the two vectors
-  static float Dot(const Vector2& v1, const Vector2& v2);
+  static float Dot(const Vector2 &v1, const Vector2 &v2);
 
   /// @brief The distance between two vectors
   /// @param v1 The first vector
   /// @param v2 The second vector
   /// @return The distance between the two vectors
-  static float Distance(const Vector2& v1, const Vector2& v2);
+  static float Distance(const Vector2 &v1, const Vector2 &v2);
 
   /// @brief The angle between two vectors
   /// @param v1 The first vector
@@ -177,18 +177,18 @@ struct Vector2 : Vec2 {
   /// @remark This reterns an unsigned angle which is the shortest distance
   /// between the two vectors. Use Vector2::SignedAngle if a signed angle is
   /// needed.
-  static float Angle(const Vector2& v1, const Vector2& v2);
+  static float Angle(const Vector2 &v1, const Vector2 &v2);
   /// @brief The signed angle between two vectors
   /// @param v1 The starting vector
   /// @param v2 The ending vector
   /// @return The signed angle between the two vectors
-  static float SignedAngle(const Vector2& v1, const Vector2& v2);
+  static float SignedAngle(const Vector2 &v1, const Vector2 &v2);
 
   /// @brief Rotate the vector
   /// @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
@@ -198,11 +198,12 @@ struct Vector2 : Vec2 {
   /// @remark The factor f is unclamped. Value 0 matches the vector *v1*, Value
   /// 1 matches vector *v2*. Value -1 is vector *v1* minus the difference
   /// between *v1* and *v2* etc.
-  static Vector2 Lerp(const Vector2& v1, const Vector2& v2, float f);
+  static Vector2 Lerp(const Vector2 &v1, const Vector2 &v2, float f);
 };
 
 } // 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);
@@ -65,21 +67,17 @@ const Vector3 Vector3::back = Vector3(0, 0, -1);
 //   return Vector3(v.x, 0, v.y);
 // }
 
-float Vector3::Magnitude(const Vector3& v) {
+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) {
+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) {
+Vector3 Vector3::Normalize(const Vector3 &v) {
   float num = Vector3::Magnitude(v);
   Vector3 result = Vector3::zero;
   if (num > epsilon) {
@@ -100,26 +98,26 @@ Vector3 Vector3::operator-() const {
   return Vector3(-this->x, -this->y, -this->z);
 }
 
-Vector3 Vector3::operator-(const Vector3& v) const {
+Vector3 Vector3::operator-(const Vector3 &v) const {
   return Vector3(this->x - v.x, this->y - v.y, this->z - v.z);
 }
-Vector3 Vector3::operator-=(const Vector3& v) {
+Vector3 Vector3::operator-=(const Vector3 &v) {
   this->x -= v.x;
   this->y -= v.y;
   this->z -= v.z;
   return *this;
 }
-Vector3 Vector3::operator+(const Vector3& v) const {
+Vector3 Vector3::operator+(const Vector3 &v) const {
   return Vector3(this->x + v.x, this->y + v.y, this->z + v.z);
 }
-Vector3 Vector3::operator+=(const Vector3& v) {
+Vector3 Vector3::operator+=(const Vector3 &v) {
   this->x += v.x;
   this->y += v.y;
   this->z += v.z;
   return *this;
 }
 
-Vector3 Vector3::Scale(const Vector3& v1, const Vector3& v2) {
+Vector3 Vector3::Scale(const Vector3 &v1, const Vector3 &v2) {
   return Vector3(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z);
 }
 // Vector3 Passer::LinearAlgebra::operator*(const Vector3 &v, float f) {
@@ -147,24 +145,24 @@ Vector3 Vector3::operator/=(float f) {
   return *this;
 }
 
-float Vector3::Dot(const Vector3& v1, const Vector3& v2) {
+float Vector3::Dot(const Vector3 &v1, const Vector3 &v2) {
   return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
 }
 
-bool Vector3::operator==(const Vector3& v) const {
+bool Vector3::operator==(const Vector3 &v) const {
   return (this->x == v.x && this->y == v.y && this->z == v.z);
 }
 
-float Vector3::Distance(const Vector3& v1, const Vector3& v2) {
+float Vector3::Distance(const Vector3 &v1, const Vector3 &v2) {
   return Magnitude(v1 - v2);
 }
 
-Vector3 Vector3::Cross(const Vector3& v1, const Vector3& v2) {
+Vector3 Vector3::Cross(const Vector3 &v1, const Vector3 &v2) {
   return Vector3(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z,
                  v1.x * v2.y - v1.y * v2.x);
 }
 
-Vector3 Vector3::Project(const Vector3& v, const Vector3& n) {
+Vector3 Vector3::Project(const Vector3 &v, const Vector3 &n) {
   float sqrMagnitude = Dot(n, n);
   if (sqrMagnitude < epsilon)
     return Vector3::zero;
@@ -175,7 +173,7 @@ Vector3 Vector3::Project(const Vector3& v, const Vector3& n) {
   }
 }
 
-Vector3 Vector3::ProjectOnPlane(const Vector3& v, const Vector3& n) {
+Vector3 Vector3::ProjectOnPlane(const Vector3 &v, const Vector3 &n) {
   Vector3 r = v - Project(v, n);
   return r;
 }
@@ -186,7 +184,7 @@ float clamp(float x, float lower, float upper) {
   return upperClamp;
 }
 
-AngleOf<float> Vector3::Angle(const Vector3& v1, const Vector3& v2) {
+AngleOf<float> Vector3::Angle(const Vector3 &v1, const Vector3 &v2) {
   float denominator = sqrtf(v1.sqrMagnitude() * v2.sqrMagnitude());
   if (denominator < epsilon)
     return AngleOf<float>();
@@ -202,9 +200,8 @@ 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) {
-                                    const Vector3& axis) {
   // angle in [0,180]
   AngleOf<float> angle = Vector3::Angle(v1, v2);
 
@@ -218,7 +215,7 @@ AngleOf<float> Vector3::SignedAngle(const Vector3& v1,
   return AngleOf<float>(signed_angle);
 }
 
-Vector3 Vector3::Lerp(const Vector3& v1, const Vector3& v2, float f) {
+Vector3 Vector3::Lerp(const Vector3 &v1, const Vector3 &v2, float f) {
   Vector3 v = v1 + (v2 - v1) * f;
   return v;
 }

LinearAlgebra/Vector3.h

@@ -14,7 +14,7 @@ extern "C" {
 /// This is a C-style implementation
 /// This uses the right-handed coordinate system.
 typedef struct Vec3 {
- public:
+protected:
   /// <summary>
   /// The right axis of the vector
   /// </summary>
@@ -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.
@@ -42,7 +42,7 @@ class SphericalOf;
 struct Vector3 : Vec3 {
   friend struct Vec3;
 
- public:
+public:
   /// @brief A new 3-dimensional zero vector
   Vector3();
   /// @brief A new 3-dimensional vector
@@ -88,12 +88,12 @@ struct Vector3 : Vec3 {
   /// @return true if it is identical to the given vector
   /// @note This uses float comparison to check equality which may have strange
   /// effects. Equality on floats should be avoided.
-  bool operator==(const Vector3& v) const;
+  bool operator==(const Vector3 &v) const;
 
   /// @brief The vector length
   /// @param v The vector for which you need the length
   /// @return The vector length
-  static float Magnitude(const Vector3& v);
+  static float Magnitude(const Vector3 &v);
   /// @brief The vector length
   /// @return The vector length
   float magnitude() const;
@@ -103,7 +103,7 @@ struct Vector3 : Vec3 {
   /// @remark The squared length is computationally simpler than the real
   /// length. Think of Pythagoras A^2 + B^2 = C^2. This leaves out the
   /// calculation of the squared root of C.
-  static float SqrMagnitude(const Vector3& v);
+  static float SqrMagnitude(const Vector3 &v);
   /// @brief The squared vector length
   /// @return The squared vector length
   /// @remark The squared length is computationally simpler than the real
@@ -114,7 +114,7 @@ struct Vector3 : Vec3 {
   /// @brief Convert the vector to a length of 1
   /// @param v The vector to convert
   /// @return The vector normalized to a length of 1
-  static Vector3 Normalize(const Vector3& v);
+  static Vector3 Normalize(const Vector3 &v);
   /// @brief Convert the vector to a length of 1
   /// @return The vector normalized to a length of 1
   Vector3 normalized() const;
@@ -126,13 +126,13 @@ struct Vector3 : Vec3 {
   /// @brief Subtract a vector from this vector
   /// @param v The vector to subtract from this vector
   /// @return The result of this subtraction
-  Vector3 operator-(const Vector3& v) const;
+  Vector3 operator-(const Vector3 &v) const;
-  Vector3 operator-=(const Vector3& v);
+  Vector3 operator-=(const Vector3 &v);
   /// @brief Add a vector to this vector
   /// @param v The vector to add to this vector
   /// @return The result of the addition
-  Vector3 operator+(const Vector3& v) const;
+  Vector3 operator+(const Vector3 &v) const;
-  Vector3 operator+=(const Vector3& v);
+  Vector3 operator+=(const Vector3 &v);
 
   /// @brief Scale the vector using another vector
   /// @param v1 The vector to scale
@@ -140,16 +140,16 @@ struct Vector3 : Vec3 {
   /// @return The scaled vector
   /// @remark Each component of the vector v1 will be multiplied with the
   /// matching component from the scaling vector v2.
-  static Vector3 Scale(const Vector3& v1, const Vector3& v2);
+  static Vector3 Scale(const Vector3 &v1, const Vector3 &v2);
   /// @brief Scale the vector uniformly up
   /// @param f The scaling factor
   /// @return The scaled vector
   /// @remark Each component of the vector will be multipled with the same
   /// factor f.
-  friend Vector3 operator*(const Vector3& v, float f) {
+  friend Vector3 operator*(const Vector3 &v, float f) {
     return Vector3(v.x * f, v.y * f, v.z * f);
   }
-  friend Vector3 operator*(float f, const Vector3& v) {
+  friend Vector3 operator*(float f, const Vector3 &v) {
     // return Vector3(f * v.x, f * v.y, f * v.z);
     return Vector3(v.x * f, v.y * f, v.z * f);
   }
@@ -158,10 +158,10 @@ struct Vector3 : Vec3 {
   /// @param f The scaling factor
   /// @return The scaled vector
   /// @remark Each componet of the vector will be divided by the same factor.
-  friend Vector3 operator/(const Vector3& v, float f) {
+  friend Vector3 operator/(const Vector3 &v, float f) {
     return Vector3(v.x / f, v.y / f, v.z / f);
   }
-  friend Vector3 operator/(float f, const Vector3& v) {
+  friend Vector3 operator/(float f, const Vector3 &v) {
     // return Vector3(f / v.x, f / v.y, f / v.z);
     return Vector3(v.x / f, v.y / f, v.z / f);
   }
@@ -171,31 +171,31 @@ struct Vector3 : Vec3 {
   /// @param v1 The first vector
   /// @param v2 The second vector
   /// @return The distance between the two vectors
-  static float Distance(const Vector3& v1, const Vector3& v2);
+  static float Distance(const Vector3 &v1, const Vector3 &v2);
 
   /// @brief The dot product of two vectors
   /// @param v1 The first vector
   /// @param v2 The second vector
   /// @return The dot product of the two vectors
-  static float Dot(const Vector3& v1, const Vector3& v2);
+  static float Dot(const Vector3 &v1, const Vector3 &v2);
 
   /// @brief The cross product of two vectors
   /// @param v1 The first vector
   /// @param v2 The second vector
   /// @return The cross product of the two vectors
-  static Vector3 Cross(const Vector3& v1, const Vector3& v2);
+  static Vector3 Cross(const Vector3 &v1, const Vector3 &v2);
 
   /// @brief Project the vector on another vector
   /// @param v The vector to project
   /// @param n The normal vecto to project on
   /// @return The projected vector
-  static Vector3 Project(const Vector3& v, const Vector3& n);
+  static Vector3 Project(const Vector3 &v, const Vector3 &n);
   /// @brief Project the vector on a plane defined by a normal orthogonal to the
   /// plane.
   /// @param v The vector to project
   /// @param n The normal of the plane to project on
   /// @return Teh projected vector
-  static Vector3 ProjectOnPlane(const Vector3& v, const Vector3& n);
+  static Vector3 ProjectOnPlane(const Vector3 &v, const Vector3 &n);
 
   /// @brief The angle between two vectors
   /// @param v1 The first vector
@@ -204,15 +204,14 @@ struct Vector3 : Vec3 {
   /// @remark This reterns an unsigned angle which is the shortest distance
   /// between the two vectors. Use Vector3::SignedAngle if a signed angle is
   /// needed.
-  static AngleOf<float> Angle(const Vector3& v1, const Vector3& v2);
+  static AngleOf<float> Angle(const Vector3 &v1, const Vector3 &v2);
   /// @brief The signed angle between two vectors
   /// @param v1 The starting vector
   /// @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);
-                                    const Vector3& axis);
 
   /// @brief Lerp (linear interpolation) between two vectors
   /// @param v1 The starting vector
@@ -222,11 +221,12 @@ struct Vector3 : Vec3 {
   /// @remark The factor f is unclamped. Value 0 matches the vector *v1*, Value
   /// 1 matches vector *v2*. Value -1 is vector *v1* minus the difference
   /// between *v1* and *v2* etc.
-  static Vector3 Lerp(const Vector3& v1, const Vector3& v2, float f);
+  static Vector3 Lerp(const Vector3 &v1, const Vector3 &v2, float f);
 };
 
 } // 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) {

LinearAlgebra/test/Direction_test.cc

@@ -6,8 +6,6 @@
 
 #include "Direction.h"
 
-using namespace LinearAlgebra;
-
 #define FLOAT_INFINITY std::numeric_limits<float>::infinity()
 
 TEST(Direction16, Compare) {

LinearAlgebra/test/Matrix_test.cc

@@ -1,90 +1,10 @@
 #if GTEST
 #include <gtest/gtest.h>
-#include <math.h>
 #include <limits>
+#include <math.h>
 
 #include "Matrix.h"
 
-TEST(Matrix2, Zero) {
-      // Test case 1: 2x2 zero matrix
-      Matrix2 zeroMatrix = Matrix2::Zero(2, 2);
-      EXPECT_TRUE(zeroMatrix.nRows == 2);
-      EXPECT_TRUE(zeroMatrix.nCols == 2);
-      for (int i = 0; i < zeroMatrix.nValues; ++i) {
-        EXPECT_TRUE(zeroMatrix.data[i] == 0.0f);
-      }
-      std::cout << "Test case 1 passed: 2x2 zero matrix\n";
-  
-      // Test case 2: 3x3 zero matrix
-      zeroMatrix = Matrix2::Zero(3, 3);
-      EXPECT_TRUE(zeroMatrix.nRows == 3);
-      EXPECT_TRUE(zeroMatrix.nCols == 3);
-      for (int i = 0; i < zeroMatrix.nValues; ++i) {
-        EXPECT_TRUE(zeroMatrix.data[i] == 0.0f);
-      }
-      std::cout << "Test case 2 passed: 3x3 zero matrix\n";
-  
-      // Test case 3: 1x1 zero matrix
-      zeroMatrix = Matrix2::Zero(1, 1);
-      EXPECT_TRUE(zeroMatrix.nRows == 1);
-      EXPECT_TRUE(zeroMatrix.nCols == 1);
-      EXPECT_TRUE(zeroMatrix.data[0] == 0.0f);
-      std::cout << "Test case 3 passed: 1x1 zero matrix\n";
-  
-      // Test case 4: 0x0 matrix (edge case)
-      zeroMatrix = Matrix2::Zero(0, 0);
-      EXPECT_TRUE(zeroMatrix.nRows == 0);
-      EXPECT_TRUE(zeroMatrix.nCols == 0);
-      EXPECT_TRUE(zeroMatrix.data == nullptr);
-      std::cout << "Test case 4 passed: 0x0 matrix\n";
-}
-
-TEST(Matrix2, Multiplication) {
-  // Test 1: Multiplying two 2x2 matrices
-  float dataA[] = {1, 2, 3, 4};
-  float dataB[] = {5, 6, 7, 8};
-  Matrix2 A(dataA, 2, 2);
-  Matrix2 B(dataB, 2, 2);
-
-  Matrix2 result = A * B;
-
-  float expectedData[] = {19, 22, 43, 50};
-  for (int i = 0; i < 4; ++i) 
-    EXPECT_TRUE(result.data[i] == expectedData[i]);  
-  std::cout << "Test 1 passed: 2x2 matrix multiplication.\n";
-
-  
-  // Test 2: Multiplying a 3x2 matrix with a 2x3 matrix
-  float dataC[] = {1, 2, 3, 4, 5, 6};
-  float dataD[] = {7, 8, 9, 10, 11, 12};
-  Matrix2 C(dataC, 3, 2);
-  Matrix2 D(dataD, 2, 3);
-
-  Matrix2 result2 = C * D;
-
-  float expectedData2[] = {27, 30, 33, 61, 68, 75, 95, 106, 117};
-  for (int i = 0; i < 9; ++i)
-    EXPECT_TRUE(result2.data[i] == expectedData2[i]);
-  std::cout << "Test 2 passed: 3x2 * 2x3 matrix multiplication.\n";
-  
-  // Test 3: Multiplying with a zero matrix
-  Matrix2 zeroMatrix = Matrix2::Zero(2, 2);
-  Matrix2 result3 = A * zeroMatrix;
-
-  for (int i = 0; i < 4; ++i)
-    EXPECT_TRUE(result3.data[i] == 0);
-  std::cout << "Test 3 passed: Multiplication with zero matrix.\n";
-  
-  // Test 4: Multiplying with an identity matrix
-  Matrix2 identityMatrix = Matrix2::Identity(2);
-  Matrix2 result4 = A * identityMatrix;
-
-  for (int i = 0; i < 4; ++i)
-    EXPECT_TRUE(result4.data[i] == A.data[i]);
-  std::cout << "Test 4 passed: Multiplication with identity matrix.\n";
-  
-}
-
 TEST(MatrixSingle, Init) {
   // zero
   MatrixOf<float> m0 = MatrixOf<float>(0, 0);

LinearAlgebra/test/Polar_test.cc

@@ -2,7 +2,6 @@
 #include <gtest/gtest.h>
 #include <limits>
 #include <math.h>
-#include <chrono>
 
 #include "Polar.h"
 #include "Spherical.h"

LinearAlgebra/test/Spherical16_test.cc

@@ -2,7 +2,6 @@
 #include <gtest/gtest.h>
 #include <limits>
 #include <math.h>
-#include <chrono>
 
 #include "Spherical.h"
 #include "Vector3.h"

LinearAlgebra/test/SphericalSingle_test.cc

@@ -2,7 +2,6 @@
 #include <gtest/gtest.h>
 #include <limits>
 #include <math.h>
-#include <chrono>
 
 #include "Spherical.h"
 

LocalParticipant.cpp

@@ -0,0 +1,330 @@
+#include "LocalParticipant.h"
+
+#include "Thing.h"
+
+#include "Arduino/ArduinoParticipant.h"
+
+#if defined(_WIN32) || defined(_WIN64)
+#include "Windows/WindowsParticipant.h"
+#include <winsock2.h>
+#include <ws2tcpip.h>
+#pragma comment(lib, "ws2_32.lib")
+
+#elif defined(__unix__) || defined(__APPLE__)
+#include "Posix/PosixParticipant.h"
+#include <arpa/inet.h>
+#include <fcntl.h>  // For fcntl
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#include <chrono>
+#endif
+
+namespace RoboidControl {
+
+// LocalParticipant::LocalParticipant() {}
+
+LocalParticipant::LocalParticipant(int port) {
+  this->ipAddress = "0.0.0.0";
+  this->port = port;
+  if (this->port == 0)
+    this->isIsolated = true;
+}
+
+LocalParticipant::LocalParticipant(const char* ipAddress, int port) {
+  this->ipAddress = "0.0.0.0";  // ipAddress;  // maybe this is not needed anymore, keeping it to "0.0.0.0"
+  this->port = port;
+  if (this->port == 0)
+    this->isIsolated = true;
+  else
+    this->remoteSite = new Participant(ipAddress, port);
+}
+
+static LocalParticipant* isolatedParticipant = nullptr;
+
+LocalParticipant* LocalParticipant::Isolated() {
+  if (isolatedParticipant == nullptr)
+    isolatedParticipant = new LocalParticipant(0);
+  return isolatedParticipant;
+}
+
+void LocalParticipant::begin() {
+  if (this->isIsolated)
+    return;
+
+  SetupUDP(this->port, this->ipAddress, this->port);
+}
+
+void LocalParticipant::SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) {
+#if defined(_WIN32) || defined(_WIN64)
+  Windows::LocalParticipant* thisWindows = static_cast<Windows::LocalParticipant*>(this);
+  thisWindows->Setup(localPort, remoteIpAddress, remotePort);
+#elif defined(__unix__) || defined(__APPLE__)
+  Posix::LocalParticipant* thisPosix = static_cast<Posix::LocalParticipant*>(this);
+  thisPosix->Setup(localPort, remoteIpAddress, remotePort);
+#elif defined(ARDUINO)
+  Arduino::LocalParticipant* thisArduino = static_cast<Arduino::LocalParticipant*>(this);
+  thisArduino->Setup(localPort, remoteIpAddress, remotePort);
+#endif
+  this->connected = true;
+}
+
+void LocalParticipant::Update(unsigned long currentTimeMs) {
+  if (currentTimeMs == 0) {
+#if defined(ARDUINO)
+    currentTimeMs = millis();
+#elif defined(__unix__) || defined(__APPLE__)
+    auto now = std::chrono::steady_clock::now();
+    auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch());
+    currentTimeMs = static_cast<unsigned long>(ms.count());
+#endif
+  }
+
+  if (this->isIsolated == false) {
+    if (this->connected == false)
+      begin();
+
+    if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
+      ParticipantMsg* msg = new ParticipantMsg(this->networkId);
+      if (this->remoteSite == nullptr)
+        this->Publish(msg);
+      else
+        this->Send(this->remoteSite, msg);
+      delete msg;
+
+      this->nextPublishMe = currentTimeMs + this->publishInterval;
+    }
+    this->ReceiveUDP();
+  }
+
+  for (Thing* thing : this->things) {
+    if (thing != nullptr) {
+      thing->Update(currentTimeMs);
+      if (this->isIsolated == false) {
+        PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
+        for (Participant* sender : this->senders)
+          this->Send(sender, poseMsg);
+        delete poseMsg;
+      }
+    }
+  }
+}
+
+void LocalParticipant::ReceiveUDP() {
+#if defined(_WIN32) || defined(_WIN64)
+  Windows::LocalParticipant* thisWindows = static_cast<Windows::LocalParticipant*>(this);
+  thisWindows->Receive();
+#elif defined(__unix__) || defined(__APPLE__)
+  Posix::LocalParticipant* thisPosix = static_cast<Posix::LocalParticipant*>(this);
+  thisPosix->Receive();
+#elif defined(ARDUINO)
+  Arduino::LocalParticipant* thisArduino = static_cast<Arduino::LocalParticipant*>(this);
+  thisArduino->Receive();
+#endif
+}
+
+Participant* LocalParticipant::GetParticipant(const char* ipAddress, int port) {
+  for (Participant* sender : this->senders) {
+    if (strcmp(sender->ipAddress, ipAddress) == 0 && sender->port == port)
+      return sender;
+  }
+  return nullptr;
+}
+
+Participant* LocalParticipant::AddParticipant(const char* ipAddress, int port) {
+  std::cout << "New Participant " << ipAddress << ":" << port << "\n";
+  Participant* participant = new Participant(ipAddress, port);
+  participant->networkId = (unsigned char)this->senders.size();
+  this->senders.push_back(participant);
+  return participant;
+}
+
+#pragma region Send
+
+void LocalParticipant::SendThingInfo(Participant* remoteParticipant, Thing* thing) {
+  std::cout << "Send thing info " << (int)thing->id << " \n";
+  ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
+  this->Send(remoteParticipant, thingMsg);
+  delete thingMsg;
+  NameMsg* nameMsg = new NameMsg(this->networkId, thing);
+  this->Send(remoteParticipant, nameMsg);
+  delete nameMsg;
+  ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
+  this->Send(remoteParticipant, modelMsg);
+  delete modelMsg;
+  PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
+  this->Send(remoteParticipant, poseMsg);
+  delete poseMsg;
+  BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
+  this->Send(remoteParticipant, customMsg);
+  delete customMsg;
+}
+
+bool LocalParticipant::Send(Participant* remoteParticipant, IMessage* msg) {
+  int bufferSize = msg->Serialize(this->buffer);
+  if (bufferSize <= 0)
+    return true;
+
+#if defined(_WIN32) || defined(_WIN64)
+  Windows::LocalParticipant* thisWindows = static_cast<Windows::LocalParticipant*>(this);
+  return thisWindows->Send(remoteParticipant, bufferSize);
+#elif defined(__unix__) || defined(__APPLE__)
+  Posix::LocalParticipant* thisPosix = static_cast<Posix::LocalParticipant*>(this);
+  return thisPosix->Send(remoteParticipant, bufferSize);
+#elif defined(ARDUINO)
+  Arduino::LocalParticipant* thisArduino = static_cast<Arduino::LocalParticipant*>(this);
+  return thisArduino->Send(remoteParticipant, bufferSize);
+#endif
+}
+
+void LocalParticipant::PublishThingInfo(Thing* thing) {
+  // std::cout << "Publish thing info" << thing->networkId << "\n";
+  //  Strange, when publishing, the network id is irrelevant, because it is
+  //  connected to a specific site...
+  ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
+  this->Publish(thingMsg);
+  delete thingMsg;
+  NameMsg* nameMsg = new NameMsg(this->networkId, thing);
+  this->Publish(nameMsg);
+  delete nameMsg;
+  ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
+  this->Publish(modelMsg);
+  delete modelMsg;
+  PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
+  this->Publish(poseMsg);
+  delete poseMsg;
+  BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
+  this->Publish(customMsg);
+  delete customMsg;
+}
+
+bool LocalParticipant::Publish(IMessage* msg) {
+#if defined(_WIN32) || defined(_WIN64)
+  Windows::LocalParticipant* thisWindows = static_cast<Windows::LocalParticipant*>(this);
+  return thisWindows->Publish(msg);
+#elif defined(__unix__) || defined(__APPLE__)
+  Posix::LocalParticipant* thisPosix = static_cast<Posix::LocalParticipant*>(this);
+  return thisPosix->Publish(msg);
+#elif defined(ARDUINO)
+  Arduino::LocalParticipant* thisArduino = static_cast<Arduino::LocalParticipant*>(this);
+  return thisArduino->Publish(msg);
+#endif
+}
+
+// Send
+#pragma endregion
+
+#pragma region Receive
+
+void LocalParticipant::ReceiveData(unsigned char packetSize, char* senderIpAddress, unsigned int senderPort) {
+  Participant* remoteParticipant = this->GetParticipant(senderIpAddress, senderPort);
+  if (remoteParticipant == nullptr) {
+    remoteParticipant = this->AddParticipant(senderIpAddress, senderPort);
+    // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
+    //           << "\n";
+    // std::cout << "New remote participant " << remoteParticipant->ipAddress
+    //           << ":" << remoteParticipant->port << " "
+    //           << (int)remoteParticipant->networkId << "\n";
+  }
+
+  ReceiveData(packetSize, remoteParticipant);
+}
+
+void LocalParticipant::ReceiveData(unsigned char bufferSize, Participant* remoteParticipant) {
+  unsigned char msgId = this->buffer[0];
+  // std::cout << "receive msg " << (int)msgId << "\n";
+  switch (msgId) {
+    case ParticipantMsg::id: {
+      ParticipantMsg* msg = new ParticipantMsg(this->buffer);
+      Process(remoteParticipant, msg);
+      delete msg;
+    } break;
+    case SiteMsg::id: {
+      SiteMsg* msg = new SiteMsg(this->buffer);
+      Process(remoteParticipant, msg);
+      delete msg;
+    } break;
+    case InvestigateMsg::id: {
+      InvestigateMsg* msg = new InvestigateMsg(this->buffer);
+      Process(remoteParticipant, msg);
+      delete msg;
+    } break;
+    case ThingMsg::id: {
+      ThingMsg* msg = new ThingMsg(this->buffer);
+      Process(remoteParticipant, msg);
+      delete msg;
+    } break;
+    case NameMsg::id: {
+      NameMsg* msg = new NameMsg(this->buffer);
+      Process(remoteParticipant, msg);
+      delete msg;
+    } break;
+    case PoseMsg::id: {
+      PoseMsg* msg = new PoseMsg(this->buffer);
+      Process(remoteParticipant, msg);
+      delete msg;
+    } break;
+    case BinaryMsg::id: {
+      BinaryMsg* msg = new BinaryMsg(this->buffer);
+      Process(remoteParticipant, msg);
+      delete msg;
+    } break;
+  };
+}
+
+void LocalParticipant::Process(Participant* sender, ParticipantMsg* msg) {}
+
+void LocalParticipant::Process(Participant* sender, SiteMsg* msg) {
+  std::cout << this->name << ": process NetworkId [" << (int)this->networkId << "/" << (int)msg->networkId << "]\n";
+  if (this->networkId != msg->networkId) {
+    this->networkId = msg->networkId;
+    std::cout << this->things.size() << " things\n";
+    for (Thing* thing : this->things)
+      this->SendThingInfo(sender, thing);
+  }
+}
+
+void LocalParticipant::Process(Participant* sender, InvestigateMsg* msg) {}
+
+void LocalParticipant::Process(Participant* sender, ThingMsg* msg) {}
+
+void LocalParticipant::Process(Participant* sender, NameMsg* msg) {
+  Thing* thing = sender->Get(msg->networkId, msg->thingId);
+  if (thing != nullptr) {
+    int nameLength = msg->nameLength;
+    int stringLen = nameLength + 1;
+    char* thingName = new char[stringLen];
+#if defined(_WIN32) || defined(_WIN64)
+    strncpy_s(thingName, stringLen, msg->name, stringLen - 1);  // Leave space for null terminator
+#else
+    // Use strncpy with bounds checking for other platforms (Arduino, POSIX, ESP-IDF)
+    strncpy(thingName, msg->name, stringLen - 1);  // Leave space for null terminator
+    thingName[stringLen - 1] = '\0';               // Ensure null termination
+#endif
+    thingName[nameLength] = '\0';
+    thing->name = thingName;
+    std::cout << "thing name = " << thing->name << " length = " << nameLength << "\n";
+  }
+}
+
+void LocalParticipant::Process(Participant* sender, PoseMsg* msg) {}
+
+void LocalParticipant::Process(Participant* sender, BinaryMsg* msg) {
+  // std::cout << this->name << ": process Binary [" << (int)this->networkId << "/"
+  //           << (int)msg->networkId << "]\n";
+  Thing* thing = sender->Get(msg->networkId, msg->thingId);
+  if (thing != nullptr)
+    thing->ProcessBinary(msg->bytes);
+  else {
+    thing = this->Get(msg->networkId, msg->thingId);
+    if (thing != nullptr)
+      thing->ProcessBinary(msg->bytes);
+    else
+      std::cout << "custom msg for unknown thing [" << (int)msg->networkId << "/" << (int)msg->thingId << "]\n";
+  }
+}
+
+// Receive
+#pragma endregion
+
+}  // namespace RoboidControl

LocalParticipant.h

@@ -1,21 +1,16 @@
 #pragma once
 
 #include "Messages/BinaryMsg.h"
-#include "Messages/DestroyMsg.h"
 #include "Messages/InvestigateMsg.h"
 #include "Messages/ModelUrlMsg.h"
 #include "Messages/NameMsg.h"
 #include "Messages/ParticipantMsg.h"
 #include "Messages/PoseMsg.h"
-#include "Messages/NetworkIdMsg.h"
+#include "Messages/SiteMsg.h"
 #include "Messages/ThingMsg.h"
 #include "Participant.h"
 
-#if !defined(NO_STD)
-#include <functional>
 #include <list>
-// #include <unordered_map>
-#endif
 
 #if defined(_WIN32) || defined(_WIN64)
 #include <winsock2.h>
@@ -24,92 +19,74 @@
 #include <netinet/in.h>
 #include <sys/socket.h>
 #include <unistd.h>
+#elif defined(ARDUINO)
+#include <WiFiUdp.h>
 #endif
 
 namespace RoboidControl {
 
-constexpr int MAX_SENDER_COUNT = 256;
+/// @brief A local participant is the local device which can communicate with other participants
-
+/// It manages all local things and communcation with other participants.
-/// @brief A participant using UDP communication
+/// Each application has a local participant which is usually explicit in the code.
-/// A local participant is the local device which can communicate with
+/// An participant can be isolated. In that case it is standalong and does not communicate with other participants.
-/// other participants It manages all local things and communcation with other
-/// participants. Each application has a local participant which is usually
-/// explicit in the code. An participant can be isolated. In that case it is
-/// standalong and does not communicate with other participants.
 ///
-/// It is possible to work with an hidden participant by creating things without
+/// It is possible to work with an hidden participant by creating things without specifying a participant in the
-/// specifying a participant in the constructor. In that case an hidden isolated
+/// constructor. In that case an hidden isolated participant is created which can be
-/// participant is created which can be obtained using
+/// obtained using RoboidControl::LocalParticipant::Isolated().
-/// RoboidControl::IsolatedParticipant::Isolated().
 /// @sa RoboidControl::Thing::Thing()
-class ParticipantUDP : public Participant {
+class LocalParticipant : public Participant {
-#pragma region Init
-
  public:
   /// @brief Create a participant without connecting to a site
   /// @param port The port on which the participant communicates
-  /// These participant typically broadcast Participant messages to let site
+  /// These participant typically broadcast Participant messages to let site servers on the local network know their presence.
-  /// servers on the local network know their presence. Alternatively they can
+  /// Alternatively they can broadcast information which can be used directly by other participants.
-  /// broadcast information which can be used directly by other participants.
+  LocalParticipant(int port = 7681);
-  ParticipantUDP(int port = 7681);
   /// @brief Create a participant which will try to connect to a site.
   /// @param ipAddress The IP address of the site
   /// @param port The port used by the site
-  /// @param localPort The port used by the local participant
+  LocalParticipant(const char* ipAddress, int port = 7681);
-  ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681);
+  // Note to self: one cannot specify the port used by the local participant now!!
 
-  /// @brief Isolated participant is used when the application is run without
+  /// @brief Isolated participant is used when the application is run without networking
-  /// networking
   /// @return A participant without networking support
-  static ParticipantUDP* Isolated();
+  static LocalParticipant* Isolated();
-
-  /// @brief True if the participant is running isolated.
-  /// Isolated participants do not communicate with other participants
-
-#pragma endregion Init
 
   /// @brief True if the participant is running isolated.
   /// Isolated participants do not communicate with other participants
   bool isIsolated = false;
+
+  /// The interval in milliseconds for publishing (broadcasting) data on the local network
+  long publishInterval = 3000;  // 3 seconds
+
+  /// @brief The name of the participant
+  const char* name = "LocalParticipant";
+
+  // int localPort = 0;
+
   /// @brief The remote site when this participant is connected to a site
   Participant* remoteSite = nullptr;
 
-  /// The interval in milliseconds for publishing (broadcasting) data on the
+#if defined(ARDUINO)
-  /// local network
+  const char* remoteIpAddress = nullptr;
-  long publishInterval = 3000;  // 3 seconds
+  unsigned short remotePort = 0;
+  char* broadcastIpAddress = nullptr;
 
- protected:
+  WiFiUDP udp;
-  char buffer[1024];
+#else
 
-#if !defined(ARDUINO)
 #if defined(__unix__) || defined(__APPLE__)
   int sock;
-#elif defined(_WIN32) || defined(_WIN64)
+#endif
   sockaddr_in remote_addr;
   sockaddr_in server_addr;
   sockaddr_in broadcast_addr;
+
 #endif
-#endif
+
- public:
   void begin();
   bool connected = false;
 
-#pragma region Update
+  virtual void Update(unsigned long currentTimeMs = 0);
-
- public:
-  virtual void Update() override;
-
- protected:
-  unsigned long nextPublishMe = 0;
-
-  /// @brief Prepare the local things for the next update
-  virtual void PrepMyThings();
-  virtual void UpdateMyThings();
-  virtual void UpdateOtherThings();
-
-#pragma endregion Update
-
-#pragma region Send
 
   void SendThingInfo(Participant* remoteParticipant, Thing* thing);
   void PublishThingInfo(Thing* thing);
@@ -117,31 +94,30 @@ class ParticipantUDP : public Participant {
   bool Send(Participant* remoteParticipant, IMessage* msg);
   bool Publish(IMessage* msg);
 
-#pragma endregion Send
+  void ReceiveData(unsigned char bufferSize, char* senderIpAddress, unsigned int senderPort);
-
-#pragma region Receive
-
-protected:
-  void ReceiveData(unsigned char bufferSize,
-                   char* senderIpAddress,
-                   unsigned int senderPort);
   void ReceiveData(unsigned char bufferSize, Participant* remoteParticipant);
 
+  std::list<Participant*> senders;
+
+ protected:
+  unsigned long nextPublishMe = 0;
+
+  char buffer[1024];
+
   void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort);
 
+  Participant* GetParticipant(const char* ipAddress, int port);
+  Participant* AddParticipant(const char* ipAddress, int port);
+
   void ReceiveUDP();
 
   virtual void Process(Participant* sender, ParticipantMsg* msg);
-  virtual void Process(Participant* sender, NetworkIdMsg* msg);
+  virtual void Process(Participant* sender, SiteMsg* msg);
   virtual void Process(Participant* sender, InvestigateMsg* msg);
   virtual void Process(Participant* sender, ThingMsg* msg);
   virtual void Process(Participant* sender, NameMsg* msg);
-  virtual void Process(Participant* sender, ModelUrlMsg* msg);
   virtual void Process(Participant* sender, PoseMsg* msg);
   virtual void Process(Participant* sender, BinaryMsg* msg);
-
-#pragma endregion Receive
-
 };
 
 }  // namespace RoboidControl

Messages/BinaryMsg.cpp

@@ -2,48 +2,33 @@
 
 namespace RoboidControl {
 
-BinaryMsg::BinaryMsg(unsigned char networkId, Thing* thing) {
-  this->networkId = networkId;
-  this->thingId = thing->id;
-  this->thing = thing;
-  unsigned char ix = 0; //BinaryMsg::length;
-  this->data = new char[255];
-  this->dataLength = this->thing->GenerateBinary(this->data, &ix);
-}
-
 BinaryMsg::BinaryMsg(char* buffer) {
   unsigned char ix = 1;
   this->networkId = buffer[ix++];
   this->thingId = buffer[ix++];
-  this->dataLength = buffer[ix++];
+  this->bytes = buffer + ix;  // This is only valid because the code ensures the the msg
-  char* data = new char[this->dataLength];
+                              // lifetime is shorter than the buffer lifetime...
-  for (int i = 0; i < this->dataLength; i++)
-    data[i] = buffer[ix++];
-  this->data = data;
 }
 
-BinaryMsg::~BinaryMsg() {
+BinaryMsg::BinaryMsg(unsigned char networkId, Thing* thing) {
-  delete[] this->data;
+  this->networkId = networkId;
+  this->thingId = thing->id;
+  this->thing = thing;
 }
 
+BinaryMsg::~BinaryMsg() {}
+
 unsigned char BinaryMsg::Serialize(char* buffer) {
-  // unsigned char ix = this->length;
+  unsigned char ix = this->length;
-  //  this->dataLength = this->thing->GenerateBinary(buffer, &ix);
+  this->thing->GenerateBinary(buffer, &ix);
-  if (this->dataLength <= 0)  // in this case, no data is actually sent
+  if (ix <= this->length)  // in this case, no data is actually sent
     return 0;
 
-#if defined(DEBUG)
+  buffer[0] = this->id;
-  std::cout << "Send BinaryMsg [" << (int)this->networkId << "/" << (int)this->thingId
+  buffer[1] = this->networkId;
-            << "] " << (int)this->dataLength << std::endl;
+  buffer[2] = this->thingId;
-#endif
+  return ix;
-  unsigned char ix = 0;
-  buffer[ix++] = this->id;
-  buffer[ix++] = this->networkId;
-  buffer[ix++] = this->thingId;
-  buffer[ix++] = this->dataLength;
-  for (int dataIx = 0; dataIx < this->dataLength; dataIx++)
-    buffer[ix++] = this->data[dataIx];
-  return this->length + this->dataLength;
 }
 
+
 }  // namespace RoboidControl

Messages/BinaryMsg.h

@@ -1,18 +1,16 @@
 #pragma once
 
-#include "IMessage.h"
+#include "Messages.h"
-#include "Thing.h"
 
 namespace RoboidControl {
 
-/// @brief A message containing binary data for custom communication
+/// @brief Message to send thing-specific data
 class BinaryMsg : public IMessage {
  public:
   /// @brief The message ID
   static const unsigned char id = 0xB1;
-  /// @brief The length of the message in bytes, excluding the binary data
+  /// @brief The length of the message without the binary data itslef
-  /// For the total size of the message this.bytes.Length should be added to this value.
+  static const unsigned length = 3;
-  static const unsigned length = 4;
 
   /// @brief The network ID of the thing
   unsigned char networkId;
@@ -21,11 +19,10 @@ class BinaryMsg : public IMessage {
   /// @brief The thing for which the binary data is communicated
   Thing* thing;
 
-  unsigned char dataLength;
   /// @brief The binary data which is communicated
-  char* data = nullptr;
+  char* bytes = nullptr;
 
-  /// @brief Create a BinaryMsg
+  /// @brief Create a new message for sending
   /// @param networkId The network ID of the thing
   /// @param thing The thing for which binary data is sent
   BinaryMsg(unsigned char networkId, Thing* thing);

Messages/DestroyMsg.cpp

@@ -2,23 +2,16 @@
 
 namespace RoboidControl {
 
-DestroyMsg::DestroyMsg(unsigned char networkId, Thing* thing) {
+DestroyMsg::DestroyMsg(unsigned char networkId, Thing *thing) {
   this->networkId = networkId;
   this->thingId = thing->id;
 }
 
-DestroyMsg::DestroyMsg(char* buffer) {
+DestroyMsg::DestroyMsg(char* buffer) {}
-  this->networkId = buffer[1];
-  this->thingId = buffer[2];
-}
 
 DestroyMsg::~DestroyMsg() {}
 
-unsigned char DestroyMsg::Serialize(char* buffer) {
+unsigned char DestroyMsg::Serialize(char *buffer) {
-#if defined(DEBUG)
-  std::cout << "Send DestroyMsg [" << (int)this->networkId << "/"
-            << (int)this->thingId << "] " << std::endl;
-#endif
   unsigned char ix = 0;
   buffer[ix++] = this->id;
   buffer[ix++] = this->networkId;

Messages/DestroyMsg.h

@@ -1,16 +1,13 @@
-#pragma once
+#include "Messages.h"
-
-#include "IMessage.h"
-#include "Thing.h"
 
 namespace RoboidControl {
 
-/// @brief A Message notifiying that a Thing no longer exists
+/// @brief Message notifiying that a Thing no longer exists
 class DestroyMsg : public IMessage {
  public:
   /// @brief The message ID
   static const unsigned char id = 0x20;
-  /// @brief The length of the message in bytes
+  /// @brief The length of the message
   static const unsigned length = 3;
   /// @brief The network ID of the thing
   unsigned char networkId;

Messages/IMessage.cpp

@@ -1,16 +0,0 @@
-#include "IMessage.h"
-
-namespace RoboidControl {
-
-#pragma region IMessage
-
-IMessage::IMessage() {}
-
-unsigned char IMessage::Serialize(char* buffer) {
-  return 0;
-}
-
-// IMessage
-#pragma endregion
-
-}  // namespace RoboidControl

Messages/IMessage.h

@@ -1,16 +0,0 @@
-#pragma once
-
-namespace RoboidControl {
-
-/// @brief Root structure for all communcation messages
-class IMessage {
- public:
-  IMessage();
-  /// @brief Serialize the message into a byte array for sending
-  /// @param buffer The buffer to serilize into
-  /// @return The length of the message in the buffer
-  virtual unsigned char Serialize(char* buffer);
-
-};
-
-}  // namespace RoboidControl

Messages/InvestigateMsg.cpp

@@ -7,17 +7,13 @@ InvestigateMsg::InvestigateMsg(char* buffer) {
   this->networkId = buffer[ix++];
   this->thingId = buffer[ix++];
 }
-InvestigateMsg::InvestigateMsg(unsigned char networkId, Thing* thing) {
+InvestigateMsg::InvestigateMsg(unsigned char networkId, unsigned char thingId) {
   this->networkId = networkId;
-  this->thingId = thing->id;
+  this->thingId = thingId;
 }
 
 InvestigateMsg::~InvestigateMsg() {}
 unsigned char InvestigateMsg::Serialize(char* buffer) {
-  #if defined(DEBUG)
-  std::cout << "Send InvestigateMsg [" << (int)this->networkId << "/" << (int)this->thingId
-            << "] " << std::endl;
-#endif
   unsigned char ix = 0;
   buffer[ix++] = this->id;
   buffer[ix++] = this->networkId;

Messages/InvestigateMsg.h

@@ -1,7 +1,4 @@
-#pragma once
+#include "Messages.h"
-
-#include "IMessage.h"
-#include "Thing.h"
 
 namespace RoboidControl {
 
@@ -17,10 +14,10 @@ class InvestigateMsg : public IMessage {
   /// @brief the ID of the thing
   unsigned char thingId;
 
-  /// @brief Create an investigate message
+  /// @brief Create a new message for sending
   /// @param networkId The network ID for the thing
-  /// @param thing The thing for which the details are requested
+  /// @param thingId The ID of the thing
-  InvestigateMsg(unsigned char networkId, Thing* thing);
+  InvestigateMsg(unsigned char networkId, unsigned char thingId);
   /// @copydoc RoboidControl::IMessage::IMessage(char*)
   InvestigateMsg(char* buffer);
   /// @brief Destructor for the message

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

@@ -1,5 +1,3 @@
-#pragma once
-
 #include "LinearAlgebra/Spherical.h"
 #include "LinearAlgebra/SwingTwist.h"
 
@@ -7,18 +5,17 @@ namespace RoboidControl {
 
 class LowLevelMessages {
  public:
-  static void SendSpherical(char* buffer, unsigned char* ix, Spherical s);
-  static Spherical ReceiveSpherical(const char* buffer,
-                                    unsigned char* startIndex);
-
-  static void SendQuat32(char* buffer, unsigned char* ix, SwingTwist q);
-  static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix);
-
   static void SendAngle8(char* buffer, unsigned char* ix, const float angle);
   static Angle8 ReceiveAngle8(const char* buffer, unsigned char* startIndex);
 
   static void SendFloat16(char* buffer, unsigned char* ix, float value);
   static float ReceiveFloat16(const char* buffer, unsigned char* startIndex);
+
+  static void SendSpherical16(char* buffer, unsigned char* ix, Spherical16 s);
+  static Spherical16 ReceiveSpherical16(const char* buffer, unsigned char* startIndex);
+
+  static void SendQuat32(char* buffer, unsigned char* ix, SwingTwist16 q);
+  static SwingTwist16 ReceiveQuat32(const char* buffer, unsigned char* ix);
 };
 
 }  // namespace RoboidControl

Messages/Messages.cpp

@@ -0,0 +1,36 @@
+#include "Messages.h"
+
+#include "LowLevelMessages.h"
+//#include "Participant.h"
+#include "string.h"
+
+namespace RoboidControl {
+
+#pragma region IMessage
+
+IMessage::IMessage() {}
+
+// IMessage::IMessage(unsigned char *buffer) { Deserialize(buffer); }
+
+// IMessage::IMessage(char* buffer) {}
+
+unsigned char IMessage::Serialize(char* buffer) {
+  return 0;
+}
+
+// bool IMessage::SendMsg(LocalParticipant *client, IMessage msg) {
+//   // return SendMsg(client, client.buffer, );nameLength
+//   return client->SendBuffer(msg.Serialize(client->buffer));
+// }
+
+// bool IMessage::Publish(LocalParticipant *participant) {
+//   return participant->PublishBuffer(Serialize(participant->buffer));
+// }
+// bool IMessage::SendTo(LocalParticipant *participant) {
+//   return participant->SendBuffer(Serialize(participant->buffer));
+// }
+
+// IMessage
+#pragma endregion
+
+}  // namespace RoboidControl

Messages/Messages.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include "LinearAlgebra/Spherical.h"
+#include "LinearAlgebra/SwingTwist.h"
+#include "Thing.h"
+
+namespace RoboidControl {
+
+class LocalParticipant;
+
+class IMessage {
+ public:
+  IMessage();
+  virtual unsigned char Serialize(char* buffer);
+
+  static unsigned char* ReceiveMsg(unsigned char packetSize);
+
+  // bool Publish(LocalParticipant *participant);
+  // bool SendTo(LocalParticipant *participant);
+};
+
+}  // namespace RoboidControl

Messages/ModelUrlMsg.cpp

@@ -4,6 +4,16 @@
 
 namespace RoboidControl {
 
+// ModelUrlMsg::ModelUrlMsg(unsigned char networkId, unsigned char thingId,
+//                          unsigned char urlLength, const char *url,
+//                          float scale) {
+//   this->networkId = networkId;
+//   this->thingId = thingId;
+//   this->urlLength = urlLength;
+//   this->url = url;
+//   this->scale = scale;
+// }
+
 ModelUrlMsg::ModelUrlMsg(unsigned char networkId, Thing* thing) {
   this->networkId = networkId;
   this->thingId = thing->id;
@@ -12,21 +22,21 @@ ModelUrlMsg::ModelUrlMsg(unsigned char networkId, Thing* thing) {
   else
     this->urlLength = (unsigned char)strlen(thing->modelUrl);
 
+  //this->url = thing->modelUrl;  // dangerous!
+
   // the url string in the buffer is not \0 terminated!
   char* url = new char[this->urlLength + 1];
   for (int i = 0; i < this->urlLength; i++)
     url[i] = thing->modelUrl[i];
   url[this->urlLength] = '\0';
-  this->url = url;
+  this->url = url;}
-}
 
 ModelUrlMsg::ModelUrlMsg(const char* buffer) {
   unsigned char ix = 1;  // first byte is msg id
   this->networkId = buffer[ix++];
   this->thingId = buffer[ix++];
   this->urlLength = buffer[ix++];
-  // this->url = &buffer[ix];  // dangerous! name should not be used anymore
+  // this->url = &buffer[ix];  // dangerous! name should not be used anymore after
-  // after
   //                           // buffer has been re-used...
 
   // the url string in the buffer is not \0 terminated!
@@ -44,11 +54,6 @@ ModelUrlMsg::~ModelUrlMsg() {
 unsigned char ModelUrlMsg::Serialize(char* buffer) {
   if (this->urlLength == 0 || this->url == nullptr)
     return 0;
-#if defined(DEBUG)
-  std::cout << "Send ModelUrlMsg [" << (int)this->networkId << "/"
-            << (int)this->thingId << "] " << (int)this->urlLength << " "
-            << this->url << std::endl;
-#endif
   unsigned char ix = 0;
   buffer[ix++] = this->id;
   buffer[ix++] = this->networkId;

Messages/ModelUrlMsg.h

@@ -1,7 +1,4 @@
-#pragma once
+#include "Messages.h"
-
-#include "IMessage.h"
-#include "Thing.h"
 
 namespace RoboidControl {
 
@@ -11,14 +8,14 @@ class ModelUrlMsg : public IMessage {
   /// @brief The message ID
   static const unsigned char id = 0x90;
   /// @brief The length of the message without the URL string itself
-  static const unsigned char length = 4;
+  static const unsigned char length = 3;
 
   /// @brief The network ID of the thing
   unsigned char networkId;
   /// @brief The ID of the thing
   unsigned char thingId;
 
-  /// @brief The length of the url string, excluding the null terminator
+  /// @brief The length of the url st5ring, excluding the null terminator
   unsigned char urlLength;
   /// @brief The url of the model, not terminated by a null character
   const char* url;
@@ -29,6 +26,8 @@ class ModelUrlMsg : public IMessage {
   ModelUrlMsg(unsigned char networkId, Thing* thing);
   /// @copydoc RoboidControl::IMessage::IMessage(char*)
   ModelUrlMsg(const char* buffer);
+  //   ModelUrlMsg(unsigned char networkId, unsigned char thingId,
+  //               unsigned char urlLegth, const char *url, float scale = 1);
 
   /// @brief Destructor for the message
   virtual ~ModelUrlMsg();

Messages/NameMsg.cpp

@@ -7,16 +7,15 @@ namespace RoboidControl {
 NameMsg::NameMsg(unsigned char networkId, Thing* thing) {
   this->networkId = networkId;
   this->thingId = thing->id;
-  const char* thingName = thing->GetName();
+  if (thing->name == nullptr)
-  if (thingName == nullptr)
     this->nameLength = 0;
   else
-    this->nameLength = (unsigned char)strlen(thingName);
+    this->nameLength = (unsigned char)strlen(thing->name);
 
   // the name string in the buffer is not \0 terminated!
   char* name = new char[this->nameLength + 1];
   for (int i = 0; i < this->nameLength; i++)
-    name[i] = thingName[i];
+    name[i] = thing->name[i];
   name[this->nameLength] = '\0';
   this->name = name;
 }
@@ -26,7 +25,6 @@ NameMsg::NameMsg(const char* buffer) {
   this->networkId = buffer[ix++];
   this->thingId = buffer[ix++];
   this->nameLength = buffer[ix++];
-
   // the name string in the buffer is not \0 terminated!
   char* name = new char[this->nameLength + 1];
   for (int i = 0; i < this->nameLength; i++)
@@ -43,11 +41,6 @@ unsigned char NameMsg::Serialize(char* buffer) {
   if (this->nameLength == 0 || this->name == nullptr)
     return 0;
 
-#if defined(DEBUG)
-  std::cout << "Send NameMsg [" << (int)this->networkId << "/"
-            << (int)this->thingId << "] " << (int)this->nameLength << " "
-            << this->name << std::endl;
-#endif
   unsigned char ix = 0;
   buffer[ix++] = this->id;
   buffer[ix++] = this->networkId;

Messages/NameMsg.h

@@ -1,7 +1,4 @@
-#pragma once
+#include "Messages.h"
-
-#include "IMessage.h"
-#include "Thing.h"
 
 namespace RoboidControl {
 
@@ -25,6 +22,9 @@ class NameMsg : public IMessage {
   /// @param networkId The network ID of the thing
   /// @param thing The ID of the thing
   NameMsg(unsigned char networkId, Thing* thing);
+  //   NameMsg(unsigned char networkId, unsigned char thingId, const char *name,
+  //           unsigned char nameLength);
+
   /// @copydoc RoboidControl::IMessage::IMessage(char*)
   NameMsg(const char* buffer);
   /// @brief Destructor for the message

Messages/NetworkIdMsg.cpp

@@ -1,27 +0,0 @@
-#include "NetworkIdMsg.h"
-
-#include <iostream>
-
-namespace RoboidControl {
-
-NetworkIdMsg::NetworkIdMsg(const char* buffer) {
-  this->networkId = buffer[1];
-}
-
-NetworkIdMsg::NetworkIdMsg(unsigned char networkId) {
-  this->networkId = networkId;
-}
-
-NetworkIdMsg::~NetworkIdMsg() {}
-
-unsigned char NetworkIdMsg::Serialize(char* buffer) {
-#if defined(DEBUG)
-  std::cout << "Send NetworkIdMsg [" << (int)this->networkId << "] " << std::endl;
-#endif
-  unsigned char ix = 0;
-  buffer[ix++] = this->id;
-  buffer[ix++] = this->networkId;
-  return NetworkIdMsg::length;
-}
-
-}  // namespace RoboidControl

Messages/ParticipantMsg.cpp

@@ -1,9 +1,5 @@
 #include "ParticipantMsg.h"
 
-#if !defined(NO_STD)
-#include <iostream>
-#endif
-
 namespace RoboidControl {
 
 ParticipantMsg::ParticipantMsg(char networkId) {
@@ -17,19 +13,13 @@ ParticipantMsg::ParticipantMsg(const char* buffer) {
 ParticipantMsg::~ParticipantMsg() {}
 
 unsigned char ParticipantMsg::Serialize(char* buffer) {
-#if defined(DEBUG) && !defined(NO_STD)
-  std::cout << "Send ParticipantMsg [" << (int)this->networkId << "] "
-            << std::endl;
-#endif
-
   unsigned char ix = 0;
   buffer[ix++] = this->id;
   buffer[ix++] = this->networkId;
   return ParticipantMsg::length;
 }
 
-// bool ParticipantMsg::Send(ParticipantUDP *participant, unsigned char
+// bool ParticipantMsg::Send(LocalParticipant *participant, unsigned char networkId) {
-// networkId) {
 //   ParticipantMsg msg = ParticipantMsg()
 // }
 // Client Msg

Messages/ParticipantMsg.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "IMessage.h"
+#include "Messages.h"
 
 namespace RoboidControl {
 

Messages/PoseMsg.cpp

@@ -3,18 +3,36 @@
 
 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;
 
   this->poseType = 0;
-  if (thing->positionUpdated || (force && thing->IsRoot())) {
+  if (thing->positionUpdated || force) {
     this->position = thing->GetPosition();
     this->poseType |= Pose_Position;
+    thing->positionUpdated = false;
   }
-  if (thing->orientationUpdated || (force && thing->IsRoot())) {
+  if (thing->orientationUpdated || force ) {
     this->orientation = thing->GetOrientation();
     this->poseType |= Pose_Orientation;
+    thing->orientationUpdated = false;
   }
   if (thing->linearVelocityUpdated) {
     this->linearVelocity = thing->GetLinearVelocity();
@@ -33,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
@@ -45,23 +63,19 @@ unsigned char PoseMsg::Serialize(char* buffer) {
   if (this->poseType == 0)
     return 0;
 
-#if defined(DEBUG) && DEBUG > 1
-  std::cout << "Send PoseMsg [" << (int)this->networkId << "/"
-            << (int)this->thingId << "] " << (int)this->poseType << std::endl;
-#endif
   unsigned char ix = 0;
   buffer[ix++] = PoseMsg::id;
   buffer[ix++] = this->networkId;
   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

@@ -1,17 +1,15 @@
-#pragma once
+#include "Messages.h"
-#include "IMessage.h"
-#include "Thing.h"
 
 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
   static const unsigned char id = 0x10;
-  /// @brief The length of the message in bytes
+  /// @brief The length of the message
   unsigned char length = 4 + 4 + 4;
 
   /// @brief The network ID of the thing
@@ -31,19 +29,33 @@ 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
-  /// @param thing The thing for which the pose should be sent
+  /// @param thing The thing for which the pose shouldbe sent
-  /// @param force If true, position and orientation are always included, even when they are not updated
   PoseMsg(unsigned char networkId, Thing* thing, bool force = false);
 
   /// @copydoc RoboidControl::IMessage::IMessage(char*)

Messages/SiteMsg.cpp

@@ -0,0 +1,22 @@
+#include "SiteMsg.h"
+
+namespace RoboidControl {
+
+SiteMsg::SiteMsg(const char* buffer) {
+  this->networkId = buffer[1];
+}
+
+SiteMsg::SiteMsg(unsigned char networkId) {
+  this->networkId = networkId;
+}
+
+SiteMsg::~SiteMsg() {}
+
+unsigned char SiteMsg::Serialize(char* buffer) {
+  unsigned char ix = 0;
+  buffer[ix++] = this->id;
+  buffer[ix++] = this->networkId;
+  return SiteMsg::length;
+}
+
+}  // namespace RoboidControl

Messages/SiteMsg.h

@@ -1,11 +1,9 @@
-#pragma once
+#include "Messages.h"
-
-#include "IMessage.h"
 
 namespace RoboidControl {
 
 /// @brief A message communicating the network ID for that participant
-class NetworkIdMsg : public IMessage {
+class SiteMsg : public IMessage {
 public:
   /// @brief The message ID
   static const unsigned char id = 0xA1;
@@ -16,11 +14,11 @@ public:
 
   /// @brief Create a new message for sending
   /// @param networkId The network ID for the participant
-  NetworkIdMsg(unsigned char networkId);
+  SiteMsg(unsigned char networkId);
   /// @copydoc RoboidControl::IMessage::IMessage(char*)
-  NetworkIdMsg(const char *buffer);
+  SiteMsg(const char *buffer);
   /// @brief Destructor for the message
-  virtual ~NetworkIdMsg();
+  virtual ~SiteMsg();
 
   /// @copydoc RoboidControl::IMessage::Serialize
   virtual unsigned char Serialize(char *buffer) override;

Messages/TextMsg.cpp

@@ -1,9 +1,5 @@
 #include "TextMsg.h"
 
-#if !defined(NO_STD)
-#include <iostream>
-#endif
-
 namespace RoboidControl {
 
 TextMsg::TextMsg(const char* text, unsigned char textLength) {
@@ -28,9 +24,6 @@ unsigned char TextMsg::Serialize(char* buffer) {
   if (this->textLength == 0 || this->text == nullptr)
     return 0;
 
-#if defined(DEBUG) && !defined(NO_STD)
-  std::cout << "Send TextMsg " << (int)this->textLength << " " << this->text << std::endl;
-#endif
   unsigned char ix = 0;
   buffer[ix++] = this->id;
   buffer[ix++] = this->textLength;

Messages/TextMsg.h

@@ -1,4 +1,4 @@
-#include "IMessage.h"
+#include "Messages.h"
 
 namespace RoboidControl {
 
@@ -9,6 +9,10 @@ class TextMsg : public IMessage {
   static const unsigned char id = 0xB0;
   /// @brief The length of the message without the text itself
   static const unsigned char length = 2;
+  /// @brief The network ID of the thing
+  unsigned char networkId;
+  /// @brief the ID of the thing
+  unsigned char thingId;
   /// @brief The text without the null terminator
   const char* text;
   /// @brief The length of the text

Messages/ThingMsg.cpp

@@ -14,22 +14,24 @@ ThingMsg::ThingMsg(unsigned char networkId, Thing* thing) {
   this->networkId = networkId;
   this->thingId = thing->id;
   this->thingType = thing->type;
-  if (thing->IsRoot())
-    this->parentId = 0;
-  else {
   Thing* parent = thing->GetParent();
+  if (parent != nullptr)
     this->parentId = parent->id;
-  }
+  else
+    this->parentId = 0;
 }
 
+// ThingMsg::ThingMsg(unsigned char networkId, unsigned char thingId,
+//                    unsigned char thingType, unsigned char parentId) {
+//   this->networkId = networkId;
+//   this->thingId = thingId;
+//   this->thingType = thingType;
+//   this->parentId = parentId;
+// }
+
 ThingMsg::~ThingMsg() {}
 
 unsigned char ThingMsg::Serialize(char* buffer) {
-#if defined(DEBUG)
-  std::cout << "Send ThingMsg [" << (int)this->networkId << "/"
-            << (int)this->thingId << "] " << (int)this->thingType << " "
-            << (int)this->parentId << std::endl;
-#endif
   unsigned char ix = 0;
   buffer[ix++] = this->id;
   buffer[ix++] = this->networkId;

Messages/ThingMsg.h

@@ -1,9 +1,8 @@
-#include "IMessage.h"
+#include "Messages.h"
-#include "Thing.h"
 
 namespace RoboidControl {
 
-/// @brief Message providing generic details about a Thing
+/// @brief  Message providing generic information about a Thing
 class ThingMsg : public IMessage {
  public:
   /// @brief The message ID
@@ -14,15 +13,17 @@ class ThingMsg : public IMessage {
   unsigned char networkId;
   /// @brief The ID of the thing
   unsigned char thingId;
-  /// @brief The type of thing
+  /// @brief The Thing.Type of the thing
   unsigned char thingType;
-  /// @brief The ID of the parent thing in the hierarchy. This is zero for root things
+  /// @brief The parent of the thing in the hierarachy. This is null for root Things
   unsigned char parentId;
 
   /// @brief Create a message for sending
   /// @param networkId The network ID of the thing</param>
   /// @param thing The thing
   ThingMsg(unsigned char networkId, Thing* thing);
+  //   ThingMsg(unsigned char networkId, unsigned char thingId,
+  //            unsigned char thingType, unsigned char parentId);
 
   /// @copydoc RoboidControl::IMessage::IMessage(char*)
   ThingMsg(const char* buffer);

Participant.cpp

@@ -4,37 +4,15 @@
 
 namespace RoboidControl {
 
-#pragma region Participant
+Participant::Participant() {}
-
-ParticipantRegistry Participant::registry;
-
-Participant* Participant::LocalParticipant = new Participant();
-
-void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
-  LocalParticipant = &newParticipant;
-  std::cout << "Replaced local participant" << std::endl;
-}
-
-Participant::Participant() {
-  std::cout << "P\n";
-  //this->root.name = "Isolated";
-  this->root = new Thing(this);
-  this->root->name = "Root";
-  this->Add(this->root);
-}
 
 Participant::Participant(const char* ipAddress, int port) {
-  // Add the root thing to the list of things, because we could not do that
-  // earlier (I think)
-  this->Add(this->root);
-
   // 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
@@ -45,169 +23,56 @@ Participant::Participant(const char* ipAddress, int port) {
 }
 
 Participant::~Participant() {
-  // registry.Remove(this);
   delete[] this->ipAddress;
 }
 
-void Participant::Update() {
+Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
-  for (Thing* thing : this->things) {
-    if (thing != nullptr)
-      thing->Update(true);
-  }
-}
-
-Thing* Participant::Get(unsigned char thingId) {
   for (Thing* thing : this->things) {
+    // if (thing->networkId == networkId && thing->id == 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";
-  //           << "[" << (int)thingId << "]\n";
   return nullptr;
 }
 
 void Participant::Add(Thing* thing, bool checkId) {
   if (checkId && thing->id == 0) {
     // allocate a new thing ID
-#if defined(NO_STD)
+    thing->id = (unsigned char)this->things.size() + 1;
-    thing->id = this->thingCount + 1;
-    this->things[this->thingCount++] = thing;
-#else
-    // find highest id
-    int highestIx = 0;
-    for (Thing* thing : this->things) {
-      if (thing == nullptr)
-        continue;
-      if (thing->id > highestIx)
-        highestIx = thing->id;
-    }
-    thing->id = highestIx + 1;
     this->things.push_back(thing);
-#endif
+    // std::cout << "Add thing with generated ID " << this->ipAddress << ":" << this->port << "[" <<
-    // std::cout << "Add thing with generated ID " << this->ipAddress << ":"
+    // (int)thing->networkId << "/"
-    //           << this->port << "[" << (int)thing->id << "]\n";
-  } else {
-    Thing* foundThing = Get(thing->id);
-    if (foundThing == nullptr) {
-#if defined(NO_STD)
-      this->things[this->thingCount++] = thing;
-#else
-      this->things.push_back(thing);
-#endif
-      // std::cout << "Add thing " << this->ipAddress << ":" << this->port <<
-      // "["
     //           << (int)thing->id << "]\n";
   } else {
-      // std::cout << "Did not add, existing thing " << this->ipAddress << ":"
+    Thing* foundThing = Get(thing->networkId, thing->id);
-      //           << this->port << "[" << (int)thing->id << "]\n";
+    if (foundThing == nullptr) {
+      this->things.push_back(thing);
+      // std::cout << "Add thing " << this->ipAddress << ":" << this->port << "[" << (int)thing->networkId << "/"
+      //           << (int)thing->id << "]\n";
     }
+    // else
+    //   std::cout << "Did not add, existing thing " << this->ipAddress << ":" << this->port << "["
+    //             << (int)thing->networkId << "/" << (int)thing->id << "]\n";
   }
 }
 
 void Participant::Remove(Thing* thing) {
-#if defined(NO_STD)
-  for (unsigned char thingIx = 0; thingIx < this->thingCount; thingIx++)
-    if (this->things[thingIx] == thing)
-      this->things[thingIx] = nullptr;
-  // compacting
-  unsigned char lastThingIx = 0;
-  for (unsigned char thingIx = 0; thingIx < this->thingCount; thingIx++) {
-    if (this->things[thingIx] == nullptr)
-      continue;
-    this->things[lastThingIx] = this->things[thingIx];
-    lastThingIx++;
-  }
-  this->thingCount = lastThingIx;
-#else
   this->things.remove_if([thing](Thing* obj) { return obj == thing; });
-  // std::cout << "Removing [" << (int)thing->networkId << "/" << (int)thing->id
+  std::cout << "Removing " << thing->networkId << "/" << thing->id << " list size = " << this->things.size() << "\n";
-  //           << "] list size = " << this->things.size() << "\n";
-#endif
 }
 
-#pragma endregion
+// void Participant::UpdateAll(unsigned long currentTimeMs) {
+//   // Not very efficient, but it works for now.
 
-#pragma region ParticipantRegistry
+//   for (Thing* thing : this->things) {
-
+//     if (thing != nullptr && thing->GetParent() == nullptr) {  // update all root things
-Participant* ParticipantRegistry::Get(const char* ipAddress,
+//       // std::cout << " update " << (int)ix << " thingid " << (int)thing->id
-                                      unsigned int port) {
+//       //           << "\n";
-#if !defined(NO_STD)
+//       thing->Update(currentTimeMs);
-  for (Participant* participant : ParticipantRegistry::participants) {
+//     }
-    if (participant == nullptr)
+//   }
-      continue;
+// }
-    if (strcmp(participant->ipAddress, ipAddress) == 0 &&
-        participant->port == port) {
-      // std::cout << "found participant " << participant->ipAddress << ":"
-      //           << (int)participant->port << std::endl;
-      return participant;
-    }
-  }
-  std::cout << "Could not find participant " << ipAddress << ":" << (int)port
-            << std::endl;
-#endif
-  return nullptr;
-}
-
-Participant* ParticipantRegistry::Get(unsigned char participantId) {
-#if !defined(NO_STD)
-  for (Participant* participant : ParticipantRegistry::participants) {
-    if (participant == nullptr)
-      continue;
-    if (participant->networkId == participantId)
-      return participant;
-  }
-  std::cout << "Could not find participant " << (int)participantId << std::endl;
-#endif
-  return nullptr;
-}
-
-Participant* ParticipantRegistry::Add(const char* ipAddress,
-                                      unsigned int port) {
-  Participant* participant = new Participant(ipAddress, port);
-  Add(participant);
-  return participant;
-}
-
-void ParticipantRegistry::Add(Participant* participant) {
-  Participant* foundParticipant =
-      Get(participant->ipAddress, participant->port);
-
-  if (foundParticipant == nullptr) {
-#if defined(NO_STD)
-    // this->things[this->thingCount++] = thing;
-#else
-    ParticipantRegistry::participants.push_back(participant);
-#endif
-    //   std::cout << "Add participant " << participant->ipAddress << ":"
-    //             << participant->port << "[" << (int)participant->networkId
-    //             << "]\n";
-    //   std::cout << "participants " <<
-    //   ParticipantRegistry::participants.size()
-    //             << "\n";
-    // } else {
-    //   std::cout << "Did not add, existing participant " <<
-    //   participant->ipAddress
-    //             << ":" << participant->port << "[" <<
-    //             (int)participant->networkId
-    //             << "]\n";
-  }
-}
-
-void ParticipantRegistry::Remove(Participant* participant) {
-  // participants.remove(participant);
-}
-
-#if defined(NO_STD)
-Participant** ParticipantRegistry::GetAll() const {
-  return ParticipantRegistry::participants;
-}
-#else
-const std::list<Participant*>& ParticipantRegistry::GetAll() const {
-  return ParticipantRegistry::participants;
-}
-#endif
-
-#pragma endregion ParticipantRegistry
 
 }  // namespace RoboidControl

Participant.h

@@ -1,115 +1,52 @@
 #pragma once
-
 #include "Thing.h"
 
 namespace RoboidControl {
 
-constexpr int MAX_THING_COUNT = 256;
-
-/// @brief class which manages all known participants
-class ParticipantRegistry {
- public:
-  /// @brief Retrieve a participant by its address
-  /// @param ipAddress The IP address of the participant
-  /// @param port The port number of the participant
-  /// @return The participant or a nullptr when it could not be found
-  Participant* Get(const char* ipAddress, unsigned int port);
-  /// @brief Retrieve a participant by its network ID
-  /// @param networkID The network ID of the participant
-  /// @return The participant or a nullptr when it could not be found
-  Participant* Get(unsigned char networkID);
-
-  /// @brief Add a participant with the given details
-  /// @param ipAddress The IP address of the participant
-  /// @param port The port number of the participant
-  /// @return The added participant
-  Participant* Add(const char* ipAddress, unsigned int port);
-  /// @brief Add a participant
-  /// @param participant The participant to add
-  void Add(Participant* participant);
-
-  /// @brief Remove a participant
-  /// @param participant The participant to remove
-  void Remove(Participant* participant);
-
- private:
-#if defined(NO_STD)
- public:
-  Participant** GetAll() const;
-  int count = 0;
-
- private:
-  Participant** participants;
-#else
- public:
-  /// @brief Get all participants
-  /// @return All participants
-  const std::list<Participant*>& GetAll() const;
-
- private:
-  /// @brief The list of known participants
-  std::list<Participant*> participants;
-#endif
-};
-
 /// @brief A participant is a device which manages things.
-/// It can communicate with other participant to synchronise the state of
+/// It can communicate with other participant to synchronise the state of things.
-/// things. This class is used to register the things the participant is
+/// This class is used to register the things the participant is managing.
-/// managing. It also maintains the communcation information to contact the
+/// It also maintains the communcation information to contact the participant.
-/// participant. It is used as a basis for the local participant, but also as a
+/// It is used as a basis for the local participant, but also as a reference to remote participants.
-/// reference to remote participants.
 class Participant {
- public:
+public:
-  /// @brief The name of the participant
+  /// @brief The Ip Address of a participant. When the participant is local, this contains 0.0.0.0
-  const char* name = "Participant";
+  const char *ipAddress = "0.0.0.0";
+  /// @brief The port number for UDP communication with the participant. This is 0 for isolated participants.
+  int port = 0;
 
-  /// @brief The Ip Address of a participant.
+  /// @brief The network Id to identify the participant.
-  const char* ipAddress = "0.0.0.0";
+  /// @note This field is likely to disappear in future versions
-  /// @brief The port number for UDP communication with the participant.
-  unsigned int port = 0;
-
-  /// @brief The network Id to identify the participant
   unsigned char networkId = 0;
 
+  /// @brief Default constructor
   Participant();
   /// @brief Create a new participant with the given communcation info
   /// @param ipAddress The IP address of the participant
-  /// @param port The UDP port of the participant
+  /// @param port The port of the participant
-  Participant(const char* ipAddress, int port);
+  Participant(const char *ipAddress, int port);
   /// @brief Destructor for the participant
   ~Participant();
 
-  static Participant* LocalParticipant;
+protected:
-  static void ReplaceLocalParticipant(Participant& newParticipant);
+  /// @brief The list of things managed by this participant
+  std::list<Thing *> things;
 
-  Thing* root = new Thing(this);
+public:
-
- public:
-#if defined(NO_STD)
-  unsigned char thingCount = 0;
-  Thing* things[MAX_THING_COUNT];
-#else
-  /// @brief  The things managed by this participant
-  std::list<Thing*> things;
-#endif
   /// @brief Find a thing managed by this participant
+  /// @param networkId The network ID for the thing
   /// @param thingId The ID of the thing
-  /// @return The thing if found, nullptr when no thing has been found
+  /// @return The thing if found or nullptr when no thing has been found
-  Thing* Get(unsigned char thingId);
+  /// @note The use of the network ID is likely to disappear in future versions.
+  Thing *Get(unsigned char networkId, unsigned char thingId);
   /// @brief Add a new thing for this participant. 
   /// @param thing The thing to add
-  /// @param checkId If true, the thing.id is regenerated if it is zero
+  /// @param checkId Checks the thing ID of the thing. If it is 0, a new thing Id will be assigned.
-  void Add(Thing* thing, bool checkId = true);
+  void Add(Thing *thing, bool checkId = true);
   /// @brief Remove a thing for this participant
   /// @param thing The thing to remove
-  void Remove(Thing* thing);
+  void Remove(Thing *thing);
 
-  /// @brief Update all things for this participant
-  /// @param currentTimeMs The current time in milliseconds (optional)
-  virtual void Update();
-
- public:
-  static ParticipantRegistry registry;
 };
 
-}  // namespace RoboidControl
+} // namespace Control

Participants/IsolatedParticipant.cpp

@@ -1,14 +0,0 @@
-#include "IsolatedParticipant.h"
-#include "ParticipantUDP.h"
-
-namespace RoboidControl {
-
-static ParticipantUDP* isolatedParticipant = nullptr;
-
-Participant* IsolatedParticipant::Isolated() {
-  if (isolatedParticipant == nullptr)
-    isolatedParticipant = new ParticipantUDP(0);
-  return isolatedParticipant;
-}
-
-}  // namespace RoboidControl

Participants/IsolatedParticipant.h

@@ -1,13 +0,0 @@
-#include "Participant.h"
-
-namespace RoboidControl {
-
-class IsolatedParticipant {
- public:
-  /// @brief Isolated participant is used when the application is run without
-  /// networking
-  /// @return A participant without networking support
-  static Participant* Isolated();
-};
-
-}

Participants/ParticipantUDP.cpp

@@ -1,538 +0,0 @@
-#include "ParticipantUDP.h"
-
-#include "Participant.h"
-#include "Thing.h"
-
-#include "Arduino/ArduinoParticipant.h"
-#include "EspIdf/EspIdfParticipant.h"
-#include "Posix/PosixParticipant.h"
-#include "Windows/WindowsParticipant.h"
-
-#include <string.h>
-
-namespace RoboidControl {
-
-#pragma region Init
-
-ParticipantUDP::ParticipantUDP(int port) : Participant("127.0.0.1", port) {
-  this->name = "ParticipantUDP";
-  this->remoteSite = nullptr;
-  if (this->port == 0)
-    this->isIsolated = true;
-  Participant::registry.Add(this);
-
-  this->root = Thing::LocalRoot();  //::LocalParticipant->root;
-  this->root->owner = this;
-  this->root->name = "UDP Root";
-  this->Add(this->root);
-
-  Participant::ReplaceLocalParticipant(*this);
-}
-
-ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
-    : Participant("127.0.0.1", localPort) {
-  this->name = "ParticipantUDP";
-  if (this->port == 0)
-    this->isIsolated = true;
-  else
-    this->remoteSite = new Participant(ipAddress, port);
-  Participant::registry.Add(this);
-
-  this->root = Thing::LocalRoot();  // Participant::LocalParticipant->root;
-  this->root->owner = this;
-  this->root->name = "UDP Root";
-  this->Add(this->root);
-
-  Participant::ReplaceLocalParticipant(*this);
-}
-
-static ParticipantUDP* isolatedParticipant = nullptr;
-
-ParticipantUDP* ParticipantUDP::Isolated() {
-  if (isolatedParticipant == nullptr)
-    isolatedParticipant = new ParticipantUDP(0);
-  return isolatedParticipant;
-}
-
-void ParticipantUDP::begin() {
-  if (this->isIsolated || this->remoteSite == nullptr)
-    return;
-
-  SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port);
-}
-
-void ParticipantUDP::SetupUDP(int localPort,
-                              const char* remoteIpAddress,
-                              int remotePort) {
-#if defined(_WIN32) || defined(_WIN64)
-  Windows::ParticipantUDP* thisWindows =
-      static_cast<Windows::ParticipantUDP*>(this);
-  thisWindows->Setup(localPort, remoteIpAddress, remotePort);
-#elif defined(__unix__) || defined(__APPLE__)
-  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
-  thisPosix->Setup(localPort, remoteIpAddress, remotePort);
-#elif defined(ARDUINO)
-  Arduino::ParticipantUDP* thisArduino =
-      static_cast<Arduino::ParticipantUDP*>(this);
-  thisArduino->Setup();
-#elif defined(IDF_VER)
-  EspIdf::ParticipantUDP* thisEspIdf =
-      static_cast<EspIdf::ParticipantUDP*>(this);
-  thisEspIdf->Setup(localPort, remoteIpAddress, remotePort);
-#endif
-  this->connected = true;
-}
-
-#pragma endregion Init
-
-#pragma region Update
-
-// The update order
-// 1. receive external messages
-// 2. update the state
-// 3. send out the updated messages
-void ParticipantUDP::Update() {
-  unsigned long currentTimeMs = Thing::GetTimeMs();
-
-  PrepMyThings();
-
-  if (this->isIsolated == false) {
-    if (this->connected == false)
-      begin();
-
-    if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
-      ParticipantMsg* msg = new ParticipantMsg(this->networkId);
-      if (this->remoteSite == nullptr)
-        this->Publish(msg);
-      else
-        this->Send(this->remoteSite, msg);
-      delete msg;
-
-      this->nextPublishMe = currentTimeMs + this->publishInterval;
-    }
-
-    this->ReceiveUDP();
-  }
-
-  UpdateMyThings();
-  UpdateOtherThings();
-}
-
-void ParticipantUDP::PrepMyThings() {
-  for (Thing* thing : this->things) {
-    if (thing == nullptr)
-      continue;
-      
-    thing->PrepareForUpdate();
-  }
-}
-
-void ParticipantUDP::UpdateMyThings() {
-  // std::cout << this->things.size() << std::endl;
-  for (Thing* thing : this->things) {
-    if (thing == nullptr)  // || thing->GetParent() != nullptr)
-      continue;
-
-    // std::cout << thing->name << "\n";
-    if (thing->hierarchyChanged) {
-      if (!(this->isIsolated || this->networkId == 0)) {
-        ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
-        this->Send(this->remoteSite, thingMsg);
-        delete thingMsg;
-
-        if (thing->nameChanged) {
-          NameMsg* nameMsg = new NameMsg(this->networkId, thing);
-          this->Send(this->remoteSite, nameMsg);
-          delete nameMsg;
-        }
-      }
-    }
-
-    // std::cout << "B\n";
-    // Why don't we do recursive?
-    // Because when a thing creates a thing in the update,
-    // that new thing is not sent out (because of hierarchyChanged)
-    // before it is updated itself: it is immediatedly updated!
-    thing->Update(false);
-
-    // std::cout << "C\n";
-    if (!(this->isIsolated || this->networkId == 0)) {
-      if (thing->terminate) {
-        DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing);
-        this->Send(this->remoteSite, destroyMsg);
-        delete destroyMsg;
-      } else {
-        //  Send to remote site
-        if (thing->nameChanged) {
-          NameMsg* nameMsg = new NameMsg(this->networkId, thing);
-          this->Send(this->remoteSite, nameMsg);
-          delete nameMsg;
-        }
-        PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
-        this->Send(this->remoteSite, poseMsg);
-        delete poseMsg;
-        BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
-        this->Send(this->remoteSite, binaryMsg);
-        delete binaryMsg;
-      }
-    }
-    // std::cout << "D\n";
-    if (thing->terminate)
-      this->Remove(thing);
-    // std::cout << "E\n";
-  }
-}
-
-void ParticipantUDP::UpdateOtherThings() {
-#if defined(NO_STD)
-  Participant** participants = Participant::registry.GetAll();
-  for (int ix = 0; ix < Participant::registry.count; ix++) {
-    Participant* participant = participants[ix];
-#else
-  for (Participant* participant : Participant::registry.GetAll()) {
-#endif
-    if (participant == nullptr || participant == this)
-      continue;
-
-    // Call only the Participant version of the Update.
-    // This is to deal with the function where one of the (remote)
-    // participants is actually a local participant
-    participant->Participant::Update();
-    if (this->isIsolated)
-      continue;
-
-    for (Thing* thing : participant->things) {
-      PoseMsg* poseMsg = new PoseMsg(participant->networkId, thing);
-      this->Send(participant, poseMsg);
-      delete poseMsg;
-      BinaryMsg* binaryMsg = new BinaryMsg(participant->networkId, thing);
-      this->Send(participant, binaryMsg);
-      delete binaryMsg;
-    }
-  }
-}
-
-// Update
-#pragma endregion
-
-#pragma region Send
-
-void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
-                                   Thing* thing) {
-  // std::cout << "Send thing info [" << (int)thing->id << "] \n";
-  ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
-  this->Send(remoteParticipant, thingMsg);
-  delete thingMsg;
-  NameMsg* nameMsg = new NameMsg(this->networkId, thing);
-  this->Send(remoteParticipant, nameMsg);
-  delete nameMsg;
-  ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
-  this->Send(remoteParticipant, modelMsg);
-  delete modelMsg;
-  PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
-  this->Send(remoteParticipant, poseMsg);
-  delete poseMsg;
-  BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
-  this->Send(remoteParticipant, customMsg);
-  delete customMsg;
-}
-
-bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
-  int bufferSize = msg->Serialize(this->buffer);
-  if (bufferSize <= 0)
-    return true;
-
-  // std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
-  //           << " to " << remoteParticipant->ipAddress << std::endl;
-
-#if defined(_WIN32) || defined(_WIN64)
-  Windows::ParticipantUDP* thisWindows =
-      static_cast<Windows::ParticipantUDP*>(this);
-  return thisWindows->Send(remoteParticipant, bufferSize);
-#elif defined(__unix__) || defined(__APPLE__)
-  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
-  return thisPosix->Send(remoteParticipant, bufferSize);
-#elif defined(ARDUINO)
-  Arduino::ParticipantUDP* thisArduino =
-      static_cast<Arduino::ParticipantUDP*>(this);
-  return thisArduino->Send(remoteParticipant, bufferSize);
-#elif defined(IDF_VER)
-  EspIdf::ParticipantUDP* thisEspIdf =
-      static_cast<EspIdf::ParticipantUDP*>(this);
-  return thisEspIdf->Send(remoteParticipant, bufferSize);
-#else
-  return false;
-#endif
-}
-
-void ParticipantUDP::PublishThingInfo(Thing* thing) {
-  // std::cout << "Publish thing info" << thing->networkId << "\n";
-  //  Strange, when publishing, the network id is irrelevant, because it is
-  //  connected to a specific site...
-  ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
-  this->Publish(thingMsg);
-  delete thingMsg;
-  NameMsg* nameMsg = new NameMsg(this->networkId, thing);
-  this->Publish(nameMsg);
-  delete nameMsg;
-  ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
-  this->Publish(modelMsg);
-  delete modelMsg;
-  PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
-  this->Publish(poseMsg);
-  delete poseMsg;
-  BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
-  this->Publish(customMsg);
-  delete customMsg;
-}
-
-bool ParticipantUDP::Publish(IMessage* msg) {
-  // std::cout << "publish msg\n";
-#if defined(_WIN32) || defined(_WIN64)
-  Windows::ParticipantUDP* thisWindows =
-      static_cast<Windows::ParticipantUDP*>(this);
-  return thisWindows->Publish(msg);
-#elif defined(__unix__) || defined(__APPLE__)
-  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
-  return thisPosix->Publish(msg);
-#elif defined(ARDUINO)
-  Arduino::ParticipantUDP* thisArduino =
-      static_cast<Arduino::ParticipantUDP*>(this);
-  return thisArduino->Publish(msg);
-#elif defined(IDF_VER)
-  EspIdf::ParticipantUDP* thisEspIdf =
-      static_cast<EspIdf::ParticipantUDP*>(this);
-  return thisEspIdf->Publish(msg);
-#else
-  return false;
-#endif
-}
-
-// Send
-#pragma endregion
-
-#pragma region Receive
-
-void ParticipantUDP::ReceiveUDP() {
-#if defined(_WIN32) || defined(_WIN64)
-  Windows::ParticipantUDP* thisWindows =
-      static_cast<Windows::ParticipantUDP*>(this);
-  thisWindows->Receive();
-#elif defined(__unix__) || defined(__APPLE__)
-  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
-  thisPosix->Receive();
-#elif defined(ARDUINO)
-  Arduino::ParticipantUDP* thisArduino =
-      static_cast<Arduino::ParticipantUDP*>(this);
-  thisArduino->Receive();
-#elif defined(IDF_VER)
-  EspIdf::ParticipantUDP* thisEspIdf =
-      static_cast<EspIdf::ParticipantUDP*>(this);
-  thisEspIdf->Receive();
-#endif
-}
-
-void ParticipantUDP::ReceiveData(unsigned char packetSize,
-                                 char* senderIpAddress,
-                                 unsigned int senderPort) {
-  // std::cout << "Receive data from " << senderIpAddress << ":" << senderPort
-  //           << std::endl;
-  Participant* sender = this->registry.Get(senderIpAddress, senderPort);
-  if (sender == nullptr) {
-    sender = this->registry.Add(senderIpAddress, senderPort);
-#if !defined(NO_STD)
-    std::cout << "New remote participant " << sender->ipAddress << ":"
-              << sender->port << std::endl;
-#endif
-  }
-
-  ReceiveData(packetSize, sender);
-}
-
-void ParticipantUDP::ReceiveData(unsigned char bufferSize,
-                                 Participant* sender) {
-  unsigned char msgId = this->buffer[0];
-  // std::cout << "receive msg " << (int)msgId << "\n";
-  // std::cout << "  buffer size = " <<(int) bufferSize << "\n";
-  switch (msgId) {
-    case ParticipantMsg::id: {
-      ParticipantMsg* msg = new ParticipantMsg(this->buffer);
-      bufferSize -= msg->length;
-      Process(sender, msg);
-      delete msg;
-    } break;
-    case NetworkIdMsg::id: {
-      NetworkIdMsg* msg = new NetworkIdMsg(this->buffer);
-      bufferSize -= msg->length;
-      Process(sender, msg);
-      delete msg;
-    } break;
-    case InvestigateMsg::id: {
-      InvestigateMsg* msg = new InvestigateMsg(this->buffer);
-      Process(sender, msg);
-      delete msg;
-    } break;
-    case ThingMsg::id: {
-      ThingMsg* msg = new ThingMsg(this->buffer);
-      bufferSize -= msg->length;
-      Process(sender, msg);
-      delete msg;
-    } break;
-    case NameMsg::id: {
-      NameMsg* msg = new NameMsg(this->buffer);
-      bufferSize -= msg->length + msg->nameLength;
-      Process(sender, msg);
-      delete msg;
-    } break;
-    case ModelUrlMsg::id: {
-      ModelUrlMsg* msg = new ModelUrlMsg(this->buffer);
-      bufferSize -= msg->length + msg->urlLength;
-      Process(sender, msg);
-      delete msg;
-    } break;
-    case PoseMsg::id: {
-      PoseMsg* msg = new PoseMsg(this->buffer);
-      bufferSize -= msg->length;
-      Process(sender, msg);
-      delete msg;
-    } break;
-    case BinaryMsg::id: {
-      BinaryMsg* msg = new BinaryMsg(this->buffer);
-      bufferSize -= msg->length + msg->dataLength;
-      Process(sender, msg);
-      delete msg;
-    } break;
-  };
-
-// Check if the buffer has been read completely
-#if !defined(NO_STD)
-  if (bufferSize > 0)
-    std::cout << "Buffer not fully read, remaining " << (int)bufferSize << "\n";
-#endif
-}
-
-void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
-#if defined(DEBUG)
-  std::cout << this->name << ": Process ParticipantMsg " << (int)msg->networkId
-            << "\n";
-#endif
-}
-
-void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) {
-#if defined(DEBUG)
-  std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId
-            << " -> " << (int)msg->networkId << "\n";
-#endif
-
-  if (this->networkId != msg->networkId) {
-    this->networkId = msg->networkId;
-
-    std::cout << this->things.size() << " things\n";
-    for (Thing* thing : this->things)
-      this->SendThingInfo(sender, thing);
-  }
-}
-
-void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) {
-#if defined(DEBUG)
-  std::cout << this->name << ": Process InvestigateMsg [" << (int)msg->networkId
-            << "/" << (int)msg->thingId << "]\n";
-#endif
-}
-
-void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) {
-#if defined(DEBUG)
-  std::cout << this->name << ": process ThingMsg [" << (int)msg->networkId
-            << "/" << (int)msg->thingId << "] " << (int)msg->thingType << " "
-            << (int)msg->parentId << "\n";
-#endif
-}
-
-void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
-#if defined(DEBUG)
-  std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/"
-            << (int)msg->thingId << "] ";
-#endif
-
-  Thing* thing = sender->Get(msg->thingId);
-  if (thing != nullptr) {
-    int nameLength = msg->nameLength;
-    int stringLen = nameLength + 1;
-    char* thingName = new char[stringLen];
-#if defined(_WIN32) || defined(_WIN64)
-    strncpy_s(thingName, stringLen, msg->name,
-              stringLen - 1);  // Leave space for null terminator
-#else
-    // Use strncpy with bounds checking for other platforms (Arduino, POSIX,
-    // ESP-IDF)
-    strncpy(thingName, msg->name,
-            nameLength);  // Leave space for null terminator
-#endif
-    thingName[nameLength] = '\0';
-    thing->SetName(thingName);
-
-#if !defined(NO_STD)
-    std::cout << thing->GetName();
-#endif
-  }
-#if !defined(NO_STD)
-  std::cout << std::endl;
-#endif
-}
-
-void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) {
-#if defined(DEBUG)
-  std::cout << this->name << ": process ModelUrlMsg [" << (int)msg->networkId
-            << "/" << (int)msg->thingId << "]\n";
-#endif
-}
-
-void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
-#if !defined(DEBUG) && !defined(NO_STD)
-  std::cout << this->name << ": process PoseMsg [" << (int)this->networkId
-            << "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n";
-#endif
-  Participant* owner = Participant::registry.Get(msg->networkId);
-  if (owner == nullptr)
-    return;
-
-  Thing* thing = owner->Get(msg->thingId);
-  if (thing == nullptr)
-    return;
-
-  if ((msg->poseType & PoseMsg::Pose_Position) != 0)
-    thing->SetPosition(msg->position);
-  if ((msg->poseType & PoseMsg::Pose_Orientation) != 0)
-    thing->SetOrientation(msg->orientation);
-  if ((msg->poseType & PoseMsg::Pose_LinearVelocity) != 0)
-    thing->SetLinearVelocity(msg->linearVelocity);
-  if ((msg->poseType & PoseMsg::Pose_AngularVelocity) != 0)
-    thing->SetAngularVelocity(msg->angularVelocity);
-}
-
-void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
-#if defined(DEBUG)
-  std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId
-            << "/" << (int)msg->thingId << "]\n";
-#endif
-
-  Participant* owner = Participant::registry.Get(msg->networkId);
-  if (owner != nullptr) {
-    Thing* thing = owner->Get(msg->thingId);
-    if (thing != nullptr)
-      thing->ProcessBinary(msg->data);
-#if !defined(NO_STD)
-    else {
-#if defined(DEBUG)
-      std::cout << "  unknown thing [" << (int)msg->networkId << "/"
-                << (int)msg->thingId << "]";
-#endif
-    }
-#endif
-  }
-}
-
-// Receive
-#pragma endregion
-
-}  // namespace RoboidControl

Participants/SiteServer.cpp

@@ -1,97 +0,0 @@
-#include "SiteServer.h"
-
-#include "Things/TemperatureSensor.h"
-
-#if !defined(NO_STD)
-#include <functional>
-#include <memory>
-#endif
-
-namespace RoboidControl {
-
-#pragma region Init
-
-SiteServer::SiteServer(int port) : ParticipantUDP(port) {
-  this->name = "Site Server";
-  this->publishInterval = 0;
-
-  SetupUDP(port, ipAddress, 0);
-}
-
-#pragma endregion Init
-
-#pragma region Update
-
-void SiteServer::UpdateMyThings() {
-  for (Thing* thing : this->things) {
-    if (thing == nullptr)
-      continue;
-
-    thing->Update(true);
-
-    if (this->isIsolated == false) {
-      // Send to all other participants
-#if defined(NO_STD)
-      Participant** participants = Participant::registry.GetAll();
-      for (int ix = 0; ix < Participant::registry.count; ix++) {
-        Participant* participant = participants[ix];
-#else
-      for (Participant* participant : Participant::registry.GetAll()) {
-#endif
-        if (participant == nullptr || participant == this)
-          continue;
-
-        PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
-        this->Send(participant, poseMsg);
-        delete poseMsg;
-        BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
-        this->Send(participant, binaryMsg);
-        delete binaryMsg;
-      }
-    }
-  }
-}
-
-#pragma endregion Update
-
-#pragma region Receive
-
-void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
-  if (msg->networkId != sender->networkId) {
-    // std::cout << this->name << " received New Client -> " <<
-    // sender->ipAddress
-    //           << ":" << (int)sender->port << "\n";
-    NetworkIdMsg* msg = new NetworkIdMsg(sender->networkId);
-    this->Send(sender, msg);
-    delete msg;
-  }
-}
-
-void SiteServer::Process(Participant* sender, NetworkIdMsg* msg) {}
-
-void SiteServer::Process(Participant* sender, ThingMsg* msg) {
-  Thing* thing = sender->Get(msg->thingId);
-  if (thing == nullptr)
-    // new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);
-    // Thing::Reconstruct(sender, msg->thingType, msg->thingId);
-    //thing = new Thing(msg->thingType, sender->root);
-    ;
-  thing->id = msg->thingId;
-
-  if (msg->parentId != 0) {
-    thing->SetParent(Get(msg->parentId));
-    if (thing->IsRoot())
-    // if (thing->GetParent() != nullptr)
-#if defined(NO_STD)
-      ;
-#else
-      std::cout << "Could not find parent [" << (int)msg->networkId << "/"
-                << (int)msg->parentId << "]\n";
-#endif
-  } else
-    thing->SetParent(Thing::LocalRoot());
-}
-
-#pragma endregion Receive
-
-}  // namespace RoboidControl

Participants/SiteServer.h

@@ -1,44 +0,0 @@
-#pragma once
-
-#include "ParticipantUDP.h"
-
-#if !defined(NO_STD)
-#include <functional>
-#include <memory>
-#include <unordered_map>
-#endif
-
-namespace RoboidControl {
-
-/// @brief A participant is device which can communicate with other participants
-class SiteServer : public ParticipantUDP {
-  
-#pragma region Init
-
- public:
-  /// @brief Create a new site server
-  /// @param port The port of which to receive the messages
-  SiteServer(int port = 7681);
-
-#pragma endregion Init
-
-#pragma region Update
-
-  virtual void UpdateMyThings() override;
-
-#pragma endregion Update
-
-#pragma region Receive
-
- protected:
-  unsigned long nextPublishMe = 0;
-
-  virtual void Process(Participant* sender, ParticipantMsg* msg) override;
-  virtual void Process(Participant* sender, NetworkIdMsg* msg) override;
-  virtual void Process(Participant* sender, ThingMsg* msg) override;
-
-#pragma endregion Receive
-
-};
-
-}  // namespace RoboidControl

Posix/PosixParticipant.cpp

@@ -11,7 +11,7 @@
 namespace RoboidControl {
 namespace Posix {
 
-void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
+void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
 #if defined(__unix__) || defined(__APPLE__)
 
   // Create a UDP socket
@@ -63,7 +63,7 @@ void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remot
 #endif
 }
 
-void ParticipantUDP::Receive() {
+void LocalParticipant::Receive() {
 #if defined(__unix__) || defined(__APPLE__)
   sockaddr_in client_addr;
   socklen_t len = sizeof(client_addr);
@@ -74,9 +74,9 @@ void ParticipantUDP::Receive() {
     unsigned int sender_port = ntohs(client_addr.sin_port);
 
     ReceiveData(packetSize, sender_ipAddress, sender_port);
-    // RoboidControl::Participant* remoteParticipant = this->Get(sender_ipAddress, sender_port);
+    // RoboidControl::Participant* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port);
     // if (remoteParticipant == nullptr) {
-    //   remoteParticipant = this->Add(sender_ipAddress, sender_port);
+    //   remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port);
     //   // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
     //   //           << "\n";
     //   // std::cout << "New remote participant " << remoteParticipant->ipAddress
@@ -90,7 +90,7 @@ void ParticipantUDP::Receive() {
 #endif
 }
 
-bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
+bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
 #if defined(__unix__) || defined(__APPLE__)
   // std::cout << "Send to " << remoteParticipant->ipAddress << ":" << ntohs(remoteParticipant->port)
   //           << "\n";
@@ -113,7 +113,7 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
   return true;
 }
 
-bool ParticipantUDP::Publish(IMessage* msg) {
+bool LocalParticipant::Publish(IMessage* msg) {
 #if defined(__unix__) || defined(__APPLE__)
   int bufferSize = msg->Serialize(this->buffer);
   if (bufferSize <= 0)

Posix/PosixParticipant.h

@@ -1,23 +1,16 @@
 #pragma once
 
-#include "Participants/ParticipantUDP.h"
+#include "../LocalParticipant.h"
 
 namespace RoboidControl {
 namespace Posix {
 
-class ParticipantUDP : public RoboidControl::ParticipantUDP {
+class LocalParticipant : public RoboidControl::LocalParticipant {
  public:
   void Setup(int localPort, const char* remoteIpAddress, int remotePort);
   void Receive();
   bool Send(Participant* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);
-
- protected:
-#if defined(__unix__) || defined(__APPLE__)
-  sockaddr_in remote_addr;
-  sockaddr_in server_addr;
-  sockaddr_in broadcast_addr;
-#endif
 };
 
 }  // namespace Posix

README.md

@@ -9,9 +9,9 @@ Supporting:
     - ESP8266
     - ESP32
     - UNO R4 WiFi
-    - UNO (without networking support)
 
 # Basic components
 
 - RoboidControl::Thing
-- RoboidControl::Participant
+- RoboidControl::LocalParticipant
+- RoboidControl::SiteServer

SiteServer.cpp

@@ -0,0 +1,50 @@
+#include "SiteServer.h"
+
+#include "Things/TemperatureSensor.h"
+
+#include <functional>
+#include <memory>
+
+namespace RoboidControl {
+
+SiteServer::SiteServer(int port) {
+  this->name = "Site Server";
+  this->publishInterval = 0;
+
+  this->ipAddress = "0.0.0.0";
+  this->port = port;
+
+  this->senders.push_back(this);
+
+  SetupUDP(port, ipAddress, 0);
+
+  Register<TemperatureSensor>((unsigned char)Thing::Type::TemperatureSensor);
+}
+
+void SiteServer::Process(Participant *sender, ParticipantMsg *msg) {
+  if (msg->networkId == 0) {
+    std::cout << this->name << " received New Client -> " << sender->ipAddress
+              << ":" << (int)sender->port << "\n";
+    SiteMsg *msg = new SiteMsg(sender->networkId);
+    this->Send(sender, msg);
+    delete msg;
+  }
+}
+
+void SiteServer::Process(Participant *sender, SiteMsg *msg) {}
+
+void SiteServer::Process(Participant *sender, ThingMsg *msg) {
+  Thing *thing = sender->Get(msg->networkId, msg->thingId);
+  if (thing == nullptr) {
+    auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType);
+    Thing *newThing;
+    if (thingMsgProcessor != thingMsgProcessors.end()) // found item
+      newThing = thingMsgProcessor->second(sender, msg->networkId, msg->thingId);
+    else
+      newThing = new Thing(sender, msg->networkId, msg->thingId,
+                           (Thing::Type)msg->thingType);
+    //sender->Add(newThing);
+  }
+}
+
+} // namespace Control

SiteServer.h

@@ -0,0 +1,36 @@
+#pragma once
+
+#include "LocalParticipant.h"
+
+#include <functional>
+#include <memory>
+#include <unordered_map>
+
+namespace RoboidControl {
+
+/// @brief A participant is device which can communicate with other participants
+class SiteServer : public LocalParticipant {
+ public:
+  SiteServer(int port = 7681);
+
+  // virtual void Update(unsigned long currentTimeMs = 0) override;
+
+  template <typename ThingClass>
+  void Register(unsigned char thingType) {
+    thingMsgProcessors[thingType] = [](Participant* participant, unsigned char networkId, unsigned char thingId) {
+      return new ThingClass(participant, networkId, thingId);
+    };
+  };
+
+ protected:
+  unsigned long nextPublishMe = 0;
+
+  virtual void Process(Participant* sender, ParticipantMsg* msg) override;
+  virtual void Process(Participant* sender, SiteMsg* msg) override;
+  virtual void Process(Participant* sender, ThingMsg* msg) override;
+
+  using ThingConstructor = std::function<Thing*(Participant* participant, unsigned char networkId, unsigned char thingId)>;
+  std::unordered_map<unsigned char, ThingConstructor> thingMsgProcessors;
+};
+
+}  // namespace RoboidControl

Thing.cpp

@@ -1,96 +1,76 @@
 #include "Thing.h"
 
-#include "Messages/PoseMsg.h"
-#include "Participant.h"
-#include "Participants/IsolatedParticipant.h"
-
 #include <string.h>
-// #include <iostream>
-
-#if defined(ARDUINO)
-#include "Arduino.h"
-#else
 #include <algorithm>
-#include <chrono>
 #include <iostream>
 #include <list>
-#endif
+#include <chrono>
+
+#include "LocalParticipant.h"
 
 namespace RoboidControl {
 
-#pragma region Init
 
-Thing* Thing::LocalRoot() {
+// LocalParticipant* Thing::CheckHiddenParticipant() {
-  Participant* p = Participant::LocalParticipant;
+//   if (isolatedParticipant == nullptr)
-  Thing* localRoot = p->root;
+//     isolatedParticipant = new LocalParticipant(0);
-  return localRoot;
+//   return isolatedParticipant;
-}
-
-// Only use this for root things
-Thing::Thing(Participant* owner) {
-  this->type = Type::Roboid;  // should become root
-
-  this->position = Spherical::zero;
-  this->positionUpdated = true;
-  this->orientation = SwingTwist::identity;
-  this->orientationUpdated = true;
-  this->hierarchyChanged = true;
-
-  this->linearVelocity = Spherical::zero;
-  this->angularVelocity = Spherical::zero;
-
-  this->owner = owner;
-  //this->owner->Add(this, true);
-  std::cout << this->owner->name << ": New root thing " << std::endl;
-}
-
-Thing::Thing(unsigned char thingType, Thing* parent) {
-  this->type = thingType;
-
-  this->position = Spherical::zero;
-  this->positionUpdated = true;
-  this->orientation = SwingTwist::identity;
-  this->orientationUpdated = true;
-  this->hierarchyChanged = true;
-
-  this->linearVelocity = Spherical::zero;
-  this->angularVelocity = Spherical::zero;
-
-  this->owner = parent->owner;
-  this->owner->Add(this, true);
-  this->SetParent(parent);
-
-  std::cout << this->owner->name << ": New thing for " << parent->name
-            << std::endl;
-}
-
-Thing::~Thing() {
-  std::cout << "Destroy thing " << this->name << std::endl;
-}
-
-// Thing Thing::Reconstruct(Participant* owner, unsigned char thingType,
-// unsigned char thingId) {
-//   Thing thing = Thing(owner, thingType);
-//   thing.id = thingId;
-//   return thing;
 // }
 
-#pragma endregion Init
+Thing::Thing(int thingType) : Thing(LocalParticipant::Isolated(), thingType) {
-
-void Thing::SetName(const char* name) {
-  this->name = name;
-  this->nameChanged = true;
 }
 
-const char* Thing::GetName() const {
+Thing::Thing(Participant* owner, Type thingType) : Thing(owner, (unsigned char)thingType) {}
-  return this->name;
+
+Thing::Thing(Participant* owner, int thingType) {
+  this->owner = owner;
+  this->id = 0;
+  this->type = thingType;
+  this->networkId = 0;
+
+  this->position = Spherical16::zero;
+  this->orientation = SwingTwist16::identity;
+
+  this->linearVelocity = Spherical16::zero;
+  this->angularVelocity = Spherical16::zero;
+
+  // std::cout << "add thing to participant\n";
+  owner->Add(this);
 }
 
-void Thing::SetModel(const char* url) {
+Thing::Thing(Participant* owner, unsigned char networkId, unsigned char thingId, Type thingType) {
-  this->modelUrl = url;
+  // no participant reference yet..
+  this->owner = owner;
+  this->networkId = networkId;
+  this->id = thingId;
+  this->type = (unsigned char)thingType;
+
+  this->linearVelocity = Spherical16::zero;
+  this->angularVelocity = Spherical16::zero;
+  // std::cout << "Created thing " << (int)this->networkId << "/" << (int)this->id
+  //           << "\n";
+  owner->Add(this, false);
 }
 
-#pragma region Hierarchy
+void Thing::Terminate() {
+  // Thing::Remove(this);
+}
+
+Thing* Thing::FindThing(const char* name) {
+  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
+    Thing* child = this->children[childIx];
+    if (child == nullptr || child->name == nullptr)
+      continue;
+
+    if (strcmp(child->name, name) == 0)
+      return child;
+
+    Thing* foundChild = child->FindThing(name);
+    if (foundChild != nullptr)
+      return foundChild;
+  }
+  return nullptr;
+}
 
 void Thing::SetParent(Thing* parent) {
   if (parent == nullptr) {
@@ -100,36 +80,18 @@ void Thing::SetParent(Thing* parent) {
     this->parent = nullptr;
   } else
     parent->AddChild(this);
-  this->hierarchyChanged = true;
 }
 
-// void Thing::SetParent(Thing* parent) {
+void Thing::SetParent(Thing* root, const char* name) {
-//   parent->AddChild(this);
+  Thing* thing = root->FindThing(name);
-//   this->hierarchyChanged = true;
+  if (thing != nullptr)
-// }
+    this->SetParent(thing);
-
-// const Thing& Thing::GetParent() {
-//   return *this->parent;
-// }
-
-bool Thing::IsRoot() const {
-  return this == LocalRoot() || this->parent == nullptr;  //&Thing::Root;
 }
 
-// void Thing::SetParent(Thing* root, const char* name) {
-//   Thing* thing = root->FindChild(name);
-//   if (thing != nullptr)
-//     this->SetParent(thing);
-// }
-
 Thing* Thing::GetParent() {
   return this->parent;
 }
 
-Thing* Thing::GetChildByIndex(unsigned char ix) {
-  return this->children[ix];
-}
-
 void Thing::AddChild(Thing* child) {
   unsigned char newChildCount = this->childCount + 1;
   Thing** newChildren = new Thing*[newChildCount];
@@ -169,7 +131,7 @@ Thing* Thing::RemoveChild(Thing* child) {
     }
   }
 
-  child->parent = Thing::LocalRoot();
+  child->parent = nullptr;
 
   delete[] this->children;
   this->children = newChildren;
@@ -178,7 +140,7 @@ Thing* Thing::RemoveChild(Thing* child) {
   return child;
 }
 
-Thing* Thing::GetChild(unsigned char id, bool recurse) {
+Thing* Thing::GetChild(unsigned char id, bool recursive) {
   for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
     Thing* child = this->children[childIx];
     if (child == nullptr)
@@ -186,8 +148,8 @@ Thing* Thing::GetChild(unsigned char id, bool recurse) {
     if (child->id == id)
       return child;
 
-    if (recurse) {
+    if (recursive) {
-      Thing* foundChild = child->GetChild(id, recurse);
+      Thing* foundChild = child->GetChild(id, recursive);
       if (foundChild != nullptr)
         return foundChild;
     }
@@ -195,121 +157,83 @@ Thing* Thing::GetChild(unsigned char id, bool recurse) {
   return nullptr;
 }
 
-Thing* Thing::FindChild(const char* name, bool recurse) {
+Thing* Thing::GetChildByIndex(unsigned char ix) {
-  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
+  return this->children[ix];
-    Thing* child = this->children[childIx];
-    if (child == nullptr || child->name == nullptr)
-      continue;
-
-    if (strcmp(child->name, name) == 0)
-      return child;
-
-    Thing* foundChild = child->FindChild(name);
-    if (foundChild != nullptr)
-      return foundChild;
-  }
-  return nullptr;
 }
 
-#pragma endregion Hierarchy
+void Thing::SetModel(const char* url) {
-
+  this->modelUrl = url;
-#pragma region Pose
-
-void Thing::SetPosition(Spherical position) {
-  this->position = position;
-  this->positionUpdated = true;
 }
-Spherical Thing::GetPosition() {
-  return this->position;
-}
-
-void Thing::SetOrientation(SwingTwist orientation) {
-  this->orientation = orientation;
-  this->orientationUpdated = true;
-}
-
-SwingTwist Thing::GetOrientation() {
-  return this->orientation;
-}
-
-void Thing::SetLinearVelocity(Spherical linearVelocity) {
-  if (this->linearVelocity.distance != linearVelocity.distance) {
-    this->linearVelocity = linearVelocity;
-    this->linearVelocityUpdated = true;
-  }
-}
-
-Spherical Thing::GetLinearVelocity() {
-  return this->linearVelocity;
-}
-
-void Thing::SetAngularVelocity(Spherical angularVelocity) {
-  if (this->angularVelocity.distance != angularVelocity.distance) {
-    this->angularVelocity = angularVelocity;
-    this->angularVelocityUpdated = true;
-  }
-}
-
-Spherical Thing::GetAngularVelocity() {
-  return this->angularVelocity;
-}
-
-#pragma endregion Pose
-
-#pragma region Update
 
 unsigned long Thing::GetTimeMs() {
-#if defined(ARDUINO)
-  unsigned long ms = millis();
-  return ms;
-#else
   auto now = std::chrono::steady_clock::now();
   auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
       now.time_since_epoch());
   return static_cast<unsigned long>(ms.count());
-#endif
 }
 
-// void Thing::Update(bool recursive) {
+void Thing::Update() {
-//   Update(GetTimeMs(), recursive);
+  #if defined(ARDUINO)
-// }
+  Update(millis());
-
+  #else
-void Thing::PrepareForUpdate() {}
+  Update(GetTimeMs());
-
+  #endif
-void Thing::Update(bool recursive) {
-  // if (this->positionUpdated || this->orientationUpdated)
-  //   OnPoseChanged callback
-  this->positionUpdated = false;
-  this->orientationUpdated = false;
-  // this->linearVelocityUpdated = false;
-  // this->angularVelocityUpdated = false;
-  this->hierarchyChanged = false;
-  this->nameChanged = false;
-
-  if (recursive) {
-    // std::cout << "# children: " << (int)this->childCount << std::endl;
-    for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
-      Thing* child = this->children[childIx];
-      if (child == nullptr)
-        continue;
-      child->Update(recursive);
-    }
-  }
 }
 
-void Thing::UpdateThings() {
+void Thing::Update(unsigned long currentTimeMs) {
-  IsolatedParticipant::Isolated()->Update();
+  (void)currentTimeMs;
+
+  // PoseMsg* poseMsg = new PoseMsg(this->networkId, this);
+  // participant->Send(remoteParticipant, poseMsg);
+  // delete poseMsg;
+
 }
 
-#pragma endregion Update
+void Thing::UpdateThings(unsigned long currentTimeMs) {
+  LocalParticipant::Isolated()->Update(currentTimeMs);
+}
 
-int Thing::GenerateBinary(char* buffer, unsigned char* ix) {
+void Thing::GenerateBinary(char* buffer, unsigned char* ix) {
   (void)buffer;
   (void)ix;
-  return 0;
 }
 void Thing::ProcessBinary(char* bytes) {
   (void)bytes;
 };
 
+void Thing::SetPosition(Spherical16 position) {
+  this->position = position;
+  this->positionUpdated = true;
+}
+Spherical16 Thing::GetPosition() {
+  return this->position;
+}
+
+void Thing::SetOrientation(SwingTwist16 orientation) {
+  this->orientation = orientation;
+  this->orientationUpdated = true;
+}
+
+SwingTwist16 Thing::GetOrientation() {
+  return this->orientation;
+}
+
+void Thing::SetLinearVelocity(Spherical16 linearVelocity) {
+  this->linearVelocity = linearVelocity;
+  this->linearVelocityUpdated = true;
+}
+
+Spherical16 Thing::GetLinearVelocity() {
+  return this->linearVelocity;
+}
+
+void Thing::SetAngularVelocity(Spherical16 angularVelocity) {
+  this->angularVelocity = angularVelocity;
+  this->angularVelocityUpdated = true;
+}
+
+Spherical16 Thing::GetAngularVelocity() {
+  return this->angularVelocity;
+}
+
 }  // namespace RoboidControl

Thing.h

@@ -1,16 +1,13 @@
 #pragma once
-
-#if !defined(NO_STD)
 #include <iostream>
 #include <list>
-#endif
 #include "LinearAlgebra/Spherical.h"
 #include "LinearAlgebra/SwingTwist.h"
 
 namespace RoboidControl {
 
 class Participant;
-class ParticipantUDP;
+class LocalParticipant;
 
 #define THING_STORE_SIZE 256
 // IMPORTANT: values higher than 256 will need to change the Thing::id type
@@ -20,7 +17,7 @@ class ParticipantUDP;
 class Thing {
  public:
   /// @brief Predefined thing types
-  enum Type : unsigned char {
+  enum Type {
     Undetermined,
     // Sensor,
     Switch,
@@ -32,107 +29,54 @@ class Thing {
     ControlledMotor,
     UncontrolledMotor,
     Servo,
-    IncrementalEncoder,
     // Other
     Roboid,
     Humanoid,
     ExternalSensor,
-    DifferentialDrive
   };
 
-#pragma region Init
+  /// @brief Create a new thing using an implicit local participant
-  static Thing* LocalRoot();
+  /// @param thingType The type of thing
-
+  Thing(int thingType = Type::Undetermined);
- private:
+  /// @brief Create a new thing of the given type
-  // Special constructor to create a root thing
+  /// @param thingType The predefined type of thing
-  Thing(Participant* parent);
+  Thing(Participant* participant, Type thingType = Type::Undetermined);
-  // Which can only be used by the Participant
+  /// @brief Create a new thing of the give type
-  friend class Participant;
+  /// @param thingType The custom type of the thing
-
+  Thing(Participant* participant, int thingType);
- public:
+  /// @brief Create a new thing for the given participant
-  /// @brief Create a new thing
+  /// @param participant The participant for which this thing is created
-  /// @param thingType The type of thing (can use Thing::Type)
+  /// @param networkId The network ID of the thing
-  /// @param parent (optional) The parent thing
+  /// @param thingId The ID of the thing
-  /// The owner will be the same as the owner of the parent thing, it will
+  /// @param thingType The type of thing
-  /// be Participant::LocalParticipant if the parent is not specified. A thing
+  Thing(Participant* participant, unsigned char networkId, unsigned char thingId, Type thingType = Type::Undetermined);
-  /// without a parent will be a root thing.
-  Thing(unsigned char thingType = Thing::Type::Undetermined,
-        Thing* parent = LocalRoot());
-
-  /// @brief Create a new child thing
-  /// @param parent The parent thing
-  /// @param thingType The type of thing (can use Thing::Type)
-  /// @param thingId The ID of the thing, leave out or set to zero to generate
-  /// an ID
-  /// @note The owner will be the same as the owner of the parent thing
-
-  ~Thing();
-
-  static Thing Reconstruct(Participant* owner,
-                           unsigned char thingType,
-                           unsigned char thingId);
-
-#pragma endregion Init
-
- public:
-  /// @brief Terminated things are no longer updated
-  bool terminate = false;
-
-#pragma region Properties
 
   /// @brief The participant managing this thing
-  Participant* owner = nullptr;
+  Participant* owner;
-
+  /// @brief The network ID of this thing
+  /// @note This field will likely disappear in future versions
+  unsigned char networkId = 0;
   /// @brief The ID of the thing
   unsigned char id = 0;
 
   /// @brief The type of Thing
   /// This can be either a Thing::Type of a byte value for custom types
-  unsigned char type = Type::Undetermined;
+  unsigned char type = 0;
 
-  /// @brief The name of the thing
+  /// @brief Find a thing by name
-  const char* name = nullptr;
+  /// @param name Rhe name of the thing
+  /// @return The found thing or nullptr when nothing is found
+  Thing* FindThing(const char* name);
 
- public:
+  /// @brief Sets the parent Thing
-  void SetName(const char* name);
+  /// @param parent The Thing which should become the parnet
-  const char* GetName() const;
+  /// @remark This is equivalent to calling parent->AddChild(this);
-  bool nameChanged = false;
+  virtual void SetParent(Thing* parent);
-
+  void SetParent(Thing* root, const char* name);
-  /// @brief Sets the location from where the 3D model of this Thing can be
+  /// @brief Gets the parent Thing
-  /// loaded from
-  /// @param url The url of the model
-  /// @remark Although the roboid implementation is not dependent on the model,
-  /// the only official supported model format is .obj
-  void SetModel(const char* url);
-
-  /// @brief An URL pointing to the location where a model of the thing can be
-  /// found
-  const char* modelUrl = nullptr;
-  /// @brief The scale of the model (deprecated I think)
-  float modelScale = 1;
-
-#pragma endregion Properties
-
-#pragma region Hierarchy
-
-  /// @brief Sets the parent of this Thing
-  /// @param parent The Thing which should become the parent
-  // virtual void SetParent(Thing* parent);
-  void SetParent(Thing* parent);
-  /// @brief Gets the parent of this Thing
   /// @return The parent Thing
-  // Thing* GetParent();
   Thing* GetParent();
 
-  bool IsRoot() const;
-
-  /// @brief The number of children
-  unsigned char childCount = 0;
-  /// @brief Get a child by index
-  /// @param ix The child index
-  /// @return The found thing of nullptr when nothing is found
-  Thing* GetChildByIndex(unsigned char ix);
-
   /// @brief Add a child Thing to this Thing
   /// @param child The Thing which should become a child
   /// @remark When the Thing is already a child, it will not be added again
@@ -142,112 +86,106 @@ class Thing {
   /// @return The removed child or nullptr if the child could not be found
   Thing* RemoveChild(Thing* child);
 
+  /// @brief The number of children
+  unsigned char childCount = 0;
   /// @brief Get a child by thing Id
   /// @param id The thing ID to find
-  /// @param recurse Look recursively through all descendants
+  /// @param recursive Look recursively through all descendants
   /// @return The found thing of nullptr when nothing is found
-  Thing* GetChild(unsigned char id, bool recurse = false);
+  Thing* GetChild(unsigned char id, bool recursive = false);
+  /// @brief Get a child by index
+  /// @param ix The child index
+  /// @return The found thing of nullptr when nothing is found
+  Thing* GetChildByIndex(unsigned char ix);
 
-  /// @brief Find a thing by name
+ protected:
-  /// @param name The name of the thing
-  /// @param recurse Look recursively through all descendants
-  /// @return The found thing or nullptr when nothing is found
-  Thing* FindChild(const char* name, bool recurse = true);
-
-  /// @brief Indicator that the hierarchy of the thing has changed
-  bool hierarchyChanged = true;
-
- private:
   Thing* parent = nullptr;
   Thing** children = nullptr;
 
-#pragma endregion Hierarchy
-
-#pragma region Pose
-
  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 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 Boolean indicating that the thing has an updated position
-  bool positionUpdated = false;
-
   /// @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 Boolean indicating the thing has an updated orientation
+  /// @brief The scale of the thing (deprecated I think)
+  //float scale = 1;  // assuming uniform scale
+
+  /// @brief boolean indicating if the position was updated
+  bool positionUpdated = false;
+  /// @brief boolean indicating if the orientation was updated
   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 Boolean indicating the thing has an updated linear velocity
-  bool linearVelocityUpdated = false;
-
   /// @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();
-  /// @brief Boolean indicating the thing has an updated angular velocity
+  bool linearVelocityUpdated = false;
   bool angularVelocityUpdated = false;
 
  private:
-  /// @brief The position of the thing in local space, in meters
+  /// @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 of the thing in local space
+  /// @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 of the thing in local space, in meters per
+  /// @brief The linear velocity in local space
-  /// second
+  Spherical16 linearVelocity;
-  Spherical linearVelocity;
+  /// @brief The angular velocity in local spze
-  /// @brief The angular velocity of the thing in local space, in degrees per
+  Spherical16 angularVelocity;
-  /// second
-  Spherical angularVelocity;
-
-#pragma endregion Pose
-
-#pragma region Update
 
  public:
-  virtual void PrepareForUpdate();
+  /// @brief Terminated things are no longer updated
+  void Terminate();
 
-  /// @brief Updates the state of the thing
+  /// @brief Sets the location from where the 3D model of this Thing can be
-  /// @param currentTimeMs The current clock time in milliseconds; if this is
+  /// loaded from
-  /// zero, the current time is retrieved automatically
+  /// @param url The url of the model
-  /// @param recurse When true, this will Update the descendants recursively
+  /// @remark Although the roboid implementation is not dependent on the model,
-  virtual void Update(bool recurse = false);
+  /// the only official supported model format is .obj
+  void SetModel(const char* url);
 
-  static void UpdateThings();
-
-  /// @brief Get the current time in milliseconds
-  /// @return The current time in milliseconds
   static unsigned long GetTimeMs();
 
-#pragma endregion Update
+  void Update();
+
+  /// @brief Updates the state of the thing
+  /// @param currentTimeMs The current clock time in milliseconds
+  virtual void Update(unsigned long currentTimeMs);  // { (void)currentTimeMs; };
+
+  static void UpdateThings(unsigned long currentTimeMs);
 
- public:
   /// @brief Function used to generate binary data for this thing
   /// @param buffer The byte array for thw binary data
   /// @param ix The starting position for writing the binary data
-  /// @return The size of the binary data
+  virtual void GenerateBinary(char* buffer, unsigned char* ix);
-  virtual int GenerateBinary(char* buffer, unsigned char* ix);
+  // /// @brief FUnction used to process binary data received for this thing
-  /// @brief Function used to process binary data received for this thing
   /// @param bytes The binary data
   virtual void ProcessBinary(char* bytes);
 };

Things/ControlledMotor.cpp

@@ -1,68 +0,0 @@
-#include "ControlledMotor.h"
-#include "LinearAlgebra/FloatSingle.h"
-
-namespace RoboidControl {
-
-ControlledMotor::ControlledMotor(Motor* motor,
-                                 RelativeEncoder* encoder,
-                                 Thing* parent)
-    : Motor(parent), motor(motor), encoder(encoder) {
-  this->type = Type::ControlledMotor;
-  //encoder->SetParent(null);
-  //  Thing parent = motor.GetParent();
-  //  this->SetParent(parent);
-  this->integral = 0;
-}
-
-void ControlledMotor::SetTargetVelocity(float velocity) {
-  this->targetVelocity = velocity;
-  this->rotationDirection =
-      (targetVelocity < 0) ? Direction::Reverse : Direction::Forward;
-}
-
-void ControlledMotor::Update(bool recurse) {
-  unsigned long currentTimeMs = GetTimeMs();
-  float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f;
-  this->lastUpdateTime = currentTimeMs;
-  if (timeStep <= 0)
-    return;
-
-  // encoder->Update(false);
-
-  this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed;
-
-  float error = this->targetVelocity - this->actualVelocity;
-  float p_term = error * pidP;
-  this->integral += error * timeStep;
-  float i_term = pidI * this->integral;
-  float d_term = pidD * (error - this->lastError) / timeStep;
-  this->lastError = error;
-
-  float output = p_term + i_term + d_term;
-  std::cout << "target " << this->targetVelocity << " actual "
-            << this->actualVelocity << " output = " << output << std::endl;
-  // float acceleration =
-  //     error * timeStep * pidP;  // Just P is used at this moment
-  // std::cout << "motor acc. " << acceleration << std::endl;
-
-  // float newTargetVelocity = motor->targetVelocity + acceleration;
-  output = LinearAlgebra::Float::Clamp(output, -1, 1);
-  motor->SetTargetVelocity(output);  // or something like that
-  //motor->Update(false);
-}
-
-// float ControlledMotor::GetActualVelocity() {
-//   return (int)rotationDirection * encoder->rotationSpeed;
-// }
-
-// bool ControlledMotor::Drive(float distance) {
-//   if (!driving) {
-//     targetDistance = distance;
-//     startDistance = encoder->GetDistance();
-//     driving = true;
-//   }
-//   float totalDistance = encoder->GetDistance() - startDistance;
-//   bool completed = totalDistance > targetDistance;
-//   return completed;
-// }
-}  // namespace RoboidControl

Things/ControlledMotor.h

@@ -1,40 +0,0 @@
-#pragma once
-
-#include "Motor.h"
-#include "RelativeEncoder.h"
-
-namespace RoboidControl {
-
-/// @brief A motor with speed control
-/// It uses a feedback loop from an encoder to regulate the speed
-/// The speed is measured in revolutions per second.
-class ControlledMotor : public Motor {
- public:
-  ControlledMotor(Motor* motor, RelativeEncoder* encoder, Thing* parent = Thing::LocalRoot());
-
-  float pidP = 0.5;
-  float pidD = 0;
-  float pidI = 0.2;
-
-  /// @brief The actual velocity in revolutions per second
-  float actualVelocity;
-
-  enum Direction { Forward = 1, Reverse = -1 };
-  Direction rotationDirection;
-
-  virtual void Update(bool recurse = false) override;
-
-  /// @brief Set the target verlocity for the motor controller
-  /// @param speed the target velocity in revolutions per second
-  virtual void SetTargetVelocity(float velocity) override;
-
-  Motor* motor;
-  RelativeEncoder* encoder;
-
- protected:
-  float integral = 0;
-  float lastError = 0;
-  float lastUpdateTime;
-};
-
-}  // namespace RoboidControl

Things/DifferentialDrive.cpp

@@ -1,36 +1,16 @@
 #include "DifferentialDrive.h"
 
-#include "Messages/LowLevelMessages.h"
-
 namespace RoboidControl {
 
-DifferentialDrive::DifferentialDrive(Thing* parent)
+RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant) : Thing(participant) {
-    : Thing(Type::DifferentialDrive, parent) {
+  // this->leftWheel = new Thing(participant);
-  this->name = "Differential drive";
+  // this->rightWheel = new Thing(participant);
-
-  this->leftWheel = new Motor(this);
-  this->leftWheel->name = "Left motor";
-
-  this->rightWheel = new Motor(this);
-  this->rightWheel->name = "Right motor";
 }
 
-DifferentialDrive::DifferentialDrive(Motor* leftMotor,
+void DifferentialDrive::SetDimensions(float wheelDiameter, float wheelSeparation) {
-                                     Motor* rightMotor,
+  this->wheelRadius = wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2;
-                                     Thing* parent)
+  this->wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation;
-    : Thing(Type::DifferentialDrive, parent) {
+  this->rpsToMs = wheelDiameter * Passer::LinearAlgebra::pi;
-  this->name = "Differential drive";
-  this->leftWheel = leftMotor;
-  this->rightWheel = rightMotor;
-}
-
-void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
-                                           float wheelSeparation) {
-  this->wheelRadius =
-      wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2;
-  this->wheelSeparation =
-      wheelSeparation > 0 ? wheelSeparation : -wheelSeparation;
-  this->rpsToMs = wheelDiameter * LinearAlgebra::pi;
 
   float distance = this->wheelSeparation / 2;
   if (leftWheel != nullptr)
@@ -39,39 +19,18 @@ void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
     this->rightWheel->SetPosition(Spherical(distance, Direction::right));
 }
 
-// Motor& DifferentialDrive::GetMotorLeft() {
+void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) {
-//   return *this->leftWheel;
-// }
-
-// Motor& DifferentialDrive::GetMotorRight() {
-//   return *this->rightWheel;
-// }
-
-void DifferentialDrive::SetMotors(Motor& leftMotor, Motor& rightMotor) {
   float distance = this->wheelSeparation / 2;
-  this->leftWheel = &leftMotor;
+  this->leftWheel = leftWheel;
+  if (leftWheel != nullptr)
     this->leftWheel->SetPosition(Spherical(distance, Direction::left));
 
-  this->rightWheel = &rightMotor;
+  this->rightWheel = rightWheel;
+  if (rightWheel != nullptr)
     this->rightWheel->SetPosition(Spherical(distance, Direction::right));
 }
 
-void DifferentialDrive::SetWheelVelocity(float velocityLeft,
+void DifferentialDrive::Update(unsigned long currentMs) {
-                                         float velocityRight) {
-  // if (this->leftWheel != nullptr)
-  //   this->leftWheel->SetAngularVelocity(Spherical(velocityLeft,
-  //   Direction::left));
-  // if (this->rightWheel != nullptr)
-  //   this->rightWheel->SetAngularVelocity(
-  //       Spherical(velocityRight, Direction::right));
-  if (this->leftWheel != nullptr)
-    this->leftWheel->SetTargetVelocity(velocityLeft);
-  if (this->rightWheel != nullptr)
-    this->rightWheel->SetTargetVelocity(velocityRight);
-}
-
-void DifferentialDrive::Update(bool recursive) {
-  if (this->linearVelocityUpdated) {
   // this assumes forward velocity only....
   float linearVelocity = this->GetLinearVelocity().distance;
 
@@ -82,23 +41,16 @@ void DifferentialDrive::Update(bool recursive) {
     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) /
+  if (this->leftWheel != nullptr)
-        this->wheelRadius * Rad2Deg;
+    this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left));
-    float speedRight =
-        (linearVelocity - angularSpeed * this->wheelSeparation / 2) /
-        this->wheelRadius * Rad2Deg;
 
-    this->SetWheelVelocity(speedLeft, speedRight);
+  float speedRight = (linearVelocity - angularSpeed * this->wheelSeparation / 2) / this->wheelRadius * Rad2Deg;
-  }
+  if (this->rightWheel != nullptr)
-  Thing::Update(recursive);
+    this->rightWheel->SetAngularVelocity(Spherical(speedRight, Direction::right));
-}
 
-int DifferentialDrive::GenerateBinary(char* data, unsigned char* ix) {
+    // std::cout << "lin. speed " << linearVelocity << "   ang. speed " << angularVelocity.distance << "   left wheel  "
-  data[(*ix)++] = this->leftWheel->id;
+    //         << speedLeft << "   right wheel " << speedRight << "\n";
-  data[(*ix)++] = this->rightWheel->id;
-  LowLevelMessages::SendFloat16(data, ix, this->wheelRadius);
-  return 4;
 }
 
 }  // namespace RoboidControl

Things/DifferentialDrive.h

@@ -1,66 +1,30 @@
 #pragma once
 
 #include "Thing.h"
-#include "Motor.h"
 
 namespace RoboidControl {
 
 /// @brief A thing which can move itself using a differential drive system
-///
-/// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink
 class DifferentialDrive : public Thing {
  public:
-  /// @brief Create a new child differential drive
+  DifferentialDrive(Participant* participant);
-  /// @param parent The parent thing
-  /// @param thingId The ID of the thing, leave out or set to zero to generate
-  /// an ID
-  DifferentialDrive(Thing* parent = Thing::LocalRoot());
 
-  DifferentialDrive(Motor* leftMotor, Motor* rightMotor, Thing* parent = Thing::LocalRoot());
+  void SetDimensions(float wheelDiameter, float wheelSeparation);
+  void SetMotors(Thing* leftWheel, Thing* rightWheel);
 
-  /// @brief Configures the dimensions of the drive
+  virtual void Update(unsigned long currentMs) override;
-  /// @param wheelDiameter The diameter of the wheels in meters
-  /// @param wheelSeparation The distance between the wheels in meters
-  ///
-  /// These values are used to compute the desired wheel speed from the set
-  /// linear and angular velocity.
-  /// @sa SetLinearVelocity SetAngularVelocity
-  void SetDriveDimensions(float wheelDiameter, float wheelSeparation);
-
-  // Motor& GetMotorLeft();
-  // Motor& GetMotorRight();
-  /// @brief Congures the motors for the wheels
-  /// @param leftWheel The motor for the left wheel
-  /// @param rightWheel The motor for the right wheel
-  void SetMotors(Motor& leftMotor, Motor& rightMotor);
-
-  /// @brief Directly specify the speeds of the motors
-  /// @param speedLeft The speed of the left wheel in degrees per second.
-  /// Positive moves the robot in the forward direction.
-  /// @param speedRight The speed of the right wheel in degrees per second.
-  /// Positive moves the robot in the forward direction.
-  void SetWheelVelocity(float speedLeft, float speedRight);
-
-  /// @copydoc RoboidControl::Thing::Update(unsigned long)
-  virtual void Update(bool recursive = true) override;
-
-  int GenerateBinary(char* bytes, unsigned char* ix) override;
-  // virtual void ProcessBinary(char* bytes) override;
-
-    /// @brief The left wheel
-  Motor* leftWheel = nullptr;
-  /// @brief The right wheel
-  Motor* rightWheel = nullptr;
 
  protected:
   /// @brief The radius of a wheel in meters
-  float wheelRadius = 0.0f;
+  float wheelRadius = 1.0f;
   /// @brief The distance between the wheels in meters
-  float wheelSeparation = 0.0f;
+  float wheelSeparation = 1.0f;
 
   /// @brief Convert revolutions per second to meters per second
   float rpsToMs = 1.0f;
 
+  Thing* leftWheel = nullptr;
+  Thing* rightWheel = nullptr;
 };
 
 }  // namespace RoboidControl

Things/DigitalSensor.cpp

@@ -2,26 +2,8 @@
 
 namespace RoboidControl {
 
-//DigitalSensor::DigitalSensor() : Thing(Type::Switch) {}
+//DigitalSensor::DigitalSensor() {}
 
-// DigitalSensor::DigitalSensor(Participant* owner, unsigned char thingId)
+DigitalSensor::DigitalSensor(Participant* participant, unsigned char networkId, unsigned char thingId) : Thing(participant) {}
-//     : Thing(owner, Type::Switch, thingId) {}
-
-// DigitalSensor::DigitalSensor(Thing* parent, unsigned char thingId)
-//     : Thing(parent, Type::Switch) {}
-
-// DigitalSensor::DigitalSensor(Participant* owner) : Thing(owner, Type::Switch) {}
-
-// DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent, Type::Switch) {}
-DigitalSensor::DigitalSensor(Thing* parent) : Thing(Type::Switch, parent) {}
-
-int DigitalSensor::GenerateBinary(char* bytes, unsigned char* ix) {
-  bytes[(*ix)++] = state ? 1 : 0;
-  return 1;
-}
-
-void DigitalSensor::ProcessBinary(char* bytes) {
-  this->state |= (bytes[0] == 1);
-}
 
 }  // namespace RoboidControl

Things/DigitalSensor.h

@@ -7,31 +7,15 @@ namespace RoboidControl {
 /// @brief A digital (on/off, 1/0, true/false) sensor
 class DigitalSensor : public Thing {
  public:
-  /// @brief Create a digital sensor without communication abilities
-  //DigitalSensor();
-  /// @brief Create a digital sensor for a participant
-  /// @param owner The owning participant
-  /// @param thingId The ID of the thing, leave out or set to zero to generate
-  /// an ID
-  DigitalSensor(Participant* owner = nullptr); //, unsigned char thingId = 0);
-  /// @brief Create a new child digital sensor
-  /// @param parent The parent thing
-  /// @param thingId The ID of the thing, leave out or set to zero to generate
-  /// an ID
-  // DigitalSensor(Thing* parent); //, unsigned char thingId = 0);
-  DigitalSensor(Thing* parent = Thing::LocalRoot());
-
   /// @brief The sigital state
   bool state = 0;
 
-  /// @brief Function used to generate binary data for this digital sensor
+  /// @brief The default constructor
-  /// @param buffer The byte array for thw binary data
+  //DigitalSensor();
-  /// @param ix The starting position for writing the binary data
+  /// @brief Create a temperature sensor with the given ID
-  int GenerateBinary(char* bytes, unsigned char* ix) override;
+  /// @param networkId The network ID of the sensor
-  /// @brief Function used to process binary data received for this digital
+  /// @param thingId The ID of the thing
-  /// sensor
+  DigitalSensor(Participant* participant, unsigned char networkId, unsigned char thingId);
-  /// @param bytes The binary data to process
-  virtual void ProcessBinary(char* bytes) override;
 };
 
 }  // namespace RoboidControl

Things/Motor.cpp

@@ -1,16 +0,0 @@
-#include "Motor.h"
-
-namespace RoboidControl {
-
-Motor::Motor(Thing* parent) : Thing(Type::UncontrolledMotor, parent) {}
-
-void Motor::SetTargetVelocity(float targetSpeed) {
-    this->targetVelocity = targetSpeed;
-}
-
-int Motor::GenerateBinary(char* data, unsigned char* ix) {
-  data[(*ix)++] = this->targetVelocity * 127.0f;
-  return 1;
-}
-
-}  // namespace RoboidControl

Things/Motor.h

@@ -1,25 +0,0 @@
-#pragma once
-
-#include "Thing.h"
-
-namespace RoboidControl {
-
-class Motor : public Thing {
- public:
-  Motor(Thing* parent = Thing::LocalRoot());
-
-  /// @brief Motor turning direction
-  enum class Direction { Clockwise = 1, CounterClockwise = -1 };
-  /// @brief The forward turning direction of the motor
-  Direction direction;
-
-  virtual void SetTargetVelocity(float velocity);  // -1..0..1
-
-  int GenerateBinary(char* bytes, unsigned char* ix) override;
-  // virtual void ProcessBinary(char* bytes) override;
-
- //protected:
-  float targetVelocity = 0;
-};
-
-}  // namespace RoboidControl

Things/RelativeEncoder.cpp

@@ -1,21 +0,0 @@
-#include "RelativeEncoder.h"
-
-namespace RoboidControl {
-
-RelativeEncoder::RelativeEncoder(Thing* parent)
-    : Thing(Type::IncrementalEncoder, parent) {}
-
-float RelativeEncoder::GetRotationSpeed() {
-  return rotationSpeed;
-}
-
-int RelativeEncoder::GenerateBinary(char* data, unsigned char* ix) {
-  data[(*ix)++] = (unsigned char)(this->rotationSpeed * 127);
-  return 1;
-}
-
-void RelativeEncoder::ProcessBinary(char* data) {
-  this->rotationSpeed = (float)data[0] / 127;
-}
-
-}  // namespace RoboidControl

Things/RelativeEncoder.h

@@ -1,39 +0,0 @@
-#pragma once
-
-#include "Thing.h"
-
-namespace RoboidControl {
-
-/// @brief An Incremental Encoder measures the rotations of an axle using a
-/// rotary sensor. Some encoders are able to detect direction, while others can
-/// not.
-class RelativeEncoder : public Thing {
- public:
-  /// @brief Creates a sensor which measures distance from pulses
-  /// @param pulsesPerRevolution The number of pulse edges which make a
-  /// full rotation
-  /// @param distancePerRevolution The distance a wheel travels per full
-  /// rotation
-  //RelativeEncoder(Participant* owner);
-  // RelativeEncoder(Thing* parent);
-  RelativeEncoder(Thing* parent = Thing::LocalRoot());
-
-  /// @brief Get the rotation speed 
-  /// @return The speed in revolutions per second
-  virtual float GetRotationSpeed();
-  float rotationSpeed = 0;
-
-  /// @brief Function used to generate binary data for this touch sensor
-  /// @param buffer The byte array for thw binary data
-  /// @param ix The starting position for writing the binary data
-  int GenerateBinary(char* bytes, unsigned char* ix) override;
-  /// @brief Function used to process binary data received for this touch sensor
-  /// @param bytes The binary data to process
-  virtual void ProcessBinary(char* bytes) override;
-
-protected:
-    /// @brief rotation speed in revolutions per second
-    //float _rotationSpeed;
-};
-
-}  // namespace RoboidControl

Things/TemperatureSensor.cpp

@@ -4,25 +4,20 @@
 
 namespace RoboidControl {
 
-// TemperatureSensor::TemperatureSensor(Participant* participant,
+// TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {}
-//                                      unsigned char thingId)
-//     : Thing(participant, Type::TemperatureSensor, thingId) {}
 
-// TemperatureSensor::TemperatureSensor(Participant* owner) : Thing(owner, Type::TemperatureSensor) {}
+//TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {}
 
-TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(Type::TemperatureSensor, parent) {}
+TemperatureSensor::TemperatureSensor(Participant* participant, unsigned char networkId, unsigned char thingId)
-
+    : Thing(participant, networkId, thingId, Type::TemperatureSensor) {}
-// TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent, Type::TemperatureSensor) {}
 
 void TemperatureSensor::SetTemperature(float temp) {
   this->temperature = temp;
 }
 
-int TemperatureSensor::GenerateBinary(char* buffer, unsigned char* ix) {
+void TemperatureSensor::GenerateBinary(char* buffer, unsigned char* ix) {
-  unsigned char startIx = *ix;
+  std::cout << "Send temperature: " << this->temperature << "\n";
-  // std::cout << "Send temperature: " << this->temperature << "\n";
   LowLevelMessages::SendFloat16(buffer, ix, this->temperature);
-  return *ix - startIx;
 }
 
 void TemperatureSensor::ProcessBinary(char* bytes) {

Things/TemperatureSensor.h

@@ -7,17 +7,15 @@ namespace RoboidControl {
 /// @brief A temperature sensor
 class TemperatureSensor : public Thing {
  public:
+  /// @brief The measured temperature
+  float temperature = 0;
+
   /// @brief The default constructor
   //TemperatureSensor();
   /// @brief Create a temperature sensor with the given ID
   /// @param networkId The network ID of the sensor
   /// @param thingId The ID of the thing
-  TemperatureSensor(Participant* participant); //, unsigned char thingId);
+  TemperatureSensor(Participant* participant, unsigned char networkId, unsigned char thingId);
-  // TemperatureSensor(Thing* parent);
-  TemperatureSensor(Thing* parent = Thing::LocalRoot());
-
-      /// @brief The measured temperature
-      float temperature = 0;
 
   /// @brief Manually override the measured temperature
   /// @param temperature The new temperature
@@ -26,7 +24,7 @@ class TemperatureSensor : public Thing {
   /// @brief Function to create a binary message with the temperature
   /// @param buffer The byte array for thw binary data
   /// @param ix The starting position for writing the binary data
-  int GenerateBinary(char* bytes, unsigned char* ix) override;
+  void GenerateBinary(char* bytes, unsigned char* ix) override;
   /// @brief Function to extract the temperature received in the binary message
   /// @param bytes The binary data
   virtual void ProcessBinary(char* bytes) override;

Things/TouchSensor.cpp

@@ -2,29 +2,17 @@
 
 namespace RoboidControl {
 
-TouchSensor::TouchSensor(Thing* parent) : Thing(Type::TouchSensor, parent) {
+TouchSensor::TouchSensor(Participant* participant) : Thing(participant) {
-  this->name = "Touch sensor";
+  this->touchedSomething = false;
+  this->type = (unsigned char)Thing::Type::TouchSensor;
 }
 
-void TouchSensor::PrepareForUpdate() {
+void TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {}
-  this->touchedSomething = this->externalTouch;
-}
-
-void TouchSensor::Update(bool recursive) {
-  Thing::Update(recursive);
-}
-
-int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {
-  bytes[(*ix)++] = this->touchedSomething ? 1 : 0;
-  return 1;
-}
 
 void TouchSensor::ProcessBinary(char* bytes) {
-  this->externalTouch = (bytes[0] == 1);
+  if (bytes[0] == 1)
-  if (this->externalTouch)
+    std::cout << this->name << " is Touching something!\n";
-    std::cout << "touching!\n";
+  this->touchedSomething |= (bytes[0] == 1);
-  else
-    std::cout << "not touching\n";
 }
 
 }  // namespace RoboidControl

Things/TouchSensor.h

@@ -6,30 +6,24 @@ namespace RoboidControl {
 
 /// @brief A sensor which can detect touches
 class TouchSensor : public Thing {
-// Why finishing this release (0.3), I notice that this is equivalent to a digital sensor
  public:
-  /// @brief Create a new child touch sensor
+  /// @brief Value which is true when the sensor is touching something, false otherwise
-  /// @param parent The parent thing
-  /// @param thingId The ID of the thing, leave out or set to zero to generate
-  /// an ID
-  TouchSensor(Thing* parent = Thing::LocalRoot());
-
-  /// @brief Value which is true when the sensor is touching something, false
-  /// otherwise
   bool touchedSomething = false;
 
-  virtual void PrepareForUpdate() override;
+  /// @brief Create a touch sensor
-  virtual void Update(bool recursive) override;
+  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(RemoteParticipant* participant, unsigned char networkId, unsigned char thingId);
 
-  /// @brief Function used to generate binary data for this touch sensor
+  /// @brief Function to create a binary message with the temperature
   /// @param buffer The byte array for thw binary data
   /// @param ix The starting position for writing the binary data
-  int GenerateBinary(char* bytes, unsigned char* ix) override;
+  void GenerateBinary(char* bytes, unsigned char* ix) override;
-  /// @brief Function used to process binary data received for this touch sensor
+  /// @brief Function to extract the temperature received in the binary message
-  /// @param bytes The binary data to process
+  /// @param bytes The binary data
   virtual void ProcessBinary(char* bytes) override;
-protected:
-  bool externalTouch = false;
 };
 
 }  // namespace RoboidControl

Windows/WindowsParticipant.cpp

@@ -4,22 +4,36 @@
 #include <winsock2.h>
 #include <ws2tcpip.h>
 #pragma comment(lib, "ws2_32.lib")
+
+#elif defined(__unix__) || defined(__APPLE__)
+#include <arpa/inet.h>
+#include <fcntl.h>  // For fcntl
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <unistd.h>
 #endif
 
 namespace RoboidControl {
 namespace Windows {
 
-void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
+void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
 #if defined(_WIN32) || defined(_WIN64)
 
   // Create a UDP socket
+#if defined(_WIN32) || defined(_WIN64)
   // Windows-specific Winsock initialization
   WSADATA wsaData;
   if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
     std::cerr << "WSAStartup failed" << std::endl;
     return;
   }
+#endif
+
+#if defined(_WIN32) || defined(_WIN64)
   this->sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+#elif defined(__unix__) || defined(__APPLE__)
+  this->sock = socket(AF_INET, SOCK_DGRAM, 0);
+#endif
 
   if (this->sock < 0) {
     std::cerr << "Error creating socket" << std::endl;
@@ -27,8 +41,13 @@ void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remot
   }
 
 // Set the socket to non-blocking mode
+#if defined(_WIN32) || defined(_WIN64)
   u_long mode = 1;  // 1 to enable non-blocking socket
   ioctlsocket(this->sock, FIONBIO, &mode);
+#elif defined(__unix__) || defined(__APPLE__)
+  int flags = fcntl(this->sock, F_GETFL, 0);
+  fcntl(this->sock, F_SETFL, flags | O_NONBLOCK);
+#endif
 
   if (remotePort != 0) {
     // Set up the address to send to
@@ -37,8 +56,12 @@ void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remot
     remote_addr.sin_port = htons((u_short)remotePort);
     if (inet_pton(AF_INET, remoteIpAddress, &remote_addr.sin_addr) <= 0) {
       std::cerr << "Invalid address" << std::endl;
+#if defined(_WIN32) || defined(_WIN64)
       closesocket(sock);
       WSACleanup();
+#elif defined(__unix__) || defined(__APPLE__)
+      close(sock);
+#endif
       return;
     }
   }
@@ -49,22 +72,30 @@ void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remot
   server_addr.sin_port = htons((u_short)localPort);
   if (inet_pton(AF_INET, "0.0.0.0", &server_addr.sin_addr) <= 0) {
     std::cerr << "Invalid address" << std::endl;
+#if defined(_WIN32) || defined(_WIN64)
     closesocket(sock);
     WSACleanup();
+#elif defined(__unix__) || defined(__APPLE__)
+    close(sock);
+#endif
     return;
   }
 
   // Bind the socket to the specified port
   if (bind(this->sock, (const struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) {
     std::cerr << "Bind failed" << std::endl;
+#if defined(_WIN32) || defined(_WIN64)
     closesocket(sock);
     WSACleanup();
+#elif defined(__unix__) || defined(__APPLE__)
+    close(sock);
+#endif
   }
 
-#endif // _WIN32 || _WIN64
+#endif
 }
 
-void ParticipantUDP::Receive() {
+void LocalParticipant::Receive() {
 #if defined(_WIN32) || defined(_WIN64)
   // char ip_str[INET_ADDRSTRLEN];
   // inet_ntop(AF_INET, &(server_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
@@ -72,22 +103,30 @@ void ParticipantUDP::Receive() {
   //           << ntohs(server_addr.sin_port) << "\n";
 
   sockaddr_in client_addr;
+#if defined(_WIN32) || defined(_WIN64)
   int len = sizeof(client_addr);
+#elif defined(__unix__) || defined(__APPLE__)
+  socklen_t len = sizeof(client_addr);
+#endif
   int packetSize = recvfrom(this->sock, buffer, sizeof(buffer), 0, (struct sockaddr*)&client_addr, &len);
   // std::cout << "received data " << packetSize << "\n";
   if (packetSize < 0) {
+#if defined(_WIN32) || defined(_WIN64)
     int error_code = WSAGetLastError();  // Get the error code on Windows
     if (error_code != WSAEWOULDBLOCK)
       std::cerr << "recvfrom failed with error: " << error_code << std::endl;
+#else
+    // std::cerr << "recvfrom failed with error: " << packetSize << std::endl;
+#endif
   } else if (packetSize > 0) {
     char sender_ipAddress[INET_ADDRSTRLEN];
     inet_ntop(AF_INET, &(client_addr.sin_addr), sender_ipAddress, INET_ADDRSTRLEN);
     unsigned int sender_port = ntohs(client_addr.sin_port);
 
     ReceiveData(packetSize, sender_ipAddress, sender_port);
-    // RoboidControl::ParticipantUDP* remoteParticipant = this->Get(sender_ipAddress, sender_port);
+    // RoboidControl::LocalParticipant* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port);
     // if (remoteParticipant == nullptr) {
-    //   remoteParticipant = this->Add(sender_ipAddress, sender_port);
+    //   remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port);
     //   // std::cout << "New sender " << sender_ipAddress << ":"
     //   //           << sender_port << "\n";
     //   // std::cout << "New remote participant " <<
@@ -99,16 +138,16 @@ void ParticipantUDP::Receive() {
 
     // ReceiveData(packetSize, remoteParticipant);
   }
-#endif // _WIN32 || _WIN64
+#endif
 }
 
-bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
+bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
 #if defined(_WIN32) || defined(_WIN64)
   char ip_str[INET_ADDRSTRLEN];
   inet_ntop(AF_INET, &(remote_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
   std::cout << "Send to " << ip_str << ":" << ntohs(remote_addr.sin_port) << "\n";
   int sent_bytes = sendto(sock, this->buffer, bufferSize, 0, (struct sockaddr*)&remote_addr, sizeof(remote_addr));
-
+#if defined(_WIN32) || defined(_WIN64)
   if (sent_bytes <= SOCKET_ERROR) {
     int error_code = WSAGetLastError();  // Get the error code on Windows
     std::cerr << "sendto failed with error: " << error_code << std::endl;
@@ -116,11 +155,18 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
     WSACleanup();
     return false;
   }
-#endif // _WIN32 || _WIN64
+#elif defined(__unix__) || defined(__APPLE__)
+  if (sent_bytes < 0) {
+    std::cerr << "sendto failed with error: " << sent_bytes << std::endl;
+    close(sock);
+    return false;
+  }
+#endif
+#endif
   return true;
 }
 
-bool ParticipantUDP::Publish(IMessage* msg) {
+bool LocalParticipant::Publish(IMessage* msg) {
 #if defined(_WIN32) || defined(_WIN64)
   int bufferSize = msg->Serialize(this->buffer);
   if (bufferSize <= 0)
@@ -130,7 +176,7 @@ bool ParticipantUDP::Publish(IMessage* msg) {
   inet_ntop(AF_INET, &(broadcast_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
   std::cout << "Publish to " << ip_str << ":" << ntohs(broadcast_addr.sin_port) << "\n";
   int sent_bytes = sendto(sock, this->buffer, bufferSize, 0, (struct sockaddr*)&broadcast_addr, sizeof(broadcast_addr));
-
+#if defined(_WIN32) || defined(_WIN64)
   if (sent_bytes <= SOCKET_ERROR) {
     int error_code = WSAGetLastError();  // Get the error code on Windows
     std::cerr << "sendto failed with error: " << error_code << std::endl;
@@ -138,7 +184,14 @@ bool ParticipantUDP::Publish(IMessage* msg) {
     WSACleanup();
     return false;
   }
-#endif // _WIN32 || _WIN64
+#elif defined(__unix__) || defined(__APPLE__)
+  if (sent_bytes < 0) {
+    std::cerr << "sendto failed with error: " << sent_bytes << std::endl;
+    close(sock);
+    return false;
+  }
+#endif
+#endif
   return true;
 }
 

Windows/WindowsParticipant.h

@@ -1,11 +1,11 @@
 #pragma once
 
-#include "Participants/ParticipantUDP.h"
+#include "../LocalParticipant.h"
 
 namespace RoboidControl {
 namespace Windows {
 
-class ParticipantUDP : public RoboidControl::ParticipantUDP {
+class LocalParticipant : public RoboidControl::LocalParticipant {
  public:
   void Setup(int localPort, const char* remoteIpAddress, int remotePort);
   void Receive();