From 11489682eb38adf427947eb2d8a8629f2a80ed94 Mon Sep 17 00:00:00 2001 From: Luca Warmenhoven Date: Wed, 15 May 2024 01:10:51 +0200 Subject: [PATCH] Updated path math, started with 3d path to 2d screen space projection --- code/src/Fitbot/app/build.gradle | 1 + .../Fitbot/app/src/main/AndroidManifest.xml | 4 - .../java/com/example/fitbot/MainScreen.java | 4 - .../PersonalMotionPreviewElement.java | 132 +++++++++++-- .../example/fitbot/util/path/GesturePath.java | 124 +++++++++++++ .../example/fitbot/util/path/PathSegment.java | 175 ++++++++++++++++++ .../Vector3.java => path/Point3D.java} | 70 +++---- .../fitbot/util/processing/GesturePath.java | 104 ----------- .../fitbot/util/processing/MotionData.java | 24 +-- .../util/processing/MotionProcessor.java | 35 ++-- 10 files changed, 488 insertions(+), 185 deletions(-) create mode 100644 code/src/Fitbot/app/src/main/java/com/example/fitbot/util/path/GesturePath.java create mode 100644 code/src/Fitbot/app/src/main/java/com/example/fitbot/util/path/PathSegment.java rename code/src/Fitbot/app/src/main/java/com/example/fitbot/util/{processing/Vector3.java => path/Point3D.java} (78%) delete mode 100644 code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/GesturePath.java diff --git a/code/src/Fitbot/app/build.gradle b/code/src/Fitbot/app/build.gradle index 38cf8be..f3a2fea 100644 --- a/code/src/Fitbot/app/build.gradle +++ b/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' diff --git a/code/src/Fitbot/app/src/main/AndroidManifest.xml b/code/src/Fitbot/app/src/main/AndroidManifest.xml index bd9a6b6..1705cd8 100644 --- a/code/src/Fitbot/app/src/main/AndroidManifest.xml +++ b/code/src/Fitbot/app/src/main/AndroidManifest.xml @@ -14,10 +14,6 @@ - - diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/MainScreen.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/MainScreen.java index f8ddf0a..7315545 100644 --- a/code/src/Fitbot/app/src/main/java/com/example/fitbot/MainScreen.java +++ b/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 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 b5161d8..d70c88c 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 @@ -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 - * canvas. This method will be called every time new motion data is received. + * Constants for the preview path projection. */ - 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.targetPath = new Path(); - this.personalPath = new Path(); + this.motionProcessor.setMotionDataEventHandler((processed, preprocessed, sampleIndex, sampleRate) -> { + // TODO: Implement the calculation of the drawing 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 + + + } } diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/path/GesturePath.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/path/GesturePath.java new file mode 100644 index 0000000..5507c71 --- /dev/null +++ b/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 vectors; + + /** + * Constructor for the Builder object. + * + * @param vectors The list of vectors to add. + */ + public Builder(List 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])); + } + + } + +} diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/path/PathSegment.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/path/PathSegment.java new file mode 100644 index 0000000..05f58a7 --- /dev/null +++ b/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; + } +} diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/Vector3.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/path/Point3D.java similarity index 78% rename from code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/Vector3.java rename to code/src/Fitbot/app/src/main/java/com/example/fitbot/util/path/Point3D.java index 47c251c..de14c78 100644 --- a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/Vector3.java +++ b/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() { - return new Vector3(this.x, this.y, this.z); + public Point3D copy() { + return new Point3D(this.x, this.y, this.z); } /** @@ -34,8 +34,8 @@ public class Vector3 { * * @return The zero vector. */ - public static Vector3 zero() { - return new Vector3(0, 0, 0); + public static Point3D zero() { + 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) { - return new Vector3(this.x - other.x, this.y - other.y, this.z - other.z); + public Point3D subtract(Point3D other) { + 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) { - return new Vector3(this.x + other.x, this.y + other.y, this.z + other.z); + public Point3D add(Point3D other) { + 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) { - return new Vector3(this.x * scalar, this.y * scalar, this.z * scalar); + public Point3D multiply(double 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() { - return new Vector3(-this.x, -this.y, -this.z); + public Point3D negate() { + 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 diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/GesturePath.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/GesturePath.java deleted file mode 100644 index 451de9d..0000000 --- a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/GesturePath.java +++ /dev/null @@ -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 vectors; - - /** - * Constructor for the Builder object. - * - * @param vectors The list of vectors to add. - */ - public Builder(List 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])); - } - - } - -} diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/MotionData.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/MotionData.java index 2c10030..37e016f 100644 --- a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/MotionData.java +++ b/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) { - this.acceleration = new Vector3(accelerationX, accelerationY, accelerationZ); - this.rotation = new Vector3(rotationX, rotationY, rotationZ); + public MotionData(float accelerationX, float accelerationY, float accelerationZ, float rotationX, float rotationY, float rotationZ) { + this.acceleration = new Vector3f(accelerationX, accelerationY, accelerationZ); + 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]), - Double.parseDouble(parts[1]), - Double.parseDouble(parts[2]), - Double.parseDouble(parts[3]), - Double.parseDouble(parts[4]), - Double.parseDouble(parts[5]) + Float.parseFloat(parts[0]), + Float.parseFloat(parts[1]), + Float.parseFloat(parts[2]), + Float.parseFloat(parts[3]), + Float.parseFloat(parts[4]), + Float.parseFloat(parts[5]) ); } } 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 43539ba..b4e7de3 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 @@ -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 preprocessedData = new ArrayList<>(); // Preprocessed motion data - private final List relativePath = new ArrayList<>(); // Relative path of the motion data - private Vector3 ZERO = new Vector3(0, 0, 0); - private double sampleRate = 1.0D; // samples/second + private final List relativePath = new ArrayList<>(); // Relative path of the motion data + private Vector3f ZERO = new Vector3f(0, 0, 0); + + 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); - Vector3 relativeVector = getRelativeVector(data).add(previous); + Vector3f previous = this.relativePath.isEmpty() ? ZERO : this.relativePath.get(this.relativePath.size() - 1); + 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 relativePath) { + public void setRelativePath(List 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()) - .divide(2) - .multiply(sampleRate * sampleRate); + .rotateX(-motionData.rotation.x) + .rotateY(-motionData.rotation.y) + .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); } }