Updated motion processing and WebSocket code.
This commit is contained in:
@@ -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.
|
||||
|
@@ -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> motionData = new ArrayList<>();
|
||||
private final List<MotionData> preprocessedData = new ArrayList<>(); // Preprocessed motion data
|
||||
private final List<Vector3> 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<Vector3> 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<Vector3> consumer) {
|
||||
if ( consumer != null)
|
||||
this.motionDataConsumer = consumer;
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -144,10 +159,6 @@ public class MotionProcessor {
|
||||
*/
|
||||
public List<Double> 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<Double> 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")));
|
||||
|
||||
}
|
||||
}
|
||||
|
@@ -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);
|
||||
}
|
||||
|
@@ -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
|
||||
|
@@ -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<Socket> 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.");
|
||||
}
|
||||
}
|
||||
|
Reference in New Issue
Block a user