C++ PalmData类代码示例

void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) {
    if (jointIndex == -1 || jointIndex >= _jointStates.size()) {
        return;
    }
    const FBXGeometry& geometry = _geometry->getFBXGeometry();
    float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
    int parentJointIndex = geometry.joints.at(jointIndex).parentIndex;
    if (parentJointIndex == -1) {
        return;
    }
  
    // rotate palm to align with its normal (normal points out of hand's palm)
    glm::quat inverseRotation = glm::inverse(_rotation);
    glm::vec3 palmPosition = inverseRotation * (palm.getPosition() - _translation);
    glm::vec3 palmNormal = inverseRotation * palm.getNormal();
    glm::vec3 fingerDirection = inverseRotation * palm.getFingerDirection();

    glm::quat palmRotation = rotationBetween(geometry.palmDirection, palmNormal);
    palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), fingerDirection) * palmRotation;

    if (Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK)) {
        setHandPosition(jointIndex, palmPosition, palmRotation);  
    } else if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {
        float forearmLength = geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale);
        glm::vec3 forearm = palmRotation * glm::vec3(sign * forearmLength, 0.0f, 0.0f);
        setJointPosition(parentJointIndex, palmPosition + forearm,
            glm::quat(), false, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY);
        JointState& parentState = _jointStates[parentJointIndex];
        parentState.setRotationInBindFrame(palmRotation, PALM_PRIORITY);
        // lock hand to forearm by slamming its rotation (in parent-frame) to identity
        _jointStates[jointIndex].setRotationInConstrainedFrame(glm::quat(), PALM_PRIORITY);
    } else {
        inverseKinematics(jointIndex, palmPosition, palmRotation, PALM_PRIORITY);
    }
}

void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJointIndices,
        const QVector<int>& fingertipJointIndices, PalmData& palm) {
    if (jointIndex == -1) {
        return;
    }
    const FBXGeometry& geometry = _geometry->getFBXGeometry();
    float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
    glm::quat palmRotation;
    getJointRotation(jointIndex, palmRotation, true);
    applyRotationDelta(jointIndex, rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()), false);
    getJointRotation(jointIndex, palmRotation, true);

    // sort the finger indices by raw x, get the average direction
    QVector<IndexValue> fingerIndices;
    glm::vec3 direction;
    for (size_t i = 0; i < palm.getNumFingers(); i++) {
        glm::vec3 fingerVector = palm.getFingers()[i].getTipPosition() - palm.getPosition();
        float length = glm::length(fingerVector);
        if (length > EPSILON) {
            direction += fingerVector / length;
        }
        fingerVector = glm::inverse(palmRotation) * fingerVector * -sign;
        IndexValue indexValue = { i, atan2f(fingerVector.z, fingerVector.x) };
        fingerIndices.append(indexValue);
    }
    qSort(fingerIndices.begin(), fingerIndices.end());

    // rotate palm according to average finger direction
    float directionLength = glm::length(direction);
    const int MIN_ROTATION_FINGERS = 3;
    if (directionLength > EPSILON && palm.getNumFingers() >= MIN_ROTATION_FINGERS) {
        applyRotationDelta(jointIndex, rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction), false);
        getJointRotation(jointIndex, palmRotation, true);
    }

    // no point in continuing if there are no fingers
    if (palm.getNumFingers() == 0 || fingerJointIndices.isEmpty()) {
        stretchArm(jointIndex, palm.getPosition());
        return;
    }

    // match them up as best we can
    float proportion = fingerIndices.size() / (float)fingerJointIndices.size();
    for (int i = 0; i < fingerJointIndices.size(); i++) {
        int fingerIndex = fingerIndices.at(roundf(i * proportion)).index;
        glm::vec3 fingerVector = palm.getFingers()[fingerIndex].getTipPosition() -
            palm.getFingers()[fingerIndex].getRootPosition();

        int fingerJointIndex = fingerJointIndices.at(i);
        int fingertipJointIndex = fingertipJointIndices.at(i);
        glm::vec3 jointVector = extractTranslation(geometry.joints.at(fingertipJointIndex).bindTransform) -
            extractTranslation(geometry.joints.at(fingerJointIndex).bindTransform);

        setJointRotation(fingerJointIndex, rotationBetween(palmRotation * jointVector, fingerVector) * palmRotation, true);
    }
    
    stretchArm(jointIndex, palm.getPosition());
}

