From 2db87e0d2d094c0891f35a6eced4428e5af065d4 Mon Sep 17 00:00:00 2001 From: samcake Date: Thu, 22 Oct 2015 15:55:36 -0700 Subject: [PATCH 1/3] Remove hydramove.js and expose the velocity and angular Velocity for hydra --- examples/controllers/hydra/hydraMove.js | 303 ------------------ .../src/input-plugins/SixenseManager.cpp | 20 +- .../src/input-plugins/SixenseManager.h | 2 +- 3 files changed, 18 insertions(+), 307 deletions(-) delete mode 100644 examples/controllers/hydra/hydraMove.js diff --git a/examples/controllers/hydra/hydraMove.js b/examples/controllers/hydra/hydraMove.js deleted file mode 100644 index 46715839c3..0000000000 --- a/examples/controllers/hydra/hydraMove.js +++ /dev/null @@ -1,303 +0,0 @@ -// -// hydraMove.js -// examples -// -// Created by Brad Hefta-Gaub on February 10, 2014 -// Updated by Philip Rosedale on September 8, 2014 -// -// Copyright 2014 High Fidelity, Inc. -// -// This is an example script that demonstrates use of the Controller and MyAvatar classes to implement -// avatar flying through the hydra/controller joysticks -// -// The joysticks (on hydra) will drive the avatar much like a playstation controller. -// -// Pressing the '4' or the 'FWD' button and moving/banking the hand will allow you to move and fly. -// -// Distributed under the Apache License, Version 2.0. -// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html -// - -var damping = 0.9; -var position = { x: MyAvatar.position.x, y: MyAvatar.position.y, z: MyAvatar.position.z }; -var joysticksCaptured = false; -var THRUST_CONTROLLER = 0; -var VIEW_CONTROLLER = 1; -var INITIAL_THRUST_MULTIPLIER = 1.0; -var THRUST_INCREASE_RATE = 1.05; -var MAX_THRUST_MULTIPLIER = 75.0; -var thrustMultiplier = INITIAL_THRUST_MULTIPLIER; -var grabDelta = { x: 0, y: 0, z: 0}; -var grabStartPosition = { x: 0, y: 0, z: 0}; -var grabDeltaVelocity = { x: 0, y: 0, z: 0}; -var grabStartRotation = { x: 0, y: 0, z: 0, w: 1}; -var grabCurrentRotation = { x: 0, y: 0, z: 0, w: 1}; -var grabbingWithRightHand = false; -var wasGrabbingWithRightHand = false; -var grabbingWithLeftHand = false; -var wasGrabbingWithLeftHand = false; - -var EPSILON = 0.000001; -var velocity = { x: 0, y: 0, z: 0}; -var THRUST_MAG_UP = 100.0; -var THRUST_MAG_DOWN = 100.0; -var THRUST_MAG_FWD = 150.0; -var THRUST_MAG_BACK = 100.0; -var THRUST_MAG_LATERAL = 150.0; -var THRUST_JUMP = 120.0; - -var YAW_MAG = 100.0; -var PITCH_MAG = 100.0; -var THRUST_MAG_HAND_JETS = THRUST_MAG_FWD; -var JOYSTICK_YAW_MAG = YAW_MAG; -var JOYSTICK_PITCH_MAG = PITCH_MAG * 0.5; - - -var LEFT_PALM = 0; -var LEFT_BUTTON_4 = 4; -var LEFT_BUTTON_FWD = 5; -var RIGHT_PALM = 2; -var RIGHT_BUTTON_4 = 10; -var RIGHT_BUTTON_FWD = 11; - - - -function printVector(text, v, decimals) { - print(text + " " + v.x.toFixed(decimals) + ", " + v.y.toFixed(decimals) + ", " + v.z.toFixed(decimals)); -} - -var debug = false; -var RED_COLOR = { red: 255, green: 0, blue: 0 }; -var GRAY_COLOR = { red: 25, green: 25, blue: 25 }; -var defaultPosition = { x: 0, y: 0, z: 0}; -var RADIUS = 0.05; -var greenSphere = -1; -var redSphere = -1; - -function createDebugOverlay() { - - if (greenSphere == -1) { - greenSphere = Overlays.addOverlay("sphere", { - position: defaultPosition, - size: RADIUS, - color: GRAY_COLOR, - alpha: 0.75, - visible: true, - solid: true, - anchor: "MyAvatar" - }); - redSphere = Overlays.addOverlay("sphere", { - position: defaultPosition, - size: RADIUS, - color: RED_COLOR, - alpha: 0.5, - visible: true, - solid: true, - anchor: "MyAvatar" - }); - } -} - -function destroyDebugOverlay() { - if (greenSphere != -1) { - Overlays.deleteOverlay(greenSphere); - Overlays.deleteOverlay(redSphere); - greenSphere = -1; - redSphere = -1; - } -} - -function displayDebug() { - if (!(grabbingWithRightHand || grabbingWithLeftHand)) { - if (greenSphere != -1) { - destroyDebugOverlay(); - } - } else { - // update debug indicator - if (greenSphere == -1) { - createDebugOverlay(); - } - - var displayOffset = { x:0, y:0.5, z:-0.5 }; - - Overlays.editOverlay(greenSphere, { position: Vec3.sum(grabStartPosition, displayOffset) } ); - Overlays.editOverlay(redSphere, { position: Vec3.sum(Vec3.sum(grabStartPosition, grabDelta), displayOffset), size: RADIUS + (0.25 * Vec3.length(grabDelta)) } ); - } -} - -function getJoystickPosition(palm) { - // returns CONTROLLER_ID position in avatar local frame - var invRotation = Quat.inverse(MyAvatar.orientation); - var palmWorld = Controller.getSpatialControlPosition(palm); - var palmRelative = Vec3.subtract(palmWorld, MyAvatar.position); - var palmLocal = Vec3.multiplyQbyV(invRotation, palmRelative); - return palmLocal; -} - -// Used by handleGrabBehavior() for managing the grab position changes -function getAndResetGrabDelta() { - var HAND_GRAB_SCALE_DISTANCE = 2.0; - var delta = Vec3.multiply(grabDelta, (MyAvatar.scale * HAND_GRAB_SCALE_DISTANCE)); - grabDelta = { x: 0, y: 0, z: 0}; - var avatarRotation = MyAvatar.orientation; - var result = Vec3.multiplyQbyV(avatarRotation, Vec3.multiply(delta, -1)); - return result; -} - -function getGrabRotation() { - var quatDiff = Quat.multiply(grabCurrentRotation, Quat.inverse(grabStartRotation)); - return quatDiff; -} - -// When move button is pressed, process results -function handleGrabBehavior(deltaTime) { - // check for and handle grab behaviors - grabbingWithRightHand = Controller.isButtonPressed(RIGHT_BUTTON_4); - grabbingWithLeftHand = Controller.isButtonPressed(LEFT_BUTTON_4); - stoppedGrabbingWithLeftHand = false; - stoppedGrabbingWithRightHand = false; - - if (grabbingWithRightHand && !wasGrabbingWithRightHand) { - // Just starting grab, capture starting rotation - grabStartRotation = Controller.getSpatialControlRawRotation(RIGHT_PALM); - grabStartPosition = getJoystickPosition(RIGHT_PALM); - if (debug) printVector("start position", grabStartPosition, 3); - } - if (grabbingWithRightHand) { - grabDelta = Vec3.subtract(getJoystickPosition(RIGHT_PALM), grabStartPosition); - grabCurrentRotation = Controller.getSpatialControlRawRotation(RIGHT_PALM); - } - if (!grabbingWithRightHand && wasGrabbingWithRightHand) { - // Just ending grab, capture velocity - grabDeltaVelocity = Controller.getSpatialControlVelocity(RIGHT_PALM); - stoppedGrabbingWithRightHand = true; - } - - if (grabbingWithLeftHand && !wasGrabbingWithLeftHand) { - // Just starting grab, capture starting rotation - grabStartRotation = Controller.getSpatialControlRawRotation(LEFT_PALM); - grabStartPosition = getJoystickPosition(LEFT_PALM); - if (debug) printVector("start position", grabStartPosition, 3); - } - - if (grabbingWithLeftHand) { - grabDelta = Vec3.subtract(getJoystickPosition(LEFT_PALM), grabStartPosition); - grabCurrentRotation = Controller.getSpatialControlRawRotation(LEFT_PALM); - } - if (!grabbingWithLeftHand && wasGrabbingWithLeftHand) { - // Just ending grab, capture velocity - grabDeltaVelocity = Controller.getSpatialControlVelocity(LEFT_PALM); - stoppedGrabbingWithLeftHand = true; - } - - grabbing = grabbingWithRightHand || grabbingWithLeftHand; - stoppedGrabbing = stoppedGrabbingWithRightHand || stoppedGrabbingWithLeftHand; - - if (grabbing) { - - var headOrientation = MyAvatar.headOrientation; - var front = Quat.getFront(headOrientation); - var right = Quat.getRight(headOrientation); - var up = Quat.getUp(headOrientation); - - if (debug) { - printVector("grabDelta: ", grabDelta, 3); - } - - var thrust = Vec3.multiply(grabDelta, Math.abs(Vec3.length(grabDelta))); - - var THRUST_GRAB_SCALING = 100000.0; - - var thrustFront = Vec3.multiply(front, MyAvatar.scale * -thrust.z * THRUST_GRAB_SCALING * deltaTime); - MyAvatar.addThrust(thrustFront); - var thrustRight = Vec3.multiply(right, MyAvatar.scale * thrust.x * THRUST_GRAB_SCALING * deltaTime); - MyAvatar.addThrust(thrustRight); - var thrustUp = Vec3.multiply(up, MyAvatar.scale * thrust.y * THRUST_GRAB_SCALING * deltaTime); - MyAvatar.addThrust(thrustUp); - - // add some rotation... - var deltaRotation = getGrabRotation(); - var PITCH_SCALING = 2.5; - var PITCH_DEAD_ZONE = 2.0; - var YAW_SCALING = 2.5; - var ROLL_SCALING = 2.0; - - var euler = Quat.safeEulerAngles(deltaRotation); - - // Adjust body yaw by roll from controller - var orientation = Quat.multiply(Quat.angleAxis(((euler.y * YAW_SCALING) + - (euler.z * ROLL_SCALING)) * deltaTime, {x:0, y: 1, z:0}), MyAvatar.orientation); - MyAvatar.orientation = orientation; - - // Adjust head pitch from controller - var pitch = 0.0; - if (Math.abs(euler.x) > PITCH_DEAD_ZONE) { - pitch = (euler.x < 0.0) ? (euler.x + PITCH_DEAD_ZONE) : (euler.x - PITCH_DEAD_ZONE); - } - MyAvatar.headPitch = MyAvatar.headPitch + (pitch * PITCH_SCALING * deltaTime); - - // TODO: Add some camera roll proportional to the rate of turn (so it feels like an airplane or roller coaster) - - } - - wasGrabbingWithRightHand = grabbingWithRightHand; - wasGrabbingWithLeftHand = grabbingWithLeftHand; -} - -// Update for joysticks and move button -var THRUST_DEAD_ZONE = 0.1; -var ROTATE_DEAD_ZONE = 0.1; -function flyWithHydra(deltaTime) { - var thrustJoystickPosition = Controller.getJoystickPosition(THRUST_CONTROLLER); - - if (Math.abs(thrustJoystickPosition.x) > THRUST_DEAD_ZONE || Math.abs(thrustJoystickPosition.y) > THRUST_DEAD_ZONE) { - if (thrustMultiplier < MAX_THRUST_MULTIPLIER) { - thrustMultiplier *= 1 + (deltaTime * THRUST_INCREASE_RATE); - } - var headOrientation = MyAvatar.headOrientation; - - var front = Quat.getFront(headOrientation); - var right = Quat.getRight(headOrientation); - var up = Quat.getUp(headOrientation); - - var thrustFront = Vec3.multiply(front, MyAvatar.scale * THRUST_MAG_HAND_JETS * - thrustJoystickPosition.y * thrustMultiplier * deltaTime); - MyAvatar.addThrust(thrustFront); - var thrustRight = Vec3.multiply(right, MyAvatar.scale * THRUST_MAG_HAND_JETS * - thrustJoystickPosition.x * thrustMultiplier * deltaTime); - MyAvatar.addThrust(thrustRight); - } else { - thrustMultiplier = INITIAL_THRUST_MULTIPLIER; - } - - // View Controller - var viewJoystickPosition = Controller.getJoystickPosition(VIEW_CONTROLLER); - if (Math.abs(viewJoystickPosition.x) > ROTATE_DEAD_ZONE || Math.abs(viewJoystickPosition.y) > ROTATE_DEAD_ZONE) { - - // change the body yaw based on our x controller - var orientation = MyAvatar.orientation; - var deltaOrientation = Quat.fromPitchYawRollDegrees(0, (-1 * viewJoystickPosition.x * JOYSTICK_YAW_MAG * deltaTime), 0); - MyAvatar.orientation = Quat.multiply(orientation, deltaOrientation); - - // change the headPitch based on our x controller - var newPitch = MyAvatar.headPitch + (viewJoystickPosition.y * JOYSTICK_PITCH_MAG * deltaTime); - MyAvatar.headPitch = newPitch; - } - handleGrabBehavior(deltaTime); - displayDebug(); - -} - -Script.update.connect(flyWithHydra); -Controller.captureJoystick(THRUST_CONTROLLER); -Controller.captureJoystick(VIEW_CONTROLLER); - -// Map keyPress and mouse move events to our callbacks -function scriptEnding() { - // re-enabled the standard application for touch events - Controller.releaseJoystick(THRUST_CONTROLLER); - Controller.releaseJoystick(VIEW_CONTROLLER); -} -Script.scriptEnding.connect(scriptEnding); - - diff --git a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp b/libraries/input-plugins/src/input-plugins/SixenseManager.cpp index 3dc983cb34..29e60ca5ec 100644 --- a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp +++ b/libraries/input-plugins/src/input-plugins/SixenseManager.cpp @@ -230,7 +230,8 @@ void SixenseManager::update(float deltaTime, bool jointsCaptured) { if (!jointsCaptured) { // Rotation of Palm glm::quat rotation(data->rot_quat[3], data->rot_quat[0], data->rot_quat[1], data->rot_quat[2]); - handlePoseEvent(position, rotation, left); + handlePoseEvent(deltaTime, position, rotation, left); + } else { _poseStateMap.clear(); } @@ -384,7 +385,7 @@ void SixenseManager::handleButtonEvent(unsigned int buttons, bool left) { } } -void SixenseManager::handlePoseEvent(glm::vec3 position, glm::quat rotation, bool left) { +void SixenseManager::handlePoseEvent(float deltaTime, glm::vec3 position, glm::quat rotation, bool left) { #ifdef HAVE_SIXENSE // From ABOVE the sixense coordinate frame looks like this: // @@ -400,6 +401,7 @@ void SixenseManager::handlePoseEvent(glm::vec3 position, glm::quat rotation, boo // | // | // z + auto prevPose = _poseStateMap[left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND]; // Transform the measured position into body frame. position = _avatarRotation * (position + _avatarPosition); @@ -443,8 +445,20 @@ void SixenseManager::handlePoseEvent(glm::vec3 position, glm::quat rotation, boo // TODO: find a shortcut with fewer rotations. rotation = _avatarRotation * postOffset * glm::inverse(sixenseToHand) * rotation * preOffset * sixenseToHand; + glm::vec3 velocity(0.0f); + glm::quat angularVelocity; - _poseStateMap[left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND] = controller::Pose(position, rotation); + if (prevPose.isValid() && deltaTime > std::numeric_limits::epsilon()) { + velocity = (position - prevPose.getTranslation()) / deltaTime; + + auto deltaRot = rotation * glm::conjugate(prevPose.getRotation()); + auto axis = glm::axis(deltaRot); + auto angle = glm::angle(deltaRot); + angularVelocity = glm::angleAxis(angle / deltaTime, axis); + + } + + _poseStateMap[left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND] = controller::Pose(position, rotation, velocity, angularVelocity); #endif // HAVE_SIXENSE } diff --git a/libraries/input-plugins/src/input-plugins/SixenseManager.h b/libraries/input-plugins/src/input-plugins/SixenseManager.h index 91cdb5f60e..368321e669 100644 --- a/libraries/input-plugins/src/input-plugins/SixenseManager.h +++ b/libraries/input-plugins/src/input-plugins/SixenseManager.h @@ -75,7 +75,7 @@ public slots: private: void handleButtonEvent(unsigned int buttons, bool left); void handleAxisEvent(float x, float y, float trigger, bool left); - void handlePoseEvent(glm::vec3 position, glm::quat rotation, bool left); + void handlePoseEvent(float deltaTime, glm::vec3 position, glm::quat rotation, bool left); void updateCalibration(void* controllers); From 0ad4634eac482dc3e68d57a78513573de3c9164f Mon Sep 17 00:00:00 2001 From: samcake Date: Thu, 22 Oct 2015 16:13:12 -0700 Subject: [PATCH 2/3] Check that the velocity information is passed correctly and fixed clap.js --- examples/clap.js | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/examples/clap.js b/examples/clap.js index 4ec740e3f8..54ca937786 100644 --- a/examples/clap.js +++ b/examples/clap.js @@ -71,8 +71,12 @@ function maybePlaySound(deltaTime) { var palm2Position = MyAvatar.getRightPalmPosition(); var distanceBetween = Vec3.length(Vec3.subtract(palm1Position, palm2Position)); - var palm1Velocity = Controller.getSpatialControlVelocity(1); - var palm2Velocity = Controller.getSpatialControlVelocity(3); + var palmPose = Controller.getPoseValue(Controller.Standard.LeftHand); + + + + var palm1Velocity = Controller.getPoseValue(Controller.Standard.LeftHand).velocity; + var palm2Velocity = Controller.getPoseValue(Controller.Standard.RightHand).velocity; var closingVelocity = Vec3.length(Vec3.subtract(palm1Velocity, palm2Velocity)); const CLAP_SPEED = 0.7; From 3e4bada313c65e485a35d014ee11dad213b46ef2 Mon Sep 17 00:00:00 2001 From: samcake Date: Thu, 22 Oct 2015 18:03:47 -0700 Subject: [PATCH 3/3] remove uneeded var --- examples/clap.js | 4 ---- 1 file changed, 4 deletions(-) diff --git a/examples/clap.js b/examples/clap.js index 54ca937786..9b21075ae7 100644 --- a/examples/clap.js +++ b/examples/clap.js @@ -71,10 +71,6 @@ function maybePlaySound(deltaTime) { var palm2Position = MyAvatar.getRightPalmPosition(); var distanceBetween = Vec3.length(Vec3.subtract(palm1Position, palm2Position)); - var palmPose = Controller.getPoseValue(Controller.Standard.LeftHand); - - - var palm1Velocity = Controller.getPoseValue(Controller.Standard.LeftHand).velocity; var palm2Velocity = Controller.getPoseValue(Controller.Standard.RightHand).velocity; var closingVelocity = Vec3.length(Vec3.subtract(palm1Velocity, palm2Velocity));