From f3dfbe49580bd2c850311978d1c39513b5ac2b74 Mon Sep 17 00:00:00 2001
From: Martin <martin.krauser@gmail.com>
Date: Fri, 4 Mar 2022 22:05:04 +0100
Subject: [PATCH] now it compiles

---
 osc32bt/osc32bt.ino | 62 +++++++++++++++++++++++----------------------
 1 file changed, 32 insertions(+), 30 deletions(-)

diff --git a/osc32bt/osc32bt.ino b/osc32bt/osc32bt.ino
index 8390fee..149ecd6 100644
--- a/osc32bt/osc32bt.ino
+++ b/osc32bt/osc32bt.ino
@@ -91,6 +91,38 @@ OSCMessage emsg("/error/");
 OSCMessage kmsg("/keys/");
 OSCMessage quaternionMessage("/quaternion/");
 OSCMessage quaternionDiffMessage("/quaternionDiff/");
+OSCMessage eulerDiffMessage("eulerDiff");
+
+BLA::Matrix<3> eulerFromQuaternion(Quaternion q) {
+  float x2 = q.x + q.x; float y2 = q.y + q.y; float z2 = q.z + q.z;
+  float xx = q.x * x2; float xy = q.x * y2; float xz = q.x * z2;
+  float yy = q.y * y2; float yz = q.y * z2; float zz = q.z * z2;
+  float wx = q.w * x2; float wy = q.w * y2; float wz = q.w * z2;
+
+  BLA::Matrix<4,4> rotationMatrix = {
+    1 - (yy + zz), xy + wz, xz - wy, 0,
+    xy - wz,  1 - ( xx + zz ), yz + wx, 0,
+    xz + wy,  yz - wx, 1 - ( xx + yy ), 0,
+    0,  0,  0,  1
+  };
+
+//TODO: test whether BLA library uses column-major matrix notation in code
+  BLA::Matrix<3> eulerVector;
+  eulerVector.Fill(0);
+  eulerVector(1) = asin(clamp(rotationMatrix(1,3),-1,1));
+  if (fabsf(rotationMatrix(1,3)) < 0.9999999) {
+    eulerVector(0) = atan2f(-rotationMatrix(2,3), rotationMatrix(3,3));
+    eulerVector(2) = atan2f( -rotationMatrix(1,2), rotationMatrix(1,1));
+  } else {
+    eulerVector(0) = atan2f(rotationMatrix(3,2), rotationMatrix(2,2));
+    eulerVector(2) = 0;
+  }
+  return eulerVector;
+}
+
+float clamp(float value,float min,float max) {
+  return fmaxf( min, fminf(max, value));
+}
 
 void setup() {
   Wire.begin();
@@ -313,33 +345,3 @@ void loop() {
 }
 
 
-BLA::Matrix<3> eulerFromQuaternion(Quaternion q) {
-  float x2 = q.x + q.x; float y2 = q.y + q.y; float z2 = q.z + q.z;
-  float xx = q.x * x2; float xy = q.x * y2; float xz = q.x * z2;
-  float yy = q.y * y2; float yz = q.y * z2; float zz = q.z * z2;
-  float wx = q.w * x2; float wy = q.w * y2; float wz = q.w * z2;
-
-  BLA::Matrix<4,4> rotationMatrix = {
-    1 - (yy + zz), xy + wz, xz - wy, 0,
-    xy - wz,  1 - ( xx + zz ), yz + wx, 0,
-    xz + wy,  yz - wx, 1 - ( xx + yy ), 0,
-    0,  0,  0,  1
-  };
-
-//TODO: test whether BLA library uses column-major matrix notation in code
-  BLA::Matrix<3> eulerVector;
-  eulerVector.Fill(0);
-  eulerVector(1) = asin(clamp(rotationMatrix(1,3),-1,1));
-  if (fabsf(rotationMatrix(1,3)) < 0.9999999) {
-    eulerVector(0) = atan2f(-rotationMatrix(2,3), rotationMatrix(3,3));
-    eulerVector(2) = atan2f( -rotationMatrix(1,2), rotationMatrix(1,1));
-  } else {
-    eulerVector(0) = atan2f(rotationMatrix(3,2), rotationMatrix(2,2));
-    eulerVector(2) = 0;
-  }
-  return eulerVector;
-}
-
-float clamp(float value,float min,float max) {
-  return fmaxf( min, fminf(max, value));
-}
\ No newline at end of file