moving the hydra grab behavior to JS

examples/hydraMove.js

@@ -19,6 +19,46 @@ var INITIAL_THRUST_MULTPLIER = 1.0;
 var THRUST_INCREASE_RATE = 1.05;
 var MAX_THRUST_MULTIPLIER = 75.0;
 var thrustMultiplier = INITIAL_THRUST_MULTPLIER;
+var grabDelta = { 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; //smallish positive number - used as margin of error for some computations
+var velocity = { x: 0, y: 0, z: 0};
+
+
+var LEFT_PALM = 0;
+var LEFT_BUTTON_4 = 4;
+var RIGHT_PALM = 2;
+var RIGHT_BUTTON_4 = 10;
+
+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 getAndResetGrabDeltaVelocity() {
+    var HAND_GRAB_SCALE_VELOCITY = 5.0;
+    var delta = Vec3.multiply(grabDeltaVelocity, (MyAvatar.scale * HAND_GRAB_SCALE_VELOCITY));
+    grabDeltaVelocity = { x: 0, y: 0, z: 0};
+    var avatarRotation = MyAvatar.orientation;
+    var result = Quat.multiply(avatarRotation, Vec3.multiply(delta, -1));
+    return result;
+}
+
+function getAndResetGrabRotation() {
+    var quatDiff = Quat.multiply(grabCurrentRotation, Quat.inverse(grabStartRotation));
+    grabStartRotation = grabCurrentRotation;
+    return quatDiff;
+}
 
 function flyWithHydra() {
     var deltaTime = 1/60; // approximately our FPS - maybe better to be elapsed time since last call
@@ -70,6 +110,54 @@ function flyWithHydra() {
         var newPitch = MyAvatar.headPitch + (viewJoystickPosition.y * JOYSTICK_PITCH_MAG * deltaTime);
         MyAvatar.headPitch = newPitch;
     }
+    
+    // check for and handle grab behaviors
+    grabbingWithRightHand = Controller.isButtonPressed(RIGHT_BUTTON_4);
+    grabbingWithLeftHand = Controller.isButtonPressed(LEFT_BUTTON_4);
+
+    if (grabbingWithRightHand) {
+        grabDelta = Vec3.sum(grabDelta, Vec3.multiply(Controller.getSpatialControlVelocity(RIGHT_PALM), deltaTime));
+        grabCurrentRotation = Controller.getSpatialControlRawRotation(RIGHT_PALM);
+    }
+    if (!grabbingWithRightHand && wasGrabbingWithRightHand) {
+        // Just ending grab, capture velocity
+        grabDeltaVelocity = Controller.getSpatialControlVelocity(RIGHT_PALM);
+    }
+    if (grabbingWithRightHand && !wasGrabbingWithRightHand) {
+        // Just starting grab, capture starting rotation
+        grabStartRotation = Controller.getSpatialControlRawRotation(RIGHT_PALM);
+    }
+
+    grabbing = grabbingWithRightHand || grabbingWithLeftHand;
+
+    if (grabbing) {
+    
+        // move position
+        var moveFromGrab = getAndResetGrabDelta();
+        if (Vec3.length(moveFromGrab) > EPSILON) {
+            position = Vec3.sum(position, moveFromGrab);
+            velocity = { x: 0, y: 0, z: 0};
+        }
+        
+        // add some velocity...
+        velocity = Vec3.sum(velocity, getAndResetGrabDeltaVelocity());
+        
+        // add some rotation...
+        var deltaRotation = getAndResetGrabRotation();
+        var GRAB_CONTROLLER_TURN_SCALING = 0.5;
+        var euler = Vec3.multiply(Quat.safeEulerAngles(deltaRotation), GRAB_CONTROLLER_TURN_SCALING);
+    
+        //  Adjust body yaw by yaw from controller
+        var orientation = Quat.multiply(Quat.angleAxis(-euler.y, {x:0, y: 1, z:0}), MyAvatar.orientation);
+        MyAvatar.orientation = orientation;
+
+        //  Adjust head pitch from controller
+        //getHead().setPitch(getHead().getPitch() - euler.x);
+        MyAvatar.headPitch = MyAvatar.headPitch - euler.x;
+    }
+    
+    wasGrabbingWithRightHand = grabbingWithRightHand;
+    wasGrabbingWithLeftHand = grabbingWithLeftHand;
 }
 
 Script.willSendVisualDataCallback.connect(flyWithHydra);

interface/src/ControllerScriptingInterface.cpp

@@ -172,6 +172,21 @@ glm::vec3 ControllerScriptingInterface::getSpatialControlVelocity(int controlInd
     return glm::vec3(0); // bad index
 }
 
+glm::quat ControllerScriptingInterface::getSpatialControlRawRotation(int controlIndex) const {
+    int palmIndex = controlIndex / NUMBER_OF_SPATIALCONTROLS_PER_PALM;
+    int controlOfPalm = controlIndex % NUMBER_OF_SPATIALCONTROLS_PER_PALM;
+    const PalmData* palmData = getActivePalm(palmIndex);
+    if (palmData) {
+        switch (controlOfPalm) {
+            case PALM_SPATIALCONTROL:
+                return palmData->getRawRotation();
+            case TIP_SPATIALCONTROL:
+                return palmData->getRawRotation(); // currently the tip doesn't have a unique rotation, use the palm rotation
+        }
+    }
+    return glm::quat(); // bad index
+}
+
 glm::vec3 ControllerScriptingInterface::getSpatialControlNormal(int controlIndex) const {
     int palmIndex = controlIndex / NUMBER_OF_SPATIALCONTROLS_PER_PALM;
     int controlOfPalm = controlIndex % NUMBER_OF_SPATIALCONTROLS_PER_PALM;

interface/src/ControllerScriptingInterface.h

@@ -58,7 +58,7 @@ public slots:
     virtual glm::vec3 getSpatialControlPosition(int controlIndex) const;
     virtual glm::vec3 getSpatialControlVelocity(int controlIndex) const;
     virtual glm::vec3 getSpatialControlNormal(int controlIndex) const;
-
+    virtual glm::quat getSpatialControlRawRotation(int controlIndex) const;
     virtual void captureKeyEvents(const KeyEvent& event);
     virtual void releaseKeyEvents(const KeyEvent& event);
 

interface/src/avatar/Hand.cpp

@@ -32,11 +32,7 @@ Hand::Hand(Avatar* owningAvatar) :
     _ballColor(0.0, 0.0, 0.4),
     _collisionCenter(0,0,0),
     _collisionAge(0),
-    _collisionDuration(0),
-    _grabDelta(0, 0, 0),
-    _grabDeltaVelocity(0, 0, 0),
-    _grabStartRotation(0, 0, 0, 1),
-    _grabCurrentRotation(0, 0, 0, 1)
+    _collisionDuration(0)
 {
 }
 
@@ -53,28 +49,6 @@ void Hand::init() {
 void Hand::reset() {
 }
 
-glm::vec3 Hand::getAndResetGrabDelta() {
-    const float HAND_GRAB_SCALE_DISTANCE = 2.f;
-    glm::vec3 delta = _grabDelta * _owningAvatar->getScale() * HAND_GRAB_SCALE_DISTANCE;
-    _grabDelta = glm::vec3(0,0,0);
-    glm::quat avatarRotation = _owningAvatar->getOrientation();
-    return avatarRotation * -delta;
-}
-
-glm::vec3 Hand::getAndResetGrabDeltaVelocity() {
-    const float HAND_GRAB_SCALE_VELOCITY = 5.f;
-    glm::vec3 delta = _grabDeltaVelocity * _owningAvatar->getScale() * HAND_GRAB_SCALE_VELOCITY;
-    _grabDeltaVelocity = glm::vec3(0,0,0);
-    glm::quat avatarRotation = _owningAvatar->getOrientation();
-    return avatarRotation * -delta;
-    
-}
-glm::quat Hand::getAndResetGrabRotation() {
-    glm::quat diff = _grabCurrentRotation * glm::inverse(_grabStartRotation);
-    _grabStartRotation = _grabCurrentRotation;
-    return diff;
-}
-
 void Hand::simulate(float deltaTime, bool isMine) {
     
     if (_collisionAge > 0.f) {
@@ -100,19 +74,6 @@ void Hand::simulate(float deltaTime, bool isMine) {
                 
                 _buckyBalls.grab(palm, fingerTipPosition, _owningAvatar->getOrientation(), deltaTime);
                 
-                if (palm.getControllerButtons() & BUTTON_4) {
-                    _grabDelta +=  palm.getRawVelocity() * deltaTime;
-                    _grabCurrentRotation = palm.getRawRotation();
-                }
-                if ((palm.getLastControllerButtons() & BUTTON_4) && !(palm.getControllerButtons() & BUTTON_4)) {
-                    // Just ending grab, capture velocity
-                    _grabDeltaVelocity = palm.getRawVelocity();
-                }
-                if (!(palm.getLastControllerButtons() & BUTTON_4) && (palm.getControllerButtons() & BUTTON_4)) {
-                    // Just starting grab, capture starting rotation
-                    _grabStartRotation = palm.getRawRotation();
-                }
-                
                 if (palm.getControllerButtons() & BUTTON_1) {
                     if (glm::length(fingerTipPosition - _lastFingerAddVoxel) > (FINGERTIP_VOXEL_SIZE / 2.f)) {
                         QColor paintColor = Menu::getInstance()->getActionForOption(MenuOption::VoxelPaintColor)->data().value<QColor>();

interface/src/avatar/Hand.h

@@ -58,11 +58,6 @@ public:
     const glm::vec3& getLeapFingerTipBallPosition (int ball) const { return _leapFingerTipBalls [ball].position;}
     const glm::vec3& getLeapFingerRootBallPosition(int ball) const { return _leapFingerRootBalls[ball].position;}
     
-    // Get the drag distance to move
-    glm::vec3 getAndResetGrabDelta();
-    glm::vec3 getAndResetGrabDeltaVelocity();
-    glm::quat getAndResetGrabRotation();
-
 private:
     // disallow copies of the Hand, copy of owning Avatar is disallowed too
     Hand(const Hand&);
@@ -96,11 +91,6 @@ private:
     void calculateGeometry();
     
     void handleVoxelCollision(PalmData* palm, const glm::vec3& fingerTipPosition, VoxelTreeElement* voxel, float deltaTime);
-    
-    glm::vec3 _grabDelta;
-    glm::vec3 _grabDeltaVelocity;
-    glm::quat _grabStartRotation;
-    glm::quat _grabCurrentRotation;
 };
 
 #endif

interface/src/avatar/MyAvatar.cpp

@@ -323,22 +323,6 @@ void MyAvatar::simulate(float deltaTime) {
 
     updateChatCircle(deltaTime);
 
-    // TODO: this should be removed and turned into JS instead
-    //  Get any position, velocity, or rotation update from Grab Drag controller
-    glm::vec3 moveFromGrab = _hand.getAndResetGrabDelta();
-    if (glm::length(moveFromGrab) > EPSILON) {
-        _position += moveFromGrab;
-        _velocity = glm::vec3(0, 0, 0);
-    }
-    _velocity += _hand.getAndResetGrabDeltaVelocity();
-    glm::quat deltaRotation = _hand.getAndResetGrabRotation();
-    const float GRAB_CONTROLLER_TURN_SCALING = 0.5f;
-    glm::vec3 euler = safeEulerAngles(deltaRotation) * GRAB_CONTROLLER_TURN_SCALING;
-    //  Adjust body yaw by yaw from controller
-    setOrientation(glm::angleAxis(-euler.y, glm::vec3(0, 1, 0)) * getOrientation());
-    //  Adjust head pitch from controller
-    getHead().setPitch(getHead().getPitch() - euler.x);
-
     _position += _velocity * deltaTime;
 
     // update avatar skeleton and simulate hand and head

libraries/script-engine/src/AbstractControllerScriptingInterface.h

@@ -12,6 +12,7 @@
 #include <QtCore/QObject>
 
 #include <glm/glm.hpp>
+#include <glm/gtc/quaternion.hpp>
 
 #include "EventTypes.h"
 
@@ -37,6 +38,7 @@ public slots:
     virtual glm::vec3 getSpatialControlPosition(int controlIndex) const = 0;
     virtual glm::vec3 getSpatialControlVelocity(int controlIndex) const = 0;
     virtual glm::vec3 getSpatialControlNormal(int controlIndex) const = 0;
+    virtual glm::quat getSpatialControlRawRotation(int controlIndex) const = 0;
 
     virtual void captureKeyEvents(const KeyEvent& event) = 0;
     virtual void releaseKeyEvents(const KeyEvent& event) = 0;

libraries/script-engine/src/Quat.cpp

@@ -9,6 +9,8 @@
 //
 //
 
+#include <glm/gtx/vector_angle.hpp>
+
 #include <OctreeConstants.h>
 #include <SharedUtil.h>
 #include "Quat.h"
@@ -25,6 +27,11 @@ glm::quat Quat::fromPitchYawRoll(float pitch, float yaw, float roll) {
     return glm::quat(glm::radians(glm::vec3(pitch, yaw, roll)));
 }
 
+glm::quat Quat::inverse(const glm::quat& q) {
+    return glm::inverse(q);
+}
+
+
 glm::vec3 Quat::getFront(const glm::quat& orientation) {
     return orientation * IDENTITY_FRONT;
 }
@@ -38,5 +45,10 @@ glm::vec3 Quat::getUp(const glm::quat& orientation) {
 }
 
 glm::vec3 Quat::safeEulerAngles(const glm::quat& orientation) {
-    return safeEulerAngles(orientation);
+    return ::safeEulerAngles(orientation);
 }
+
+glm::quat Quat::angleAxis(float angle, const glm::vec3& v) {
+    return glm::angleAxis(angle, v);
+}
+

libraries/script-engine/src/Quat.h

@@ -23,10 +23,12 @@ public slots:
     glm::quat multiply(const glm::quat& q1, const glm::quat& q2);
     glm::quat fromVec3(const glm::vec3& vec3);
     glm::quat fromPitchYawRoll(float pitch, float yaw, float roll);
+    glm::quat inverse(const glm::quat& q);
     glm::vec3 getFront(const glm::quat& orientation);
     glm::vec3 getRight(const glm::quat& orientation);
     glm::vec3 getUp(const glm::quat& orientation);
     glm::vec3 safeEulerAngles(const glm::quat& orientation);
+    glm::quat angleAxis(float angle, const glm::vec3& v);
 };
 
 

libraries/script-engine/src/Vec3.cpp

@@ -19,6 +19,14 @@ glm::vec3 Vec3::multiply(const glm::vec3& v1, float f) {
     return v1 * f;
 }
 
+glm::vec3 Vec3::multiplyQbyV(const glm::quat& q, const glm::vec3& v) {
+    return q * v;
+}
+
 glm::vec3 Vec3::sum(const glm::vec3& v1, const glm::vec3& v2) {
     return v1 + v2;
 }
+
+float Vec3::length(const glm::vec3& v) {
+    return glm::length(v);
+}

libraries/script-engine/src/Vec3.h

@@ -23,7 +23,9 @@ class Vec3 : public QObject {
 public slots:
     glm::vec3 multiply(const glm::vec3& v1, const glm::vec3& v2);
     glm::vec3 multiply(const glm::vec3& v1, float f);
+    glm::vec3 multiplyQbyV(const glm::quat& q, const glm::vec3& v);
     glm::vec3 sum(const glm::vec3& v1, const glm::vec3& v2);
+    float length(const glm::vec3& v);
 };