Merge pull request #2862 from AndrewMeadows/inertia

improved model bounding shapes

interface/src/avatar/Avatar.cpp

@@ -214,29 +214,28 @@ void Avatar::render(const glm::vec3& cameraPosition, RenderMode renderMode) {
         if (Menu::getInstance()->isOptionChecked(MenuOption::Avatars)) {
             renderBody(renderMode, glowLevel);
         }
-        if (renderMode != SHADOW_RENDER_MODE && 
+        if (renderMode != SHADOW_RENDER_MODE) {
-                Menu::getInstance()->isOptionChecked(MenuOption::RenderSkeletonCollisionShapes)) {
+            bool renderSkeleton = Menu::getInstance()->isOptionChecked(MenuOption::RenderSkeletonCollisionShapes);
-            _skeletonModel.updateShapePositions();
+            bool renderHead = Menu::getInstance()->isOptionChecked(MenuOption::RenderHeadCollisionShapes);
+            bool renderBounding = Menu::getInstance()->isOptionChecked(MenuOption::RenderBoundingCollisionShapes);
+            if (renderSkeleton || renderHead || renderBounding) {
+                updateShapePositions();
+            }
+
+            if (renderSkeleton) {
                 _skeletonModel.renderJointCollisionShapes(0.7f);
             }
-        if (renderMode != SHADOW_RENDER_MODE && 
+
-                Menu::getInstance()->isOptionChecked(MenuOption::RenderHeadCollisionShapes)) {
+            if (renderHead && shouldRenderHead(cameraPosition, renderMode)) {
-            if (shouldRenderHead(cameraPosition, renderMode)) {
-                getHead()->getFaceModel().updateShapePositions();
                 getHead()->getFaceModel().renderJointCollisionShapes(0.7f);
             }
-        }
+            if (renderBounding && shouldRenderHead(cameraPosition, renderMode)) {
-        if (renderMode != SHADOW_RENDER_MODE && 
-                Menu::getInstance()->isOptionChecked(MenuOption::RenderBoundingCollisionShapes)) {
-            if (shouldRenderHead(cameraPosition, renderMode)) {
-                getHead()->getFaceModel().updateShapePositions();
                 getHead()->getFaceModel().renderBoundingCollisionShapes(0.7f);
-                _skeletonModel.updateShapePositions();
                 _skeletonModel.renderBoundingCollisionShapes(0.7f);
             }
-        }
+
             // If this is the avatar being looked at, render a little ball above their head
-        if (renderMode != SHADOW_RENDER_MODE &&_isLookAtTarget) {
+            if (_isLookAtTarget) {
                 const float LOOK_AT_INDICATOR_RADIUS = 0.03f;
                 const float LOOK_AT_INDICATOR_HEIGHT = 0.60f;
                 const float LOOK_AT_INDICATOR_COLOR[] = { 0.8f, 0.0f, 0.0f, 0.5f };
@@ -246,6 +245,7 @@ void Avatar::render(const glm::vec3& cameraPosition, RenderMode renderMode) {
                 glutSolidSphere(LOOK_AT_INDICATOR_RADIUS, 15, 15);
                 glPopMatrix();
             }
+        }
 
         // quick check before falling into the code below:
         // (a 10 degree breadth of an almost 2 meter avatar kicks in at about 12m)
@@ -585,6 +585,12 @@ void Avatar::updateShapePositions() {
     _skeletonModel.updateShapePositions();
     Model& headModel = getHead()->getFaceModel();
     headModel.updateShapePositions();
+    /* KEEP FOR DEBUG: use this in rather than code above to see shapes 
+     * in their default positions where the bounding shape is computed.
+    _skeletonModel.resetShapePositions();
+    Model& headModel = getHead()->getFaceModel();
+    headModel.resetShapePositions();
+    */
 }
 
 bool Avatar::findCollisions(const QVector<const Shape*>& shapes, CollisionList& collisions) {

interface/src/renderer/Model.cpp

@@ -39,6 +39,7 @@ Model::Model(QObject* parent) :
     _scaledToFit(false),
     _snapModelToCenter(false),
     _snappedToCenter(false),
+    _rootIndex(-1),
     _shapesAreDirty(true),
     _boundingRadius(0.f),
     _boundingShape(), 
@@ -128,7 +129,6 @@ QVector<Model::JointState> Model::createJointStates(const FBXGeometry& geometry)
     jointIsSet.fill(false, numJoints);
     int numJointsSet = 0;
     int lastNumJointsSet = -1;
-    glm::mat4 baseTransform = glm::mat4_cast(_rotation) * glm::scale(_scale) * glm::translate(_offset);
     while (numJointsSet < numJoints && numJointsSet != lastNumJointsSet) {
         lastNumJointsSet = numJointsSet;
         for (int i = 0; i < numJoints; ++i) {
@@ -139,6 +139,8 @@ QVector<Model::JointState> Model::createJointStates(const FBXGeometry& geometry)
             const FBXJoint& joint = geometry.joints[i];
             int parentIndex = joint.parentIndex;
             if (parentIndex == -1) {
+                _rootIndex = i;
+                glm::mat4 baseTransform = glm::mat4_cast(_rotation) * glm::scale(_scale) * glm::translate(_offset);
                 glm::quat combinedRotation = joint.preRotation * state.rotation * joint.postRotation;    
                 state.transform = baseTransform * geometry.offset * glm::translate(state.translation) * joint.preTransform *
                     glm::mat4_cast(combinedRotation) * joint.postTransform;
@@ -603,66 +605,20 @@ void Model::clearShapes() {
 void Model::rebuildShapes() {
     clearShapes();
     
-    if (!_geometry) {
+    if (!_geometry || _rootIndex == -1) {
         return;
     }
     
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
-    
     if (geometry.joints.isEmpty()) {
         return;
     }
 
-    int numJoints = geometry.joints.size();
+    // We create the shapes with proper dimensions, but we set their transforms later.
-    QVector<glm::mat4> transforms;
-    transforms.fill(glm::mat4(), numJoints);
-    QVector<glm::quat> combinedRotations;
-    combinedRotations.fill(glm::quat(), numJoints);
-    QVector<bool> shapeIsSet;
-    shapeIsSet.fill(false, numJoints);
-    int rootIndex = 0;
-
     float uniformScale = extractUniformScale(_scale);
-    int numShapesSet = 0;
-    int lastNumShapesSet = -1;
-    while (numShapesSet < numJoints && numShapesSet != lastNumShapesSet) {
-        lastNumShapesSet = numShapesSet;
-        for (int i = 0; i < numJoints; ++i) {
-            if (shapeIsSet[i]) {
-                continue;
-            }
-            const FBXJoint& joint = geometry.joints[i];
-            int parentIndex = joint.parentIndex;
-            if (parentIndex == -1) {
-                rootIndex = i;
-                glm::mat4 baseTransform = glm::mat4_cast(_rotation) * uniformScale * glm::translate(_offset);
-                glm::quat combinedRotation = joint.preRotation * joint.rotation * joint.postRotation;    
-                transforms[i] = baseTransform * geometry.offset * glm::translate(joint.translation) * joint.preTransform *
-                    glm::mat4_cast(combinedRotation) * joint.postTransform;
-                combinedRotations[i] = _rotation * combinedRotation;
-                ++numShapesSet;
-                shapeIsSet[i] = true;
-            } else if (shapeIsSet[parentIndex]) {
-                glm::quat combinedRotation = joint.preRotation * joint.rotation * joint.postRotation;    
-                transforms[i] = transforms[parentIndex] * glm::translate(joint.translation) * joint.preTransform *
-                    glm::mat4_cast(combinedRotation) * joint.postTransform;
-                combinedRotations[i] = combinedRotations[parentIndex] * combinedRotation;
-                ++numShapesSet;
-                shapeIsSet[i] = true;
-            }
-        }
-    }
-
-    // joint shapes
-    Extents totalExtents;
-    totalExtents.reset();
     for (int i = 0; i < _jointStates.size(); i++) {
         const FBXJoint& joint = geometry.joints[i];
 
-        glm::vec3 worldPosition = extractTranslation(transforms[i]);
-        Extents shapeExtents;
-        shapeExtents.reset();
-
         float radius = uniformScale * joint.boneRadius;
         float halfHeight = 0.5f * uniformScale * joint.distanceToParent;
         Shape::Type type = joint.shapeType;
@@ -672,47 +628,150 @@ void Model::rebuildShapes() {
         }
         if (type == Shape::CAPSULE_SHAPE) {
             CapsuleShape* capsule = new CapsuleShape(radius, halfHeight);
-            capsule->setPosition(worldPosition);
-            capsule->setRotation(combinedRotations[i] * joint.shapeRotation);
             _jointShapes.push_back(capsule);
-
-            // add the two furthest surface points of the capsule
-            glm::vec3 axis;
-            capsule->computeNormalizedAxis(axis);
-            axis = halfHeight * axis + glm::vec3(radius);
-            shapeExtents.addPoint(worldPosition + axis);
-            shapeExtents.addPoint(worldPosition - axis);
-
-            totalExtents.addExtents(shapeExtents);
         } else if (type == Shape::SPHERE_SHAPE) {
-            SphereShape* sphere = new SphereShape(radius, worldPosition);
+            SphereShape* sphere = new SphereShape(radius, glm::vec3(0.0f));
             _jointShapes.push_back(sphere);
-
-            glm::vec3 axis = glm::vec3(radius);
-            shapeExtents.addPoint(worldPosition + axis);
-            shapeExtents.addPoint(worldPosition - axis);
-            totalExtents.addExtents(shapeExtents);
         } else {
             // this shape type is not handled and the joint shouldn't collide, 
             // however we must have a shape for each joint, 
             // so we make a bogus sphere with zero radius.
             // TODO: implement collision groups for more control over what collides with what
-            SphereShape* sphere = new SphereShape(0.f, worldPosition); 
+            SphereShape* sphere = new SphereShape(0.f, glm::vec3(0.0f)); 
             _jointShapes.push_back(sphere);
         }
     }
 
-    // bounding shape
+    // This method moves the shapes to their default positions in Model frame
-    // NOTE: we assume that the longest side of totalExtents is the yAxis
+    // which is where we compute the bounding shape's parameters.
+    computeBoundingShape(geometry);
+
+    // finally sync shapes to joint positions
+    _shapesAreDirty = true;
+    updateShapePositions();
+}
+
+void Model::computeBoundingShape(const FBXGeometry& geometry) {
+    // compute default joint transforms and rotations 
+    // (in local frame, ignoring Model translation and rotation)
+    int numJoints = geometry.joints.size();
+    QVector<glm::mat4> transforms;
+    transforms.fill(glm::mat4(), numJoints);
+    QVector<glm::quat> finalRotations;
+    finalRotations.fill(glm::quat(), numJoints);
+
+    QVector<bool> shapeIsSet;
+    shapeIsSet.fill(false, numJoints);
+    int numShapesSet = 0;
+    int lastNumShapesSet = -1;
+    glm::vec3 rootOffset(0.0f);
+    while (numShapesSet < numJoints && numShapesSet != lastNumShapesSet) {
+        lastNumShapesSet = numShapesSet;
+        for (int i = 0; i < numJoints; i++) {
+            const FBXJoint& joint = geometry.joints.at(i);
+            int parentIndex = joint.parentIndex;
+            
+            if (parentIndex == -1) {
+                glm::mat4 baseTransform = glm::scale(_scale) * glm::translate(_offset);
+                glm::quat combinedRotation = joint.preRotation * joint.rotation * joint.postRotation;    
+                transforms[i] = baseTransform * geometry.offset * glm::translate(joint.translation) 
+                    * joint.preTransform * glm::mat4_cast(combinedRotation) * joint.postTransform;
+                rootOffset = extractTranslation(transforms[i]);
+                finalRotations[i] = combinedRotation;
+                ++numShapesSet;
+                shapeIsSet[i] = true;
+            } else if (shapeIsSet[parentIndex]) {
+                glm::quat combinedRotation = joint.preRotation * joint.rotation * joint.postRotation;    
+                transforms[i] = transforms[parentIndex] * glm::translate(joint.translation) 
+                    * joint.preTransform * glm::mat4_cast(combinedRotation) * joint.postTransform;
+                finalRotations[i] = finalRotations[parentIndex] * combinedRotation;
+                ++numShapesSet;
+                shapeIsSet[i] = true;
+            }
+        }
+    }
+
+    // sync shapes to joints
+    _boundingRadius = 0.0f;
+    float uniformScale = extractUniformScale(_scale);
+    for (int i = 0; i < _jointShapes.size(); i++) {
+        const FBXJoint& joint = geometry.joints[i];
+        glm::vec3 jointToShapeOffset = uniformScale * (finalRotations[i] * joint.shapePosition);
+        glm::vec3 localPosition = extractTranslation(transforms[i]) + jointToShapeOffset- rootOffset;
+        Shape* shape = _jointShapes[i];
+        shape->setPosition(localPosition);
+        shape->setRotation(finalRotations[i] * joint.shapeRotation);
+        float distance = glm::length(localPosition) + shape->getBoundingRadius();
+        if (distance > _boundingRadius) {
+            _boundingRadius = distance;
+        }
+    }
+
+    // compute bounding box
+    Extents totalExtents;
+    totalExtents.reset();
+    for (int i = 0; i < _jointShapes.size(); i++) {
+        Extents shapeExtents;
+        shapeExtents.reset();
+
+        Shape* shape = _jointShapes[i];
+        glm::vec3 localPosition = shape->getPosition();
+        int type = shape->getType();
+        if (type == Shape::CAPSULE_SHAPE) {
+            // add the two furthest surface points of the capsule
+            CapsuleShape* capsule = static_cast<CapsuleShape*>(shape);
+            glm::vec3 axis;
+            capsule->computeNormalizedAxis(axis);
+            float radius = capsule->getRadius();
+            float halfHeight = capsule->getHalfHeight();
+            axis = halfHeight * axis + glm::vec3(radius);
+
+            shapeExtents.addPoint(localPosition + axis);
+            shapeExtents.addPoint(localPosition - axis);
+            totalExtents.addExtents(shapeExtents);
+        } else if (type == Shape::SPHERE_SHAPE) {
+            float radius = shape->getBoundingRadius();
+            glm::vec3 axis = glm::vec3(radius);
+            shapeExtents.addPoint(localPosition + axis);
+            shapeExtents.addPoint(localPosition - axis);
+            totalExtents.addExtents(shapeExtents);
+        }
+    }
+
+    // compute bounding shape parameters
+    // NOTE: we assume that the longest side of totalExtents is the yAxis...
     glm::vec3 diagonal = totalExtents.maximum - totalExtents.minimum;
-    // the radius is half the RMS of the X and Z sides:
+    // ... and assume the radius is half the RMS of the X and Z sides:
     float capsuleRadius = 0.5f * sqrtf(0.5f * (diagonal.x * diagonal.x + diagonal.z * diagonal.z));
     _boundingShape.setRadius(capsuleRadius);
     _boundingShape.setHalfHeight(0.5f * diagonal.y - capsuleRadius);
+    _boundingShapeLocalOffset = 0.5f * (totalExtents.maximum + totalExtents.minimum);
+}
 
-    glm::quat inverseRotation = glm::inverse(_rotation);
+void Model::resetShapePositions() {
-    glm::vec3 rootPosition = extractTranslation(transforms[rootIndex]);
+    // DEBUG method.
-    _boundingShapeLocalOffset = inverseRotation * (0.5f * (totalExtents.maximum + totalExtents.minimum) - rootPosition);
+    // Moves shapes to the joint default locations for debug visibility into
+    // how the bounding shape is computed.
+
+    if (!_geometry || _rootIndex == -1) {
+        // geometry or joints have not yet been created
+        return;
+    }
+    
+    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    if (geometry.joints.isEmpty() || _jointShapes.size() != geometry.joints.size()) {
+        return;
+    }
+
+    // The shapes are moved to their default positions in computeBoundingShape().
+    computeBoundingShape(geometry);
+
+    // Then we move them into world frame for rendering at the Model's location.
+    for (int i = 0; i < _jointShapes.size(); i++) {
+        Shape* shape = _jointShapes[i];
+        shape->setPosition(_translation + _rotation * shape->getPosition());
+        shape->setRotation(_rotation * shape->getRotation());
+    }
     _boundingShape.setPosition(_translation + _rotation * _boundingShapeLocalOffset);
     _boundingShape.setRotation(_rotation);
 }
@@ -728,17 +787,17 @@ void Model::updateShapePositions() {
             // shape position and rotation need to be in world-frame
             glm::vec3 jointToShapeOffset = uniformScale * (_jointStates[i].combinedRotation * joint.shapePosition);
             glm::vec3 worldPosition = extractTranslation(_jointStates[i].transform) + jointToShapeOffset + _translation;
-            _jointShapes[i]->setPosition(worldPosition);
+            Shape* shape = _jointShapes[i];
-            _jointShapes[i]->setRotation(_jointStates[i].combinedRotation * joint.shapeRotation);
+            shape->setPosition(worldPosition);
-            float distance2 = glm::distance2(worldPosition, _translation);
+            shape->setRotation(_jointStates[i].combinedRotation * joint.shapeRotation);
-            if (distance2 > _boundingRadius) {
+            float distance = glm::distance(worldPosition, _translation) + shape->getBoundingRadius();
-                _boundingRadius = distance2;
+            if (distance > _boundingRadius) {
+                _boundingRadius = distance;
             }
             if (joint.parentIndex == -1) {
                 rootPosition = worldPosition;
             }
         }
-        _boundingRadius = sqrtf(_boundingRadius);
         _shapesAreDirty = false;
         _boundingShape.setPosition(rootPosition + _rotation * _boundingShapeLocalOffset);
         _boundingShape.setRotation(_rotation);

interface/src/renderer/Model.h

@@ -187,6 +187,7 @@ public:
     
     void clearShapes();
     void rebuildShapes();
+    void resetShapePositions();
     void updateShapePositions();
     void renderJointCollisionShapes(float alpha);
     void renderBoundingCollisionShapes(float alpha);
@@ -234,6 +235,7 @@ protected:
     
     bool _snapModelToCenter; /// is the model's offset automatically adjusted to center around 0,0,0 in model space
     bool _snappedToCenter; /// are we currently snapped to center
+    int _rootIndex;
     
     class JointState {
     public:
@@ -293,6 +295,8 @@ protected:
 
     void applyRotationDelta(int jointIndex, const glm::quat& delta, bool constrain = true);
     
+    void computeBoundingShape(const FBXGeometry& geometry);
+
 private:
     
     void applyNextGeometry();