diff --git a/libraries/shared/src/ContactPoint.cpp b/libraries/shared/src/ContactPoint.cpp index 1f6dbcdb11..c34d248035 100644 --- a/libraries/shared/src/ContactPoint.cpp +++ b/libraries/shared/src/ContactPoint.cpp @@ -18,8 +18,10 @@ ContactPoint::ContactPoint() : _lastFrame(0), _shapeA(NULL), _shapeB(NULL), } ContactPoint::ContactPoint(const CollisionInfo& collision, quint32 frame) : _lastFrame(frame), - _shapeA(collision.getShapeA()), _shapeB(collision.getShapeB()), _offsetA(0.0f), _offsetB(0.0f), _normal(0.0f) { + _shapeA(collision.getShapeA()), _shapeB(collision.getShapeB()), _offsetA(0.0f), _offsetB(0.0f), + _numPointsA(0), _numPoints(0), _normal(0.0f) { + _contactPoint = collision._contactPoint - 0.5f * collision._penetration; _offsetA = collision._contactPoint - _shapeA->getTranslation(); _offsetB = collision._contactPoint - collision._penetration - _shapeB->getTranslation(); float pLength = glm::length(collision._penetration); @@ -37,36 +39,94 @@ ContactPoint::ContactPoint(const CollisionInfo& collision, quint32 frame) : _las _offsetB = temp; _normal = - _normal; } + + _shapeA->getVerletPoints(_points); + _numPointsA = _points.size(); + _shapeB->getVerletPoints(_points); + _numPoints = _points.size(); + + // compute offsets for shapeA + for (int i = 0; i < _numPointsA; ++i) { + glm::vec3 offset = _points[i]->_position - collision._contactPoint; + _offsets.push_back(offset); + _distances.push_back(glm::length(offset)); + } + // compute offsets for shapeB + for (int i = _numPointsA; i < _numPoints; ++i) { + glm::vec3 offset = _points[i]->_position - collision._contactPoint + collision._penetration; + _offsets.push_back(offset); + _distances.push_back(glm::length(offset)); + } } // virtual float ContactPoint::enforce() { - glm::vec3 pointA = _shapeA->getTranslation() + _offsetA; - glm::vec3 pointB = _shapeB->getTranslation() + _offsetB; - glm::vec3 penetration = pointA - pointB; - float pDotN = glm::dot(penetration, _normal); - if (pDotN > EPSILON) { - penetration = (0.99f * pDotN) * _normal; - // NOTE: Shape::computeEffectiveMass() has side effects: computes and caches partial Lagrangian coefficients - // which are then used in the accumulateDelta() calls below. - float massA = _shapeA->computeEffectiveMass(penetration, pointA); - float massB = _shapeB->computeEffectiveMass(-penetration, pointB); - float totalMass = massA + massB; - if (totalMass < EPSILON) { - massA = massB = 1.0f; - totalMass = 2.0f; + int numPoints = _points.size(); + for (int i = 0; i < numPoints; ++i) { + glm::vec3& position = _points[i]->_position; + // TODO: use a fast distance approximation + float newDistance = glm::distance(_contactPoint, position); + float constrainedDistance = _distances[i]; + // NOTE: these "distance" constraints only push OUT, don't pull IN. + if (newDistance > EPSILON && newDistance < constrainedDistance) { + glm::vec3 direction = (_contactPoint - position) / newDistance; + glm::vec3 center = 0.5f * (_contactPoint + position); + _contactPoint = center + (0.5f * constrainedDistance) * direction; + position = center - (0.5f * constrainedDistance) * direction; } - // NOTE: Shape::accumulateDelta() uses the coefficients from previous call to Shape::computeEffectiveMass() - // and remember that penetration points from A into B - _shapeA->accumulateDelta(massB / totalMass, -penetration); - _shapeB->accumulateDelta(massA / totalMass, penetration); - return pDotN; } return 0.0f; } +void ContactPoint::buildConstraints() { + glm::vec3 pointA = _shapeA->getTranslation() + _offsetA; + glm::vec3 pointB = _shapeB->getTranslation() + _offsetB; + glm::vec3 penetration = pointA - pointB; + float pDotN = glm::dot(penetration, _normal); + bool actuallyMovePoints = (pDotN > EPSILON); + + // the contact point will be the average of the two points on the shapes + _contactPoint = 0.5f * (pointA + pointB); + + // TODO: Andrew to compute more correct lagrangian weights that provide a more realistic response. + // + // HACK: since the weights are naively equal for all points (which is what the above TODO is about) we + // don't want to use the full-strength delta because otherwise there can be annoying oscillations. We + // reduce this problem by in the short-term by attenuating the delta that is applied, the tradeoff is + // that this makes it easier for limbs to tunnel through during collisions. + const float HACK_STRENGTH = 0.5f; + + int numPoints = _points.size(); + for (int i = 0; i < numPoints; ++i) { + VerletPoint* point = _points[i]; + glm::vec3 offset = _offsets[i]; + + // split delta into parallel and perpendicular components + glm::vec3 delta = _contactPoint + offset - point->_position; + glm::vec3 paraDelta = glm::dot(delta, _normal) * _normal; + glm::vec3 perpDelta = delta - paraDelta; + + // use the relative sizes of the components to decide how much perpenducular delta to use + // perpendicular < parallel ==> static friciton ==> perpFactor = 1.0 + // perpendicular > parallel ==> dynamic friciton ==> cap to length of paraDelta ==> perpFactor < 1.0 + float paraLength = glm::length(paraDelta); + float perpLength = glm::length(perpDelta); + float perpFactor = (perpLength > paraLength && perpLength > EPSILON) ? (paraLength / perpLength) : 1.0f; + + // recombine the two components to get the final delta + delta = paraDelta + perpFactor * perpDelta; + + glm::vec3 targetPosition = point->_position + delta; + _distances[i] = glm::distance(_contactPoint, targetPosition); + if (actuallyMovePoints) { + point->_position += HACK_STRENGTH * delta; + } + } +} + void ContactPoint::updateContact(const CollisionInfo& collision, quint32 frame) { _lastFrame = frame; + _contactPoint = collision._contactPoint - 0.5f * collision._penetration; _offsetA = collision._contactPoint - collision._shapeA->getTranslation(); _offsetB = collision._contactPoint - collision._penetration - collision._shapeB->getTranslation(); float pLength = glm::length(collision._penetration); @@ -75,6 +135,7 @@ void ContactPoint::updateContact(const CollisionInfo& collision, quint32 frame) } else { _normal = glm::vec3(0.0f); } + if (collision._shapeA->getID() > collision._shapeB->getID()) { // our _shapeA always has lower ID glm::vec3 temp = _offsetA; @@ -82,4 +143,14 @@ void ContactPoint::updateContact(const CollisionInfo& collision, quint32 frame) _offsetB = temp; _normal = - _normal; } + + // compute offsets for shapeA + assert(_offsets.size() == _numPoints); + for (int i = 0; i < _numPointsA; ++i) { + _offsets[i] = (_points[i]->_position - collision._contactPoint); + } + // compute offsets for shapeB + for (int i = _numPointsA; i < _numPoints; ++i) { + _offsets[i] = (_points[i]->_position - collision._contactPoint + collision._penetration); + } } diff --git a/libraries/shared/src/ContactPoint.h b/libraries/shared/src/ContactPoint.h index 84e15a85ee..b7e0775bc1 100644 --- a/libraries/shared/src/ContactPoint.h +++ b/libraries/shared/src/ContactPoint.h @@ -16,6 +16,7 @@ #include #include "CollisionInfo.h" +#include "VerletPoint.h" class Shape; @@ -26,6 +27,7 @@ public: virtual float enforce(); + void buildConstraints(); void updateContact(const CollisionInfo& collision, quint32 frame); quint32 getLastFrame() const { return _lastFrame; } @@ -38,6 +40,12 @@ protected: Shape* _shapeB; glm::vec3 _offsetA; // contact point relative to A's center glm::vec3 _offsetB; // contact point relative to B's center + glm::vec3 _contactPoint; // a "virtual" point that is added to the simulation + int _numPointsA; // number of VerletPoints that belong to _shapeA + int _numPoints; // total number of VerletPoints + QVector _points; // points that belong to colliding shapes + QVector _offsets; // offsets to _points from contactPoint + QVector _distances; // distances to _points from contactPoint (during enforcement stage) glm::vec3 _normal; // (points from A toward B) }; diff --git a/libraries/shared/src/PhysicsSimulation.cpp b/libraries/shared/src/PhysicsSimulation.cpp index 8bcbb7255a..bac4f2ad77 100644 --- a/libraries/shared/src/PhysicsSimulation.cpp +++ b/libraries/shared/src/PhysicsSimulation.cpp @@ -10,7 +10,6 @@ // #include -#include #include "PhysicsSimulation.h" @@ -143,11 +142,13 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter quint64 expiry = startTime + maxUsec; moveRagdolls(deltaTime); - computeCollisions(); - enforceContacts(); + buildContactConstraints(); int numDolls = _dolls.size(); - for (int i = 0; i < numDolls; ++i) { - _dolls[i]->enforceRagdollConstraints(); + { + PerformanceTimer perfTimer("enforce"); + for (int i = 0; i < numDolls; ++i) { + _dolls[i]->enforceRagdollConstraints(); + } } int iterations = 0; @@ -158,30 +159,23 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter resolveCollisions(); { // enforce constraints - PerformanceTimer perfTimer("5-enforce"); + PerformanceTimer perfTimer("enforce"); error = 0.0f; for (int i = 0; i < numDolls; ++i) { error = glm::max(error, _dolls[i]->enforceRagdollConstraints()); } } + enforceContactConstraints(); ++iterations; now = usecTimestampNow(); } while (_collisions.size() != 0 && (iterations < maxIterations) && (error > minError) && (now < expiry)); - -#ifdef ANDREW_DEBUG - quint64 stepTime = usecTimestampNow()- startTime; - // temporary debug info for watching simulation performance - if (0 == (_frame % 100)) { - std::cout << "Ni = " << iterations << " E = " << error << " t = " << stepTime << std::endl; - } -#endif // ANDREW_DEBUG pruneContacts(); } void PhysicsSimulation::moveRagdolls(float deltaTime) { - PerformanceTimer perfTimer("1-integrate"); + PerformanceTimer perfTimer("integrate"); int numDolls = _dolls.size(); for (int i = 0; i < numDolls; ++i) { _dolls.at(i)->stepRagdollForward(deltaTime); @@ -189,7 +183,7 @@ void PhysicsSimulation::moveRagdolls(float deltaTime) { } void PhysicsSimulation::computeCollisions() { - PerformanceTimer perfTimer("2-collide"); + PerformanceTimer perfTimer("collide"); _collisions.clear(); // TODO: keep track of QSet collidedEntities; int numEntities = _entities.size(); @@ -220,7 +214,7 @@ void PhysicsSimulation::computeCollisions() { } void PhysicsSimulation::resolveCollisions() { - PerformanceTimer perfTimer("4-resolve"); + PerformanceTimer perfTimer("resolve"); // walk all collisions, accumulate movement on shapes, and build a list of affected shapes QSet shapes; int numCollisions = _collisions.size(); @@ -242,33 +236,26 @@ void PhysicsSimulation::resolveCollisions() { } } -void PhysicsSimulation::enforceContacts() { - QSet shapes; - int numCollisions = _collisions.size(); - for (int i = 0; i < numCollisions; ++i) { - CollisionInfo* collision = _collisions.getCollision(i); - quint64 key = collision->getShapePairKey(); - if (key == 0) { - continue; - } - QMap::iterator itr = _contacts.find(key); - if (itr != _contacts.end()) { - if (itr.value().enforce() > 0.0f) { - shapes.insert(collision->getShapeA()); - shapes.insert(collision->getShapeB()); - } - } +void PhysicsSimulation::buildContactConstraints() { + PerformanceTimer perfTimer("contacts"); + QMap::iterator itr = _contacts.begin(); + while (itr != _contacts.end()) { + itr.value().buildConstraints(); + ++itr; } - // walk all affected shapes and apply accumulated movement - QSet::const_iterator shapeItr = shapes.constBegin(); - while (shapeItr != shapes.constEnd()) { - (*shapeItr)->applyAccumulatedDelta(); - ++shapeItr; +} + +void PhysicsSimulation::enforceContactConstraints() { + PerformanceTimer perfTimer("contacts"); + QMap::iterator itr = _contacts.begin(); + while (itr != _contacts.end()) { + itr.value().enforce(); + ++itr; } } void PhysicsSimulation::updateContacts() { - PerformanceTimer perfTimer("3-updateContacts"); + PerformanceTimer perfTimer("contacts"); int numCollisions = _collisions.size(); for (int i = 0; i < numCollisions; ++i) { CollisionInfo* collision = _collisions.getCollision(i); diff --git a/libraries/shared/src/PhysicsSimulation.h b/libraries/shared/src/PhysicsSimulation.h index 8cbc39b9d3..fc6d518d62 100644 --- a/libraries/shared/src/PhysicsSimulation.h +++ b/libraries/shared/src/PhysicsSimulation.h @@ -49,7 +49,8 @@ protected: void computeCollisions(); void resolveCollisions(); - void enforceContacts(); + void buildContactConstraints(); + void enforceContactConstraints(); void updateContacts(); void pruneContacts();