diff --git a/interface/src/avatar/Avatar.cpp b/interface/src/avatar/Avatar.cpp index ab815679a9..ab8fdbed60 100644 --- a/interface/src/avatar/Avatar.cpp +++ b/interface/src/avatar/Avatar.cpp @@ -276,16 +276,12 @@ bool Avatar::findSphereCollisions(const glm::vec3& penetratorCenter, float penet bool didPenetrate = false; glm::vec3 skeletonPenetration; ModelCollisionInfo collisionInfo; - int jointIndex = _skeletonModel.findSphereCollision(penetratorCenter, penetratorRadius, - collisionInfo, 1.0f, skeletonSkipIndex); - if (jointIndex != -1) { + if (_skeletonModel.findSphereCollision(penetratorCenter, penetratorRadius, collisionInfo, 1.0f, skeletonSkipIndex)) { collisionInfo._model = &_skeletonModel; collisions.push_back(collisionInfo); didPenetrate = true; } - glm::vec3 facePenetration; - jointIndex = _head.getFaceModel().findSphereCollision(penetratorCenter, penetratorRadius, collisionInfo); - if (jointIndex != -1) { + if (_head.getFaceModel().findSphereCollision(penetratorCenter, penetratorRadius, collisionInfo)) { collisionInfo._model = &(_head.getFaceModel()); collisions.push_back(collisionInfo); didPenetrate = true; @@ -447,18 +443,13 @@ float Avatar::getHeight() const { return extents.maximum.y - extents.minimum.y; } -void Avatar::poke(ModelCollisionInfo& collision) { - if (collision._model == &_skeletonModel - && collision._jointIndex != -1) { - // TODO: Andrew to make this work - printf("ADEBUG model = 0x%x joint = %d p = [%e, %e, %e]\n", - collision._model, - collision._jointIndex, - collision._contactPoint.x, - collision._contactPoint.y, - collision._contactPoint.z - ); +bool Avatar::poke(ModelCollisionInfo& collision) { + // ATM poke() can only affect the Skeleton (not the head) + // TODO: make poke affect head + if (collision._model == &_skeletonModel && collision._jointIndex != -1) { + return _skeletonModel.poke(collision); } + return false; } float Avatar::getPelvisFloatingHeight() const { diff --git a/interface/src/avatar/Avatar.h b/interface/src/avatar/Avatar.h index 4a076b1867..8290115240 100755 --- a/interface/src/avatar/Avatar.h +++ b/interface/src/avatar/Avatar.h @@ -128,7 +128,9 @@ public: float getHeight() const; - void poke(ModelCollisionInfo& collision); + /// \param collision a data structure for storing info about collisions against Models + /// \return true if the collision affects the Avatar models + bool poke(ModelCollisionInfo& collision); public slots: void updateCollisionFlags(); diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp index a67aa788e3..c2ea3f8f31 100644 --- a/interface/src/avatar/Hand.cpp +++ b/interface/src/avatar/Hand.cpp @@ -224,9 +224,9 @@ void Hand::updateCollisions() { } if (avatar->findSphereCollisions(palm.getPosition(), scaledPalmRadius, collisions)) { for (size_t j = 0; j < collisions.size(); ++j) { - totalPenetration = addPenetrations(totalPenetration, collisions[j]._penetration); - // TODO: Andrew to poke avatar using collision info - avatar->poke(collisions[j]); + if (!avatar->poke(collisions[j])) { + totalPenetration = addPenetrations(totalPenetration, collisions[j]._penetration); + } } } } diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp index d71732cbde..e1652b1237 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -552,6 +552,9 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& position, int last glm::vec3 relativePosition = position - _translation; const FBXGeometry& geometry = _geometry->getFBXGeometry(); const QVector& freeLineage = geometry.joints.at(jointIndex).freeLineage; + if (freeLineage.isEmpty()) { + return false; + } if (lastFreeIndex == -1) { lastFreeIndex = freeLineage.last(); } @@ -710,6 +713,37 @@ void Model::renderCollisionProxies(float alpha) { glPopMatrix(); } +bool Model::poke(ModelCollisionInfo& collision) { + // This needs work. At the moment it can wiggle joints that are free to move (such as arms) + // but unmovable joints (such as torso) cannot be influenced at all. + glm::vec3 jointPosition(0.f); + if (getJointPosition(collision._jointIndex, jointPosition)) { + int jointIndex = collision._jointIndex; + const FBXJoint& joint = _geometry->getFBXGeometry().joints[jointIndex]; + if (joint.parentIndex != -1) { + // compute the approximate distance (travel) that the joint needs to move + glm::vec3 start; + getJointPosition(joint.parentIndex, start); + glm::vec3 contactPoint = collision._contactPoint - start; + glm::vec3 penetrationEnd = contactPoint + collision._penetration; + glm::vec3 axis = glm::cross(contactPoint, penetrationEnd); + float travel = glm::length(axis); + const float MIN_TRAVEL = 1.0e-8f; + if (travel > MIN_TRAVEL) { + // compute the new position of the joint + float angle = asinf(travel / (glm::length(contactPoint) * glm::length(penetrationEnd))); + axis = glm::normalize(axis); + glm::vec3 end; + getJointPosition(jointIndex, end); + glm::vec3 newEnd = start + glm::angleAxis(glm::degrees(angle), axis) * (end - start); + // try to move it + return setJointPosition(jointIndex, newEnd, -1, true); + } + } + } + return false; +} + void Model::deleteGeometry() { foreach (Model* attachment, _attachments) { delete attachment; diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h index 035dd689dc..f99e46c5f4 100644 --- a/interface/src/renderer/Model.h +++ b/interface/src/renderer/Model.h @@ -164,6 +164,10 @@ public: void renderCollisionProxies(float alpha); + /// \param collisionInfo info about the collision + /// \return true if collision affects the Model + bool poke(ModelCollisionInfo& collisionInfo); + protected: QSharedPointer _geometry;