add ContactConstraint and use to impede muscles

libraries/shared/src/ContactConstraint.cpp

@@ -0,0 +1,83 @@
+//
+//  ContactConstraint.cpp
+//  libraries/shared/src
+//
+//  Created by Andrew Meadows 2014.07.30
+//  Copyright 2014 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include "ContactConstraint.h"
+#include "Shape.h"
+#include "SharedUtil.h"
+
+ContactConstraint::ContactConstraint() : _lastFrame(0), _shapeA(NULL), _shapeB(NULL), 
+        _offsetA(0.0f), _offsetB(0.0f), _normal(0.0f) {
+}
+
+ContactConstraint::ContactConstraint(const CollisionInfo& collision, quint32 frame) : _lastFrame(frame),
+        _shapeA(collision.getShapeA()), _shapeB(collision.getShapeB()), _offsetA(0.0f), _offsetB(0.0f), _normal(0.0f) {
+
+    _offsetA = collision._contactPoint - _shapeA->getTranslation();
+    _offsetB = collision._contactPoint - collision._penetration - _shapeB->getTranslation();
+    float pLength = glm::length(collision._penetration);
+    if (pLength > EPSILON) {
+        _normal = collision._penetration / pLength;
+    }
+
+    if (_shapeA->getID() > _shapeB->getID()) {
+        // swap so that _shapeA always has lower ID
+        _shapeA = collision.getShapeB();
+        _shapeB = collision.getShapeA();
+
+        glm::vec3 temp = _offsetA;
+        _offsetA = _offsetB;
+        _offsetB = temp;
+        _normal = - _normal;
+    }
+}
+
+// virtual 
+float ContactConstraint::enforce() {
+    glm::vec3 pointA = _shapeA->getTranslation() + _offsetA;
+    glm::vec3 pointB = _shapeB->getTranslation() + _offsetB;
+    glm::vec3 penetration = pointA - pointB;
+    if (glm::dot(penetration, _normal) > EPSILON) {
+        // 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;
+        }
+        // NOTE: Shape::accumulateDelta() uses the coefficients from previous call to Shape::computeEffectiveMass()
+        // and remember that penetration points from A into B
+        _shapeA->accumulateDelta(0.99f * massB / totalMass, -penetration);
+        _shapeB->accumulateDelta(0.99f * massA / totalMass, penetration);
+        return glm::length(penetration);
+    }
+    return 0.0f;
+}
+
+void ContactConstraint::updateContact(const CollisionInfo& collision, quint32 frame) {
+    _lastFrame = frame;
+    _offsetA = collision._contactPoint - collision._shapeA->getTranslation();
+    _offsetB = collision._contactPoint - collision._penetration - collision._shapeB->getTranslation();
+    float pLength = glm::length(collision._penetration);
+    if (pLength > EPSILON) {
+        _normal = collision._penetration / pLength;
+    } else {
+        _normal = glm::vec3(0.0f);
+    }
+    if (collision._shapeA->getID() > collision._shapeB->getID()) {
+        // our _shapeA always has lower ID
+        glm::vec3 temp = _offsetA;
+        _offsetA = _offsetB;
+        _offsetB = temp;
+        _normal = - _normal;
+    }
+}

libraries/shared/src/ContactConstraint.h

@@ -0,0 +1,44 @@
+//
+//  ContactConstraint.h
+//  libraries/shared/src
+//
+//  Created by Andrew Meadows 2014.07.30
+//  Copyright 2014 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#ifndef hifi_ContactConstraint_h
+#define hifi_ContactConstraint_h
+
+#include <QtGlobal>
+#include <glm/glm.hpp>
+
+#include "CollisionInfo.h"
+
+class Shape;
+
+class ContactConstraint {
+public:
+    ContactConstraint();
+    ContactConstraint(const CollisionInfo& collision, quint32 frame);
+
+    virtual float enforce();
+   
+    void updateContact(const CollisionInfo& collision, quint32 frame);
+    quint32 getLastFrame() const { return _lastFrame; }
+
+    Shape* getShapeA() const { return _shapeA; }
+    Shape* getShapeB() const { return _shapeB; }
+
+protected:
+    quint32 _lastFrame; // frame count of last update
+    Shape* _shapeA;
+    Shape* _shapeB;
+    glm::vec3 _offsetA; // contact point relative to A's center
+    glm::vec3 _offsetB; // contact point relative to B's center
+    glm::vec3 _normal; // (points from A toward B)
+};
+
+#endif // hifi_ContactConstraint_h

