Add JointShapeInfo for joint shape calculations

interface/src/renderer/FBXReader.cpp

@@ -22,6 +22,7 @@
 #include <OctalCode.h>
 
 #include <GeometryUtil.h>
+#include <Shape.h>
 #include <VoxelTree.h>
 
 #include "FBXReader.h"
@@ -34,6 +35,22 @@ std::ostream& operator<<(std::ostream& s, const glm::vec3& v) {
 
 using namespace std;
 
+void Extents::reset() {
+    minimum = glm::vec3(FLT_MAX);
+    maximum = glm::vec3(-FLT_MAX);
+}
+
+bool Extents::containsPoint(const glm::vec3& point) const {
+    return (point.x >= minimum.x && point.x <= maximum.x
+        && point.y >= minimum.y && point.y <= maximum.y
+        && point.z >= minimum.z && point.z <= maximum.z);
+}
+
+void Extents::addPoint(const glm::vec3& point) {
+    minimum = glm::min(minimum, point);
+    maximum = glm::max(maximum, point);
+}
+
 template<class T> QVariant readBinaryArray(QDataStream& in) {
     quint32 arrayLength;
     quint32 encoding;
@@ -847,6 +864,21 @@ QString getString(const QVariant& value) {
     return list.isEmpty() ? value.toString() : list.at(0).toString();
 }
 
+class JointShapeInfo {
+public:
+    JointShapeInfo() : numVertices(0), numProjectedVertices(0), averageVertex(0.f), boneBegin(0.f), averageRadius(0.f) {
+        extents.reset();
+    }
+
+    // NOTE: the points here are in the "joint frame" which has the "jointEnd" at the origin
+    int numVertices;            // num vertices from contributing meshes
+    int numProjectedVertices;   // num vertices that successfully project onto bone axis
+    Extents extents;            // max and min extents of mesh vertices (in joint frame)
+    glm::vec3 averageVertex;    // average of all mesh vertices (in joint frame)
+    glm::vec3 boneBegin;        // parent joint location (in joint frame)
+    float averageRadius;        // average distance from mesh points to averageVertex
+};
+
 FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) {
     QHash<QString, ExtractedMesh> meshes;
     QVector<ExtractedBlendshape> blendshapes;
@@ -1256,14 +1288,14 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
         joint.boneRadius = 0.0f;
         joint.inverseBindRotation = joint.inverseDefaultRotation;
         joint.name = model.name;
-        joint.extents.minimum = glm::vec3(FLT_MAX);
+        joint.shapePosition = glm::vec3(0.f);
-        joint.extents.maximum = glm::vec3(-FLT_MAX);
+        joint.shapeType = Shape::UNKNOWN_SHAPE;
-        joint.averageVertex = glm::vec3(0.f);
-        joint.numVertices = 0;
-        joint.averageRadius = 0.f;
         geometry.joints.append(joint);
         geometry.jointIndices.insert(model.name, geometry.joints.size() - 1);
     }
+    // for each joint we allocate a JointShapeInfo in which we'll store collision shape info
+    QVector<JointShapeInfo> jointShapeInfos;
+    jointShapeInfos.resize(geometry.joints.size());
 
     // find our special joints
     geometry.leftEyeJointIndex = modelIDs.indexOf(jointEyeLeftID);
@@ -1285,10 +1317,8 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
         geometry.neckPivot = glm::vec3(transform[3][0], transform[3][1], transform[3][2]);
     }
 
-    geometry.bindExtents.minimum = glm::vec3(FLT_MAX);
+    geometry.bindExtents.reset();
-    geometry.bindExtents.maximum = glm::vec3(-FLT_MAX);
+    geometry.staticExtents.reset();
-    geometry.staticExtents.minimum = glm::vec3(FLT_MAX);
-    geometry.staticExtents.maximum = glm::vec3(-FLT_MAX);
     
     QVariantHash springs = mapping.value("spring").toHash();
     QVariant defaultSpring = springs.value("default");
@@ -1404,8 +1434,7 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
                 
                 // update the bind pose extents
                 glm::vec3 bindTranslation = extractTranslation(geometry.offset * joint.bindTransform);
-                geometry.bindExtents.minimum = glm::min(geometry.bindExtents.minimum, bindTranslation);
+                geometry.bindExtents.addPoint(bindTranslation);
-                geometry.bindExtents.maximum = glm::max(geometry.bindExtents.maximum, bindTranslation);
             }
         }
 
@@ -1448,9 +1477,12 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
                         boneDirection /= boneLength;
                     }
                 }
