diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 4d2d679956..41eba45ac4 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/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::max(); + Avatar* nearestAvatar = NULL; foreach (const AvatarSharedPointer& avatarPointer, avatars) { Avatar* avatar = static_cast(avatarPointer.data()); if (static_cast(this) == avatar) { // don't collide with ourselves continue; } - float distance = glm::length(_position - avatar->getPosition()); - if (_distanceToNearestAvatar > distance) { - _distanceToNearestAvatar = distance; + float distance2 = glm::distance2(_position, avatar->getPosition()); + if (nearestDistance2 > distance2) { + nearestDistance2 = distance2; + nearestAvatar = avatar; } - float theirBoundingRadius = avatar->getBoundingRadius(); - if (distance < myBoundingRadius + theirBoundingRadius) { - // collide our body against theirs - QVector myShapes; - _skeletonModel.getBodyShapes(myShapes); - QVector theirShapes; - avatar->getSkeletonModel().getBodyShapes(theirShapes); + } + _distanceToNearestAvatar = glm::sqrt(nearestDistance2); - CollisionInfo collision; - if (ShapeCollider::collideShapesCoarse(myShapes, theirShapes, collision)) { - float penetrationDepth = glm::length(collision._penetration); - if (penetrationDepth > myBoundingRadius) { - qDebug() << "WARNING: ignoring avatar-avatar penetration depth " << penetrationDepth; - } - else if (penetrationDepth > EPSILON) { - setPosition(getPosition() - BODY_COLLISION_RESOLUTION_FACTOR * collision._penetration); - _lastBodyPenetration += collision._penetration; - emit collisionWithAvatar(getSessionUUID(), avatar->getSessionUUID(), collision); - } + if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) { + if (nearestAvatar != NULL) { + if (_distanceToNearestAvatar > myBoundingRadius + nearestAvatar->getBoundingRadius()) { + // they aren't close enough to put into the _physicsSimulation + // so we clear the pointer + nearestAvatar = NULL; + } + } + + foreach (const AvatarSharedPointer& avatarPointer, avatars) { + Avatar* avatar = static_cast(avatarPointer.data()); + if (static_cast(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); } - - // collide their hands against us - avatar->getHand()->collideAgainstAvatar(this, false); } } } diff --git a/libraries/shared/src/PhysicsSimulation.cpp b/libraries/shared/src/PhysicsSimulation.cpp index c2a852c32b..5fd40543e7 100644 --- a/libraries/shared/src/PhysicsSimulation.cpp +++ b/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]; - _entities.pop_back(); - _entities[i] = lastEntity; + PhysicsEntity* lastEntity = _otherEntities[numEntities - 1]; + _otherEntities.pop_back(); + _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]; - _dolls.pop_back(); - _dolls[i] = lastDoll; + Ragdoll* lastDoll = _otherRagdolls[numDolls - 1]; + _otherRagdolls.pop_back(); + _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 collidedEntities; - int numEntities = _entities.size(); - for (int i = 0; i < numEntities; ++i) { - PhysicsEntity* entity = _entities.at(i); - const QVector 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 - for (int j = i+1; j < numEntities; ++j) { - const QVector otherShapes = _entities.at(j)->getShapes(); - ShapeCollider::collideShapesWithShapes(shapes, otherShapes, _collisions); + const QVector shapes = _entity->getShapes(); + int numShapes = shapes.size(); + // collide with self + 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 otherShapes = _otherEntities.at(i)->getShapes(); + ShapeCollider::collideShapesWithShapes(shapes, otherShapes, _collisions); } } diff --git a/libraries/shared/src/PhysicsSimulation.h b/libraries/shared/src/PhysicsSimulation.h index 506b37533a..c8f41bb656 100644 --- a/libraries/shared/src/PhysicsSimulation.h +++ b/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 _dolls; - QVector _entities; + PhysicsEntity* _entity; + Ragdoll* _ragdoll; + + QVector _otherRagdolls; + QVector _otherEntities; CollisionList _collisions; QMap _contacts; };