libraries/shared/src/PhysicsSimulation.cpp

@@ -25,8 +25,7 @@ int MAX_ENTITIES_PER_SIMULATION = 64;
 int MAX_COLLISIONS_PER_SIMULATION = 256;
 
 
-PhysicsSimulation::PhysicsSimulation() : _collisionList(MAX_COLLISIONS_PER_SIMULATION), 
-        _frame(0), _numIterations(0), _numCollisions(0), _constraintError(0.0f), _stepTime(0) {
+PhysicsSimulation::PhysicsSimulation() : _frame(0), _collisions(MAX_COLLISIONS_PER_SIMULATION) {
 }
 
 PhysicsSimulation::~PhysicsSimulation() {
@@ -88,6 +87,15 @@ void PhysicsSimulation::removeEntity(PhysicsEntity* entity) {
             break;
         }
     }
+    // remove corresponding contacts
+    QMap<quint64, ContactConstraint>::iterator itr = _contacts.begin();
+    while (itr != _contacts.end()) {
+        if (entity == itr.value().getShapeA()->getEntity() || entity == itr.value().getShapeB()->getEntity()) {
+            itr = _contacts.erase(itr);
+        } else {
+            ++itr;
+        }
+    }
 }
 
 bool PhysicsSimulation::addRagdoll(Ragdoll* doll) {
@@ -135,14 +143,19 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter
     quint64 expiry = startTime + maxUsec;
 
     moveRagdolls(deltaTime);
-
+    computeCollisions();
+    enforceContacts();
     int numDolls = _dolls.size();
-    _numCollisions = 0;
+    for (int i = 0; i < numDolls; ++i) {
+        _dolls[i]->enforceRagdollConstraints();
+    }
+
     int iterations = 0;
     float error = 0.0f;
     do {
         computeCollisions();
-        processCollisions();
+        updateContacts();
+        resolveCollisions();
 
         { // enforce constraints
             PerformanceTimer perfTimer("4-enforce");
@@ -154,19 +167,17 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter
         ++iterations;
 
         now = usecTimestampNow();
-    } while (_numCollisions != 0 && (iterations < maxIterations) && (error > minError) && (now < expiry));
+    } while (_collisions.size() != 0 && (iterations < maxIterations) && (error > minError) && (now < expiry));
 
-    _numIterations = iterations;
-    _constraintError = error;
-    _stepTime = usecTimestampNow()- startTime;
 
 #ifdef ANDREW_DEBUG
+    quint64 stepTime = usecTimestampNow()- startTime;
     // temporary debug info for watching simulation performance
-    static int adebug = 0; ++adebug;
-    if (0 == (adebug % 100)) {
-        std::cout << "adebug Ni = " << _numIterations << "  E = " << error  << "  t = " << _stepTime << std::endl;  // adebug
+    if (0 == (_frame % 100)) {
+        std::cout << "Ni = " << iterations << "  E = " << error  << "  t = " << stepTime << std::endl;
     }
 #endif // ANDREW_DEBUG
+    pruneContacts();
 }
 
 void PhysicsSimulation::moveRagdolls(float deltaTime) {
@@ -179,7 +190,7 @@ void PhysicsSimulation::moveRagdolls(float deltaTime) {
 
 void PhysicsSimulation::computeCollisions() {
     PerformanceTimer perfTimer("2-collide");
-    _collisionList.clear();
+    _collisions.clear();
     // TODO: keep track of QSet<PhysicsEntity*> collidedEntities;
     int numEntities = _entities.size();
     for (int i = 0; i < numEntities; ++i) {
@@ -195,7 +206,7 @@ void PhysicsSimulation::computeCollisions() {
             for (int k = j+1; k < numShapes; ++k) {
                 const Shape* otherShape = shapes.at(k);
                 if (otherShape && entity->collisionsAreEnabled(j, k)) {
-                    ShapeCollider::collideShapes(shape, otherShape, _collisionList);
+                    ShapeCollider::collideShapes(shape, otherShape, _collisions);
                 }
             }
         }
@@ -203,19 +214,18 @@ void PhysicsSimulation::computeCollisions() {
         // collide with others
         for (int j = i+1; j < numEntities; ++j) {
             const QVector<Shape*> otherShapes = _entities.at(j)->getShapes();
-            ShapeCollider::collideShapesWithShapes(shapes, otherShapes, _collisionList);
+            ShapeCollider::collideShapesWithShapes(shapes, otherShapes, _collisions);
         }
     }
-    _numCollisions = _collisionList.size();
 }
 
-void PhysicsSimulation::processCollisions() {
+void PhysicsSimulation::resolveCollisions() {
     PerformanceTimer perfTimer("3-resolve");
     // walk all collisions, accumulate movement on shapes, and build a list of affected shapes
     QSet<Shape*> shapes;
-    int numCollisions = _collisionList.size();
+    int numCollisions = _collisions.size();
     for (int i = 0; i < numCollisions; ++i) {
-        CollisionInfo* collision = _collisionList.getCollision(i);
+        CollisionInfo* collision = _collisions.getCollision(i);
         collision->apply();
         // there is always a shapeA
         shapes.insert(collision->getShapeA());
@@ -231,3 +241,59 @@ void PhysicsSimulation::processCollisions() {
         ++shapeItr;
     }
 }
+
+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, ContactConstraint>::iterator itr = _contacts.find(key);
+        if (itr != _contacts.end()) {
+            if (itr.value().enforce() > 0.0f) {
+                shapes.insert(collision->getShapeA());
+                shapes.insert(collision->getShapeB());
+            }
+        }
+    }
+    // walk all affected shapes and apply accumulated movement
+    QSet<Shape*>::const_iterator shapeItr = shapes.constBegin();
+    while (shapeItr != shapes.constEnd()) {
+        (*shapeItr)->applyAccumulatedDelta();
+        ++shapeItr;
+    }
+}
+
+void PhysicsSimulation::updateContacts() {
+    PerformanceTimer perfTimer("3.5-updateContacts");
+    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, ContactConstraint>::iterator itr = _contacts.find(key);
+        if (itr == _contacts.end()) {
+            _contacts.insert(key, ContactConstraint(*collision, _frame));
+        } else {
+            itr.value().updateContact(*collision, _frame);
+        }
+    }
+}
+
+const quint32 MAX_CONTACT_FRAME_LIFETIME = 2;
+
+void PhysicsSimulation::pruneContacts() {
+    QMap<quint64, ContactConstraint>::iterator itr = _contacts.begin();
+    while (itr != _contacts.end()) {
+        if (_frame - itr.value().getLastFrame() > MAX_CONTACT_FRAME_LIFETIME) {
+            itr = _contacts.erase(itr);
+        } else {
+            ++itr;
+        }
+    }
+}

libraries/shared/src/PhysicsSimulation.h

@@ -13,9 +13,11 @@
 #define hifi_PhysicsSimulation
 
 #include <QtGlobal>
+#include <QMap>
 #include <QVector>
 
 #include "CollisionInfo.h"
+#include "ContactConstraint.h"
 
 class PhysicsEntity;
 class Ragdoll;
@@ -42,21 +44,22 @@ public:
     /// \return distance of largest movement
     void stepForward(float deltaTime, float minError, int maxIterations, quint64 maxUsec);
 
+protected:
     void moveRagdolls(float deltaTime);
     void computeCollisions();
-    void processCollisions();
+    void resolveCollisions();
+
+    void enforceContacts();
+    void updateContacts();
+    void pruneContacts();
 
 private:
-    CollisionList _collisionList;
-    QVector<PhysicsEntity*> _entities;
-    QVector<Ragdoll*> _dolls;
-
-    // some stats
     quint32 _frame;
-    int _numIterations;
-    int _numCollisions;
-    float _constraintError;
-    quint64 _stepTime;
+
+    QVector<Ragdoll*> _dolls;
+    QVector<PhysicsEntity*> _entities;
+    CollisionList _collisions;
+    QMap<quint64, ContactConstraint> _contacts;
 };
 
 #endif // hifi_PhysicsSimulation