diff --git a/interface/src/Util.cpp b/interface/src/Util.cpp index 8898ab9eab..2057198daf 100644 --- a/interface/src/Util.cpp +++ b/interface/src/Util.cpp @@ -158,6 +158,66 @@ glm::quat safeMix(const glm::quat& q1, const glm::quat& q2, float proportion) { return glm::normalize(glm::quat(s0 * q1.w + s1 * ow, s0 * q1.x + s1 * ox, s0 * q1.y + s1 * oy, s0 * q1.z + s1 * oz)); } +glm::vec3 extractTranslation(const glm::mat4& matrix) { + return glm::vec3(matrix[3][0], matrix[3][1], matrix[3][2]); +} + +glm::quat extractRotation(const glm::mat4& matrix, bool assumeOrthogonal) { + // uses the iterative polar decomposition algorithm described by Ken Shoemake at + // http://www.cs.wisc.edu/graphics/Courses/838-s2002/Papers/polar-decomp.pdf + // code adapted from Clyde, https://github.com/threerings/clyde/blob/master/src/main/java/com/threerings/math/Matrix4f.java + + // start with the contents of the upper 3x3 portion of the matrix + glm::mat3 upper = glm::mat3(matrix); + if (!assumeOrthogonal) { + for (int i = 0; i < 10; i++) { + // store the results of the previous iteration + glm::mat3 previous = upper; + + // compute average of the matrix with its inverse transpose + float sd00 = previous[1][1] * previous[2][2] - previous[2][1] * previous[1][2]; + float sd10 = previous[0][1] * previous[2][2] - previous[2][1] * previous[0][2]; + float sd20 = previous[0][1] * previous[1][2] - previous[1][1] * previous[0][2]; + float det = previous[0][0] * sd00 + previous[2][0] * sd20 - previous[1][0] * sd10; + if (fabs(det) == 0.0f) { + // determinant is zero; matrix is not invertible + break; + } + float hrdet = 0.5f / det; + upper[0][0] = +sd00 * hrdet + previous[0][0] * 0.5f; + upper[1][0] = -sd10 * hrdet + previous[1][0] * 0.5f; + upper[2][0] = +sd20 * hrdet + previous[2][0] * 0.5f; + + upper[0][1] = -(previous[1][0] * previous[2][2] - previous[2][0] * previous[1][2]) * hrdet + previous[0][1] * 0.5f; + upper[1][1] = +(previous[0][0] * previous[2][2] - previous[2][0] * previous[0][2]) * hrdet + previous[1][1] * 0.5f; + upper[2][1] = -(previous[0][0] * previous[1][2] - previous[1][0] * previous[0][2]) * hrdet + previous[2][1] * 0.5f; + + upper[0][2] = +(previous[1][0] * previous[2][1] - previous[2][0] * previous[1][1]) * hrdet + previous[0][2] * 0.5f; + upper[1][2] = -(previous[0][0] * previous[2][1] - previous[2][0] * previous[0][1]) * hrdet + previous[1][2] * 0.5f; + upper[2][2] = +(previous[0][0] * previous[1][1] - previous[1][0] * previous[0][1]) * hrdet + previous[2][2] * 0.5f; + + // compute the difference; if it's small enough, we're done + glm::mat3 diff = upper - previous; + if (diff[0][0] * diff[0][0] + diff[1][0] * diff[1][0] + diff[2][0] * diff[2][0] + diff[0][1] * diff[0][1] + + diff[1][1] * diff[1][1] + diff[2][1] * diff[2][1] + diff[0][2] * diff[0][2] + diff[1][2] * diff[1][2] + + diff[2][2] * diff[2][2] < EPSILON) { + break; + } + } + } + + // now that we have a nice orthogonal matrix, we can extract the rotation quaternion + // using the method described in http://en.wikipedia.org/wiki/Rotation_matrix#Conversions + float x2 = fabs(1.0f + upper[0][0] - upper[1][1] - upper[2][2]); + float y2 = fabs(1.0f - upper[0][0] + upper[1][1] - upper[2][2]); + float z2 = fabs(1.0f - upper[0][0] - upper[1][1] + upper[2][2]); + float w2 = fabs(1.0f + upper[0][0] + upper[1][1] + upper[2][2]); + return glm::normalize(glm::quat(0.5f * sqrtf(w2), + 0.5f * sqrtf(x2) * (upper[1][2] >= upper[2][1] ? 1.0f : -1.0f), + 0.5f * sqrtf(y2) * (upper[2][0] >= upper[0][2] ? 1.0f : -1.0f), + 0.5f * sqrtf(z2) * (upper[0][1] >= upper[1][0] ? 1.0f : -1.0f))); +} + // Draw a 3D vector floating in space void drawVector(glm::vec3 * vector) { glDisable(GL_LIGHTING); diff --git a/interface/src/Util.h b/interface/src/Util.h index 3039ab1a31..f304535d70 100644 --- a/interface/src/Util.h +++ b/interface/src/Util.h @@ -55,6 +55,10 @@ glm::vec3 safeEulerAngles(const glm::quat& q); glm::quat safeMix(const glm::quat& q1, const glm::quat& q2, float alpha); +glm::vec3 extractTranslation(const glm::mat4& matrix); + +glm::quat extractRotation(const glm::mat4& matrix, bool assumeOrthogonal = false); + double diffclock(timeval *clock1,timeval *clock2); void renderGroundPlaneGrid(float size, float impact); diff --git a/interface/src/avatar/Avatar.cpp b/interface/src/avatar/Avatar.cpp index c7acfd149a..272604e58c 100755 --- a/interface/src/avatar/Avatar.cpp +++ b/interface/src/avatar/Avatar.cpp @@ -412,6 +412,7 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) { } } + _skeletonModel.simulate(deltaTime); _head.setBodyRotation(glm::vec3(_bodyPitch, _bodyYaw, _bodyRoll)); glm::vec3 headPosition; if (!_skeletonModel.getHeadPosition(headPosition)) { @@ -420,7 +421,6 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) { _head.setPosition(headPosition); _head.setSkinColor(glm::vec3(SKIN_COLOR[0], SKIN_COLOR[1], SKIN_COLOR[2])); _head.simulate(deltaTime, false); - _skeletonModel.simulate(deltaTime); _hand.simulate(deltaTime, false); // use speed and angular velocity to determine walking vs. standing diff --git a/interface/src/avatar/FaceModel.cpp b/interface/src/avatar/FaceModel.cpp index 494bc5b59f..eb2ca70805 100644 --- a/interface/src/avatar/FaceModel.cpp +++ b/interface/src/avatar/FaceModel.cpp @@ -30,7 +30,7 @@ void FaceModel::simulate(float deltaTime) { } setTranslation(neckPosition); glm::quat neckRotation; - if (true || !owningAvatar->getSkeletonModel().getNeckRotation(neckRotation)) { + if (!owningAvatar->getSkeletonModel().getNeckRotation(neckRotation)) { neckRotation = owningAvatar->getSkeleton().joint[AVATAR_JOINT_NECK_BASE].absoluteRotation; } setRotation(neckRotation); @@ -50,18 +50,21 @@ void FaceModel::updateJointState(int index) { const FBXGeometry& geometry = _geometry->getFBXGeometry(); const FBXJoint& joint = geometry.joints.at(index); + glm::quat combinedRotation = joint.preRotation * state.rotation * joint.postRotation; if (joint.parentIndex == -1) { glm::mat4 baseTransform = glm::translate(_translation) * glm::mat4_cast(_rotation) * glm::scale(_scale) * glm::translate(_offset); - state.transform = baseTransform * geometry.offset * joint.preRotation * - glm::mat4_cast(state.rotation) * joint.postRotation; + + state.transform = baseTransform * geometry.offset * joint.preTransform * + glm::mat4_cast(combinedRotation) * joint.postTransform; + state.combinedRotation = _rotation * combinedRotation; } else { if (index == geometry.neckJointIndex) { // get the rotation axes in joint space and use them to adjust the rotation glm::mat3 axes = glm::mat3_cast(getRotation()); glm::mat3 inverse = glm::inverse(glm::mat3(_jointStates[joint.parentIndex].transform * - joint.preRotation * glm::mat4_cast(joint.rotation))); + joint.preTransform * glm::mat4_cast(joint.preRotation * joint.rotation))); state.rotation = glm::angleAxis(_owningHead->getRoll(), glm::normalize(inverse * axes[2])) * glm::angleAxis(_owningHead->getYaw(), glm::normalize(inverse * axes[1])) * glm::angleAxis(_owningHead->getPitch(), glm::normalize(inverse * axes[0])) * joint.rotation; @@ -69,13 +72,15 @@ void FaceModel::updateJointState(int index) { } else if (index == geometry.leftEyeJointIndex || index == geometry.rightEyeJointIndex) { // likewise with the eye joints glm::mat4 inverse = glm::inverse(_jointStates[joint.parentIndex].transform * - joint.preRotation * glm::mat4_cast(joint.rotation)); + 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(), 1.0f)); state.rotation = rotationBetween(front, lookAt) * joint.rotation; - } - state.transform = _jointStates[joint.parentIndex].transform * joint.preRotation * - glm::mat4_cast(state.rotation) * joint.postRotation; + } + const JointState& parentState = _jointStates.at(joint.parentIndex); + state.transform = parentState.transform * joint.preTransform * + glm::mat4_cast(combinedRotation) * joint.postTransform; + state.combinedRotation = parentState.combinedRotation * combinedRotation; } } diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index b7ec4ce4dc..b32af7dba6 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -317,6 +317,7 @@ void MyAvatar::simulate(float deltaTime, Transmitter* transmitter) { } } + _skeletonModel.simulate(deltaTime); _head.setBodyRotation(glm::vec3(_bodyPitch, _bodyYaw, _bodyRoll)); glm::vec3 headPosition; if (!_skeletonModel.getHeadPosition(headPosition)) { @@ -326,7 +327,6 @@ void MyAvatar::simulate(float deltaTime, Transmitter* transmitter) { _head.setScale(_scale); _head.setSkinColor(glm::vec3(SKIN_COLOR[0], SKIN_COLOR[1], SKIN_COLOR[2])); _head.simulate(deltaTime, true); - _skeletonModel.simulate(deltaTime); _hand.simulate(deltaTime, true); const float WALKING_SPEED_THRESHOLD = 0.2f; diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 0b2b426ef0..fbffa49fc1 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -62,23 +62,28 @@ void SkeletonModel::updateJointState(int index) { const FBXGeometry& geometry = _geometry->getFBXGeometry(); const FBXJoint& joint = geometry.joints.at(index); + glm::quat combinedRotation = joint.preRotation * state.rotation * joint.postRotation; if (joint.parentIndex == -1) { glm::mat4 baseTransform = glm::translate(_translation) * glm::mat4_cast(_rotation) * glm::scale(_scale) * glm::translate(_offset); - state.transform = baseTransform * geometry.offset * joint.preRotation * - glm::mat4_cast(state.rotation) * joint.postRotation; + + state.transform = baseTransform * geometry.offset * joint.preTransform * + glm::mat4_cast(combinedRotation) * joint.postTransform; + state.combinedRotation = _rotation * combinedRotation; } else { if (index == geometry.leanJointIndex) { // get the rotation axes in joint space and use them to adjust the rotation glm::mat3 axes = glm::mat3_cast(_rotation); glm::mat3 inverse = glm::inverse(glm::mat3(_jointStates[joint.parentIndex].transform * - joint.preRotation * glm::mat4_cast(joint.rotation))); + joint.preTransform * glm::mat4_cast(joint.preRotation * joint.rotation))); state.rotation = glm::angleAxis(_owningAvatar->getHead().getLeanSideways(), glm::normalize(inverse * axes[2])) * glm::angleAxis(_owningAvatar->getHead().getLeanForward(), glm::normalize(inverse * axes[0])) * joint.rotation; } - state.transform = _jointStates[joint.parentIndex].transform * joint.preRotation * - glm::mat4_cast(state.rotation) * joint.postRotation; + const JointState& parentState = _jointStates.at(joint.parentIndex); + state.transform = parentState.transform * joint.preTransform * + glm::mat4_cast(combinedRotation) * joint.postTransform; + state.combinedRotation = parentState.combinedRotation * combinedRotation; } if (index == geometry.rootJointIndex) { diff --git a/interface/src/renderer/FBXReader.cpp b/interface/src/renderer/FBXReader.cpp index 3a2c2607a0..143fb2390e 100644 --- a/interface/src/renderer/FBXReader.cpp +++ b/interface/src/renderer/FBXReader.cpp @@ -17,6 +17,7 @@ #include #include "FBXReader.h" +#include "Util.h" using namespace std; @@ -453,9 +454,11 @@ public: int parentIndex; - glm::mat4 preRotation; + glm::mat4 preTransform; + glm::quat preRotation; glm::quat rotation; - glm::mat4 postRotation; + glm::quat postRotation; + glm::mat4 postTransform; }; glm::mat4 getGlobalTransform(const QMultiHash& parentMap, @@ -463,7 +466,8 @@ glm::mat4 getGlobalTransform(const QMultiHash& parentMap, glm::mat4 globalTransform; while (!nodeID.isNull()) { const FBXModel& model = models.value(nodeID); - globalTransform = model.preRotation * glm::mat4_cast(model.rotation) * model.postRotation * globalTransform; + globalTransform = model.preTransform * glm::mat4_cast(model.preRotation * model.rotation * model.postRotation) * + model.postTransform * globalTransform; QList parentIDs = parentMap.values(nodeID); nodeID = QString(); @@ -798,11 +802,12 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) } } // see FBX documentation, http://download.autodesk.com/us/fbx/20112/FBX_SDK_HELP/index.html - model.preRotation = glm::translate(translation) * glm::translate(rotationOffset) * - glm::translate(rotationPivot) * glm::mat4_cast(glm::quat(glm::radians(preRotation))); + model.preTransform = glm::translate(translation) * glm::translate(rotationOffset) * + glm::translate(rotationPivot); + model.preRotation = glm::quat(glm::radians(preRotation)); model.rotation = glm::quat(glm::radians(rotation)); - model.postRotation = glm::mat4_cast(glm::quat(glm::radians(postRotation))) * - glm::translate(-rotationPivot) * glm::translate(scalePivot) * + model.postRotation = glm::quat(glm::radians(postRotation)); + model.postTransform = glm::translate(-rotationPivot) * glm::translate(scalePivot) * glm::scale(scale) * glm::translate(-scalePivot); models.insert(object.properties.at(0).toString(), model); @@ -921,10 +926,10 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) // get offset transform from mapping FBXGeometry geometry; float offsetScale = mapping.value("scale", 1.0f).toFloat(); + glm::quat offsetRotation = glm::quat(glm::radians(glm::vec3(mapping.value("rx").toFloat(), + mapping.value("ry").toFloat(), mapping.value("rz").toFloat()))); geometry.offset = glm::translate(mapping.value("tx").toFloat(), mapping.value("ty").toFloat(), - mapping.value("tz").toFloat()) * glm::mat4_cast(glm::quat(glm::radians(glm::vec3(mapping.value("rx").toFloat(), - mapping.value("ry").toFloat(), mapping.value("rz").toFloat())))) * - glm::scale(offsetScale, offsetScale, offsetScale); + mapping.value("tz").toFloat()) * glm::mat4_cast(offsetRotation) * glm::scale(offsetScale, offsetScale, offsetScale); // get the list of models in depth-first traversal order QVector modelIDs; @@ -950,15 +955,21 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) const FBXModel& model = models[modelID]; FBXJoint joint; joint.parentIndex = model.parentIndex; + joint.preTransform = model.preTransform; joint.preRotation = model.preRotation; joint.rotation = model.rotation; joint.postRotation = model.postRotation; - if (joint.parentIndex == -1) { - joint.transform = geometry.offset * model.preRotation * glm::mat4_cast(model.rotation) * model.postRotation; + joint.postTransform = model.postTransform; + glm::quat combinedRotation = model.preRotation * model.rotation * model.postRotation; + if (joint.parentIndex == -1) { + joint.transform = geometry.offset * model.preTransform * glm::mat4_cast(combinedRotation) * model.postTransform; + joint.inverseBindRotation = glm::inverse(offsetRotation * combinedRotation); } else { - joint.transform = geometry.joints.at(joint.parentIndex).transform * - model.preRotation * glm::mat4_cast(model.rotation) * model.postRotation; + const FBXJoint& parentJoint = geometry.joints.at(joint.parentIndex); + joint.transform = parentJoint.transform * + model.preTransform * glm::mat4_cast(combinedRotation) * model.postTransform; + joint.inverseBindRotation = glm::inverse(combinedRotation) * parentJoint.inverseBindRotation; } geometry.joints.append(joint); geometry.jointIndices.insert(model.name, geometry.joints.size() - 1); diff --git a/interface/src/renderer/FBXReader.h b/interface/src/renderer/FBXReader.h index 9f96a67634..17f743ec64 100644 --- a/interface/src/renderer/FBXReader.h +++ b/interface/src/renderer/FBXReader.h @@ -43,10 +43,13 @@ class FBXJoint { public: int parentIndex; - glm::mat4 preRotation; + glm::mat4 preTransform; + glm::quat preRotation; glm::quat rotation; - glm::mat4 postRotation; + glm::quat postRotation; + glm::mat4 postTransform; glm::mat4 transform; + glm::quat inverseBindRotation; }; /// A single binding to a joint in an FBX document. diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp index c3aa074f05..4f889da997 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -387,15 +387,20 @@ void Model::updateJointState(int index) { const FBXGeometry& geometry = _geometry->getFBXGeometry(); const FBXJoint& joint = geometry.joints.at(index); + glm::quat combinedRotation = joint.preRotation * state.rotation * joint.postRotation; if (joint.parentIndex == -1) { glm::mat4 baseTransform = glm::translate(_translation) * glm::mat4_cast(_rotation) * glm::scale(_scale) * glm::translate(_offset); - state.transform = baseTransform * geometry.offset * joint.preRotation * - glm::mat4_cast(state.rotation) * joint.postRotation; + + state.transform = baseTransform * geometry.offset * joint.preTransform * + glm::mat4_cast(combinedRotation) * joint.postTransform; + state.combinedRotation = _rotation * combinedRotation; } else { - state.transform = _jointStates[joint.parentIndex].transform * joint.preRotation * - glm::mat4_cast(state.rotation) * joint.postRotation; + const JointState& parentState = _jointStates.at(joint.parentIndex); + state.transform = parentState.transform * joint.preTransform * + glm::mat4_cast(combinedRotation) * joint.postTransform; + state.combinedRotation = parentState.combinedRotation * combinedRotation; } } @@ -403,8 +408,7 @@ bool Model::getJointPosition(int jointIndex, glm::vec3& position) const { if (jointIndex == -1 || _jointStates.isEmpty()) { return false; } - const glm::mat4& transform = _jointStates[jointIndex].transform; - position = glm::vec3(transform[3][0], transform[3][1], transform[3][2]); + position = extractTranslation(_jointStates[jointIndex].transform); return true; } @@ -412,8 +416,8 @@ bool Model::getJointRotation(int jointIndex, glm::quat& rotation) const { if (jointIndex == -1 || _jointStates.isEmpty()) { return false; } - const glm::mat4& transform = _jointStates[jointIndex].transform; - rotation = glm::normalize(glm::quat_cast(transform)); + rotation = _jointStates[jointIndex].combinedRotation * + _geometry->getFBXGeometry().joints[jointIndex].inverseBindRotation; return true; } diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h index 4d848057d8..894de7bfe1 100644 --- a/interface/src/renderer/Model.h +++ b/interface/src/renderer/Model.h @@ -86,6 +86,7 @@ protected: public: glm::quat rotation; glm::mat4 transform; + glm::quat combinedRotation; }; QVector _jointStates;