Merge pull request #1 from AndrewMeadows/fix-avatar-capsule

compute bounding capsule of avatars

interface/src/avatar/SkeletonModel.cpp

@@ -344,114 +344,10 @@ void SkeletonModel::computeBoundingShape() {
         return;
     }
 
-    /*
+    _rig->computeAvatarBoundingCapsule(geometry,
-    AJT: TODO HACK DISABLED FIXME
+            _boundingCapsuleRadius,
-
+            _boundingCapsuleHeight,
-    // BOUNDING SHAPE HACK: before we measure the bounds of the joints we use IK to put the
+            _boundingCapsuleLocalOffset);
-    // hands and feet into positions that are more correct than the default pose.
-
-    // Measure limb lengths so we can specify IK targets that will pull hands and feet tight to body
-    QVector<QString> endEffectors;
-    endEffectors.push_back("RightHand");
-    endEffectors.push_back("LeftHand");
-    endEffectors.push_back("RightFoot");
-    endEffectors.push_back("LeftFoot");
-
-    QVector<QString> baseJoints;
-    baseJoints.push_back("RightArm");
-    baseJoints.push_back("LeftArm");
-    baseJoints.push_back("RightUpLeg");
-    baseJoints.push_back("LeftUpLeg");
-
-    for (int i = 0; i < endEffectors.size(); ++i) {
-        QString tipName = endEffectors[i];
-        QString baseName = baseJoints[i];
-        float limbLength = 0.0f;
-        int tipIndex = _rig->indexOfJoint(tipName);
-        if (tipIndex == -1) {
-            continue;
-        }
-        // save tip's relative rotation for later
-        glm::quat tipRotation = _rig->getJointState(tipIndex).getRotationInConstrainedFrame();
-
-        // IK on each endpoint
-        int jointIndex = tipIndex;
-        QVector<int> freeLineage;
-        float priority = 1.0f;
-        while (jointIndex > -1) {
-            JointState limbJoint = _rig->getJointState(jointIndex);
-            freeLineage.push_back(jointIndex);
-            if (limbJoint.getName() == baseName) {
-                glm::vec3 targetPosition = limbJoint.getPosition() - glm::vec3(0.0f, 1.5f * limbLength, 0.0f);
-                // do IK a few times to make sure the endpoint gets close to its target
-                for (int j = 0; j < 5; ++j) {
-                    _rig->inverseKinematics(tipIndex,
-                            targetPosition,
-                            glm::quat(),
-                            priority,
-                            freeLineage,
-                            glm::mat4());
-                }
-                break;
-            }
-            limbLength += limbJoint.getDistanceToParent();
-            jointIndex = limbJoint.getParentIndex();
-        }
-
-        // since this IK target is totally bogus we restore the tip's relative rotation
-        _rig->setJointRotationInConstrainedFrame(tipIndex, tipRotation, priority);
-    }
-
-    // recompute all joint model-frame transforms
-    glm::mat4 rootTransform = glm::scale(_scale) * glm::translate(_offset) * geometry.offset;
-    for (int i = 0; i < _rig->getJointStateCount(); i++) {
-        _rig->updateJointState(i, rootTransform);
-    }
-    // END BOUNDING SHAPE HACK
-
-    // compute bounding box that encloses all shapes
-    Extents totalExtents;
-    totalExtents.reset();
-    totalExtents.addPoint(glm::vec3(0.0f));
-    int numStates = _rig->getJointStateCount();
-    for (int i = 0; i < numStates; i++) {
-        // Each joint contributes a capsule defined by FBXJoint.shapeInfo.
-        // For totalExtents we use the capsule endpoints expanded by the radius.
-        const JointState& state = _rig->getJointState(i);
-        const glm::mat4& jointTransform = state.getTransform();
-        const FBXJointShapeInfo& shapeInfo = geometry.joints.at(i).shapeInfo;
-        if (shapeInfo.points.size() > 0) {
-            for (int j = 0; j < shapeInfo.points.size(); ++j) {
-                totalExtents.addPoint(extractTranslation(jointTransform * glm::translate(shapeInfo.points[j])));
-            }
-        }
-        // HACK so that default legless robot doesn't knuckle-drag
-        if (shapeInfo.points.size() == 0 && (state.getName() == "LeftFoot" || state.getName() == "RightFoot")) {
-            totalExtents.addPoint(extractTranslation(jointTransform));
-        }
-    }
-
-    // compute bounding shape parameters
-    // NOTE: we assume that the longest side of totalExtents is the yAxis...
-    glm::vec3 diagonal = totalExtents.maximum - totalExtents.minimum;
-    // ... and assume the radius is half the RMS of the X and Z sides:
-    _boundingCapsuleRadius = 0.5f * sqrtf(0.5f * (diagonal.x * diagonal.x + diagonal.z * diagonal.z));
-    _boundingCapsuleHeight = diagonal.y - 2.0f * _boundingCapsuleRadius;
-
-    glm::vec3 rootPosition = _rig->getJointState(geometry.rootJointIndex).getPosition();
-    _boundingCapsuleLocalOffset = 0.5f * (totalExtents.maximum + totalExtents.minimum) - rootPosition;
-
-    // RECOVER FROM BOUNINDG SHAPE HACK: now that we're all done, restore the default pose
-    for (int i = 0; i < numStates; i++) {
-        _rig->restoreJointRotation(i, 1.0f, 1.0f);
-        _rig->restoreJointTranslation(i, 1.0f, 1.0f);
-    }
-    */
-
-    // AJT: REMOVE HARDCODED BOUNDING VOLUME
-    _boundingCapsuleRadius = 0.3f;
-    _boundingCapsuleHeight = 1.3f;
-    _boundingCapsuleLocalOffset = glm::vec3(0.0f, -0.25f, 0.0f);
 }
 
 void SkeletonModel::renderBoundingCollisionShapes(gpu::Batch& batch, float alpha) {

libraries/animation/src/AnimInverseKinematics.h

@@ -54,9 +54,9 @@ protected:
     AnimInverseKinematics& operator=(const AnimInverseKinematics&) = delete;
 
     struct IKTargetVar {
-        IKTargetVar(const QString& jointNameIn, 
+        IKTargetVar(const QString& jointNameIn,
-                const QString& positionVarIn, 
+                const QString& positionVarIn,
-                const QString& rotationVarIn, 
+                const QString& rotationVarIn,
                 const QString& typeVarIn) :
             positionVar(positionVarIn),
             rotationVar(rotationVarIn),

libraries/animation/src/AnimSkeleton.cpp

@@ -96,6 +96,16 @@ AnimPose AnimSkeleton::getAbsolutePose(int jointIndex, const AnimPoseVec& poses)
     }
 }
 
+void AnimSkeleton::convertRelativePosesToAbsolute(AnimPoseVec& poses) const {
+    // poses start off relative and leave in absolute frame
+    for (int i = 0; i < poses.size() && i < _joints.size(); ++i) {
+        int parentIndex = _joints[i].parentIndex;
+        if (parentIndex != -1) {
+            poses[i] = poses[parentIndex] * poses[i];
+        }
+    }
+}
+
 void AnimSkeleton::buildSkeletonFromJoints(const std::vector<FBXJoint>& joints) {
     _joints = joints;
 

libraries/animation/src/AnimSkeleton.h

@@ -51,6 +51,8 @@ public:
 
     AnimPose getAbsolutePose(int jointIndex, const AnimPoseVec& poses) const;
 
+    void convertRelativePosesToAbsolute(AnimPoseVec& poses) const;
+
 #ifndef NDEBUG
     void dump() const;
     void dump(const AnimPoseVec& poses) const;

libraries/animation/src/Rig.cpp

@@ -19,8 +19,9 @@
 #include <DebugDraw.h>
 
 #include "AnimationLogging.h"
-#include "AnimSkeleton.h"
 #include "AnimClip.h"
+#include "AnimInverseKinematics.h"
+#include "AnimSkeleton.h"
 #include "IKTarget.h"
 
 static bool isEqual(const glm::vec3& u, const glm::vec3& v) {
@@ -1065,3 +1066,105 @@ void Rig::copyJointsFromJointData(const QVector<JointData>& jointDataVec) {
         setJointTranslation(i, data.translationSet, invGeometryOffset * data.translation, 1.0f);
     }
 }
+
+void Rig::computeAvatarBoundingCapsule(
+        const FBXGeometry& geometry,
+        float& radiusOut,
+        float& heightOut,
+        glm::vec3& localOffsetOut) const {
+    if (! _animSkeleton) {
+        const float DEFAULT_AVATAR_CAPSULE_RADIUS = 0.3f;
+        const float DEFAULT_AVATAR_CAPSULE_HEIGHT = 1.3f;
+        const glm::vec3 DEFAULT_AVATAR_CAPSULE_LOCAL_OFFSET = glm::vec3(0.0f, -0.25f, 0.0f);
+        radiusOut = DEFAULT_AVATAR_CAPSULE_RADIUS;
+        heightOut = DEFAULT_AVATAR_CAPSULE_HEIGHT;
+        localOffsetOut = DEFAULT_AVATAR_CAPSULE_LOCAL_OFFSET;
+        return;
+    }
+
+    AnimInverseKinematics ikNode("boundingShape");
+    ikNode.setSkeleton(_animSkeleton);
+    ikNode.setTargetVars("LeftHand",
+                         "leftHandPosition",
+                         "leftHandRotation",
+                         "leftHandType");
+    ikNode.setTargetVars("RightHand",
+                         "rightHandPosition",
+                         "rightHandRotation",
+                         "rightHandType");
+    ikNode.setTargetVars("LeftFoot",
+                         "leftFootPosition",
+                         "leftFootRotation",
+                         "leftFootType");
+    ikNode.setTargetVars("RightFoot",
+                         "rightFootPosition",
+                         "rightFootRotation",
+                         "rightFootType");
+
+    AnimPose geometryToRig = _modelOffset * _geometryOffset;
+
+    AnimPose hips = geometryToRig * _animSkeleton->getAbsoluteBindPose(_animSkeleton->nameToJointIndex("Hips"));
+    AnimVariantMap animVars;
+    glm::quat handRotation = glm::angleAxis(PI, Vectors::UNIT_X);
+    animVars.set("leftHandPosition", hips.trans);
+    animVars.set("leftHandRotation", handRotation);
+    animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition);
+    animVars.set("rightHandPosition", hips.trans);
+    animVars.set("rightHandRotation", handRotation);
+    animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition);
+
+    int rightFootIndex = _animSkeleton->nameToJointIndex("RightFoot");
+    int leftFootIndex = _animSkeleton->nameToJointIndex("LeftFoot");
+    if (rightFootIndex != -1 && leftFootIndex != -1) {
+        glm::vec3 foot = Vectors::ZERO;
+        glm::quat footRotation = glm::angleAxis(0.5f * PI, Vectors::UNIT_X);
+        animVars.set("leftFootPosition", foot);
+        animVars.set("leftFootRotation", footRotation);
+        animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition);
+        animVars.set("rightFootPosition", foot);
+        animVars.set("rightFootRotation", footRotation);
+        animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition);
+    }
+
+    // call overlay twice: once to verify AnimPoseVec joints and again to do the IK
+    AnimNode::Triggers triggersOut;
+    float dt = 1.0f; // the value of this does not matter
+    ikNode.overlay(animVars, dt, triggersOut, _animSkeleton->getRelativeBindPoses());
+    AnimPoseVec finalPoses =  ikNode.overlay(animVars, dt, triggersOut, _animSkeleton->getRelativeBindPoses());
+
+    // convert relative poses to absolute
+    _animSkeleton->convertRelativePosesToAbsolute(finalPoses);
+
+    // compute bounding box that encloses all points
+    Extents totalExtents;
+    totalExtents.reset();
+    int numPoses = finalPoses.size();
+    for (int i = 0; i < numPoses; i++) {
+        const FBXJointShapeInfo& shapeInfo = geometry.joints.at(i).shapeInfo;
+        AnimPose pose = finalPoses[i];
+        if (shapeInfo.points.size() > 0) {
+            for (int j = 0; j < shapeInfo.points.size(); ++j) {
+                totalExtents.addPoint((pose * shapeInfo.points[j]));
+            }
+        } else {
+            totalExtents.addPoint(pose.trans);
+        }
+    }
+    // HACK so that default legless robot doesn't knuckle-drag
+    if (rightFootIndex > -1) {
+        totalExtents.addPoint(finalPoses[rightFootIndex].trans);
+    }
+    if (leftFootIndex > -1) {
+        totalExtents.addPoint(finalPoses[leftFootIndex].trans);
+    }
+
+    // compute bounding shape parameters
+    // NOTE: we assume that the longest side of totalExtents is the yAxis...
+    glm::vec3 diagonal = geometryToRig * (totalExtents.maximum - totalExtents.minimum);
+    // ... and assume the radiusOut is half the RMS of the X and Z sides:
+    radiusOut = 0.5f * sqrtf(0.5f * (diagonal.x * diagonal.x + diagonal.z * diagonal.z));
+    heightOut = diagonal.y - 2.0f * radiusOut;
+
+    glm::vec3 rootPosition = finalPoses[geometry.rootJointIndex].trans;
+    localOffsetOut = geometryToRig * (0.5f * (totalExtents.maximum + totalExtents.minimum) - rootPosition);
+}

libraries/animation/src/Rig.h

@@ -197,6 +197,8 @@ public:
     void copyJointsIntoJointData(QVector<JointData>& jointDataVec) const;
     void copyJointsFromJointData(const QVector<JointData>& jointDataVec);
 
+    void computeAvatarBoundingCapsule(const FBXGeometry& geometry, float& radiusOut, float& heightOut, glm::vec3& offsetOut) const;
+
  protected:
     bool isIndexValid(int index) const { return _animSkeleton && index >= 0 && index < _animSkeleton->getNumJoints(); }
     void updateAnimationStateHandlers();