Fix include

git-subtree-dir: ControlCore
git-subtree-mainline: 2b5f5a58ac608aca3aad452a87f6cb27f428cbde
git-subtree-split: 0a57e6d99abadc3257c6b1fdf5880b993e0d0fcb

.gitignore

@@ -1,3 +1,4 @@
+doxygen/html
+doxygen/DoxyWarnLogfile.txt
 build
 .vscode
-DoxyGen/DoxyWarnLogfile.txt

.gitlab-ci.yml

@@ -0,0 +1,61 @@
+# This file is a template, and might need editing before it works on your project.
+# This is a sample GitLab CI/CD configuration file that should run without any modifications.
+# It demonstrates a basic 3 stage CI/CD pipeline. Instead of real tests or scripts,
+# it uses echo commands to simulate the pipeline execution.
+#
+# A pipeline is composed of independent jobs that run scripts, grouped into stages.
+# Stages run in sequential order, but jobs within stages run in parallel.
+#
+# For more information, see: https://docs.gitlab.com/ee/ci/yaml/index.html#stages
+#
+# You can copy and paste this template into a new `.gitlab-ci.yml` file.
+# You should not add this template to an existing `.gitlab-ci.yml` file by using the `include:` keyword.
+#
+# To contribute improvements to CI/CD templates, please follow the Development guide at:
+# https://docs.gitlab.com/ee/development/cicd/templates.html
+# This specific template is located at:
+# https://gitlab.com/gitlab-org/gitlab/-/blob/master/lib/gitlab/ci/templates/Getting-Started.gitlab-ci.yml
+
+# This file is a template, and might need editing before it works on your project.
+# This is a sample GitLab CI/CD configuration file that should run without any modifications.
+# It demonstrates a basic 3 stage CI/CD pipeline. Instead of real tests or scripts,
+# it uses echo commands to simulate the pipeline execution.
+#
+# A pipeline is composed of independent jobs that run scripts, grouped into stages.
+# Stages run in sequential order, but jobs within stages run in parallel.
+#
+# For more information, see: https://docs.gitlab.com/ee/ci/yaml/index.html#stages
+#
+# You can copy and paste this template into a new `.gitlab-ci.yml` file.
+# You should not add this template to an existing `.gitlab-ci.yml` file by using the `include:` keyword.
+#
+# To contribute improvements to CI/CD templates, please follow the Development guide at:
+# https://docs.gitlab.com/ee/development/cicd/templates.html
+# This specific template is located at:
+# https://gitlab.com/gitlab-org/gitlab/-/blob/master/lib/gitlab/ci/templates/Getting-Started.gitlab-ci.yml
+
+variables:
+  GIT_SUBMODULE_STRATEGY: recursive
+default:
+  image: rikorose/gcc-cmake
+stages:
+- test
+unit-test-job:
+  stage: test
+  script:
+  - mkdir build
+  - cd build
+  - cmake ..
+  - cmake --build .
+  - export GTEST_OUTPUT="xml:report.xml"
+  - ls -la
+  - ls -ls test
+  - "./test/RoboidControlTest"
+  artifacts:
+    when: always
+    reports:
+      junit: build/report.xml
+sast:
+  stage: test
+include:
+- template: Security/SAST.gitlab-ci.yml

.gitmodules

@@ -0,0 +1,7 @@
+[submodule "LinearAlgebra"]
+	path = LinearAlgebra
+	url = ../linear-algebra.git
+	brnach = main
+[submodule "float16"]
+	path = float16
+	url = ../float16.git

AbsoluteEncoder.cpp

@@ -0,0 +1,2 @@
+#include "AbsoluteEncoder.h"
+

AbsoluteEncoder.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "ServoMotor.h"
+
+namespace Passer {
+namespace RoboidContol {
+
+class AbsoluteEncoder {
+ public:
+  AbsoluteEncoder() {}
+
+  virtual Angle16 GetActualAngle() = 0;
+  virtual float GetActualVelocity() = 0;
+};
+
+}  // namespace RoboidContol
+}  // namespace Passer

Accelerometer.cpp

@@ -0,0 +1,5 @@
+#include "Accelerometer.h"
+
+Accelerometer::Accelerometer() {}
+
+Spherical Accelerometer::GetVector() { return Spherical(); }

