From 730720d6e22fd49ade45aa132ed445f89d18b7d1 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Tue, 5 Mar 2024 09:55:29 +0100
Subject: [PATCH] Using Discrete angles

---
 LinearAlgebra   |  2 +-
 NetworkSync.cpp | 25 ++++++++++++++++++++++---
 NetworkSync.h   |  9 ++++++---
 3 files changed, 29 insertions(+), 7 deletions(-)

diff --git a/LinearAlgebra b/LinearAlgebra
index c016404..3714507 160000
--- a/LinearAlgebra
+++ b/LinearAlgebra
@@ -1 +1 @@
-Subproject commit c01640429c3c9829e81f38788a6c5973ee84eb60
+Subproject commit 3714507059d558c586685e1af286adc20356eb60
diff --git a/NetworkSync.cpp b/NetworkSync.cpp
index 9e763f5..1cf50b9 100644
--- a/NetworkSync.cpp
+++ b/NetworkSync.cpp
@@ -4,13 +4,31 @@
 #include <Arduino.h>
 #endif
 
-void NetworkSync::SendVector3(unsigned char *data, int startIndex, Vector3 v) {
+#include "LinearAlgebra/DiscreteAngle.h"
+
+void NetworkSync::SendVector3(unsigned char *data, const int startIndex,
+                              const Vector3 v) {
   SendSingle100(data, startIndex, v.x);
   SendSingle100(data, startIndex + 4, v.y);
   SendSingle100(data, startIndex + 8, v.z);
 }
 
-void NetworkSync::SendSingle100(unsigned char *data, int startIndex,
+void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex,
+                                 const Quaternion q) {
+  Vector3 angles = Quaternion::ToAngles(q);
+  int ix = startIndex;
+  SendAngle(data, ix++, angles.x);
+  SendAngle(data, ix++, angles.y);
+  SendAngle(data, ix, angles.z);
+}
+
+void NetworkSync::SendAngle(unsigned char *data, const int startIndex,
+                            float angle) {
+  AngleUsing<unsigned char> packedAngle = AngleUsing<unsigned char>(angle);
+  data[startIndex] = packedAngle.GetValue();
+}
+
+void NetworkSync::SendSingle100(unsigned char *data, const int startIndex,
                                 float value) {
   // Sends a float with truncated 2 decimal precision
   Int32 intValue = value * 100;
@@ -20,7 +38,8 @@ void NetworkSync::SendSingle100(unsigned char *data, int startIndex,
   // }
 }
 
-void NetworkSync::SendInt32(unsigned char *data, int startIndex, Int32 value) {
+void NetworkSync::SendInt32(unsigned char *data, const int startIndex,
+                            Int32 value) {
   for (unsigned char ix = 0; ix < 4; ix++) {
     data[startIndex + ix] = ((unsigned char *)&value)[ix];
   }
diff --git a/NetworkSync.h b/NetworkSync.h
index 2805685..71805f5 100644
--- a/NetworkSync.h
+++ b/NetworkSync.h
@@ -50,9 +50,12 @@ protected:
   NetworkPerception *networkPerception;
   void PublishTrackedObject(SendBuffer sendBuffer, InterestingThing *object);
 
-  void SendVector3(unsigned char *data, int startIndex, Vector3 v);
-  void SendSingle100(unsigned char *data, int startIndex, float value);
-  void SendInt32(unsigned char *data, int startIndex, Int32 value);
+  void SendSingle100(unsigned char *data, const int startIndex, float value);
+  void SendInt32(unsigned char *data, const int startIndex, Int32 value);
+  void SendAngle(unsigned char *data, const int startIndex, float value);
+  void SendVector3(unsigned char *data, const int startIndex, const Vector3 v);
+  void SendQuaternion(unsigned char *data, const int startIndex,
+                      const Quaternion q);
 };
 
 } // namespace RoboidControl