348 lines · 13.1 KB · JavaScript
RawDownload
/* global Xform */Script.include("/~/system/libraries/Xform.js"); var TRACKED_OBJECT_POSES = [    "TrackedObject00", "TrackedObject01", "TrackedObject02", "TrackedObject03",    "TrackedObject04", "TrackedObject05", "TrackedObject06", "TrackedObject07",    "TrackedObject08", "TrackedObject09", "TrackedObject10", "TrackedObject11",    "TrackedObject12", "TrackedObject13", "TrackedObject14", "TrackedObject15"]; var triggerPressHandled = false;var rightTriggerPressed = false;var leftTriggerPressed = false;var calibrationCount = 0; var TRIGGER_MAPPING_NAME = "com.highfidelity.viveMotionCapture.triggers";var triggerMapping = Controller.newMapping(TRIGGER_MAPPING_NAME);triggerMapping.from([Controller.Standard.RTClick]).peek().to(function (value) {    rightTriggerPressed = (value !== 0) ? true : false;});triggerMapping.from([Controller.Standard.LTClick]).peek().to(function (value) {    leftTriggerPressed = (value !== 0) ? true : false;});Controller.enableMapping(TRIGGER_MAPPING_NAME); var CONTROLLER_MAPPING_NAME = "com.highfidelity.viveMotionCapture.controller";var controllerMapping; var head;var leftFoot;var rightFoot;var hips;var spine2; var FEET_ONLY = 0;var FEET_AND_HIPS = 1;var FEET_AND_CHEST = 2;var FEET_HIPS_AND_CHEST = 3;var AUTO = 4; var SENSOR_CONFIG_NAMES = [    "FeetOnly",    "FeetAndHips",    "FeetAndChest",    "FeetHipsAndChest",    "Auto"]; var sensorConfig = AUTO; var Y_180 = {x: 0, y: 1, z: 0, w: 0};var Y_180_XFORM = new Xform(Y_180, {x: 0, y: 0, z: 0}); function computeOffsetXform(defaultToReferenceXform, pose, jointIndex) {    var poseXform = new Xform(pose.rotation, pose.translation);     var defaultJointXform = new Xform(MyAvatar.getAbsoluteDefaultJointRotationInObjectFrame(jointIndex),                                      MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(jointIndex));     var referenceJointXform = Xform.mul(defaultToReferenceXform, defaultJointXform);     return Xform.mul(poseXform.inv(), referenceJointXform);} function computeDefaultToReferenceXform() {    var headIndex = MyAvatar.getJointIndex("Head");    if (headIndex >= 0) {        var defaultHeadXform = new Xform(MyAvatar.getAbsoluteDefaultJointRotationInObjectFrame(headIndex),                                         MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(headIndex));        var currentHeadXform = new Xform(Quat.cancelOutRollAndPitch(MyAvatar.getAbsoluteJointRotationInObjectFrame(headIndex)),                                         MyAvatar.getAbsoluteJointTranslationInObjectFrame(headIndex));         var defaultToReferenceXform = Xform.mul(currentHeadXform, defaultHeadXform.inv());         return defaultToReferenceXform;    } else {        return Xform.ident();    }} function computeHeadOffsetXform() {    var leftEyeIndex = MyAvatar.getJointIndex("LeftEye");    var rightEyeIndex = MyAvatar.getJointIndex("RightEye");    var headIndex = MyAvatar.getJointIndex("Head");    if (leftEyeIndex > 0 && rightEyeIndex > 0 && headIndex > 0) {        var defaultHeadXform = new Xform(MyAvatar.getAbsoluteDefaultJointRotationInObjectFrame(headIndex),                                         MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(headIndex));        var defaultLeftEyeXform = new Xform(MyAvatar.getAbsoluteDefaultJointRotationInObjectFrame(leftEyeIndex),                                            MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(leftEyeIndex));        var defaultRightEyeXform = new Xform(MyAvatar.getAbsoluteDefaultJointRotationInObjectFrame(rightEyeIndex),                                             MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(rightEyeIndex));        var defaultCenterEyePos = Vec3.multiply(0.5, Vec3.sum(defaultLeftEyeXform.pos, defaultRightEyeXform.pos));        var defaultCenterEyeXform = new Xform(defaultLeftEyeXform.rot, defaultCenterEyePos);         return Xform.mul(defaultCenterEyeXform.inv(), defaultHeadXform);    } else {        return undefined;    }} function calibrate() {     head = undefined;    leftFoot = undefined;    rightFoot = undefined;    hips = undefined;    spine2 = undefined;     var defaultToReferenceXform = computeDefaultToReferenceXform();     var headOffsetXform = computeHeadOffsetXform();    print("AJT: computed headOffsetXform " + (headOffsetXform ? JSON.stringify(headOffsetXform) : "undefined"));     if (headOffsetXform) {        head = { offsetXform: headOffsetXform };    }     var poses = [];    if (Controller.Hardware.Vive) {        TRACKED_OBJECT_POSES.forEach(function (key) {            var channel = Controller.Hardware.Vive[key];            var pose = Controller.getPoseValue(channel);            if (pose.valid) {                poses.push({                    channel: channel,                    pose: pose,                    lastestPose: pose                });            }        });    }     print("AJT: calibrating, num tracked poses = " + poses.length + ", sensorConfig = " + SENSOR_CONFIG_NAMES[sensorConfig]);     var config = sensorConfig;     if (config === AUTO) {        if (poses.length === 2) {            config = FEET_ONLY;        } else if (poses.length === 3) {            config = FEET_AND_HIPS;        } else if (poses.length >= 4) {            config = FEET_HIPS_AND_CHEST;        } else {            print("AJT: auto config failed: poses.length = " + poses.length);            config = FEET_ONLY;        }    }     if (poses.length >= 2) {        // sort by y        poses.sort(function(a, b) {            var ay = a.pose.translation.y;            var by = b.pose.translation.y;            return ay - by;        });         if (poses[0].pose.translation.x > poses[1].pose.translation.x) {            rightFoot = poses[0];            leftFoot = poses[1];        } else {            rightFoot = poses[1];            leftFoot = poses[0];        }         // compute offsets        rightFoot.offsetXform = computeOffsetXform(defaultToReferenceXform, rightFoot.pose, MyAvatar.getJointIndex("RightFoot"));        leftFoot.offsetXform = computeOffsetXform(defaultToReferenceXform, leftFoot.pose, MyAvatar.getJointIndex("LeftFoot"));         print("AJT: rightFoot = " + JSON.stringify(rightFoot));        print("AJT: leftFoot = " + JSON.stringify(leftFoot));         if (config === FEET_ONLY) {            // we're done!        } else if (config === FEET_AND_HIPS && poses.length >= 3) {            hips = poses[2];        } else if (config === FEET_AND_CHEST && poses.length >= 3) {            spine2 = poses[2];        } else if (config === FEET_HIPS_AND_CHEST && poses.length >= 4) {            hips = poses[2];            spine2 = poses[3];        } else {            // TODO: better error messages            print("AJT: could not calibrate for sensor config " + SENSOR_CONFIG_NAMES[config] + ", please try again!");        }         if (hips) {            hips.offsetXform = computeOffsetXform(defaultToReferenceXform, hips.pose, MyAvatar.getJointIndex("Hips"));            print("AJT: hips = " + JSON.stringify(hips));        }         if (spine2) {            spine2.offsetXform = computeOffsetXform(defaultToReferenceXform, spine2.pose, MyAvatar.getJointIndex("Spine2"));            print("AJT: spine2 = " + JSON.stringify(spine2));        }     } else {        print("AJT: could not find two trackers, try again!");    }} var ikTypes = {    RotationAndPosition: 0,    RotationOnly: 1,    HmdHead: 2,    HipsRelativeRotationAndPosition: 3,    Off: 4}; var handlerId; function convertJointInfoToPose(jointInfo) {    var latestPose = jointInfo.latestPose;    var offsetXform = jointInfo.offsetXform;    var xform = Xform.mul(new Xform(latestPose.rotation, latestPose.translation), offsetXform);    return {        valid: true,        translation: xform.pos,        rotation: xform.rot,        velocity: Vec3.sum(latestPose.velocity, Vec3.cross(latestPose.angularVelocity, Vec3.subtract(xform.pos, latestPose.translation))),        angularVelocity: latestPose.angularVelocity    };} function update(dt) {    if (rightTriggerPressed && leftTriggerPressed) {        if (!triggerPressHandled) {            triggerPressHandled = true;            if (controllerMapping) {                 // go back to normal, vive pucks will be ignored.                print("AJT: UN-CALIBRATE!");                 head = undefined;                leftFoot = undefined;                rightFoot = undefined;                hips = undefined;                spine2 = undefined;                 Controller.disableMapping(CONTROLLER_MAPPING_NAME + calibrationCount);                controllerMapping = undefined;             } else {                print("AJT: CALIBRATE!");                calibrate();                calibrationCount++;                 controllerMapping = Controller.newMapping(CONTROLLER_MAPPING_NAME + calibrationCount);                 if (head) {                    controllerMapping.from(function () {                        var worldToAvatarXform = (new Xform(MyAvatar.orientation, MyAvatar.position)).inv();                        head.latestPose = {                            valid: true,                            translation: worldToAvatarXform.xformPoint(HMD.position),                            rotation: Quat.multiply(worldToAvatarXform.rot, Quat.multiply(HMD.orientation, Y_180)), // postMult 180 rot flips head direction                            velocity: {x: 0, y: 0, z: 0},  // TODO: currently this is unused anyway...                            angularVelocity: {x: 0, y: 0, z: 0}                        };                        return convertJointInfoToPose(head);                    }).to(Controller.Standard.Head);                }                 if (leftFoot) {                    controllerMapping.from(leftFoot.channel).to(function (pose) {                        leftFoot.latestPose = pose;                    });                    controllerMapping.from(function () {                        return convertJointInfoToPose(leftFoot);                    }).to(Controller.Standard.LeftFoot);                }                if (rightFoot) {                    controllerMapping.from(rightFoot.channel).to(function (pose) {                        rightFoot.latestPose = pose;                    });                    controllerMapping.from(function () {                        return convertJointInfoToPose(rightFoot);                    }).to(Controller.Standard.RightFoot);                }                if (hips) {                    controllerMapping.from(hips.channel).to(function (pose) {                        hips.latestPose = pose;                    });                    controllerMapping.from(function () {                        return convertJointInfoToPose(hips);                    }).to(Controller.Standard.Hips);                }                if (spine2) {                    controllerMapping.from(spine2.channel).to(function (pose) {                        spine2.latestPose = pose;                    });                    controllerMapping.from(function () {                        return convertJointInfoToPose(spine2);                    }).to(Controller.Standard.Spine2);                }                Controller.enableMapping(CONTROLLER_MAPPING_NAME + calibrationCount);            }        }    } else {        triggerPressHandled = false;    }     var drawMarkers = false;    if (drawMarkers) {        var RED = {x: 1, y: 0, z: 0, w: 1};        var BLUE = {x: 0, y: 0, z: 1, w: 1};         if (leftFoot) {            var leftFootPose = Controller.getPoseValue(leftFoot.channel);            DebugDraw.addMyAvatarMarker("leftFootTracker", leftFootPose.rotation, leftFootPose.translation, BLUE);        }         if (rightFoot) {            var rightFootPose = Controller.getPoseValue(rightFoot.channel);            DebugDraw.addMyAvatarMarker("rightFootTracker", rightFootPose.rotation, rightFootPose.translation, RED);        }         if (hips) {            var hipsPose = Controller.getPoseValue(hips.channel);            DebugDraw.addMyAvatarMarker("hipsTracker", hipsPose.rotation, hipsPose.translation, GREEN);        }    }     var drawReferencePose = false;    if (drawReferencePose) {        var GREEN = {x: 0, y: 1, z: 0, w: 1};        var defaultToReferenceXform = computeDefaultToReferenceXform();        var leftFootIndex = MyAvatar.getJointIndex("LeftFoot");        if (leftFootIndex > 0) {            var defaultLeftFootXform = new Xform(MyAvatar.getAbsoluteDefaultJointRotationInObjectFrame(leftFootIndex),                                                 MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(leftFootIndex));            var referenceLeftFootXform = Xform.mul(defaultToReferenceXform, defaultLeftFootXform);            DebugDraw.addMyAvatarMarker("leftFootTracker", referenceLeftFootXform.rot, referenceLeftFootXform.pos, GREEN);        }    } } Script.update.connect(update); Script.scriptEnding.connect(function () {    Controller.disableMapping(TRIGGER_MAPPING_NAME);    if (controllerMapping) {        Controller.disableMapping(CONTROLLER_MAPPING_NAME + calibrationCount);    }    Script.update.disconnect(update);});