Merge pull request #3429 from ctrlaltdavid/20021

Code Review for Job #20021

examples/leapHands.js

@@ -0,0 +1,395 @@
+//
+//  leapHands.js
+//  examples
+//
+//  Created by David Rowe on 8 Sep 2014.
+//  Copyright 2014 High Fidelity, Inc.
+//
+//  This is an example script that uses the Leap Motion to make the avatar's hands replicate the user's hand actions.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+var leapHands = (function () {
+
+    var hands,
+        wrists,
+        NUM_HANDS = 2,  // 0 = left; 1 = right
+        fingers,
+        NUM_FINGERS = 5,  // 0 = thumb; ...; 4 = pinky
+        THUMB = 0,
+        NUM_FINGER_JOINTS = 3,  // 0 = metacarpal(hand)-proximal(finger) joint; ...; 2 = intermediate-distal(tip) joint
+        MAX_HAND_INACTIVE_COUNT = 20,
+        calibrationStatus,
+        UNCALIBRATED = 0,
+        CALIBRATING = 1,
+        CALIBRATED = 2,
+        CALIBRATION_TIME = 1000,  // milliseconds
+        PI = 3.141593,
+        isWindows;
+
+    function printSkeletonJointNames() {
+        var jointNames,
+            i;
+
+        print(MyAvatar.skeletonModelURL);
+
+        print("Skeleton joint names ...");
+        jointNames = MyAvatar.getJointNames();
+        for (i = 0; i < jointNames.length; i += 1) {
+            print(i + ": " + jointNames[i]);
+        }
+        print("... skeleton joint names");
+
+        /*
+        http://public.highfidelity.io/models/skeletons/ron_standing.fst
+        Skeleton joint names ...
+        0: Hips
+        1: RightUpLeg
+        2: RightLeg
+        3: RightFoot
+        4: RightToeBase
+        5: RightToe_End
+        6: LeftUpLeg
+        7: LeftLeg
+        8: LeftFoot
+        9: LeftToeBase
+        10: LeftToe_End
+        11: Spine
+        12: Spine1
+        13: Spine2
+        14: RightShoulder
+        15: RightArm
+        16: RightForeArm
+        17: RightHand
+        18: RightHandPinky1
+        19: RightHandPinky2
+        20: RightHandPinky3
+        21: RightHandPinky4
+        22: RightHandRing1
+        23: RightHandRing2
+        24: RightHandRing3
+        25: RightHandRing4
+        26: RightHandMiddle1
+        27: RightHandMiddle2
+        28: RightHandMiddle3
+        29: RightHandMiddle4
+        30: RightHandIndex1
+        31: RightHandIndex2
+        32: RightHandIndex3
+        33: RightHandIndex4
+        34: RightHandThumb1
+        35: RightHandThumb2
+        36: RightHandThumb3
+        37: RightHandThumb4
+        38: LeftShoulder
+        39: LeftArm
+        40: LeftForeArm
+        41: LeftHand
+        42: LeftHandPinky1
+        43: LeftHandPinky2
+        44: LeftHandPinky3
+        45: LeftHandPinky4
+        46: LeftHandRing1
+        47: LeftHandRing2
+        48: LeftHandRing3
+        49: LeftHandRing4
+        50: LeftHandMiddle1
+        51: LeftHandMiddle2
+        52: LeftHandMiddle3
+        53: LeftHandMiddle4
+        54: LeftHandIndex1
+        55: LeftHandIndex2
+        56: LeftHandIndex3
+        57: LeftHandIndex4
+        58: LeftHandThumb1
+        59: LeftHandThumb2
+        60: LeftHandThumb3
+        61: LeftHandThumb4
+        62: Neck
+        63: Head
+        64: HeadTop_End
+        65: body
+        ... skeleton joint names
+        */
+    }
+
+    function finishCalibration() {
+        var avatarPosition,
+            avatarOrientation,
+            avatarHandPosition,
+            leapHandHeight,
+            h;
+
+        if (hands[0].controller.isActive() && hands[1].controller.isActive()) {
+            leapHandHeight = (hands[0].controller.getAbsTranslation().y + hands[1].controller.getAbsTranslation().y) / 2.0;
+
+            // TODO: Temporary detection of Windows to work around Leap Controller problem.
+            isWindows = (hands[1].controller.getAbsRotation().z > (0.25 * PI));
+        } else {
+            calibrationStatus = UNCALIBRATED;
+            return;
+        }
+
+        avatarPosition = MyAvatar.position;
+        avatarOrientation = MyAvatar.orientation;
+
+        for (h = 0; h < NUM_HANDS; h += 1) {
+            avatarHandPosition = MyAvatar.getJointPosition(hands[h].jointName);
+            hands[h].zeroPosition = {
+                x: avatarHandPosition.x - avatarPosition.x,
+                y: avatarHandPosition.y - avatarPosition.y,
+                z: avatarPosition.z - avatarHandPosition.z
+            };
+            hands[h].zeroPosition = Vec3.multiplyQbyV(MyAvatar.orientation, hands[h].zeroPosition);
+            hands[h].zeroPosition.y = hands[h].zeroPosition.y - leapHandHeight;
+        }
+
+        MyAvatar.clearJointData("LeftHand");
+        MyAvatar.clearJointData("LeftForeArm");
+        MyAvatar.clearJointData("LeftArm");
+        MyAvatar.clearJointData("RightHand");
+        MyAvatar.clearJointData("RightForeArm");
+        MyAvatar.clearJointData("RightArm");
+
+        calibrationStatus = CALIBRATED;
+        print("Leap Motion: Calibrated");
+    }
+
+    function calibrate() {
+
+        calibrationStatus = CALIBRATING;
+
+        // Set avatar arms vertical, forearms horizontal, as "zero" position for calibration
+        MyAvatar.setJointData("LeftArm", Quat.fromPitchYawRollDegrees(90.0, 0.0, -90.0));
+        MyAvatar.setJointData("LeftForeArm", Quat.fromPitchYawRollDegrees(90.0, 0.0, 180.0));
+        MyAvatar.setJointData("LeftHand", Quat.fromPitchYawRollRadians(0.0, 0.0, 0.0));
+        MyAvatar.setJointData("RightArm", Quat.fromPitchYawRollDegrees(90.0, 0.0, 90.0));
+        MyAvatar.setJointData("RightForeArm", Quat.fromPitchYawRollDegrees(90.0, 0.0, 180.0));
+        MyAvatar.setJointData("RightHand", Quat.fromPitchYawRollRadians(0.0, 0.0, 0.0));
+
+        // Wait for arms to assume their positions before calculating
+        Script.setTimeout(finishCalibration, CALIBRATION_TIME);
+    }
+
+    function checkCalibration() {
+
+        if (calibrationStatus === CALIBRATED) {
+            return true;
+        }
+
+        if (calibrationStatus !== CALIBRATING) {
+            calibrate();
+        }
+
+        return false;
+    }
+
+    function setUp() {
+
+        calibrationStatus = UNCALIBRATED;
+
+        // TODO: Leap Motion controller joint naming doesn't match up with skeleton joint naming; numbers are out by 1.
+
+        hands = [
+            {
+                jointName: "LeftHand",
+                controller: Controller.createInputController("Spatial", "joint_L_hand"),
+                inactiveCount: 0
+            },
+            {
+                jointName: "RightHand",
+                controller: Controller.createInputController("Spatial", "joint_R_hand"),
+                inactiveCount: 0
+            }
+        ];
+
+        wrists = [
+            { controller: Controller.createInputController("Spatial", "joint_L_wrist") },
+            { controller: Controller.createInputController("Spatial", "joint_R_wrist") }
+        ];
+
+        fingers = [{}, {}];
+        fingers[0] = [
+            [
+                { jointName: "LeftHandThumb1", controller: Controller.createInputController("Spatial", "joint_L_thumb2") },
+                { jointName: "LeftHandThumb2", controller: Controller.createInputController("Spatial", "joint_L_thumb3") },
+                { jointName: "LeftHandThumb3", controller: Controller.createInputController("Spatial", "joint_L_thumb4") }
+            ],
+            [
+                { jointName: "LeftHandIndex1", controller: Controller.createInputController("Spatial", "joint_L_index2") },
+                { jointName: "LeftHandIndex2", controller: Controller.createInputController("Spatial", "joint_L_index3") },
+                { jointName: "LeftHandIndex3", controller: Controller.createInputController("Spatial", "joint_L_index4") }
+            ],
+            [
+                { jointName: "LeftHandMiddle1", controller: Controller.createInputController("Spatial", "joint_L_middle2") },
+                { jointName: "LeftHandMiddle2", controller: Controller.createInputController("Spatial", "joint_L_middle3") },
+                { jointName: "LeftHandMiddle3", controller: Controller.createInputController("Spatial", "joint_L_middle4") }
+            ],
+            [
+                { jointName: "LeftHandRing1", controller: Controller.createInputController("Spatial", "joint_L_ring2") },
+                { jointName: "LeftHandRing2", controller: Controller.createInputController("Spatial", "joint_L_ring3") },
+                { jointName: "LeftHandRing3", controller: Controller.createInputController("Spatial", "joint_L_ring4") }
+            ],
+            [
+                { jointName: "LeftHandPinky1", controller: Controller.createInputController("Spatial", "joint_L_pinky2") },
+                { jointName: "LeftHandPinky2", controller: Controller.createInputController("Spatial", "joint_L_pinky3") },
+                { jointName: "LeftHandPinky3", controller: Controller.createInputController("Spatial", "joint_L_pinky4") }
+            ]
+        ];
+        fingers[1] = [
+            [
+                { jointName: "RightHandThumb1", controller: Controller.createInputController("Spatial", "joint_R_thumb2") },
+                { jointName: "RightHandThumb2", controller: Controller.createInputController("Spatial", "joint_R_thumb3") },
+                { jointName: "RightHandThumb3", controller: Controller.createInputController("Spatial", "joint_R_thumb4") }
+            ],
+            [
+                { jointName: "RightHandIndex1", controller: Controller.createInputController("Spatial", "joint_R_index2") },
+                { jointName: "RightHandIndex2", controller: Controller.createInputController("Spatial", "joint_R_index3") },
+                { jointName: "RightHandIndex3", controller: Controller.createInputController("Spatial", "joint_R_index4") }
+            ],
+            [
+                { jointName: "RightHandMiddle1", controller: Controller.createInputController("Spatial", "joint_R_middle2") },
+                { jointName: "RightHandMiddle2", controller: Controller.createInputController("Spatial", "joint_R_middle3") },
+                { jointName: "RightHandMiddle3", controller: Controller.createInputController("Spatial", "joint_R_middle4") }
+            ],
+            [
+                { jointName: "RightHandRing1", controller: Controller.createInputController("Spatial", "joint_R_ring2") },
+                { jointName: "RightHandRing2", controller: Controller.createInputController("Spatial", "joint_R_ring3") },
+                { jointName: "RightHandRing3", controller: Controller.createInputController("Spatial", "joint_R_ring4") }
+            ],
+            [
+                { jointName: "RightHandPinky1", controller: Controller.createInputController("Spatial", "joint_R_pinky2") },
+                { jointName: "RightHandPinky2", controller: Controller.createInputController("Spatial", "joint_R_pinky3") },
+                { jointName: "RightHandPinky3", controller: Controller.createInputController("Spatial", "joint_R_pinky4") }
+            ]
+        ];
+    }
+
+    function moveHands() {
+        var h,
+            i,
+            j,
+            side,
+            handOffset,
+            handRoll,
+            handPitch,
+            handYaw,
+            handRotation,
+            wristAbsRotation,
+            locRotation;
+
+        for (h = 0; h < NUM_HANDS; h += 1) {
+            side = h === 0 ? -1.0 : 1.0;
+
+            if (hands[h].controller.isActive()) {
+
+                // Calibrate when and if a controller is first active.
+                if (!checkCalibration()) {
+                    return;
+                }
+
+                // Hand position ...
+                handOffset = hands[h].controller.getAbsTranslation();
+                handOffset = {
+                    x: -handOffset.x,
+                    y: hands[h].zeroPosition.y + handOffset.y,
+                    z: hands[h].zeroPosition.z - handOffset.z
+                };
+
+                // TODO: 2.0* scale factor should not be necessary; Leap Motion controller code needs investigating.
+                handRoll = 2.0 * -hands[h].controller.getAbsRotation().z;
+                wristAbsRotation = wrists[h].controller.getAbsRotation();
+                handPitch = 2.0 * -wristAbsRotation.x;
+                handYaw = 2.0 * wristAbsRotation.y;
+
+                // TODO: Leap Motion controller's right-hand roll calculation only works if physical hand is upside down.
+                // Approximate fix is to add a fudge factor.
+                if (h === 1 && isWindows) {
+                    handRoll = handRoll + 0.6 * PI;
+                }
+
+                // Hand position and orientation ...
+                if (h === 0) {
+                    handRotation = Quat.multiply(Quat.angleAxis(-90.0, { x: 0, y: 1, z: 0 }),
+                        Quat.fromVec3Radians({ x: handRoll, y: handYaw, z: -handPitch }));
+
+
+                } else {
+                    handRotation = Quat.multiply(Quat.angleAxis(90.0, { x: 0, y: 1, z: 0 }),
+                        Quat.fromVec3Radians({ x: -handRoll, y: handYaw, z: handPitch }));
+                }
+                MyAvatar.setJointModelPositionAndOrientation(hands[h].jointName, handOffset, handRotation, true);
+
+                // Finger joints ...
+                // TODO: 2.0 * scale factors should not be necessary; Leap Motion controller code needs investigating.
+                for (i = 0; i < NUM_FINGERS; i += 1) {
+                    for (j = 0; j < NUM_FINGER_JOINTS; j += 1) {
+                        if (fingers[h][i][j].controller !== null) {
+                            locRotation = fingers[h][i][j].controller.getLocRotation();
+                            if (i === THUMB) {
+                                MyAvatar.setJointData(fingers[h][i][j].jointName,
+                                    Quat.fromPitchYawRollRadians(2.0 * side * locRotation.y, 2.0 * -locRotation.z,
+                                        2.0 * side * -locRotation.x));
+                            } else {
+                                MyAvatar.setJointData(fingers[h][i][j].jointName,
+                                    Quat.fromPitchYawRollRadians(2.0 * -locRotation.x, 0.0, 2.0 * -locRotation.y));
+                            }
+                        }
+                    }
+                }
+
+                hands[h].inactiveCount = 0;
+
+            } else {
+
+                hands[h].inactiveCount += 1;
+
+                if (hands[h].inactiveCount === MAX_HAND_INACTIVE_COUNT) {
+                    if (h === 0) {
+                        MyAvatar.clearJointData("LeftHand");
+                        MyAvatar.clearJointData("LeftForeArm");
+                        MyAvatar.clearJointData("LeftArm");
+                    } else {
+                        MyAvatar.clearJointData("RightHand");
+                        MyAvatar.clearJointData("RightForeArm");
+                        MyAvatar.clearJointData("RightArm");
+                    }
+                }
+            }
+        }
+    }
+
+    function tearDown() {
+        var h,
+            i,
+            j;
+
+        for (h = 0; h < NUM_HANDS; h += 1) {
+            Controller.releaseInputController(hands[h].controller);
+            Controller.releaseInputController(wrists[h].controller);
+            for (i = 0; i < NUM_FINGERS; i += 1) {
+                for (j = 0; j < NUM_FINGER_JOINTS; j += 1) {
+                    if (fingers[h][i][j].controller !== null) {
+                        Controller.releaseInputController(fingers[h][i][j].controller);
+                    }
+                }
+            }
+        }
+    }
+
+    return {
+        printSkeletonJointNames: printSkeletonJointNames,
+        setUp : setUp,
+        moveHands : moveHands,
+        tearDown : tearDown
+    };
+}());
+
+
+//leapHands.printSkeletonJointNames();
+
+leapHands.setUp();
+Script.update.connect(leapHands.moveHands);
+Script.scriptEnding.connect(leapHands.tearDown);

