package io.eiren.vr.processor; import java.util.HashMap; import java.util.List; import java.util.Map; import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; import io.eiren.util.ann.VRServerThread; import io.eiren.vr.VRServer; import io.eiren.vr.trackers.HMDTracker; import io.eiren.vr.trackers.Tracker; import io.eiren.vr.trackers.TrackerPosition; import io.eiren.vr.trackers.TrackerStatus; import io.eiren.vr.trackers.TrackerUtils; public class HumanSkeletonWithWaist extends HumanSkeleton { public static final float HEAD_SHIFT_DEFAULT = 0.1f; public static final float NECK_LENGTH_DEFAULT = 0.1f; protected final Map configMap = new HashMap<>(); protected final VRServer server; protected final float[] waistAngles = new float[3]; protected final Quaternion qBuf = new Quaternion(); protected final Vector3f vBuf = new Vector3f(); protected final Tracker waistTracker; protected final Tracker chestTracker; protected final Tracker hipTracker; protected final HMDTracker hmdTracker; protected final ComputedHumanPoseTracker computedWaistTracker; protected final ComputedHumanPoseTracker computedChestTracker; protected final TransformNode hmdNode = new TransformNode("HMD", false); protected final TransformNode headNode = new TransformNode("Head", false); protected final TransformNode neckNode = new TransformNode("Neck", false); protected final TransformNode waistNode = new TransformNode("Waist", false); protected final TransformNode chestNode = new TransformNode("Chest", false); protected final TransformNode trackerWaistNode = new TransformNode("Waist-Tracker", false); protected final TransformNode hipNode = new TransformNode("Hip", false); protected float chestDistance = 0.35f; /** * Distance from eyes to waist */ protected float waistDistance = 0.6f; /** * Distance from waist to hip */ protected float hipDistance = 0.1f; /** * Distance from eyes to waist, defines reported * tracker position, if you want to move resulting * tracker up or down from actual waist */ protected float trackerWaistDistance = 0.0f; /** * Distance from eyes to the base of the neck */ protected float neckLength = NECK_LENGTH_DEFAULT; /** * Distance from eyes to ear */ protected float headShift = HEAD_SHIFT_DEFAULT; public HumanSkeletonWithWaist(VRServer server, List computedTrackers) { List allTrackers = server.getAllTrackers(); this.waistTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(allTrackers, TrackerPosition.WAIST, TrackerPosition.CHEST, TrackerPosition.HIP); this.chestTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(allTrackers, TrackerPosition.CHEST, TrackerPosition.WAIST, TrackerPosition.HIP); this.hipTracker = TrackerUtils.findTrackerForBodyPositionOrEmpty(allTrackers, TrackerPosition.HIP, TrackerPosition.WAIST, TrackerPosition.CHEST); this.hmdTracker = server.hmdTracker; this.server = server; ComputedHumanPoseTracker cwt = null; ComputedHumanPoseTracker cct = null; for(int i = 0; i < computedTrackers.size(); ++i) { ComputedHumanPoseTracker t = computedTrackers.get(i); if(t.skeletonPosition == ComputedHumanPoseTrackerPosition.WAIST) cwt = t; if(t.skeletonPosition == ComputedHumanPoseTrackerPosition.CHEST) cct = t; } computedWaistTracker = cwt; computedChestTracker = cct; cwt.setStatus(TrackerStatus.OK); headShift = server.config.getFloat("body.headShift", headShift); neckLength = server.config.getFloat("body.neckLength", neckLength); chestDistance = server.config.getFloat("body.chestDistance", chestDistance); waistDistance = server.config.getFloat("body.waistDistance", waistDistance); hipDistance = server.config.getFloat("body.hipDistance", hipDistance); trackerWaistDistance = server.config.getFloat("body.trackerWaistDistance", trackerWaistDistance); // Build skeleton hmdNode.attachChild(headNode); headNode.localTransform.setTranslation(0, 0, headShift); headNode.attachChild(neckNode); neckNode.localTransform.setTranslation(0, -neckLength, 0); neckNode.attachChild(chestNode); chestNode.localTransform.setTranslation(0, -chestDistance, 0); chestNode.attachChild(waistNode); waistNode.localTransform.setTranslation(0, -(waistDistance - chestDistance), 0); waistNode.attachChild(hipNode); hipNode.localTransform.setTranslation(0, -hipDistance, 0); hipNode.attachChild(trackerWaistNode); trackerWaistNode.localTransform.setTranslation(0, trackerWaistDistance, 0); configMap.put("Head", headShift); configMap.put("Neck", neckLength); configMap.put("Chest", chestDistance); configMap.put("Waist", waistDistance); configMap.put("Virtual waist", trackerWaistDistance); configMap.put("Hip", hipDistance); } @Override public void resetSkeletonConfig(String joint) { switch(joint) { case "All": // Reset all joints according to height resetSkeletonConfig("Head"); resetSkeletonConfig("Neck"); resetSkeletonConfig("Virtual waist"); resetSkeletonConfig("Hip"); resetSkeletonConfig("Waist"); resetSkeletonConfig("Chest"); break; case "Head": setSkeletonConfig(joint, HEAD_SHIFT_DEFAULT); break; case "Neck": setSkeletonConfig(joint, NECK_LENGTH_DEFAULT); break; case "Virtual waist": setSkeletonConfig(joint, 0.0f); break; case "Chest": //Chest is half of the upper body (half of the waist with the hip) setSkeletonConfig(joint, (waistDistance + hipDistance) / 2.0f); break; case "Hip": // Distance between waist and hip setSkeletonConfig(joint, 0.1f); break; case "Waist": // waist length is from shoulders (bottom of neck) to waist (right above the hip) Vector3f vec = new Vector3f(); hmdTracker.getPosition(vec); float 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 setSkeletonConfig(joint, ((height) / 2.0f) - hipDistance - neckLength); } break; } } @Override public Map getSkeletonConfig() { return configMap; } @Override public void setSkeletonConfig(String joint, float newLength) { configMap.put(joint, newLength); switch(joint) { case "Head": headShift = newLength; server.config.setProperty("body.headShift", headShift); headNode.localTransform.setTranslation(0, 0, headShift); break; case "Neck": neckLength = newLength; server.config.setProperty("body.neckLength", neckLength); neckNode.localTransform.setTranslation(0, -neckLength, 0); break; case "Waist": waistDistance = newLength; server.config.setProperty("body.waistDistance", waistDistance); waistNode.localTransform.setTranslation(0, -(waistDistance - chestDistance), 0); break; case "Chest": chestDistance = newLength; server.config.setProperty("body.chestDistance", chestDistance); chestNode.localTransform.setTranslation(0, -chestDistance, 0); waistNode.localTransform.setTranslation(0, -(waistDistance - chestDistance), 0); trackerWaistNode.localTransform.setTranslation(0, trackerWaistDistance, 0); break; case "Hip": hipDistance = newLength; server.config.setProperty("body.hipDistance", hipDistance); hipNode.localTransform.setTranslation(0, -hipDistance, 0); trackerWaistNode.localTransform.setTranslation(0, trackerWaistDistance, 0); break; case "Virtual waist": trackerWaistDistance = newLength; server.config.setProperty("body.trackerWaistDistance", trackerWaistDistance); trackerWaistNode.localTransform.setTranslation(0, trackerWaistDistance, 0); break; } } public boolean getSkeletonConfigBoolean(String config) { return false; } public void setSkeletonConfigBoolean(String config, boolean newState) { } @Override public TransformNode getRootNode() { return hmdNode; } @Override @VRServerThread public void updatePose() { updateLocalTransforms(); hmdNode.update(); updateComputedTrackers(); } protected void updateLocalTransforms() { if(hmdTracker.getPosition(vBuf)) { hmdNode.localTransform.setTranslation(vBuf); } if(hmdTracker.getRotation(qBuf)) { hmdNode.localTransform.setRotation(qBuf); headNode.localTransform.setRotation(qBuf); } if(chestTracker.getRotation(qBuf)) neckNode.localTransform.setRotation(qBuf); if(waistTracker.getRotation(qBuf)) { chestNode.localTransform.setRotation(qBuf); } if(hipTracker.getRotation(qBuf)) { waistNode.localTransform.setRotation(qBuf); trackerWaistNode.localTransform.setRotation(qBuf); hipNode.localTransform.setRotation(qBuf); } } protected void updateComputedTrackers() { if(computedWaistTracker != null) { computedWaistTracker.position.set(trackerWaistNode.worldTransform.getTranslation()); computedWaistTracker.rotation.set(trackerWaistNode.worldTransform.getRotation()); computedWaistTracker.dataTick(); } if(computedChestTracker != null) { computedChestTracker.position.set(chestNode.worldTransform.getTranslation()); computedChestTracker.rotation.set(neckNode.worldTransform.getRotation()); computedChestTracker.dataTick(); } } @Override @VRServerThread public void resetTrackersFull() { // Each tracker uses the tracker before it to adjust iteself, // so trackers that don't need adjustments could be used too Quaternion referenceRotation = new Quaternion(); server.hmdTracker.getRotation(referenceRotation); this.chestTracker.resetFull(referenceRotation); this.chestTracker.getRotation(referenceRotation); this.waistTracker.resetFull(referenceRotation); this.waistTracker.getRotation(referenceRotation); this.hipTracker.resetFull(referenceRotation); } @Override @VRServerThread public void resetTrackersYaw() { // Each tracker uses the tracker before it to adjust iteself, // so trackers that don't need adjustments could be used too Quaternion referenceRotation = new Quaternion(); server.hmdTracker.getRotation(referenceRotation); this.chestTracker.resetYaw(referenceRotation); this.chestTracker.getRotation(referenceRotation); this.waistTracker.resetYaw(referenceRotation); this.waistTracker.getRotation(referenceRotation); this.hipTracker.resetYaw(referenceRotation); } }