mirror of
https://github.com/overte-org/overte.git
synced 2025-08-07 22:50:54 +02:00
Add joint manipulations in model-frame
This commit is contained in:
parent
f99489c157
commit
03bf1fe69d
4 changed files with 238 additions and 6 deletions
|
@ -73,8 +73,8 @@ void SkeletonModel::simulate(float deltaTime, bool fullUpdate) {
|
|||
restoreLeftHandPosition(HAND_RESTORATION_RATE, PALM_PRIORITY);
|
||||
|
||||
} else {
|
||||
applyPalmData(geometry.leftHandJointIndex, hand->getPalms()[leftPalmIndex]);
|
||||
applyPalmData(geometry.rightHandJointIndex, hand->getPalms()[rightPalmIndex]);
|
||||
applyPalmDataInModelFrame(geometry.leftHandJointIndex, hand->getPalms()[leftPalmIndex]);
|
||||
applyPalmDataInModelFrame(geometry.rightHandJointIndex, hand->getPalms()[rightPalmIndex]);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -194,13 +194,64 @@ void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) {
|
|||
parentState.setRotation(palmRotation, PALM_PRIORITY);
|
||||
// slam parent-relative rotation to identity
|
||||
_jointStates[jointIndex]._rotation = glm::quat();
|
||||
|
||||
} else {
|
||||
setJointPosition(jointIndex, palm.getPosition(), palmRotation,
|
||||
true, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY);
|
||||
}
|
||||
}
|
||||
|
||||
void SkeletonModel::applyPalmDataInModelFrame(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 palmRotation;
|
||||
glm::quat r0, r1;
|
||||
if (!Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK) &&
|
||||
Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {
|
||||
JointState parentState = _jointStates[parentJointIndex];
|
||||
palmRotation = parentState.getRotationFromBindToModelFrame();
|
||||
r0 = palmRotation;
|
||||
} else {
|
||||
JointState state = _jointStates[jointIndex];
|
||||
palmRotation = state.getRotationFromBindToModelFrame();
|
||||
}
|
||||
glm::quat inverseRotation = glm::inverse(_rotation);
|
||||
glm::vec3 palmNormal = inverseRotation * palm.getNormal();
|
||||
palmRotation = rotationBetween(palmRotation * geometry.palmDirection, palmNormal) * palmRotation;
|
||||
r1 = palmRotation;
|
||||
|
||||
// rotate palm to align with finger direction
|
||||
glm::vec3 direction = inverseRotation * palm.getFingerDirection();
|
||||
palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction) * palmRotation;
|
||||
|
||||
// set hand position, rotation
|
||||
glm::vec3 palmPosition = inverseRotation * (palm.getPosition() - _translation);
|
||||
if (Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK)) {
|
||||
setHandPositionInModelFrame(jointIndex, palmPosition, palmRotation);
|
||||
|
||||
} else if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {
|
||||
glm::vec3 forearmVector = palmRotation * glm::vec3(sign, 0.0f, 0.0f);
|
||||
setJointPositionInModelFrame(parentJointIndex, palmPosition + forearmVector *
|
||||
geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale),
|
||||
glm::quat(), false, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY);
|
||||
JointState& parentState = _jointStates[parentJointIndex];
|
||||
parentState.setRotationInModelFrame(palmRotation, PALM_PRIORITY);
|
||||
// slam parent-relative rotation to identity
|
||||
_jointStates[jointIndex]._rotation = glm::quat();
|
||||
} else {
|
||||
setJointPositionInModelFrame(jointIndex, palmPosition, palmRotation,
|
||||
true, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY);
|
||||
}
|
||||
}
|
||||
|
||||
void SkeletonModel::updateJointState(int index) {
|
||||
JointState& state = _jointStates[index];
|
||||
const FBXJoint& joint = state.getFBXJoint();
|
||||
|
@ -365,6 +416,70 @@ void SkeletonModel::setHandPosition(int jointIndex, const glm::vec3& position, c
|
|||
JointState& handState = _jointStates[jointIndex];
|
||||
handState.setRotation(rotation, PALM_PRIORITY);
|
||||
}
|
||||
|
||||
void SkeletonModel::setHandPositionInModelFrame(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 (!getJointPositionInModelFrame(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.setRotationInModelFrame(shoulderRotation, PALM_PRIORITY);
|
||||
|
||||
JointState& elbowState = _jointStates[elbowJointIndex];
|
||||
elbowState.setRotationInModelFrame(rotationBetween(shoulderRotation * forwardVector, wristPosition - elbowPosition) * shoulderRotation, PALM_PRIORITY);
|
||||
|
||||
JointState& handState = _jointStates[jointIndex];
|
||||
handState.setRotationInModelFrame(rotation, PALM_PRIORITY);
|
||||
}
|
||||
|
||||
bool SkeletonModel::getLeftHandPosition(glm::vec3& position) const {
|
||||
return getJointPosition(getLeftHandJointIndex(), position);
|
||||
|
|
|
@ -94,6 +94,7 @@ protected:
|
|||
void applyHandPosition(int jointIndex, const glm::vec3& position);
|
||||
|
||||
void applyPalmData(int jointIndex, PalmData& palm);
|
||||
void applyPalmDataInModelFrame(int jointIndex, PalmData& palm);
|
||||
|
||||
/// Updates the state of the joint at the specified index.
|
||||
virtual void updateJointState(int index);
|
||||
|
@ -106,6 +107,7 @@ private:
|
|||
|
||||
void renderJointConstraints(int jointIndex);
|
||||
void setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation);
|
||||
void setHandPositionInModelFrame(int jointIndex, const glm::vec3& position, const glm::quat& rotation);
|
||||
|
||||
Avatar* _owningAvatar;
|
||||
};
|
||||
|
|
|
@ -756,6 +756,14 @@ bool Model::getJointPosition(int jointIndex, glm::vec3& position) const {
|
|||
return true;
|
||||
}
|
||||
|
||||
bool Model::getJointPositionInModelFrame(int jointIndex, glm::vec3& position) const {
|
||||
if (jointIndex == -1 || jointIndex >= _jointStates.size()) {
|
||||
return false;
|
||||
}
|
||||
position = extractTranslation(_jointStates[jointIndex].getTransformInModelFrame());
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Model::getJointRotationInWorldFrame(int jointIndex, glm::quat& rotation) const {
|
||||
if (jointIndex == -1 || jointIndex >= _jointStates.size()) {
|
||||
return false;
|
||||
|
@ -1369,6 +1377,94 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& translation, const
|
|||
return true;
|
||||
}
|
||||
|
||||
bool Model::setJointPositionInModelFrame(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()) {
|
||||
return false;
|
||||
}
|
||||
const FBXGeometry& geometry = _geometry->getFBXGeometry();
|
||||
const QVector<int>& freeLineage = geometry.joints.at(jointIndex).freeLineage;
|
||||
if (freeLineage.isEmpty()) {
|
||||
return false;
|
||||
}
|
||||
if (lastFreeIndex == -1) {
|
||||
lastFreeIndex = freeLineage.last();
|
||||
}
|
||||
|
||||
// this is a cyclic coordinate descent algorithm: see
|
||||
// http://www.ryanjuckett.com/programming/animation/21-cyclic-coordinate-descent-in-2d
|
||||
const int ITERATION_COUNT = 1;
|
||||
glm::vec3 worldAlignment = alignment;
|
||||
for (int i = 0; i < ITERATION_COUNT; i++) {
|
||||
// first, try to rotate the end effector as close as possible to the target rotation, if any
|
||||
glm::quat endRotation;
|
||||
if (useRotation) {
|
||||
JointState& state = _jointStates[jointIndex];
|
||||
|
||||
// TODO: figure out what this is trying to do and combine it into one JointState method
|
||||
endRotation = state.getJointRotation();
|
||||
state.applyRotationDelta(rotation * glm::inverse(endRotation), true, priority);
|
||||
endRotation = state.getJointRotation();
|
||||
}
|
||||
|
||||
// then, we go from the joint upwards, rotating the end as close as possible to the target
|
||||
glm::vec3 endPosition = extractTranslation(_jointStates[jointIndex].getTransformInModelFrame());
|
||||
for (int j = 1; freeLineage.at(j - 1) != lastFreeIndex; j++) {
|
||||
int index = freeLineage.at(j);
|
||||
JointState& state = _jointStates[index];
|
||||
const FBXJoint& joint = state.getFBXJoint();
|
||||
if (!(joint.isFree || allIntermediatesFree)) {
|
||||
continue;
|
||||
}
|
||||
glm::vec3 jointPosition = extractTranslation(state.getTransformInModelFrame());
|
||||
glm::vec3 jointVector = endPosition - jointPosition;
|
||||
glm::quat oldCombinedRotation = state.getRotationInModelFrame();
|
||||
glm::quat combinedDelta;
|
||||
float combinedWeight;
|
||||
if (useRotation) {
|
||||
combinedDelta = safeMix(rotation * glm::inverse(endRotation),
|
||||
rotationBetween(jointVector, position - jointPosition), 0.5f);
|
||||
combinedWeight = 2.0f;
|
||||
|
||||
} else {
|
||||
combinedDelta = rotationBetween(jointVector, position - jointPosition);
|
||||
combinedWeight = 1.0f;
|
||||
}
|
||||
if (alignment != glm::vec3() && j > 1) {
|
||||
jointVector = endPosition - jointPosition;
|
||||
glm::vec3 positionSum;
|
||||
for (int k = j - 1; k > 0; k--) {
|
||||
int index = freeLineage.at(k);
|
||||
updateJointState(index);
|
||||
positionSum += extractTranslation(_jointStates.at(index).getTransformInModelFrame());
|
||||
}
|
||||
glm::vec3 projectedCenterOfMass = glm::cross(jointVector,
|
||||
glm::cross(positionSum / (j - 1.0f) - jointPosition, jointVector));
|
||||
glm::vec3 projectedAlignment = glm::cross(jointVector, glm::cross(worldAlignment, jointVector));
|
||||
const float LENGTH_EPSILON = 0.001f;
|
||||
if (glm::length(projectedCenterOfMass) > LENGTH_EPSILON && glm::length(projectedAlignment) > LENGTH_EPSILON) {
|
||||
combinedDelta = safeMix(combinedDelta, rotationBetween(projectedCenterOfMass, projectedAlignment),
|
||||
1.0f / (combinedWeight + 1.0f));
|
||||
}
|
||||
}
|
||||
state.applyRotationDeltaInModelFrame(combinedDelta, true, priority);
|
||||
glm::quat actualDelta = state.getRotationInModelFrame() * glm::inverse(oldCombinedRotation);
|
||||
endPosition = actualDelta * jointVector + jointPosition;
|
||||
if (useRotation) {
|
||||
endRotation = actualDelta * endRotation;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// now update the joint states from the top
|
||||
for (int j = freeLineage.size() - 1; j >= 0; j--) {
|
||||
updateJointState(freeLineage.at(j));
|
||||
}
|
||||
_shapesAreDirty = true;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Model::restoreJointPosition(int jointIndex, float fraction, float priority) {
|
||||
if (jointIndex == -1 || _jointStates.isEmpty()) {
|
||||
return false;
|
||||
|
@ -2072,6 +2168,10 @@ glm::quat JointState::getJointRotation() const {
|
|||
return _combinedRotation * _fbxJoint->inverseBindRotation;
|
||||
}
|
||||
|
||||
glm::quat JointState::getRotationFromBindToModelFrame() const {
|
||||
return _rotationInModelFrame * _fbxJoint->inverseBindRotation;
|
||||
}
|
||||
|
||||
void JointState::restoreRotation(float fraction, float priority) {
|
||||
assert(_fbxJoint != NULL);
|
||||
if (priority == _animationPriority) {
|
||||
|
@ -2088,6 +2188,14 @@ void JointState::setRotation(const glm::quat& rotation, float priority) {
|
|||
}
|
||||
}
|
||||
|
||||
void JointState::setRotationInModelFrame(const glm::quat& rotation, float priority) {
|
||||
assert(_fbxJoint != NULL);
|
||||
if (priority >= _animationPriority) {
|
||||
_rotation = _rotation * glm::inverse(_rotationInModelFrame) * rotation * glm::inverse(_fbxJoint->inverseBindRotation);
|
||||
_animationPriority = priority;
|
||||
}
|
||||
}
|
||||
|
||||
void JointState::clearTransformTranslation() {
|
||||
_transform[3][0] = 0.0f;
|
||||
_transform[3][1] = 0.0f;
|
||||
|
@ -2117,8 +2225,7 @@ void JointState::applyRotationDelta(const glm::quat& delta, bool constrain, floa
|
|||
_rotation = newRotation;
|
||||
}
|
||||
|
||||
/*
|
||||
void JointState::applyRotationDelta(const glm::quat& delta, bool constrain, float priority) {
|
||||
void JointState::applyRotationDeltaInModelFrame(const glm::quat& delta, bool constrain, float priority) {
|
||||
assert(_fbxJoint != NULL);
|
||||
if (priority < _animationPriority) {
|
||||
return;
|
||||
|
@ -2137,7 +2244,6 @@ void JointState::applyRotationDelta(const glm::quat& delta, bool constrain, floa
|
|||
_rotationInModelFrame = _rotationInModelFrame * glm::inverse(_rotation) * newRotation;
|
||||
_rotation = newRotation;
|
||||
}
|
||||
*/
|
||||
|
||||
const glm::vec3& JointState::getDefaultTranslationInParentFrame() const {
|
||||
assert(_fbxJoint != NULL);
|
||||
|
|
|
@ -54,8 +54,10 @@ public:
|
|||
|
||||
/// \return rotation from the joint's default (or bind) frame to world frame
|
||||
glm::quat getJointRotation() const;
|
||||
glm::quat getRotationFromBindToModelFrame() const;
|
||||
|
||||
void applyRotationDelta(const glm::quat& delta, bool constrain = true, float priority = 1.0f);
|
||||
void applyRotationDeltaInModelFrame(const glm::quat& delta, bool constrain = true, float priority = 1.0f);
|
||||
|
||||
const glm::vec3& getDefaultTranslationInParentFrame() const;
|
||||
|
||||
|
@ -65,6 +67,7 @@ public:
|
|||
/// computes parent relative _rotation and sets that
|
||||
/// \warning no combined transforms are updated!
|
||||
void setRotation(const glm::quat& rotation, float priority);
|
||||
void setRotationInModelFrame(const glm::quat& rotation, float priority);
|
||||
|
||||
const glm::mat4& getHybridTransform() const { return _transform; }
|
||||
//const glm::quat& getRotationInWorldFrame() const { return _combinedRotation; }
|
||||
|
@ -176,6 +179,8 @@ public:
|
|||
bool getJointRotationInWorldFrame(int jointIndex, glm::quat& rotation) const;
|
||||
bool getJointCombinedRotation(int jointIndex, glm::quat& rotation) const;
|
||||
|
||||
bool getJointPositionInModelFrame(int jointIndex, glm::vec3& position) const;
|
||||
|
||||
QStringList getJointNames() const;
|
||||
|
||||
AnimationHandlePointer createAnimationHandle();
|
||||
|
@ -266,6 +271,10 @@ protected:
|
|||
bool setJointPosition(int jointIndex, const glm::vec3& translation, 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);
|
||||
|
||||
bool setJointPositionInModelFrame(int jointIndex, const glm::vec3& translation, 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);
|
||||
|
||||
/// 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
|
||||
|
|
Loading…
Reference in a new issue