mirror of
https://github.com/lubosz/overte.git
synced 2025-04-14 01:46:18 +02:00
moving the theta calculation to rig
This commit is contained in:
amantley
parent
1924018d2c
commit
5c26bbec5c
3 changed files with 410 additions and 8 deletions
|
|
@ -177,8 +177,16 @@ const AnimPoseVec& AnimPoleVectorConstraint::evaluate(const AnimVariantMap& anim
|
|||
_poses = underPoses;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// Look up poleVector from animVars, make sure to convert into geom space.
|
||||
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;
|
||||
}
|
||||
|
||||
// determine if we should interpolate
|
||||
bool enabled = animVars.lookup(_enabledVar, _enabled);
|
||||
|
|
@ -238,11 +246,14 @@ const AnimPoseVec& AnimPoleVectorConstraint::evaluate(const AnimVariantMap& anim
|
|||
glm::vec3 pretendPoleVector;
|
||||
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;
|
||||
}
|
||||
} else {
|
||||
poleVector = glm::vec3(1.0f, 0.0f, 0.0f);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// project poleVector on plane formed by axis.
|
||||
glm::vec3 poleVectorProj = poleVector - glm::dot(poleVector, unitAxis) * unitAxis;
|
||||
float poleVectorProjLength = glm::length(poleVectorProj);
|
||||
|
|
@ -262,7 +273,18 @@ const AnimPoseVec& AnimPoleVectorConstraint::evaluate(const AnimVariantMap& anim
|
|||
if (_skeleton->nameToJointIndex("LeftHand") == _tipJointIndex) {
|
||||
isLeft = true;
|
||||
}
|
||||
fred = findThetaNewWay(tipPose.trans(), basePose.trans(), isLeft);
|
||||
//qCDebug(animation) << "hand pose anim pole vector: " << isLeft << " isLeft " << tipPose;
|
||||
//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;
|
||||
fred = thetaFromRig;
|
||||
|
||||
} else {
|
||||
float thetaFromRig = animVars.lookup("thetaRight", 0.0f);
|
||||
qCDebug(animation) << " anim pole vector theta from rig right" << thetaFromRig;
|
||||
fred = thetaFromRig;
|
||||
}
|
||||
|
||||
glm::quat relativeHandRotation = (midPose.inverse() * tipPose).rot();
|
||||
|
||||
|
|
@ -354,11 +376,11 @@ const AnimPoseVec& AnimPoleVectorConstraint::evaluate(const AnimVariantMap& anim
|
|||
if (!isLeft) {
|
||||
deltaThetaUlnar = correctElbowForHandUlnarRadialDeviation(tipPose, midPose);
|
||||
}
|
||||
|
||||
|
||||
if (isLeft) {
|
||||
fred *= -1.0f;
|
||||
// fred *= -1.0f;
|
||||
}
|
||||
|
||||
|
||||
// make the dead zone PI/6.0
|
||||
|
||||
const float POWER = 2.0f;
|
||||
|
|
@ -386,6 +408,7 @@ const AnimPoseVec& AnimPoleVectorConstraint::evaluate(const AnimVariantMap& anim
|
|||
}
|
||||
fred -= flexCorrection;
|
||||
}
|
||||
//qCDebug(animation) << "flexCorrection anim" << flexCorrection;
|
||||
|
||||
const float TWIST_ULNAR_DEADZONE = 0.0f;
|
||||
const float ULNAR_BOUNDARY_MINUS = -PI / 12.0f;
|
||||
|
|
@ -425,6 +448,7 @@ const AnimPoseVec& AnimPoleVectorConstraint::evaluate(const AnimVariantMap& anim
|
|||
fred += ulnarCorrection;
|
||||
}
|
||||
}
|
||||
//qCDebug(animation) << "ulnarCorrection anim" << ulnarCorrection;
|
||||
|
||||
// remember direction of travel.
|
||||
const float TWIST_DEADZONE = PI / 2.0f;
|
||||
|
|
@ -458,7 +482,7 @@ const AnimPoseVec& AnimPoleVectorConstraint::evaluate(const AnimVariantMap& anim
|
|||
|
||||
float poleVectorTheta = theta;
|
||||
theta = ((180.0f - _lastTheta) / 180.0f)*PI;
|
||||
qCDebug(animation) << "fake theta " << poleVectorTheta << " newly computed theta " << theta << " dot " << dot << " last dot "<< lastDot;
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1465,7 +1465,8 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab
|
|||
int oppositeArmJointIndex = _animSkeleton->nameToJointIndex("RightArm");
|
||||
if (ENABLE_POLE_VECTORS && handJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0 && oppositeArmJointIndex >= 0) {
|
||||
glm::vec3 poleVector;
|
||||
bool usePoleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, oppositeArmJointIndex, poleVector);
|
||||
//bool usePoleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, oppositeArmJointIndex, poleVector);
|
||||
bool usePoleVector = calculateElbowPoleVectorOptimized(handJointIndex, elbowJointIndex, armJointIndex, true, poleVector);
|
||||
if (usePoleVector) {
|
||||
glm::vec3 sensorPoleVector = transformVectorFast(rigToSensorMatrix, poleVector);
|
||||
_animVars.set("leftHandPoleVectorEnabled", true);
|
||||
|
|
@ -1520,7 +1521,8 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab
|
|||
|
||||
if (ENABLE_POLE_VECTORS && handJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0 && oppositeArmJointIndex >= 0) {
|
||||
glm::vec3 poleVector;
|
||||
bool usePoleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, oppositeArmJointIndex, poleVector);
|
||||
//bool usePoleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, oppositeArmJointIndex, poleVector);
|
||||
bool usePoleVector = calculateElbowPoleVectorOptimized(handJointIndex, elbowJointIndex, armJointIndex, false, poleVector);
|
||||
if (usePoleVector) {
|
||||
glm::vec3 sensorPoleVector = transformVectorFast(rigToSensorMatrix, poleVector);
|
||||
_animVars.set("rightHandPoleVectorEnabled", true);
|
||||
|
|
@ -1686,6 +1688,373 @@ void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm
|
|||
}
|
||||
}
|
||||
|
||||
bool Rig::calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int shoulderIndex, bool left, glm::vec3& poleVector) {
|
||||
// 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.
|
||||
// then create weights that make the elbow angle less when the x value is large in either direction.
|
||||
// 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;
|
||||
} else {
|
||||
referenceVector = -Vectors::UNIT_X;
|
||||
}
|
||||
|
||||
AnimPose handPose = _externalPoseSet._absolutePoses[handIndex];
|
||||
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;
|
||||
float refVectorLength = glm::length(refVector);
|
||||
glm::vec3 unitRef = refVector / refVectorLength;
|
||||
|
||||
glm::vec3 axis = shoulderPose.trans() - handPose.trans();
|
||||
float axisLength = glm::length(axis);
|
||||
glm::vec3 unitAxis = axis / axisLength;
|
||||
|
||||
glm::vec3 sideVector = glm::cross(unitAxis, unitRef);
|
||||
float sideVectorLength = glm::length(sideVector);
|
||||
|
||||
// project refVector onto axis plane
|
||||
glm::vec3 refVectorProj = unitRef - glm::dot(unitRef, 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;
|
||||
const float xStart = 0.1f;
|
||||
// biases
|
||||
//const glm::vec3 biases(30.0f, 120.0f, -30.0f);
|
||||
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;
|
||||
if (armToHand[1] > 0.0f) {
|
||||
initial_valuesZ = 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);
|
||||
}
|
||||
|
||||
//1.0f + armToHand[1]/defaultArmLength
|
||||
|
||||
float initial_valuesX;
|
||||
if (left) {
|
||||
initial_valuesX = 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));
|
||||
}
|
||||
|
||||
float theta = initial_valuesX + initial_valuesY + initial_valuesZ;
|
||||
|
||||
if (theta < 13.0f) {
|
||||
theta = 13.0f;
|
||||
}
|
||||
if (theta > 175.0f) {
|
||||
theta = 175.0f;
|
||||
}
|
||||
|
||||
if (left) {
|
||||
theta *= -1.0f;
|
||||
}
|
||||
|
||||
float halfTheta = theta;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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 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) {
|
||||
// don't allow the theta to cross the 180 degree limit.
|
||||
ulnarDeviationTheta = -1.0f * ulnarDeviationTheta;
|
||||
}
|
||||
_ulnarRadialThetaRunningAverageLeft = 0.5f * _ulnarRadialThetaRunningAverageLeft + 0.5f * ulnarDeviationTheta;
|
||||
} else {
|
||||
if (glm::sign(ulnarDeviationTheta) != glm::sign(_ulnarRadialThetaRunningAverageRight) && fabsf(ulnarDeviationTheta) > (5.0f * PI) / 6.0f) {
|
||||
// don't allow the theta to cross the 180 degree limit.
|
||||
ulnarDeviationTheta = -1.0f * ulnarDeviationTheta;
|
||||
}
|
||||
_ulnarRadialThetaRunningAverageRight = 0.5f * _ulnarRadialThetaRunningAverageRight + 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 (left) {
|
||||
if (glm::sign(flexTheta) != glm::sign(_flexThetaRunningAverageLeft) && fabsf(flexTheta) > (5.0f * PI) / 6.0f) {
|
||||
// don't allow the theta to cross the 180 degree limit.
|
||||
flexTheta = -1.0f * flexTheta;
|
||||
}
|
||||
_flexThetaRunningAverageLeft = 0.5f * _flexThetaRunningAverageLeft + 0.5f * flexTheta;
|
||||
} else {
|
||||
if (glm::sign(flexTheta) != glm::sign(_flexThetaRunningAverageRight) && fabsf(flexTheta) > (5.0f * PI) / 6.0f) {
|
||||
// don't allow the theta to cross the 180 degree limit.
|
||||
flexTheta = -1.0f * flexTheta;
|
||||
}
|
||||
_flexThetaRunningAverageRight = 0.5f * _flexThetaRunningAverageRight + 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 (left) {
|
||||
if (glm::sign(trueTwistTheta) != glm::sign(_twistThetaRunningAverageLeft) && fabsf(trueTwistTheta) > (5.0f * PI) / 6.0f) {
|
||||
// don't allow the theta to cross the 180 degree limit.
|
||||
trueTwistTheta = -1.0f * trueTwistTheta;
|
||||
}
|
||||
_twistThetaRunningAverageLeft = 0.5f * _twistThetaRunningAverageLeft + 0.5f * trueTwistTheta;
|
||||
} else {
|
||||
if (glm::sign(trueTwistTheta) != glm::sign(_twistThetaRunningAverageRight) && fabsf(trueTwistTheta) > (5.0f * PI) / 6.0f) {
|
||||
// don't allow the theta to cross the 180 degree limit.
|
||||
trueTwistTheta = -1.0f * 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;
|
||||
//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;
|
||||
float flexCorrection = 0.0f;
|
||||
if (left) {
|
||||
if (_flexThetaRunningAverageLeft > FLEX_BOUNDARY) {
|
||||
flexCorrection = ((_flexThetaRunningAverageLeft - FLEX_BOUNDARY) / PI) * 180.0f;
|
||||
} else if (_flexThetaRunningAverageLeft < EXTEND_BOUNDARY) {
|
||||
flexCorrection = ((_flexThetaRunningAverageLeft - EXTEND_BOUNDARY) / PI) * 180.0f;
|
||||
}
|
||||
if (fabs(flexCorrection) > 30.0f) {
|
||||
flexCorrection = glm::sign(flexCorrection) * 30.0f;
|
||||
}
|
||||
theta += flexCorrection;
|
||||
} else {
|
||||
if (_flexThetaRunningAverageRight > FLEX_BOUNDARY) {
|
||||
flexCorrection = ((_flexThetaRunningAverageRight - FLEX_BOUNDARY) / PI) * 180.0f;
|
||||
} else if (_flexThetaRunningAverageRight < EXTEND_BOUNDARY) {
|
||||
flexCorrection = ((_flexThetaRunningAverageRight - EXTEND_BOUNDARY) / PI) * 180.0f;
|
||||
}
|
||||
if (fabs(flexCorrection) > 30.0f) {
|
||||
flexCorrection = glm::sign(flexCorrection) * 30.0f;
|
||||
}
|
||||
theta -= flexCorrection;
|
||||
}
|
||||
//qCDebug(animation) << "flexCorrection rig" << 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 (left) {
|
||||
if (_ulnarRadialThetaRunningAverageLeft > ULNAR_BOUNDARY_PLUS) {
|
||||
ulnarDiff = _ulnarRadialThetaRunningAverageLeft - ULNAR_BOUNDARY_PLUS;
|
||||
} else if (_ulnarRadialThetaRunningAverageLeft < ULNAR_BOUNDARY_MINUS) {
|
||||
ulnarDiff = _ulnarRadialThetaRunningAverageLeft - ULNAR_BOUNDARY_MINUS;
|
||||
}
|
||||
if (fabs(ulnarDiff) > 0.0f) {
|
||||
if (fabs(_twistThetaRunningAverageLeft) > TWIST_ULNAR_DEADZONE) {
|
||||
float twistCoefficient = (fabs(_twistThetaRunningAverageLeft) / (PI / 20.0f));
|
||||
if (twistCoefficient > 1.0f) {
|
||||
twistCoefficient = 1.0f;
|
||||
}
|
||||
|
||||
if (left) {
|
||||
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;
|
||||
}
|
||||
theta += ulnarCorrection;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
if (_ulnarRadialThetaRunningAverageRight > ULNAR_BOUNDARY_PLUS) {
|
||||
ulnarDiff = _ulnarRadialThetaRunningAverageRight - ULNAR_BOUNDARY_PLUS;
|
||||
} else if (_ulnarRadialThetaRunningAverageRight < ULNAR_BOUNDARY_MINUS) {
|
||||
ulnarDiff = _ulnarRadialThetaRunningAverageRight - ULNAR_BOUNDARY_MINUS;
|
||||
}
|
||||
if (fabs(ulnarDiff) > 0.0f) {
|
||||
if (fabs(_twistThetaRunningAverageRight) > TWIST_ULNAR_DEADZONE) {
|
||||
float twistCoefficient = (fabs(_twistThetaRunningAverageRight) / (PI / 20.0f));
|
||||
if (twistCoefficient > 1.0f) {
|
||||
twistCoefficient = 1.0f;
|
||||
}
|
||||
|
||||
if (left) {
|
||||
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;
|
||||
}
|
||||
theta += ulnarCorrection;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
// qCDebug(animation) << "ulnarCorrection rig" << ulnarCorrection;
|
||||
|
||||
// remember direction of travel.
|
||||
const float TWIST_DEADZONE = PI / 2.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;
|
||||
}
|
||||
}
|
||||
} 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(twistCorrection) > 30.0f) {
|
||||
theta += glm::sign(twistCorrection) * 30.0f;
|
||||
} else {
|
||||
theta += twistCorrection;
|
||||
}
|
||||
//qCDebug(animation) << "twistCorrection rig" << twistCorrection;
|
||||
|
||||
//qCDebug(animation) << "theta in rig " << left << " isLeft " << theta;
|
||||
//return theta;
|
||||
if (left) {
|
||||
_animVars.set("thetaLeft", halfTheta);
|
||||
} else {
|
||||
_animVars.set("thetaRight", halfTheta);
|
||||
}
|
||||
// convert theta back to pole vector
|
||||
float lastDot = cosf(((180.0f - theta) / 180.0f)*PI);
|
||||
float lastSideDot = sqrt(1.0f - (lastDot*lastDot));
|
||||
|
||||
const float MIN_LENGTH = 1.0e-4f;
|
||||
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;
|
||||
}
|
||||
//
|
||||
} else {
|
||||
poleVector = glm::vec3(1.0f, 0.0f, 0.0f);
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
return true;
|
||||
|
||||
|
||||
}
|
||||
|
||||
bool Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int oppositeArmIndex, glm::vec3& poleVector) const {
|
||||
// The resulting Pole Vector is calculated as the sum of a three vectors.
|
||||
// The first is the vector with direction shoulder-hand. The module of this vector is inversely proportional to the strength of the resulting Pole Vector.
|
||||
|
|
|
|||
|
|
@ -258,6 +258,7 @@ protected:
|
|||
void calcAnimAlpha(float speed, const std::vector<float>& referenceSpeeds, float* alphaOut) const;
|
||||
|
||||
bool calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int oppositeArmIndex, glm::vec3& poleVector) const;
|
||||
bool calculateElbowPoleVectorOptimized(int handIndex, int elbowIndex, int shoulderIndex, bool left, glm::vec3& poleVector);
|
||||
glm::vec3 calculateKneePoleVector(int footJointIndex, int kneeJoint, int upLegIndex, int hipsIndex, const AnimPose& targetFootPose) const;
|
||||
glm::vec3 deflectHandFromTorso(const glm::vec3& handPosition, const HFMJointShapeInfo& hipsShapeInfo, const HFMJointShapeInfo& spineShapeInfo,
|
||||
const HFMJointShapeInfo& spine1ShapeInfo, const HFMJointShapeInfo& spine2ShapeInfo) const;
|
||||
|
|
@ -425,6 +426,14 @@ protected:
|
|||
bool _computeNetworkAnimation { false };
|
||||
bool _sendNetworkNode { false };
|
||||
|
||||
float _twistThetaRunningAverageLeft { 0.0f };
|
||||
float _flexThetaRunningAverageLeft { 0.0f };
|
||||
float _ulnarRadialThetaRunningAverageLeft { 0.0f };
|
||||
float _twistThetaRunningAverageRight{ 0.0f };
|
||||
float _flexThetaRunningAverageRight { 0.0f };
|
||||
float _ulnarRadialThetaRunningAverageRight { 0.0f };
|
||||
float _lastTheta { 0.0f };
|
||||
|
||||
AnimContext _lastContext;
|
||||
AnimVariantMap _lastAnimVars;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue