From ab19d3e5a2a4ec261377c42a53efa768a803b386 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 10 Mar 2016 20:44:35 -0800 Subject: [PATCH] Moved animation after physics, hand poses are in world frame. There are still some debug hacks enabled so I can test in the vive room. --- interface/src/Application.cpp | 7 +- interface/src/avatar/AvatarActionHold.cpp | 4 +- interface/src/avatar/MyAvatar.cpp | 68 ++++++++++++-------- interface/src/avatar/MyAvatar.h | 13 ++-- interface/src/avatar/SkeletonModel.cpp | 4 +- libraries/render-utils/src/AnimDebugDraw.cpp | 7 +- plugins/hifiSixense/src/SixenseManager.cpp | 6 ++ 7 files changed, 68 insertions(+), 41 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 728691a0e1..42430b3a4c 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -3218,13 +3218,12 @@ void Application::update(float deltaTime) { controller::Pose leftHandPose = userInputMapper->getPoseState(controller::Action::LEFT_HAND); controller::Pose rightHandPose = userInputMapper->getPoseState(controller::Action::RIGHT_HAND); - myAvatar->setHandControllerPoses(leftHandPose, rightHandPose); + auto myAvatarMatrix = createMatFromQuatAndPos(myAvatar->getOrientation(), myAvatar->getPosition()); + myAvatar->setHandControllerPosesInWorldFrame(leftHandPose.transform(myAvatarMatrix), rightHandPose.transform(myAvatarMatrix)); updateThreads(deltaTime); // If running non-threaded, then give the threads some time to process... updateDialogs(deltaTime); // update various stats dialogs if present - _avatarUpdate->synchronousProcess(); - if (_physicsEnabled) { PerformanceTimer perfTimer("physics"); AvatarManager* avatarManager = DependencyManager::get().data(); @@ -3296,6 +3295,8 @@ void Application::update(float deltaTime) { } } + _avatarUpdate->synchronousProcess(); + { PerformanceTimer perfTimer("overlays"); _overlays.update(deltaTime); diff --git a/interface/src/avatar/AvatarActionHold.cpp b/interface/src/avatar/AvatarActionHold.cpp index 0ce0b5a190..5087f7955d 100644 --- a/interface/src/avatar/AvatarActionHold.cpp +++ b/interface/src/avatar/AvatarActionHold.cpp @@ -113,9 +113,9 @@ std::shared_ptr AvatarActionHold::getTarget(float deltaTimeStep, glm::qu // fetch the hand controller pose controller::Pose pose; if (isRightHand) { - pose = avatarManager->getMyAvatar()->getRightHandControllerPose(); + pose = avatarManager->getMyAvatar()->getRightHandControllerPoseInWorldFrame(); } else { - pose = avatarManager->getMyAvatar()->getLeftHandControllerPose(); + pose = avatarManager->getMyAvatar()->getLeftHandControllerPoseInWorldFrame(); } if (pose.isValid()) { diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 016e2a464c..c018a76168 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -512,39 +512,38 @@ void MyAvatar::updateFromTrackers(float deltaTime) { -MAX_LEAN, MAX_LEAN)); } - glm::vec3 MyAvatar::getLeftHandPosition() const { - auto pose = getLeftHandControllerPose(); + auto pose = getLeftHandControllerPoseInAvatarFrame(); return pose.isValid() ? pose.getTranslation() : glm::vec3(0.0f); } glm::vec3 MyAvatar::getRightHandPosition() const { - auto pose = getRightHandControllerPose(); + auto pose = getRightHandControllerPoseInAvatarFrame(); return pose.isValid() ? pose.getTranslation() : glm::vec3(0.0f); } glm::vec3 MyAvatar::getLeftHandTipPosition() const { const float TIP_LENGTH = 0.3f; - auto pose = getLeftHandControllerPose(); + auto pose = getLeftHandControllerPoseInAvatarFrame(); return pose.isValid() ? pose.getTranslation() * pose.getRotation() + glm::vec3(0.0f, TIP_LENGTH, 0.0f) : glm::vec3(0.0f); } glm::vec3 MyAvatar::getRightHandTipPosition() const { const float TIP_LENGTH = 0.3f; - auto pose = getRightHandControllerPose(); + auto pose = getRightHandControllerPoseInAvatarFrame(); return pose.isValid() ? pose.getTranslation() * pose.getRotation() + glm::vec3(0.0f, TIP_LENGTH, 0.0f) : glm::vec3(0.0f); } controller::Pose MyAvatar::getLeftHandPose() const { - return getLeftHandControllerPose(); + return getLeftHandControllerPoseInAvatarFrame(); } controller::Pose MyAvatar::getRightHandPose() const { - return getRightHandControllerPose(); + return getRightHandControllerPoseInAvatarFrame(); } controller::Pose MyAvatar::getLeftHandTipPose() const { - auto pose = getLeftHandControllerPose(); + auto pose = getLeftHandControllerPoseInAvatarFrame(); glm::vec3 tipTrans = getLeftHandTipPosition(); pose.velocity += glm::cross(pose.getAngularVelocity(), pose.getTranslation() - tipTrans); pose.translation = tipTrans; @@ -552,7 +551,7 @@ controller::Pose MyAvatar::getLeftHandTipPose() const { } controller::Pose MyAvatar::getRightHandTipPose() const { - auto pose = getRightHandControllerPose(); + auto pose = getRightHandControllerPoseInAvatarFrame(); glm::vec3 tipTrans = getRightHandTipPosition(); pose.velocity += glm::cross(pose.getAngularVelocity(), pose.getTranslation() - tipTrans); pose.translation = tipTrans; @@ -697,8 +696,8 @@ void MyAvatar::setEnableDebugDrawPosition(bool isEnabled) { void MyAvatar::setEnableDebugDrawHandControllers(bool isEnabled) { _enableDebugDrawHandControllers = isEnabled; if (!isEnabled) { - DebugDraw::getInstance().removeMyAvatarMarker("leftHandController"); - DebugDraw::getInstance().removeMyAvatarMarker("rightHandController"); + DebugDraw::getInstance().removeMarker("leftHandController"); + DebugDraw::getInstance().removeMarker("rightHandController"); } } @@ -1096,24 +1095,34 @@ static controller::Pose applyLowVelocityFilter(const controller::Pose& oldPose, return finalPose; } -void MyAvatar::setHandControllerPoses(const controller::Pose& left, const controller::Pose& right) { +void MyAvatar::setHandControllerPosesInWorldFrame(const controller::Pose& left, const controller::Pose& right) { if (controller::InputDevice::getLowVelocityFilter()) { - auto oldLeftPose = getLeftHandControllerPose(); - auto oldRightPose = getRightHandControllerPose(); - _leftHandControllerPoseCache.set(applyLowVelocityFilter(oldLeftPose, left)); - _rightHandControllerPoseCache.set(applyLowVelocityFilter(oldRightPose, right)); + auto oldLeftPose = getLeftHandControllerPoseInWorldFrame(); + auto oldRightPose = getRightHandControllerPoseInWorldFrame(); + _leftHandControllerPoseInWorldFrameCache.set(applyLowVelocityFilter(oldLeftPose, left)); + _rightHandControllerPoseInWorldFrameCache.set(applyLowVelocityFilter(oldRightPose, right)); } else { - _leftHandControllerPoseCache.set(left); - _rightHandControllerPoseCache.set(right); + _leftHandControllerPoseInWorldFrameCache.set(left); + _rightHandControllerPoseInWorldFrameCache.set(right); } } -controller::Pose MyAvatar::getLeftHandControllerPose() const { - return _leftHandControllerPoseCache.get(); +controller::Pose MyAvatar::getLeftHandControllerPoseInWorldFrame() const { + return _leftHandControllerPoseInWorldFrameCache.get(); } -controller::Pose MyAvatar::getRightHandControllerPose() const { - return _rightHandControllerPoseCache.get(); +controller::Pose MyAvatar::getRightHandControllerPoseInWorldFrame() const { + return _rightHandControllerPoseInWorldFrameCache.get(); +} + +controller::Pose MyAvatar::getLeftHandControllerPoseInAvatarFrame() const { + glm::mat4 invAvatarMatrix = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition())); + return getLeftHandControllerPoseInWorldFrame().transform(invAvatarMatrix); +} + +controller::Pose MyAvatar::getRightHandControllerPoseInAvatarFrame() const { + glm::mat4 invAvatarMatrix = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition())); + return getRightHandControllerPoseInWorldFrame().transform(invAvatarMatrix); } void MyAvatar::prepareForPhysicsSimulation() { @@ -1360,20 +1369,23 @@ void MyAvatar::preRender(RenderArgs* renderArgs) { } if (_enableDebugDrawHandControllers) { - auto leftHandPose = getLeftHandControllerPose(); - auto rightHandPose = getRightHandControllerPose(); + auto leftHandPose = getLeftHandControllerPoseInWorldFrame(); + auto rightHandPose = getRightHandControllerPoseInWorldFrame(); if (leftHandPose.isValid()) { - DebugDraw::getInstance().addMyAvatarMarker("leftHandController", leftHandPose.getRotation(), leftHandPose.getTranslation(), glm::vec4(1)); + DebugDraw::getInstance().addMarker("leftHandController", leftHandPose.getRotation(), leftHandPose.getTranslation(), glm::vec4(1)); } else { - DebugDraw::getInstance().removeMyAvatarMarker("leftHandController"); + DebugDraw::getInstance().removeMarker("leftHandController"); } if (rightHandPose.isValid()) { - DebugDraw::getInstance().addMyAvatarMarker("rightHandController", rightHandPose.getRotation(), rightHandPose.getTranslation(), glm::vec4(1)); + DebugDraw::getInstance().addMarker("rightHandController", rightHandPose.getRotation(), rightHandPose.getTranslation(), glm::vec4(1)); } else { - DebugDraw::getInstance().removeMyAvatarMarker("rightHandController"); + DebugDraw::getInstance().removeMarker("rightHandController"); } + + // AJT: REMOVE + DebugDraw::getInstance().addMyAvatarMarker("REFERENCE", glm::quat(), glm::vec3(0.0f, 0.7f, -0.5f), glm::vec4(1)); } DebugDraw::getInstance().updateMyAvatarPos(getPosition()); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 0092280762..2301e37a5b 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -249,9 +249,11 @@ public: virtual void rebuildCollisionShape() override; - void setHandControllerPoses(const controller::Pose& left, const controller::Pose& right); - controller::Pose MyAvatar::getLeftHandControllerPose() const; - controller::Pose MyAvatar::getRightHandControllerPose() const; + void setHandControllerPosesInWorldFrame(const controller::Pose& left, const controller::Pose& right); + controller::Pose getLeftHandControllerPoseInWorldFrame() const; + controller::Pose getRightHandControllerPoseInWorldFrame() const; + controller::Pose getLeftHandControllerPoseInAvatarFrame() const; + controller::Pose getRightHandControllerPoseInAvatarFrame() const; public slots: void increaseSize(); @@ -450,8 +452,9 @@ private: bool _hoverReferenceCameraFacingIsCaptured { false }; glm::vec3 _hoverReferenceCameraFacing { 0.0f, 0.0f, -1.0f }; // hmd sensor space - ThreadSafeValueCache _leftHandControllerPoseCache { controller::Pose() }; - ThreadSafeValueCache _rightHandControllerPoseCache { controller::Pose() }; + // These are stored in WORLD frame + ThreadSafeValueCache _leftHandControllerPoseInWorldFrameCache { controller::Pose() }; + ThreadSafeValueCache _rightHandControllerPoseInWorldFrameCache { controller::Pose() }; float AVATAR_MOVEMENT_ENERGY_CONSTANT { 0.001f }; float AUDIO_ENERGY_CONSTANT { 0.000001f }; diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index d659d79dd9..46a091cf35 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -126,7 +126,7 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { Rig::HandParameters handParams; - auto leftPose = myAvatar->getLeftHandControllerPose(); + auto leftPose = myAvatar->getLeftHandControllerPoseInAvatarFrame(); if (leftPose.isValid()) { handParams.isLeftEnabled = true; handParams.leftPosition = Quaternions::Y_180 * leftPose.getTranslation(); @@ -135,7 +135,7 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { handParams.isLeftEnabled = false; } - auto rightPose = myAvatar->getRightHandControllerPose(); + auto rightPose = myAvatar->getRightHandControllerPoseInAvatarFrame(); if (rightPose.isValid()) { handParams.isRightEnabled = true; handParams.rightPosition = Quaternions::Y_180 * rightPose.getTranslation(); diff --git a/libraries/render-utils/src/AnimDebugDraw.cpp b/libraries/render-utils/src/AnimDebugDraw.cpp index c9abf71fad..44bf533739 100644 --- a/libraries/render-utils/src/AnimDebugDraw.cpp +++ b/libraries/render-utils/src/AnimDebugDraw.cpp @@ -392,13 +392,18 @@ void AnimDebugDraw::update() { assert(numVerts == (v - verts)); + // This render item bound shit is broken. + // Fuck that, use the big ass bound instead. + /* render::Item::Bound theBound; for (int i = 0; i < numVerts; i++) { theBound += verts[i].pos; } + */ data._isVisible = (numVerts > 0); - data._bound = theBound; + + //data._bound = theBound; data._indexBuffer->resize(sizeof(uint16_t) * numVerts); uint16_t* indices = (uint16_t*)data._indexBuffer->editData(); for (int i = 0; i < numVerts; i++) { diff --git a/plugins/hifiSixense/src/SixenseManager.cpp b/plugins/hifiSixense/src/SixenseManager.cpp index 9fdce3add4..67192edebd 100644 --- a/plugins/hifiSixense/src/SixenseManager.cpp +++ b/plugins/hifiSixense/src/SixenseManager.cpp @@ -481,6 +481,12 @@ void SixenseManager::InputDevice::handlePoseEvent(float deltaTime, const control // transform pose into avatar frame. auto nextPose = controller::Pose(pos, rot, velocity, angularVelocity).transform(controllerToAvatar); + if (!left) { + // AJT: HACK TO DEBUG IK + nextPose.translation = glm::vec3(0.25f, 0.7f, -0.5f); + nextPose.rotation = glm::quat(); + } + if (prevPose.isValid() && (deltaTime > std::numeric_limits::epsilon())) { nextPose.velocity = (nextPose.getTranslation() - prevPose.getTranslation()) / deltaTime;