643 lines
31 KiB
JavaScript
643 lines
31 KiB
JavaScript
/* jslint bitwise: true */
|
|
|
|
/* global Script, Vec3, MyAvatar, Tablet, Messages, Quat, DebugDraw, Mat4, Entities, Xform, Controller, Camera, console, document*/
|
|
|
|
Script.registerValue("STEPAPP", true);
|
|
|
|
var LEFT = 0;
|
|
var RIGHT = 1;
|
|
var DEFAULT_AVATAR_HEIGHT = 1.64;
|
|
var TABLET_BUTTON_NAME = "STEP";
|
|
var CHANGE_OF_BASIS_ROTATION = { x: 0, y: 1, z: 0, w: 0 };
|
|
// in centimeters
|
|
var DEFAULT_ANTERIOR = 0.14;
|
|
var DEFAULT_POSTERIOR = 0.14;
|
|
var DEFAULT_LATERAL = 0.10;
|
|
var DEFAULT_HEIGHT_DIFFERENCE = -0.01;
|
|
// zero to ten
|
|
var DEFAULT_ANGULAR_VELOCITY = 0.3;
|
|
var DEFAULT_HAND_VELOCITY = -1.0;
|
|
var DEFAULT_ANGULAR_HAND_VELOCITY = 0.3;
|
|
var VELOCITY_EPSILON = 0.02;
|
|
var ROT_Y180 = {x: 0, y: 1, z: 0, w: 0};
|
|
var MAX_LEVEL_PITCH = 3;
|
|
var MAX_LEVEL_ROLL = 3;
|
|
// this should be changed by the actual base of support of the person? or Avatar?
|
|
// You must have moved at least this far laterally to take a step
|
|
var MIN_STEP_DISTANCE = 0.03;
|
|
var DONE_STEPPING_DISTANCE = 0.01;
|
|
var AVERAGING_RATE = 0.02;
|
|
var HEIGHT_AVERAGING_RATE = 0.0025;
|
|
// The velocity of head and hands in same direction that triggers stepping
|
|
var STEP_VELOCITY_THRESHOLD = 0.05;
|
|
// The time required for observed velocity to continue to trigger stepping
|
|
var STEP_TIME_SECS = 0.15;
|
|
|
|
var AZIMUTH_TURN_MIN_DEGREES = 22;
|
|
var AZIMUTH_TIME_SECS = 2.5;
|
|
var HEAD_HAND_BALANCE = 2.0;
|
|
|
|
var RECENTER = false;
|
|
MyAvatar.hmdLeanRecenterEnabled = RECENTER;
|
|
var STEPTURN = true;
|
|
var RESET_MODE = false;
|
|
var MY_HEIGHT = 1.15;
|
|
|
|
var failsafeFlag = false;
|
|
var failsafeSignalTimer = -1.0;
|
|
var maxHeightChange = 0.010;
|
|
var angularVelocityThreshold = 0.3;
|
|
var handVelocityThreshold = -1.0;
|
|
var handAngularVelocityThreshold = 0.3;
|
|
var lateralEdge = 0.14;
|
|
var frontEdge = -0.10;
|
|
var backEdge = 0.13;
|
|
var frontLeft = { x: -lateralEdge, y: 0, z: frontEdge };
|
|
var frontRight = { x: lateralEdge, y: 0, z: frontEdge };
|
|
var backLeft = { x: -lateralEdge, y: 0, z: backEdge };
|
|
var backRight = { x: lateralEdge, y: 0, z: backEdge };
|
|
|
|
var overFront = false;
|
|
var overBack = false;
|
|
var overLateral = false;
|
|
|
|
var modeArray = new Array(100);
|
|
var modeHeight = -10.0;
|
|
var stepTimer = -1.0;
|
|
var stationaryTimer = 0.0;
|
|
var heightFudgeFactor = 0.01;
|
|
var torsoLength = 1.0;
|
|
var defaultLength = 1.0;
|
|
var hipsRotation = {x: 0, y: 0, z: 0, w: 1};
|
|
var hipsPosition = {x: 0, y: 0, z: 0};
|
|
var spine2Rotation = {x: 0, y: -0.85, z: 0, w: 0.5};
|
|
var spine2Position = { x: 0, y: 0, z: 0 };
|
|
var handDotHead = [];
|
|
var headPosition;
|
|
var headAverageOrientation = MyAvatar.orientation;
|
|
var headPoseAverageOrientation = { x: 0, y: 0, z: 0, w: 1 };
|
|
var averageHeight = 1.0;
|
|
var headVelocityThreshold = 0.0;
|
|
var handPosition;
|
|
var handOrientation;
|
|
var headOrientation;
|
|
var isStepping = false;
|
|
var handSteppingDetect = false;
|
|
var handSteppingTimer = 0;
|
|
var lastHeadAzimuth = 0;
|
|
var azimuthChangeTimer = 0;
|
|
var hipToHandAverage = [];
|
|
var averageAzimuth = 0.0;
|
|
var headPosAvatarSpace;
|
|
var rightHandPosAvatarSpace;
|
|
var leftHandPosAvatarSpace;
|
|
var hands = [];
|
|
var head, headAverage;
|
|
var headEulers;
|
|
var headAverageEulers;
|
|
var oldAngularVelocity = { x: 0.0, y: 0.0, z: 0.0 };
|
|
var headSwayCount = 0;
|
|
var accelerationArray = new Array(30);
|
|
var debugDrawBase = true;
|
|
var activated = false;
|
|
var documentLoaded = false;
|
|
|
|
var HTML_URL = Script.resolvePath("http://hifi-content.s3.amazonaws.com/angus/stepApp/stepApp.html");
|
|
// var HTML_URL = Script.resolvePath("file:///c:/dev/hifi_fork/hifi/scripts/developer/stepApp.html");
|
|
var tablet = Tablet.getTablet("com.highfidelity.interface.tablet.system");
|
|
|
|
function manageClick() {
|
|
if (activated) {
|
|
tablet.gotoHomeScreen();
|
|
} else {
|
|
tablet.gotoWebScreen(HTML_URL);
|
|
}
|
|
}
|
|
|
|
var tabletButton = tablet.addButton({
|
|
text: TABLET_BUTTON_NAME,
|
|
icon: Script.resolvePath("http://hifi-content.s3.amazonaws.com/angus/stepApp/foot.svg"),
|
|
activeIcon: Script.resolvePath("http://hifi-content.s3.amazonaws.com/angus/stepApp/foot.svg")
|
|
});
|
|
|
|
function drawBase() {
|
|
// transform corners into world space, for rendering.
|
|
// var avatarXform = new Xform(MyAvatar.orientation, MyAvatar.position);
|
|
var worldPointLf = Vec3.sum(MyAvatar.position,Vec3.multiplyQbyV(MyAvatar.orientation, frontLeft));
|
|
var worldPointRf = Vec3.sum(MyAvatar.position,Vec3.multiplyQbyV(MyAvatar.orientation, frontRight));
|
|
var worldPointLb = Vec3.sum(MyAvatar.position,Vec3.multiplyQbyV(MyAvatar.orientation, backLeft));
|
|
var worldPointRb = Vec3.sum(MyAvatar.position,Vec3.multiplyQbyV(MyAvatar.orientation, backRight));
|
|
|
|
// print("world point is left front is: " + worldPointLf.x + " " + worldPointLf.y + " " + worldPointLf.z);
|
|
// print("front left point is " + frontLeft.x + " " + frontLeft.y + " " + frontLeft.z);
|
|
|
|
var GREEN = { r: 0, g: 1, b: 0, a: 1 };
|
|
|
|
// draw border
|
|
DebugDraw.drawRay(worldPointLf, worldPointRf, GREEN);
|
|
DebugDraw.drawRay(worldPointRf, worldPointRb, GREEN);
|
|
DebugDraw.drawRay(worldPointRb, worldPointLb, GREEN);
|
|
DebugDraw.drawRay(worldPointLb, worldPointLf, GREEN);
|
|
}
|
|
|
|
function onKeyPress(event) {
|
|
if (event.text === "'") {
|
|
// when the sensors are reset, then reset the mode.
|
|
RESET_MODE = false;
|
|
}
|
|
}
|
|
|
|
function onWebEventReceived(msg) {
|
|
var message = JSON.parse(msg);
|
|
print(" we have a message from html dialog " + message.type);
|
|
switch (message.type) {
|
|
case "onAnteriorBaseSlider":
|
|
print("anterior slider " + message.data.value);
|
|
var frontBaseMeters = message.data.value / 100.0;
|
|
setAnteriorDistance(frontBaseMeters);
|
|
break;
|
|
case "onPosteriorBaseSlider":
|
|
print("posterior slider " + message.data.value);
|
|
var backBaseMeters = message.data.value / 100.0;
|
|
setPosteriorDistance(backBaseMeters);
|
|
break;
|
|
case "onLateralBaseSlider":
|
|
print("lateral slider " + message.data.value);
|
|
var lateralBaseMeters = message.data.value / 100.0;
|
|
setLateralDistance(lateralBaseMeters);
|
|
break;
|
|
case "onAngularVelocitySlider":
|
|
print("angular velocity value " + message.data.value);
|
|
// the scale of this slider is logarithmic to cover a greater range of values
|
|
// the range of values is 4 raised to the power of the slider input value, which is scaled to 0-2.
|
|
// this makes the real range 15 to 0 for angular velocity of the head
|
|
var angularVelocityHeadExponential = Math.pow(4, (2.0 - message.data.value)) - 1.0;
|
|
setAngularThreshold(angularVelocityHeadExponential);
|
|
break;
|
|
case "onHeightDifferenceSlider":
|
|
print("height slider " + message.data.value);
|
|
var heightDifferenceToleranceMeters = -(message.data.value / 100.0);
|
|
setHeightThreshold(heightDifferenceToleranceMeters);
|
|
break;
|
|
case "onHandsVelocitySlider":
|
|
print("hands velocity slider " + message.data.value);
|
|
var handDirectionToHeadDirectionInverseCosine = message.data.value;
|
|
setHandVelocityThreshold(handDirectionToHeadDirectionInverseCosine);
|
|
break;
|
|
case "onHandsAngularVelocitySlider":
|
|
print("hands angular velocity slider " + message.data.value);
|
|
// the scale of this slider is logarithmic to cover a greater range of values
|
|
// the range of values is 7 raised to the power of the slider input value, which is scaled to 0-2.
|
|
// this makes the real range 48 to 0 for angular velocity tolerance in the hands
|
|
var angularVelocityHandExponential = Math.pow(7, (2.0 - message.data.value)) - 1.0;
|
|
setHandAngularVelocityThreshold(angularVelocityHandExponential);
|
|
break;
|
|
case "onHeadVelocitySlider":
|
|
print("head velocity slider " + message.data.value);
|
|
// the scale of this slider is logarithmic to cover a greater range of values
|
|
// the range of values is 2 raised to the power of the slider input value, which is scaled to 0-5.
|
|
// this makes the real range 31 to 0 for the velocity tolerance of the head
|
|
var headVelocityThreshold = Math.pow(2, message.data.value) - 1.0;
|
|
setHeadVelocityThreshold(headVelocityThreshold);
|
|
break;
|
|
case "onCreateStepApp":
|
|
print("on create step app ");
|
|
break;
|
|
default:
|
|
print("unknown message from step html!!");
|
|
break;
|
|
}
|
|
}
|
|
|
|
function initAppForm() {
|
|
print("step app is loaded: " + documentLoaded);
|
|
var frontEdgeCentimeters = 100.0 * frontEdge;
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "frontBase", "data": { "value": frontEdgeCentimeters } }));
|
|
var backEdgeCentimeters = 100.0 * backEdge;
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "backBase", "data": { "value": backEdgeCentimeters } }));
|
|
var lateralEdgeCentimeters = 100.0 * lateralEdge;
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "lateralBase", "data": { "value": lateralEdgeCentimeters } }));
|
|
var angularVelocityHeadLogarithmic = (-1.0 * getLog(4, (angularVelocityThreshold + 1)) + 2.0);
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "angularHeadVelocity", "data": { "value": angularVelocityHeadLogarithmic } }));
|
|
var heightDifferenceToleranceCentimeters = -100.0 * maxHeightChange;
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "heightDifference", "data": { "value": heightDifferenceToleranceCentimeters } }));
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "handsVelocity", "data": { "value": handVelocityThreshold } }));
|
|
var angularVelocityHandLogarithmic = (-1.0 * getLog(7, (handAngularVelocityThreshold + 1)) + 2.0);
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "handsAngularVelocity", "data": { "value": angularVelocityHandLogarithmic } }));
|
|
var headVelocityLogarithmic = getLog(2, (headVelocityThreshold + 1));
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "headVelocity", "data": { "value": headVelocityLogarithmic } }));
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "frontSignal", "data": { "value": "green" } }));
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "backSignal", "data": { "value": "green" } }));
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "lateralSignal", "data": { "value": "green" } }));
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "angularHeadSignal", "data": { "value": "green" } }));
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "heightSignal", "data": { "value": "green" } }));
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "handVelocitySignal", "data": { "value": "green" } }));
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "handAngularSignal", "data": { "value": "green" } }));
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "headVelocitySignal", "data": { "value": "green" } }));
|
|
}
|
|
|
|
function updateSignalColors(isSupported, xzAngVel, heightDiff, lhPose, rhPose, xzRHAngVel, xzLHAngVel, headPoseValid, headSpeed) {
|
|
// if we are outside the support base in one direction we get a green light to translate
|
|
// in that case make the non crossed edges signals blue to reflect that they are no longer blocking translation
|
|
if (!isSupported) {
|
|
if (overFront) {
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "frontSignal", "data": { "value": "green" } }));
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "backSignal", "data": { "value": "blue" } }));
|
|
} else if (overBack) {
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "backSignal", "data": { "value": "green" } }));
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "frontSignal", "data": { "value": "blue" } }));
|
|
} else {
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "backSignal", "data": { "value": "blue" } }));
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "frontSignal", "data": { "value": "blue" } }));
|
|
}
|
|
if (overLateral) {
|
|
// over lateral
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "lateralSignal", "data": { "value": "green" } }));
|
|
} else {
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "lateralSignal", "data": { "value": "blue" } }));
|
|
}
|
|
} else {
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "frontSignal", "data": { "value": "red" } }));
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "backSignal", "data": { "value": "red" } }));
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "lateralSignal", "data": { "value": "red" } }));
|
|
}
|
|
// console.log("angular velocity threshold " + angularVelocityThreshold);
|
|
// if we have too much head angular velocity, thus triggering this break on translation.
|
|
if (headPoseValid && !(xzAngVel < angularVelocityThreshold)) {
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "angularHeadSignal", "data": { "value": "red" } }));
|
|
} else {
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "angularHeadSignal", "data": { "value": "green" } }));
|
|
}
|
|
// if we are lower than the height tolerance relative to the height mode
|
|
if (!(heightDiff < maxHeightChange)) {
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "heightSignal", "data": { "value": "red" } }));
|
|
} else {
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "heightSignal", "data": { "value": "green" } }));
|
|
}
|
|
// if both hand poses are valid but not matching the direction of the head more than our handVelocityThreshold
|
|
if (!((!lhPose.valid || ((handDotHead[LEFT] > handVelocityThreshold) && (Vec3.length(lhPose.velocity) > VELOCITY_EPSILON)))
|
|
&& (!rhPose.valid || ((handDotHead[RIGHT] > handVelocityThreshold) && (Vec3.length(rhPose.velocity) > VELOCITY_EPSILON))))) {
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "handVelocitySignal", "data": { "value": "red" } }));
|
|
} else {
|
|
// otherwise we are not blocked from translating
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "handVelocitySignal", "data": { "value": "green" } }));
|
|
}
|
|
|
|
// if the hand angular velocity for both hands is not lower than the threshold
|
|
if (!((!rhPose.valid || (xzRHAngVel < handAngularVelocityThreshold)) && (!lhPose.valid || (xzLHAngVel < handAngularVelocityThreshold)))) {
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "handAngularSignal", "data": { "value": "red" } }));
|
|
} else {
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "handAngularSignal", "data": { "value": "green" } }));
|
|
}
|
|
|
|
if (headSpeed < headVelocityThreshold) {
|
|
// if head speed is below threshold then don't translate
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "headVelocitySignal", "data": { "value": "red" } }));
|
|
} else {
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "trigger", "id": "headVelocitySignal", "data": { "value": "green" } }));
|
|
}
|
|
|
|
}
|
|
|
|
function onScreenChanged(type, url) {
|
|
print("Screen changed");
|
|
if (type === "Web" && url === HTML_URL) {
|
|
if (!activated) {
|
|
// hook up to event bridge
|
|
tablet.webEventReceived.connect(onWebEventReceived);
|
|
print("after connect web event");
|
|
Script.setTimeout(initAppForm, 500);
|
|
}
|
|
activated = true;
|
|
} else {
|
|
if (activated) {
|
|
// disconnect from event bridge
|
|
tablet.webEventReceived.disconnect(onWebEventReceived);
|
|
}
|
|
activated = false;
|
|
}
|
|
}
|
|
|
|
function getLog(x, y) {
|
|
return Math.log(y) / Math.log(x);
|
|
}
|
|
|
|
function isInsideLine(a, b, c) {
|
|
return (((b.x - a.x)*(c.z - a.z) - (b.z - a.z)*(c.x - a.x)) > 0);
|
|
}
|
|
|
|
function setAngularThreshold(num) {
|
|
angularVelocityThreshold = num;
|
|
print("angular threshold " + angularVelocityThreshold);
|
|
}
|
|
|
|
function setHeightThreshold(num) {
|
|
maxHeightChange = num;
|
|
print("height threshold " + maxHeightChange);
|
|
}
|
|
|
|
function setLateralDistance(num) {
|
|
lateralEdge = num;
|
|
frontLeft.x = -lateralEdge;
|
|
frontRight.x = lateralEdge;
|
|
backLeft.x = -lateralEdge;
|
|
backRight.x = lateralEdge;
|
|
print("lateral distance " + lateralEdge);
|
|
}
|
|
|
|
function setAnteriorDistance(num) {
|
|
frontEdge = num;
|
|
frontLeft.z = -frontEdge;
|
|
frontRight.z = -frontEdge;
|
|
print("anterior distance " + frontEdge);
|
|
}
|
|
|
|
function setPosteriorDistance(num) {
|
|
backEdge = num;
|
|
backLeft.z = backEdge;
|
|
backRight.z = backEdge;
|
|
print("posterior distance " + frontEdge);
|
|
}
|
|
|
|
function setHandAngularVelocityThreshold(num) {
|
|
handAngularVelocityThreshold = num;
|
|
print("hand angular velocity threshold " + handAngularVelocityThreshold);
|
|
}
|
|
|
|
function setHandVelocityThreshold(num) {
|
|
handVelocityThreshold = num;
|
|
print("hand velocity threshold " + handVelocityThreshold);
|
|
}
|
|
|
|
function setHeadVelocityThreshold(num) {
|
|
headVelocityThreshold = num;
|
|
print("headvelocity threshold " + headVelocityThreshold);
|
|
}
|
|
|
|
function withinBaseOfSupport(pos) {
|
|
var userScale = 1.0;
|
|
overFront = !(isInsideLine(Vec3.multiply(userScale, frontLeft), Vec3.multiply(userScale, frontRight), pos));
|
|
overBack = !(isInsideLine(Vec3.multiply(userScale, backRight), Vec3.multiply(userScale, backLeft), pos));
|
|
overLateral = !(isInsideLine(Vec3.multiply(userScale, frontRight), Vec3.multiply(userScale, backRight), pos) && isInsideLine(Vec3.multiply(userScale, backLeft), Vec3.multiply(userScale, frontLeft), pos));
|
|
return (!overFront && !overBack && !overLateral);
|
|
}
|
|
|
|
function getStationaryFudgeFactor() {
|
|
// if no translation for 30 seconds then we raise the bar for translating by 1cm
|
|
var percentStationary = stationaryTimer / 30.0;
|
|
if (percentStationary > 1.0) {
|
|
percentStationary = 1.0;
|
|
}
|
|
return percentStationary * heightFudgeFactor;
|
|
}
|
|
|
|
|
|
function limitAngle(angle) {
|
|
return (angle + 180) % 360 - 180;
|
|
}
|
|
|
|
function findAverage(arr) {
|
|
var sum = arr.reduce(function (acc, val) {
|
|
return acc + val;
|
|
},0);
|
|
return sum / arr.length;
|
|
}
|
|
|
|
function addToAccelerationArray(arr, num) {
|
|
for (var i = 0 ; i < (arr.length - 1) ; i++) {
|
|
arr[i] = arr[i + 1];
|
|
}
|
|
arr[arr.length - 1] = num;
|
|
}
|
|
|
|
function addToModeArray(arr,num) {
|
|
for (var i = 0 ;i < (arr.length - 1); i++) {
|
|
arr[i] = arr[i+1];
|
|
}
|
|
arr[arr.length - 1] = (Math.floor(num*100))/100.00;
|
|
}
|
|
|
|
function findMode(ary, currentMode, backLength, defaultBack, currentHeight) {
|
|
var numMapping = {};
|
|
var greatestFreq = 0;
|
|
var mode;
|
|
ary.forEach(function (number) {
|
|
numMapping[number] = (numMapping[number] || 0) + 1;
|
|
if ((greatestFreq < numMapping[number]) || ((numMapping[number] === 100) && (number > currentMode) )) {
|
|
greatestFreq = numMapping[number];
|
|
mode = number;
|
|
}
|
|
});
|
|
if (mode > currentMode) {
|
|
return Number(mode);
|
|
} else {
|
|
if (failsafeFlag || (!RESET_MODE && HMD.active)) {
|
|
print("resetting the mode............................................. ");
|
|
print("resetting the mode............................................. ");
|
|
RESET_MODE = true;
|
|
failsafeFlag = false;
|
|
stationaryTimer = 0.0;
|
|
return currentHeight - 0.02;
|
|
} else {
|
|
return currentMode;
|
|
}
|
|
}
|
|
}
|
|
|
|
function update(dt) {
|
|
if (debugDrawBase) {
|
|
drawBase();
|
|
}
|
|
var currentHeadPos = MyAvatar.getAbsoluteJointTranslationInObjectFrame(MyAvatar.getJointIndex("Head"));
|
|
var defaultHipsPos = MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(MyAvatar.getJointIndex("Hips"));
|
|
var defaultHeadPos = MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(MyAvatar.getJointIndex("Head"));
|
|
defaultLength = Vec3.length(Vec3.subtract(defaultHeadPos, defaultHipsPos));
|
|
var headMinusHipLean = Vec3.subtract(currentHeadPos,defaultHipsPos);
|
|
torsoLength = Vec3.length(headMinusHipLean);
|
|
|
|
|
|
// Update head information
|
|
var headPose = Controller.getPoseValue(Controller.Standard.Head);
|
|
var rightHandPose = Controller.getPoseValue(Controller.Standard.RightHand);
|
|
var leftHandPose = Controller.getPoseValue(Controller.Standard.LeftHand);
|
|
var headPoseRigSpace = Quat.multiply(CHANGE_OF_BASIS_ROTATION, headPose.rotation);
|
|
headPosition = Camera.getPosition();
|
|
headOrientation = Camera.getOrientation();
|
|
headEulers = Quat.safeEulerAngles(headOrientation);
|
|
headAverageOrientation = Quat.slerp(headAverageOrientation, headOrientation, AVERAGING_RATE);
|
|
headAverageEulers = Quat.safeEulerAngles(headAverageOrientation);
|
|
headPoseAverageOrientation = Quat.slerp(headPoseAverageOrientation, headPoseRigSpace, AVERAGING_RATE);
|
|
var headPoseAverageEulers = Quat.safeEulerAngles(headPoseAverageOrientation);
|
|
// print("head pose average orientation " + headPose.rotation.x + " " + headPose.rotation.y + " " + headPose.rotation.z + " " + headPose.rotation.w);
|
|
// print("head pose average rig space orientation " + headPoseRigSpace.x + " " + headPoseRigSpace.y + " " + headPoseRigSpace.z + " " + headPoseRigSpace.w);
|
|
// print("head pose average yaw " + headPoseAverageEulers.y);
|
|
var azimuth = headEulers.y;
|
|
if (!lastHeadAzimuth) {
|
|
lastHeadAzimuth = azimuth;
|
|
}
|
|
|
|
// head position in object space
|
|
headPosAvatarSpace = MyAvatar.getAbsoluteJointTranslationInObjectFrame(MyAvatar.getJointIndex("Head"));
|
|
rightHandPosAvatarSpace = MyAvatar.getAbsoluteJointTranslationInObjectFrame(MyAvatar.getJointIndex("RightHand"));
|
|
leftHandPosAvatarSpace = MyAvatar.getAbsoluteJointTranslationInObjectFrame(MyAvatar.getJointIndex("LeftHand"));
|
|
var inSupport = withinBaseOfSupport(headPosAvatarSpace);
|
|
|
|
addToModeArray(modeArray,headPose.translation.y);
|
|
modeHeight = findMode(modeArray, modeHeight, torsoLength, defaultLength, headPose.translation.y);
|
|
var xzAngularVelocity = Vec3.length({ x: headPose.angularVelocity.x, y: 0.0, z: headPose.angularVelocity.z });
|
|
var xzRHandAngularVelocity = Vec3.length({ x: rightHandPose.angularVelocity.x, y: 0.0, z: rightHandPose.angularVelocity.z });
|
|
var xzLHandAngularVelocity = Vec3.length({ x: leftHandPose.angularVelocity.x, y: 0.0, z: leftHandPose.angularVelocity.z });
|
|
var isHeadLevel = (Math.abs(headEulers.z - headAverageEulers.z) < MAX_LEVEL_ROLL)
|
|
&& (Math.abs(headEulers.x - headAverageEulers.x) < MAX_LEVEL_PITCH);
|
|
var heightDifferenceFromAverage = modeHeight - headPose.translation.y;
|
|
|
|
// Get the hands velocity relative to the head
|
|
// and the hand to hip vector to determine when to change head rotation.
|
|
for (var hand = LEFT; hand <= RIGHT; hand++) {
|
|
// Update hand object
|
|
var pose = Controller.getPoseValue((hand === 1) ? Controller.Standard.RightHand : Controller.Standard.LeftHand);
|
|
if (hand === 1) {
|
|
// print("right hand velocity" + pose.velocity.x + " " + pose.velocity.y + " " + pose.velocity.z);
|
|
// print("magnitude " + Vec3.length({x: pose.velocity.x,y: 0.0,z: pose.velocity.z}));
|
|
}
|
|
var lateralPoseVelocity = {x: 0, y: 0, z: 0};
|
|
if (pose.valid && headPose.valid) {
|
|
lateralPoseVelocity = pose.velocity;
|
|
lateralPoseVelocity.y = 0;
|
|
var lateralHeadVelocity = headPose.velocity;
|
|
lateralHeadVelocity.y = 0;
|
|
handDotHead[hand] = Vec3.dot(Vec3.normalize(lateralPoseVelocity), Vec3.normalize(lateralHeadVelocity));
|
|
}
|
|
|
|
// handPosition = Mat4.transformPoint(avatarToWorldMatrix, pose.translation);
|
|
handPosition = (hand === 1) ? rightHandPosAvatarSpace : leftHandPosAvatarSpace;
|
|
handOrientation = Quat.multiply(MyAvatar.orientation, pose.rotation);
|
|
// Update angle from hips to hand, to be used for turning
|
|
var hipToHand = Quat.lookAtSimple({ x: 0, y: 0, z: 0 }, { x: handPosition.x, y: 0, z: handPosition.z });
|
|
hipToHandAverage[hand] = Quat.slerp(hipToHandAverage[hand], hipToHand, AVERAGING_RATE);
|
|
}
|
|
// print("hand dot head " + handDotHead[LEFT] + " " + handDotHead[RIGHT]);
|
|
// make the signal colors reflect the current thresholds that have been crossed
|
|
updateSignalColors(inSupport, xzAngularVelocity, heightDifferenceFromAverage, leftHandPose, rightHandPose, xzRHandAngularVelocity, xzLHandAngularVelocity, headPose.valid, Vec3.length(headPose.velocity));
|
|
|
|
// Conditions for taking a step.
|
|
// 1. off the base of support. front, lateral, back edges.
|
|
// 2. head is not lower than the height mode value by more than the maxHeightChange tolerance
|
|
// 3. the angular velocity of the head is not greater than the threshold value
|
|
// ie this reflects the speed the head is rotating away from having up = (0,1,0) in Avatar frame..
|
|
// 4. the hands velocity vector has the same direction as the head, within the given tolerance
|
|
// the tolerance is an acos value, -1 means the hands going in any direction will not block translating
|
|
// up to 1 where the hands velocity direction must exactly match that of the head.
|
|
// 5. the angular velocity xz magnitude for each hand is below the threshold value
|
|
// ie here this reflects the speed that each hand is rotating away from having up = (0,1,0) in Avatar frame.
|
|
// to do: -getStationaryFudgeFactor()
|
|
if ( !inSupport && (heightDifferenceFromAverage < maxHeightChange) && (xzAngularVelocity < angularVelocityThreshold)
|
|
&& ((!leftHandPose.valid || ((handDotHead[LEFT] > handVelocityThreshold) && (Vec3.length(leftHandPose.velocity) > VELOCITY_EPSILON)))
|
|
&& (!rightHandPose.valid || ((handDotHead[RIGHT] > handVelocityThreshold) && (Vec3.length(rightHandPose.velocity) > VELOCITY_EPSILON))))
|
|
&& ((!rightHandPose.valid ||(xzRHandAngularVelocity < handAngularVelocityThreshold))
|
|
&& (!leftHandPose.valid || (xzLHandAngularVelocity < handAngularVelocityThreshold)))
|
|
&& (Vec3.length(headPose.velocity) > headVelocityThreshold)) {
|
|
|
|
if (STEPTURN && (stepTimer < 0.0)) {
|
|
print("trigger recenter========================================================");
|
|
MyAvatar.triggerHorizontalRecenter();
|
|
stepTimer = 0.6;
|
|
stationaryTimer = 0.0;
|
|
}
|
|
} else if ((torsoLength > (defaultLength + 0.07)) && (failsafeSignalTimer < 0.0)) {
|
|
// do the failsafe recenter.
|
|
// failsafeFlag resets the mode.
|
|
|
|
failsafeFlag = true;
|
|
failsafeSignalTimer = 2.5;
|
|
stepTimer = 0.6;
|
|
MyAvatar.triggerHorizontalRecenter();
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "failsafe", "id": "failsafeSignal", "data": { "value": "green" } }));
|
|
// in fail safe we debug print the values that were blocking us.
|
|
print("failsafe debug---------------------------------------------------------------");
|
|
if (!inSupport) {
|
|
print("1. inSupport: false");
|
|
}
|
|
if (!(xzAngularVelocity < angularVelocityThreshold)) {
|
|
print("2. angular velocity exceeded threshold");
|
|
}
|
|
// if we are lower than the height tolerance relative to the height mode
|
|
if (!(heightDifferenceFromAverage < maxHeightChange)) {
|
|
print("3. height was too far below the mode");
|
|
}
|
|
// if both hand poses are valid but not matching the direction of the head more than our handVelocityThreshold
|
|
if (!((!leftHandPose.valid || ((handDotHead[LEFT] > handVelocityThreshold) && (Vec3.length(leftHandPose.velocity) > VELOCITY_EPSILON)))
|
|
&& (!rightHandPose.valid || ((handDotHead[RIGHT] > handVelocityThreshold) && (Vec3.length(rightHandPose.velocity) > VELOCITY_EPSILON))))) {
|
|
print("4. hands were not following the head direction");
|
|
}
|
|
// if the hand angular velocity for both hands is not lower than the threshold
|
|
if (!((!rightHandPose.valid || (xzRHandAngularVelocity < handAngularVelocityThreshold)) && (!leftHandPose.valid || (xzLHandAngularVelocity < handAngularVelocityThreshold)))) {
|
|
print("5. hands angular velocity exceeded threshold");
|
|
}
|
|
if (Vec3.length(headPose.velocity) < headVelocityThreshold) {
|
|
// if head speed is below threshold then don't translate
|
|
print("6. head speed was too low");
|
|
}
|
|
print("end failsafe debug---------------------------------------------------------------");
|
|
|
|
}
|
|
|
|
if (failsafeSignalTimer < 0.0) {
|
|
tablet.emitScriptEvent(JSON.stringify({ "type": "failsafe", "id": "failsafeSignal", "data": { "value": "orange" } }));
|
|
}
|
|
|
|
stepTimer -= dt;
|
|
stationaryTimer += dt;
|
|
failsafeSignalTimer -= dt;
|
|
|
|
if (!HMD.active) {
|
|
RESET_MODE = false;
|
|
}
|
|
|
|
// advance the timer for turning
|
|
var leftRightMidpoint = (Quat.safeEulerAngles(hipToHandAverage[LEFT]).y + Quat.safeEulerAngles(hipToHandAverage[RIGHT]).y) / 2.0;
|
|
if ((headPosition.y - modeHeight) > -0.02) {
|
|
averageAzimuth = leftRightMidpoint * (0.01) + averageAzimuth * (0.99);
|
|
hipsRotation = Quat.angleAxis(averageAzimuth, { x: 0, y: 1, z: 0 });
|
|
}
|
|
if (Math.abs(leftRightMidpoint) > 20) {
|
|
if (Math.abs(leftRightMidpoint) > 60) {
|
|
azimuthChangeTimer += 2 * dt;
|
|
} else {
|
|
azimuthChangeTimer += dt;
|
|
}
|
|
} else {
|
|
azimuthChangeTimer = 0;
|
|
}
|
|
|
|
// if (azimuthChangeTimer > AZIMUTH_TIME_SECS) {
|
|
if (Math.abs(headPoseAverageEulers.y) > 25.0) {
|
|
// Turn feet
|
|
if (STEPTURN) {
|
|
MyAvatar.triggerRotationRecenter();
|
|
headPoseAverageOrientation = { x: 0, y: 0, z: 0, w: 1 };
|
|
}
|
|
// lastHeadAzimuth = averageAzimuth;
|
|
// azimuthChangeTimer = 0;
|
|
}
|
|
}
|
|
|
|
function shutdownTabletApp() {
|
|
// GlobalDebugger.stop();
|
|
tablet.removeButton(tabletButton);
|
|
if (activated) {
|
|
tablet.webEventReceived.disconnect(onWebEventReceived);
|
|
tablet.gotoHomeScreen();
|
|
}
|
|
tablet.screenChanged.disconnect(onScreenChanged);
|
|
}
|
|
|
|
tabletButton.clicked.connect(manageClick);
|
|
tablet.screenChanged.connect(onScreenChanged);
|
|
|
|
Script.update.connect(update);
|
|
Controller.keyPressEvent.connect(onKeyPress);
|
|
Script.scriptEnding.connect(function () {
|
|
Script.update.disconnect(update);
|
|
shutdownTabletApp();
|
|
});
|