From e992d6703a8faf21e5a38cab5a694bd48da8ff06 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 3 May 2017 18:51:17 -0700 Subject: [PATCH] WIP: debug render joint constraints. --- interface/src/avatar/MyAvatar.cpp | 4 +- libraries/animation/src/AnimContext.cpp | 6 +- libraries/animation/src/AnimContext.h | 4 +- .../animation/src/AnimInverseKinematics.cpp | 120 +++++++++++++++++- .../animation/src/AnimInverseKinematics.h | 3 +- libraries/animation/src/AnimPose.cpp | 7 +- libraries/animation/src/AnimPose.h | 3 +- libraries/animation/src/ElbowConstraint.h | 5 + libraries/animation/src/Rig.cpp | 6 +- libraries/animation/src/Rig.h | 2 +- .../animation/src/SwingTwistConstraint.cpp | 22 ++-- .../animation/src/SwingTwistConstraint.h | 3 + libraries/render-utils/src/Model.cpp | 3 +- 13 files changed, 164 insertions(+), 24 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 7d2aefa7bf..526b80e3f1 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -1703,6 +1703,7 @@ void MyAvatar::postUpdate(float deltaTime) { } // AJT: REMOVE. + /* { auto ikNode = _rig->getAnimInverseKinematicsNode(); if (ikNode) { @@ -1714,7 +1715,7 @@ void MyAvatar::postUpdate(float deltaTime) { int hipsIndex = _rig->indexOfJoint("Hips"); for (size_t i = 0; i < limitCenterPoses.size(); i++) { if (i == hipsIndex) { - //limitCenterPoses[i].trans() = glm::vec3(); // zero the hips + limitCenterPoses[i].trans() = glm::vec3(); // zero the hips } // convert from cm to m limitCenterPoses[i].trans() = 0.01f * limitCenterPoses[i].trans(); @@ -1723,6 +1724,7 @@ void MyAvatar::postUpdate(float deltaTime) { AnimDebugDraw::getInstance().addAbsolutePoses("myAvatarLimitCenterPoses", _rig->getAnimSkeleton(), limitCenterPoses, xform, glm::vec4(1)); } } + */ if (_enableDebugDrawDefaultPose || _enableDebugDrawAnimPose) { diff --git a/libraries/animation/src/AnimContext.cpp b/libraries/animation/src/AnimContext.cpp index c8d3e7bcda..c59c75b191 100644 --- a/libraries/animation/src/AnimContext.cpp +++ b/libraries/animation/src/AnimContext.cpp @@ -10,7 +10,9 @@ #include "AnimContext.h" -AnimContext::AnimContext(bool enableDebugDrawIKTargets, const glm::mat4& geometryToRigMatrix) : +AnimContext::AnimContext(bool enableDebugDrawIKTargets, const glm::mat4& geometryToRigMatrix, const glm::mat4& rigToWorldMatrix) : _enableDebugDrawIKTargets(enableDebugDrawIKTargets), - _geometryToRigMatrix(geometryToRigMatrix) { + _geometryToRigMatrix(geometryToRigMatrix), + _rigToWorldMatrix(rigToWorldMatrix) +{ } diff --git a/libraries/animation/src/AnimContext.h b/libraries/animation/src/AnimContext.h index 3170911e14..067e64026a 100644 --- a/libraries/animation/src/AnimContext.h +++ b/libraries/animation/src/AnimContext.h @@ -16,15 +16,17 @@ class AnimContext { public: - AnimContext(bool enableDebugDrawIKTargets, const glm::mat4& geometryToRigMatrix); + AnimContext(bool enableDebugDrawIKTargets, const glm::mat4& geometryToRigMatrix, const glm::mat4& rigToWorldMatrix); bool getEnableDebugDrawIKTargets() const { return _enableDebugDrawIKTargets; } const glm::mat4& getGeometryToRigMatrix() const { return _geometryToRigMatrix; } + const glm::mat4& getRigToWorldMatrix() const { return _rigToWorldMatrix; } protected: bool _enableDebugDrawIKTargets { false }; glm::mat4 _geometryToRigMatrix; + glm::mat4 _rigToWorldMatrix; }; #endif // hifi_AnimContext_h diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index e21db11eed..ae2ca94d66 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -399,6 +399,8 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar //virtual const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) { + debugDrawConstraints(context); + const float MAX_OVERLAY_DT = 1.0f / 30.0f; // what to clamp delta-time to in AnimInverseKinematics::overlay if (dt > MAX_OVERLAY_DT) { dt = MAX_OVERLAY_DT; @@ -604,9 +606,9 @@ void AnimInverseKinematics::clearIKJointLimitHistory() { } } -RotationConstraint* AnimInverseKinematics::getConstraint(int index) { +RotationConstraint* AnimInverseKinematics::getConstraint(int index) const { RotationConstraint* constraint = nullptr; - std::map::iterator constraintItr = _constraints.find(index); + std::map::const_iterator constraintItr = _constraints.find(index); if (constraintItr != _constraints.end()) { constraint = constraintItr->second; } @@ -1003,3 +1005,117 @@ void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skele _hipsParentIndex = -1; } } + +void AnimInverseKinematics::debugDrawConstraints(const AnimContext& context) const { + + if (_skeleton) { + const vec4 RED(1.0f, 0.0f, 0.0f, 1.0f); + const vec4 GREEN(0.0f, 1.0f, 0.0f, 1.0f); + const vec4 BLUE(0.0f, 0.0f, 1.0f, 1.0f); + const vec4 PURPLE(0.5f, 0.0f, 1.0f, 1.0f); + const vec4 CYAN(0.0f, 1.0f, 1.0f, 1.0f); + const vec4 GRAY(0.2f, 0.2f, 0.2f, 1.0f); + const vec4 MAGENTA(1.0f, 0.0f, 1.0f, 1.0f); + const float AXIS_LENGTH = 2.0f; // cm + const float TWIST_LENGTH = 4.0f; // cm + const float HINGE_LENGTH = 6.0f; // cm + const float SWING_LENGTH = 5.0f; // cm + AnimPoseVec absPoses = /*_limitCenterPoses;*/ _skeleton->getRelativeDefaultPoses(); + _skeleton->convertRelativePosesToAbsolute(absPoses); + + mat4 geomToWorldMatrix = context.getRigToWorldMatrix() * context.getGeometryToRigMatrix(); + for (int i = 0; i < absPoses.size(); i++) { + // transform local axes into world space. + auto pose = absPoses[i]; + glm::vec3 xAxis = transformVectorFast(geomToWorldMatrix, pose.rot() * Vectors::UNIT_X); + glm::vec3 yAxis = transformVectorFast(geomToWorldMatrix, pose.rot() * Vectors::UNIT_Y); + glm::vec3 zAxis = transformVectorFast(geomToWorldMatrix, pose.rot() * Vectors::UNIT_Z); + glm::vec3 pos = transformPoint(geomToWorldMatrix, pose.trans()); + DebugDraw::getInstance().drawRay(pos, pos + AXIS_LENGTH * xAxis, RED); + DebugDraw::getInstance().drawRay(pos, pos + AXIS_LENGTH * yAxis, GREEN); + DebugDraw::getInstance().drawRay(pos, pos + AXIS_LENGTH * zAxis, BLUE); + + // draw line to parent + int parentIndex = _skeleton->getParentIndex(i); + if (parentIndex != -1) { + glm::vec3 parentPos = transformPoint(geomToWorldMatrix, absPoses[parentIndex].trans()); + DebugDraw::getInstance().drawRay(pos, parentPos, GRAY); + } + + glm::quat parentAbsRot; + if (parentIndex != -1) { + parentAbsRot = absPoses[parentIndex].rot(); + } + + const RotationConstraint* constraint = getConstraint(i); + if (constraint) { + glm::quat refRot = constraint->getReferenceRotation(); + const ElbowConstraint* elbowConstraint = dynamic_cast(constraint); + if (elbowConstraint) { + glm::vec3 hingeAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * refRot * elbowConstraint->getHingeAxis()); + DebugDraw::getInstance().drawRay(pos, pos + HINGE_LENGTH * hingeAxis, MAGENTA); + + // draw elbow constraints + glm::quat minRot = glm::angleAxis(elbowConstraint->getMinAngle(), elbowConstraint->getHingeAxis()); + glm::quat maxRot = glm::angleAxis(elbowConstraint->getMaxAngle(), elbowConstraint->getHingeAxis()); + + glm::vec3 minYAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * minRot * refRot * Vectors::UNIT_Y); + glm::vec3 maxYAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * maxRot * refRot * Vectors::UNIT_Y); + + const int NUM_SWING_STEPS = 10; + for (int i = 0; i < NUM_SWING_STEPS + 1; i++) { + glm::quat rot = glm::normalize(glm::lerp(minRot, maxRot, i * (1.0f / NUM_SWING_STEPS))); + glm::vec3 axis = transformVectorFast(geomToWorldMatrix, parentAbsRot * rot * refRot * Vectors::UNIT_Y); + DebugDraw::getInstance().drawRay(pos, pos + TWIST_LENGTH * axis, CYAN); + } + + } else { + const SwingTwistConstraint* swingTwistConstraint = dynamic_cast(constraint); + if (swingTwistConstraint) { + // twist constraints + + glm::vec3 hingeAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * refRot * Vectors::UNIT_Y); + DebugDraw::getInstance().drawRay(pos, pos + HINGE_LENGTH * hingeAxis, MAGENTA); + + glm::quat minRot = glm::angleAxis(swingTwistConstraint->getMinTwist(), Vectors::UNIT_Y); + glm::quat maxRot = glm::angleAxis(swingTwistConstraint->getMaxTwist(), Vectors::UNIT_Y); + + glm::vec3 minTwistYAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * minRot * refRot * Vectors::UNIT_X); + glm::vec3 maxTwistYAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * maxRot * refRot * Vectors::UNIT_X); + + const int NUM_SWING_STEPS = 10; + for (int i = 0; i < NUM_SWING_STEPS + 1; i++) { + glm::quat rot = glm::normalize(glm::lerp(minRot, maxRot, i * (1.0f / NUM_SWING_STEPS))); + glm::vec3 axis = transformVectorFast(geomToWorldMatrix, parentAbsRot * rot * refRot * Vectors::UNIT_X); + DebugDraw::getInstance().drawRay(pos, pos + TWIST_LENGTH * axis, CYAN); + } + + // draw swing constraints. + glm::vec3 previousSwingTip; + const size_t NUM_MIN_DOTS = swingTwistConstraint->getMinDots().size(); + const float D_THETA = TWO_PI / NUM_MIN_DOTS; + float theta = 0.0f; + for (size_t i = 0; i < NUM_MIN_DOTS; i++, theta += D_THETA) { + // compute swing rotation from theta and phi angles. + float phi = acos(swingTwistConstraint->getMinDots()[i]); + float cos_phi = swingTwistConstraint->getMinDots()[i]; + float sin_phi = sinf(phi); + glm::vec3 swungAxis(sin_phi * cosf(theta), cos_phi, sin_phi * sinf(theta)); + glm::vec3 worldSwungAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * refRot * swungAxis); + + glm::vec3 swingTip = pos + SWING_LENGTH * worldSwungAxis; + DebugDraw::getInstance().drawRay(pos, swingTip, PURPLE); + + if (previousSwingTipValid) { + DebugDraw::getInstance().drawRay(previousSwingTip, swingTip, PURPLE); + } + previousSwingTip = swingTip; + previousSwingTipValid = true; + } + } + } + pose.rot() = constraint->computeCenterRotation(); + } + } + } +} diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index e7427d9ebc..5ad5638709 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -51,11 +51,12 @@ protected: void solveWithCyclicCoordinateDescent(const std::vector& targets); int solveTargetWithCCD(const IKTarget& target, AnimPoseVec& absolutePoses); virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) override; + void debugDrawConstraints(const AnimContext& context) const; // for AnimDebugDraw rendering virtual const AnimPoseVec& getPosesInternal() const override { return _relativePoses; } - RotationConstraint* getConstraint(int index); + RotationConstraint* getConstraint(int index) const; void clearConstraints(); void initConstraints(); void initLimitCenterPoses(); diff --git a/libraries/animation/src/AnimPose.cpp b/libraries/animation/src/AnimPose.cpp index e1c8528e0b..470bbab8b6 100644 --- a/libraries/animation/src/AnimPose.cpp +++ b/libraries/animation/src/AnimPose.cpp @@ -39,7 +39,7 @@ glm::vec3 AnimPose::xformPoint(const glm::vec3& rhs) const { return *this * rhs; } -// really slow +// 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); @@ -49,6 +49,11 @@ glm::vec3 AnimPose::xformVector(const glm::vec3& rhs) const { 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); diff --git a/libraries/animation/src/AnimPose.h b/libraries/animation/src/AnimPose.h index 893a5c1382..a2e22a24be 100644 --- a/libraries/animation/src/AnimPose.h +++ b/libraries/animation/src/AnimPose.h @@ -25,7 +25,8 @@ public: static const AnimPose identity; glm::vec3 xformPoint(const glm::vec3& rhs) const; - glm::vec3 xformVector(const glm::vec3& rhs) const; // really slow + 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; diff --git a/libraries/animation/src/ElbowConstraint.h b/libraries/animation/src/ElbowConstraint.h index 868f5cdc6b..d3f080374a 100644 --- a/libraries/animation/src/ElbowConstraint.h +++ b/libraries/animation/src/ElbowConstraint.h @@ -19,6 +19,11 @@ public: void setAngleLimits(float minAngle, float maxAngle); virtual bool apply(glm::quat& rotation) const override; virtual glm::quat computeCenterRotation() const override; + + glm::vec3 getHingeAxis() const { return _axis; } + float getMinAngle() const { return _minAngle; } + float getMaxAngle() const { return _maxAngle; } + protected: glm::vec3 _axis; glm::vec3 _perpAxis; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index b66b0eafa5..2689fe5be8 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -941,7 +941,7 @@ void Rig::updateAnimationStateHandlers() { // called on avatar update thread (wh } } -void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform) { +void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform, glm::mat4 rigToWorldTransform) { PROFILE_RANGE_EX(simulation_animation_detail, __FUNCTION__, 0xffff00ff, 0); PerformanceTimer perfTimer("updateAnimations"); @@ -954,7 +954,7 @@ void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform) { updateAnimationStateHandlers(); _animVars.setRigToGeometryTransform(_rigToGeometryTransform); - AnimContext context(_enableDebugDrawIKTargets, getGeometryToRigTransform()); + AnimContext context(_enableDebugDrawIKTargets, getGeometryToRigTransform(), rigToWorldTransform); // evaluate the animation AnimNode::Triggers triggersOut; @@ -1445,7 +1445,7 @@ void Rig::computeAvatarBoundingCapsule( // call overlay twice: once to verify AnimPoseVec joints and again to do the IK AnimNode::Triggers triggersOut; - AnimContext context(false, glm::mat4()); + AnimContext context(false, glm::mat4(), glm::mat4()); float dt = 1.0f; // the value of this does not matter ikNode.overlay(animVars, context, dt, triggersOut, _animSkeleton->getRelativeBindPoses()); AnimPoseVec finalPoses = ikNode.overlay(animVars, context, dt, triggersOut, _animSkeleton->getRelativeBindPoses()); diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index e0c5e9f421..f8ae0bdfae 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -162,7 +162,7 @@ public: void computeMotionAnimationState(float deltaTime, const glm::vec3& worldPosition, const glm::vec3& worldVelocity, const glm::quat& worldRotation, CharacterControllerState ccState); // Regardless of who started the animations or how many, update the joints. - void updateAnimations(float deltaTime, glm::mat4 rootTransform); + void updateAnimations(float deltaTime, glm::mat4 rootTransform, glm::mat4 rigToWorldTransform); // legacy void inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm::quat& targetRotation, float priority, diff --git a/libraries/animation/src/SwingTwistConstraint.cpp b/libraries/animation/src/SwingTwistConstraint.cpp index c1b325a74a..475ee6a59e 100644 --- a/libraries/animation/src/SwingTwistConstraint.cpp +++ b/libraries/animation/src/SwingTwistConstraint.cpp @@ -435,13 +435,13 @@ void SwingTwistConstraint::clearHistory() { glm::quat SwingTwistConstraint::computeCenterRotation() const { const size_t NUM_MIN_DOTS = getMinDots().size(); const size_t NUM_LIMITS = 2 * NUM_MIN_DOTS; - std::vector limits; - limits.reserve(NUM_LIMITS); - glm::quat minTwistRot; - glm::quat maxTwistRot; + std::vector swingLimits; + swingLimits.reserve(NUM_LIMITS); + + glm::quat twistLimits[2]; if (_minTwist != _maxTwist) { - minTwistRot = glm::angleAxis(_minTwist, _referenceRotation * Vectors::UNIT_Y); - minTwistRot = glm::angleAxis(_maxTwist, _referenceRotation * Vectors::UNIT_Y); + twistLimits[0] = glm::angleAxis(_minTwist, _referenceRotation * Vectors::UNIT_Y); + twistLimits[1] = glm::angleAxis(_maxTwist, _referenceRotation * Vectors::UNIT_Y); } const float D_THETA = TWO_PI / NUM_MIN_DOTS; float theta = 0.0f; @@ -451,9 +451,11 @@ glm::quat SwingTwistConstraint::computeCenterRotation() const { float cos_phi = getMinDots()[i]; float sin_phi = sinf(phi); glm::vec3 swungAxis(sin_phi * cosf(theta), cos_phi, sin_phi * sinf(theta)); - glm::quat swing = glm::angleAxis(phi, glm::cross(Vectors::UNIT_Y, swungAxis)); - limits.push_back(swing * minTwistRot); - limits.push_back(swing * maxTwistRot); + glm::quat swing = glm::angleAxis(phi, glm::normalize(glm::cross(Vectors::UNIT_Y, swungAxis))); + swingLimits.push_back(swing); } - return averageQuats(limits.size(), &limits[0]); + glm::quat limits[2]; + limits[0] = averageQuats(swingLimits.size(), &swingLimits[0]); + limits[1] = averageQuats(2, twistLimits); + return averageQuats(2, limits); } diff --git a/libraries/animation/src/SwingTwistConstraint.h b/libraries/animation/src/SwingTwistConstraint.h index 295edb3ebf..ffe9a1d800 100644 --- a/libraries/animation/src/SwingTwistConstraint.h +++ b/libraries/animation/src/SwingTwistConstraint.h @@ -101,6 +101,9 @@ public: virtual glm::quat computeCenterRotation() const override; + const float getMinTwist() const { return _minTwist; } + const float getMaxTwist() const { return _maxTwist; } + private: float handleTwistBoundaryConditions(float twistAngle) const; diff --git a/libraries/render-utils/src/Model.cpp b/libraries/render-utils/src/Model.cpp index acc84646c5..766a584b85 100644 --- a/libraries/render-utils/src/Model.cpp +++ b/libraries/render-utils/src/Model.cpp @@ -1046,7 +1046,8 @@ void Model::simulate(float deltaTime, bool fullUpdate) { //virtual void Model::updateRig(float deltaTime, glm::mat4 parentTransform) { _needsUpdateClusterMatrices = true; - _rig->updateAnimations(deltaTime, parentTransform); + glm::mat4 rigToWorldTransform = createMatFromQuatAndPos(getRotation(), getTranslation()); + _rig->updateAnimations(deltaTime, parentTransform, rigToWorldTransform); } void Model::computeMeshPartLocalBounds() {