interface/src/avatar/Avatar.cpp

@@ -830,6 +830,28 @@ glm::quat Avatar::getJointCombinedRotation(const QString& name) const {
     return rotation;
 }
 
+const float SCRIPT_PRIORITY = DEFAULT_PRIORITY + 1.0f;
+
+void Avatar::setJointModelPositionAndOrientation(int index, glm::vec3 position, const glm::quat& rotation) {
+    if (QThread::currentThread() != thread()) {
+        QMetaObject::invokeMethod(const_cast<Avatar*>(this), "setJointModelPositionAndOrientation", 
+            Qt::BlockingQueuedConnection, Q_ARG(const int, index), Q_ARG(const glm::vec3, position),
+            Q_ARG(const glm::quat&, rotation));
+    } else {
+        _skeletonModel.inverseKinematics(index, position, rotation, SCRIPT_PRIORITY);
+    }
+}
+
+void Avatar::setJointModelPositionAndOrientation(const QString& name, glm::vec3 position, const glm::quat& rotation) {
+    if (QThread::currentThread() != thread()) {
+        QMetaObject::invokeMethod(const_cast<Avatar*>(this), "setJointModelPositionAndOrientation", 
+            Qt::BlockingQueuedConnection, Q_ARG(const QString&, name), Q_ARG(const glm::vec3, position),
+            Q_ARG(const glm::quat&, rotation));
+    } else {
+        _skeletonModel.inverseKinematics(getJointIndex(name), position, rotation, SCRIPT_PRIORITY);
+    }
+}
+
 void Avatar::scaleVectorRelativeToPosition(glm::vec3 &positionToScale) const {
     //Scale a world space vector as if it was relative to the position
     positionToScale = _position + _scale * (positionToScale - _position);

interface/src/avatar/Avatar.h

@@ -151,6 +151,10 @@ public:
     Q_INVOKABLE glm::quat getJointCombinedRotation(int index) const;
     Q_INVOKABLE glm::quat getJointCombinedRotation(const QString& name) const;
     
+    Q_INVOKABLE void setJointModelPositionAndOrientation(int index, const glm::vec3 position, const glm::quat& rotation);
+    Q_INVOKABLE void setJointModelPositionAndOrientation(const QString& name, const glm::vec3 position, 
+        const glm::quat& rotation);
+
     Q_INVOKABLE glm::vec3 getVelocity() const { return _velocity; }
     Q_INVOKABLE glm::vec3 getAcceleration() const { return _acceleration; }
     Q_INVOKABLE glm::vec3 getAngularVelocity() const { return _angularVelocity; }

interface/src/avatar/MyAvatar.cpp

@@ -995,6 +995,7 @@ void MyAvatar::clearJointData(int index) {
     if (QThread::currentThread() == thread()) {
         // HACK: ATM only JS scripts call clearJointData() on MyAvatar so we hardcode the priority
         _skeletonModel.setJointState(index, false, glm::quat(), 0.0f);
+        _skeletonModel.clearJointAnimationPriority(index);
     }
 }
 

interface/src/renderer/Model.h

@@ -183,6 +183,8 @@ public:
     QVector<JointState>& getJointStates() { return _jointStates; }
     const QVector<JointState>& getJointStates() const { return _jointStates; }
  
+    void inverseKinematics(int jointIndex, glm::vec3 position, const glm::quat& rotation, float priority);
+
 protected:
     QSharedPointer<NetworkGeometry> _geometry;
     
@@ -238,8 +240,6 @@ protected:
         bool useRotation = false, int lastFreeIndex = -1, bool allIntermediatesFree = false,
         const glm::vec3& alignment = glm::vec3(0.0f, -1.0f, 0.0f), float priority = 1.0f);
 
-    void inverseKinematics(int jointIndex, glm::vec3 position, const glm::quat& rotation, float priority);
-    
     /// Restores the indexed joint to its default position.
     /// \param fraction the fraction of the default position to apply (i.e., 0.25f to slerp one fourth of the way to
     /// the original position

interface/src/scripting/ControllerScriptingInterface.cpp

@@ -312,7 +312,8 @@ void ControllerScriptingInterface::updateInputControllers() {
 InputController::InputController(int deviceTrackerId, int subTrackerId, QObject* parent) :
     AbstractInputController(),
     _deviceTrackerId(deviceTrackerId),
-    _subTrackerId(subTrackerId)
+    _subTrackerId(subTrackerId),
+    _isActive(false)
 {
 }