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 index ed761f8..451de9d 100644 --- 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 @@ -5,38 +5,13 @@ import java.util.List; public class GesturePath { - public Vector3[] vectors; + // The vectors that make up the path. + private final Vector3[] vectors; public GesturePath(Vector3[] vectors) { this.vectors = vectors; } - /** - * Get the error between a line segment and the reference point. - * - * @param linePointA The first point of the line. - * @param linePointB The second point of the line. - * @param referencePoint The reference point to calculate the error of. - * @return The error offset between the line and the reference point. - */ - public static double getError(Vector3 linePointA, Vector3 linePointB, Vector3 referencePoint) { - double lineDistance = linePointA.distanceSq(linePointB); - if (lineDistance == 0) - return referencePoint.distanceSq(linePointA); - - double t = ((referencePoint.x - linePointA.x) * (linePointB.x - linePointA.x) + - (referencePoint.y - linePointA.y) * (linePointB.y - linePointA.y) + - (referencePoint.z - linePointA.z) * (linePointB.z - linePointA.z)) / lineDistance; - - t = Math.max(0, Math.min(1, t)); - - return referencePoint.distanceSq(new Vector3( - linePointA.x + t * (linePointB.x - linePointA.x), - linePointA.y + t * (linePointB.y - linePointA.y), - linePointA.z + t * (linePointB.z - linePointA.z)) - ); - } - /** * Get the error between an arbitrary path segment and the reference point. * @@ -79,7 +54,7 @@ public class GesturePath { vectors[closestVectorIdx - 1] : vectors[closestVectorIdx + 1]; - return getError(vectors[closestVectorIdx], pointB, referencePoint); + return referencePoint.distanceToLine(vectors[closestVectorIdx], pointB); } // Builder class for the GesturePath object. 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 4fac2c0..8f6d049 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,16 +9,20 @@ import org.jetbrains.annotations.NotNull; import java.util.ArrayList; import java.util.List; +import java.util.function.Consumer; import java.util.stream.Collectors; public class MotionProcessor { public static final String DELIMITER = ";"; - private final List motionData = new ArrayList<>(); + 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 Consumer motionDataConsumer = (data) -> {}; + private GesturePath path; + private WebSocket socket; public MotionProcessor() {} @@ -31,7 +35,7 @@ public class MotionProcessor { */ public void startListening() { // Create socket server - WebSocket socket = WebSocket.createServer(); + this.socket = WebSocket.createServer(); Log.i("MotionProcessor", "Listening for incoming connections."); @@ -48,6 +52,17 @@ public class MotionProcessor { } } + /** + * Function for stopping the listening process + * of the motion sensor. This function will stop + * the WebSocket server. + */ + public void stopListening() { + if (socket != null) { + socket.stop(); + } + } + /** * Function for parsing arbitrary packet data. * @param data The data to parse. @@ -58,10 +73,10 @@ public class MotionProcessor { Log.i("MotionProcessor", "Received data packet: " + data.split(" ")[1]); MotionData parsedData = MotionData.decode(data.split(" ")[1]); if (parsedData != null) { - this.motionData.add(parsedData); + addMotionData(parsedData); } // Otherwise check if it starts with 'calibrate', this is the ZERO point. - } else if ( data.startsWith("calibrate")) { // message to calibrate device + } else if ( data.startsWith("zero")) { // message to calibrate device String[] vectorData = data.split(" ")[1].split(DELIMITER); ZERO = new Vector3( Float.parseFloat(vectorData[0]), @@ -74,13 +89,26 @@ public class MotionProcessor { } } + /** + * Function for setting the gesture path of the processor. + * + * @param path The path to set. + */ + public void setGesturePath(GesturePath path) { + this.path = path; + } + /** * Function for adding motion data to the processor. * * @param data The motion data to add. */ public void addMotionData(MotionData data) { - this.motionData.add(data); + preprocessedData.add(data); + Vector3 previous = this.relativePath.isEmpty() ? ZERO : this.relativePath.get(this.relativePath.size() - 1); + Vector3 relativeVector = getRelativeVector(data).add(previous); + this.relativePath.add(relativeVector); + motionDataConsumer.accept(relativeVector); } /** @@ -94,25 +122,12 @@ public class MotionProcessor { } /** - * Function for calculating the 'absolute' path of the motion data. - * This converts the relative acceleration and rotation vectors and converts - * them to a position that is relative to the ZERO point. + * Function for setting the motion data receiver. + * @param consumer The consumer to set. */ - public void calculateRelativePath() { - - relativePath.clear(); // Clear previous path - - Vector3 previous = ZERO.copy(); - - for (int i = 0; i < motionData.size(); i++) { - MotionData data = motionData.get(i); - - Vector3 relativeVector = getRelativeVector(data) - .add(previous); - - relativePath.add(relativeVector); - previous = relativeVector; - } + public void setMotionDataEventHandler(Consumer consumer) { + if ( consumer != null) + this.motionDataConsumer = consumer; } /** @@ -144,10 +159,6 @@ public class MotionProcessor { */ public List getErrors(GesturePath referencePath) { - // If the relative path is empty, calculate it. - if ( relativePath.isEmpty()) - calculateRelativePath(); - // Return the errors of the relative path compared to the reference path. return relativePath .stream() @@ -155,6 +166,44 @@ public class MotionProcessor { .collect(Collectors.toList()); } + /** + * Function for getting the error offsets of the motion data compared to the + * reference path. + * + * @return A list of error offsets of the motion data compared to the reference path. + * If no path is set, an empty list will be returned. + */ + public List getErrors() { + if ( path == null) + return new ArrayList<>(); + return getErrors(path); + } + + /** + * Function for getting the error of the motion data compared to the reference path. + * + * @param path The path to compare the motion data to. + * @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) + { + return path.getError(referencePoint); + } + + /** + * Function for getting the error of the provided vector and the set path. + * If no path is set, the error will be 0. + * + * @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) { + if ( path == null) + return 0; + return path.getError(referencePoint); + } + /** * Function for calculating the average error of the motion data * compared to the reference path. @@ -162,7 +211,7 @@ public class MotionProcessor { * @param referencePath The reference path to compare the motion data to. * @return The average error of the motion data compared to the reference path. */ - public double averageError(GesturePath referencePath) { + public double getAverageError(GesturePath referencePath) { return getErrors(referencePath) .stream() .mapToDouble(Double::doubleValue) @@ -170,14 +219,28 @@ public class MotionProcessor { .orElse(0.0D); } + /** + * Function for calculating the average error of the motion data + * compared to the reference path. + * + * @return The average error of the motion data compared to the reference path. + */ + public double getAverageError() { + if ( path == null) + return 0; + return getAverageError(path); + } + + /** + * Function for logging the statistics of the motion data. + * + * @param referencePath The reference path to compare the motion data to. + */ public void logStatistics(GesturePath referencePath) { - Log.i("MotionProcessor", "Average path error: " + averageError(referencePath)); + Log.i("MotionProcessor", "Average path error: " + getAverageError(referencePath)); Log.i("MotionProcessor", "Path length: " + relativePath.size()); Log.i("MotionProcessor", "Sample rate: " + sampleRate); Log.i("MotionProcessor", "Calibration point: " + ZERO.toString()); - Log.i("MotionProcessor", "Path comparison:\n" + - relativePath.stream().map(Vector3::toString).collect(Collectors.joining(", \t")) + - "\n" + getErrors(referencePath).stream().map(Object::toString).collect(Collectors.joining(", \t"))); } } 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/processing/Vector3.java index 0f86df5..47c251c 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/processing/Vector3.java @@ -1,5 +1,8 @@ package com.example.fitbot.util.processing; +import java.util.Arrays; +import java.util.Comparator; + public class Vector3 { public double x, y, z; @@ -25,6 +28,15 @@ public class Vector3 { public Vector3 copy() { return new Vector3(this.x, this.y, this.z); } + + /** + * Get the zero vector. + * + * @return The zero vector. + */ + public static Vector3 zero() { + return new Vector3(0, 0, 0); + } /** * Get the magnitude of the vector. @@ -195,6 +207,41 @@ public class Vector3 { return Math.sqrt(distanceSq(compare)); } + /** + * Calculate the distance to a line defined by two points. + * + * @param lineStart The starting point of the line. + * @param lineEnd The ending point of the line. + * @return The distance to the line. + */ + public double distanceToLine(Vector3 lineStart, Vector3 lineEnd) { + double lineDistance = lineStart.distanceSq(lineEnd); + if (lineDistance == 0) + return this.distanceSq(lineStart); + + double t = ((this.x - lineStart.x) * (lineEnd.x - lineStart.x) + + (this.y - lineStart.y) * (lineEnd.y - lineStart.y) + + (this.z - lineStart.z) * (lineEnd.z - lineStart.z)) / lineDistance; + + t = Math.max(0, Math.min(1, t)); + + return this.distanceSq(new Vector3( + lineStart.x + t * (lineEnd.x - lineStart.x), + lineStart.y + t * (lineEnd.y - lineStart.y), + lineStart.z + t * (lineEnd.z - lineStart.z)) + ); + } + + /** + * Retrieve the closest vector to this one given a list of vectors. + * + * @param vectors The list of vectors to compare. + * @return The closest vector. + */ + public Vector3 closest(Vector3 ... vectors) { + return Arrays.stream(vectors).min(Comparator.comparingDouble(this::distanceSq)).orElse(null); + } + public Vector3 map(VectorMapFunction function) { return function.apply(this); } diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/server/WebSocket.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/server/WebSocket.java index 837da04..60fbf6b 100644 --- a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/server/WebSocket.java +++ b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/server/WebSocket.java @@ -49,6 +49,19 @@ public class WebSocket { this.connectionHandler.listen(); } + /** + * Method for stopping the WebSocket server. + */ + public void stop() { + try { + this.serverSocket.close(); + this.connectionHandler.stop(); + } catch (IOException error) { + String cause = error.getMessage() == null ? "Unknown reason" : error.getMessage(); + Log.e("WebSocket -- Closing server connection", cause); + } + } + /** * Method for setting the event handler for this WebSocket server. * @param handler The handler to use. This handler will parse all events diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/server/WebSocketConnectionHandler.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/server/WebSocketConnectionHandler.java index f1748ce..7dc0329 100644 --- a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/server/WebSocketConnectionHandler.java +++ b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/server/WebSocketConnectionHandler.java @@ -9,7 +9,10 @@ import java.net.Socket; import java.nio.charset.StandardCharsets; import java.security.MessageDigest; import java.security.NoSuchAlgorithmException; +import java.util.ArrayList; import java.util.Base64; +import java.util.Collections; +import java.util.List; import java.util.Scanner; import java.util.regex.Matcher; import java.util.regex.Pattern; @@ -17,7 +20,9 @@ import java.util.regex.Pattern; public class WebSocketConnectionHandler implements Runnable { private final WebSocket theSocket; + private List clients = Collections.synchronizedList(new ArrayList<>()); private Thread thread; + private boolean forceClose = false; /** * Constructor for WebSocketConnectionHandler. @@ -31,13 +36,13 @@ public class WebSocketConnectionHandler implements Runnable { @Override public void run() { - // Listen for new connections until the socket - // closes. + // Listen for new connections until the socket closes. while (theSocket.isConnected()) { try { // Find a new connection Socket newSocket = this.theSocket.getSocket().accept(); this.theSocket.eventHandler.onConnected(newSocket); + clients.add(newSocket); InputStream streamIn = newSocket.getInputStream(); OutputStream streamOut = newSocket.getOutputStream(); @@ -179,4 +184,34 @@ public class WebSocketConnectionHandler implements Runnable { this.thread.start(); Log.i("WebSocketConnectionHandler", "Listening started."); } + + /** + * Method for stopping the connection handler. + */ + public void stop() { + // Close the socket connection if not already closed + if (!this.theSocket.getSocket().isClosed()) { + try { + this.theSocket.getSocket().close(); + } catch (IOException error) { + Log.e("WebSocketConnectionHandler", "Failed to close the socket connection: " + error.getMessage()); + } + } + + // Interrupt the thread + this.thread.interrupt(); + + // Close all connections + this.clients.forEach(client -> { + try { + client.close(); + } catch (IOException error) { + Log.e("WebSocketConnectionHandler", "Failed to close client: " + error.getMessage()); + } + }); + this.clients.clear(); + + + Log.i("WebSocketConnectionHandler", "Listening stopped."); + } }