From 70ca1106b11a3d38704056b20855d8e41f2dbaa0 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Tue, 3 Dec 2013 17:28:36 -0800 Subject: [PATCH] Working on testing the Leap/Hydra hands against the avatar body. --- interface/src/Util.cpp | 5 ++- interface/src/Util.h | 2 + interface/src/avatar/Hand.cpp | 20 +++++++++ interface/src/avatar/SkeletonModel.cpp | 21 ++------- interface/src/renderer/Model.cpp | 60 ++++++++++++++++++++++---- interface/src/renderer/Model.h | 9 ++++ libraries/avatars/src/HandData.cpp | 42 ++++++++++++++++++ libraries/avatars/src/HandData.h | 15 +++++-- 8 files changed, 144 insertions(+), 30 deletions(-) diff --git a/interface/src/Util.cpp b/interface/src/Util.cpp index a5c12b3edc..476e77cd97 100644 --- a/interface/src/Util.cpp +++ b/interface/src/Util.cpp @@ -229,7 +229,10 @@ glm::vec3 extractScale(const glm::mat4& matrix) { } float extractUniformScale(const glm::mat4& matrix) { - glm::vec3 scale = extractScale(matrix); + return extractUniformScale(extractScale(matrix)); +} + +float extractUniformScale(const glm::vec3& scale) { return (scale.x + scale.y + scale.z) / 3.0f; } diff --git a/interface/src/Util.h b/interface/src/Util.h index b80731bd20..d5254a3904 100644 --- a/interface/src/Util.h +++ b/interface/src/Util.h @@ -65,6 +65,8 @@ glm::vec3 extractScale(const glm::mat4& matrix); float extractUniformScale(const glm::mat4& matrix); +float extractUniformScale(const glm::vec3& scale); + double diffclock(timeval *clock1,timeval *clock2); void renderMouseVoxelGrid(const float& mouseVoxelX, const float& mouseVoxelY, const float& mouseVoxelZ, const float& mouseVoxelS); diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp index f733a40c9f..658ca2bd89 100755 --- a/interface/src/avatar/Hand.cpp +++ b/interface/src/avatar/Hand.cpp @@ -97,6 +97,26 @@ void Hand::calculateGeometry() { _baseOrientation = _owningAvatar->getOrientation(); _basePosition = head.calculateAverageEyePosition() + _baseOrientation * leapHandsOffsetFromFace * head.getScale(); + // use position to obtain the left and right palm indices + int leftPalmIndex, rightPalmIndex; + getLeftRightPalmIndices(leftPalmIndex, rightPalmIndex); + + // check for collisions + for (int i = 0; i < getNumPalms(); i++) { + PalmData& palm = getPalms()[i]; + if (!palm.isActive()) { + continue; + } + const float PALM_RADIUS = 0.01f; + glm::vec3 penetration; + int skipIndex = (i == leftPalmIndex) ? _owningAvatar->getSkeletonModel().getLeftHandJointIndex() : + (i == rightPalmIndex) ? _owningAvatar->getSkeletonModel().getRightHandJointIndex() : -1; + if (_owningAvatar->getSkeletonModel().findSpherePenetration(palm.getPosition(), + PALM_RADIUS * _owningAvatar->getScale(), penetration, skipIndex)) { + palm.addToPosition(-penetration); + } + } + // generate finger tip balls.... _leapFingerTipBalls.clear(); for (size_t i = 0; i < getNumPalms(); ++i) { diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 42c57ed5bd..d3d7b6198c 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -29,24 +29,9 @@ void SkeletonModel::simulate(float deltaTime) { Model::simulate(deltaTime); // find the left and rightmost active Leap palms - HandData& hand = _owningAvatar->getHand(); - int leftPalmIndex = -1; - float leftPalmX = FLT_MAX; - int rightPalmIndex = -1; - float rightPalmX = -FLT_MAX; - for (int i = 0; i < hand.getNumPalms(); i++) { - if (hand.getPalms()[i].isActive()) { - float x = hand.getPalms()[i].getRawPosition().x; - if (x < leftPalmX) { - leftPalmIndex = i; - leftPalmX = x; - } - if (x > rightPalmX) { - rightPalmIndex = i; - rightPalmX = x; - } - } - } + int leftPalmIndex, rightPalmIndex; + HandData& hand = _owningAvatar->getHand(); + hand.getLeftRightPalmIndices(leftPalmIndex, rightPalmIndex); const float HAND_RESTORATION_RATE = 0.25f; diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp index 8be7a967f7..c2937f4cea 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -8,6 +8,8 @@ #include +#include + #include "Application.h" #include "Model.h" @@ -474,35 +476,35 @@ bool Model::getEyePositions(glm::vec3& firstEyePosition, glm::vec3& secondEyePos } bool Model::setLeftHandPosition(const glm::vec3& position) { - return isActive() && setJointPosition(_geometry->getFBXGeometry().leftHandJointIndex, position); + return setJointPosition(getLeftHandJointIndex(), position); } bool Model::restoreLeftHandPosition(float percent) { - return isActive() && restoreJointPosition(_geometry->getFBXGeometry().leftHandJointIndex, percent); + return restoreJointPosition(getLeftHandJointIndex(), percent); } bool Model::setLeftHandRotation(const glm::quat& rotation) { - return isActive() && setJointRotation(_geometry->getFBXGeometry().leftHandJointIndex, rotation); + return setJointRotation(getLeftHandJointIndex(), rotation); } float Model::getLeftArmLength() const { - return isActive() ? getLimbLength(_geometry->getFBXGeometry().leftHandJointIndex) : 0.0f; + return getLimbLength(getLeftHandJointIndex()); } bool Model::setRightHandPosition(const glm::vec3& position) { - return isActive() && setJointPosition(_geometry->getFBXGeometry().rightHandJointIndex, position); + return setJointPosition(getRightHandJointIndex(), position); } bool Model::restoreRightHandPosition(float percent) { - return isActive() && restoreJointPosition(_geometry->getFBXGeometry().rightHandJointIndex, percent); + return restoreJointPosition(getRightHandJointIndex(), percent); } bool Model::setRightHandRotation(const glm::quat& rotation) { - return isActive() && setJointRotation(_geometry->getFBXGeometry().rightHandJointIndex, rotation); + return setJointRotation(getRightHandJointIndex(), rotation); } float Model::getRightArmLength() const { - return isActive() ? getLimbLength(_geometry->getFBXGeometry().rightHandJointIndex) : 0.0f; + return getLimbLength(getRightHandJointIndex()); } void Model::setURL(const QUrl& url) { @@ -523,6 +525,48 @@ glm::vec4 Model::computeAverageColor() const { return _geometry ? _geometry->computeAverageColor() : glm::vec4(1.0f, 1.0f, 1.0f, 1.0f); } +bool Model::findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, + glm::vec3& penetration, int skipIndex) const { + const glm::vec3 relativeCenter = penetratorCenter - _translation; + const FBXGeometry& geometry = _geometry->getFBXGeometry(); + bool didPenetrate = false; + glm::vec3 totalPenetration; + float radiusScale = extractUniformScale(_scale); + for (int i = 0; i < _jointStates.size(); i++) { + const FBXJoint& joint = geometry.joints[i]; + glm::vec3 end = extractTranslation(_jointStates[i].transform); + float endRadius = joint.boneRadius * radiusScale; + glm::vec3 start = end; + float startRadius = joint.boneRadius * radiusScale; + glm::vec3 bonePenetration; + if (joint.parentIndex != -1) { + if (skipIndex != -1) { + int ancestorIndex = joint.parentIndex; + do { + if (ancestorIndex == skipIndex) { + goto outerContinue; + } + ancestorIndex = geometry.joints[ancestorIndex].parentIndex; + + } while (ancestorIndex != -1); + } + start = extractTranslation(_jointStates[joint.parentIndex].transform); + startRadius = geometry.joints[joint.parentIndex].boneRadius; + } + if (findSphereCapsulePenetration(relativeCenter, penetratorRadius, start, end, + (startRadius + endRadius) / 2.0f, bonePenetration)) { + totalPenetration = addPenetrations(totalPenetration, bonePenetration); + didPenetrate = true; + } + outerContinue: ; + } + if (didPenetrate) { + penetration = totalPenetration; + return true; + } + return false; +} + void Model::updateJointState(int index) { JointState& state = _jointStates[index]; const FBXGeometry& geometry = _geometry->getFBXGeometry(); diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h index ed2d292ee4..171d45442f 100644 --- a/interface/src/renderer/Model.h +++ b/interface/src/renderer/Model.h @@ -54,6 +54,12 @@ public: Q_INVOKABLE void setURL(const QUrl& url); const QUrl& getURL() const { return _url; } + /// Returns the index of the left hand joint, or -1 if not found. + int getLeftHandJointIndex() const { return isActive() ? _geometry->getFBXGeometry().leftHandJointIndex : -1; } + + /// Returns the index of the right hand joint, or -1 if not found. + int getRightHandJointIndex() const { return isActive() ? _geometry->getFBXGeometry().rightHandJointIndex : -1; } + /// Returns the position of the head joint. /// \return whether or not the head was found bool getHeadPosition(glm::vec3& headPosition) const; @@ -105,6 +111,9 @@ public: /// Returns the average color of all meshes in the geometry. glm::vec4 computeAverageColor() const; + bool findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, + glm::vec3& penetration, int skipIndex = -1) const; + protected: QSharedPointer _geometry; diff --git a/libraries/avatars/src/HandData.cpp b/libraries/avatars/src/HandData.cpp index cfa43725b8..9ea3084ffa 100644 --- a/libraries/avatars/src/HandData.cpp +++ b/libraries/avatars/src/HandData.cpp @@ -29,11 +29,40 @@ HandData::HandData(AvatarData* owningAvatar) : addNewPalm(); } +glm::vec3 HandData::worldPositionToLeapPosition(const glm::vec3& worldPosition) const { + return glm::inverse(_baseOrientation) * (worldPosition - _basePosition) / LEAP_UNIT_SCALE; +} + +glm::vec3 HandData::worldVectorToLeapVector(const glm::vec3& worldVector) const { + return glm::inverse(_baseOrientation) * worldVector / LEAP_UNIT_SCALE; +} + PalmData& HandData::addNewPalm() { _palms.push_back(PalmData(this)); return _palms.back(); } +void HandData::getLeftRightPalmIndices(int& leftPalmIndex, int& rightPalmIndex) const { + leftPalmIndex = -1; + float leftPalmX = FLT_MAX; + rightPalmIndex = -1; + float rightPalmX = -FLT_MAX; + for (int i = 0; i < _palms.size(); i++) { + const PalmData& palm = _palms[i]; + if (palm.isActive()) { + float x = palm.getRawPosition().x; + if (x < leftPalmX) { + leftPalmIndex = i; + leftPalmX = x; + } + if (x > rightPalmX) { + rightPalmIndex = i; + rightPalmX = x; + } + } + } +} + PalmData::PalmData(HandData* owningHandData) : _rawPosition(0, 0, 0), _rawNormal(0, 1, 0), @@ -49,6 +78,19 @@ _owningHandData(owningHandData) } } +void PalmData::addToPosition(const glm::vec3& delta) { + // convert to Leap coordinates, then add to palm and finger positions + glm::vec3 leapDelta = _owningHandData->worldVectorToLeapVector(delta); + _rawPosition += leapDelta; + for (int i = 0; i < getNumFingers(); i++) { + FingerData& finger = _fingers[i]; + if (finger.isActive()) { + finger.setRawTipPosition(finger.getTipRawPosition() + leapDelta); + finger.setRawRootPosition(finger.getRootRawPosition() + leapDelta); + } + } +} + FingerData::FingerData(PalmData* owningPalmData, HandData* owningHandData) : _tipRawPosition(0, 0, 0), _rootRawPosition(0, 0, 0), diff --git a/libraries/avatars/src/HandData.h b/libraries/avatars/src/HandData.h index b8a06d53ad..a267707b41 100755 --- a/libraries/avatars/src/HandData.h +++ b/libraries/avatars/src/HandData.h @@ -47,6 +47,8 @@ const int BUTTON_3 = 8; const int BUTTON_4 = 16; const int BUTTON_FWD = 128; +const float LEAP_UNIT_SCALE = 0.001f; ///< convert mm to meters + class HandData { public: HandData(AvatarData* owningAvatar); @@ -54,20 +56,25 @@ public: // These methods return the positions in Leap-relative space. // To convert to world coordinates, use Hand::leapPositionToWorldPosition. - + // position conversion glm::vec3 leapPositionToWorldPosition(const glm::vec3& leapPosition) { - const float unitScale = 0.001; // convert mm to meters - return _basePosition + _baseOrientation * (leapPosition * unitScale); + return _basePosition + _baseOrientation * (leapPosition * LEAP_UNIT_SCALE); } glm::vec3 leapDirectionToWorldDirection(const glm::vec3& leapDirection) { return glm::normalize(_baseOrientation * leapDirection); } + glm::vec3 worldPositionToLeapPosition(const glm::vec3& worldPosition) const; + glm::vec3 worldVectorToLeapVector(const glm::vec3& worldVector) const; std::vector& getPalms() { return _palms; } size_t getNumPalms() { return _palms.size(); } PalmData& addNewPalm(); + /// Finds the indices of the left and right palms according to their locations, or -1 if either or + /// both is not found. + void getLeftRightPalmIndices(int& leftPalmIndex, int& rightPalmIndex) const; + void setFingerTrailLength(unsigned int length); void updateFingerTrails(); @@ -153,6 +160,8 @@ public: void setVelocity(const glm::vec3& velocity) { _velocity = velocity; } const glm::vec3& getVelocity() const { return _velocity; } + void addToPosition(const glm::vec3& delta); + void incrementFramesWithoutData() { _numFramesWithoutData++; } void resetFramesWithoutData() { _numFramesWithoutData = 0; } int getFramesWithoutData() const { return _numFramesWithoutData; }