ContactPoint = set of assymetric distance consraints

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);
+    }
 }

libraries/shared/src/ContactPoint.h

@@ -16,6 +16,7 @@
 #include <glm/glm.hpp>
 
 #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<VerletPoint*> _points; // points that belong to colliding shapes
+    QVector<glm::vec3> _offsets;  // offsets to _points from contactPoint
+    QVector<float> _distances; // distances to _points from contactPoint (during enforcement stage)
     glm::vec3 _normal; // (points from A toward B)
 };
 

libraries/shared/src/PhysicsSimulation.cpp

@@ -10,7 +10,6 @@
 //
 
 #include <glm/glm.hpp>
-#include <iostream>
 
 #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<PhysicsEntity*> 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<Shape*> shapes;
     int numCollisions = _collisions.size();
@@ -242,33 +236,26 @@ void PhysicsSimulation::resolveCollisions() {
     }
 }
 
-void PhysicsSimulation::enforceContacts() {
-    QSet<Shape*> 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<quint64, ContactPoint>::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<quint64, ContactPoint>::iterator itr = _contacts.begin();
+    while (itr != _contacts.end()) {
+        itr.value().buildConstraints();
+        ++itr;
     }
-    // walk all affected shapes and apply accumulated movement
-    QSet<Shape*>::const_iterator shapeItr = shapes.constBegin();
-    while (shapeItr != shapes.constEnd()) {
-        (*shapeItr)->applyAccumulatedDelta();
-        ++shapeItr;
+}
+
+void PhysicsSimulation::enforceContactConstraints() {
+    PerformanceTimer perfTimer("contacts");
+    QMap<quint64, ContactPoint>::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);

libraries/shared/src/PhysicsSimulation.h

@@ -49,7 +49,8 @@ protected:
     void computeCollisions();
     void resolveCollisions();
 
-    void enforceContacts();
+    void buildContactConstraints();
+    void enforceContactConstraints();
     void updateContacts();
     void pruneContacts();