Mirrors/overte-lubosz
3
0
Fork 0
mirror of https://github.com/lubosz/overte.git synced 2025-04-14 01:46:18 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

starting cleanup

Browse source
This commit is contained in:
Angus Antley 2019-02-14 23:45:45 -08:00
parent 40196dcb40
commit f8554d10a8
3 changed files with 80 additions and 369 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -34,69 +34,6 @@ AnimPoleVectorConstraint::~AnimPoleVectorConstraint() {
}
static float correctElbowForHandFlexionExtension(const AnimPose& hand, const AnimPose& lowerArm) {
// first calculate the ulnar/radial deviation
// use the lower arm x-axis and the hand x-axis
glm::vec3 xAxisLowerArm = lowerArm.rot() * glm::vec3(1.0f, 0.0f, 0.0f);
glm::vec3 yAxisLowerArm = lowerArm.rot() * glm::vec3(0.0f, 1.0f, 0.0f);
glm::vec3 zAxisLowerArm = lowerArm.rot() * glm::vec3(0.0f, 0.0f, 1.0f);
glm::vec3 xAxisHand = hand.rot() * glm::vec3(1.0f, 0.0f, 0.0f);
glm::vec3 yAxisHand = hand.rot() * glm::vec3(0.0f, 1.0f, 0.0f);
//float ulnarRadialDeviation = atan2(glm::dot(xAxisHand, xAxisLowerArm), glm::dot(xAxisHand, yAxisLowerArm));
float flexionExtension = atan2(glm::dot(yAxisHand, zAxisLowerArm), glm::dot(yAxisHand, yAxisLowerArm));
//qCDebug(animation) << "flexion angle " << flexionExtension;
float deltaInDegrees = (flexionExtension / PI) * 180.0f;
//qCDebug(animation) << "delta in degrees " << deltaInDegrees;
float deltaFinal = glm::sign(deltaInDegrees) * powf(fabsf(deltaInDegrees/180.0f), 1.5f) * 180.0f * -0.3f;
return deltaInDegrees;// deltaFinal;
}
static float correctElbowForHandUlnarRadialDeviation(const AnimPose& hand, const AnimPose& lowerArm) {
const float DEAD_ZONE = 0.3f;
const float FILTER_EXPONENT = 2.0f;
// first calculate the ulnar/radial deviation
// use the lower arm x-axis and the hand x-axis
glm::vec3 xAxisLowerArm = lowerArm.rot() * glm::vec3(1.0f, 0.0f, 0.0f);
glm::vec3 yAxisLowerArm = lowerArm.rot() * glm::vec3(0.0f, 1.0f, 0.0f);
glm::vec3 zAxisLowerArm = lowerArm.rot() * glm::vec3(0.0f, 0.0f, 1.0f);
glm::vec3 xAxisHand = hand.rot() * glm::vec3(1.0f, 0.0f, 0.0f);
glm::vec3 yAxisHand = hand.rot() * glm::vec3(0.0f, 1.0f, 0.0f);
float ulnarRadialDeviation = atan2(glm::dot(xAxisHand, xAxisLowerArm), glm::dot(xAxisHand, yAxisLowerArm));
//float flexionExtension = atan2(glm::dot(yAxisHand, zAxisLowerArm), glm::dot(yAxisHand, yAxisLowerArm));
float makeForwardZeroRadians = ulnarRadialDeviation - (PI / 2.0f);
//qCDebug(animation) << "calibrated ulnar " << makeForwardZeroRadians;
float deltaFractionOfPi = (makeForwardZeroRadians / PI);
float deltaUlnarRadial;
if (fabsf(deltaFractionOfPi) < DEAD_ZONE) {
deltaUlnarRadial = 0.0f;
} else {
deltaUlnarRadial = (deltaFractionOfPi - glm::sign(deltaFractionOfPi) * DEAD_ZONE) / (1.0f - DEAD_ZONE);
}
float deltaUlnarRadialDegrees = glm::sign(deltaUlnarRadial) * powf(fabsf(deltaUlnarRadial), FILTER_EXPONENT) * 180.0f;
//qCDebug(animation) << "ulnar delta in degrees " << deltaUlnarRadialDegrees;
float deltaFinal = deltaUlnarRadialDegrees;
return deltaFractionOfPi * 180.0f; // deltaFinal;
}
float AnimPoleVectorConstraint::findThetaNewWay(const glm::vec3& hand, const glm::vec3& shoulder, bool left) const {
// get the default poses for the upper and lower arm
// then use this length to judge how far the hand is away from the shoulder.
@ -184,8 +121,8 @@ const AnimPoseVec& AnimPoleVectorConstraint::evaluate(const AnimVariantMap& anim
glm::vec3 poleVector = animVars.lookupRigToGeometryVector(_poleVectorVar, Vectors::UNIT_Z);
if (_skeleton->nameToJointIndex("LeftHand") == _tipJointIndex) {
float thetaFromRig = animVars.lookup("thetaRight", 0.0f);
qCDebug(animation) << " anim pole vector theta from rig " << thetaFromRig;
//float thetaFromRig = animVars.lookup("thetaRight", 0.0f);
//qCDebug(animation) << " anim pole vector theta from rig " << thetaFromRig;
}
// determine if we should interpolate
@ -227,6 +164,11 @@ const AnimPoseVec& AnimPoleVectorConstraint::evaluate(const AnimVariantMap& anim
glm::vec3 refVector = midPose.xformVectorFast(_referenceVector);
float refVectorLength = glm::length(refVector);
if (_skeleton->nameToJointIndex("LeftHand") == _tipJointIndex) {
//qCDebug(animation) << "mid pose anim " << midPose;
//qCDebug(animation) << "ref vector anim " << refVector;
}
glm::vec3 axis = basePose.trans() - tipPose.trans();
float axisLength = glm::length(axis);
glm::vec3 unitAxis = axis / axisLength;
@ -247,7 +189,7 @@ const AnimPoseVec& AnimPoleVectorConstraint::evaluate(const AnimVariantMap& anim
if (refVectorLength > MIN_LENGTH && sideVectorLength > MIN_LENGTH) {
poleVector = lastDot * (refVectorProj / refVectorProjLength) + glm::sign(_lastTheta) * lastSideDot * (sideVector / sideVectorLength);
if (_skeleton->nameToJointIndex("LeftHand") == _tipJointIndex) {
qCDebug(animation) << " anim pole vector computed: " << poleVector;
//qCDebug(animation) << "pole vector anim: " << poleVector;
}
} else {
poleVector = glm::vec3(1.0f, 0.0f, 0.0f);
@ -277,196 +219,15 @@ const AnimPoseVec& AnimPoleVectorConstraint::evaluate(const AnimVariantMap& anim
//fred = findThetaNewWay(tipPose.trans(), basePose.trans(), isLeft);
if (isLeft) {
float thetaFromRig = animVars.lookup("thetaLeft", 0.0f);
qCDebug(animation) << " anim pole vector theta from rig left" << thetaFromRig;
//qCDebug(animation) << " anim pole vector theta from rig left" << thetaFromRig;
fred = thetaFromRig;
} else {
float thetaFromRig = animVars.lookup("thetaRight", 0.0f);
qCDebug(animation) << " anim pole vector theta from rig right" << thetaFromRig;
//qCDebug(animation) << " anim pole vector theta from rig right" << thetaFromRig;
fred = thetaFromRig;
}
/*
glm::quat relativeHandRotation = (midPose.inverse() * tipPose).rot();
relativeHandRotation = glm::normalize(relativeHandRotation);
if (relativeHandRotation.w < 0.0f) {
relativeHandRotation.x *= -1.0f;
relativeHandRotation.y *= -1.0f;
relativeHandRotation.z *= -1.0f;
relativeHandRotation.w *= -1.0f;
}
glm::quat twist;
glm::quat ulnarDeviation;
//swingTwistDecomposition(twistUlnarSwing, Vectors::UNIT_Z, twist, ulnarDeviation);
swingTwistDecomposition(relativeHandRotation, Vectors::UNIT_Z, twist, ulnarDeviation);
ulnarDeviation = glm::normalize(ulnarDeviation);
if (ulnarDeviation.w < 0.0f) {
ulnarDeviation.x *= -1.0f;
ulnarDeviation.y *= -1.0f;
ulnarDeviation.z *= -1.0f;
ulnarDeviation.w *= -1.0f;
}
//glm::vec3 twistAxis = glm::axis(twist);
glm::vec3 ulnarAxis = glm::axis(ulnarDeviation);
//float twistTheta = glm::sign(twistAxis[1]) * glm::angle(twist);
float ulnarDeviationTheta = glm::sign(ulnarAxis[2]) * glm::angle(ulnarDeviation);
if (glm::sign(ulnarDeviationTheta) != glm::sign(_ulnarRadialThetaRunningAverage) && fabsf(ulnarDeviationTheta) >(5.0f * PI) / 6.0f) {
// don't allow the theta to cross the 180 degree limit.
ulnarDeviationTheta = -1.0f * ulnarDeviationTheta;
}
_ulnarRadialThetaRunningAverage = 0.5f * _ulnarRadialThetaRunningAverage + 0.5f * ulnarDeviationTheta;
//get the swingTwist of the hand to lower arm
glm::quat flex;
glm::quat twistUlnarSwing;
swingTwistDecomposition(relativeHandRotation, Vectors::UNIT_X, twistUlnarSwing, flex);
flex = glm::normalize(flex);
if (flex.w < 0.0f) {
flex.x *= -1.0f;
flex.y *= -1.0f;
flex.z *= -1.0f;
flex.w *= -1.0f;
}
glm::vec3 flexAxis = glm::axis(flex);
//float swingTheta = glm::angle(twistUlnarSwing);
float flexTheta = glm::sign(flexAxis[0]) * glm::angle(flex);
if (glm::sign(flexTheta) != glm::sign(_flexThetaRunningAverage) && fabsf(flexTheta) > (5.0f * PI) / 6.0f) {
// don't allow the theta to cross the 180 degree limit.
flexTheta = -1.0f * flexTheta;
}
_flexThetaRunningAverage = 0.5f * _flexThetaRunningAverage + 0.5f * flexTheta;
glm::quat trueTwist;
glm::quat nonTwist;
swingTwistDecomposition(relativeHandRotation, Vectors::UNIT_Y, nonTwist, trueTwist);
trueTwist = glm::normalize(trueTwist);
if (trueTwist.w < 0.0f) {
trueTwist.x *= -1.0f;
trueTwist.y *= -1.0f;
trueTwist.z *= -1.0f;
trueTwist.w *= -1.0f;
}
glm::vec3 trueTwistAxis = glm::axis(trueTwist);
float trueTwistTheta;
trueTwistTheta = glm::sign(trueTwistAxis[1]) * glm::angle(trueTwist);
if (glm::sign(trueTwistTheta) != glm::sign(_twistThetaRunningAverage) && fabsf(trueTwistTheta) >(5.0f * PI) / 6.0f) {
// don't allow the theta to cross the 180 degree limit.
trueTwistTheta = -1.0f * trueTwistTheta;
}
_twistThetaRunningAverage = 0.5f * _twistThetaRunningAverage + 0.5f * trueTwistTheta;
if (!isLeft) {
//qCDebug(animation) << "flex ave: " << (_flexThetaRunningAverage / PI) * 180.0f << " twist ave: " << (_twistThetaRunningAverage / PI) * 180.0f << " ulnar deviation ave: " << (_ulnarRadialThetaRunningAverage / PI) * 180.0f;
//qCDebug(animation) << "flex: " << (flexTheta / PI) * 180.0f << " twist: " << (trueTwistTheta / PI) * 180.0f << " ulnar deviation: " << (ulnarDeviationTheta / PI) * 180.0f;
}
// here is where we would do the wrist correction.
float deltaTheta = correctElbowForHandFlexionExtension(tipPose, midPose);
float deltaThetaUlnar;
if (!isLeft) {
deltaThetaUlnar = correctElbowForHandUlnarRadialDeviation(tipPose, midPose);
}
if (isLeft) {
// fred *= -1.0f;
}
// make the dead zone PI/6.0
const float POWER = 2.0f;
const float FLEX_BOUNDARY = PI / 4.0f;
const float EXTEND_BOUNDARY = -PI / 5.0f;
float flexCorrection = 0.0f;
if (isLeft) {
if (_flexThetaRunningAverage > FLEX_BOUNDARY) {
flexCorrection = ((_flexThetaRunningAverage - FLEX_BOUNDARY) / PI) * 180.0f;
} else if (_flexThetaRunningAverage < EXTEND_BOUNDARY) {
flexCorrection = ((_flexThetaRunningAverage - EXTEND_BOUNDARY) / PI) * 180.0f;
}
if (fabs(flexCorrection) > 30.0f) {
flexCorrection = glm::sign(flexCorrection) * 30.0f;
}
fred += flexCorrection;
} else {
if (_flexThetaRunningAverage > FLEX_BOUNDARY) {
flexCorrection = ((_flexThetaRunningAverage - FLEX_BOUNDARY) / PI) * 180.0f;
} else if (_flexThetaRunningAverage < EXTEND_BOUNDARY) {
flexCorrection = ((_flexThetaRunningAverage - EXTEND_BOUNDARY) / PI) * 180.0f;
}
if (fabs(flexCorrection) > 30.0f) {
flexCorrection = glm::sign(flexCorrection) * 30.0f;
}
fred -= flexCorrection;
}
//qCDebug(animation) << "flexCorrection anim" << flexCorrection;
const float TWIST_ULNAR_DEADZONE = 0.0f;
const float ULNAR_BOUNDARY_MINUS = -PI / 12.0f;
const float ULNAR_BOUNDARY_PLUS = PI / 24.0f;
float ulnarDiff = 0.0f;
float ulnarCorrection = 0.0f;
if (_ulnarRadialThetaRunningAverage > ULNAR_BOUNDARY_PLUS) {
ulnarDiff = _ulnarRadialThetaRunningAverage - ULNAR_BOUNDARY_PLUS;
} else if (_ulnarRadialThetaRunningAverage < ULNAR_BOUNDARY_MINUS) {
ulnarDiff = _ulnarRadialThetaRunningAverage - ULNAR_BOUNDARY_MINUS;
}
if(fabs(ulnarDiff) > 0.0f){
if (fabs(_twistThetaRunningAverage) > TWIST_ULNAR_DEADZONE) {
float twistCoefficient = (fabs(_twistThetaRunningAverage) / (PI / 20.0f));
if (twistCoefficient > 1.0f) {
twistCoefficient = 1.0f;
}
if (isLeft) {
if (trueTwistTheta < 0.0f) {
ulnarCorrection -= glm::sign(ulnarDiff) * (fabs(ulnarDiff) / PI) * 90.0f * twistCoefficient;
} else {
ulnarCorrection += glm::sign(ulnarDiff) * (fabs(ulnarDiff) / PI) * 90.0f * twistCoefficient;
}
} else {
// right hand
if (trueTwistTheta > 0.0f) {
ulnarCorrection -= glm::sign(ulnarDiff) * (fabs(ulnarDiff) / PI) * 90.0f * twistCoefficient;
} else {
ulnarCorrection += glm::sign(ulnarDiff) * (fabs(ulnarDiff) / PI) * 90.0f * twistCoefficient;
}
}
if (fabsf(ulnarCorrection) > 20.0f) {
ulnarCorrection = glm::sign(ulnarCorrection) * 20.0f;
}
fred += ulnarCorrection;
}
}
//qCDebug(animation) << "ulnarCorrection anim" << ulnarCorrection;
// remember direction of travel.
const float TWIST_DEADZONE = PI / 2.0f;
//if (!isLeft) {
float twistCorrection = 0.0f;
if (_twistThetaRunningAverage < -TWIST_DEADZONE) {
twistCorrection = glm::sign(_twistThetaRunningAverage) * ((fabsf(_twistThetaRunningAverage) - TWIST_DEADZONE) / PI) * 60.0f;
} else {
if (_twistThetaRunningAverage > TWIST_DEADZONE) {
twistCorrection = glm::sign(_twistThetaRunningAverage) * ((fabsf(_twistThetaRunningAverage) - TWIST_DEADZONE) / PI) * 60.0f;
}
}
if (fabsf(twistCorrection) > 30.0f) {
fred += glm::sign(twistCorrection) * 30.0f;
} else {
fred += twistCorrection;
}
*/
_lastTheta = 0.5f * _lastTheta + 0.5f * fred;
//qCDebug(animation) << "twist correction: " << twistCorrection << " flex correction: " << flexCorrection << " ulnar correction " << ulnarCorrection;
@ -482,12 +243,9 @@ const AnimPoseVec& AnimPoleVectorConstraint::evaluate(const AnimVariantMap& anim
float poleVectorTheta = theta;
theta = ((180.0f - _lastTheta) / 180.0f)*PI;
}
//glm::quat deltaRot = glm::angleAxis(theta, unitAxis);
glm::quat deltaRot = glm::angleAxis(theta, unitAxis);

3
libraries/animation/src/AnimPoleVectorConstraint.h
View file

@ -66,9 +66,6 @@ protected:
float _interpAlphaVel { 0.0f };
float _interpAlpha { 0.0f };
float _twistThetaRunningAverage { 0.0f };
float _flexThetaRunningAverage { 0.0f };
float _ulnarRadialThetaRunningAverage { 0.0f };
float _lastTheta { 0.0f };
AnimChain _snapshotChain;

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

@ -1695,7 +1695,7 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
// make the angle less when z is small.
// lower y with x center lower angle
// lower y with x out higher angle
//AnimPose oppositeArmPose = _externalPoseSet._absolutePoses[oppositeArmIndex];
glm::vec3 referenceVector;
if (left) {
referenceVector = Vectors::UNIT_X;
@ -1707,40 +1707,38 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
AnimPose shoulderPose = _externalPoseSet._absolutePoses[shoulderIndex];
AnimPose elbowPose = _externalPoseSet._absolutePoses[elbowIndex];
//qCDebug(animation) << "handPose Rig " << left << "isleft" << handPose;
AnimPose absoluteShoulderPose = getAbsoluteDefaultPose(shoulderIndex);
AnimPose absoluteHandPose = getAbsoluteDefaultPose(handIndex);
// subtract 10 centimeters from the arm length for some reason actual arm position is clamped to length - 10cm.
float defaultArmLength = glm::length(absoluteHandPose.trans() - absoluteShoulderPose.trans());
// calculate the reference axis and the side axis.
// Look up refVector from animVars, make sure to convert into geom space.
glm::vec3 refVector = glmExtractRotation(_rigToGeometryTransform) * elbowPose.rot() * Vectors::UNIT_X;
glm::vec3 refVector = (AnimPose(_rigToGeometryTransform) * elbowPose).xformVectorFast(Vectors::UNIT_X);
float refVectorLength = glm::length(refVector);
glm::vec3 unitRef = refVector / refVectorLength;
glm::vec3 axis = shoulderPose.trans() - handPose.trans();
if (left) {
//AnimPose temp(_rigToGeometryTransform);
//glm::mat4 elbowMat(elbowPose);
//AnimPose result3(_rigToGeometryTransform * elbowMat);
//AnimPose geomElbow2 = temp * elbowPose;
//qCDebug(animation) << "mid pose geom2 rig" << geomElbow2;
//qCDebug(animation) << "mid pose result rig" << result3;
//qCDebug(animation) << "ref vector rig" << refVector;
}
AnimPose geomShoulder = AnimPose(_rigToGeometryTransform) * shoulderPose;
AnimPose geomHand = AnimPose(_rigToGeometryTransform) * handPose;
glm::vec3 axis = geomShoulder.trans() - geomHand.trans();
float axisLength = glm::length(axis);
glm::vec3 unitAxis = axis / axisLength;
glm::vec3 sideVector = glm::cross(unitAxis, unitRef);
glm::vec3 sideVector = glm::cross(unitAxis, refVector);
float sideVectorLength = glm::length(sideVector);
// project refVector onto axis plane
glm::vec3 refVectorProj = unitRef - glm::dot(unitRef, unitAxis) * unitAxis;
glm::vec3 refVectorProj = refVector - glm::dot(refVector, unitAxis) * unitAxis;
float refVectorProjLength = glm::length(refVectorProj);
if (left) {
//qCDebug(animation) << "rig ref proj " << refVectorProj/refVectorProjLength; // "rig reference vector: " << refVector / refVectorLength;
}
//qCDebug(animation) << "rig reference vector projected: " << refVectorProj << " left is " << left;
// qCDebug(animation) << "default arm length " << defaultArmLength;
// phi_0 is the lowest angle we can have
const float phi_0 = 15.0f;
const float zStart = 0.6f;
@ -1750,28 +1748,25 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
const glm::vec3 biases(0.0f, 135.0f, 0.0f);
// weights
const float zWeightBottom = -100.0f;
//const glm::vec3 weights(-30.0f, 30.0f, 210.0f);
const glm::vec3 weights(-50.0f, 60.0f, 260.0f);
glm::vec3 armToHand = handPose.trans() - shoulderPose.trans();
// qCDebug(animation) << "current arm length " << glm::length(armToHand);
float initial_valuesY = (fabsf(armToHand[1] / defaultArmLength) * weights[1]) + biases[1];
float initial_valuesZ;
float yFactor = (fabsf(armToHand[1] / defaultArmLength) * weights[1]) + biases[1];
float zFactor;
if (armToHand[1] > 0.0f) {
initial_valuesZ = weights[2] * glm::max(zStart - (armToHand[2] / defaultArmLength), 0.0f) * fabs(armToHand[1] / defaultArmLength);
zFactor = weights[2] * glm::max(zStart - (armToHand[2] / defaultArmLength), 0.0f) * fabs(armToHand[1] / defaultArmLength);
} else {
initial_valuesZ = zWeightBottom * glm::max(zStart - (armToHand[2] / defaultArmLength), 0.0f) * fabs(armToHand[1] / defaultArmLength);
zFactor = zWeightBottom * glm::max(zStart - (armToHand[2] / defaultArmLength), 0.0f) * fabs(armToHand[1] / defaultArmLength);
}
//1.0f + armToHand[1]/defaultArmLength
float initial_valuesX;
float xFactor;
if (left) {
initial_valuesX = weights[0] * glm::max(-1.0f * (armToHand[0] / defaultArmLength) + xStart, 0.0f);
xFactor = weights[0] * glm::max(-1.0f * (armToHand[0] / defaultArmLength) + xStart, 0.0f);
} else {
initial_valuesX = weights[0] * (((armToHand[0] / defaultArmLength) + xStart) - (0.3f) * ((1.0f + (armToHand[1] / defaultArmLength)) / 2.0f));
xFactor = weights[0] * (((armToHand[0] / defaultArmLength) + xStart) - (0.3f) * ((1.0f + (armToHand[1] / defaultArmLength)) / 2.0f));
}
float theta = initial_valuesX + initial_valuesY + initial_valuesZ;
float theta = xFactor + yFactor + zFactor;
if (theta < 13.0f) {
theta = 13.0f;
@ -1784,117 +1779,93 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
theta *= -1.0f;
}
float halfTheta = theta;
// now we calculate the contribution of the hand
glm::quat relativeHandRotation = (elbowPose.inverse() * handPose).rot();
relativeHandRotation = glm::normalize(relativeHandRotation);
if (relativeHandRotation.w < 0.0f) {
relativeHandRotation.x *= -1.0f;
relativeHandRotation.y *= -1.0f;
relativeHandRotation.z *= -1.0f;
relativeHandRotation.w *= -1.0f;
relativeHandRotation *= -1.0f;
}
glm::quat twist;
glm::quat ulnarDeviation;
//swingTwistDecomposition(twistUlnarSwing, Vectors::UNIT_Z, twist, ulnarDeviation);
swingTwistDecomposition(relativeHandRotation, Vectors::UNIT_Z, twist, ulnarDeviation);
ulnarDeviation = glm::normalize(ulnarDeviation);
glm::quat nonUlnarDeviation;
swingTwistDecomposition(relativeHandRotation, Vectors::UNIT_Z, nonUlnarDeviation, ulnarDeviation);
if (ulnarDeviation.w < 0.0f) {
ulnarDeviation.x *= -1.0f;
ulnarDeviation.y *= -1.0f;
ulnarDeviation.z *= -1.0f;
ulnarDeviation.w *= -1.0f;
ulnarDeviation *= 1.0f;
}
glm::vec3 ulnarAxis = glm::axis(ulnarDeviation);
float ulnarDeviationTheta = glm::sign(ulnarAxis[2]) * glm::angle(ulnarDeviation);
if (left) {
if (glm::sign(ulnarDeviationTheta) != glm::sign(_ulnarRadialThetaRunningAverageLeft) && fabsf(ulnarDeviationTheta) > (5.0f * PI) / 6.0f) {
if (glm::sign(ulnarDeviationTheta) != glm::sign(_ulnarRadialThetaRunningAverageLeft) && fabsf(ulnarDeviationTheta) > (PI / 2.0f)) {
// don't allow the theta to cross the 180 degree limit.
ulnarDeviationTheta = -1.0f * ulnarDeviationTheta;
}
// put some smoothing on the theta
_ulnarRadialThetaRunningAverageLeft = 0.5f * _ulnarRadialThetaRunningAverageLeft + 0.5f * ulnarDeviationTheta;
} else {
if (glm::sign(ulnarDeviationTheta) != glm::sign(_ulnarRadialThetaRunningAverageRight) && fabsf(ulnarDeviationTheta) > (5.0f * PI) / 6.0f) {
if (glm::sign(ulnarDeviationTheta) != glm::sign(_ulnarRadialThetaRunningAverageRight) && fabsf(ulnarDeviationTheta) > (PI / 2.0f)) {
// don't allow the theta to cross the 180 degree limit.
ulnarDeviationTheta = -1.0f * ulnarDeviationTheta;
}
// put some smoothing on the theta
_ulnarRadialThetaRunningAverageRight = 0.5f * _ulnarRadialThetaRunningAverageRight + 0.5f * ulnarDeviationTheta;
}
//get the swingTwist of the hand to lower arm
//get the flex/extension of the wrist rotation
glm::quat flex;
glm::quat twistUlnarSwing;
swingTwistDecomposition(relativeHandRotation, Vectors::UNIT_X, twistUlnarSwing, flex);
flex = glm::normalize(flex);
glm::quat nonFlex;
swingTwistDecomposition(relativeHandRotation, Vectors::UNIT_X, nonFlex, flex);
if (flex.w < 0.0f) {
flex.x *= -1.0f;
flex.y *= -1.0f;
flex.z *= -1.0f;
flex.w *= -1.0f;
flex *= 1.0f;
}
glm::vec3 flexAxis = glm::axis(flex);
//float swingTheta = glm::angle(twistUlnarSwing);
float flexTheta = glm::sign(flexAxis[0]) * glm::angle(flex);
if (left) {
if (glm::sign(flexTheta) != glm::sign(_flexThetaRunningAverageLeft) && fabsf(flexTheta) > (5.0f * PI) / 6.0f) {
if (glm::sign(flexTheta) != glm::sign(_flexThetaRunningAverageLeft) && fabsf(flexTheta) > (PI / 2.0f)) {
// don't allow the theta to cross the 180 degree limit.
flexTheta = -1.0f * flexTheta;
}
// put some smoothing on the theta
_flexThetaRunningAverageLeft = 0.5f * _flexThetaRunningAverageLeft + 0.5f * flexTheta;
} else {
if (glm::sign(flexTheta) != glm::sign(_flexThetaRunningAverageRight) && fabsf(flexTheta) > (5.0f * PI) / 6.0f) {
if (glm::sign(flexTheta) != glm::sign(_flexThetaRunningAverageRight) && fabsf(flexTheta) > (PI / 2.0f)) {
// don't allow the theta to cross the 180 degree limit.
flexTheta = -1.0f * flexTheta;
}
// put some smoothing on the theta
_flexThetaRunningAverageRight = 0.5f * _flexThetaRunningAverageRight + 0.5f * flexTheta;
}
glm::quat trueTwist;
glm::quat twist;
glm::quat nonTwist;
swingTwistDecomposition(relativeHandRotation, Vectors::UNIT_Y, nonTwist, trueTwist);
trueTwist = glm::normalize(trueTwist);
if (trueTwist.w < 0.0f) {
trueTwist.x *= -1.0f;
trueTwist.y *= -1.0f;
trueTwist.z *= -1.0f;
trueTwist.w *= -1.0f;
swingTwistDecomposition(relativeHandRotation, Vectors::UNIT_Y, nonTwist, twist);
if (twist.w < 0.0f) {
twist *= 1.0f;
}
glm::vec3 trueTwistAxis = glm::axis(trueTwist);
float trueTwistTheta;
trueTwistTheta = glm::sign(trueTwistAxis[1]) * glm::angle(trueTwist);
glm::vec3 trueTwistAxis = glm::axis(twist);
float trueTwistTheta = glm::sign(trueTwistAxis[1]) * glm::angle(twist);
if (left) {
if (glm::sign(trueTwistTheta) != glm::sign(_twistThetaRunningAverageLeft) && fabsf(trueTwistTheta) > (5.0f * PI) / 6.0f) {
if (glm::sign(trueTwistTheta) != glm::sign(_twistThetaRunningAverageLeft) && fabsf(trueTwistTheta) > (PI / 2.0f)) {
// don't allow the theta to cross the 180 degree limit.
trueTwistTheta = -1.0f * trueTwistTheta;
}
// put some smoothing on the theta
_twistThetaRunningAverageLeft = 0.5f * _twistThetaRunningAverageLeft + 0.5f * trueTwistTheta;
} else {
if (glm::sign(trueTwistTheta) != glm::sign(_twistThetaRunningAverageRight) && fabsf(trueTwistTheta) > (5.0f * PI) / 6.0f) {
if (glm::sign(trueTwistTheta) != glm::sign(_twistThetaRunningAverageRight) && fabsf(trueTwistTheta) > (PI / 2.0f)) {
// don't allow the theta to cross the 180 degree limit.
trueTwistTheta = -1.0f * trueTwistTheta;
}
// put some smoothing on the theta
_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;
//qCDebug(animation) << "flex: " << (flexTheta / PI) * 180.0f << " twist: " << (trueTwistTheta / PI) * 180.0f << " ulnar deviation: " << (ulnarDeviationTheta / PI) * 180.0f;
}
// make the dead zone PI/6.0
const float POWER = 2.0f;
const float FLEX_BOUNDARY = PI / 4.0f;
const float EXTEND_BOUNDARY = -PI / 5.0f;
const float FLEX_BOUNDARY = PI / 5.0f;
const float EXTEND_BOUNDARY = -PI / 6.0f;
float flexCorrection = 0.0f;
if (left) {
if (_flexThetaRunningAverageLeft > FLEX_BOUNDARY) {
@ -1917,7 +1888,7 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
}
theta -= flexCorrection;
}
//qCDebug(animation) << "flexCorrection rig" << flexCorrection;
qCDebug(animation) << "flexCorrection rig" << flexCorrection;
const float TWIST_ULNAR_DEADZONE = 0.0f;
const float ULNAR_BOUNDARY_MINUS = -PI / 12.0f;
@ -1936,7 +1907,6 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
if (twistCoefficient > 1.0f) {
twistCoefficient = 1.0f;
}
if (left) {
if (trueTwistTheta < 0.0f) {
ulnarCorrection -= glm::sign(ulnarDiff) * (fabs(ulnarDiff) / PI) * 90.0f * twistCoefficient;
@ -1950,7 +1920,6 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
} else {
ulnarCorrection += glm::sign(ulnarDiff) * (fabs(ulnarDiff) / PI) * 90.0f * twistCoefficient;
}
}
if (fabsf(ulnarCorrection) > 20.0f) {
ulnarCorrection = glm::sign(ulnarCorrection) * 20.0f;
@ -1970,7 +1939,6 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
if (twistCoefficient > 1.0f) {
twistCoefficient = 1.0f;
}
if (left) {
if (trueTwistTheta < 0.0f) {
ulnarCorrection -= glm::sign(ulnarDiff) * (fabs(ulnarDiff) / PI) * 90.0f * twistCoefficient;
@ -1984,7 +1952,6 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
} else {
ulnarCorrection += glm::sign(ulnarDiff) * (fabs(ulnarDiff) / PI) * 90.0f * twistCoefficient;
}
}
if (fabsf(ulnarCorrection) > 20.0f) {
ulnarCorrection = glm::sign(ulnarCorrection) * 20.0f;
@ -1992,41 +1959,31 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
theta += ulnarCorrection;
}
}
}
// qCDebug(animation) << "ulnarCorrection rig" << ulnarCorrection;
qCDebug(animation) << "ulnarCorrection rig" << ulnarCorrection;
// remember direction of travel.
const float TWIST_DEADZONE = PI / 2.0f;
const float TWIST_DEADZONE = (4 * PI) / 9.0f;
//if (!isLeft) {
float twistCorrection = 0.0f;
if (left) {
if (_twistThetaRunningAverageLeft < -TWIST_DEADZONE) {
twistCorrection = glm::sign(_twistThetaRunningAverageLeft) * ((fabsf(_twistThetaRunningAverageLeft) - TWIST_DEADZONE) / PI) * 60.0f;
} else {
if (_twistThetaRunningAverageLeft > TWIST_DEADZONE) {
twistCorrection = glm::sign(_twistThetaRunningAverageLeft) * ((fabsf(_twistThetaRunningAverageLeft) - TWIST_DEADZONE) / PI) * 60.0f;
}
}
if (fabsf(_twistThetaRunningAverageLeft) > TWIST_DEADZONE) {
twistCorrection = glm::sign(_twistThetaRunningAverageLeft) * ((fabsf(_twistThetaRunningAverageLeft) - TWIST_DEADZONE) / PI) * 80.0f;
}
} else {
if (_twistThetaRunningAverageRight < -TWIST_DEADZONE) {
twistCorrection = glm::sign(_twistThetaRunningAverageRight) * ((fabsf(_twistThetaRunningAverageRight) - TWIST_DEADZONE) / PI) * 60.0f;
} else {
if (_twistThetaRunningAverageRight > TWIST_DEADZONE) {
twistCorrection = glm::sign(_twistThetaRunningAverageRight) * ((fabsf(_twistThetaRunningAverageRight) - TWIST_DEADZONE) / PI) * 60.0f;
}
}
if (fabsf(_twistThetaRunningAverageRight) > TWIST_DEADZONE) {
twistCorrection = glm::sign(_twistThetaRunningAverageRight) * ((fabsf(_twistThetaRunningAverageRight) - TWIST_DEADZONE) / PI) * 80.0f;
}
}
if (fabsf(twistCorrection) > 30.0f) {
theta += glm::sign(twistCorrection) * 30.0f;
} else {
theta += twistCorrection;
}
//qCDebug(animation) << "twistCorrection rig" << twistCorrection;
qCDebug(animation) << "twistCorrection rig" << twistCorrection;
// put final global limiter here.......
//qCDebug(animation) << "theta in rig " << left << " isLeft " << theta;
//return theta;
if (left) {
_animVars.set("thetaLeft", theta);
} else {
@ -2040,7 +1997,6 @@ bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int s
if (refVectorLength > MIN_LENGTH && sideVectorLength > MIN_LENGTH) {
poleVector = lastDot * (refVectorProj / refVectorProjLength) + glm::sign(theta) * lastSideDot * (sideVector / sideVectorLength);
if (left) {
//qCDebug(animation) << "pole vector in rig " << poleVector;
}
//