Option to render collision proxies, more general avatar penetration test, fix

for meshes with only one joint influence.

interface/src/Menu.cpp

@@ -318,6 +318,7 @@ Menu::Menu() :
     
     addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::Avatars, 0, true);
     addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::AvatarAsBalls);
+    addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::CollisionProxies);
     
     addActionToQMenuAndActionHash(avatarOptionsMenu,
                                   MenuOption::VoxelMode,

interface/src/Menu.h

@@ -153,6 +153,7 @@ namespace MenuOption {
     const QString Bandwidth = "Bandwidth Display";
     const QString BandwidthDetails = "Bandwidth Details";
     const QString ChatCircling = "Chat Circling";
+    const QString CollisionProxies = "Collision Proxies";
     const QString Collisions = "Collisions";
     const QString CopyVoxels = "Copy";
     const QString CoverageMap = "Render Coverage Map";

interface/src/avatar/Avatar.cpp

@@ -16,6 +16,8 @@
 #include <PacketHeaders.h>
 #include <SharedUtil.h>
 
+#include <GeometryUtil.h>
+
 #include "Application.h"
 #include "Avatar.h"
 #include "DataServerClient.h"
@@ -818,6 +820,28 @@ bool Avatar::findRayIntersection(const glm::vec3& origin, const glm::vec3& direc
     return true;
 }
 
+bool Avatar::findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
+        glm::vec3& penetration, int skeletonSkipIndex) {
+    bool didPenetrate = false;
+    glm::vec3 totalPenetration;
+    glm::vec3 skeletonPenetration;
+    if (_skeletonModel.findSpherePenetration(penetratorCenter, penetratorRadius,
+            skeletonPenetration, 1.0f, skeletonSkipIndex)) {
+        addPenetrations(totalPenetration, skeletonPenetration);
+        didPenetrate = true; 
+    }
+    glm::vec3 facePenetration;
+    if (_head.getFaceModel().findSpherePenetration(penetratorCenter, penetratorRadius, facePenetration)) {
+        addPenetrations(totalPenetration, facePenetration);
+        didPenetrate = true; 
+    }
+    if (didPenetrate) {
+        penetration = totalPenetration;
+        return true;
+    }
+    return false;
+}
+
 int Avatar::parseData(unsigned char* sourceBuffer, int numBytes) {
     // change in position implies movement
     glm::vec3 oldPosition = _position;

interface/src/avatar/Avatar.h

@@ -174,6 +174,15 @@ public:
     /// \return whether or not the ray intersected
     bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const;
 
+    /// Checks for penetration between the described sphere and the avatar.
+    /// \param penetratorCenter the center of the penetration test sphere
+    /// \param penetratorRadius the radius of the penetration test sphere
+    /// \param penetration[out] the vector in which to store the penetration
+    /// \param skeletonSkipIndex if not -1, the index of a joint to skip (along with its descendents) in the skeleton model
+    /// \return whether or not the sphere penetrated
+    bool findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
+        glm::vec3& penetration, int skeletonSkipIndex = -1);
+
     virtual int parseData(unsigned char* sourceBuffer, int numBytes);
 
     static void renderJointConnectingCone(glm::vec3 position1, glm::vec3 position2, float radius1, float radius2);

interface/src/avatar/FaceModel.cpp

@@ -11,6 +11,7 @@
 #include "Avatar.h"
 #include "FaceModel.h"
 #include "Head.h"
+#include "Menu.h"
 
 FaceModel::FaceModel(Head* owningHead) :
     _owningHead(owningHead)
@@ -46,6 +47,16 @@ void FaceModel::simulate(float deltaTime) {
     Model::simulate(deltaTime);
 }
 
+bool FaceModel::render(float alpha) {
+    if (!Model::render(alpha)) {
+        return false;
+    }
+    if (Menu::getInstance()->isOptionChecked(MenuOption::CollisionProxies)) {
+        renderCollisionProxies(alpha);
+    }
+    return true;
+}
+
 void FaceModel::maybeUpdateNeckRotation(const JointState& parentState, const FBXJoint& joint, JointState& state) {
     // get the rotation axes in joint space and use them to adjust the rotation
     glm::mat3 axes = glm::mat3_cast(_rotation);

interface/src/avatar/FaceModel.h

@@ -22,6 +22,7 @@ public:
     FaceModel(Head* owningHead);
 
     void simulate(float deltaTime);
+    bool render(float alpha);
     
 protected:
 

interface/src/avatar/Hand.cpp

@@ -169,8 +169,8 @@ void Hand::calculateGeometry() {
         int skipIndex = skeletonModel.getParentJointIndex(
             (i == leftPalmIndex) ? skeletonModel.getLeftHandJointIndex() :
                 (i == rightPalmIndex) ? skeletonModel.getRightHandJointIndex() : -1);
-        if (skeletonModel.findSpherePenetration(palm.getPosition(),
-                PALM_RADIUS * _owningAvatar->getScale(), penetration, 0.001f, skipIndex)) {
+        if (_owningAvatar->findSpherePenetration(palm.getPosition(),
+                PALM_RADIUS * _owningAvatar->getScale(), penetration, skipIndex)) {
             palm.addToPosition(-penetration);
         }
     }

interface/src/avatar/SkeletonModel.cpp

@@ -10,6 +10,7 @@
 
 #include "Application.h"
 #include "Avatar.h"
+#include "Menu.h"
 #include "SkeletonModel.h"
 
 SkeletonModel::SkeletonModel(Avatar* owningAvatar) :
@@ -106,6 +107,10 @@ bool SkeletonModel::render(float alpha) {
     
     Model::render(alpha);
     
+    if (Menu::getInstance()->isOptionChecked(MenuOption::CollisionProxies)) {
+        renderCollisionProxies(alpha);
+    }
+    
     return true;
 }
 

interface/src/renderer/FBXReader.cpp

@@ -1308,15 +1308,16 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
                     totalWeight += weight;
                     for (QMultiHash<int, int>::const_iterator it = extracted.newIndices.constFind(oldIndex);
                             it != extracted.newIndices.end() && it.key() == oldIndex; it++) {
-                        // expand the bone radius
-                        if (weight > 0.25f) {
+                        // expand the bone radius for vertices with at least 1/4 weight
+                        const float EXPANSION_WEIGHT_THRESHOLD = 0.25f;
+                        if (weight > EXPANSION_WEIGHT_THRESHOLD) {
                             const glm::vec3& vertex = extracted.mesh.vertices.at(it.value());
                             float proj = glm::dot(boneDirection, vertex - boneEnd);
                             if (proj < 0.0f && proj > -boneLength) {
                                 joint.boneRadius = glm::max(joint.boneRadius, radiusScale * glm::distance(
                                     vertex, boneEnd + boneDirection * proj));
-                        }   }
-                        
+                            }
+                        }
                         
                         // look for an unused slot in the weights vector
                         glm::vec4& weights = extracted.mesh.clusterWeights[it.value()];
@@ -1338,15 +1339,23 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
             int jointIndex = maxJointIndex;
             FBXJoint& joint = geometry.joints[jointIndex];
             glm::vec3 boneEnd = extractTranslation(inverseModelTransform * joint.bindTransform);
-            glm::vec3 boneStart = boneEnd;
+            glm::vec3 boneDirection;
+            float boneLength;
             if (joint.parentIndex != -1) {
-                boneStart = extractTranslation(inverseModelTransform * geometry.joints[joint.parentIndex].bindTransform);
+                boneDirection = boneEnd - extractTranslation(inverseModelTransform *
+                    geometry.joints[joint.parentIndex].bindTransform);
+                boneLength = glm::length(boneDirection);
+                if (boneLength > EPSILON) {
+                    boneDirection /= boneLength;
+                }
             }
             float radiusScale = extractUniformScale(joint.transform * firstFBXCluster.inverseBindMatrix);
             foreach (const glm::vec3& vertex, extracted.mesh.vertices) {
-                // expand the bone radius
-                joint.boneRadius = glm::max(joint.boneRadius, radiusScale * glm::length(
-                    computeVectorFromPointToSegment(vertex, boneStart, boneEnd)));
+                float proj = glm::dot(boneDirection, vertex - boneEnd);
+                if (proj < 0.0f && proj > -boneLength) {
+                    joint.boneRadius = glm::max(joint.boneRadius, radiusScale * glm::distance(
+                        vertex, boneEnd + boneDirection * proj));
+                }
             }
         }
         extracted.mesh.isEye = (maxJointIndex == geometry.leftEyeJointIndex || maxJointIndex == geometry.rightEyeJointIndex);

interface/src/renderer/Model.cpp

@@ -744,6 +744,40 @@ void Model::applyRotationDelta(int jointIndex, const glm::quat& delta, bool cons
     state.rotation = newRotation;
 }
 
+void Model::renderCollisionProxies(float alpha) {
+    glPushMatrix();
+    Application::getInstance()->loadTranslatedViewMatrix(_translation);
+
+    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    float uniformScale = extractUniformScale(_scale);
+    for (int i = 0; i < _jointStates.size(); i++) {
+        glPushMatrix();
+        
+        glm::vec3 position = extractTranslation(_jointStates[i].transform);
+        glTranslatef(position.x, position.y, position.z);
+        
+        glm::quat rotation;
+        getJointRotation(i, rotation);
+        glm::vec3 axis = glm::axis(rotation);
+        glRotatef(glm::angle(rotation), axis.x, axis.y, axis.z);
+        
+        glColor4f(0.75f, 0.75f, 0.75f, alpha);
+        float scaledRadius = geometry.joints[i].boneRadius * uniformScale;
+        const int BALL_SUBDIVISIONS = 10;
+        glutSolidSphere(scaledRadius, BALL_SUBDIVISIONS, BALL_SUBDIVISIONS);
+        
+        glPopMatrix();
+        
+        int parentIndex = geometry.joints[i].parentIndex;
+        if (parentIndex != -1) {
+            Avatar::renderJointConnectingCone(extractTranslation(_jointStates[parentIndex].transform), position,
+                geometry.joints[parentIndex].boneRadius * uniformScale, scaledRadius);
+        }
+    }
+    
+    glPopMatrix();
+}
+
 void Model::setJointTranslation(int jointIndex, int parentIndex, int childIndex, const glm::vec3& translation) {
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
     JointState& state = _jointStates[jointIndex];

interface/src/renderer/Model.h

@@ -171,6 +171,8 @@ protected:
     
     void applyRotationDelta(int jointIndex, const glm::quat& delta, bool constrain = true);
     
+    void renderCollisionProxies(float alpha);
+    
 private:
     
     void setJointTranslation(int jointIndex, int parentIndex, int childIndex, const glm::vec3& translation);