void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJointIndices,
        const QVector<int>& fingertipJointIndices, PalmData& palm) {
    if (jointIndex == -1) {
        return;
    }
    const FBXGeometry& geometry = _geometry->getFBXGeometry();
    float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
    int parentJointIndex = geometry.joints.at(jointIndex).parentIndex;
    if (parentJointIndex == -1) {
        return;
    }
    
    // rotate forearm to align with palm direction
    glm::quat palmRotation;
    getJointRotation(parentJointIndex, palmRotation, true);
    applyRotationDelta(parentJointIndex, rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()), false);
    getJointRotation(parentJointIndex, palmRotation, true);

    // sort the finger indices by raw x, get the average direction
    QVector<IndexValue> fingerIndices;
    glm::vec3 direction;
    for (size_t i = 0; i < palm.getNumFingers(); i++) {
        glm::vec3 fingerVector = palm.getFingers()[i].getTipPosition() - palm.getPosition();
        float length = glm::length(fingerVector);
        if (length > EPSILON) {
            direction += fingerVector / length;
        }
        fingerVector = glm::inverse(palmRotation) * fingerVector * -sign;
        IndexValue indexValue = { (int)i, atan2f(fingerVector.z, fingerVector.x) };
        fingerIndices.append(indexValue);
    }
    qSort(fingerIndices.begin(), fingerIndices.end());

    // rotate forearm according to average finger direction
    float directionLength = glm::length(direction);
    const unsigned int MIN_ROTATION_FINGERS = 3;
    if (directionLength > EPSILON && palm.getNumFingers() >= MIN_ROTATION_FINGERS) {
        applyRotationDelta(parentJointIndex, rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction), false);
        getJointRotation(parentJointIndex, palmRotation, true);
    }

    // let wrist inherit forearm rotation
    _jointStates[jointIndex].rotation = glm::quat();

    // set elbow position from wrist position
    glm::vec3 forearmVector = palmRotation * glm::vec3(sign, 0.0f, 0.0f);
    setJointPosition(parentJointIndex, palm.getPosition() + forearmVector *
        geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale));
}

void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) {
    if (jointIndex == -1) {
        return;
    }
    const FBXGeometry& geometry = _geometry->getFBXGeometry();
    float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
    int parentJointIndex = geometry.joints.at(jointIndex).parentIndex;
    if (parentJointIndex == -1) {
        return;
    }
    
    // rotate palm to align with its normal (normal points out of hand's palm)
    glm::quat palmRotation;
    if (!Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK) &&
            Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {
        getJointRotation(parentJointIndex, palmRotation, true);
    } else {
        getJointRotation(jointIndex, palmRotation, true);
    }
    palmRotation = rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()) * palmRotation;
    
    // rotate palm to align with finger direction
    glm::vec3 direction = palm.getFingerDirection();
    palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction) * palmRotation;

    // set hand position, rotation
    if (Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK)) {
        setHandPosition(jointIndex, palm.getPosition(), palmRotation);  
        
    } else if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {
        glm::vec3 forearmVector = palmRotation * glm::vec3(sign, 0.0f, 0.0f);
        setJointPosition(parentJointIndex, palm.getPosition() + forearmVector *
            geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale));
        setJointRotation(parentJointIndex, palmRotation, true);
        _jointStates[jointIndex].rotation = glm::quat();
        
    } else {
        setJointPosition(jointIndex, palm.getPosition(), palmRotation, true);
    }
}

