make main ragdoll and entity special

also addded some logic (unused) to add ragdolls of other avatars

interface/src/avatar/MyAvatar.cpp

@@ -85,12 +85,12 @@ MyAvatar::MyAvatar() :
     _skeletonModel.setEnableShapes(true);
     // The skeleton is both a PhysicsEntity and Ragdoll, so we add it to the simulation once for each type.
     _physicsSimulation.addEntity(&_skeletonModel);
-    _physicsSimulation.addRagdoll(&_skeletonModel);
+    _physicsSimulation.setRagdoll(&_skeletonModel);
 }
 
 MyAvatar::~MyAvatar() {
+    _physicsSimulation.setRagdoll(NULL);
     _physicsSimulation.removeEntity(&_skeletonModel);
-    _physicsSimulation.removeRagdoll(&_skeletonModel);
     _lookAtTargetAvatar.clear();
 }
 
@@ -1532,41 +1532,53 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
     }
     float myBoundingRadius = getBoundingRadius();
 
-    const float BODY_COLLISION_RESOLUTION_FACTOR = glm::max(1.0f, deltaTime / BODY_COLLISION_RESOLUTION_TIMESCALE);
+    // find nearest avatar
-
+    float nearestDistance2 = std::numeric_limits<float>::max();
+    Avatar* nearestAvatar = NULL;
     foreach (const AvatarSharedPointer& avatarPointer, avatars) {
         Avatar* avatar = static_cast<Avatar*>(avatarPointer.data());
         if (static_cast<Avatar*>(this) == avatar) {
             // don't collide with ourselves
             continue;
         }
-        float distance = glm::length(_position - avatar->getPosition());        
+        float distance2 = glm::distance2(_position, avatar->getPosition());        
-        if (_distanceToNearestAvatar > distance) {
+        if (nearestDistance2 > distance2) {
-            _distanceToNearestAvatar = distance;
+            nearestDistance2 = distance2;
+            nearestAvatar = avatar;
         }
-        float theirBoundingRadius = avatar->getBoundingRadius();
+    }
-        if (distance < myBoundingRadius + theirBoundingRadius) {
+    _distanceToNearestAvatar = glm::sqrt(nearestDistance2);
-            // collide our body against theirs
-            QVector<const Shape*> myShapes;
-            _skeletonModel.getBodyShapes(myShapes);
-            QVector<const Shape*> theirShapes;
-            avatar->getSkeletonModel().getBodyShapes(theirShapes);
 
-            CollisionInfo collision;
+    if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
-            if (ShapeCollider::collideShapesCoarse(myShapes, theirShapes, collision)) {
+        if (nearestAvatar != NULL) {
-                float penetrationDepth = glm::length(collision._penetration);
+            if (_distanceToNearestAvatar > myBoundingRadius + nearestAvatar->getBoundingRadius()) {
-                if (penetrationDepth > myBoundingRadius) {
+                // they aren't close enough to put into the _physicsSimulation
-                    qDebug() << "WARNING: ignoring avatar-avatar penetration depth " << penetrationDepth;
+                // so we clear the pointer
-                }
+                nearestAvatar = NULL;
-                else if (penetrationDepth > EPSILON) {
-                    setPosition(getPosition() - BODY_COLLISION_RESOLUTION_FACTOR * collision._penetration);
-                    _lastBodyPenetration += collision._penetration;
-                    emit collisionWithAvatar(getSessionUUID(), avatar->getSessionUUID(), collision);
-                }
             }
+        }
     
-            // collide their hands against us
+        foreach (const AvatarSharedPointer& avatarPointer, avatars) {
-            avatar->getHand()->collideAgainstAvatar(this, false);
+            Avatar* avatar = static_cast<Avatar*>(avatarPointer.data());
+            if (static_cast<Avatar*>(this) == avatar) {
+                // don't collide with ourselves
+                continue;
+            }
+            SkeletonModel* skeleton = &(avatar->getSkeletonModel());
+            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

@@ -24,19 +24,37 @@ int MAX_ENTITIES_PER_SIMULATION = 64;
 int MAX_COLLISIONS_PER_SIMULATION = 256;
 
 
-PhysicsSimulation::PhysicsSimulation() : _frame(0), _collisions(MAX_COLLISIONS_PER_SIMULATION) {
+PhysicsSimulation::PhysicsSimulation() : _frame(0), _entity(NULL), _ragdoll(NULL), _collisions(MAX_COLLISIONS_PER_SIMULATION) {
 }
 
 PhysicsSimulation::~PhysicsSimulation() {
     // entities have a backpointer to this simulator that must be cleaned up
-    int numEntities = _entities.size();
+    int numEntities = _otherEntities.size();
     for (int i = 0; i < numEntities; ++i) {
-        _entities[i]->_simulation = NULL;
+        _otherEntities[i]->_simulation = NULL;
+    }
+    _otherEntities.clear();
+    if (_entity) {
+        _entity->_simulation = NULL;
     }
-    _entities.clear();
 
     // but Ragdolls do not
-    _dolls.clear();
+    _ragdoll = NULL;
+    _otherRagdolls.clear();
+}
+
+void PhysicsSimulation::setEntity(PhysicsEntity* entity) {
+    if (_entity != entity) {
+        if (_entity) {
+            assert(_entity->_simulation == this);
+            _entity->_simulation = NULL;
+        }
+        _entity = entity;
+        if (_entity) {
+            assert(!(_entity->_simulation));
+            _entity->_simulation = this;
+        }
+    }
 }
 
 bool PhysicsSimulation::addEntity(PhysicsEntity* entity) {
@@ -44,25 +62,24 @@ bool PhysicsSimulation::addEntity(PhysicsEntity* entity) {
         return false;
     }
     if (entity->_simulation == this) {
-        int numEntities = _entities.size();
+        int numEntities = _otherEntities.size();
         for (int i = 0; i < numEntities; ++i) {
-            if (entity == _entities.at(i)) {
+            if (entity == _otherEntities.at(i)) {
                 // already in list
-                assert(entity->_simulation == this);
                 return true;
             }
         }
         // belongs to some other simulation
         return false;
     }
-    int numEntities = _entities.size();
+    int numEntities = _otherEntities.size();
     if (numEntities > MAX_ENTITIES_PER_SIMULATION) {
         // list is full
         return false;
     }
     // add to list
     entity->_simulation = this;
-    _entities.push_back(entity);
+    _otherEntities.push_back(entity);
     return true;
 }
 
@@ -71,17 +88,17 @@ void PhysicsSimulation::removeEntity(PhysicsEntity* entity) {
         return;
     }
     removeShapes(entity);
-    int numEntities = _entities.size();
+    int numEntities = _otherEntities.size();
     for (int i = 0; i < numEntities; ++i) {
-        if (entity == _entities.at(i)) {
+        if (entity == _otherEntities.at(i)) {
             if (i == numEntities - 1) {
                 // remove it
-                _entities.pop_back();
+                _otherEntities.pop_back();
             } else {
                 // swap the last for this one
-                PhysicsEntity* lastEntity = _entities[numEntities - 1];
+                PhysicsEntity* lastEntity = _otherEntities[numEntities - 1];
-                _entities.pop_back();
+                _otherEntities.pop_back();
-                _entities[i] = lastEntity;
+                _otherEntities[i] = lastEntity;
             }
             entity->_simulation = NULL;
             break;
@@ -105,34 +122,34 @@ bool PhysicsSimulation::addRagdoll(Ragdoll* doll) {
     if (!doll) {
         return false;
     }
-    int numDolls = _dolls.size();
+    int numDolls = _otherRagdolls.size();
     if (numDolls > MAX_DOLLS_PER_SIMULATION) {
         // list is full
         return false;
     }
     for (int i = 0; i < numDolls; ++i) {
-        if (doll == _dolls[i]) {
+        if (doll == _otherRagdolls[i]) {
             // already in list
             return true;
         }
     }
     // add to list
-    _dolls.push_back(doll);
+    _otherRagdolls.push_back(doll);
     return true;
 }
 
 void PhysicsSimulation::removeRagdoll(Ragdoll* doll) {
-    int numDolls = _dolls.size();
+    int numDolls = _otherRagdolls.size();
     for (int i = 0; i < numDolls; ++i) {
-        if (doll == _dolls[i]) {
+        if (doll == _otherRagdolls[i]) {
             if (i == numDolls - 1) {
                 // remove it
-                _dolls.pop_back();
+                _otherRagdolls.pop_back();
             } else {
                 // swap the last for this one
-                Ragdoll* lastDoll = _dolls[numDolls - 1];
+                Ragdoll* lastDoll = _otherRagdolls[numDolls - 1];
-                _dolls.pop_back();
+                _otherRagdolls.pop_back();
-                _dolls[i] = lastDoll;
+                _otherRagdolls[i] = lastDoll;
             }
             break;
         }
@@ -141,17 +158,21 @@ void PhysicsSimulation::removeRagdoll(Ragdoll* doll) {
 
 void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIterations, quint64 maxUsec) {
     ++_frame;
+    if (!_ragdoll) {
+        return;
+    }
     quint64 now = usecTimestampNow();
     quint64 startTime = now;
     quint64 expiry = startTime + maxUsec;
 
     moveRagdolls(deltaTime);
     buildContactConstraints();
-    int numDolls = _dolls.size();
+    int numDolls = _otherRagdolls.size();
     {
         PerformanceTimer perfTimer("enforce");
+        _ragdoll->enforceRagdollConstraints();
         for (int i = 0; i < numDolls; ++i) {
-            _dolls[i]->enforceRagdollConstraints();
+            _otherRagdolls[i]->enforceRagdollConstraints();
         }
     }
 
@@ -164,9 +185,9 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter
 
         { // enforce constraints
             PerformanceTimer perfTimer("enforce");
-            error = 0.0f;
+            error = _ragdoll->enforceRagdollConstraints();
             for (int i = 0; i < numDolls; ++i) {
-                error = glm::max(error, _dolls[i]->enforceRagdollConstraints());
+                error = glm::max(error, _otherRagdolls[i]->enforceRagdollConstraints());
             }
         }
         enforceContactConstraints();
@@ -180,40 +201,38 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter
 
 void PhysicsSimulation::moveRagdolls(float deltaTime) {
     PerformanceTimer perfTimer("integrate");
-    int numDolls = _dolls.size();
+    _ragdoll->stepRagdollForward(deltaTime);
+    int numDolls = _otherRagdolls.size();
     for (int i = 0; i < numDolls; ++i) {
-        _dolls.at(i)->stepRagdollForward(deltaTime);
+        _otherRagdolls.at(i)->stepRagdollForward(deltaTime);
     }
 }
 
 void PhysicsSimulation::computeCollisions() {
     PerformanceTimer perfTimer("collide");
     _collisions.clear();
-    // TODO: keep track of QSet<PhysicsEntity*> collidedEntities;
-    int numEntities = _entities.size();
-    for (int i = 0; i < numEntities; ++i) {
-        PhysicsEntity* entity = _entities.at(i);
-        const QVector<Shape*> shapes = entity->getShapes();
-        int numShapes = shapes.size();
-        // collide with self
-        for (int j = 0; j < numShapes; ++j) {
-            const Shape* shape = shapes.at(j);
-            if (!shape) {
-                continue;
-            }
-            for (int k = j+1; k < numShapes; ++k) {
-                const Shape* otherShape = shapes.at(k);
-                if (otherShape && entity->collisionsAreEnabled(j, k)) {
-                    ShapeCollider::collideShapes(shape, otherShape, _collisions);
-                }
-            }
-        }
 
-        // collide with others
+    const QVector<Shape*> shapes = _entity->getShapes();
-        for (int j = i+1; j < numEntities; ++j) {
+    int numShapes = shapes.size();
-            const QVector<Shape*> otherShapes = _entities.at(j)->getShapes();
+    // collide with self
-            ShapeCollider::collideShapesWithShapes(shapes, otherShapes, _collisions);
+    for (int i = 0; i < numShapes; ++i) {
+        const Shape* shape = shapes.at(i);
+        if (!shape) {
+            continue;
         }
+        for (int j = i+1; j < numShapes; ++j) {
+            const Shape* otherShape = shapes.at(j);
+            if (otherShape && _entity->collisionsAreEnabled(i, j)) {
+                ShapeCollider::collideShapes(shape, otherShape, _collisions);
+            }
+        }
+    }
+
+    // collide with others
+    int numEntities = _otherEntities.size();
+    for (int i = 0; i < numEntities; ++i) {
+        const QVector<Shape*> otherShapes = _otherEntities.at(i)->getShapes();
+        ShapeCollider::collideShapesWithShapes(shapes, otherShapes, _collisions);
     }
 }
 

libraries/shared/src/PhysicsSimulation.h

@@ -28,6 +28,9 @@ public:
     PhysicsSimulation();
     ~PhysicsSimulation();
 
+    void setRagdoll(Ragdoll* ragdoll) { _ragdoll = ragdoll; }
+    void setEntity(PhysicsEntity* entity);
+
     /// \return true if entity was added to or is already in the list
     bool addEntity(PhysicsEntity* entity);
 
@@ -58,8 +61,11 @@ protected:
 private:
     quint32 _frame;
 
-    QVector<Ragdoll*> _dolls;
+    PhysicsEntity* _entity;
-    QVector<PhysicsEntity*> _entities;
+    Ragdoll* _ragdoll;
+
+    QVector<Ragdoll*> _otherRagdolls;
+    QVector<PhysicsEntity*> _otherEntities;
     CollisionList _collisions;
     QMap<quint64, ContactPoint> _contacts;
 };