Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-08-09 11:07:07 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions 2

fixed relative wrist correction problem

Browse source
This commit is contained in:
Angus Antley 2019-02-19 07:35:13 -08:00
parent 748368bfda
commit f2301e7dac
3 changed files with 45 additions and 56 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

6
libraries/animation/src/AnimPoleVectorConstraint.cpp
View file

@ -117,13 +117,15 @@ const AnimPoseVec& AnimPoleVectorConstraint::evaluate(const AnimVariantMap& anim
if (axisLength > MIN_LENGTH && refVectorLength > MIN_LENGTH && sideVectorLength > MIN_LENGTH && if (axisLength > MIN_LENGTH && refVectorLength > MIN_LENGTH && sideVectorLength > MIN_LENGTH &&
refVectorProjLength > MIN_LENGTH && poleVectorProjLength > MIN_LENGTH) { refVectorProjLength > MIN_LENGTH && poleVectorProjLength > MIN_LENGTH) {
float dot = glm::clamp(glm::dot(refVectorProj / refVectorProjLength, poleVectorProj / poleVectorProjLength), 0.0f, 1.0f); float dot = glm::clamp(glm::dot(refVectorProj / refVectorProjLength, poleVectorProj / poleVectorProjLength), -1.0f, 1.0f);
float sideDot = glm::dot(poleVector, sideVector); float sideDot = glm::dot(poleVector, sideVector);
float theta = copysignf(1.0f, sideDot) * acosf(dot); float theta = copysignf(1.0f, sideDot) * acosf(dot);
glm::quat deltaRot = glm::angleAxis(theta, unitAxis); glm::quat deltaRot = glm::angleAxis(theta, unitAxis);
//qCDebug(animation) << "anim ik theta " << (theta/PI)*180.0f; if (_tipJointName == "RightHand") {
qCDebug(animation) << "anim ik theta " << (theta / PI)*180.0f;
}
// transform result back into parent relative frame. // transform result back into parent relative frame.
glm::quat relBaseRot = glm::inverse(baseParentPose.rot()) * deltaRot * basePose.rot(); glm::quat relBaseRot = glm::inverse(baseParentPose.rot()) * deltaRot * basePose.rot();

93
libraries/animation/src/Rig.cpp
View file

@ -1723,8 +1723,9 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
} else { } else {
unitAxis = Vectors::UNIT_Y; unitAxis = Vectors::UNIT_Y;
} }
float yFactor = (fabsf(armToHand[1] / defaultArmLength) * weights[1]) + biases[1]; float yFactor = (fabsf(armToHand[1] / defaultArmLength) * weights[1]) + biases[1];
float zFactor; float zFactor;
if (armToHand[1] > 0.0f) { if (armToHand[1] > 0.0f) {
zFactor = weights[2] * glm::max(zStart - (armToHand[2] / defaultArmLength), 0.0f) * glm::max(fabsf((armToHand[1] - 0.1f) / defaultArmLength), 0.0f); zFactor = weights[2] * glm::max(zStart - (armToHand[2] / defaultArmLength), 0.0f) * glm::max(fabsf((armToHand[1] - 0.1f) / defaultArmLength), 0.0f);
@ -1739,7 +1740,7 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
} else { } else {
xFactor = weights[0] * (((armToHand[0] / defaultArmLength) + xStart) - (0.2f) * ((1.0f + (armToHand[1] / defaultArmLength)) / 2.0f)); xFactor = weights[0] * (((armToHand[0] / defaultArmLength) + xStart) - (0.2f) * ((1.0f + (armToHand[1] / defaultArmLength)) / 2.0f));
} }
float theta = xFactor + yFactor + zFactor; float theta = xFactor + yFactor + zFactor;
//float theta = yFactor; //float theta = yFactor;
@ -1766,7 +1767,7 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
AnimPose updatedBase = shoulderPose * deltaRot; AnimPose updatedBase = shoulderPose * deltaRot;
AnimPose newAbsMid = updatedBase * relMid; AnimPose newAbsMid = updatedBase * relMid;
/*
// now we calculate the contribution of the hand rotation relative to the arm // now we calculate the contribution of the hand rotation relative to the arm
// we are adding in the delta rotation so that we have the hand correction relative to the // we are adding in the delta rotation so that we have the hand correction relative to the
// latest theta for hand position // latest theta for hand position
@ -1799,7 +1800,7 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
// put some smoothing on the theta // put some smoothing on the theta
_ulnarRadialThetaRunningAverageRight = 0.75f * _ulnarRadialThetaRunningAverageRight + 0.25f * ulnarDeviationTheta; _ulnarRadialThetaRunningAverageRight = 0.75f * _ulnarRadialThetaRunningAverageRight + 0.25f * ulnarDeviationTheta;
} }
//get the flex/extension of the wrist rotation //get the flex/extension of the wrist rotation
glm::quat flex; glm::quat flex;
glm::quat nonFlex; glm::quat nonFlex;
@ -1825,7 +1826,7 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
// put some smoothing on the theta // put some smoothing on the theta
_flexThetaRunningAverageRight = 0.5f * _flexThetaRunningAverageRight + 0.5f * flexTheta; _flexThetaRunningAverageRight = 0.5f * _flexThetaRunningAverageRight + 0.5f * flexTheta;
} }
glm::quat twist; glm::quat twist;
glm::quat nonTwist; glm::quat nonTwist;
swingTwistDecomposition(relativeHandRotation, Vectors::UNIT_Y, nonTwist, twist); swingTwistDecomposition(relativeHandRotation, Vectors::UNIT_Y, nonTwist, twist);
@ -1849,13 +1850,11 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
// put some smoothing on the theta // put some smoothing on the theta
_twistThetaRunningAverageRight = 0.5f * _twistThetaRunningAverageRight + 0.5f * trueTwistTheta; _twistThetaRunningAverageRight = 0.5f * _twistThetaRunningAverageRight + 0.5f * trueTwistTheta;
} }
if (!left) {
qCDebug(animation) << "flex ave: " << (_flexThetaRunningAverageRight / PI) * 180.0f << " twist ave: " << (_twistThetaRunningAverageRight / PI) * 180.0f << " ulnar deviation ave: " << (_ulnarRadialThetaRunningAverageRight / PI) * 180.0f; float currentWristCoefficient = 0.0f;
} const float FLEX_BOUNDARY = 0.0f; // PI / 6.0f;
const float EXTEND_BOUNDARY = 0.0f; //-PI / 4.0f;
const float FLEX_BOUNDARY = PI / 6.0f;
const float EXTEND_BOUNDARY = -PI / 4.0f;
float flexCorrection = 0.0f; float flexCorrection = 0.0f;
if (left) { if (left) {
if (_flexThetaRunningAverageLeft > FLEX_BOUNDARY) { if (_flexThetaRunningAverageLeft > FLEX_BOUNDARY) {
@ -1863,22 +1862,21 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
} else if (_flexThetaRunningAverageLeft < EXTEND_BOUNDARY) { } else if (_flexThetaRunningAverageLeft < EXTEND_BOUNDARY) {
flexCorrection = ((_flexThetaRunningAverageLeft - EXTEND_BOUNDARY) / PI) * 180.0f; flexCorrection = ((_flexThetaRunningAverageLeft - EXTEND_BOUNDARY) / PI) * 180.0f;
} }
if (fabsf(flexCorrection) > 30.0f) { if (fabsf(flexCorrection) > 175.0f) {
flexCorrection = glm::sign(flexCorrection) * 30.0f; flexCorrection = glm::sign(flexCorrection) * 175.0f;
} }
theta += flexCorrection; currentWristCoefficient += flexCorrection;
} else { } else {
if (_flexThetaRunningAverageRight > FLEX_BOUNDARY) { if (_flexThetaRunningAverageRight > FLEX_BOUNDARY) {
flexCorrection = ((_flexThetaRunningAverageRight - FLEX_BOUNDARY) / PI) * 180.0f; flexCorrection = ((_flexThetaRunningAverageRight - FLEX_BOUNDARY) / PI) * 180.0f;
} else if (_flexThetaRunningAverageRight < EXTEND_BOUNDARY) { } else if (_flexThetaRunningAverageRight < EXTEND_BOUNDARY) {
flexCorrection = ((_flexThetaRunningAverageRight - EXTEND_BOUNDARY) / PI) * 180.0f; flexCorrection = ((_flexThetaRunningAverageRight - EXTEND_BOUNDARY) / PI) * 180.0f;
} }
if (fabsf(flexCorrection) > 30.0f) { if (fabsf(flexCorrection) > 175.0f) {
flexCorrection = glm::sign(flexCorrection) * 30.0f; flexCorrection = glm::sign(flexCorrection) * 175.0f;
} }
theta -= flexCorrection; currentWristCoefficient -= flexCorrection;
} }
const float ULNAR_BOUNDARY_MINUS = -PI / 6.0f; const float ULNAR_BOUNDARY_MINUS = -PI / 6.0f;
const float ULNAR_BOUNDARY_PLUS = PI / 4.0f; const float ULNAR_BOUNDARY_PLUS = PI / 4.0f;
@ -1901,18 +1899,11 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
} else { } else {
ulnarCorrection += glm::sign(ulnarDiff) * (fabsf(ulnarDiff) / PI) * 80.0f * twistCoefficient; ulnarCorrection += glm::sign(ulnarDiff) * (fabsf(ulnarDiff) / PI) * 80.0f * twistCoefficient;
} }
} else {
// right hand
if (_twistThetaRunningAverageLeft > 0.0f) {
ulnarCorrection -= glm::sign(ulnarDiff) * (fabsf(ulnarDiff) / PI) * 80.0f * twistCoefficient;
} else {
ulnarCorrection += glm::sign(ulnarDiff) * (fabsf(ulnarDiff) / PI) * 80.0f * twistCoefficient;
}
} }
if (fabsf(ulnarCorrection) > 20.0f) { if (fabsf(ulnarCorrection) > 20.0f) {
ulnarCorrection = glm::sign(ulnarCorrection) * 20.0f; ulnarCorrection = glm::sign(ulnarCorrection) * 20.0f;
} }
theta += ulnarCorrection; currentWristCoefficient += ulnarCorrection;
} }
} else { } else {
if (_ulnarRadialThetaRunningAverageRight > ULNAR_BOUNDARY_PLUS) { if (_ulnarRadialThetaRunningAverageRight > ULNAR_BOUNDARY_PLUS) {
@ -1931,22 +1922,14 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
} else { } else {
ulnarCorrection += glm::sign(ulnarDiff) * (fabsf(ulnarDiff) / PI) * 80.0f * twistCoefficient; ulnarCorrection += glm::sign(ulnarDiff) * (fabsf(ulnarDiff) / PI) * 80.0f * twistCoefficient;
} }
} else {
// right hand
if (_twistThetaRunningAverageRight > 0.0f) {
ulnarCorrection -= glm::sign(ulnarDiff) * (fabsf(ulnarDiff) / PI) * 80.0f * twistCoefficient;
} else {
ulnarCorrection += glm::sign(ulnarDiff) * (fabsf(ulnarDiff) / PI) * 80.0f * twistCoefficient;
}
} }
if (fabsf(ulnarCorrection) > 20.0f) { if (fabsf(ulnarCorrection) > 20.0f) {
ulnarCorrection = glm::sign(ulnarCorrection) * 20.0f; ulnarCorrection = glm::sign(ulnarCorrection) * 20.0f;
} }
theta += ulnarCorrection; currentWristCoefficient += ulnarCorrection;
} }
} }
const float TWIST_DEADZONE = (4 * PI) / 9.0f; const float TWIST_DEADZONE = (4 * PI) / 9.0f;
float twistCorrection = 0.0f; float twistCorrection = 0.0f;
if (left) { if (left) {
@ -1960,11 +1943,24 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
} }
// limit the twist correction // limit the twist correction
if (fabsf(twistCorrection) > 30.0f) { if (fabsf(twistCorrection) > 30.0f) {
theta += glm::sign(twistCorrection) * 30.0f; currentWristCoefficient += glm::sign(twistCorrection) * 30.0f;
} else { } else {
theta += twistCorrection; currentWristCoefficient += twistCorrection;
}
if (left) {
_lastWristCoefficientLeft = _lastThetaLeft - theta;
_lastWristCoefficientLeft += currentWristCoefficient;
theta += _lastWristCoefficientLeft;
} else {
_lastWristCoefficientRight = _lastThetaRight - theta;
_lastWristCoefficientRight += currentWristCoefficient;
theta += _lastWristCoefficientRight;
}
if (left) {
qCDebug(animation) << "theta " << theta << "Last wrist" << _lastWristCoefficientLeft << " flex ave: " << (_flexThetaRunningAverageLeft / PI) * 180.0f << " twist ave: " << (_twistThetaRunningAverageLeft / PI) * 180.0f << " ulnar deviation ave: " << (_ulnarRadialThetaRunningAverageLeft / PI) * 180.0f;
} }
*/
// global limiting // global limiting
float thetaRadians = 0.0f; float thetaRadians = 0.0f;
@ -1973,9 +1969,7 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
_lastThetaLeft = 0.5f * _lastThetaLeft + 0.5f * theta; _lastThetaLeft = 0.5f * _lastThetaLeft + 0.5f * theta;
if (fabsf(_lastThetaLeft) < 50.0f) { if (fabsf(_lastThetaLeft) < 50.0f) {
if (fabsf(_lastThetaLeft) < 50.0f) { _lastThetaLeft = glm::sign(_lastThetaLeft) * 50.0f;
_lastThetaLeft = glm::sign(_lastThetaLeft) * 50.0f;
}
} }
if (fabsf(_lastThetaLeft) > 175.0f) { if (fabsf(_lastThetaLeft) > 175.0f) {
_lastThetaLeft = glm::sign(_lastThetaLeft) * 175.0f; _lastThetaLeft = glm::sign(_lastThetaLeft) * 175.0f;
@ -1986,10 +1980,9 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
// final global smoothing // final global smoothing
_lastThetaRight = 0.5f * _lastThetaRight + 0.5f * theta; _lastThetaRight = 0.5f * _lastThetaRight + 0.5f * theta;
if (fabsf(_lastThetaRight) < 50.0f) {
if (fabsf(_lastThetaRight) < 50.0f) { if (fabsf(_lastThetaRight) < 10.0f) {
_lastThetaRight = glm::sign(_lastThetaRight) * 50.0f; _lastThetaRight = glm::sign(_lastThetaRight) * 10.0f;
}
} }
if (fabsf(_lastThetaRight) > 175.0f) { if (fabsf(_lastThetaRight) > 175.0f) {
_lastThetaRight = glm::sign(_lastThetaRight) * 175.0f; _lastThetaRight = glm::sign(_lastThetaRight) * 175.0f;
@ -2011,14 +2004,6 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
AnimPose armAxisPose(glm::mat4(glm::vec4(-w, 0.0f), glm::vec4(v, 0.0f), glm::vec4(u, 0.0f), glm::vec4(glm::vec3(0.0f, 0.0f, 0.0f), 1.0f))); AnimPose armAxisPose(glm::mat4(glm::vec4(-w, 0.0f), glm::vec4(v, 0.0f), glm::vec4(u, 0.0f), glm::vec4(glm::vec3(0.0f, 0.0f, 0.0f), 1.0f)));
poleVector = armAxisPose * thetaVector; poleVector = armAxisPose * thetaVector;
if (left) {
//qCDebug(animation) << "theta vector " << thetaVector;
//qCDebug(animation) << "fwd " << fwd;
//qCDebug(animation) << "the x is " << w;
//qCDebug(animation) << "the y is " << v;
//qCDebug(animation) << "the z is " << u;
}
return true; return true;
} }

2
libraries/animation/src/Rig.h
View file

@ -428,6 +428,8 @@ protected:
float _ulnarRadialThetaRunningAverageRight { 0.0f }; float _ulnarRadialThetaRunningAverageRight { 0.0f };
float _lastThetaLeft { 0.0f }; float _lastThetaLeft { 0.0f };
float _lastThetaRight { 0.0f }; float _lastThetaRight { 0.0f };
float _lastWristCoefficientRight { 0.0f };
float _lastWristCoefficientLeft { 0.0f };
AnimContext _lastContext; AnimContext _lastContext;
AnimVariantMap _lastAnimVars; AnimVariantMap _lastAnimVars;