Exemplo n.º 1
0
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);
    }
}
Exemplo n.º 2
0
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());
}
Exemplo n.º 3
0
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));
}
Exemplo n.º 4
0
void SkeletonModel::applyHandPosition(int jointIndex, const glm::vec3& position) {
    if (jointIndex == -1 || jointIndex >= _rig->getJointStateCount()) {
        return;
    }
    // NOTE: 'position' is in model-frame
    setJointPosition(jointIndex, position, glm::quat(), false, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY);

    const FBXGeometry& geometry = _geometry->getFBXGeometry();
    glm::vec3 handPosition, elbowPosition;
    getJointPosition(jointIndex, handPosition);
    getJointPosition(geometry.joints.at(jointIndex).parentIndex, elbowPosition);
    glm::vec3 forearmVector = handPosition - elbowPosition;
    float forearmLength = glm::length(forearmVector);
    if (forearmLength < EPSILON) {
        return;
    }
    glm::quat handRotation;
    if (!_rig->getJointStateRotation(jointIndex, handRotation)) {
        return;
    }

    // align hand with forearm
    float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
    _rig->applyJointRotationDelta(jointIndex,
                                  rotationBetween(handRotation * glm::vec3(-sign, 0.0f, 0.0f), forearmVector),
                                  true, PALM_PRIORITY);
}
Exemplo n.º 5
0
void FaceModel::maybeUpdateEyeRotation(const JointState& parentState, const FBXJoint& joint, JointState& state) {
    // likewise with the eye joints
    glm::mat4 inverse = glm::inverse(parentState.transform *
        joint.preTransform * glm::mat4_cast(joint.preRotation * joint.rotation));
    glm::vec3 front = glm::vec3(inverse * glm::vec4(_owningHead->getOrientation() * IDENTITY_FRONT, 0.0f));
    glm::vec3 lookAt = glm::vec3(inverse * glm::vec4(_owningHead->getLookAtPosition() +
        _owningHead->getSaccade() - _translation, 1.0f));
    glm::quat between = rotationBetween(front, lookAt);
    const float MAX_ANGLE = 30.0f;
    state.rotation = glm::angleAxis(glm::clamp(glm::angle(between), -MAX_ANGLE, MAX_ANGLE), glm::axis(between)) *
        joint.rotation;
}
Exemplo n.º 6
0
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);
    }
}
Exemplo n.º 7
0
void SkeletonModel::stretchArm(int jointIndex, const glm::vec3& position) {
    // find out where the hand is pointing
    glm::quat handRotation;
    getJointRotation(jointIndex, handRotation, true);
    const FBXGeometry& geometry = _geometry->getFBXGeometry();
    glm::vec3 forwardVector(jointIndex == geometry.rightHandJointIndex ? -1.0f : 1.0f, 0.0f, 0.0f);
    glm::vec3 handVector = handRotation * forwardVector;
    
    // align elbow with hand
    const FBXJoint& joint = geometry.joints.at(jointIndex);
    if (joint.parentIndex == -1) {
        return;
    }
    glm::quat elbowRotation;
    getJointRotation(joint.parentIndex, elbowRotation, true);
    applyRotationDelta(joint.parentIndex, rotationBetween(elbowRotation * forwardVector, handVector), false);
    
    // set position according to normal length
    float scale = extractUniformScale(_scale);
    glm::vec3 handPosition = position - _translation;
    glm::vec3 elbowPosition = handPosition - handVector * joint.distanceToParent * scale;
    
    // set shoulder orientation to point to elbow
    const FBXJoint& parentJoint = geometry.joints.at(joint.parentIndex);
    if (parentJoint.parentIndex == -1) {
        return;
    }
    glm::quat shoulderRotation;
    getJointRotation(parentJoint.parentIndex, shoulderRotation, true);
    applyRotationDelta(parentJoint.parentIndex, rotationBetween(shoulderRotation * forwardVector,
        elbowPosition - extractTranslation(_jointStates.at(parentJoint.parentIndex).transform)), false);
        
    // update the shoulder state
    updateJointState(parentJoint.parentIndex);
    
    // adjust the elbow's local translation
    setJointTranslation(joint.parentIndex, elbowPosition);
}
Exemplo n.º 8
0
void FaceModel::maybeUpdateEyeRotation(const JointState& parentState, const FBXJoint& joint, JointState& state) {
    // likewise with the eye joints
    // NOTE: at the moment we do the math in the world-frame, hence the inverse transform is more complex than usual.
    glm::mat4 inverse = glm::inverse(glm::mat4_cast(_rotation) * parentState.getTransform() * 
            glm::translate(state.getDefaultTranslationInParentFrame()) *
            joint.preTransform * glm::mat4_cast(joint.preRotation * joint.rotation));
    glm::vec3 front = glm::vec3(inverse * glm::vec4(_owningHead->getFinalOrientationInWorldFrame() * IDENTITY_FRONT, 0.0f));
    glm::vec3 lookAt = glm::vec3(inverse * glm::vec4(_owningHead->getLookAtPosition() +
        _owningHead->getSaccade() - _translation, 1.0f));
    glm::quat between = rotationBetween(front, lookAt);
    const float MAX_ANGLE = 30.0f * RADIANS_PER_DEGREE;
    state.setRotationInParentFrame(glm::angleAxis(glm::clamp(glm::angle(between), -MAX_ANGLE, MAX_ANGLE), glm::axis(between)) *
        joint.rotation);
}
Exemplo n.º 9
0
void FlowThread::computeJointRotations() {

    auto pos0 = _rootFramePositions[0];
    auto pos1 = _rootFramePositions[1];

    auto joint0 = _jointsPointer->at(_joints[0]);
    auto joint1 = _jointsPointer->at(_joints[1]);

    auto initial_pos1 = pos0 + (joint0._initialRotation * (joint1._initialTranslation * 0.01f));

    auto vec0 = initial_pos1 - pos0;
    auto vec1 = pos1 - pos0;

    auto delta = rotationBetween(vec0, vec1);

    joint0._currentRotation = _jointsPointer->at(_joints[0])._currentRotation = delta * joint0._initialRotation;
    
    for (size_t i = 1; i < _joints.size() - 1; i++) {
        auto nextJoint = _jointsPointer->at(_joints[i + 1]);
        for (size_t j = i; j < _joints.size(); j++) {
            _rootFramePositions[j] = glm::inverse(joint0._currentRotation) * _rootFramePositions[j] - (joint0._initialTranslation * 0.01f);
        }
        pos0 = _rootFramePositions[i];
        pos1 = _rootFramePositions[i + 1];
        initial_pos1 = pos0 + joint1._initialRotation * (nextJoint._initialTranslation * 0.01f);

        vec0 = initial_pos1 - pos0;
        vec1 = pos1 - pos0;

        delta = rotationBetween(vec0, vec1);

        joint1._currentRotation = _jointsPointer->at(joint1._index)._currentRotation = delta * joint1._initialRotation;
        joint0 = joint1;
        joint1 = nextJoint;
    }
    
}
Exemplo n.º 10
0
void FaceModel::maybeUpdateEyeRotation(Model* model, const JointState& parentState, const JointState& state, int index) {
    // likewise with the eye joints
    // NOTE: at the moment we do the math in the world-frame, hence the inverse transform is more complex than usual.
    glm::mat4 inverse = glm::inverse(glm::mat4_cast(model->getRotation()) * parentState.getTransform() *
                                     glm::translate(_rig->getJointDefaultTranslationInConstrainedFrame(index)) *
                                     state.getPreTransform() * glm::mat4_cast(state.getPreRotation() * state.getDefaultRotation()));
    glm::vec3 front = glm::vec3(inverse * glm::vec4(_owningHead->getFinalOrientationInWorldFrame() * IDENTITY_FRONT, 0.0f));
    glm::vec3 lookAtDelta = _owningHead->getCorrectedLookAtPosition() - model->getTranslation();
    glm::vec3 lookAt = glm::vec3(inverse * glm::vec4(lookAtDelta + glm::length(lookAtDelta) * _owningHead->getSaccade(), 1.0f));
    glm::quat between = rotationBetween(front, lookAt);
    const float MAX_ANGLE = 30.0f * RADIANS_PER_DEGREE;
    _rig->setJointRotationInConstrainedFrame(index, glm::angleAxis(glm::clamp(glm::angle(between),
                                                                              -MAX_ANGLE, MAX_ANGLE), glm::axis(between)) *
                                             state.getDefaultRotation(), DEFAULT_PRIORITY);
}
Exemplo n.º 11
0
void SkeletonModel::updateVisibleJointStates() {
    if (_showTrueJointTransforms || !_ragdoll) {
        // no need to update visible transforms
        return;
    }
    const QVector<VerletPoint>& ragdollPoints = _ragdoll->getPoints();
    QVector<glm::vec3> points;
    points.reserve(_jointStates.size());
    glm::quat invRotation = glm::inverse(_rotation);
    for (int i = 0; i < _jointStates.size(); i++) {
        JointState& state = _jointStates[i];
        points.push_back(ragdollPoints[i]._position);

        // get the parent state (this is the state that we want to rotate)
        int parentIndex = state.getParentIndex();
        if (parentIndex == -1) {
            _jointStates[i].slaveVisibleTransform();
            continue;
        }
        JointState& parentState = _jointStates[parentIndex];

        // check the grand-parent index (for now we don't want to rotate any root states)
        int grandParentIndex = parentState.getParentIndex();
        if (grandParentIndex == -1) {
            continue;
        }

        // make sure state's visibleTransform is up to date
        const glm::mat4& parentTransform = parentState.getVisibleTransform();
        state.computeVisibleTransform(parentTransform);

        // we're looking for the rotation that moves visible bone parallel to ragdoll bone
        // rotationBetween(jointTip - jointPivot, shapeTip - shapePivot)
        // NOTE: points are in simulation-frame so rotate line segment into model-frame
        glm::quat delta = rotationBetween(state.getVisiblePosition() - extractTranslation(parentTransform), 
                invRotation * (points[i] - points[parentIndex]));

        // apply
        parentState.mixVisibleRotationDelta(delta, 0.01f);
        // update transforms
        parentState.computeVisibleTransform(_jointStates[grandParentIndex].getVisibleTransform());
        state.computeVisibleTransform(parentState.getVisibleTransform());
    }
}
Exemplo n.º 12
0
void SkeletonModel::applyHandPosition(int jointIndex, const glm::vec3& position) {
    if (jointIndex == -1) {
        return;
    }
    setJointPosition(jointIndex, position);

    const FBXGeometry& geometry = _geometry->getFBXGeometry();
    glm::vec3 handPosition, elbowPosition;
    getJointPosition(jointIndex, handPosition);
    getJointPosition(geometry.joints.at(jointIndex).parentIndex, elbowPosition);
    glm::vec3 forearmVector = handPosition - elbowPosition;
    float forearmLength = glm::length(forearmVector);
    if (forearmLength < EPSILON) {
        return;
    }
    glm::quat handRotation;
    getJointRotation(jointIndex, handRotation, true);

    // align hand with forearm
    float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
    applyRotationDelta(jointIndex, rotationBetween(handRotation * glm::vec3(-sign, 0.0f, 0.0f), forearmVector), false);
}
Exemplo n.º 13
0
void SkeletonModel::setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation) {
    // this algorithm is from sample code from sixense
    const FBXGeometry& geometry = _geometry->getFBXGeometry();
    int elbowJointIndex = geometry.joints.at(jointIndex).parentIndex;
    if (elbowJointIndex == -1) {
        return;
    }
    int shoulderJointIndex = geometry.joints.at(elbowJointIndex).parentIndex;
    glm::vec3 shoulderPosition;
    if (!getJointPosition(shoulderJointIndex, shoulderPosition)) {
        return;
    }
    // precomputed lengths
    float scale = extractUniformScale(_scale);
    float upperArmLength = geometry.joints.at(elbowJointIndex).distanceToParent * scale;
    float lowerArmLength = geometry.joints.at(jointIndex).distanceToParent * scale;
    
    // first set wrist position
    glm::vec3 wristPosition = position;
    
    glm::vec3 shoulderToWrist = wristPosition - shoulderPosition;
    float distanceToWrist = glm::length(shoulderToWrist);
    
    // prevent gimbal lock
    if (distanceToWrist > upperArmLength + lowerArmLength - EPSILON) {
        distanceToWrist = upperArmLength + lowerArmLength - EPSILON;
        shoulderToWrist = glm::normalize(shoulderToWrist) * distanceToWrist;
        wristPosition = shoulderPosition + shoulderToWrist;
    }
    
    // cosine of angle from upper arm to hand vector 
    float cosA = (upperArmLength * upperArmLength + distanceToWrist * distanceToWrist - lowerArmLength * lowerArmLength) / 
        (2 * upperArmLength * distanceToWrist);
    float mid = upperArmLength * cosA;
    float height = sqrt(upperArmLength * upperArmLength + mid * mid - 2 * upperArmLength * mid * cosA);
    
    // direction of the elbow
    glm::vec3 handNormal = glm::cross(rotation * glm::vec3(0.0f, 1.0f, 0.0f), shoulderToWrist); // elbow rotating with wrist
    glm::vec3 relaxedNormal = glm::cross(glm::vec3(0.0f, 1.0f, 0.0f), shoulderToWrist); // elbow pointing straight down
    const float NORMAL_WEIGHT = 0.5f;
    glm::vec3 finalNormal = glm::mix(relaxedNormal, handNormal, NORMAL_WEIGHT);
    
    bool rightHand = (jointIndex == geometry.rightHandJointIndex);
    if (rightHand ? (finalNormal.y > 0.0f) : (finalNormal.y < 0.0f)) {
        finalNormal.y = 0.0f; // dont allow elbows to point inward (y is vertical axis)
    }
    
    glm::vec3 tangent = glm::normalize(glm::cross(shoulderToWrist, finalNormal));
    
    // ik solution
    glm::vec3 elbowPosition = shoulderPosition + glm::normalize(shoulderToWrist) * mid - tangent * height;
    glm::vec3 forwardVector(rightHand ? -1.0f : 1.0f, 0.0f, 0.0f);
    glm::quat shoulderRotation = rotationBetween(forwardVector, elbowPosition - shoulderPosition);

    JointState& shoulderState = _jointStates[shoulderJointIndex];
    shoulderState.setRotationInBindFrame(shoulderRotation, PALM_PRIORITY);
    
    JointState& elbowState = _jointStates[elbowJointIndex];
    elbowState.setRotationInBindFrame(rotationBetween(shoulderRotation * forwardVector, wristPosition - elbowPosition) * shoulderRotation, PALM_PRIORITY);
    
    JointState& handState = _jointStates[jointIndex];
    handState.setRotationInBindFrame(rotation, PALM_PRIORITY);
}
Exemplo n.º 14
0
void SerialInterface::readData(float deltaTime) {
#ifdef __APPLE__
    
    int initialSamples = totalSamples;
    
    if (USING_INVENSENSE_MPU9150) { 
        unsigned char sensorBuffer[36];
        
        // ask the invensense for raw gyro data
        write(_serialDescriptor, "RD683B0E\n", 9);
        read(_serialDescriptor, sensorBuffer, 36);
        
        int accelXRate, accelYRate, accelZRate;
        
        convertHexToInt(sensorBuffer + 6, accelZRate);
        convertHexToInt(sensorBuffer + 10, accelYRate);
        convertHexToInt(sensorBuffer + 14, accelXRate);
        
        const float LSB_TO_METERS_PER_SECOND2 = 1.f / 16384.f * GRAVITY_EARTH;
                                                                //  From MPU-9150 register map, with setting on
                                                                //  highest resolution = +/- 2G
        
        _lastAcceleration = glm::vec3(-accelXRate, -accelYRate, -accelZRate) * LSB_TO_METERS_PER_SECOND2;
                
        
        int rollRate, yawRate, pitchRate;
        
        convertHexToInt(sensorBuffer + 22, rollRate);
        convertHexToInt(sensorBuffer + 26, yawRate);
        convertHexToInt(sensorBuffer + 30, pitchRate);
        
        //  Convert the integer rates to floats
        const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f;     //  From MPU-9150 register map, 2000 deg/sec.
        glm::vec3 rotationRates;
        rotationRates[0] = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND;
        rotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
        rotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND;

        // update and subtract the long term average
        _averageRotationRates = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates +
                1.f/(float)LONG_TERM_RATE_SAMPLES * rotationRates;
        rotationRates -= _averageRotationRates;

        // compute the angular acceleration
        glm::vec3 angularAcceleration = (deltaTime < EPSILON) ? glm::vec3() : (rotationRates - _lastRotationRates) / deltaTime;
        _lastRotationRates = rotationRates;
        
        //  Update raw rotation estimates
        glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) *
            glm::quat(glm::radians(deltaTime * _lastRotationRates));
        
        //  Update acceleration estimate: first, subtract gravity as rotated into current frame
        _estimatedAcceleration = (totalSamples < GRAVITY_SAMPLES) ? glm::vec3() :
            _lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
        
        // update and subtract the long term average
        _averageAcceleration = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration +
                1.f/(float)LONG_TERM_RATE_SAMPLES * _estimatedAcceleration;
        _estimatedAcceleration -= _averageAcceleration;
        
        //  Consider updating our angular velocity/acceleration to linear acceleration mapping
        if (glm::length(_estimatedAcceleration) > EPSILON &&
                (glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON)) {
            // compute predicted linear acceleration, find error between actual and predicted
            glm::vec3 predictedAcceleration = _angularVelocityToLinearAccel * _lastRotationRates +
                _angularAccelToLinearAccel * angularAcceleration;
            glm::vec3 error = _estimatedAcceleration - predictedAcceleration;
            
            // the "error" is actually what we want: the linear acceleration minus rotational influences
            _estimatedAcceleration = error;
            
            // adjust according to error in each dimension, in proportion to input magnitudes
            for (int i = 0; i < 3; i++) {
                if (fabsf(error[i]) < EPSILON) {
                    continue;
                }
                const float LEARNING_RATE = 0.001f;
                float rateSum = fabsf(_lastRotationRates.x) + fabsf(_lastRotationRates.y) + fabsf(_lastRotationRates.z);
                if (rateSum > EPSILON) {
                    for (int j = 0; j < 3; j++) {
                        float proportion = LEARNING_RATE * fabsf(_lastRotationRates[j]) / rateSum;
                        if (proportion > EPSILON) {
                            _angularVelocityToLinearAccel[j][i] += error[i] * proportion / _lastRotationRates[j];
                        }
                    }
                }
                float accelSum = fabsf(angularAcceleration.x) + fabsf(angularAcceleration.y) + fabsf(angularAcceleration.z);
                if (accelSum > EPSILON) {
                    for (int j = 0; j < 3; j++) {
                        float proportion = LEARNING_RATE * fabsf(angularAcceleration[j]) / accelSum;
                        if (proportion > EPSILON) {
                            _angularAccelToLinearAccel[j][i] += error[i] * proportion / angularAcceleration[j];
                        }
                    }                
                }
            }
        }
        
        // rotate estimated acceleration into global rotation frame
        _estimatedAcceleration = estimatedRotation * _estimatedAcceleration;
        
        //  Update estimated position and velocity
        float const DECAY_VELOCITY = 0.975f;
        float const DECAY_POSITION = 0.975f;
        _estimatedVelocity += deltaTime * _estimatedAcceleration;
        _estimatedPosition += deltaTime * _estimatedVelocity;
        _estimatedVelocity *= DECAY_VELOCITY;
        
        //  Attempt to fuse gyro position with webcam position
        Webcam* webcam = Application::getInstance()->getWebcam();
        if (webcam->isActive()) {
            const float WEBCAM_POSITION_FUSION = 0.5f;
            _estimatedPosition = glm::mix(_estimatedPosition, webcam->getEstimatedPosition(), WEBCAM_POSITION_FUSION);
               
        } else {
            _estimatedPosition *= DECAY_POSITION;
        }
            
        //  Accumulate a set of initial baseline readings for setting gravity
        if (totalSamples == 0) {
            _gravity = _lastAcceleration;
        } 
        else {
            if (totalSamples < GRAVITY_SAMPLES) {
                _gravity = (1.f - 1.f/(float)GRAVITY_SAMPLES) * _gravity +
                1.f/(float)GRAVITY_SAMPLES * _lastAcceleration;
            } else {
                //  Use gravity reading to do sensor fusion on the pitch and roll estimation
                estimatedRotation = safeMix(estimatedRotation,
                    rotationBetween(estimatedRotation * _lastAcceleration, _gravity) * estimatedRotation,
                    1.0f / SENSOR_FUSION_SAMPLES);
                
                //  Without a compass heading, always decay estimated Yaw slightly
                const float YAW_DECAY = 0.999f;
                glm::vec3 forward = estimatedRotation * glm::vec3(0.0f, 0.0f, -1.0f);
                estimatedRotation = safeMix(glm::angleAxis(glm::degrees(atan2f(forward.x, -forward.z)),
                    glm::vec3(0.0f, 1.0f, 0.0f)) * estimatedRotation, estimatedRotation, YAW_DECAY);
            }
        }
        
        _estimatedRotation = safeEulerAngles(estimatedRotation); 
        
        totalSamples++;
    } 
    
    if (initialSamples == totalSamples) {        
        timeval now;
        gettimeofday(&now, NULL);
        
        if (diffclock(&lastGoodRead, &now) > NO_READ_MAXIMUM_MSECS) {
            printLog("No data - Shutting down SerialInterface.\n");
            resetSerial();
        }
    } else {
        gettimeofday(&lastGoodRead, NULL);
    }
