diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index 6d468c3f30..89e4368515 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -124,12 +124,26 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { } } - params.bodyCapsuleRadius = myAvatar->getCharacterController()->getCapsuleRadius(); - params.bodyCapsuleHalfHeight = myAvatar->getCharacterController()->getCapsuleHalfHeight(); - params.bodyCapsuleLocalOffset = myAvatar->getCharacterController()->getCapsuleLocalOffset(); - params.isTalking = head->getTimeWithoutTalking() <= 1.5f; + // pass detailed torso k-dops to rig. + int hipsJoint = _rig.indexOfJoint("Hips"); + if (hipsJoint >= 0) { + params.hipsShapeInfo = geometry.joints[hipsJoint].shapeInfo; + } + int spineJoint = _rig.indexOfJoint("Spine"); + if (spineJoint >= 0) { + params.spineShapeInfo = geometry.joints[spineJoint].shapeInfo; + } + int spine1Joint = _rig.indexOfJoint("Spine1"); + if (spine1Joint >= 0) { + params.spine1ShapeInfo = geometry.joints[spine1Joint].shapeInfo; + } + int spine2Joint = _rig.indexOfJoint("Spine2"); + if (spine2Joint >= 0) { + params.spine2ShapeInfo = geometry.joints[spine2Joint].shapeInfo; + } + _rig.updateFromControllerParameters(params, deltaTime); Rig::CharacterControllerState ccState = convertCharacterControllerState(myAvatar->getCharacterController()->getState()); diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 21f98d3e01..15886a1212 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1099,35 +1099,139 @@ void Rig::updateHead(bool headEnabled, bool hipsEnabled, const AnimPose& headPos } } -void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, bool leftArmEnabled, bool rightArmEnabled, float dt, - const AnimPose& leftHandPose, const AnimPose& rightHandPose, - float bodyCapsuleRadius, float bodyCapsuleHalfHeight, const glm::vec3& bodyCapsuleLocalOffset) { +const float INV_SQRT_3 = 1.0f / sqrtf(3.0f); +const int DOP14_COUNT = 14; +const glm::vec3 DOP14_NORMALS[DOP14_COUNT] = { + Vectors::UNIT_X, + -Vectors::UNIT_X, + Vectors::UNIT_Y, + -Vectors::UNIT_Y, + Vectors::UNIT_Z, + -Vectors::UNIT_Z, + glm::vec3(INV_SQRT_3, INV_SQRT_3, INV_SQRT_3), + -glm::vec3(INV_SQRT_3, INV_SQRT_3, INV_SQRT_3), + glm::vec3(INV_SQRT_3, -INV_SQRT_3, INV_SQRT_3), + -glm::vec3(INV_SQRT_3, -INV_SQRT_3, INV_SQRT_3), + glm::vec3(INV_SQRT_3, INV_SQRT_3, -INV_SQRT_3), + -glm::vec3(INV_SQRT_3, INV_SQRT_3, -INV_SQRT_3), + glm::vec3(INV_SQRT_3, -INV_SQRT_3, -INV_SQRT_3), + -glm::vec3(INV_SQRT_3, -INV_SQRT_3, -INV_SQRT_3) +}; - // Use this capsule to represent the avatar body. - int hipsIndex = indexOfJoint("Hips"); - glm::vec3 hipsTrans; - if (hipsIndex >= 0) { - hipsTrans = _internalPoseSet._absolutePoses[hipsIndex].trans(); +// returns true if the given point lies in side of the k-dop, specified by shapeInfo & shapePose. +// if the given point does lie within the k-dop, it also returns the amount of displacement necessary to push that point outward +// such that it lies on the surface of the kdop. +static bool findPointKDopDisplacement(const glm::vec3& point, const AnimPose& shapePose, const FBXJointShapeInfo& shapeInfo, glm::vec3& displacementOut) { + + // transform point into local space of jointShape. + glm::vec3 localPoint = shapePose.inverse().xformPoint(point); + + // Only works for 14-dop shape infos. + assert(shapeInfo.dots.size() == DOP14_COUNT); + if (shapeInfo.dots.size() != DOP14_COUNT) { + return false; } - const glm::vec3 bodyCapsuleCenter = hipsTrans - bodyCapsuleLocalOffset; - const glm::vec3 bodyCapsuleStart = bodyCapsuleCenter - glm::vec3(0, bodyCapsuleHalfHeight, 0); - const glm::vec3 bodyCapsuleEnd = bodyCapsuleCenter + glm::vec3(0, bodyCapsuleHalfHeight, 0); + glm::vec3 minDisplacement(FLT_MAX); + float minDisplacementLen = FLT_MAX; + glm::vec3 p = localPoint - shapeInfo.avgPoint; + float pLen = glm::length(p); + if (pLen > 0.0f) { + int slabCount = 0; + for (int i = 0; i < DOP14_COUNT; i++) { + float dot = glm::dot(p, DOP14_NORMALS[i]); + if (dot > 0.0f && dot < shapeInfo.dots[i]) { + slabCount++; + float distToPlane = pLen * (shapeInfo.dots[i] / dot); + float displacementLen = distToPlane - pLen; + + // keep track of the smallest displacement + if (displacementLen < minDisplacementLen) { + minDisplacementLen = displacementLen; + minDisplacement = (p / pLen) * displacementLen; + } + } + } + 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); + return true; + } else { + // point is outside of kdop + return false; + } + } else { + // point is directly on top of shapeInfo.avgPoint. + // push the point out along the x axis. + displacementOut = shapePose.xformVectorFast(shapeInfo.points[0]); + return true; + } +} + +glm::vec3 Rig::deflectHandFromTorso(const glm::vec3& handPosition, const FBXJointShapeInfo& hipsShapeInfo, const FBXJointShapeInfo& spineShapeInfo, + const FBXJointShapeInfo& spine1ShapeInfo, const FBXJointShapeInfo& spine2ShapeInfo) const { + glm::vec3 position = handPosition; + glm::vec3 displacement; + int hipsJoint = indexOfJoint("Hips"); + if (hipsJoint >= 0) { + AnimPose hipsPose; + if (getAbsoluteJointPoseInRigFrame(hipsJoint, hipsPose)) { + if (findPointKDopDisplacement(position, hipsPose, hipsShapeInfo, displacement)) { + position += displacement; + } + } + } + + int spineJoint = indexOfJoint("Spine"); + if (spineJoint >= 0) { + AnimPose spinePose; + if (getAbsoluteJointPoseInRigFrame(spineJoint, spinePose)) { + if (findPointKDopDisplacement(position, spinePose, spineShapeInfo, displacement)) { + position += displacement; + } + } + } + + int spine1Joint = indexOfJoint("Spine1"); + if (spine1Joint >= 0) { + AnimPose spine1Pose; + if (getAbsoluteJointPoseInRigFrame(spine1Joint, spine1Pose)) { + if (findPointKDopDisplacement(position, spine1Pose, spine1ShapeInfo, displacement)) { + position += displacement; + } + } + } + + int spine2Joint = indexOfJoint("Spine2"); + if (spine2Joint >= 0) { + AnimPose spine2Pose; + if (getAbsoluteJointPoseInRigFrame(spine2Joint, spine2Pose)) { + if (findPointKDopDisplacement(position, spine2Pose, spine2ShapeInfo, displacement)) { + position += displacement; + } + } + } + + return position; +} + +void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, bool leftArmEnabled, bool rightArmEnabled, float dt, + const AnimPose& leftHandPose, const AnimPose& rightHandPose, + const FBXJointShapeInfo& hipsShapeInfo, const FBXJointShapeInfo& spineShapeInfo, + const FBXJointShapeInfo& spine1ShapeInfo, const FBXJointShapeInfo& spine2ShapeInfo) { - const float HAND_RADIUS = 0.05f; const float ELBOW_POLE_VECTOR_BLEND_FACTOR = 0.95f; + int hipsIndex = indexOfJoint("Hips"); + if (leftHandEnabled) { glm::vec3 handPosition = leftHandPose.trans(); glm::quat handRotation = leftHandPose.rot(); if (!hipsEnabled) { - // prevent the hand IK targets from intersecting the body capsule - glm::vec3 displacement; - if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) { - handPosition -= displacement; - } + // prevent the hand IK targets from intersecting the torso + handPosition = deflectHandFromTorso(handPosition, hipsShapeInfo, spineShapeInfo, spine1ShapeInfo, spine2ShapeInfo); } _animVars.set("leftHandPosition", handPosition); @@ -1173,11 +1277,8 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab glm::quat handRotation = rightHandPose.rot(); if (!hipsEnabled) { - // prevent the hand IK targets from intersecting the body capsule - glm::vec3 displacement; - if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) { - handPosition -= displacement; - } + // prevent the hand IK targets from intersecting the torso + handPosition = deflectHandFromTorso(handPosition, hipsShapeInfo, spineShapeInfo, spine1ShapeInfo, spine2ShapeInfo); } _animVars.set("rightHandPosition", handPosition); @@ -1414,7 +1515,7 @@ void Rig::updateFromControllerParameters(const ControllerParameters& params, flo updateHands(leftHandEnabled, rightHandEnabled, hipsEnabled, leftArmEnabled, rightArmEnabled, dt, params.primaryControllerPoses[PrimaryControllerType_LeftHand], params.primaryControllerPoses[PrimaryControllerType_RightHand], - params.bodyCapsuleRadius, params.bodyCapsuleHalfHeight, params.bodyCapsuleLocalOffset); + params.hipsShapeInfo, params.spineShapeInfo, params.spine1ShapeInfo, params.spine2ShapeInfo); updateFeet(leftFootEnabled, rightFootEnabled, params.primaryControllerPoses[PrimaryControllerType_LeftFoot], params.primaryControllerPoses[PrimaryControllerType_RightFoot]); diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 7e1504e461..ca55635250 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -75,9 +75,10 @@ public: AnimPose secondaryControllerPoses[NumSecondaryControllerTypes]; // rig space bool secondaryControllerActiveFlags[NumSecondaryControllerTypes]; bool isTalking; - float bodyCapsuleRadius; - float bodyCapsuleHalfHeight; - glm::vec3 bodyCapsuleLocalOffset; + FBXJointShapeInfo hipsShapeInfo; + FBXJointShapeInfo spineShapeInfo; + FBXJointShapeInfo spine1ShapeInfo; + FBXJointShapeInfo spine2ShapeInfo; }; struct EyeParameters { @@ -249,7 +250,8 @@ protected: void updateHead(bool headEnabled, bool hipsEnabled, const AnimPose& headMatrix); void updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, bool leftArmEnabled, bool rightArmEnabled, float dt, const AnimPose& leftHandPose, const AnimPose& rightHandPose, - float bodyCapsuleRadius, float bodyCapsuleHalfHeight, const glm::vec3& bodyCapsuleLocalOffset); + const FBXJointShapeInfo& hipsShapeInfo, const FBXJointShapeInfo& spineShapeInfo, + const FBXJointShapeInfo& spine1ShapeInfo, const FBXJointShapeInfo& spine2ShapeInfo); void updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose& leftFootPose, const AnimPose& rightFootPose); void updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm::quat& modelRotation, const glm::vec3& lookAt, const glm::vec3& saccade); @@ -257,6 +259,8 @@ protected: glm::vec3 calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int hipsIndex, bool isLeft) const; glm::vec3 calculateKneePoleVector(int footJointIndex, int kneeJoint, int upLegIndex, int hipsIndex, const AnimPose& targetFootPose) const; + glm::vec3 deflectHandFromTorso(const glm::vec3& handPosition, const FBXJointShapeInfo& hipsShapeInfo, const FBXJointShapeInfo& spineShapeInfo, + const FBXJointShapeInfo& spine1ShapeInfo, const FBXJointShapeInfo& spine2ShapeInfo) const; AnimPose _modelOffset; // model to rig space AnimPose _geometryOffset; // geometry to model space (includes unit offset & fst offsets)