float Motor::getPosition() {
// Returns the position AFTER the gearbox in the range (-pi, pi). Since there is no
// calibraton routine, the output must start within (-pi,pi)/gearRatio of the zero.
// Additionally, this won't work if the output shaft can complete a full revolution.
// E.g. with a 2:1 gear ratio, the output shaft must start in the range (-pi/2, pi/2) of
// your desired output zero.
curPos = fmodf_mpi_pi(getRawPosition()-zero);
// curPos = getRawPosition()-zero*direction;
if (!isnan(prevPosition)) {
// if (curPos - prevPosition < -PI)
// unwrapOffset += 1;
// else if (curPos - prevPosition > PI)
// unwrapOffset -= 1;
// Handle unwrapping around +/- PI, does same thing as lines above, but without branching
if (fabsf(curPos - prevPosition) > PI) {
unwrapOffset += (curPos < prevPosition) - (curPos > prevPosition);
// If unwrapped values apart by too much, revert unwrapOffset and ignore curPos
// if (TWO_PI - fabsf(curPos - prevPosition) > posLimit) {
// unwrapOffset -= (curPos < prevPosition) - (curPos > prevPosition);
// curPos = prevPosition;
// }
}
// Separate posLimit check needed if not near +/- PI
// else if (fabsf(curPos - prevPosition) > posLimit)
// curPos = prevPosition;
}
prevPosition = curPos;
return fmodf_mpi_pi((TWO_PI*unwrapOffset + curPos) * direction / gearRatio);
}
// Called within Model::simulate call, below.
void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
Head* head = _owningAvatar->getHead();
if (_owningAvatar->isMyAvatar()) {
MyAvatar* myAvatar = static_cast<MyAvatar*>(_owningAvatar);
const FBXGeometry& geometry = _geometry->getFBXGeometry();
Rig::HeadParameters headParams;
headParams.enableLean = qApp->getAvatarUpdater()->isHMDMode();
headParams.leanSideways = head->getFinalLeanSideways();
headParams.leanForward = head->getFinalLeanForward();
headParams.torsoTwist = head->getTorsoTwist();
if (qApp->getAvatarUpdater()->isHMDMode()) {
headParams.isInHMD = true;
// get HMD position from sensor space into world space, and back into rig space
glm::mat4 worldHMDMat = myAvatar->getSensorToWorldMatrix() * myAvatar->getHMDSensorMatrix();
glm::mat4 rigToWorld = createMatFromQuatAndPos(getRotation(), getTranslation());
glm::mat4 worldToRig = glm::inverse(rigToWorld);
glm::mat4 rigHMDMat = worldToRig * worldHMDMat;
headParams.rigHeadPosition = extractTranslation(rigHMDMat);
headParams.rigHeadOrientation = extractRotation(rigHMDMat);
headParams.worldHeadOrientation = extractRotation(worldHMDMat);
} else {
headParams.isInHMD = false;
// We don't have a valid localHeadPosition.
headParams.rigHeadOrientation = Quaternions::Y_180 * head->getFinalOrientationInLocalFrame();
headParams.worldHeadOrientation = head->getFinalOrientationInWorldFrame();
}
headParams.leanJointIndex = geometry.leanJointIndex;
headParams.neckJointIndex = geometry.neckJointIndex;
headParams.isTalking = head->getTimeWithoutTalking() <= 1.5f;
_rig->updateFromHeadParameters(headParams, deltaTime);
Rig::HandParameters handParams;
auto leftPalm = myAvatar->getHand()->getCopyOfPalmData(HandData::LeftHand);
if (leftPalm.isValid() && leftPalm.isActive()) {
handParams.isLeftEnabled = true;
handParams.leftPosition = Quaternions::Y_180 * leftPalm.getRawPosition();
handParams.leftOrientation = Quaternions::Y_180 * leftPalm.getRawRotation();
} else {
handParams.isLeftEnabled = false;
}
auto rightPalm = myAvatar->getHand()->getCopyOfPalmData(HandData::RightHand);
if (rightPalm.isValid() && rightPalm.isActive()) {
handParams.isRightEnabled = true;
handParams.rightPosition = Quaternions::Y_180 * rightPalm.getRawPosition();
handParams.rightOrientation = Quaternions::Y_180 * rightPalm.getRawRotation();
} else {
handParams.isRightEnabled = false;
}
_rig->updateFromHandParameters(handParams, deltaTime);
Rig::CharacterControllerState ccState = convertCharacterControllerState(myAvatar->getCharacterController()->getState());
_rig->computeMotionAnimationState(deltaTime, _owningAvatar->getPosition(), _owningAvatar->getVelocity(), _owningAvatar->getOrientation(), ccState);
// evaluate AnimGraph animation and update jointStates.
Model::updateRig(deltaTime, parentTransform);
Rig::EyeParameters eyeParams;
eyeParams.worldHeadOrientation = headParams.worldHeadOrientation;
eyeParams.eyeLookAt = head->getLookAtPosition();
eyeParams.eyeSaccade = head->getSaccade();
eyeParams.modelRotation = getRotation();
eyeParams.modelTranslation = getTranslation();
eyeParams.leftEyeJointIndex = geometry.leftEyeJointIndex;
eyeParams.rightEyeJointIndex = geometry.rightEyeJointIndex;
_rig->updateFromEyeParameters(eyeParams);
} else {
Model::updateRig(deltaTime, parentTransform);
// This is a little more work than we really want.
//
// Other avatars joint, including their eyes, should already be set just like any other joints
// from the wire data. But when looking at me, we want the eyes to use the corrected lookAt.
//
// Thus this should really only be ... else if (_owningAvatar->getHead()->isLookingAtMe()) {...
// However, in the !isLookingAtMe case, the eyes aren't rotating the way they should right now.
// We will revisit that as priorities allow, and particularly after the new rig/animation/joints.
const FBXGeometry& geometry = _geometry->getFBXGeometry();
// If the head is not positioned, updateEyeJoints won't get the math right
glm::quat headOrientation;
_rig->getJointRotation(geometry.headJointIndex, headOrientation);
glm::vec3 eulers = safeEulerAngles(headOrientation);
head->setBasePitch(glm::degrees(-eulers.x));
head->setBaseYaw(glm::degrees(eulers.y));
head->setBaseRoll(glm::degrees(-eulers.z));
Rig::EyeParameters eyeParams;
eyeParams.worldHeadOrientation = head->getFinalOrientationInWorldFrame();
eyeParams.eyeLookAt = head->getCorrectedLookAtPosition();
eyeParams.eyeSaccade = glm::vec3();
eyeParams.modelRotation = getRotation();
eyeParams.modelTranslation = getTranslation();
eyeParams.leftEyeJointIndex = geometry.leftEyeJointIndex;
eyeParams.rightEyeJointIndex = geometry.rightEyeJointIndex;
_rig->updateFromEyeParameters(eyeParams);
}
}
