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

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,
y: 0,
z: 0,
w: Math.PI
w: 1
},
emitRadiusStart: 0,
polarStart: 0,
@ -84,7 +84,7 @@ function emitter(jointName) {
alpha: 1,
alphaSpread: 0,
alphaStart: 1,
alphaFinish: 1
alphaFinish: 1
});
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)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
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 MIN_HAND_TWIST = -PI / 2.0f;
if (isLeft) {
@ -634,8 +637,9 @@ void AnimInverseKinematics::initConstraints() {
} else {
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
// these directions are approximate swing limits in parent-frame
// NOTE: they don't need to be normalized
@ -670,7 +674,7 @@ void AnimInverseKinematics::initConstraints() {
stConstraint->setTwistLimits(-MAX_SHOULDER_TWIST, MAX_SHOULDER_TWIST);
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));
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;
twistAngle *= copysignf(1.0f, glm::dot(glm::cross(xAxis, twistedX), yAxis));
// adjust measured twistAngle according to clamping history
switch (_lastTwistBoundary) {
case LAST_CLAMP_LOW_BOUNDARY:
// clamp to min
if (twistAngle > _maxTwist) {
twistAngle -= TWO_PI;
}
break;
case LAST_CLAMP_HIGH_BOUNDARY:
// clamp to max
if (twistAngle < _minTwist) {
twistAngle += TWO_PI;
}
break;
default: // LAST_CLAMP_NO_BOUNDARY
// clamp to nearest boundary
float midBoundary = 0.5f * (_maxTwist + _minTwist + TWO_PI);
if (twistAngle > midBoundary) {
// lower boundary is closer --> phase down one cycle
twistAngle -= TWO_PI;
} else if (twistAngle < midBoundary - TWO_PI) {
// higher boundary is closer --> phase up one cycle
twistAngle += TWO_PI;
}
break;
}
bool somethingClamped = false;
if (_minTwist != _maxTwist) {
// adjust measured twistAngle according to clamping history
switch (_lastTwistBoundary) {
case LAST_CLAMP_LOW_BOUNDARY:
// clamp to min
if (twistAngle > _maxTwist) {
twistAngle -= TWO_PI;
}
break;
case LAST_CLAMP_HIGH_BOUNDARY:
// clamp to max
if (twistAngle < _minTwist) {
twistAngle += TWO_PI;
}
break;
default: // LAST_CLAMP_NO_BOUNDARY
// clamp to nearest boundary
float midBoundary = 0.5f * (_maxTwist + _minTwist + TWO_PI);
if (twistAngle > midBoundary) {
// lower boundary is closer --> phase down one cycle
twistAngle -= TWO_PI;
} else if (twistAngle < midBoundary - TWO_PI) {
// higher boundary is closer --> phase up one cycle
twistAngle += TWO_PI;
}
break;
}
// clamp twistAngle
float clampedTwistAngle = glm::clamp(twistAngle, _minTwist, _maxTwist);
bool twistWasClamped = (twistAngle != clampedTwistAngle);
// clamp twistAngle
float clampedTwistAngle = glm::clamp(twistAngle, _minTwist, _maxTwist);
somethingClamped = (twistAngle != clampedTwistAngle);
// remember twist's clamp boundary history
if (twistWasClamped) {
_lastTwistBoundary = (twistAngle > clampedTwistAngle) ? LAST_CLAMP_HIGH_BOUNDARY : LAST_CLAMP_LOW_BOUNDARY;
} else {
_lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY;
// remember twist's clamp boundary history
if (somethingClamped) {
_lastTwistBoundary = (twistAngle > clampedTwistAngle) ? LAST_CLAMP_HIGH_BOUNDARY : LAST_CLAMP_LOW_BOUNDARY;
twistAngle = clampedTwistAngle;
} else {
_lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY;
}
}
// clamp the swing
// The swingAxis is always perpendicular to the reference axis (yAxis in the constraint's frame).
glm::vec3 swungY = swingRotation * yAxis;
glm::vec3 swingAxis = glm::cross(yAxis, swungY);
bool swingWasClamped = false;
float axisLength = glm::length(swingAxis);
if (axisLength > EPSILON) {
// 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.
swingAxis /= axisLength;
swingRotation = glm::angleAxis(acosf(minDot), swingAxis);
swingWasClamped = true;
somethingClamped = true;
}
}
if (swingWasClamped || twistWasClamped) {
if (somethingClamped) {
// update the rotation
twistRotation = glm::angleAxis(clampedTwistAngle, yAxis);
twistRotation = glm::angleAxis(twistAngle, yAxis);
rotation = swingRotation * twistRotation * _referenceRotation;
return true;
}