+                float radiusScale = extractUniformScale(joint.transform * fbxCluster.inverseBindMatrix);
+                JointShapeInfo& jointShapeInfo = jointShapeInfos[jointIndex];
+                jointShapeInfo.boneBegin = rotateMeshToJoint * (radiusScale * (boneBegin - boneEnd));
+
                 bool jointIsStatic = joint.freeLineage.isEmpty();
                 glm::vec3 jointTranslation = extractTranslation(geometry.offset * joint.bindTransform);
-                float radiusScale = extractUniformScale(joint.transform * fbxCluster.inverseBindMatrix);
                 float totalWeight = 0.0f;
                 for (int j = 0; j < cluster.indices.size(); j++) {
                     int oldIndex = cluster.indices.at(j);
@@ -1464,17 +1496,17 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
                             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));
+                                joint.boneRadius = glm::max(joint.boneRadius, 
+                                        radiusScale * glm::distance(vertex, boneEnd + boneDirection * proj));
+                                ++jointShapeInfo.numProjectedVertices;
                             }
                             glm::vec3 vertexInJointFrame = rotateMeshToJoint * (radiusScale * (vertex - boneEnd));
-                            joint.extents.minimum = glm::min(joint.extents.minimum, vertexInJointFrame);
+                            jointShapeInfo.extents.addPoint(vertexInJointFrame);
-                            joint.extents.maximum = glm::max(joint.extents.maximum, vertexInJointFrame);
+                            jointShapeInfo.averageVertex += vertexInJointFrame;
-                            joint.averageVertex += vertexInJointFrame;
+                            ++jointShapeInfo.numVertices;
-                            ++joint.numVertices;
                             if (jointIsStatic) {
                                 // expand the extents of static (nonmovable) joints
-                                geometry.staticExtents.minimum = glm::min(geometry.staticExtents.minimum, vertex + jointTranslation);
+                                geometry.staticExtents.addPoint(vertex + jointTranslation);
-                                geometry.staticExtents.maximum = glm::max(geometry.staticExtents.maximum, vertex + jointTranslation);
                             }
                         }
 
@@ -1497,40 +1529,47 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
         } else {
             int jointIndex = maxJointIndex;
             FBXJoint& joint = geometry.joints[jointIndex];
+            JointShapeInfo& jointShapeInfo = jointShapeInfos[jointIndex];
 
             glm::mat4 transformJointToMesh = inverseModelTransform * joint.bindTransform;
             glm::quat rotateMeshToJoint = glm::inverse(extractRotation(transformJointToMesh));
             glm::vec3 boneEnd = extractTranslation(transformJointToMesh);
+            glm::vec3 boneBegin = boneEnd;
 
             glm::vec3 boneDirection;
             float boneLength;
             if (joint.parentIndex != -1) {
-                boneDirection = boneEnd - extractTranslation(inverseModelTransform *
+                boneBegin = extractTranslation(inverseModelTransform * geometry.joints[joint.parentIndex].bindTransform);
-                    geometry.joints[joint.parentIndex].bindTransform);
+                boneDirection = boneEnd - boneBegin;
                 boneLength = glm::length(boneDirection);
                 if (boneLength > EPSILON) {
                     boneDirection /= boneLength;
                 }
             }
-
             float radiusScale = extractUniformScale(joint.transform * firstFBXCluster.inverseBindMatrix);
+            jointShapeInfo.boneBegin = rotateMeshToJoint * (radiusScale * (boneBegin - boneEnd));
+
             glm::vec3 averageVertex(0.f);
             foreach (const glm::vec3& vertex, extracted.mesh.vertices) {
+                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));
+                    ++jointShapeInfo.numProjectedVertices;
+                }
                 glm::vec3 vertexInJointFrame = rotateMeshToJoint * (radiusScale * (vertex - boneEnd));
-                joint.extents.minimum = glm::min(joint.extents.minimum, vertexInJointFrame);
+                jointShapeInfo.extents.addPoint(vertexInJointFrame);
-                joint.extents.maximum = glm::max(joint.extents.maximum, vertexInJointFrame);
+                jointShapeInfo.averageVertex += vertexInJointFrame;
-                joint.averageVertex += vertexInJointFrame;
                 averageVertex += vertex;
             }
             int numVertices = extracted.mesh.vertices.size();
-            joint.numVertices = numVertices;
+            jointShapeInfo.numVertices = numVertices;
             if (numVertices > 0) {
-                averageVertex /= float(joint.numVertices);
+                averageVertex /= float(jointShapeInfo.numVertices);
                 float averageRadius = 0.f;
                 foreach (const glm::vec3& vertex, extracted.mesh.vertices) {
                     averageRadius += glm::distance(vertex, averageVertex);
                 }
-                joint.boneRadius = averageRadius * (radiusScale / float(numVertices));
+                jointShapeInfo.averageRadius = averageRadius * radiusScale;
             }
         }
         extracted.mesh.isEye = (maxJointIndex == geometry.leftEyeJointIndex || maxJointIndex == geometry.rightEyeJointIndex);
