From 09366aa962ddde2c4b9962c8c4f1ec81e89d60bd Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Thu, 18 Apr 2024 12:30:15 +0200
Subject: [PATCH] Add SendPolar

---
 NetworkSync.cpp | 33 ++++++++++++++++++---------------
 NetworkSync.h   |  6 ++++--
 2 files changed, 22 insertions(+), 17 deletions(-)

diff --git a/NetworkSync.cpp b/NetworkSync.cpp
index 194656c..c5b556d 100644
--- a/NetworkSync.cpp
+++ b/NetworkSync.cpp
@@ -6,10 +6,7 @@
 #include <Arduino.h>
 #endif
 
-#include "LinearAlgebra/Angle.h"
-// #include "LinearAlgebra/Angle16.h"
-// #include "LinearAlgebra/Angle32.h"
-// #include "LinearAlgebra/AngleUsing.h"
+#include "LinearAlgebra/Angle8.h"
 #include "LinearAlgebra/Spherical.h"
 
 void NetworkSync::SendVector3(unsigned char *data, unsigned int startIndex,
@@ -23,16 +20,22 @@ 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);
+  SendAngle8(data, ix++, angles.x);
+  SendAngle8(data, ix++, angles.y);
+  SendAngle8(data, ix++, angles.z);
+}
+
+void NetworkSync::SendPolar(unsigned char *data, unsigned char *startIndex,
+                            Polar p) {
+  SendAngle8(data, *startIndex, (const float)p.angle);
+  SendSingle100(data, (*startIndex) + 1, p.distance);
 }
 
 void NetworkSync::SendSpherical(unsigned char *data, int startIndex,
                                 Spherical s) {
-  SendAngle(data, startIndex++, s.horizontalAngle);
-  SendAngle(data, startIndex++, s.verticalAngle);
-  SendAngle(data, startIndex++, s.distance);
+  SendAngle8(data, startIndex++, s.horizontalAngle);
+  SendAngle8(data, startIndex++, s.verticalAngle);
+  SendAngle8(data, startIndex++, s.distance);
 }
 
 void NetworkSync::SendSpherical16(unsigned char *data, int startIndex,
@@ -68,11 +71,11 @@ void NetworkSync::SendQuat32(unsigned char *data, int startIndex,
   data[startIndex++] = qw;
 }
 
-// void NetworkSync::SendAngle(unsigned char *data, unsigned int startIndex,
-//                             const float angle) {
-//   AngleUsing<unsigned char> packedAngle = AngleUsing<unsigned char>(angle);
-//   data[startIndex] = packedAngle.GetValue();
-// }
+void NetworkSync::SendAngle8(unsigned char *data, unsigned int startIndex,
+                             const float angle) {
+  AngleUsing<signed char> packedAngle = AngleUsing<signed char>(angle);
+  data[startIndex] = packedAngle.GetValue();
+}
 
 // void NetworkSync::SendAngle16(unsigned char *data, unsigned int startIndex,
 //                               const float angle) {
diff --git a/NetworkSync.h b/NetworkSync.h
index efca1e9..914a9d0 100644
--- a/NetworkSync.h
+++ b/NetworkSync.h
@@ -55,8 +55,8 @@ protected:
 
   void SendSingle100(unsigned char *data, unsigned int startIndex, float value);
   void SendInt32(unsigned char *data, unsigned int startIndex, Int32 value);
-  void SendAngle(unsigned char *data, unsigned int startIndex,
-                 const float value);
+  void SendAngle8(unsigned char *data, unsigned int startIndex,
+                  const float value);
   void SendAngle16(unsigned char *data, unsigned int startIndex,
                    const float value);
   void SendAngle32(unsigned char *data, unsigned int startIndex,
@@ -65,6 +65,8 @@ protected:
                    const Vector3 v);
   void SendQuaternion(unsigned char *data, const int startIndex,
                       const Quaternion q);
+
+  void SendPolar(unsigned char *data, unsigned char *startIndex, Polar p);
   void SendSpherical(unsigned char *data, int startIndex, Spherical s);
   void SendSpherical16(unsigned char *data, int startIndex, Spherical s);
   void SendSpherical32(unsigned char *data, int startIndex, Spherical s);