Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-04-21 20:44:14 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions 2

Merge pull request #3109 from AndrewMeadows/ragdoll

Browse source 
stable arm IK without constraints
This commit is contained in:
Philip Rosedale 2014-07-02 15:26:10 -07:00
commit ec1f909210
7 changed files with 309 additions and 47 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
interface/src/avatar/FaceModel.cpp
View file

@ -51,10 +51,10 @@ void FaceModel::maybeUpdateNeckRotation(const JointState& parentState, const FBX
glm::mat3 axes = glm::mat3_cast(glm::quat());
glm::mat3 inverse = glm::mat3(glm::inverse(parentState.getTransform() * glm::translate(state.getDefaultTranslationInParentFrame()) *
joint.preTransform * glm::mat4_cast(joint.preRotation)));
state._rotationInParentFrame = glm::angleAxis(- RADIANS_PER_DEGREE * _owningHead->getFinalRoll(), glm::normalize(inverse * axes[2]))
state.setRotationInParentFrame(glm::angleAxis(- RADIANS_PER_DEGREE * _owningHead->getFinalRoll(), glm::normalize(inverse * axes[2]))
* glm::angleAxis(RADIANS_PER_DEGREE * _owningHead->getFinalYaw(), glm::normalize(inverse * axes[1]))
* glm::angleAxis(- RADIANS_PER_DEGREE * _owningHead->getFinalPitch(), glm::normalize(inverse * axes[0]))
* joint.rotation;
* joint.rotation);
}
void FaceModel::maybeUpdateEyeRotation(const JointState& parentState, const FBXJoint& joint, JointState& state) {
@ -68,8 +68,8 @@ void FaceModel::maybeUpdateEyeRotation(const JointState& parentState, const FBXJ
_owningHead->getSaccade() - _translation, 1.0f));
glm::quat between = rotationBetween(front, lookAt);
const float MAX_ANGLE = 30.0f * RADIANS_PER_DEGREE;
state._rotationInParentFrame = glm::angleAxis(glm::clamp(glm::angle(between), -MAX_ANGLE, MAX_ANGLE), glm::axis(between)) *
joint.rotation;
state.setRotationInParentFrame(glm::angleAxis(glm::clamp(glm::angle(between), -MAX_ANGLE, MAX_ANGLE), glm::axis(between)) *
joint.rotation);
}
void FaceModel::updateJointState(int index) {

17
interface/src/avatar/SkeletonModel.cpp
View file

@ -219,10 +219,9 @@ void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) {
JointState& parentState = _jointStates[parentJointIndex];
parentState.setRotationFromBindFrame(palmRotation, PALM_PRIORITY);
// lock hand to forearm by slamming its rotation (in parent-frame) to identity
_jointStates[jointIndex]._rotationInParentFrame = glm::quat();
_jointStates[jointIndex].setRotationInParentFrame(glm::quat());
} else {
setJointPosition(jointIndex, palmPosition, palmRotation,
true, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY);
inverseKinematics(jointIndex, palmPosition, palmRotation, PALM_PRIORITY);
}
}
@ -258,9 +257,9 @@ void SkeletonModel::maybeUpdateLeanRotation(const JointState& parentState, const
glm::mat3 axes = glm::mat3_cast(glm::quat());
glm::mat3 inverse = glm::mat3(glm::inverse(parentState.getTransform() * glm::translate(state.getDefaultTranslationInParentFrame()) *
joint.preTransform * glm::mat4_cast(joint.preRotation * joint.rotation)));
state._rotationInParentFrame = glm::angleAxis(- RADIANS_PER_DEGREE * _owningAvatar->getHead()->getFinalLeanSideways(),
state.setRotationInParentFrame(glm::angleAxis(- RADIANS_PER_DEGREE * _owningAvatar->getHead()->getFinalLeanSideways(),
glm::normalize(inverse * axes[2])) * glm::angleAxis(- RADIANS_PER_DEGREE * _owningAvatar->getHead()->getFinalLeanForward(),
glm::normalize(inverse * axes[0])) * joint.rotation;
glm::normalize(inverse * axes[0])) * joint.rotation);
}
void SkeletonModel::maybeUpdateNeckRotation(const JointState& parentState, const FBXJoint& joint, JointState& state) {
@ -537,6 +536,11 @@ void SkeletonModel::buildRagdollConstraints() {
}
}
void SkeletonModel::updateVisibleJointStates() {
Model::updateVisibleJointStates();
// TODO: implement this to move visible joints to agree with joint shape positions
}
// virtual
void SkeletonModel::stepRagdollForward(float deltaTime) {
const float RAGDOLL_FOLLOWS_JOINTS_TIMESCALE = 0.03f;
@ -644,7 +648,6 @@ void SkeletonModel::computeBoundingShape(const FBXGeometry& geometry) {
_ragdollPoints[i]._lastPosition = _ragdollPoints[i]._position;
continue;
}
assert(parentIndex != -1);
glm::quat modifiedRotation = joint.preRotation * joint.rotation * joint.postRotation;
transforms[i] = transforms[parentIndex] * glm::translate(joint.translation)
@ -700,7 +703,7 @@ void SkeletonModel::computeBoundingShape(const FBXGeometry& geometry) {
_boundingRadius = 0.5f * glm::length(diagonal);
}
void SkeletonModel::resetShapePositions() {
void SkeletonModel::resetShapePositionsToDefaultPose() {
// DEBUG method.
// Moves shapes to the joint default locations for debug visibility into
// how the bounding shape is computed.

4
interface/src/avatar/SkeletonModel.h
View file

@ -93,6 +93,8 @@ public:
/// Retrieve the positions of up to two eye meshes.
/// \return whether or not both eye meshes were found
bool getEyePositions(glm::vec3& firstEyePosition, glm::vec3& secondEyePosition) const;
virtual void updateVisibleJointStates();
// virtual overrride from Ragdoll
virtual void stepRagdollForward(float deltaTime);
@ -104,7 +106,7 @@ public:
float getBoundingShapeRadius() const { return _boundingShape.getRadius(); }
const CapsuleShape& getBoundingShape() const { return _boundingShape; }
void resetShapePositions(); // DEBUG method
void resetShapePositionsToDefaultPose(); // DEBUG method
void renderRagdoll();
protected:

92
interface/src/renderer/JointState.cpp
View file

@ -11,14 +11,26 @@
#include <glm/gtx/norm.hpp>
//#include <GeometryUtil.h>
#include <SharedUtil.h>
#include "JointState.h"
JointState::JointState() :
_animationPriority(0.0f),
_fbxJoint(NULL) {
_fbxJoint(NULL),
_isConstrained(false) {
}
void JointState::copyState(const JointState& state) {
_animationPriority = state._animationPriority;
_transform = state._transform;
_rotation = extractRotation(_transform);
_rotationInParentFrame = state._rotationInParentFrame;
_visibleTransform = state._visibleTransform;
_visibleRotation = extractRotation(_visibleTransform);
_visibleRotationInParentFrame = state._visibleRotationInParentFrame;
// DO NOT copy _fbxJoint
}
void JointState::setFBXJoint(const FBXJoint* joint) {
@ -26,14 +38,10 @@ void JointState::setFBXJoint(const FBXJoint* joint) {
_rotationInParentFrame = joint->rotation;
// NOTE: JointState does not own the FBXJoint to which it points.
_fbxJoint = joint;
}
void JointState::copyState(const JointState& state) {
_rotationInParentFrame = state._rotationInParentFrame;
_transform = state._transform;
_rotation = extractRotation(_transform);
_animationPriority = state._animationPriority;
// DO NOT copy _fbxJoint
// precompute whether there are any constraints or not
float distanceMin = glm::distance(_fbxJoint->rotationMin, glm::vec3(-PI));
float distanceMax = glm::distance(_fbxJoint->rotationMax, glm::vec3(PI));
_isConstrained = distanceMin > EPSILON || distanceMax > EPSILON;
}
void JointState::computeTransform(const glm::mat4& parentTransform) {
@ -43,6 +51,13 @@ void JointState::computeTransform(const glm::mat4& parentTransform) {
_rotation = extractRotation(_transform);
}
void JointState::computeVisibleTransform(const glm::mat4& parentTransform) {
glm::quat modifiedRotation = _fbxJoint->preRotation * _visibleRotationInParentFrame * _fbxJoint->postRotation;
glm::mat4 modifiedTransform = _fbxJoint->preTransform * glm::mat4_cast(modifiedRotation) * _fbxJoint->postTransform;
_visibleTransform = parentTransform * glm::translate(_fbxJoint->translation) * modifiedTransform;
_visibleRotation = extractRotation(_visibleTransform);
}
glm::quat JointState::getRotationFromBindToModelFrame() const {
return _rotation * _fbxJoint->inverseBindRotation;
}
@ -50,16 +65,16 @@ glm::quat JointState::getRotationFromBindToModelFrame() const {
void JointState::restoreRotation(float fraction, float priority) {
assert(_fbxJoint != NULL);
if (priority == _animationPriority || _animationPriority == 0.0f) {
_rotationInParentFrame = safeMix(_rotationInParentFrame, _fbxJoint->rotation, fraction);
setRotationInParentFrame(safeMix(_rotationInParentFrame, _fbxJoint->rotation, fraction));
_animationPriority = 0.0f;
}
}
void JointState::setRotationFromBindFrame(const glm::quat& rotation, float priority) {
// rotation is from bind- to model-frame
assert(_fbxJoint != NULL);
if (priority >= _animationPriority) {
// rotation is from bind- to model-frame
_rotationInParentFrame = _rotationInParentFrame * glm::inverse(_rotation) * rotation * glm::inverse(_fbxJoint->inverseBindRotation);
setRotationInParentFrame(_rotationInParentFrame * glm::inverse(_rotation) * rotation * glm::inverse(_fbxJoint->inverseBindRotation));
_animationPriority = priority;
}
}
@ -68,6 +83,9 @@ void JointState::clearTransformTranslation() {
_transform[3][0] = 0.0f;
_transform[3][1] = 0.0f;
_transform[3][2] = 0.0f;
_visibleTransform[3][0] = 0.0f;
_visibleTransform[3][1] = 0.0f;
_visibleTransform[3][2] = 0.0f;
}
void JointState::setRotation(const glm::quat& rotation, bool constrain, float priority) {
@ -75,27 +93,59 @@ void JointState::setRotation(const glm::quat& rotation, bool constrain, float pr
}
void JointState::applyRotationDelta(const glm::quat& delta, bool constrain, float priority) {
// NOTE: delta is in jointParent-frame
// NOTE: delta is in model-frame
assert(_fbxJoint != NULL);
if (priority < _animationPriority) {
return;
}
_animationPriority = priority;
if (!constrain || (_fbxJoint->rotationMin == glm::vec3(-PI, -PI, -PI) &&
_fbxJoint->rotationMax == glm::vec3(PI, PI, PI))) {
if (!constrain || !_isConstrained) {
// no constraints
_rotationInParentFrame = _rotationInParentFrame * glm::inverse(_rotation) * delta * _rotation;
_rotation = delta * _rotation;
return;
}
glm::quat targetRotation = delta * _rotation;
glm::vec3 eulers = safeEulerAngles(_rotationInParentFrame * glm::inverse(_rotation) * targetRotation);
glm::quat newRotation = glm::quat(glm::clamp(eulers, _fbxJoint->rotationMin, _fbxJoint->rotationMax));
_rotation = _rotation * glm::inverse(_rotationInParentFrame) * newRotation;
_rotationInParentFrame = newRotation;
glm::quat targetRotation = _rotationInParentFrame * glm::inverse(_rotation) * delta * _rotation;
setRotationInParentFrame(targetRotation);
}
/// Applies delta rotation to joint but mixes a little bit of the default pose as well.
/// This helps keep an IK solution stable.
void JointState::mixRotationDelta(const glm::quat& delta, float mixFactor, float priority) {
// NOTE: delta is in model-frame
assert(_fbxJoint != NULL);
if (priority < _animationPriority) {
return;
}
_animationPriority = priority;
glm::quat targetRotation = _rotationInParentFrame * glm::inverse(_rotation) * delta * _rotation;
if (mixFactor > 0.0f && mixFactor <= 1.0f) {
targetRotation = safeMix(targetRotation, _fbxJoint->rotation, mixFactor);
}
setRotationInParentFrame(targetRotation);
}
glm::quat JointState::computeParentRotation() const {
// R = Rp * Rpre * r * Rpost
// Rp = R * (Rpre * r * Rpost)^
return _rotation * glm::inverse(_fbxJoint->preRotation * _rotationInParentFrame * _fbxJoint->postRotation);
}
void JointState::setRotationInParentFrame(const glm::quat& targetRotation) {
glm::quat parentRotation = computeParentRotation();
_rotationInParentFrame = targetRotation;
// R' = Rp * Rpre * r' * Rpost
_rotation = parentRotation * _fbxJoint->preRotation * _rotationInParentFrame * _fbxJoint->postRotation;
}
const glm::vec3& JointState::getDefaultTranslationInParentFrame() const {
assert(_fbxJoint != NULL);
return _fbxJoint->translation;
}
void JointState::slaveVisibleTransform() {
_visibleTransform = _transform;
_visibleRotation = _rotation;
_visibleRotationInParentFrame = _rotationInParentFrame;
}

43
interface/src/renderer/JointState.h
View file

@ -22,12 +22,19 @@ class JointState {
public:
JointState();
void copyState(const JointState& state);
void setFBXJoint(const FBXJoint* joint);
const FBXJoint& getFBXJoint() const { return *_fbxJoint; }
void copyState(const JointState& state);
void computeTransform(const glm::mat4& parentTransform);
void computeVisibleTransform(const glm::mat4& parentTransform);
const glm::mat4& getVisibleTransform() const { return _visibleTransform; }
glm::quat getVisibleRotation() const { return _visibleRotation; }
glm::vec3 getVisiblePosition() const { return extractTranslation(_visibleTransform); }
const glm::mat4& getTransform() const { return _transform; }
glm::quat getRotation() const { return _rotation; }
@ -39,11 +46,19 @@ public:
/// \param rotation rotation of joint in model-frame
void setRotation(const glm::quat& rotation, bool constrain, float priority);
/// \param delta is in the jointParent-frame
/// \param delta is in the model-frame
void applyRotationDelta(const glm::quat& delta, bool constrain = true, float priority = 1.0f);
const glm::vec3& getDefaultTranslationInParentFrame() const;
/// Applies delta rotation to joint but mixes a little bit of the default pose as well.
/// This helps keep an IK solution stable.
/// \param delta rotation change in model-frame
/// \param mixFactor fraction in range [0,1] of how much default pose to blend in (0 is none, 1 is all)
/// \param priority priority level of this animation blend
void mixRotationDelta(const glm::quat& delta, float mixFactor, float priority = 1.0f);
/// Blends a fraciton of default pose into joint rotation.
/// \param fraction fraction in range [0,1] of how much default pose to blend in (0 is none, 1 is all)
/// \param priority priority level of this animation blend
void restoreRotation(float fraction, float priority);
/// \param rotation is from bind- to model-frame
@ -51,16 +66,36 @@ public:
/// NOTE: the JointState's model-frame transform/rotation are NOT updated!
void setRotationFromBindFrame(const glm::quat& rotation, float priority);
void setRotationInParentFrame(const glm::quat& targetRotation);
const glm::quat& getRotationInParentFrame() const { return _rotationInParentFrame; }
const glm::vec3& getDefaultTranslationInParentFrame() const;
void clearTransformTranslation();
glm::quat _rotationInParentFrame; // joint- to parentJoint-frame
void slaveVisibleTransform();
float _animationPriority; // the priority of the animation affecting this joint
private:
/// \return parent model-frame rotation
// (used to keep _rotation consistent when modifying _rotationInWorldFrame directly)
glm::quat computeParentRotation() const;
/// debug helper function
void loadBindRotation();
glm::mat4 _transform; // joint- to model-frame
glm::quat _rotation; // joint- to model-frame
glm::quat _rotationInParentFrame; // joint- to parentJoint-frame
glm::mat4 _visibleTransform;
glm::quat _visibleRotation;
glm::quat _visibleRotationInParentFrame;
const FBXJoint* _fbxJoint; // JointState does NOT own its FBXJoint
bool _isConstrained;
};
#endif // hifi_JointState_h

184
interface/src/renderer/Model.cpp
View file

@ -39,8 +39,8 @@ Model::Model(QObject* parent) :
_scaledToFit(false),
_snapModelToCenter(false),
_snappedToCenter(false),
_showTrueJointTransforms(false),
_rootIndex(-1),
//_enableCollisionShapes(false),
_lodDistance(0.0f),
_pupilDilation(0.0f),
_url("http://invalid.com") {
@ -460,7 +460,7 @@ void Model::reset() {
}
const FBXGeometry& geometry = _geometry->getFBXGeometry();
for (int i = 0; i < _jointStates.size(); i++) {
_jointStates[i]._rotationInParentFrame = geometry.joints.at(i).rotation;
_jointStates[i].setRotationInParentFrame(geometry.joints.at(i).rotation);
}
}
@ -571,6 +571,9 @@ void Model::setJointStates(QVector<JointState> states) {
radius = distance;
}
}
for (int i = 0; i < _jointStates.size(); i++) {
_jointStates[i].slaveVisibleTransform();
}
_boundingRadius = radius;
}
@ -686,7 +689,7 @@ bool Model::getJointState(int index, glm::quat& rotation) const {
if (index == -1 || index >= _jointStates.size()) {
return false;
}
rotation = _jointStates.at(index)._rotationInParentFrame;
rotation = _jointStates.at(index).getRotationInParentFrame();
const glm::quat& defaultRotation = _geometry->getFBXGeometry().joints.at(index).rotation;
return glm::abs(rotation.x - defaultRotation.x) >= EPSILON ||
glm::abs(rotation.y - defaultRotation.y) >= EPSILON ||
@ -699,7 +702,7 @@ void Model::setJointState(int index, bool valid, const glm::quat& rotation, floa
JointState& state = _jointStates[index];
if (priority >= state._animationPriority) {
if (valid) {
state._rotationInParentFrame = rotation;
state.setRotationInParentFrame(rotation);
state._animationPriority = priority;
} else {
state.restoreRotation(1.0f, priority);
@ -765,6 +768,23 @@ bool Model::getJointCombinedRotation(int jointIndex, glm::quat& rotation) const
return true;
}
bool Model::getVisibleJointPositionInWorldFrame(int jointIndex, glm::vec3& position) const {
if (jointIndex == -1 || jointIndex >= _jointStates.size()) {
return false;
}
// position is in world-frame
position = _translation + _rotation * _jointStates[jointIndex].getVisiblePosition();
return true;
}
bool Model::getVisibleJointRotationInWorldFrame(int jointIndex, glm::quat& rotation) const {
if (jointIndex == -1 || jointIndex >= _jointStates.size()) {
return false;
}
rotation = _rotation * _jointStates[jointIndex].getVisibleRotation();
return true;
}
QStringList Model::getJointNames() const {
if (QThread::currentThread() != thread()) {
QStringList result;
@ -918,6 +938,8 @@ void Model::simulateInternal(float deltaTime) {
for (int i = 0; i < _jointStates.size(); i++) {
updateJointState(i);
}
updateVisibleJointStates();
_shapesAreDirty = ! _shapes.isEmpty();
// update the attachment transforms and simulate them
@ -928,8 +950,13 @@ void Model::simulateInternal(float deltaTime) {
glm::vec3 jointTranslation = _translation;
glm::quat jointRotation = _rotation;
getJointPositionInWorldFrame(attachment.jointIndex, jointTranslation);
getJointRotationInWorldFrame(attachment.jointIndex, jointRotation);
if (_showTrueJointTransforms) {
getJointPositionInWorldFrame(attachment.jointIndex, jointTranslation);
getJointRotationInWorldFrame(attachment.jointIndex, jointRotation);
} else {
getVisibleJointPositionInWorldFrame(attachment.jointIndex, jointTranslation);
getVisibleJointRotationInWorldFrame(attachment.jointIndex, jointRotation);
}
model->setTranslation(jointTranslation + jointRotation * attachment.translation * _scale);
model->setRotation(jointRotation * attachment.rotation);
@ -944,9 +971,16 @@ void Model::simulateInternal(float deltaTime) {
for (int i = 0; i < _meshStates.size(); i++) {
MeshState& state = _meshStates[i];
const FBXMesh& mesh = geometry.meshes.at(i);
for (int j = 0; j < mesh.clusters.size(); j++) {
const FBXCluster& cluster = mesh.clusters.at(j);
state.clusterMatrices[j] = modelToWorld * _jointStates[cluster.jointIndex].getTransform() * cluster.inverseBindMatrix;
if (_showTrueJointTransforms) {
for (int j = 0; j < mesh.clusters.size(); j++) {
const FBXCluster& cluster = mesh.clusters.at(j);
state.clusterMatrices[j] = modelToWorld * _jointStates[cluster.jointIndex].getTransform() * cluster.inverseBindMatrix;
}
} else {
for (int j = 0; j < mesh.clusters.size(); j++) {
const FBXCluster& cluster = mesh.clusters.at(j);
state.clusterMatrices[j] = modelToWorld * _jointStates[cluster.jointIndex].getVisibleTransform() * cluster.inverseBindMatrix;
}
}
}
@ -972,6 +1006,14 @@ void Model::updateJointState(int index) {
}
}
void Model::updateVisibleJointStates() {
if (!_showTrueJointTransforms) {
for (int i = 0; i < _jointStates.size(); i++) {
_jointStates[i].slaveVisibleTransform();
}
}
}
bool Model::setJointPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation, bool useRotation,
int lastFreeIndex, bool allIntermediatesFree, const glm::vec3& alignment, float priority) {
if (jointIndex == -1 || _jointStates.isEmpty()) {
@ -1058,6 +1100,128 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& position, const gl
return true;
}
void Model::inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm::quat& targetRotation, float priority) {
// NOTE: targetRotation is from bind- to model-frame
if (endIndex == -1 || _jointStates.isEmpty()) {
return;
}
const FBXGeometry& geometry = _geometry->getFBXGeometry();
const QVector<int>& freeLineage = geometry.joints.at(endIndex).freeLineage;
if (freeLineage.isEmpty()) {
return;
}
int numFree = freeLineage.size();
// store and remember topmost parent transform
glm::mat4 topParentTransform;
{
int index = freeLineage.last();
const JointState& state = _jointStates.at(index);
const FBXJoint& joint = state.getFBXJoint();
int parentIndex = joint.parentIndex;
if (parentIndex == -1) {
const FBXGeometry& geometry = _geometry->getFBXGeometry();
topParentTransform = glm::scale(_scale) * glm::translate(_offset) * geometry.offset;
} else {
topParentTransform = _jointStates[parentIndex].getTransform();
}
}
// this is a cyclic coordinate descent algorithm: see
// http://www.ryanjuckett.com/programming/animation/21-cyclic-coordinate-descent-in-2d
// keep track of the position of the end-effector
JointState& endState = _jointStates[endIndex];
glm::vec3 endPosition = endState.getPosition();
float distanceToGo = glm::distance(targetPosition, endPosition);
const int MAX_ITERATION_COUNT = 2;
const float ACCEPTABLE_IK_ERROR = 0.005f; // 5mm
int numIterations = 0;
do {
++numIterations;
// moving up, rotate each free joint to get endPosition closer to target
for (int j = 1; j < numFree; j++) {
int nextIndex = freeLineage.at(j);
JointState& nextState = _jointStates[nextIndex];
FBXJoint nextJoint = nextState.getFBXJoint();
if (! nextJoint.isFree) {
continue;
}
glm::vec3 pivot = nextState.getPosition();
glm::vec3 leverArm = endPosition - pivot;
float leverLength = glm::length(leverArm);
if (leverLength < EPSILON) {
continue;
}
glm::quat deltaRotation = rotationBetween(leverArm, targetPosition - pivot);
/* DON'T REMOVE! This code provides the gravitational effect on the IK solution.
* It is commented out for the moment because we're blending the IK solution with
* the default pose which provides similar stability, but we might want to use
* gravity again later.
// We want to mix the shortest rotation with one that will pull the system down with gravity.
// So we compute a simplified center of mass, where each joint has a mass of 1.0 and we don't
// bother averaging it because we only need direction.
if (j > 1) {
glm::vec3 centerOfMass(0.0f);
for (int k = 0; k < j; ++k) {
int massIndex = freeLineage.at(k);
centerOfMass += _jointStates[massIndex].getPosition() - pivot;
}
// the gravitational effect is a rotation that tends to align the two cross products
const glm::vec3 worldAlignment = glm::vec3(0.0f, -1.f, 0.0f);
glm::quat gravityDelta = rotationBetween(glm::cross(centerOfMass, leverArm),
glm::cross(worldAlignment, leverArm));
float gravityAngle = glm::angle(gravityDelta);
const float MIN_GRAVITY_ANGLE = 0.1f;
float mixFactor = 0.5f;
if (gravityAngle < MIN_GRAVITY_ANGLE) {
// the final rotation is a mix of the two
mixFactor = 0.5f * gravityAngle / MIN_GRAVITY_ANGLE;
}
deltaRotation = safeMix(deltaRotation, gravityDelta, mixFactor);
}
*/
// Apply the rotation, but use mixRotationDelta() which blends a bit of the default pose
// at in the process. This provides stability to the IK solution and removes the necessity
// for the gravity effect.
glm::quat oldNextRotation = nextState.getRotation();
float mixFactor = 0.03f;
nextState.mixRotationDelta(deltaRotation, mixFactor, priority);
// measure the result of the rotation which may have been modified by
// blending and constraints
glm::quat actualDelta = nextState.getRotation() * glm::inverse(oldNextRotation);
endPosition = pivot + actualDelta * leverArm;
}
// recompute transforms from the top down
glm::mat4 parentTransform = topParentTransform;
for (int j = numFree - 1; j >= 0; --j) {
JointState& freeState = _jointStates[freeLineage.at(j)];
freeState.computeTransform(parentTransform);
parentTransform = freeState.getTransform();
}
// measure our success
endPosition = endState.getPosition();
distanceToGo = glm::distance(targetPosition, endPosition);
} while (numIterations < MAX_ITERATION_COUNT && distanceToGo < ACCEPTABLE_IK_ERROR);
// set final rotation of the end joint
endState.setRotationFromBindFrame(targetRotation, priority);
_shapesAreDirty = !_shapes.isEmpty();
}
bool Model::restoreJointPosition(int jointIndex, float fraction, float priority) {
if (jointIndex == -1 || _jointStates.isEmpty()) {
return false;
@ -1605,7 +1769,7 @@ void AnimationHandle::applyFrame(float frameIndex) {
if (mapping != -1) {
JointState& state = _model->_jointStates[mapping];
if (_priority >= state._animationPriority) {
state._rotationInParentFrame = safeMix(floorFrame.rotations.at(i), ceilFrame.rotations.at(i), frameFraction);
state.setRotationInParentFrame(safeMix(floorFrame.rotations.at(i), ceilFrame.rotations.at(i), frameFraction));
state._animationPriority = _priority;
}
}

8
interface/src/renderer/Model.h
View file

@ -120,6 +120,9 @@ public:
bool getJointRotationInWorldFrame(int jointIndex, glm::quat& rotation) const;
bool getJointCombinedRotation(int jointIndex, glm::quat& rotation) const;
bool getVisibleJointPositionInWorldFrame(int jointIndex, glm::vec3& position) const;
bool getVisibleJointRotationInWorldFrame(int jointIndex, glm::quat& rotation) const;
/// \param jointIndex index of joint in model structure
/// \param position[out] position of joint in model-frame
/// \return true if joint exists
@ -152,6 +155,7 @@ protected:
bool _snapModelToCenter; /// is the model's offset automatically adjusted to center around 0,0,0 in model space
bool _snappedToCenter; /// are we currently snapped to center
bool _showTrueJointTransforms;
int _rootIndex;
QVector<JointState> _jointStates;
@ -176,6 +180,8 @@ protected:
/// Updates the state of the joint at the specified index.
virtual void updateJointState(int index);
virtual void updateVisibleJointStates();
/// \param jointIndex index of joint in model structure
/// \param position position of joint in model-frame
@ -188,6 +194,8 @@ protected:
bool setJointPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation = glm::quat(),
bool useRotation = false, int lastFreeIndex = -1, bool allIntermediatesFree = false,
const glm::vec3& alignment = glm::vec3(0.0f, -1.0f, 0.0f), float priority = 1.0f);
void inverseKinematics(int jointIndex, glm::vec3 position, const glm::quat& rotation, float priority);
/// Restores the indexed joint to its default position.
/// \param fraction the fraction of the default position to apply (i.e., 0.25f to slerp one fourth of the way to