@@ -1580,11 +1619,38 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
         geometry.meshes.append(extracted.mesh);
     }
 
-    // now that all joints have been scanned, divide the averages by number of vertices
+    // now that all joints have been scanned, compute a collision shape for each joint
+    glm::vec3 yAxis(0.f, 1.f, 0.f);
     for (int i = 0; i < geometry.joints.size(); ++i) {
         FBXJoint& joint = geometry.joints[i];
-        if (joint.numVertices > 0) {
+        JointShapeInfo& jointShapeInfo = jointShapeInfos[i];
-            joint.averageVertex /= float(joint.numVertices);
+
+        // we use a capsule if the joint ANY mesh vertices that successfully projected onto the bone
+        // AND its boneRadius is not too close to zero
+        bool collideLikeCapsule = jointShapeInfo.numProjectedVertices > 0
+                && glm::length(jointShapeInfo.boneBegin) > EPSILON;
+
+        if (collideLikeCapsule) {
+            joint.shapeRotation = rotationBetween(yAxis, jointShapeInfo.boneBegin);
+            joint.shapePosition = 0.5f * jointShapeInfo.boneBegin;
+            //joint.shapePosition = glm::vec3(0.f);
+            joint.shapeType = Shape::CAPSULE_SHAPE;
+        } else {
+            // collide the joint like a sphere
+            if (jointShapeInfo.numVertices > 0) {
+                jointShapeInfo.averageVertex /= float(jointShapeInfo.numVertices);
+                joint.shapePosition = jointShapeInfo.averageVertex;
+            } else {
+                joint.shapePosition = glm::vec3(0.f);
+                joint.shapeType = Shape::SPHERE_SHAPE;
+            }
+            if (jointShapeInfo.numProjectedVertices == 0
+                   && jointShapeInfo.numVertices > 0) {
+                // the bone projection algorithm was not able to compute the joint radius
+                // so we use an alternative measure
+                jointShapeInfo.averageRadius /= float(jointShapeInfo.numVertices);
+                joint.boneRadius = jointShapeInfo.averageRadius;
+            }
         }
     }
     geometry.palmDirection = parseVec3(mapping.value("palmDirection", "0, -1, 0").toString());

interface/src/renderer/FBXReader.h

@@ -26,7 +26,10 @@ extern const char* FACESHIFT_BLENDSHAPES[];
 
 class Extents {
 public:
-    
+    //Extents() : minimum(FLT_MAX), maximum(-FLT_MAX) {}
+    void reset();
+    void addPoint(const glm::vec3& point);
+    bool containsPoint(const glm::vec3& point) const;
     glm::vec3 minimum;
     glm::vec3 maximum;
 };
@@ -70,13 +73,15 @@ public:
     glm::quat inverseDefaultRotation;
     glm::quat inverseBindRotation;
     glm::mat4 bindTransform;
+    // TODO: add some comments to these data members
+    // Trying to provide enough info so that the proper shape can be generated in Model
     QString name;
-    Extents extents;
+    glm::vec3 shapePosition;  // in joint frame (where boneEnd = origin)
-    int numVertices;
+    glm::quat shapeRotation;
-    glm::vec3 averageVertex;
+    int shapeType;
-    float averageRadius;
 };
 
+
 /// A single binding to a joint in an FBX document.
 class FBXCluster {
 public:

interface/src/renderer/Model.cpp

@@ -138,7 +138,7 @@ void Model::createCollisionShapes() {
     float uniformScale = extractUniformScale(_scale);
     for (int i = 0; i < _jointStates.size(); i++) {
         const FBXJoint& joint = geometry.joints[i];
-        glm::vec3 meshCenter = _jointStates[i].combinedRotation * joint.averageVertex;
+        glm::vec3 meshCenter = _jointStates[i].combinedRotation * joint.shapePosition;
         glm::vec3 position = _rotation * (extractTranslation(_jointStates[i].transform) + uniformScale * meshCenter) + _translation;
 
         float radius = uniformScale * joint.boneRadius;
@@ -154,7 +154,7 @@ void Model::updateShapePositions() {
         for (int i = 0; i < _jointStates.size(); i++) {
             const FBXJoint& joint = geometry.joints[i];
             // shape positions are stored in world-frame
-            glm::vec3 meshCenter = _jointStates[i].combinedRotation * joint.averageVertex;
+            glm::vec3 meshCenter = _jointStates[i].combinedRotation * joint.shapePosition;
             _shapes[i]->setPosition(_rotation * (extractTranslation(_jointStates[i].transform) + uniformScale * meshCenter) + _translation);
             _shapes[i]->setRotation(_jointStates[i].combinedRotation);
         }