diff --git a/src/main/java/dev/slimevr/posestreamer/BVHFileStream.java b/src/main/java/dev/slimevr/posestreamer/BVHFileStream.java index 1dd41ea2d..4adfcecc0 100644 --- a/src/main/java/dev/slimevr/posestreamer/BVHFileStream.java +++ b/src/main/java/dev/slimevr/posestreamer/BVHFileStream.java @@ -19,7 +19,7 @@ import io.eiren.vr.processor.TransformNode; public class BVHFileStream extends PoseDataStream { private static final int LONG_MAX_VALUE_DIGITS = Long.toString(Long.MAX_VALUE).length(); - private static final float POS_SCALE = 10f; + private static final float POS_SCALE = 100f; private long frameCount = 0; private final BufferedWriter writer; @@ -75,17 +75,7 @@ public class BVHFileStream extends PoseDataStream { } protected TransformNodeWrapper wrapSkeletonNodes(TransformNode rootNode) { - return wrapNodeHierarchy(rootNode); - } - - protected TransformNodeWrapper wrapNodeHierarchy(TransformNode node) { - TransformNodeWrapper wrapper = new TransformNodeWrapper(node); - - for (TransformNode child : node.children) { - wrapper.attachChild(wrapNodeHierarchy(child)); - } - - return wrapper; + return TransformNodeWrapper.wrapFullHierarchy(rootNode); } private void writeNodeHierarchy(TransformNodeWrapper node) throws IOException { @@ -112,7 +102,8 @@ public class BVHFileStream extends PoseDataStream { // Ignore the root offset and original root offset if (level > 0 && node.wrappedNode.getParent() != null) { Vector3f offset = node.localTransform.getTranslation(); - writer.write(nextIndentLevel + "OFFSET " + Float.toString(offset.getX() * POS_SCALE) + " " + Float.toString(offset.getY() * POS_SCALE * (offset.getY() != 0 && node.flippedOffset ? -1 : 1)) + " " + Float.toString(offset.getZ() * POS_SCALE) + "\n"); + float reverseMultiplier = node.reversedHierarchy ? -1 : 1; + writer.write(nextIndentLevel + "OFFSET " + Float.toString(offset.getX() * POS_SCALE * reverseMultiplier) + " " + Float.toString(offset.getY() * POS_SCALE * reverseMultiplier) + " " + Float.toString(offset.getZ() * POS_SCALE * reverseMultiplier) + "\n"); } else { writer.write(nextIndentLevel + "OFFSET 0.0 0.0 0.0\n"); } diff --git a/src/main/java/dev/slimevr/posestreamer/StdBVHFileStream.java b/src/main/java/dev/slimevr/posestreamer/StdBVHFileStream.java index c3914484e..4436bd1ec 100644 --- a/src/main/java/dev/slimevr/posestreamer/StdBVHFileStream.java +++ b/src/main/java/dev/slimevr/posestreamer/StdBVHFileStream.java @@ -22,50 +22,7 @@ public class StdBVHFileStream extends BVHFileStream { @Override protected TransformNodeWrapper wrapSkeletonNodes(TransformNode rootNode) { - TransformNode waistNode = getNodeFromHierarchy(rootNode, "Waist"); - - TransformNodeWrapper waistWrapper = wrapNodeHierarchy(waistNode); - - addReverseWrappedNodeHierarchy(waistWrapper); - - return waistWrapper; - } - - protected void addReverseWrappedNodeHierarchy(TransformNodeWrapper node) { - TransformNode parent = node.wrappedNode.getParent(); - if (parent == null) { - return; - } - - // Flip the offset for these reversed nodes - TransformNodeWrapper wrapper = new TransformNodeWrapper(parent, true); - node.attachChild(wrapper); - - // Re-attach other children - if (parent.children.size() > 1) { - for (TransformNode child : parent.children) { - // Skip the original node - if (child == node.wrappedNode) { - continue; - } - - wrapper.attachChild(wrapNodeHierarchy(child)); - } - } - - // Continue up the hierarchy - addReverseWrappedNodeHierarchy(wrapper); - } - - @Override - protected TransformNodeWrapper wrapNodeHierarchy(TransformNode node) { - TransformNodeWrapper wrapper = new TransformNodeWrapper(node); - - for (TransformNode child : node.children) { - wrapper.attachChild(wrapNodeHierarchy(child)); - } - - return wrapper; + return TransformNodeWrapper.wrapFullHierarchy(getNodeFromHierarchy(rootNode, "Waist")); } private TransformNode getNodeFromHierarchy(TransformNode node, String name) { diff --git a/src/main/java/dev/slimevr/posestreamer/TransformNodeWrapper.java b/src/main/java/dev/slimevr/posestreamer/TransformNodeWrapper.java index d2c39bba7..07538c529 100644 --- a/src/main/java/dev/slimevr/posestreamer/TransformNodeWrapper.java +++ b/src/main/java/dev/slimevr/posestreamer/TransformNodeWrapper.java @@ -2,8 +2,11 @@ package dev.slimevr.posestreamer; import java.util.List; +import com.jme3.math.Quaternion; import com.jme3.math.Transform; +import org.apache.commons.lang3.NotImplementedException; + import io.eiren.util.collections.FastList; import io.eiren.vr.processor.TransformNode; @@ -16,12 +19,12 @@ public class TransformNodeWrapper { public final Transform localTransform; public final Transform worldTransform; - public final boolean flippedOffset; + public final boolean reversedHierarchy; protected TransformNodeWrapper parent; - public final List children = new FastList<>(); + public final List children; - public TransformNodeWrapper(TransformNode nodeToWrap, String name, boolean flippedOffset) { + public TransformNodeWrapper(TransformNode nodeToWrap, String name, boolean reversedHierarchy, int initialChildCapacity) { this.wrappedNode = nodeToWrap; this.name = name; @@ -29,25 +32,91 @@ public class TransformNodeWrapper { this.localTransform = nodeToWrap.localTransform; this.worldTransform = nodeToWrap.worldTransform; - this.flippedOffset = flippedOffset; + 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); + this(nodeToWrap, name, false, 5); + } + + public TransformNodeWrapper(TransformNode nodeToWrap, boolean flippedOffset, int initialChildCapacity) { + this(nodeToWrap, nodeToWrap.getName(), flippedOffset, initialChildCapacity); } public TransformNodeWrapper(TransformNode nodeToWrap, boolean flippedOffset) { - this(nodeToWrap, nodeToWrap.getName(), flippedOffset); + this(nodeToWrap, nodeToWrap.getName(), flippedOffset, 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) { + 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) { + TransformNode parent = root.wrappedNode.getParent(); + if (parent == null) { + return root; + } + + // 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)); + root.attachChild(wrapper); + + // Re-attach other children + if (parent.children.size() > 1) { + for (TransformNode child : parent.children) { + // Skip the original node + if (child == root.wrappedNode) { + continue; + } + + wrapper.attachChild(wrapHierarchyDown(child)); + } + } + + // Continue up the hierarchy + wrapNodeHierarchyUp(wrapper); + // Return original node + return root; + } + public boolean hasLocalRotation () { return wrappedNode.localRotation; } + public Quaternion calculateLocalRotation () { + throw new NotImplementedException("Local rotation calculation is not yet implemented"); + } + public void attachChild(TransformNodeWrapper node) { if (node.parent != null) { throw new IllegalArgumentException("The child node must not already have a parent"); @@ -64,4 +133,8 @@ public class TransformNodeWrapper { public String getName() { return name; } + + public void setName(String name) { + this.name = name; + } }