From baee5180c1a74e5825565beaf99282d9eb9758a3 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 12 Sep 2016 11:52:27 -0700 Subject: [PATCH] Moved outOfBody detection into MyAvatar::FollowHelper in preparation for different follow behavior for in-body and out-of-body. --- interface/src/avatar/MyAvatar.cpp | 21 +++++++++ interface/src/avatar/MyAvatar.h | 6 ++- interface/src/avatar/SkeletonModel.cpp | 33 +++++++++++++- libraries/animation/src/Rig.cpp | 63 -------------------------- libraries/shared/src/GeometryUtil.cpp | 31 +++++++++++++ libraries/shared/src/GeometryUtil.h | 3 ++ 6 files changed, 91 insertions(+), 66 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 53f37635f3..94b6c2f93c 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -82,8 +82,14 @@ const float MyAvatar::ZOOM_MIN = 0.5f; const float MyAvatar::ZOOM_MAX = 25.0f; const float MyAvatar::ZOOM_DEFAULT = 1.5f; +// OUTOFBODY_HACK defined in Rig.cpp extern bool OUTOFBODY_HACK_ENABLE_DEBUG_DRAW_IK_TARGETS; +// OUTOFBODY_HACK defined in SkeletonModel.cpp +extern const glm::vec3 TRUNCATE_IK_CAPSULE_POSITION; +extern const float TRUNCATE_IK_CAPSULE_LENGTH; +extern const float TRUNCATE_IK_CAPSULE_RADIUS; + MyAvatar::MyAvatar(RigPointer rig) : Avatar(rig), _wasPushing(false), @@ -1338,6 +1344,8 @@ void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) { } else { setVelocity(getVelocity() + _characterController.getFollowVelocity()); } + + _follow.postPhysicsUpdate(*this); } QString MyAvatar::getScriptedMotorFrame() const { @@ -2183,6 +2191,19 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat } } +void MyAvatar::FollowHelper::postPhysicsUpdate(MyAvatar& myAvatar) { + + // 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(myAvatar.getRotation() * Quaternions::Y_180, myAvatar.getPosition()); + glm::mat4 worldToRig = glm::inverse(rigToWorld); + glm::mat4 rigHMDMat = worldToRig * worldHMDMat; + glm::vec3 rigHMDPosition = extractTranslation(rigHMDMat); + + // detect if the rig head position is too far from the avatar's position. + _isOutOfBody = !pointIsInsideCapsule(rigHMDPosition, TRUNCATE_IK_CAPSULE_POSITION, TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS); +} + float MyAvatar::getAccelerationEnergy() { glm::vec3 velocity = getVelocity(); int changeInVelocity = abs(velocity.length() - priorVelocity.length()); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 65d0de3801..f6833c6f33 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -315,6 +315,8 @@ public slots: glm::vec3 getPositionForAudio(); glm::quat getOrientationForAudio(); + bool isOutOfBody() const { return _follow._isOutOfBody; } + signals: void audioListenerModeChanged(); void transformChanged(); @@ -445,7 +447,8 @@ private: NumFollowTypes }; glm::mat4 _desiredBodyMatrix; - uint8_t _activeBits; + uint8_t _activeBits { 0 }; + bool _isOutOfBody { false }; void deactivate(); void deactivate(FollowType type); @@ -457,6 +460,7 @@ private: void updateHorizontalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix); void updateVerticalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix); void prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& bodySensorMatrix, const glm::mat4& currentBodyMatrix); + void postPhysicsUpdate(MyAvatar& myAvatar); }; FollowHelper _follow; diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 889f0ef36b..be1d071f6b 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -23,6 +23,10 @@ #include "InterfaceLogging.h" #include "AnimDebugDraw.h" +const glm::vec3 TRUNCATE_IK_CAPSULE_POSITION(0.0f, 0.0f, 0.0f); +const float TRUNCATE_IK_CAPSULE_LENGTH = 1000.0f; +const float TRUNCATE_IK_CAPSULE_RADIUS = 0.5f; + SkeletonModel::SkeletonModel(Avatar* owningAvatar, QObject* parent, RigPointer rig) : Model(rig, parent), _owningAvatar(owningAvatar), @@ -86,7 +90,6 @@ Rig::CharacterControllerState convertCharacterControllerState(CharacterControlle }; } - // Called within Model::simulate call, below. void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { const FBXGeometry& geometry = getFBXGeometry(); @@ -107,6 +110,9 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { Rig::HeadParameters headParams; + glm::vec3 hmdPositionInRigSpace; + glm::vec3 truncatedHMDPositionInRigSpace; + if (qApp->isHMDMode()) { headParams.isInHMD = true; @@ -116,9 +122,20 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { glm::mat4 worldToRig = glm::inverse(rigToWorld); glm::mat4 rigHMDMat = worldToRig * worldHMDMat; - headParams.rigHeadPosition = extractTranslation(rigHMDMat); + hmdPositionInRigSpace = extractTranslation(rigHMDMat); + + // truncate head IK target if it's out of body + if (myAvatar->isOutOfBody()) { + truncatedHMDPositionInRigSpace = projectPointOntoCapsule(hmdPositionInRigSpace, TRUNCATE_IK_CAPSULE_POSITION, + TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS); + } else { + truncatedHMDPositionInRigSpace = hmdPositionInRigSpace; + } + + headParams.rigHeadPosition = truncatedHMDPositionInRigSpace; headParams.rigHeadOrientation = extractRotation(rigHMDMat); headParams.worldHeadOrientation = extractRotation(worldHMDMat); + } else { headParams.isInHMD = false; @@ -139,6 +156,12 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { handParams.isLeftEnabled = true; handParams.leftPosition = Quaternions::Y_180 * leftPose.getTranslation(); handParams.leftOrientation = Quaternions::Y_180 * leftPose.getRotation(); + + // truncate hand target + if (myAvatar->isOutOfBody() && qApp->isHMDMode()) { + glm::vec3 offset = handParams.leftPosition - hmdPositionInRigSpace; + handParams.leftPosition = truncatedHMDPositionInRigSpace + offset; + } } else { handParams.isLeftEnabled = false; } @@ -148,6 +171,12 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { handParams.isRightEnabled = true; handParams.rightPosition = Quaternions::Y_180 * rightPose.getTranslation(); handParams.rightOrientation = Quaternions::Y_180 * rightPose.getRotation(); + + // truncate hand target + if (myAvatar->isOutOfBody() && qApp->isHMDMode()) { + glm::vec3 offset = handParams.rightPosition - hmdPositionInRigSpace; + handParams.rightPosition = truncatedHMDPositionInRigSpace + offset; + } } else { handParams.isRightEnabled = false; } diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index f0c7f85011..2d11a1e17f 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -47,10 +47,6 @@ const glm::vec3 DEFAULT_LEFT_EYE_POS(0.3f, 0.9f, 0.0f); const glm::vec3 DEFAULT_HEAD_POS(0.0f, 0.75f, 0.0f); const glm::vec3 DEFAULT_NECK_POS(0.0f, 0.70f, 0.0f); -const glm::vec3 TRUNCATE_IK_CAPSULE_POSITION(0.0f, 0.0f, 0.0f); -float TRUNCATE_IK_CAPSULE_LENGTH = 1000.0; -float TRUNCATE_IK_CAPSULE_RADIUS = 0.5; - extern Rig* OUTOFBODY_HACK_RIG_POINTER; void Rig::overrideAnimation(const QString& url, float fps, bool loop, float firstFrame, float lastFrame) { @@ -1003,37 +999,6 @@ void Rig::computeHeadNeckAnimVars(const AnimPose& hmdPose, glm::vec3& headPositi neckOrientationOut = safeMix(hmdOrientation, _animSkeleton->getRelativeDefaultPose(neckIndex).rot, 0.5f); } -static bool pointIsInsideCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius) { - glm::vec3 top = capsulePosition.y + glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); - glm::vec3 bottom = capsulePosition.y - glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); - if (point.y > top.y + capsuleRadius) { - return false; - } else if (point.y > top.y) { - return glm::length(point - top) < capsuleRadius; - } else if (point.y < bottom.y - capsuleRadius) { - return false; - } else if (point.y < bottom.y) { - return glm::length(point - bottom) < capsuleRadius; - } else { - return glm::length(glm::vec2(point.x, point.z) - glm::vec2(capsulePosition.x, capsulePosition.z)) < capsuleRadius; - } -} - -static glm::vec3 projectPointOntoCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius) { - glm::vec3 top = capsulePosition.y + glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); - glm::vec3 bottom = capsulePosition.y - glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); - if (point.y > top.y) { - return capsuleRadius * glm::normalize(point - top) + top; - } else if (point.y < bottom.y) { - return capsuleRadius * glm::normalize(point - bottom) + bottom; - } else { - glm::vec2 capsulePosition2D(capsulePosition.x, capsulePosition.z); - glm::vec2 point2D(point.x, point.z); - glm::vec2 projectedPoint2D = capsuleRadius * glm::normalize(point2D - capsulePosition2D) + capsulePosition2D; - return glm::vec3(projectedPoint2D.x, point.y, projectedPoint2D.y); - } -} - void Rig::updateNeckJoint(int index, const HeadParameters& params) { if (_animSkeleton && index >= 0 && index < _animSkeleton->getNumJoints()) { glm::quat yFlip180 = glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f)); @@ -1044,20 +1009,6 @@ void Rig::updateNeckJoint(int index, const HeadParameters& params) { AnimPose hmdPose(glm::vec3(1.0f), params.rigHeadOrientation * yFlip180, params.rigHeadPosition); computeHeadNeckAnimVars(hmdPose, headPos, headRot, neckPos, neckRot); - // decide if we SHOULD truncate IK targets - if (!pointIsInsideCapsule(params.rigHeadPosition, TRUNCATE_IK_CAPSULE_POSITION, TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS)) { - _desiredRigHeadPosition = headPos; - _truncateIKTargets = true; - } else { - _truncateIKTargets = false; - } - - // truncate head IK target. - if (_truncateIKTargets) { - headPos = projectPointOntoCapsule(_desiredRigHeadPosition, TRUNCATE_IK_CAPSULE_POSITION, TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS); - neckPos = (neckPos - _desiredRigHeadPosition) + headPos; - } - _animVars.set("headPosition", headPos); _animVars.set("headRotation", headRot); _animVars.set("headType", (int)IKTarget::Type::HmdHead); @@ -1132,13 +1083,6 @@ void Rig::updateFromHandParameters(const HandParameters& params, float dt) { glm::vec3 handPosition = params.leftPosition; - // truncate hand IK target - if (_truncateIKTargets) { - glm::vec3 offset = handPosition - _desiredRigHeadPosition; - glm::vec3 headPos = projectPointOntoCapsule(_desiredRigHeadPosition, TRUNCATE_IK_CAPSULE_POSITION, TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS); - handPosition = headPos + offset; - } - // prevent the hand IK targets from intersecting the body capsule glm::vec3 displacement(glm::vec3::_null); if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) { @@ -1158,13 +1102,6 @@ void Rig::updateFromHandParameters(const HandParameters& params, float dt) { glm::vec3 handPosition = params.rightPosition; - // truncate hand IK target - if (_truncateIKTargets) { - glm::vec3 offset = handPosition - _desiredRigHeadPosition; - glm::vec3 headPos = projectPointOntoCapsule(_desiredRigHeadPosition, TRUNCATE_IK_CAPSULE_POSITION, TRUNCATE_IK_CAPSULE_LENGTH, TRUNCATE_IK_CAPSULE_RADIUS); - handPosition = headPos + offset; - } - // prevent the hand IK targets from intersecting the body capsule glm::vec3 displacement(glm::vec3::_null); if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) { diff --git a/libraries/shared/src/GeometryUtil.cpp b/libraries/shared/src/GeometryUtil.cpp index 92fe138021..bc4dc7cb9a 100644 --- a/libraries/shared/src/GeometryUtil.cpp +++ b/libraries/shared/src/GeometryUtil.cpp @@ -578,3 +578,34 @@ float coneSphereAngle(const glm::vec3& coneCenter, const glm::vec3& coneDirectio return glm::max(0.0f, theta - phi); } + +bool pointIsInsideCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius) { + glm::vec3 top = capsulePosition.y + glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); + glm::vec3 bottom = capsulePosition.y - glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); + if (point.y > top.y + capsuleRadius) { + return false; + } else if (point.y > top.y) { + return glm::length(point - top) < capsuleRadius; + } else if (point.y < bottom.y - capsuleRadius) { + return false; + } else if (point.y < bottom.y) { + return glm::length(point - bottom) < capsuleRadius; + } else { + return glm::length(glm::vec2(point.x, point.z) - glm::vec2(capsulePosition.x, capsulePosition.z)) < capsuleRadius; + } +} + +glm::vec3 projectPointOntoCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius) { + glm::vec3 top = capsulePosition.y + glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); + glm::vec3 bottom = capsulePosition.y - glm::vec3(0.0f, capsuleLength / 2.0f, 0.0f); + if (point.y > top.y) { + return capsuleRadius * glm::normalize(point - top) + top; + } else if (point.y < bottom.y) { + return capsuleRadius * glm::normalize(point - bottom) + bottom; + } else { + glm::vec2 capsulePosition2D(capsulePosition.x, capsulePosition.z); + glm::vec2 point2D(point.x, point.z); + glm::vec2 projectedPoint2D = capsuleRadius * glm::normalize(point2D - capsulePosition2D) + capsulePosition2D; + return glm::vec3(projectedPoint2D.x, point.y, projectedPoint2D.y); + } +} diff --git a/libraries/shared/src/GeometryUtil.h b/libraries/shared/src/GeometryUtil.h index 1c951ca09a..01459e51a9 100644 --- a/libraries/shared/src/GeometryUtil.h +++ b/libraries/shared/src/GeometryUtil.h @@ -160,5 +160,8 @@ private: static void copyCleanArray(int& lengthA, glm::vec2* vertexArrayA, int& lengthB, glm::vec2* vertexArrayB); }; +// vertical capsule +bool pointIsInsideCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius); +glm::vec3 projectPointOntoCapsule(const glm::vec3& point, const glm::vec3& capsulePosition, float capsuleLength, float capsuleRadius); #endif // hifi_GeometryUtil_h