diff --git a/interface/src/avatar/AvatarActionHold.cpp b/interface/src/avatar/AvatarActionHold.cpp index 5fb9c9a0ee..a1826076fa 100644 --- a/interface/src/avatar/AvatarActionHold.cpp +++ b/interface/src/avatar/AvatarActionHold.cpp @@ -569,7 +569,7 @@ void AvatarActionHold::lateAvatarUpdate(const AnimPose& prePhysicsRoomPose, cons } btTransform worldTrans = rigidBody->getWorldTransform(); - AnimPose worldBodyPose(glm::vec3(1), bulletToGLM(worldTrans.getRotation()), bulletToGLM(worldTrans.getOrigin())); + AnimPose worldBodyPose(1.0f, bulletToGLM(worldTrans.getRotation()), bulletToGLM(worldTrans.getOrigin())); // transform the body transform into sensor space with the prePhysics sensor-to-world matrix. // then transform it back into world uisng the postAvatarUpdate sensor-to-world matrix. diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 63c80ed27a..6fef47da8e 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -2980,7 +2980,7 @@ void MyAvatar::postUpdate(float deltaTime, const render::ScenePointer& scene) { auto animSkeleton = _skeletonModel->getRig().getAnimSkeleton(); // the rig is in the skeletonModel frame - AnimPose xform(glm::vec3(1), _skeletonModel->getRotation(), _skeletonModel->getTranslation()); + AnimPose xform(1.0f, _skeletonModel->getRotation(), _skeletonModel->getTranslation()); if (_enableDebugDrawDefaultPose && animSkeleton) { glm::vec4 gray(0.2f, 0.2f, 0.2f, 0.2f); @@ -3025,7 +3025,7 @@ void MyAvatar::postUpdate(float deltaTime, const render::ScenePointer& scene) { updateHoldActions(_prePhysicsRoomPose, postUpdateRoomPose); if (_enableDebugDrawDetailedCollision) { - AnimPose rigToWorldPose(glm::vec3(1.0f), getWorldOrientation() * Quaternions::Y_180, getWorldPosition()); + AnimPose rigToWorldPose(1.0f, getWorldOrientation() * Quaternions::Y_180, getWorldPosition()); const int NUM_DEBUG_COLORS = 8; const glm::vec4 DEBUG_COLORS[NUM_DEBUG_COLORS] = { glm::vec4(1.0f, 1.0f, 1.0f, 1.0f), @@ -4821,7 +4821,7 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat swingTwistDecomposition(hipsinWorldSpace, avatarUpWorld, resultingSwingInWorld, resultingTwistInWorld); // remove scale present from sensorToWorldMatrix - followWorldPose.scale() = glm::vec3(1.0f); + followWorldPose.scale() = 1.0f; if (isActive(Rotation)) { //use the hmd reading for the hips follow diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index 26d69841d0..253cc891ee 100755 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -41,7 +41,7 @@ static AnimPose computeHipsInSensorFrame(MyAvatar* myAvatar, bool isFlying) { if (myAvatar->isJointPinned(hipsIndex)) { Transform avatarTransform = myAvatar->getTransform(); AnimPose result = AnimPose(worldToSensorMat * avatarTransform.getMatrix() * Matrices::Y_180); - result.scale() = glm::vec3(1.0f, 1.0f, 1.0f); + result.scale() = 1.0f; return result; } @@ -108,7 +108,7 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { Rig::ControllerParameters params; - AnimPose avatarToRigPose(glm::vec3(1.0f), Quaternions::Y_180, glm::vec3(0.0f)); + AnimPose avatarToRigPose(1.0f, Quaternions::Y_180, glm::vec3(0.0f)); glm::mat4 rigToAvatarMatrix = Matrices::Y_180; glm::mat4 avatarToWorldMatrix = createMatFromQuatAndPos(myAvatar->getWorldOrientation(), myAvatar->getWorldPosition()); @@ -127,7 +127,7 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { // preMult 180 is necessary to convert from avatar to rig coordinates. // postMult 180 is necessary to convert head from -z forward to z forward. glm::quat headRot = Quaternions::Y_180 * head->getFinalOrientationInLocalFrame() * Quaternions::Y_180; - params.primaryControllerPoses[Rig::PrimaryControllerType_Head] = AnimPose(glm::vec3(1.0f), headRot, glm::vec3(0.0f)); + params.primaryControllerPoses[Rig::PrimaryControllerType_Head] = AnimPose(1.0f, headRot, glm::vec3(0.0f)); params.primaryControllerFlags[Rig::PrimaryControllerType_Head] = 0; } diff --git a/libraries/animation/src/AnimClip.cpp b/libraries/animation/src/AnimClip.cpp index 1adc04ee1b..71b876ff8c 100644 --- a/libraries/animation/src/AnimClip.cpp +++ b/libraries/animation/src/AnimClip.cpp @@ -140,10 +140,10 @@ void AnimClip::copyFromNetworkAnim() { postRot = animSkeleton.getPostRotationPose(animJoint); // cancel out scale - preRot.scale() = glm::vec3(1.0f); - postRot.scale() = glm::vec3(1.0f); + preRot.scale() = 1.0f; + postRot.scale() = 1.0f; - AnimPose rot(glm::vec3(1.0f), hfmAnimRot, glm::vec3()); + AnimPose rot(1.0f, hfmAnimRot, glm::vec3()); // adjust translation offsets, so large translation animatons on the reference skeleton // will be adjusted when played on a skeleton with short limbs. @@ -155,7 +155,7 @@ void AnimClip::copyFromNetworkAnim() { boneLengthScale = glm::length(relDefaultPose.trans()) / glm::length(hfmZeroTrans); } - AnimPose trans = AnimPose(glm::vec3(1.0f), glm::quat(), relDefaultPose.trans() + boneLengthScale * (hfmAnimTrans - hfmZeroTrans)); + AnimPose trans = AnimPose(1.0f, glm::quat(), relDefaultPose.trans() + boneLengthScale * (hfmAnimTrans - hfmZeroTrans)); _anim[frame][skeletonJoint] = trans * preRot * rot * postRot; } diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index a1809f3438..7af9e81889 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -552,7 +552,7 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const AnimPose accum = absolutePoses[_hipsIndex]; AnimPose baseParentPose = absolutePoses[_hipsIndex]; for (int i = (int)chainDepth - 1; i >= 0; i--) { - accum = accum * AnimPose(glm::vec3(1.0f), jointChainInfoOut.jointInfoVec[i].rot, jointChainInfoOut.jointInfoVec[i].trans); + accum = accum * AnimPose(1.0f, jointChainInfoOut.jointInfoVec[i].rot, jointChainInfoOut.jointInfoVec[i].trans); postAbsPoses[i] = accum; if (jointChainInfoOut.jointInfoVec[i].jointIndex == topJointIndex) { topChainIndex = i; @@ -734,7 +734,7 @@ void AnimInverseKinematics::computeAndCacheSplineJointInfosForIKTarget(const Ani glm::mat3 m(u, v, glm::cross(u, v)); glm::quat rot = glm::normalize(glm::quat_cast(m)); - AnimPose pose(glm::vec3(1.0f), rot, spline(t)); + AnimPose pose(1.0f, rot, spline(t)); AnimPose offsetPose = pose.inverse() * defaultPose; SplineJointInfo splineJointInfo = { index, ratio, offsetPose }; @@ -767,7 +767,7 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co const int baseIndex = _hipsIndex; // build spline from tip to base - AnimPose tipPose = AnimPose(glm::vec3(1.0f), target.getRotation(), target.getTranslation()); + AnimPose tipPose = AnimPose(1.0f, target.getRotation(), target.getTranslation()); AnimPose basePose = absolutePoses[baseIndex]; CubicHermiteSplineFunctorWithArcLength spline; if (target.getIndex() == _headIndex) { @@ -815,7 +815,7 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co glm::mat3 m(u, v, glm::cross(u, v)); glm::quat rot = glm::normalize(glm::quat_cast(m)); - AnimPose desiredAbsPose = AnimPose(glm::vec3(1.0f), rot, trans) * splineJointInfo.offsetPose; + AnimPose desiredAbsPose = AnimPose(1.0f, rot, trans) * splineJointInfo.offsetPose; // apply flex coefficent AnimPose flexedAbsPose; @@ -965,7 +965,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars } _relativePoses[_hipsIndex] = parentAbsPose.inverse() * absPose; - _relativePoses[_hipsIndex].scale() = glm::vec3(1.0f); + _relativePoses[_hipsIndex].scale() = 1.0f; } // if there is an active jointChainInfo for the hips store the post shifted hips into it. @@ -1753,7 +1753,7 @@ void AnimInverseKinematics::setSecondaryTargets(const AnimContext& context) { AnimPose rigToGeometryPose = AnimPose(glm::inverse(context.getGeometryToRigMatrix())); for (auto& iter : _secondaryTargetsInRigFrame) { AnimPose absPose = rigToGeometryPose * iter.second; - absPose.scale() = glm::vec3(1.0f); + absPose.scale() = 1.0f; AnimPose parentAbsPose; int parentIndex = _skeleton->getParentIndex(iter.first); @@ -1825,7 +1825,7 @@ void AnimInverseKinematics::debugDrawSpineSplines(const AnimContext& context, co const int baseIndex = _hipsIndex; // build spline - AnimPose tipPose = AnimPose(glm::vec3(1.0f), target.getRotation(), target.getTranslation()); + AnimPose tipPose = AnimPose(1.0f, target.getRotation(), target.getTranslation()); AnimPose basePose = _skeleton->getAbsolutePose(baseIndex, _relativePoses); CubicHermiteSplineFunctorWithArcLength spline; diff --git a/libraries/animation/src/AnimManipulator.cpp b/libraries/animation/src/AnimManipulator.cpp index 1146cbb19a..c75c9865bb 100644 --- a/libraries/animation/src/AnimManipulator.cpp +++ b/libraries/animation/src/AnimManipulator.cpp @@ -172,5 +172,5 @@ AnimPose AnimManipulator::computeRelativePoseFromJointVar(const AnimVariantMap& break; } - return AnimPose(glm::vec3(1), relRot, relTrans); + return AnimPose(1.0f, relRot, relTrans); } diff --git a/libraries/animation/src/AnimPoleVectorConstraint.cpp b/libraries/animation/src/AnimPoleVectorConstraint.cpp index f017fe2348..96e4e67261 100644 --- a/libraries/animation/src/AnimPoleVectorConstraint.cpp +++ b/libraries/animation/src/AnimPoleVectorConstraint.cpp @@ -95,7 +95,7 @@ const AnimPoseVec& AnimPoleVectorConstraint::evaluate(const AnimVariantMap& anim AnimPose tipPose = ikChain.getAbsolutePoseFromJointIndex(_tipJointIndex); // Look up refVector from animVars, make sure to convert into geom space. - glm::vec3 refVector = midPose.xformVectorFast(_referenceVector); + glm::vec3 refVector = midPose.xformVector(_referenceVector); float refVectorLength = glm::length(refVector); glm::vec3 axis = basePose.trans() - tipPose.trans(); diff --git a/libraries/animation/src/AnimPose.cpp b/libraries/animation/src/AnimPose.cpp index d77514e691..366d863c3d 100644 --- a/libraries/animation/src/AnimPose.cpp +++ b/libraries/animation/src/AnimPose.cpp @@ -14,15 +14,14 @@ #include #include "AnimUtil.h" -const AnimPose AnimPose::identity = AnimPose(glm::vec3(1.0f), - glm::quat(), - glm::vec3(0.0f)); +const AnimPose AnimPose::identity = AnimPose(1.0f, glm::quat(), glm::vec3(0.0f)); AnimPose::AnimPose(const glm::mat4& mat) { static const float EPSILON = 0.0001f; - _scale = extractScale(mat); + glm::vec3 scale = extractScale(mat); // quat_cast doesn't work so well with scaled matrices, so cancel it out. - glm::mat4 tmp = glm::scale(mat, 1.0f / _scale); + glm::mat4 tmp = glm::scale(mat, 1.0f / scale); + _scale = extractUniformScale(scale); _rot = glm::quat_cast(tmp); float lengthSquared = glm::length2(_rot); if (glm::abs(lengthSquared - 1.0f) > EPSILON) { @@ -40,29 +39,22 @@ glm::vec3 AnimPose::xformPoint(const glm::vec3& rhs) const { return *this * rhs; } -// really slow, but accurate for transforms with non-uniform scale glm::vec3 AnimPose::xformVector(const glm::vec3& rhs) const { - glm::vec3 xAxis = _rot * glm::vec3(_scale.x, 0.0f, 0.0f); - glm::vec3 yAxis = _rot * glm::vec3(0.0f, _scale.y, 0.0f); - glm::vec3 zAxis = _rot * glm::vec3(0.0f, 0.0f, _scale.z); - glm::mat3 mat(xAxis, yAxis, zAxis); - glm::mat3 transInvMat = glm::inverse(glm::transpose(mat)); - return transInvMat * rhs; -} - -// faster, but does not handle non-uniform scale correctly. -glm::vec3 AnimPose::xformVectorFast(const glm::vec3& rhs) const { return _rot * (_scale * rhs); } AnimPose AnimPose::operator*(const AnimPose& rhs) const { - glm::mat4 result; - glm_mat4u_mul(*this, rhs, result); - return AnimPose(result); + float scale = _scale * rhs._scale; + glm::quat rot = _rot * rhs._rot; + glm::vec3 trans = _trans + (_rot * (_scale * rhs._trans)); + return AnimPose(scale, rot, trans); } AnimPose AnimPose::inverse() const { - return AnimPose(glm::inverse(static_cast(*this))); + float invScale = 1.0f / _scale; + glm::quat invRot = glm::inverse(_rot); + glm::vec3 invTrans = invScale * (invRot * -_trans); + return AnimPose(invScale, invRot, invTrans); } // mirror about x-axis without applying negative scale. @@ -71,11 +63,10 @@ AnimPose AnimPose::mirror() const { } AnimPose::operator glm::mat4() const { - glm::vec3 xAxis = _rot * glm::vec3(_scale.x, 0.0f, 0.0f); - glm::vec3 yAxis = _rot * glm::vec3(0.0f, _scale.y, 0.0f); - glm::vec3 zAxis = _rot * glm::vec3(0.0f, 0.0f, _scale.z); - return glm::mat4(glm::vec4(xAxis, 0.0f), glm::vec4(yAxis, 0.0f), - glm::vec4(zAxis, 0.0f), glm::vec4(_trans, 1.0f)); + glm::vec3 xAxis = _rot * glm::vec3(_scale, 0.0f, 0.0f); + glm::vec3 yAxis = _rot * glm::vec3(0.0f, _scale, 0.0f); + glm::vec3 zAxis = _rot * glm::vec3(0.0f, 0.0f, _scale); + return glm::mat4(glm::vec4(xAxis, 0.0f), glm::vec4(yAxis, 0.0f), glm::vec4(zAxis, 0.0f), glm::vec4(_trans, 1.0f)); } void AnimPose::blend(const AnimPose& srcPose, float alpha) { diff --git a/libraries/animation/src/AnimPose.h b/libraries/animation/src/AnimPose.h index 1558a6b881..4d6dee1987 100644 --- a/libraries/animation/src/AnimPose.h +++ b/libraries/animation/src/AnimPose.h @@ -21,14 +21,13 @@ class AnimPose { public: AnimPose() {} explicit AnimPose(const glm::mat4& mat); - explicit AnimPose(const glm::quat& rotIn) : _scale(1.0f), _rot(rotIn), _trans(0.0f) {} - AnimPose(const glm::quat& rotIn, const glm::vec3& transIn) : _scale(1.0f), _rot(rotIn), _trans(transIn) {} - AnimPose(const glm::vec3& scaleIn, const glm::quat& rotIn, const glm::vec3& transIn) : _scale(scaleIn), _rot(rotIn), _trans(transIn) {} + explicit AnimPose(const glm::quat& rotIn) : _rot(rotIn), _trans(0.0f), _scale(1.0f) {} + AnimPose(const glm::quat& rotIn, const glm::vec3& transIn) : _rot(rotIn), _trans(transIn), _scale(1.0f) {} + AnimPose(float scaleIn, const glm::quat& rotIn, const glm::vec3& transIn) : _rot(rotIn), _trans(transIn), _scale(scaleIn) {} static const AnimPose identity; glm::vec3 xformPoint(const glm::vec3& rhs) const; glm::vec3 xformVector(const glm::vec3& rhs) const; // really slow, but accurate for transforms with non-uniform scale - glm::vec3 xformVectorFast(const glm::vec3& rhs) const; // faster, but does not handle non-uniform scale correctly. glm::vec3 operator*(const glm::vec3& rhs) const; // same as xformPoint AnimPose operator*(const AnimPose& rhs) const; @@ -37,8 +36,8 @@ public: AnimPose mirror() const; operator glm::mat4() const; - const glm::vec3& scale() const { return _scale; } - glm::vec3& scale() { return _scale; } + float scale() const { return _scale; } + float& scale() { return _scale; } const glm::quat& rot() const { return _rot; } glm::quat& rot() { return _rot; } @@ -50,13 +49,13 @@ public: private: friend QDebug operator<<(QDebug debug, const AnimPose& pose); - glm::vec3 _scale { 1.0f }; glm::quat _rot; glm::vec3 _trans; + float _scale { 1.0f }; // uniform scale only. }; inline QDebug operator<<(QDebug debug, const AnimPose& pose) { - debug << "AnimPose, trans = (" << pose.trans().x << pose.trans().y << pose.trans().z << "), rot = (" << pose.rot().x << pose.rot().y << pose.rot().z << pose.rot().w << "), scale = (" << pose.scale().x << pose.scale().y << pose.scale().z << ")"; + debug << "AnimPose, trans = (" << pose.trans().x << pose.trans().y << pose.trans().z << "), rot = (" << pose.rot().x << pose.rot().y << pose.rot().z << pose.rot().w << "), scale =" << pose.scale(); return debug; } diff --git a/libraries/animation/src/AnimSkeleton.cpp b/libraries/animation/src/AnimSkeleton.cpp index cc48308f17..844ad5aef4 100644 --- a/libraries/animation/src/AnimSkeleton.cpp +++ b/libraries/animation/src/AnimSkeleton.cpp @@ -76,7 +76,7 @@ int AnimSkeleton::getChainDepth(int jointIndex) const { int index = jointIndex; do { chainDepth++; - index = _joints[index].parentIndex; + index = _parentIndices[index]; } while (index != -1); return chainDepth; } else { @@ -102,17 +102,12 @@ const AnimPose& AnimSkeleton::getPostRotationPose(int jointIndex) const { return _relativePostRotationPoses[jointIndex]; } -int AnimSkeleton::getParentIndex(int jointIndex) const { - return _joints[jointIndex].parentIndex; -} - std::vector AnimSkeleton::getChildrenOfJoint(int jointIndex) const { // Children and grandchildren, etc. std::vector result; if (jointIndex != -1) { - for (int i = jointIndex + 1; i < (int)_joints.size(); i++) { - if (_joints[i].parentIndex == jointIndex - || (std::find(result.begin(), result.end(), _joints[i].parentIndex) != result.end())) { + for (int i = jointIndex + 1; i < (int)_parentIndices.size(); i++) { + if (_parentIndices[i] == jointIndex || (std::find(result.begin(), result.end(), _parentIndices[i]) != result.end())) { result.push_back(i); } } @@ -128,7 +123,7 @@ AnimPose AnimSkeleton::getAbsolutePose(int jointIndex, const AnimPoseVec& relati if (jointIndex < 0 || jointIndex >= (int)relativePoses.size() || jointIndex >= _jointsSize) { return AnimPose::identity; } else { - return getAbsolutePose(_joints[jointIndex].parentIndex, relativePoses) * relativePoses[jointIndex]; + return getAbsolutePose(_parentIndices[jointIndex], relativePoses) * relativePoses[jointIndex]; } } @@ -136,7 +131,7 @@ void AnimSkeleton::convertRelativePosesToAbsolute(AnimPoseVec& poses) const { // poses start off relative and leave in absolute frame int lastIndex = std::min((int)poses.size(), _jointsSize); for (int i = 0; i < lastIndex; ++i) { - int parentIndex = _joints[i].parentIndex; + int parentIndex = _parentIndices[i]; if (parentIndex != -1) { poses[i] = poses[parentIndex] * poses[i]; } @@ -147,7 +142,7 @@ void AnimSkeleton::convertAbsolutePosesToRelative(AnimPoseVec& poses) const { // poses start off absolute and leave in relative frame int lastIndex = std::min((int)poses.size(), _jointsSize); for (int i = lastIndex - 1; i >= 0; --i) { - int parentIndex = _joints[i].parentIndex; + int parentIndex = _parentIndices[i]; if (parentIndex != -1) { poses[i] = poses[parentIndex].inverse() * poses[i]; } @@ -158,7 +153,7 @@ void AnimSkeleton::convertAbsoluteRotationsToRelative(std::vector& ro // poses start off absolute and leave in relative frame int lastIndex = std::min((int)rotations.size(), _jointsSize); for (int i = lastIndex - 1; i >= 0; --i) { - int parentIndex = _joints[i].parentIndex; + int parentIndex = _parentIndices[i]; if (parentIndex != -1) { rotations[i] = glm::inverse(rotations[parentIndex]) * rotations[i]; } @@ -197,6 +192,14 @@ void AnimSkeleton::mirrorAbsolutePoses(AnimPoseVec& poses) const { void AnimSkeleton::buildSkeletonFromJoints(const std::vector& joints, const QMap jointOffsets) { _joints = joints; + + // build a seperate vector of parentIndices for cache coherency + // AnimSkeleton::getParentIndex is called very frequently in tight loops. + _parentIndices.reserve(_joints.size()); + for (auto& joint : _joints) { + _parentIndices.push_back(joint.parentIndex); + } + _jointsSize = (int)joints.size(); // build a cache of bind poses diff --git a/libraries/animation/src/AnimSkeleton.h b/libraries/animation/src/AnimSkeleton.h index 14f39eedbc..0eefbf973e 100644 --- a/libraries/animation/src/AnimSkeleton.h +++ b/libraries/animation/src/AnimSkeleton.h @@ -43,7 +43,10 @@ public: // get post transform which might include FBX offset transformations const AnimPose& getPostRotationPose(int jointIndex) const; - int getParentIndex(int jointIndex) const; + int getParentIndex(int jointIndex) const { + return _parentIndices[jointIndex]; + } + std::vector getChildrenOfJoint(int jointIndex) const; AnimPose getAbsolutePose(int jointIndex, const AnimPoseVec& relativePoses) const; @@ -69,6 +72,7 @@ protected: void buildSkeletonFromJoints(const std::vector& joints, const QMap jointOffsets); std::vector _joints; + std::vector _parentIndices; int _jointsSize { 0 }; AnimPoseVec _relativeDefaultPoses; AnimPoseVec _absoluteDefaultPoses; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 7842ec0804..d09de36a14 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -39,14 +39,13 @@ static int nextRigId = 1; static std::map rigRegistry; static std::mutex rigRegistryMutex; +static bool isEqual(const float p, float q) { + const float EPSILON = 0.00001f; + return fabsf(p - q) <= EPSILON; +} + static bool isEqual(const glm::vec3& u, const glm::vec3& v) { - const float EPSILON = 0.0001f; - float uLen = glm::length(u); - if (uLen == 0.0f) { - return glm::length(v) <= EPSILON; - } else { - return (glm::length(u - v) / uLen) <= EPSILON; - } + return isEqual(u.x, v.x) && isEqual(u.y, v.y) && isEqual(u.z, v.z); } static bool isEqual(const glm::quat& p, const glm::quat& q) { @@ -494,10 +493,8 @@ std::shared_ptr Rig::getAnimInverseKinematicsNode() const std::shared_ptr result; if (_animNode) { _animNode->traverse([&](AnimNode::Pointer node) { - // only report clip nodes as valid roles. - auto ikNode = std::dynamic_pointer_cast(node); - if (ikNode) { - result = ikNode; + if (node->getType() == AnimNodeType::InverseKinematics) { + result = std::dynamic_pointer_cast(node); return false; } else { return true; @@ -1329,7 +1326,7 @@ static bool findPointKDopDisplacement(const glm::vec3& point, const AnimPose& sh } if (slabCount == (DOP14_COUNT / 2) && minDisplacementLen != FLT_MAX) { // we are within the k-dop so push the point along the minimum displacement found - displacementOut = shapePose.xformVectorFast(minDisplacement); + displacementOut = shapePose.xformVector(minDisplacement); return true; } else { // point is outside of kdop @@ -1338,7 +1335,7 @@ static bool findPointKDopDisplacement(const glm::vec3& point, const AnimPose& sh } else { // point is directly on top of shapeInfo.avgPoint. // push the point out along the x axis. - displacementOut = shapePose.xformVectorFast(shapeInfo.points[0]); + displacementOut = shapePose.xformVector(shapeInfo.points[0]); return true; } } diff --git a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp index b8626c813e..f765208940 100644 --- a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp +++ b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp @@ -1975,12 +1975,12 @@ float Avatar::getUnscaledEyeHeightFromSkeleton() const { auto& rig = _skeletonModel->getRig(); // Normally the model offset transform will contain the avatar scale factor, we explicitly remove it here. - AnimPose modelOffsetWithoutAvatarScale(glm::vec3(1.0f), rig.getModelOffsetPose().rot(), rig.getModelOffsetPose().trans()); + AnimPose modelOffsetWithoutAvatarScale(1.0f, rig.getModelOffsetPose().rot(), rig.getModelOffsetPose().trans()); AnimPose geomToRigWithoutAvatarScale = modelOffsetWithoutAvatarScale * rig.getGeometryOffsetPose(); // This factor can be used to scale distances in the geometry frame into the unscaled rig frame. // Typically it will be the unit conversion from cm to m. - float scaleFactor = geomToRigWithoutAvatarScale.scale().x; // in practice this always a uniform scale factor. + float scaleFactor = geomToRigWithoutAvatarScale.scale(); int headTopJoint = rig.indexOfJoint("HeadTop_End"); int headJoint = rig.indexOfJoint("Head"); diff --git a/libraries/render-utils/src/AnimDebugDraw.cpp b/libraries/render-utils/src/AnimDebugDraw.cpp index bf528ee5f0..8944ae7996 100644 --- a/libraries/render-utils/src/AnimDebugDraw.cpp +++ b/libraries/render-utils/src/AnimDebugDraw.cpp @@ -374,7 +374,7 @@ void AnimDebugDraw::update() { glm::vec4 color = std::get<3>(iter.second); for (int i = 0; i < skeleton->getNumJoints(); i++) { - const float radius = BONE_RADIUS / (absPoses[i].scale().x * rootPose.scale().x); + const float radius = BONE_RADIUS / (absPoses[i].scale() * rootPose.scale()); // draw bone addBone(rootPose, absPoses[i], radius, color, v); @@ -394,16 +394,16 @@ void AnimDebugDraw::update() { glm::vec3 pos = std::get<1>(iter.second); glm::vec4 color = std::get<2>(iter.second); const float radius = POSE_RADIUS; - addBone(AnimPose::identity, AnimPose(glm::vec3(1), rot, pos), radius, color, v); + addBone(AnimPose::identity, AnimPose(1.0f, rot, pos), radius, color, v); } - AnimPose myAvatarPose(glm::vec3(1), DebugDraw::getInstance().getMyAvatarRot(), DebugDraw::getInstance().getMyAvatarPos()); + AnimPose myAvatarPose(1.0f, DebugDraw::getInstance().getMyAvatarRot(), DebugDraw::getInstance().getMyAvatarPos()); for (auto& iter : myAvatarMarkerMap) { glm::quat rot = std::get<0>(iter.second); glm::vec3 pos = std::get<1>(iter.second); glm::vec4 color = std::get<2>(iter.second); const float radius = POSE_RADIUS; - addBone(myAvatarPose, AnimPose(glm::vec3(1), rot, pos), radius, color, v); + addBone(myAvatarPose, AnimPose(1.0f, rot, pos), radius, color, v); } // draw rays from shared DebugDraw singleton diff --git a/libraries/render-utils/src/CauterizedModel.cpp b/libraries/render-utils/src/CauterizedModel.cpp index 81a81c5602..b70925201a 100644 --- a/libraries/render-utils/src/CauterizedModel.cpp +++ b/libraries/render-utils/src/CauterizedModel.cpp @@ -122,7 +122,7 @@ void CauterizedModel::updateClusterMatrices() { if (_useDualQuaternionSkinning) { auto jointPose = _rig.getJointPose(cluster.jointIndex); - Transform jointTransform(jointPose.rot(), jointPose.scale(), jointPose.trans()); + Transform jointTransform(jointPose.rot(), glm::vec3(jointPose.scale()), jointPose.trans()); Transform clusterTransform; Transform::mult(clusterTransform, jointTransform, _rig.getAnimSkeleton()->getClusterBindMatricesOriginalValues(meshIndex, clusterIndex).inverseBindTransform); state.clusterDualQuaternions[j] = Model::TransformDualQuaternion(clusterTransform); @@ -138,7 +138,7 @@ void CauterizedModel::updateClusterMatrices() { if (!_cauterizeBoneSet.empty()) { AnimPose cauterizePose = _rig.getJointPose(_rig.indexOfJoint("Neck")); - cauterizePose.scale() = glm::vec3(0.0001f, 0.0001f, 0.0001f); + cauterizePose.scale() = 0.0001f; static const glm::mat4 zeroScale( glm::vec4(0.0001f, 0.0f, 0.0f, 0.0f), @@ -161,7 +161,7 @@ void CauterizedModel::updateClusterMatrices() { // not cauterized so just copy the value from the non-cauterized version. state.clusterDualQuaternions[j] = _meshStates[i].clusterDualQuaternions[j]; } else { - Transform jointTransform(cauterizePose.rot(), cauterizePose.scale(), cauterizePose.trans()); + Transform jointTransform(cauterizePose.rot(), glm::vec3(cauterizePose.scale()), cauterizePose.trans()); Transform clusterTransform; Transform::mult(clusterTransform, jointTransform, _rig.getAnimSkeleton()->getClusterBindMatricesOriginalValues(meshIndex, clusterIndex).inverseBindTransform); state.clusterDualQuaternions[j] = Model::TransformDualQuaternion(clusterTransform); diff --git a/libraries/render-utils/src/Model.cpp b/libraries/render-utils/src/Model.cpp index da8dceb176..c68c9259e5 100644 --- a/libraries/render-utils/src/Model.cpp +++ b/libraries/render-utils/src/Model.cpp @@ -1368,7 +1368,7 @@ void Model::updateClusterMatrices() { if (_useDualQuaternionSkinning) { auto jointPose = _rig.getJointPose(cluster.jointIndex); - Transform jointTransform(jointPose.rot(), jointPose.scale(), jointPose.trans()); + Transform jointTransform(jointPose.rot(), glm::vec3(jointPose.scale()), jointPose.trans()); Transform clusterTransform; Transform::mult(clusterTransform, jointTransform, _rig.getAnimSkeleton()->getClusterBindMatricesOriginalValues(meshIndex, clusterIndex).inverseBindTransform); state.clusterDualQuaternions[j] = Model::TransformDualQuaternion(clusterTransform); diff --git a/libraries/render-utils/src/Model.h b/libraries/render-utils/src/Model.h index 16e08c2b23..cdd10a560a 100644 --- a/libraries/render-utils/src/Model.h +++ b/libraries/render-utils/src/Model.h @@ -298,9 +298,9 @@ public: TransformDualQuaternion() {} TransformDualQuaternion(const glm::mat4& m) { AnimPose p(m); - _scale.x = p.scale().x; - _scale.y = p.scale().y; - _scale.z = p.scale().z; + _scale.x = p.scale(); + _scale.y = p.scale(); + _scale.z = p.scale(); _scale.w = 0.0f; _dq = DualQuaternion(p.rot(), p.trans()); } diff --git a/libraries/shared/src/CubicHermiteSpline.h b/libraries/shared/src/CubicHermiteSpline.h index da2ed26de4..cdbc64308d 100644 --- a/libraries/shared/src/CubicHermiteSpline.h +++ b/libraries/shared/src/CubicHermiteSpline.h @@ -60,7 +60,7 @@ protected: class CubicHermiteSplineFunctorWithArcLength : public CubicHermiteSplineFunctor { public: - enum Constants { NUM_SUBDIVISIONS = 30 }; + enum Constants { NUM_SUBDIVISIONS = 15 }; CubicHermiteSplineFunctorWithArcLength() : CubicHermiteSplineFunctor() { memset(_values, 0, sizeof(float) * (NUM_SUBDIVISIONS + 1)); @@ -71,11 +71,13 @@ public: float alpha = 0.0f; float accum = 0.0f; _values[0] = 0.0f; + glm::vec3 prevValue = this->operator()(alpha); for (int i = 1; i < NUM_SUBDIVISIONS + 1; i++) { - accum += glm::distance(this->operator()(alpha), - this->operator()(alpha + DELTA)); + glm::vec3 nextValue = this->operator()(alpha + DELTA); + accum += glm::distance(prevValue, nextValue); alpha += DELTA; _values[i] = accum; + prevValue = nextValue; } } diff --git a/tests/animation/src/AnimTests.cpp b/tests/animation/src/AnimTests.cpp index 0cd9571e22..b1926efb71 100644 --- a/tests/animation/src/AnimTests.cpp +++ b/tests/animation/src/AnimTests.cpp @@ -22,9 +22,11 @@ #include #include #include +#include QTEST_MAIN(AnimTests) + const float TEST_EPSILON = 0.001f; void AnimTests::initTestCase() { @@ -372,16 +374,10 @@ void AnimTests::testAnimPose() { const glm::quat ROT_Y_180 = glm::angleAxis(PI, glm::vec3(0.0f, 1.0, 0.0f)); const glm::quat ROT_Z_30 = glm::angleAxis(PI / 6.0f, glm::vec3(1.0f, 0.0f, 0.0f)); - std::vector scaleVec = { - glm::vec3(1), - glm::vec3(2.0f, 1.0f, 1.0f), - glm::vec3(1.0f, 0.5f, 1.0f), - glm::vec3(1.0f, 1.0f, 1.5f), - glm::vec3(2.0f, 0.5f, 1.5f), - glm::vec3(-2.0f, 0.5f, 1.5f), - glm::vec3(2.0f, -0.5f, 1.5f), - glm::vec3(2.0f, 0.5f, -1.5f), - glm::vec3(-2.0f, -0.5f, -1.5f), + std::vector scaleVec = { + 1.0f, + 2.0f, + 0.5f }; std::vector rotVec = { @@ -411,7 +407,7 @@ void AnimTests::testAnimPose() { for (auto& trans : transVec) { // build a matrix the old fashioned way. - glm::mat4 scaleMat = glm::scale(glm::mat4(), scale); + glm::mat4 scaleMat = glm::scale(glm::mat4(), glm::vec3(scale)); glm::mat4 rotTransMat = createMatFromQuatAndPos(rot, trans); glm::mat4 rawMat = rotTransMat * scaleMat; @@ -429,7 +425,7 @@ void AnimTests::testAnimPose() { for (auto& trans : transVec) { // build a matrix the old fashioned way. - glm::mat4 scaleMat = glm::scale(glm::mat4(), scale); + glm::mat4 scaleMat = glm::scale(glm::mat4(), glm::vec3(scale)); glm::mat4 rotTransMat = createMatFromQuatAndPos(rot, trans); glm::mat4 rawMat = rotTransMat * scaleMat; @@ -445,6 +441,145 @@ void AnimTests::testAnimPose() { } } +void AnimTests::testAnimPoseMultiply() { + const float PI = (float)M_PI; + const glm::quat ROT_X_90 = glm::angleAxis(PI / 2.0f, glm::vec3(1.0f, 0.0f, 0.0f)); + const glm::quat ROT_Y_180 = glm::angleAxis(PI, glm::vec3(0.0f, 1.0, 0.0f)); + const glm::quat ROT_Z_30 = glm::angleAxis(PI / 6.0f, glm::vec3(1.0f, 0.0f, 0.0f)); + + std::vector scaleVec = { + 1.0f, + 2.0f, + 0.5f, + }; + + std::vector rotVec = { + glm::quat(), + ROT_X_90, + ROT_Y_180, + ROT_Z_30, + ROT_X_90 * ROT_Y_180 * ROT_Z_30, + -ROT_Y_180 + }; + + std::vector transVec = { + glm::vec3(), + glm::vec3(10.0f, 0.0f, 0.0f), + glm::vec3(0.0f, 5.0f, 0.0f), + glm::vec3(0.0f, 0.0f, 7.5f), + glm::vec3(10.0f, 5.0f, 7.5f), + glm::vec3(-10.0f, 5.0f, 7.5f), + glm::vec3(10.0f, -5.0f, 7.5f), + glm::vec3(10.0f, 5.0f, -7.5f) + }; + + const float TEST_EPSILON = 0.001f; + + std::vector matrixVec; + std::vector poseVec; + + for (auto& scale : scaleVec) { + for (auto& rot : rotVec) { + for (auto& trans : transVec) { + + // build a matrix the old fashioned way. + glm::mat4 scaleMat = glm::scale(glm::mat4(), glm::vec3(scale)); + glm::mat4 rotTransMat = createMatFromQuatAndPos(rot, trans); + glm::mat4 rawMat = rotTransMat * scaleMat; + + matrixVec.push_back(rawMat); + + // use an anim pose to build a matrix by parts. + AnimPose pose(scale, rot, trans); + poseVec.push_back(pose); + } + } + } + + for (int i = 0; i < matrixVec.size(); i++) { + for (int j = 0; j < matrixVec.size(); j++) { + + // multiply the matrices together + glm::mat4 matrix = matrixVec[i] * matrixVec[j]; + + // convert to matrix (note this will remove sheer from the matrix) + AnimPose resultA(matrix); + + // multiply the poses together directly + AnimPose resultB = poseVec[i] * poseVec[j]; + + /* + qDebug() << "matrixVec[" << i << "] =" << matrixVec[i]; + qDebug() << "matrixVec[" << j << "] =" << matrixVec[j]; + qDebug() << "matrixResult =" << resultA; + + qDebug() << "poseVec[" << i << "] =" << poseVec[i]; + qDebug() << "poseVec[" << j << "] =" << poseVec[j]; + qDebug() << "poseResult =" << resultB; + */ + + // compare results. + QCOMPARE_WITH_ABS_ERROR(resultA.scale(), resultB.scale(), TEST_EPSILON); + QCOMPARE_WITH_ABS_ERROR(resultA.rot(), resultB.rot(), TEST_EPSILON); + QCOMPARE_WITH_ABS_ERROR(resultA.trans(), resultB.trans(), TEST_EPSILON); + } + } +} + +void AnimTests::testAnimPoseInverse() { + const float PI = (float)M_PI; + const glm::quat ROT_X_90 = glm::angleAxis(PI / 2.0f, glm::vec3(1.0f, 0.0f, 0.0f)); + const glm::quat ROT_Y_180 = glm::angleAxis(PI, glm::vec3(0.0f, 1.0, 0.0f)); + const glm::quat ROT_Z_30 = glm::angleAxis(PI / 6.0f, glm::vec3(1.0f, 0.0f, 0.0f)); + + std::vector scaleVec = { + 1.0f, + 2.0f, + 0.5f + }; + + std::vector rotVec = { + glm::quat(), + ROT_X_90, + ROT_Y_180, + ROT_Z_30, + ROT_X_90 * ROT_Y_180 * ROT_Z_30, + -ROT_Y_180 + }; + + std::vector transVec = { + glm::vec3(), + glm::vec3(10.0f, 0.0f, 0.0f), + glm::vec3(0.0f, 5.0f, 0.0f), + glm::vec3(0.0f, 0.0f, 7.5f), + glm::vec3(10.0f, 5.0f, 7.5f), + glm::vec3(-10.0f, 5.0f, 7.5f), + glm::vec3(10.0f, -5.0f, 7.5f), + glm::vec3(10.0f, 5.0f, -7.5f) + }; + + const float TEST_EPSILON = 0.001f; + + for (auto& scale : scaleVec) { + for (auto& rot : rotVec) { + for (auto& trans : transVec) { + + // build a matrix the old fashioned way. + glm::mat4 scaleMat = glm::scale(glm::mat4(), glm::vec3(scale)); + glm::mat4 rotTransMat = createMatFromQuatAndPos(rot, trans); + glm::mat4 rawMat = glm::inverse(rotTransMat * scaleMat); + + // use an anim pose to build a matrix by parts. + AnimPose pose(scale, rot, trans); + glm::mat4 poseMat = pose.inverse(); + + QCOMPARE_WITH_ABS_ERROR(rawMat, poseMat, TEST_EPSILON); + } + } + } +} + + void AnimTests::testExpressionTokenizer() { QString str = "(10 + x) >= 20.1 && (y != !z)"; AnimExpression e("x"); diff --git a/tests/animation/src/AnimTests.h b/tests/animation/src/AnimTests.h index 439793f21d..326545b0a9 100644 --- a/tests/animation/src/AnimTests.h +++ b/tests/animation/src/AnimTests.h @@ -27,6 +27,8 @@ private slots: void testVariant(); void testAccumulateTime(); void testAnimPose(); + void testAnimPoseMultiply(); + void testAnimPoseInverse(); void testExpressionTokenizer(); void testExpressionParser(); void testExpressionEvaluator();