From e2d61895470f3a3114d7c06dbe2f85372ed46a8b Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Fri, 24 Dec 2021 21:25:40 +0200 Subject: [PATCH] Code formatting --- .../java/dev/slimevr/autobone/AutoBone.java | 64 ++--- .../gui/AbstractComponentListener.java | 3 +- .../slimevr/gui/AbstractWindowListener.java | 2 +- src/main/java/dev/slimevr/gui/Keybinding.java | 18 +- .../java/dev/slimevr/gui/ScalableFont.java | 7 +- .../dev/slimevr/poserecorder/PoseFrameIO.java | 1 - .../poserecorder/PoseFrameSkeleton.java | 16 +- .../poserecorder/PoseFrameTracker.java | 2 +- .../slimevr/poserecorder/PoseRecorder.java | 74 +++--- .../slimevr/poserecorder/TrackerFrame.java | 2 +- .../slimevr/posestreamer/BVHFileStream.java | 146 +++++------ .../slimevr/posestreamer/PoseDataStream.java | 18 +- .../slimevr/posestreamer/PoseStreamer.java | 58 ++--- .../posestreamer/StdBVHFileStream.java | 30 +-- .../posestreamer/TransformNodeWrapper.java | 100 ++++---- .../processor/ComputedHumanPoseTracker.java | 4 +- .../vr/processor/HumanPoseProcessor.java | 24 +- .../slimevr/vr/processor/TransformNode.java | 23 +- .../vr/processor/skeleton/HumanSkeleton.java | 10 +- .../vr/processor/skeleton/SimpleSkeleton.java | 228 +++++++++--------- .../vr/processor/skeleton/SkeletonConfig.java | 196 +++++++-------- .../skeleton/SkeletonConfigCallback.java | 3 + .../skeleton/SkeletonConfigToggle.java | 10 +- .../skeleton/SkeletonConfigValue.java | 36 +-- .../skeleton/SkeletonNodeOffset.java | 2 +- 25 files changed, 534 insertions(+), 543 deletions(-) diff --git a/src/main/java/dev/slimevr/autobone/AutoBone.java b/src/main/java/dev/slimevr/autobone/AutoBone.java index 29a6583f82..cbb97442de 100644 --- a/src/main/java/dev/slimevr/autobone/AutoBone.java +++ b/src/main/java/dev/slimevr/autobone/AutoBone.java @@ -65,7 +65,7 @@ public String toString() { // TODO Needs much more work, probably going to rethink how the errors work to avoid this barely functional workaround @ButterscotchVanilla // For scaling distances, since smaller sizes will cause smaller distances //private float totalLengthBase = 2f; - + // Human average is probably 1.1235 (SD 0.07) public float legBodyRatio = 1.1235f; // SD of 0.07, capture 68% within range @@ -83,10 +83,10 @@ public String toString() { public final EnumMap configs = new EnumMap(SkeletonConfigValue.class); public final EnumMap staticConfigs = new EnumMap(SkeletonConfigValue.class); - public final FastList heightConfigs = new FastList(new SkeletonConfigValue[] { - SkeletonConfigValue.NECK, SkeletonConfigValue.TORSO, SkeletonConfigValue.LEGS_LENGTH}); - public final FastList lengthConfigs = new FastList(new SkeletonConfigValue[] { - SkeletonConfigValue.HEAD, SkeletonConfigValue.NECK, SkeletonConfigValue.TORSO, SkeletonConfigValue.HIPS_WIDTH, SkeletonConfigValue.LEGS_LENGTH}); + public final FastList heightConfigs = new FastList(new SkeletonConfigValue[]{ + SkeletonConfigValue.NECK, SkeletonConfigValue.TORSO, SkeletonConfigValue.LEGS_LENGTH}); + public final FastList lengthConfigs = new FastList(new SkeletonConfigValue[]{ + SkeletonConfigValue.HEAD, SkeletonConfigValue.NECK, SkeletonConfigValue.TORSO, SkeletonConfigValue.HIPS_WIDTH, SkeletonConfigValue.LEGS_LENGTH}); public AutoBone(VRServer server) { this.server = server; @@ -132,7 +132,7 @@ public void reloadConfigValues(List trackers) { staticConfigs.put(SkeletonConfigValue.HIPS_WIDTH, readFromConfig(SkeletonConfigValue.HIPS_WIDTH)); configs.put(SkeletonConfigValue.LEGS_LENGTH, readFromConfig(SkeletonConfigValue.LEGS_LENGTH)); configs.put(SkeletonConfigValue.KNEE_HEIGHT, readFromConfig(SkeletonConfigValue.KNEE_HEIGHT)); - + // Keep "feet" at ankles staticConfigs.put(SkeletonConfigValue.FOOT_LENGTH, 0f); staticConfigs.put(SkeletonConfigValue.FOOT_OFFSET, 0f); @@ -177,7 +177,7 @@ private void setConfig(SkeletonConfigValue config) { // This doesn't require a skeleton, therefore can be used if skeleton is null public void saveConfigs() { - for (SkeletonConfigValue config : SkeletonConfigValue.values) { + for(SkeletonConfigValue config : SkeletonConfigValue.values) { setConfig(config); } @@ -197,11 +197,11 @@ public Float getConfig(SkeletonConfigValue config, Map selection, Map configs, Map configsAlt) { float sum = 0f; - for (SkeletonConfigValue config : selection) { + for(SkeletonConfigValue config : selection) { Float length = getConfig(config, configs, configsAlt); if(length != null) { sum += length; @@ -214,29 +214,29 @@ public float sumSelectConfigs(List selection, Map selection, SkeletonConfig skeletonConfig) { float sum = 0f; - for (SkeletonConfigValue config : selection) { + for(SkeletonConfigValue config : selection) { sum += skeletonConfig.getConfig(config); } return sum; } - + public float getLengthSum(Map configs) { return getLengthSum(configs, null); } - + public float getLengthSum(Map configs, Map configsAlt) { float length = 0f; - if (configsAlt != null) { + if(configsAlt != null) { for(Entry config : configsAlt.entrySet()) { // If there isn't a duplicate config - if (!configs.containsKey(config.getKey())) { + if(!configs.containsKey(config.getKey())) { length += config.getValue(); } } } - + for(Float boneLength : configs.values()) { length += boneLength; } @@ -277,7 +277,7 @@ public float processFrames(PoseFrames frames, boolean calcInitError, float targe public float processFrames(PoseFrames frames, boolean calcInitError, float targetHeight, Consumer epochCallback) { final int frameCount = frames.getMaxFrameCount(); - + List trackers = frames.getTrackers(); reloadConfigValues(trackers); // Reload configs and detect chest tracker from the first frame @@ -311,13 +311,13 @@ public float processFrames(PoseFrames frames, boolean calcInitError, float targe for(int cursorOffset = minDataDistance; cursorOffset <= maxDataDistance && cursorOffset < frameCount; cursorOffset++) { for(int frameCursor = 0; frameCursor < frameCount - cursorOffset; frameCursor += cursorIncrement) { int frameCursor2 = frameCursor + cursorOffset; - + skeleton1.skeletonConfig.setConfigs(configs, null); skeleton2.skeletonConfig.setConfigs(configs, null); skeleton1.setCursor(frameCursor); skeleton1.updatePose(); - + skeleton2.setCursor(frameCursor2); skeleton2.updatePose(); @@ -348,7 +348,7 @@ public float processFrames(PoseFrames frames, boolean calcInitError, float targe float adjustVal = error * adjustRate; // If there is no adjustment whatsoever, skip this - if (adjustVal == 0f) { + if(adjustVal == 0f) { continue; } @@ -420,21 +420,21 @@ protected float getSlideErrorDeriv(PoseFrameSkeleton skeleton1, PoseFrameSkeleto // Divide by 4 to halve and average, it's halved because you want to approach a midpoint, not the other point return (slideLeft + slideRight) / 4f; } - + // The change in distance between both of the ankles over time protected float getOffsetSlideErrorDeriv(PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2) { Vector3f leftFoot1 = skeleton1.getComputedTracker(TrackerRole.LEFT_FOOT).position; Vector3f rightFoot1 = skeleton1.getComputedTracker(TrackerRole.RIGHT_FOOT).position; - + Vector3f leftFoot2 = skeleton2.getComputedTracker(TrackerRole.LEFT_FOOT).position; Vector3f rightFoot2 = skeleton2.getComputedTracker(TrackerRole.RIGHT_FOOT).position; - + float slideDist1 = leftFoot1.distance(rightFoot1); float slideDist2 = leftFoot2.distance(rightFoot2); - + float slideDist3 = leftFoot1.distance(rightFoot2); float slideDist4 = leftFoot2.distance(rightFoot1); - + float dist1 = FastMath.abs(slideDist1 - slideDist2); float dist2 = FastMath.abs(slideDist3 - slideDist4); @@ -452,7 +452,7 @@ protected float getOffsetSlideErrorDeriv(PoseFrameSkeleton skeleton1, PoseFrameS protected float getOffsetErrorDeriv(PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2) { float leftFoot1 = skeleton1.getComputedTracker(TrackerRole.LEFT_FOOT).position.getY(); float rightFoot1 = skeleton1.getComputedTracker(TrackerRole.RIGHT_FOOT).position.getY(); - + float leftFoot2 = skeleton2.getComputedTracker(TrackerRole.LEFT_FOOT).position.getY(); float rightFoot2 = skeleton2.getComputedTracker(TrackerRole.RIGHT_FOOT).position.getY(); @@ -476,7 +476,7 @@ protected float getProportionErrorDeriv(SkeletonConfig skeleton) { float torsoLength = skeleton.getConfig(SkeletonConfigValue.TORSO); float legsLength = skeleton.getConfig(SkeletonConfigValue.LEGS_LENGTH); float kneeHeight = skeleton.getConfig(SkeletonConfigValue.KNEE_HEIGHT); - + float chestTorso = FastMath.abs((chestLength / torsoLength) - chestTorsoRatio); float legBody = FastMath.abs((legsLength / (torsoLength + neckLength)) - legBodyRatio); float kneeLeg = FastMath.abs((kneeHeight / legsLength) - kneeLegRatio); @@ -496,9 +496,9 @@ protected float getPositionErrorDeriv(PoseFrames frames, int cursor, PoseFrameSk int offsetCount = 0; List trackers = frames.getTrackers(); - for (int i = 0; i < trackers.size(); i++) { + for(int i = 0; i < trackers.size(); i++) { PoseFrameTracker tracker = trackers.get(i); - + TrackerFrame trackerFrame = tracker.safeGetFrame(cursor); if(trackerFrame == null || !trackerFrame.hasData(TrackerFrameData.POSITION)) { continue; @@ -520,9 +520,9 @@ protected float getPositionOffsetErrorDeriv(PoseFrames frames, int cursor1, int int offsetCount = 0; List trackers = frames.getTrackers(); - for (int i = 0; i < trackers.size(); i++) { + for(int i = 0; i < trackers.size(); i++) { PoseFrameTracker tracker = trackers.get(i); - + TrackerFrame trackerFrame1 = tracker.safeGetFrame(cursor1); if(trackerFrame1 == null || !trackerFrame1.hasData(TrackerFrameData.POSITION)) { continue; @@ -561,7 +561,7 @@ protected float getErrorDeriv(PoseFrames frames, int cursor1, int cursor2, PoseF totalError += getSlideErrorDeriv(skeleton1, skeleton2) * distScale * slideErrorFactor; sumWeight += slideErrorFactor; } - + if(offsetSlideErrorFactor > 0f) { totalError += getOffsetSlideErrorDeriv(skeleton1, skeleton2) * distScale * offsetSlideErrorFactor; sumWeight += offsetSlideErrorFactor; @@ -605,7 +605,7 @@ protected static float errorFunc(float errorDeriv) { protected void updateSkeletonBoneLength(PoseFrameSkeleton skeleton1, PoseFrameSkeleton skeleton2, SkeletonConfigValue config, float newLength) { skeleton1.skeletonConfig.setConfig(config, newLength); skeleton1.updatePoseAffectedByConfig(config); - + skeleton2.skeletonConfig.setConfig(config, newLength); skeleton2.updatePoseAffectedByConfig(config); } diff --git a/src/main/java/dev/slimevr/gui/AbstractComponentListener.java b/src/main/java/dev/slimevr/gui/AbstractComponentListener.java index 19468a5e02..e1c0015ce8 100644 --- a/src/main/java/dev/slimevr/gui/AbstractComponentListener.java +++ b/src/main/java/dev/slimevr/gui/AbstractComponentListener.java @@ -20,5 +20,4 @@ public void componentShown(ComponentEvent e) { @Override public void componentHidden(ComponentEvent e) { } - -} \ No newline at end of file +} diff --git a/src/main/java/dev/slimevr/gui/AbstractWindowListener.java b/src/main/java/dev/slimevr/gui/AbstractWindowListener.java index 70bfeba90e..0a6586c3ae 100644 --- a/src/main/java/dev/slimevr/gui/AbstractWindowListener.java +++ b/src/main/java/dev/slimevr/gui/AbstractWindowListener.java @@ -32,4 +32,4 @@ public void windowActivated(WindowEvent e) { @Override public void windowDeactivated(WindowEvent e) { } -} \ No newline at end of file +} diff --git a/src/main/java/dev/slimevr/gui/Keybinding.java b/src/main/java/dev/slimevr/gui/Keybinding.java index 6b3f3abd1a..e80882effa 100644 --- a/src/main/java/dev/slimevr/gui/Keybinding.java +++ b/src/main/java/dev/slimevr/gui/Keybinding.java @@ -13,15 +13,15 @@ public class Keybinding implements HotkeyListener { public final VRServer server; private static final int RESET = 1; private static final int QUICK_RESET = 2; - + @AWTThread public Keybinding(VRServer server) { this.server = server; - + try { if(JIntellitype.getInstance() instanceof JIntellitype) { JIntellitype.getInstance().addHotKeyListener(this); - + String resetBinding = this.server.config.getString("keybindings.reset"); if(resetBinding == null) { resetBinding = "CTRL+ALT+SHIFT+Y"; @@ -29,7 +29,7 @@ public Keybinding(VRServer server) { } JIntellitype.getInstance().registerHotKey(RESET, resetBinding); LogManager.log.info("[Keybinding] Bound reset to " + resetBinding); - + String quickResetBinding = this.server.config.getString("keybindings.quickReset"); if(quickResetBinding == null) { quickResetBinding = "CTRL+ALT+SHIFT+U"; @@ -38,17 +38,13 @@ public Keybinding(VRServer server) { JIntellitype.getInstance().registerHotKey(QUICK_RESET, quickResetBinding); LogManager.log.info("[Keybinding] Bound quick reset to " + quickResetBinding); } - } - catch (JIntellitypeException je) - { + } catch(JIntellitypeException je) { LogManager.log.info("[Keybinding] JIntellitype initialization failed. Keybindings will be disabled. Try restarting your computer."); - } - catch (ExceptionInInitializerError e) - { + } catch(ExceptionInInitializerError e) { LogManager.log.info("[Keybinding] JIntellitype initialization failed. Keybindings will be disabled. Try restarting your computer."); } } - + @AWTThread @Override public void onHotKey(int identifier) { diff --git a/src/main/java/dev/slimevr/gui/ScalableFont.java b/src/main/java/dev/slimevr/gui/ScalableFont.java index 64a35b9ff8..8e03a45d47 100644 --- a/src/main/java/dev/slimevr/gui/ScalableFont.java +++ b/src/main/java/dev/slimevr/gui/ScalableFont.java @@ -22,7 +22,7 @@ public ScalableFont(Font font) { super(font); if(font instanceof ScalableFont) { - ScalableFont sourceFont = (ScalableFont)font; + ScalableFont sourceFont = (ScalableFont) font; this.initSize = sourceFont.getInitSize(); this.initPointSize = sourceFont.getInitSize2D(); @@ -39,7 +39,7 @@ public ScalableFont(Font font, float scale) { super(font); if(font instanceof ScalableFont) { - ScalableFont sourceFont = (ScalableFont)font; + ScalableFont sourceFont = (ScalableFont) font; this.initSize = sourceFont.getInitSize(); this.initPointSize = sourceFont.getInitSize2D(); @@ -84,8 +84,7 @@ private void setScale(float scale) { float newPointSize = initPointSize * scale; - this.size = (int)(newPointSize + 0.5); + this.size = (int) (newPointSize + 0.5); this.pointSize = newPointSize; } - } diff --git a/src/main/java/dev/slimevr/poserecorder/PoseFrameIO.java b/src/main/java/dev/slimevr/poserecorder/PoseFrameIO.java index 0bab85f277..8e9ef7fb43 100644 --- a/src/main/java/dev/slimevr/poserecorder/PoseFrameIO.java +++ b/src/main/java/dev/slimevr/poserecorder/PoseFrameIO.java @@ -26,7 +26,6 @@ public static boolean writeFrames(DataOutputStream outputStream, PoseFrames fram if(frames != null) { outputStream.writeInt(frames.getTrackerCount()); for(PoseFrameTracker tracker : frames.getTrackers()) { - outputStream.writeUTF(tracker.name); outputStream.writeInt(tracker.getFrameCount()); for(int i = 0; i < tracker.getFrameCount(); i++) { diff --git a/src/main/java/dev/slimevr/poserecorder/PoseFrameSkeleton.java b/src/main/java/dev/slimevr/poserecorder/PoseFrameSkeleton.java index ecc5a622ab..eb22ca4a57 100644 --- a/src/main/java/dev/slimevr/poserecorder/PoseFrameSkeleton.java +++ b/src/main/java/dev/slimevr/poserecorder/PoseFrameSkeleton.java @@ -10,13 +10,13 @@ import dev.slimevr.vr.trackers.Tracker; public class PoseFrameSkeleton extends SimpleSkeleton { - + private int frameCursor = 0; - + protected PoseFrameSkeleton(List computedTrackers) { super(computedTrackers); } - + public PoseFrameSkeleton(VRServer server, List computedTrackers) { super(server, computedTrackers); } @@ -24,7 +24,7 @@ public PoseFrameSkeleton(VRServer server, List trackers, List computedTrackers) { super(trackers, computedTrackers); } - + public PoseFrameSkeleton(List trackers, List computedTrackers, Map configs, Map altConfigs) { super(trackers, computedTrackers, configs, altConfigs); } @@ -32,7 +32,7 @@ public PoseFrameSkeleton(List trackers, List trackers, List computedTrackers, Map configs) { super(trackers, computedTrackers, configs); } - + private int limitCursor() { if(frameCursor < 0) { frameCursor = 0; @@ -58,13 +58,13 @@ public int incrementCursor() { public int getCursor() { return frameCursor; } - + // Get tracker for specific frame @Override protected Tracker trackerPreUpdate(Tracker tracker) { - if (tracker instanceof PoseFrameTracker) { + if(tracker instanceof PoseFrameTracker) { // Return frame if available, otherwise return the original tracker - TrackerFrame frame = ((PoseFrameTracker)tracker).safeGetFrame(frameCursor); + TrackerFrame frame = ((PoseFrameTracker) tracker).safeGetFrame(frameCursor); return frame == null ? tracker : frame; } return tracker; diff --git a/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java b/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java index e309c5f66e..d2a1dd911f 100644 --- a/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java +++ b/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java @@ -231,7 +231,7 @@ public boolean isComputed() { public Iterator iterator() { return frames.iterator(); } - + @Override public int getTrackerId() { return this.trackerId; diff --git a/src/main/java/dev/slimevr/poserecorder/PoseRecorder.java b/src/main/java/dev/slimevr/poserecorder/PoseRecorder.java index 107c456a86..42fdbed27c 100644 --- a/src/main/java/dev/slimevr/poserecorder/PoseRecorder.java +++ b/src/main/java/dev/slimevr/poserecorder/PoseRecorder.java @@ -14,75 +14,75 @@ import io.eiren.util.logging.LogManager; public class PoseRecorder { - + protected PoseFrames poseFrame = null; - + protected int numFrames = -1; protected int frameCursor = 0; protected long frameRecordingInterval = 60L; protected long nextFrameTimeMs = -1L; - + protected CompletableFuture currentRecording; - + protected final VRServer server; FastList> trackers = new FastList>(); - + public PoseRecorder(VRServer server) { this.server = server; server.addOnTick(this::onTick); } - + @VRServerThread public void onTick() { - if (numFrames <= 0) { + if(numFrames <= 0) { return; } - + PoseFrames poseFrame = this.poseFrame; List> trackers = this.trackers; - if (poseFrame == null || trackers == null) { + if(poseFrame == null || trackers == null) { return; } - - if (frameCursor >= numFrames) { + + if(frameCursor >= numFrames) { // If done and hasn't yet, send finished recording stopFrameRecording(); return; } long curTime = System.currentTimeMillis(); - if (curTime < nextFrameTimeMs) { + if(curTime < nextFrameTimeMs) { return; } - + nextFrameTimeMs += frameRecordingInterval; - + // To prevent duplicate frames, make sure the frame time is always in the future - if (nextFrameTimeMs <= curTime) { + if(nextFrameTimeMs <= curTime) { nextFrameTimeMs = curTime + frameRecordingInterval; } - + // Make sure it's synchronized since this is the server thread interacting with // an unknown outside thread controlling this class - synchronized (this) { + synchronized(this) { // A stopped recording will be accounted for by an empty "trackers" list int cursor = frameCursor++; for(Pair tracker : trackers) { // Add a frame for each tracker tracker.getRight().addFrame(cursor, tracker.getLeft()); } - + // If done, send finished recording if(frameCursor >= numFrames) { stopFrameRecording(); } } } - + public synchronized Future startFrameRecording(int numFrames, long intervalMs) { return startFrameRecording(numFrames, intervalMs, server.getAllTrackers()); } - + public synchronized Future startFrameRecording(int numFrames, long intervalMs, List trackers) { if(numFrames < 1) { throw new IllegalArgumentException("numFrames must at least have a value of 1"); @@ -99,11 +99,11 @@ public synchronized Future startFrameRecording(int numFrames, long i if(!isReadyToRecord()) { throw new IllegalStateException("PoseRecorder isn't ready to record!"); } - + cancelFrameRecording(); - + poseFrame = new PoseFrames(trackers.size()); - + // Update tracker list this.trackers.ensureCapacity(trackers.size()); for(Tracker tracker : trackers) { @@ -111,65 +111,65 @@ public synchronized Future startFrameRecording(int numFrames, long i if(tracker == null || tracker.isComputed()) { continue; } - + // Pair tracker with recording this.trackers.add(Pair.of(tracker, poseFrame.addTracker(tracker, numFrames))); } - + this.frameCursor = 0; this.numFrames = numFrames; - + frameRecordingInterval = intervalMs; nextFrameTimeMs = -1L; - + LogManager.log.info("[PoseRecorder] Recording " + numFrames + " samples at a " + intervalMs + " ms frame interval"); - + currentRecording = new CompletableFuture(); return currentRecording; } - + public synchronized void stopFrameRecording() { CompletableFuture currentRecording = this.currentRecording; if(currentRecording != null && !currentRecording.isDone()) { // Stop the recording, returning the frames recorded currentRecording.complete(poseFrame); } - + numFrames = -1; frameCursor = 0; trackers.clear(); poseFrame = null; } - + public synchronized void cancelFrameRecording() { CompletableFuture currentRecording = this.currentRecording; if(currentRecording != null && !currentRecording.isDone()) { // Cancel the current recording and return nothing currentRecording.cancel(true); } - + numFrames = -1; frameCursor = 0; trackers.clear(); poseFrame = null; } - + public synchronized boolean isReadyToRecord() { return server.getTrackersCount() > 0; } - + public synchronized boolean isRecording() { return numFrames > frameCursor; } - + public synchronized boolean hasRecording() { return currentRecording != null; } - + public synchronized Future getFramesAsync() { return currentRecording; } - + public synchronized PoseFrames getFrames() throws ExecutionException, InterruptedException { CompletableFuture currentRecording = this.currentRecording; return currentRecording != null ? currentRecording.get() : null; diff --git a/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java b/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java index 3a871ae34d..889b3a2e16 100644 --- a/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java +++ b/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java @@ -171,7 +171,7 @@ public boolean isComputed() { return true; } //#endregion - + @Override public int getTrackerId() { return this.trackerId; diff --git a/src/main/java/dev/slimevr/posestreamer/BVHFileStream.java b/src/main/java/dev/slimevr/posestreamer/BVHFileStream.java index 517db04b09..1b2cbbd956 100644 --- a/src/main/java/dev/slimevr/posestreamer/BVHFileStream.java +++ b/src/main/java/dev/slimevr/posestreamer/BVHFileStream.java @@ -19,167 +19,167 @@ import dev.slimevr.vr.processor.skeleton.HumanSkeleton; public class BVHFileStream extends PoseDataStream { - + private static final int LONG_MAX_VALUE_DIGITS = Long.toString(Long.MAX_VALUE).length(); private static final float OFFSET_SCALE = 100f; private static final float POSITION_SCALE = 100f; - + private long frameCount = 0; private final BufferedWriter writer; - + private long frameCountOffset; - + private float[] angleBuf = new float[3]; private Quaternion rotBuf = new Quaternion(); - + private HumanSkeleton wrappedSkeleton; private TransformNodeWrapper rootNode; - + public BVHFileStream(OutputStream outputStream) { super(outputStream); writer = new BufferedWriter(new OutputStreamWriter(outputStream), 4096); } - + public BVHFileStream(File file) throws FileNotFoundException { super(file); writer = new BufferedWriter(new OutputStreamWriter(outputStream), 4096); } - + public BVHFileStream(String file) throws FileNotFoundException { super(file); writer = new BufferedWriter(new OutputStreamWriter(outputStream), 4096); } - + private String getBufferedFrameCount(long frameCount) { String frameString = Long.toString(frameCount); int bufferCount = LONG_MAX_VALUE_DIGITS - frameString.length(); - + return bufferCount > 0 ? frameString + StringUtils.repeat(' ', bufferCount) : frameString; } - + private TransformNodeWrapper wrapSkeletonIfNew(HumanSkeleton skeleton) { TransformNodeWrapper wrapper = rootNode; - + // If the wrapped skeleton is missing or the skeleton is updated - if (wrapper == null || skeleton != wrappedSkeleton) { + if(wrapper == null || skeleton != wrappedSkeleton) { wrapper = wrapSkeleton(skeleton); } - + return wrapper; } - + private TransformNodeWrapper wrapSkeleton(HumanSkeleton skeleton) { TransformNodeWrapper wrapper = wrapSkeletonNodes(skeleton.getRootNode()); - + wrappedSkeleton = skeleton; rootNode = wrapper; - + return wrapper; } - + protected TransformNodeWrapper wrapSkeletonNodes(TransformNode rootNode) { return TransformNodeWrapper.wrapFullHierarchy(rootNode); } - + private void writeNodeHierarchy(TransformNodeWrapper node) throws IOException { writeNodeHierarchy(node, 0); } - + private void writeNodeHierarchy(TransformNodeWrapper node, int level) throws IOException { // Don't write end sites at populated nodes - if (node.children.isEmpty() && node.getParent().children.size() > 1) { + if(node.children.isEmpty() && node.getParent().children.size() > 1) { return; } - + String indentLevel = StringUtils.repeat("\t", level); String nextIndentLevel = indentLevel + "\t"; - + // Handle ends - if (node.children.isEmpty()) { + if(node.children.isEmpty()) { writer.write(indentLevel + "End Site\n"); } else { writer.write((level > 0 ? indentLevel + "JOINT " : "ROOT ") + node.getName() + "\n"); } writer.write(indentLevel + "{\n"); - + // Ignore the root offset and original root offset - if (level > 0 && node.wrappedNode.getParent() != null) { + if(level > 0 && node.wrappedNode.getParent() != null) { Vector3f offset = node.localTransform.getTranslation(); float reverseMultiplier = node.hasReversedHierarchy() ? -1 : 1; writer.write(nextIndentLevel + "OFFSET " + Float.toString(offset.getX() * OFFSET_SCALE * reverseMultiplier) + " " + Float.toString(offset.getY() * OFFSET_SCALE * reverseMultiplier) + " " + Float.toString(offset.getZ() * OFFSET_SCALE * reverseMultiplier) + "\n"); } else { writer.write(nextIndentLevel + "OFFSET 0.0 0.0 0.0\n"); } - + // Handle ends - if (!node.children.isEmpty()) { + if(!node.children.isEmpty()) { // Only give position for root - if (level > 0) { + if(level > 0) { writer.write(nextIndentLevel + "CHANNELS 3 Zrotation Xrotation Yrotation\n"); } else { writer.write(nextIndentLevel + "CHANNELS 6 Xposition Yposition Zposition Zrotation Xrotation Yrotation\n"); } - - for (TransformNodeWrapper childNode : node.children) { + + for(TransformNodeWrapper childNode : node.children) { writeNodeHierarchy(childNode, level + 1); } } - + writer.write(indentLevel + "}\n"); } - + @Override public void writeHeader(HumanSkeleton skeleton, PoseStreamer streamer) throws IOException { - if (skeleton == null) { + if(skeleton == null) { throw new NullPointerException("skeleton must not be null"); } - if (streamer == null) { + if(streamer == null) { throw new NullPointerException("streamer must not be null"); } - + writer.write("HIERARCHY\n"); writeNodeHierarchy(wrapSkeletonIfNew(skeleton)); - + writer.write("MOTION\n"); writer.write("Frames: "); - + // Get frame offset for finishing writing the file - if (outputStream instanceof FileOutputStream) { - FileOutputStream fileOutputStream = (FileOutputStream)outputStream; + if(outputStream instanceof FileOutputStream) { + FileOutputStream fileOutputStream = (FileOutputStream) outputStream; // Flush buffer to get proper offset writer.flush(); frameCountOffset = fileOutputStream.getChannel().position(); } - + writer.write(getBufferedFrameCount(frameCount) + "\n"); - + // Frame time in seconds writer.write("Frame Time: " + (streamer.frameRecordingInterval / 1000d) + "\n"); } - + // Roughly based off code from https://github.com/TrackLab/ViRe/blob/50a987eff4db31036b2ebaeb5a28983cd473f267/Assets/Scripts/BVH/BVHRecorder.cs private float[] quatToXyzAngles(Quaternion q, float[] angles) { - if (angles == null) { + if(angles == null) { angles = new float[3]; - } else if (angles.length != 3) { + } else if(angles.length != 3) { throw new IllegalArgumentException("Angles array must have three elements"); } - + float x = q.getX(); float y = q.getY(); float z = q.getZ(); float w = q.getW(); - + // Roll (X) float sinrCosp = -2f * (x * y - w * z); float cosrCosp = w * w - x * x + y * y - z * z; angles[0] = FastMath.atan2(sinrCosp, cosrCosp); - + // Pitch (Y) float sinp = 2f * (y * z + w * x); // Use 90 degrees if out of range angles[1] = FastMath.abs(sinp) >= 1f ? FastMath.copysign(FastMath.PI / 2f, sinp) : FastMath.asin(sinp); - + // Yaw (Z) float sinyCosp = -2f * (x * z - w * y); float cosyCosp = w * w - x * x - y * y + z * z; @@ -187,10 +187,10 @@ private float[] quatToXyzAngles(Quaternion q, float[] angles) { return angles; } - + private void writeNodeHierarchyRotation(TransformNodeWrapper node, Quaternion inverseRootRot) throws IOException { Transform transform = node.worldTransform; - + /* if (node.hasReversedHierarchy()) { for (TransformNodeWrapper childNode : node.children) { @@ -202,63 +202,63 @@ private void writeNodeHierarchyRotation(TransformNodeWrapper node, Quaternion in } } */ - + rotBuf = transform.getRotation(rotBuf); - + // Adjust to local rotation - if (inverseRootRot != null) { + if(inverseRootRot != null) { rotBuf = rotBuf.multLocal(inverseRootRot); } - + // Yaw (Z), roll (X), pitch (Y) (intrinsic) // angleBuf = rotBuf.toAngles(angleBuf); - + // Roll (X), pitch (Y), yaw (Z) (intrinsic) angleBuf = quatToXyzAngles(rotBuf.normalizeLocal(), angleBuf); - + // Output in order of roll (Z), pitch (X), yaw (Y) (extrinsic) writer.write(Float.toString(angleBuf[0] * FastMath.RAD_TO_DEG) + " " + Float.toString(angleBuf[1] * FastMath.RAD_TO_DEG) + " " + Float.toString(angleBuf[2] * FastMath.RAD_TO_DEG)); - + // Get inverse rotation for child local rotations - if (!node.children.isEmpty()) { + if(!node.children.isEmpty()) { Quaternion inverseRot = transform.getRotation().inverse(); - for (TransformNodeWrapper childNode : node.children) { - if (childNode.children.isEmpty()) { + for(TransformNodeWrapper childNode : node.children) { + if(childNode.children.isEmpty()) { // If it's an end node, skip continue; } - + // Add spacing writer.write(" "); writeNodeHierarchyRotation(childNode, inverseRot); } } } - + @Override public void writeFrame(HumanSkeleton skeleton) throws IOException { - if (skeleton == null) { + if(skeleton == null) { throw new NullPointerException("skeleton must not be null"); } - + TransformNodeWrapper rootNode = wrapSkeletonIfNew(skeleton); - + Vector3f rootPos = rootNode.worldTransform.getTranslation(); - + // Write root position writer.write(Float.toString(rootPos.getX() * POSITION_SCALE) + " " + Float.toString(rootPos.getY() * POSITION_SCALE) + " " + Float.toString(rootPos.getZ() * POSITION_SCALE) + " "); writeNodeHierarchyRotation(rootNode, null); - + writer.newLine(); - + frameCount++; } - + @Override public void writeFooter(HumanSkeleton skeleton) throws IOException { // Write the final frame count for files - if (outputStream instanceof FileOutputStream) { - FileOutputStream fileOutputStream = (FileOutputStream)outputStream; + if(outputStream instanceof FileOutputStream) { + FileOutputStream fileOutputStream = (FileOutputStream) outputStream; // Flush before anything else writer.flush(); // Seek to the count offset @@ -267,7 +267,7 @@ public void writeFooter(HumanSkeleton skeleton) throws IOException { writer.write(Long.toString(frameCount)); } } - + @Override public void close() throws IOException { writer.close(); diff --git a/src/main/java/dev/slimevr/posestreamer/PoseDataStream.java b/src/main/java/dev/slimevr/posestreamer/PoseDataStream.java index b92a6aa227..c849aafb07 100644 --- a/src/main/java/dev/slimevr/posestreamer/PoseDataStream.java +++ b/src/main/java/dev/slimevr/posestreamer/PoseDataStream.java @@ -9,34 +9,34 @@ import dev.slimevr.vr.processor.skeleton.HumanSkeleton; public abstract class PoseDataStream implements AutoCloseable { - + protected boolean closed = false; protected final OutputStream outputStream; - + protected PoseDataStream(OutputStream outputStream) { this.outputStream = outputStream; } - + protected PoseDataStream(File file) throws FileNotFoundException { this(new FileOutputStream(file)); } - + protected PoseDataStream(String file) throws FileNotFoundException { this(new FileOutputStream(file)); } - + public void writeHeader(HumanSkeleton skeleton, PoseStreamer streamer) throws IOException { } - + abstract void writeFrame(HumanSkeleton skeleton) throws IOException; - + public void writeFooter(HumanSkeleton skeleton) throws IOException { } - + public boolean isClosed() { return closed; } - + @Override public void close() throws IOException { outputStream.close(); diff --git a/src/main/java/dev/slimevr/posestreamer/PoseStreamer.java b/src/main/java/dev/slimevr/posestreamer/PoseStreamer.java index 7a6787f0b3..0a85b7a583 100644 --- a/src/main/java/dev/slimevr/posestreamer/PoseStreamer.java +++ b/src/main/java/dev/slimevr/posestreamer/PoseStreamer.java @@ -8,107 +8,107 @@ import io.eiren.util.logging.LogManager; public class PoseStreamer { - + protected long frameRecordingInterval = 60L; protected long nextFrameTimeMs = -1L; - + private HumanSkeleton skeleton; private PoseDataStream poseFileStream; - + protected final VRServer server; - + public PoseStreamer(VRServer server) { this.server = server; - + // Register callbacks/events server.addSkeletonUpdatedCallback(this::onSkeletonUpdated); server.addOnTick(this::onTick); } - + @VRServerThread public void onSkeletonUpdated(HumanSkeleton skeleton) { this.skeleton = skeleton; } - + @VRServerThread public void onTick() { PoseDataStream poseFileStream = this.poseFileStream; - if (poseFileStream == null) { + if(poseFileStream == null) { return; } - + HumanSkeleton skeleton = this.skeleton; - if (skeleton == null) { + if(skeleton == null) { return; } - + long curTime = System.currentTimeMillis(); - if (curTime < nextFrameTimeMs) { + if(curTime < nextFrameTimeMs) { return; } - + nextFrameTimeMs += frameRecordingInterval; - + // To prevent duplicate frames, make sure the frame time is always in the future - if (nextFrameTimeMs <= curTime) { + if(nextFrameTimeMs <= curTime) { nextFrameTimeMs = curTime + frameRecordingInterval; } // Make sure it's synchronized since this is the server thread interacting with // an unknown outside thread controlling this class - synchronized (this) { + synchronized(this) { // Make sure the stream is open before trying to write - if (poseFileStream.isClosed()) { + if(poseFileStream.isClosed()) { return; } try { poseFileStream.writeFrame(skeleton); - } catch (Exception e) { + } catch(Exception e) { // Handle any exceptions without crashing the program LogManager.log.severe("[PoseStreamer] Exception while saving frame", e); } } } - + public synchronized void setFrameInterval(long intervalMs) { if(intervalMs < 1) { throw new IllegalArgumentException("intervalMs must at least have a value of 1"); } - + this.frameRecordingInterval = intervalMs; } - + public synchronized long getFrameInterval() { return frameRecordingInterval; } - + public synchronized void setOutput(PoseDataStream poseFileStream) throws IOException { poseFileStream.writeHeader(skeleton, this); this.poseFileStream = poseFileStream; nextFrameTimeMs = -1L; // Reset the frame timing } - + public synchronized void setOutput(PoseDataStream poseFileStream, long intervalMs) throws IOException { setFrameInterval(intervalMs); setOutput(poseFileStream); } - + public synchronized PoseDataStream getOutput() { return poseFileStream; } - + public synchronized void closeOutput() throws IOException { PoseDataStream poseFileStream = this.poseFileStream; - - if (poseFileStream != null) { + + if(poseFileStream != null) { closeOutput(poseFileStream); this.poseFileStream = null; } } - + public synchronized void closeOutput(PoseDataStream poseFileStream) throws IOException { - if (poseFileStream != null) { + if(poseFileStream != null) { poseFileStream.writeFooter(skeleton); poseFileStream.close(); } diff --git a/src/main/java/dev/slimevr/posestreamer/StdBVHFileStream.java b/src/main/java/dev/slimevr/posestreamer/StdBVHFileStream.java index 01e0a5ee1b..9fa5d0ed24 100644 --- a/src/main/java/dev/slimevr/posestreamer/StdBVHFileStream.java +++ b/src/main/java/dev/slimevr/posestreamer/StdBVHFileStream.java @@ -7,15 +7,15 @@ import dev.slimevr.vr.processor.TransformNode; public class StdBVHFileStream extends BVHFileStream { - + public StdBVHFileStream(OutputStream outputStream) { super(outputStream); } - + public StdBVHFileStream(File file) throws FileNotFoundException { super(file); } - + public StdBVHFileStream(String file) throws FileNotFoundException { super(file); } @@ -23,41 +23,41 @@ public StdBVHFileStream(String file) throws FileNotFoundException { @Override protected TransformNodeWrapper wrapSkeletonNodes(TransformNode rootNode) { TransformNode newRoot = getNodeFromHierarchy(rootNode, "Hip"); - if (newRoot == null) { + if(newRoot == null) { return null; } - + TransformNodeWrapper wrappedRoot = TransformNodeWrapper.wrapHierarchyDown(newRoot); - + /* // If should wrap up hierarchy if (newRoot.getParent() != null) { // Create an extra node for full proper rotation TransformNodeWrapper spineWrapper = new TransformNodeWrapper(new TransformNode("Spine", false), true, 1); wrappedRoot.attachChild(spineWrapper); - + // Wrap up on top of the spine node TransformNodeWrapper.wrapNodeHierarchyUp(newRoot, spineWrapper); } */ - + TransformNodeWrapper.wrapNodeHierarchyUp(wrappedRoot); - + return wrappedRoot; } - + private TransformNode getNodeFromHierarchy(TransformNode node, String name) { - if (node.getName().equalsIgnoreCase(name)) { + if(node.getName().equalsIgnoreCase(name)) { return node; } - - for (TransformNode child : node.children) { + + for(TransformNode child : node.children) { TransformNode result = getNodeFromHierarchy(child, name); - if (result != null) { + if(result != null) { return result; } } - + return null; } } diff --git a/src/main/java/dev/slimevr/posestreamer/TransformNodeWrapper.java b/src/main/java/dev/slimevr/posestreamer/TransformNodeWrapper.java index 4ca22bd64e..7c1daef656 100644 --- a/src/main/java/dev/slimevr/posestreamer/TransformNodeWrapper.java +++ b/src/main/java/dev/slimevr/posestreamer/TransformNodeWrapper.java @@ -9,149 +9,149 @@ import io.eiren.util.collections.FastList; public class TransformNodeWrapper { - + public final TransformNode wrappedNode; - + protected String name; - + public final Transform localTransform; public final Transform worldTransform; - + private boolean reversedHierarchy = false; - + protected TransformNodeWrapper parent; public final List children; - + public TransformNodeWrapper(TransformNode nodeToWrap, String name, boolean reversedHierarchy, int initialChildCapacity) { this.wrappedNode = nodeToWrap; - + this.name = name; - + this.localTransform = nodeToWrap.localTransform; this.worldTransform = nodeToWrap.worldTransform; - + this.reversedHierarchy = reversedHierarchy; - + this.children = new FastList<>(initialChildCapacity); } - + public TransformNodeWrapper(TransformNode nodeToWrap, String name, int initialChildCapacity) { this(nodeToWrap, name, false, initialChildCapacity); } - + public TransformNodeWrapper(TransformNode nodeToWrap, String name) { this(nodeToWrap, name, false, 5); } - + public TransformNodeWrapper(TransformNode nodeToWrap, boolean reversedHierarchy, int initialChildCapacity) { this(nodeToWrap, nodeToWrap.getName(), reversedHierarchy, initialChildCapacity); } - + public TransformNodeWrapper(TransformNode nodeToWrap, boolean reversedHierarchy) { this(nodeToWrap, nodeToWrap.getName(), reversedHierarchy, 5); } - + public TransformNodeWrapper(TransformNode nodeToWrap, int initialChildCapacity) { this(nodeToWrap, nodeToWrap.getName(), initialChildCapacity); } - + public TransformNodeWrapper(TransformNode nodeToWrap) { this(nodeToWrap, nodeToWrap.getName()); } - + public static TransformNodeWrapper wrapFullHierarchy(TransformNode root) { return wrapNodeHierarchyUp(wrapHierarchyDown(root)); } - + public static TransformNodeWrapper wrapHierarchyDown(TransformNode root) { return wrapNodeHierarchyDown(new TransformNodeWrapper(root, root.children.size())); } - + public static TransformNodeWrapper wrapNodeHierarchyDown(TransformNodeWrapper root) { - for (TransformNode child : root.wrappedNode.children) { + for(TransformNode child : root.wrappedNode.children) { root.attachChild(wrapHierarchyDown(child)); } - + return root; } - + public static TransformNodeWrapper wrapHierarchyUp(TransformNode root) { return wrapNodeHierarchyUp(new TransformNodeWrapper(root, root.getParent() != null ? 1 : 0)); } - + public static TransformNodeWrapper wrapNodeHierarchyUp(TransformNodeWrapper root) { return wrapNodeHierarchyUp(root.wrappedNode, root); } - + public static TransformNodeWrapper wrapNodeHierarchyUp(TransformNode root, TransformNodeWrapper target) { TransformNode parent = root.getParent(); - if (parent == null) { + if(parent == null) { return target; } - + // Flip the offset for these reversed nodes TransformNodeWrapper wrapper = new TransformNodeWrapper(parent, true, (parent.getParent() != null ? 1 : 0) + Math.max(0, parent.children.size() - 1)); target.attachChild(wrapper); - + // Re-attach other children - if (parent.children.size() > 1) { - for (TransformNode child : parent.children) { + if(parent.children.size() > 1) { + for(TransformNode child : parent.children) { // Skip the original node - if (child == target.wrappedNode) { + if(child == target.wrappedNode) { continue; } - + wrapper.attachChild(wrapHierarchyDown(child)); } } - + // Continue up the hierarchy wrapNodeHierarchyUp(wrapper); // Return original node return target; } - - public boolean hasReversedHierarchy () { + + public boolean hasReversedHierarchy() { return reversedHierarchy; } - - public void setReversedHierarchy (boolean reversedHierarchy) { + + public void setReversedHierarchy(boolean reversedHierarchy) { this.reversedHierarchy = reversedHierarchy; } - - public boolean hasLocalRotation () { + + public boolean hasLocalRotation() { return wrappedNode.localRotation; } - - public Quaternion calculateLocalRotation (Quaternion relativeTo, Quaternion result) { + + public Quaternion calculateLocalRotation(Quaternion relativeTo, Quaternion result) { return calculateLocalRotationInverse(relativeTo.inverse(), result); } - - public Quaternion calculateLocalRotationInverse (Quaternion inverseRelativeTo, Quaternion result) { - if (result == null) { + + public Quaternion calculateLocalRotationInverse(Quaternion inverseRelativeTo, Quaternion result) { + if(result == null) { result = new Quaternion(); } - + return worldTransform.getRotation().mult(inverseRelativeTo, result); } - + public void attachChild(TransformNodeWrapper node) { - if (node.parent != null) { + if(node.parent != null) { throw new IllegalArgumentException("The child node must not already have a parent"); } - + this.children.add(node); node.parent = this; } - + public TransformNodeWrapper getParent() { return parent; } - + public String getName() { return name; } - + public void setName(String name) { this.name = name; } diff --git a/src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTracker.java b/src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTracker.java index 4e54e65ecd..6956c31db5 100644 --- a/src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTracker.java +++ b/src/main/java/dev/slimevr/vr/processor/ComputedHumanPoseTracker.java @@ -11,7 +11,7 @@ public class ComputedHumanPoseTracker extends ComputedTracker implements Tracker public final ComputedHumanPoseTrackerPosition skeletonPosition; protected final TrackerRole trackerRole; protected BufferedTimer timer = new BufferedTimer(1f); - + public ComputedHumanPoseTracker(int trackerId, ComputedHumanPoseTrackerPosition skeletonPosition, TrackerRole role) { super(trackerId, "human://" + skeletonPosition.name(), true, true); this.skeletonPosition = skeletonPosition; @@ -27,7 +27,7 @@ public float getTPS() { public void dataTick() { timer.update(); } - + @Override public TrackerRole getTrackerRole() { return trackerRole; diff --git a/src/main/java/dev/slimevr/vr/processor/HumanPoseProcessor.java b/src/main/java/dev/slimevr/vr/processor/HumanPoseProcessor.java index 94d4a9d16a..8cb518213a 100644 --- a/src/main/java/dev/slimevr/vr/processor/HumanPoseProcessor.java +++ b/src/main/java/dev/slimevr/vr/processor/HumanPoseProcessor.java @@ -23,7 +23,7 @@ public class HumanPoseProcessor { private final List computedTrackers = new FastList<>(); private final List> onSkeletonUpdated = new FastList<>(); private HumanSkeleton skeleton; - + public HumanPoseProcessor(VRServer server, HMDTracker hmd) { this.server = server; computedTrackers.add(new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.WAIST, TrackerRole.WAIST)); @@ -37,7 +37,7 @@ public HumanPoseProcessor(VRServer server, HMDTracker hmd) { public HumanSkeleton getSkeleton() { return skeleton; } - + @VRServerThread public void addSkeletonUpdatedCallback(Consumer consumer) { onSkeletonUpdated.add(consumer); @@ -50,13 +50,13 @@ public void setSkeletonConfig(SkeletonConfigValue key, float newLength) { if(skeleton != null) skeleton.getSkeletonConfig().setConfig(key, newLength); } - + @ThreadSafe public void resetSkeletonConfig(SkeletonConfigValue key) { if(skeleton != null) skeleton.resetSkeletonConfig(key); } - + @ThreadSafe public void resetAllSkeletonConfigs() { if(skeleton != null) @@ -67,7 +67,7 @@ public void resetAllSkeletonConfigs() { public SkeletonConfig getSkeletonConfig() { return skeleton.getSkeletonConfig(); } - + @ThreadSafe public float getSkeletonConfig(SkeletonConfigValue key) { if(skeleton != null) { @@ -80,17 +80,17 @@ public float getSkeletonConfig(SkeletonConfigValue key) { public List getComputedTrackers() { return computedTrackers; } - + @VRServerThread public void trackerAdded(Tracker tracker) { updateSekeltonModel(); } - + @VRServerThread public void trackerUpdated(Tracker tracker) { updateSekeltonModel(); } - + @VRServerThread private void updateSekeltonModel() { disconnectAllTrackers(); @@ -98,26 +98,26 @@ private void updateSekeltonModel() { for(int i = 0; i < onSkeletonUpdated.size(); ++i) onSkeletonUpdated.get(i).accept(skeleton); } - + @VRServerThread private void disconnectAllTrackers() { for(int i = 0; i < computedTrackers.size(); ++i) { computedTrackers.get(i).setStatus(TrackerStatus.DISCONNECTED); } } - + @VRServerThread public void update() { if(skeleton != null) skeleton.updatePose(); } - + @VRServerThread public void resetTrackers() { if(skeleton != null) skeleton.resetTrackersFull(); } - + @VRServerThread public void resetTrackersYaw() { if(skeleton != null) diff --git a/src/main/java/dev/slimevr/vr/processor/TransformNode.java b/src/main/java/dev/slimevr/vr/processor/TransformNode.java index 6b9b430913..d319888b46 100644 --- a/src/main/java/dev/slimevr/vr/processor/TransformNode.java +++ b/src/main/java/dev/slimevr/vr/processor/TransformNode.java @@ -26,10 +26,10 @@ public TransformNode(String name) { } public void attachChild(TransformNode node) { - if (node.parent != null) { + if(node.parent != null) { throw new IllegalArgumentException("The child node must not already have a parent"); } - + this.children.add(node); node.parent = this; } @@ -37,7 +37,7 @@ public void attachChild(TransformNode node) { public TransformNode getParent() { return parent; } - + public void update() { updateWorldTransforms(); // Call update on each frame because we have relatively few nodes for(int i = 0; i < children.size(); ++i) @@ -66,14 +66,11 @@ public void depthFirstTraversal(Consumer visitor) { public String getName() { return name; } - - public void combineWithParentGlobalRotation(Transform parent) { - worldTransform.getScale().multLocal(parent.getScale()); - worldTransform.getTranslation().multLocal(parent.getScale()); - - parent - .getRotation() - .multLocal(worldTransform.getTranslation()) - .addLocal(parent.getTranslation()); - } + + public void combineWithParentGlobalRotation(Transform parent) { + worldTransform.getScale().multLocal(parent.getScale()); + worldTransform.getTranslation().multLocal(parent.getScale()); + + parent.getRotation().multLocal(worldTransform.getTranslation()).addLocal(parent.getTranslation()); + } } 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 5c73e5796c..3df83f733e 100644 --- a/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/HumanSkeleton.java @@ -5,7 +5,7 @@ import io.eiren.util.ann.ThreadSafe; public abstract class HumanSkeleton { - + @VRServerThread public abstract void updatePose(); @@ -14,17 +14,17 @@ public abstract class HumanSkeleton { @ThreadSafe public abstract SkeletonConfig getSkeletonConfig(); - + @ThreadSafe public abstract void resetSkeletonConfig(SkeletonConfigValue config); - + @ThreadSafe public void resetAllSkeletonConfigs() { - for (SkeletonConfigValue config : SkeletonConfigValue.values) { + for(SkeletonConfigValue config : SkeletonConfigValue.values) { resetSkeletonConfig(config); } } - + @VRServerThread public abstract void resetTrackersFull(); diff --git a/src/main/java/dev/slimevr/vr/processor/skeleton/SimpleSkeleton.java b/src/main/java/dev/slimevr/vr/processor/skeleton/SimpleSkeleton.java index b25f52fdfb..c95abcf284 100644 --- a/src/main/java/dev/slimevr/vr/processor/skeleton/SimpleSkeleton.java +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/SimpleSkeleton.java @@ -22,7 +22,7 @@ public class SimpleSkeleton extends HumanSkeleton implements SkeletonConfigCallback { public static final float DEFAULT_FLOOR_OFFSET = 0.05f; - + //#region Upper body nodes (torso) protected final TransformNode hmdNode = new TransformNode("HMD", false); protected final TransformNode headNode = new TransformNode("Head", false); @@ -38,7 +38,7 @@ public class SimpleSkeleton extends HumanSkeleton implements SkeletonConfigCallb protected final TransformNode leftKneeNode = new TransformNode("Left-Knee", false); protected final TransformNode leftAnkleNode = new TransformNode("Left-Ankle", false); protected final TransformNode leftFootNode = new TransformNode("Left-Foot", false); - + protected final TransformNode rightHipNode = new TransformNode("Right-Hip", false); protected final TransformNode rightKneeNode = new TransformNode("Right-Knee", false); protected final TransformNode rightAnkleNode = new TransformNode("Right-Ankle", false); @@ -55,20 +55,20 @@ public class SimpleSkeleton extends HumanSkeleton implements SkeletonConfigCallb protected Tracker chestTracker; protected Tracker waistTracker; protected Tracker hipTracker; - + protected Tracker leftLegTracker; protected Tracker leftAnkleTracker; protected Tracker leftFootTracker; - + protected Tracker rightLegTracker; protected Tracker rightAnkleTracker; protected Tracker rightFootTracker; //#endregion - + //#region Tracker Output protected ComputedHumanPoseTracker computedChestTracker; protected ComputedHumanPoseTracker computedWaistTracker; - + protected ComputedHumanPoseTracker computedLeftKneeTracker; protected ComputedHumanPoseTracker computedLeftFootTracker; @@ -78,18 +78,18 @@ public class SimpleSkeleton extends HumanSkeleton implements SkeletonConfigCallb protected boolean extendedPelvisModel = true; protected boolean extendedKneeModel = false; - + public final SkeletonConfig skeletonConfig; - + //#region Buffers private Vector3f posBuf = new Vector3f(); - + private Quaternion rotBuf1 = new Quaternion(); private Quaternion rotBuf2 = new Quaternion(); - + protected final Vector3f hipVector = new Vector3f(); protected final Vector3f ankleVector = new Vector3f(); - + protected final Quaternion kneeRotation = new Quaternion(); //#endregion @@ -107,26 +107,26 @@ protected SimpleSkeleton(List computedTracke //#region Assemble skeleton 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 - + // Set default skeleton configuration (callback automatically sets initial offsets) skeletonConfig = new SkeletonConfig(true, this); - - if (computedTrackers != null) { + + if(computedTrackers != null) { setComputedTrackers(computedTrackers); } fillNullComputedTrackers(true); } - + public SimpleSkeleton(VRServer server, List computedTrackers) { this(computedTrackers); setTrackersFromServer(server); @@ -135,14 +135,14 @@ public SimpleSkeleton(VRServer server, List public SimpleSkeleton(List trackers, List computedTrackers) { this(computedTrackers); - - if (trackers != null) { + + if(trackers != null) { setTrackersFromList(trackers); } else { setTrackersFromList(new FastList(0)); } } - + public SimpleSkeleton(List trackers, List computedTrackers, Map configs, Map altConfigs) { // Initialize this(trackers, computedTrackers); @@ -159,51 +159,51 @@ public SimpleSkeleton(List trackers, List trackers, boolean setHmd) { - if (setHmd) { + if(setHmd) { this.hmdTracker = TrackerUtils.findTrackerForBodyPosition(trackers, TrackerPosition.HMD); } - + this.chestTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(trackers, TrackerPosition.CHEST, TrackerPosition.WAIST, TrackerPosition.HIP); this.waistTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(trackers, TrackerPosition.WAIST, TrackerPosition.CHEST, TrackerPosition.HIP); this.hipTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(trackers, TrackerPosition.HIP, TrackerPosition.WAIST, TrackerPosition.CHEST); - + this.leftLegTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(trackers, TrackerPosition.LEFT_LEG, TrackerPosition.LEFT_ANKLE, null); this.leftAnkleTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(trackers, TrackerPosition.LEFT_ANKLE, TrackerPosition.LEFT_LEG, null); this.leftFootTracker = TrackerUtils.findTrackerForBodyPosition(trackers, TrackerPosition.LEFT_FOOT); - + this.rightLegTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(trackers, TrackerPosition.RIGHT_LEG, TrackerPosition.RIGHT_ANKLE, null); this.rightAnkleTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(trackers, TrackerPosition.RIGHT_ANKLE, TrackerPosition.RIGHT_LEG, null); this.rightFootTracker = TrackerUtils.findTrackerForBodyPosition(trackers, TrackerPosition.RIGHT_FOOT); } - + 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()) { + 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; @@ -212,82 +212,82 @@ public void setComputedTracker(ComputedHumanPoseTracker tracker) { break; } } - + public void setComputedTrackers(List trackers) { for(int i = 0; i < trackers.size(); ++i) { setComputedTracker(trackers.get(i)); } } - + public void setComputedTrackersAndFillNull(List trackers, boolean onlyFillWaistAndFeet) { setComputedTrackers(trackers); fillNullComputedTrackers(onlyFillWaistAndFeet); } - + public void fillNullComputedTrackers(boolean onlyFillWaistAndFeet) { - if (computedWaistTracker == null) { + if(computedWaistTracker == null) { computedWaistTracker = new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.WAIST, TrackerRole.WAIST); computedWaistTracker.setStatus(TrackerStatus.OK); } - - if (computedLeftFootTracker == null) { + + if(computedLeftFootTracker == null) { computedLeftFootTracker = new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.LEFT_FOOT, TrackerRole.LEFT_FOOT); computedLeftFootTracker.setStatus(TrackerStatus.OK); } - - if (computedRightFootTracker == null) { + + if(computedRightFootTracker == null) { computedRightFootTracker = new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.RIGHT_FOOT, TrackerRole.RIGHT_FOOT); computedRightFootTracker.setStatus(TrackerStatus.OK); } - - if (!onlyFillWaistAndFeet) { - if (computedChestTracker == null) { + + if(!onlyFillWaistAndFeet) { + if(computedChestTracker == null) { computedChestTracker = new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.CHEST, TrackerRole.CHEST); computedChestTracker.setStatus(TrackerStatus.OK); } - - if (computedLeftKneeTracker == null) { + + if(computedLeftKneeTracker == null) { computedLeftKneeTracker = new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.LEFT_KNEE, TrackerRole.LEFT_KNEE); computedLeftKneeTracker.setStatus(TrackerStatus.OK); } - - if (computedRightKneeTracker == null) { + + if(computedRightKneeTracker == null) { computedRightKneeTracker = new ComputedHumanPoseTracker(Tracker.getNextLocalTrackerId(), ComputedHumanPoseTrackerPosition.RIGHT_KNEE, TrackerRole.RIGHT_KNEE); computedRightKneeTracker.setStatus(TrackerStatus.OK); } } } //#endregion - + //#region Get Trackers public ComputedHumanPoseTracker getComputedTracker(TrackerRole trackerRole) { - switch (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; } - + return null; } //#endregion - + //#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 @@ -296,30 +296,30 @@ public void updatePose() { hmdNode.update(); updateComputedTrackers(); } - + //#region Update the node transforms from the trackers protected void updateLocalTransforms() { //#region Pass all trackers through trackerPreUpdate Tracker hmdTracker = trackerPreUpdate(this.hmdTracker); - + Tracker chestTracker = trackerPreUpdate(this.chestTracker); Tracker waistTracker = trackerPreUpdate(this.waistTracker); Tracker hipTracker = trackerPreUpdate(this.hipTracker); - + Tracker leftLegTracker = trackerPreUpdate(this.leftLegTracker); Tracker leftAnkleTracker = trackerPreUpdate(this.leftAnkleTracker); Tracker leftFootTracker = trackerPreUpdate(this.leftFootTracker); - + Tracker rightLegTracker = trackerPreUpdate(this.rightLegTracker); Tracker rightAnkleTracker = trackerPreUpdate(this.rightAnkleTracker); Tracker rightFootTracker = trackerPreUpdate(this.rightFootTracker); //#endregion - - if (hmdTracker != null) { - if (hmdTracker.getPosition(posBuf)) { + + if(hmdTracker != null) { + if(hmdTracker.getPosition(posBuf)) { hmdNode.localTransform.setTranslation(posBuf); } - if (hmdTracker.getRotation(rotBuf1)) { + if(hmdTracker.getRotation(rotBuf1)) { hmdNode.localTransform.setRotation(rotBuf1); headNode.localTransform.setRotation(rotBuf1); } @@ -330,22 +330,22 @@ protected void updateLocalTransforms() { headNode.localTransform.setRotation(Quaternion.IDENTITY); } - if (chestTracker.getRotation(rotBuf1)) { + if(chestTracker.getRotation(rotBuf1)) { neckNode.localTransform.setRotation(rotBuf1); } - if (waistTracker.getRotation(rotBuf1)) { + if(waistTracker.getRotation(rotBuf1)) { chestNode.localTransform.setRotation(rotBuf1); } - if (hipTracker.getRotation(rotBuf1)) { + if(hipTracker.getRotation(rotBuf1)) { waistNode.localTransform.setRotation(rotBuf1); trackerWaistNode.localTransform.setRotation(rotBuf1); hipNode.localTransform.setRotation(rotBuf1); } - + // Left Leg leftLegTracker.getRotation(rotBuf1); leftAnkleTracker.getRotation(rotBuf2); - + if(extendedKneeModel) calculateKneeLimits(rotBuf1, rotBuf2, leftLegTracker.getConfidenceLevel(), leftAnkleTracker.getConfidenceLevel()); @@ -353,7 +353,7 @@ protected void updateLocalTransforms() { leftKneeNode.localTransform.setRotation(rotBuf2); leftAnkleNode.localTransform.setRotation(rotBuf2); leftFootNode.localTransform.setRotation(rotBuf2); - + if(leftFootTracker != null) { leftFootTracker.getRotation(rotBuf2); leftAnkleNode.localTransform.setRotation(rotBuf2); @@ -377,7 +377,7 @@ protected void updateLocalTransforms() { rightAnkleNode.localTransform.setRotation(rotBuf2); rightFootNode.localTransform.setRotation(rotBuf2); } - + if(extendedPelvisModel) { // Average pelvis between two legs leftHipNode.localTransform.getRotation(rotBuf1); @@ -392,7 +392,7 @@ protected void updateLocalTransforms() { } } //#endregion - + //#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) { @@ -409,7 +409,7 @@ protected void calculateKneeLimits(Quaternion hipBuf, Quaternion kneeBuf, float // 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); } @@ -431,40 +431,40 @@ public static float interpolateRadians(float factor, float start, float end) { return normalizeRad(val); } //#endregion - + //#region Update the output trackers protected void updateComputedTrackers() { - if (computedChestTracker != null) { + if(computedChestTracker != null) { computedChestTracker.position.set(chestNode.worldTransform.getTranslation()); computedChestTracker.rotation.set(neckNode.worldTransform.getRotation()); computedChestTracker.dataTick(); } - - if (computedWaistTracker != null) { + + if(computedWaistTracker != null) { computedWaistTracker.position.set(trackerWaistNode.worldTransform.getTranslation()); computedWaistTracker.rotation.set(trackerWaistNode.worldTransform.getRotation()); computedWaistTracker.dataTick(); } - if (computedLeftKneeTracker != null) { + if(computedLeftKneeTracker != null) { computedLeftKneeTracker.position.set(leftKneeNode.worldTransform.getTranslation()); computedLeftKneeTracker.rotation.set(leftHipNode.worldTransform.getRotation()); computedLeftKneeTracker.dataTick(); } - - if (computedLeftFootTracker != null) { + + if(computedLeftFootTracker != null) { computedLeftFootTracker.position.set(leftFootNode.worldTransform.getTranslation()); computedLeftFootTracker.rotation.set(leftFootNode.worldTransform.getRotation()); computedLeftFootTracker.dataTick(); } - - if (computedRightKneeTracker != null) { + + if(computedRightKneeTracker != null) { computedRightKneeTracker.position.set(rightKneeNode.worldTransform.getTranslation()); computedRightKneeTracker.rotation.set(rightHipNode.worldTransform.getRotation()); computedRightKneeTracker.dataTick(); } - if (computedRightFootTracker != null) { + if(computedRightFootTracker != null) { computedRightFootTracker.position.set(rightFootNode.worldTransform.getTranslation()); computedRightFootTracker.rotation.set(rightFootNode.worldTransform.getRotation()); computedRightFootTracker.dataTick(); @@ -478,15 +478,15 @@ protected void updateComputedTrackers() { 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) { + if(configToggle == null) { return; } - + // Cache the values of these configs - switch (configToggle) { + switch(configToggle) { case EXTENDED_PELVIS_MODEL: extendedPelvisModel = newValue; break; @@ -495,14 +495,14 @@ public void updateToggleState(SkeletonConfigToggle configToggle, boolean newValu break; } } - + @Override public void updateNodeOffset(SkeletonNodeOffset nodeOffset, Vector3f offset) { - if (nodeOffset == null) { + if(nodeOffset == null) { return; } - - switch (nodeOffset) { + + switch(nodeOffset) { case HEAD: headNode.localTransform.setTranslation(offset); break; @@ -521,14 +521,14 @@ public void updateNodeOffset(SkeletonNodeOffset nodeOffset, Vector3f offset) { 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); @@ -543,7 +543,7 @@ public void updateNodeOffset(SkeletonNodeOffset nodeOffset, Vector3f offset) { break; } } - + public void updatePoseAffectedByConfig(SkeletonConfigValue config) { switch(config) { case HEAD: @@ -597,23 +597,23 @@ public void updatePoseAffectedByConfig(SkeletonConfigValue config) { } } //#endregion - + @Override public TransformNode getRootNode() { return hmdNode; } - + @Override public SkeletonConfig getSkeletonConfig() { return skeletonConfig; } - + @Override public void resetSkeletonConfig(SkeletonConfigValue config) { - if (config == null) { + if(config == null) { return; } - + Vector3f vec; float height; switch(config) { @@ -629,8 +629,7 @@ public void resetSkeletonConfig(SkeletonConfigValue config) { 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) / 2.0f) - skeletonConfig.getConfig(SkeletonConfigValue.NECK)); - } - else// if floor level is incorrect + } else// if floor level is incorrect { skeletonConfig.setConfig(SkeletonConfigValue.TORSO, null); } @@ -659,8 +658,7 @@ public void resetSkeletonConfig(SkeletonConfigValue config) { 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) - DEFAULT_FLOOR_OFFSET); - } - else //if floor level is incorrect + } else //if floor level is incorrect { skeletonConfig.setConfig(SkeletonConfigValue.LEGS_LENGTH, null); } @@ -671,25 +669,25 @@ public void resetSkeletonConfig(SkeletonConfigValue config) { break; } } - + @Override public void resetTrackersFull() { //#region Pass all trackers through trackerPreUpdate Tracker hmdTracker = trackerPreUpdate(this.hmdTracker); - + Tracker chestTracker = trackerPreUpdate(this.chestTracker); Tracker waistTracker = trackerPreUpdate(this.waistTracker); Tracker hipTracker = trackerPreUpdate(this.hipTracker); - + Tracker leftLegTracker = trackerPreUpdate(this.leftLegTracker); Tracker leftAnkleTracker = trackerPreUpdate(this.leftAnkleTracker); Tracker leftFootTracker = trackerPreUpdate(this.leftFootTracker); - + Tracker rightLegTracker = trackerPreUpdate(this.rightLegTracker); Tracker rightAnkleTracker = trackerPreUpdate(this.rightAnkleTracker); Tracker rightFootTracker = trackerPreUpdate(this.rightFootTracker); //#endregion - + // Each tracker uses the tracker before it to adjust itself, // so trackers that don't need adjustments could be used too Quaternion referenceRotation = new Quaternion(); @@ -700,7 +698,7 @@ public void resetTrackersFull() { waistTracker.resetFull(referenceRotation); waistTracker.getRotation(referenceRotation); - + hipTracker.resetFull(referenceRotation); hipTracker.getRotation(referenceRotation); @@ -714,7 +712,7 @@ public void resetTrackersFull() { if(leftFootTracker != null) { leftFootTracker.resetFull(referenceRotation); } - + rightLegTracker.getRotation(referenceRotation); rightAnkleTracker.resetFull(referenceRotation); @@ -724,26 +722,26 @@ public void resetTrackersFull() { rightFootTracker.resetFull(referenceRotation); } } - + @Override @VRServerThread public void resetTrackersYaw() { //#region Pass all trackers through trackerPreUpdate Tracker hmdTracker = trackerPreUpdate(this.hmdTracker); - + Tracker chestTracker = trackerPreUpdate(this.chestTracker); Tracker waistTracker = trackerPreUpdate(this.waistTracker); Tracker hipTracker = trackerPreUpdate(this.hipTracker); - + Tracker leftLegTracker = trackerPreUpdate(this.leftLegTracker); Tracker leftAnkleTracker = trackerPreUpdate(this.leftAnkleTracker); Tracker leftFootTracker = trackerPreUpdate(this.leftFootTracker); - + Tracker rightLegTracker = trackerPreUpdate(this.rightLegTracker); Tracker rightAnkleTracker = trackerPreUpdate(this.rightAnkleTracker); Tracker rightFootTracker = trackerPreUpdate(this.rightFootTracker); //#endregion - + // Each tracker uses the tracker before it to adjust itself, // so trackers that don't need adjustments could be used too Quaternion referenceRotation = new Quaternion(); @@ -754,7 +752,7 @@ public void resetTrackersYaw() { waistTracker.resetYaw(referenceRotation); waistTracker.getRotation(referenceRotation); - + hipTracker.resetYaw(referenceRotation); hipTracker.getRotation(referenceRotation); @@ -768,7 +766,7 @@ public void resetTrackersYaw() { if(leftFootTracker != null) { leftFootTracker.resetYaw(referenceRotation); } - + rightLegTracker.getRotation(referenceRotation); rightAnkleTracker.resetYaw(referenceRotation); 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 97742afc1b..96547dd844 100644 --- a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfig.java +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfig.java @@ -9,204 +9,204 @@ import io.eiren.yaml.YamlFile; public class SkeletonConfig { - + protected final EnumMap configs = new EnumMap(SkeletonConfigValue.class); protected final EnumMap toggles = new EnumMap(SkeletonConfigToggle.class); protected final EnumMap nodeOffsets = new EnumMap(SkeletonNodeOffset.class); - + protected final boolean autoUpdateOffsets; protected final SkeletonConfigCallback callback; - + public SkeletonConfig(boolean autoUpdateOffsets) { this.autoUpdateOffsets = autoUpdateOffsets; this.callback = null; - + callCallbackOnAll(true); - - if (autoUpdateOffsets) { + + if(autoUpdateOffsets) { computeAllNodeOffsets(); } } - + public SkeletonConfig(boolean autoUpdateOffsets, SkeletonConfigCallback callback) { this.autoUpdateOffsets = autoUpdateOffsets; this.callback = callback; - + callCallbackOnAll(true); - - if (autoUpdateOffsets) { + + if(autoUpdateOffsets) { computeAllNodeOffsets(); } } - + public SkeletonConfig(Map configs, Map toggles, boolean autoUpdateOffsets, SkeletonConfigCallback callback) { this.autoUpdateOffsets = autoUpdateOffsets; this.callback = callback; setConfigs(configs, toggles); - + callCallbackOnAll(true); } - + public SkeletonConfig(Map configs, Map toggles, boolean autoUpdateOffsets) { this(configs, toggles, autoUpdateOffsets, null); } - + public SkeletonConfig(SkeletonConfig skeletonConfig, boolean autoUpdateOffsets, SkeletonConfigCallback callback) { this.autoUpdateOffsets = autoUpdateOffsets; this.callback = callback; setConfigs(skeletonConfig); - + callCallbackOnAll(true); } - + public SkeletonConfig(SkeletonConfig skeletonConfig, boolean autoUpdateOffsets) { this(skeletonConfig, autoUpdateOffsets, null); } - + private void callCallbackOnAll(boolean defaultOnly) { - if (callback == null) { + if(callback == null) { return; } - - for (SkeletonConfigValue config : SkeletonConfigValue.values) { + + for(SkeletonConfigValue config : SkeletonConfigValue.values) { try { Float val = configs.get(config); - if (!defaultOnly || val == null) { + if(!defaultOnly || val == null) { callback.updateConfigState(config, val == null ? config.defaultValue : val); } - } catch (Exception e) { + } catch(Exception e) { LogManager.log.severe("[SkeletonConfig] Exception while calling callback", e); } } - - for (SkeletonConfigToggle config : SkeletonConfigToggle.values) { + + for(SkeletonConfigToggle config : SkeletonConfigToggle.values) { try { Boolean val = toggles.get(config); - if (!defaultOnly || val == null) { + if(!defaultOnly || val == null) { callback.updateToggleState(config, val == null ? config.defaultValue : val); } - } catch (Exception e) { + } catch(Exception e) { LogManager.log.severe("[SkeletonConfig] Exception while calling callback", e); } } } - + public Float setConfig(SkeletonConfigValue config, Float newValue, boolean computeOffsets) { Float origVal = newValue != null ? configs.put(config, newValue) : configs.remove(config); - + // Re-compute the affected offsets - if (computeOffsets && autoUpdateOffsets && config.affectedOffsets != null) { - for (SkeletonNodeOffset offset : config.affectedOffsets) { + if(computeOffsets && autoUpdateOffsets && config.affectedOffsets != null) { + for(SkeletonNodeOffset offset : config.affectedOffsets) { computeNodeOffset(offset); } } - - if (callback != null) { + + if(callback != null) { try { callback.updateConfigState(config, newValue != null ? newValue : config.defaultValue); - } catch (Exception e) { + } catch(Exception e) { LogManager.log.severe("[SkeletonConfig] Exception while calling callback", e); } } - + return origVal; } - + public Float setConfig(SkeletonConfigValue config, Float newValue) { return setConfig(config, newValue, true); } - + public Float setConfig(String config, Float newValue) { return setConfig(SkeletonConfigValue.getByStringValue(config), newValue); } - + public float getConfig(SkeletonConfigValue config) { - if (config == null) { + if(config == null) { return 0f; } - + // IMPORTANT!! This null check is necessary, getOrDefault seems to randomly decide to return null at times, so this is a secondary check Float val = configs.getOrDefault(config, config.defaultValue); return val != null ? val : config.defaultValue; } - + public float getConfig(String config) { - if (config == null) { + if(config == null) { return 0f; } return getConfig(SkeletonConfigValue.getByStringValue(config)); } - + public Boolean setToggle(SkeletonConfigToggle config, Boolean newValue) { Boolean origVal = newValue != null ? toggles.put(config, newValue) : toggles.remove(config); - - if (callback != null) { + + if(callback != null) { try { callback.updateToggleState(config, newValue != null ? newValue : config.defaultValue); - } catch (Exception e) { + } catch(Exception e) { LogManager.log.severe("[SkeletonConfig] Exception while calling callback", e); } } - + return origVal; } - + public Boolean setToggle(String config, Boolean newValue) { return setToggle(SkeletonConfigToggle.getByStringValue(config), newValue); } - + public boolean getToggle(SkeletonConfigToggle config) { - if (config == null) { + if(config == null) { return false; } - + // IMPORTANT!! This null check is necessary, getOrDefault seems to randomly decide to return null at times, so this is a secondary check Boolean val = toggles.getOrDefault(config, config.defaultValue); return val != null ? val : config.defaultValue; } - + public boolean getToggle(String config) { - if (config == null) { + if(config == null) { return false; } - + return getToggle(SkeletonConfigToggle.getByStringValue(config)); } - + protected void setNodeOffset(SkeletonNodeOffset nodeOffset, float x, float y, float z) { Vector3f offset = nodeOffsets.get(nodeOffset); - - if (offset == null) { + + if(offset == null) { offset = new Vector3f(x, y, z); nodeOffsets.put(nodeOffset, offset); } else { offset.set(x, y, z); } - - if (callback != null) { + + if(callback != null) { try { callback.updateNodeOffset(nodeOffset, offset); - } catch (Exception e) { + } catch(Exception e) { LogManager.log.severe("[SkeletonConfig] Exception while calling callback", e); } } } - + protected void setNodeOffset(SkeletonNodeOffset nodeOffset, Vector3f offset) { - if (offset == null) { + if(offset == null) { setNodeOffset(nodeOffset, 0f, 0f, 0f); return; } - + setNodeOffset(nodeOffset, offset.x, offset.y, offset.z); } - + public Vector3f getNodeOffset(SkeletonNodeOffset nodeOffset) { return nodeOffsets.getOrDefault(nodeOffset, Vector3f.ZERO); } - + public void computeNodeOffset(SkeletonNodeOffset nodeOffset) { - switch (nodeOffset) { + switch(nodeOffset) { case HEAD: setNodeOffset(nodeOffset, 0, 0, getConfig(SkeletonConfigValue.HEAD)); break; @@ -225,14 +225,14 @@ public void computeNodeOffset(SkeletonNodeOffset nodeOffset) { case HIP_TRACKER: setNodeOffset(nodeOffset, 0, getConfig(SkeletonConfigValue.HIP_OFFSET), 0); break; - + case LEFT_HIP: setNodeOffset(nodeOffset, -getConfig(SkeletonConfigValue.HIPS_WIDTH) / 2f, 0, 0); break; case RIGHT_HIP: setNodeOffset(nodeOffset, getConfig(SkeletonConfigValue.HIPS_WIDTH) / 2f, 0, 0); break; - + case KNEE: setNodeOffset(nodeOffset, 0, -(getConfig(SkeletonConfigValue.LEGS_LENGTH) - getConfig(SkeletonConfigValue.KNEE_HEIGHT)), 0); break; @@ -244,53 +244,53 @@ public void computeNodeOffset(SkeletonNodeOffset nodeOffset) { break; } } - + public void computeAllNodeOffsets() { - for (SkeletonNodeOffset offset : SkeletonNodeOffset.values) { + for(SkeletonNodeOffset offset : SkeletonNodeOffset.values) { computeNodeOffset(offset); } } - + public void setConfigs(Map configs, Map toggles) { - if (configs != null) { + if(configs != null) { configs.forEach((key, value) -> { // Do not recalculate the offsets, these are done in bulk at the end setConfig(key, value, false); }); } - if (toggles != null) { + if(toggles != null) { toggles.forEach(this::setToggle); } - - if (autoUpdateOffsets) { + + if(autoUpdateOffsets) { computeAllNodeOffsets(); } } - + public void setStringConfigs(Map configs, Map toggles) { - if (configs != null) { + if(configs != null) { configs.forEach((key, value) -> { // Do not recalculate the offsets, these are done in bulk at the end setConfig(SkeletonConfigValue.getByStringValue(key), value, false); }); } - if (toggles != null) { + if(toggles != null) { toggles.forEach((key, value) -> { setToggle(SkeletonConfigToggle.getByStringValue(key), value); }); } - - if (autoUpdateOffsets) { + + if(autoUpdateOffsets) { computeAllNodeOffsets(); } } - + public void setConfigs(SkeletonConfig skeletonConfig) { setConfigs(skeletonConfig.configs, skeletonConfig.toggles); } - + //#region Cast utilities for config reading private static Float castFloat(Object o) { if(o == null) { @@ -309,7 +309,7 @@ private static Float castFloat(Object o) { return null; } } - + private static Boolean castBoolean(Object o) { if(o == null) { return null; @@ -320,46 +320,46 @@ private static Boolean castBoolean(Object o) { } } //#endregion - + public void loadFromConfig(YamlFile config) { - for (SkeletonConfigValue configValue : SkeletonConfigValue.values) { + for(SkeletonConfigValue configValue : SkeletonConfigValue.values) { Float val = castFloat(config.getProperty(configValue.configKey)); - if (val != null) { + if(val != null) { // Do not recalculate the offsets, these are done in bulk at the end setConfig(configValue, val, false); } } - - for (SkeletonConfigToggle configValue : SkeletonConfigToggle.values) { + + for(SkeletonConfigToggle configValue : SkeletonConfigToggle.values) { Boolean val = castBoolean(config.getProperty(configValue.configKey)); - if (val != null) { + if(val != null) { setToggle(configValue, val); } } - - if (autoUpdateOffsets) { + + if(autoUpdateOffsets) { computeAllNodeOffsets(); } } - + public void saveToConfig(YamlFile config) { // Write all possible values, this keeps configs consistent even if defaults were changed - for (SkeletonConfigValue value : SkeletonConfigValue.values) { + for(SkeletonConfigValue value : SkeletonConfigValue.values) { config.setProperty(value.configKey, getConfig(value)); } - - for (SkeletonConfigToggle value : SkeletonConfigToggle.values) { + + for(SkeletonConfigToggle value : SkeletonConfigToggle.values) { config.setProperty(value.configKey, getToggle(value)); } } - + public void resetConfigs() { configs.clear(); toggles.clear(); - + callCallbackOnAll(false); - - if (autoUpdateOffsets) { + + if(autoUpdateOffsets) { computeAllNodeOffsets(); } } 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 780534fc89..4d7bf4f774 100644 --- a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigCallback.java +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigCallback.java @@ -3,7 +3,10 @@ import com.jme3.math.Vector3f; public interface SkeletonConfigCallback { + public void updateConfigState(SkeletonConfigValue config, float newValue); + public void updateToggleState(SkeletonConfigToggle configToggle, boolean newValue); + public void updateNodeOffset(SkeletonNodeOffset nodeOffset, Vector3f offset); } diff --git a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigToggle.java b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigToggle.java index e6e3160526..864f1bc1ea 100644 --- a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigToggle.java +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigToggle.java @@ -8,21 +8,21 @@ public enum SkeletonConfigToggle { EXTENDED_PELVIS_MODEL("Extended pelvis model", "extendedPelvis", true), EXTENDED_KNEE_MODEL("Extended knee model", "extendedKnee", false), ; - + private static final String CONFIG_PREFIX = "body.model."; public final String stringVal; public final String configKey; - + public final boolean defaultValue; - + public static final SkeletonConfigToggle[] values = values(); private static final Map byStringVal = new HashMap<>(); private SkeletonConfigToggle(String stringVal, String configKey, boolean defaultValue) { this.stringVal = stringVal; this.configKey = CONFIG_PREFIX + configKey; - + this.defaultValue = defaultValue; } @@ -31,7 +31,7 @@ public static SkeletonConfigToggle getByStringValue(String stringVal) { } static { - for (SkeletonConfigToggle configVal : values()) { + for(SkeletonConfigToggle configVal : values()) { byStringVal.put(configVal.stringVal.toLowerCase(), configVal); } } 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 8da0598b5d..443e7e0ccf 100644 --- a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigValue.java +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonConfigValue.java @@ -5,29 +5,29 @@ public enum SkeletonConfigValue { - HEAD("Head", "headShift", "Head shift", 0.1f, new SkeletonNodeOffset[] { SkeletonNodeOffset.HEAD }), - NECK("Neck", "neckLength", "Neck length", 0.1f, new SkeletonNodeOffset[] { SkeletonNodeOffset.NECK }), - TORSO("Torso", "torsoLength", "Torso length", 0.64f, new SkeletonNodeOffset[] { SkeletonNodeOffset.WAIST }), - CHEST("Chest", "chestDistance", "Chest distance", 0.32f, new SkeletonNodeOffset[] { SkeletonNodeOffset.CHEST, SkeletonNodeOffset.WAIST }), - WAIST("Waist", "waistDistance", "Waist distance", 0.05f, new SkeletonNodeOffset[] { SkeletonNodeOffset.WAIST, SkeletonNodeOffset.HIP }), - HIP_OFFSET("Hip offset", "hipOffset", "Hip offset", 0.0f, new SkeletonNodeOffset[] { SkeletonNodeOffset.HIP_TRACKER }), - HIPS_WIDTH("Hips width", "hipsWidth", "Hips width", 0.3f, new SkeletonNodeOffset[] { SkeletonNodeOffset.LEFT_HIP, SkeletonNodeOffset.RIGHT_HIP }), - LEGS_LENGTH("Legs length", "legsLength", "Legs length", 0.86f, new SkeletonNodeOffset[] { SkeletonNodeOffset.KNEE }), - KNEE_HEIGHT("Knee height", "kneeHeight", "Knee height", 0.43f, new SkeletonNodeOffset[] { SkeletonNodeOffset.KNEE, SkeletonNodeOffset.ANKLE }), - FOOT_LENGTH("Foot length", "footLength", "Foot length", 0.05f, new SkeletonNodeOffset[] { SkeletonNodeOffset.FOOT }), - FOOT_OFFSET("Foot offset", "footOffset", "Foot offset", 0.0f, new SkeletonNodeOffset[] { SkeletonNodeOffset.ANKLE }), + HEAD("Head", "headShift", "Head shift", 0.1f, new SkeletonNodeOffset[]{SkeletonNodeOffset.HEAD}), + NECK("Neck", "neckLength", "Neck length", 0.1f, new SkeletonNodeOffset[]{SkeletonNodeOffset.NECK}), + TORSO("Torso", "torsoLength", "Torso length", 0.64f, new SkeletonNodeOffset[]{SkeletonNodeOffset.WAIST}), + CHEST("Chest", "chestDistance", "Chest distance", 0.32f, new SkeletonNodeOffset[]{SkeletonNodeOffset.CHEST, SkeletonNodeOffset.WAIST}), + WAIST("Waist", "waistDistance", "Waist distance", 0.05f, new SkeletonNodeOffset[]{SkeletonNodeOffset.WAIST, SkeletonNodeOffset.HIP}), + HIP_OFFSET("Hip offset", "hipOffset", "Hip offset", 0.0f, new SkeletonNodeOffset[]{SkeletonNodeOffset.HIP_TRACKER}), + HIPS_WIDTH("Hips width", "hipsWidth", "Hips width", 0.3f, new SkeletonNodeOffset[]{SkeletonNodeOffset.LEFT_HIP, SkeletonNodeOffset.RIGHT_HIP}), + LEGS_LENGTH("Legs length", "legsLength", "Legs length", 0.86f, new SkeletonNodeOffset[]{SkeletonNodeOffset.KNEE}), + KNEE_HEIGHT("Knee height", "kneeHeight", "Knee height", 0.43f, new SkeletonNodeOffset[]{SkeletonNodeOffset.KNEE, SkeletonNodeOffset.ANKLE}), + FOOT_LENGTH("Foot length", "footLength", "Foot length", 0.05f, new SkeletonNodeOffset[]{SkeletonNodeOffset.FOOT}), + FOOT_OFFSET("Foot offset", "footOffset", "Foot offset", 0.0f, new SkeletonNodeOffset[]{SkeletonNodeOffset.ANKLE}), ; - + private static final String CONFIG_PREFIX = "body."; public final String stringVal; public final String configKey; public final String label; - + public final float defaultValue; - + public final SkeletonNodeOffset[] affectedOffsets; - + public static final SkeletonConfigValue[] values = values(); private static final Map byStringVal = new HashMap<>(); @@ -35,9 +35,9 @@ private SkeletonConfigValue(String stringVal, String configKey, String label, fl this.stringVal = stringVal; this.configKey = CONFIG_PREFIX + configKey; this.label = label; - + this.defaultValue = defaultValue; - + this.affectedOffsets = affectedOffsets == null ? new SkeletonNodeOffset[0] : affectedOffsets; } @@ -46,7 +46,7 @@ public static SkeletonConfigValue getByStringValue(String stringVal) { } static { - for (SkeletonConfigValue configVal : values()) { + for(SkeletonConfigValue configVal : values()) { byStringVal.put(configVal.stringVal.toLowerCase(), configVal); } } diff --git a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonNodeOffset.java b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonNodeOffset.java index 45ed9fe926..6ed46820fd 100644 --- a/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonNodeOffset.java +++ b/src/main/java/dev/slimevr/vr/processor/skeleton/SkeletonNodeOffset.java @@ -14,6 +14,6 @@ public enum SkeletonNodeOffset { ANKLE, FOOT, ; - + public static final SkeletonNodeOffset[] values = values(); }