Accelerometer.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include "LinearAlgebra/Spherical.h"
+#include "Sensor.h"
+
+namespace Passer {
+namespace RoboidControl {
+
+class Accelerometer : public Sensor {
+public:
+  Accelerometer();
+
+  virtual Spherical GetVector();
+};
+
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;

Activation.cpp

@@ -0,0 +1,46 @@
+#include "Activation.h"
+
+float Activation::HeavisideStep(float inputValue, float bias) {
+  return (inputValue + bias > 0) ? 1.0F : 0.0F;
+}
+
+float Activation::Tanh(float inputValue) {
+    return (exp(inputValue) - exp(-inputValue)) / (exp(inputValue) + exp(-inputValue));
+}
+
+float Activation::Sigmoid(float inputValue) {
+    return 1 / (1 + expf(-inputValue));
+}
+
+float Activation::Linear(float inputValue, float minValue, float range) {
+  if (inputValue > minValue + range)
+    return 0;
+  if (inputValue < minValue)
+    return 1;
+
+  float f = (inputValue - minValue) * (1 / range);  // normalize to 1..0
+  float influence = 1 - f;             // invert
+  return influence;
+}
+
+
+float Activation::Quadratic(float inputValue, float minValue, float range) {
+  if (inputValue > minValue + range)
+    return 0;
+  if (inputValue < minValue)
+    return 1;
+
+  float f = (inputValue - minValue) * (1 / range);  // normalize to 1..0
+  float influence = 1 - (f * f);       // quadratic & invert
+  return influence;
+}
+
+float Activation::ParticleLife(float minValue, float maxValue, float attraction, float inputValue) {
+  if (inputValue < minValue)
+    return inputValue / minValue - 1;
+
+  if (inputValue < maxValue)
+    return attraction * (1 - fabs(2 * inputValue - minValue - maxValue) / (maxValue - minValue));
+
+  return 0;
+}

Activation.h

@@ -0,0 +1,32 @@
+#ifndef RC_ACTIVATION_H
+#define RC_ACTIVATION_H
+
+#include <math.h>
+
+namespace Passer {
+namespace RoboidControl {
+
+/// @brief Activation function for control
+/// @note This is mainly for future use :-)
+class Activation {
+public:
+  static float HeavisideStep(float inputValue, float bias = 0); // Range: {0,1}
+
+  static float Tanh(float inputValue); // Range: (-1, 1)
+
+  static float Sigmoid(float inputValue); // Range: (0, 1)
+
+  static float Linear(float inputValue, float bias = 0, float range = 0);
+
+  static float Quadratic(float inputValue, float bias = 0,
+                         float range = 0); // minValue = bias
+
+  static float ParticleLife(float minValue, float maxValue, float attraction,
+                            float inputValue); // minValue = bias
+};
+
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;
+
+#endif

Arduino/ArduinoParticipant.cpp

@@ -1,135 +0,0 @@
-#include "ArduinoParticipant.h"
-
-#if !defined(NO_STD)
-#include <iostream>
-#endif
-
-#if defined(ARDUINO)
-#if defined(ARDUINO_ARCH_ESP8266)
-#include <ESP8266WiFi.h>
-
-#elif defined(ESP32)
-#include <WiFi.h>
-
-#elif defined(UNO_R4)
-#include <WiFi.h>
-
-#elif defined(ARDUINO_ARCH_RP2040)  // not functional, for future use
-#include <WifiNINA.h>
-
-#endif
-#endif
-
-namespace RoboidControl {
-namespace Arduino {
-
-#if defined(ARDUINO) && defined(HAS_WIFI)
-WiFiUDP* udp;
-#endif
-
-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
-  if (WiFi.isConnected() == false) {
-    std::cout << "No network available!\n";
-    return;
-  }
-#endif
-
-  udp = new WiFiUDP();
-  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() {
-#if defined(ARDUINO) && defined(HAS_WIFI)
-  IPAddress broadcastAddress = WiFi.localIP();
-  broadcastAddress[3] = 255;
-  String broadcastIpString = broadcastAddress.toString();
-  this->broadcastIpAddress = new char[broadcastIpString.length() + 1];
-  broadcastIpString.toCharArray(this->broadcastIpAddress,
-                                broadcastIpString.length() + 1);
-#if !defined(NO_STD)
-  std::cout << "Broadcast address: " << broadcastIpAddress << "\n";
-#endif
-#endif
-}
-
-void ParticipantUDP::Receive() {
-#if defined(ARDUINO) && defined(HAS_WIFI)
-  int packetSize = udp->parsePacket();
-  while (packetSize > 0) {
-    udp->read(buffer, packetSize);
-
-    String senderAddress = udp->remoteIP().toString();
-    char sender_ipAddress[16];
-    senderAddress.toCharArray(sender_ipAddress, 16);
-    unsigned int sender_port = udp->remotePort();
-
-    ReceiveData(packetSize, sender_ipAddress, sender_port);
-    packetSize = udp->parsePacket();
-  }
-#endif
-}
-
-bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
-#if defined(ARDUINO) && defined(HAS_WIFI)
-  // 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->write((unsigned char*)buffer, bufferSize);
-    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) {
-#if defined(ARDUINO) && defined(HAS_WIFI)
-  int bufferSize = msg->Serialize((char*)this->buffer);
-  if (bufferSize <= 0)
-    return true;
-
-  udp->beginPacket(this->broadcastIpAddress, this->port);
-  udp->write((unsigned char*)buffer, bufferSize);
-  udp->endPacket();
-
-  //   std::cout << "Publish to " << this->broadcastIpAddress << ":"
-  //             << this->remotePort << "\n";
-#endif
-  return true;
-};
-
-}  // namespace Arduino
-}  // namespace RoboidControl

Arduino/ArduinoParticipant.h

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

Arduino/ArduinoUtils.cpp

@@ -1,218 +0,0 @@
-#include "ArduinoUtils.h"
-
-#if defined(ARDUINO)
-
-#include <Arduino.h>
-
-#if defined(ARDUINO_ARCH_ESP8266)
-#include <ESP8266WiFi.h>
-#include <ESP8266httpUpdate.h>
-#include <HTTPClient.h>
-
-#elif defined(ESP32)
-#include <HTTPClient.h>
-#include <HTTPUpdate.h>
-#include <WiFi.h>
-
-#elif defined(UNO_R4)
-#include <WiFi.h>
-
-#endif
-
-const char* hotspotSSID = "Roboid";
-const char* hotspotPassword = "alchemy7000";
-
-#if ESP32
-// Flash storage
-#include "Preferences.h"
-
-#define PREFERENCES_NAMESPACE "roboidControl"
-Preferences wifiPreferences;
-
-#define STORAGE_KEY_WIFI "rc/wifi"
-struct WifiCredentials {
-  char ssid[32] = "\0";
-  char password[32] = "\0";
-} credentials;
-
-#define STORAGE_KEY_NSS "rc/nss"
-struct NssServer {
-  char ipAddress[16] = "127.0.0.1\0";
-  unsigned short port = 7681;
-} nssServer;
-#endif
-
-bool StartWifi(const char* wifiSsid,
-               const char* wifiPassword,
-               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;
-  }
-#endif
-
-#if ESP32
-  printf("Connecting to WiFi %s\n", wifiSsid);
-#else
-  Serial.print("Connecting to WiFi ");
-  Serial.println(wifiSsid);
-#endif
-
-  // Connect to Wifi
-  WiFi.begin(wifiSsid, wifiPassword);
-  uint32_t notConnectedCounter = 0;
-  bool connected = false;
-  bool hotSpotEnabled = false;
-
-  while (WiFi.status() != WL_CONNECTED && !hotSpotEnabled) {
-#if ESP32
-    printf(".");
-#else
-    Serial.print(".");
-#endif
-    delay(500);
-
-    notConnectedCounter++;
-    if (notConnectedCounter > 20 && hotspotFallback) {
-#if ESP32
-      printf("\nCould not connect to home network.\n");
-#else
-      Serial.println();
-      Serial.println("Could not connect to home network");
-#endif
-      WiFi.disconnect();
-      if (hotspotFallback) {
-#if ESP32
-        WiFi.mode(WIFI_OFF);
-        WiFi.mode(WIFI_AP);
-        IPAddress wifiMyIp(192, 168, 4, 1);
-        WiFi.softAPConfig(wifiMyIp, wifiMyIp, IPAddress(255, 255, 255, 0));
-
-        WiFi.softAP(hotspotSSID, hotspotPassword);
-#elif UNO_R4 || ARDUINO_ARCH_RP2040
-        WiFi.beginAP(hotspotSSID);
-#endif
-        printf("Setup WiFi hotspot...\n");
-        // printf("ssid = %s, password = %s\n", hotspotSSID, hotspotPassword);
-#if ARDUINO_ARCH_RP2040
-        String ipAddress = WiFi.localIP().toString();
-#else
-        String ipAddress = WiFi.softAPIP().toString();
-#endif
-        char buf[20];
-        ipAddress.toCharArray(buf, 20);
-        printf("IP address: %s\n", buf);
-        hotSpotEnabled = true;
-      }
-    }
-  }
-  connected = notConnectedCounter <= 20;
-
-  if (connected) {
-    char buf[20];
-    String ipAddress = WiFi.localIP().toString();
-    ipAddress.toCharArray(buf, 20);
-#if ESP32 || ESP8266
-    printf("\nWifi connected, IP address: %s\n", buf);
-#else
-    Serial.println();
-    Serial.println("Wifi connected");
-#endif
-#if ESP32
-    printf("Checking credentials in flash\n");
-    wifiPreferences.begin(PREFERENCES_NAMESPACE);
-    wifiPreferences.getBytes(STORAGE_KEY_WIFI, &credentials,
-                             sizeof(credentials));
-    if (strcmp(wifiSsid, credentials.ssid) != 0 ||
-        strcmp(wifiPassword, credentials.password) != 0) {
-      printf("Updating credentials in flash...");
-      const int ssidLen = strlen(wifiSsid);
-      if (ssidLen < 32) {
-        memcpy(credentials.ssid, wifiSsid, ssidLen);
-        credentials.ssid[ssidLen] = '\0';
-      }
-
-      const int pwdLen = strlen(wifiPassword);
-      if (pwdLen < 32) {
-        memcpy(credentials.password, wifiPassword, pwdLen);
-        credentials.password[pwdLen] = '\0';
-      }
-      wifiPreferences.putBytes(STORAGE_KEY_WIFI, &credentials,
-                               sizeof(credentials));
-      printf(" completed.\n");
-    }
-    wifiPreferences.end();
-#endif
-  }
-
-  return (!hotSpotEnabled);
-#endif
-}
-
-void CheckFirmware(String url, String FIRMWARE_NAME, int FIRMWARE_VERSION) {
-#if !defined(HAS_WIFI)
-  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.");
-
-  WiFiClient client;
-  HTTPClient httpClient;
-  String versionURL = url + FIRMWARE_NAME + ".version";
-  httpClient.begin(client, versionURL);
-  int httpCode = httpClient.GET();
-  if (httpCode == 200) {
-    String newFWVersion = httpClient.getString();
-
-    Serial.print("Current firmware version: ");
-    Serial.println(FIRMWARE_VERSION);
-    Serial.print("Available firmware version: ");
-    Serial.println(newFWVersion);
-
-    int newVersion = newFWVersion.toInt();
-
-    if (newVersion > FIRMWARE_VERSION) {
-      Serial.println("Preparing to update firmware.");
-
-      String firmwareURL = url + FIRMWARE_NAME + ".bin";
-#if defined(ESP32)
-      t_httpUpdate_return ret = httpUpdate.update(client, firmwareURL);
-#else
-      t_httpUpdate_return ret = ESPhttpUpdate.update(client, firmwareURL);
-#endif
-      switch (ret) {
-        case HTTP_UPDATE_FAILED:
-#if defined(ESP32)
-          Serial.printf("HTTP_UPDATE_FAILED Error (%d): %s",
-                        httpUpdate.getLastError(),
-                        httpUpdate.getLastErrorString().c_str());
-#else
-          Serial.printf("HTTP_UPDATE_FAILED Error (%d): %s",
-                        ESPhttpUpdate.getLastError(),
-                        ESPhttpUpdate.getLastErrorString().c_str());
-#endif
-          break;
-        case HTTP_UPDATE_NO_UPDATES:
-          Serial.println("HTTP_UPDATE_NO_UPDATES");
-          break;
-        case HTTP_UPDATE_OK:
-          break;
-      }
-    } else {
-      Serial.println("No Firmware update necessary.");
-    }
-  } else {
-    Serial.print("Http Error: ");
-    Serial.println(httpCode);
-  }
-#endif
-#endif
-}
-#endif

Arduino/ArduinoUtils.h

@@ -1,10 +0,0 @@
-#pragma once
-#if defined(ARDUINO)
-
-#include <Arduino.h>
-bool StartWifi(const char *wifiSsid, const char *wifiPassword,
-               bool hotspotFallback = false);
-
-void CheckFirmware(String url, String FIRMWARE_NAME, int FIRMWARE_VERSION);
-
-#endif

Arduino/Things/DRV8833.cpp

@@ -1,135 +0,0 @@
-#include "DRV8833.h"
-
-#include <Arduino.h>
-
-namespace RoboidControl {
-namespace Arduino {
-
-#pragma region DRV8833
-
-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() {
-  this->SetParent(driver);
-
-  this->pinIn1 = pinIn1;
-  this->pinIn2 = pinIn2;
-
-#if (ESP32)
-  in1Ch = DRV8833Motor::nextAvailablePwmChannel++;
-  ledcSetup(in1Ch, 500, 8);
-  ledcAttachPin(pinIn1, in1Ch);
-
-  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;
-}
-
-// void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
-//   this->maxRpm = rpm;
-// }
-
-void DRV8833Motor::SetTargetVelocity(float motorSpeed) {
-  Motor::SetTargetVelocity(motorSpeed);
-
-  uint8_t motorSignal =
-      (uint8_t)(motorSpeed > 0 ? motorSpeed * 255 : -motorSpeed * 255);
-
-  // std::cout << "moto speed " << this->name << " = " << motorSpeed
-  //           << ", motor signal = " << (int)motorSignal << "\n";
-
-#if (ESP32)
-  if (motorSpeed == 0) {  // stop
-    ledcWrite(in1Ch, 0);
-    ledcWrite(in2Ch, 0);
-
-  } else if (motorSpeed > 0) {  // forward
-#if FAST_DECAY
-    ledcWrite(in1Ch, motorSignal);
-    ledcWrite(in2Ch, 0);
-#else
-    ledcWrite(in1Ch, 255);
-    ledcWrite(in2Ch, 255 - motorSignal);
-#endif
-  } else {  // (motorSpeed < 0) reverse
-#if FAST_DECAY
-    ledcWrite(in1Ch, 0);
-    ledcWrite(in2Ch, motorSignal);
-#else
-    ledcWrite(in1Ch, 255 - motorSignal);
-    ledcWrite(in2Ch, 255);
-#endif
-  }
-#else  // not ESP32
-  if (motorSpeed == 0) {  // stop
-    analogWrite(pinIn1, 0);
-    analogWrite(pinIn2, 0);
-
-  } else if (motorSpeed > 0) {  // forward
-#if FAST_DECAY
-    analogWrite(pinIn1, motorSignal);
-    analogWrite(pinIn2, 0);
-#else
-    analogWrite(pinIn1, 255);
-    analogWrite(pinIn2, 255 - motorSignal);
-#endif
-  } else {  // (motorSpeed < 0) reverse
-#if FAST_DECAY
-    analogWrite(pinIn1, 0);
-    analogWrite(pinIn2, motorSignal);
-#else
-    analogWrite(pinIn1, 255 - motorSignal);
-    analogWrite(pinIn2, 255);
-#endif
-  }
-#endif
-}
-
-#pragma endregion Motor
-
-}  // namespace Arduino
-}  // namespace RoboidControl

Arduino/Things/DRV8833.h

@@ -1,85 +0,0 @@
-#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;
-
-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());
-
-  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

@@ -1,129 +0,0 @@
-#include "DigitalInput.h"
-
-#include <Arduino.h>
-
-namespace RoboidControl {
-namespace Arduino {
-
-#pragma region Digital input
-
-DigitalInput::DigitalInput(unsigned char pin, Thing* parent)
-    : Thing(Type::Undetermined, parent) {
-  this->pin = pin;
-  pinMode(this->pin, INPUT);
-  std::cout << "digital input start\n";
-}
-
-void DigitalInput::Update(bool recursive) {
-  this->isHigh = digitalRead(this->pin);
-  this->isLow = !this->isHigh;
-  Thing::Update(recursive);
-}
-
-#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 +0,0 @@
-#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 {
- 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(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;
-
- 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,74 +0,0 @@
-#include "UltrasonicSensor.h"
-
-#include <Arduino.h>
-#include <iostream>
-
-namespace RoboidControl {
-namespace Arduino {
-
-UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
-    : Thing(Type::Undetermined, parent) {
-  this->name = "Ultrasonic sensor";
-  this->pinTrigger = config.trigger;
-  this->pinEcho = config.echo;
-
-  pinMode(pinTrigger, OUTPUT);  // configure the trigger pin to output mode
-  pinMode(pinEcho, INPUT);      // configure the echo pin to input mode
-}
-
-float UltrasonicSensor::GetDistance() {
-  // Start the ultrasonic 'ping'
-  digitalWrite(pinTrigger, LOW);
-  delayMicroseconds(2);
-  digitalWrite(pinTrigger, HIGH);
-  delayMicroseconds(10);
-  digitalWrite(pinTrigger, LOW);
-
-  // Measure the duration of the pulse on the echo pin
-  unsigned long duration_us =
-      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
-  // and back again. The distance to the object is therefore half the duration
-  // of the pulse: duration_us /= 2;
-  // * Then we need to convert from microseconds to seconds: duration_sec =
-  // duration_us / 1000000;
-  // * 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;
-
-  // 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;
-}
-
-void UltrasonicSensor::Update(bool recursive) {
-  GetDistance();
-  Thing::Update(recursive);
-}
-
-#pragma region Touch sensor
-
-UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent)
-    : RoboidControl::TouchSensor(parent), ultrasonic(config, this) {}
-
-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

@@ -1,64 +0,0 @@
-#pragma once
-
-#include "Things/TouchSensor.h"
-
-namespace RoboidControl {
-namespace Arduino {
-
-/// @brief An HC-SR04 ultrasonic distance sensor
-class UltrasonicSensor : Thing {
- public:
-  struct Configuration {
-    int trigger;
-    int echo;
-  };
-
-  UltrasonicSensor(Configuration config, Thing* parent = Thing::LocalRoot());
-
-  // parameters
-
-  /// @brief The distance at which the object is considered to be touched
-  // float touchDistance = 0.2f;
-
-  // state
-
-  /// @brief The last read distance
-  float distance = 0;
-  /// @brief erform an ultrasonic 'ping' to determine the distance to the
-  /// 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;
-
- 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,64 +1,65 @@
 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}
-        INCLUDE_DIRS "." "LinearAlgebra"
-        REQUIRES esp_netif esp_wifi
-    )
-else() 
-    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(
-        googletest
-        DOWNLOAD_EXTRACT_TIMESTAMP ON
-        URL https://github.com/google/googletest/archive/refs/heads/main.zip
-    )
-
-    # For Windows: Prevent overriding the parent project's compiler/linker settings
-    set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
-    FetchContent_MakeAvailable(googletest)
-
-    include_directories(
+    set(sourcedirs
         .
         LinearAlgebra
+        ControlCore
     )
-    add_library(RoboidControl STATIC ${srcs})
-    
-    enable_testing()
 
-    file(GLOB_RECURSE test_srcs test/*_test.cc)
-    add_executable(
-        RoboidControlTest
-        ${test_srcs}
-    )
-    target_link_libraries(
-        RoboidControlTest
-        gtest_main
-        RoboidControl
+    set(includedirs
+        .
         LinearAlgebra
+        ControlCore
     )
 
-    include(GoogleTest)
-    gtest_discover_tests(RoboidControlTest)
+    idf_component_register(
+        SRC_DIRS ${sourcedirs}
+        INCLUDE_DIRS ${includedirs}
+        REQUIRES arduino
+    )
 endif()
 
+project(RoboidControl)
+add_subdirectory(ControlCore)
+#add_subdirectory(LinearAlgebra)
+add_subdirectory(test)
+
+set(CMAKE_CXX_STANDARD 11)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+add_compile_definitions(GTEST)
+include(FetchContent)
+FetchContent_Declare(
+    googletest
+    DOWNLOAD_EXTRACT_TIMESTAMP ON
+    URL https://github.com/google/googletest/archive/refs/heads/main.zip
+)
+
+# For Windows: Prevent overriding the parent project's compiler/linker settings
+set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+FetchContent_MakeAvailable(googletest)
+
+include_directories(
+    .
+    ControlCore
+    LinearAlgebra
+)
+
+add_library(RoboidControl STATIC 
+    "Roboid.cpp"
+    "Perception.cpp"
+    "TrackedObject.cpp"
+    "Propulsion.cpp"
+    "Motor.cpp"
+    "DifferentialDrive.cpp"
+    "DistanceSensor.cpp"
+    "Sensor.cpp"
+    "Switch.cpp"
+   
+    "Quadcopter.cpp"
+    #"Messages.cpp"
+)
+
+enable_testing()
+
+include(GoogleTest)

ControlCore/.gitignore

@@ -0,0 +1,2 @@
+build
+.vscode

ControlCore/CMakeLists.txt

@@ -0,0 +1,61 @@
+cmake_minimum_required(VERSION 3.13)  # CMake version check
+if(ESP_PLATFORM)
+    idf_component_register(
+        SRC_DIRS "."
+        INCLUDE_DIRS "."
+    )
+else() 
+    project(ControlCore)
+    add_subdirectory(LinearAlgebra)
+
+    set(CMAKE_CXX_STANDARD 11)            # Enable c++11 standard
+    set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+    add_compile_definitions(GTEST)
+    include(FetchContent)
+    FetchContent_Declare(
+        googletest
+        DOWNLOAD_EXTRACT_TIMESTAMP ON
+        URL https://github.com/google/googletest/archive/refs/heads/main.zip
+    )
+
+    # For Windows: Prevent overriding the parent project's compiler/linker settings
+    set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+    FetchContent_MakeAvailable(googletest)
+
+    include_directories(
+        .
+        LinearAlgebra
+    )
+    add_library(ControlCore STATIC 
+        "Thing.cpp"
+        "LowLevelMessages.cpp"
+        "Messages.cpp" 
+        "Participant.cpp" 
+        "float16.cpp"
+    )
+
+    enable_testing()
+
+    file(GLOB_RECURSE test_srcs test/*_test.cc)
+    add_executable(
+        ControlCoreTest
+        ${test_srcs}
+    )
+    target_link_libraries(
+        ControlCoreTest
+        gtest_main
+        ControlCore
+    )
+
+    if(MSVC)
+    target_compile_options(ControlCoreTest PRIVATE /W4 /WX)
+  else()
+    target_compile_options(ControlCoreTest PRIVATE -Wall -Wextra -Wpedantic -Werror)
+  endif()
+  
+
+    include(GoogleTest)
+    gtest_discover_tests(ControlCoreTest)
+endif()
+

ControlCore/LinearAlgebra/Angle.cpp

@@ -3,133 +3,28 @@
 // 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 {
-
-//===== AngleSingle, AngleOf<float>
-
-template <>
-AngleOf<float> AngleOf<float>::Degrees(float degrees) {
-  if (isfinite(degrees)) {
-    while (degrees < -180)
-      degrees += 360;
-    while (degrees >= 180)
-      degrees -= 360;
-  }
-
-  return AngleOf<float>(degrees);
-}
-
-template <>
-AngleOf<float> AngleOf<float>::Radians(float radians) {
-  if (isfinite(radians)) {
-    while (radians <= -pi)
-      radians += 2 * pi;
-    while (radians > pi)
-      radians -= 2 * pi;
-  }
-
-  return Binary(radians * Rad2Deg);
-}
-
-template <>
-float AngleOf<float>::InDegrees() const {
-  return this->value;
-}
-
-template <>
-float AngleOf<float>::InRadians() const {
-  return this->value * Deg2Rad;
-}
-
-//===== Angle16, AngleOf<signed short>
-
-template <>
-AngleOf<signed short> AngleOf<signed short>::Degrees(float degrees) {
-  // map float [-180..180) to integer [-32768..32767]
-  signed short value = (signed short)roundf(degrees / 360.0F * 65536.0F);
-  return Binary(value);
-}
-
-template <>
-AngleOf<signed short> AngleOf<signed short>::Radians(float radians) {
-  if (!isfinite(radians))
-    return AngleOf<signed short>::zero;
-
-  // map float [-PI..PI) to integer [-32768..32767]
-  signed short value = (signed short)roundf(radians / pi * 32768.0F);
-  return Binary(value);
-}
-
-template <>
-float AngleOf<signed short>::InDegrees() const {
-  float degrees = this->value / 65536.0f * 360.0f;
-  return degrees;
-}
-
-template <>
-float AngleOf<signed short>::InRadians() const {
-  float radians = this->value / 65536.0f * (2 * pi);
-  return radians;
-}
-
-//===== Angle8, AngleOf<signed char>
-
-template <>
-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 <>
-AngleOf<signed char> AngleOf<signed char>::Radians(float radians) {
-  if (!isfinite(radians))
-    return AngleOf<signed char>::zero;
-
-  // map float [-pi..pi) to integer [-128..127)
-  signed char value = (signed char)roundf(radians / pi * 128.0f);
-  return Binary(value);
-}
-
-template <>
-float AngleOf<signed char>::InDegrees() const {
-  float degrees = this->value / 256.0f * 360.0f;
-  return degrees;
-}
-
-template <>
-float AngleOf<signed char>::InRadians() const {
-  float radians = this->value / 128.0f * pi;
-  return radians;
-}
+const float Rad2Deg = 57.29578F;
+const float Deg2Rad = 0.0174532924F;
 
 //===== Generic
 
-template <typename T>
-AngleOf<T>::AngleOf() : value(0) {}
+template <typename T> AngleOf<T>::AngleOf() : value(0) {}
 
-template <typename T>
-AngleOf<T>::AngleOf(T rawValue) : value(rawValue) {}
+template <typename T> AngleOf<T>::AngleOf(T rawValue) : value(rawValue) {}
 
-template <typename T>
-const AngleOf<T> AngleOf<T>::zero = AngleOf<T>();
+template <typename T> const AngleOf<T> AngleOf<T>::zero = AngleOf<T>();
 
-template <typename T>
-AngleOf<T> AngleOf<T>::Binary(T rawValue) {
+template <typename T> AngleOf<T> AngleOf<T>::Binary(T rawValue) {
   AngleOf<T> angle = AngleOf<T>();
   angle.SetBinary(rawValue);
   return angle;
 }
 
-template <typename T>
-T AngleOf<T>::GetBinary() const {
-  return this->value;
-}
-template <typename T>
-void AngleOf<T>::SetBinary(T rawValue) {
+template <typename T> T AngleOf<T>::GetBinary() const { return this->value; }
+template <typename T> void AngleOf<T>::SetBinary(T rawValue) {
   this->value = rawValue;
 }
 
@@ -138,28 +33,24 @@ bool AngleOf<T>::operator==(const AngleOf<T> angle) const {
   return this->value == angle.value;
 }
 
-template <typename T>
-bool AngleOf<T>::operator>(AngleOf<T> angle) const {
+template <typename T> bool AngleOf<T>::operator>(AngleOf<T> angle) const {
   return this->value > angle.value;
 }
 
-template <typename T>
-bool AngleOf<T>::operator>=(AngleOf<T> angle) const {
+template <typename T> bool AngleOf<T>::operator>=(AngleOf<T> angle) const {
   return this->value >= angle.value;
 }
 
-template <typename T>
-bool AngleOf<T>::operator<(AngleOf<T> angle) const {
+template <typename T> bool AngleOf<T>::operator<(AngleOf<T> angle) const {
   return this->value < angle.value;
 }
 
-template <typename T>
-bool AngleOf<T>::operator<=(AngleOf<T> angle) const {
+template <typename T> 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 +59,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>
-AngleOf<T> AngleOf<T>::operator-() const {
+template <typename T> 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 +120,7 @@ AngleOf<T> AngleOf<T>::operator+=(const AngleOf<T>& angle) {
 //   return AngleOf::Degrees((float)factor * angle.InDegrees());
 // }
 
-template <typename T>
-void AngleOf<T>::Normalize() {
+template <typename T> void AngleOf<T>::Normalize() {
   float angleValue = this->InDegrees();
   if (!isfinite(angleValue))
     return;
@@ -243,8 +132,7 @@ void AngleOf<T>::Normalize() {
   *this = AngleOf::Degrees(angleValue);
 }
 
-template <typename T>
-AngleOf<T> AngleOf<T>::Normalize(AngleOf<T> angle) {
+template <typename T> AngleOf<T> AngleOf<T>::Normalize(AngleOf<T> angle) {
   float angleValue = angle.InDegrees();
   if (!isfinite(angleValue))
     return angle;
@@ -263,10 +151,9 @@ 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> toAngle,
+AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle, AngleOf<T> toAngle,
                                    float maxDegrees) {
-  maxDegrees = fmaxf(0, maxDegrees);  // filter out negative distances
+  maxDegrees = fmaxf(0, maxDegrees); // filter out negative distances
   AngleOf<T> d = toAngle - fromAngle;
   float dDegrees = Abs(d).InDegrees();
   d = AngleOf<T>::Degrees(Float::Clamp(dDegrees, 0, maxDegrees));
@@ -276,34 +163,28 @@ AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle,
   return fromAngle + d;
 }
 
-template <typename T>
-float AngleOf<T>::Cos(AngleOf<T> angle) {
+template <typename T> float AngleOf<T>::Cos(AngleOf<T> angle) {
   return cosf(angle.InRadians());
 }
-template <typename T>
-float AngleOf<T>::Sin(AngleOf<T> angle) {
+template <typename T> float AngleOf<T>::Sin(AngleOf<T> angle) {
   return sinf(angle.InRadians());
 }
-template <typename T>
-float AngleOf<T>::Tan(AngleOf<T> angle) {
+template <typename T> float AngleOf<T>::Tan(AngleOf<T> angle) {
   return tanf(angle.InRadians());
 }
 
-template <typename T>
-AngleOf<T> AngleOf<T>::Acos(float f) {
+template <typename T> AngleOf<T> AngleOf<T>::Acos(float f) {
   return AngleOf<T>::Radians(acosf(f));
 }
-template <typename T>
-AngleOf<T> AngleOf<T>::Asin(float f) {
+template <typename T> AngleOf<T> AngleOf<T>::Asin(float f) {
   return AngleOf<T>::Radians(asinf(f));
 }
-template <typename T>
-AngleOf<T> AngleOf<T>::Atan(float f) {
+template <typename T> 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));
 }
 
@@ -330,7 +211,7 @@ float AngleOf<T>::CosineRuleSide(float a, float b, AngleOf<T> gamma) {
   float b2 = b * b;
   float d =
       a2 + b2 -
-      2 * a * b * Cos(gamma);  // cosf(gamma * Passer::LinearAlgebra::Deg2Rad);
+      2 * a * b * Cos(gamma); // cosf(gamma * Passer::LinearAlgebra::Deg2Rad);
   // Catch edge cases where float inacuracies lead tot nans
   if (d < 0)
     return 0;
@@ -391,4 +272,88 @@ template class AngleOf<float>;
 template class AngleOf<signed char>;
 template class AngleOf<signed short>;
 
-}  // namespace LinearAlgebra
+//===== AngleSingle, AngleOf<float>
+
+template <> AngleOf<float> AngleOf<float>::Degrees(float degrees) {
+  if (isfinite(degrees)) {
+    while (degrees < -180)
+      degrees += 360;
+    while (degrees >= 180)
+      degrees -= 360;
+  }
+
+  return AngleOf<float>(degrees);
+}
+
+template <> AngleOf<float> AngleOf<float>::Radians(float radians) {
+  if (isfinite(radians)) {
+    while (radians <= -pi)
+      radians += 2 * pi;
+    while (radians > pi)
+      radians -= 2 * pi;
+  }
+
+  return Binary(radians * Rad2Deg);
+}
+
+template <> float AngleOf<float>::InDegrees() const { return this->value; }
+
+template <> float AngleOf<float>::InRadians() const {
+  return this->value * Deg2Rad;
+}
+
+//===== Angle16, AngleOf<signed short>
+
+template <>
+AngleOf<signed short> AngleOf<signed short>::Degrees(float degrees) {
+  // map float [-180..180) to integer [-32768..32767]
+  signed short value = (signed short)roundf(degrees / 360.0F * 65536.0F);
+  return Binary(value);
+}
+
+template <>
+AngleOf<signed short> AngleOf<signed short>::Radians(float radians) {
+  if (!isfinite(radians))
+    return AngleOf<signed short>::zero;
+
+  // map float [-PI..PI) to integer [-32768..32767]
+  signed short value = (signed short)roundf(radians / pi * 32768.0F);
+  return Binary(value);
+}
+
+template <> float AngleOf<signed short>::InDegrees() const {
+  float degrees = this->value / 65536.0f * 360.0f;
+  return degrees;
+}
+
+template <> float AngleOf<signed short>::InRadians() const {
+  float radians = this->value / 65536.0f * (2 * pi);
+  return radians;
+}
+
+//===== Angle8, AngleOf<signed char>
+
+template <> 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 <> AngleOf<signed char> AngleOf<signed char>::Radians(float radians) {
+  if (!isfinite(radians))
+    return AngleOf<signed char>::zero;
+
+  // map float [-pi..pi) to integer [-128..127)
+  signed char value = (signed char)roundf(radians / pi * 128.0f);
+  return Binary(value);
+}
+
+template <> float AngleOf<signed char>::InDegrees() const {
+  float degrees = this->value / 256.0f * 360.0f;
+  return degrees;
+}
+
+template <> float AngleOf<signed char>::InRadians() const {
+  float radians = this->value / 128.0f * pi;
+  return radians;
+}

ControlCore/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>
-class AngleOf {
- public:
+template <typename T> class AngleOf {
+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,
-                                AngleOf<T> toAngle,
+  static AngleOf<T> MoveTowards(AngleOf<T> fromAngle, 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 LinearAlgebra
+} // namespace Passer
+using namespace Passer::LinearAlgebra;
 
 #endif

ControlCore/LinearAlgebra/AngleAxis.cpp

@@ -0,0 +1,53 @@
+/*
+// This Source Code Form is subject to the terms of the Mozilla Public
+// License, v. 2.0.If a copy of the MPL was not distributed with this
+// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
+
+#include "AngleAxis.h"
+
+template <typename T>
+AngleAxisOf<T>::AngleAxisOf() {
+  this->angle = 0.0F;
+  this->axis = DirectionOf<T>();
+}
+
+template <typename T>
+AngleAxisOf<T>::AngleAxisOf(float angle, DirectionOf<T> axis) {
+  this->angle = angle;
+  this->axis = axis;
+}
+
+template <typename T>
+AngleAxisOf<T>::AngleAxisOf(float angle, Vector3 axis) {
+  this->angle = angle;
+  this->axis = DirectionOf<T>::FromVector3(axis);
+}
+
+template <typename T>
+AngleAxisOf<T>::AngleAxisOf(Quaternion q) {
+  float angle;
+  Vector3 axis;
+  q.ToAngleAxis(&angle, &axis);
+  this->angle = angle;
+  this->axis = DirectionOf<T>::FromVector3(axis);
+}
+
+template <typename T>
+const AngleAxisOf<T> AngleAxisOf<T>::zero =
+    AngleAxisOf<T>(0.0, DirectionOf<T>(AngleOf<T>(), AngleOf<T>()));
+
+template <typename T>
+Quaternion AngleAxisOf<T>::ToQuaternion() {
+  Vector3 axisVector = this->axis.ToVector3();
+  Quaternion q = Quaternion::AngleAxis(this->angle, axisVector);
+  return q;
+}
+
+template <typename T>
+DirectionOf<T> AngleAxisOf<T>::GetSwing() {
+  return this->axis;
+}
+
+template class AngleAxisOf<float>;
+template class AngleAxisOf<signed short>;
+*/

ControlCore/LinearAlgebra/AngleUsing.h

@@ -0,0 +1,52 @@
+/*
+#ifndef DISCRETEANGLE_H
+#define DISCRETEANGLE_H
+
+#include "Angle.h"
+#include "Range.h"
+
+namespace Passer {
+namespace LinearAlgebra {
+
+// A fixed angle between (-180..180]
+
+template <typename T>
+class AngleUsing {
+ public:
+  AngleUsing(T sourceValue) { this->value = sourceValue; }
+  AngleUsing(float f);
+  float ToFloat() const;
+  inline T GetValue() const { return this->value; }
+
+  AngleUsing<T> operator+(const AngleUsing<T> a) {
+    AngleUsing<T> r = AngleUsing((float)this->value + a.value);
+    return r;
+  }
+
+  inline AngleUsing<T> operator+=(const AngleUsing<T> a) {
+    return this->value + a.value;
+  }
+
+  inline AngleUsing<T> operator-(const AngleUsing<T> a) {
+    return this->value - a.value;
+  }
+
+  inline AngleUsing<T> operator-() {
+    this->value = -this->value;
+    return *this;
+  }
+
+  inline bool operator==(const AngleUsing<T> a) {
+    return this->value == a.value;
+  }
+
+  // protected:
+  T value;
+};
+
+}  // namespace LinearAlgebra
+}  // namespace Passer
+using namespace Passer::LinearAlgebra;
+
+#endif
+*/

ControlCore/LinearAlgebra/CMakeLists.txt

@@ -7,14 +7,14 @@ if(ESP_PLATFORM)
 else() 
     project(LinearAlgebra)
 
-    set(CMAKE_CXX_STANDARD 17)            # Enable c++11 standard
+    set(CMAKE_CXX_STANDARD 11)            # Enable c++11 standard
     set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
     add_compile_definitions(GTEST)
     include(FetchContent)
     FetchContent_Declare(
         googletest
-        DOWNLOAD_EXTRACT_TIMESTAMP ON
+        DOWNLOAD_EXTRACT_TIMESTAMP 
         URL https://github.com/google/googletest/archive/refs/heads/main.zip
     )
 
@@ -23,20 +23,19 @@ else()
     FetchContent_MakeAvailable(googletest)
 
     include_directories(.)
-    file(GLOB srcs
-        *.cpp
-    )
-    add_library(LinearAlgebra STATIC ${srcs}
-        # "FloatSingle.cpp"
-        # "Angle.cpp"
-        # "Vector2.cpp" 
-        # "Vector3.cpp" 
-        # "Quaternion.cpp"
-        # "Polar.cpp" 
-        # "Spherical.cpp"
-        # "Matrix.cpp"
-        # "SwingTwist.cpp"
-        # "Direction.cpp"
+    add_library(LinearAlgebra STATIC 
+        "FloatSingle.cpp"
+        "Angle.cpp"
+        "Vector2.cpp" 
+        "Vector3.cpp" 
+        "Quaternion.cpp"
+        "Polar.cpp" 
+        "Spherical.cpp"
+        "Matrix.cpp"
+        # "Axis.cpp"
+        # "AngleAxis.cpp"
+        "SwingTwist.cpp"
+        "Direction.cpp"
     )
 
     enable_testing()
@@ -54,9 +53,9 @@ else()
 
     if(MSVC)
     target_compile_options(LinearAlgebraTest PRIVATE /W4 /WX)
-#   else()
-#     target_compile_options(LinearAlgebraTest PRIVATE -Wall -Wextra -Wpedantic -Werror)
-   endif()
+  else()
+    target_compile_options(LinearAlgebraTest PRIVATE -Wall -Wextra -Wpedantic -Werror)
+  endif()
   
 
     include(GoogleTest)

ControlCore/LinearAlgebra/Direction.cpp

@@ -9,7 +9,6 @@
 
 #include <math.h>
 
-namespace LinearAlgebra {
 template <typename T>
 DirectionOf<T>::DirectionOf() {
   this->horizontal = AngleOf<T>();
@@ -43,7 +42,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,45 +50,54 @@ Vector3 DirectionOf<T>::ToVector3() const {
 }
 
 template <typename T>
-DirectionOf<T> DirectionOf<T>::FromVector3(Vector3 vector) {
+DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::FromVector3(Vector3 v) {
   DirectionOf<T> d;
   d.horizontal = AngleOf<T>::Atan2(
-      vector.Right(),
-      vector
-          .Forward());  // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
+      v.Right(),
+      v.Forward());  // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
   d.vertical =
       AngleOf<T>::Degrees(-90) -
       AngleOf<T>::Acos(
-          vector.Up());  // AngleOf<T>::Radians(-(0.5f * pi) - acosf(v.Up()));
+          v.Up());  // AngleOf<T>::Radians(-(0.5f * pi) - acosf(v.Up()));
   d.Normalize();
   return d;
 }
 
 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 {
-  return (this->horizontal == direction.horizontal) &&
-         (this->vertical == direction.vertical);
+bool Passer::LinearAlgebra::DirectionOf<T>::operator==(
+    const DirectionOf<T> d) const {
+  return (this->horizontal == d.horizontal) && (this->vertical == d.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>
+Vector3 DirectionOf<T>::ToVector3() {
+  Vector3 v = Quaternion::Euler(-this->vertical.InDegrees(),
+                                this->horizontal.InDegrees(), 0) *
+              Vector3::forward;
+  return v;
+}
+
 template <typename T>
 void DirectionOf<T>::Normalize() {
   if (this->vertical > AngleOf<T>::Degrees(90) ||
@@ -99,6 +107,5 @@ void DirectionOf<T>::Normalize() {
   }
 }
 
-template class LinearAlgebra::DirectionOf<float>;
-template class LinearAlgebra::DirectionOf<signed short>;
-}
+template class DirectionOf<float>;
+template class DirectionOf<signed short>;

ControlCore/LinearAlgebra/Direction.h

@@ -0,0 +1,57 @@
+// This Source Code Form is subject to the terms of the Mozilla Public
+// License, v. 2.0.If a copy of the MPL was not distributed with this
+// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
+
+#ifndef DIRECTION_H
+#define DIRECTION_H
+
+#include "Angle.h"
+
+namespace Passer {
+namespace LinearAlgebra {
+
+struct Vector3;
+
+template <typename T>
+class DirectionOf {
+ public:
+  /// @brief horizontal angle, range= (-180..180]
+  AngleOf<T> horizontal;
+  /// @brief vertical angle, range in degrees = (-90..90]
+  AngleOf<T> vertical;
+
+  DirectionOf<T>();
+  DirectionOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical);
+
+  Vector3 ToVector3() const;
+  static DirectionOf<T> FromVector3(Vector3 v);
+
+  static DirectionOf<T> Degrees(float horizontal, float vertical);
+  static DirectionOf<T> Radians(float horizontal, float vertical);
+
+  const static DirectionOf forward;
+  const static DirectionOf back;
+  const static DirectionOf up;
+  const static DirectionOf down;
+  const static DirectionOf left;
+  const static DirectionOf right;
+
+  bool operator==(const DirectionOf<T> d) const;
+
+  DirectionOf<T> operator-() const;
+
+  Vector3 ToVector3();
+
+ protected:
+  void Normalize();
+};
+
+using DirectionSingle = DirectionOf<float>;
+using Direction16 = DirectionOf<signed short>;
+using Direction = DirectionOf<float>;
+
+}  // namespace LinearAlgebra
+}  // namespace Passer
+using namespace Passer::LinearAlgebra;
+
+#endif

ControlCore/LinearAlgebra/DoxyGen/DoxyWarnLogfile.txt

@@ -2,39 +2,39 @@ warning: source 'images' is not a readable file or directory... skipping.
 d:/PlatformIO/linear-algebra/Quaternion.cpp:100: warning: no uniquely matching class member found for 
   Quaternion Quaternion::operator*(const Quaternion &r2) const
 Possible candidates:
-  'friend AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator*(const AngleOf< T > &angle, float factor)' at line 117 of file d:/PlatformIO/linear-algebra/Angle.h
-  'friend AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator*(float factor, const AngleOf< T > &angle)' at line 124 of file d:/PlatformIO/linear-algebra/Angle.h
+  'friend AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator*(const AngleOf< T > &angle, float factor)' at line 120 of file d:/PlatformIO/linear-algebra/Angle.h
+  'friend AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator*(float factor, const AngleOf< T > &angle)' at line 127 of file d:/PlatformIO/linear-algebra/Angle.h
   'Vector3 Passer::LinearAlgebra::MatrixOf< T >::operator*(const Vector3 v) const' at line 64 of file d:/PlatformIO/linear-algebra/Matrix.h
-  'friend PolarOf Passer::LinearAlgebra::PolarOf< T >::operator*(const PolarOf &v, float f)' at line 118 of file d:/PlatformIO/linear-algebra/Polar.h
-  'friend PolarOf Passer::LinearAlgebra::PolarOf< T >::operator*(float f, const PolarOf &v)' at line 121 of file d:/PlatformIO/linear-algebra/Polar.h
+  'friend PolarOf Passer::LinearAlgebra::PolarOf< T >::operator*(const PolarOf &v, float f)' at line 100 of file d:/PlatformIO/linear-algebra/Polar.h
+  'friend PolarOf Passer::LinearAlgebra::PolarOf< T >::operator*(float f, const PolarOf &v)' at line 103 of file d:/PlatformIO/linear-algebra/Polar.h
   'Vector3 Passer::LinearAlgebra::Quaternion::operator*(const Vector3 &vector) const' at line 98 of file d:/PlatformIO/linear-algebra/Quaternion.h
   'Quaternion Passer::LinearAlgebra::Quaternion::operator*(const Quaternion &rotation) const' at line 106 of file d:/PlatformIO/linear-algebra/Quaternion.h
-  'friend SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator*(const SphericalOf< T > &v, float f)' at line 111 of file d:/PlatformIO/linear-algebra/Spherical.h
-  'friend SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator*(float f, const SphericalOf< T > &v)' at line 114 of file d:/PlatformIO/linear-algebra/Spherical.h
-  'SphericalOf< T > Passer::LinearAlgebra::SwingTwistOf< T >::operator*(const SphericalOf< T > &vector) const' at line 46 of file d:/PlatformIO/linear-algebra/SwingTwist.h
-  'SwingTwistOf< T > Passer::LinearAlgebra::SwingTwistOf< T >::operator*(const SwingTwistOf< T > &rotation) const' at line 54 of file d:/PlatformIO/linear-algebra/SwingTwist.h
-  'friend Vector2 Passer::LinearAlgebra::Vector2::operator*(const Vector2 &v, float f)' at line 141 of file d:/PlatformIO/linear-algebra/Vector2.h
-  'friend Vector2 Passer::LinearAlgebra::Vector2::operator*(float f, const Vector2 &v)' at line 144 of file d:/PlatformIO/linear-algebra/Vector2.h
-  'friend Vector3 Passer::LinearAlgebra::Vector3::operator*(const Vector3 &v, float f)' at line 149 of file d:/PlatformIO/linear-algebra/Vector3.h
-  'friend Vector3 Passer::LinearAlgebra::Vector3::operator*(float f, const Vector3 &v)' at line 152 of file d:/PlatformIO/linear-algebra/Vector3.h
+  'friend SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator*(const SphericalOf< T > &v, float f)' at line 109 of file d:/PlatformIO/linear-algebra/Spherical.h
+  'friend SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator*(float f, const SphericalOf< T > &v)' at line 112 of file d:/PlatformIO/linear-algebra/Spherical.h
+  'SphericalOf< T > Passer::LinearAlgebra::SwingTwistOf< T >::operator*(const SphericalOf< T > &vector) const' at line 45 of file d:/PlatformIO/linear-algebra/SwingTwist.h
+  'SwingTwistOf< T > Passer::LinearAlgebra::SwingTwistOf< T >::operator*(const SwingTwistOf< T > &rotation) const' at line 53 of file d:/PlatformIO/linear-algebra/SwingTwist.h
+  'friend Vector2 Passer::LinearAlgebra::Vector2::operator*(const Vector2 &v, float f)' at line 143 of file d:/PlatformIO/linear-algebra/Vector2.h
+  'friend Vector2 Passer::LinearAlgebra::Vector2::operator*(float f, const Vector2 &v)' at line 146 of file d:/PlatformIO/linear-algebra/Vector2.h
+  'friend Vector3 Passer::LinearAlgebra::Vector3::operator*(const Vector3 &v, float f)' at line 151 of file d:/PlatformIO/linear-algebra/Vector3.h
+  'friend Vector3 Passer::LinearAlgebra::Vector3::operator*(float f, const Vector3 &v)' at line 154 of file d:/PlatformIO/linear-algebra/Vector3.h
 d:/PlatformIO/linear-algebra/Quaternion.cpp:108: warning: no uniquely matching class member found for 
   Vector3 Quaternion::operator*(const Vector3 &p) const
 Possible candidates:
-  'friend AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator*(const AngleOf< T > &angle, float factor)' at line 117 of file d:/PlatformIO/linear-algebra/Angle.h
-  'friend AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator*(float factor, const AngleOf< T > &angle)' at line 124 of file d:/PlatformIO/linear-algebra/Angle.h
+  'friend AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator*(const AngleOf< T > &angle, float factor)' at line 120 of file d:/PlatformIO/linear-algebra/Angle.h
+  'friend AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator*(float factor, const AngleOf< T > &angle)' at line 127 of file d:/PlatformIO/linear-algebra/Angle.h
   'Vector3 Passer::LinearAlgebra::MatrixOf< T >::operator*(const Vector3 v) const' at line 64 of file d:/PlatformIO/linear-algebra/Matrix.h
-  'friend PolarOf Passer::LinearAlgebra::PolarOf< T >::operator*(const PolarOf &v, float f)' at line 118 of file d:/PlatformIO/linear-algebra/Polar.h
-  'friend PolarOf Passer::LinearAlgebra::PolarOf< T >::operator*(float f, const PolarOf &v)' at line 121 of file d:/PlatformIO/linear-algebra/Polar.h
+  'friend PolarOf Passer::LinearAlgebra::PolarOf< T >::operator*(const PolarOf &v, float f)' at line 100 of file d:/PlatformIO/linear-algebra/Polar.h
+  'friend PolarOf Passer::LinearAlgebra::PolarOf< T >::operator*(float f, const PolarOf &v)' at line 103 of file d:/PlatformIO/linear-algebra/Polar.h
   'Vector3 Passer::LinearAlgebra::Quaternion::operator*(const Vector3 &vector) const' at line 98 of file d:/PlatformIO/linear-algebra/Quaternion.h
   'Quaternion Passer::LinearAlgebra::Quaternion::operator*(const Quaternion &rotation) const' at line 106 of file d:/PlatformIO/linear-algebra/Quaternion.h
-  'friend SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator*(const SphericalOf< T > &v, float f)' at line 111 of file d:/PlatformIO/linear-algebra/Spherical.h
-  'friend SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator*(float f, const SphericalOf< T > &v)' at line 114 of file d:/PlatformIO/linear-algebra/Spherical.h
-  'SphericalOf< T > Passer::LinearAlgebra::SwingTwistOf< T >::operator*(const SphericalOf< T > &vector) const' at line 46 of file d:/PlatformIO/linear-algebra/SwingTwist.h
-  'SwingTwistOf< T > Passer::LinearAlgebra::SwingTwistOf< T >::operator*(const SwingTwistOf< T > &rotation) const' at line 54 of file d:/PlatformIO/linear-algebra/SwingTwist.h
-  'friend Vector2 Passer::LinearAlgebra::Vector2::operator*(const Vector2 &v, float f)' at line 141 of file d:/PlatformIO/linear-algebra/Vector2.h
-  'friend Vector2 Passer::LinearAlgebra::Vector2::operator*(float f, const Vector2 &v)' at line 144 of file d:/PlatformIO/linear-algebra/Vector2.h
-  'friend Vector3 Passer::LinearAlgebra::Vector3::operator*(const Vector3 &v, float f)' at line 149 of file d:/PlatformIO/linear-algebra/Vector3.h
-  'friend Vector3 Passer::LinearAlgebra::Vector3::operator*(float f, const Vector3 &v)' at line 152 of file d:/PlatformIO/linear-algebra/Vector3.h
+  'friend SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator*(const SphericalOf< T > &v, float f)' at line 109 of file d:/PlatformIO/linear-algebra/Spherical.h
+  'friend SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator*(float f, const SphericalOf< T > &v)' at line 112 of file d:/PlatformIO/linear-algebra/Spherical.h
+  'SphericalOf< T > Passer::LinearAlgebra::SwingTwistOf< T >::operator*(const SphericalOf< T > &vector) const' at line 45 of file d:/PlatformIO/linear-algebra/SwingTwist.h
+  'SwingTwistOf< T > Passer::LinearAlgebra::SwingTwistOf< T >::operator*(const SwingTwistOf< T > &rotation) const' at line 53 of file d:/PlatformIO/linear-algebra/SwingTwist.h
+  'friend Vector2 Passer::LinearAlgebra::Vector2::operator*(const Vector2 &v, float f)' at line 143 of file d:/PlatformIO/linear-algebra/Vector2.h
+  'friend Vector2 Passer::LinearAlgebra::Vector2::operator*(float f, const Vector2 &v)' at line 146 of file d:/PlatformIO/linear-algebra/Vector2.h
+  'friend Vector3 Passer::LinearAlgebra::Vector3::operator*(const Vector3 &v, float f)' at line 151 of file d:/PlatformIO/linear-algebra/Vector3.h
+  'friend Vector3 Passer::LinearAlgebra::Vector3::operator*(float f, const Vector3 &v)' at line 154 of file d:/PlatformIO/linear-algebra/Vector3.h
 d:/PlatformIO/linear-algebra/Quaternion.cpp:152: warning: no uniquely matching class member found for 
   Quaternion Quaternion::LookRotation(const Vector3 &forward, const Vector3 &up)
 Possible candidates:
@@ -43,52 +43,51 @@ Possible candidates:
 d:/PlatformIO/linear-algebra/Quaternion.cpp:330: warning: no uniquely matching class member found for 
   Quaternion Quaternion::Euler(Vector3 euler)
 Possible candidates:
-  'static Quaternion Passer::LinearAlgebra::Quaternion::Euler(float x, float y, float z)' at line 215 of file d:/PlatformIO/linear-algebra/Quaternion.h
-  'static Quaternion Passer::LinearAlgebra::Quaternion::Euler(Vector3 eulerAngles)' at line 222 of file d:/PlatformIO/linear-algebra/Quaternion.h
+  'static Quaternion Passer::LinearAlgebra::Quaternion::Euler(float x, float y, float z)' at line 218 of file d:/PlatformIO/linear-algebra/Quaternion.h
+  'static Quaternion Passer::LinearAlgebra::Quaternion::Euler(Vector3 eulerAngles)' at line 225 of file d:/PlatformIO/linear-algebra/Quaternion.h
 d:/PlatformIO/linear-algebra/Quaternion.cpp:362: warning: no uniquely matching class member found for 
   Quaternion Quaternion::EulerXYZ(Vector3 euler)
 Possible candidates:
-  'static Quaternion Passer::LinearAlgebra::Quaternion::EulerXYZ(float x, float y, float z)' at line 232 of file d:/PlatformIO/linear-algebra/Quaternion.h
-  'static Quaternion Passer::LinearAlgebra::Quaternion::EulerXYZ(Vector3 eulerAngles)' at line 239 of file d:/PlatformIO/linear-algebra/Quaternion.h
+  'static Quaternion Passer::LinearAlgebra::Quaternion::EulerXYZ(float x, float y, float z)' at line 235 of file d:/PlatformIO/linear-algebra/Quaternion.h
+  'static Quaternion Passer::LinearAlgebra::Quaternion::EulerXYZ(Vector3 eulerAngles)' at line 242 of file d:/PlatformIO/linear-algebra/Quaternion.h
 d:/PlatformIO/linear-algebra/Spherical.cpp:137: warning: no uniquely matching class member found for 
   template < T >
   SphericalOf< T > SphericalOf::operator-(const SphericalOf< T > &s2) const
 Possible candidates:
-  'AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator-() const' at line 99 of file d:/PlatformIO/linear-algebra/Angle.h
-  'AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator-(const AngleOf< T > &angle) const' at line 103 of file d:/PlatformIO/linear-algebra/Angle.h
-  'DirectionOf< T > Passer::LinearAlgebra::DirectionOf< T >::operator-() const' at line 84 of file d:/PlatformIO/linear-algebra/Direction.h
-  'PolarOf Passer::LinearAlgebra::PolarOf< T >::operator-() const' at line 100 of file d:/PlatformIO/linear-algebra/Polar.h
-  'PolarOf Passer::LinearAlgebra::PolarOf< T >::operator-(const PolarOf &v) const' at line 105 of file d:/PlatformIO/linear-algebra/Polar.h
-  'SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator-() const' at line 93 of file d:/PlatformIO/linear-algebra/Spherical.h
-  'SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator-(const SphericalOf< T > &v) const' at line 98 of file d:/PlatformIO/linear-algebra/Spherical.h
-  'Vector2 Passer::LinearAlgebra::Vector2::operator-()' at line 116 of file d:/PlatformIO/linear-algebra/Vector2.h
-  'Vector2 Passer::LinearAlgebra::Vector2::operator-(const Vector2 &v) const' at line 121 of file d:/PlatformIO/linear-algebra/Vector2.h
-  'Vector3 Passer::LinearAlgebra::Vector3::operator-() const' at line 124 of file d:/PlatformIO/linear-algebra/Vector3.h
-  'Vector3 Passer::LinearAlgebra::Vector3::operator-(const Vector3 &v) const' at line 129 of file d:/PlatformIO/linear-algebra/Vector3.h
+  'AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator-() const' at line 102 of file d:/PlatformIO/linear-algebra/Angle.h
+  'AngleOf< T > Passer::LinearAlgebra::AngleOf< T >::operator-(const AngleOf< T > &angle) const' at line 106 of file d:/PlatformIO/linear-algebra/Angle.h
+  'DirectionOf< T > Passer::LinearAlgebra::DirectionOf< T >::operator-() const' at line 41 of file d:/PlatformIO/linear-algebra/Direction.h
+  'PolarOf Passer::LinearAlgebra::PolarOf< T >::operator-() const' at line 82 of file d:/PlatformIO/linear-algebra/Polar.h
+  'PolarOf Passer::LinearAlgebra::PolarOf< T >::operator-(const PolarOf &v) const' at line 87 of file d:/PlatformIO/linear-algebra/Polar.h
+  'RangeUsing< T > Passer::LinearAlgebra::RangeUsing< T >::operator-(RangeUsing< T > a)' at line 38 of file d:/PlatformIO/linear-algebra/Range.h
+  'RangeUsing< T > Passer::LinearAlgebra::RangeUsing< T >::operator-()' at line 40 of file d:/PlatformIO/linear-algebra/Range.h
+  'SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator-() const' at line 91 of file d:/PlatformIO/linear-algebra/Spherical.h
+  'SphericalOf< T > Passer::LinearAlgebra::SphericalOf< T >::operator-(const SphericalOf< T > &v) const' at line 96 of file d:/PlatformIO/linear-algebra/Spherical.h
+  'Vector2 Passer::LinearAlgebra::Vector2::operator-()' at line 118 of file d:/PlatformIO/linear-algebra/Vector2.h
+  'Vector2 Passer::LinearAlgebra::Vector2::operator-(const Vector2 &v) const' at line 123 of file d:/PlatformIO/linear-algebra/Vector2.h
+  'Vector3 Passer::LinearAlgebra::Vector3::operator-() const' at line 126 of file d:/PlatformIO/linear-algebra/Vector3.h
+  'Vector3 Passer::LinearAlgebra::Vector3::operator-(const Vector3 &v) const' at line 131 of file d:/PlatformIO/linear-algebra/Vector3.h
 d:/PlatformIO/linear-algebra/Vector2.cpp:20: warning: no uniquely matching class member found for 
   Vector2::Vector2(float _x, float _y)
 Possible candidates:
-  'Passer::LinearAlgebra::Vector2::Vector2()' at line 43 of file d:/PlatformIO/linear-algebra/Vector2.h
-  'Passer::LinearAlgebra::Vector2::Vector2(float right, float forward)' at line 47 of file d:/PlatformIO/linear-algebra/Vector2.h
-  'Passer::LinearAlgebra::Vector2::Vector2(Vector3 v)' at line 51 of file d:/PlatformIO/linear-algebra/Vector2.h
-  'Passer::LinearAlgebra::Vector2::Vector2(PolarOf< float > v)' at line 54 of file d:/PlatformIO/linear-algebra/Vector2.h
+  'Passer::LinearAlgebra::Vector2::Vector2()' at line 45 of file d:/PlatformIO/linear-algebra/Vector2.h
+  'Passer::LinearAlgebra::Vector2::Vector2(float right, float forward)' at line 49 of file d:/PlatformIO/linear-algebra/Vector2.h
+  'Passer::LinearAlgebra::Vector2::Vector2(Vector3 v)' at line 53 of file d:/PlatformIO/linear-algebra/Vector2.h
+  'Passer::LinearAlgebra::Vector2::Vector2(PolarOf< float > v)' at line 56 of file d:/PlatformIO/linear-algebra/Vector2.h
 d:/PlatformIO/linear-algebra/Vector2.cpp:32: warning: no uniquely matching class member found for 
-  Vector2::Vector2(PolarSingle p)
+  Vector2::Vector2(Polar p)
 Possible candidates:
-  'Passer::LinearAlgebra::Vector2::Vector2()' at line 43 of file d:/PlatformIO/linear-algebra/Vector2.h
-  'Passer::LinearAlgebra::Vector2::Vector2(float right, float forward)' at line 47 of file d:/PlatformIO/linear-algebra/Vector2.h
-  'Passer::LinearAlgebra::Vector2::Vector2(Vector3 v)' at line 51 of file d:/PlatformIO/linear-algebra/Vector2.h
-  'Passer::LinearAlgebra::Vector2::Vector2(PolarOf< float > v)' at line 54 of file d:/PlatformIO/linear-algebra/Vector2.h
+  'Passer::LinearAlgebra::Vector2::Vector2()' at line 45 of file d:/PlatformIO/linear-algebra/Vector2.h
+  'Passer::LinearAlgebra::Vector2::Vector2(float right, float forward)' at line 49 of file d:/PlatformIO/linear-algebra/Vector2.h
+  'Passer::LinearAlgebra::Vector2::Vector2(Vector3 v)' at line 53 of file d:/PlatformIO/linear-algebra/Vector2.h
+  'Passer::LinearAlgebra::Vector2::Vector2(PolarOf< float > v)' at line 56 of file d:/PlatformIO/linear-algebra/Vector2.h
 d:/PlatformIO/linear-algebra/Vector3.cpp:33: warning: no uniquely matching class member found for 
   Vector3::Vector3(SphericalOf< float > s)
 Possible candidates:
-  'Passer::LinearAlgebra::Vector3::Vector3()' at line 47 of file d:/PlatformIO/linear-algebra/Vector3.h
-  'Passer::LinearAlgebra::Vector3::Vector3(float right, float up, float forward)' at line 52 of file d:/PlatformIO/linear-algebra/Vector3.h
-  'Passer::LinearAlgebra::Vector3::Vector3(Vector2 v)' at line 55 of file d:/PlatformIO/linear-algebra/Vector3.h
-  'Passer::LinearAlgebra::Vector3::Vector3(SphericalOf< float > v)' at line 59 of file d:/PlatformIO/linear-algebra/Vector3.h
-d:/PlatformIO/linear-algebra/Direction.h:43: warning: argument 'v' of command @param is not found in the argument list of Passer::LinearAlgebra::DirectionOf< T >::FromVector3(Vector3 vector)
-d:/PlatformIO/linear-algebra/Direction.h:43: warning: The following parameter of Passer::LinearAlgebra::DirectionOf::FromVector3(Vector3 vector) is not documented:
-  parameter 'vector'
+  'Passer::LinearAlgebra::Vector3::Vector3()' at line 49 of file d:/PlatformIO/linear-algebra/Vector3.h
+  'Passer::LinearAlgebra::Vector3::Vector3(float right, float up, float forward)' at line 54 of file d:/PlatformIO/linear-algebra/Vector3.h
+  'Passer::LinearAlgebra::Vector3::Vector3(Vector2 v)' at line 57 of file d:/PlatformIO/linear-algebra/Vector3.h
+  'Passer::LinearAlgebra::Vector3::Vector3(SphericalOf< float > v)' at line 61 of file d:/PlatformIO/linear-algebra/Vector3.h
 d:/PlatformIO/linear-algebra/Matrix.h:12: warning: Member MatrixOf(unsigned int rows, unsigned int cols) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
 d:/PlatformIO/linear-algebra/Matrix.h:13: warning: Member MatrixOf(unsigned int rows, unsigned int cols, const T *source) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
 d:/PlatformIO/linear-algebra/Matrix.h:17: warning: Member MatrixOf(Vector3 v) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
@@ -104,51 +103,20 @@ d:/PlatformIO/linear-algebra/Matrix.h:108: warning: Member RowCount() const (fun
 d:/PlatformIO/linear-algebra/Matrix.h:109: warning: Member ColCount() const (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
 d:/PlatformIO/linear-algebra/Matrix.h:57: warning: Member Multiply(const MatrixOf< T > *m1, const MatrixOf< T > *m2, MatrixOf< T > *r) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
 d:/PlatformIO/linear-algebra/Matrix.h:63: warning: Member Multiply(const MatrixOf< T > *m, Vector3 v) (function) of class Passer::LinearAlgebra::MatrixOf is not documented.
-d:/PlatformIO/linear-algebra/Polar.h:106: warning: Member operator-=(const PolarOf &v) (function) of class Passer::LinearAlgebra::PolarOf is not documented.
-d:/PlatformIO/linear-algebra/Polar.h:111: warning: Member operator+=(const PolarOf &v) (function) of class Passer::LinearAlgebra::PolarOf is not documented.
-d:/PlatformIO/linear-algebra/Polar.h:124: warning: Member operator*=(float f) (function) of class Passer::LinearAlgebra::PolarOf is not documented.
-d:/PlatformIO/linear-algebra/Polar.h:136: warning: Member operator/=(float f) (function) of class Passer::LinearAlgebra::PolarOf is not documented.
-d:/PlatformIO/linear-algebra/Polar.h:121: warning: Member operator*(float f, const PolarOf &v) (friend) of class Passer::LinearAlgebra::PolarOf is not documented.
-d:/PlatformIO/linear-algebra/Polar.h:133: warning: Member operator/(float f, const PolarOf &v) (friend) of class Passer::LinearAlgebra::PolarOf is not documented.
-d:/PlatformIO/linear-algebra/Polar.h:59: warning: argument 's' of command @param is not found in the argument list of Passer::LinearAlgebra::PolarOf< T >::FromSpherical(SphericalOf< T > v)
-d:/PlatformIO/linear-algebra/Polar.h:59: warning: The following parameter of Passer::LinearAlgebra::PolarOf::FromSpherical(SphericalOf< T > v) is not documented:
-  parameter 'v'
-d:/PlatformIO/linear-algebra/Spherical.h:29: warning: Member SphericalOf(float distance, AngleOf< T > horizontal, AngleOf< T > vertical) (function) of class Passer::LinearAlgebra::SphericalOf is not documented.
-d:/PlatformIO/linear-algebra/Spherical.h:29: warning: Member SphericalOf(float distance, DirectionOf< T > direction) (function) of class Passer::LinearAlgebra::SphericalOf is not documented.
-d:/PlatformIO/linear-algebra/Spherical.h:99: warning: Member operator-=(const SphericalOf< T > &v) (function) of class Passer::LinearAlgebra::SphericalOf is not documented.
-d:/PlatformIO/linear-algebra/Spherical.h:104: warning: Member operator+=(const SphericalOf< T > &v) (function) of class Passer::LinearAlgebra::SphericalOf is not documented.
-d:/PlatformIO/linear-algebra/Spherical.h:117: warning: Member operator*=(float f) (function) of class Passer::LinearAlgebra::SphericalOf is not documented.
-d:/PlatformIO/linear-algebra/Spherical.h:129: warning: Member operator/=(float f) (function) of class Passer::LinearAlgebra::SphericalOf is not documented.
-d:/PlatformIO/linear-algebra/Spherical.h:54: warning: Member Rad (variable) of class Passer::LinearAlgebra::SphericalOf is not documented.
-d:/PlatformIO/linear-algebra/Spherical.h:114: warning: Member operator*(float f, const SphericalOf< T > &v) (friend) of class Passer::LinearAlgebra::SphericalOf is not documented.
-d:/PlatformIO/linear-algebra/Spherical.h:126: warning: Member operator/(float f, const SphericalOf< T > &v) (friend) of class Passer::LinearAlgebra::SphericalOf is not documented.
-d:/PlatformIO/linear-algebra/SwingTwist.h:22: warning: Member SwingTwistOf(DirectionOf< T > swing, AngleOf< T > twist) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
-d:/PlatformIO/linear-algebra/SwingTwist.h:22: warning: Member SwingTwistOf(AngleOf< T > horizontal, AngleOf< T > vertical, AngleOf< T > twist) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
-d:/PlatformIO/linear-algebra/SwingTwist.h:31: warning: Member ToQuaternion() const (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
-d:/PlatformIO/linear-algebra/SwingTwist.h:34: warning: Member ToAngleAxis() const (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
-d:/PlatformIO/linear-algebra/SwingTwist.h:39: warning: Member operator==(const SwingTwistOf< T > d) const (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
-d:/PlatformIO/linear-algebra/SwingTwist.h:55: warning: Member operator*=(const SwingTwistOf< T > &rotation) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
-d:/PlatformIO/linear-algebra/SwingTwist.h:69: warning: Member Normalize() (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
-d:/PlatformIO/linear-algebra/SwingTwist.h:28: warning: Member Degrees(float horizontal, float vertical=0, float twist=0) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
-d:/PlatformIO/linear-algebra/SwingTwist.h:32: warning: Member FromQuaternion(Quaternion q) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
-d:/PlatformIO/linear-algebra/SwingTwist.h:35: warning: Member FromAngleAxis(SphericalOf< T > aa) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
-d:/PlatformIO/linear-algebra/SwingTwist.h:57: warning: Member Inverse(SwingTwistOf< T > rotation) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
-d:/PlatformIO/linear-algebra/SwingTwist.h:67: warning: Member Angle(const SwingTwistOf< T > &r1, const SwingTwistOf< T > &r2) (function) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
-d:/PlatformIO/linear-algebra/SwingTwist.h:21: warning: Member swing (variable) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
-d:/PlatformIO/linear-algebra/SwingTwist.h:22: warning: Member twist (variable) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
-d:/PlatformIO/linear-algebra/SwingTwist.h:37: warning: Member identity (variable) of class Passer::LinearAlgebra::SwingTwistOf is not documented.
-d:/PlatformIO/linear-algebra/Vector2.h:122: warning: Member operator-=(const Vector2 &v) (function) of struct Passer::LinearAlgebra::Vector2 is not documented.
-d:/PlatformIO/linear-algebra/Vector2.h:127: warning: Member operator+=(const Vector2 &v) (function) of struct Passer::LinearAlgebra::Vector2 is not documented.
-d:/PlatformIO/linear-algebra/Vector2.h:148: warning: Member operator*=(float f) (function) of struct Passer::LinearAlgebra::Vector2 is not documented.
-d:/PlatformIO/linear-algebra/Vector2.h:159: warning: Member operator/=(float f) (function) of struct Passer::LinearAlgebra::Vector2 is not documented.
-d:/PlatformIO/linear-algebra/Vector2.h:144: warning: Member operator*(float f, const Vector2 &v) (friend) of struct Passer::LinearAlgebra::Vector2 is not documented.
-d:/PlatformIO/linear-algebra/Vector2.h:156: warning: Member operator/(float f, const Vector2 &v) (friend) of struct Passer::LinearAlgebra::Vector2 is not documented.
-d:/PlatformIO/linear-algebra/Vector3.h:82: warning: Member Forward() const (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
-d:/PlatformIO/linear-algebra/Vector3.h:83: warning: Member Up() const (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
-d:/PlatformIO/linear-algebra/Vector3.h:84: warning: Member Right() const (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
-d:/PlatformIO/linear-algebra/Vector3.h:130: warning: Member operator-=(const Vector3 &v) (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
-d:/PlatformIO/linear-algebra/Vector3.h:135: warning: Member operator+=(const Vector3 &v) (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
-d:/PlatformIO/linear-algebra/Vector3.h:156: warning: Member operator*=(float f) (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
-d:/PlatformIO/linear-algebra/Vector3.h:168: warning: Member operator/=(float f) (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
-d:/PlatformIO/linear-algebra/Vector3.h:152: warning: Member operator*(float f, const Vector3 &v) (friend) of struct Passer::LinearAlgebra::Vector3 is not documented.
-d:/PlatformIO/linear-algebra/Vector3.h:164: warning: Member operator/(float f, const Vector3 &v) (friend) of struct Passer::LinearAlgebra::Vector3 is not documented.
+d:/PlatformIO/linear-algebra/Vector2.h:124: warning: Member operator-=(const Vector2 &v) (function) of struct Passer::LinearAlgebra::Vector2 is not documented.
+d:/PlatformIO/linear-algebra/Vector2.h:129: warning: Member operator+=(const Vector2 &v) (function) of struct Passer::LinearAlgebra::Vector2 is not documented.
+d:/PlatformIO/linear-algebra/Vector2.h:150: warning: Member operator*=(float f) (function) of struct Passer::LinearAlgebra::Vector2 is not documented.
+d:/PlatformIO/linear-algebra/Vector2.h:161: warning: Member operator/=(float f) (function) of struct Passer::LinearAlgebra::Vector2 is not documented.
+d:/PlatformIO/linear-algebra/Vector2.h:146: warning: Member operator*(float f, const Vector2 &v) (friend) of struct Passer::LinearAlgebra::Vector2 is not documented.
+d:/PlatformIO/linear-algebra/Vector2.h:158: warning: Member operator/(float f, const Vector2 &v) (friend) of struct Passer::LinearAlgebra::Vector2 is not documented.
+d:/PlatformIO/linear-algebra/Vector3.h:84: warning: Member Forward() const (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
+d:/PlatformIO/linear-algebra/Vector3.h:85: warning: Member Up() const (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
+d:/PlatformIO/linear-algebra/Vector3.h:86: warning: Member Right() const (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
+d:/PlatformIO/linear-algebra/Vector3.h:132: warning: Member operator-=(const Vector3 &v) (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
+d:/PlatformIO/linear-algebra/Vector3.h:137: warning: Member operator+=(const Vector3 &v) (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
+d:/PlatformIO/linear-algebra/Vector3.h:158: warning: Member operator*=(float f) (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
+d:/PlatformIO/linear-algebra/Vector3.h:170: warning: Member operator/=(float f) (function) of struct Passer::LinearAlgebra::Vector3 is not documented.
+d:/PlatformIO/linear-algebra/Vector3.h:154: warning: Member operator*(float f, const Vector3 &v) (friend) of struct Passer::LinearAlgebra::Vector3 is not documented.
+d:/PlatformIO/linear-algebra/Vector3.h:166: warning: Member operator/(float f, const Vector3 &v) (friend) of struct Passer::LinearAlgebra::Vector3 is not documented.
+t f, const Vector3 &v) (friend) of struct Passer::LinearAlgebra::Vector3 is not documented.
+d:/PlatformIO/linear-algebra/Vector3.h:166: warning: Member operator/(float f, const Vector3 &v) (friend) of struct Passer::LinearAlgebra::Vector3 is not documented.

ControlCore/LinearAlgebra/DoxyGen/Doxyfile

@@ -1,4 +1,4 @@
-# Doxyfile 1.9.8
+# Doxyfile 1.9.2
 
 # This file describes the settings to be used by the documentation system
 # doxygen (www.doxygen.org) for a project.
@@ -12,16 +12,6 @@
 # For lists, items can also be appended using:
 # TAG += value [value, ...]
 # Values that contain spaces should be placed between quotes (\" \").
-#
-# Note:
-#
-# Use doxygen to compare the used configuration file with the template
-# configuration file:
-# doxygen -x [configFile]
-# Use doxygen to compare the used configuration file with the template
-# configuration file without replacing the environment variables or CMake type
-# replacement variables:
-# doxygen -x_noenv [configFile]
 
 #---------------------------------------------------------------------------
 # Project related configuration options
@@ -42,7 +32,7 @@ DOXYFILE_ENCODING      = UTF-8
 # title of most generated pages and in a few other places.
 # The default value is: My Project.
 
-PROJECT_NAME           = "Roboid Control for C++"
+PROJECT_NAME           = LinearAlgebra
 
 # The PROJECT_NUMBER tag can be used to enter a project or revision number. This
 # could be handy for archiving the generated documentation or if some version
@@ -61,37 +51,25 @@ PROJECT_BRIEF          =
 # pixels and the maximum width should not exceed 200 pixels. Doxygen will copy
 # the logo to the output directory.
 
-PROJECT_LOGO           = //intranet/home/Afbeeldingen/PasserVR/Logos/PasserLife/PasserLifeLogoLeft_300.png
+PROJECT_LOGO           = //intranet/home/Afbeeldingen/PasserVR/Logos/Logo3NameRight100.png
 
 # The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
 # into which the generated documentation will be written. If a relative path is
 # entered, it will be relative to the location where doxygen was started. If
 # left blank the current directory will be used.
 
-OUTPUT_DIRECTORY       = //intranet/web/roboidcontrol_doc/Cpp/
+OUTPUT_DIRECTORY       = //intranet/web/apis/LinearAlgebra
 
-# If the CREATE_SUBDIRS tag is set to YES then doxygen will create up to 4096
-# sub-directories (in 2 levels) under the output directory of each output format
-# and will distribute the generated files over these directories. Enabling this
+# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub-
+# directories (in 2 levels) under the output directory of each output format and
+# will distribute the generated files over these directories. Enabling this
 # option can be useful when feeding doxygen a huge amount of source files, where
 # putting all generated files in the same directory would otherwise causes
-# performance problems for the file system. Adapt CREATE_SUBDIRS_LEVEL to
-# control the number of sub-directories.
+# performance problems for the file system.
 # The default value is: NO.
 
 CREATE_SUBDIRS         = NO
 
-# Controls the number of sub-directories that will be created when
-# CREATE_SUBDIRS tag is set to YES. Level 0 represents 16 directories, and every
-# level increment doubles the number of directories, resulting in 4096
-# directories at level 8 which is the default and also the maximum value. The
-# sub-directories are organized in 2 levels, the first level always has a fixed
-# number of 16 directories.
-# Minimum value: 0, maximum value: 8, default value: 8.
-# This tag requires that the tag CREATE_SUBDIRS is set to YES.
-
-CREATE_SUBDIRS_LEVEL   = 8
-
 # If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII
 # characters to appear in the names of generated files. If set to NO, non-ASCII
 # characters will be escaped, for example _xE3_x81_x84 will be used for Unicode
@@ -103,14 +81,14 @@ ALLOW_UNICODE_NAMES    = NO
 # The OUTPUT_LANGUAGE tag is used to specify the language in which all
 # documentation generated by doxygen is written. Doxygen will use this
 # information to generate all constant output in the proper language.
-# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Bulgarian,
-# Catalan, Chinese, Chinese-Traditional, Croatian, Czech, Danish, Dutch, English
-# (United States), Esperanto, Farsi (Persian), Finnish, French, German, Greek,
-# Hindi, Hungarian, Indonesian, Italian, Japanese, Japanese-en (Japanese with
-# English messages), Korean, Korean-en (Korean with English messages), Latvian,
-# Lithuanian, Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese,
-# Romanian, Russian, Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish,
-# Swedish, Turkish, Ukrainian and Vietnamese.
+# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese,
+# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States),
+# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian,
+# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages),
+# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian,
+# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian,
+# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish,
+# Ukrainian and Vietnamese.
 # The default value is: English.
 
 OUTPUT_LANGUAGE        = English
@@ -363,17 +341,6 @@ MARKDOWN_SUPPORT       = YES
 
 TOC_INCLUDE_HEADINGS   = 0
 
-# The MARKDOWN_ID_STYLE tag can be used to specify the algorithm used to
-# generate identifiers for the Markdown headings. Note: Every identifier is
-# unique.
-# Possible values are: DOXYGEN use a fixed 'autotoc_md' string followed by a
-# sequence number starting at 0 and GITHUB use the lower case version of title
-# with any whitespace replaced by '-' and punctuation characters removed.
-# The default value is: DOXYGEN.
-# This tag requires that the tag MARKDOWN_SUPPORT is set to YES.
-
-MARKDOWN_ID_STYLE      = DOXYGEN
-
 # When enabled doxygen tries to link words that correspond to documented
 # classes, or namespaces to their corresponding documentation. Such a link can
 # be prevented in individual cases by putting a % sign in front of the word or
@@ -485,7 +452,7 @@ TYPEDEF_HIDES_STRUCT   = NO
 
 LOOKUP_CACHE_SIZE      = 0
 
-# The NUM_PROC_THREADS specifies the number of threads doxygen is allowed to use
+# The NUM_PROC_THREADS specifies the number threads doxygen is allowed to use
 # during processing. When set to 0 doxygen will based this on the number of
 # cores available in the system. You can set it explicitly to a value larger
 # than 0 to get more control over the balance between CPU load and processing
@@ -498,14 +465,6 @@ LOOKUP_CACHE_SIZE      = 0
 
 NUM_PROC_THREADS       = 1
 
-# If the TIMESTAMP tag is set different from NO then each generated page will
-# contain the date or date and time when the page was generated. Setting this to
-# NO can help when comparing the output of multiple runs.
-# Possible values are: YES, NO, DATETIME and DATE.
-# The default value is: NO.
-
-TIMESTAMP              = NO
-
 #---------------------------------------------------------------------------
 # Build related configuration options
 #---------------------------------------------------------------------------
@@ -587,8 +546,7 @@ HIDE_UNDOC_MEMBERS     = NO
 # If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all
 # undocumented classes that are normally visible in the class hierarchy. If set
 # to NO, these classes will be included in the various overviews. This option
-# will also hide undocumented C++ concepts if enabled. This option has no effect
-# if EXTRACT_ALL is enabled.
+# has no effect if EXTRACT_ALL is enabled.
 # The default value is: NO.
 
 HIDE_UNDOC_CLASSES     = YES
@@ -619,15 +577,14 @@ INTERNAL_DOCS          = NO
 # filesystem is case sensitive (i.e. it supports files in the same directory
 # whose names only differ in casing), the option must be set to YES to properly
 # deal with such files in case they appear in the input. For filesystems that
-# are not case sensitive the option should be set to NO to properly deal with
+# are not case sensitive the option should be be set to NO to properly deal with
 # output files written for symbols that only differ in casing, such as for two
 # classes, one named CLASS and the other named Class, and to also support
 # references to files without having to specify the exact matching casing. On
 # Windows (including Cygwin) and MacOS, users should typically set this option
 # to NO, whereas on Linux or other Unix flavors it should typically be set to
 # YES.
-# Possible values are: SYSTEM, NO and YES.
-# The default value is: SYSTEM.
+# The default value is: system dependent.
 
 CASE_SENSE_NAMES       = NO
 
@@ -777,7 +734,7 @@ MAX_INITIALIZER_LINES  = 30
 # list will mention the files that were used to generate the documentation.
 # The default value is: YES.
 
-SHOW_USED_FILES        = NO
+SHOW_USED_FILES        = YES
 
 # Set the SHOW_FILES tag to NO to disable the generation of the Files page. This
 # will remove the Files entry from the Quick Index and from the Folder Tree View
@@ -879,26 +836,11 @@ WARN_IF_INCOMPLETE_DOC = YES
 
 WARN_NO_PARAMDOC       = NO
 
-# If WARN_IF_UNDOC_ENUM_VAL option is set to YES, doxygen will warn about
-# undocumented enumeration values. If set to NO, doxygen will accept
-# undocumented enumeration values. If EXTRACT_ALL is set to YES then this flag
-# will automatically be disabled.
-# The default value is: NO.
-
-WARN_IF_UNDOC_ENUM_VAL = NO
-
 # If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when
 # a warning is encountered. If the WARN_AS_ERROR tag is set to FAIL_ON_WARNINGS
 # then doxygen will continue running as if WARN_AS_ERROR tag is set to NO, but
 # at the end of the doxygen process doxygen will return with a non-zero status.
-# If the WARN_AS_ERROR tag is set to FAIL_ON_WARNINGS_PRINT then doxygen behaves
-# like FAIL_ON_WARNINGS but in case no WARN_LOGFILE is defined doxygen will not
-# write the warning messages in between other messages but write them at the end
-# of a run, in case a WARN_LOGFILE is defined the warning messages will be
-# besides being in the defined file also be shown at the end of a run, unless
-# the WARN_LOGFILE is defined as - i.e. standard output (stdout) in that case
-# the behavior will remain as with the setting FAIL_ON_WARNINGS.
-# Possible values are: NO, YES, FAIL_ON_WARNINGS and FAIL_ON_WARNINGS_PRINT.
+# Possible values are: NO, YES and FAIL_ON_WARNINGS.
 # The default value is: NO.
 
 WARN_AS_ERROR          = NO
@@ -909,27 +851,13 @@ WARN_AS_ERROR          = NO
 # and the warning text. Optionally the format may contain $version, which will
 # be replaced by the version of the file (if it could be obtained via
 # FILE_VERSION_FILTER)
-# See also: WARN_LINE_FORMAT
 # The default value is: $file:$line: $text.
 
 WARN_FORMAT            = "$file:$line: $text"
 
-# In the $text part of the WARN_FORMAT command it is possible that a reference
-# to a more specific place is given. To make it easier to jump to this place
-# (outside of doxygen) the user can define a custom "cut" / "paste" string.
-# Example:
-# WARN_LINE_FORMAT = "'vi $file +$line'"
-# See also: WARN_FORMAT
-# The default value is: at line $line of file $file.
-
-WARN_LINE_FORMAT       = "at line $line of file $file"
-
 # The WARN_LOGFILE tag can be used to specify a file to which warning and error
 # messages should be written. If left blank the output is written to standard
-# error (stderr). In case the file specified cannot be opened for writing the
-# warning and error messages are written to standard error. When as file - is
-# specified the warning and error messages are written to standard output
-# (stdout).
+# error (stderr).
 
 WARN_LOGFILE           = DoxyWarnLogfile.txt
 
@@ -951,21 +879,10 @@ INPUT                  = .. \
 # libiconv (or the iconv built into libc) for the transcoding. See the libiconv
 # documentation (see:
 # https://www.gnu.org/software/libiconv/) for the list of possible encodings.
-# See also: INPUT_FILE_ENCODING
 # The default value is: UTF-8.
 
 INPUT_ENCODING         = UTF-8
 
-# This tag can be used to specify the character encoding of the source files
-# that doxygen parses The INPUT_FILE_ENCODING tag can be used to specify
-# character encoding on a per file pattern basis. Doxygen will compare the file
-# name with each pattern and apply the encoding instead of the default
-# INPUT_ENCODING) if there is a match. The character encodings are a list of the
-# form: pattern=encoding (like *.php=ISO-8859-1). See cfg_input_encoding
-# "INPUT_ENCODING" for further information on supported encodings.
-
-INPUT_FILE_ENCODING    =
-
 # If the value of the INPUT tag contains directories, you can use the
 # FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and
 # *.h) to filter out the source-files in the directories.
@@ -977,12 +894,12 @@ INPUT_FILE_ENCODING    =
 # Note the list of default checked file patterns might differ from the list of
 # default file extension mappings.
 #
-# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cxxm,
-# *.cpp, *.cppm, *.c++, *.c++m, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl,
-# *.ddl, *.odl, *.h, *.hh, *.hxx, *.hpp, *.h++, *.ixx, *.l, *.cs, *.d, *.php,
-# *.php4, *.php5, *.phtml, *.inc, *.m, *.markdown, *.md, *.mm, *.dox (to be
-# provided as doxygen C comment), *.py, *.pyw, *.f90, *.f95, *.f03, *.f08,
-# *.f18, *.f, *.for, *.vhd, *.vhdl, *.ucf, *.qsf and *.ice.
+# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp,
+# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h,
+# *.hh, *.hxx, *.hpp, *.h++, *.l, *.cs, *.d, *.php, *.php4, *.php5, *.phtml,
+# *.inc, *.m, *.markdown, *.md, *.mm, *.dox (to be provided as doxygen C
+# comment), *.py, *.pyw, *.f90, *.f95, *.f03, *.f08, *.f18, *.f, *.for, *.vhd,
+# *.vhdl, *.ucf, *.qsf and *.ice.
 
 FILE_PATTERNS          = *.c \
                          *.cc \
@@ -1044,7 +961,7 @@ RECURSIVE              = YES
 # Note that relative paths are relative to the directory from which doxygen is
 # run.
 
-EXCLUDE                = ../LinearAlgebra
+EXCLUDE                =
 
 # The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
 # directories that are symbolic links (a Unix file system feature) are excluded
@@ -1060,14 +977,16 @@ EXCLUDE_SYMLINKS       = NO
 # Note that the wildcards are matched against the file with absolute path, so to
 # exclude all test directories for example use the pattern */test/*
 
-EXCLUDE_PATTERNS       = gtest* \
-                         googletest*
+EXCLUDE_PATTERNS       = gtest*, googletest*
 
 # The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names
 # (namespaces, classes, functions, etc.) that should be excluded from the
 # output. The symbol name can be a fully qualified name, a word, or if the
 # wildcard * is used, a substring. Examples: ANamespace, AClass,
-# ANamespace::AClass, ANamespace::*Test
+# AClass::ANamespace, ANamespace::*Test
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories use the pattern */test/*
 
 EXCLUDE_SYMBOLS        =
 
@@ -1113,11 +1032,6 @@ IMAGE_PATH             = images \
 # code is scanned, but not when the output code is generated. If lines are added
 # or removed, the anchors will not be placed correctly.
 #
-# Note that doxygen will use the data processed and written to standard output
-# for further processing, therefore nothing else, like debug statements or used
-# commands (so in case of a Windows batch file always use @echo OFF), should be
-# written to standard output.
-#
 # Note that for custom extensions or not directly supported extensions you also
 # need to set EXTENSION_MAPPING for the extension otherwise the files are not
 # properly processed by doxygen.
@@ -1159,15 +1073,6 @@ FILTER_SOURCE_PATTERNS =
 
 USE_MDFILE_AS_MAINPAGE =
 
-# The Fortran standard specifies that for fixed formatted Fortran code all
-# characters from position 72 are to be considered as comment. A common
-# extension is to allow longer lines before the automatic comment starts. The
-# setting FORTRAN_COMMENT_AFTER will also make it possible that longer lines can
-# be processed before the automatic comment starts.
-# Minimum value: 7, maximum value: 10000, default value: 72.
-
-FORTRAN_COMMENT_AFTER  = 72
-
 #---------------------------------------------------------------------------
 # Configuration options related to source browsing
 #---------------------------------------------------------------------------
@@ -1305,11 +1210,10 @@ CLANG_DATABASE_PATH    =
 
 ALPHABETICAL_INDEX     = YES
 
-# The IGNORE_PREFIX tag can be used to specify a prefix (or a list of prefixes)
-# that should be ignored while generating the index headers. The IGNORE_PREFIX
-# tag works for classes, function and member names. The entity will be placed in
-# the alphabetical list under the first letter of the entity name that remains
-# after removing the prefix.
+# In case all classes in a project start with a common prefix, all classes will
+# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag
+# can be used to specify a prefix (or a list of prefixes) that should be ignored
+# while generating the index headers.
 # This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
 
 IGNORE_PREFIX          =
@@ -1388,12 +1292,7 @@ HTML_STYLESHEET        =
 # Doxygen will copy the style sheet files to the output directory.
 # Note: The order of the extra style sheet files is of importance (e.g. the last
 # style sheet in the list overrules the setting of the previous ones in the
-# list).
-# Note: Since the styling of scrollbars can currently not be overruled in
-# Webkit/Chromium, the styling will be left out of the default doxygen.css if
-# one or more extra stylesheets have been specified. So if scrollbar
-# customization is desired it has to be added explicitly. For an example see the
-# documentation.
+# list). For an example see the documentation.
 # This tag requires that the tag GENERATE_HTML is set to YES.
 
 HTML_EXTRA_STYLESHEET  = custom_doxygen.css
@@ -1408,19 +1307,6 @@ HTML_EXTRA_STYLESHEET  = custom_doxygen.css
 
 HTML_EXTRA_FILES       =
 
-# The HTML_COLORSTYLE tag can be used to specify if the generated HTML output
-# should be rendered with a dark or light theme.
-# Possible values are: LIGHT always generate light mode output, DARK always
-# generate dark mode output, AUTO_LIGHT automatically set the mode according to
-# the user preference, use light mode if no preference is set (the default),
-# AUTO_DARK automatically set the mode according to the user preference, use
-# dark mode if no preference is set and TOGGLE allow to user to switch between
-# light and dark mode via a button.
-# The default value is: AUTO_LIGHT.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_COLORSTYLE        = LIGHT
-
 # The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen
 # will adjust the colors in the style sheet and background images according to
 # this color. Hue is specified as an angle on a color-wheel, see
@@ -1451,6 +1337,15 @@ HTML_COLORSTYLE_SAT    = 0
 
 HTML_COLORSTYLE_GAMMA  = 103
 
+# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML
+# page will contain the date and time when the page was generated. Setting this
+# to YES can help to show when doxygen was last run and thus if the
+# documentation is up to date.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_TIMESTAMP         = NO
+
 # If the HTML_DYNAMIC_MENUS tag is set to YES then the generated HTML
 # documentation will contain a main index with vertical navigation menus that
 # are dynamically created via JavaScript. If disabled, the navigation index will
@@ -1470,13 +1365,6 @@ HTML_DYNAMIC_MENUS     = YES
 
 HTML_DYNAMIC_SECTIONS  = NO
 
-# If the HTML_CODE_FOLDING tag is set to YES then classes and functions can be
-# dynamically folded and expanded in the generated HTML source code.
-# The default value is: YES.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_CODE_FOLDING      = YES
-
 # With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries
 # shown in the various tree structured indices initially; the user can expand
 # and collapse entries dynamically later on. Doxygen will expand the tree to
@@ -1513,13 +1401,6 @@ GENERATE_DOCSET        = NO
 
 DOCSET_FEEDNAME        = "Doxygen generated docs"
 
-# This tag determines the URL of the docset feed. A documentation feed provides
-# an umbrella under which multiple documentation sets from a single provider
-# (such as a company or product suite) can be grouped.
-# This tag requires that the tag GENERATE_DOCSET is set to YES.
-
-DOCSET_FEEDURL         =
-
 # This tag specifies a string that should uniquely identify the documentation
 # set bundle. This should be a reverse domain-name style string, e.g.
 # com.mycompany.MyDocSet. Doxygen will append .docset to the name.
@@ -1607,16 +1488,6 @@ BINARY_TOC             = NO
 
 TOC_EXPAND             = NO
 
-# The SITEMAP_URL tag is used to specify the full URL of the place where the
-# generated documentation will be placed on the server by the user during the
-# deployment of the documentation. The generated sitemap is called sitemap.xml
-# and placed on the directory specified by HTML_OUTPUT. In case no SITEMAP_URL
-# is specified no sitemap is generated. For information about the sitemap
-# protocol see https://www.sitemaps.org
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-SITEMAP_URL            =
-
 # If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and
 # QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that
 # can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help
@@ -1734,7 +1605,7 @@ GENERATE_TREEVIEW      = NO
 # area (value NO) or if it should extend to the full height of the window (value
 # YES). Setting this to YES gives a layout similar to
 # https://docs.readthedocs.io with more room for contents, but less room for the
-# project logo, title, and description. If either GENERATE_TREEVIEW or
+# project logo, title, and description. If either GENERATOR_TREEVIEW or
 # DISABLE_INDEX is set to NO, this option has no effect.
 # The default value is: NO.
 # This tag requires that the tag GENERATE_HTML is set to YES.
@@ -1765,13 +1636,6 @@ TREEVIEW_WIDTH         = 250
 
 EXT_LINKS_IN_WINDOW    = NO
 
-# If the OBFUSCATE_EMAILS tag is set to YES, doxygen will obfuscate email
-# addresses.
-# The default value is: YES.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-OBFUSCATE_EMAILS       = YES
-
 # If the HTML_FORMULA_FORMAT option is set to svg, doxygen will use the pdf2svg
 # tool (see https://github.com/dawbarton/pdf2svg) or inkscape (see
 # https://inkscape.org) to generate formulas as SVG images instead of PNGs for
@@ -1792,6 +1656,17 @@ HTML_FORMULA_FORMAT    = png
 
 FORMULA_FONTSIZE       = 10
 
+# Use the FORMULA_TRANSPARENT tag to determine whether or not the images
+# generated for formulas are transparent PNGs. Transparent PNGs are not
+# supported properly for IE 6.0, but are supported on all modern browsers.
+#
+# Note that when changing this option you need to delete any form_*.png files in
+# the HTML output directory before the changes have effect.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_TRANSPARENT    = YES
+
 # The FORMULA_MACROFILE can contain LaTeX \newcommand and \renewcommand commands
 # to create new LaTeX commands to be used in formulas as building blocks. See
 # the section "Including formulas" for details.
@@ -2105,16 +1980,9 @@ PDF_HYPERLINKS         = YES
 
 USE_PDFLATEX           = YES
 
-# The LATEX_BATCHMODE tag signals the behavior of LaTeX in case of an error.
-# Possible values are: NO same as ERROR_STOP, YES same as BATCH, BATCH In batch
-# mode nothing is printed on the terminal, errors are scrolled as if <return> is
-# hit at every error; missing files that TeX tries to input or request from
-# keyboard input (\read on a not open input stream) cause the job to abort,
-# NON_STOP In nonstop mode the diagnostic message will appear on the terminal,
-# but there is no possibility of user interaction just like in batch mode,
-# SCROLL In scroll mode, TeX will stop only for missing files to input or if
-# keyboard input is necessary and ERROR_STOP In errorstop mode, TeX will stop at
-# each error, asking for user intervention.
+# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode
+# command to the generated LaTeX files. This will instruct LaTeX to keep running
+# if errors occur, instead of asking the user for help.
 # The default value is: NO.
 # This tag requires that the tag GENERATE_LATEX is set to YES.
 
@@ -2135,6 +2003,14 @@ LATEX_HIDE_INDICES     = NO
 
 LATEX_BIB_STYLE        = plain
 
+# If the LATEX_TIMESTAMP tag is set to YES then the footer of each generated
+# page will contain the date and time when the page was generated. Setting this
+# to NO can help when comparing the output of multiple runs.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_TIMESTAMP        = NO
+
 # The LATEX_EMOJI_DIRECTORY tag is used to specify the (relative or absolute)
 # path from which the emoji images will be read. If a relative path is entered,
 # it will be relative to the LATEX_OUTPUT directory. If left blank the
@@ -2300,7 +2176,7 @@ DOCBOOK_OUTPUT         = docbook
 #---------------------------------------------------------------------------
 
 # If the GENERATE_AUTOGEN_DEF tag is set to YES, doxygen will generate an
-# AutoGen Definitions (see https://autogen.sourceforge.net/) file that captures
+# AutoGen Definitions (see http://autogen.sourceforge.net/) file that captures
 # the structure of the code including all documentation. Note that this feature
 # is still experimental and incomplete at the moment.
 # The default value is: NO.
@@ -2311,28 +2187,6 @@ GENERATE_AUTOGEN_DEF   = NO
 # Configuration options related to Sqlite3 output
 #---------------------------------------------------------------------------
 
-# If the GENERATE_SQLITE3 tag is set to YES doxygen will generate a Sqlite3
-# database with symbols found by doxygen stored in tables.
-# The default value is: NO.
-
-GENERATE_SQLITE3       = NO
-
-# The SQLITE3_OUTPUT tag is used to specify where the Sqlite3 database will be
-# put. If a relative path is entered the value of OUTPUT_DIRECTORY will be put
-# in front of it.
-# The default directory is: sqlite3.
-# This tag requires that the tag GENERATE_SQLITE3 is set to YES.
-
-SQLITE3_OUTPUT         = sqlite3
-
-# The SQLITE3_OVERWRITE_DB tag is set to YES, the existing doxygen_sqlite3.db
-# database file will be recreated with each doxygen run. If set to NO, doxygen
-# will warn if an a database file is already found and not modify it.
-# The default value is: YES.
-# This tag requires that the tag GENERATE_SQLITE3 is set to YES.
-
-SQLITE3_RECREATE_DB    = YES
-
 #---------------------------------------------------------------------------
 # Configuration options related to the Perl module output
 #---------------------------------------------------------------------------
@@ -2407,8 +2261,7 @@ SEARCH_INCLUDES        = YES
 
 # The INCLUDE_PATH tag can be used to specify one or more directories that
 # contain include files that are not input files but should be processed by the
-# preprocessor. Note that the INCLUDE_PATH is not recursive, so the setting of
-# RECURSIVE has no effect here.
+# preprocessor.
 # This tag requires that the tag SEARCH_INCLUDES is set to YES.
 
 INCLUDE_PATH           =
@@ -2475,15 +2328,15 @@ TAGFILES               =
 
 GENERATE_TAGFILE       =
 
-# If the ALLEXTERNALS tag is set to YES, all external classes and namespaces
-# will be listed in the class and namespace index. If set to NO, only the
-# inherited external classes will be listed.
+# If the ALLEXTERNALS tag is set to YES, all external class will be listed in
+# the class index. If set to NO, only the inherited external classes will be
+# listed.
 # The default value is: NO.
 
 ALLEXTERNALS           = NO
 
 # If the EXTERNAL_GROUPS tag is set to YES, all external groups will be listed
-# in the topic index. If set to NO, only the current project's groups will be
+# in the modules index. If set to NO, only the current project's groups will be
 # listed.
 # The default value is: YES.
 
@@ -2497,9 +2350,25 @@ EXTERNAL_GROUPS        = YES
 EXTERNAL_PAGES         = YES
 
 #---------------------------------------------------------------------------
-# Configuration options related to diagram generator tools
+# Configuration options related to the dot tool
 #---------------------------------------------------------------------------
 
+# If the CLASS_DIAGRAMS tag is set to YES, doxygen will generate a class diagram
+# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to
+# NO turns the diagrams off. Note that this option also works with HAVE_DOT
+# disabled, but it is recommended to install and use dot, since it yields more
+# powerful graphs.
+# The default value is: YES.
+
+CLASS_DIAGRAMS         = YES
+
+# You can include diagrams made with dia in doxygen documentation. Doxygen will
+# then run dia to produce the diagram and insert it in the documentation. The
+# DIA_PATH tag allows you to specify the directory where the dia binary resides.
+# If left empty dia is assumed to be found in the default search path.
+
+DIA_PATH               =
+
 # If set to YES the inheritance and collaboration graphs will hide inheritance
 # and usage relations if the target is undocumented or is not a class.
 # The default value is: YES.
@@ -2508,7 +2377,7 @@ HIDE_UNDOC_RELATIONS   = YES
 
 # If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is
 # available from the path. This tool is part of Graphviz (see:
-# https://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent
+# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent
 # Bell Labs. The other options in this section have no effect if this option is
 # set to NO
 # The default value is: NO.
@@ -2525,73 +2394,49 @@ HAVE_DOT               = NO
 
 DOT_NUM_THREADS        = 0
 
-# DOT_COMMON_ATTR is common attributes for nodes, edges and labels of
-# subgraphs. When you want a differently looking font in the dot files that
-# doxygen generates you can specify fontname, fontcolor and fontsize attributes.
-# For details please see <a href=https://graphviz.org/doc/info/attrs.html>Node,
-# Edge and Graph Attributes specification</a> You need to make sure dot is able
-# to find the font, which can be done by putting it in a standard location or by
-# setting the DOTFONTPATH environment variable or by setting DOT_FONTPATH to the
-# directory containing the font. Default graphviz fontsize is 14.
-# The default value is: fontname=Helvetica,fontsize=10.
+# When you want a differently looking font in the dot files that doxygen
+# generates you can specify the font name using DOT_FONTNAME. You need to make
+# sure dot is able to find the font, which can be done by putting it in a
+# standard location or by setting the DOTFONTPATH environment variable or by
+# setting DOT_FONTPATH to the directory containing the font.
+# The default value is: Helvetica.
 # This tag requires that the tag HAVE_DOT is set to YES.
 
-DOT_COMMON_ATTR        = "fontname=Helvetica,fontsize=10"
+DOT_FONTNAME           = Helvetica
 
-# DOT_EDGE_ATTR is concatenated with DOT_COMMON_ATTR. For elegant style you can
-# add 'arrowhead=open, arrowtail=open, arrowsize=0.5'. <a
-# href=https://graphviz.org/doc/info/arrows.html>Complete documentation about
-# arrows shapes.</a>
-# The default value is: labelfontname=Helvetica,labelfontsize=10.
+# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of
+# dot graphs.
+# Minimum value: 4, maximum value: 24, default value: 10.
 # This tag requires that the tag HAVE_DOT is set to YES.
 
-DOT_EDGE_ATTR          = "labelfontname=Helvetica,labelfontsize=10"
+DOT_FONTSIZE           = 10
 
-# DOT_NODE_ATTR is concatenated with DOT_COMMON_ATTR. For view without boxes
-# around nodes set 'shape=plain' or 'shape=plaintext' <a
-# href=https://www.graphviz.org/doc/info/shapes.html>Shapes specification</a>
-# The default value is: shape=box,height=0.2,width=0.4.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_NODE_ATTR          = "shape=box,height=0.2,width=0.4"
-
-# You can set the path where dot can find font specified with fontname in
-# DOT_COMMON_ATTR and others dot attributes.
+# By default doxygen will tell dot to use the default font as specified with
+# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set
+# the path where dot can find it using this tag.
 # This tag requires that the tag HAVE_DOT is set to YES.
 
 DOT_FONTPATH           =
 
-# If the CLASS_GRAPH tag is set to YES or GRAPH or BUILTIN then doxygen will
-# generate a graph for each documented class showing the direct and indirect
-# inheritance relations. In case the CLASS_GRAPH tag is set to YES or GRAPH and
-# HAVE_DOT is enabled as well, then dot will be used to draw the graph. In case
-# the CLASS_GRAPH tag is set to YES and HAVE_DOT is disabled or if the
-# CLASS_GRAPH tag is set to BUILTIN, then the built-in generator will be used.
-# If the CLASS_GRAPH tag is set to TEXT the direct and indirect inheritance
-# relations will be shown as texts / links.
-# Possible values are: NO, YES, TEXT, GRAPH and BUILTIN.
+# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for
+# each documented class showing the direct and indirect inheritance relations.
+# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO.
 # The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
 
 CLASS_GRAPH            = YES
 
 # If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a
 # graph for each documented class showing the direct and indirect implementation
 # dependencies (inheritance, containment, and class references variables) of the
-# class with other documented classes. Explicit enabling a collaboration graph,
-# when COLLABORATION_GRAPH is set to NO, can be accomplished by means of the
-# command \collaborationgraph. Disabling a collaboration graph can be
-# accomplished by means of the command \hidecollaborationgraph.
+# class with other documented classes.
 # The default value is: YES.
 # This tag requires that the tag HAVE_DOT is set to YES.
 
 COLLABORATION_GRAPH    = YES
 
 # If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for
-# groups, showing the direct groups dependencies. Explicit enabling a group
-# dependency graph, when GROUP_GRAPHS is set to NO, can be accomplished by means
-# of the command \groupgraph. Disabling a directory graph can be accomplished by
-# means of the command \hidegroupgraph. See also the chapter Grouping in the
-# manual.
+# groups, showing the direct groups dependencies.
 # The default value is: YES.
 # This tag requires that the tag HAVE_DOT is set to YES.
 
@@ -2651,9 +2496,7 @@ TEMPLATE_RELATIONS     = NO
 # If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to
 # YES then doxygen will generate a graph for each documented file showing the
 # direct and indirect include dependencies of the file with other documented
-# files. Explicit enabling an include graph, when INCLUDE_GRAPH is is set to NO,
-# can be accomplished by means of the command \includegraph. Disabling an
-# include graph can be accomplished by means of the command \hideincludegraph.
+# files.
 # The default value is: YES.
 # This tag requires that the tag HAVE_DOT is set to YES.
 
@@ -2662,10 +2505,7 @@ INCLUDE_GRAPH          = YES
 # If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are
 # set to YES then doxygen will generate a graph for each documented file showing
 # the direct and indirect include dependencies of the file with other documented
-# files. Explicit enabling an included by graph, when INCLUDED_BY_GRAPH is set
-# to NO, can be accomplished by means of the command \includedbygraph. Disabling
-# an included by graph can be accomplished by means of the command
-# \hideincludedbygraph.
+# files.
 # The default value is: YES.
 # This tag requires that the tag HAVE_DOT is set to YES.
 
@@ -2705,26 +2545,16 @@ GRAPHICAL_HIERARCHY    = YES
 # If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the
 # dependencies a directory has on other directories in a graphical way. The
 # dependency relations are determined by the #include relations between the
-# files in the directories. Explicit enabling a directory graph, when
-# DIRECTORY_GRAPH is set to NO, can be accomplished by means of the command
-# \directorygraph. Disabling a directory graph can be accomplished by means of
-# the command \hidedirectorygraph.
+# files in the directories.
 # The default value is: YES.
 # This tag requires that the tag HAVE_DOT is set to YES.
 
 DIRECTORY_GRAPH        = YES
 
-# The DIR_GRAPH_MAX_DEPTH tag can be used to limit the maximum number of levels
-# of child directories generated in directory dependency graphs by dot.
-# Minimum value: 1, maximum value: 25, default value: 1.
-# This tag requires that the tag DIRECTORY_GRAPH is set to YES.
-
-DIR_GRAPH_MAX_DEPTH    = 1
-
 # The DOT_IMAGE_FORMAT tag can be used to set the image format of the images
 # generated by dot. For an explanation of the image formats see the section
 # output formats in the documentation of the dot tool (Graphviz (see:
-# https://www.graphviz.org/)).
+# http://www.graphviz.org/)).
 # Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order
 # to make the SVG files visible in IE 9+ (other browsers do not have this
 # requirement).
@@ -2761,12 +2591,11 @@ DOT_PATH               =
 
 DOTFILE_DIRS           =
 
-# You can include diagrams made with dia in doxygen documentation. Doxygen will
-# then run dia to produce the diagram and insert it in the documentation. The
-# DIA_PATH tag allows you to specify the directory where the dia binary resides.
-# If left empty dia is assumed to be found in the default search path.
+# The MSCFILE_DIRS tag can be used to specify one or more directories that
+# contain msc files that are included in the documentation (see the \mscfile
+# command).
 
-DIA_PATH               =
+MSCFILE_DIRS           =
 
 # The DIAFILE_DIRS tag can be used to specify one or more directories that
 # contain dia files that are included in the documentation (see the \diafile
@@ -2775,10 +2604,10 @@ DIA_PATH               =
 DIAFILE_DIRS           =
 
 # When using plantuml, the PLANTUML_JAR_PATH tag should be used to specify the
-# path where java can find the plantuml.jar file or to the filename of jar file
-# to be used. If left blank, it is assumed PlantUML is not used or called during
-# a preprocessing step. Doxygen will generate a warning when it encounters a
-# \startuml command in this case and will not generate output for the diagram.
+# path where java can find the plantuml.jar file. If left blank, it is assumed
+# PlantUML is not used or called during a preprocessing step. Doxygen will
+# generate a warning when it encounters a \startuml command in this case and
+# will not generate output for the diagram.
 
 PLANTUML_JAR_PATH      =
 
@@ -2816,6 +2645,18 @@ DOT_GRAPH_MAX_NODES    = 50
 
 MAX_DOT_GRAPH_DEPTH    = 0
 
+# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent
+# background. This is disabled by default, because dot on Windows does not seem
+# to support this out of the box.
+#
+# Warning: Depending on the platform used, enabling this option may lead to
+# badly anti-aliased labels on the edges of a graph (i.e. they become hard to
+# read).
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_TRANSPARENT        = NO
+
 # Set the DOT_MULTI_TARGETS tag to YES to allow dot to generate multiple output
 # files in one run (i.e. multiple -o and -T options on the command line). This
 # makes dot run faster, but since only newer versions of dot (>1.8.10) support
@@ -2828,8 +2669,6 @@ DOT_MULTI_TARGETS      = NO
 # If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page
 # explaining the meaning of the various boxes and arrows in the dot generated
 # graphs.
-# Note: This tag requires that UML_LOOK isn't set, i.e. the doxygen internal
-# graphical representation for inheritance and collaboration diagrams is used.
 # The default value is: YES.
 # This tag requires that the tag HAVE_DOT is set to YES.
 
@@ -2843,19 +2682,3 @@ GENERATE_LEGEND        = YES
 # The default value is: YES.
 
 DOT_CLEANUP            = YES
-
-# You can define message sequence charts within doxygen comments using the \msc
-# command. If the MSCGEN_TOOL tag is left empty (the default), then doxygen will
-# use a built-in version of mscgen tool to produce the charts. Alternatively,
-# the MSCGEN_TOOL tag can also specify the name an external tool. For instance,
-# specifying prog as the value, doxygen will call the tool as prog -T
-# <outfile_format> -o <outputfile> <inputfile>. The external tool should support
-# output file formats "png", "eps", "svg", and "ismap".
-
-MSCGEN_TOOL            =
-
-# The MSCFILE_DIRS tag can be used to specify one or more directories that
-# contain msc files that are included in the documentation (see the \mscfile
-# command).
-
-MSCFILE_DIRS           =

ControlCore/LinearAlgebra/FloatSingle.h

@@ -5,18 +5,19 @@
 #ifndef FLOAT_H
 #define FLOAT_H
 
+namespace Passer {
 namespace LinearAlgebra {
 
 class Float {
- public:
+public:
   static const float epsilon;
   static const float sqrEpsilon;
 
   static float Clamp(float f, float min, float max);
 };
 
-}  // namespace LinearAlgebra
-
-using namespace LinearAlgebra;
+} // namespace LinearAlgebra
+} // namespace Passer
+using namespace Passer::LinearAlgebra;
 
 #endif

ControlCore/LinearAlgebra/Matrix.cpp

@@ -0,0 +1,62 @@
+#include "Matrix.h"
+
+template <> MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
+  if (rows <= 0 || cols <= 0) {
+    this->rows = 0;
+    this->cols = 0;
+    this->data = nullptr;
+    return;
+  }
+  this->rows = rows;
+  this->cols = cols;
+
+  unsigned int matrixSize = this->cols * this->rows;
+  this->data = new float[matrixSize]{0.0f};
+}
+
+template <> 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,
+                               const MatrixOf<float> *m2, 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;
+      r->data[rDataIx] = 0.0F;
+      for (unsigned int kIx = 0; kIx < m2->rows; kIx++) {
+        unsigned int dataIx1 = rowIx1 * m1->cols + kIx;
+        unsigned int dataIx2 = kIx * m2->cols + colIx2;
+        r->data[rDataIx] += m1->data[dataIx1] * m2->data[dataIx2];
+      }
+    }
+  }
+}
+
+template <>
+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);
+
+  Multiply(m, &v_m, &r_m);
+
+  Vector3 r = Vector3(r_m.data[0], r_m.data[1], r_m.data[2]);
+  return r;
+}
+
+template <typename T> Vector3 MatrixOf<T>::operator*(const Vector3 v) const {
+  float *vData = new float[3]{v.x, v.y, v.z};
+  MatrixOf<float> v_m = MatrixOf<float>(3, 1, vData);
+  float *rData = new float[3]{};
+  MatrixOf<float> r_m = MatrixOf<float>(3, 1, rData);
+
+  Multiply(this, &v_m, &r_m);
+
+  Vector3 r = Vector3(r_m.data[0], r_m.data[1], r_m.data[2]);
+  delete[] vData;
+  delete[] rData;
+  return r;
+}

ControlCore/LinearAlgebra/Matrix.h

@@ -0,0 +1,121 @@
+#ifndef MATRIX_H
+#define MATRIX_H
+
+#include "Vector3.h"
+
+namespace Passer {
+namespace LinearAlgebra {
+
+/// @brief Single precision float matrix
+template <typename T> class MatrixOf {
+public:
+  MatrixOf(unsigned int rows, unsigned int cols);
+  MatrixOf(unsigned int rows, unsigned int cols, const T *source)
+      : MatrixOf(rows, cols) {
+    Set(source);
+  }
+  MatrixOf(Vector3 v); // creates a 3,1 matrix
+
+  ~MatrixOf() {
+    if (this->data == nullptr)
+      return;
+
+    delete[] this->data;
+  }
+
+  /// @brief Transpose with result in matrix m
+  /// @param r The matrix in which the transposed matrix is stored
+  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
+    // We cannot check the data size directly, but the row*col should be equal
+    unsigned int matrixSize = this->cols * this->rows;
+    unsigned int resultSize = r->rows * r->cols;
+    if (matrixSize != resultSize) {
+      // Return a null matrix;
+      // We dont set data to nullptr because it is allocated memory
+      // Instead we write all zeros
+      for (unsigned int dataIx = 0; dataIx < resultSize; dataIx++)
+        r->data[dataIx] = 0.0f;
+      r->rows = 0;
+      r->cols = 0;
+      return;
+    }
+
+    r->cols = this->rows;
+    r->rows = this->cols;
+
+    for (unsigned int rDataIx = 0; rDataIx < matrixSize; rDataIx++) {
+      unsigned int rowIx = rDataIx / this->rows;
+      unsigned int colIx = rDataIx % this->rows;
+      unsigned int mDataIx = this->cols * colIx + rowIx;
+      r->data[rDataIx] = this->data[mDataIx];
+    }
+  }
+
+  static void Multiply(const MatrixOf<T> *m1, const MatrixOf<T> *m2,
+                       MatrixOf<T> *r);
+  void Multiply(const MatrixOf<T> *m, MatrixOf<T> *r) const {
+    Multiply(this, m, r);
+  }
+
+  static Vector3 Multiply(const MatrixOf<T> *m, Vector3 v);
+  Vector3 operator*(const Vector3 v) const;
+
+  T Get(unsigned int rowIx, unsigned int colIx) const {
+    unsigned int dataIx = rowIx * this->cols + colIx;
+    return this->data[dataIx];
+  }
+
+  void Set(unsigned int rowIx, unsigned int colIx, T value) {
+    unsigned int dataIx = rowIx * this->cols + colIx;
+    this->data[dataIx] = value;
+  }
+
+  // This function does not check on source size!
+  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) {
+    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) {
+    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) {
+    unsigned int thisMatrixSize = this->cols * this->rows;
+    unsigned int mMatrixSize = m->cols * m->rows;
+    if (mMatrixSize != thisMatrixSize)
+      return;
+
+    for (unsigned int dataIx = 0; dataIx < thisMatrixSize; dataIx++)
+      this->data[dataIx] = m->data[dataIx];
+  }
+
+  unsigned int RowCount() const { return rows; }
+  unsigned int ColCount() const { return cols; }
+
+private:
+  unsigned int rows;
+  unsigned int cols;
+  T *data;
+};
+
+} // namespace LinearAlgebra
+} // namespace Passer
+using namespace Passer::LinearAlgebra;
+
+#endif

ControlCore/LinearAlgebra/Polar.cpp

@@ -3,13 +3,11 @@
 #include "Polar.h"
 #include "Vector2.h"
 
-template <typename T>
-PolarOf<T>::PolarOf() {
+template <typename T> PolarOf<T>::PolarOf() {
   this->distance = 0.0f;
   this->angle = AngleOf<T>();
 }
-template <typename T>
-PolarOf<T>::PolarOf(float distance, AngleOf<T> angle) {
+template <typename T> 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>
-PolarOf<T> PolarOf<T>::FromVector2(Vector2 v) {
+template <typename T> 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>
-PolarOf<T> PolarOf<T>::FromSpherical(SphericalOf<T> v) {
-  float distance =
-      v.distance * cosf(v.direction.vertical.InDegrees() * Deg2Rad);
+template <typename T> PolarOf<T> PolarOf<T>::FromSpherical(SphericalOf<T> v) {
+  float distance = v.distance * cosf(v.direction.vertical.InDegrees() *
+                                     Passer::LinearAlgebra::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>
-bool PolarOf<T>::operator==(const PolarOf& v) const {
+template <typename T> bool PolarOf<T>::operator==(const PolarOf &v) const {
   return (this->distance == v.distance &&
           this->angle.InDegrees() == v.angle.InDegrees());
 }
 
-template <typename T>
-PolarOf<T> PolarOf<T>::Normalize(const PolarOf& v) {
+template <typename T> PolarOf<T> PolarOf<T>::Normalize(const PolarOf &v) {
   PolarOf<T> r = PolarOf(1, v.angle);
   return r;
 }
-template <typename T>
-PolarOf<T> PolarOf<T>::normalized() const {
+template <typename T> PolarOf<T> PolarOf<T>::normalized() const {
   PolarOf<T> r = PolarOf(1, this->angle);
   return r;
 }
 
-template <typename T>
-PolarOf<T> PolarOf<T>::operator-() const {
+template <typename T> PolarOf<T> PolarOf<T>::operator-() const {
   PolarOf<T> v =
       PolarOf(this->distance, this->angle + AngleOf<T>::Degrees(180));
   return v;
 }
 
-template <typename T>
-PolarOf<T> PolarOf<T>::operator-(const PolarOf& v) const {
+template <typename T> PolarOf<T> PolarOf<T>::operator-(const PolarOf &v) const {
   PolarOf<T> r = -v;
   return *this + r;
 }
-template <typename T>
-PolarOf<T> PolarOf<T>::operator-=(const PolarOf& v) {
+template <typename T> 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>
-PolarOf<T> PolarOf<T>::operator+(const PolarOf& v) const {
+template <typename T> 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>
-PolarOf<T> PolarOf<T>::operator+=(const PolarOf& v) {
+template <typename T> PolarOf<T> PolarOf<T>::operator+=(const PolarOf &v) {
   *this = *this + v;
   return *this;
 }
 
-template <typename T>
-PolarOf<T> PolarOf<T>::operator*=(float f) {
+template <typename T> PolarOf<T> PolarOf<T>::operator*=(float f) {
   this->distance *= f;
   return *this;
 }
-template <typename T>
-PolarOf<T> PolarOf<T>::operator/=(float f) {
+template <typename T> 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 LinearAlgebra::PolarOf<signed short>;
+template class PolarOf<float>;
+template class PolarOf<signed short>;

ControlCore/LinearAlgebra/Polar.h

@@ -7,17 +7,14 @@
 
 #include "Angle.h"
 
+namespace Passer {
 namespace LinearAlgebra {
 
 struct Vector2;
-template <typename T>
-class SphericalOf;
+template <typename T> 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>
-class PolarOf {
- public:
+template <typename T> class PolarOf {
+public:
   /// @brief The distance in meters
   /// @remark The distance shall never be negative
   float distance;
@@ -77,12 +74,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 +87,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 +100,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);
+  PolarOf operator-(const PolarOf &v) const;
+  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);
+  PolarOf operator+(const PolarOf &v) const;
+  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 +125,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,21 +137,22 @@ 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>;
 using Polar16 = PolarOf<signed short>;
 // using Polar = PolarSingle;
 
-}  // namespace LinearAlgebra
-using namespace LinearAlgebra;
+} // namespace LinearAlgebra
+} // namespace Passer
+using namespace Passer::LinearAlgebra;
 
 #include "Spherical.h"
 #include "Vector2.h"

ControlCore/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,

ControlCore/LinearAlgebra/Quaternion.h

@@ -7,9 +7,12 @@
 
 #include "Vector3.h"
 
+namespace Passer {
+namespace LinearAlgebra {
+
 extern "C" {
 /// <summary>
-/// A quaternion (C-style)
+/// A quaternion
 /// </summary>
 /// This is a C-style implementation
 typedef struct Quat {
@@ -32,10 +35,6 @@ typedef struct Quat {
 } Quat;
 }
 
-namespace LinearAlgebra {
-
-class Matrix2;
-
 /// <summary>
 /// A quaternion
 /// </summary>
@@ -91,8 +90,6 @@ struct Quaternion : Quat {
   /// The euler angles performed in the order: Z, X, Y
   static Vector3 ToAngles(const Quaternion& q);
 
-  Matrix2 ToRotationMatrix();
-
   /// <summary>
   /// Rotate a vector using this quaterion
   /// </summary>
@@ -292,6 +289,7 @@ struct Quaternion : Quat {
 };
 
 }  // namespace LinearAlgebra
-using namespace LinearAlgebra;
+}  // namespace Passer
+using namespace Passer::LinearAlgebra;
 
 #endif

ControlCore/LinearAlgebra/README.md

@@ -0,0 +1,12 @@
+\mainpage Vector Algebra
+
+Vector algebra library
+
+Main components
+---------------
+* [Vector3](struct_passer_1_1_linear_algebra_1_1_vector3.html)
+* [Quaternion](struct_passer_1_1_linear_algebra_1_1_quaternion.html)
+
+* [Vector2](https://serrarens.nl/apis/LinearAlgebra/struct_vector2.html)
+
+* [Polar](https://passervr.com/apis/LinearAlgebra/struct_polar.html)

ControlCore/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,
-                            AngleOf<T> horizontal,
+SphericalOf<T>::SphericalOf(float distance, 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,
-                                       float horizontal,
+SphericalOf<T> SphericalOf<T>::Degrees(float distance, 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,
-                                       float horizontal,
+SphericalOf<T> SphericalOf<T>::Radians(float distance, 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>
-SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
+template <typename T> 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>
-Vector3 SphericalOf<T>::ToVector3() const {
+template <typename T> Vector3 SphericalOf<T>::ToVector3() const {
   float verticalRad = (pi / 2) - this->direction.vertical.InRadians();
   float horizontalRad = this->direction.horizontal.InRadians();
 
@@ -131,11 +123,10 @@ const SphericalOf<T> SphericalOf<T>::down =
 template <typename T>
 SphericalOf<T> SphericalOf<T>::WithDistance(float distance) {
   SphericalOf<T> v = SphericalOf<T>(distance, this->direction);
-  return v;
+  return SphericalOf<T>();
 }
 
-template <typename T>
-SphericalOf<T> SphericalOf<T>::operator-() const {
+template <typename T> 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>
-SphericalOf<T> SphericalOf<T>::operator*=(float f) {
+template <typename T> SphericalOf<T> SphericalOf<T>::operator*=(float f) {
   this->distance *= f;
   return *this;
 }
 
-template <typename T>
-SphericalOf<T> SphericalOf<T>::operator/=(float f) {
+template <typename T> 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,
-                                      const SphericalOf<T>& v2) {
+float SphericalOf<T>::DistanceBetween(const SphericalOf<T> &v1,
+                                      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,
-                                        const SphericalOf& v2) {
+AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf &v1,
+                                        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,
-                                              const SphericalOf<T>& v2,
-                                              const SphericalOf<T>& axis) {
+AngleOf<T> Passer::LinearAlgebra::SphericalOf<T>::SignedAngleBetween(
+    const SphericalOf<T> &v1, const SphericalOf<T> &v2,
+    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,14 +276,14 @@ 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);
@@ -303,5 +292,3 @@ SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T>& v,
 
 template class SphericalOf<float>;
 template class SphericalOf<signed short>;
-
-}  // namespace LinearAlgebra

ControlCore/LinearAlgebra/Spherical.h

@@ -7,26 +7,28 @@
 
 #include "Direction.h"
 
+namespace Passer {
 namespace LinearAlgebra {
 
 struct Vector3;
-template <typename T>
-class PolarOf;
+template <typename T> 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>
-class SphericalOf {
- public:
+template <typename T> class SphericalOf {
+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(float distance, AngleOf<T> horizontal, AngleOf<T> vertical);
-  SphericalOf(float distance, DirectionOf<T> direction);
+  SphericalOf<T>();
+  SphericalOf<T>(float distance, AngleOf<T> horizontal, AngleOf<T> vertical);
+  SphericalOf<T>(float distance, DirectionOf<T> direction);
 
   /// @brief Create spherical vector without using AngleOf type. All given
   /// angles are in degrees
@@ -34,8 +36,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,
-                                float horizontal,
+  static SphericalOf<T> Degrees(float distance, float horizontal,
                                 float vertical);
   /// @brief Short-hand Deg alias for the Degrees function
   constexpr static auto Deg = Degrees;
@@ -45,8 +46,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,
-                                float horizontal,
+  static SphericalOf<T> Radians(float distance, float horizontal,
                                 float vertical);
   // Short-hand Rad alias for the Radians function
   constexpr static auto Rad = Radians;
@@ -93,23 +93,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);
+  SphericalOf<T> operator-(const SphericalOf<T> &v) const;
+  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);
+  SphericalOf<T> operator+(const SphericalOf<T> &v) const;
+  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 +118,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 +130,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,
-                               const SphericalOf<T>& v2);
+  static float DistanceBetween(const SphericalOf<T> &v1,
+                               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,
-                                 const SphericalOf<T>& v2);
+  static AngleOf<T> AngleBetween(const SphericalOf<T> &v1,
+                                 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,
-                                       const SphericalOf<T>& v2,
-                                       const SphericalOf<T>& axis);
+  static AngleOf<T> SignedAngleBetween(const SphericalOf<T> &v1,
+                                       const SphericalOf<T> &v2,
+                                       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,
-                               AngleOf<T> horizontalAngle,
+  static SphericalOf<T> Rotate(const SphericalOf &v, 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);
 };
 
@@ -179,13 +178,9 @@ using SphericalSingle = SphericalOf<float>;
 /// hardware
 using Spherical16 = SphericalOf<signed short>;
 
-#if defined(ARDUINO)
-using Spherical = Spherical16;
-#else
-using Spherical = SphericalSingle;
-#endif
-
-}  // namespace LinearAlgebra
+} // namespace LinearAlgebra
+} // namespace Passer
+using namespace Passer::LinearAlgebra;
 
 #include "Polar.h"
 #include "Vector3.h"

ControlCore/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>());
@@ -167,6 +165,4 @@ void SwingTwistOf<T>::Normalize() {
 }
 
 template class SwingTwistOf<float>;
-template class SwingTwistOf<signed short>;
-
-}
+template class SwingTwistOf<signed short>;

ControlCore/LinearAlgebra/SwingTwist.h

@@ -10,20 +10,18 @@
 #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>
 class SwingTwistOf {
  public:
   DirectionOf<T> swing;
   AngleOf<T> twist;
 
-  SwingTwistOf();
-  SwingTwistOf(DirectionOf<T> swing, AngleOf<T> twist);
-  SwingTwistOf(AngleOf<T> horizontal, AngleOf<T> vertical, AngleOf<T> twist);
+  SwingTwistOf<T>();
+  SwingTwistOf<T>(DirectionOf<T> swing, AngleOf<T> twist);
+  SwingTwistOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical, AngleOf<T> twist);
 
   static SwingTwistOf<T> Degrees(float horizontal,
                                  float vertical = 0,
@@ -73,13 +71,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

ControlCore/LinearAlgebra/Vector2.cpp

@@ -26,11 +26,11 @@ Vector2::Vector2(float _x, float _y) {
 //   y = v.y;
 // }
 Vector2::Vector2(Vector3 v) {
-  x = v.Right();    // x;
-  y = v.Forward();  // z;
+  x = v.Right();   // x;
+  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 {
-  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::magnitude() const { 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); }
 
-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-() {
-  return Vector2(-this->x, -this->y);
-}
+Vector2 Vector2::operator-() { 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,14 +148,15 @@ 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) {
-  float angleRad = a.InDegrees() * Deg2Rad;
+Vector2 Vector2::Rotate(const Vector2 &v,
+                        Passer::LinearAlgebra::AngleSingle a) {
+  float angleRad = a.InDegrees() * Passer::LinearAlgebra::Deg2Rad;
 #if defined(AVR)
   float sinValue = sin(angleRad);
-  float cosValue = cos(angleRad);  // * Angle::Deg2Rad);
+  float cosValue = cos(angleRad); // * Angle::Deg2Rad);
 #else
   float sinValue = (float)sinf(angleRad);
   float cosValue = (float)cosf(angleRad);
@@ -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;
 }

ControlCore/LinearAlgebra/Vector2.h

@@ -9,7 +9,7 @@
 
 extern "C" {
 /// <summary>
-/// 2-dimensional Vector representation (C-style)
+/// 2-dimensional Vector representation
 /// </summary>
 /// This is a C-style implementation
 /// This uses the right-handed coordinate system.
@@ -26,19 +26,20 @@ typedef struct Vec2 {
 } Vec2;
 }
 
+namespace Passer {
 namespace LinearAlgebra {
 
 struct Vector3;
-template <typename T>
-class PolarOf;
+template <typename T> class PolarOf;
+// using Polar = PolarOf<float>
 
-/// @brief A 2-dimensional vector
+/// @brief A 2=dimensional vector
 /// @remark This uses the right=handed carthesian coordinate system.
 /// @note This implementation intentionally avoids the use of x and y
 struct Vector2 : Vec2 {
   friend struct Vec2;
 
- public:
+public:
   /// @brief A new 2-dimensional zero vector
   Vector2();
   /// @brief A new 2-dimensional vector
@@ -80,12 +81,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 +96,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 +107,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 +119,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);
+  Vector2 operator-(const Vector2 &v) const;
+  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);
+  Vector2 operator+(const Vector2 &v) const;
+  Vector2 operator+=(const Vector2 &v);
 
   /// @brief Scale the vector using another vector
   /// @param v1 The vector to scale
@@ -132,16 +133,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 +151,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 +163,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 +178,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 +199,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 LinearAlgebra
+} // namespace Passer
+using namespace Passer::LinearAlgebra;
 
 #include "Polar.h"
 

ControlCore/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 horizontalRad = s.direction.horizontal.InDegrees() * Deg2Rad;
+  float verticalRad = (90.0f - s.direction.vertical.InDegrees()) *
+                      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 {
-  return (float)sqrtf(x * x + y * y + z * z);
-}
+float Vector3::magnitude() const { 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 {
-  return (x * x + y * y + z * z);
-}
+float Vector3::sqrMagnitude() const { 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>();
@@ -195,16 +193,15 @@ AngleOf<float> Vector3::Angle(const Vector3& v1, const Vector3& v2) {
   float fraction = dot / denominator;
   if (isnan(fraction))
     return AngleOf<float>::Degrees(
-        fraction);  // short cut to returning NaN universally
+        fraction); // short cut to returning NaN universally
 
   float cdot = clamp(fraction, -1.0, 1.0);
   float r = ((float)acos(cdot));
   return AngleOf<float>::Radians(r);
 }
 
-AngleOf<float> Vector3::SignedAngle(const Vector3& v1,
-                                    const Vector3& v2,
-                                    const Vector3& axis) {
+AngleOf<float> Vector3::SignedAngle(const Vector3 &v1, const Vector3 &v2,
+                                    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;
 }

ControlCore/LinearAlgebra/Vector3.h

@@ -7,14 +7,20 @@
 
 #include "Vector2.h"
 
+namespace Passer {
+namespace LinearAlgebra {
+
+// struct Spherical;
+template <typename T> class SphericalOf;
+
 extern "C" {
 /// <summary>
-/// 3-dimensional Vector representation (C-style)
+/// 3-dimensional Vector representation
 /// </summary>
 /// 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,18 +37,13 @@ typedef struct Vec3 {
 } Vec3;
 }
 
-namespace LinearAlgebra {
-
-template <typename T>
-class SphericalOf;
-
 /// @brief A 3-dimensional vector
 /// @remark This uses a right-handed carthesian coordinate system.
 /// @note This implementation intentionally avoids the use of x, y and z values.
 struct Vector3 : Vec3 {
   friend struct Vec3;
 
- public:
+public:
   /// @brief A new 3-dimensional zero vector
   Vector3();
   /// @brief A new 3-dimensional vector
@@ -88,12 +89,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 +104,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 +115,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 +127,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);
+  Vector3 operator-(const Vector3 &v) const;
+  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);
+  Vector3 operator+(const Vector3 &v) const;
+  Vector3 operator+=(const Vector3 &v);
 
   /// @brief Scale the vector using another vector
   /// @param v1 The vector to scale
@@ -140,16 +141,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 +159,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 +172,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 +205,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,
-                                    const Vector3& v2,
-                                    const Vector3& axis);
+  static AngleOf<float> SignedAngle(const Vector3 &v1, const Vector3 &v2,
+                                    const Vector3 &axis);
 
   /// @brief Lerp (linear interpolation) between two vectors
   /// @param v1 The starting vector
@@ -222,11 +222,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 LinearAlgebra
+} // namespace Passer
+using namespace Passer::LinearAlgebra;
 
 #include "Spherical.h"
 

ControlCore/LinearAlgebra/test/Angle16_test.cc

@@ -1,13 +1,11 @@
 #if GTEST
-#include "gtest/gtest.h"
+#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) {
@@ -88,7 +86,7 @@ TEST(Angle16, Normalize) {
   r = Angle16::Normalize(Angle16::Degrees(0));
   EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0";
 
-  if (false) {  // std::numeric_limits<float>::is_iec559) {
+  if (false) { // std::numeric_limits<float>::is_iec559) {
     // Infinites are not supported
     r = Angle16::Normalize(Angle16::Degrees(FLOAT_INFINITY));
     EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) << "Normalize INFINITY";
@@ -127,7 +125,7 @@ TEST(Angle16, Clamp) {
                      Angle16::Degrees(-10));
   EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 10 -10";
 
-  if (false) {  // std::numeric_limits<float>::is_iec559) {
+  if (false) { // std::numeric_limits<float>::is_iec559) {
     // Infinites are not supported
     r = Angle16::Clamp(Angle16::Degrees(10), Angle16::Degrees(0),
                        Angle16::Degrees(FLOAT_INFINITY));
@@ -218,7 +216,7 @@ TEST(Angle16, MoveTowards) {
   r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(0), 30);
   EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 30";
 
-  if (false) {  // std::numeric_limits<float>::is_iec559) {
+  if (false) { // std::numeric_limits<float>::is_iec559) {
     // infinites are not supported
     r = Angle16::MoveTowards(Angle16::Degrees(0), Angle16::Degrees(90),
                              FLOAT_INFINITY);

ControlCore/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) {
@@ -88,7 +86,7 @@ TEST(Angle8, Normalize) {
   r = Angle8::Normalize(Angle8::Degrees(0));
   EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0";
 
-  if (false) {  // std::numeric_limits<float>::is_iec559) {
+  if (false) { // std::numeric_limits<float>::is_iec559) {
     // Infinites are not supported
     r = Angle8::Normalize(Angle8::Degrees(FLOAT_INFINITY));
     EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) << "Normalize INFINITY";
@@ -126,7 +124,7 @@ TEST(Angle8, Clamp) {
                     Angle8::Degrees(-10));
   EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 10 -10";
 
-  if (false) {  // std::numeric_limits<float>::is_iec559) {
+  if (false) { // std::numeric_limits<float>::is_iec559) {
     // Infinites are not supported
     r = Angle8::Clamp(Angle8::Degrees(10), Angle8::Degrees(0),
                       Angle8::Degrees(FLOAT_INFINITY));
@@ -217,7 +215,7 @@ TEST(Angle8, MoveTowards) {
   r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(0), 30);
   EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 30";
 
-  if (false) {  // std::numeric_limits<float>::is_iec559) {
+  if (false) { // std::numeric_limits<float>::is_iec559) {
     // infinites are not supported
     r = Angle8::MoveTowards(Angle8::Degrees(0), Angle8::Degrees(90),
                             FLOAT_INFINITY);

ControlCore/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) {

ControlCore/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) {

ControlCore/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);

ControlCore/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"

ControlCore/LinearAlgebra/test/Quaternion_test.cc

@@ -36,8 +36,7 @@ TEST(Quaternion, ToAngles) {
 	q1 = Quaternion(1, 0, 0, 0);
 	v = Quaternion::ToAngles(q1);
 	r = v == Vector3(180, 0, 0);
-	// EXPECT_TRUE(r) << "Quaternion::ToAngles 1 0 0 0";
-	// fails on MacOS?
+	EXPECT_TRUE(r) << "Quaternion::ToAngles 1 0 0 0";
 }
 
 TEST(Quaternion, Multiplication) {

ControlCore/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"

ControlCore/LinearAlgebra/test/SphericalSingle_test.cc

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

ControlCore/LowLevelMessages.cpp

@@ -1,18 +1,14 @@
 #include "LowLevelMessages.h"
 
-// #include <iostream>
-#include "LinearAlgebra/float16.h"
+#include "float16.h"
 
-namespace RoboidControl {
-
-void LowLevelMessages::SendAngle8(char* buffer,
-                                  unsigned char* ix,
+void LowLevelMessages::SendAngle8(unsigned char *buffer, unsigned char *ix,
                                   const float angle) {
   Angle8 packedAngle2 = Angle8::Degrees(angle);
   buffer[(*ix)++] = packedAngle2.GetBinary();
 }
-Angle8 LowLevelMessages::ReceiveAngle8(const char* buffer,
-                                       unsigned char* startIndex) {
+Angle8 LowLevelMessages::ReceiveAngle8(const unsigned char *buffer,
+                                       unsigned char *startIndex) {
   unsigned char binary = buffer[(*startIndex)++];
 
   Angle8 angle = Angle8::Binary(binary);
@@ -20,8 +16,7 @@ Angle8 LowLevelMessages::ReceiveAngle8(const char* buffer,
   return angle;
 }
 
-void LowLevelMessages::SendFloat16(char* buffer,
-                                   unsigned char* ix,
+void LowLevelMessages::SendFloat16(unsigned char *buffer, unsigned char *ix,
                                    float value) {
   float16 value16 = float16(value);
   short binary = value16.getBinary();
@@ -29,12 +24,10 @@ void LowLevelMessages::SendFloat16(char* buffer,
   buffer[(*ix)++] = (binary >> 8) & 0xFF;
   buffer[(*ix)++] = binary & 0xFF;
 }
-float LowLevelMessages::ReceiveFloat16(const char* buffer,
-                                       unsigned char* startIndex) {
+float LowLevelMessages::ReceiveFloat16(const unsigned char *buffer,
+                                       unsigned char *startIndex) {
   unsigned char ix = *startIndex;
-  unsigned char msb = buffer[ix++];
-  unsigned char lsb = buffer[ix++];
-  unsigned short value = msb << 8 | lsb;
+  unsigned short value = buffer[ix++] << 8 | buffer[ix++];
   float16 f = float16();
   f.setBinary(value);
 
@@ -42,30 +35,29 @@ float LowLevelMessages::ReceiveFloat16(const char* buffer,
   return (float)f.toFloat();
 }
 
-void LowLevelMessages::SendSpherical(char* buffer,
-                                     unsigned char* ix,
-                                     Spherical s) {
+void LowLevelMessages::SendSpherical16(unsigned char *buffer, unsigned char *ix,
+                                       Spherical16 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,
-                                             unsigned char* startIndex) {
+Spherical16 LowLevelMessages::ReceiveSpherical16(const unsigned char *buffer,
+                                                 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,
-                                  unsigned char* ix,
-                                  SwingTwist rotation) {
+void Passer::Control::LowLevelMessages::SendQuat32(unsigned char *buffer,
+                                                   unsigned char *ix,
+                                                   SwingTwist16 rotation) {
   Quaternion q = rotation.ToQuaternion();
   unsigned char qx = (char)(q.x * 127 + 128);
   unsigned char qy = (char)(q.y * 127 + 128);
@@ -77,23 +69,20 @@ void LowLevelMessages::SendQuat32(char* buffer,
     qz = -qz;
     qw = -qw;
   }
-  // std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw
-  // << "\n";
+  // Serial.printf(" (%d) %d:%d:%d:%d ", startIndex, qx, qy, qz, qw);
   buffer[(*ix)++] = qx;
   buffer[(*ix)++] = qy;
   buffer[(*ix)++] = qz;
   buffer[(*ix)++] = qw;
 }
 
-SwingTwist LowLevelMessages::ReceiveQuat32(const char* buffer,
-                                           unsigned char* ix) {
+SwingTwist16 LowLevelMessages::ReceiveQuat32(const unsigned char *buffer,
+                                             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;
-}
-
-}  // namespace RoboidControl
+}

ControlCore/LowLevelMessages.h

@@ -0,0 +1,32 @@
+#include "LinearAlgebra/Spherical.h"
+#include "LinearAlgebra/SwingTwist.h"
+
+namespace Passer {
+namespace Control {
+
+class LowLevelMessages {
+public:
+  static void SendAngle8(unsigned char *buffer, unsigned char *ix,
+                         const float angle);
+  static Angle8 ReceiveAngle8(const unsigned char *buffer,
+                              unsigned char *startIndex);
+
+  static void SendFloat16(unsigned char *buffer, unsigned char *ix,
+                          float value);
+  static float ReceiveFloat16(const unsigned char *buffer,
+                              unsigned char *startIndex);
+
+  static void SendSpherical16(unsigned char *buffer, unsigned char *ix,
+                              Spherical16 s);
+  static Spherical16 ReceiveSpherical16(const unsigned char *buffer,
+                                        unsigned char *startIndex);
+
+  static void SendQuat32(unsigned char *buffer, unsigned char *ix,
+                         SwingTwist16 q);
+  static SwingTwist16 ReceiveQuat32(const unsigned char *buffer,
+                                    unsigned char *ix);
+};
+
+} // namespace Control
+} // namespace Passer
+using namespace Passer::Control;

ControlCore/Messages.cpp

@@ -0,0 +1,300 @@
+#include "Messages.h"
+
+#include "LowLevelMessages.h"
+#include "Participant.h"
+#include "string.h"
+
+#pragma region IMessage
+
+IMessage::IMessage() {}
+
+// IMessage::IMessage(unsigned char *buffer) { Deserialize(buffer); }
+
+unsigned char IMessage::Serialize(unsigned char *buffer) { return 0; }
+
+// void IMessage::Deserialize(unsigned char *buffer) {}
+
+// bool IMessage::SendMsg(Participant *client, IMessage msg) {
+//   // return SendMsg(client, client.buffer, );nameLength
+//   return client->SendBuffer(msg.Serialize(client->buffer));
+// }
+
+unsigned char *IMessage::ReceiveMsg(unsigned char packetSize) {
+  return nullptr;
+}
+
+bool IMessage::Publish(Participant *participant) {
+  return participant->PublishBuffer(Serialize(participant->buffer));
+}
+bool IMessage::SendTo(Participant *participant) {
+  return participant->SendBuffer(Serialize(participant->buffer));
+}
+
+// IMessage
+#pragma endregion
+
+#pragma region Client
+
+ClientMsg::ClientMsg(unsigned char networkId) { this->networkId = networkId; }
+
+unsigned char ClientMsg::Serialize(unsigned char *buffer) {
+  unsigned char ix = 0;
+  buffer[ix++] = this->id;
+  buffer[ix++] = this->networkId;
+  return ix;
+}
+
+// bool ClientMsg::Send(Participant *participant, unsigned char networkId) {
+//   ClientMsg msg = ClientMsg()
+// }
+// Client Msg
+#pragma endregion
+
+#pragma region Network Id
+
+NetworkIdMsg::NetworkIdMsg(unsigned char *buffer) {
+  this->networkId = buffer[1];
+}
+
+// void NetworkIdMsg::Deserialize(unsigned char *buffer) {
+//   this->networkId = buffer[1];
+// }
+
+NetworkIdMsg NetworkIdMsg::Receive(unsigned char *buffer,
+                                   unsigned char bufferSize) {
+  NetworkIdMsg msg = NetworkIdMsg(buffer);
+  return msg;
+}
+
+// Network Id
+#pragma endregion
+
+#pragma region Investigate
+
+InvestigateMsg::InvestigateMsg(unsigned char *buffer) {
+  unsigned ix = 1; // first byte is msgId
+  this->networkId = buffer[ix++];
+  this->thingId = buffer[ix++];
+}
+InvestigateMsg::InvestigateMsg(unsigned char networkId, unsigned char thingId) {
+  this->networkId = networkId;
+  this->thingId = thingId;
+}
+
+unsigned char InvestigateMsg::Serialize(unsigned char *buffer) {
+  unsigned char ix = 0;
+  buffer[ix++] = this->id;
+  buffer[ix++] = this->networkId;
+  buffer[ix++] = this->thingId;
+  return ix;
+}
+
+// bool InvestigateMsg::Send(Participant *participant, unsigned char networkId,
+//                           unsigned char thingId) {
+//   InvestigateMsg msg = InvestigateMsg(networkId, thingId);
+//   return msg.Send(participant);
+// }
+
+// Investigate
+#pragma endregion
+
+#pragma region Thing
+
+ThingMsg::ThingMsg(const unsigned char *buffer) {
+  unsigned char ix = 1; // first byte is msg id
+  this->networkId = buffer[ix++];
+  this->thingId = buffer[ix++];
+  this->thingType = buffer[ix++];
+  this->parentId = buffer[ix++];
+}
+
+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;
+}
+
+unsigned char ThingMsg::Serialize(unsigned char *buffer) {
+  unsigned char ix = 0;
+  buffer[ix++] = this->id;
+  buffer[ix++] = this->networkId;
+  buffer[ix++] = this->thingId;
+  buffer[ix++] = this->thingType;
+  buffer[ix++] = this->parentId;
+  return ix;
+}
+
+// bool ThingMsg::Send(Participant *participant, unsigned char networkId,
+//                     unsigned char thingId, unsigned char thingType,
+//                     unsigned char parentId) {
+//   ThingMsg msg = ThingMsg(networkId, thingId, thingType, parentId);
+//   return msg.Send(participant);
+// }
+
+// Thing
+#pragma endregion
+
+#pragma region Name
+
+NameMsg::NameMsg(unsigned char networkId, unsigned char thingId,
+                 const char *name, unsigned char nameLength) {
+  this->networkId = networkId;
+  this->thingId = thingId;
+  this->name = name;
+  this->nameLength = nameLength;
+}
+
+unsigned char NameMsg::Serialize(unsigned char *buffer) {
+  unsigned char ix = 0;
+  buffer[ix++] = this->id;
+  buffer[ix++] = this->networkId;
+  buffer[ix++] = this->thingId;
+  buffer[ix++] = this->nameLength;
+  for (int nameIx = 0; nameIx < this->nameLength; nameIx++)
+    buffer[ix++] = this->name[nameIx];
+
+  return ix;
+}
+
+// bool NameMsg::Send(Participant *participant, Thing *thing,
+//                    unsigned char nameLength) {
+//   if (thing->name == nullptr)
+//     return true; // nothing sent, but still a success!
+
+//   if (strlen(thing->name) == 0)
+//     return true;
+
+//   NameMsg msg = NameMsg(thing->networkId, thing->id, thing->name,
+//   nameLength); return msg.Send(participant);
+// }
+
+// Name
+#pragma endregion
+
+#pragma region ModelUrl
+
+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;
+}
+
+unsigned char ModelUrlMsg::Serialize(unsigned char *buffer) {
+  unsigned char ix = 0;
+  buffer[ix++] = this->id;
+  buffer[ix++] = this->networkId;
+  buffer[ix++] = this->thingId;
+  LowLevelMessages::SendFloat16(buffer, &ix, this->scale);
+  buffer[ix++] = this->urlLength;
+  for (int urlIx = 0; urlIx < this->urlLength; urlIx++)
+    buffer[ix++] = url[urlIx];
+  return ix;
+}
+
+// Model Url
+#pragma endregion
+
+#pragma region PoseMsg
+
+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(const unsigned char *buffer) {
+  unsigned char ix = 1; // First byte is msg id
+  this->networkId = buffer[ix++];
+  this->thingId = buffer[ix++];
+  this->poseType = buffer[ix++];
+  this->position = LowLevelMessages::ReceiveSpherical16(buffer, &ix);
+  this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
+}
+
+unsigned char PoseMsg::Serialize(unsigned char *buffer) {
+  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::SendSpherical16(buffer, &ix, this->position);
+  if ((this->poseType & Pose_Orientation) != 0)
+    LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
+  if ((this->poseType & Pose_LinearVelocity) != 0)
+    LowLevelMessages::SendSpherical16(buffer, &ix, this->linearVelocity);
+  if ((this->poseType & Pose_AngularVelocity) != 0)
+    LowLevelMessages::SendSpherical16(buffer, &ix, this->angularVelocity);
+  return ix;
+}
+
+// Pose
+#pragma endregion
+
+#pragma region CustomMsg
+
+CustomMsg::CustomMsg(unsigned char *buffer) {
+  unsigned char ix = 1;
+  this->networkId = buffer[ix++];
+  this->thingId = buffer[ix++];
+  this->data =
+      buffer + ix; // This is only valid because the code ensures the the msg
+                   // lifetime is shorter than the buffer lifetime...
+}
+
+CustomMsg::CustomMsg(unsigned char networkId, Thing *thing) {
+  this->networkId = networkId;
+  this->thingId = thing->id;
+  this->thing = thing;
+}
+
+unsigned char CustomMsg::Serialize(unsigned char *buffer) {
+  unsigned char ix = this->length;
+  this->thing->SendBytes(buffer, &ix);
+  if (ix <= this->length) // in this case, no data is actually sent
+    return 0;
+
+  buffer[0] = this->id;
+  buffer[1] = this->networkId;
+  buffer[2] = this->thingId;
+  return ix;
+}
+
+CustomMsg CustomMsg::Receive(unsigned char *buffer, unsigned char bufferSize) {
+  CustomMsg msg = CustomMsg(buffer);
+  return msg;
+}
+
+// CustomMsg
+#pragma endregion
+
+#pragma region DestroyMsg
+
+DestroyMsg::DestroyMsg(unsigned char networkId, Thing *thing) {
+  this->networkId = networkId;
+  this->thingId = thing->id;
+}
+
+unsigned char DestroyMsg::Serialize(unsigned char *buffer) {
+  unsigned char ix = 0;
+  buffer[ix++] = this->id;
+  buffer[ix++] = this->networkId;
+  buffer[ix++] = this->thingId;
+  return ix;
+}
+
+// DestroyMsg
+#pragma endregion

ControlCore/Messages.h

@@ -0,0 +1,168 @@
+#pragma once
+
+#include "LinearAlgebra/Spherical.h"
+#include "LinearAlgebra/SwingTwist.h"
+#include "Thing.h"
+#include "float16.h"
+
+namespace Passer {
+namespace Control {
+
+class Participant;
+
+class IMessage {
+public:
+  IMessage();
+  virtual unsigned char Serialize(unsigned char *buffer);
+
+  static unsigned char *ReceiveMsg(unsigned char packetSize);
+
+  bool Publish(Participant *participant);
+  bool SendTo(Participant *participant);
+};
+
+class ClientMsg : public IMessage {
+public:
+  static const unsigned char id = 0xA0;
+  unsigned char networkId;
+
+  ClientMsg(unsigned char networkId);
+  virtual unsigned char Serialize(unsigned char *buffer) override;
+};
+
+class NetworkIdMsg : public IMessage {
+public:
+  static const unsigned char id = 0xA1;
+  static const unsigned char length = 2;
+  unsigned char networkId;
+
+  NetworkIdMsg(unsigned char *buffer);
+
+  static NetworkIdMsg Receive(unsigned char *buffer, unsigned char bufferSize);
+};
+
+class InvestigateMsg : public IMessage {
+public:
+  static const unsigned char id = 0x81;
+  static const unsigned char length = 3;
+  unsigned char networkId;
+  unsigned char thingId;
+
+  InvestigateMsg(unsigned char *buffer);
+  InvestigateMsg(unsigned char networkId, unsigned char thingId);
+
+  virtual unsigned char Serialize(unsigned char *buffer) override;
+};
+
+class ThingMsg : public IMessage {
+public:
+  static const unsigned char id = 0x80;
+  static const unsigned char length = 5;
+  unsigned char networkId;
+  unsigned char thingId;
+  unsigned char thingType;
+  unsigned char parentId;
+
+  ThingMsg(const unsigned char *buffer);
+  ThingMsg(unsigned char networkId, unsigned char thingId,
+           unsigned char thingType, unsigned char parentId);
+
+  virtual unsigned char Serialize(unsigned char *buffer) override;
+};
+
+class NameMsg : public IMessage {
+public:
+  static const unsigned char id = 0x91;
+  static const unsigned char length = 4;
+  unsigned char networkId;
+  unsigned char thingId;
+  unsigned char nameLength;
+  const char *name;
+
+  NameMsg(unsigned char networkId, unsigned char thingId, const char *name,
+          unsigned char nameLength);
+
+  virtual unsigned char Serialize(unsigned char *buffer) override;
+};
+
+class ModelUrlMsg : public IMessage {
+public:
+  static const unsigned char id = 0x90;
+
+  unsigned char networkId;
+  unsigned char thingId;
+
+  float scale;
+  unsigned char urlLength;
+  const char *url;
+
+  ModelUrlMsg(unsigned char networkId, unsigned char thingId,
+              unsigned char urlLegth, const char *url, float scale = 1);
+
+  virtual unsigned char Serialize(unsigned char *buffer) override;
+};
+
+class PoseMsg : public IMessage {
+public:
+  static const unsigned char id = 0x10;
+  unsigned char length = 4 + 4 + 4;
+
+  unsigned char networkId;
+  unsigned char thingId;
+
+  unsigned char poseType;
+  static const unsigned char Pose_Position = 0x01;
+  static const unsigned char Pose_Orientation = 0x02;
+  static const unsigned char Pose_LinearVelocity = 0x04;  // For future use
+  static const unsigned char Pose_AngularVelocity = 0x08; // For future use
+
+  Spherical16 position;
+  SwingTwist16 orientation;
+  Spherical16 linearVelocity;
+  Spherical16 angularVelocity;
+
+  PoseMsg(unsigned char networkId, unsigned char thingId,
+          unsigned char poseType, Spherical16 position,
+          SwingTwist16 orientation, Spherical16 linearVelocity = Spherical16(),
+          Spherical16 angularVelocity = Spherical16());
+  PoseMsg(const unsigned char *buffer);
+
+  virtual unsigned char Serialize(unsigned char *buffer) override;
+};
+
+class CustomMsg : public IMessage {
+public:
+  static const unsigned char id = 0xB1;
+  static const unsigned length = 3;
+
+  unsigned char networkId;
+  unsigned char thingId;
+  Thing *thing;
+
+  unsigned char dataSize;
+  unsigned char *data;
+
+  CustomMsg(unsigned char *buffer);
+  CustomMsg(unsigned char networkId, Thing *thing);
+
+  virtual unsigned char Serialize(unsigned char *buffer) override;
+
+  static CustomMsg Receive(unsigned char *buffer, unsigned char bufferSize);
+};
+
+class DestroyMsg : public IMessage {
+public:
+  static const unsigned char id = 0x20;
+  static const unsigned length = 3;
+  unsigned char networkId;
+  unsigned char thingId;
+
+  DestroyMsg(unsigned char networkId, Thing *thing);
+
+  virtual unsigned char Serialize(unsigned char *buffer) override;
+};
+
+} // namespace Control
+} // namespace Passer
+
+using namespace Passer::Control;

ControlCore/Participant.cpp

@@ -0,0 +1,40 @@
+#include "Participant.h"
+
+bool Participant::SendBuffer(unsigned char bufferSize) { return false; }
+
+bool Participant::PublishBuffer(unsigned char bufferSize) { return false; }
+
+void Participant::ReceiveData(unsigned char bufferSize) {
+  unsigned char msgId = this->buffer[0];
+  switch (msgId) {
+  case NetworkIdMsg::id: {
+    // NetworkIdMsg msg = NetworkIdMsg::Receive(this->buffer, bufferSize);
+    NetworkIdMsg msg = NetworkIdMsg(this->buffer);
+    ProcessNetworkIdMsg(msg);
+  } break;
+  case InvestigateMsg::id: {
+    InvestigateMsg msg = InvestigateMsg(this->buffer);
+    ProcessInvestigateMsg(msg);
+  } break;
+  case ThingMsg::id: {
+    ThingMsg msg = ThingMsg(this->buffer);
+    ProcessThingMsg(msg);
+  } break;
+  case PoseMsg::id: {
+    PoseMsg msg = PoseMsg(this->buffer);
+    ProcessPoseMsg(msg);
+  } break;
+  case CustomMsg::id: {
+    CustomMsg msg = CustomMsg(this->buffer);
+    ProcessCustomMsg(msg);
+  } break;
+  };
+}
+
+void Participant::ProcessNetworkIdMsg(NetworkIdMsg msg) {}
+
+void Participant::ProcessInvestigateMsg(InvestigateMsg msg) {}
+
+void Participant::ProcessThingMsg(ThingMsg msg) {}
+
+void Participant::ProcessCustomMsg(CustomMsg msg) {}

ControlCore/Participant.h

@@ -0,0 +1,27 @@
+#pragma once
+
+#include "Messages.h"
+
+namespace Passer {
+namespace Control {
+
+class Participant {
+public:
+  unsigned char buffer[1024];
+
+  virtual bool SendBuffer(unsigned char bufferSize);
+  virtual bool PublishBuffer(unsigned char bufferSize);
+
+  void ReceiveData(unsigned char bufferSize);
+
+protected:
+  virtual void ProcessNetworkIdMsg(NetworkIdMsg msg);
+  virtual void ProcessInvestigateMsg(InvestigateMsg msg);
+  virtual void ProcessThingMsg(ThingMsg msg);
+  virtual void ProcessPoseMsg(PoseMsg msg);
+  virtual void ProcessCustomMsg(CustomMsg msg);
+};
+
+} // namespace Control
+} // namespace Passer
+using namespace Passer::Control;

ControlCore/Thing.cpp

@@ -0,0 +1,195 @@
+#include "Thing.h"
+
+#include "Participant.h"
+#include <iostream>
+#include <string.h>
+
+Thing::Thing(unsigned char networkId, unsigned char thingType) {
+  this->type = thingType;
+  this->networkId = networkId;
+  this->Init();
+
+  int thingId = Thing::Add(this);
+
+  if (thingId < 0) {
+    std::cout << "ERROR: Thing store is full\n";
+    this->id = 0; // what to do when we cannot store any more things?
+  } else
+    this->id = thingId;
+
+  this->linearVelocity = Spherical16::zero;
+  this->angularVelocity = Spherical16::zero;
+}
+
+void Thing::Terminate() { Thing::Remove(this); }
+
+void Thing::Init() {}
+
+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) {
+    Thing *parentThing = this->parent;
+    if (parentThing != nullptr)
+      parentThing->RemoveChild(this);
+    this->parent = nullptr;
+  } else
+    parent->AddChild(this);
+}
+
+void Thing::SetParent(Thing *root, const char *name) {
+  Thing *thing = root->FindThing(name);
+  if (thing != nullptr)
+    this->SetParent(thing);
+}
+
+Thing *Thing::GetParent() { return this->parent; }
+
+void Thing::AddChild(Thing *child) {
+
+  unsigned char newChildCount = this->childCount + 1;
+  Thing **newChildren = new Thing *[newChildCount];
+
+  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
+    newChildren[childIx] = this->children[childIx];
+    if (this->children[childIx] == child) {
+      // child is already present, stop copying do not update the children
+      delete[] newChildren;
+      return;
+    }
+  }
+
+  newChildren[this->childCount] = child;
+  child->parent = this;
+
+  if (this->children != nullptr)
+    delete[] this->children;
+
+  this->children = newChildren;
+  this->childCount = newChildCount;
+}
+
+Thing *Thing::RemoveChild(Thing *child) {
+  unsigned char newChildCount = this->childCount - 1;
+  Thing **newChildren = new Thing *[newChildCount];
+
+  unsigned char newChildIx = 0;
+  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
+    if (this->children[childIx] != child) {
+      if (newChildIx == newChildCount) { // We did not find the child
+        // stop copying and return nothing
+        delete[] newChildren;
+        return nullptr;
+      } else
+        newChildren[newChildIx++] = this->children[childIx];
+    }
+  }
+
+  child->parent = nullptr;
+
+  delete[] this->children;
+  this->children = newChildren;
+  this->childCount = newChildCount;
+
+  return child;
+}
+
+Thing *Passer::Control::Thing::GetChild(unsigned char id, bool recursive) {
+  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
+    Thing *child = this->children[childIx];
+    if (child == nullptr)
+      continue;
+    if (child->id == id)
+      return child;
+
+    if (recursive) {
+      Thing *foundChild = child->GetChild(id, recursive);
+      if (foundChild != nullptr)
+        return foundChild;
+    }
+  }
+  return nullptr;
+}
+
+Thing *Passer::Control::Thing::GetChildByIndex(unsigned char ix) {
+  return this->children[ix];
+}
+
+void Thing::SetModel(const char *url) { this->modelUrl = url; }
+
+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; }
+
+Spherical16 Thing::GetLinearVelocity() { return this->linearVelocity; }
+
+Spherical16 Thing::GetAngularVelocity() { return this->angularVelocity; }
+
+// All things
+Thing *Thing::allThings[THING_STORE_SIZE] = {nullptr};
+
+Thing *Thing::Get(unsigned char networkId, unsigned char thingId) {
+  for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) {
+    Thing *thing = allThings[ix];
+    if (thing == nullptr)
+      continue;
+    if (thing->networkId == networkId && thing->id == thingId)
+      return thing;
+  }
+  return nullptr;
+}
+
+int Thing::Add(Thing *newThing) {
+  // Exclude 0 because that value is reserved for 'no thing'
+  for (uint16_t ix = 1; ix < THING_STORE_SIZE; ix++) {
+    Thing *thing = allThings[ix];
+    if (thing == nullptr) {
+      allThings[ix] = newThing;
+
+      // std::cout << " Add new thing " << (int)ix << "\n";
+      return ix;
+    }
+  }
+  return -1;
+}
+
+void Thing::Remove(Thing *thing) {
+  // std::cout << " remove " << (int)thing->id << "\n";
+  allThings[thing->id] = nullptr;
+}
+
+void Thing::UpdateAll(unsigned long currentTimeMs) {
+  // Not very efficient, but it works for now.
+  for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) {
+    Thing *thing = allThings[ix];
+    if (thing != nullptr &&
+        thing->parent == nullptr) { // update all root things
+      // std::cout << " update " << (int)ix << " thingid " << (int)thing->id
+      //           << "\n";
+      thing->Update(currentTimeMs);
+    }
+  }
+}

ControlCore/Thing.h

@@ -0,0 +1,133 @@
+#pragma once
+#include "LinearAlgebra/Spherical.h"
+#include "LinearAlgebra/SwingTwist.h"
+#include <iostream>
+
+namespace Passer {
+namespace Control {
+
+#define THING_STORE_SIZE 256
+// IMPORTANT: values higher than 256 will need to change the Thing::id type
+// to 16-bit or higher, breaking the networking protocol!
+
+class Thing {
+public:
+  // Participant *client;
+  unsigned char networkId = 0;
+  /// @char The id of the thing
+  unsigned char id = 0;
+
+  Thing *FindThing(const char *name);
+  //  Thing *FindChild(unsigned char id);
+
+  /// @brief Sets the parent Thing
+  /// @param parent The Thing which should become the parnet
+  /// @remark This is equivalent to calling parent->AddChild(this);
+  virtual void SetParent(Thing *parent);
+  void SetParent(Thing *root, const char *name);
+  /// @brief Gets the parent Thing
+  /// @return The parent Thing
+  Thing *GetParent();
+
+  /// @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
+  virtual void AddChild(Thing *child);
+  Thing *RemoveChild(Thing *child);
+
+  unsigned char childCount = 0;
+  Thing *GetChild(unsigned char id, bool recursive = false);
+  Thing *GetChildByIndex(unsigned char ix);
+
+protected:
+  Thing *parent = nullptr;
+  Thing **children = nullptr;
+
+public:
+  /// @brief The type of Thing
+  unsigned char type = 0;
+  const char *name = nullptr;
+  const char *modelUrl = nullptr;
+  float modelScale = 1;
+  // protected Sensor sensor;
+
+  /// @brief Basic Thing types
+  enum class Type {
+    Undetermined,
+    // Sensor,
+    Switch,
+    DistanceSensor,
+    DirectionalSensor,
+    TemperatureSensor,
+    // Motor,
+    ControlledMotor,
+    UncontrolledMotor,
+    Servo,
+    // Other
+    Roboid,
+    Humanoid,
+    ExternalSensor,
+  };
+
+  void SetPosition(Spherical16 position);
+  Spherical16 GetPosition();
+  void SetOrientation(SwingTwist16 orientation);
+  SwingTwist16 GetOrientation();
+  float scale = 1; // assuming uniform scale
+
+  bool positionUpdated = false;
+  bool orientationUpdated = false;
+
+protected:
+  /// @brief The position in local space
+  /// @remark When this Thing has a parent, the position is relative to the
+  /// parent's position and orientation
+  Spherical16 position;
+  /// @brief The orientation in local space
+  /// @remark When this Thing has a parent, the orientation is relative to the
+  /// parent's orientation
+  SwingTwist16 orientation;
+
+public:
+  Spherical16 linearVelocity;
+  Spherical16 angularVelocity;
+  virtual Spherical16 GetLinearVelocity();
+  virtual Spherical16 GetAngularVelocity();
+
+public:
+  Thing(unsigned char networkId = 0,
+        unsigned char thingType = (unsigned char)Type::Undetermined);
+  /// @brief Terminated thins are no longer updated
+  void Terminate();
+
+  /// @brief Sets the location from where the 3D model of this Thing can be
+  /// 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 Updates the state of the thing
+  /// @param currentTimeMs The current clock time in milliseconds
+  virtual void Update(unsigned long currentTimeMs) {};
+
+  virtual void SendBytes(unsigned char *buffer, unsigned char *ix) {};
+  virtual void ProcessBytes(unsigned char *bytes) {};
+
+protected:
+  virtual void Init();
+
+  //------------ All things
+public:
+  static Thing *Get(unsigned char networkId, unsigned char thingId);
+  static int Add(Thing *thing);
+  static void Remove(Thing *thing);
+  static void UpdateAll(unsigned long currentTimeMs);
+
+private:
+  static Thing *allThings[];
+};
+
+} // namespace Control
+} // namespace Passer
+using namespace Passer::Control;

ControlCore/float16.cpp

@@ -60,7 +60,7 @@ bool float16::operator<(const float16 &f) {
 }
 
 bool float16::operator<=(const float16 &f) {
-  if ((_value & (uint16_t)0x8000) && (f._value & (uint16_t)0x8000))
+  if ((_value & 0x8000) && (f._value & 0x8000))
     return _value >= f._value;
   if (_value & 0x8000)
     return true;
@@ -188,13 +188,6 @@ float float16::f16tof32(uint16_t _value) const {
 }
 
 uint16_t float16::f32tof16(float f) const {
-  // untested code, but will avoid strict aliasing warning
-  // union {
-  //   float f;
-  //   uint32_t t;
-  // } u;
-  // u.f = f;
-  // uint32_t t = u.t;
   uint32_t t = *(uint32_t *)&f;
   //  man bits = 10; but we keep 11 for rounding
   uint16_t man = (t & 0x007FFFFF) >> 12;

ControlCore/float16.h

@@ -8,11 +8,12 @@
 //          used for efficient storage and transport.
 //     URL: https://github.com/RobTillaart/float16
 
+// #include "Arduino.h"
 #include <stdint.h>
 
 #define FLOAT16_LIB_VERSION (F("0.1.8"))
 
-// typedef uint16_t _fp16;
+typedef uint16_t __fp16;
 
 class float16 {
 public:
@@ -68,7 +69,7 @@ public:
 
 private:
   uint8_t _decimals = 4;
-  uint16_t _value;
+  __fp16 _value;
 };
 
 //  -- END OF FILE --

ControlCore/test/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.13)  # CMake version check
+Project(ControlCoreTest)
+
+set(CMAKE_CXX_STANDARD 11)            # Enable c++11 standard
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+add_compile_definitions(GTEST)
+include(FetchContent)
+FetchContent_Declare(
+    googletest
+    DOWNLOAD_EXTRACT_TIMESTAMP ON
+    URL https://github.com/google/googletest/archive/refs/heads/main.zip
+)
+
+# For Windows: Prevent overriding the parent project's compiler/linker settings
+set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+FetchContent_MakeAvailable(googletest)
+
+include_directories(
+    . 
+    ..
+)
+enable_testing()
+
+add_executable(
+    ControlCoreTest
+    "dummy_test.cc"
+)
+
+target_link_libraries(
+    ControlCoreTest
+    gtest_main
+)
+
+include(GoogleTest)
+gtest_discover_tests(ControlCoreTest)

ControlCore/test/dummy_test.cc

@@ -0,0 +1,9 @@
+#if GTEST
+
+// #include <gmock/gmock.h>
+// not supported using Visual Studio 2022 compiler...
+#include <gtest/gtest.h>
+
+TEST(Dummy, Dummytest) {}
+
+#endif

ControlledMotor.cpp

@@ -0,0 +1,65 @@
+#include "ControlledMotor.h"
+
+ControlledMotor::ControlledMotor() {
+  this->type = (unsigned char)Type::ControlledMotor;
+}
+
+ControlledMotor::ControlledMotor(Motor *motor, Encoder *encoder)
+    : ControlledMotor() {
+  this->motor = motor;
+  this->encoder = encoder;
+  // this->rotationDirection = Direction::Forward;
+}
+
+#include <Arduino.h>
+
+void ControlledMotor::SetTargetSpeed(float speed) {
+  this->currentTargetSpeed = speed;
+  // this->rotationDirection =
+  //     (targetSpeed < 0) ? Direction::Reverse : Direction::Forward;
+  // this->direction = (targetSpeed < 0) ? Motor::Direction::CounterClockwise
+  //                                     : Motor::Direction::Clockwise;
+}
+
+void ControlledMotor::Update(unsigned long currentTimeMs) {
+  this->actualSpeed = encoder->GetRevolutionsPerSecond(currentTimeMs);
+  if (this->currentTargetSpeed < 0)
+    this->actualSpeed = -this->actualSpeed;
+
+  float deltaTime = currentTimeMs - this->lastUpdateTime;
+
+  float error = this->currentTargetSpeed - this->actualSpeed;
+  float errorChange = (error - lastError) * deltaTime;
+
+  float delta = error * pidP + errorChange * pidD;
+
+  Serial.print("  actual Speed ");
+  Serial.print(actualSpeed);
+  Serial.print("  target Speed ");
+  Serial.print(this->currentTargetSpeed);
+  Serial.print("    motor target speed ");
+  Serial.print(motor->currentTargetSpeed);
+  Serial.print(" + ");
+  Serial.print(error * pidP);
+  Serial.print(" + ");
+  Serial.print(errorChange * pidD);
+  Serial.print(" = ");
+  Serial.println(motor->currentTargetSpeed + delta);
+
+  motor->SetTargetSpeed(motor->currentTargetSpeed +
+                        delta); // or something like that
+  this->lastUpdateTime = currentTimeMs;
+}
+
+float ControlledMotor::GetActualSpeed() { return actualSpeed; }
+
+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;
+}

ControlledMotor.h

@@ -0,0 +1,60 @@
+#pragma once
+
+#include "Encoder.h"
+#include "Motor.h"
+
+namespace Passer {
+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();
+  ControlledMotor(Motor *motor, Encoder *encoder);
+
+  inline static bool CheckType(Thing *thing) {
+    return (thing->type & (int)Thing::Type::ControlledMotor) != 0;
+  }
+  float velocity;
+
+  float pidP = 0.1F;
+  float pidD = 0.0F;
+  float pidI = 0.0F;
+
+  void Update(unsigned long currentTimeMs) override;
+
+  /// @brief Set the target speed for the motor controller
+  /// @param speed the target in revolutions per second.
+  virtual void SetTargetSpeed(float speed) override;
+
+  /// @brief Get the actual speed from the encoder
+  /// @return The speed in revolutions per second
+  virtual float GetActualSpeed() override;
+
+  bool Drive(float distance);
+
+  Motor *motor;
+  Encoder *encoder;
+
+protected:
+  float lastUpdateTime = 0;
+  float lastError = 0;
+  // float targetSpeed;
+  float actualSpeed;
+  float netDistance = 0;
+  float startDistance = 0;
+
+  // enum Direction { Forward = 1, Reverse = -1 };
+
+  // Direction rotationDirection;
+
+  bool driving = false;
+  float targetDistance = 0;
+  float lastEncoderPosition = 0;
+};
+
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;

DifferentialDrive.cpp

@@ -0,0 +1,119 @@
+#include "DifferentialDrive.h"
+#include "LinearAlgebra/Angle.h"
+#include "LinearAlgebra/FloatSingle.h"
+
+#include <math.h>
+
+DifferentialDrive::DifferentialDrive() {};
+
+DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor) {
+  this->motorCount = 2;
+  this->motors = new Motor *[2];
+  this->motors[0] = leftMotor;
+  this->motors[1] = rightMotor;
+
+  float distance = this->wheelSeparation / 2;
+  leftMotor->direction = Motor::Direction::CounterClockwise;
+  leftMotor->SetPosition(Spherical16(distance, Direction16::left));
+  // leftMotor->position.direction.horizontal = Angle16::Degrees(-90);
+  // leftMotor->position.distance = distance;
+  rightMotor->direction = Motor::Direction::Clockwise;
+  rightMotor->SetPosition(Spherical16(distance, Direction16::right));
+  // rightMotor->position.direction.horizontal = Angle16::Degrees(90);
+  // rightMotor->position.distance = distance;
+}
+
+void DifferentialDrive::SetDimensions(float wheelDiameter,
+                                      float wheelSeparation) {
+  this->wheelDiameter = wheelDiameter;
+  this->wheelSeparation = wheelSeparation;
+  this->rpsToMs = wheelDiameter * Passer::LinearAlgebra::pi;
+
+  float distance = this->wheelSeparation / 2;
+  Spherical16 motor0Position = this->motors[0]->GetPosition();
+  motor0Position.distance = distance;
+  this->motors[0]->SetPosition(motor0Position);
+  Spherical16 motor1Position = this->motors[0]->GetPosition();
+  motor1Position.distance = distance;
+  this->motors[1]->SetPosition(motor1Position);
+}
+
+void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed,
+                                             float rightSpeed) {
+  for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
+    Motor *motor = motors[motorIx];
+    if (motor == nullptr)
+      continue;
+
+    float xPosition =
+        motors[motorIx]->GetPosition().direction.horizontal.InDegrees();
+    if (xPosition < 0)
+      motor->SetTargetSpeed(leftSpeed);
+    else if (xPosition > 0)
+      motor->SetTargetSpeed(rightSpeed);
+  };
+}
+
+void DifferentialDrive::SetTwistSpeed(float forward, float yaw) {
+  float leftSpeed =
+      Float::Clamp(forward + yaw, -1, 1); // revolutions per second
+  float rightSpeed =
+      Float::Clamp(forward - yaw, -1, 1); // revolutions per second
+
+  float leftMotorSpeed = leftSpeed / rpsToMs;   // meters per second
+  float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second
+
+  SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
+}
+
+void DifferentialDrive::SetTwistSpeed(Vector2 linear, float yaw) {
+  SetTwistSpeed(linear.y, yaw);
+}
+
+void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
+                                      float roll) {
+  SetTwistSpeed(linear.Forward(), yaw);
+}
+
+// void DifferentialDrive::SetVelocity(Polar velocity) {
+//   SetTwistSpeed(velocity.distance, velocity.angle.InDegrees());
+// }
+
+// Spherical16 DifferentialDrive::GetVelocity() {
+//   Motor *leftMotor = motors[0];
+//   Motor *rightMotor = motors[1];
+//   float leftSpeed = leftMotor->GetActualSpeed();   // in revolutions per
+//   second float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions
+//   per second
+
+//   leftSpeed = leftSpeed * rpsToMs;   // in meters per second
+//   rightSpeed = rightSpeed * rpsToMs; // in meters per second
+//   float speed = (leftSpeed + rightSpeed) / 2;
+
+//   float direction = speed >= 0 ? 0.0F : 180.0F;
+//   float magnitude = fabsf(speed);
+//   Spherical16 velocity =
+//       Spherical16(magnitude, Angle16::Degrees(direction),
+//                   Angle16::Degrees(0)); // Polar(direction, magnitude);
+//   return velocity;
+// }
+
+// float DifferentialDrive::GetAngularVelocity() {
+//   Motor *leftMotor = motors[0];
+//   Motor *rightMotor = motors[1];
+//   float leftSpeed = leftMotor->GetActualSpeed();   // in revolutions per
+//   second float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions
+//   per second
+
+//   leftSpeed = leftSpeed * rpsToMs;   // in meters per second
+//   rightSpeed = rightSpeed * rpsToMs; // in meters per second
+
+//   float angularSpeed = (leftSpeed - rightSpeed) / 2;
+//   float angularDistance = wheelSeparation / 2 * Passer::LinearAlgebra::pi;
+//   float rotationsPerSecond = angularSpeed / angularDistance;
+//   float degreesPerSecond =
+//       Angle::Normalize(Angle::Degrees(360 * rotationsPerSecond)).InDegrees();
+//   float angularVelocity = degreesPerSecond;
+
+//   return angularVelocity;
+// }

DifferentialDrive.h

@@ -0,0 +1,90 @@
+#pragma once
+
+#include "Propulsion.h"
+
+namespace Passer {
+namespace RoboidControl {
+
+/// @brief A two-wheeled Propulsion method
+///
+/// The wheels are put at either side of the roboid with the following behaviour
+/// * When both wheels spin forward, the Roboid moves forward
+/// * When both wheels spin backward, the Roboid moves backward
+/// * When both wheels are spinning in opposite directions, the Roboid rotates
+/// wihout moving forward or backward
+/// * When just one wheel is spinning, the Roboid turnes while moving forward or
+/// backward.
+class DifferentialDrive : public Propulsion {
+public:
+  /// @brief Default constructor
+  DifferentialDrive();
+  /// @brief Setup of the DifferentialDrive with the Placement of the motors
+  /// @param leftMotorPlacement Placement of the left Motor
+  /// @param rightMotorPlacement Placement of the right Motor
+  /// In this setup, the left motor Direction will be CounterClockWise when
+  /// driving forward, while the right motor will turn Clockwise.
+  /// @note When not using controlled motors, the placement of the motors is
+  /// irrelevant.
+  // DifferentialDrive(Placement leftMotorPlacement,
+  //                   Placement rightMotorPlacement);
+
+  DifferentialDrive(Motor *leftMotor, Motor *rightMotor);
+
+  void SetDimensions(float wheelDiameter, float wheelSeparation);
+
+  /// @brief Set the target speeds of the motors directly
+  /// @param leftSpeed The target speed of the left Motor
+  /// @param rightSpeed The target speed of the right Motor
+  void SetMotorTargetSpeeds(float leftSpeed, float rightSpeed);
+
+  /// @brief Controls the motors through forward and rotation speeds
+  /// @param forward The target forward speed of the Roboid
+  /// @param yaw The target rotation speed of the Roboid
+  virtual void SetTwistSpeed(float forward, float yaw) override;
+  /// @brief Controls the motors through forward and rotation speeds
+  /// @param linear The target linear speed of the Roboid
+  /// @param yaw The target rotation speed of the Roboid
+  /// @note As a DifferentialDrive cannot move sideward, this function has the
+  /// same effect as using the void SetTwistSpeed(float forward, float yaw)
+  /// function.
+  virtual void SetTwistSpeed(Vector2 linear, float yaw = 0.0F);
+  /// @brief Controls the motors through forward and rotation speeds
+  /// @param linear The target linear speed
+  /// @param yaw The target rotation speed around the vertical axis
+  /// @param pitch Pitch is not supported and is ignored
+  /// @param roll Roll is not supported and is ignores
+  /// @note As a DifferentialDrive cannot move sideward or vertical, this
+  /// function has the same effect as using the void SetTwistSpeed(float
+  /// forward, float yaw) function.
+  virtual void SetTwistSpeed(Vector3 linear, float yaw = 0.0F,
+                             float pitch = 0.0F, float roll = 0.0F);
+
+  // virtual void SetVelocity(Polar velocity);
+
+  /// @brief Calculate the linear velocity of the roboid based on the wheel
+  /// velocities
+  /// @return The velocity of the roboid in local space
+  /// @details The actual values may not be accurate, depending on the available
+  /// information
+  /// @remark This will be more expanded/detailed in a future version of Roboid
+  /// Control
+  // virtual Spherical16 GetVelocity() override;
+  /// @brief Calculate the angular velocity of the roboid based on the wheel
+  /// velocities
+  /// @return The angular speed of the roboid in local space
+  /// @details The actual value may not be accurate, depending on the available
+  /// information
+  /// @remark This will be more expanded/detailed in a future version of Roboid
+  /// Control
+  // virtual float GetAngularVelocity() override;
+
+protected:
+  float wheelDiameter = 1.0F;   // in meters
+  float wheelSeparation = 1.0F; // in meters;
+
+  float rpsToMs = 1.0F; // convert revolutions per second to meters per second
+};
+
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;

DirectionalSensor.cpp

@@ -0,0 +1,15 @@
+#include "DirectionalSensor.h"
+
+#include "ControlCore/LowLevelMessages.h"
+
+DirectionalSensor::DirectionalSensor() : Sensor() {
+  this->type = (unsigned char)Type::DirectionalSensor;
+  this->vector = Spherical16::zero;
+}
+
+Spherical16 DirectionalSensor::GetVector() { return Spherical16::zero; }
+
+void DirectionalSensor::ProcessBytes(unsigned char *data) {
+  unsigned char ix = 0;
+  this->vector = LowLevelMessages::ReceiveSpherical16(data, &ix);
+}

DirectionalSensor.h

@@ -0,0 +1,30 @@
+#pragma once
+
+#include "Sensor.h"
+
+namespace Passer {
+namespace RoboidControl {
+
+/// @brief A Sensor which can measure the distance to the nearest object
+class DirectionalSensor : public Sensor {
+public:
+  /// @brief Default constructor
+  DirectionalSensor();
+
+  // const unsigned int Type = SensorType | (unsigned
+  // int)Type::DirectionalSensor;
+
+  /// @brief Determine the distance to the nearest object
+  /// @return the measured distance in meters to the nearest object
+  virtual Spherical16 GetVector();
+
+  virtual void ProcessBytes(unsigned char *bytes) override;
+
+protected:
+  /// @brief Distance to the closest object
+  Spherical16 vector = Spherical16::zero;
+};
+
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;

DistanceSensor.cpp

@@ -0,0 +1,37 @@
+#include "DistanceSensor.h"
+
+#include "ControlCore/LowLevelMessages.h"
+#include <math.h>
+
+DistanceSensor::DistanceSensor() : Sensor() {
+  this->type = (unsigned char)Type::DistanceSensor;
+  this->distance = INFINITY;
+  this->triggerDistance = 1.0F;
+}
+
+DistanceSensor::DistanceSensor(float triggerDistance) : DistanceSensor() {
+  this->triggerDistance = triggerDistance;
+}
+
+float DistanceSensor::GetDistance() {
+  if (this->distance < minRange || this->distance > maxRange)
+    return INFINITY; // invalid distance
+
+  float d = this->distance;
+  this->distance = INFINITY;
+  return d;
+};
+
+bool DistanceSensor::ObjectNearby() {
+  if (distance < minRange || distance > maxRange)
+    return false;
+
+  bool isOn = distance <= triggerDistance;
+  return isOn;
+}
+
+void DistanceSensor::ProcessBytes(unsigned char *bytes) {
+  unsigned char ix = 0;
+  this->distance = LowLevelMessages::ReceiveFloat16(bytes, &ix);
+  // std::cout << "received distance " << this->distance << "\n";
+}

DistanceSensor.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include "Sensor.h"
+
+namespace Passer {
+namespace RoboidControl {
+
+/// @brief A Sensor which can measure the distance to the nearest object
+class DistanceSensor : public Sensor {
+public:
+  /// @brief Default constructor
+  DistanceSensor();
+  /// @brief Creates a DistanceSensor with the given trigger distance
+  /// @param triggerDistance The distance at which the sensors indicates that a
+  /// object is close by
+  DistanceSensor(float triggerDistance);
+
+  /// @brief Determine the distance to the nearest object
+  /// @return the measured distance in meters to the nearest object
+  virtual float GetDistance();
+
+  /// @brief The minimum range at which the sensor gives reliable measurements
+  float minRange = 0.01F;
+  /// @brief The maximum range at which the sensor gives reliable measurements
+  float maxRange = 10.0F;
+
+  /// @brief The distance at which ObjectNearby triggers
+  float triggerDistance = 1;
+
+  /// @brief Indicate that an object is nearby
+  /// @return True when an object is nearby
+  bool ObjectNearby();
+
+  virtual void ProcessBytes(unsigned char *bytes) override;
+
+protected:
+  /// @brief Distance to the closest object
+  float distance = 0;
+};
+
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;

Encoder.cpp

@@ -0,0 +1,21 @@
+/*
+#include "Encoder.h"
+
+IncrementalEncoder::IncrementalEncoder(unsigned char pulsesPerRevolution,
+                                       unsigned char distancePerRotation) {
+  this->pulsesPerRevolution = pulsesPerRevolution;
+  this->distancePerRevolution = distancePerRotation;
+}
+
+int IncrementalEncoder::GetPulseCount() { return 0; }
+
+float IncrementalEncoder::GetDistance() { return 0; }
+
+float IncrementalEncoder::GetPulsesPerSecond(float currentTimeMs) { return 0; }
+
+float IncrementalEncoder::GetRevolutionsPerSecond(float currentTimeMs) {
+  return 0;
+}
+
+float IncrementalEncoder::GetSpeed(float currentTimeMs) { return 0; }
+*/

Encoder.h

@@ -0,0 +1,57 @@
+#pragma once
+
+#include "IncrementalEncoder.h"
+
+namespace Passer {
+namespace RoboidControl {
+
+// Deprecated, use explicit IncrementalEncoder in the future
+using Encoder = IncrementalEncoder;
+
+/*
+/// @brief An Encoder measures the rotations of an axle using a rotary
+/// sensor Some encoders are able to detect direction, while others can not.
+class IncrementalEncoder {
+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
+  IncrementalEncoder(unsigned char pulsesPerRevolution = 1,
+                     unsigned char distancePerRevolution = 1);
+
+  /// @brief Get the total number of pulses since the previous call
+  /// @return The number of pulses, is zero or greater
+  virtual int GetPulseCount();
+  /// @brief Get the pulse speed since the previous call
+  /// @param currentTimeMs The clock time in milliseconds
+  /// @return The average pulses per second in the last period.
+  virtual float GetPulsesPerSecond(float currentTimeMs);
+
+  /// @brief Get the distance traveled since the previous call
+  /// @return The distance in meters.
+  virtual float GetDistance();
+
+  /// @brief Get the rotation speed since the previous call
+  /// @param currentTimeMs The clock time in milliseconds
+  /// @return The speed in rotations per second
+  virtual float GetRevolutionsPerSecond(float currentTimeMs);
+
+  /// @brief Get the speed since the previous call
+  /// @param currentTimeMs The clock time in milliseconds
+  /// @return The speed in meters per second.
+  /// @note this value is dependent on the accurate setting of the
+  /// pulsesPerRevolution and distancePerRevolution parameters;
+  virtual float GetSpeed(float currentTimeMs);
+
+  /// @brief The numer of pulses corresponding to a full rotation of the axle
+  unsigned char pulsesPerRevolution = 1;
+  /// @brief The number of revolutions which makes the wheel move forward 1
+  /// meter
+  unsigned char distancePerRevolution = 1;
+};
+*/
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;

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

IncrementalEncoder.cpp

@@ -0,0 +1,19 @@
+#include "Encoder.h"
+
+IncrementalEncoder::IncrementalEncoder(unsigned char pulsesPerRevolution,
+                                       unsigned char distancePerRotation) {
+  this->pulsesPerRevolution = pulsesPerRevolution;
+  this->distancePerRevolution = distancePerRotation;
+}
+
+int IncrementalEncoder::GetPulseCount() { return 0; }
+
+float IncrementalEncoder::GetDistance() { return 0; }
+
+float IncrementalEncoder::GetPulsesPerSecond(float currentTimeMs) { return 0; }
+
+float IncrementalEncoder::GetRevolutionsPerSecond(float currentTimeMs) {
+  return 0;
+}
+
+float IncrementalEncoder::GetSpeed(float currentTimeMs) { return 0; }

IncrementalEncoder.h

@@ -0,0 +1,51 @@
+#pragma once
+
+namespace Passer {
+namespace RoboidControl {
+
+/// @brief An Encoder measures the rotations of an axle using a rotary
+/// sensor Some encoders are able to detect direction, while others can not.
+class IncrementalEncoder {
+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
+  IncrementalEncoder(unsigned char pulsesPerRevolution = 1,
+                     unsigned char distancePerRevolution = 1);
+
+  /// @brief Get the total number of pulses since the previous call
+  /// @return The number of pulses, is zero or greater
+  virtual int GetPulseCount();
+  /// @brief Get the pulse speed since the previous call
+  /// @param currentTimeMs The clock time in milliseconds
+  /// @return The average pulses per second in the last period.
+  virtual float GetPulsesPerSecond(float currentTimeMs);
+
+  /// @brief Get the distance traveled since the previous call
+  /// @return The distance in meters.
+  virtual float GetDistance();
+
+  /// @brief Get the rotation speed since the previous call
+  /// @param currentTimeMs The clock time in milliseconds
+  /// @return The speed in rotations per second
+  virtual float GetRevolutionsPerSecond(float currentTimeMs);
+
+  /// @brief Get the speed since the previous call
+  /// @param currentTimeMs The clock time in milliseconds
+  /// @return The speed in meters per second.
+  /// @note this value is dependent on the accurate setting of the
+  /// pulsesPerRevolution and distancePerRevolution parameters;
+  virtual float GetSpeed(float currentTimeMs);
+
+  /// @brief The numer of pulses corresponding to a full rotation of the axle
+  unsigned char pulsesPerRevolution = 1;
+  /// @brief The number of revolutions which makes the wheel move forward 1
+  /// meter
+  unsigned char distancePerRevolution = 1;
+};
+
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;

LICENSE

@@ -0,0 +1,373 @@
+Mozilla Public License Version 2.0
+==================================
+
+1. Definitions
+--------------
+
+1.1. "Contributor"
+    means each individual or legal entity that creates, contributes to
+    the creation of, or owns Covered Software.
+
+1.2. "Contributor Version"
+    means the combination of the Contributions of others (if any) used
+    by a Contributor and that particular Contributor's Contribution.
+
+1.3. "Contribution"
+    means Covered Software of a particular Contributor.
+
+1.4. "Covered Software"
+    means Source Code Form to which the initial Contributor has attached
+    the notice in Exhibit A, the Executable Form of such Source Code
+    Form, and Modifications of such Source Code Form, in each case
+    including portions thereof.
+
+1.5. "Incompatible With Secondary Licenses"
+    means
+
+    (a) that the initial Contributor has attached the notice described
+        in Exhibit B to the Covered Software; or
+
+    (b) that the Covered Software was made available under the terms of
+        version 1.1 or earlier of the License, but not also under the
+        terms of a Secondary License.
+
+1.6. "Executable Form"
+    means any form of the work other than Source Code Form.
+
+1.7. "Larger Work"
+    means a work that combines Covered Software with other material, in
+    a separate file or files, that is not Covered Software.
+
+1.8. "License"
+    means this document.
+
+1.9. "Licensable"
+    means having the right to grant, to the maximum extent possible,
+    whether at the time of the initial grant or subsequently, any and
+    all of the rights conveyed by this License.
+
+1.10. "Modifications"
+    means any of the following:
+
+    (a) any file in Source Code Form that results from an addition to,
+        deletion from, or modification of the contents of Covered
+        Software; or
+
+    (b) any new file in Source Code Form that contains any Covered
+        Software.
+
+1.11. "Patent Claims" of a Contributor
+    means any patent claim(s), including without limitation, method,
+    process, and apparatus claims, in any patent Licensable by such
+    Contributor that would be infringed, but for the grant of the
+    License, by the making, using, selling, offering for sale, having
+    made, import, or transfer of either its Contributions or its
+    Contributor Version.
+
+1.12. "Secondary License"
+    means either the GNU General Public License, Version 2.0, the GNU
+    Lesser General Public License, Version 2.1, the GNU Affero General
+    Public License, Version 3.0, or any later versions of those
+    licenses.
+
+1.13. "Source Code Form"
+    means the form of the work preferred for making modifications.
+
+1.14. "You" (or "Your")
+    means an individual or a legal entity exercising rights under this
+    License. For legal entities, "You" includes any entity that
+    controls, is controlled by, or is under common control with You. For
+    purposes of this definition, "control" means (a) the power, direct
+    or indirect, to cause the direction or management of such entity,
+    whether by contract or otherwise, or (b) ownership of more than
+    fifty percent (50%) of the outstanding shares or beneficial
+    ownership of such entity.
+
+2. License Grants and Conditions
+--------------------------------
+
+2.1. Grants
+
+Each Contributor hereby grants You a world-wide, royalty-free,
+non-exclusive license:
+
+(a) under intellectual property rights (other than patent or trademark)
+    Licensable by such Contributor to use, reproduce, make available,
+    modify, display, perform, distribute, and otherwise exploit its
+    Contributions, either on an unmodified basis, with Modifications, or
+    as part of a Larger Work; and
+
+(b) under Patent Claims of such Contributor to make, use, sell, offer
+    for sale, have made, import, and otherwise transfer either its
+    Contributions or its Contributor Version.
+
+2.2. Effective Date
+
+The licenses granted in Section 2.1 with respect to any Contribution
+become effective for each Contribution on the date the Contributor first
+distributes such Contribution.
+
+2.3. Limitations on Grant Scope
+
+The licenses granted in this Section 2 are the only rights granted under
+this License. No additional rights or licenses will be implied from the
+distribution or licensing of Covered Software under this License.
+Notwithstanding Section 2.1(b) above, no patent license is granted by a
+Contributor:
+
+(a) for any code that a Contributor has removed from Covered Software;
+    or
+
+(b) for infringements caused by: (i) Your and any other third party's
+    modifications of Covered Software, or (ii) the combination of its
+    Contributions with other software (except as part of its Contributor
+    Version); or
+
+(c) under Patent Claims infringed by Covered Software in the absence of
+    its Contributions.
+
+This License does not grant any rights in the trademarks, service marks,
+or logos of any Contributor (except as may be necessary to comply with
+the notice requirements in Section 3.4).
+
+2.4. Subsequent Licenses
+
+No Contributor makes additional grants as a result of Your choice to
+distribute the Covered Software under a subsequent version of this
+License (see Section 10.2) or under the terms of a Secondary License (if
+permitted under the terms of Section 3.3).
+
+2.5. Representation
+
+Each Contributor represents that the Contributor believes its
+Contributions are its original creation(s) or it has sufficient rights
+to grant the rights to its Contributions conveyed by this License.
+
+2.6. Fair Use
+
+This License is not intended to limit any rights You have under
+applicable copyright doctrines of fair use, fair dealing, or other
+equivalents.
+
+2.7. Conditions
+
+Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted
+in Section 2.1.
+
+3. Responsibilities
+-------------------
+
+3.1. Distribution of Source Form
+
+All distribution of Covered Software in Source Code Form, including any
+Modifications that You create or to which You contribute, must be under
+the terms of this License. You must inform recipients that the Source
+Code Form of the Covered Software is governed by the terms of this
+License, and how they can obtain a copy of this License. You may not
+attempt to alter or restrict the recipients' rights in the Source Code
+Form.
+
+3.2. Distribution of Executable Form
+
+If You distribute Covered Software in Executable Form then:
+
+(a) such Covered Software must also be made available in Source Code
+    Form, as described in Section 3.1, and You must inform recipients of
+    the Executable Form how they can obtain a copy of such Source Code
+    Form by reasonable means in a timely manner, at a charge no more
+    than the cost of distribution to the recipient; and
+
+(b) You may distribute such Executable Form under the terms of this
+    License, or sublicense it under different terms, provided that the
+    license for the Executable Form does not attempt to limit or alter
+    the recipients' rights in the Source Code Form under this License.
+
+3.3. Distribution of a Larger Work
+
+You may create and distribute a Larger Work under terms of Your choice,
+provided that You also comply with the requirements of this License for
+the Covered Software. If the Larger Work is a combination of Covered
+Software with a work governed by one or more Secondary Licenses, and the
+Covered Software is not Incompatible With Secondary Licenses, this
+License permits You to additionally distribute such Covered Software
+under the terms of such Secondary License(s), so that the recipient of
+the Larger Work may, at their option, further distribute the Covered
+Software under the terms of either this License or such Secondary
+License(s).
+
+3.4. Notices
+
+You may not remove or alter the substance of any license notices
+(including copyright notices, patent notices, disclaimers of warranty,
+or limitations of liability) contained within the Source Code Form of
+the Covered Software, except that You may alter any license notices to
+the extent required to remedy known factual inaccuracies.
+
+3.5. Application of Additional Terms
+
+You may choose to offer, and to charge a fee for, warranty, support,
+indemnity or liability obligations to one or more recipients of Covered
+Software. However, You may do so only on Your own behalf, and not on
+behalf of any Contributor. You must make it absolutely clear that any
+such warranty, support, indemnity, or liability obligation is offered by
+You alone, and You hereby agree to indemnify every Contributor for any
+liability incurred by such Contributor as a result of warranty, support,
+indemnity or liability terms You offer. You may include additional
+disclaimers of warranty and limitations of liability specific to any
+jurisdiction.
+
+4. Inability to Comply Due to Statute or Regulation
+---------------------------------------------------
+
+If it is impossible for You to comply with any of the terms of this
+License with respect to some or all of the Covered Software due to
+statute, judicial order, or regulation then You must: (a) comply with
+the terms of this License to the maximum extent possible; and (b)
+describe the limitations and the code they affect. Such description must
+be placed in a text file included with all distributions of the Covered
+Software under this License. Except to the extent prohibited by statute
+or regulation, such description must be sufficiently detailed for a
+recipient of ordinary skill to be able to understand it.
+
+5. Termination
+--------------
+
+5.1. The rights granted under this License will terminate automatically
+if You fail to comply with any of its terms. However, if You become
+compliant, then the rights granted under this License from a particular
+Contributor are reinstated (a) provisionally, unless and until such
+Contributor explicitly and finally terminates Your grants, and (b) on an
+ongoing basis, if such Contributor fails to notify You of the
+non-compliance by some reasonable means prior to 60 days after You have
+come back into compliance. Moreover, Your grants from a particular
+Contributor are reinstated on an ongoing basis if such Contributor
+notifies You of the non-compliance by some reasonable means, this is the
+first time You have received notice of non-compliance with this License
+from such Contributor, and You become compliant prior to 30 days after
+Your receipt of the notice.
+
+5.2. If You initiate litigation against any entity by asserting a patent
+infringement claim (excluding declaratory judgment actions,
+counter-claims, and cross-claims) alleging that a Contributor Version
+directly or indirectly infringes any patent, then the rights granted to
+You by any and all Contributors for the Covered Software under Section
+2.1 of this License shall terminate.
+
+5.3. In the event of termination under Sections 5.1 or 5.2 above, all
+end user license agreements (excluding distributors and resellers) which
+have been validly granted by You or Your distributors under this License
+prior to termination shall survive termination.
+
+************************************************************************
+*                                                                      *
+*  6. Disclaimer of Warranty                                           *
+*  -------------------------                                           *
+*                                                                      *
+*  Covered Software is provided under this License on an "as is"       *
+*  basis, without warranty of any kind, either expressed, implied, or  *
+*  statutory, including, without limitation, warranties that the       *
+*  Covered Software is free of defects, merchantable, fit for a        *
+*  particular purpose or non-infringing. The entire risk as to the     *
+*  quality and performance of the Covered Software is with You.        *
+*  Should any Covered Software prove defective in any respect, You     *
+*  (not any Contributor) assume the cost of any necessary servicing,   *
+*  repair, or correction. This disclaimer of warranty constitutes an   *
+*  essential part of this License. No use of any Covered Software is   *
+*  authorized under this License except under this disclaimer.         *
+*                                                                      *
+************************************************************************
+
+************************************************************************
+*                                                                      *
+*  7. Limitation of Liability                                          *
+*  --------------------------                                          *
+*                                                                      *
+*  Under no circumstances and under no legal theory, whether tort      *
+*  (including negligence), contract, or otherwise, shall any           *
+*  Contributor, or anyone who distributes Covered Software as          *
+*  permitted above, be liable to You for any direct, indirect,         *
+*  special, incidental, or consequential damages of any character      *
+*  including, without limitation, damages for lost profits, loss of    *
+*  goodwill, work stoppage, computer failure or malfunction, or any    *
+*  and all other commercial damages or losses, even if such party      *
+*  shall have been informed of the possibility of such damages. This   *
+*  limitation of liability shall not apply to liability for death or   *
+*  personal injury resulting from such party's negligence to the       *
+*  extent applicable law prohibits such limitation. Some               *
+*  jurisdictions do not allow the exclusion or limitation of           *
+*  incidental or consequential damages, so this exclusion and          *
+*  limitation may not apply to You.                                    *
+*                                                                      *
+************************************************************************
+
+8. Litigation
+-------------
+
+Any litigation relating to this License may be brought only in the
+courts of a jurisdiction where the defendant maintains its principal
+place of business and such litigation shall be governed by laws of that
+jurisdiction, without reference to its conflict-of-law provisions.
+Nothing in this Section shall prevent a party's ability to bring
+cross-claims or counter-claims.
+
+9. Miscellaneous
+----------------
+
+This License represents the complete agreement concerning the subject
+matter hereof. If any provision of this License is held to be
+unenforceable, such provision shall be reformed only to the extent
+necessary to make it enforceable. Any law or regulation which provides
+that the language of a contract shall be construed against the drafter
+shall not be used to construe this License against a Contributor.
+
+10. Versions of the License
+---------------------------
+
+10.1. New Versions
+
+Mozilla Foundation is the license steward. Except as provided in Section
+10.3, no one other than the license steward has the right to modify or
+publish new versions of this License. Each version will be given a
+distinguishing version number.
+
+10.2. Effect of New Versions
+
+You may distribute the Covered Software under the terms of the version
+of the License under which You originally received the Covered Software,
+or under the terms of any subsequent version published by the license
+steward.
+
+10.3. Modified Versions
+
+If you create software not governed by this License, and you want to
+create a new license for such software, you may create and use a
+modified version of this License if you rename the license and remove
+any references to the name of the license steward (except to note that
+such modified license differs from this License).
+
+10.4. Distributing Source Code Form that is Incompatible With Secondary
+Licenses
+
+If You choose to distribute Source Code Form that is Incompatible With
+Secondary Licenses under the terms of this version of the License, the
+notice described in Exhibit B of this License must be attached.
+
+Exhibit A - Source Code Form License Notice
+-------------------------------------------
+
+  This Source Code Form is subject to the terms of the Mozilla Public
+  License, v. 2.0. If a copy of the MPL was not distributed with this
+  file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+If it is not possible or desirable to put the notice in a particular
+file, then You may include the notice in a location (such as a LICENSE
+file in a relevant directory) where a recipient would be likely to look
+for such a notice.
+
+You may add additional accurate notices of copyright ownership.
+
+Exhibit B - "Incompatible With Secondary Licenses" Notice
+---------------------------------------------------------
+
+  This Source Code Form is "Incompatible With Secondary Licenses", as
+  defined by the Mozilla Public License, v. 2.0.

LinearAlgebra/Direction.h

@@ -1,102 +0,0 @@
-// This Source Code Form is subject to the terms of the Mozilla Public
-// License, v. 2.0.If a copy of the MPL was not distributed with this
-// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
-
-#ifndef DIRECTION_H
-#define DIRECTION_H
-
-#include "Angle.h"
-
-namespace LinearAlgebra {
-
-struct Vector3;
-
-/// @brief A direction using angles in various representations
-/// @tparam T The implementation type used for the representation of the angles
-/// A direction is represented using two angles:
-/// * The horizontal angle ranging from -180 (inclusive) to 180 (exclusive)
-/// degrees which is a rotation in the horizontal plane
-/// * A vertical angle ranging from -90 (inclusive) to 90 (exclusive) degrees
-/// which is the rotation in the up/down direction applied after the horizontal
-/// rotation has been applied.
-/// The angles are automatically normalized to stay within the abovenmentioned
-/// ranges.
-template <typename T>
-class DirectionOf {
- 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();
-  /// @brief Create a new direction
-  /// @param horizontal The horizontal angle
-  /// @param vertical The vertical angle.
-  DirectionOf(AngleOf<T> horizontal, AngleOf<T> vertical);
-
-  /// @brief Convert the direction into a carthesian vector
-  /// @return The carthesian vector corresponding to this direction.
-  Vector3 ToVector3() const;
-  /// @brief Convert a carthesian vector into a direction
-  /// @param v The carthesian vector
-  /// @return The direction.
-  /// @note Information about the length of the carthesian vector is not
-  /// included in this transformation.
-  static DirectionOf<T> FromVector3(Vector3 vector);
-
-  /// @brief Create a direction using angle values in degrees
-  /// @param horizontal The horizontal angle in degrees
-  /// @param vertical The vertical angle in degrees
-  /// @return The direction
-  static DirectionOf<T> Degrees(float horizontal, float vertical);
-  /// @brief Create a direction using angle values in radians
-  /// @param horizontal The horizontal angle in radians
-  /// @param vertical The vertical angle in radians
-  /// @return The direction
-  static DirectionOf<T> Radians(float horizontal, float vertical);
-
-  /// @brief A forward direction with zero for both angles
-  const static DirectionOf forward;
-  /// @brief A backward direction with horizontal angle -180 and zero vertical
-  /// angle
-  const static DirectionOf back;
-  /// @brief A upward direction with zero horizontal angle and vertical angle 90
-  const static DirectionOf up;
-  /// @brief A downward direction with zero horizontal angle and vertical angle
-  /// -90
-  const static DirectionOf down;
-  /// @brief A left-pointing direction with horizontal angle -90 and zero
-  /// vertical angle
-  const static DirectionOf left;
-  /// @brief A right-pointing direction with horizontal angle 90 and zero
-  /// vertical angle
-  const static DirectionOf right;
-
-  /// @brief Test whether this direction is equal to another direction
-  /// @param direction The direction to compare to
-  /// @return True when the direction angles are equal, false otherwise.
-  bool operator==(const DirectionOf<T> direction) const;
-
-  /// @brief Negate/reverse the direction
-  /// @return The reversed direction.
-  DirectionOf<T> operator-() const;
-
- protected:
-  /// @brief Normalize this vector to the specified ranges
-  void Normalize();
-};
-
-using DirectionSingle = DirectionOf<float>;
-using Direction16 = DirectionOf<signed short>;
-
-#if defined(ARDUINO)
-using Direction = Direction16;
-#else
-using Direction = DirectionSingle;
-#endif
-
-}  // namespace LinearAlgebra
-
-#endif

LinearAlgebra/DoxyGen/DoxygenLayout.xml

@@ -1,226 +0,0 @@
-<doxygenlayout version="1.0">
-  <!-- Generated by doxygen 1.8.18 -->
-  <!-- Navigation index tabs for HTML output -->
-  <navindex>
-    <tab type="mainpage" visible="yes" title=""/>
-    <tab type="pages" visible="yes" title="" intro=""/>
-    <tab type="modules" visible="yes" title="" intro=""/>
-    <tab type="namespaces" visible="yes" title="">
-      <tab type="namespacelist" visible="yes" title="" intro=""/>
-      <tab type="namespacemembers" visible="yes" title="" intro=""/>
-    </tab>
-    <tab type="interfaces" visible="yes" title="">
-      <tab type="interfacelist" visible="yes" title="" intro=""/>
-      <tab type="interfaceindex" visible="$ALPHABETICAL_INDEX" title=""/> 
-      <tab type="interfacehierarchy" visible="yes" title="" intro=""/>
-    </tab>
-    <tab type="classes" visible="yes" title="">
-      <tab type="classlist" visible="yes" title="" intro=""/>
-      <tab type="classindex" visible="$ALPHABETICAL_INDEX" title=""/> 
-      <tab type="hierarchy" visible="yes" title="" intro=""/>
-      <tab type="classmembers" visible="yes" title="" intro=""/>
-    </tab>
-    <tab type="structs" visible="yes" title="">
-      <tab type="structlist" visible="yes" title="" intro=""/>
-      <tab type="structindex" visible="$ALPHABETICAL_INDEX" title=""/> 
-    </tab>
-    <tab type="exceptions" visible="yes" title="">
-      <tab type="exceptionlist" visible="yes" title="" intro=""/>
-      <tab type="exceptionindex" visible="$ALPHABETICAL_INDEX" title=""/> 
-      <tab type="exceptionhierarchy" visible="yes" title="" intro=""/>
-    </tab>
-    <tab type="files" visible="yes" title="">
-      <tab type="filelist" visible="yes" title="" intro=""/>
-      <tab type="globals" visible="yes" title="" intro=""/>
-    </tab>
-    <tab type="examples" visible="yes" title="" intro=""/>  
-  </navindex>
-
-  <!-- Layout definition for a class page -->
-  <class>
-    <briefdescription visible="no"/>
-    <detaileddescription title=""/>
-    <includes visible="$SHOW_INCLUDE_FILES"/>
-    <inheritancegraph visible="$CLASS_GRAPH"/>
-    <collaborationgraph visible="$COLLABORATION_GRAPH"/>
-    <memberdecl>
-      <nestedclasses visible="yes" title=""/>
-      <publictypes title=""/>
-      <services title=""/>
-      <interfaces title=""/>
-      <publicslots title=""/>
-      <signals title=""/>
-      <publicmethods title=""/>
-      <publicstaticmethods title=""/>
-      <publicattributes title=""/>
-      <publicstaticattributes title=""/>
-      <protectedtypes title=""/>
-      <protectedslots title=""/>
-      <protectedmethods title=""/>
-      <protectedstaticmethods title=""/>
-      <protectedattributes title=""/>
-      <protectedstaticattributes title=""/>
-      <packagetypes title=""/>
-      <packagemethods title=""/>
-      <packagestaticmethods title=""/>
-      <packageattributes title=""/>
-      <packagestaticattributes title=""/>
-      <properties title=""/>
-      <events title=""/>
-      <privatetypes title=""/>
-      <privateslots title=""/>
-      <privatemethods title=""/>
-      <privatestaticmethods title=""/>
-      <privateattributes title=""/>
-      <privatestaticattributes title=""/>
-      <friends title=""/>
-      <related title="" subtitle=""/>
-      <membergroups visible="yes"/>
-    </memberdecl>
-    <memberdef>
-      <inlineclasses title=""/>
-      <typedefs title=""/>
-      <enums title=""/>
-      <services title=""/>
-      <interfaces title=""/>
-      <constructors title=""/>
-      <functions title=""/>
-      <related title=""/>
-      <variables title=""/>
-      <properties title=""/>
-      <events title=""/>
-    </memberdef>
-    <allmemberslink visible="yes"/>
-    <usedfiles visible="$SHOW_USED_FILES"/>
-    <authorsection visible="yes"/>
-  </class>
-
-  <!-- Layout definition for a namespace page -->
-  <namespace>
-    <briefdescription visible="yes"/>
-    <memberdecl>
-      <nestednamespaces visible="yes" title=""/>
-      <constantgroups visible="yes" title=""/>
-      <interfaces visible="yes" title=""/>
-      <classes visible="yes" title=""/>
-      <structs visible="yes" title=""/>
-      <exceptions visible="yes" title=""/>
-      <typedefs title=""/>
-      <sequences title=""/>
-      <dictionaries title=""/>
-      <enums title=""/>
-      <functions title=""/>
-      <variables title=""/>
-      <membergroups visible="yes"/>
-    </memberdecl>
-    <detaileddescription title=""/>
-    <memberdef>
-      <inlineclasses title=""/>
-      <typedefs title=""/>
-      <sequences title=""/>
-      <dictionaries title=""/>
-      <enums title=""/>
-      <functions title=""/>
-      <variables title=""/>
-    </memberdef>
-    <authorsection visible="yes"/>
-  </namespace>
-
-  <!-- Layout definition for a file page -->
-  <file>
-    <briefdescription visible="yes"/>
-    <includes visible="$SHOW_INCLUDE_FILES"/>
-    <includegraph visible="$INCLUDE_GRAPH"/>
-    <includedbygraph visible="$INCLUDED_BY_GRAPH"/>
-    <sourcelink visible="yes"/>
-    <memberdecl>
-      <interfaces visible="yes" title=""/>
-      <classes visible="yes" title=""/>
-      <structs visible="yes" title=""/>
-      <exceptions visible="yes" title=""/>
-      <namespaces visible="yes" title=""/>
-      <constantgroups visible="yes" title=""/>
-      <defines title=""/>
-      <typedefs title=""/>
-      <sequences title=""/>
-      <dictionaries title=""/>
-      <enums title=""/>
-      <functions title=""/>
-      <variables title=""/>
-      <membergroups visible="yes"/>
-    </memberdecl>
-    <detaileddescription title=""/>
-    <memberdef>
-      <inlineclasses title=""/>
-      <defines title=""/>
-      <typedefs title=""/>
-      <sequences title=""/>
-      <dictionaries title=""/>
-      <enums title=""/>
-      <functions title=""/>
-      <variables title=""/>
-    </memberdef>
-    <authorsection/>
-  </file>
-
-  <!-- Layout definition for a group page -->
-  <group>
-    <briefdescription visible="yes"/>
-    <groupgraph visible="$GROUP_GRAPHS"/>
-    <memberdecl>
-      <nestedgroups visible="yes" title=""/>
-      <dirs visible="yes" title=""/>
-      <files visible="yes" title=""/>
-      <namespaces visible="yes" title=""/>
-      <classes visible="yes" title=""/>
-      <defines title=""/>
-      <typedefs title=""/>
-      <sequences title=""/>
-      <dictionaries title=""/>
-      <enums title=""/>
-      <enumvalues title=""/>
-      <functions title=""/>
-      <variables title=""/>
-      <signals title=""/>
-      <publicslots title=""/>
-      <protectedslots title=""/>
-      <privateslots title=""/>
-      <events title=""/>
-      <properties title=""/>
-      <friends title=""/>
-      <membergroups visible="yes"/>
-    </memberdecl>
-    <detaileddescription title=""/>
-    <memberdef>
-      <pagedocs/>
-      <inlineclasses title=""/>
-      <defines title=""/>
-      <typedefs title=""/>
-      <sequences title=""/>
-      <dictionaries title=""/>
-      <enums title=""/>
-      <enumvalues title=""/>
-      <functions title=""/>
-      <variables title=""/>
-      <signals title=""/>
-      <publicslots title=""/>
-      <protectedslots title=""/>
-      <privateslots title=""/>
-      <events title=""/>
-      <properties title=""/>
-      <friends title=""/>
-    </memberdef>
-    <authorsection visible="yes"/>
-  </group>
-
-  <!-- Layout definition for a directory page -->
-  <directory>
-    <briefdescription visible="yes"/>
-    <directorygraph visible="yes"/>
-    <memberdecl>
-      <dirs visible="yes"/>
-      <files visible="yes"/>
-    </memberdecl>
-    <detaileddescription title=""/>
-  </directory>
-</doxygenlayout>