From 825fa13c36e59355eee799e609e7a98746edb99e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jurij=20Podgor=C5=A1ek?= <jurij@kompot.si>
Date: Thu, 26 Sep 2024 15:04:21 +0200
Subject: [PATCH] Kotni vektorji, kup popravkov

---
 AHRSensor/AHRSensor.sc | 284 ++++++++++++++++++++++++++++++-----------
 1 file changed, 208 insertions(+), 76 deletions(-)

diff --git a/AHRSensor/AHRSensor.sc b/AHRSensor/AHRSensor.sc
index a728a6e..3a50673 100644
--- a/AHRSensor/AHRSensor.sc
+++ b/AHRSensor/AHRSensor.sc
@@ -1,13 +1,21 @@
 AHRSensor {
   var <id,
-  <parent,
-
-  // Kalibracijski quaternion (kao nula)
-  <>calibQuat,
+  <>parent,
 
+  // Surov kvaternion iz senzorjev
   <>quat,
+  // Kalibracijski quaternion (kao "nula")
+  <>refQuat,
+  // Relativni kvaternion (razlika od parenta)
+  <relQuat,
+  // Razlika od prejsnjega do trenutnega quaterniona
+  <quatDiff,
+
   <>euler,
-  <>eulerD,
+  // "Prednji" vektor
+  <>vecF,
+  // "Zgornji" vektor
+  <>vecT,
   <>accel,
   <>accelSum,
 
@@ -20,24 +28,42 @@ AHRSensor {
   <gQx,
   <gQy,
   <gQz,
-  // Kalibriraj quaternion
-  <gQc,
+
+  // Referencni quaternion
+  <gQr,
+
   // Eulerjevi koti
   <gEx,
   <gEy,
   <gEz,
-  // Relativni eulerjevi koti
-  <gEdx,
-  <gEdy,
-  <gEdz,
+  <gExl,
+  <gEyl,
+  <gEzl,
+
+  // Rotacijski vektor
+  <gVfx,
+  <gVfy,
+  <gVfz,
+  <gVfxl,
+  <gVfyl,
+  <gVfzl,
+  <gVtx,
+  <gVty,
+  <gVtz,
+  <gVtxl,
+  <gVtyl,
+  <gVtzl,
+
   // Pospeskomer
   <gAx,
   <gAy,
   <gAz,
   <gAs,
+
   // Baterija
   <gB,
   <gBi,
+
   // Dogodkov na sekundo
   <gEps;
 
@@ -48,16 +74,22 @@ AHRSensor {
   quat2euler { |quat|
     // Quaternion
     //var w = quat.a, x = quat.b, y = quat.c, z = quat.d,
-    //var q = [quat.b, quat.c, quat.d, quat.a],
+
+    // Quaternion v supercolliderju gre po zaporedju wxyz = abcd
+    // Iz foruma (BNO055): x = -y, y = x, z = -z
     var q = [quat.a, quat.b, quat.c, quat.d],
+    //var q = [quat.a, quat.c.neg, quat.b, quat.d.neg],
     // Euler angles; roll, pitch and yaw
     e = [0, 0, 0];
 
-    /**/
+    e[0] = atan2((2 * q[2] * q[0]) - (2 * q[1] * q[3]), 1 - (2 * q[2] * q[2]) - (2 * q[3] * q[3]));
+    e[1] = asin((2 * q[1] * q[2]) + (2 * q[3] * q[0]));
+    e[2] = atan2((2 * q[1] * q[0]) - (2 * q[2] * q[3]), 1 - (2 * q[1] * q[1]) - (2 * q[3] * q[3]));
+    /*
     e[0] = atan2( (2 * q[1] * q[2]) - (2 * q[0] * q[3]), (2 * q[0]*q[0]) + (2 * q[1] * q[1]) - 1); // psi
     e[1] = asin( (2 * q[1] * q[3]) + (2 * q[0] * q[2])).neg; // theta
     e[2] = atan2( (2 * q[2] * q[3]) - (2 * q[0] * q[1]), (2 * q[0] * q[0]) + (2 * q[3] * q[3]) - 1); // phi
-    /**/
+    */
 
     /*
     // Variables
@@ -116,12 +148,15 @@ AHRSensor {
   }
 
   init {
-	quat = Quaternion;
-	calibQuat = Quaternion.new(1, 0, 0, 0);
+	quat = Quaternion.new(1, 0, 0, 0);
+	refQuat = Quaternion.new(1, 0, 0, 0);
+    relQuat = Quaternion.new(1, 0, 0, 0);
 	euler = [0, 0, 0];
 	accel = [0, 0, 0];
 	battery = 0;
     eps = 0;
+    vecF = Cartesian.new(1, 0, 0);
+    vecT = Cartesian.new(0, 0, 1);
 
 	this.guiInit;
   }
@@ -140,11 +175,23 @@ AHRSensor {
 	gEx = StaticText().string_(0).stringColor_(cRed);
 	gEy = StaticText().string_(0).stringColor_(cGreen);
 	gEz = StaticText().string_(0).stringColor_(cBlue);
+    gExl = LevelIndicator();
+    gEyl = LevelIndicator();
+    gEzl = LevelIndicator();
 
-    // Razlika v eulerju
-	gEdx = StaticText().string_(0).stringColor_(cRed);
-	gEdy = StaticText().string_(0).stringColor_(cGreen);
-	gEdz = StaticText().string_(0).stringColor_(cBlue);
+	gVfx = StaticText().string_(0).stringColor_(cRed);
+	gVfy = StaticText().string_(0).stringColor_(cGreen);
+	gVfz = StaticText().string_(0).stringColor_(cBlue);
+    gVfxl = LevelIndicator();
+    gVfyl = LevelIndicator();
+    gVfzl = LevelIndicator();
+
+	gVtx = StaticText().string_(0).stringColor_(cRed);
+	gVty = StaticText().string_(0).stringColor_(cGreen);
+	gVtz = StaticText().string_(0).stringColor_(cBlue);
+    gVtxl = LevelIndicator();
+    gVtyl = LevelIndicator();
+    gVtzl = LevelIndicator();
 
 	//gAx = StaticText().string_(0).stringColor_(cRed);
 	//gAy = StaticText().string_(0).stringColor_(cGreen);
@@ -159,8 +206,8 @@ AHRSensor {
 	gQy = StaticText().string_(0).stringColor_(cGreen);
 	gQz = StaticText().string_(0).stringColor_(cBlue);
 
-	gQc = Button().string_("cal").action_({ |butt|
-      this.calibrateQuat;
+	gQr = Button().string_("ref").action_({ |butt|
+      this.setRefQuat;
     });
 
 	gB = StaticText().string_(0).stringColor_(cRed);
@@ -170,19 +217,21 @@ AHRSensor {
 
   getGui {
     var napis = "Sensor " ++ id, elementi;
-    if (parent != nil) { napis = napis ++ " (parent " ++ parent ++ ")"};
+    if (parent != nil) { napis = napis ++ " (parent " ++ parent.id ++ ")"};
 
 	elementi = [
       [
         StaticText().font_(Font("OpenSans", 12, true)).string_(napis),
         nil,
         [StaticText().string_("bat: "), align: \right],
-        gB,
+        [gB, columns: 2],
         gBi,
+        nil,
         [StaticText().string_("events/s: "), align: \right],
         gEps,
-        gQc
+        gQr
       ],
+      /*
 	  [
 		StaticText().string_("quaternion: "),
 		[StaticText().string_("w: "), align: \right],
@@ -194,77 +243,162 @@ AHRSensor {
 		[StaticText().string_("z: "), align: \right],
 		gQz,
 	  ],
+      */
+      [
+        [StaticText().string_("e"), align: \right],
+        [gExl, columns: 2],
+        [gEyl, columns: 2],
+        [gEzl, columns: 2],
+          gEx, gEy, gEz
+      ],
+      /*
       [ 
-		StaticText().string_("euler: "),
+		StaticText().string_("vec F: "),
 		[StaticText().string_("x: "), align: \right],
-		gEx,
+		gVfx,
 		[StaticText().string_("y: "), align: \right],
-		gEy,
+		gVfy,
 		[StaticText().string_("z: "), align: \right],
-		gEz,
+		gVfz
 	  ],
+      [
+		[StaticText().string_("vec F: "), align: \right],
+        [gVfxl, columns: 2],
+        [gVfyl, columns: 2],
+        [gVfzl, columns: 2]
+      ],
+      */
+      /*
+      [ 
+		StaticText().string_("vec T: "),
+		[StaticText().string_("x: "), align: \right],
+		gVtx,
+		[StaticText().string_("y: "), align: \right],
+		gVty,
+		[StaticText().string_("z: "), align: \right],
+		gVtz
+	  ],
+      [
+        [StaticText().string_("vec T:"), align: \right],
+        [gVtxl, columns: 2],
+        [gVtyl, columns: 2],
+        [gVtzl, columns: 2]
+      ],
+      */
 	  [ 
-		StaticText().string_("accel: "),
-		[StaticText().string_("x: "), align: \right],
-		gAx,
-		[StaticText().string_("y: "), align: \right],
-		gAy,
-		[StaticText().string_("z: "), align: \right],
-		gAz,
-		[StaticText().string_("sum: "), align: \right],
+		[StaticText().string_("accel: "), align: \right],
+		[gAx, columns: 2],
+		[gAy, columns: 2],
+		[gAz, columns: 2],
         gAs
-	  ],
-      if (parent != nil) {
-        [ 
-          StaticText().string_("euler d: "),
-          [StaticText().string_("x: "), align: \right],
-          gEdx,
-          [StaticText().string_("y: "), align: \right],
-          gEdy,
-          [StaticText().string_("z: "), align: \right],
-          gEdz,
-       ];
-      }
+	  ]
 	];
     ^elementi;
   }
 
-  calibrateQuat {
-    calibQuat = quat;
+  setQuat { |q|
+    var rq;
+    // Razlika od prejsnjega quaterniona
+    quatDiff = quat.conjugate * q;
+    // "Surov quat"
+    quat = q;
+    // Referencen quat
+    rq = refQuat.conjugate * q;
+    // Ce imamo parenta, shrani razliko, sicer samo referencen
+    relQuat = if ((parent != nil), {
+      parent.relQuat.conjugate * rq;
+      // TODO ali bi tudi childe poapdejtali? Mislim, da ni treba, ker dokaj pogosto pride nov q
+    }, {
+      rq;
+    });
   }
 
-  updateEuler { |newQuat|
-    euler = this.quat2euler(newQuat);
+  setRefQuat {
+    refQuat = quat;
+    euler = [0, 0, 0];
   }
 
-  updateEulerD { |newQuat|
-    eulerD = this.quat2euler(newQuat);
+  updateEuler { |q|
+    // Normalize first ?
+    euler = [q.tilt, q.tumble, q.rotate];
+    //euler = euler + [quatDiff.tilt, quatDiff.tumble, quatDiff.rotate];
+    //euler = this.quat2euler(newQuat);
+  }
+
+  rotateVector { |q|
+    // rotiramo vektor s quaternionom
+    var vF = Cartesian.new(1, 0, 0), vT = Cartesian.new(0, 0, 1),
+        u, s, newVecF, newVecT;
+
+    u = Cartesian.new(q.b, q.c, q.d);
+    s = q.a;
+
+    vecF = (
+      (u * (2.0 * this.vecDotProd(u, vF)))
+      + (vF * ((s * s) - this.vecDotProd(u, u)))
+    ) + (
+      this.vecCrossProd(u, vF) * (2.0 * s)
+    );
+
+    vecT = (
+      (u * (2.0 * this.vecDotProd(u, vT)))
+      + (vT * ((s * s) - this.vecDotProd(u, u)))
+    ) + (
+      this.vecCrossProd(u, vT) * (2.0 * s)
+    );
+  }
+
+  vecDotProd { |v1, v2|
+    ^((v1.x * v2.x) + (v1.y * v2.y) + (v1.z * v2.z));
+  }
+
+  vecCrossProd { |v1, v2|
+    ^Cartesian.new(
+      (v1.y * v2.z) - (v1.z * v2.y),
+      (v1.z * v2.x) - (v1.x * v2.z),
+      (v1.x * v2.y) - (v1.y * v2.x));
+  }
+
+  getDecimal { |num|
+    // Stevilo decimalk
+    var div = 0.01, rem;
+    rem = num % div;
+    ^(num - rem);
   }
 
   refreshGuiQuat {
-    // Stevilo decimalk
-    var prec = 3;
-	gQw.string_(quat.a.asStringPrec(prec));
-	gQx.string_(quat.b.asStringPrec(prec));
-	gQy.string_(quat.c.asStringPrec(prec));
-	gQz.string_(quat.d.asStringPrec(prec));
+	gQw.string_(this.getDecimal(quat.a));
+	gQx.string_(this.getDecimal(quat.b));
+	gQy.string_(this.getDecimal(quat.c));
+	gQz.string_(this.getDecimal(quat.d));
   }
 
   refreshGuiEuler {
-    // Stevilo decimalk
-    var prec = 3;
-	gEx.string_(euler[0].asStringPrec(prec));
-	gEy.string_(euler[1].asStringPrec(prec));
-	gEz.string_(euler[2].asStringPrec(prec));
+	gEx.string_(this.getDecimal(euler[0]));
+	gEy.string_(this.getDecimal(euler[1]));
+	gEz.string_(this.getDecimal(euler[2]));
+	gExl.value_(sin(euler[0]).linlin(-1, 1, 0, 1));
+	gEyl.value_(sin(euler[1]).linlin(-1, 1, 0, 1));
+	gEzl.value_(sin(euler[2]).linlin(-1, 1, 0, 1));
   }
-  refreshGuiEulerD {
-    // Stevilo decimalk
-    var prec = 3;
-	gEdx.string_(eulerD[0].asStringPrec(prec));
-	gEdy.string_(eulerD[1].asStringPrec(prec));
-	gEdz.string_(eulerD[2].asStringPrec(prec));
+
+  refreshGuiVector {
+	gVfx.string_(this.getDecimal(vecF.x));
+	gVfy.string_(this.getDecimal(vecF.y));
+	gVfz.string_(this.getDecimal(vecF.z));
+	gVfxl.value_(vecF.x.linlin(-1, 1, 0, 1));
+	gVfyl.value_(vecF.y.linlin(-1, 1, 0, 1));
+	gVfzl.value_(vecF.z.linlin(-1, 1, 0, 1));
+
+	gVtx.string_(this.getDecimal(vecT.x));
+	gVty.string_(this.getDecimal(vecT.y));
+	gVtz.string_(this.getDecimal(vecT.z));
+	gVtxl.value_(vecT.x.linlin(-1, 1, 0, 1));
+	gVtyl.value_(vecT.y.linlin(-1, 1, 0, 1));
+	gVtzl.value_(vecT.z.linlin(-1, 1, 0, 1));
   }
 
+
   refreshGuiAccel {
     // Stevilo decimalk
     //var prec = 2;
@@ -275,13 +409,11 @@ AHRSensor {
     [gAx, gAy, gAz].do({|el, i|
       el.value_(accel[i].linlin(from, to, 0, 1));
     });
-    gAs.value_(accelSum.linlin(0, 100, 0, 1));
+    gAs.value_(accelSum.linlin(0, 50, 0, 1));
   }
 
   refreshGuiBat {
-    // Stevilo decimalk
-    var prec = 2;
-	gB.string_(battery.asStringPrec(prec));
+	gB.string_(this.getDecimal(battery));
     gBi.value_(battery.linlin(3.6, 4.2, 0, 1));
   }