From bd9ace61e9724c8688a7dc57823b3c2db2feddaa Mon Sep 17 00:00:00 2001
From: Luca Warmenhoven <luca.warmenhoven@hva.nl>
Date: Wed, 29 May 2024 15:55:37 +0200
Subject: [PATCH] Added more logging for debugging functionality

---
 .../PersonalMotionPreviewElement.java         | 24 +++++++++++++++++--
 .../util/processing/MotionProcessor.java      | 14 +++++++----
 2 files changed, 31 insertions(+), 7 deletions(-)

diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/components/PersonalMotionPreviewElement.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/components/PersonalMotionPreviewElement.java
index d498761..eca81f6 100644
--- a/code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/components/PersonalMotionPreviewElement.java
+++ b/code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/components/PersonalMotionPreviewElement.java
@@ -24,6 +24,7 @@ import org.joml.Vector4f;
 
 import java.util.ArrayList;
 import java.util.List;
+import java.util.concurrent.ConcurrentLinkedQueue;
 import java.util.concurrent.atomic.AtomicInteger;
 import java.util.concurrent.atomic.AtomicReference;
 
@@ -48,8 +49,11 @@ public class PersonalMotionPreviewElement extends View {
 
     private Matrix4f modelViewMatrix = new Matrix4f();  // The model view matrix for the 3D to 2D transformation.
     private Matrix4f projectionMatrix = new Matrix4f(); // The projection matrix for the 3D to 2D transformation.
-    private final Vector4f objectPosition = new Vector4f(0, 0, -1, 1); // The location of the object in 3D space.
-    private List<Vector2f> vectors = new ArrayList<>();
+    private final Vector4f objectPosition = new Vector4f(0, 0, -2, 1); // The location of the object in 3D space.
+    private ConcurrentLinkedQueue<Vector2f> vectors = new ConcurrentLinkedQueue<>();
+
+    private Vector2f[] axisVectors;
+
 
 
     private static final String[] USER_PHRASES = {
@@ -99,6 +103,17 @@ public class PersonalMotionPreviewElement extends View {
 
         this.exercise = exercise;
         this.paths = exercise.getPath();
+
+        this.axisVectors = new Vector2f[] {
+
+                projectVertex(new Vector3f(-1.0f, 0, 0), getWidth(), getHeight()),
+                projectVertex(new Vector3f(1.0f, 0, 0), getWidth(), getHeight()),
+                projectVertex(new Vector3f(0, -1.0f, 0), getWidth(), getHeight()),
+                projectVertex(new Vector3f(0, 1.0f, 0), getWidth(), getHeight()),
+                projectVertex(new Vector3f(0, 0, -1.0f), getWidth(), getHeight()),
+                projectVertex(new Vector3f(0, 0, 1.0f), getWidth(), getHeight())
+
+        };
     }
 
     public void onDestroy()
@@ -188,6 +203,11 @@ public class PersonalMotionPreviewElement extends View {
         if (this.exercise == null)
             return;
 
+        for (int i = 0; i < axisVectors.length/2; i++)
+        {
+            canvas.drawLine(axisVectors[i*2].x, axisVectors[i*2].y, axisVectors[i*2+1].x, axisVectors[i*2+1].y, this.targetPaint);
+        }
+
         for ( Vector2f point : this.vectors)
         {
             canvas.drawRect(point.x, point.y, point.x + 5, point.y + 5, this.referencePaint);
diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/MotionProcessor.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/MotionProcessor.java
index 7264805..016a520 100644
--- a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/MotionProcessor.java
+++ b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/MotionProcessor.java
@@ -9,6 +9,8 @@ import com.google.gson.JsonObject;
 import com.google.gson.JsonParser;
 
 import org.jetbrains.annotations.NotNull;
+import org.joml.Matrix3d;
+import org.joml.Vector3d;
 import org.joml.Vector3f;
 
 import java.util.ArrayList;
@@ -24,7 +26,7 @@ public class MotionProcessor {
 
     private Vector3f ZERO = new Vector3f(0, 0, 0);
 
-    private final float sampleRate = 10.0F; // samples/second
+    private final float sampleRate = 1.0f / 10.0F; // samples/second
 
     private IMotionDataConsumer motionDataConsumer = (p1, p2, p3, p4, p5) -> { };
     private WebServer server;
@@ -164,12 +166,14 @@ public class MotionProcessor {
         // Rotate the acceleration vector back by the rotation vector to make it
         // perpendicular to the gravity vector, then apply double integration to get the relative position.
         // s = 1/2 * a * t^2
+        // Step 2: Create rotation matrices for each axis
+        // Step 4: Rotate the acceleration vector
+
         return motionData.acceleration
-                .rotateX(-motionData.rotation.x)
-                .rotateY(-motionData.rotation.y)
                 .rotateZ(-motionData.rotation.z)
-                .div(2)
-                .mul(sampleRate * sampleRate);
+                .rotateY(-motionData.rotation.y)
+                .rotateX(-motionData.rotation.x)
+                .mul(sampleRate * sampleRate / 2);
     }
 
     /**