#endif
}
Exemplo n.º 15
0
void PerlinFace::render() {
    glPushMatrix();

    update();

    // Jump to head position, orientation and scale
    glm::quat orientation = _owningHead->getOrientation();
    glm::vec3 axis = glm::axis(orientation);
    glTranslatef(_owningHead->getPosition().x, _owningHead->getPosition().y, _owningHead->getPosition().z);
    glScalef(_owningHead->getScale(), _owningHead->getScale(), _owningHead->getScale());
    glRotatef(glm::angle(orientation), axis.x, axis.y, axis.z);


    glPushMatrix();
    { // This block use the coordinates system of perlin's face points.
        // Correct head scale and offset from hard coded points coordinates.
        glScalef(2.0f * BODY_BALL_RADIUS_HEAD_BASE / (_vertices[HAIR_2].y - _vertices[JAW_BOTTOM].y),
                 2.0f * BODY_BALL_RADIUS_HEAD_BASE / (_vertices[HAIR_2].y - _vertices[JAW_BOTTOM].y),
                 2.0f * BODY_BALL_RADIUS_HEAD_BASE / (_vertices[HAIR_2].y - _vertices[JAW_BOTTOM].y));
        glTranslatef(0, -60.0f, 20.0f);

        /**/
        glEnableClientState(GL_VERTEX_ARRAY);
        glBindBuffer(GL_ARRAY_BUFFER, _vboID);
        glVertexPointer(FLOAT_PER_VERTEX, GL_FLOAT, 0, 0);

        glEnableClientState(GL_NORMAL_ARRAY);
        glBindBuffer(GL_ARRAY_BUFFER, _nboID);
        glNormalPointer(GL_FLOAT, 0, 0);

        glEnableClientState(GL_COLOR_ARRAY);
        glBindBuffer(GL_ARRAY_BUFFER, _cboID);
        glColorPointer(3, GL_UNSIGNED_BYTE, 0, 0);

        glDrawArrays(GL_TRIANGLES, 0, VERTEX_PER_TRIANGLE * _trianglesCount);

        // Draw eyes
        glColor3d(0, 0, 0);
        glBegin(GL_LINE_LOOP);
        glVertex3d(_vertices[EYE_LEFT].x, _vertices[EYE_LEFT].y, _vertices[EYE_LEFT].z);
        glVertex3d(_vertices[EYE_MID_TOP].x, _vertices[EYE_MID_TOP].y, _vertices[EYE_MID_TOP].z);
        glVertex3d(_vertices[EYE_RIGHT].x, _vertices[EYE_RIGHT].y, _vertices[EYE_RIGHT].z);
        glVertex3d(_vertices[EYE_MID_BOTTOM].x, _vertices[EYE_MID_BOTTOM].y, _vertices[EYE_MID_BOTTOM].z);
        glEnd();

        glBegin(GL_LINE_LOOP);
        glVertex3d(_vertices[NUM_VERTICES + EYE_LEFT].x,
                   _vertices[NUM_VERTICES + EYE_LEFT].y,
                   _vertices[NUM_VERTICES + EYE_LEFT].z);
        glVertex3d(_vertices[NUM_VERTICES + EYE_MID_TOP].x,
                   _vertices[NUM_VERTICES + EYE_MID_TOP].y,
                   _vertices[NUM_VERTICES + EYE_MID_TOP].z);
        glVertex3d(_vertices[NUM_VERTICES + EYE_RIGHT].x,
                   _vertices[NUM_VERTICES + EYE_RIGHT].y,
                   _vertices[NUM_VERTICES + EYE_RIGHT].z);
        glVertex3d(_vertices[NUM_VERTICES + EYE_MID_BOTTOM].x,
                   _vertices[NUM_VERTICES + EYE_MID_BOTTOM].y,
                   _vertices[NUM_VERTICES + EYE_MID_BOTTOM].z);
        glEnd();
    }
    glPopMatrix();

    const float EYEBALL_RADIUS           =  0.008f;
    const float EYEBALL_COLOR[4]         =  { 0.9f, 0.9f, 0.8f, 1.0f };
    const float IRIS_RADIUS              =  0.0035;
    const float IRIS_PROTRUSION          =  0.0065f;
    // render white ball of left eyeball

    glm::vec3 eyePos = glm::vec3(0.024f, 0.0f, -0.032f);


    Head::_irisProgram.bind();
    _owningHead->_dilatedIrisTexture = Head::_irisTexture->getDilatedTexture(_owningHead->_pupilDilation);
    glBindTexture(GL_TEXTURE_2D, _owningHead->_dilatedIrisTexture->getID());
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_BORDER);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_BORDER);

    glEnable(GL_TEXTURE_2D);

    orientation = _owningHead->getOrientation();
    glm::vec3 front = orientation * IDENTITY_FRONT;

    // render left iris
    glm::quat leftIrisRotation;
    glPushMatrix();
    {
        glTranslatef(eyePos.x, eyePos.y, eyePos.z); //translate to eyeball position

        //rotate the eyeball to aim towards the lookat position
        glm::vec3 targetLookatVector = _owningHead->_lookAtPosition - eyePos;
        leftIrisRotation = rotationBetween(front, targetLookatVector) * orientation;
        glm::vec3 rotationAxis = glm::axis(leftIrisRotation);
        glRotatef(glm::angle(leftIrisRotation), rotationAxis.x, rotationAxis.y, rotationAxis.z);
        glTranslatef(0.0f, 0.0f, IRIS_PROTRUSION);
        glScalef(IRIS_RADIUS * 2.0f,
                 IRIS_RADIUS * 2.0f,
                 IRIS_RADIUS); // flatten the iris

        // this ugliness is simply to invert the model transform and get the eye position in model space
        Head::_irisProgram.setUniform(Head::_eyePositionLocation, (glm::inverse(leftIrisRotation) *
                                      (Application::getInstance()->getCamera()->getPosition() - eyePos) +
                                      glm::vec3(0.0f, 0.0f, IRIS_PROTRUSION)) * glm::vec3(1.0f / (IRIS_RADIUS * 2.0f),
                                              1.0f / (IRIS_RADIUS * 2.0f), 1.0f / IRIS_RADIUS));

        glutSolidSphere(0.5f, 15, 15);
    }
    glPopMatrix();

    eyePos.x = - eyePos.x;

    // render right iris
    glm::quat rightIrisRotation;
    glPushMatrix();
    {
        glTranslatef(eyePos.x, eyePos.y, eyePos.z); //translate to eyeball position

        //rotate the eyeball to aim towards the lookat position
        glm::vec3 targetLookatVector = _owningHead->_lookAtPosition - eyePos;
        rightIrisRotation = rotationBetween(front, targetLookatVector) * orientation;
        glm::vec3 rotationAxis = glm::axis(rightIrisRotation);
        glRotatef(glm::angle(rightIrisRotation), rotationAxis.x, rotationAxis.y, rotationAxis.z);
        glTranslatef(0.0f, 0.0f, IRIS_PROTRUSION);
        glScalef(IRIS_RADIUS * 2.0f,
                 IRIS_RADIUS * 2.0f,
                 IRIS_RADIUS); // flatten the iris

        // this ugliness is simply to invert the model transform and get the eye position in model space
        Head::_irisProgram.setUniform(Head::_eyePositionLocation, (glm::inverse(rightIrisRotation) *
                                      (Application::getInstance()->getCamera()->getPosition() - eyePos) +
                                      glm::vec3(0.0f, 0.0f, IRIS_PROTRUSION)) * glm::vec3(1.0f / (IRIS_RADIUS * 2.0f),
                                              1.0f / (IRIS_RADIUS * 2.0f), 1.0f / IRIS_RADIUS));

        glutSolidSphere(0.5f, 15, 15);
    }
    glPopMatrix();

    Head::_irisProgram.release();
    glBindTexture(GL_TEXTURE_2D, 0);
    glDisable(GL_TEXTURE_2D);




    glPushMatrix();
    glColor4fv(EYEBALL_COLOR);
    glTranslatef(eyePos.x, eyePos.y, eyePos.z);
    glutSolidSphere(EYEBALL_RADIUS, 30, 30);
    glPopMatrix();

    //render white ball of right eyeball
    glPushMatrix();
    glColor4fv(EYEBALL_COLOR);
    glTranslatef(-eyePos.x, eyePos.y, eyePos.z);
    glutSolidSphere(EYEBALL_RADIUS, 30, 30);
    glPopMatrix();


    glPopMatrix();
}
Exemplo n.º 16
0
void Skeleton::initialize() {    
    
    for (int b = 0; b < NUM_AVATAR_JOINTS; b++) {
        joint[b].parent              = AVATAR_JOINT_NULL;
        joint[b].position            = glm::vec3(0.0, 0.0, 0.0);
        joint[b].defaultPosePosition = glm::vec3(0.0, 0.0, 0.0);
        joint[b].rotation            = glm::quat(1.0f, 0.0f, 0.0f, 0.0f);
        joint[b].length              = 0.0;
        joint[b].bindRadius          = 1.0f / 8;
    }
    
    // specify the parental hierarchy
    joint[ AVATAR_JOINT_PELVIS		      ].parent = AVATAR_JOINT_NULL;
    joint[ AVATAR_JOINT_TORSO             ].parent = AVATAR_JOINT_PELVIS;
    joint[ AVATAR_JOINT_CHEST		      ].parent = AVATAR_JOINT_TORSO;
    joint[ AVATAR_JOINT_NECK_BASE	      ].parent = AVATAR_JOINT_CHEST;
    joint[ AVATAR_JOINT_HEAD_BASE         ].parent = AVATAR_JOINT_NECK_BASE;
    joint[ AVATAR_JOINT_HEAD_TOP          ].parent = AVATAR_JOINT_HEAD_BASE;
    joint[ AVATAR_JOINT_LEFT_COLLAR       ].parent = AVATAR_JOINT_CHEST;
    joint[ AVATAR_JOINT_LEFT_SHOULDER     ].parent = AVATAR_JOINT_LEFT_COLLAR;
    joint[ AVATAR_JOINT_LEFT_ELBOW	      ].parent = AVATAR_JOINT_LEFT_SHOULDER;
    joint[ AVATAR_JOINT_LEFT_WRIST		  ].parent = AVATAR_JOINT_LEFT_ELBOW;
    joint[ AVATAR_JOINT_LEFT_FINGERTIPS   ].parent = AVATAR_JOINT_LEFT_WRIST;
    joint[ AVATAR_JOINT_RIGHT_COLLAR      ].parent = AVATAR_JOINT_CHEST;
    joint[ AVATAR_JOINT_RIGHT_SHOULDER	  ].parent = AVATAR_JOINT_RIGHT_COLLAR;
    joint[ AVATAR_JOINT_RIGHT_ELBOW	      ].parent = AVATAR_JOINT_RIGHT_SHOULDER;
    joint[ AVATAR_JOINT_RIGHT_WRIST       ].parent = AVATAR_JOINT_RIGHT_ELBOW;
    joint[ AVATAR_JOINT_RIGHT_FINGERTIPS  ].parent = AVATAR_JOINT_RIGHT_WRIST;
    joint[ AVATAR_JOINT_LEFT_HIP		  ].parent = AVATAR_JOINT_PELVIS;
    joint[ AVATAR_JOINT_LEFT_KNEE		  ].parent = AVATAR_JOINT_LEFT_HIP;
    joint[ AVATAR_JOINT_LEFT_HEEL		  ].parent = AVATAR_JOINT_LEFT_KNEE;
    joint[ AVATAR_JOINT_LEFT_TOES		  ].parent = AVATAR_JOINT_LEFT_HEEL;
    joint[ AVATAR_JOINT_RIGHT_HIP		  ].parent = AVATAR_JOINT_PELVIS;
    joint[ AVATAR_JOINT_RIGHT_KNEE		  ].parent = AVATAR_JOINT_RIGHT_HIP;
    joint[ AVATAR_JOINT_RIGHT_HEEL		  ].parent = AVATAR_JOINT_RIGHT_KNEE;
    joint[ AVATAR_JOINT_RIGHT_TOES		  ].parent = AVATAR_JOINT_RIGHT_HEEL;
    
    // specify the bind pose position
    joint[ AVATAR_JOINT_PELVIS           ].bindPosePosition = glm::vec3(  0.0,   0.0,    0.0  );
    joint[ AVATAR_JOINT_TORSO            ].bindPosePosition = glm::vec3(  0.0,   0.09,  -0.01 );
    joint[ AVATAR_JOINT_CHEST            ].bindPosePosition = glm::vec3(  0.0,   0.09,  -0.01 );
    joint[ AVATAR_JOINT_NECK_BASE        ].bindPosePosition = glm::vec3(  0.0,   0.14,   0.01 );
    joint[ AVATAR_JOINT_HEAD_BASE        ].bindPosePosition = glm::vec3(  0.0,   0.04,   0.00 );
    joint[ AVATAR_JOINT_HEAD_TOP         ].bindPosePosition = glm::vec3(  0.0,   0.04,   0.00 );
    
    joint[ AVATAR_JOINT_LEFT_COLLAR      ].bindPosePosition = glm::vec3( -0.06,  0.04,   0.01 );
    joint[ AVATAR_JOINT_LEFT_SHOULDER    ].bindPosePosition = glm::vec3( -0.05,  0.0,    0.01 );
    joint[ AVATAR_JOINT_LEFT_ELBOW       ].bindPosePosition = glm::vec3( -0.16,  0.0,    0.0  );
    joint[ AVATAR_JOINT_LEFT_WRIST       ].bindPosePosition = glm::vec3( -0.12,  0.0,    0.0  );
    joint[ AVATAR_JOINT_LEFT_FINGERTIPS  ].bindPosePosition = glm::vec3( -0.1,   0.0,    0.0  );
    
    joint[ AVATAR_JOINT_RIGHT_COLLAR     ].bindPosePosition = glm::vec3(  0.06,  0.04,   0.01 );
    joint[ AVATAR_JOINT_RIGHT_SHOULDER   ].bindPosePosition = glm::vec3(  0.05,  0.0,    0.01 );
    joint[ AVATAR_JOINT_RIGHT_ELBOW      ].bindPosePosition = glm::vec3(  0.16,  0.0,    0.0  );
    joint[ AVATAR_JOINT_RIGHT_WRIST      ].bindPosePosition = glm::vec3(  0.12,  0.0,    0.0  );
    joint[ AVATAR_JOINT_RIGHT_FINGERTIPS ].bindPosePosition = glm::vec3(  0.1,   0.0,    0.0  );
    
    joint[ AVATAR_JOINT_LEFT_HIP         ].bindPosePosition = glm::vec3( -0.05,  0.0,    0.02 );
    joint[ AVATAR_JOINT_LEFT_KNEE        ].bindPosePosition = glm::vec3(  0.00, -0.25,   0.00 );
    joint[ AVATAR_JOINT_LEFT_HEEL        ].bindPosePosition = glm::vec3(  0.00, -0.23,   0.00 );
    joint[ AVATAR_JOINT_LEFT_TOES        ].bindPosePosition = glm::vec3(  0.00,  0.00,  -0.06 );
    
    joint[ AVATAR_JOINT_RIGHT_HIP        ].bindPosePosition = glm::vec3(  0.05,  0.0,    0.02 );
    joint[ AVATAR_JOINT_RIGHT_KNEE       ].bindPosePosition = glm::vec3(  0.00, -0.25,   0.00 );
    joint[ AVATAR_JOINT_RIGHT_HEEL       ].bindPosePosition = glm::vec3(  0.00, -0.23,   0.00 );
    joint[ AVATAR_JOINT_RIGHT_TOES       ].bindPosePosition = glm::vec3(  0.00,  0.00,  -0.06 );
    
    // specify the default pose position
    joint[ AVATAR_JOINT_PELVIS           ].defaultPosePosition = glm::vec3(  0.0,   0.0,    0.0  );
    joint[ AVATAR_JOINT_TORSO            ].defaultPosePosition = glm::vec3(  0.0,   0.09,  -0.01 );
    joint[ AVATAR_JOINT_CHEST            ].defaultPosePosition = glm::vec3(  0.0,   0.09,  -0.01 );
    joint[ AVATAR_JOINT_NECK_BASE        ].defaultPosePosition = glm::vec3(  0.0,   0.14,   0.01 );
    joint[ AVATAR_JOINT_HEAD_BASE        ].defaultPosePosition = glm::vec3(  0.0,   0.04,   0.00 );
    joint[ AVATAR_JOINT_HEAD_TOP         ].defaultPosePosition = glm::vec3(  0.0,   0.04,   0.00 );
    
    joint[ AVATAR_JOINT_LEFT_COLLAR      ].defaultPosePosition = glm::vec3( -0.06,  0.04,   0.01 );
    joint[ AVATAR_JOINT_LEFT_SHOULDER    ].defaultPosePosition = glm::vec3( -0.05,  0.0,    0.01 );
    joint[ AVATAR_JOINT_LEFT_ELBOW       ].defaultPosePosition = glm::vec3(  0.0,  -0.16,   0.0  );
    joint[ AVATAR_JOINT_LEFT_WRIST       ].defaultPosePosition = glm::vec3(  0.0,  -0.117,  0.0  );
    joint[ AVATAR_JOINT_LEFT_FINGERTIPS  ].defaultPosePosition = glm::vec3(  0.0,  -0.1,    0.0  );
    
    joint[ AVATAR_JOINT_RIGHT_COLLAR     ].defaultPosePosition = glm::vec3(  0.06,  0.04,   0.01 );
    joint[ AVATAR_JOINT_RIGHT_SHOULDER   ].defaultPosePosition = glm::vec3(  0.05,  0.0,    0.01 );
    joint[ AVATAR_JOINT_RIGHT_ELBOW      ].defaultPosePosition = glm::vec3(  0.0,  -0.16,   0.0  );
    joint[ AVATAR_JOINT_RIGHT_WRIST      ].defaultPosePosition = glm::vec3(  0.0,  -0.117,  0.0  );
    joint[ AVATAR_JOINT_RIGHT_FINGERTIPS ].defaultPosePosition = glm::vec3(  0.0,  -0.1,    0.0  );
    
    joint[ AVATAR_JOINT_LEFT_HIP         ].defaultPosePosition = glm::vec3( -0.05,  0.0,    0.02 );
    joint[ AVATAR_JOINT_LEFT_KNEE        ].defaultPosePosition = glm::vec3(  0.01, -0.25,  -0.03 );
    joint[ AVATAR_JOINT_LEFT_HEEL        ].defaultPosePosition = glm::vec3(  0.01, -0.22,   0.08 );
    joint[ AVATAR_JOINT_LEFT_TOES        ].defaultPosePosition = glm::vec3(  0.00, -0.03,  -0.05 );
    
    joint[ AVATAR_JOINT_RIGHT_HIP        ].defaultPosePosition = glm::vec3(  0.05,  0.0,    0.02 );
    joint[ AVATAR_JOINT_RIGHT_KNEE       ].defaultPosePosition = glm::vec3( -0.01, -0.25,  -0.03 );
    joint[ AVATAR_JOINT_RIGHT_HEEL       ].defaultPosePosition = glm::vec3( -0.01, -0.22,   0.08 );
    joint[ AVATAR_JOINT_RIGHT_TOES       ].defaultPosePosition = glm::vec3(  0.00, -0.03,  -0.05 );
             
    // calculate bone length, absolute bind positions/rotations
    for (int b = 0; b < NUM_AVATAR_JOINTS; b++) {
        joint[b].length = glm::length(joint[b].defaultPosePosition);
        
        if (joint[b].parent == AVATAR_JOINT_NULL) {
            joint[b].absoluteBindPosePosition = joint[b].bindPosePosition;
            joint[b].absoluteBindPoseRotation = glm::quat();
        } else {
            joint[b].absoluteBindPosePosition = joint[ joint[b].parent ].absoluteBindPosePosition +
                joint[b].bindPosePosition;
            glm::vec3 parentDirection = joint[ joint[b].parent ].absoluteBindPoseRotation * JOINT_DIRECTION;
            joint[b].absoluteBindPoseRotation = rotationBetween(parentDirection, joint[b].bindPosePosition) *
                joint[ joint[b].parent ].absoluteBindPoseRotation;
        }
    }
}
Exemplo n.º 17
0
void DeferredLightingEffect::render(RenderArgs* args) {
    // perform deferred lighting, rendering to free fbo
    glDisable(GL_BLEND);
    glDisable(GL_LIGHTING);
    glDisable(GL_DEPTH_TEST);
    glDisable(GL_COLOR_MATERIAL);
    glDepthMask(false);

    auto textureCache = DependencyManager::get<TextureCache>();
    
    glBindFramebuffer(GL_FRAMEBUFFER, 0 );

    QSize framebufferSize = textureCache->getFrameBufferSize();
    
    // binding the first framebuffer
    auto freeFBO = DependencyManager::get<GlowEffect>()->getFreeFramebuffer();
    glBindFramebuffer(GL_FRAMEBUFFER, gpu::GLBackend::getFramebufferID(freeFBO));
 
    glClear(GL_COLOR_BUFFER_BIT);
   // glEnable(GL_FRAMEBUFFER_SRGB);

   // glBindTexture(GL_TEXTURE_2D, primaryFBO->texture());
    glBindTexture(GL_TEXTURE_2D, textureCache->getPrimaryColorTextureID());
    
    glActiveTexture(GL_TEXTURE1);
    glBindTexture(GL_TEXTURE_2D, textureCache->getPrimaryNormalTextureID());
    
    glActiveTexture(GL_TEXTURE2);
    glBindTexture(GL_TEXTURE_2D, textureCache->getPrimarySpecularTextureID());
    
    glActiveTexture(GL_TEXTURE3);
    glBindTexture(GL_TEXTURE_2D, textureCache->getPrimaryDepthTextureID());
        
    // get the viewport side (left, right, both)
    int viewport[4];
    glGetIntegerv(GL_VIEWPORT, viewport);
    const int VIEWPORT_X_INDEX = 0;
    const int VIEWPORT_Y_INDEX = 1;
    const int VIEWPORT_WIDTH_INDEX = 2;
    const int VIEWPORT_HEIGHT_INDEX = 3;

    float sMin = viewport[VIEWPORT_X_INDEX] / (float)framebufferSize.width();
    float sWidth = viewport[VIEWPORT_WIDTH_INDEX] / (float)framebufferSize.width();
    float tMin = viewport[VIEWPORT_Y_INDEX] / (float)framebufferSize.height();
    float tHeight = viewport[VIEWPORT_HEIGHT_INDEX] / (float)framebufferSize.height();

    bool useSkyboxCubemap = (_skybox) && (_skybox->getCubemap());

    // Fetch the ViewMatrix;
    glm::mat4 invViewMat;
    _viewState->getViewTransform().getMatrix(invViewMat);

    ProgramObject* program = &_directionalLight;
    const LightLocations* locations = &_directionalLightLocations;
    bool shadowsEnabled = _viewState->getShadowsEnabled();
    if (shadowsEnabled) {
        glActiveTexture(GL_TEXTURE4);
        glBindTexture(GL_TEXTURE_2D, textureCache->getShadowDepthTextureID());
        
        program = &_directionalLightShadowMap;
        locations = &_directionalLightShadowMapLocations;
        if (_viewState->getCascadeShadowsEnabled()) {
            program = &_directionalLightCascadedShadowMap;
            locations = &_directionalLightCascadedShadowMapLocations;
            if (useSkyboxCubemap) {
                program = &_directionalSkyboxLightCascadedShadowMap;
                locations = &_directionalSkyboxLightCascadedShadowMapLocations;
            } else if (_ambientLightMode > -1) {
                program = &_directionalAmbientSphereLightCascadedShadowMap;
                locations = &_directionalAmbientSphereLightCascadedShadowMapLocations;
            }
            program->bind();
            program->setUniform(locations->shadowDistances, _viewState->getShadowDistances());
        
        } else {
            if (useSkyboxCubemap) {
                program = &_directionalSkyboxLightShadowMap;
                locations = &_directionalSkyboxLightShadowMapLocations;
            } else if (_ambientLightMode > -1) {
                program = &_directionalAmbientSphereLightShadowMap;
                locations = &_directionalAmbientSphereLightShadowMapLocations;
            }
            program->bind();
        }
        program->setUniformValue(locations->shadowScale,
            1.0f / textureCache->getShadowFramebuffer()->getWidth());
        
    } else {
        if (useSkyboxCubemap) {
                program = &_directionalSkyboxLight;
                locations = &_directionalSkyboxLightLocations;
        } else if (_ambientLightMode > -1) {
            program = &_directionalAmbientSphereLight;
            locations = &_directionalAmbientSphereLightLocations;
        }
        program->bind();
    }

    {
        auto globalLight = _allocatedLights[_globalLights.front()];
    
        if (locations->ambientSphere >= 0) {
            gpu::SphericalHarmonics sh = globalLight->getAmbientSphere();
            if (useSkyboxCubemap && _skybox->getCubemap()->getIrradiance()) {
                sh = (*_skybox->getCubemap()->getIrradiance());
            }
            for (int i =0; i <gpu::SphericalHarmonics::NUM_COEFFICIENTS; i++) {
                program->setUniformValue(locations->ambientSphere + i, *(((QVector4D*) &sh) + i)); 
            }
        }
    
        if (useSkyboxCubemap) {
            glActiveTexture(GL_TEXTURE5);
            glBindTexture(GL_TEXTURE_CUBE_MAP, gpu::GLBackend::getTextureID(_skybox->getCubemap()));
        }

        if (locations->lightBufferUnit >= 0) {
            gpu::Batch batch;
            batch.setUniformBuffer(locations->lightBufferUnit, globalLight->getSchemaBuffer());
            gpu::GLBackend::renderBatch(batch);
        }
        
        if (_atmosphere && (locations->atmosphereBufferUnit >= 0)) {
            gpu::Batch batch;
            batch.setUniformBuffer(locations->atmosphereBufferUnit, _atmosphere->getDataBuffer());
            gpu::GLBackend::renderBatch(batch);
        }
        glUniformMatrix4fv(locations->invViewMat, 1, false, reinterpret_cast< const GLfloat* >(&invViewMat));
    }

    float left, right, bottom, top, nearVal, farVal;
    glm::vec4 nearClipPlane, farClipPlane;
    _viewState->computeOffAxisFrustum(left, right, bottom, top, nearVal, farVal, nearClipPlane, farClipPlane);
    program->setUniformValue(locations->nearLocation, nearVal);
    float depthScale = (farVal - nearVal) / farVal;
    program->setUniformValue(locations->depthScale, depthScale);
    float nearScale = -1.0f / nearVal;
    float depthTexCoordScaleS = (right - left) * nearScale / sWidth;
    float depthTexCoordScaleT = (top - bottom) * nearScale / tHeight;
    float depthTexCoordOffsetS = left * nearScale - sMin * depthTexCoordScaleS;
    float depthTexCoordOffsetT = bottom * nearScale - tMin * depthTexCoordScaleT;
    program->setUniformValue(locations->depthTexCoordOffset, depthTexCoordOffsetS, depthTexCoordOffsetT);
    program->setUniformValue(locations->depthTexCoordScale, depthTexCoordScaleS, depthTexCoordScaleT);
    
    renderFullscreenQuad(sMin, sMin + sWidth, tMin, tMin + tHeight);
    
    program->release();

    if (useSkyboxCubemap) {
        glBindTexture(GL_TEXTURE_CUBE_MAP, 0);
        if (!shadowsEnabled) {
            glActiveTexture(GL_TEXTURE3);
        }
    }

    if (shadowsEnabled) {
        glBindTexture(GL_TEXTURE_2D, 0);        
        glActiveTexture(GL_TEXTURE3);
    }
    
    // additive blending
    glEnable(GL_BLEND);
    glBlendFunc(GL_ONE, GL_ONE);
    
    glEnable(GL_CULL_FACE);
    
    glm::vec4 sCoefficients(sWidth / 2.0f, 0.0f, 0.0f, sMin + sWidth / 2.0f);
    glm::vec4 tCoefficients(0.0f, tHeight / 2.0f, 0.0f, tMin + tHeight / 2.0f);
    glTexGenfv(GL_S, GL_OBJECT_PLANE, (const GLfloat*)&sCoefficients);
    glTexGenfv(GL_T, GL_OBJECT_PLANE, (const GLfloat*)&tCoefficients);
    
    // enlarge the scales slightly to account for tesselation
    const float SCALE_EXPANSION = 0.05f;
    
    const glm::vec3& eyePoint = _viewState->getCurrentViewFrustum()->getPosition();
    float nearRadius = glm::distance(eyePoint, _viewState->getCurrentViewFrustum()->getNearTopLeft());

    auto geometryCache = DependencyManager::get<GeometryCache>();
    
    if (!_pointLights.empty()) {
        _pointLight.bind();
        _pointLight.setUniformValue(_pointLightLocations.nearLocation, nearVal);
        _pointLight.setUniformValue(_pointLightLocations.depthScale, depthScale);
        _pointLight.setUniformValue(_pointLightLocations.depthTexCoordOffset, depthTexCoordOffsetS, depthTexCoordOffsetT);
        _pointLight.setUniformValue(_pointLightLocations.depthTexCoordScale, depthTexCoordScaleS, depthTexCoordScaleT);

        for (auto lightID : _pointLights) {
            auto light = _allocatedLights[lightID];

            if (_pointLightLocations.lightBufferUnit >= 0) {
                gpu::Batch batch;
                batch.setUniformBuffer(_pointLightLocations.lightBufferUnit, light->getSchemaBuffer());
                gpu::GLBackend::renderBatch(batch);
            }
            glUniformMatrix4fv(_pointLightLocations.invViewMat, 1, false, reinterpret_cast< const GLfloat* >(&invViewMat));

            glPushMatrix();
            
            float expandedRadius = light->getMaximumRadius() * (1.0f + SCALE_EXPANSION);
            if (glm::distance(eyePoint, glm::vec3(light->getPosition())) < expandedRadius + nearRadius) {
                glLoadIdentity();
                glTranslatef(0.0f, 0.0f, -1.0f);
                
                glMatrixMode(GL_PROJECTION);
                glPushMatrix();
                glLoadIdentity();
                
                renderFullscreenQuad();
            
                glPopMatrix();
                glMatrixMode(GL_MODELVIEW);
                
            } else {
                glTranslatef(light->getPosition().x, light->getPosition().y, light->getPosition().z);   
                geometryCache->renderSphere(expandedRadius, 32, 32, glm::vec4(1.0f, 1.0f, 1.0f, 1.0f));
            }
            
            glPopMatrix();
        }
        _pointLights.clear();
        
        _pointLight.release();
    }
    
    if (!_spotLights.empty()) {
        _spotLight.bind();
        _spotLight.setUniformValue(_spotLightLocations.nearLocation, nearVal);
        _spotLight.setUniformValue(_spotLightLocations.depthScale, depthScale);
        _spotLight.setUniformValue(_spotLightLocations.depthTexCoordOffset, depthTexCoordOffsetS, depthTexCoordOffsetT);
        _spotLight.setUniformValue(_spotLightLocations.depthTexCoordScale, depthTexCoordScaleS, depthTexCoordScaleT);
        
        for (auto lightID : _spotLights) {
            auto light = _allocatedLights[lightID];

            if (_spotLightLocations.lightBufferUnit >= 0) {
                gpu::Batch batch;
                batch.setUniformBuffer(_spotLightLocations.lightBufferUnit, light->getSchemaBuffer());
                gpu::GLBackend::renderBatch(batch);
            }
            glUniformMatrix4fv(_spotLightLocations.invViewMat, 1, false, reinterpret_cast< const GLfloat* >(&invViewMat));

            glPushMatrix();
            
            float expandedRadius = light->getMaximumRadius() * (1.0f + SCALE_EXPANSION);
            float edgeRadius = expandedRadius / glm::cos(light->getSpotAngle());
            if (glm::distance(eyePoint, glm::vec3(light->getPosition())) < edgeRadius + nearRadius) {
                glLoadIdentity();
                glTranslatef(0.0f, 0.0f, -1.0f);
                
                glMatrixMode(GL_PROJECTION);
                glPushMatrix();
                glLoadIdentity();
                
                renderFullscreenQuad();
                
                glPopMatrix();
                glMatrixMode(GL_MODELVIEW);
                
            } else {
                glTranslatef(light->getPosition().x, light->getPosition().y, light->getPosition().z);
                glm::quat spotRotation = rotationBetween(glm::vec3(0.0f, 0.0f, -1.0f), light->getDirection());
                glm::vec3 axis = glm::axis(spotRotation);
                glRotatef(glm::degrees(glm::angle(spotRotation)), axis.x, axis.y, axis.z);   
                glTranslatef(0.0f, 0.0f, -light->getMaximumRadius() * (1.0f + SCALE_EXPANSION * 0.5f));  
                geometryCache->renderCone(expandedRadius * glm::tan(light->getSpotAngle()),
                    expandedRadius, 32, 1);
            }
            
            glPopMatrix();
        }
        _spotLights.clear();
        
        _spotLight.release();
    }
    
    glBindTexture(GL_TEXTURE_2D, 0);
        
    glActiveTexture(GL_TEXTURE2);
    glBindTexture(GL_TEXTURE_2D, 0);
    
    glActiveTexture(GL_TEXTURE1);
    glBindTexture(GL_TEXTURE_2D, 0);
    
    glActiveTexture(GL_TEXTURE0);
    glBindTexture(GL_TEXTURE_2D, 0);
  //  glDisable(GL_FRAMEBUFFER_SRGB);
    
    // End of the Lighting pass
}
Exemplo n.º 18
0
void SerialInterface::readData(float deltaTime) {
#ifndef _WIN32
    
    int initialSamples = totalSamples;
    
    if (USING_INVENSENSE_MPU9150) { 

        // ask the invensense for raw gyro data
        short accelData[3];
        if (mpu_get_accel_reg(accelData, 0)) {
            close(_serialDescriptor);
            qDebug("Disconnected SerialUSB.\n");
            _active = false;
            return; // disconnected
        }
        
        const float LSB_TO_METERS_PER_SECOND2 = 1.f / 16384.f * GRAVITY_EARTH;
                                                                //  From MPU-9150 register map, with setting on
                                                                //  highest resolution = +/- 2G
        
        _lastAcceleration = glm::vec3(-accelData[2], -accelData[1], -accelData[0]) * LSB_TO_METERS_PER_SECOND2;
          
        short gyroData[3];
        mpu_get_gyro_reg(gyroData, 0);
        
        //  Convert the integer rates to floats
        const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f;     //  From MPU-9150 register map, 2000 deg/sec.
        glm::vec3 rotationRates;
        rotationRates[0] = ((float) -gyroData[2]) * LSB_TO_DEGREES_PER_SECOND;
        rotationRates[1] = ((float) -gyroData[1]) * LSB_TO_DEGREES_PER_SECOND;
        rotationRates[2] = ((float) -gyroData[0]) * LSB_TO_DEGREES_PER_SECOND;
      
        short compassData[3];
        mpu_get_compass_reg(compassData, 0);
      
        // Convert integer values to floats, update extents
        _lastCompass = glm::vec3(compassData[2], -compassData[0], -compassData[1]);
        
        // update and subtract the long term average
        _averageRotationRates = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates +
                1.f/(float)LONG_TERM_RATE_SAMPLES * rotationRates;
        rotationRates -= _averageRotationRates;

        // compute the angular acceleration
        glm::vec3 angularAcceleration = (deltaTime < EPSILON) ? glm::vec3() : (rotationRates - _lastRotationRates) / deltaTime;
        _lastRotationRates = rotationRates;
        
        //  Update raw rotation estimates
        glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) *
            glm::quat(glm::radians(deltaTime * _lastRotationRates));
        
        //  Update acceleration estimate: first, subtract gravity as rotated into current frame
        _estimatedAcceleration = (totalSamples < GRAVITY_SAMPLES) ? glm::vec3() :
            _lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
        
        // update and subtract the long term average
        _averageAcceleration = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration +
                1.f/(float)LONG_TERM_RATE_SAMPLES * _estimatedAcceleration;
        _estimatedAcceleration -= _averageAcceleration;
        
        //  Consider updating our angular velocity/acceleration to linear acceleration mapping
        if (glm::length(_estimatedAcceleration) > EPSILON &&
                (glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON)) {
            // compute predicted linear acceleration, find error between actual and predicted
            glm::vec3 predictedAcceleration = _angularVelocityToLinearAccel * _lastRotationRates +
                _angularAccelToLinearAccel * angularAcceleration;
            glm::vec3 error = _estimatedAcceleration - predictedAcceleration;
            
            // the "error" is actually what we want: the linear acceleration minus rotational influences
            _estimatedAcceleration = error;
            
            // adjust according to error in each dimension, in proportion to input magnitudes
            for (int i = 0; i < 3; i++) {
                if (fabsf(error[i]) < EPSILON) {
                    continue;
                }
                const float LEARNING_RATE = 0.001f;
                float rateSum = fabsf(_lastRotationRates.x) + fabsf(_lastRotationRates.y) + fabsf(_lastRotationRates.z);
                if (rateSum > EPSILON) {
                    for (int j = 0; j < 3; j++) {
                        float proportion = LEARNING_RATE * fabsf(_lastRotationRates[j]) / rateSum;
                        if (proportion > EPSILON) {
                            _angularVelocityToLinearAccel[j][i] += error[i] * proportion / _lastRotationRates[j];
                        }
                    }
                }
                float accelSum = fabsf(angularAcceleration.x) + fabsf(angularAcceleration.y) + fabsf(angularAcceleration.z);
                if (accelSum > EPSILON) {
                    for (int j = 0; j < 3; j++) {
                        float proportion = LEARNING_RATE * fabsf(angularAcceleration[j]) / accelSum;
                        if (proportion > EPSILON) {
                            _angularAccelToLinearAccel[j][i] += error[i] * proportion / angularAcceleration[j];
                        }
                    }                
                }
            }
        }
        
        // rotate estimated acceleration into global rotation frame
        _estimatedAcceleration = estimatedRotation * _estimatedAcceleration;
        
        //  Update estimated position and velocity
        float const DECAY_VELOCITY = 0.975f;
        float const DECAY_POSITION = 0.975f;
        _estimatedVelocity += deltaTime * _estimatedAcceleration;
        _estimatedPosition += deltaTime * _estimatedVelocity;
        _estimatedVelocity *= DECAY_VELOCITY;
        
        //  Attempt to fuse gyro position with webcam position
        Webcam* webcam = Application::getInstance()->getWebcam();
        if (webcam->isActive()) {
            const float WEBCAM_POSITION_FUSION = 0.5f;
            _estimatedPosition = glm::mix(_estimatedPosition, webcam->getEstimatedPosition(), WEBCAM_POSITION_FUSION);
               
        } else {
            _estimatedPosition *= DECAY_POSITION;
        }
            
        //  Accumulate a set of initial baseline readings for setting gravity
        if (totalSamples == 0) {
            _gravity = _lastAcceleration;
        } 
        else {
            if (totalSamples < GRAVITY_SAMPLES) {
                _gravity = glm::mix(_gravity, _lastAcceleration, 1.0f / GRAVITY_SAMPLES);
                
                //  North samples start later, because the initial compass readings are screwy
                int northSample = totalSamples - (GRAVITY_SAMPLES - NORTH_SAMPLES);
                if (northSample == 0) {
                    _north = _lastCompass;
                    
                } else if (northSample > 0) {
                    _north = glm::mix(_north, _lastCompass, 1.0f / NORTH_SAMPLES);
                }
            } else {
                //  Use gravity reading to do sensor fusion on the pitch and roll estimation
                estimatedRotation = safeMix(estimatedRotation,
                    rotationBetween(estimatedRotation * _lastAcceleration, _gravity) * estimatedRotation,
                    1.0f / ACCELERATION_SENSOR_FUSION_SAMPLES);
                
                //  Update the compass extents
                _compassMinima = glm::min(_compassMinima, _lastCompass);
                _compassMaxima = glm::max(_compassMaxima, _lastCompass);
        
                //  Same deal with the compass heading
                estimatedRotation = safeMix(estimatedRotation,
                    rotationBetween(estimatedRotation * recenterCompass(_lastCompass),
                        recenterCompass(_north)) * estimatedRotation,
                    1.0f / COMPASS_SENSOR_FUSION_SAMPLES);
            }
        }
        
        _estimatedRotation = safeEulerAngles(estimatedRotation); 
        
        totalSamples++;
    } 
    
    if (initialSamples == totalSamples) {        
        timeval now;
        gettimeofday(&now, NULL);
        
        if (diffclock(&lastGoodRead, &now) > NO_READ_MAXIMUM_MSECS) {
            qDebug("No data - Shutting down SerialInterface.\n");
            resetSerial();
        }
    } else {
        gettimeofday(&lastGoodRead, NULL);
    }
#endif
}