add other ragdolls to simulation

interface/src/avatar/MyAvatar.cpp

@@ -204,6 +204,7 @@ void MyAvatar::simulate(float deltaTime) {
             const int minError = 0.01f;
             const float maxIterations = 10;
             const quint64 maxUsec = 2000;
+            _physicsSimulation.setTranslation(_position);
             _physicsSimulation.stepForward(deltaTime, minError, maxIterations, maxUsec);
         } else {
             _skeletonModel.moveShapesTowardJoints(1.0f);
@@ -230,12 +231,10 @@ void MyAvatar::simulate(float deltaTime) {
         } else {
             _trapDuration = 0.0f;
         }
-    /* TODO: Andrew to make this work
         if (_collisionGroups & COLLISION_GROUP_AVATARS) {
             PerformanceTimer perfTimer("avatars");
             updateCollisionWithAvatars(deltaTime);
         }
-    */
     }
 
     // consider updating our billboard
@@ -1566,13 +1565,11 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
             PhysicsSimulation* simulation = skeleton->getSimulation();
             if (avatar == nearestAvatar) {
                 if (simulation != &(_physicsSimulation)) {
-                    std::cout << "adebug adding other avatar " << avatar << " to simulation" << std::endl;  // adebug
                     skeleton->setEnableShapes(true);
                     _physicsSimulation.addEntity(skeleton);
                     _physicsSimulation.addRagdoll(skeleton);
                 }
             } else if (simulation == &(_physicsSimulation)) {
-                std::cout << "adebug removing other avatar " << avatar << " from simulation" << std::endl;  // adebug
                 _physicsSimulation.removeRagdoll(skeleton);
                 _physicsSimulation.removeEntity(skeleton);
                 skeleton->setEnableShapes(false);

libraries/shared/src/PhysicsSimulation.cpp

@@ -44,6 +44,19 @@ PhysicsSimulation::~PhysicsSimulation() {
     _otherRagdolls.clear();
 }
 
+void PhysicsSimulation::setRagdoll(Ragdoll* ragdoll) { 
+    if (_ragdoll != ragdoll) {
+        if (_ragdoll) {
+            _ragdoll->_ragdollSimulation = NULL;
+        }
+        _ragdoll = ragdoll;
+        if (_ragdoll) {
+            assert(!(_ragdoll->_ragdollSimulation));
+            _ragdoll->_ragdollSimulation = this;
+        }
+    }
+}
+
 void PhysicsSimulation::setEntity(PhysicsEntity* entity) {
     if (_entity != entity) {
         if (_entity) {
@@ -79,6 +92,7 @@ bool PhysicsSimulation::addEntity(PhysicsEntity* entity) {
         return false;
     }
     // add to list
+    assert(!(entity->_simulation));
     entity->_simulation = this;
     _otherEntities.push_back(entity);
     return true;
@@ -128,19 +142,26 @@ bool PhysicsSimulation::addRagdoll(Ragdoll* doll) {
         // list is full
         return false;
     }
-    for (int i = 0; i < numDolls; ++i) {
+    if (doll->_ragdollSimulation == this) {
-        if (doll == _otherRagdolls[i]) {
+        for (int i = 0; i < numDolls; ++i) {
-            // already in list
+            if (doll == _otherRagdolls[i]) {
-            return true;
+                // already in list
+                return true;
+            }
         }
     }
     // add to list
+    assert(!(doll->_ragdollSimulation));
+    doll->_ragdollSimulation = this;
     _otherRagdolls.push_back(doll);
     return true;
 }
 
 void PhysicsSimulation::removeRagdoll(Ragdoll* doll) {
     int numDolls = _otherRagdolls.size();
+    if (doll->_ragdollSimulation != this) {
+        return;
+    }
     for (int i = 0; i < numDolls; ++i) {
         if (doll == _otherRagdolls[i]) {
             if (i == numDolls - 1) {
@@ -152,6 +173,7 @@ void PhysicsSimulation::removeRagdoll(Ragdoll* doll) {
                 _otherRagdolls.pop_back();
                 _otherRagdolls[i] = lastDoll;
             }
+            doll->_ragdollSimulation = NULL;
             break;
         }
     }

libraries/shared/src/PhysicsSimulation.h

@@ -31,7 +31,7 @@ public:
     void setTranslation(const glm::vec3& translation) { _translation = translation; }
     const glm::vec3& getTranslation() const { return _translation; }
 
-    void setRagdoll(Ragdoll* ragdoll) { _ragdoll = ragdoll; }
+    void setRagdoll(Ragdoll* ragdoll);
     void setEntity(PhysicsEntity* entity);
 
     /// \return true if entity was added to or is already in the list