diff --git a/src/main/java/dev/slimevr/VRServer.java b/src/main/java/dev/slimevr/VRServer.java index 775e3edbc..21f7a58e4 100644 --- a/src/main/java/dev/slimevr/VRServer.java +++ b/src/main/java/dev/slimevr/VRServer.java @@ -8,7 +8,7 @@ import dev.slimevr.protocol.ProtocolAPI; import dev.slimevr.serial.SerialHandler; import dev.slimevr.util.ann.VRServerThread; import dev.slimevr.vr.processor.HumanPoseProcessor; -import dev.slimevr.vr.processor.skeleton.HumanSkeleton; +import dev.slimevr.vr.processor.skeleton.Skeleton; import dev.slimevr.vr.trackers.*; import dev.slimevr.vr.trackers.udp.TrackersUDPServer; import dev.slimevr.websocketapi.WebSocketVRBridge; @@ -194,7 +194,7 @@ public class VRServer extends Thread { } @ThreadSafe - public void addSkeletonUpdatedCallback(Consumer consumer) { + public void addSkeletonUpdatedCallback(Consumer consumer) { queueTask(() -> { humanPoseProcessor.addSkeletonUpdatedCallback(consumer); }); diff --git a/src/main/java/dev/slimevr/autobone/AutoBone.java b/src/main/java/dev/slimevr/autobone/AutoBone.java index ba845dfdb..620751697 100644 --- a/src/main/java/dev/slimevr/autobone/AutoBone.java +++ b/src/main/java/dev/slimevr/autobone/AutoBone.java @@ -5,7 +5,7 @@ import com.jme3.math.Vector3f; import dev.slimevr.VRServer; import dev.slimevr.poserecorder.*; import dev.slimevr.vr.processor.HumanPoseProcessor; -import dev.slimevr.vr.processor.skeleton.HumanSkeleton; +import dev.slimevr.vr.processor.skeleton.Skeleton; import dev.slimevr.vr.processor.skeleton.SkeletonConfig; import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue; import dev.slimevr.vr.trackers.TrackerPosition; @@ -187,14 +187,14 @@ public class AutoBone { } /** - * A simple utility method to get the {@link HumanSkeleton} from the + * A simple utility method to get the {@link Skeleton} from the * {@link VRServer} * - * @return The {@link HumanSkeleton} associated with the {@link VRServer}, - * or null if there is none available - * @see {@link VRServer}, {@link HumanSkeleton} + * @return The {@link Skeleton} associated with the {@link VRServer}, or + * null if there is none available + * @see {@link VRServer}, {@link Skeleton} */ - private HumanSkeleton getSkeleton() { + private Skeleton getSkeleton() { HumanPoseProcessor humanPoseProcessor = server != null ? server.humanPoseProcessor : null; return humanPoseProcessor != null ? humanPoseProcessor.getSkeleton() : null; } @@ -206,7 +206,7 @@ public class AutoBone { } } - public boolean applyConfigToSkeleton(HumanSkeleton skeleton) { + public boolean applyConfigToSkeleton(Skeleton skeleton) { if (skeleton == null) { return false; } @@ -351,7 +351,7 @@ public class AutoBone { // If target height isn't specified, auto-detect if (targetHeight < 0f) { // Get the current skeleton from the server - HumanSkeleton skeleton = getSkeleton(); + Skeleton skeleton = getSkeleton(); if (skeleton != null) { // If there is a skeleton available, calculate the target height // from its @@ -647,7 +647,7 @@ public class AutoBone { } Vector3f nodePos = skeleton - .getComputedTracker(trackerFrame.designation.trackerRole).position; + .getComputedTracker(trackerFrame.designation.trackerRole.get()).position; if (nodePos != null) { offset += FastMath.abs(nodePos.distance(trackerFrame.position)); offsetCount++; @@ -682,13 +682,13 @@ public class AutoBone { } Vector3f nodePos1 = skeleton1 - .getComputedTracker(trackerFrame1.designation.trackerRole).position; + .getComputedTracker(trackerFrame1.designation.trackerRole.get()).position; if (nodePos1 == null) { continue; } Vector3f nodePos2 = skeleton2 - .getComputedTracker(trackerFrame2.designation.trackerRole).position; + .getComputedTracker(trackerFrame2.designation.trackerRole.get()).position; if (nodePos2 == null) { continue; } diff --git a/src/main/java/dev/slimevr/gui/SkeletonConfigGUI.java b/src/main/java/dev/slimevr/gui/SkeletonConfigGUI.java index 9a0d79786..568905447 100644 --- a/src/main/java/dev/slimevr/gui/SkeletonConfigGUI.java +++ b/src/main/java/dev/slimevr/gui/SkeletonConfigGUI.java @@ -3,7 +3,7 @@ package dev.slimevr.gui; import dev.slimevr.VRServer; import dev.slimevr.gui.swing.ButtonTimer; import dev.slimevr.gui.swing.EJBagNoStretch; -import dev.slimevr.vr.processor.skeleton.HumanSkeleton; +import dev.slimevr.vr.processor.skeleton.Skeleton; import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue; import io.eiren.util.StringUtils; import io.eiren.util.ann.ThreadSafe; @@ -35,7 +35,7 @@ public class SkeletonConfigGUI extends EJBagNoStretch { } @ThreadSafe - public void skeletonUpdated(HumanSkeleton newSkeleton) { + public void skeletonUpdated(Skeleton newSkeleton) { java.awt.EventQueue.invokeLater(() -> { removeAll(); diff --git a/src/main/java/dev/slimevr/gui/SkeletonList.java b/src/main/java/dev/slimevr/gui/SkeletonList.java index 0bbeaf7b2..9604d7d81 100644 --- a/src/main/java/dev/slimevr/gui/SkeletonList.java +++ b/src/main/java/dev/slimevr/gui/SkeletonList.java @@ -7,7 +7,7 @@ import dev.slimevr.VRServer; import dev.slimevr.gui.swing.EJBagNoStretch; import dev.slimevr.util.ann.VRServerThread; import dev.slimevr.vr.processor.TransformNode; -import dev.slimevr.vr.processor.skeleton.HumanSkeleton; +import dev.slimevr.vr.processor.skeleton.Skeleton; import io.eiren.util.StringUtils; import io.eiren.util.ann.ThreadSafe; import io.eiren.util.collections.FastList; @@ -36,7 +36,7 @@ public class SkeletonList extends EJBagNoStretch { } @ThreadSafe - public void skeletonUpdated(HumanSkeleton newSkeleton) { + public void skeletonUpdated(Skeleton newSkeleton) { java.awt.EventQueue.invokeLater(() -> { removeAll(); nodes.clear(); diff --git a/src/main/java/dev/slimevr/gui/TrackersList.java b/src/main/java/dev/slimevr/gui/TrackersList.java index 633f0afb0..54047cf50 100644 --- a/src/main/java/dev/slimevr/gui/TrackersList.java +++ b/src/main/java/dev/slimevr/gui/TrackersList.java @@ -185,7 +185,7 @@ public class TrackersList extends EJBoxNoStretch { desSelect.addItem(p.name()); } if (cfg.designation != null) { - TrackerPosition p = TrackerPosition.getByDesignation(cfg.designation); + TrackerPosition p = TrackerPosition.getByDesignation(cfg.designation).get(); if (p != null) desSelect.setSelectedItem(p.name()); } diff --git a/src/main/java/dev/slimevr/platform/windows/WindowsNamedPipeBridge.java b/src/main/java/dev/slimevr/platform/windows/WindowsNamedPipeBridge.java index c1aa6263a..57648dacf 100644 --- a/src/main/java/dev/slimevr/platform/windows/WindowsNamedPipeBridge.java +++ b/src/main/java/dev/slimevr/platform/windows/WindowsNamedPipeBridge.java @@ -114,7 +114,7 @@ public class WindowsNamedPipeBridge extends ProtobufBridge implements ); TrackerRole role = TrackerRole.getById(trackerAdded.getTrackerRole()); if (role != null) { - tracker.setBodyPosition(TrackerPosition.getByRole(role)); + tracker.setBodyPosition(TrackerPosition.getByTrackerRole(role).get()); } return tracker; } diff --git a/src/main/java/dev/slimevr/platform/windows/WindowsSteamVRPipeInputBridge.java b/src/main/java/dev/slimevr/platform/windows/WindowsSteamVRPipeInputBridge.java deleted file mode 100644 index 541c2325c..000000000 --- a/src/main/java/dev/slimevr/platform/windows/WindowsSteamVRPipeInputBridge.java +++ /dev/null @@ -1,360 +0,0 @@ -package dev.slimevr.platform.windows; - -import com.jme3.math.Quaternion; -import com.jme3.math.Vector3f; -import com.sun.jna.platform.win32.Kernel32; -import com.sun.jna.platform.win32.WinBase; -import com.sun.jna.platform.win32.WinError; -import com.sun.jna.ptr.IntByReference; -import dev.slimevr.VRServer; -import dev.slimevr.bridge.Bridge; -import dev.slimevr.bridge.PipeState; -import dev.slimevr.vr.trackers.ShareableTracker; -import dev.slimevr.vr.trackers.TrackerPosition; -import dev.slimevr.vr.trackers.TrackerStatus; -import dev.slimevr.vr.trackers.VRTracker; -import io.eiren.util.collections.FastList; -import io.eiren.util.logging.LogManager; -import org.apache.commons.lang3.StringUtils; - -import java.io.IOException; -import java.util.HashMap; -import java.util.Iterator; -import java.util.List; -import java.util.Map; -import java.util.concurrent.atomic.AtomicBoolean; - - -public class WindowsSteamVRPipeInputBridge extends Thread implements Bridge { - - public static final String PipeName = "\\\\.\\pipe\\SlimeVRInput"; - private static final int MAX_COMMAND_LENGTH = 2048; - private final byte[] buffArray = new byte[1024]; - private final VRServer server; - private final StringBuilder commandBuilder = new StringBuilder(1024); - private final List trackers = new FastList<>(); - private final Map trackersInternal = new HashMap<>(); - private final Vector3f vBuffer = new Vector3f(); - private final Quaternion qBuffer = new Quaternion(); - private final AtomicBoolean newData = new AtomicBoolean(false); - private WindowsPipe pipe; - - public WindowsSteamVRPipeInputBridge(VRServer server) { - this.server = server; - } - - @Override - public void run() { - try { - createPipes(); - while (true) { - boolean pipesUpdated = false; - if (pipe.state == PipeState.CREATED) { - tryOpeningPipe(pipe); - } - if (pipe.state == PipeState.OPEN) { - pipesUpdated = updatePipes(); - } - if (pipe.state == PipeState.ERROR) { - resetPipe(); - } - if (!pipesUpdated) { - try { - Thread.sleep(5); // Up to 200Hz - } catch (InterruptedException e) { - e.printStackTrace(); - } - } - } - } catch (Exception e) { - e.printStackTrace(); - } - } - - public boolean updatePipes() throws IOException { - if (pipe.state == PipeState.OPEN) { - IntByReference bytesAvailable = new IntByReference(0); - if ( - Kernel32.INSTANCE - .PeekNamedPipe(pipe.pipeHandle, null, 0, null, bytesAvailable, null) - ) { - if (bytesAvailable.getValue() > 0) { - while ( - Kernel32.INSTANCE - .ReadFile( - pipe.pipeHandle, - buffArray, - buffArray.length, - bytesAvailable, - null - ) - ) { - int bytesRead = bytesAvailable.getValue(); - for (int i = 0; i < bytesRead; ++i) { - char c = (char) buffArray[i]; - if (c == '\n') { - executeInputCommand(); - commandBuilder.setLength(0); - } else { - commandBuilder.append(c); - if (commandBuilder.length() >= MAX_COMMAND_LENGTH) { - LogManager - .severe( - "[SteamVRPipeInputBridge] Command from the pipe is too long, flushing buffer" - ); - commandBuilder.setLength(0); - } - } - } - if (bytesRead < buffArray.length) - return true; // All pipe data read - } - } else { - return false; // Pipe was empty, it's okay - } - } - // PeekNamedPipe or ReadFile returned an error - pipe.state = PipeState.ERROR; - LogManager - .severe("[SteamVRPipeInputBridge] Pipe error: " + Kernel32.INSTANCE.GetLastError()); - } - return false; - } - - private void executeInputCommand() throws IOException { - String[] command = commandBuilder.toString().split(" "); - switch (command[0]) { - case "ADD": // Add new tracker - if (command.length < 4) { - LogManager - .severe( - "[SteamVRPipeInputBridge] Error in ADD command. Command requires at least 4 arguments. Supplied: " - + commandBuilder - ); - return; - } - VRTracker internalTracker = new VRTracker( - Integer.parseInt(command[1]), - StringUtils.join(command, " ", 3, command.length), - true, - true - ); - int roleId = Integer.parseInt(command[2]); - if (roleId >= 0 && roleId < SteamVRInputRoles.values.length) { - SteamVRInputRoles svrRole = SteamVRInputRoles.values[roleId]; - internalTracker.bodyPosition = svrRole.bodyPosition; - } - VRTracker oldTracker; - synchronized (trackersInternal) { - oldTracker = trackersInternal - .put(internalTracker.getTrackerId(), internalTracker); - } - if (oldTracker != null) { - LogManager - .severe( - "[SteamVRPipeInputBridge] New tracker added with the same id. Supplied: " - + commandBuilder - ); - return; - } - newData.set(true); - break; - case "UPD": // Update tracker data - if (command.length < 9) { - LogManager - .severe( - "[SteamVRPipeInputBridge] Error in UPD command. Command requires at least 9 arguments. Supplied: " - + commandBuilder - ); - return; - } - int id = Integer.parseInt(command[1]); - double x = Double.parseDouble(command[2]); - double y = Double.parseDouble(command[3]); - double z = Double.parseDouble(command[4]); - double qw = Double.parseDouble(command[5]); - double qx = Double.parseDouble(command[6]); - double qy = Double.parseDouble(command[7]); - double qz = Double.parseDouble(command[8]); - internalTracker = trackersInternal.get(id); - if (internalTracker != null) { - internalTracker.position.set((float) x, (float) y, (float) z); - internalTracker.rotation.set((float) qx, (float) qy, (float) qz, (float) qw); - internalTracker.dataTick(); - newData.set(true); - } - break; - case "STA": // Update tracker status - if (command.length < 3) { - LogManager - .severe( - "[SteamVRPipeInputBridge] Error in STA command. Command requires at least 3 arguments. Supplied: " - + commandBuilder - ); - return; - } - id = Integer.parseInt(command[1]); - int status = Integer.parseInt(command[2]); - TrackerStatus st = TrackerStatus.getById(status); - if (st == null) { - LogManager - .severe( - "[SteamVRPipeInputBridge] Unrecognized status id. Supplied: " - + commandBuilder - ); - return; - } - internalTracker = trackersInternal.get(id); - if (internalTracker != null) { - internalTracker.setStatus(st); - newData.set(true); - } - break; - } - } - - @Override - public void dataRead() { - if (newData.getAndSet(false)) { - if (trackers.size() < trackersInternal.size()) { - // Add new trackers - synchronized (trackersInternal) { - Iterator iterator = trackersInternal.values().iterator(); - internal: while (iterator.hasNext()) { - VRTracker internalTracker = iterator.next(); - for (VRTracker t : trackers) { - if (t.getTrackerId() == internalTracker.getTrackerId()) - continue internal; - } - // Tracker is not found in current trackers - VRTracker tracker = new VRTracker( - internalTracker.getTrackerId(), - internalTracker.getName(), - true, - true - ); - tracker.bodyPosition = internalTracker.bodyPosition; - trackers.add(tracker); - server.registerTracker(tracker); - } - } - } - for (VRTracker tracker : trackers) { - VRTracker internal = trackersInternal.get(tracker.getTrackerId()); - if (internal == null) - throw new NullPointerException( - "Lost internal tracker somehow: " + tracker.getTrackerId() - ); // Shouln't - // really - // happen - // even, - // but - // better - // to - // catch - // it - // like - // this - if (internal.getPosition(vBuffer)) - tracker.position.set(vBuffer); - if (internal.getRotation(qBuffer)) - tracker.rotation.set(qBuffer); - tracker.setStatus(internal.getStatus()); - tracker.dataTick(); - } - } - } - - @Override - public void dataWrite() { - // Not used, only input - } - - private void resetPipe() { - WindowsPipe.safeDisconnect(pipe); - pipe.state = PipeState.CREATED; - // Main.vrServer.queueTask(this::disconnected); - } - - private boolean tryOpeningPipe(WindowsPipe pipe) { - if ( - Kernel32.INSTANCE.ConnectNamedPipe(pipe.pipeHandle, null) - || Kernel32.INSTANCE.GetLastError() == WinError.ERROR_PIPE_CONNECTED - ) { - pipe.state = PipeState.OPEN; - LogManager.info("[SteamVRPipeInputBridge] Pipe " + pipe.name + " is open"); - return true; - } - - LogManager - .info( - "[SteamVRPipeInputBridge] Error connecting to pipe " - + pipe.name - + ": " - + Kernel32.INSTANCE.GetLastError() - ); - return false; - } - - private void createPipes() throws IOException { - try { - pipe = new WindowsPipe( - Kernel32.INSTANCE - .CreateNamedPipe( - PipeName, - WinBase.PIPE_ACCESS_DUPLEX, // dwOpenMode - WinBase.PIPE_TYPE_BYTE | WinBase.PIPE_READMODE_BYTE | WinBase.PIPE_WAIT, // dwPipeMode - 1, // nMaxInstances, - 1024 * 16, // nOutBufferSize, - 1024 * 16, // nInBufferSize, - 0, // nDefaultTimeOut, - null - ), - PipeName - ); // lpSecurityAttributes - LogManager.info("[SteamVRPipeInputBridge] Pipe " + pipe.name + " created"); - if (WinBase.INVALID_HANDLE_VALUE.equals(pipe.pipeHandle)) - throw new IOException( - "Can't open " + PipeName + " pipe: " + Kernel32.INSTANCE.GetLastError() - ); - LogManager.info("[SteamVRPipeInputBridge] Pipes are open"); - } catch (IOException e) { - WindowsPipe.safeDisconnect(pipe); - throw e; - } - } - - @Override - public void addSharedTracker(ShareableTracker tracker) { - // TODO Auto-generated method stub - - } - - @Override - public void removeSharedTracker(ShareableTracker tracker) { - // TODO Auto-generated method stub - - } - - @Override - public void startBridge() { - start(); - } - - public enum SteamVRInputRoles { - HEAD(TrackerPosition.HMD), LEFT_HAND(TrackerPosition.LEFT_CONTROLLER), - RIGHT_HAND(TrackerPosition.RIGHT_CONTROLLER), LEFT_FOOT(TrackerPosition.LEFT_FOOT), - RIGHT_FOOT(TrackerPosition.RIGHT_FOOT), LEFT_SHOULDER(TrackerPosition.NONE), - RIGHT_SHOULDER(TrackerPosition.NONE), LEFT_ELBOW(TrackerPosition.LEFT_FOREARM), - RIGHT_ELBOW(TrackerPosition.RIGHT_FOREARM), LEFT_KNEE(TrackerPosition.LEFT_KNEE), - RIGHT_KNEE(TrackerPosition.RIGHT_KNEE), WAIST(TrackerPosition.WAIST), - CHEST(TrackerPosition.CHEST),; - - private static final SteamVRInputRoles[] values = values(); - public final TrackerPosition bodyPosition; - - SteamVRInputRoles(TrackerPosition slimeVrPosition) { - this.bodyPosition = slimeVrPosition; - } - } -} diff --git a/src/main/java/dev/slimevr/poserecorder/PoseFrameIO.java b/src/main/java/dev/slimevr/poserecorder/PoseFrameIO.java index 7aeb69669..8fd09ae1d 100644 --- a/src/main/java/dev/slimevr/poserecorder/PoseFrameIO.java +++ b/src/main/java/dev/slimevr/poserecorder/PoseFrameIO.java @@ -92,7 +92,7 @@ public final class PoseFrameIO { TrackerPosition designation = null; if (TrackerFrameData.DESIGNATION.check(dataFlags)) { - designation = TrackerPosition.getByDesignation(inputStream.readUTF()); + designation = TrackerPosition.getByDesignation(inputStream.readUTF()).get(); } Quaternion rotation = null; diff --git a/src/main/java/dev/slimevr/poserecorder/PoseFrameSkeleton.java b/src/main/java/dev/slimevr/poserecorder/PoseFrameSkeleton.java index dc31a6f43..7c7c6a582 100644 --- a/src/main/java/dev/slimevr/poserecorder/PoseFrameSkeleton.java +++ b/src/main/java/dev/slimevr/poserecorder/PoseFrameSkeleton.java @@ -2,7 +2,7 @@ package dev.slimevr.poserecorder; import dev.slimevr.VRServer; import dev.slimevr.vr.processor.ComputedHumanPoseTracker; -import dev.slimevr.vr.processor.skeleton.SimpleSkeleton; +import dev.slimevr.vr.processor.skeleton.HumanSkeleton; import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue; import dev.slimevr.vr.trackers.Tracker; @@ -10,7 +10,7 @@ import java.util.List; import java.util.Map; -public class PoseFrameSkeleton extends SimpleSkeleton { +public class PoseFrameSkeleton extends HumanSkeleton { private int frameCursor = 0; diff --git a/src/main/java/dev/slimevr/posestreamer/BVHFileStream.java b/src/main/java/dev/slimevr/posestreamer/BVHFileStream.java index 26e1500d7..8b93599d6 100644 --- a/src/main/java/dev/slimevr/posestreamer/BVHFileStream.java +++ b/src/main/java/dev/slimevr/posestreamer/BVHFileStream.java @@ -5,7 +5,7 @@ import com.jme3.math.Quaternion; import com.jme3.math.Transform; import com.jme3.math.Vector3f; import dev.slimevr.vr.processor.TransformNode; -import dev.slimevr.vr.processor.skeleton.HumanSkeleton; +import dev.slimevr.vr.processor.skeleton.Skeleton; import org.apache.commons.lang3.StringUtils; import java.io.*; @@ -23,7 +23,7 @@ public class BVHFileStream extends PoseDataStream { private float[] angleBuf = new float[3]; private Quaternion rotBuf = new Quaternion(); - private HumanSkeleton wrappedSkeleton; + private Skeleton wrappedSkeleton; private TransformNodeWrapper rootNode; public BVHFileStream(OutputStream outputStream) { @@ -48,7 +48,7 @@ public class BVHFileStream extends PoseDataStream { return bufferCount > 0 ? frameString + StringUtils.repeat(' ', bufferCount) : frameString; } - private TransformNodeWrapper wrapSkeletonIfNew(HumanSkeleton skeleton) { + private TransformNodeWrapper wrapSkeletonIfNew(Skeleton skeleton) { TransformNodeWrapper wrapper = rootNode; // If the wrapped skeleton is missing or the skeleton is updated @@ -59,7 +59,7 @@ public class BVHFileStream extends PoseDataStream { return wrapper; } - private TransformNodeWrapper wrapSkeleton(HumanSkeleton skeleton) { + private TransformNodeWrapper wrapSkeleton(Skeleton skeleton) { TransformNodeWrapper wrapper = wrapSkeletonNodes(skeleton.getRootNode()); wrappedSkeleton = skeleton; @@ -134,7 +134,7 @@ public class BVHFileStream extends PoseDataStream { } @Override - public void writeHeader(HumanSkeleton skeleton, PoseStreamer streamer) throws IOException { + public void writeHeader(Skeleton skeleton, PoseStreamer streamer) throws IOException { if (skeleton == null) { throw new NullPointerException("skeleton must not be null"); } @@ -249,7 +249,7 @@ public class BVHFileStream extends PoseDataStream { } @Override - public void writeFrame(HumanSkeleton skeleton) throws IOException { + public void writeFrame(Skeleton skeleton) throws IOException { if (skeleton == null) { throw new NullPointerException("skeleton must not be null"); } @@ -276,7 +276,7 @@ public class BVHFileStream extends PoseDataStream { } @Override - public void writeFooter(HumanSkeleton skeleton) throws IOException { + public void writeFooter(Skeleton skeleton) throws IOException { // Write the final frame count for files if (outputStream instanceof FileOutputStream) { FileOutputStream fileOutputStream = (FileOutputStream) outputStream; diff --git a/src/main/java/dev/slimevr/posestreamer/PoseDataStream.java b/src/main/java/dev/slimevr/posestreamer/PoseDataStream.java index c4e7f72fe..8818f273a 100644 --- a/src/main/java/dev/slimevr/posestreamer/PoseDataStream.java +++ b/src/main/java/dev/slimevr/posestreamer/PoseDataStream.java @@ -1,6 +1,6 @@ package dev.slimevr.posestreamer; -import dev.slimevr.vr.processor.skeleton.HumanSkeleton; +import dev.slimevr.vr.processor.skeleton.Skeleton; import java.io.*; @@ -22,12 +22,12 @@ public abstract class PoseDataStream implements AutoCloseable { this(new FileOutputStream(file)); } - public void writeHeader(HumanSkeleton skeleton, PoseStreamer streamer) throws IOException { + public void writeHeader(Skeleton skeleton, PoseStreamer streamer) throws IOException { } - abstract void writeFrame(HumanSkeleton skeleton) throws IOException; + abstract void writeFrame(Skeleton skeleton) throws IOException; - public void writeFooter(HumanSkeleton skeleton) throws IOException { + public void writeFooter(Skeleton skeleton) throws IOException { } public boolean isClosed() { diff --git a/src/main/java/dev/slimevr/posestreamer/PoseStreamer.java b/src/main/java/dev/slimevr/posestreamer/PoseStreamer.java index 2ee450d96..cd4dcf935 100644 --- a/src/main/java/dev/slimevr/posestreamer/PoseStreamer.java +++ b/src/main/java/dev/slimevr/posestreamer/PoseStreamer.java @@ -1,6 +1,6 @@ package dev.slimevr.posestreamer; -import dev.slimevr.vr.processor.skeleton.HumanSkeleton; +import dev.slimevr.vr.processor.skeleton.Skeleton; import io.eiren.util.logging.LogManager; import java.io.IOException; @@ -10,10 +10,10 @@ public class PoseStreamer { protected long frameRecordingInterval = 60L; - protected HumanSkeleton skeleton; + protected Skeleton skeleton; protected PoseDataStream poseFileStream; - public PoseStreamer(HumanSkeleton skeleton) { + public PoseStreamer(Skeleton skeleton) { this.skeleton = skeleton; } @@ -43,7 +43,7 @@ public class PoseStreamer { this.frameRecordingInterval = intervalMs; } - public synchronized HumanSkeleton getSkeleton() { + public synchronized Skeleton getSkeleton() { return skeleton; } diff --git a/src/main/java/dev/slimevr/posestreamer/ServerPoseStreamer.java b/src/main/java/dev/slimevr/posestreamer/ServerPoseStreamer.java index a644c06d0..c04de3939 100644 --- a/src/main/java/dev/slimevr/posestreamer/ServerPoseStreamer.java +++ b/src/main/java/dev/slimevr/posestreamer/ServerPoseStreamer.java @@ -2,7 +2,7 @@ package dev.slimevr.posestreamer; import dev.slimevr.VRServer; import dev.slimevr.util.ann.VRServerThread; -import dev.slimevr.vr.processor.skeleton.HumanSkeleton; +import dev.slimevr.vr.processor.skeleton.Skeleton; public class ServerPoseStreamer extends TickPoseStreamer { @@ -19,7 +19,7 @@ public class ServerPoseStreamer extends TickPoseStreamer { } @VRServerThread - public void onSkeletonUpdated(HumanSkeleton skeleton) { + public void onSkeletonUpdated(Skeleton skeleton) { this.skeleton = skeleton; } diff --git a/src/main/java/dev/slimevr/posestreamer/TickPoseStreamer.java b/src/main/java/dev/slimevr/posestreamer/TickPoseStreamer.java index 946fb4137..5ef2e93a8 100644 --- a/src/main/java/dev/slimevr/posestreamer/TickPoseStreamer.java +++ b/src/main/java/dev/slimevr/posestreamer/TickPoseStreamer.java @@ -1,6 +1,6 @@ package dev.slimevr.posestreamer; -import dev.slimevr.vr.processor.skeleton.HumanSkeleton; +import dev.slimevr.vr.processor.skeleton.Skeleton; import java.io.IOException; @@ -9,7 +9,7 @@ public class TickPoseStreamer extends PoseStreamer { protected long nextFrameTimeMs = -1L; - public TickPoseStreamer(HumanSkeleton skeleton) { + public TickPoseStreamer(Skeleton skeleton) { super(skeleton); } @@ -19,7 +19,7 @@ public class TickPoseStreamer extends PoseStreamer { return; } - HumanSkeleton skeleton = this.skeleton; + Skeleton skeleton = this.skeleton; if (skeleton == null) { return; } diff --git a/src/main/java/dev/slimevr/protocol/DataFeedBuilder.java b/src/main/java/dev/slimevr/protocol/DataFeedBuilder.java index 00529b8ae..c7e6b25f9 100644 --- a/src/main/java/dev/slimevr/protocol/DataFeedBuilder.java +++ b/src/main/java/dev/slimevr/protocol/DataFeedBuilder.java @@ -59,7 +59,7 @@ public class DataFeedBuilder { TrackerInfo.startTrackerInfo(fbb); if (tracker.getBodyPosition() != null) - TrackerInfo.addBodyPart(fbb, tracker.getBodyPosition().id); + TrackerInfo.addBodyPart(fbb, tracker.getBodyPosition().bodyPart); TrackerInfo.addEditable(fbb, tracker.userEditable()); TrackerInfo.addComputed(fbb, tracker.isComputed()); // TODO need support: TrackerInfo.addImuType(fbb, tracker.im); diff --git a/src/main/java/dev/slimevr/protocol/RPCHandler.java b/src/main/java/dev/slimevr/protocol/RPCHandler.java index af4d43c36..625c8da6c 100644 --- a/src/main/java/dev/slimevr/protocol/RPCHandler.java +++ b/src/main/java/dev/slimevr/protocol/RPCHandler.java @@ -197,7 +197,7 @@ public class RPCHandler extends ProtocolHandler if (tracker == null) return; - tracker.setBodyPosition(TrackerPosition.getById(req.bodyPosition())); + tracker.setBodyPosition(TrackerPosition.getByBodyPart(req.bodyPosition()).get()); if (tracker instanceof ReferenceAdjustedTracker) { ReferenceAdjustedTracker refTracker = (ReferenceAdjustedTracker) tracker; diff --git a/src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTracker.java b/src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTracker.java index 462638772..5b71a14ba 100644 --- a/src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTracker.java +++ b/src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTracker.java @@ -24,7 +24,8 @@ public class ComputedHumanPoseTracker extends ComputedTracker super(trackerId, "human://" + skeletonPosition.name(), true, true); this.skeletonPosition = skeletonPosition; this.trackerRole = role; - this.bodyPosition = TrackerPosition.getByRole(role); + // TODO: Use `TrackerPosition` instead of `TrackerRole` + this.bodyPosition = TrackerPosition.getByTrackerRole(role).get(); } @Override diff --git a/src/main/java/dev/slimevr/vr/processor/HumanPoseProcessor.java b/src/main/java/dev/slimevr/vr/processor/HumanPoseProcessor.java index 59bc0e668..6c417d87b 100644 --- a/src/main/java/dev/slimevr/vr/processor/HumanPoseProcessor.java +++ b/src/main/java/dev/slimevr/vr/processor/HumanPoseProcessor.java @@ -2,8 +2,8 @@ package dev.slimevr.vr.processor; import dev.slimevr.VRServer; import dev.slimevr.util.ann.VRServerThread; +import dev.slimevr.vr.processor.skeleton.Skeleton; import dev.slimevr.vr.processor.skeleton.HumanSkeleton; -import dev.slimevr.vr.processor.skeleton.SimpleSkeleton; import dev.slimevr.vr.processor.skeleton.SkeletonConfig; import dev.slimevr.vr.processor.skeleton.SkeletonConfigValue; import dev.slimevr.vr.trackers.*; @@ -18,8 +18,8 @@ public class HumanPoseProcessor { private final VRServer server; private final List computedTrackers = new FastList<>(); - private final List> onSkeletonUpdated = new FastList<>(); - private HumanSkeleton skeleton; + private final List> onSkeletonUpdated = new FastList<>(); + private Skeleton skeleton; public HumanPoseProcessor(VRServer server, HMDTracker hmd) { this.server = server; @@ -105,12 +105,12 @@ public class HumanPoseProcessor { ); } - public HumanSkeleton getSkeleton() { + public Skeleton getSkeleton() { return skeleton; } @VRServerThread - public void addSkeletonUpdatedCallback(Consumer consumer) { + public void addSkeletonUpdatedCallback(Consumer consumer) { onSkeletonUpdated.add(consumer); if (skeleton != null) consumer.accept(skeleton); @@ -165,8 +165,8 @@ public class HumanPoseProcessor { @VRServerThread private void updateSekeltonModel() { disconnectAllTrackers(); - skeleton = new SimpleSkeleton(server, computedTrackers); - for (Consumer sc : onSkeletonUpdated) + skeleton = new HumanSkeleton(server, computedTrackers); + for (Consumer sc : onSkeletonUpdated) sc.accept(skeleton); } diff --git a/src/main/java/dev/slimevr/vr/processor/skeleton/BoneInfo.java b/src/main/java/dev/slimevr/vr/processor/skeleton/BoneInfo.java new file mode 100644 index 000000000..aff50a7f5 --- /dev/null +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/BoneInfo.java @@ -0,0 +1,22 @@ +package dev.slimevr.vr.processor.skeleton; + + +import dev.slimevr.vr.processor.TransformNode; + + +public class BoneInfo { + + public final BoneType boneType; + public final TransformNode node; + public float length; + + public BoneInfo(BoneType boneType, TransformNode node) { + this.boneType = boneType; + this.node = node; + updateLength(); + } + + public void updateLength() { + this.length = node.localTransform.getTranslation().length(); + } +} diff --git a/src/main/java/dev/slimevr/vr/processor/skeleton/BoneType.java b/src/main/java/dev/slimevr/vr/processor/skeleton/BoneType.java new file mode 100644 index 000000000..93cfb0e02 --- /dev/null +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/BoneType.java @@ -0,0 +1,83 @@ +package dev.slimevr.vr.processor.skeleton; + +import solarxr_protocol.datatypes.BodyPart; + + +/** + * Keys for all bones in the skeleton. TODO: Some bones are deprecated because + * they are still used as SkeletonNodeOffset, and represent both left and right + * offsets, but they should be split into two. + */ +public enum BoneType { + + HEAD(BodyPart.HMD), + NECK(BodyPart.NECK), + CHEST(BodyPart.CHEST), + CHEST_TRACKER(BodyPart.CHEST), + WAIST(BodyPart.WAIST), + WAIST_TRACKER(BodyPart.WAIST), + HIP(BodyPart.HIP), + HIP_TRACKER(BodyPart.HIP), + LEFT_HIP, + RIGHT_HIP, + @Deprecated + UPPER_LEG, + LEFT_UPPER_LEG(BodyPart.LEFT_KNEE), + RIGHT_UPPER_LEG(BodyPart.RIGHT_KNEE), + @Deprecated + KNEE_TRACKER, + LEFT_KNEE_TRACKER, + RIGHT_KNEE_TRACKER, + @Deprecated + LOWER_LEG, + LEFT_LOWER_LEG(BodyPart.LEFT_ANKLE), + RIGHT_LOWER_LEG(BodyPart.RIGHT_ANKLE), + @Deprecated + FOOT, + LEFT_FOOT(BodyPart.LEFT_FOOT), + RIGHT_FOOT(BodyPart.RIGHT_FOOT), + @Deprecated + FOOT_TRACKER, + LEFT_FOOT_TRACKER(BodyPart.LEFT_FOOT), + RIGHT_FOOT_TRACKER(BodyPart.RIGHT_FOOT), + @Deprecated + CONTROLLER, + LEFT_CONTROLLER(BodyPart.LEFT_CONTROLLER), + RIGHT_CONTROLLER(BodyPart.RIGHT_CONTROLLER), + @Deprecated + LOWER_ARM, + LEFT_LOWER_ARM(BodyPart.LEFT_FOREARM), + RIGHT_LOWER_ARM(BodyPart.RIGHT_FOREARM), + @Deprecated + LOWER_ARM_HMD, + @Deprecated + ELBOW_TRACKER, + LEFT_ELBOW_TRACKER, + RIGHT_ELBOW_TRACKER, + @Deprecated + UPPER_ARM, + LEFT_UPPER_ARM, + RIGHT_UPPER_ARM, + LEFT_SHOULDER, + RIGHT_SHOULDER, + @Deprecated + HAND, + LEFT_HAND, + RIGHT_HAND, + @Deprecated + HAND_TRACKER, + LEFT_HAND_TRACKER, + RIGHT_HAND_TRACKER; + + public static final BoneType[] values = values(); + + public final int bodyPart; + + private BoneType() { + this.bodyPart = BodyPart.NONE; + } + + private BoneType(int associatedBodyPart) { + this.bodyPart = associatedBodyPart; + } +} diff --git a/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java b/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java index 451f616f7..2416d4e23 100644 --- a/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java @@ -1,37 +1,1504 @@ package dev.slimevr.vr.processor.skeleton; +import com.jme3.math.FastMath; +import com.jme3.math.Quaternion; +import com.jme3.math.Vector3f; +import dev.slimevr.VRServer; import dev.slimevr.util.ann.VRServerThread; +import dev.slimevr.vr.processor.ComputedHumanPoseTracker; +import dev.slimevr.vr.processor.ComputedHumanPoseTrackerPosition; import dev.slimevr.vr.processor.TransformNode; -import io.eiren.util.ann.ThreadSafe; +import dev.slimevr.vr.trackers.*; +import io.eiren.util.collections.FastList; + +import java.util.ArrayList; +import java.util.List; +import java.util.Map; -public abstract class HumanSkeleton { +public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback { - @VRServerThread - public abstract void updatePose(); + public final SkeletonConfig skeletonConfig; + public final List currentBoneInfo = new ArrayList<>(); + // #region Upper body nodes (torso) + protected final TransformNode hmdNode = new TransformNode("HMD", false); + protected final TransformNode headNode = new TransformNode("Head", false); + protected final TransformNode neckNode = new TransformNode("Neck", false); + protected final TransformNode chestNode = new TransformNode("Chest", false); + protected final TransformNode trackerChestNode = new TransformNode("Chest-Tracker", false); + protected final TransformNode waistNode = new TransformNode("Waist", false); + protected final TransformNode hipNode = new TransformNode("Hip", false); + // #endregion + protected final TransformNode trackerWaistNode = new TransformNode("Waist-Tracker", false); + // #region Lower body nodes (legs) + protected final TransformNode leftHipNode = new TransformNode("Left-Hip", false); + protected final TransformNode leftKneeNode = new TransformNode("Left-Knee", false); + protected final TransformNode trackerLeftKneeNode = new TransformNode( + "Left-Knee-Tracker", + false + ); + protected final TransformNode leftAnkleNode = new TransformNode("Left-Ankle", false); + protected final TransformNode leftFootNode = new TransformNode("Left-Foot", false); + protected final TransformNode trackerLeftFootNode = new TransformNode( + "Left-Foot-Tracker", + false + ); + protected final TransformNode rightHipNode = new TransformNode("Right-Hip", false); + protected final TransformNode rightKneeNode = new TransformNode("Right-Knee", false); + protected final TransformNode trackerRightKneeNode = new TransformNode( + "Right-Knee-Tracker", + false + ); + protected final TransformNode rightAnkleNode = new TransformNode("Right-Ankle", false); + protected final TransformNode rightFootNode = new TransformNode("Right-Foot", false); + protected final TransformNode trackerRightFootNode = new TransformNode( + "Right-Foot-Tracker", + false + ); + // #region Arms (from controllers) + protected final TransformNode leftControllerNodeContrl = new TransformNode( + "Left-Controller-Contrl", + false + ); + protected final TransformNode rightControllerNodeContrl = new TransformNode( + "Right-Controller-Contrl", + false + ); + // #endregion + protected final TransformNode leftWristNodeContrl = new TransformNode( + "Left-Wrist-Contrl", + false + ); + protected final TransformNode rightWristNodeContrl = new TransformNode( + "Right-Wrist-Contrl", + false + ); + protected final TransformNode leftElbowNodeContrl = new TransformNode( + "Left-Elbow-Contrl", + false + ); + protected final TransformNode rightElbowNodeContrl = new TransformNode( + "Right-Elbow-Contrl", + false + ); + protected final TransformNode trackerLeftElbowNodeContrl = new TransformNode( + "Left-Elbow-Tracker-Contrl", + false + ); + protected final TransformNode trackerRightElbowNodeContrl = new TransformNode( + "Right-Elbow-Tracker-Contrl", + false + ); + // #region Arms (from HMD) + protected final TransformNode leftShoulderNodeHmd = new TransformNode( + "Left-Shoulder-Hmd", + false + ); + protected final TransformNode rightShoulderNodeHmd = new TransformNode( + "Right-Shoulder-Hmd", + false + ); + // #endregion + protected final TransformNode leftElbowNodeHmd = new TransformNode("Left-Elbow-Hmd", false); + protected final TransformNode rightElbowNodeHmd = new TransformNode("Right-Elbow-Hmd", false); + protected final TransformNode trackerLeftElbowNodeHmd = new TransformNode( + "Left-Elbow-Tracker-Hmd", + false + ); + protected final TransformNode trackerRightElbowNodeHmd = new TransformNode( + "Right-Elbow-Tracker-Hmd", + false + ); + protected final TransformNode leftWristNodeHmd = new TransformNode("Left-Wrist-Hmd", false); + protected final TransformNode rightWristNodeHmd = new TransformNode("Right-Wrist-Hmd", false); + protected final TransformNode leftHandNodeHmd = new TransformNode("Left-Hand-Hmd", false); + protected final TransformNode rightHandNodeHmd = new TransformNode("Right-Hand-Hmd", false); + protected final TransformNode trackerLeftHandNodeHmd = new TransformNode( + "Left-Hand-Tracker-Hmd", + false + ); + protected final TransformNode trackerRightHandNodeHmd = new TransformNode( + "Right-Hand-Tracker-Hmd", + false + ); + protected final Vector3f hipVector = new Vector3f(); + protected final Vector3f ankleVector = new Vector3f(); + // #endregion + protected final Quaternion kneeRotation = new Quaternion(); + // #region Buffers + private final Vector3f posBuf = new Vector3f(); + private final Quaternion rotBuf2 = new Quaternion(); + protected float minKneePitch = 0f * FastMath.DEG_TO_RAD; + protected float maxKneePitch = 90f * FastMath.DEG_TO_RAD; + protected float kneeLerpFactor = 0.5f; + // #region Tracker Input + protected Tracker hmdTracker; + protected Tracker neckTracker; + protected Tracker chestTracker; + protected Tracker waistTracker; + protected Tracker hipTracker; + protected Tracker leftKneeTracker; + protected Tracker leftAnkleTracker; + protected Tracker leftFootTracker; + protected Tracker rightKneeTracker; + protected Tracker rightAnkleTracker; + protected Tracker rightFootTracker; + protected Tracker leftControllerTracker; + protected Tracker rightControllerTracker; + protected Tracker leftForearmTracker; + protected Tracker rightForearmTracker; + // #endregion + protected Tracker leftUpperArmTracker; + protected Tracker rightUpperArmTracker; + protected Tracker leftHandTracker; + protected Tracker rightHandTracker; + // #region Tracker Output + protected ComputedHumanPoseTracker computedChestTracker; + protected ComputedHumanPoseTracker computedWaistTracker; + protected ComputedHumanPoseTracker computedLeftKneeTracker; + protected ComputedHumanPoseTracker computedLeftFootTracker; + protected ComputedHumanPoseTracker computedRightKneeTracker; + protected ComputedHumanPoseTracker computedRightFootTracker; + // #endregion + protected ComputedHumanPoseTracker computedLeftElbowTracker; + protected ComputedHumanPoseTracker computedRightElbowTracker; + protected ComputedHumanPoseTracker computedLeftHandTracker; + protected ComputedHumanPoseTracker computedRightHandTracker; + protected boolean extendedPelvisModel = true; + protected boolean extendedKneeModel = false; + private Quaternion rotBuf1 = new Quaternion(); + private boolean hasSpineTracker, hasKneeTracker; + // #endregion - @ThreadSafe - public abstract TransformNode getRootNode(); + // #region Constructors + protected HumanSkeleton(List computedTrackers) { + // #region Assemble skeleton from hmd to hip + hmdNode.attachChild(headNode); + headNode.attachChild(neckNode); + neckNode.attachChild(chestNode); + chestNode.attachChild(waistNode); + waistNode.attachChild(hipNode); + // #endregion - @ThreadSafe - public abstract TransformNode[] getAllNodes(); + // #region Assemble skeleton from hips to feet + hipNode.attachChild(leftHipNode); + hipNode.attachChild(rightHipNode); - @ThreadSafe - public abstract SkeletonConfig getSkeletonConfig(); + leftHipNode.attachChild(leftKneeNode); + rightHipNode.attachChild(rightKneeNode); - @ThreadSafe - public abstract void resetSkeletonConfig(SkeletonConfigValue config); + leftKneeNode.attachChild(leftAnkleNode); + rightKneeNode.attachChild(rightAnkleNode); - @ThreadSafe - public void resetAllSkeletonConfigs() { - for (SkeletonConfigValue config : SkeletonConfigValue.values) { - resetSkeletonConfig(config); + leftAnkleNode.attachChild(leftFootNode); + rightAnkleNode.attachChild(rightFootNode); + // #endregion + + // #region Assemble skeleton arms from controllers + leftControllerNodeContrl.attachChild(leftWristNodeContrl); + rightControllerNodeContrl.attachChild(rightWristNodeContrl); + leftWristNodeContrl.attachChild(leftElbowNodeContrl); + rightWristNodeContrl.attachChild(rightElbowNodeContrl); + // #endregion + + // #region Assemble skeleton arms from chest + chestNode.attachChild(leftShoulderNodeHmd); + chestNode.attachChild(rightShoulderNodeHmd); + + leftShoulderNodeHmd.attachChild(leftElbowNodeHmd); + rightShoulderNodeHmd.attachChild(rightElbowNodeHmd); + + leftElbowNodeHmd.attachChild(leftWristNodeHmd); + rightElbowNodeHmd.attachChild(rightWristNodeHmd); + + leftWristNodeHmd.attachChild(leftHandNodeHmd); + rightWristNodeHmd.attachChild(rightHandNodeHmd); + // #endregion + + // #region Attach tracker nodes for offsets + chestNode.attachChild(trackerChestNode); + hipNode.attachChild(trackerWaistNode); + + leftKneeNode.attachChild(trackerLeftKneeNode); + rightKneeNode.attachChild(trackerRightKneeNode); + + leftFootNode.attachChild(trackerLeftFootNode); + rightFootNode.attachChild(trackerRightFootNode); + + leftElbowNodeContrl.attachChild(trackerLeftElbowNodeContrl); + rightElbowNodeContrl.attachChild(trackerRightElbowNodeContrl); + + leftElbowNodeHmd.attachChild(trackerLeftElbowNodeHmd); + rightElbowNodeHmd.attachChild(trackerRightElbowNodeHmd); + + leftHandNodeHmd.attachChild(trackerLeftHandNodeHmd); + rightHandNodeHmd.attachChild(trackerRightHandNodeHmd); + // #endregion + + // Set default skeleton configuration (callback automatically sets + // initial + // offsets) + skeletonConfig = new SkeletonConfig(true, this); + + if (computedTrackers != null) { + setComputedTrackers(computedTrackers); + } + fillNullComputedTrackers(true); + resetBones(); + } + + public HumanSkeleton( + VRServer server, + List computedTrackers + ) { + this(computedTrackers); + setTrackersFromServer(server); + skeletonConfig.loadFromConfig(server.config); + } + + public HumanSkeleton( + List trackers, + List computedTrackers + ) { + this(computedTrackers); + + if (trackers != null) { + setTrackersFromList(trackers); + } else { + setTrackersFromList(new FastList(0)); } } - @VRServerThread - public abstract void resetTrackersFull(); + public HumanSkeleton( + List trackers, + List computedTrackers, + Map configs, + Map altConfigs + ) { + // Initialize + this(trackers, computedTrackers); + // Set configs + if (altConfigs != null) { + // Set alts first, so if there's any overlap it doesn't affect the + // values + skeletonConfig.setConfigs(altConfigs, null); + } + skeletonConfig.setConfigs(configs, null); + } + + public HumanSkeleton( + List trackers, + List computedTrackers, + Map configs + ) { + this(trackers, computedTrackers, configs, null); + } + // #endregion + + public static float normalizeRad(float angle) { + return FastMath.normalize(angle, -FastMath.PI, FastMath.PI); + } + + public static float interpolateRadians(float factor, float start, float end) { + float angle = FastMath.abs(end - start); + if (angle > FastMath.PI) { + if (end > start) { + start += FastMath.TWO_PI; + } else { + end += FastMath.TWO_PI; + } + } + float val = start + (end - start) * factor; + return normalizeRad(val); + } + + protected void resetBones() { + currentBoneInfo.clear(); + // #region Assemble skeleton from hmd to hip + currentBoneInfo.add(new BoneInfo(BoneType.HEAD, headNode)); + currentBoneInfo.add(new BoneInfo(BoneType.NECK, neckNode)); + currentBoneInfo.add(new BoneInfo(BoneType.CHEST, chestNode)); + currentBoneInfo.add(new BoneInfo(BoneType.WAIST, waistNode)); + currentBoneInfo.add(new BoneInfo(BoneType.HIP, hipNode)); + // #endregion + + // #region Assemble skeleton from hips to feet + if (leftAnkleTracker != null || leftKneeTracker != null || leftFootTracker != null) { + currentBoneInfo.add(new BoneInfo(BoneType.LEFT_HIP, leftHipNode)); + currentBoneInfo.add(new BoneInfo(BoneType.LEFT_UPPER_LEG, leftKneeNode)); + currentBoneInfo.add(new BoneInfo(BoneType.LEFT_LOWER_LEG, leftKneeNode)); + currentBoneInfo.add(new BoneInfo(BoneType.LEFT_FOOT, leftKneeNode)); + } + + if (rightAnkleTracker != null || rightKneeTracker != null || rightFootTracker != null) { + currentBoneInfo.add(new BoneInfo(BoneType.RIGHT_HIP, rightHipNode)); + currentBoneInfo.add(new BoneInfo(BoneType.RIGHT_UPPER_LEG, rightKneeNode)); + currentBoneInfo.add(new BoneInfo(BoneType.RIGHT_LOWER_LEG, rightKneeNode)); + currentBoneInfo.add(new BoneInfo(BoneType.RIGHT_FOOT, rightKneeNode)); + } + // #endregion + // #region Assemble skeleton arms from controllers + if (leftControllerTracker != null) { + currentBoneInfo.add(new BoneInfo(BoneType.LEFT_CONTROLLER, leftWristNodeContrl)); + if (leftForearmTracker != null) { + currentBoneInfo.add(new BoneInfo(BoneType.LEFT_LOWER_ARM, leftElbowNodeContrl)); + } + } + if (rightControllerTracker != null) { + currentBoneInfo.add(new BoneInfo(BoneType.RIGHT_CONTROLLER, rightWristNodeContrl)); + if (rightForearmTracker != null) { + currentBoneInfo.add(new BoneInfo(BoneType.RIGHT_LOWER_ARM, rightElbowNodeContrl)); + } + } + // #endregion + + // #region Assemble skeleton arms from chest + if (rightUpperArmTracker != null) { + currentBoneInfo.add(new BoneInfo(BoneType.RIGHT_SHOULDER, rightShoulderNodeHmd)); + currentBoneInfo.add(new BoneInfo(BoneType.RIGHT_UPPER_ARM, rightElbowNodeHmd)); + if (rightControllerTracker == null && rightForearmTracker != null) { + currentBoneInfo.add(new BoneInfo(BoneType.RIGHT_LOWER_ARM, rightWristNodeHmd)); + if (rightHandTracker != null) { + currentBoneInfo.add(new BoneInfo(BoneType.RIGHT_HAND, rightHandNodeHmd)); + } + } + } + if (leftUpperArmTracker != null) { + currentBoneInfo.add(new BoneInfo(BoneType.LEFT_SHOULDER, leftShoulderNodeHmd)); + currentBoneInfo.add(new BoneInfo(BoneType.LEFT_UPPER_ARM, leftElbowNodeHmd)); + if (leftControllerTracker == null && leftForearmTracker != null) { + currentBoneInfo.add(new BoneInfo(BoneType.LEFT_LOWER_ARM, leftWristNodeHmd)); + if (leftHandTracker != null) { + currentBoneInfo.add(new BoneInfo(BoneType.LEFT_HAND, leftHandNodeHmd)); + } + } + } + // #endregion + + // #region Attach tracker nodes for offsets + if (true) { // Set to false to skip tracker bones + currentBoneInfo.add(new BoneInfo(BoneType.CHEST_TRACKER, trackerChestNode)); + currentBoneInfo.add(new BoneInfo(BoneType.HIP_TRACKER, trackerWaistNode)); + + if (leftAnkleTracker != null || leftKneeTracker != null || leftFootTracker != null) { + currentBoneInfo.add(new BoneInfo(BoneType.LEFT_KNEE_TRACKER, trackerLeftKneeNode)); + currentBoneInfo.add(new BoneInfo(BoneType.LEFT_FOOT_TRACKER, trackerLeftFootNode)); + } + if (rightAnkleTracker != null || rightKneeTracker != null || rightFootTracker != null) { + currentBoneInfo + .add(new BoneInfo(BoneType.RIGHT_KNEE_TRACKER, trackerRightKneeNode)); + currentBoneInfo + .add(new BoneInfo(BoneType.RIGHT_FOOT_TRACKER, trackerRightFootNode)); + } + if (leftControllerTracker != null && leftForearmTracker != null) { + currentBoneInfo + .add(new BoneInfo(BoneType.LEFT_ELBOW_TRACKER, trackerLeftElbowNodeContrl)); + } + if (rightControllerTracker != null && rightForearmTracker != null) { + currentBoneInfo + .add( + new BoneInfo(BoneType.RIGHT_ELBOW_TRACKER, trackerRightElbowNodeContrl) + ); + } + + if (leftControllerTracker == null && leftForearmTracker != null) { + currentBoneInfo + .add(new BoneInfo(BoneType.LEFT_ELBOW_TRACKER, trackerLeftElbowNodeHmd)); + } + if (rightControllerTracker != null && rightForearmTracker != null) { + currentBoneInfo + .add(new BoneInfo(BoneType.RIGHT_ELBOW_TRACKER, trackerLeftElbowNodeHmd)); + } + + if (leftHandTracker != null) { + currentBoneInfo + .add(new BoneInfo(BoneType.LEFT_HAND_TRACKER, trackerLeftHandNodeHmd)); + } + if (rightHandTracker != null) { + currentBoneInfo + .add(new BoneInfo(BoneType.RIGHT_HAND_TRACKER, trackerLeftHandNodeHmd)); + } + } + // #endregion + } + + // #region Set Trackers + public void setTrackersFromList(List trackers, boolean setHmd) { + if (setHmd) { + this.hmdTracker = TrackerUtils + .findTrackerForBodyPosition(trackers, TrackerPosition.HMD); + } + + this.neckTracker = TrackerUtils + .findTrackerForBodyPositionOrEmpty( + trackers, + TrackerPosition.NECK, + TrackerPosition.HMD, + null + ); + this.chestTracker = TrackerUtils + .findTrackerForBodyPositionOrEmpty( + trackers, + TrackerPosition.CHEST, + TrackerPosition.WAIST, + TrackerPosition.HIP + ); + this.waistTracker = TrackerUtils + .findTrackerForBodyPositionOrEmpty( + trackers, + TrackerPosition.WAIST, + TrackerPosition.HIP, + TrackerPosition.CHEST + ); + this.hipTracker = TrackerUtils + .findTrackerForBodyPositionOrEmpty( + trackers, + TrackerPosition.HIP, + TrackerPosition.WAIST, + TrackerPosition.CHEST + ); + + this.leftKneeTracker = TrackerUtils + .findTrackerForBodyPositionOrEmpty( + trackers, + TrackerPosition.LEFT_KNEE, + TrackerPosition.LEFT_ANKLE, + null + ); + this.leftAnkleTracker = TrackerUtils + .findTrackerForBodyPositionOrEmpty( + trackers, + TrackerPosition.LEFT_ANKLE, + TrackerPosition.LEFT_KNEE, + null + ); + this.leftFootTracker = TrackerUtils + .findTrackerForBodyPosition(trackers, TrackerPosition.LEFT_FOOT); + + this.rightKneeTracker = TrackerUtils + .findTrackerForBodyPositionOrEmpty( + trackers, + TrackerPosition.RIGHT_KNEE, + TrackerPosition.RIGHT_ANKLE, + null + ); + this.rightAnkleTracker = TrackerUtils + .findTrackerForBodyPositionOrEmpty( + trackers, + TrackerPosition.RIGHT_ANKLE, + TrackerPosition.RIGHT_KNEE, + null + ); + this.rightFootTracker = TrackerUtils + .findTrackerForBodyPosition(trackers, TrackerPosition.RIGHT_FOOT); + + this.leftControllerTracker = TrackerUtils + .findTrackerForBodyPosition(trackers, TrackerPosition.LEFT_CONTROLLER); + this.rightControllerTracker = TrackerUtils + .findTrackerForBodyPosition(trackers, TrackerPosition.RIGHT_CONTROLLER); + this.leftForearmTracker = TrackerUtils + .findTrackerForBodyPositionOrEmpty( + trackers, + TrackerPosition.LEFT_FOREARM, + TrackerPosition.LEFT_UPPER_ARM, + null + ); + this.rightForearmTracker = TrackerUtils + .findTrackerForBodyPositionOrEmpty( + trackers, + TrackerPosition.RIGHT_FOREARM, + TrackerPosition.RIGHT_UPPER_ARM, + null + ); + this.leftUpperArmTracker = TrackerUtils + .findTrackerForBodyPositionOrEmpty( + trackers, + TrackerPosition.LEFT_UPPER_ARM, + TrackerPosition.LEFT_FOREARM, + null + ); + this.rightUpperArmTracker = TrackerUtils + .findTrackerForBodyPositionOrEmpty( + trackers, + TrackerPosition.RIGHT_UPPER_ARM, + TrackerPosition.RIGHT_FOREARM, + null + ); + this.leftHandTracker = TrackerUtils + .findTrackerForBodyPosition(trackers, TrackerPosition.LEFT_HAND); + this.rightHandTracker = TrackerUtils + .findTrackerForBodyPosition(trackers, TrackerPosition.RIGHT_HAND); + resetBones(); + } + + public void setTrackersFromList(List trackers) { + setTrackersFromList(trackers, true); + } + + public void setTrackersFromServer(VRServer server) { + this.hmdTracker = server.hmdTracker; + setTrackersFromList(server.getAllTrackers(), false); + } + + public void setComputedTracker(ComputedHumanPoseTracker tracker) { + switch (tracker.getTrackerRole()) { + case CHEST: + computedChestTracker = tracker; + break; + case WAIST: + computedWaistTracker = tracker; + break; + + case LEFT_KNEE: + computedLeftKneeTracker = tracker; + break; + case LEFT_FOOT: + computedLeftFootTracker = tracker; + break; + + case RIGHT_KNEE: + computedRightKneeTracker = tracker; + break; + case RIGHT_FOOT: + computedRightFootTracker = tracker; + break; + + case LEFT_ELBOW: + computedLeftElbowTracker = tracker; + break; + case RIGHT_ELBOW: + computedRightElbowTracker = tracker; + break; + + case LEFT_HAND: + computedLeftHandTracker = tracker; + break; + case RIGHT_HAND: + computedRightHandTracker = tracker; + break; + default: + break; + } + } + + public void setComputedTrackers(List trackers) { + for (ComputedHumanPoseTracker t : trackers) { + setComputedTracker(t); + } + } + // #endregion + + public void setComputedTrackersAndFillNull( + List trackers, + boolean onlyFillWaistAndFeet + ) { + setComputedTrackers(trackers); + fillNullComputedTrackers(onlyFillWaistAndFeet); + } + // #endregion + + public void fillNullComputedTrackers(boolean onlyFillWaistAndFeet) { + if (computedWaistTracker == null) { + computedWaistTracker = new ComputedHumanPoseTracker( + Tracker.getNextLocalTrackerId(), + ComputedHumanPoseTrackerPosition.WAIST, + TrackerRole.WAIST + ); + computedWaistTracker.setStatus(TrackerStatus.OK); + } + + if (computedLeftFootTracker == null) { + computedLeftFootTracker = new ComputedHumanPoseTracker( + Tracker.getNextLocalTrackerId(), + ComputedHumanPoseTrackerPosition.LEFT_FOOT, + TrackerRole.LEFT_FOOT + ); + computedLeftFootTracker.setStatus(TrackerStatus.OK); + } + + if (computedRightFootTracker == null) { + computedRightFootTracker = new ComputedHumanPoseTracker( + Tracker.getNextLocalTrackerId(), + ComputedHumanPoseTrackerPosition.RIGHT_FOOT, + TrackerRole.RIGHT_FOOT + ); + computedRightFootTracker.setStatus(TrackerStatus.OK); + } + + if (!onlyFillWaistAndFeet) { + if (computedChestTracker == null) { + computedChestTracker = new ComputedHumanPoseTracker( + Tracker.getNextLocalTrackerId(), + ComputedHumanPoseTrackerPosition.CHEST, + TrackerRole.CHEST + ); + computedChestTracker.setStatus(TrackerStatus.OK); + } + + if (computedLeftKneeTracker == null) { + computedLeftKneeTracker = new ComputedHumanPoseTracker( + Tracker.getNextLocalTrackerId(), + ComputedHumanPoseTrackerPosition.LEFT_KNEE, + TrackerRole.LEFT_KNEE + ); + computedLeftKneeTracker.setStatus(TrackerStatus.OK); + } + + if (computedRightKneeTracker == null) { + computedRightKneeTracker = new ComputedHumanPoseTracker( + Tracker.getNextLocalTrackerId(), + ComputedHumanPoseTrackerPosition.RIGHT_KNEE, + TrackerRole.RIGHT_KNEE + ); + computedRightKneeTracker.setStatus(TrackerStatus.OK); + } + + if (computedLeftElbowTracker == null) { + computedLeftElbowTracker = new ComputedHumanPoseTracker( + Tracker.getNextLocalTrackerId(), + ComputedHumanPoseTrackerPosition.LEFT_ELBOW, + TrackerRole.LEFT_ELBOW + ); + computedLeftElbowTracker.setStatus(TrackerStatus.OK); + } + if (computedRightElbowTracker == null) { + computedRightElbowTracker = new ComputedHumanPoseTracker( + Tracker.getNextLocalTrackerId(), + ComputedHumanPoseTrackerPosition.RIGHT_ELBOW, + TrackerRole.RIGHT_ELBOW + ); + computedRightElbowTracker.setStatus(TrackerStatus.OK); + } + + if (computedLeftHandTracker == null) { + computedLeftHandTracker = new ComputedHumanPoseTracker( + Tracker.getNextLocalTrackerId(), + ComputedHumanPoseTrackerPosition.LEFT_HAND, + TrackerRole.LEFT_HAND + ); + computedLeftHandTracker.setStatus(TrackerStatus.OK); + } + if (computedRightHandTracker == null) { + computedRightHandTracker = new ComputedHumanPoseTracker( + Tracker.getNextLocalTrackerId(), + ComputedHumanPoseTrackerPosition.RIGHT_HAND, + TrackerRole.RIGHT_HAND + ); + computedRightHandTracker.setStatus(TrackerStatus.OK); + } + } + } + + // #region Get Trackers + public ComputedHumanPoseTracker getComputedTracker(TrackerRole trackerRole) { + switch (trackerRole) { + case CHEST: + return computedChestTracker; + case WAIST: + return computedWaistTracker; + + case LEFT_KNEE: + return computedLeftKneeTracker; + case LEFT_FOOT: + return computedLeftFootTracker; + + case RIGHT_KNEE: + return computedRightKneeTracker; + case RIGHT_FOOT: + return computedRightFootTracker; + + case LEFT_ELBOW: + return computedLeftElbowTracker; + case RIGHT_ELBOW: + return computedRightElbowTracker; + + case LEFT_HAND: + return computedLeftHandTracker; + case RIGHT_HAND: + return computedRightHandTracker; + default: + break; + } + + return null; + } + + // #region Processing + // Useful for sub-classes that need to return a sub-tracker (like + // PoseFrameTracker -> TrackerFrame) + protected Tracker trackerPreUpdate(Tracker tracker) { + return tracker; + } + + // Updates the pose from tracker positions @VRServerThread - public abstract void resetTrackersYaw(); + @Override + public void updatePose() { + updateLocalTransforms(); + updateRootTrackers(); + updateComputedTrackers(); + } + // #endregion + + void updateRootTrackers() { + hmdNode.update(); + leftControllerNodeContrl.update(); + rightControllerNodeContrl.update(); + } + + // #region Update the node transforms from the trackers + protected void updateLocalTransforms() { + // #region Pass all trackers through trackerPreUpdate + Tracker hmdTracker = trackerPreUpdate(this.hmdTracker); + + Tracker neckTracker = trackerPreUpdate(this.neckTracker); + Tracker chestTracker = trackerPreUpdate(this.chestTracker); + Tracker waistTracker = trackerPreUpdate(this.waistTracker); + Tracker hipTracker = trackerPreUpdate(this.hipTracker); + + Tracker leftKneeTracker = trackerPreUpdate(this.leftKneeTracker); + Tracker leftAnkleTracker = trackerPreUpdate(this.leftAnkleTracker); + Tracker leftFootTracker = trackerPreUpdate(this.leftFootTracker); + + Tracker rightKneeTracker = trackerPreUpdate(this.rightKneeTracker); + Tracker rightAnkleTracker = trackerPreUpdate(this.rightAnkleTracker); + Tracker rightFootTracker = trackerPreUpdate(this.rightFootTracker); + + Tracker leftControllerTracker = trackerPreUpdate(this.leftControllerTracker); + Tracker rightControllerTracker = trackerPreUpdate(this.rightControllerTracker); + Tracker rightForearmTracker = trackerPreUpdate(this.rightForearmTracker); + Tracker leftForearmTracker = trackerPreUpdate(this.leftForearmTracker); + Tracker rightUpperArmTracker = trackerPreUpdate(this.rightUpperArmTracker); + Tracker leftUpperArmTracker = trackerPreUpdate(this.leftUpperArmTracker); + Tracker leftHandTracker = trackerPreUpdate(this.leftHandTracker); + Tracker rightHandTracker = trackerPreUpdate(this.rightHandTracker); + // #endregion + + hasSpineTracker = chestTracker.hasRotation() + || waistTracker.hasRotation() + || hipTracker.hasRotation(); + hasKneeTracker = leftKneeTracker.hasRotation() || rightKneeTracker.hasRotation(); + + if (hmdTracker != null) { + if (hmdTracker.getPosition(posBuf)) { + hmdNode.localTransform.setTranslation(posBuf); + } + if (hmdTracker.getRotation(rotBuf1)) { + hmdNode.localTransform.setRotation(rotBuf1); + neckTracker.getRotation(rotBuf1); + headNode.localTransform.setRotation(rotBuf1); + } + } else { + // Set to zero + hmdNode.localTransform.setTranslation(Vector3f.ZERO); + hmdNode.localTransform.setRotation(Quaternion.IDENTITY); + headNode.localTransform.setRotation(Quaternion.IDENTITY); + } + + // Spine + if (hasSpineTracker) { + if (chestTracker.getRotation(rotBuf1)) { + neckNode.localTransform.setRotation(rotBuf1); + } + if (waistTracker.getRotation(rotBuf1)) { + chestNode.localTransform.setRotation(rotBuf1); + trackerChestNode.localTransform.setRotation(rotBuf1); + } + if (hipTracker.getRotation(rotBuf1)) { + waistNode.localTransform.setRotation(rotBuf1); + hipNode.localTransform.setRotation(rotBuf1); + trackerWaistNode.localTransform.setRotation(rotBuf1); + } + } else if (hmdTracker != null) { // If no spine tracker, allign spine + // yaw with HMD + rotBuf1 = rotBuf1.fromAngles(0, rotBuf1.getYaw(), 0); + neckNode.localTransform.setRotation(rotBuf1); + chestNode.localTransform.setRotation(rotBuf1); + trackerChestNode.localTransform.setRotation(rotBuf1); + waistNode.localTransform.setRotation(rotBuf1); + hipNode.localTransform.setRotation(rotBuf1); + trackerWaistNode.localTransform.setRotation(rotBuf1); + } + + // Left Leg + leftKneeTracker.getRotation(rotBuf1); + leftAnkleTracker.getRotation(rotBuf2); + + if (extendedKneeModel) { + calculateKneeLimits( + rotBuf1, + rotBuf2, + leftKneeTracker.getConfidenceLevel(), + leftAnkleTracker.getConfidenceLevel() + ); + } + + leftHipNode.localTransform.setRotation(rotBuf1); + leftKneeNode.localTransform.setRotation(rotBuf2); + leftAnkleNode.localTransform.setRotation(rotBuf2); + leftFootNode.localTransform.setRotation(rotBuf2); + + trackerLeftKneeNode.localTransform.setRotation(rotBuf2); + trackerLeftFootNode.localTransform.setRotation(rotBuf2); + + if (leftFootTracker != null) { + leftFootTracker.getRotation(rotBuf2); + leftAnkleNode.localTransform.setRotation(rotBuf2); + leftFootNode.localTransform.setRotation(rotBuf2); + trackerLeftFootNode.localTransform.setRotation(rotBuf2); + } + + // Right Leg + rightKneeTracker.getRotation(rotBuf1); + rightAnkleTracker.getRotation(rotBuf2); + + if (extendedKneeModel) { + calculateKneeLimits( + rotBuf1, + rotBuf2, + rightKneeTracker.getConfidenceLevel(), + rightAnkleTracker.getConfidenceLevel() + ); + } + + rightHipNode.localTransform.setRotation(rotBuf1); + rightKneeNode.localTransform.setRotation(rotBuf2); + rightAnkleNode.localTransform.setRotation(rotBuf2); + rightFootNode.localTransform.setRotation(rotBuf2); + + trackerRightKneeNode.localTransform.setRotation(rotBuf2); + trackerRightFootNode.localTransform.setRotation(rotBuf2); + + if (rightFootTracker != null) { + rightFootTracker.getRotation(rotBuf2); + rightAnkleNode.localTransform.setRotation(rotBuf2); + rightFootNode.localTransform.setRotation(rotBuf2); + trackerRightFootNode.localTransform.setRotation(rotBuf2); + } + + if (extendedPelvisModel && hasKneeTracker) { + // Average pelvis between two legs + leftHipNode.localTransform.getRotation(rotBuf1); + rightHipNode.localTransform.getRotation(rotBuf2); + rotBuf2.nlerp(rotBuf1, 0.5f); + chestNode.localTransform.getRotation(rotBuf1); + rotBuf2.nlerp(rotBuf1, FastMath.ONE_THIRD); + hipNode.localTransform.setRotation(rotBuf2); + // trackerWaistNode.localTransform.setRotation(rotBuf2); // <== + // Provides cursed + // results from my test in VRChat when sitting or laying down + // -Erimel + // TODO : Correct the trackerWaistNode without getting cursed + // results (only + // correct yaw?) + // TODO : Use vectors to add like 50% of waist tracker yaw to waist + // node to + // reduce drift and let user take weird poses + } + + // Left arm from HMD + if (leftUpperArmTracker != null) { + leftUpperArmTracker.getRotation(rotBuf1); + leftShoulderNodeHmd.localTransform.setRotation(rotBuf1); + trackerLeftElbowNodeHmd.localTransform.setRotation(rotBuf1); + leftForearmTracker.getRotation(rotBuf1); + leftElbowNodeHmd.localTransform.setRotation(rotBuf1); + } + if (leftHandTracker != null) { + leftHandTracker.getRotation(rotBuf1); + leftWristNodeHmd.localTransform.setRotation(rotBuf1); + leftHandNodeHmd.localTransform.setRotation(rotBuf1); + trackerLeftHandNodeHmd.localTransform.setRotation(rotBuf1); + } + + // Right arm from HMD + if (rightUpperArmTracker != null) { + rightUpperArmTracker.getRotation(rotBuf1); + rightShoulderNodeHmd.localTransform.setRotation(rotBuf1); + trackerRightElbowNodeHmd.localTransform.setRotation(rotBuf1); + rightForearmTracker.getRotation(rotBuf1); + rightElbowNodeHmd.localTransform.setRotation(rotBuf1); + } + if (rightHandTracker != null) { + rightHandTracker.getRotation(rotBuf1); + rightWristNodeHmd.localTransform.setRotation(rotBuf1); + rightHandNodeHmd.localTransform.setRotation(rotBuf1); + trackerRightHandNodeHmd.localTransform.setRotation(rotBuf1); + } + + // Left elbow from SteamVR controller + if (leftControllerTracker != null) { + leftControllerTracker.getPosition(posBuf); + leftControllerTracker.getRotation(rotBuf1); + leftControllerNodeContrl.localTransform.setTranslation(posBuf); + leftControllerNodeContrl.localTransform.setRotation(rotBuf1); + if (leftForearmTracker != null) { + leftForearmTracker.getRotation(rotBuf1); + leftWristNodeContrl.localTransform.setRotation(rotBuf1); + leftUpperArmTracker.getRotation(rotBuf1); + leftElbowNodeContrl.localTransform.setRotation(rotBuf1); + trackerLeftElbowNodeContrl.localTransform.setRotation(rotBuf1); + } + } + + // Right elbow from SteamVR controller + if (rightControllerTracker != null) { + rightControllerTracker.getPosition(posBuf); + rightControllerTracker.getRotation(rotBuf1); + rightControllerNodeContrl.localTransform.setTranslation(posBuf); + rightControllerNodeContrl.localTransform.setRotation(rotBuf1); + if (rightForearmTracker != null) { + rightForearmTracker.getRotation(rotBuf1); + rightWristNodeContrl.localTransform.setRotation(rotBuf1); + rightUpperArmTracker.getRotation(rotBuf1); + rightElbowNodeContrl.localTransform.setRotation(rotBuf1); + trackerRightElbowNodeContrl.localTransform.setRotation(rotBuf1); + } + } + } + + // #region Knee Model + // Knee basically has only 1 DoF (pitch), average yaw and roll between knee + // and + // hip + protected void calculateKneeLimits( + Quaternion hipBuf, + Quaternion kneeBuf, + float hipConfidence, + float kneeConfidence + ) { + ankleVector.set(0, -1, 0); + hipVector.set(0, -1, 0); + hipBuf.multLocal(hipVector); + kneeBuf.multLocal(ankleVector); + kneeRotation.angleBetweenVectors(hipVector, ankleVector); // Find knee + // angle + + // Substract knee angle from knee rotation. With perfect leg and perfect + // sensors result should match hip rotation perfectly + kneeBuf.multLocal(kneeRotation.inverse()); + + // Average knee and hip with a slerp + hipBuf.slerp(kneeBuf, 0.5f); // TODO : Use confidence to calculate + // changeAmt + kneeBuf.set(hipBuf); + + // Return knee angle into knee rotation + kneeBuf.multLocal(kneeRotation); + } + // #endregion + + // #region Update the output trackers + protected void updateComputedTrackers() { + if (computedChestTracker != null) { + computedChestTracker.position.set(trackerChestNode.worldTransform.getTranslation()); + computedChestTracker.rotation.set(neckNode.worldTransform.getRotation()); + computedChestTracker.dataTick(); + } + + if (computedWaistTracker != null) { + computedWaistTracker.position.set(trackerWaistNode.worldTransform.getTranslation()); + computedWaistTracker.rotation.set(trackerWaistNode.worldTransform.getRotation()); + computedWaistTracker.dataTick(); + } + + if (computedLeftKneeTracker != null) { + computedLeftKneeTracker.position + .set(trackerLeftKneeNode.worldTransform.getTranslation()); + computedLeftKneeTracker.rotation.set(leftHipNode.worldTransform.getRotation()); + computedLeftKneeTracker.dataTick(); + } + + if (computedLeftFootTracker != null) { + computedLeftFootTracker.position + .set(trackerLeftFootNode.worldTransform.getTranslation()); + computedLeftFootTracker.rotation.set(trackerLeftFootNode.worldTransform.getRotation()); + computedLeftFootTracker.dataTick(); + } + + if (computedRightKneeTracker != null) { + computedRightKneeTracker.position + .set(trackerRightKneeNode.worldTransform.getTranslation()); + computedRightKneeTracker.rotation.set(rightHipNode.worldTransform.getRotation()); + computedRightKneeTracker.dataTick(); + } + + if (computedRightFootTracker != null) { + computedRightFootTracker.position + .set(trackerRightFootNode.worldTransform.getTranslation()); + computedRightFootTracker.rotation + .set(trackerRightFootNode.worldTransform.getRotation()); + computedRightFootTracker.dataTick(); + } + + if (computedLeftElbowTracker != null) { + if (leftControllerTracker != null) { // From controller + computedLeftElbowTracker.position + .set(trackerLeftElbowNodeContrl.worldTransform.getTranslation()); + computedLeftElbowTracker.rotation + .set(trackerLeftElbowNodeContrl.worldTransform.getRotation()); + } else { // From shoulders + computedLeftElbowTracker.position + .set(trackerLeftElbowNodeHmd.worldTransform.getTranslation()); + computedLeftElbowTracker.rotation + .set(trackerLeftElbowNodeHmd.worldTransform.getRotation()); + } + computedLeftElbowTracker.dataTick(); + } + + if (computedRightElbowTracker != null) { + if (rightControllerTracker != null) { // From controller + computedRightElbowTracker.position + .set(trackerRightElbowNodeContrl.worldTransform.getTranslation()); + computedRightElbowTracker.rotation + .set(trackerRightElbowNodeContrl.worldTransform.getRotation()); + } else { // From shoulders + computedRightElbowTracker.position + .set(trackerRightElbowNodeHmd.worldTransform.getTranslation()); + computedRightElbowTracker.rotation + .set(trackerRightElbowNodeHmd.worldTransform.getRotation()); + } + computedRightElbowTracker.dataTick(); + } + + if (computedLeftHandTracker != null) { + computedLeftHandTracker.position + .set(trackerLeftHandNodeHmd.worldTransform.getTranslation()); + computedLeftHandTracker.rotation + .set(trackerLeftHandNodeHmd.worldTransform.getRotation()); + computedLeftHandTracker.dataTick(); + } + + if (computedRightHandTracker != null) { + computedRightHandTracker.position + .set(trackerRightHandNodeHmd.worldTransform.getTranslation()); + computedRightHandTracker.rotation + .set(trackerRightHandNodeHmd.worldTransform.getRotation()); + computedRightHandTracker.dataTick(); + } + } + // #endregion + // #endregion + + // #region Skeleton Config + @Override + public void updateConfigState(SkeletonConfigValue config, float newValue) { + // Do nothing, the node offset callback handles all that's needed + } + + @Override + public void updateToggleState(SkeletonConfigToggle configToggle, boolean newValue) { + if (configToggle == null) { + return; + } + + // Cache the values of these configs + switch (configToggle) { + case EXTENDED_PELVIS_MODEL: + extendedPelvisModel = newValue; + break; + case EXTENDED_KNEE_MODEL: + extendedKneeModel = newValue; + break; + } + } + + @Override + public void updateNodeOffset(BoneType nodeOffset, Vector3f offset) { + if (nodeOffset == null) { + return; + } + + switch (nodeOffset) { + case HEAD: + headNode.localTransform.setTranslation(offset); + break; + case NECK: + neckNode.localTransform.setTranslation(offset); + break; + case CHEST: + chestNode.localTransform.setTranslation(offset); + break; + case CHEST_TRACKER: + trackerChestNode.localTransform.setTranslation(offset); + break; + case WAIST: + waistNode.localTransform.setTranslation(offset); + break; + case HIP: + hipNode.localTransform.setTranslation(offset); + break; + case HIP_TRACKER: + trackerWaistNode.localTransform.setTranslation(offset); + break; + + case LEFT_HIP: + leftHipNode.localTransform.setTranslation(offset); + break; + case RIGHT_HIP: + rightHipNode.localTransform.setTranslation(offset); + break; + + case UPPER_LEG: + case LEFT_UPPER_LEG: + case RIGHT_UPPER_LEG: + leftKneeNode.localTransform.setTranslation(offset); + rightKneeNode.localTransform.setTranslation(offset); + break; + case KNEE_TRACKER: + case LEFT_KNEE_TRACKER: + case RIGHT_KNEE_TRACKER: + trackerLeftKneeNode.localTransform.setTranslation(offset); + trackerRightKneeNode.localTransform.setTranslation(offset); + break; + case LOWER_LEG: + case LEFT_LOWER_LEG: + case RIGHT_LOWER_LEG: + leftAnkleNode.localTransform.setTranslation(offset); + rightAnkleNode.localTransform.setTranslation(offset); + break; + case FOOT: + case LEFT_FOOT: + case RIGHT_FOOT: + leftFootNode.localTransform.setTranslation(offset); + rightFootNode.localTransform.setTranslation(offset); + break; + case FOOT_TRACKER: + case LEFT_FOOT_TRACKER: + case RIGHT_FOOT_TRACKER: + trackerLeftFootNode.localTransform.setTranslation(offset); + trackerRightFootNode.localTransform.setTranslation(offset); + break; + + case CONTROLLER: + case LEFT_CONTROLLER: + case RIGHT_CONTROLLER: + leftWristNodeContrl.localTransform.setTranslation(offset); + rightWristNodeContrl.localTransform.setTranslation(offset); + break; + case LOWER_ARM: + case LEFT_LOWER_ARM: + case RIGHT_LOWER_ARM: + leftElbowNodeContrl.localTransform.setTranslation(offset); + rightElbowNodeContrl.localTransform.setTranslation(offset); + break; + case ELBOW_TRACKER: + case LEFT_ELBOW_TRACKER: + case RIGHT_ELBOW_TRACKER: + trackerLeftElbowNodeContrl.localTransform.setTranslation(offset); + trackerRightElbowNodeContrl.localTransform.setTranslation(offset); + trackerLeftElbowNodeHmd.localTransform.setTranslation(offset); + trackerRightElbowNodeHmd.localTransform.setTranslation(offset); + break; + case LEFT_SHOULDER: + leftShoulderNodeHmd.localTransform.setTranslation(offset); + break; + case RIGHT_SHOULDER: + rightShoulderNodeHmd.localTransform.setTranslation(offset); + break; + case HAND: + case LEFT_HAND: + case RIGHT_HAND: + leftHandNodeHmd.localTransform.setTranslation(offset); + rightHandNodeHmd.localTransform.setTranslation(offset); + break; + case UPPER_ARM: + case LEFT_UPPER_ARM: + case RIGHT_UPPER_ARM: + leftElbowNodeHmd.localTransform.setTranslation(offset); + rightElbowNodeHmd.localTransform.setTranslation(offset); + break; + case LOWER_ARM_HMD: + leftWristNodeHmd.localTransform.setTranslation(offset); + rightWristNodeHmd.localTransform.setTranslation(offset); + break; + default: + break; + } + for (BoneInfo bone : currentBoneInfo) + bone.updateLength(); + } + + public void updatePoseAffectedByConfig(SkeletonConfigValue config) { + switch (config) { + case HEAD: + headNode.update(); + updateComputedTrackers(); + break; + case NECK: + neckNode.update(); + updateComputedTrackers(); + break; + case TORSO: + hipNode.update(); + updateComputedTrackers(); + break; + case CHEST: + chestNode.update(); + updateComputedTrackers(); + break; + case WAIST: + waistNode.update(); + updateComputedTrackers(); + break; + case HIP_OFFSET: + trackerWaistNode.update(); + updateComputedTrackers(); + break; + case HIPS_WIDTH: + leftHipNode.update(); + rightHipNode.update(); + updateComputedTrackers(); + break; + case KNEE_HEIGHT: + leftKneeNode.update(); + rightKneeNode.update(); + break; + case LEGS_LENGTH: + leftKneeNode.update(); + rightKneeNode.update(); + updateComputedTrackers(); + break; + case FOOT_LENGTH: + leftFootNode.update(); + rightFootNode.update(); + updateComputedTrackers(); + break; + case FOOT_OFFSET: + leftAnkleNode.update(); + rightAnkleNode.update(); + updateComputedTrackers(); + break; + case SKELETON_OFFSET: + trackerChestNode.update(); + trackerWaistNode.update(); + trackerLeftKneeNode.update(); + trackerRightKneeNode.update(); + trackerLeftFootNode.update(); + trackerRightFootNode.update(); + updateComputedTrackers(); + break; + case CONTROLLER_DISTANCE_Z: + case CONTROLLER_DISTANCE_Y: + leftWristNodeContrl.update(); + rightWristNodeContrl.update(); + updateComputedTrackers(); + break; + case FOREARM_LENGTH: + leftElbowNodeContrl.update(); + rightElbowNodeContrl.update(); + leftElbowNodeHmd.update(); + rightElbowNodeHmd.update(); + updateComputedTrackers(); + break; + case ELBOW_OFFSET: + trackerLeftElbowNodeContrl.update(); + trackerRightElbowNodeContrl.update(); + trackerLeftElbowNodeHmd.update(); + trackerRightElbowNodeHmd.update(); + updateComputedTrackers(); + break; + case SHOULDERS_DISTANCE: + case SHOULDERS_WIDTH: + leftShoulderNodeHmd.update(); + rightShoulderNodeHmd.update(); + updateComputedTrackers(); + break; + case UPPER_ARM_LENGTH: + leftElbowNodeHmd.update(); + rightElbowNodeHmd.update(); + updateComputedTrackers(); + break; + } + } + // #endregion + + @Override + public TransformNode getRootNode() { + return hmdNode; + } + + @Override + public TransformNode[] getAllNodes() { + List nodesList = new FastList<>(); + + hmdNode.depthFirstTraversal((node) -> { + nodesList.add(node); + }); + leftControllerNodeContrl.depthFirstTraversal((node) -> { + nodesList.add(node); + }); + rightControllerNodeContrl.depthFirstTraversal((node) -> { + nodesList.add(node); + }); + + return nodesList.toArray(new TransformNode[0]); + } + + @Override + public SkeletonConfig getSkeletonConfig() { + return skeletonConfig; + } + + @Override + public void resetSkeletonConfig(SkeletonConfigValue config) { + if (config == null) { + return; + } + + Vector3f vec; + float height; + switch (config) { + case HEAD: + skeletonConfig.setConfig(SkeletonConfigValue.HEAD, null); + break; + case NECK: + skeletonConfig.setConfig(SkeletonConfigValue.NECK, null); + break; + case TORSO: // Distance from shoulders to hip (full torso length) + vec = new Vector3f(); + hmdTracker.getPosition(vec); + height = vec.y; + if (height > 0.5f) { // Reset only if floor level is right, + // TODO: read floor level from SteamVR + // if + // it's not 0 + skeletonConfig + .setConfig( + SkeletonConfigValue.TORSO, + ((height) * 0.42f) - skeletonConfig.getConfig(SkeletonConfigValue.NECK) + ); + } else// if floor level is incorrect + { + skeletonConfig.setConfig(SkeletonConfigValue.TORSO, null); + } + break; + case CHEST: // Chest is 57% of the upper body by default (shoulders + // to chest) + skeletonConfig + .setConfig( + SkeletonConfigValue.CHEST, + skeletonConfig.getConfig(SkeletonConfigValue.TORSO) * 0.57f + ); + break; + case WAIST: // Waist length is from hip to waist + skeletonConfig.setConfig(SkeletonConfigValue.WAIST, null); + break; + case HIP_OFFSET: + skeletonConfig.setConfig(SkeletonConfigValue.HIP_OFFSET, null); + break; + case HIPS_WIDTH: + skeletonConfig.setConfig(SkeletonConfigValue.HIPS_WIDTH, null); + break; + case FOOT_LENGTH: + skeletonConfig.setConfig(SkeletonConfigValue.FOOT_LENGTH, null); + break; + case FOOT_OFFSET: + skeletonConfig.setConfig(SkeletonConfigValue.FOOT_OFFSET, null); + break; + case SKELETON_OFFSET: + skeletonConfig.setConfig(SkeletonConfigValue.SKELETON_OFFSET, null); + break; + case LEGS_LENGTH: // Set legs length to be 5cm above floor level + vec = new Vector3f(); + hmdTracker.getPosition(vec); + height = vec.y; + if (height > 0.5f) { // Reset only if floor level is right, + // todo: read floor level from SteamVR + // if + // it's not 0 + skeletonConfig + .setConfig( + SkeletonConfigValue.LEGS_LENGTH, + height + - skeletonConfig.getConfig(SkeletonConfigValue.NECK) + - skeletonConfig.getConfig(SkeletonConfigValue.TORSO) + - 0.05f + ); + } else // if floor level is incorrect + { + skeletonConfig.setConfig(SkeletonConfigValue.LEGS_LENGTH, null); + } + resetSkeletonConfig(SkeletonConfigValue.KNEE_HEIGHT); + break; + case KNEE_HEIGHT: // Knees are at 55% of the legs by default + skeletonConfig + .setConfig( + SkeletonConfigValue.KNEE_HEIGHT, + skeletonConfig.getConfig(SkeletonConfigValue.LEGS_LENGTH) * 0.55f + ); + break; + case CONTROLLER_DISTANCE_Z: + skeletonConfig.setConfig(SkeletonConfigValue.CONTROLLER_DISTANCE_Z, null); + break; + case CONTROLLER_DISTANCE_Y: + skeletonConfig.setConfig(SkeletonConfigValue.CONTROLLER_DISTANCE_Y, null); + break; + case FOREARM_LENGTH: + skeletonConfig.setConfig(SkeletonConfigValue.FOREARM_LENGTH, null); + break; + case ELBOW_OFFSET: + skeletonConfig.setConfig(SkeletonConfigValue.ELBOW_OFFSET, null); + break; + case SHOULDERS_DISTANCE: + skeletonConfig.setConfig(SkeletonConfigValue.SHOULDERS_DISTANCE, null); + break; + case SHOULDERS_WIDTH: + skeletonConfig.setConfig(SkeletonConfigValue.SHOULDERS_WIDTH, null); + break; + case UPPER_ARM_LENGTH: + skeletonConfig.setConfig(SkeletonConfigValue.UPPER_ARM_LENGTH, null); + break; + } + } + + protected Tracker[] getTrackersToReset() { + return new Tracker[] { trackerPreUpdate(this.neckTracker), + trackerPreUpdate(this.chestTracker), trackerPreUpdate(this.waistTracker), + trackerPreUpdate(this.hipTracker), trackerPreUpdate(this.leftKneeTracker), + trackerPreUpdate(this.leftAnkleTracker), trackerPreUpdate(this.leftFootTracker), + trackerPreUpdate(this.rightKneeTracker), trackerPreUpdate(this.rightAnkleTracker), + trackerPreUpdate(this.rightFootTracker), trackerPreUpdate(this.rightForearmTracker), + trackerPreUpdate(this.leftForearmTracker), trackerPreUpdate(this.rightUpperArmTracker), + trackerPreUpdate(this.leftUpperArmTracker), trackerPreUpdate(this.leftHandTracker), + trackerPreUpdate(this.rightHandTracker) }; + } + + @Override + public void resetTrackersFull() { + // #region Pass all trackers through trackerPreUpdate + Tracker hmdTracker = trackerPreUpdate(this.hmdTracker); + Tracker[] trackersToReset = getTrackersToReset(); + // #endregion + + // Resets all axis of the trackers with the HMD as reference. + Quaternion referenceRotation = new Quaternion(); + hmdTracker.getRotation(referenceRotation); + + for (Tracker tracker : trackersToReset) { + if (tracker != null) { + tracker.resetFull(referenceRotation); + } + } + } + + @Override + @VRServerThread + public void resetTrackersYaw() { + // #region Pass all trackers through trackerPreUpdate + Tracker hmdTracker = trackerPreUpdate(this.hmdTracker); + Tracker[] trackersToReset = getTrackersToReset(); + // #endregion + + // Resets the yaw of the trackers with the HMD as reference. + Quaternion referenceRotation = new Quaternion(); + hmdTracker.getRotation(referenceRotation); + + for (Tracker tracker : trackersToReset) { + if (tracker != null) { + tracker.resetYaw(referenceRotation); + } + } + } } diff --git a/src/main/java/dev/slimevr/vr/processor/skeleton/SimpleSkeleton.java b/src/main/java/dev/slimevr/vr/processor/skeleton/SimpleSkeleton.java deleted file mode 100644 index 8c5fe4ca9..000000000 --- a/src/main/java/dev/slimevr/vr/processor/skeleton/SimpleSkeleton.java +++ /dev/null @@ -1,1360 +0,0 @@ -package dev.slimevr.vr.processor.skeleton; - -import com.jme3.math.FastMath; -import com.jme3.math.Quaternion; -import com.jme3.math.Vector3f; -import dev.slimevr.VRServer; -import dev.slimevr.util.ann.VRServerThread; -import dev.slimevr.vr.processor.ComputedHumanPoseTracker; -import dev.slimevr.vr.processor.ComputedHumanPoseTrackerPosition; -import dev.slimevr.vr.processor.TransformNode; -import dev.slimevr.vr.trackers.*; -import io.eiren.util.collections.FastList; - -import java.util.List; -import java.util.Map; - - -public class SimpleSkeleton extends HumanSkeleton implements SkeletonConfigCallback { - - public final SkeletonConfig skeletonConfig; - // #region Upper body nodes (torso) - protected final TransformNode hmdNode = new TransformNode("HMD", false); - protected final TransformNode headNode = new TransformNode("Head", false); - protected final TransformNode neckNode = new TransformNode("Neck", false); - protected final TransformNode chestNode = new TransformNode("Chest", false); - protected final TransformNode trackerChestNode = new TransformNode("Chest-Tracker", false); - protected final TransformNode waistNode = new TransformNode("Waist", false); - protected final TransformNode hipNode = new TransformNode("Hip", false); - // #endregion - protected final TransformNode trackerWaistNode = new TransformNode("Waist-Tracker", false); - // #region Lower body nodes (legs) - protected final TransformNode leftHipNode = new TransformNode("Left-Hip", false); - protected final TransformNode leftKneeNode = new TransformNode("Left-Knee", false); - protected final TransformNode trackerLeftKneeNode = new TransformNode( - "Left-Knee-Tracker", - false - ); - protected final TransformNode leftAnkleNode = new TransformNode("Left-Ankle", false); - protected final TransformNode leftFootNode = new TransformNode("Left-Foot", false); - protected final TransformNode trackerLeftFootNode = new TransformNode( - "Left-Foot-Tracker", - false - ); - protected final TransformNode rightHipNode = new TransformNode("Right-Hip", false); - protected final TransformNode rightKneeNode = new TransformNode("Right-Knee", false); - protected final TransformNode trackerRightKneeNode = new TransformNode( - "Right-Knee-Tracker", - false - ); - protected final TransformNode rightAnkleNode = new TransformNode("Right-Ankle", false); - protected final TransformNode rightFootNode = new TransformNode("Right-Foot", false); - protected final TransformNode trackerRightFootNode = new TransformNode( - "Right-Foot-Tracker", - false - ); - // #region Arms (from controllers) - protected final TransformNode leftControllerNodeContrl = new TransformNode( - "Left-Controller-Contrl", - false - ); - protected final TransformNode rightControllerNodeContrl = new TransformNode( - "Right-Controller-Contrl", - false - ); - // #endregion - protected final TransformNode leftWristNodeContrl = new TransformNode( - "Left-Wrist-Contrl", - false - ); - protected final TransformNode rightWristNodeContrl = new TransformNode( - "Right-Wrist-Contrl", - false - ); - protected final TransformNode leftElbowNodeContrl = new TransformNode( - "Left-Elbow-Contrl", - false - ); - protected final TransformNode rightElbowNodeContrl = new TransformNode( - "Right-Elbow-Contrl", - false - ); - protected final TransformNode trackerLeftElbowNodeContrl = new TransformNode( - "Left-Elbow-Tracker-Contrl", - false - ); - protected final TransformNode trackerRightElbowNodeContrl = new TransformNode( - "Right-Elbow-Tracker-Contrl", - false - ); - // #region Arms (from HMD) - protected final TransformNode leftShoulderNodeHmd = new TransformNode( - "Left-Shoulder-Hmd", - false - ); - protected final TransformNode rightShoulderNodeHmd = new TransformNode( - "Right-Shoulder-Hmd", - false - ); - // #endregion - protected final TransformNode leftElbowNodeHmd = new TransformNode("Left-Elbow-Hmd", false); - protected final TransformNode rightElbowNodeHmd = new TransformNode("Right-Elbow-Hmd", false); - protected final TransformNode trackerLeftElbowNodeHmd = new TransformNode( - "Left-Elbow-Tracker-Hmd", - false - ); - protected final TransformNode trackerRightElbowNodeHmd = new TransformNode( - "Right-Elbow-Tracker-Hmd", - false - ); - protected final TransformNode leftWristNodeHmd = new TransformNode("Left-Wrist-Hmd", false); - protected final TransformNode rightWristNodeHmd = new TransformNode("Right-Wrist-Hmd", false); - protected final TransformNode leftHandNodeHmd = new TransformNode("Left-Hand-Hmd", false); - protected final TransformNode rightHandNodeHmd = new TransformNode("Right-Hand-Hmd", false); - protected final TransformNode trackerLeftHandNodeHmd = new TransformNode( - "Left-Hand-Tracker-Hmd", - false - ); - protected final TransformNode trackerRightHandNodeHmd = new TransformNode( - "Right-Hand-Tracker-Hmd", - false - ); - protected final Vector3f hipVector = new Vector3f(); - protected final Vector3f ankleVector = new Vector3f(); - // #endregion - protected final Quaternion kneeRotation = new Quaternion(); - // #region Buffers - private final Vector3f posBuf = new Vector3f(); - private final Quaternion rotBuf2 = new Quaternion(); - protected float minKneePitch = 0f * FastMath.DEG_TO_RAD; - protected float maxKneePitch = 90f * FastMath.DEG_TO_RAD; - protected float kneeLerpFactor = 0.5f; - // #region Tracker Input - protected Tracker hmdTracker; - protected Tracker neckTracker; - protected Tracker chestTracker; - protected Tracker waistTracker; - protected Tracker hipTracker; - protected Tracker leftKneeTracker; - protected Tracker leftAnkleTracker; - protected Tracker leftFootTracker; - protected Tracker rightKneeTracker; - protected Tracker rightAnkleTracker; - protected Tracker rightFootTracker; - protected Tracker leftControllerTracker; - protected Tracker rightControllerTracker; - protected Tracker leftForearmTracker; - protected Tracker rightForearmTracker; - // #endregion - protected Tracker leftUpperArmTracker; - protected Tracker rightUpperArmTracker; - protected Tracker leftHandTracker; - protected Tracker rightHandTracker; - // #region Tracker Output - protected ComputedHumanPoseTracker computedChestTracker; - protected ComputedHumanPoseTracker computedWaistTracker; - protected ComputedHumanPoseTracker computedLeftKneeTracker; - protected ComputedHumanPoseTracker computedLeftFootTracker; - protected ComputedHumanPoseTracker computedRightKneeTracker; - protected ComputedHumanPoseTracker computedRightFootTracker; - // #endregion - protected ComputedHumanPoseTracker computedLeftElbowTracker; - protected ComputedHumanPoseTracker computedRightElbowTracker; - protected ComputedHumanPoseTracker computedLeftHandTracker; - protected ComputedHumanPoseTracker computedRightHandTracker; - protected boolean extendedPelvisModel = true; - protected boolean extendedKneeModel = false; - private Quaternion rotBuf1 = new Quaternion(); - private boolean hasSpineTracker, hasKneeTracker; - // #endregion - - // #region Constructors - protected SimpleSkeleton(List computedTrackers) { - // #region Assemble skeleton from hmd to hip - hmdNode.attachChild(headNode); - headNode.attachChild(neckNode); - neckNode.attachChild(chestNode); - chestNode.attachChild(waistNode); - waistNode.attachChild(hipNode); - // #endregion - - // #region Assemble skeleton from hips to feet - hipNode.attachChild(leftHipNode); - hipNode.attachChild(rightHipNode); - - leftHipNode.attachChild(leftKneeNode); - rightHipNode.attachChild(rightKneeNode); - - leftKneeNode.attachChild(leftAnkleNode); - rightKneeNode.attachChild(rightAnkleNode); - - leftAnkleNode.attachChild(leftFootNode); - rightAnkleNode.attachChild(rightFootNode); - // #endregion - - // #region Assemble skeleton arms from controllers - leftControllerNodeContrl.attachChild(leftWristNodeContrl); - rightControllerNodeContrl.attachChild(rightWristNodeContrl); - leftWristNodeContrl.attachChild(leftElbowNodeContrl); - rightWristNodeContrl.attachChild(rightElbowNodeContrl); - // #endregion - - // #region Assemble skeleton arms from chest - chestNode.attachChild(leftShoulderNodeHmd); - chestNode.attachChild(rightShoulderNodeHmd); - - leftShoulderNodeHmd.attachChild(leftElbowNodeHmd); - rightShoulderNodeHmd.attachChild(rightElbowNodeHmd); - - leftElbowNodeHmd.attachChild(leftWristNodeHmd); - rightElbowNodeHmd.attachChild(rightWristNodeHmd); - - leftWristNodeHmd.attachChild(leftHandNodeHmd); - rightWristNodeHmd.attachChild(rightHandNodeHmd); - // #endregion - - // #region Attach tracker nodes for offsets - chestNode.attachChild(trackerChestNode); - hipNode.attachChild(trackerWaistNode); - - leftKneeNode.attachChild(trackerLeftKneeNode); - rightKneeNode.attachChild(trackerRightKneeNode); - - leftFootNode.attachChild(trackerLeftFootNode); - rightFootNode.attachChild(trackerRightFootNode); - - leftElbowNodeContrl.attachChild(trackerLeftElbowNodeContrl); - rightElbowNodeContrl.attachChild(trackerRightElbowNodeContrl); - - leftElbowNodeHmd.attachChild(trackerLeftElbowNodeHmd); - rightElbowNodeHmd.attachChild(trackerRightElbowNodeHmd); - - leftHandNodeHmd.attachChild(trackerLeftHandNodeHmd); - rightHandNodeHmd.attachChild(trackerRightHandNodeHmd); - // #endregion - - // Set default skeleton configuration (callback automatically sets - // initial - // offsets) - skeletonConfig = new SkeletonConfig(true, this); - - if (computedTrackers != null) { - setComputedTrackers(computedTrackers); - } - fillNullComputedTrackers(true); - } - - public SimpleSkeleton( - VRServer server, - List computedTrackers - ) { - this(computedTrackers); - setTrackersFromServer(server); - skeletonConfig.loadFromConfig(server.config); - } - - public SimpleSkeleton( - List trackers, - List computedTrackers - ) { - this(computedTrackers); - - if (trackers != null) { - setTrackersFromList(trackers); - } else { - setTrackersFromList(new FastList(0)); - } - } - - public SimpleSkeleton( - List trackers, - List computedTrackers, - Map configs, - Map altConfigs - ) { - // Initialize - this(trackers, computedTrackers); - - // Set configs - if (altConfigs != null) { - // Set alts first, so if there's any overlap it doesn't affect the - // values - skeletonConfig.setConfigs(altConfigs, null); - } - skeletonConfig.setConfigs(configs, null); - } - - public SimpleSkeleton( - List trackers, - List computedTrackers, - Map configs - ) { - this(trackers, computedTrackers, configs, null); - } - // #endregion - - public static float normalizeRad(float angle) { - return FastMath.normalize(angle, -FastMath.PI, FastMath.PI); - } - - public static float interpolateRadians(float factor, float start, float end) { - float angle = FastMath.abs(end - start); - if (angle > FastMath.PI) { - if (end > start) { - start += FastMath.TWO_PI; - } else { - end += FastMath.TWO_PI; - } - } - float val = start + (end - start) * factor; - return normalizeRad(val); - } - - // #region Set Trackers - public void setTrackersFromList(List trackers, boolean setHmd) { - if (setHmd) { - this.hmdTracker = TrackerUtils - .findTrackerForBodyPosition(trackers, TrackerPosition.HMD); - } - - this.neckTracker = TrackerUtils - .findTrackerForBodyPositionOrEmpty( - trackers, - TrackerPosition.NECK, - TrackerPosition.HMD, - null - ); - this.chestTracker = TrackerUtils - .findTrackerForBodyPositionOrEmpty( - trackers, - TrackerPosition.CHEST, - TrackerPosition.WAIST, - TrackerPosition.HIP - ); - this.waistTracker = TrackerUtils - .findTrackerForBodyPositionOrEmpty( - trackers, - TrackerPosition.WAIST, - TrackerPosition.HIP, - TrackerPosition.CHEST - ); - this.hipTracker = TrackerUtils - .findTrackerForBodyPositionOrEmpty( - trackers, - TrackerPosition.HIP, - TrackerPosition.WAIST, - TrackerPosition.CHEST - ); - - this.leftKneeTracker = TrackerUtils - .findTrackerForBodyPositionOrEmpty( - trackers, - TrackerPosition.LEFT_KNEE, - TrackerPosition.LEFT_ANKLE, - null - ); - this.leftAnkleTracker = TrackerUtils - .findTrackerForBodyPositionOrEmpty( - trackers, - TrackerPosition.LEFT_ANKLE, - TrackerPosition.LEFT_KNEE, - null - ); - this.leftFootTracker = TrackerUtils - .findTrackerForBodyPosition(trackers, TrackerPosition.LEFT_FOOT); - - this.rightKneeTracker = TrackerUtils - .findTrackerForBodyPositionOrEmpty( - trackers, - TrackerPosition.RIGHT_KNEE, - TrackerPosition.RIGHT_ANKLE, - null - ); - this.rightAnkleTracker = TrackerUtils - .findTrackerForBodyPositionOrEmpty( - trackers, - TrackerPosition.RIGHT_ANKLE, - TrackerPosition.RIGHT_KNEE, - null - ); - this.rightFootTracker = TrackerUtils - .findTrackerForBodyPosition(trackers, TrackerPosition.RIGHT_FOOT); - - this.leftControllerTracker = TrackerUtils - .findTrackerForBodyPosition(trackers, TrackerPosition.LEFT_CONTROLLER); - this.rightControllerTracker = TrackerUtils - .findTrackerForBodyPosition(trackers, TrackerPosition.RIGHT_CONTROLLER); - this.leftForearmTracker = TrackerUtils - .findTrackerForBodyPositionOrEmpty( - trackers, - TrackerPosition.LEFT_FOREARM, - TrackerPosition.LEFT_UPPER_ARM, - null - ); - this.rightForearmTracker = TrackerUtils - .findTrackerForBodyPositionOrEmpty( - trackers, - TrackerPosition.RIGHT_FOREARM, - TrackerPosition.RIGHT_UPPER_ARM, - null - ); - this.leftUpperArmTracker = TrackerUtils - .findTrackerForBodyPositionOrEmpty( - trackers, - TrackerPosition.LEFT_UPPER_ARM, - TrackerPosition.LEFT_FOREARM, - null - ); - this.rightUpperArmTracker = TrackerUtils - .findTrackerForBodyPositionOrEmpty( - trackers, - TrackerPosition.RIGHT_UPPER_ARM, - TrackerPosition.RIGHT_FOREARM, - null - ); - this.leftHandTracker = TrackerUtils - .findTrackerForBodyPosition(trackers, TrackerPosition.LEFT_HAND); - this.rightHandTracker = TrackerUtils - .findTrackerForBodyPosition(trackers, TrackerPosition.RIGHT_HAND); - } - - public void setTrackersFromList(List trackers) { - setTrackersFromList(trackers, true); - } - - public void setTrackersFromServer(VRServer server) { - this.hmdTracker = server.hmdTracker; - setTrackersFromList(server.getAllTrackers(), false); - } - - public void setComputedTracker(ComputedHumanPoseTracker tracker) { - switch (tracker.getTrackerRole()) { - case CHEST: - computedChestTracker = tracker; - break; - case WAIST: - computedWaistTracker = tracker; - break; - - case LEFT_KNEE: - computedLeftKneeTracker = tracker; - break; - case LEFT_FOOT: - computedLeftFootTracker = tracker; - break; - - case RIGHT_KNEE: - computedRightKneeTracker = tracker; - break; - case RIGHT_FOOT: - computedRightFootTracker = tracker; - break; - - case LEFT_ELBOW: - computedLeftElbowTracker = tracker; - break; - case RIGHT_ELBOW: - computedRightElbowTracker = tracker; - break; - - case LEFT_HAND: - computedLeftHandTracker = tracker; - break; - case RIGHT_HAND: - computedRightHandTracker = tracker; - break; - } - } - - public void setComputedTrackers(List trackers) { - for (ComputedHumanPoseTracker t : trackers) { - setComputedTracker(t); - } - } - // #endregion - - public void setComputedTrackersAndFillNull( - List trackers, - boolean onlyFillWaistAndFeet - ) { - setComputedTrackers(trackers); - fillNullComputedTrackers(onlyFillWaistAndFeet); - } - // #endregion - - public void fillNullComputedTrackers(boolean onlyFillWaistAndFeet) { - if (computedWaistTracker == null) { - computedWaistTracker = new ComputedHumanPoseTracker( - Tracker.getNextLocalTrackerId(), - ComputedHumanPoseTrackerPosition.WAIST, - TrackerRole.WAIST - ); - computedWaistTracker.setStatus(TrackerStatus.OK); - } - - if (computedLeftFootTracker == null) { - computedLeftFootTracker = new ComputedHumanPoseTracker( - Tracker.getNextLocalTrackerId(), - ComputedHumanPoseTrackerPosition.LEFT_FOOT, - TrackerRole.LEFT_FOOT - ); - computedLeftFootTracker.setStatus(TrackerStatus.OK); - } - - if (computedRightFootTracker == null) { - computedRightFootTracker = new ComputedHumanPoseTracker( - Tracker.getNextLocalTrackerId(), - ComputedHumanPoseTrackerPosition.RIGHT_FOOT, - TrackerRole.RIGHT_FOOT - ); - computedRightFootTracker.setStatus(TrackerStatus.OK); - } - - if (!onlyFillWaistAndFeet) { - if (computedChestTracker == null) { - computedChestTracker = new ComputedHumanPoseTracker( - Tracker.getNextLocalTrackerId(), - ComputedHumanPoseTrackerPosition.CHEST, - TrackerRole.CHEST - ); - computedChestTracker.setStatus(TrackerStatus.OK); - } - - if (computedLeftKneeTracker == null) { - computedLeftKneeTracker = new ComputedHumanPoseTracker( - Tracker.getNextLocalTrackerId(), - ComputedHumanPoseTrackerPosition.LEFT_KNEE, - TrackerRole.LEFT_KNEE - ); - computedLeftKneeTracker.setStatus(TrackerStatus.OK); - } - - if (computedRightKneeTracker == null) { - computedRightKneeTracker = new ComputedHumanPoseTracker( - Tracker.getNextLocalTrackerId(), - ComputedHumanPoseTrackerPosition.RIGHT_KNEE, - TrackerRole.RIGHT_KNEE - ); - computedRightKneeTracker.setStatus(TrackerStatus.OK); - } - - if (computedLeftElbowTracker == null) { - computedLeftElbowTracker = new ComputedHumanPoseTracker( - Tracker.getNextLocalTrackerId(), - ComputedHumanPoseTrackerPosition.LEFT_ELBOW, - TrackerRole.LEFT_ELBOW - ); - computedLeftElbowTracker.setStatus(TrackerStatus.OK); - } - if (computedRightElbowTracker == null) { - computedRightElbowTracker = new ComputedHumanPoseTracker( - Tracker.getNextLocalTrackerId(), - ComputedHumanPoseTrackerPosition.RIGHT_ELBOW, - TrackerRole.RIGHT_ELBOW - ); - computedRightElbowTracker.setStatus(TrackerStatus.OK); - } - - if (computedLeftHandTracker == null) { - computedLeftHandTracker = new ComputedHumanPoseTracker( - Tracker.getNextLocalTrackerId(), - ComputedHumanPoseTrackerPosition.LEFT_HAND, - TrackerRole.LEFT_HAND - ); - computedLeftHandTracker.setStatus(TrackerStatus.OK); - } - if (computedRightHandTracker == null) { - computedRightHandTracker = new ComputedHumanPoseTracker( - Tracker.getNextLocalTrackerId(), - ComputedHumanPoseTrackerPosition.RIGHT_HAND, - TrackerRole.RIGHT_HAND - ); - computedRightHandTracker.setStatus(TrackerStatus.OK); - } - } - } - - // #region Get Trackers - public ComputedHumanPoseTracker getComputedTracker(TrackerRole trackerRole) { - switch (trackerRole) { - case CHEST: - return computedChestTracker; - case WAIST: - return computedWaistTracker; - - case LEFT_KNEE: - return computedLeftKneeTracker; - case LEFT_FOOT: - return computedLeftFootTracker; - - case RIGHT_KNEE: - return computedRightKneeTracker; - case RIGHT_FOOT: - return computedRightFootTracker; - - case LEFT_ELBOW: - return computedLeftElbowTracker; - case RIGHT_ELBOW: - return computedRightElbowTracker; - - case LEFT_HAND: - return computedLeftHandTracker; - case RIGHT_HAND: - return computedRightHandTracker; - } - - return null; - } - - // #region Processing - // Useful for sub-classes that need to return a sub-tracker (like - // PoseFrameTracker -> TrackerFrame) - protected Tracker trackerPreUpdate(Tracker tracker) { - return tracker; - } - - // Updates the pose from tracker positions - @VRServerThread - @Override - public void updatePose() { - updateLocalTransforms(); - updateRootTrackers(); - updateComputedTrackers(); - } - // #endregion - - void updateRootTrackers() { - hmdNode.update(); - leftControllerNodeContrl.update(); - rightControllerNodeContrl.update(); - } - - // #region Update the node transforms from the trackers - protected void updateLocalTransforms() { - // #region Pass all trackers through trackerPreUpdate - Tracker hmdTracker = trackerPreUpdate(this.hmdTracker); - - Tracker neckTracker = trackerPreUpdate(this.neckTracker); - Tracker chestTracker = trackerPreUpdate(this.chestTracker); - Tracker waistTracker = trackerPreUpdate(this.waistTracker); - Tracker hipTracker = trackerPreUpdate(this.hipTracker); - - Tracker leftKneeTracker = trackerPreUpdate(this.leftKneeTracker); - Tracker leftAnkleTracker = trackerPreUpdate(this.leftAnkleTracker); - Tracker leftFootTracker = trackerPreUpdate(this.leftFootTracker); - - Tracker rightKneeTracker = trackerPreUpdate(this.rightKneeTracker); - Tracker rightAnkleTracker = trackerPreUpdate(this.rightAnkleTracker); - Tracker rightFootTracker = trackerPreUpdate(this.rightFootTracker); - - Tracker leftControllerTracker = trackerPreUpdate(this.leftControllerTracker); - Tracker rightControllerTracker = trackerPreUpdate(this.rightControllerTracker); - Tracker rightForearmTracker = trackerPreUpdate(this.rightForearmTracker); - Tracker leftForearmTracker = trackerPreUpdate(this.leftForearmTracker); - Tracker rightUpperArmTracker = trackerPreUpdate(this.rightUpperArmTracker); - Tracker leftUpperArmTracker = trackerPreUpdate(this.leftUpperArmTracker); - Tracker leftHandTracker = trackerPreUpdate(this.leftHandTracker); - Tracker rightHandTracker = trackerPreUpdate(this.rightHandTracker); - // #endregion - - hasSpineTracker = chestTracker.hasRotation() - || waistTracker.hasRotation() - || hipTracker.hasRotation(); - hasKneeTracker = leftKneeTracker.hasRotation() || rightKneeTracker.hasRotation(); - - if (hmdTracker != null) { - if (hmdTracker.getPosition(posBuf)) { - hmdNode.localTransform.setTranslation(posBuf); - } - if (hmdTracker.getRotation(rotBuf1)) { - hmdNode.localTransform.setRotation(rotBuf1); - neckTracker.getRotation(rotBuf1); - headNode.localTransform.setRotation(rotBuf1); - } - } else { - // Set to zero - hmdNode.localTransform.setTranslation(Vector3f.ZERO); - hmdNode.localTransform.setRotation(Quaternion.IDENTITY); - headNode.localTransform.setRotation(Quaternion.IDENTITY); - } - - // Spine - if (hasSpineTracker) { - if (chestTracker.getRotation(rotBuf1)) { - neckNode.localTransform.setRotation(rotBuf1); - } - if (waistTracker.getRotation(rotBuf1)) { - chestNode.localTransform.setRotation(rotBuf1); - trackerChestNode.localTransform.setRotation(rotBuf1); - } - if (hipTracker.getRotation(rotBuf1)) { - waistNode.localTransform.setRotation(rotBuf1); - hipNode.localTransform.setRotation(rotBuf1); - trackerWaistNode.localTransform.setRotation(rotBuf1); - } - } else if (hmdTracker != null) { // If no spine tracker, allign spine - // yaw with HMD - rotBuf1 = rotBuf1.fromAngles(0, rotBuf1.getYaw(), 0); - neckNode.localTransform.setRotation(rotBuf1); - chestNode.localTransform.setRotation(rotBuf1); - trackerChestNode.localTransform.setRotation(rotBuf1); - waistNode.localTransform.setRotation(rotBuf1); - hipNode.localTransform.setRotation(rotBuf1); - trackerWaistNode.localTransform.setRotation(rotBuf1); - } - - // Left Leg - leftKneeTracker.getRotation(rotBuf1); - leftAnkleTracker.getRotation(rotBuf2); - - if (extendedKneeModel) - calculateKneeLimits( - rotBuf1, - rotBuf2, - leftKneeTracker.getConfidenceLevel(), - leftAnkleTracker.getConfidenceLevel() - ); - - leftHipNode.localTransform.setRotation(rotBuf1); - leftKneeNode.localTransform.setRotation(rotBuf2); - leftAnkleNode.localTransform.setRotation(rotBuf2); - leftFootNode.localTransform.setRotation(rotBuf2); - - trackerLeftKneeNode.localTransform.setRotation(rotBuf2); - trackerLeftFootNode.localTransform.setRotation(rotBuf2); - - if (leftFootTracker != null) { - leftFootTracker.getRotation(rotBuf2); - leftAnkleNode.localTransform.setRotation(rotBuf2); - leftFootNode.localTransform.setRotation(rotBuf2); - trackerLeftFootNode.localTransform.setRotation(rotBuf2); - } - - // Right Leg - rightKneeTracker.getRotation(rotBuf1); - rightAnkleTracker.getRotation(rotBuf2); - - if (extendedKneeModel) - calculateKneeLimits( - rotBuf1, - rotBuf2, - rightKneeTracker.getConfidenceLevel(), - rightAnkleTracker.getConfidenceLevel() - ); - - rightHipNode.localTransform.setRotation(rotBuf1); - rightKneeNode.localTransform.setRotation(rotBuf2); - rightAnkleNode.localTransform.setRotation(rotBuf2); - rightFootNode.localTransform.setRotation(rotBuf2); - - trackerRightKneeNode.localTransform.setRotation(rotBuf2); - trackerRightFootNode.localTransform.setRotation(rotBuf2); - - if (rightFootTracker != null) { - rightFootTracker.getRotation(rotBuf2); - rightAnkleNode.localTransform.setRotation(rotBuf2); - rightFootNode.localTransform.setRotation(rotBuf2); - trackerRightFootNode.localTransform.setRotation(rotBuf2); - } - - if (extendedPelvisModel && hasKneeTracker) { - // Average pelvis between two legs - leftHipNode.localTransform.getRotation(rotBuf1); - rightHipNode.localTransform.getRotation(rotBuf2); - rotBuf2.nlerp(rotBuf1, 0.5f); - chestNode.localTransform.getRotation(rotBuf1); - rotBuf2.nlerp(rotBuf1, FastMath.ONE_THIRD); - hipNode.localTransform.setRotation(rotBuf2); - // trackerWaistNode.localTransform.setRotation(rotBuf2); // <== - // Provides cursed - // results from my test in VRChat when sitting or laying down - // -Erimel - // TODO : Correct the trackerWaistNode without getting cursed - // results (only - // correct yaw?) - // TODO : Use vectors to add like 50% of waist tracker yaw to waist - // node to - // reduce drift and let user take weird poses - } - - // Left arm from HMD - if (leftUpperArmTracker != null) { - leftUpperArmTracker.getRotation(rotBuf1); - leftShoulderNodeHmd.localTransform.setRotation(rotBuf1); - trackerLeftElbowNodeHmd.localTransform.setRotation(rotBuf1); - leftForearmTracker.getRotation(rotBuf1); - leftElbowNodeHmd.localTransform.setRotation(rotBuf1); - } - if (leftHandTracker != null) { - leftHandTracker.getRotation(rotBuf1); - leftWristNodeHmd.localTransform.setRotation(rotBuf1); - leftHandNodeHmd.localTransform.setRotation(rotBuf1); - trackerLeftHandNodeHmd.localTransform.setRotation(rotBuf1); - } - - // Right arm from HMD - if (rightUpperArmTracker != null) { - rightUpperArmTracker.getRotation(rotBuf1); - rightShoulderNodeHmd.localTransform.setRotation(rotBuf1); - trackerRightElbowNodeHmd.localTransform.setRotation(rotBuf1); - rightForearmTracker.getRotation(rotBuf1); - rightElbowNodeHmd.localTransform.setRotation(rotBuf1); - } - if (rightHandTracker != null) { - rightHandTracker.getRotation(rotBuf1); - rightWristNodeHmd.localTransform.setRotation(rotBuf1); - rightHandNodeHmd.localTransform.setRotation(rotBuf1); - trackerRightHandNodeHmd.localTransform.setRotation(rotBuf1); - } - - // Left elbow from SteamVR controller - if (leftControllerTracker != null) { - leftControllerTracker.getPosition(posBuf); - leftControllerTracker.getRotation(rotBuf1); - leftControllerNodeContrl.localTransform.setTranslation(posBuf); - leftControllerNodeContrl.localTransform.setRotation(rotBuf1); - if (leftForearmTracker != null) { - leftForearmTracker.getRotation(rotBuf1); - leftWristNodeContrl.localTransform.setRotation(rotBuf1); - leftUpperArmTracker.getRotation(rotBuf1); - leftElbowNodeContrl.localTransform.setRotation(rotBuf1); - trackerLeftElbowNodeContrl.localTransform.setRotation(rotBuf1); - } - } - - // Right elbow from SteamVR controller - if (rightControllerTracker != null) { - rightControllerTracker.getPosition(posBuf); - rightControllerTracker.getRotation(rotBuf1); - rightControllerNodeContrl.localTransform.setTranslation(posBuf); - rightControllerNodeContrl.localTransform.setRotation(rotBuf1); - if (rightForearmTracker != null) { - rightForearmTracker.getRotation(rotBuf1); - rightWristNodeContrl.localTransform.setRotation(rotBuf1); - rightUpperArmTracker.getRotation(rotBuf1); - rightElbowNodeContrl.localTransform.setRotation(rotBuf1); - trackerRightElbowNodeContrl.localTransform.setRotation(rotBuf1); - } - } - } - - // #region Knee Model - // Knee basically has only 1 DoF (pitch), average yaw and roll between knee - // and - // hip - protected void calculateKneeLimits( - Quaternion hipBuf, - Quaternion kneeBuf, - float hipConfidence, - float kneeConfidence - ) { - ankleVector.set(0, -1, 0); - hipVector.set(0, -1, 0); - hipBuf.multLocal(hipVector); - kneeBuf.multLocal(ankleVector); - kneeRotation.angleBetweenVectors(hipVector, ankleVector); // Find knee - // angle - - // Substract knee angle from knee rotation. With perfect leg and perfect - // sensors result should match hip rotation perfectly - kneeBuf.multLocal(kneeRotation.inverse()); - - // Average knee and hip with a slerp - hipBuf.slerp(kneeBuf, 0.5f); // TODO : Use confidence to calculate - // changeAmt - kneeBuf.set(hipBuf); - - // Return knee angle into knee rotation - kneeBuf.multLocal(kneeRotation); - } - // #endregion - - // #region Update the output trackers - protected void updateComputedTrackers() { - if (computedChestTracker != null) { - computedChestTracker.position.set(trackerChestNode.worldTransform.getTranslation()); - computedChestTracker.rotation.set(neckNode.worldTransform.getRotation()); - computedChestTracker.dataTick(); - } - - if (computedWaistTracker != null) { - computedWaistTracker.position.set(trackerWaistNode.worldTransform.getTranslation()); - computedWaistTracker.rotation.set(trackerWaistNode.worldTransform.getRotation()); - computedWaistTracker.dataTick(); - } - - if (computedLeftKneeTracker != null) { - computedLeftKneeTracker.position - .set(trackerLeftKneeNode.worldTransform.getTranslation()); - computedLeftKneeTracker.rotation.set(leftHipNode.worldTransform.getRotation()); - computedLeftKneeTracker.dataTick(); - } - - if (computedLeftFootTracker != null) { - computedLeftFootTracker.position - .set(trackerLeftFootNode.worldTransform.getTranslation()); - computedLeftFootTracker.rotation.set(trackerLeftFootNode.worldTransform.getRotation()); - computedLeftFootTracker.dataTick(); - } - - if (computedRightKneeTracker != null) { - computedRightKneeTracker.position - .set(trackerRightKneeNode.worldTransform.getTranslation()); - computedRightKneeTracker.rotation.set(rightHipNode.worldTransform.getRotation()); - computedRightKneeTracker.dataTick(); - } - - if (computedRightFootTracker != null) { - computedRightFootTracker.position - .set(trackerRightFootNode.worldTransform.getTranslation()); - computedRightFootTracker.rotation - .set(trackerRightFootNode.worldTransform.getRotation()); - computedRightFootTracker.dataTick(); - } - - if (computedLeftElbowTracker != null) { - if (leftControllerTracker != null) { // From controller - computedLeftElbowTracker.position - .set(trackerLeftElbowNodeContrl.worldTransform.getTranslation()); - computedLeftElbowTracker.rotation - .set(trackerLeftElbowNodeContrl.worldTransform.getRotation()); - } else { // From shoulders - computedLeftElbowTracker.position - .set(trackerLeftElbowNodeHmd.worldTransform.getTranslation()); - computedLeftElbowTracker.rotation - .set(trackerLeftElbowNodeHmd.worldTransform.getRotation()); - } - computedLeftElbowTracker.dataTick(); - } - - if (computedRightElbowTracker != null) { - if (rightControllerTracker != null) { // From controller - computedRightElbowTracker.position - .set(trackerRightElbowNodeContrl.worldTransform.getTranslation()); - computedRightElbowTracker.rotation - .set(trackerRightElbowNodeContrl.worldTransform.getRotation()); - } else { // From shoulders - computedRightElbowTracker.position - .set(trackerRightElbowNodeHmd.worldTransform.getTranslation()); - computedRightElbowTracker.rotation - .set(trackerRightElbowNodeHmd.worldTransform.getRotation()); - } - computedRightElbowTracker.dataTick(); - } - - if (computedLeftHandTracker != null) { - computedLeftHandTracker.position - .set(trackerLeftHandNodeHmd.worldTransform.getTranslation()); - computedLeftHandTracker.rotation - .set(trackerLeftHandNodeHmd.worldTransform.getRotation()); - computedLeftHandTracker.dataTick(); - } - - if (computedRightHandTracker != null) { - computedRightHandTracker.position - .set(trackerRightHandNodeHmd.worldTransform.getTranslation()); - computedRightHandTracker.rotation - .set(trackerRightHandNodeHmd.worldTransform.getRotation()); - computedRightHandTracker.dataTick(); - } - } - // #endregion - // #endregion - - // #region Skeleton Config - @Override - public void updateConfigState(SkeletonConfigValue config, float newValue) { - // Do nothing, the node offset callback handles all that's needed - } - - @Override - public void updateToggleState(SkeletonConfigToggle configToggle, boolean newValue) { - if (configToggle == null) { - return; - } - - // Cache the values of these configs - switch (configToggle) { - case EXTENDED_PELVIS_MODEL: - extendedPelvisModel = newValue; - break; - case EXTENDED_KNEE_MODEL: - extendedKneeModel = newValue; - break; - } - } - - @Override - public void updateNodeOffset(SkeletonNodeOffset nodeOffset, Vector3f offset) { - if (nodeOffset == null) { - return; - } - - switch (nodeOffset) { - case HEAD: - headNode.localTransform.setTranslation(offset); - break; - case NECK: - neckNode.localTransform.setTranslation(offset); - break; - case CHEST: - chestNode.localTransform.setTranslation(offset); - break; - case CHEST_TRACKER: - trackerChestNode.localTransform.setTranslation(offset); - break; - case WAIST: - waistNode.localTransform.setTranslation(offset); - break; - case HIP: - hipNode.localTransform.setTranslation(offset); - break; - case HIP_TRACKER: - trackerWaistNode.localTransform.setTranslation(offset); - break; - - case LEFT_HIP: - leftHipNode.localTransform.setTranslation(offset); - break; - case RIGHT_HIP: - rightHipNode.localTransform.setTranslation(offset); - break; - - case KNEE: - leftKneeNode.localTransform.setTranslation(offset); - rightKneeNode.localTransform.setTranslation(offset); - break; - case KNEE_TRACKER: - trackerLeftKneeNode.localTransform.setTranslation(offset); - trackerRightKneeNode.localTransform.setTranslation(offset); - break; - case ANKLE: - leftAnkleNode.localTransform.setTranslation(offset); - rightAnkleNode.localTransform.setTranslation(offset); - break; - case FOOT: - leftFootNode.localTransform.setTranslation(offset); - rightFootNode.localTransform.setTranslation(offset); - break; - case FOOT_TRACKER: - trackerLeftFootNode.localTransform.setTranslation(offset); - trackerRightFootNode.localTransform.setTranslation(offset); - break; - - case CONTROLLER: - leftWristNodeContrl.localTransform.setTranslation(offset); - rightWristNodeContrl.localTransform.setTranslation(offset); - break; - case FOREARM: - leftElbowNodeContrl.localTransform.setTranslation(offset); - rightElbowNodeContrl.localTransform.setTranslation(offset); - break; - case ELBOW_TRACKER: - trackerLeftElbowNodeContrl.localTransform.setTranslation(offset); - trackerRightElbowNodeContrl.localTransform.setTranslation(offset); - trackerLeftElbowNodeHmd.localTransform.setTranslation(offset); - trackerRightElbowNodeHmd.localTransform.setTranslation(offset); - break; - case LEFT_SHOULDER: - leftShoulderNodeHmd.localTransform.setTranslation(offset); - break; - case RIGHT_SHOULDER: - rightShoulderNodeHmd.localTransform.setTranslation(offset); - break; - case HAND: - leftHandNodeHmd.localTransform.setTranslation(offset); - rightHandNodeHmd.localTransform.setTranslation(offset); - break; - case UPPER_ARM: - leftElbowNodeHmd.localTransform.setTranslation(offset); - rightElbowNodeHmd.localTransform.setTranslation(offset); - break; - case FOREARM_HMD: - leftWristNodeHmd.localTransform.setTranslation(offset); - rightWristNodeHmd.localTransform.setTranslation(offset); - break; - } - } - - public void updatePoseAffectedByConfig(SkeletonConfigValue config) { - switch (config) { - case HEAD: - headNode.update(); - updateComputedTrackers(); - break; - case NECK: - neckNode.update(); - updateComputedTrackers(); - break; - case TORSO: - hipNode.update(); - updateComputedTrackers(); - break; - case CHEST: - chestNode.update(); - updateComputedTrackers(); - break; - case WAIST: - waistNode.update(); - updateComputedTrackers(); - break; - case HIP_OFFSET: - trackerWaistNode.update(); - updateComputedTrackers(); - break; - case HIPS_WIDTH: - leftHipNode.update(); - rightHipNode.update(); - updateComputedTrackers(); - break; - case KNEE_HEIGHT: - leftKneeNode.update(); - rightKneeNode.update(); - break; - case LEGS_LENGTH: - leftKneeNode.update(); - rightKneeNode.update(); - updateComputedTrackers(); - break; - case FOOT_LENGTH: - leftFootNode.update(); - rightFootNode.update(); - updateComputedTrackers(); - break; - case FOOT_OFFSET: - leftAnkleNode.update(); - rightAnkleNode.update(); - updateComputedTrackers(); - break; - case SKELETON_OFFSET: - trackerChestNode.update(); - trackerWaistNode.update(); - trackerLeftKneeNode.update(); - trackerRightKneeNode.update(); - trackerLeftFootNode.update(); - trackerRightFootNode.update(); - updateComputedTrackers(); - break; - case CONTROLLER_DISTANCE_Z: - case CONTROLLER_DISTANCE_Y: - leftWristNodeContrl.update(); - rightWristNodeContrl.update(); - updateComputedTrackers(); - break; - case FOREARM_LENGTH: - leftElbowNodeContrl.update(); - rightElbowNodeContrl.update(); - leftElbowNodeHmd.update(); - rightElbowNodeHmd.update(); - updateComputedTrackers(); - break; - case ELBOW_OFFSET: - trackerLeftElbowNodeContrl.update(); - trackerRightElbowNodeContrl.update(); - trackerLeftElbowNodeHmd.update(); - trackerRightElbowNodeHmd.update(); - updateComputedTrackers(); - break; - case SHOULDERS_DISTANCE: - case SHOULDERS_WIDTH: - leftShoulderNodeHmd.update(); - rightShoulderNodeHmd.update(); - updateComputedTrackers(); - break; - case UPPER_ARM_LENGTH: - leftElbowNodeHmd.update(); - rightElbowNodeHmd.update(); - updateComputedTrackers(); - break; - } - } - // #endregion - - @Override - public TransformNode getRootNode() { - return hmdNode; - } - - @Override - public TransformNode[] getAllNodes() { - List nodesList = new FastList<>(); - - hmdNode.depthFirstTraversal((node) -> { - nodesList.add(node); - }); - leftControllerNodeContrl.depthFirstTraversal((node) -> { - nodesList.add(node); - }); - rightControllerNodeContrl.depthFirstTraversal((node) -> { - nodesList.add(node); - }); - - return nodesList.toArray(new TransformNode[0]); - } - - @Override - public SkeletonConfig getSkeletonConfig() { - return skeletonConfig; - } - - @Override - public void resetSkeletonConfig(SkeletonConfigValue config) { - if (config == null) { - return; - } - - Vector3f vec; - float height; - switch (config) { - case HEAD: - skeletonConfig.setConfig(SkeletonConfigValue.HEAD, null); - break; - case NECK: - skeletonConfig.setConfig(SkeletonConfigValue.NECK, null); - break; - case TORSO: // Distance from shoulders to hip (full torso length) - vec = new Vector3f(); - hmdTracker.getPosition(vec); - height = vec.y; - if (height > 0.5f) { // Reset only if floor level is right, - // TODO: read floor level from SteamVR - // if - // it's not 0 - skeletonConfig - .setConfig( - SkeletonConfigValue.TORSO, - ((height) * 0.42f) - skeletonConfig.getConfig(SkeletonConfigValue.NECK) - ); - } else// if floor level is incorrect - { - skeletonConfig.setConfig(SkeletonConfigValue.TORSO, null); - } - break; - case CHEST: // Chest is 57% of the upper body by default (shoulders - // to chest) - skeletonConfig - .setConfig( - SkeletonConfigValue.CHEST, - skeletonConfig.getConfig(SkeletonConfigValue.TORSO) * 0.57f - ); - break; - case WAIST: // Waist length is from hip to waist - skeletonConfig.setConfig(SkeletonConfigValue.WAIST, null); - break; - case HIP_OFFSET: - skeletonConfig.setConfig(SkeletonConfigValue.HIP_OFFSET, null); - break; - case HIPS_WIDTH: - skeletonConfig.setConfig(SkeletonConfigValue.HIPS_WIDTH, null); - break; - case FOOT_LENGTH: - skeletonConfig.setConfig(SkeletonConfigValue.FOOT_LENGTH, null); - break; - case FOOT_OFFSET: - skeletonConfig.setConfig(SkeletonConfigValue.FOOT_OFFSET, null); - break; - case SKELETON_OFFSET: - skeletonConfig.setConfig(SkeletonConfigValue.SKELETON_OFFSET, null); - break; - case LEGS_LENGTH: // Set legs length to be 5cm above floor level - vec = new Vector3f(); - hmdTracker.getPosition(vec); - height = vec.y; - if (height > 0.5f) { // Reset only if floor level is right, - // todo: read floor level from SteamVR - // if - // it's not 0 - skeletonConfig - .setConfig( - SkeletonConfigValue.LEGS_LENGTH, - height - - skeletonConfig.getConfig(SkeletonConfigValue.NECK) - - skeletonConfig.getConfig(SkeletonConfigValue.TORSO) - - 0.05f - ); - } else // if floor level is incorrect - { - skeletonConfig.setConfig(SkeletonConfigValue.LEGS_LENGTH, null); - } - resetSkeletonConfig(SkeletonConfigValue.KNEE_HEIGHT); - break; - case KNEE_HEIGHT: // Knees are at 55% of the legs by default - skeletonConfig - .setConfig( - SkeletonConfigValue.KNEE_HEIGHT, - skeletonConfig.getConfig(SkeletonConfigValue.LEGS_LENGTH) * 0.55f - ); - break; - case CONTROLLER_DISTANCE_Z: - skeletonConfig.setConfig(SkeletonConfigValue.CONTROLLER_DISTANCE_Z, null); - break; - case CONTROLLER_DISTANCE_Y: - skeletonConfig.setConfig(SkeletonConfigValue.CONTROLLER_DISTANCE_Y, null); - break; - case FOREARM_LENGTH: - skeletonConfig.setConfig(SkeletonConfigValue.FOREARM_LENGTH, null); - break; - case ELBOW_OFFSET: - skeletonConfig.setConfig(SkeletonConfigValue.ELBOW_OFFSET, null); - break; - case SHOULDERS_DISTANCE: - skeletonConfig.setConfig(SkeletonConfigValue.SHOULDERS_DISTANCE, null); - break; - case SHOULDERS_WIDTH: - skeletonConfig.setConfig(SkeletonConfigValue.SHOULDERS_WIDTH, null); - break; - case UPPER_ARM_LENGTH: - skeletonConfig.setConfig(SkeletonConfigValue.UPPER_ARM_LENGTH, null); - break; - } - } - - Tracker[] getTrackerToReset() { - return new Tracker[] { trackerPreUpdate(this.neckTracker), - trackerPreUpdate(this.chestTracker), trackerPreUpdate(this.waistTracker), - trackerPreUpdate(this.hipTracker), trackerPreUpdate(this.leftKneeTracker), - trackerPreUpdate(this.leftAnkleTracker), trackerPreUpdate(this.leftFootTracker), - trackerPreUpdate(this.rightKneeTracker), trackerPreUpdate(this.rightAnkleTracker), - trackerPreUpdate(this.rightFootTracker), trackerPreUpdate(this.rightForearmTracker), - trackerPreUpdate(this.leftForearmTracker), trackerPreUpdate(this.rightUpperArmTracker), - trackerPreUpdate(this.leftUpperArmTracker), trackerPreUpdate(this.leftHandTracker), - trackerPreUpdate(this.rightHandTracker) }; - } - - @Override - public void resetTrackersFull() { - // #region Pass all trackers through trackerPreUpdate - Tracker hmdTracker = trackerPreUpdate(this.hmdTracker); - Tracker[] trackersToReset = getTrackerToReset(); - // #endregion - - // Resets all axis of the trackers with the HMD as reference. - Quaternion referenceRotation = new Quaternion(); - hmdTracker.getRotation(referenceRotation); - - for (Tracker tracker : trackersToReset) { - if (tracker != null) { - tracker.resetFull(referenceRotation); - } - } - } - - @Override - @VRServerThread - public void resetTrackersYaw() { - // #region Pass all trackers through trackerPreUpdate - Tracker hmdTracker = trackerPreUpdate(this.hmdTracker); - Tracker[] trackersToReset = getTrackerToReset(); - // #endregion - - // Resets the yaw of the trackers with the HMD as reference. - Quaternion referenceRotation = new Quaternion(); - hmdTracker.getRotation(referenceRotation); - - for (Tracker tracker : trackersToReset) { - if (tracker != null) { - tracker.resetYaw(referenceRotation); - } - } - } -} diff --git a/src/main/java/dev/slimevr/vr/processor/skeleton/Skeleton.java b/src/main/java/dev/slimevr/vr/processor/skeleton/Skeleton.java new file mode 100644 index 000000000..8edefe859 --- /dev/null +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/Skeleton.java @@ -0,0 +1,37 @@ +package dev.slimevr.vr.processor.skeleton; + +import dev.slimevr.util.ann.VRServerThread; +import dev.slimevr.vr.processor.TransformNode; +import io.eiren.util.ann.ThreadSafe; + + +public abstract class Skeleton { + + @VRServerThread + public abstract void updatePose(); + + @ThreadSafe + public abstract TransformNode getRootNode(); + + @ThreadSafe + public abstract TransformNode[] getAllNodes(); + + @ThreadSafe + public abstract SkeletonConfig getSkeletonConfig(); + + @ThreadSafe + public abstract void resetSkeletonConfig(SkeletonConfigValue config); + + @ThreadSafe + public void resetAllSkeletonConfigs() { + for (SkeletonConfigValue config : SkeletonConfigValue.values) { + resetSkeletonConfig(config); + } + } + + @VRServerThread + public abstract void resetTrackersFull(); + + @VRServerThread + public abstract void resetTrackersYaw(); +} diff --git a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfig.java b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfig.java index b4a0ae011..efad59080 100644 --- a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfig.java +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfig.java @@ -16,8 +16,8 @@ public class SkeletonConfig { protected final EnumMap toggles = new EnumMap( SkeletonConfigToggle.class ); - protected final EnumMap nodeOffsets = new EnumMap( - SkeletonNodeOffset.class + protected final EnumMap nodeOffsets = new EnumMap( + BoneType.class ); protected final boolean autoUpdateOffsets; @@ -144,7 +144,7 @@ public class SkeletonConfig { // Re-compute the affected offsets if (computeOffsets && autoUpdateOffsets && config.affectedOffsets != null) { - for (SkeletonNodeOffset offset : config.affectedOffsets) { + for (BoneType offset : config.affectedOffsets) { computeNodeOffset(offset); } } @@ -227,7 +227,7 @@ public class SkeletonConfig { return getToggle(SkeletonConfigToggle.getByStringValue(config)); } - protected void setNodeOffset(SkeletonNodeOffset nodeOffset, float x, float y, float z) { + protected void setNodeOffset(BoneType nodeOffset, float x, float y, float z) { Vector3f offset = nodeOffsets.get(nodeOffset); if (offset == null) { @@ -246,7 +246,7 @@ public class SkeletonConfig { } } - protected void setNodeOffset(SkeletonNodeOffset nodeOffset, Vector3f offset) { + protected void setNodeOffset(BoneType nodeOffset, Vector3f offset) { if (offset == null) { setNodeOffset(nodeOffset, 0f, 0f, 0f); return; @@ -255,11 +255,11 @@ public class SkeletonConfig { setNodeOffset(nodeOffset, offset.x, offset.y, offset.z); } - public Vector3f getNodeOffset(SkeletonNodeOffset nodeOffset) { + public Vector3f getNodeOffset(BoneType nodeOffset) { return nodeOffsets.getOrDefault(nodeOffset, Vector3f.ZERO); } - public void computeNodeOffset(SkeletonNodeOffset nodeOffset) { + public void computeNodeOffset(BoneType nodeOffset) { switch (nodeOffset) { case HEAD: setNodeOffset(nodeOffset, 0, 0, getConfig(SkeletonConfigValue.HEAD)); @@ -302,7 +302,7 @@ public class SkeletonConfig { setNodeOffset(nodeOffset, getConfig(SkeletonConfigValue.HIPS_WIDTH) / 2f, 0, 0); break; - case KNEE: + case UPPER_LEG: setNodeOffset( nodeOffset, 0, @@ -314,7 +314,7 @@ public class SkeletonConfig { case KNEE_TRACKER: setNodeOffset(nodeOffset, 0, 0, -getConfig(SkeletonConfigValue.SKELETON_OFFSET)); break; - case ANKLE: + case LOWER_LEG: setNodeOffset( nodeOffset, 0, @@ -345,10 +345,10 @@ public class SkeletonConfig { -getConfig(SkeletonConfigValue.CONTROLLER_DISTANCE_Z) ); break; - case FOREARM: + case LOWER_ARM: setNodeOffset(nodeOffset, 0, getConfig(SkeletonConfigValue.FOREARM_LENGTH), 0); break; - case FOREARM_HMD: + case LOWER_ARM_HMD: setNodeOffset(nodeOffset, 0, -getConfig(SkeletonConfigValue.FOREARM_LENGTH), 0); break; case ELBOW_TRACKER: @@ -379,7 +379,7 @@ public class SkeletonConfig { } public void computeAllNodeOffsets() { - for (SkeletonNodeOffset offset : SkeletonNodeOffset.values) { + for (BoneType offset : BoneType.values) { computeNodeOffset(offset); } } diff --git a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigCallback.java b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigCallback.java index 6f243a336..bbffb1207 100644 --- a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigCallback.java +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigCallback.java @@ -9,5 +9,5 @@ public interface SkeletonConfigCallback { void updateToggleState(SkeletonConfigToggle configToggle, boolean newValue); - void updateNodeOffset(SkeletonNodeOffset nodeOffset, Vector3f offset); + void updateNodeOffset(BoneType nodeOffset, Vector3f offset); } diff --git a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigValue.java b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigValue.java index 48d2b17ea..8822cc228 100644 --- a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigValue.java +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigValue.java @@ -11,7 +11,7 @@ public enum SkeletonConfigValue { "headShift", "Head shift", 0.1f, - new SkeletonNodeOffset[] { SkeletonNodeOffset.HEAD } + new BoneType[] { BoneType.HEAD } ), NECK( 2, @@ -19,7 +19,7 @@ public enum SkeletonConfigValue { "neckLength", "Neck length", 0.1f, - new SkeletonNodeOffset[] { SkeletonNodeOffset.NECK } + new BoneType[] { BoneType.NECK } ), TORSO( 3, @@ -27,7 +27,7 @@ public enum SkeletonConfigValue { "torsoLength", "Torso length", 0.56f, - new SkeletonNodeOffset[] { SkeletonNodeOffset.WAIST } + new BoneType[] { BoneType.WAIST } ), CHEST( 4, @@ -35,8 +35,8 @@ public enum SkeletonConfigValue { "chestDistance", "Chest distance", 0.32f, - new SkeletonNodeOffset[] { SkeletonNodeOffset.CHEST, SkeletonNodeOffset.WAIST, - SkeletonNodeOffset.LEFT_SHOULDER, SkeletonNodeOffset.RIGHT_SHOULDER } + new BoneType[] { BoneType.CHEST, BoneType.WAIST, + BoneType.LEFT_SHOULDER, BoneType.RIGHT_SHOULDER } ), WAIST( 5, @@ -44,7 +44,7 @@ public enum SkeletonConfigValue { "waistDistance", "Waist distance", 0.04f, - new SkeletonNodeOffset[] { SkeletonNodeOffset.WAIST, SkeletonNodeOffset.HIP } + new BoneType[] { BoneType.WAIST, BoneType.HIP } ), HIP_OFFSET( 6, @@ -52,7 +52,7 @@ public enum SkeletonConfigValue { "hipOffset", "Hip offset", 0.0f, - new SkeletonNodeOffset[] { SkeletonNodeOffset.HIP_TRACKER } + new BoneType[] { BoneType.HIP_TRACKER } ), HIPS_WIDTH( 7, @@ -60,7 +60,7 @@ public enum SkeletonConfigValue { "hipsWidth", "Hips width", 0.26f, - new SkeletonNodeOffset[] { SkeletonNodeOffset.LEFT_HIP, SkeletonNodeOffset.RIGHT_HIP } + new BoneType[] { BoneType.LEFT_HIP, BoneType.RIGHT_HIP } ), LEGS_LENGTH( 8, @@ -68,7 +68,7 @@ public enum SkeletonConfigValue { "legsLength", "Legs length", 0.92f, - new SkeletonNodeOffset[] { SkeletonNodeOffset.KNEE } + new BoneType[] { BoneType.UPPER_LEG } ), KNEE_HEIGHT( 9, @@ -76,7 +76,7 @@ public enum SkeletonConfigValue { "kneeHeight", "Knee height", 0.50f, - new SkeletonNodeOffset[] { SkeletonNodeOffset.KNEE, SkeletonNodeOffset.ANKLE } + new BoneType[] { BoneType.UPPER_LEG, BoneType.LOWER_LEG } ), FOOT_LENGTH( 10, @@ -84,7 +84,7 @@ public enum SkeletonConfigValue { "footLength", "Foot length", 0.05f, - new SkeletonNodeOffset[] { SkeletonNodeOffset.FOOT } + new BoneType[] { BoneType.FOOT } ), FOOT_OFFSET( 11, @@ -92,7 +92,7 @@ public enum SkeletonConfigValue { "footOffset", "Foot offset", -0.05f, - new SkeletonNodeOffset[] { SkeletonNodeOffset.ANKLE } + new BoneType[] { BoneType.LOWER_LEG } ), SKELETON_OFFSET( 12, @@ -100,8 +100,8 @@ public enum SkeletonConfigValue { "skeletonOffset", "Skeleton offset", 0.0f, - new SkeletonNodeOffset[] { SkeletonNodeOffset.CHEST_TRACKER, SkeletonNodeOffset.HIP_TRACKER, - SkeletonNodeOffset.KNEE_TRACKER, SkeletonNodeOffset.FOOT_TRACKER } + new BoneType[] { BoneType.CHEST_TRACKER, BoneType.HIP_TRACKER, + BoneType.KNEE_TRACKER, BoneType.FOOT_TRACKER } ), CONTROLLER_DISTANCE_Z( 13, @@ -109,7 +109,7 @@ public enum SkeletonConfigValue { "controllerDistanceZ", "Controller distance z", 0.15f, - new SkeletonNodeOffset[] { SkeletonNodeOffset.CONTROLLER, SkeletonNodeOffset.HAND } + new BoneType[] { BoneType.CONTROLLER, BoneType.HAND } ), CONTROLLER_DISTANCE_Y( 14, @@ -117,7 +117,7 @@ public enum SkeletonConfigValue { "controllerDistanceY", "Controller distance y", 0.05f, - new SkeletonNodeOffset[] { SkeletonNodeOffset.CONTROLLER, SkeletonNodeOffset.HAND } + new BoneType[] { BoneType.CONTROLLER, BoneType.HAND } ), FOREARM_LENGTH( 15, @@ -125,8 +125,8 @@ public enum SkeletonConfigValue { "forearmLength", "Forearm length", 0.25f, - new SkeletonNodeOffset[] { SkeletonNodeOffset.FOREARM, - SkeletonNodeOffset.FOREARM_HMD } + new BoneType[] { BoneType.LOWER_ARM, + BoneType.LOWER_ARM_HMD } ), SHOULDERS_DISTANCE( 16, @@ -134,8 +134,8 @@ public enum SkeletonConfigValue { "shoulersDistance", "Shoulders distance", 0.08f, - new SkeletonNodeOffset[] { SkeletonNodeOffset.LEFT_SHOULDER, - SkeletonNodeOffset.RIGHT_SHOULDER } + new BoneType[] { BoneType.LEFT_SHOULDER, + BoneType.RIGHT_SHOULDER } ), SHOULDERS_WIDTH( 17, @@ -143,8 +143,8 @@ public enum SkeletonConfigValue { "shoulersWidth", "Shoulders width", 0.36f, - new SkeletonNodeOffset[] { SkeletonNodeOffset.LEFT_SHOULDER, - SkeletonNodeOffset.RIGHT_SHOULDER } + new BoneType[] { BoneType.LEFT_SHOULDER, + BoneType.RIGHT_SHOULDER } ), UPPER_ARM_LENGTH( 18, @@ -152,7 +152,7 @@ public enum SkeletonConfigValue { "upperArmLength", "Upper arm length", 0.25f, - new SkeletonNodeOffset[] { SkeletonNodeOffset.UPPER_ARM } + new BoneType[] { BoneType.UPPER_ARM } ), ELBOW_OFFSET( 19, @@ -160,7 +160,7 @@ public enum SkeletonConfigValue { "elbowOffset", "Elbow offset", 0f, - new SkeletonNodeOffset[] { SkeletonNodeOffset.ELBOW_TRACKER } + new BoneType[] { BoneType.ELBOW_TRACKER } ),; public static final SkeletonConfigValue[] values = values(); @@ -180,7 +180,7 @@ public enum SkeletonConfigValue { public final String configKey; public final String label; public final float defaultValue; - public final SkeletonNodeOffset[] affectedOffsets; + public final BoneType[] affectedOffsets; SkeletonConfigValue( int id, @@ -188,7 +188,7 @@ public enum SkeletonConfigValue { String configKey, String label, float defaultValue, - SkeletonNodeOffset[] affectedOffsets + BoneType[] affectedOffsets ) { this.id = id; this.stringVal = stringVal; @@ -198,7 +198,7 @@ public enum SkeletonConfigValue { this.defaultValue = defaultValue; this.affectedOffsets = affectedOffsets - == null ? new SkeletonNodeOffset[0] : affectedOffsets; + == null ? new BoneType[0] : affectedOffsets; } public static SkeletonConfigValue getByStringValue(String stringVal) { diff --git a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonData.java b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonData.java index 50558a352..e58a32e81 100644 --- a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonData.java +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonData.java @@ -9,14 +9,18 @@ import java.util.Map; import dev.slimevr.vr.processor.TransformNode; +/** + * WIP Saved as part of skeleton rework. Is not finished. + */ +@Deprecated public class SkeletonData { public final Joint[] joints; public final List bones = new ArrayList<>(); private final SkeletonConfig config; - private Map> bonesByOffsetKey = new EnumMap<>( - SkeletonNodeOffset.class + private Map> bonesByOffsetKey = new EnumMap<>( + BoneType.class ); // #region Upper body nodes (torso) @@ -70,68 +74,68 @@ public class SkeletonData { // Forward is Negative Z, left iz Negative X. See head joint and hip // joints as examples. // #region Assemble skeleton from hmd to hip - hmdJoint.attachJoint(headJoint, SkeletonNodeOffset.HEAD); - headJoint.attachJoint(neckJoint, SkeletonNodeOffset.NECK); - neckJoint.attachJoint(chestJoint, SkeletonNodeOffset.CHEST); - chestJoint.attachJoint(waistJoint, SkeletonNodeOffset.WAIST); - waistJoint.attachJoint(hipJoint, SkeletonNodeOffset.HIP); + hmdJoint.attachJoint(headJoint, BoneType.HEAD); + headJoint.attachJoint(neckJoint, BoneType.NECK); + neckJoint.attachJoint(chestJoint, BoneType.CHEST); + chestJoint.attachJoint(waistJoint, BoneType.WAIST); + waistJoint.attachJoint(hipJoint, BoneType.HIP); // #endregion // #region Assemble skeleton from hips to feet - hipJoint.attachJoint(leftHipJoint, SkeletonNodeOffset.LEFT_HIP); - hipJoint.attachJoint(rightHipJoint, SkeletonNodeOffset.RIGHT_HIP); + hipJoint.attachJoint(leftHipJoint, BoneType.LEFT_HIP); + hipJoint.attachJoint(rightHipJoint, BoneType.RIGHT_HIP); - leftHipJoint.attachJoint(leftKneeJoint, SkeletonNodeOffset.KNEE); - rightHipJoint.attachJoint(rightKneeJoint, SkeletonNodeOffset.KNEE); + leftHipJoint.attachJoint(leftKneeJoint, BoneType.UPPER_LEG); + rightHipJoint.attachJoint(rightKneeJoint, BoneType.UPPER_LEG); - leftKneeJoint.attachJoint(leftAnkleJoint, SkeletonNodeOffset.ANKLE); - rightKneeJoint.attachJoint(rightAnkleJoint, SkeletonNodeOffset.ANKLE); + leftKneeJoint.attachJoint(leftAnkleJoint, BoneType.LOWER_LEG); + rightKneeJoint.attachJoint(rightAnkleJoint, BoneType.LOWER_LEG); - leftAnkleJoint.attachJoint(leftFootJoint, SkeletonNodeOffset.FOOT); - rightAnkleJoint.attachJoint(rightFootJoint, SkeletonNodeOffset.FOOT); + leftAnkleJoint.attachJoint(leftFootJoint, BoneType.FOOT); + rightAnkleJoint.attachJoint(rightFootJoint, BoneType.FOOT); // #endregion // #region Assemble skeleton arms from controllers // TODO : Rebuild skeleton depending on if it's from controllers or from // shoulders // if (fromControllers) - leftHandJoint.attachJoint(leftWristJoint, SkeletonNodeOffset.HAND); - rightHandJoint.attachJoint(rightWristJoint, SkeletonNodeOffset.HAND); - rightWristJoint.attachJoint(leftElbowJoint, SkeletonNodeOffset.FOREARM); - leftWristJoint.attachJoint(rightElbowJoint, SkeletonNodeOffset.FOREARM); + leftHandJoint.attachJoint(leftWristJoint, BoneType.HAND); + rightHandJoint.attachJoint(rightWristJoint, BoneType.HAND); + rightWristJoint.attachJoint(leftElbowJoint, BoneType.LOWER_ARM); + leftWristJoint.attachJoint(rightElbowJoint, BoneType.LOWER_ARM); // } else { // #endregion // #region Assemble skeleton arms from chest - chestJoint.attachJoint(leftShoulderJoint, SkeletonNodeOffset.LEFT_SHOULDER); - chestJoint.attachJoint(rightShoulderJoint, SkeletonNodeOffset.RIGHT_SHOULDER); + chestJoint.attachJoint(leftShoulderJoint, BoneType.LEFT_SHOULDER); + chestJoint.attachJoint(rightShoulderJoint, BoneType.RIGHT_SHOULDER); - leftShoulderJoint.attachJoint(leftElbowJoint, SkeletonNodeOffset.FOREARM); - rightShoulderJoint.attachJoint(rightElbowJoint, SkeletonNodeOffset.FOREARM); + leftShoulderJoint.attachJoint(leftElbowJoint, BoneType.LOWER_ARM); + rightShoulderJoint.attachJoint(rightElbowJoint, BoneType.LOWER_ARM); - leftElbowJoint.attachJoint(leftWristJoint, SkeletonNodeOffset.HAND); - rightElbowJoint.attachJoint(rightWristJoint, SkeletonNodeOffset.HAND); + leftElbowJoint.attachJoint(leftWristJoint, BoneType.HAND); + rightElbowJoint.attachJoint(rightWristJoint, BoneType.HAND); - leftWristJoint.attachJoint(leftHandJoint, SkeletonNodeOffset.HAND); - rightWristJoint.attachJoint(rightHandJoint, SkeletonNodeOffset.HAND); + leftWristJoint.attachJoint(leftHandJoint, BoneType.HAND); + rightWristJoint.attachJoint(rightHandJoint, BoneType.HAND); // } // #endregion // #region Attach tracker nodes for offsets - chestJoint.attachJoint(trackerChestJoint, SkeletonNodeOffset.CHEST_TRACKER); - hipJoint.attachJoint(trackerWaistJoint, SkeletonNodeOffset.WAIST_TRACKER); + chestJoint.attachJoint(trackerChestJoint, BoneType.CHEST_TRACKER); + hipJoint.attachJoint(trackerWaistJoint, BoneType.WAIST_TRACKER); - leftKneeJoint.attachJoint(trackerLeftKneeJoint, SkeletonNodeOffset.KNEE_TRACKER); - rightKneeJoint.attachJoint(trackerRightKneeJoint, SkeletonNodeOffset.KNEE_TRACKER); + leftKneeJoint.attachJoint(trackerLeftKneeJoint, BoneType.KNEE_TRACKER); + rightKneeJoint.attachJoint(trackerRightKneeJoint, BoneType.KNEE_TRACKER); - leftFootJoint.attachJoint(trackerLeftFootJoint, SkeletonNodeOffset.FOOT_TRACKER); - rightFootJoint.attachJoint(trackerRightFootJoint, SkeletonNodeOffset.FOOT_TRACKER); + leftFootJoint.attachJoint(trackerLeftFootJoint, BoneType.FOOT_TRACKER); + rightFootJoint.attachJoint(trackerRightFootJoint, BoneType.FOOT_TRACKER); - leftElbowJoint.attachJoint(trackerLeftElbowJoint, SkeletonNodeOffset.ELBOW_TRACKER); - rightElbowJoint.attachJoint(trackerRightElbowJoint, SkeletonNodeOffset.ELBOW_TRACKER); + leftElbowJoint.attachJoint(trackerLeftElbowJoint, BoneType.ELBOW_TRACKER); + rightElbowJoint.attachJoint(trackerRightElbowJoint, BoneType.ELBOW_TRACKER); - leftHandJoint.attachJoint(trackerLeftHandJoint, SkeletonNodeOffset.HAND_TRACKER); - rightHandJoint.attachJoint(trackerRightHandJoint, SkeletonNodeOffset.HAND_TRACKER); + leftHandJoint.attachJoint(trackerLeftHandJoint, BoneType.HAND_TRACKER); + rightHandJoint.attachJoint(trackerRightHandJoint, BoneType.HAND_TRACKER); // #endregion } @@ -144,7 +148,7 @@ public class SkeletonData { node = new TransformNode(name, false); } - public Bone attachJoint(Joint childJoint, SkeletonNodeOffset offsetKey) { + public Bone attachJoint(Joint childJoint, BoneType offsetKey) { Bone bone = childrenBones.get(childJoint); if (bone == null) { bone = new Bone( @@ -193,13 +197,13 @@ public class SkeletonData { public class Bone { public final Joint parent; public final Joint child; - public SkeletonNodeOffset offsetKey; + public BoneType offsetKey; public Bone( String name, Joint firstJoint, Joint secondJoint, - SkeletonNodeOffset offsetKey + BoneType offsetKey ) { this.parent = firstJoint; this.child = secondJoint; diff --git a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonNodeOffset.java b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonNodeOffset.java deleted file mode 100644 index f7471ebbe..000000000 --- a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonNodeOffset.java +++ /dev/null @@ -1,31 +0,0 @@ -package dev.slimevr.vr.processor.skeleton; - -public enum SkeletonNodeOffset { - - HEAD, - NECK, - CHEST, - CHEST_TRACKER, - WAIST, - WAIST_TRACKER, - HIP, - HIP_TRACKER, - LEFT_HIP, - RIGHT_HIP, - KNEE, - KNEE_TRACKER, - ANKLE, - FOOT, - FOOT_TRACKER, - CONTROLLER, - FOREARM, - FOREARM_HMD, - ELBOW_TRACKER, - UPPER_ARM, - LEFT_SHOULDER, - RIGHT_SHOULDER, - HAND, - HAND_TRACKER; - - public static final SkeletonNodeOffset[] values = values(); -} diff --git a/src/main/java/dev/slimevr/vr/trackers/ComputedTracker.java b/src/main/java/dev/slimevr/vr/trackers/ComputedTracker.java index f90c2c5d5..603693abf 100644 --- a/src/main/java/dev/slimevr/vr/trackers/ComputedTracker.java +++ b/src/main/java/dev/slimevr/vr/trackers/ComputedTracker.java @@ -46,7 +46,7 @@ public class ComputedTracker implements Tracker, TrackerWithTPS { // not be // allowed if editing is not allowed if (userEditable()) { - bodyPosition = TrackerPosition.getByDesignation(config.designation); + bodyPosition = TrackerPosition.getByDesignation(config.designation).get(); } } diff --git a/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java b/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java index 21c323d17..db5bb3c28 100644 --- a/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java +++ b/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java @@ -83,7 +83,7 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery { } else { rotAdjust.loadIdentity(); } - bodyPosition = TrackerPosition.getByDesignation(config.designation); + bodyPosition = TrackerPosition.getByDesignation(config.designation).get(); setFilter( vrserver.config.getString("filters.type"), vrserver.config.getFloat("filters.amount", 0.3f), diff --git a/src/main/java/dev/slimevr/vr/trackers/ReferenceAdjustedTracker.java b/src/main/java/dev/slimevr/vr/trackers/ReferenceAdjustedTracker.java index 0de725366..042303eca 100644 --- a/src/main/java/dev/slimevr/vr/trackers/ReferenceAdjustedTracker.java +++ b/src/main/java/dev/slimevr/vr/trackers/ReferenceAdjustedTracker.java @@ -2,7 +2,6 @@ package dev.slimevr.vr.trackers; import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; -import dev.slimevr.vr.trackers.udp.UDPDevice; public class ReferenceAdjustedTracker implements Tracker { @@ -177,7 +176,7 @@ public class ReferenceAdjustedTracker implements Tracker { } @Override - public UDPDevice getDevice() { + public Device getDevice() { return tracker.getDevice(); } } diff --git a/src/main/java/dev/slimevr/vr/trackers/Tracker.java b/src/main/java/dev/slimevr/vr/trackers/Tracker.java index 4c2b358e0..24a2424ac 100644 --- a/src/main/java/dev/slimevr/vr/trackers/Tracker.java +++ b/src/main/java/dev/slimevr/vr/trackers/Tracker.java @@ -2,7 +2,6 @@ package dev.slimevr.vr.trackers; import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; -import dev.slimevr.vr.trackers.udp.UDPDevice; import java.util.concurrent.atomic.AtomicInteger; @@ -51,7 +50,7 @@ public interface Tracker { int getTrackerNum(); - UDPDevice getDevice(); + Device getDevice(); default String getDescriptiveName() { return getName(); diff --git a/src/main/java/dev/slimevr/vr/trackers/TrackerPosition.java b/src/main/java/dev/slimevr/vr/trackers/TrackerPosition.java index e70f771e6..a4c739f8b 100644 --- a/src/main/java/dev/slimevr/vr/trackers/TrackerPosition.java +++ b/src/main/java/dev/slimevr/vr/trackers/TrackerPosition.java @@ -1,87 +1,127 @@ package dev.slimevr.vr.trackers; +import java.lang.reflect.Modifier; import java.util.EnumMap; import java.util.HashMap; import java.util.Map; +import java.util.Optional; +import java.util.logging.Level; + +import solarxr_protocol.datatypes.BodyPart; +/** + * Represents a position on the body that a tracker could be placed. Any bone is + * a valid position. + * + * TrackerPosition intentionally lacks a numerical id to avoid breakage. + */ public enum TrackerPosition { - - NONE(0, "", TrackerRole.NONE), - HMD(1, "HMD", TrackerRole.HMD), - NECK(2, "body:neck", null), - CHEST(3, "body:chest", TrackerRole.CHEST), - WAIST(4, "body:waist", TrackerRole.WAIST), - HIP(5, "body:hip", null), - LEFT_KNEE(6, "body:left_knee", TrackerRole.LEFT_KNEE), - RIGHT_KNEE(7, "body:right_knee", TrackerRole.RIGHT_KNEE), - LEFT_ANKLE(8, "body:left_ankle", null), - RIGHT_ANKLE(9, "body:right_ankle", null), - LEFT_FOOT(10, "body:left_foot", TrackerRole.LEFT_FOOT), - RIGHT_FOOT(11, "body:right_foot", TrackerRole.RIGHT_FOOT), - LEFT_CONTROLLER(12, "body:left_controller", TrackerRole.LEFT_CONTROLLER), - RIGHT_CONTROLLER(13, "body:right_controller", TrackerRole.RIGHT_CONTROLLER), - LEFT_FOREARM(14, "body:left_forearm", TrackerRole.LEFT_ELBOW), - RIGHT_FOREARM(15, "body:right_forearm", TrackerRole.RIGHT_ELBOW), - LEFT_UPPER_ARM(16, "body:left_upperarm", null), - RIGHT_UPPER_ARM(17, "body:right_upperarm", null), - LEFT_HAND(18, "body:left_hand", TrackerRole.LEFT_HAND), - RIGHT_HAND(19, "body:right_hand", TrackerRole.RIGHT_HAND),; + // @formatter:off + HMD("HMD", TrackerRole.HMD, BodyPart.HMD), + NECK("body:neck", TrackerRole.NECK, BodyPart.NECK), + CHEST("body:chest", TrackerRole.CHEST, BodyPart.CHEST), + WAIST("body:waist", Optional.empty(), BodyPart.WAIST), + HIP("body:hip", TrackerRole.WAIST, BodyPart.HIP), + LEFT_KNEE("body:left_knee", TrackerRole.LEFT_KNEE, BodyPart.LEFT_KNEE), + RIGHT_KNEE("body:right_knee", TrackerRole.RIGHT_KNEE, BodyPart.RIGHT_KNEE), + LEFT_ANKLE("body:left_ankle", Optional.empty(), BodyPart.LEFT_ANKLE), + RIGHT_ANKLE("body:right_ankle", Optional.empty(), BodyPart.RIGHT_ANKLE), + LEFT_FOOT("body:left_foot", TrackerRole.LEFT_FOOT, BodyPart.LEFT_FOOT), + RIGHT_FOOT("body:right_foot", TrackerRole.RIGHT_FOOT, BodyPart.RIGHT_FOOT), + LEFT_CONTROLLER("body:left_controller", TrackerRole.LEFT_CONTROLLER, BodyPart.LEFT_CONTROLLER), + RIGHT_CONTROLLER("body:right_controller", TrackerRole.RIGHT_CONTROLLER, BodyPart.RIGHT_CONTROLLER), + LEFT_FOREARM("body:left_forearm", TrackerRole.LEFT_ELBOW, BodyPart.LEFT_FOREARM), + RIGHT_FOREARM("body:right_forearm", TrackerRole.RIGHT_ELBOW, BodyPart.RIGHT_FOREARM), + LEFT_UPPER_ARM("body:left_upperarm", TrackerRole.LEFT_SHOULDER, BodyPart.LEFT_UPPER_ARM), + RIGHT_UPPER_ARM("body:right_upperarm", TrackerRole.RIGHT_SHOULDER, BodyPart.RIGHT_UPPER_ARM), + LEFT_HAND("body:left_hand", TrackerRole.LEFT_HAND, BodyPart.LEFT_HAND), + RIGHT_HAND("body:right_hand", TrackerRole.RIGHT_HAND, BodyPart.RIGHT_HAND),; + // @formatter:on public static final TrackerPosition[] values = values(); - private static final Map byId = new HashMap<>(); - private static final Map byDesignation = new HashMap<>(); - private static final EnumMap byRole = new EnumMap<>( - TrackerRole.class - ); - static { - for (TrackerPosition tbp : values()) { - byDesignation.put(tbp.designation.toLowerCase(), tbp); - byId.put(tbp.id, tbp); - if (tbp.trackerRole != null) { - TrackerPosition old = byRole.get(tbp.trackerRole); - if (old != null) - throw new AssertionError( - "Only one tracker position can match tracker role. " - + tbp.trackerRole - + " is occupied by " - + old - + " when adding " - + tbp - ); - byRole.put(tbp.trackerRole, tbp); - } - } + public final String designation; + public final Optional trackerRole; + /** The associated `BodyPart` */ + public final int bodyPart; + + private TrackerPosition(String designation, TrackerRole nullableTrackerRole, int bodyPart) { + this(designation, Optional.ofNullable(nullableTrackerRole), bodyPart); } - public final int id; - public final String designation; - public final TrackerRole trackerRole; - - TrackerPosition(int id, String designation, TrackerRole trackerRole) { - this.id = id; + private TrackerPosition(String designation, Optional trackerRole, int bodyPart) { this.designation = designation; this.trackerRole = trackerRole; + this.bodyPart = bodyPart; } - public static TrackerPosition getByDesignation(String designation) { - // Support old configs. leg was renamed to knee. - if (designation != null) { - if (designation.equals("body:left_leg")) - designation = "body:left_knee"; - if (designation.equals("body:right_leg")) - designation = "body:right_knee"; + /** Indexed by `BodyPart` int value. EFFICIENCY FTW */ + private static final TrackerPosition[] byBodyPart; + static { + // Determine maximum value of BodyPart. Kinda hacky, but this will + // prevent breakage if the max value changes. + int max = 0; + for (var field : BodyPart.class.getFields()) { + if (!Modifier.isStatic(field.getModifiers()) || !field.getType().equals(int.class)) { + continue; + } + try { + var v = field.getInt(null); + max = Math.max(max, v); + } catch (IllegalAccessException e) { + // unreachable + java.util.logging.Logger.getGlobal().log(Level.SEVERE, "Reached unreachable code"); + } + } + byBodyPart = new TrackerPosition[max + 1]; + + for (var tp : TrackerPosition.values) { + byBodyPart[tp.bodyPart] = tp; + } + } + + private static final Map byDesignation = new HashMap<>(); + private static final EnumMap byTrackerRole = new EnumMap<>( + TrackerRole.class + ); + static { + for (TrackerPosition tp : values()) { + byDesignation.put(tp.designation.toLowerCase(), tp); + tp.trackerRole.ifPresent((tr) -> byTrackerRole.put(tr, tp)); + } + } + + + /** + * Gets the `TrackerPosition` by its string designation. + * + * @return Returns an optional as not all strings are valid designators. + */ + public static Optional getByDesignation(String designation) { + if (designation == null) { + return Optional.empty(); } - return designation == null ? null : byDesignation.get(designation.toLowerCase()); + // Support old configs. leg was renamed to knee. + if (designation.equalsIgnoreCase("body:left_leg")) + designation = "body:left_knee"; + if (designation.equalsIgnoreCase("body:right_leg")) + designation = "body:right_knee"; + + + return Optional.ofNullable(byDesignation.get(designation.toLowerCase())); } - public static TrackerPosition getByRole(TrackerRole role) { - return byRole.get(role); + public static Optional getByTrackerRole(TrackerRole role) { + return Optional.ofNullable(byTrackerRole.get(role)); } - public static TrackerPosition getById(int id) { - return byId.get(id); + public static Optional getByBodyPart(int bodyPart) { + if (bodyPart < 0 || bodyPart >= TrackerPosition.byBodyPart.length) { + return Optional.empty(); + } + return Optional.of(TrackerPosition.byBodyPart[bodyPart]); } + } diff --git a/src/main/java/dev/slimevr/vr/trackers/TrackerRole.java b/src/main/java/dev/slimevr/vr/trackers/TrackerRole.java index 463e5f0a0..9ed43ac15 100644 --- a/src/main/java/dev/slimevr/vr/trackers/TrackerRole.java +++ b/src/main/java/dev/slimevr/vr/trackers/TrackerRole.java @@ -1,7 +1,17 @@ package dev.slimevr.vr.trackers; +/** + * The SteamVR tracker role. + * + * The tracker role classifies the position and the role of a tracker on user's + * body or playspace (like CAMERA or BEACON), using SteamVR naming. Tracker + * roles are hints for interacting programs what the tracker means, and they do + * not correspond to body's bones on purpose. Example: virtual vive trackers for + * SteamVR vs actual SlimeVR trackers. + */ public enum TrackerRole { + // @formatter:off NONE(0, "", "", null), WAIST(1, "vive_tracker_waist", "TrackerRole_Waist", DeviceType.TRACKER), LEFT_FOOT(2, "vive_tracker_left_foot", "TrackerRole_LeftFoot", DeviceType.TRACKER), @@ -12,12 +22,7 @@ public enum TrackerRole { LEFT_ELBOW(7, "vive_tracker_left_elbow", "TrackerRole_LeftElbow", DeviceType.TRACKER), RIGHT_ELBOW(8, "vive_tracker_right_elbow", "TrackerRole_RightElbow", DeviceType.TRACKER), LEFT_SHOULDER(9, "vive_tracker_left_shoulder", "TrackerRole_LeftShoulder", DeviceType.TRACKER), - RIGHT_SHOULDER( - 10, - "vive_tracker_right_shoulder", - "TrackerRole_RightShoulder", - DeviceType.TRACKER - ), + RIGHT_SHOULDER(10, "vive_tracker_right_shoulder", "TrackerRole_RightShoulder", DeviceType.TRACKER), LEFT_HAND(11, "vive_tracker_handed", "TrackerRole_Handed", DeviceType.TRACKER), RIGHT_HAND(12, "vive_tracker_handed", "TrackerRole_Handed", DeviceType.TRACKER), LEFT_CONTROLLER(13, "vive_tracker_handed", "TrackerRole_Handed", DeviceType.CONTROLLER), @@ -29,6 +34,7 @@ public enum TrackerRole { HMD(19, "", "", DeviceType.HMD), BEACON(20, "", "", DeviceType.TRACKING_REFERENCE), GENERIC_CONTROLLER(21, "vive_tracker_handed", "TrackerRole_Handed", DeviceType.CONTROLLER); + // @formatter:on public static final TrackerRole[] values = values(); private static final TrackerRole[] byId = new TrackerRole[22]; @@ -53,7 +59,7 @@ public enum TrackerRole { public final String viveRole; public final DeviceType deviceType; - TrackerRole(int id, String roleHint, String viveRole, DeviceType deviceType) { + private TrackerRole(int id, String roleHint, String viveRole, DeviceType deviceType) { this.id = id; this.roleHint = roleHint; this.viveRole = viveRole;