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::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);
}
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;
}
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 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);
}
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);
}
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;
}
}
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);
}
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());
}
}
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);
}
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);
}
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
}
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();
}
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;
}
}
}
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
}
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
}
