Updated path math, started with 3d path to 2d screen space projection

code/src/Fitbot/app/build.gradle

@@ -34,6 +34,7 @@ dependencies {
     implementation 'com.android.support.constraint:constraint-layout:2.0.4'
     implementation 'com.android.support:cardview-v7:28.0.0'
     implementation 'com.android.support:design:28.0.0'
+    implementation 'org.joml:joml:1.10.5'
     testImplementation 'junit:junit:4.13.2'
     androidTestImplementation 'com.android.support.test:runner:1.0.2'
     androidTestImplementation 'com.android.support.test.espresso:espresso-core:3.0.2'

code/src/Fitbot/app/src/main/AndroidManifest.xml

@@ -14,10 +14,6 @@
         <activity
             android:name=".Completion_Screen"
             android:exported="false" />
-        <activity
-            android:name=".ui.SportMenuActivity"
-            android:exported="true">
-        </activity>
         <activity
             android:name=".BicepVideo"
             android:exported="false" />

code/src/Fitbot/app/src/main/java/com/example/fitbot/MainScreen.java

@@ -9,10 +9,6 @@ import android.support.v7.app.ActionBarDrawerToggle;
 import android.support.v7.app.AppCompatActivity;
 import android.support.v7.widget.Toolbar;
 
-import com.example.fitbot.util.processing.GesturePath;
-import com.example.fitbot.util.processing.MotionProcessor;
-import com.example.fitbot.util.processing.Vector3;
-
 public class MainScreen extends AppCompatActivity {
 
     //Variables

code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/components/PersonalMotionPreviewElement.java

@@ -5,39 +5,147 @@ import android.graphics.Canvas;
 import android.graphics.Path;
 import android.view.View;
 
-import com.example.fitbot.util.processing.GesturePath;
+import com.example.fitbot.util.path.GesturePath;
+import com.example.fitbot.util.path.PathSegment;
+import com.example.fitbot.util.path.Point3D;
 import com.example.fitbot.util.processing.MotionData;
 import com.example.fitbot.util.processing.MotionProcessor;
-import com.example.fitbot.util.processing.Vector3;
+
+import org.joml.Matrix4f;
+import org.joml.Vector2f;
+import org.joml.Vector3f;
+import org.joml.Vector4f;
 
 public class PersonalMotionPreviewElement extends View {
 
     private GesturePath path;
     private double pathTime = 0.0D; // The timestamp at which the path is currently at.
     private MotionProcessor motionProcessor;
-    private Path targetPath, personalPath;
+    private Path referencePath, performingPath;
 
     /**
-     * Method that calculates the path that will be drawn on the
+     * Constants for the preview path projection.
-     * canvas. This method will be called every time new motion data is received.
      */
-    private void calculateDrawingPath(Vector3 transformedVector, MotionData motionData, int sampleIndex, double sampleRate) {
+    private final float FOV = 70.0f;     // The field of view of the preview path
-
+    private final float Z_NEAR = 0.1f;   // The near clipping plane
-    }
+    private final float Z_FAR = 1000.0f; // The far clipping plane
-
+    private Vector3f cameraPosition = new Vector3f(0.0f, 0.0f, 0.0f); // The position of the camera
+    private Vector2f screenDimensions = new Vector2f(); // Width and height dimensions of the screen
+    private Vector2f rotation = new Vector2f(); // Rotation vector (yaw, pitch)
 
+    /**
+     * Constructor for the PersonalMotionPreviewElement class.
+     *
+     * @param context The context in which this element is created.
+     * @param path    The gesture path that will be drawn on the canvas.
+     */
     public PersonalMotionPreviewElement(Context context, GesturePath path) {
         super(context);
         this.path = path;
         this.motionProcessor = new MotionProcessor();
         this.motionProcessor.startListening();
-        this.motionProcessor.setMotionDataEventHandler(this::calculateDrawingPath);
+        this.motionProcessor.setMotionDataEventHandler((processed, preprocessed, sampleIndex, sampleRate) -> {
-        this.targetPath = new Path();
+            // TODO: Implement the calculation of the drawing path
-        this.personalPath = new Path();
+        });
+
+        this.referencePath = generatePath()
+        this.performingPath = new Path();
     }
 
+    /**
+     * Method that calculates the path that will be drawn on the
+     * canvas. This method will be called every time new motion data is received.
+     */
+    private void calculateDrawingPath(Point3D transformedVector, MotionData motionData, int sampleIndex, double sampleRate) {
+        // Recalculate the personal path based on the new motion data
+
+    }
+
+    /**
+     * Method for setting the rotation of the preview path.
+     *
+     * @param yaw The yaw rotation of the preview path.
+     * @param pitch The pitch rotation of the preview path.
+     */
+    public void setRotation(float yaw, float pitch) {
+        this.rotation.set(Math.toRadians(yaw), Math.toRadians(pitch));
+    }
+
+    /**
+     *  Method for projecting a 3D point onto the screen.
+     *  This method converts the 3D point to 2D space using a Model-View-Projection matrix transformation.
+     *
+     * @param point The point to cast to the screen.
+     * @param virtualWidth The width of the virtual screen.
+     *                     This is used to normalize the screen coordinates.
+     * @param virtualHeight The height of the virtual screen.
+     * @return The transformed vector in screen coordinates ranging from (0, 0) to (virtualWidth, virtualHeight).
+     */
+    private Vector2f projectVertex(Vector3f point, int virtualWidth, int virtualHeight) {
+
+        Matrix4f modelViewMatrix = new Matrix4f()
+                .rotateX((float) Math.toRadians(rotation.x))
+                .rotateY((float) Math.toRadians(rotation.y))
+                .translate(cameraPosition);
+
+        Matrix4f projectionMatrix = new Matrix4f()
+                .perspective((float) Math.toRadians(FOV), (float) virtualWidth / virtualHeight, Z_NEAR, Z_FAR);
+
+        // Calculate Model-View-Projection matrix
+        Matrix4f MVP = new Matrix4f()
+                .set(projectionMatrix)
+                .mul(modelViewMatrix);
+
+        // Convert to screen coordinates
+        Vector4f screenCoordinates = new Vector4f(point, 1.0f)
+                .mul(MVP);
+
+        // Normalize screen coordinates from (-1, 1) to (0, virtualWidth) and (0, virtualHeight)
+        float normalizedX = (screenCoordinates.x / screenCoordinates.w + 1.0f) * 0.5f * virtualWidth;
+        float normalizedY = (1.0f - screenCoordinates.y / screenCoordinates.w) * 0.5f * virtualHeight;
+
+        return new Vector2f(normalizedX, normalizedY);
+    }
+
+    /**
+     * Method that converts a sequence of vectors to a Path object.
+     * This path is a set of bezier curves that will be drawn on the canvas.
+     *
+     * @param curvature The curvature of the bezier curves.
+     *                  This number must be between 0 and 1, and it represents
+     *                  by how much the path segments will be curved.
+     *                  A value of 0 represents no curvature at all,
+     *                  while values closer to 1 approach full circular curvature.
+     * @param segments  The path segments in the path.
+     *                  These segments will be connected by bezier curves, which
+     *                  all have unique curvature values.
+     * @return The generated path object.
+     */
+    private Path getDrawablePath(double curvature, PathSegment... segments) {
+
+        Path calculatedPath = new Path();
+
+        // Starting point
+        Vector2f origin = projectVertex(segments[0].getStart(), getWidth(), getHeight());
+        calculatedPath.moveTo(origin.x, origin.y);
+
+        // Draw the path segments
+        for (PathSegment segment : segments) {
+            Vector2f startProjected = projectVertex(segment.getStart(), getWidth()/2, getHeight());
+            Vector2f endProjected = projectVertex(segment.getEnd(), getWidth()/2, getHeight());
+            calculatedPath.lineTo(startProjected.x, startProjected.y);
+            calculatedPath.lineTo(endProjected.x, endProjected.y);
+        }
+
+        return calculatedPath;
+    }
+
+
     @Override
     public void onDraw(Canvas canvas) {
         // Draw the sport preview canvas
+
+
+
     }
 }

code/src/Fitbot/app/src/main/java/com/example/fitbot/util/path/GesturePath.java

@@ -0,0 +1,124 @@
+package com.example.fitbot.util.path;
+
+import org.joml.Vector3f;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+public class GesturePath {
+
+    // The vectors that make up the path.
+    private final PathSegment[] segments;
+    private double curvature;
+
+    public GesturePath(Vector3f[] vectors) {
+        this(vectors, 0.0D);
+    }
+
+    /**
+     * Create a new gesture path with a given set of vectors and curvature.
+     *
+     * @param vectors The vectors that make up the path.
+     * @param curvature The curvature of the path.
+     */
+    public GesturePath(Vector3f[] vectors, double curvature)
+    {
+        if ( vectors.length < 2)
+            throw new IllegalArgumentException("A path must have at least two points.");
+
+        this.curvature = curvature;
+        this.segments = new PathSegment[vectors.length - 1];
+        for ( int i = 0; i < vectors.length - 1; i++)
+            segments[i] = new PathSegment(vectors[i], vectors[i + 1]);
+    }
+
+    /**
+     * Constructor for a GesturePath with provided PathSegments.
+     * @param segments The PathSegments to initialize the path with.
+     */
+    public GesturePath(PathSegment... segments) {
+        this.segments = segments;
+        this.curvature = 0.0d;
+    }
+
+    /**
+     * Getter method for retrieving the path segments of this GesturePath.
+     *
+     * @return The path segments.
+     */
+    public PathSegment[] getSegments() {
+        return segments;
+    }
+
+    /**
+     * Method for retrieving the closest path segment to a reference point.
+     *
+     * @param reference The reference point to find the closest path segment to.
+     * @return The closest path segment to the reference point.
+     */
+    public PathSegment closest(Vector3f reference) {
+        // If there's only one segment, return that one.
+        if ( segments.length == 1)
+            return segments[0];
+
+        return Arrays
+                .stream(segments)
+                .reduce(segments[0], (a, b) -> PathSegment.closer(a, b, reference));
+    }
+
+    /**
+     * Get the error between an arbitrary path segment and the reference point.
+     *
+     * @param referencePoint The reference point to calculate the error of.
+     * @return The error offset between the path and the reference point.
+     */
+    public double getError(Vector3f referencePoint) {
+        return closest(referencePoint).difference(referencePoint); // Get the closest segment and calculate the error.
+    }
+
+    // Builder class for the GesturePath object.
+    public static class Builder {
+
+        // List of vectors to add to the GesturePath object.
+        private final List<Vector3f> vectors;
+
+        /**
+         * Constructor for the Builder object.
+         *
+         * @param vectors The list of vectors to add.
+         */
+        public Builder(List<Vector3f> vectors) {
+            this.vectors = vectors;
+        }
+
+        /**
+         * Default constructor for the Builder object.
+         */
+        public Builder() {
+            this.vectors = new ArrayList<>();
+        }
+
+        /**
+         * Adds a vector to the GesturePath object.
+         *
+         * @param vector The vector to add.
+         * @return The Builder object.
+         */
+        public Builder addVector(Vector3f vector) {
+            vectors.add(vector);
+            return this;
+        }
+
+        /**
+         * Builds the GesturePath object.
+         *
+         * @return The GesturePath object.
+         */
+        public GesturePath build() {
+            return new GesturePath(vectors.toArray(new Vector3f[0]));
+        }
+
+    }
+
+}

code/src/Fitbot/app/src/main/java/com/example/fitbot/util/path/PathSegment.java

@@ -0,0 +1,175 @@
+package com.example.fitbot.util.path;
+
+import org.joml.Vector3f;
+
+public class PathSegment {
+
+    private final double curvature;
+    private final Vector3f start, end, normal;
+    private final double horizontalDistance;
+    private final double distance;
+    private final double vectorAngle; // The angle of the vector from the start to the end point.
+
+    /**
+     * Constructor for creating a PathSegment of two lines, with the normal vector
+     * pointing straight upwards relative to the line, with a curvature of 0.0.
+     *
+     * @param start The starting point of the line segment
+     * @param end The end point of the line segment.
+     */
+    public PathSegment(Vector3f start, Vector3f end) {
+        this.curvature = 0.0D;
+        this.start = start;
+        this.end = end;
+        this.horizontalDistance = Math.sqrt(Math.pow(end.x - start.x, 2) + Math.pow(end.z - start.z, 2));
+        this.vectorAngle = Math.atan2(end.y - start.y, horizontalDistance);
+        this.distance = start.distance(end);
+
+        // Normal vector calculation
+        double horizontalAngle = Math.atan2(end.z - start.z, end.x - start.x);
+        double verticalAngle = Math.atan2(end.y - start.y, horizontalDistance);
+        float sinVertical = (float)Math.sin(verticalAngle);
+        float cosVertical = (float)Math.cos(verticalAngle);
+        float sinHorizontal = (float)Math.sin(horizontalAngle);
+        float cosHorizontal = (float)Math.cos(horizontalAngle);
+        // The normal vector faces directly upward relative to the line between start and end.
+        // This means the normal vector is the perpendicular bisecting line from the line segment.
+        this.normal = new Vector3f(
+                sinVertical * cosHorizontal,
+                cosVertical,
+                sinVertical * sinHorizontal
+        );
+    }
+
+    /**
+     * Create a new path segment with a given start and end point.
+     *
+     * @param start     The start point of the path segment.
+     * @param end       The end point of the path segment.
+     *                  A value of 0 will result in the normal vector pointing upwards.
+     * @param normal The normal vector of the path segment.
+     *               This vector determines how the line curves, if it has a positive curvature.
+     * @param curvature The curvature of the path segment.
+     *                  This value is between -1 and 1, and is used to determine the curvature of the path,
+     *                  around the normal vector.
+     */
+    public PathSegment(Vector3f start, Vector3f end, Vector3f normal, double curvature) {
+        this.curvature = curvature;
+        this.start = start;
+        this.end = end;
+        this.horizontalDistance = Math.sqrt(Math.pow(end.x - start.x, 2) + Math.pow(end.z - start.z, 2));
+        this.vectorAngle = Math.atan2(end.y - start.y, horizontalDistance);
+        this.normal = normal;
+        this.distance = start.distance(end);
+    }
+
+    /**
+     * Method that interpolates between the start and end points of the path segment,
+     * depending on the curvature of the curve. If the curvature is unset, or set to 0
+     * then this method will use linear interpolation. Otherwise, it will use a curve.
+     *
+     * @param dst The destination vector to interpolate to.
+     * @param t   The interpolation value between 0 and 1.
+     */
+    public void interpolate(Vector3f dst, double t) {
+        t = Math.min(1.0D, Math.max(0.0D, t)); // Ensure boundaries
+
+        // If curvature is 0, use linear interpolation.
+        if (this.curvature == 0.0D) {
+            dst.set(start).lerp(end, (float) t);
+        } else {
+            // Interpolate over the ellipse.
+            double angle = t * Math.PI + vectorAngle; // Angle for on the ellipse
+            double cos = Math.cos(angle);
+            double sin = Math.sin(angle);
+
+            double
+
+            // Calculate the ellipse.
+
+
+        }
+    }
+
+    /**
+     * Method for returning the control point of the path segment.
+     * This point is the point that the path segment curves around.
+     *
+     * @return The control point of the path segment.
+     */
+    public Vector3f getControlPoint() {
+        return new Vector3f(
+                (this.start.x + this.end.x) / 2.0F,
+                (this.start.y + this.end.y) / 2.0F,
+                (this.start.z + this.end.z) / 2.0F
+        )
+                .add(this.normal
+                .mul((float) this.curvature * (float) this.distance / 2.0F));
+    }
+
+    /**
+     * Method for calculating the difference between the provided vector and the
+     * path segment, depending on the normal vector and the curvature.
+     * If the provided vector does not lie on the path segment, this method will return
+     * the linear distance to the path.
+     *
+     * @param other The vector to calculate the difference to.
+     * @return The difference between the vector and the path segment.
+     */
+    public double difference(Vector3f other) {
+        return 0.0D; // TODO: Implement.
+    }
+
+    /**
+     * Get the curvature of the path segment.
+     *
+     * @return The curvature of the path segment.
+     */
+    public double getVectorAngle() {
+        return this.vectorAngle;
+    }
+
+    /**
+     * Get the normal vector of the path segment.
+     *
+     * @return The normal vector of the path segment.
+     */
+    public Vector3f getStart() {
+        return start;
+    }
+
+    /**
+     * Get the end point of the path segment.
+     *
+     * @return The end point of the path segment.
+     */
+    public Vector3f getEnd() {
+        return end;
+    }
+
+    /**
+     * Method for returning the distance to the closest point on the path segment.
+     *
+     * @param reference The reference point to calculate the distance to.
+     * @return The distance to the closest point on the path segment.
+     */
+    public double distance(Vector3f reference) {
+        if ( this.start.distanceSquared(reference) > this.end.distanceSquared(reference))
+            return this.end.distance(reference);
+        return this.start.distance(reference);
+    }
+
+    /**
+     * Function for returning the closest path segment to a reference point.
+     *
+     * @param first The first path segment to compare.
+     * @param second The second path segment to compare.
+     * @param referencePoint The reference point to compare to.
+     * @return The closest path segment to the reference point.
+     */
+    public static PathSegment closer(PathSegment first, PathSegment second, Vector3f referencePoint) {
+        if (first.distance(referencePoint) < second.distance(referencePoint))
+            return first;
+        return second;
+    }
+}

code/src/Fitbot/app/src/main/java/com/example/fitbot/util/path/Point3D.java

@@ -1,9 +1,9 @@
-package com.example.fitbot.util.processing;
+package com.example.fitbot.util.path;
 
 import java.util.Arrays;
 import java.util.Comparator;
 
-public class Vector3 {
+public class Point3D {
 
     public double x, y, z;
 
@@ -14,7 +14,7 @@ public class Vector3 {
      * @param y The Y component of the vector.
      * @param z The Z component of the vector.
      */
-    public Vector3(double x, double y, double z) {
+    public Point3D(double x, double y, double z) {
         this.x = x;
         this.y = y;
         this.z = z;
@@ -25,8 +25,8 @@ public class Vector3 {
      *
      * @return A new vector with the same values.
      */
-    public Vector3 copy() {
+    public Point3D copy() {
-        return new Vector3(this.x, this.y, this.z);
+        return new Point3D(this.x, this.y, this.z);
     }
     
     /**
@@ -34,8 +34,8 @@ public class Vector3 {
      * 
      * @return The zero vector.
      */
-    public static Vector3 zero() {
+    public static Point3D zero() {
-        return new Vector3(0, 0, 0);
+        return new Point3D(0, 0, 0);
     }
 
     /**
@@ -52,10 +52,10 @@ public class Vector3 {
      *
      * @return The normalized vector.
      */
-    public Vector3 normalize() {
+    public Point3D normalize() {
         double mag = this.magnitude();
         if (mag == 0) throw new IllegalArgumentException("Cannot normalize the zero vector.");
-        return new Vector3(this.x / mag, this.y / mag, this.z / mag);
+        return new Point3D(this.x / mag, this.y / mag, this.z / mag);
     }
 
     /**
@@ -64,8 +64,8 @@ public class Vector3 {
      * @param other The other vector to subtract.
      * @return The new vector.
      */
-    public Vector3 subtract(Vector3 other) {
+    public Point3D subtract(Point3D other) {
-        return new Vector3(this.x - other.x, this.y - other.y, this.z - other.z);
+        return new Point3D(this.x - other.x, this.y - other.y, this.z - other.z);
     }
 
     /**
@@ -74,8 +74,8 @@ public class Vector3 {
      * @param other The other vector to add.
      * @return The new vector.
      */
-    public Vector3 add(Vector3 other) {
+    public Point3D add(Point3D other) {
-        return new Vector3(this.x + other.x, this.y + other.y, this.z + other.z);
+        return new Point3D(this.x + other.x, this.y + other.y, this.z + other.z);
     }
 
     /**
@@ -84,8 +84,8 @@ public class Vector3 {
      * @param scalar The scalar to multiply by.
      * @return The multiplied vector.
      */
-    public Vector3 multiply(double scalar) {
+    public Point3D multiply(double scalar) {
-        return new Vector3(this.x * scalar, this.y * scalar, this.z * scalar);
+        return new Point3D(this.x * scalar, this.y * scalar, this.z * scalar);
     }
 
     /**
@@ -94,9 +94,9 @@ public class Vector3 {
      * @param scalar The scalar to divide by.
      * @return The divided vector.
      */
-    public Vector3 divide(double scalar) {
+    public Point3D divide(double scalar) {
         if (scalar == 0) throw new IllegalArgumentException("Cannot divide by zero.");
-        return new Vector3(this.x / scalar, this.y / scalar, this.z / scalar);
+        return new Point3D(this.x / scalar, this.y / scalar, this.z / scalar);
     }
 
     /**
@@ -104,8 +104,8 @@ public class Vector3 {
      *
      * @return The negated vector.
      */
-    public Vector3 negate() {
+    public Point3D negate() {
-        return new Vector3(-this.x, -this.y, -this.z);
+        return new Point3D(-this.x, -this.y, -this.z);
     }
 
     /**
@@ -116,7 +116,7 @@ public class Vector3 {
      * @param radZ Rotation around the Z axis in radians.
      * @return The rotated vector.
      */
-    public Vector3 rotate(double radX, double radY, double radZ) {
+    public Point3D rotate(double radX, double radY, double radZ) {
         double cosX = Math.cos(radX);
         double cosY = Math.cos(radY);
         double cosZ = Math.cos(radZ);
@@ -126,7 +126,7 @@ public class Vector3 {
         double newX = x * cosY * cosZ + y * cosY * sinZ - z * sinY;
         double newY = x * (sinX * sinY * cosZ - cosX * sinZ) + y * (sinX * sinY * sinZ + cosX * cosZ) + z * sinX * cosY;
         double newZ = x * (cosX * sinY * cosZ + sinX * sinZ) + y * (cosX * sinY * sinZ - sinX * cosZ) + z * cosX * cosY;
-        return new Vector3(newX, newY, newZ);
+        return new Point3D(newX, newY, newZ);
     }
 
     /**
@@ -135,7 +135,7 @@ public class Vector3 {
      * @param rotation The rotation vector.
      * @return The rotated vector.
      */
-    public Vector3 rotate(Vector3 rotation) {
+    public Point3D rotate(Point3D rotation) {
         return rotate(rotation.x, rotation.y, rotation.z);
     }
 
@@ -145,10 +145,10 @@ public class Vector3 {
      * @param angle Rotation around the X axis in radians.
      * @return The rotated vector.
      */
-    public Vector3 rotateX(double angle) {
+    public Point3D rotateX(double angle) {
         double sinTheta = Math.sin(angle);
         double cosTheta = Math.cos(angle);
-        return new Vector3(
+        return new Point3D(
                 x,
                 y * cosTheta - z * sinTheta,
                 y * sinTheta + z * cosTheta
@@ -161,10 +161,10 @@ public class Vector3 {
      * @param angle Rotation around the Y axis in radians.
      * @return The rotated vector.
      */
-    public Vector3 rotateY(double angle) {
+    public Point3D rotateY(double angle) {
         double sinTheta = Math.sin(angle);
         double cosTheta = Math.cos(angle);
-        return new Vector3(
+        return new Point3D(
                 x * cosTheta + z * sinTheta,
                 y,
                 -x * sinTheta + z * cosTheta
@@ -177,10 +177,10 @@ public class Vector3 {
      * @param angle Rotation around the Z axis in radians.
      * @return The rotated vector.
      */
-    public Vector3 rotateZ(double angle) {
+    public Point3D rotateZ(double angle) {
         double sinTheta = Math.sin(angle);
         double cosTheta = Math.cos(angle);
-        return new Vector3(
+        return new Point3D(
                 x * cosTheta - y * sinTheta,
                 x * sinTheta + y * cosTheta,
                 z
@@ -193,7 +193,7 @@ public class Vector3 {
      * @param compare The other vector.
      * @return The squared distance between the two vectors.
      */
-    public double distanceSq(Vector3 compare) {
+    public double distanceSq(Point3D compare) {
         return Math.pow(compare.x - x, 2) + Math.pow(compare.y - y, 2) + Math.pow(compare.z - z, 2);
     }
 
@@ -203,7 +203,7 @@ public class Vector3 {
      * @param compare The other vector.
      * @return The distance between the two vectors.
      */
-    public double distance(Vector3 compare) {
+    public double distance(Point3D compare) {
         return Math.sqrt(distanceSq(compare));
     }
 
@@ -214,7 +214,7 @@ public class Vector3 {
      * @param lineEnd The ending point of the line.
      * @return The distance to the line.
      */
-    public double distanceToLine(Vector3 lineStart, Vector3 lineEnd) {
+    public double distanceToLine(Point3D lineStart, Point3D lineEnd) {
         double lineDistance = lineStart.distanceSq(lineEnd);
         if (lineDistance == 0)
             return this.distanceSq(lineStart);
@@ -225,7 +225,7 @@ public class Vector3 {
 
         t = Math.max(0, Math.min(1, t));
 
-        return this.distanceSq(new Vector3(
+        return this.distanceSq(new Point3D(
                 lineStart.x + t * (lineEnd.x - lineStart.x),
                 lineStart.y + t * (lineEnd.y - lineStart.y),
                 lineStart.z + t * (lineEnd.z - lineStart.z))
@@ -238,16 +238,16 @@ public class Vector3 {
      * @param vectors The list of vectors to compare.
      * @return The closest vector.
      */
-    public Vector3 closest(Vector3 ... vectors) {
+    public Point3D closest(Point3D... vectors) {
         return Arrays.stream(vectors).min(Comparator.comparingDouble(this::distanceSq)).orElse(null);
     }
 
-    public Vector3 map(VectorMapFunction function) {
+    public Point3D map(VectorMapFunction function) {
         return function.apply(this);
     }
 
     public interface VectorMapFunction {
-        Vector3 apply(Vector3 vector);
+        Point3D apply(Point3D vector);
     }
 
     @Override

code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/GesturePath.java

@@ -1,104 +0,0 @@
-package com.example.fitbot.util.processing;
-
-import java.util.ArrayList;
-import java.util.List;
-
-public class GesturePath {
-
-    // The vectors that make up the path.
-    private final Vector3[] vectors;
-
-    public GesturePath(Vector3[] vectors) {
-        this.vectors = vectors;
-    }
-
-    /**
-     * Get the error between an arbitrary path segment and the reference point.
-     *
-     * @param referencePoint The reference point to calculate the error of.
-     * @return The error offset between the path and the reference point.
-     */
-    public double getError(Vector3 referencePoint) {
-        // If there are no vectors, return 0.
-        if ( vectors.length == 0)
-            return 0;
-
-        // If there's only one vector, return the distance to that vector.
-        if ( vectors.length == 1)
-            return vectors[0].distance(referencePoint);
-
-        double distance = Double.MAX_VALUE;
-        double currentDistSq, nextDistSq;
-        int closestVectorIdx = 0;
-
-        // Acquire two closest points to the reference point.
-        for ( int i = 0; i < vectors.length - 1; i++) {
-            currentDistSq = vectors[i].distanceSq(referencePoint);
-            nextDistSq = vectors[i + 1].distanceSq(referencePoint);
-
-            if ( currentDistSq < distance) {
-                distance = currentDistSq;
-                closestVectorIdx = i;
-            } else if ( nextDistSq < distance) {
-                distance = nextDistSq;
-                closestVectorIdx = i + 1;
-                i++; // Skip the next iteration; this point is already closer.
-            }
-        }
-
-        // Calculate the error between the two closest points.
-        Vector3 pointB = (closestVectorIdx == vectors.length - 1) ?
-                vectors[closestVectorIdx - 1] : // If the closest point is the last point, use the 1 to last one
-                (closestVectorIdx > 0 && // Find the closer point between the surrounding points.
-                        (vectors[closestVectorIdx - 1].distanceSq(referencePoint) < vectors[closestVectorIdx + 1].distanceSq(referencePoint))) ?
-                        vectors[closestVectorIdx - 1] :
-                        vectors[closestVectorIdx + 1];
-
-        return referencePoint.distanceToLine(vectors[closestVectorIdx], pointB);
-    }
-
-    // Builder class for the GesturePath object.
-    public static class Builder {
-
-        // List of vectors to add to the GesturePath object.
-        private final List<Vector3> vectors;
-
-        /**
-         * Constructor for the Builder object.
-         *
-         * @param vectors The list of vectors to add.
-         */
-        public Builder(List<Vector3> vectors) {
-            this.vectors = vectors;
-        }
-
-        /**
-         * Default constructor for the Builder object.
-         */
-        public Builder() {
-            this.vectors = new ArrayList<>();
-        }
-
-        /**
-         * Adds a vector to the GesturePath object.
-         *
-         * @param vector The vector to add.
-         * @return The Builder object.
-         */
-        public Builder addVector(Vector3 vector) {
-            vectors.add(vector);
-            return this;
-        }
-
-        /**
-         * Builds the GesturePath object.
-         *
-         * @return The GesturePath object.
-         */
-        public GesturePath build() {
-            return new GesturePath(vectors.toArray(new Vector3[0]));
-        }
-
-    }
-
-}

code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/MotionData.java

@@ -1,11 +1,13 @@
 package com.example.fitbot.util.processing;
 
+import org.joml.Vector3f;
+
 import java.util.Objects;
 
 public class MotionData {
 
     // Data of the motion sensor
-    public Vector3 acceleration, rotation;
+    public Vector3f acceleration, rotation;
 
     // Delimiter for the data received from the motion sensor
     private static final String DATA_DELIMITER = ";";
@@ -20,9 +22,9 @@ public class MotionData {
      * @param rotationY     The rotation in the Y axis in degrees.
      * @param rotationZ     The rotation in the Z axis in degrees.
      */
-    public MotionData(double accelerationX, double accelerationY, double accelerationZ, double rotationX, double rotationY, double rotationZ) {
+    public MotionData(float accelerationX, float accelerationY, float accelerationZ, float rotationX, float rotationY, float rotationZ) {
-        this.acceleration = new Vector3(accelerationX, accelerationY, accelerationZ);
+        this.acceleration = new Vector3f(accelerationX, accelerationY, accelerationZ);
-        this.rotation = new Vector3(rotationX, rotationY, rotationZ);
+        this.rotation = new Vector3f(rotationX, rotationY, rotationZ);
     }
 
     /**
@@ -31,7 +33,7 @@ public class MotionData {
      * @param acceleration The acceleration vector in m/s^2.
      * @param rotation     The rotation vector in degrees.
      */
-    public MotionData(Vector3 acceleration, Vector3 rotation) {
+    public MotionData(Vector3f acceleration, Vector3f rotation) {
         this.acceleration = acceleration;
         this.rotation = rotation;
     }
@@ -52,12 +54,12 @@ public class MotionData {
             return null;
 
         return new MotionData(
-                Double.parseDouble(parts[0]),
+                Float.parseFloat(parts[0]),
-                Double.parseDouble(parts[1]),
+                Float.parseFloat(parts[1]),
-                Double.parseDouble(parts[2]),
+                Float.parseFloat(parts[2]),
-                Double.parseDouble(parts[3]),
+                Float.parseFloat(parts[3]),
-                Double.parseDouble(parts[4]),
+                Float.parseFloat(parts[4]),
-                Double.parseDouble(parts[5])
+                Float.parseFloat(parts[5])
         );
     }
 }

code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/MotionProcessor.java

@@ -2,10 +2,12 @@ package com.example.fitbot.util.processing;
 
 import android.util.Log;
 
+import com.example.fitbot.util.path.GesturePath;
 import com.example.fitbot.util.server.IWebSocketHandler;
 import com.example.fitbot.util.server.WebSocket;
 
 import org.jetbrains.annotations.NotNull;
+import org.joml.Vector3f;
 
 import java.util.ArrayList;
 import java.util.List;
@@ -16,9 +18,10 @@ public class MotionProcessor {
     public static final String DELIMITER = ";";
 
     private final List<MotionData> preprocessedData = new ArrayList<>(); // Preprocessed motion data
-    private final List<Vector3> relativePath = new ArrayList<>(); // Relative path of the motion data
+    private final List<Vector3f> relativePath = new ArrayList<>(); // Relative path of the motion data
-    private Vector3 ZERO = new Vector3(0, 0, 0);
+    private Vector3f ZERO = new Vector3f(0, 0, 0);
-    private double sampleRate = 1.0D; // samples/second
+
+    private float sampleRate = 1.0F; // samples/second
     private DataConsumer motionDataConsumer = (p1, p2, p3, p4) -> {};
     private GesturePath path;
     private WebSocket socket;
@@ -77,14 +80,14 @@ public class MotionProcessor {
             // Otherwise check if it starts with 'calibrate', this is the ZERO point.
         } else if ( data.startsWith("zero")) { // message to calibrate device
             String[] vectorData = data.split(" ")[1].split(DELIMITER);
-            ZERO = new Vector3(
+            ZERO = new Vector3f(
                     Float.parseFloat(vectorData[0]),
                     Float.parseFloat(vectorData[1]),
                     Float.parseFloat(vectorData[2])
             );
             Log.i("MotionProcessor", "Device calibrated at " + ZERO.toString());
         } else if ( data.startsWith("sampleRate")) {
-            this.sampleRate = Double.parseDouble(data.split(" ")[1]);
+            this.sampleRate = Float.parseFloat(data.split(" ")[1]);
         }
     }
 
@@ -104,8 +107,8 @@ public class MotionProcessor {
      */
     public void addMotionData(MotionData data) {
         preprocessedData.add(data);
-        Vector3 previous = this.relativePath.isEmpty() ? ZERO : this.relativePath.get(this.relativePath.size() - 1);
+        Vector3f previous = this.relativePath.isEmpty() ? ZERO : this.relativePath.get(this.relativePath.size() - 1);
-        Vector3 relativeVector = getRelativeVector(data).add(previous);
+        Vector3f relativeVector = getRelativeVector(data).add(previous);
         this.relativePath.add(relativeVector);
         motionDataConsumer.accept(relativeVector, data, this.relativePath.size(), this.sampleRate);
     }
@@ -115,7 +118,7 @@ public class MotionProcessor {
      *
      * @param relativePath The new relative path.
      */
-    public void setRelativePath(List<Vector3> relativePath) {
+    public void setRelativePath(List<Vector3f> relativePath) {
         this.relativePath.clear();
         this.relativePath.addAll(relativePath);
     }
@@ -138,15 +141,17 @@ public class MotionProcessor {
      * @param motionData The motion data to calculate the relative vector for.
      * @return The relative vector of the motion data.
      */
-    public Vector3 getRelativeVector(MotionData motionData) {
+    public Vector3f getRelativeVector(MotionData motionData) {
 
         // 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
         return motionData.acceleration
-                .rotate(motionData.rotation.negate())
+                .rotateX(-motionData.rotation.x)
-                .divide(2)
+                .rotateY(-motionData.rotation.y)
-                .multiply(sampleRate * sampleRate);
+                .rotateZ(-motionData.rotation.z)
+                .div(2)
+                .mul(sampleRate * sampleRate);
     }
 
     /**
@@ -185,7 +190,7 @@ public class MotionProcessor {
      * @param referencePoint The reference point to compare the motion data to.
      * @return The error of the motion data compared to the reference path.
      */
-    public double getError(GesturePath path, Vector3 referencePoint)
+    public double getError(GesturePath path, Vector3f referencePoint)
     {
         return path.getError(referencePoint);
     }
@@ -197,7 +202,7 @@ public class MotionProcessor {
      * @param referencePoint The reference point to compare the path data to.
      * @return The error of the motion data compared to the reference path.
      */
-    public double getError(Vector3 referencePoint) {
+    public double getError(Vector3f referencePoint) {
         if ( path == null)
             return 0;
         return path.getError(referencePoint);
@@ -254,6 +259,6 @@ public class MotionProcessor {
          * @param sampleIndex The index of the current sample
          * @param sampleRate The sample rate.
          */
-        void accept(Vector3 transformedVector, MotionData motionData, int sampleIndex, double sampleRate);
+        void accept(Vector3f transformedVector, MotionData motionData, int sampleIndex, double sampleRate);
     }
 }