////  LeapMotionPlugin.cpp////  Created by David Rowe on 15 Jun 2017.//  Copyright 2017 High Fidelity, Inc.////  Distributed under the Apache License, Version 2.0.//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html// #include "LeapMotionPlugin.h" #include <QLoggingCategory> #include <controllers/UserInputMapper.h>#include <NumericalConstants.h>#include <PathUtils.h>#include <Preferences.h>#include <SettingHandle.h> Q_DECLARE_LOGGING_CATEGORY(inputplugins)Q_LOGGING_CATEGORY(inputplugins, "hifi.inputplugins") const char* LeapMotionPlugin::NAME = "LeapMotion";const char* LeapMotionPlugin::LEAPMOTION_ID_STRING = "Leap Motion"; const bool DEFAULT_ENABLED = false;const char* SENSOR_ON_DESKTOP = "Desktop";const char* SENSOR_ON_HMD = "HMD";const char* DEFAULT_SENSOR_LOCATION = SENSOR_ON_DESKTOP; enum LeapMotionJointIndex {    LeftHand = 0,    LeftHandThumb1,    LeftHandThumb2,    LeftHandThumb3,    LeftHandThumb4,    LeftHandIndex1,    LeftHandIndex2,    LeftHandIndex3,    LeftHandIndex4,    LeftHandMiddle1,    LeftHandMiddle2,    LeftHandMiddle3,    LeftHandMiddle4,    LeftHandRing1,    LeftHandRing2,    LeftHandRing3,    LeftHandRing4,    LeftHandPinky1,    LeftHandPinky2,    LeftHandPinky3,    LeftHandPinky4,    RightHand,    RightHandThumb1,    RightHandThumb2,    RightHandThumb3,    RightHandThumb4,    RightHandIndex1,    RightHandIndex2,    RightHandIndex3,    RightHandIndex4,    RightHandMiddle1,    RightHandMiddle2,    RightHandMiddle3,    RightHandMiddle4,    RightHandRing1,    RightHandRing2,    RightHandRing3,    RightHandRing4,    RightHandPinky1,    RightHandPinky2,    RightHandPinky3,    RightHandPinky4,     Size}; static controller::StandardPoseChannel LeapMotionJointIndexToPoseIndexMap[LeapMotionJointIndex::Size] = {    controller::LEFT_HAND,    controller::LEFT_HAND_THUMB1,    controller::LEFT_HAND_THUMB2,    controller::LEFT_HAND_THUMB3,    controller::LEFT_HAND_THUMB4,    controller::LEFT_HAND_INDEX1,    controller::LEFT_HAND_INDEX2,    controller::LEFT_HAND_INDEX3,    controller::LEFT_HAND_INDEX4,    controller::LEFT_HAND_MIDDLE1,    controller::LEFT_HAND_MIDDLE2,    controller::LEFT_HAND_MIDDLE3,    controller::LEFT_HAND_MIDDLE4,    controller::LEFT_HAND_RING1,    controller::LEFT_HAND_RING2,    controller::LEFT_HAND_RING3,    controller::LEFT_HAND_RING4,    controller::LEFT_HAND_PINKY1,    controller::LEFT_HAND_PINKY2,    controller::LEFT_HAND_PINKY3,    controller::LEFT_HAND_PINKY4,    controller::RIGHT_HAND,    controller::RIGHT_HAND_THUMB1,    controller::RIGHT_HAND_THUMB2,    controller::RIGHT_HAND_THUMB3,    controller::RIGHT_HAND_THUMB4,    controller::RIGHT_HAND_INDEX1,    controller::RIGHT_HAND_INDEX2,    controller::RIGHT_HAND_INDEX3,    controller::RIGHT_HAND_INDEX4,    controller::RIGHT_HAND_MIDDLE1,    controller::RIGHT_HAND_MIDDLE2,    controller::RIGHT_HAND_MIDDLE3,    controller::RIGHT_HAND_MIDDLE4,    controller::RIGHT_HAND_RING1,    controller::RIGHT_HAND_RING2,    controller::RIGHT_HAND_RING3,    controller::RIGHT_HAND_RING4,    controller::RIGHT_HAND_PINKY1,    controller::RIGHT_HAND_PINKY2,    controller::RIGHT_HAND_PINKY3,    controller::RIGHT_HAND_PINKY4}; #define UNKNOWN_JOINT (controller::StandardPoseChannel)0  static controller::StandardPoseChannel LeapMotionJointIndexToPoseIndex(LeapMotionJointIndex i) {    assert(i >= 0 && i < LeapMotionJointIndex::Size);    if (i >= 0 && i < LeapMotionJointIndex::Size) {        return LeapMotionJointIndexToPoseIndexMap[i];    } else {        return UNKNOWN_JOINT;    }} QStringList controllerJointNames = {    "Hips",    "RightUpLeg",    "RightLeg",    "RightFoot",    "LeftUpLeg",    "LeftLeg",    "LeftFoot",    "Spine",    "Spine1",    "Spine2",    "Spine3",    "Neck",    "Head",    "RightShoulder",    "RightArm",    "RightForeArm",    "RightHand",    "RightHandThumb1",    "RightHandThumb2",    "RightHandThumb3",    "RightHandThumb4",    "RightHandIndex1",    "RightHandIndex2",    "RightHandIndex3",    "RightHandIndex4",    "RightHandMiddle1",    "RightHandMiddle2",    "RightHandMiddle3",    "RightHandMiddle4",    "RightHandRing1",    "RightHandRing2",    "RightHandRing3",    "RightHandRing4",    "RightHandPinky1",    "RightHandPinky2",    "RightHandPinky3",    "RightHandPinky4",    "LeftShoulder",    "LeftArm",    "LeftForeArm",    "LeftHand",    "LeftHandThumb1",    "LeftHandThumb2",    "LeftHandThumb3",    "LeftHandThumb4",    "LeftHandIndex1",    "LeftHandIndex2",    "LeftHandIndex3",    "LeftHandIndex4",    "LeftHandMiddle1",    "LeftHandMiddle2",    "LeftHandMiddle3",    "LeftHandMiddle4",    "LeftHandRing1",    "LeftHandRing2",    "LeftHandRing3",    "LeftHandRing4",    "LeftHandPinky1",    "LeftHandPinky2",    "LeftHandPinky3",    "LeftHandPinky4"}; static const char* getControllerJointName(controller::StandardPoseChannel i) {    if (i >= 0 && i < controller::NUM_STANDARD_POSES) {        return qPrintable(controllerJointNames[i]);    }    return "unknown";} void LeapMotionPlugin::pluginUpdate(float deltaTime, const controller::InputCalibrationData& inputCalibrationData) {    if (!_enabled) {        return;    }     const auto frame = _controller.frame();    const auto frameID = frame.id();    if (_lastFrameID >= frameID) {        // Leap Motion not connected or duplicate frame.        return;    }     if (!_hasLeapMotionBeenConnected) {        emit deviceConnected(getName());        _hasLeapMotionBeenConnected = true;    }     processFrame(frame);  // Updates _joints.     auto joints = _joints;     auto userInputMapper = DependencyManager::get<controller::UserInputMapper>();    userInputMapper->withLock([&, this]() {        _inputDevice->update(deltaTime, inputCalibrationData, joints, _prevJoints);    });     _prevJoints = joints;     _lastFrameID = frameID;} controller::Input::NamedVector LeapMotionPlugin::InputDevice::getAvailableInputs() const {    static controller::Input::NamedVector availableInputs;    if (availableInputs.size() == 0) {        for (int i = 0; i < LeapMotionJointIndex::Size; i++) {            auto channel = LeapMotionJointIndexToPoseIndex(static_cast<LeapMotionJointIndex>(i));            availableInputs.push_back(makePair(channel, getControllerJointName(channel)));        }    };    return availableInputs;} QString LeapMotionPlugin::InputDevice::getDefaultMappingConfig() const {    static const QString MAPPING_JSON = PathUtils::resourcesPath() + "/controllers/leapmotion.json";    return MAPPING_JSON;} void LeapMotionPlugin::InputDevice::update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData,        const std::vector<LeapMotionPlugin::LeapMotionJoint>& joints,        const std::vector<LeapMotionPlugin::LeapMotionJoint>& prevJoints) {     glm::mat4 controllerToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat;    glm::quat controllerToAvatarRotation = glmExtractRotation(controllerToAvatar);     glm::vec3 hmdSensorPosition;    // HMD    glm::quat hmdSensorOrientation; // HMD    glm::vec3 leapMotionOffset;     // Desktop    if (_isLeapOnHMD) {        hmdSensorPosition = extractTranslation(inputCalibrationData.hmdSensorMat);        hmdSensorOrientation = extractRotation(inputCalibrationData.hmdSensorMat);    } else {        // Desktop "zero" position is some distance above the Leap Motion sensor and half the avatar's shoulder-to-hand length         // in front of avatar.        float halfShouldToHandLength = fabsf(extractTranslation(inputCalibrationData.defaultLeftHand).x            - extractTranslation(inputCalibrationData.defaultLeftArm).x) / 2.0f;        leapMotionOffset = glm::vec3(0.0f, _desktopHeightOffset, halfShouldToHandLength);    }     for (size_t i = 0; i < joints.size(); i++) {        int poseIndex = LeapMotionJointIndexToPoseIndex((LeapMotionJointIndex)i);         if (joints[i].position == Vectors::ZERO) {            _poseStateMap[poseIndex] = controller::Pose();            continue;        }         glm::vec3 pos;        glm::quat rot;        glm::quat prevRot;        if (_isLeapOnHMD) {            auto jointPosition = joints[i].position;            const glm::vec3 HMD_EYE_TO_LEAP_OFFSET = glm::vec3(0.0f, 0.0f, -0.09f);  // Eyes to surface of Leap Motion.            jointPosition = glm::vec3(-jointPosition.x, -jointPosition.z, -jointPosition.y) + HMD_EYE_TO_LEAP_OFFSET;            jointPosition = hmdSensorPosition + hmdSensorOrientation * jointPosition;            pos = transformPoint(controllerToAvatar, jointPosition);             glm::quat jointOrientation = joints[i].orientation;            jointOrientation = glm::quat(jointOrientation.w, -jointOrientation.x, -jointOrientation.z, -jointOrientation.y);            rot = controllerToAvatarRotation * hmdSensorOrientation * jointOrientation;             glm::quat prevJointOrientation = prevJoints[i].orientation;            prevJointOrientation =                glm::quat(prevJointOrientation.w, -prevJointOrientation.x, -prevJointOrientation.z, -prevJointOrientation.y);            prevRot = controllerToAvatarRotation * hmdSensorOrientation * prevJointOrientation;         } else {            pos = controllerToAvatarRotation * (joints[i].position - leapMotionOffset);            const glm::quat ZERO_HAND_ORIENTATION = glm::quat(glm::vec3(PI_OVER_TWO, PI, 0.0f));            rot = controllerToAvatarRotation * joints[i].orientation * ZERO_HAND_ORIENTATION;            prevRot = controllerToAvatarRotation * prevJoints[i].orientation * ZERO_HAND_ORIENTATION;        }         glm::vec3 linearVelocity, angularVelocity;        if (i < prevJoints.size()) {            linearVelocity = (pos - (prevJoints[i].position * METERS_PER_CENTIMETER)) / deltaTime;  // m/s            // quat log imaginary part points along the axis of rotation, with length of one half the angle of rotation.            glm::quat d = glm::log(rot * glm::inverse(prevRot));            angularVelocity = glm::vec3(d.x, d.y, d.z) / (0.5f * deltaTime); // radians/s        }         _poseStateMap[poseIndex] = controller::Pose(pos, rot, linearVelocity, angularVelocity);    }} void LeapMotionPlugin::init() {    loadSettings();    auto preferences = DependencyManager::get<Preferences>();    static const QString LEAPMOTION_PLUGIN { "Leap Motion" };    {        auto getter = [this]()->bool { return _enabled; };        auto setter = [this](bool value) {            _enabled = value;            emit deviceStatusChanged(getName(), isRunning());            saveSettings();            if (!_enabled) {                auto userInputMapper = DependencyManager::get<controller::UserInputMapper>();                userInputMapper->withLock([&, this]() {                    _inputDevice->clearState();                });            }        };        auto preference = new CheckPreference(LEAPMOTION_PLUGIN, "Enabled", getter, setter);        preferences->addPreference(preference);    }    {        auto getter = [this]()->QString { return _sensorLocation; };        auto setter = [this](QString value) {            _sensorLocation = value;            saveSettings();            applySensorLocation();        };        auto preference = new ComboBoxPreference(LEAPMOTION_PLUGIN, "Sensor location", getter, setter);        QStringList list = { SENSOR_ON_DESKTOP, SENSOR_ON_HMD };        preference->setItems(list);        preferences->addPreference(preference);    }    {        auto getter = [this]()->float { return _desktopHeightOffset; };        auto setter = [this](float value) {            _desktopHeightOffset = value;            saveSettings();            applyDesktopHeightOffset();        };        auto preference = new SpinnerPreference(LEAPMOTION_PLUGIN, "Desktop height for horizontal forearms", getter, setter);        float MIN_VALUE = 0.0f;        float MAX_VALUE = 1.0f;        float DECIMALS = 2.0f;        float STEP = 0.01f;        preference->setMin(MIN_VALUE);        preference->setMax(MAX_VALUE);        preference->setDecimals(DECIMALS);        preference->setStep(STEP);        preferences->addPreference(preference);    }} bool LeapMotionPlugin::activate() {    InputPlugin::activate();     // Nothing required to be done to start up Leap Motion library.     _joints.resize(LeapMotionJointIndex::Size, { glm::vec3(), glm::quat() });     auto userInputMapper = DependencyManager::get<controller::UserInputMapper>();    userInputMapper->registerDevice(_inputDevice);     return true;} void LeapMotionPlugin::deactivate() {    if (_inputDevice->_deviceID != controller::Input::INVALID_DEVICE) {        auto userInputMapper = DependencyManager::get<controller::UserInputMapper>();        userInputMapper->removeDevice(_inputDevice->_deviceID);    }     InputPlugin::deactivate();} const char* SETTINGS_ENABLED_KEY = "enabled";const char* SETTINGS_SENSOR_LOCATION_KEY = "sensorLocation";const char* SETTINGS_DESKTOP_HEIGHT_OFFSET_KEY = "desktopHeightOffset"; void LeapMotionPlugin::saveSettings() const {    Settings settings;    QString idString = getID();    settings.beginGroup(idString);    {        settings.setValue(QString(SETTINGS_ENABLED_KEY), _enabled);        settings.setValue(QString(SETTINGS_SENSOR_LOCATION_KEY), _sensorLocation);        settings.setValue(QString(SETTINGS_DESKTOP_HEIGHT_OFFSET_KEY), _desktopHeightOffset);    }    settings.endGroup();} void LeapMotionPlugin::loadSettings() {    Settings settings;    QString idString = getID();    settings.beginGroup(idString);    {        _enabled = settings.value(SETTINGS_ENABLED_KEY, QVariant(DEFAULT_ENABLED)).toBool();        emit deviceStatusChanged(getName(), isRunning());        _sensorLocation = settings.value(SETTINGS_SENSOR_LOCATION_KEY, QVariant(DEFAULT_SENSOR_LOCATION)).toString();        _desktopHeightOffset =             settings.value(SETTINGS_DESKTOP_HEIGHT_OFFSET_KEY, QVariant(DEFAULT_DESKTOP_HEIGHT_OFFSET)).toFloat();        applySensorLocation();        applyDesktopHeightOffset();    }    settings.endGroup();} void LeapMotionPlugin::InputDevice::clearState() {    for (size_t i = 0; i < LeapMotionJointIndex::Size; i++) {        int poseIndex = LeapMotionJointIndexToPoseIndex((LeapMotionJointIndex)i);        _poseStateMap[poseIndex] = controller::Pose();    }} void LeapMotionPlugin::applySensorLocation() {    bool isLeapOnHMD = _sensorLocation == SENSOR_ON_HMD;    _controller.setPolicyFlags(isLeapOnHMD ? Leap::Controller::POLICY_OPTIMIZE_HMD : Leap::Controller::POLICY_DEFAULT);    _inputDevice->setIsLeapOnHMD(isLeapOnHMD);} void LeapMotionPlugin::applyDesktopHeightOffset() {    _inputDevice->setDektopHeightOffset(_desktopHeightOffset);} const float LEFT_SIDE_SIGN = -1.0f;const float RIGHT_SIDE_SIGN = 1.0f; glm::quat LeapBasisToQuat(float sideSign, const Leap::Matrix& basis) {    glm::vec3 xAxis = sideSign * glm::vec3(basis.xBasis.x, basis.xBasis.y, basis.xBasis.z);    glm::vec3 yAxis = glm::vec3(basis.yBasis.x, basis.yBasis.y, basis.yBasis.z);    glm::vec3 zAxis = glm::vec3(basis.zBasis.x, basis.zBasis.y, basis.zBasis.z);    glm::quat orientation = (glm::quat_cast(glm::mat3(xAxis, yAxis, zAxis)));    return orientation;} glm::vec3 LeapVectorToVec3(const Leap::Vector& vec) {    return glm::vec3(vec.x * METERS_PER_MILLIMETER, vec.y * METERS_PER_MILLIMETER, vec.z * METERS_PER_MILLIMETER);} void LeapMotionPlugin::processFrame(const Leap::Frame& frame) {    // Default to uncontrolled.    for (int i = 0; i < _joints.size(); i++) {        _joints[i].position = glm::vec3();    }     auto hands = frame.hands();    const int MAX_NUMBER_OF_HANDS = 2;    for (int i = 0; i < hands.count() && i < MAX_NUMBER_OF_HANDS; i++) {        auto hand = hands[i];         int sideSign = hand.isLeft() ? LEFT_SIDE_SIGN : RIGHT_SIDE_SIGN;        int jointIndex = hand.isLeft() ? LeapMotionJointIndex::LeftHand : LeapMotionJointIndex::RightHand;         // Hand.        _joints[jointIndex].position = LeapVectorToVec3(hand.wristPosition());        _joints[jointIndex].orientation = LeapBasisToQuat(sideSign, hand.basis());         // Fingers.        // Leap Motion SDK guarantees full set of fingers and finger joints so can straightforwardly process them all.        Leap::FingerList fingers = hand.fingers();        for (int j = Leap::Finger::Type::TYPE_THUMB; j <= Leap::Finger::Type::TYPE_PINKY; j++) {            Leap::Finger finger;            finger = fingers[j];            Leap::Bone bone;            bone = finger.bone(Leap::Bone::Type::TYPE_PROXIMAL);            jointIndex++;            _joints[jointIndex].position = LeapVectorToVec3(bone.prevJoint());            _joints[jointIndex].orientation = LeapBasisToQuat(sideSign, bone.basis());            bone = finger.bone(Leap::Bone::Type::TYPE_INTERMEDIATE);            jointIndex++;            _joints[jointIndex].position = LeapVectorToVec3(bone.prevJoint());            _joints[jointIndex].orientation = LeapBasisToQuat(sideSign, bone.basis());            bone = finger.bone(Leap::Bone::Type::TYPE_DISTAL);            jointIndex++;            _joints[jointIndex].position = LeapVectorToVec3(bone.prevJoint());            _joints[jointIndex].orientation = LeapBasisToQuat(sideSign, bone.basis());            jointIndex++;            _joints[jointIndex].position = LeapVectorToVec3(bone.nextJoint());            _joints[jointIndex].orientation = LeapBasisToQuat(sideSign, bone.basis());        }    }}

Lines 1–500 of 501. Load more