Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-04-22 16:13:28 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions2

Fix for pole vector stability and knee pole vector computation

Browse source
This commit is contained in:
Anthony J. Thibault 2017-06-23 10:38:21 -07:00
parent 79d9bbe6c5
commit 54af6af651
3 changed files with 71 additions and 38 deletions

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

@ -530,22 +530,33 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
if (dLen > EPSILON) {
glm::vec3 dUnit = d / dLen;
glm::vec3 e = midPose.xformVector(target.getPoleReferenceVector());
glm::vec3 p = target.getPoleVector();
glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit;
glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit;
float eProjLen = glm::length(eProj);
float pProjLen = glm::length(pProj);
if (eProjLen > EPSILON && pProjLen > EPSILON) {
const float MIN_EPROJ_LEN = 0.5f;
if (eProjLen < MIN_EPROJ_LEN) {
glm::vec3 midPoint = topPose.trans() + d * 0.5f;
e = midPose.trans() - midPoint;
eProj = e - glm::dot(e, dUnit) * dUnit;
eProjLen = glm::length(eProj);
}
glm::vec3 p = target.getPoleVector();
glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit;
float pProjLen = glm::length(pProj);
if (eProjLen > EPSILON && pProjLen > EPSILON) {
// as pProjLen become orthognal to d, reduce the amount of rotation.
float magnitude = easeOutExpo(pProjLen);
float dot = glm::clamp(glm::dot(eProj / eProjLen, pProj / pProjLen), 0.0f, 1.0f);
float theta = acosf(dot);
glm::vec3 cross = glm::cross(eProj, pProj);
float crossLen = glm::length(cross);
if (crossLen > EPSILON) {
glm::vec3 axis = cross / crossLen;
const float MIN_ADJUSTMENT_ANGLE = 0.001745f; // 0.1 degree
if (theta > MIN_ADJUSTMENT_ANGLE) {
glm::vec3 axis = dUnit;
if (glm::dot(cross, dUnit) < 0) {
axis = -dUnit;
}
poleRot = glm::angleAxis(magnitude * theta, axis);
}
}
@ -562,8 +573,17 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
glm::vec3 dUnit = d / dLen;
glm::vec3 e = midPose.xformVector(target.getPoleReferenceVector());
glm::vec3 p = target.getPoleVector();
glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit;
float eProjLen = glm::length(eProj);
const float MIN_EPROJ_LEN = 0.5f;
if (eProjLen < MIN_EPROJ_LEN) {
glm::vec3 midPoint = topPose.trans() + d * 0.5f;
e = midPose.trans() - midPoint;
eProj = e - glm::dot(e, dUnit) * dUnit;
eProjLen = glm::length(eProj);
}
glm::vec3 p = target.getPoleVector();
glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit;
const float PROJ_VECTOR_LEN = 10.0f;
@ -573,11 +593,8 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
geomToWorldPose.xformPoint(topPose.trans()),
YELLOW);
DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint),
geomToWorldPose.xformPoint(midPoint + PROJ_VECTOR_LEN * glm::normalize(eProj)),
geomToWorldPose.xformPoint(midPoint + PROJ_VECTOR_LEN * glm::normalize(e)),
RED);
DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint),
geomToWorldPose.xformPoint(midPoint + PROJ_VECTOR_LEN * glm::normalize(pProj)),
GREEN);
DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint),
geomToWorldPose.xformPoint(midPoint + POLE_VECTOR_LEN * glm::normalize(p)),
BLUE);
@ -1060,7 +1077,7 @@ void AnimInverseKinematics::initConstraints() {
// y |
// | |
// | O---O---O RightUpLeg
// z | | Hips2 |
// z | | Hips |
// \ | | |
// \| | |
// x -----+ O O RightLeg

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

@ -1265,14 +1265,14 @@ void Rig::updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose
int hipsIndex = indexOfJoint("Hips");
if (leftFootEnabled) {
glm::vec3 footPosition = leftFootPose.trans();
glm::quat footRotation = leftFootPose.rot();
_animVars.set("leftFootPosition", footPosition);
_animVars.set("leftFootRotation", footRotation);
_animVars.set("leftFootPosition", leftFootPose.trans());
_animVars.set("leftFootRotation", leftFootPose.rot());
_animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition);
int footJointIndex = _animSkeleton->nameToJointIndex("LeftFoot");
glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, footRotation, hipsIndex);
int kneeJointIndex = _animSkeleton->nameToJointIndex("LeftLeg");
int upLegJointIndex = _animSkeleton->nameToJointIndex("LeftUpLeg");
glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, kneeJointIndex, upLegJointIndex, hipsIndex, leftFootPose);
// smooth toward desired pole vector from previous pole vector... to reduce jitter
if (!_prevLeftFootPoleVectorValid) {
@ -1295,14 +1295,14 @@ void Rig::updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose
}
if (rightFootEnabled) {
glm::vec3 footPosition = rightFootPose.trans();
glm::quat footRotation = rightFootPose.rot();
_animVars.set("rightFootPosition", footPosition);
_animVars.set("rightFootRotation", footRotation);
_animVars.set("rightFootPosition", rightFootPose.trans());
_animVars.set("rightFootRotation", rightFootPose.rot());
_animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition);
int footJointIndex = _animSkeleton->nameToJointIndex("RightFoot");
glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, footRotation, hipsIndex);
int kneeJointIndex = _animSkeleton->nameToJointIndex("RightLeg");
int upLegJointIndex = _animSkeleton->nameToJointIndex("RightUpLeg");
glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, kneeJointIndex, upLegJointIndex, hipsIndex, rightFootPose);
// smooth toward desired pole vector from previous pole vector... to reduce jitter
if (!_prevRightFootPoleVectorValid) {
@ -1375,20 +1375,23 @@ glm::vec3 Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIn
AnimPose handPose = _externalPoseSet._absolutePoses[handIndex];
AnimPose elbowPose = _externalPoseSet._absolutePoses[elbowIndex];
AnimPose armPose = _externalPoseSet._absolutePoses[armIndex];
// ray from hand to arm.
glm::vec3 d = glm::normalize(handPose.trans() - armPose.trans());
float sign = isLeft ? 1.0f : -1.0f;
// form a plane normal to the hips x-axis.
glm::vec3 n = hipsPose.rot() * Vectors::UNIT_X;
glm::vec3 y = hipsPose.rot() * Vectors::UNIT_Y;
// project d onto n.
// project d onto this plane
glm::vec3 dProj = d - glm::dot(d, n) * n;
// give dProj a bit of offset away from the body.
float avatarScale = extractUniformScale(_modelOffset);
const float LATERAL_OFFSET = 0.333f * avatarScale;
const float VERTICAL_OFFSET = -0.333f * avatarScale;
// give dProj a bit of offset away from the body.
glm::vec3 dProjWithOffset = dProj + sign * LATERAL_OFFSET * n + y * VERTICAL_OFFSET;
// rotate dProj by 90 degrees to get the poleVector.
@ -1402,19 +1405,32 @@ glm::vec3 Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIn
return glm::normalize(poleAdjust * poleVector);
}
glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const {
glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, int kneeIndex, int upLegIndex, int hipsIndex, const AnimPose& targetFootPose) const {
AnimPose defaultPose = _animSkeleton->getAbsoluteDefaultPose(footJointIndex);
glm::vec3 localForward = glm::inverse(defaultPose.rot()) * Vectors::UNIT_Z;
glm::vec3 footForward = footTargetOrientation * localForward;
AnimPose hipsPose = _externalPoseSet._absolutePoses[hipsIndex];
AnimPose footPose = targetFootPose;
AnimPose kneePose = _externalPoseSet._absolutePoses[kneeIndex];
AnimPose upLegPose = _externalPoseSet._absolutePoses[upLegIndex];
// compute the forward direction of the hips.
glm::quat hipsRotation = _externalPoseSet._absolutePoses[hipsIndex].rot();
glm::vec3 hipsForward = hipsRotation * Vectors::UNIT_Z;
// ray from foot to upLeg
glm::vec3 d = glm::normalize(footPose.trans() - upLegPose.trans());
// blend between the hips and the foot.
const float FOOT_TO_HIPS_BLEND_FACTOR = 0.5f;
return glm::normalize(lerp(footForward, hipsForward, FOOT_TO_HIPS_BLEND_FACTOR));
// form a plane normal to the hips x-axis
glm::vec3 n = hipsPose.rot() * Vectors::UNIT_X;
glm::vec3 y = hipsPose.rot() * Vectors::UNIT_Y;
// project d onto this plane
glm::vec3 dProj = d - glm::dot(d, n) * n;
// rotate dProj by 90 degrees to get the poleVector.
glm::vec3 poleVector = glm::angleAxis(-PI / 2.0f, n) * dProj;
// blend the foot oreintation into the pole vector
glm::quat kneeToFootDelta = footPose.rot() * glm::inverse(kneePose.rot());
const float WRIST_POLE_ADJUST_FACTOR = 0.5f;
glm::quat poleAdjust = quatLerp(Quaternions::IDENTITY, kneeToFootDelta, WRIST_POLE_ADJUST_FACTOR);
return glm::normalize(poleAdjust * poleVector);
}
void Rig::updateFromControllerParameters(const ControllerParameters& params, float dt) {

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

@ -239,7 +239,7 @@ protected:
void calcAnimAlpha(float speed, const std::vector<float>& referenceSpeeds, float* alphaOut) const;
glm::vec3 calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int hipsIndex, bool isLeft) const;
glm::vec3 calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const;
glm::vec3 calculateKneePoleVector(int footJointIndex, int kneeJoint, int upLegIndex, int hipsIndex, const AnimPose& targetFootPose) const;
AnimPose _modelOffset; // model to rig space
AnimPose _geometryOffset; // geometry to model space (includes unit offset & fst offsets)