More work on extracting rotations.

This commit is contained in:
Andrzej Kapolka 2013-10-22 12:54:32 -07:00
parent 9d89baa506
commit a33c7ce20c
10 changed files with 132 additions and 39 deletions

View file

@ -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);

View file

@ -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);

View file

@ -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

View file

@ -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;
}
}

View file

@ -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;

View file

@ -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) {

View file

@ -17,6 +17,7 @@
#include <glm/gtx/transform.hpp>
#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<QString, QString>& parentMap,
@ -463,7 +466,8 @@ glm::mat4 getGlobalTransform(const QMultiHash<QString, QString>& 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<QString> 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<QString> 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);

View file

@ -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.

View file

@ -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;
}

View file

@ -86,6 +86,7 @@ protected:
public:
glm::quat rotation;
glm::mat4 transform;
glm::quat combinedRotation;
};
QVector<JointState> _jointStates;