Adding bounding capsule shape for Model

Also:
Cleaned up the automatic geometry LOD loading
Removed staticExtents data member from FBXGeometry
Split debug rendering of bounding shapes from joint shapes

interface/interface_en.ts

@@ -113,18 +113,18 @@
 <context>
     <name>Menu</name>
     <message>
-        <location filename="src/Menu.cpp" line="459"/>
+        <location filename="src/Menu.cpp" line="460"/>
         <source>Open .ini config file</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
-        <location filename="src/Menu.cpp" line="461"/>
+        <location filename="src/Menu.cpp" line="462"/>
-        <location filename="src/Menu.cpp" line="473"/>
+        <location filename="src/Menu.cpp" line="474"/>
         <source>Text files (*.ini)</source>
         <translation type="unfinished"></translation>
     </message>
     <message>
-        <location filename="src/Menu.cpp" line="471"/>
+        <location filename="src/Menu.cpp" line="472"/>
         <source>Save .ini config file</source>
         <translation type="unfinished"></translation>
     </message>

interface/src/Menu.cpp

@@ -282,8 +282,9 @@ Menu::Menu() :
     QMenu* avatarOptionsMenu = developerMenu->addMenu("Avatar Options");
 
     addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::Avatars, 0, true);
-    addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::RenderSkeletonCollisionProxies);
+    addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::RenderSkeletonCollisionShapes);
-    addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::RenderHeadCollisionProxies);
+    addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::RenderHeadCollisionShapes);
+    addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::RenderBoundingCollisionShapes);
 
     addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::LookAtVectors, 0, false);
     addCheckableActionToQMenuAndActionHash(avatarOptionsMenu,

interface/src/Menu.h

@@ -285,8 +285,9 @@ namespace MenuOption {
     const QString PlaySlaps = "Play Slaps";
     const QString Preferences = "Preferences...";
     const QString ReloadAllScripts = "Reload All Scripts";
-    const QString RenderSkeletonCollisionProxies = "Skeleton Collision Proxies";
+    const QString RenderSkeletonCollisionShapes = "Skeleton Collision Shapes";
-    const QString RenderHeadCollisionProxies = "Head Collision Proxies";
+    const QString RenderHeadCollisionShapes = "Head Collision Shapes";
+    const QString RenderBoundingCollisionShapes = "Bounding Collision Shapes";
     const QString ResetAvatarSize = "Reset Avatar Size";
     const QString RunTimingTests = "Run Timing Tests";
     const QString SettingsImport = "Import Settings";

interface/src/avatar/Avatar.cpp

@@ -56,8 +56,7 @@ Avatar::Avatar() :
     _owningAvatarMixer(),
     _collisionFlags(0),
     _initialized(false),
-    _shouldRenderBillboard(true),
+    _shouldRenderBillboard(true)
-    _modelsDirty(true)
 {
     // we may have been created in the network thread, but we live in the main thread
     moveToThread(Application::getInstance()->thread());
@@ -125,8 +124,7 @@ void Avatar::simulate(float deltaTime) {
     }
     glm::vec3 headPosition = _position;
     if (!_shouldRenderBillboard && inViewFrustum) {
-        _skeletonModel.simulate(deltaTime, _modelsDirty);
+        _skeletonModel.simulate(deltaTime);
-        _modelsDirty = false;
         _skeletonModel.getHeadPosition(headPosition);
     }
     Head* head = getHead();
@@ -210,11 +208,19 @@ void Avatar::render(const glm::vec3& cameraPosition, RenderMode renderMode) {
         if (Menu::getInstance()->isOptionChecked(MenuOption::Avatars)) {
             renderBody(renderMode);
         }
-        if (Menu::getInstance()->isOptionChecked(MenuOption::RenderSkeletonCollisionProxies)) {
+        if (Menu::getInstance()->isOptionChecked(MenuOption::RenderSkeletonCollisionShapes)) {
-            _skeletonModel.renderCollisionProxies(0.7f);
+            _skeletonModel.updateShapePositions();
+            _skeletonModel.renderJointCollisionShapes(0.7f);
         }
-        if (Menu::getInstance()->isOptionChecked(MenuOption::RenderHeadCollisionProxies)) {
+        if (Menu::getInstance()->isOptionChecked(MenuOption::RenderHeadCollisionShapes)) {
-            getHead()->getFaceModel().renderCollisionProxies(0.7f);
+            getHead()->getFaceModel().updateShapePositions();
+            getHead()->getFaceModel().renderJointCollisionShapes(0.7f);
+        }
+        if (Menu::getInstance()->isOptionChecked(MenuOption::RenderBoundingCollisionShapes)) {
+            _skeletonModel.updateShapePositions();
+            _skeletonModel.renderBoundingCollisionShapes(0.7f);
+            getHead()->getFaceModel().updateShapePositions();
+            getHead()->getFaceModel().renderBoundingCollisionShapes(0.7f);
         }
 
         // quick check before falling into the code below:
@@ -650,9 +656,6 @@ int Avatar::parseDataAtOffset(const QByteArray& packet, int offset) {
     const float MOVE_DISTANCE_THRESHOLD = 0.001f;
     _moving = glm::distance(oldPosition, _position) > MOVE_DISTANCE_THRESHOLD;
     
-    // note that we need to update our models
-    _modelsDirty = true;
-    
     return bytesRead;
 }
 

interface/src/avatar/Avatar.h

@@ -192,7 +192,6 @@ private:
     bool _initialized;
     QScopedPointer<Texture> _billboardTexture;
     bool _shouldRenderBillboard;
-    bool _modelsDirty;
 
     void renderBillboard();
     

interface/src/avatar/FaceModel.cpp

@@ -19,8 +19,8 @@ FaceModel::FaceModel(Head* owningHead) :
 }
 
 void FaceModel::simulate(float deltaTime) {
-    QVector<JointState> newJointStates = updateGeometry();
+    bool geometryIsUpToDate = updateGeometry();
-    if (!isActive()) {
+    if (!geometryIsUpToDate) {
         return;
     }
     Avatar* owningAvatar = static_cast<Avatar*>(_owningHead->_owningAvatar);
@@ -42,7 +42,7 @@ void FaceModel::simulate(float deltaTime) {
     setPupilDilation(_owningHead->getPupilDilation());
     setBlendshapeCoefficients(_owningHead->getBlendshapeCoefficients());
     
-    Model::simulate(deltaTime, true, newJointStates);
+    Model::simulateInternal(deltaTime);
 }
 
 void FaceModel::maybeUpdateNeckRotation(const JointState& parentState, const FBXJoint& joint, JointState& state) {

interface/src/avatar/Hand.cpp

@@ -253,7 +253,7 @@ void Hand::render(bool isMine) {
     
     _renderAlpha = 1.0;
     
-    if (Menu::getInstance()->isOptionChecked(MenuOption::RenderSkeletonCollisionProxies)) {
+    if (Menu::getInstance()->isOptionChecked(MenuOption::RenderSkeletonCollisionShapes)) {
         // draw a green sphere at hand joint location, which is actually near the wrist)
         for (size_t i = 0; i < getNumPalms(); i++) {
             PalmData& palm = getPalms()[i];

interface/src/avatar/MyAvatar.cpp

@@ -20,6 +20,11 @@
 #include <PacketHeaders.h>
 #include <SharedUtil.h>
 
+#ifdef ANDREW_HACKERY
+#include <ShapeCollider.h>
+#include <StreamUtils.h>
+#endif ANDREW_HACKERY
+
 #include "Application.h"
 #include "Audio.h"
 #include "Environment.h"
@@ -867,6 +872,8 @@ bool findAvatarAvatarPenetration(const glm::vec3 positionA, float radiusA, float
     return false;
 }
 
+static CollisionList bodyCollisions(16);
+
 void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
     //  Reset detector for nearest avatar
     _distanceToNearestAvatar = std::numeric_limits<float>::max();
@@ -878,15 +885,6 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
     updateShapePositions();
     float myBoundingRadius = getBoundingRadius();
 
-    /* TODO: Andrew to fix Avatar-Avatar body collisions
-    // HACK: body-body collision uses two coaxial capsules with axes parallel to y-axis
-    // TODO: make the collision work without assuming avatar orientation
-    Extents myStaticExtents = _skeletonModel.getStaticExtents();
-    glm::vec3 staticScale = myStaticExtents.maximum - myStaticExtents.minimum;
-    float myCapsuleRadius = 0.25f * (staticScale.x + staticScale.z);
-    float myCapsuleHeight = staticScale.y;
-    */
-
     foreach (const AvatarSharedPointer& avatarPointer, avatars) {
         Avatar* avatar = static_cast<Avatar*>(avatarPointer.data());
         if (static_cast<Avatar*>(this) == avatar) {
@@ -900,19 +898,26 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
         }
         float theirBoundingRadius = avatar->getBoundingRadius();
         if (distance < myBoundingRadius + theirBoundingRadius) {
-            /* TODO: Andrew to fix Avatar-Avatar body collisions
+#ifdef ANDREW_HACKERY
-            Extents theirStaticExtents = _skeletonModel.getStaticExtents();
+            QVector<const Shape*> myShapes;
-            glm::vec3 staticScale = theirStaticExtents.maximum - theirStaticExtents.minimum;
+            _skeletonModel.getBodyShapes(myShapes);
-            float theirCapsuleRadius = 0.25f * (staticScale.x + staticScale.z);
+            QVector<const Shape*> theirShapes;
-            float theirCapsuleHeight = staticScale.y;
+            avatar->getSkeletonModel().getBodyShapes(theirShapes);
-
+            bodyCollisions.clear();
-            glm::vec3 penetration(0.f);
+            foreach (const Shape* myShape, myShapes) {
-            if (findAvatarAvatarPenetration(_position, myCapsuleRadius, myCapsuleHeight,
+                foreach (const Shape* theirShape, theirShapes) {
-                avatar->getPosition(), theirCapsuleRadius, theirCapsuleHeight, penetration)) {
+                    ShapeCollider::shapeShape(myShape, theirShape, bodyCollisions);
-                // move the avatar out by half the penetration
+                    if (bodyCollisions.size() > 0) {
-                setPosition(_position - 0.5f * penetration);
+                        std::cout << "adebug myPos = " << myShape->getPosition() 
+                            << "  myRadius = " << myShape->getBoundingRadius()
+                            << "  theirPos = " << theirShape->getPosition() 
+                            << "  theirRadius = " << theirShape->getBoundingRadius()
+                            << std::endl;  // adebug
+                        std::cout << "adebug collision count = " << bodyCollisions.size() << std::endl;  // adebug
+                    }
+                }
             }
-            */
+#endif // ANDREW_HACKERY
 
             // collide our hands against them
             // TODO: make this work when we can figure out when the other avatar won't yeild

interface/src/avatar/SkeletonModel.cpp

@@ -14,17 +14,17 @@
 #include "Menu.h"
 #include "SkeletonModel.h"
 
-SkeletonModel::SkeletonModel(Avatar* owningAvatar) :
+SkeletonModel::SkeletonModel(Avatar* owningAvatar) : 
     _owningAvatar(owningAvatar) {
 }
 
-void SkeletonModel::simulate(float deltaTime, bool fullUpdate) {
+void SkeletonModel::simulate(float deltaTime) {
     setTranslation(_owningAvatar->getPosition());
     setRotation(_owningAvatar->getOrientation() * glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f)));
     const float MODEL_SCALE = 0.0006f;
     setScale(glm::vec3(1.0f, 1.0f, 1.0f) * _owningAvatar->getScale() * MODEL_SCALE);
     
-    Model::simulate(deltaTime, fullUpdate);
+    Model::simulate(deltaTime);
     
     if (!(isActive() && _owningAvatar->isMyAvatar())) {
         return; // only simulate for own avatar
@@ -94,6 +94,12 @@ void SkeletonModel::getHandShapes(int jointIndex, QVector<const Shape*>& shapes)
     }
 }
 
+void SkeletonModel::getBodyShapes(QVector<const Shape*>& shapes) const {
+    // for now we push a single bounding shape, 
+    // but later we could push a subset of joint shapes
+    shapes.push_back(&_boundingShape);
+}
+
 class IndexValue {
 public:
     int index;

interface/src/avatar/SkeletonModel.h

@@ -9,7 +9,6 @@
 #ifndef __interface__SkeletonModel__
 #define __interface__SkeletonModel__
 
-
 #include "renderer/Model.h"
 
 class Avatar;
@@ -22,14 +21,17 @@ public:
 
     SkeletonModel(Avatar* owningAvatar);
     
-    void simulate(float deltaTime, bool fullUpdate = true);
+    void simulate(float deltaTime);
 
     /// \param jointIndex index of hand joint
     /// \param shapes[out] list in which is stored pointers to hand shapes
     void getHandShapes(int jointIndex, QVector<const Shape*>& shapes) const;
 
+    ///  \param shapes[out] list of shapes for body collisions
+    void getBodyShapes(QVector<const Shape*>& shapes) const;
+
 protected:
-    
+
     void applyHandPosition(int jointIndex, const glm::vec3& position);
     
     void applyPalmData(int jointIndex, const QVector<int>& fingerJointIndices,

interface/src/renderer/FBXReader.cpp

@@ -41,6 +41,11 @@ bool Extents::containsPoint(const glm::vec3& point) const {
         && point.z >= minimum.z && point.z <= maximum.z);
 }
 
+void Extents::addExtents(const Extents& extents) {
+     minimum = glm::min(minimum, extents.minimum);
+     maximum = glm::max(maximum, extents.maximum);
+}
+
 void Extents::addPoint(const glm::vec3& point) {
     minimum = glm::min(minimum, point);
     maximum = glm::max(maximum, point);
@@ -1337,7 +1342,6 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
     }
 
     geometry.bindExtents.reset();
-    geometry.staticExtents.reset();
     geometry.meshExtents.reset();
     
     for (QHash<QString, ExtractedMesh>::iterator it = meshes.begin(); it != meshes.end(); it++) {
@@ -1505,8 +1509,6 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
                 JointShapeInfo& jointShapeInfo = jointShapeInfos[jointIndex];
                 jointShapeInfo.boneBegin = rotateMeshToJoint * (radiusScale * (boneBegin - boneEnd));
 
-                bool jointIsStatic = joint.freeLineage.isEmpty();
-                glm::vec3 jointTranslation = extractTranslation(geometry.offset * joint.bindTransform);
                 float totalWeight = 0.0f;
                 for (int j = 0; j < cluster.indices.size(); j++) {
                     int oldIndex = cluster.indices.at(j);
@@ -1528,10 +1530,6 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
                             jointShapeInfo.extents.addPoint(vertexInJointFrame);
                             jointShapeInfo.averageVertex += vertexInJointFrame;
                             ++jointShapeInfo.numVertices;
-                            if (jointIsStatic) {
-                                // expand the extents of static (nonmovable) joints
-                                geometry.staticExtents.addPoint(vertex + jointTranslation);
-                            }
                         }
 
                         // look for an unused slot in the weights vector

interface/src/renderer/FBXReader.h

@@ -30,6 +30,10 @@ public:
     /// set minimum and maximum to FLT_MAX and -FLT_MAX respectively
     void reset();
 
+    /// \param extents another intance of extents
+    /// expand current limits to contain other extents
+    void addExtents(const Extents& extents);
+
     /// \param point new point to compare against existing limits
     /// compare point to current limits and expand them if necessary to contain point
     void addPoint(const glm::vec3& point);
@@ -174,7 +178,6 @@ public:
     glm::vec3 neckPivot;
     
     Extents bindExtents;
-    Extents staticExtents;
     Extents meshExtents;
     
     QVector<FBXAttachment> attachments;

interface/src/renderer/Model.cpp

@@ -32,9 +32,11 @@ Model::Model(QObject* parent) :
     QObject(parent),
     _scale(1.0f, 1.0f, 1.0f),
     _shapesAreDirty(true),
+    _boundingRadius(0.f),
+    _boundingShape(), 
+    _boundingShapeLocalOffset(0.f),
     _lodDistance(0.0f),
-    _pupilDilation(0.0f),
+    _pupilDilation(0.0f) {
-    _boundingRadius(0.f) {
     // we may have been created in the network thread, but we live in the main thread
     moveToThread(Application::getInstance()->thread());
 }
@@ -73,6 +75,44 @@ QVector<Model::JointState> Model::createJointStates(const FBXGeometry& geometry)
         state.rotation = joint.rotation;
         jointStates.append(state);
     }
+
+    // compute transforms
+    // Unfortunately, the joints are not neccessarily in order from parents to children, 
+    // so we must iterate over the list multiple times until all are set correctly.
+    QVector<bool> jointIsSet;
+    int numJoints = jointStates.size();
+    jointIsSet.fill(false, numJoints);
+    int numJointsSet = 0;
+    int lastNumJointsSet = -1;
+    while (numJointsSet < numJoints && numJointsSet != lastNumJointsSet) {
+        lastNumJointsSet = numJointsSet;
+        for (int i = 0; i < numJoints; ++i) {
+            if (jointIsSet[i]) {
+                continue;
+            }
+            JointState& state = jointStates[i];
+            const FBXJoint& joint = geometry.joints[i];
+            int parentIndex = joint.parentIndex;
+            if (parentIndex == -1) {
+                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;
+                state.combinedRotation = _rotation * combinedRotation;
+                ++numJointsSet;
+                jointIsSet[i] = true;
+            } else if (jointIsSet[parentIndex]) {
+                const JointState& parentState = jointStates.at(parentIndex);
+                glm::quat combinedRotation = joint.preRotation * state.rotation * joint.postRotation;    
+                state.transform = parentState.transform * glm::translate(state.translation) * joint.preTransform *
+                    glm::mat4_cast(combinedRotation) * joint.postTransform;
+                state.combinedRotation = parentState.combinedRotation * combinedRotation;
+                ++numJointsSet;
+                jointIsSet[i] = true;
+            }
+        }
+    }
+
     return jointStates;
 }
 
@@ -135,65 +175,87 @@ void Model::reset() {
     }
 }
 
-void Model::clearShapes() {
+// return 'true' if geometry is up to date
-    for (int i = 0; i < _jointShapes.size(); ++i) {
+bool Model::updateGeometry() {
-        delete _jointShapes[i];
+    bool needToRebuild = false;
-    }
-    _jointShapes.clear();
-}
 
-void Model::createJointCollisionShapes() {
+    if (_nextGeometry) {
-    clearShapes();
+        _nextGeometry = _nextGeometry->getLODOrFallback(_lodDistance, _nextLODHysteresis);
-    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+        _nextGeometry->setLoadPriority(this, -_lodDistance);
-    float uniformScale = extractUniformScale(_scale);
+        _nextGeometry->ensureLoading();
-    for (int i = 0; i < _jointStates.size(); i++) {
+        if (_nextGeometry->isLoaded()) {
-        const FBXJoint& joint = geometry.joints[i];
+            applyNextGeometry();
-        glm::vec3 meshCenter = _jointStates[i].combinedRotation * joint.shapePosition;
+            needToRebuild = true;
-        glm::vec3 position = _rotation * (extractTranslation(_jointStates[i].transform) + uniformScale * meshCenter) + _translation;
-
-        float radius = uniformScale * joint.boneRadius;
-        if (joint.shapeType == Shape::CAPSULE_SHAPE) {
-            float halfHeight = 0.5f * uniformScale * joint.distanceToParent;
-            CapsuleShape* shape = new CapsuleShape(radius, halfHeight);
-            shape->setPosition(position);
-            _jointShapes.push_back(shape);
-        } else {
-            SphereShape* shape = new SphereShape(radius, position);
-            _jointShapes.push_back(shape);
         }
     }
-}
+    if (!_geometry) {
+        // geometry is not ready
+        return false;
+    }
 
-void Model::createBoundingShape() {
+    QSharedPointer<NetworkGeometry> geometry = _geometry->getLODOrFallback(_lodDistance, _lodHysteresis);
-    //const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    if (_geometry != geometry) {
-    //float uniformScale = extractUniformScale(_scale);
+        // NOTE: it is theoretically impossible to reach here after passing through the applyNextGeometry() call above.
-}
+        // Which means we don't need to worry about calling deleteGeometry() below immediately after creating new geometry.
 
-void Model::updateShapePositions() {
+        const FBXGeometry& newGeometry = geometry->getFBXGeometry();
-    if (_shapesAreDirty && _jointShapes.size() == _jointStates.size()) {
+        QVector<JointState> newJointStates = createJointStates(newGeometry);
-        _boundingRadius = 0.f;
+        if (! _jointStates.isEmpty()) {
-        float uniformScale = extractUniformScale(_scale);
+            // copy the existing joint states
-        const FBXGeometry& geometry = _geometry->getFBXGeometry();
+            const FBXGeometry& oldGeometry = _geometry->getFBXGeometry();
-        for (int i = 0; i < _jointStates.size(); i++) {
+            for (QHash<QString, int>::const_iterator it = oldGeometry.jointIndices.constBegin();
-            const FBXJoint& joint = geometry.joints[i];
+                    it != oldGeometry.jointIndices.constEnd(); it++) {
-            // shape position and rotation need to be in world-frame
+                int oldIndex = it.value() - 1;
-            glm::vec3 jointToShapeOffset = uniformScale * (_jointStates[i].combinedRotation * joint.shapePosition);
+                int newIndex = newGeometry.getJointIndex(it.key());
-            glm::vec3 worldPosition = extractTranslation(_jointStates[i].transform) + jointToShapeOffset + _translation;
+                if (newIndex != -1) {
-            _jointShapes[i]->setPosition(worldPosition);
+                    newJointStates[newIndex] = _jointStates.at(oldIndex);
-            _jointShapes[i]->setRotation(_jointStates[i].combinedRotation * joint.shapeRotation);
+                }
-            float distance2 = glm::distance2(worldPosition, _translation);
-            if (distance2 > _boundingRadius) {
-                _boundingRadius = distance2;
             }
+        } 
+        deleteGeometry();
+        _dilatedTextures.clear();
+        _geometry = geometry;
+        _jointStates = newJointStates;
+        needToRebuild = true;
+    } else if (_jointStates.isEmpty()) {
+        const FBXGeometry& fbxGeometry = geometry->getFBXGeometry();
+        if (fbxGeometry.joints.size() > 0) {
+            _jointStates = createJointStates(fbxGeometry);
+            needToRebuild = true;
         }
-        _boundingRadius = sqrtf(_boundingRadius);
-        _shapesAreDirty = false;
     }
-}
+    _geometry->setLoadPriority(this, -_lodDistance);
-
+    _geometry->ensureLoading();
-void Model::simulate(float deltaTime, bool fullUpdate) {
+   
-    // update our LOD, then simulate
+    if (needToRebuild) {
-    simulate(deltaTime, fullUpdate, updateGeometry());
+        const FBXGeometry& fbxGeometry = geometry->getFBXGeometry();
+        foreach (const FBXMesh& mesh, fbxGeometry.meshes) {
+            MeshState state;
+            state.clusterMatrices.resize(mesh.clusters.size());
+            _meshStates.append(state);    
+            
+            QOpenGLBuffer buffer;
+            if (!mesh.blendshapes.isEmpty()) {
+                buffer.setUsagePattern(QOpenGLBuffer::DynamicDraw);
+                buffer.create();
+                buffer.bind();
+                buffer.allocate((mesh.vertices.size() + mesh.normals.size()) * sizeof(glm::vec3));
+                buffer.write(0, mesh.vertices.constData(), mesh.vertices.size() * sizeof(glm::vec3));
+                buffer.write(mesh.vertices.size() * sizeof(glm::vec3), mesh.normals.constData(),
+                    mesh.normals.size() * sizeof(glm::vec3));
+                buffer.release();
+            }
+            _blendedVertexBuffers.append(buffer);
+        }
+        foreach (const FBXAttachment& attachment, fbxGeometry.attachments) {
+            Model* model = new Model(this);
+            model->init();
+            model->setURL(attachment.url);
+            _attachments.append(model);
+        }
+        createShapes();
+    }
+    return true;
 }
 
 bool Model::render(float alpha, bool forShadowMap) {
@@ -262,15 +324,6 @@ Extents Model::getBindExtents() const {
     return scaledExtents;
 }
 
-Extents Model::getStaticExtents() const {
-    if (!isActive()) {
-        return Extents();
-    }
-    const Extents& staticExtents = _geometry->getFBXGeometry().staticExtents;
-    Extents scaledExtents = { staticExtents.minimum * _scale, staticExtents.maximum * _scale };
-    return scaledExtents;
-}
-
 bool Model::getJointState(int index, glm::quat& rotation) const {
     if (index == -1 || index >= _jointStates.size()) {
         return false;
@@ -373,6 +426,90 @@ void Model::setURL(const QUrl& url, const QUrl& fallback, bool retainCurrent, bo
     }
 }
 
+void Model::clearShapes() {
+    for (int i = 0; i < _jointShapes.size(); ++i) {
+        delete _jointShapes[i];
+    }
+    _jointShapes.clear();
+}
+
+void Model::createShapes() {
+    clearShapes();
+    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    float uniformScale = extractUniformScale(_scale);
+    glm::quat inverseRotation = glm::inverse(_rotation);
+
+    for (int i = 0; i < _jointStates.size(); i++) {
+        updateJointState(i);
+    }
+
+    // joint shapes
+    Extents totalExtents;
+    for (int i = 0; i < _jointStates.size(); i++) {
+        const FBXJoint& joint = geometry.joints[i];
+
+        glm::vec3 jointToShapeOffset = uniformScale * (_jointStates[i].combinedRotation * joint.shapePosition);
+        glm::vec3 worldPosition = extractTranslation(_jointStates[i].transform) + jointToShapeOffset + _translation;
+        Extents shapeExtents;
+
+        float radius = uniformScale * joint.boneRadius;
+        float halfHeight = 0.5f * uniformScale * joint.distanceToParent;
+        if (joint.shapeType == Shape::CAPSULE_SHAPE && halfHeight > EPSILON) {
+            CapsuleShape* capsule = new CapsuleShape(radius, halfHeight);
+            capsule->setPosition(worldPosition);
+            capsule->setRotation(_jointStates[i].combinedRotation * joint.shapeRotation);
+            _jointShapes.push_back(capsule);
+
+            glm::vec3 endPoint; 
+            capsule->getEndPoint(endPoint);
+            glm::vec3 startPoint;
+            capsule->getStartPoint(startPoint);
+            glm::vec3 axis = (halfHeight + radius) * glm::normalize(endPoint - startPoint);
+            shapeExtents.addPoint(inverseRotation * (worldPosition + axis - _translation));
+            shapeExtents.addPoint(inverseRotation * (worldPosition - axis - _translation));
+        } else {
+            SphereShape* sphere = new SphereShape(radius, worldPosition);
+            _jointShapes.push_back(sphere);
+
+            glm::vec3 axis = glm::vec3(radius);
+            shapeExtents.addPoint(inverseRotation * (worldPosition + axis - _translation));
+            shapeExtents.addPoint(inverseRotation * (worldPosition - axis - _translation));
+        }
+        totalExtents.addExtents(shapeExtents);
+    }
+
+    // bounding shape
+    // NOTE: we assume that the longest side of totalExtents is the yAxis
+    glm::vec3 diagonal = totalExtents.maximum - totalExtents.minimum;
+    float capsuleRadius = 0.25f * (diagonal.x + diagonal.z);    // half the average of x and z
+    _boundingShape.setRadius(capsuleRadius);
+    _boundingShape.setHalfHeight(0.5f * diagonal.y - capsuleRadius);
+    _boundingShapeLocalOffset = 0.5f * (totalExtents.maximum + totalExtents.minimum);
+}
+
+void Model::updateShapePositions() {
+    if (_shapesAreDirty && _jointShapes.size() == _jointStates.size()) {
+        _boundingRadius = 0.f;
+        float uniformScale = extractUniformScale(_scale);
+        const FBXGeometry& geometry = _geometry->getFBXGeometry();
+        for (int i = 0; i < _jointStates.size(); i++) {
+            const FBXJoint& joint = geometry.joints[i];
+            // 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);
+            _jointShapes[i]->setRotation(_jointStates[i].combinedRotation * joint.shapeRotation);
+            float distance2 = glm::distance2(worldPosition, _translation);
+            if (distance2 > _boundingRadius) {
+                _boundingRadius = distance2;
+            }
+        }
+        _boundingRadius = sqrtf(_boundingRadius);
+        _shapesAreDirty = false;
+    }
+    _boundingShape.setPosition(_translation + _rotation * _boundingShapeLocalOffset);
+}
+
 bool Model::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const {
     const glm::vec3 relativeOrigin = origin - _translation;
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
@@ -419,7 +556,6 @@ bool Model::findCollisions(const QVector<const Shape*> shapes, CollisionList& co
 bool Model::findSphereCollisions(const glm::vec3& sphereCenter, float sphereRadius,
     CollisionList& collisions, int skipIndex) {
     bool collided = false;
-    updateShapePositions();
     SphereShape sphere(sphereRadius, sphereCenter);
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
     for (int i = 0; i < _jointShapes.size(); i++) {
@@ -448,45 +584,6 @@ bool Model::findSphereCollisions(const glm::vec3& sphereCenter, float sphereRadi
     return collided;
 }
 
-QVector<Model::JointState> Model::updateGeometry() {
-    QVector<JointState> newJointStates;
-    if (_nextGeometry) {
-        _nextGeometry = _nextGeometry->getLODOrFallback(_lodDistance, _nextLODHysteresis);
-        _nextGeometry->setLoadPriority(this, -_lodDistance);
-        _nextGeometry->ensureLoading();
-        if (_nextGeometry->isLoaded()) {
-            applyNextGeometry();
-            return newJointStates;
-        }
-    }
-    if (!_geometry) {
-        return newJointStates;
-    }
-    QSharedPointer<NetworkGeometry> geometry = _geometry->getLODOrFallback(_lodDistance, _lodHysteresis);
-    if (_geometry != geometry) {
-        if (!_jointStates.isEmpty()) {
-            // copy the existing joint states
-            const FBXGeometry& oldGeometry = _geometry->getFBXGeometry();
-            const FBXGeometry& newGeometry = geometry->getFBXGeometry();
-            newJointStates = createJointStates(newGeometry);
-            for (QHash<QString, int>::const_iterator it = oldGeometry.jointIndices.constBegin();
-                    it != oldGeometry.jointIndices.constEnd(); it++) {
-                int oldIndex = it.value() - 1;
-                int newIndex = newGeometry.getJointIndex(it.key());
-                if (newIndex != -1) {
-                    newJointStates[newIndex] = _jointStates.at(oldIndex);
-                }
-            }
-        }
-        deleteGeometry();
-        _dilatedTextures.clear();
-        _geometry = geometry;
-    }
-    _geometry->setLoadPriority(this, -_lodDistance);
-    _geometry->ensureLoading();
-    return newJointStates;
-}
-
 class Blender : public QRunnable {
 public:
 
@@ -549,53 +646,23 @@ void Blender::run() {
         Q_ARG(const QVector<glm::vec3>&, vertices), Q_ARG(const QVector<glm::vec3>&, normals));
 }
 
-void Model::simulate(float deltaTime, bool fullUpdate, const QVector<JointState>& newJointStates) {
+
-    if (!isActive()) {
+void Model::simulate(float deltaTime) {
+    bool geometryIsUpToDate = updateGeometry();
+    if (!geometryIsUpToDate) {
         return;
     }
-    
+    simulateInternal(deltaTime);
-    // set up world vertices on first simulate after load
+}
-    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+
-    if (_jointStates.isEmpty()) {
+void Model::simulateInternal(float deltaTime) {
-        _jointStates = newJointStates.isEmpty() ? createJointStates(geometry) : newJointStates;
-        foreach (const FBXMesh& mesh, geometry.meshes) {
-            MeshState state;
-            state.clusterMatrices.resize(mesh.clusters.size());
-            _meshStates.append(state);    
-            
-            QOpenGLBuffer buffer;
-            if (!mesh.blendshapes.isEmpty()) {
-                buffer.setUsagePattern(QOpenGLBuffer::DynamicDraw);
-                buffer.create();
-                buffer.bind();
-                buffer.allocate((mesh.vertices.size() + mesh.normals.size()) * sizeof(glm::vec3));
-                buffer.write(0, mesh.vertices.constData(), mesh.vertices.size() * sizeof(glm::vec3));
-                buffer.write(mesh.vertices.size() * sizeof(glm::vec3), mesh.normals.constData(),
-                    mesh.normals.size() * sizeof(glm::vec3));
-                buffer.release();
-            }
-            _blendedVertexBuffers.append(buffer);
-        }
-        foreach (const FBXAttachment& attachment, geometry.attachments) {
-            Model* model = new Model(this);
-            model->init();
-            model->setURL(attachment.url);
-            _attachments.append(model);
-        }
-        fullUpdate = true;
-        createJointCollisionShapes();
-    }
-    
-    // exit early if we don't have to perform a full update
-    if (!fullUpdate) {
-        return;
-    }
-    
     // update the world space transforms for all joints
     for (int i = 0; i < _jointStates.size(); i++) {
         updateJointState(i);
     }
     
+    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+
     // update the attachment transforms and simulate them
     for (int i = 0; i < _attachments.size(); i++) {
         const FBXAttachment& attachment = geometry.attachments.at(i);
@@ -641,7 +708,7 @@ void Model::updateJointState(int index) {
         state.transform = baseTransform * geometry.offset * glm::translate(state.translation) * joint.preTransform *
             glm::mat4_cast(combinedRotation) * joint.postTransform;
         state.combinedRotation = _rotation * combinedRotation;
-    
+        
     } else {
         const JointState& parentState = _jointStates.at(joint.parentIndex);
         if (index == geometry.leanJointIndex) {
@@ -825,11 +892,11 @@ void Model::applyRotationDelta(int jointIndex, const glm::quat& delta, bool cons
     state.rotation = newRotation;
 }
 
-void Model::renderCollisionProxies(float alpha) {
+const int BALL_SUBDIVISIONS = 10;
+
+void Model::renderJointCollisionShapes(float alpha) {
     glPushMatrix();
     Application::getInstance()->loadTranslatedViewMatrix(_translation);
-    updateShapePositions();
-    const int BALL_SUBDIVISIONS = 10;
     for (int i = 0; i < _jointShapes.size(); i++) {
         glPushMatrix();
 
@@ -876,6 +943,36 @@ void Model::renderCollisionProxies(float alpha) {
     glPopMatrix();
 }
 
+void Model::renderBoundingCollisionShapes(float alpha) {
+    glPushMatrix();
+
+    Application::getInstance()->loadTranslatedViewMatrix(_translation);
+
+    // draw a blue sphere at the capsule endpoint
+    glm::vec3 endPoint;
+    _boundingShape.getEndPoint(endPoint);
+    endPoint = endPoint - _translation;
+    glTranslatef(endPoint.x, endPoint.y, endPoint.z);
+    glColor4f(0.6f, 0.6f, 0.8f, alpha);
+    glutSolidSphere(_boundingShape.getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS);
+
+    // draw a yellow sphere at the capsule startpoint
+    glm::vec3 startPoint;
+    _boundingShape.getStartPoint(startPoint);
+    startPoint = startPoint - _translation;
+    glm::vec3 axis = endPoint - startPoint;
+    glTranslatef(-axis.x, -axis.y, -axis.z);
+    glColor4f(0.8f, 0.8f, 0.6f, alpha);
+    glutSolidSphere(_boundingShape.getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS);
+
+    // draw a green cylinder between the two points
+    glm::vec3 origin(0.f);
+    glColor4f(0.6f, 0.8f, 0.6f, alpha);
+    Avatar::renderJointConnectingCone( origin, axis, _boundingShape.getRadius(), _boundingShape.getRadius());
+
+    glPopMatrix();
+}
+
 bool Model::collisionHitsMoveableJoint(CollisionInfo& collision) const {
     if (collision._type == MODEL_COLLISION) {
         // the joint is pokable by a collision if it exists and is free to move

interface/src/renderer/Model.h

@@ -12,6 +12,8 @@
 #include <QObject>
 #include <QUrl>
 
+#include <CapsuleShape.h>
+
 #include "GeometryCache.h"
 #include "InterfaceConfig.h"
 #include "ProgramObject.h"
@@ -54,13 +56,9 @@ public:
     
     void init();
     void reset();
-    void clearShapes();
+    void simulate(float deltaTime);
-    void createJointCollisionShapes();
-    void createBoundingShape();
-    void updateShapePositions();
-    void simulate(float deltaTime, bool fullUpdate = true);
     bool render(float alpha = 1.0f, bool forShadowMap = false);
-    
+
     /// Sets the URL of the model to render.
     /// \param fallback the URL of a fallback model to render if the requested model fails to load
     /// \param retainCurrent if true, keep rendering the current model until the new one is loaded
@@ -76,9 +74,6 @@ public:
     /// Returns the extents of the model in its bind pose.
     Extents getBindExtents() const;
 
-    /// Returns the extents of the unmovable joints of the model.
-    Extents getStaticExtents() const;
-    
     /// Returns a reference to the shared geometry.
     const QSharedPointer<NetworkGeometry>& getGeometry() const { return _geometry; }
     
@@ -160,6 +155,12 @@ public:
     /// Returns the extended length from the right hand to its first free ancestor.
     float getRightArmLength() const;
     
+    void clearShapes();
+    void createShapes();
+    void updateShapePositions();
+    void renderJointCollisionShapes(float alpha);
+    void renderBoundingCollisionShapes(float alpha);
+    
     bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const;
     
     /// \param shapes list of pointers shapes to test against Model
@@ -170,8 +171,6 @@ public:
     bool findSphereCollisions(const glm::vec3& penetratorCenter, float penetratorRadius,
         CollisionList& collisions, int skipIndex = -1);
     
-    void renderCollisionProxies(float alpha);
-
     /// \param collision details about the collisions
     /// \return true if the collision is against a moveable joint
     bool collisionHitsMoveableJoint(CollisionInfo& collision) const;
@@ -206,6 +205,10 @@ protected:
     QVector<JointState> _jointStates;
     QVector<Shape*> _jointShapes;
     
+    float _boundingRadius;
+    CapsuleShape _boundingShape;
+    glm::vec3 _boundingShapeLocalOffset;
+    
     class MeshState {
     public:
         QVector<glm::mat4> clusterMatrices;
@@ -213,8 +216,8 @@ protected:
     
     QVector<MeshState> _meshStates;
     
-    QVector<JointState> updateGeometry();
+    bool updateGeometry();
-    void simulate(float deltaTime, bool fullUpdate, const QVector<JointState>& newJointStates);
+    void simulateInternal(float deltaTime);
     
     /// Updates the state of the joint at the specified index.
     virtual void updateJointState(int index);
@@ -249,6 +252,7 @@ private:
     void applyNextGeometry();
     void deleteGeometry();
     void renderMeshes(float alpha, bool forShadowMap, bool translucent);
+    QVector<JointState> createJointStates(const FBXGeometry& geometry);
     
     QSharedPointer<NetworkGeometry> _baseGeometry; ///< reference required to prevent collection of base
     QSharedPointer<NetworkGeometry> _nextBaseGeometry;
@@ -268,8 +272,6 @@ private:
     
     QVector<Model*> _attachments;
 
-    float _boundingRadius;
-    
     static ProgramObject _program;
     static ProgramObject _normalMapProgram;
     static ProgramObject _shadowProgram;
@@ -292,7 +294,6 @@ private:
     static SkinLocations _skinShadowLocations;
     
     static void initSkinProgram(ProgramObject& program, SkinLocations& locations);
-    static QVector<JointState> createJointStates(const FBXGeometry& geometry);
 };
 
 Q_DECLARE_METATYPE(QPointer<Model>)