From 577df7dc1a160e81e0c4f213b8e86cb26a629b0d Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Fri, 15 Mar 2024 17:36:34 +0100
Subject: [PATCH] Prepare for orienation sync

---
 NetworkSync.cpp | 18 ++++++++++++++++++
 NetworkSync.h   |  1 +
 2 files changed, 19 insertions(+)

diff --git a/NetworkSync.cpp b/NetworkSync.cpp
index 1cf50b9..353c336 100644
--- a/NetworkSync.cpp
+++ b/NetworkSync.cpp
@@ -22,6 +22,24 @@ void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex,
   SendAngle(data, ix, angles.z);
 }
 
+void NetworkSync::SendQuat32(unsigned char *data, int startIndex,
+                             const Quaternion q) {
+  unsigned char qx = (char)(q.x * 127 + 128);
+  unsigned char qy = (char)(q.y * 127 + 128);
+  unsigned char qz = (char)(q.z * 127 + 128);
+  unsigned char qw = (char)(q.w * 255);
+  if (q.w < 0) {
+    qx = -qx;
+    qy = -qy;
+    qz = -qz;
+    qw = -qw;
+  }
+  data[startIndex++] = qx;
+  data[startIndex++] = qy;
+  data[startIndex++] = qz;
+  data[startIndex++] = qw;
+}
+
 void NetworkSync::SendAngle(unsigned char *data, const int startIndex,
                             float angle) {
   AngleUsing<unsigned char> packedAngle = AngleUsing<unsigned char>(angle);
diff --git a/NetworkSync.h b/NetworkSync.h
index 71805f5..7495116 100644
--- a/NetworkSync.h
+++ b/NetworkSync.h
@@ -56,6 +56,7 @@ protected:
   void SendVector3(unsigned char *data, const int startIndex, const Vector3 v);
   void SendQuaternion(unsigned char *data, const int startIndex,
                       const Quaternion q);
+  void SendQuat32(unsigned char *data, int startIndex, const Quaternion q);
 };
 
 } // namespace RoboidControl