void SixenseManager::update(float deltaTime) {
#ifdef HAVE_SIXENSE
    if (sixenseGetNumActiveControllers() == 0) {
        return;
    }
    MyAvatar* avatar = Application::getInstance()->getAvatar();
    Hand* hand = avatar->getHand();
    
    int maxControllers = sixenseGetMaxControllers();

    // we only support two controllers
    sixenseControllerData controllers[2];

    int numActiveControllers = 0;
    for (int i = 0; i < maxControllers && numActiveControllers < 2; i++) {
        if (!sixenseIsControllerEnabled(i)) {
            continue;
        }
        sixenseControllerData* data = controllers + numActiveControllers;
        ++numActiveControllers;
        sixenseGetNewestData(i, data);
        
        //  Set palm position and normal based on Hydra position/orientation
        
        // Either find a palm matching the sixense controller, or make a new one
        PalmData* palm;
        bool foundHand = false;
        for (size_t j = 0; j < hand->getNumPalms(); j++) {
            if (hand->getPalms()[j].getSixenseID() == data->controller_index) {
                palm = &(hand->getPalms()[j]);
                foundHand = true;
            }
        }
        if (!foundHand) {
            PalmData newPalm(hand);
            hand->getPalms().push_back(newPalm);
            palm = &(hand->getPalms()[hand->getNumPalms() - 1]);
            palm->setSixenseID(data->controller_index);
            printf("Found new Sixense controller, ID %i\n", data->controller_index);
        }
        
        palm->setActive(true);
        
        //  Read controller buttons and joystick into the hand
        palm->setControllerButtons(data->buttons);
        palm->setTrigger(data->trigger);
        palm->setJoystick(data->joystick_x, data->joystick_y);

        glm::vec3 position(data->pos[0], data->pos[1], data->pos[2]);
        // Transform the measured position into body frame.  
        glm::vec3 neck = _neckBase;
        // Zeroing y component of the "neck" effectively raises the measured position a little bit.
        neck.y = 0.f;
        position = _orbRotation * (position - neck);

        //  Rotation of Palm
        glm::quat rotation(data->rot_quat[3], -data->rot_quat[0], data->rot_quat[1], -data->rot_quat[2]);
        rotation = glm::angleAxis(PI, glm::vec3(0.f, 1.f, 0.f)) * _orbRotation * rotation;
        const glm::vec3 PALM_VECTOR(0.0f, -1.0f, 0.0f);
        glm::vec3 newNormal = rotation * PALM_VECTOR;
        palm->setRawNormal(newNormal);
        palm->setRawRotation(rotation);
        
        //  Compute current velocity from position change
        glm::vec3 rawVelocity = (position - palm->getRawPosition()) / deltaTime / 1000.f;
        palm->setRawVelocity(rawVelocity);   //  meters/sec
        palm->setRawPosition(position);
        
        // use the velocity to determine whether there's any movement (if the hand isn't new)
        const float MOVEMENT_SPEED_THRESHOLD = 0.05f;
        if (glm::length(rawVelocity) > MOVEMENT_SPEED_THRESHOLD && foundHand) {
            _lastMovement = usecTimestampNow();
        }
        
        // initialize the "finger" based on the direction
        FingerData finger(palm, hand);
        finger.setActive(true);
        finger.setRawRootPosition(position);
        const float FINGER_LENGTH = 300.0f;   //  Millimeters
        const glm::vec3 FINGER_VECTOR(0.0f, 0.0f, FINGER_LENGTH);
        const glm::vec3 newTipPosition = position + rotation * FINGER_VECTOR;
        finger.setRawTipPosition(position + rotation * FINGER_VECTOR);
        
        // Store the one fingertip in the palm structure so we can track velocity
        glm::vec3 oldTipPosition = palm->getTipRawPosition();
        palm->setTipVelocity((newTipPosition - oldTipPosition) / deltaTime / 1000.f);
        palm->setTipPosition(newTipPosition);
        
        // three fingers indicates to the skeleton that we have enough data to determine direction
        palm->getFingers().clear();
        palm->getFingers().push_back(finger);
        palm->getFingers().push_back(finger);
        palm->getFingers().push_back(finger);
    }

    if (numActiveControllers == 2) {
        updateCalibration(controllers);
    }

    // if the controllers haven't been moved in a while, disable
//.........这里部分代码省略.........

static void setPalm(float deltaTime, int index) {
    MyAvatar* avatar = Application::getInstance()->getAvatar();
    Hand* hand = avatar->getHand();
    PalmData* palm;
    bool foundHand = false;
    for (size_t j = 0; j < hand->getNumPalms(); j++) {
        if (hand->getPalms()[j].getSixenseID() == index) {
            palm = &(hand->getPalms()[j]);
            foundHand = true;
        }
    }
    if (!foundHand) {
        PalmData newPalm(hand);
        hand->getPalms().push_back(newPalm);
        palm = &(hand->getPalms()[hand->getNumPalms() - 1]);
        palm->setSixenseID(index);
    }
    
    palm->setActive(true);
    
    // Read controller buttons and joystick into the hand
    const QString PRIO_JOYSTICK_NAME = "PrioVR";
    Joystick* prioJoystick = JoystickScriptingInterface::getInstance().joystickWithName(PRIO_JOYSTICK_NAME);
    if (prioJoystick) {
        const QVector<float> axes = prioJoystick->getAxes();
        const QVector<bool> buttons = prioJoystick->getButtons();
        
        if (axes.size() >= 4 && buttons.size() >= 4) {
            if (index == LEFT_HAND_INDEX) {
                palm->setControllerButtons(buttons[1] ? BUTTON_FWD : 0);
                palm->setTrigger(buttons[0] ? 1.0f : 0.0f);
                palm->setJoystick(axes[0], -axes[1]);
                
            } else {
                palm->setControllerButtons(buttons[3] ? BUTTON_FWD : 0);
                palm->setTrigger(buttons[2] ? 1.0f : 0.0f);
                palm->setJoystick(axes[2], -axes[3]);
            }
        }
    }
    
    // NOTE: this math is done in the worl-frame with unecessary complexity.
    // TODO: transfom this to stay in the model-frame.
    glm::vec3 position;
    glm::quat rotation;
    SkeletonModel* skeletonModel = &Application::getInstance()->getAvatar()->getSkeletonModel();
    int jointIndex;
    glm::quat inverseRotation = glm::inverse(Application::getInstance()->getAvatar()->getOrientation());
    if (index == LEFT_HAND_INDEX) {
        jointIndex = skeletonModel->getLeftHandJointIndex();
        skeletonModel->getJointRotationInWorldFrame(jointIndex, rotation);      
        rotation = inverseRotation * rotation * glm::quat(glm::vec3(0.0f, PI_OVER_TWO, 0.0f));
        
    } else {
        jointIndex = skeletonModel->getRightHandJointIndex();
        skeletonModel->getJointRotationInWorldFrame(jointIndex, rotation);
        rotation = inverseRotation * rotation * glm::quat(glm::vec3(0.0f, -PI_OVER_TWO, 0.0f));
    }
    skeletonModel->getJointPositionInWorldFrame(jointIndex, position);
    position = inverseRotation * (position - skeletonModel->getTranslation());
    
    palm->setRawRotation(rotation);
    
    //  Compute current velocity from position change
    glm::vec3 rawVelocity;
    if (deltaTime > 0.0f) {
        rawVelocity = (position - palm->getRawPosition()) / deltaTime; 
    } else {
        rawVelocity = glm::vec3(0.0f);
    }
    palm->setRawVelocity(rawVelocity);
    palm->setRawPosition(position);
    
    // Store the one fingertip in the palm structure so we can track velocity
    const float FINGER_LENGTH = 0.3f;   //  meters
    const glm::vec3 FINGER_VECTOR(0.0f, 0.0f, FINGER_LENGTH);
    const glm::vec3 newTipPosition = position + rotation * FINGER_VECTOR;
    glm::vec3 oldTipPosition = palm->getTipRawPosition();
    if (deltaTime > 0.0f) {
        palm->setTipVelocity((newTipPosition - oldTipPosition) / deltaTime);
    } else {
        palm->setTipVelocity(glm::vec3(0.0f));
    }
    palm->setTipPosition(newTipPosition);
}

void SixenseManager::update(float deltaTime) {
#ifdef HAVE_SIXENSE
    // if the controllers haven't been moved in a while, disable
    const unsigned int MOVEMENT_DISABLE_SECONDS = 3;
    if (usecTimestampNow() - _lastMovement > (MOVEMENT_DISABLE_SECONDS * USECS_PER_SECOND)) {
        Hand* hand = Application::getInstance()->getAvatar()->getHand();
        for (std::vector<PalmData>::iterator it = hand->getPalms().begin(); it != hand->getPalms().end(); it++) {
            it->setActive(false);
        }
        _lastMovement = usecTimestampNow();
    }

    if (sixenseGetNumActiveControllers() == 0) {
        _hydrasConnected = false;
        return;
    } 

    PerformanceTimer perfTimer("sixense");
    if (!_hydrasConnected) {
        _hydrasConnected = true;
        UserActivityLogger::getInstance().connectedDevice("spatial_controller", "hydra");
    }
    MyAvatar* avatar = Application::getInstance()->getAvatar();
    Hand* hand = avatar->getHand();
    
    int maxControllers = sixenseGetMaxControllers();

    // we only support two controllers
    sixenseControllerData controllers[2];

    int numActiveControllers = 0;
    for (int i = 0; i < maxControllers && numActiveControllers < 2; i++) {
        if (!sixenseIsControllerEnabled(i)) {
            continue;
        }
        sixenseControllerData* data = controllers + numActiveControllers;
        ++numActiveControllers;
        sixenseGetNewestData(i, data);
        
        //  Set palm position and normal based on Hydra position/orientation
        
        // Either find a palm matching the sixense controller, or make a new one
        PalmData* palm;
        bool foundHand = false;
        for (size_t j = 0; j < hand->getNumPalms(); j++) {
            if (hand->getPalms()[j].getSixenseID() == data->controller_index) {
                palm = &(hand->getPalms()[j]);
                foundHand = true;
            }
        }
        if (!foundHand) {
            PalmData newPalm(hand);
            hand->getPalms().push_back(newPalm);
            palm = &(hand->getPalms()[hand->getNumPalms() - 1]);
            palm->setSixenseID(data->controller_index);
            qDebug("Found new Sixense controller, ID %i", data->controller_index);
        }
        
        palm->setActive(true);
        
        //  Read controller buttons and joystick into the hand
        palm->setControllerButtons(data->buttons);
        palm->setTrigger(data->trigger);
        palm->setJoystick(data->joystick_x, data->joystick_y);


        // Emulate the mouse so we can use scripts
        if (Menu::getInstance()->isOptionChecked(MenuOption::SixenseMouseInput)) {
            emulateMouse(palm, numActiveControllers - 1);
        }

        // NOTE: Sixense API returns pos data in millimeters but we IMMEDIATELY convert to meters.
        glm::vec3 position(data->pos[0], data->pos[1], data->pos[2]);
        position *= METERS_PER_MILLIMETER;

        // Transform the measured position into body frame.  
        glm::vec3 neck = _neckBase;
        // Zeroing y component of the "neck" effectively raises the measured position a little bit.
        neck.y = 0.f;
        position = _orbRotation * (position - neck);

        //  Rotation of Palm
        glm::quat rotation(data->rot_quat[3], -data->rot_quat[0], data->rot_quat[1], -data->rot_quat[2]);
        rotation = glm::angleAxis(PI, glm::vec3(0.f, 1.f, 0.f)) * _orbRotation * rotation;
        
        //  Compute current velocity from position change
        glm::vec3 rawVelocity;
        if (deltaTime > 0.f) {
            rawVelocity = (position - palm->getRawPosition()) / deltaTime; 
        } else {
            rawVelocity = glm::vec3(0.0f);
        }
        palm->setRawVelocity(rawVelocity);   //  meters/sec
    
        // adjustment for hydra controllers fit into hands
        float sign = (i == 0) ? -1.0f : 1.0f;
        rotation *= glm::angleAxis(sign * PI/4.0f, glm::vec3(0.0f, 0.0f, 1.0f));

        if (_lowVelocityFilter) {
            //  Use a velocity sensitive filter to damp small motions and preserve large ones with
//.........这里部分代码省略.........

void SixenseManager::update(float deltaTime) {
#ifdef HAVE_SIXENSE
    Hand* hand = DependencyManager::get<AvatarManager>()->getMyAvatar()->getHand();
    if (_isInitialized && _isEnabled) {
#ifdef __APPLE__
        SixenseBaseFunction sixenseGetNumActiveControllers =
        (SixenseBaseFunction) _sixenseLibrary->resolve("sixenseGetNumActiveControllers");
#endif
        
        if (sixenseGetNumActiveControllers() == 0) {
            _hydrasConnected = false;
            return;
        }
        
        PerformanceTimer perfTimer("sixense");
        if (!_hydrasConnected) {
            _hydrasConnected = true;
            UserActivityLogger::getInstance().connectedDevice("spatial_controller", "hydra");
        }
        
#ifdef __APPLE__
        SixenseBaseFunction sixenseGetMaxControllers =
        (SixenseBaseFunction) _sixenseLibrary->resolve("sixenseGetMaxControllers");
#endif
        
        int maxControllers = sixenseGetMaxControllers();
        
        // we only support two controllers
        sixenseControllerData controllers[2];
        
#ifdef __APPLE__
        SixenseTakeIntFunction sixenseIsControllerEnabled =
        (SixenseTakeIntFunction) _sixenseLibrary->resolve("sixenseIsControllerEnabled");
        
        SixenseTakeIntAndSixenseControllerData sixenseGetNewestData =
        (SixenseTakeIntAndSixenseControllerData) _sixenseLibrary->resolve("sixenseGetNewestData");
#endif
        int numControllersAtBase = 0;
        int numActiveControllers = 0;
        for (int i = 0; i < maxControllers && numActiveControllers < 2; i++) {
            if (!sixenseIsControllerEnabled(i)) {
                continue;
            }
            sixenseControllerData* data = controllers + numActiveControllers;
            ++numActiveControllers;
            sixenseGetNewestData(i, data);
            
            //  Set palm position and normal based on Hydra position/orientation
            
            // Either find a palm matching the sixense controller, or make a new one
            PalmData* palm;
            bool foundHand = false;
            for (size_t j = 0; j < hand->getNumPalms(); j++) {
                if (hand->getPalms()[j].getSixenseID() == data->controller_index) {
                    palm = &(hand->getPalms()[j]);
                    foundHand = true;
                }
            }
            if (!foundHand) {
                PalmData newPalm(hand);
                hand->getPalms().push_back(newPalm);
                palm = &(hand->getPalms()[hand->getNumPalms() - 1]);
                palm->setSixenseID(data->controller_index);
                qCDebug(interfaceapp, "Found new Sixense controller, ID %i", data->controller_index);
            }
            
            // Disable the hands (and return to default pose) if both controllers are at base station
            if (foundHand) {
                palm->setActive(!_controllersAtBase);
            } else {
                palm->setActive(false); // if this isn't a Sixsense ID palm, always make it inactive
            }
            
            
            //  Read controller buttons and joystick into the hand
            palm->setControllerButtons(data->buttons);
            palm->setTrigger(data->trigger);
            palm->setJoystick(data->joystick_x, data->joystick_y);
            
            // Emulate the mouse so we can use scripts
            if (Menu::getInstance()->isOptionChecked(MenuOption::SixenseMouseInput) && !_controllersAtBase) {
                emulateMouse(palm, numActiveControllers - 1);
            }
            
            // NOTE: Sixense API returns pos data in millimeters but we IMMEDIATELY convert to meters.
            glm::vec3 position(data->pos[0], data->pos[1], data->pos[2]);
            position *= METERS_PER_MILLIMETER;
            
            // Check to see if this hand/controller is on the base
            const float CONTROLLER_AT_BASE_DISTANCE = 0.075f;
            if (glm::length(position) < CONTROLLER_AT_BASE_DISTANCE) {
                numControllersAtBase++;
            }
            
            // Transform the measured position into body frame.
            glm::vec3 neck = _neckBase;
            // Zeroing y component of the "neck" effectively raises the measured position a little bit.
            neck.y = 0.0f;
            position = _orbRotation * (position - neck);
            
//.........这里部分代码省略.........

void BuckyBalls::grab(PalmData& palm, float deltaTime) {
    float penetration;
    glm::vec3 fingerTipPosition = palm.getFingerTipPosition();

    if (palm.getControllerButtons() & BUTTON_FWD) {
        if (!_bballIsGrabbed[palm.getSixenseID()]) {
            // Look for a ball to grab
            for (int i = 0; i < NUM_BBALLS; i++) {
                glm::vec3 diff = _bballPosition[i] - fingerTipPosition;
                penetration = glm::length(diff) - (_bballRadius[i] + COLLISION_RADIUS);
                if (penetration < 0.f) {
                    _bballIsGrabbed[palm.getSixenseID()] = i;
                }
            }
        }
        if (_bballIsGrabbed[palm.getSixenseID()]) {
            //  If ball being grabbed, move with finger
            glm::vec3 diff = _bballPosition[_bballIsGrabbed[palm.getSixenseID()]] - fingerTipPosition;
            penetration = glm::length(diff) - (_bballRadius[_bballIsGrabbed[palm.getSixenseID()]] + COLLISION_RADIUS);
            _bballPosition[_bballIsGrabbed[palm.getSixenseID()]] -= glm::normalize(diff) * penetration;
            glm::vec3 fingerTipVelocity = palm.getTipVelocity();
            if (_bballElement[_bballIsGrabbed[palm.getSixenseID()]] != 1) {
                _bballVelocity[_bballIsGrabbed[palm.getSixenseID()]] = fingerTipVelocity;
            }
            _bballPosition[_bballIsGrabbed[palm.getSixenseID()]] = fingerTipPosition;
            _bballColliding[_bballIsGrabbed[palm.getSixenseID()]] = 1.f;
        }
    } else {
        _bballIsGrabbed[palm.getSixenseID()] = 0;
    }
}