enable shape collisions for MyAvatar skeleton

also some stubbery for main simulation loop

interface/src/avatar/MyAvatar.cpp

@@ -82,6 +82,8 @@ MyAvatar::MyAvatar() :
     for (int i = 0; i < MAX_DRIVE_KEYS; i++) {
         _driveKeys[i] = 0.0f;
     }
+    _skeletonModel.setEnableCollisionShapes(true);
+    _simulationEngine.addRagdoll(&_skeletonModel);
 }
 
 MyAvatar::~MyAvatar() {
@@ -190,6 +192,14 @@ void MyAvatar::simulate(float deltaTime) {
         head->simulate(deltaTime, true);
     }
 
+    if (!Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
+        PerformanceTimer perfTimer("MyAvatar::simulate/head Simulate");
+        const int minError = 0.005f;
+        const float maxIterations = 4;
+        const quint64 maxUsec = 500;
+        _simulationEngine.stepForward(deltaTime, minError, maxIterations, maxUsec);
+    }
+    /* TODO: Andrew to make this work again
     // now that we're done stepping the avatar forward in time, compute new collisions
     if (_collisionGroups != 0) {
         PerformanceTimer perfTimer("MyAvatar::simulate/_collisionGroups");
@@ -216,6 +226,7 @@ void MyAvatar::simulate(float deltaTime) {
             updateCollisionWithAvatars(deltaTime);
         }
     }
+    */
 
     // consider updating our billboard
     maybeUpdateBillboard();

interface/src/avatar/SkeletonModel.cpp

@@ -272,7 +272,8 @@ void SkeletonModel::updateJointState(int index) {
 void SkeletonModel::updateShapePositions() {
     if (isActive() && _owningAvatar->isMyAvatar() && 
             Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
-        updateShapes(_rotation, _translation);
+        // TODO: Andrew to move shape updates to SimulationEngine
+        //updateShapes(_rotation, _translation);
     } else {
         Model::updateShapePositions();
     }

libraries/shared/src/SimulationEngine.cpp

@@ -12,6 +12,7 @@
 #include <glm/glm.hpp>
 
 #include "SharedUtil.h"
+#include "ShapeCollider.h"
 #include "SimulationEngine.h"
 
 int MAX_DOLLS_PER_ENGINE = 32;
@@ -28,18 +29,23 @@ SimulationEngine::~SimulationEngine() {
 }
 
 bool SimulationEngine::addRagdoll(Ragdoll* doll) {
-    int numDolls = _dolls.size();
-    if (numDolls < MAX_DOLLS_PER_ENGINE) {
-        for (int i = 0; i < numDolls; ++i) {
-            if (doll == _dolls[i]) {
-                // already in list
-                return true;
-            }
-        }
-        _dolls.push_back(doll);
-        return true;
+    if (!doll) {
+        return false;
     }
-    return false;
+    int numDolls = _dolls.size();
+    if (numDolls > MAX_DOLLS_PER_ENGINE) {
+        // list is full
+        return false;
+    }
+    for (int i = 0; i < numDolls; ++i) {
+        if (doll == _dolls[i]) {
+            // already in list
+            return true;
+        }
+    }
+    // add to list
+    _dolls.push_back(doll);
+    return true;
 }
 
 void SimulationEngine::removeRagdoll(Ragdoll* doll) {
@@ -60,17 +66,59 @@ void SimulationEngine::removeRagdoll(Ragdoll* doll) {
     }
 }
 
-void SimulationEngine::enforceConstraints(float minError, int maxIterations, quint64 maxUsec) {
-    // enforce the constraints
+void SimulationEngine::stepForward(float deltaTime, float minError, int maxIterations, quint64 maxUsec) {
     int iterations = 0;
     float delta = 0.0f;
     quint64 now = usecTimestampNow();
     quint64 startTime = now;
     quint64 expiry = now + maxUsec;
+
+    int numDolls = _dolls.size();
+    for (int i = 0; i < numDolls; ++i) {
+        // TODO: need to implement:
+        // (1) joints pull points (SpecialCapsuleShape would help solve this)
+        // (2) points slam shapes (SpecialCapsuleShape would help solve this)
+        // (3) shapes collide with pairwise collision bypass
+        // (4) collisions move points (SpecialCapsuleShape would help solve this)
+        // (5) enforce constraints
+        // (6) add and enforce angular contraints for joints
+        _dolls.at(i)->stepForward(deltaTime);
+    }
+
+    
+    // collide
+    _collisionList.clear();
+    // TODO: keep track of QSet<PhysicalEntity*> collidedEntities;
+    for (int i = 0; i < numDolls; ++i) {
+        const QVector<Shape*>* shapesA = _dolls.at(i)->getShapes();
+        if (!shapesA) {
+            continue;
+        }
+        int numShapesA = shapesA->size();
+        // collide with self
+        for (int j = 0; j < numShapesA; ++j) {
+            const Shape* shapeA = shapesA->at(j);
+            if (!shapeA) {
+                continue;
+            }
+            // TODO: check for pairwise collision bypass here
+            ShapeCollider::collideShapeWithShapes(shapeA, *shapesA, j+1, _collisionList);
+        }
+
+        // collide with others
+        for (int j = i+1; j < numDolls; ++j) {
+            const QVector<Shape*>* shapesB = _dolls.at(j)->getShapes();
+            if (!shapesB) {
+                continue;
+            }
+            ShapeCollider::collideShapesWithShapes(*shapesA, *shapesB, _collisionList);
+        }
+    }
+
+    // enforce constraints
     float error = 0.0f;
     do {
         error = 0.0f;
-        int numDolls = _dolls.size();
         for (int i = 0; i < numDolls; ++i) {
             error = glm::max(error, _dolls[i]->enforceConstraints());
         }

libraries/shared/src/SimulationEngine.h

@@ -32,7 +32,7 @@ public:
     /// \param maxIterations max number of iterations before giving up
     /// \param maxUsec max number of usec to spend enforcing constraints
     /// \return distance of largest movement
-    void enforceConstraints(float minError, int maxIterations, quint64 maxUsec);
+    void stepForward(float deltaTime, float minError, int maxIterations, quint64 maxUsec);
 
     int getEnforementIterations() const { return _enforcementIterations; }
     float getEnforcementError() const { return _enforcementError; }