stubbery for processing collisions

PhysicsSimulation tells CollisionInfos to apply() themselves
CollisionInfo knows how to apply() itself to affected shapes
Shape gets _mass and some stubbed methods for accumulating movement
VerletPoint gets movement accumulator

libraries/shared/src/CollisionInfo.cpp

@@ -11,6 +11,19 @@
 
 #include "CollisionInfo.h"
 
+#include "Shape.h"
+
+CollisionInfo::CollisionInfo() :
+        _data(NULL),
+        _intData(0),
+        _shapeA(NULL),
+        _shapeB(NULL),
+        _damping(0.f),
+        _elasticity(1.f),
+        _contactPoint(0.f), 
+        _penetration(0.f), 
+        _addedVelocity(0.f) {
+}
 
 CollisionList::CollisionList(int maxSize) :
     _maxSize(maxSize),
@@ -18,6 +31,23 @@ CollisionList::CollisionList(int maxSize) :
     _collisions.resize(_maxSize);
 }
 
+void CollisionInfo::apply() {
+    assert(_shapeA);
+    // NOTE: Shape::computeEffectiveMass() has side effects: computes and caches partial Lagrangian coefficients
+    Shape* shapeA = const_cast<Shape*>(_shapeA);
+    float massA = shapeA->computeEffectiveMass(_penetration, _contactPoint);
+    float massB = Shape::MAX_MASS;
+    float totalMass = massA + massB;
+    if (_shapeB) {
+        Shape* shapeB = const_cast<Shape*>(_shapeB);
+        massB = shapeB->computeEffectiveMass(-_penetration, _contactPoint - _penetration);
+        totalMass = massA + massB;
+        shapeB->accumulateDelta(massA / totalMass, -_penetration);
+    }   
+    // NOTE: Shape::accumulateDelta() uses the coefficients from previous call to Shape::computeEffectiveMass()
+    shapeA->accumulateDelta(massB / totalMass, _penetration);
+}
+
 CollisionInfo* CollisionList::getNewCollision() {
     // return pointer to existing CollisionInfo, or NULL of list is full
     return (_size < _maxSize) ? &(_collisions[_size++]) : NULL;

libraries/shared/src/CollisionInfo.h

@@ -32,38 +32,18 @@ const quint32 VALID_COLLISION_GROUPS = 0x0f;
 
 class CollisionInfo {
 public:
-    CollisionInfo() 
-        : _data(NULL),
-        _intData(0),
-        _shapeA(NULL),
-        _shapeB(NULL),
-        _damping(0.f),
-        _elasticity(1.f),
-        _contactPoint(0.f), 
-        _penetration(0.f), 
-        _addedVelocity(0.f) {
-    }
-
-    CollisionInfo(qint32 type)
-        : _data(NULL),
-        _intData(0),
-        _shapeA(NULL),
-        _shapeB(NULL),
-        _damping(0.f),
-        _elasticity(1.f),
-        _contactPoint(0.f), 
-        _penetration(0.f), 
-        _addedVelocity(0.f) {
-    }
-
+    CollisionInfo();
     ~CollisionInfo() {}
 
-    // the value of the *Data fields depend on the type 
+    // TODO: Andrew to get rid of these data members
     void* _data;
     int _intData;       
     float _floatData;
     glm::vec3 _vecData;
 
+    /// accumulates position changes for the shapes in this collision to resolve penetration
+    void apply();
+
     Shape* getShapeA() const { return const_cast<Shape*>(_shapeA); }
     Shape* getShapeB() const { return const_cast<Shape*>(_shapeB); }
 

libraries/shared/src/PhysicsEntity.cpp

@@ -149,6 +149,7 @@ bool PhysicsEntity::findSphereCollisions(const glm::vec3& sphereCenter, float sp
 }
 
 bool PhysicsEntity::findPlaneCollisions(const glm::vec4& plane, CollisionList& collisions) {
+    // TODO: Andrew to reimplement this or make it unecessary
     bool collided = false;
     PlaneShape planeShape(plane);
     for (int i = 0; i < _shapes.size(); i++) {

libraries/shared/src/PhysicsSimulation.cpp

@@ -189,7 +189,25 @@ void PhysicsSimulation::computeCollisions() {
 }
 
 void PhysicsSimulation::processCollisions() {
-    // TODO: Andrew to implement this
+    // walk all collisions, accumulate movement on shapes, and build a list of affected shapes
+    QSet<Shape*> shapes;
+    int numCollisions = _collisionList.size();
+    for (int i = 0; i < numCollisions; ++i) {
+        CollisionInfo* collision = _collisionList.getCollision(i);
+        collision->apply();
+        // there is always a shapeA
+        shapes.insert(collision->getShapeA());
+        // but need to check for valid shapeB
+        if (collision->_shapeB) {
+            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::enforceConstraints(float minError, int maxIterations, quint64 maxUsec) {

libraries/shared/src/Ragdoll.cpp

@@ -16,6 +16,20 @@
 #include "SharedUtil.h"
 #include "SphereShape.h"
 
+// ----------------------------------------------------------------------------
+// VerletPoint
+// ----------------------------------------------------------------------------
+void VerletPoint::accumulatePush(const glm::vec3& delta) {
+    _accumulatedPush += delta;
+    ++_numPushes;
+}
+
+void VerletPoint::applyAccumulatedPush() {
+    _lastPosition = _position;
+    _position += _accumulatedPush / (float)_numPushes;
+    _numPushes = 0;
+}
+
 // ----------------------------------------------------------------------------
 // FixedConstraint
 // ----------------------------------------------------------------------------

libraries/shared/src/Ragdoll.h

@@ -21,10 +21,18 @@ class Shape;
 
 class VerletPoint {
 public:
-    VerletPoint() : _position(0.0f), _lastPosition(0.0f), _mass(0.0f) {}
+    VerletPoint() : _position(0.0f), _lastPosition(0.0f), _mass(0.0f), _accumulatedPush(0.0f), _numPushes(0) {}
+
+    void accumulatePush(const glm::vec3& delta);
+    void applyAccumulatedPush();
+
     glm::vec3 _position;
     glm::vec3 _lastPosition;
     float _mass;
+
+protected:
+    glm::vec3 _accumulatedPush;
+    int _numPushes;
 };
 
 class Constraint {

libraries/shared/src/Shape.h

@@ -19,6 +19,8 @@ class PhysicsEntity;
 
 class Shape {
 public:
+    static const float MAX_MASS = 1.0e18f; // something less than sqrt(FLT_MAX)
+
     enum Type{
         UNKNOWN_SHAPE = 0,
         SPHERE_SHAPE,
@@ -27,7 +29,7 @@ public:
         LIST_SHAPE
     };
 
-    Shape() : _type(UNKNOWN_SHAPE), _owningEntity(NULL), _boundingRadius(0.f), _translation(0.f), _rotation() { }
+    Shape() : _type(UNKNOWN_SHAPE), _owningEntity(NULL), _boundingRadius(0.f), _translation(0.f), _rotation(), _mass(MAX_MASS) { }
     virtual ~Shape() {}
 
     int getType() const { return _type; }
@@ -43,8 +45,24 @@ public:
     virtual void setTranslation(const glm::vec3& translation) { _translation = translation; }
     virtual const glm::vec3& getTranslation() const { return _translation; }
 
+    virtual void setMass(float mass) { _mass = mass; }
+    virtual float getMass() const { return _mass; }
+
     virtual bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const = 0;
 
+    /// \param penetration of collision
+    /// \param contactPoint of collision
+    /// \return the effective mass for the collision
+    /// For most shapes has side effects: computes and caches the partial Lagrangian coefficients which will be
+    /// used in the next accumulateDelta() call.
+    virtual float computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint) { return _mass; }
+
+    /// \param relativeMassFactor the final ingredient for partial Lagrangian coefficients from computeEffectiveMass()
+    /// \param penetration the delta movement
+    virtual void accumulateDelta(float relativeMassFactor, const glm::vec3& penetration) {}
+
+    virtual void applyAccumulatedDelta() {}
+
 protected:
     // these ctors are protected (used by derived classes only)
     Shape(Type type) : _type(type), _owningEntity(NULL), _boundingRadius(0.f), _translation(0.f), _rotation() {}
@@ -62,6 +80,7 @@ protected:
     float _boundingRadius;
     glm::vec3 _translation;
     glm::quat _rotation;
+    float _mass;
 };
 
 #endif // hifi_Shape_h