C++ PalmData::setRawAngularVelocity方法代码示例

//.........这里部分代码省略.........
                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);
            
            //  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.0f, 1.0f, 0.0f)) * _orbRotation * 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);   //  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));
            
            //  Angular Velocity of Palm
            glm::quat deltaRotation = rotation * glm::inverse(palm->getRawRotation());
            glm::vec3 angularVelocity(0.0f);
            float rotationAngle = glm::angle(deltaRotation);
            if ((rotationAngle > EPSILON) && (deltaTime > 0.0f)) {
                angularVelocity = glm::normalize(glm::axis(deltaRotation));
                angularVelocity *= (rotationAngle / deltaTime);
                palm->setRawAngularVelocity(angularVelocity);
            } else {
                palm->setRawAngularVelocity(glm::vec3(0.0f));
            }
            
            if (_lowVelocityFilter) {
                //  Use a velocity sensitive filter to damp small motions and preserve large ones with
                //  no latency.
                float velocityFilter = glm::clamp(1.0f - glm::length(rawVelocity), 0.0f, 1.0f);
                position = palm->getRawPosition() * velocityFilter + position * (1.0f - velocityFilter);
                rotation = safeMix(palm->getRawRotation(), rotation, 1.0f - velocityFilter);
                palm->setRawPosition(position);
                palm->setRawRotation(rotation);
            } else {
                palm->setRawPosition(position);
                palm->setRawRotation(rotation);
            }
            
            // 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);
        }
        
        if (numActiveControllers == 2) {
            updateCalibration(controllers);
        }
        _controllersAtBase = (numControllersAtBase == 2);
    }
#endif  // HAVE_SIXENSE
}