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

Merge pull request #7246 from AndrewMeadows/stable-ik

Browse source 
recover stability of arm IK
This commit is contained in:
Philip Rosedale 2016-03-04 14:38:59 -08:00
commit cb8e2b7018
3 changed files with 49 additions and 42 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
examples/tests/particleOrientationTest.js
View file

@ -46,7 +46,7 @@ function emitter(jointName) {
x:0, x:0,
y: 0, y: 0,
z: 0, z: 0,
w: Math.PI w: 1
}, },
emitRadiusStart: 0, emitRadiusStart: 0,
polarStart: 0, polarStart: 0,
@ -84,7 +84,7 @@ function emitter(jointName) {
alpha: 1, alpha: 1,
alphaSpread: 0, alphaSpread: 0,
alphaStart: 1, alphaStart: 1,
alphaFinish: 1 alphaFinish: 1
}); });
return newEmitter; return newEmitter;
} }

8
libraries/animation/src/AnimInverseKinematics.cpp
View file

@ -627,6 +627,9 @@ void AnimInverseKinematics::initConstraints() {
} else if (0 == baseName.compare("Hand", Qt::CaseSensitive)) { } else if (0 == baseName.compare("Hand", Qt::CaseSensitive)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot); stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
stConstraint->setTwistLimits(0.0f, 0.0f); // max == min, disables twist limits
/* KEEP THIS CODE for future experimentation -- twist limits for hands
const float MAX_HAND_TWIST = 3.0f * PI / 5.0f; const float MAX_HAND_TWIST = 3.0f * PI / 5.0f;
const float MIN_HAND_TWIST = -PI / 2.0f; const float MIN_HAND_TWIST = -PI / 2.0f;
if (isLeft) { if (isLeft) {
@ -634,8 +637,9 @@ void AnimInverseKinematics::initConstraints() {
} else { } else {
stConstraint->setTwistLimits(MIN_HAND_TWIST, MAX_HAND_TWIST); stConstraint->setTwistLimits(MIN_HAND_TWIST, MAX_HAND_TWIST);
} }
*/
/* KEEP THIS CODE for future experimentation /* KEEP THIS CODE for future experimentation -- non-symmetrical swing limits for wrist
* a more complicated wrist with asymmetric cone * a more complicated wrist with asymmetric cone
// these directions are approximate swing limits in parent-frame // these directions are approximate swing limits in parent-frame
// NOTE: they don't need to be normalized // NOTE: they don't need to be normalized
@ -670,7 +674,7 @@ void AnimInverseKinematics::initConstraints() {
stConstraint->setTwistLimits(-MAX_SHOULDER_TWIST, MAX_SHOULDER_TWIST); stConstraint->setTwistLimits(-MAX_SHOULDER_TWIST, MAX_SHOULDER_TWIST);
std::vector<float> minDots; std::vector<float> minDots;
const float MAX_SHOULDER_SWING = PI / 6.0f; const float MAX_SHOULDER_SWING = PI / 20.0f;
minDots.push_back(cosf(MAX_SHOULDER_SWING)); minDots.push_back(cosf(MAX_SHOULDER_SWING));
stConstraint->setSwingLimits(minDots); stConstraint->setSwingLimits(minDots);

79
libraries/animation/src/SwingTwistConstraint.cpp
View file

@ -182,49 +182,52 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const {
glm::vec3 twistedX = twistRotation * xAxis; glm::vec3 twistedX = twistRotation * xAxis;
twistAngle *= copysignf(1.0f, glm::dot(glm::cross(xAxis, twistedX), yAxis)); twistAngle *= copysignf(1.0f, glm::dot(glm::cross(xAxis, twistedX), yAxis));
// adjust measured twistAngle according to clamping history bool somethingClamped = false;
switch (_lastTwistBoundary) { if (_minTwist != _maxTwist) {
case LAST_CLAMP_LOW_BOUNDARY: // adjust measured twistAngle according to clamping history
// clamp to min switch (_lastTwistBoundary) {
if (twistAngle > _maxTwist) { case LAST_CLAMP_LOW_BOUNDARY:
twistAngle -= TWO_PI; // clamp to min
} if (twistAngle > _maxTwist) {
break; twistAngle -= TWO_PI;
case LAST_CLAMP_HIGH_BOUNDARY: }
// clamp to max break;
if (twistAngle < _minTwist) { case LAST_CLAMP_HIGH_BOUNDARY:
twistAngle += TWO_PI; // clamp to max
} if (twistAngle < _minTwist) {
break; twistAngle += TWO_PI;
default: // LAST_CLAMP_NO_BOUNDARY }
// clamp to nearest boundary break;
float midBoundary = 0.5f * (_maxTwist + _minTwist + TWO_PI); default: // LAST_CLAMP_NO_BOUNDARY
if (twistAngle > midBoundary) { // clamp to nearest boundary
// lower boundary is closer --> phase down one cycle float midBoundary = 0.5f * (_maxTwist + _minTwist + TWO_PI);
twistAngle -= TWO_PI; if (twistAngle > midBoundary) {
} else if (twistAngle < midBoundary - TWO_PI) { // lower boundary is closer --> phase down one cycle
// higher boundary is closer --> phase up one cycle twistAngle -= TWO_PI;
twistAngle += TWO_PI; } else if (twistAngle < midBoundary - TWO_PI) {
} // higher boundary is closer --> phase up one cycle
break; twistAngle += TWO_PI;
} }
break;
}
// clamp twistAngle // clamp twistAngle
float clampedTwistAngle = glm::clamp(twistAngle, _minTwist, _maxTwist); float clampedTwistAngle = glm::clamp(twistAngle, _minTwist, _maxTwist);
bool twistWasClamped = (twistAngle != clampedTwistAngle); somethingClamped = (twistAngle != clampedTwistAngle);
// remember twist's clamp boundary history // remember twist's clamp boundary history
if (twistWasClamped) { if (somethingClamped) {
_lastTwistBoundary = (twistAngle > clampedTwistAngle) ? LAST_CLAMP_HIGH_BOUNDARY : LAST_CLAMP_LOW_BOUNDARY; _lastTwistBoundary = (twistAngle > clampedTwistAngle) ? LAST_CLAMP_HIGH_BOUNDARY : LAST_CLAMP_LOW_BOUNDARY;
} else { twistAngle = clampedTwistAngle;
_lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY; } else {
_lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY;
}
} }
// clamp the swing // clamp the swing
// The swingAxis is always perpendicular to the reference axis (yAxis in the constraint's frame). // The swingAxis is always perpendicular to the reference axis (yAxis in the constraint's frame).
glm::vec3 swungY = swingRotation * yAxis; glm::vec3 swungY = swingRotation * yAxis;
glm::vec3 swingAxis = glm::cross(yAxis, swungY); glm::vec3 swingAxis = glm::cross(yAxis, swungY);
bool swingWasClamped = false;
float axisLength = glm::length(swingAxis); float axisLength = glm::length(swingAxis);
if (axisLength > EPSILON) { if (axisLength > EPSILON) {
// The limit of swing is a function of "theta" which can be computed from the swingAxis // The limit of swing is a function of "theta" which can be computed from the swingAxis
@ -236,13 +239,13 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const {
// use it to supply a new rotation. // use it to supply a new rotation.
swingAxis /= axisLength; swingAxis /= axisLength;
swingRotation = glm::angleAxis(acosf(minDot), swingAxis); swingRotation = glm::angleAxis(acosf(minDot), swingAxis);
swingWasClamped = true; somethingClamped = true;
} }
} }
if (swingWasClamped || twistWasClamped) { if (somethingClamped) {
// update the rotation // update the rotation
twistRotation = glm::angleAxis(clampedTwistAngle, yAxis); twistRotation = glm::angleAxis(twistAngle, yAxis);
rotation = swingRotation * twistRotation * _referenceRotation; rotation = swingRotation * twistRotation * _referenceRotation;
return true; return true;
} }