collisions update shapes

VerletShapes keep pointers to VerletPoints (rather than to glm::vec3's)
VerletShapes compute lagrangian coefficients
VerletShapes send their movement accumulations to their VerletPoints

interface/src/avatar/SkeletonModel.cpp

@@ -580,12 +580,12 @@ void SkeletonModel::buildShapes() {
         }
         Shape* shape = NULL;
         if (type == Shape::SPHERE_SHAPE) {
-            shape = new VerletSphereShape(radius, &(_ragdollPoints[i]._position));
+            shape = new VerletSphereShape(radius, &(_ragdollPoints[i]));
             shape->setEntity(this);
         } else if (type == Shape::CAPSULE_SHAPE) {
             int parentIndex = joint.parentIndex;
             assert(parentIndex != -1);
-            shape = new VerletCapsuleShape(radius, &(_ragdollPoints[parentIndex]._position), &(_ragdollPoints[i]._position));
+            shape = new VerletCapsuleShape(radius, &(_ragdollPoints[parentIndex]), &(_ragdollPoints[i]));
             shape->setEntity(this);
         } 
         _shapes.push_back(shape);

libraries/shared/src/CollisionInfo.cpp

@@ -12,6 +12,7 @@
 #include "CollisionInfo.h"
 
 #include "Shape.h"
+#include "SharedUtil.h"
 
 CollisionInfo::CollisionInfo() :
         _data(NULL),
@@ -42,10 +43,16 @@ void CollisionInfo::apply() {
         Shape* shapeB = const_cast<Shape*>(_shapeB);
         massB = shapeB->computeEffectiveMass(-_penetration, _contactPoint - _penetration);
         totalMass = massA + massB;
-        shapeB->accumulateDelta(massA / totalMass, -_penetration);
+        if (totalMass < EPSILON) {
+            massA = massB = 1.0f;
+            totalMass = 2.0f;
+        }
+        // remember that _penetration points from A into B
+        shapeB->accumulateDelta(massA / totalMass, _penetration);
     }   
     // NOTE: Shape::accumulateDelta() uses the coefficients from previous call to Shape::computeEffectiveMass()
-    shapeA->accumulateDelta(massB / totalMass, _penetration);
+    // remember that _penetration points from A into B
+    shapeA->accumulateDelta(massB / totalMass, -_penetration);
 }
 
 CollisionInfo* CollisionList::getNewCollision() {

libraries/shared/src/PhysicsSimulation.cpp

@@ -128,7 +128,7 @@ void PhysicsSimulation::removeRagdoll(Ragdoll* doll) {
         }
     }
 }
-// TODO: Andrew need to implement:
+// TODO: Andrew to implement:
 // DONE (1) joints pull points (SpecialCapsuleShape would help solve this)
 // DONE (2) points slam shapes (SpecialCapsuleShape would help solve this)
 // DONE (3) detect collisions
@@ -136,9 +136,16 @@ void PhysicsSimulation::removeRagdoll(Ragdoll* doll) {
 // DONE (5) enforce constraints
 // DONE (6) make sure MyAvatar creates shapes, adds to simulation with ragdoll support
 // DONE (7) support for pairwise collision bypass
-// (8) process collisions
+// DONE (8) process collisions
-// (9) add and enforce angular contraints for joints
+// DONE (8a) stubbery
+// DONE (8b) shapes actually accumulate movement
+// DONE (9) verify that avatar shapes self collide
+// (10) slave rendered SkeletonModel to physical shapes
+// (10a) give SkeletonModel duplicate JointState data
+// (10b) figure out how to slave dupe JointStates to physical shapes
+// (11) add and enforce angular contraints for joints
 void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIterations, quint64 maxUsec) {
+    static int adebug = 0; ++adebug;
     quint64 startTime = usecTimestampNow();
     quint64 expiry = startTime + maxUsec;
 
@@ -148,6 +155,9 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter
     enforceConstraints(minError, maxIterations, expiry - usecTimestampNow());
 
     _stepTime = usecTimestampNow()- startTime;
+    if (0 == (adebug % 200)) {
+        std::cout << " adebug nC = " << _numCollisions << "  i = " << _numIterations << "  e = " << _constraintError << "  t = " << _stepTime << std::endl;  // adebug
+    }
 }
 
 void PhysicsSimulation::moveRagdolls(float deltaTime) {

libraries/shared/src/Ragdoll.cpp

@@ -19,15 +19,17 @@
 // ----------------------------------------------------------------------------
 // VerletPoint
 // ----------------------------------------------------------------------------
-void VerletPoint::accumulatePush(const glm::vec3& delta) {
+void VerletPoint::accumulateDelta(const glm::vec3& delta) {
-    _accumulatedPush += delta;
+    _accumulatedDelta += delta;
-    ++_numPushes;
+    ++_numDeltas;
 }
 
-void VerletPoint::applyAccumulatedPush() {
+void VerletPoint::applyAccumulatedDelta() {
-    _lastPosition = _position;
+    if (_numDeltas > 0) { 
-    _position += _accumulatedPush / (float)_numPushes;
+        _position += _accumulatedDelta / (float)_numDeltas;
-    _numPushes = 0;
+        _accumulatedDelta = glm::vec3(0.0f);
+        _numDeltas = 0;
+    }
 }
 
 // ----------------------------------------------------------------------------

libraries/shared/src/Ragdoll.h

@@ -19,20 +19,29 @@
 
 class Shape;
 
+// TODO: Andrew to move VerletPoint class to its own file
 class VerletPoint {
 public:
-    VerletPoint() : _position(0.0f), _lastPosition(0.0f), _mass(0.0f), _accumulatedPush(0.0f), _numPushes(0) {}
+    VerletPoint() : _position(0.0f), _lastPosition(0.0f), _mass(1.0f), _accumulatedDelta(0.0f), _numDeltas(0) {}
 
-    void accumulatePush(const glm::vec3& delta);
+    void accumulateDelta(const glm::vec3& delta);
-    void applyAccumulatedPush();
+    void applyAccumulatedDelta();
+
+    glm::vec3 getAccumulatedDelta() const { 
+        glm::vec3 foo(0.0f); 
+        if (_numDeltas > 0) { 
+            foo = _accumulatedDelta / (float)_numDeltas; 
+        } 
+        return foo; 
+    }
 
     glm::vec3 _position;
     glm::vec3 _lastPosition;
     float _mass;
 
-protected:
+private:
-    glm::vec3 _accumulatedPush;
+    glm::vec3 _accumulatedDelta;
-    int _numPushes;
+    int _numDeltas;
 };
 
 class Constraint {

libraries/shared/src/VerletCapsuleShape.cpp

@@ -10,21 +10,24 @@
 //
 
 #include "VerletCapsuleShape.h"
+
+#include "Ragdoll.h"    // for VerletPoint
 #include "SharedUtil.h"
 
-VerletCapsuleShape::VerletCapsuleShape(glm::vec3* startPoint, glm::vec3* endPoint) : 
+VerletCapsuleShape::VerletCapsuleShape(VerletPoint* startPoint, VerletPoint* endPoint) : 
-        CapsuleShape(), _startPoint(startPoint), _endPoint(endPoint) {
+        CapsuleShape(), _startPoint(startPoint), _endPoint(endPoint), _startLagrangeCoef(0.5f), _endLagrangeCoef(0.5f) {
     assert(startPoint);
     assert(endPoint);
-    _halfHeight = 0.5f * glm::distance(*_startPoint, *_endPoint);
+    _halfHeight = 0.5f * glm::distance(_startPoint->_position, _endPoint->_position);
     updateBoundingRadius();
 }
 
-VerletCapsuleShape::VerletCapsuleShape(float radius, glm::vec3* startPoint, glm::vec3* endPoint) :
+VerletCapsuleShape::VerletCapsuleShape(float radius, VerletPoint* startPoint, VerletPoint* endPoint) :
-        CapsuleShape(radius, 1.0f), _startPoint(startPoint), _endPoint(endPoint) {
+        CapsuleShape(radius, 1.0f), _startPoint(startPoint), _endPoint(endPoint), 
+        _startLagrangeCoef(0.5f), _endLagrangeCoef(0.5f) {
     assert(startPoint);
     assert(endPoint);
-    _halfHeight = 0.5f * glm::distance(*_startPoint, *_endPoint);
+    _halfHeight = 0.5f * glm::distance(_startPoint->_position, _endPoint->_position);
     updateBoundingRadius();
 }
 
@@ -46,8 +49,8 @@ void VerletCapsuleShape::setRotation(const glm::quat& rotation) {
     glm::vec3 center = getTranslation();
     float halfHeight = getHalfHeight();
     glm::vec3 axis = rotation * DEFAULT_CAPSULE_AXIS;
-    *_startPoint = center - halfHeight * axis;
+    _startPoint->_position = center - halfHeight * axis;
-    *_endPoint = center + halfHeight * axis;
+    _endPoint->_position = center + halfHeight * axis;
 }
 
 void VerletCapsuleShape::setTranslation(const glm::vec3& position) {
@@ -56,35 +59,78 @@ void VerletCapsuleShape::setTranslation(const glm::vec3& position) {
 
     // update the points such that their center is at position
     glm::vec3 movement = position - getTranslation();
-    *_startPoint += movement;
+    _startPoint->_position += movement;
-    *_endPoint += movement;
+    _endPoint->_position += movement;
 }
 
 const glm::vec3& VerletCapsuleShape::getTranslation() const {
     // the "translation" of this shape must be computed on the fly
     VerletCapsuleShape* thisCapsule = const_cast<VerletCapsuleShape*>(this);
-    thisCapsule->_translation = 0.5f * ((*_startPoint) + (*_endPoint));
+    thisCapsule->_translation = 0.5f * (_startPoint->_position + _endPoint->_position);
     return _translation;
 }
 
+float VerletCapsuleShape::computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint) {
+    glm::vec3 startLeg = _startPoint->_position - contactPoint;
+    glm::vec3 endLeg = _endPoint->_position - contactPoint;
+
+    // TODO: use fast approximate distance calculations here
+    float startLength = glm::length(startLeg);
+    float endlength = glm::length(endLeg);
+
+    // The raw coefficient is proportional to the other leg's length multiplied by the dot-product
+    // of the penetration and this leg direction.  We don't worry about the common penetration length 
+    // because it is normalized out later.
+    float startCoef = glm::abs(glm::dot(startLeg, penetration)) * endlength / (startLength + EPSILON);
+    float endCoef = glm::abs(glm::dot(endLeg, penetration)) * startLength / (endlength + EPSILON);
+
+    float maxCoef = glm::max(startCoef, endCoef);
+    if (maxCoef > EPSILON) {
+        // One of these coeficients will be 1.0, the other will be less --> 
+        // one endpoint will move the full amount while the other will move less.
+        _startLagrangeCoef = startCoef / maxCoef;
+        _endLagrangeCoef = endCoef / maxCoef;
+        assert(!glm::isnan(_startLagrangeCoef));
+        assert(!glm::isnan(_startLagrangeCoef));
+    } else {
+        // The coefficients are the same --> the collision will move both equally
+        // as if the object were solid.
+        _startLagrangeCoef = 1.0f;
+        _endLagrangeCoef = 1.0f;
+    }
+    // the effective mass is the weighted sum of the two endpoints
+    return _startLagrangeCoef * _startPoint->_mass + _endLagrangeCoef * _endPoint->_mass;
+}
+
+void VerletCapsuleShape::accumulateDelta(float relativeMassFactor, const glm::vec3& penetration) {
+    assert(!glm::isnan(relativeMassFactor));
+    _startPoint->accumulateDelta(relativeMassFactor * _startLagrangeCoef * penetration);
+    _endPoint->accumulateDelta(relativeMassFactor * _endLagrangeCoef * penetration);
+}
+
+void VerletCapsuleShape::applyAccumulatedDelta() {
+    _startPoint->applyAccumulatedDelta();
+    _endPoint->applyAccumulatedDelta();
+}
+
 // virtual
 float VerletCapsuleShape::getHalfHeight() const {
-    return 0.5f * glm::distance(*_startPoint, *_endPoint);
+    return 0.5f * glm::distance(_startPoint->_position, _endPoint->_position);
 }
 
 // virtual
 void VerletCapsuleShape::getStartPoint(glm::vec3& startPoint) const {
-    startPoint = *_startPoint;
+    startPoint = _startPoint->_position;
 }
 
 // virtual
 void VerletCapsuleShape::getEndPoint(glm::vec3& endPoint) const {
-    endPoint = *_endPoint;
+    endPoint = _endPoint->_position;
 }
 
 // virtual
 void VerletCapsuleShape::computeNormalizedAxis(glm::vec3& axis) const {
-    glm::vec3 unormalizedAxis = *_endPoint - *_startPoint;
+    glm::vec3 unormalizedAxis = _endPoint->_position - _startPoint->_position;
     float fullLength = glm::length(unormalizedAxis);
     if (fullLength > EPSILON) {
         axis = unormalizedAxis / fullLength;
@@ -101,8 +147,8 @@ void VerletCapsuleShape::setHalfHeight(float halfHeight) {
     glm::vec3 center = getTranslation();
     glm::vec3 axis;
     computeNormalizedAxis(axis);
-    *_startPoint = center - halfHeight * axis;
+    _startPoint->_position = center - halfHeight * axis;
-    *_endPoint = center + halfHeight * axis;
+    _endPoint->_position = center + halfHeight * axis;
     _boundingRadius = _radius + halfHeight;
 }
 
@@ -114,7 +160,7 @@ void VerletCapsuleShape::setRadiusAndHalfHeight(float radius, float halfHeight)
 
 // virtual
 void VerletCapsuleShape::setEndPoints(const glm::vec3& startPoint, const glm::vec3& endPoint) {
-    *_startPoint = startPoint;
+    _startPoint->_position = startPoint;
-    *_endPoint = endPoint;
+    _endPoint->_position = endPoint;
     updateBoundingRadius();
 }

libraries/shared/src/VerletCapsuleShape.h

@@ -32,16 +32,21 @@
 //     down in a hot simulation loop, such as when processing collision results.  Best to
 //     just let the verlet simulation do its thing and not try to constantly force a rotation.
 
+class VerletPoint;
+
 class VerletCapsuleShape : public CapsuleShape {
 public:
-    VerletCapsuleShape(glm::vec3* startPoint, glm::vec3* endPoint);
+    VerletCapsuleShape(VerletPoint* startPoint, VerletPoint* endPoint);
-    VerletCapsuleShape(float radius, glm::vec3* startPoint, glm::vec3* endPoint);
+    VerletCapsuleShape(float radius, VerletPoint* startPoint, VerletPoint* endPoint);
 
     // virtual overrides from Shape
     const glm::quat& getRotation() const;
     void setRotation(const glm::quat& rotation);
     void setTranslation(const glm::vec3& position);
     const glm::vec3& getTranslation() const;
+    float computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint);
+    void accumulateDelta(float relativeMassFactor, const glm::vec3& penetration);
+    void applyAccumulatedDelta();
 
     //float getRadius() const { return _radius; }
     virtual float getHalfHeight() const;
@@ -64,8 +69,15 @@ public:
 
 protected:
     // NOTE: VerletCapsuleShape does NOT own the data in its points.
-    glm::vec3* _startPoint;
+    VerletPoint* _startPoint;
-    glm::vec3* _endPoint;
+    VerletPoint* _endPoint;
+
+    // The LagrangeCoef's are numerical weights for distributing collision movement
+    // between the relevant VerletPoints associated with this shape.  They are functions
+    // of the movement parameters and are computed (and cached) in computeEffectiveMass() 
+    // and then used in the subsequent accumulateDelta().
+    float _startLagrangeCoef;
+    float _endLagrangeCoef;
 };
 
 #endif // hifi_VerletCapsuleShape_h

libraries/shared/src/VerletSphereShape.cpp

@@ -11,22 +11,40 @@
 
 #include "VerletSphereShape.h"
 
-VerletSphereShape::VerletSphereShape(glm::vec3* centerPoint) : SphereShape() { 
+#include "Ragdoll.h"    // for VerletPoint
+
+VerletSphereShape::VerletSphereShape(VerletPoint* centerPoint) : SphereShape() { 
     assert(centerPoint);
     _point = centerPoint;
 }
 
-VerletSphereShape::VerletSphereShape(float radius, glm::vec3* centerPoint) : SphereShape(radius) {
+VerletSphereShape::VerletSphereShape(float radius, VerletPoint* centerPoint) : SphereShape(radius) {
     assert(centerPoint);
     _point = centerPoint;
 }
 
 // virtual from Shape class
 void VerletSphereShape::setTranslation(const glm::vec3& position) {
-    *_point = position;
+    _point->_position = position;
+    _point->_lastPosition = position;
 }
 
 // virtual from Shape class
 const glm::vec3& VerletSphereShape::getTranslation() const {
-    return *_point;
+    return _point->_position;
+}
+
+// virtual
+float VerletSphereShape::computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint) {
+    return _point->_mass;
+}
+
+// virtual
+void VerletSphereShape::accumulateDelta(float relativeMassFactor, const glm::vec3& penetration) {
+    _point->accumulateDelta(relativeMassFactor * penetration);
+}
+
+// virtual
+void VerletSphereShape::applyAccumulatedDelta() {
+    _point->applyAccumulatedDelta();
 }

libraries/shared/src/VerletSphereShape.h

@@ -24,19 +24,24 @@
 // (2) A VerletShape doesn't own the points that it uses, so you must be careful not to
 //     leave dangling pointers around.
 
+class VerletPoint;
+
 class VerletSphereShape : public SphereShape {
 public:
-    VerletSphereShape(glm::vec3* centerPoint);
+    VerletSphereShape(VerletPoint* point);
 
-    VerletSphereShape(float radius, glm::vec3* centerPoint);
+    VerletSphereShape(float radius, VerletPoint* centerPoint);
 
     // virtual overrides from Shape
     void setTranslation(const glm::vec3& position);
     const glm::vec3& getTranslation() const;
+    float computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint);
+    void accumulateDelta(float relativeMassFactor, const glm::vec3& penetration);
+    void applyAccumulatedDelta();
 
 protected:
     // NOTE: VerletSphereShape does NOT own its _point
-    glm::vec3* _point;
+    VerletPoint* _point;
 };
 
 #endif // hifi_VerletSphereShape_h

tests/physics/src/VerletShapeTests.cpp

@@ -17,6 +17,7 @@
 #include <glm/gtx/quaternion.hpp>
 
 #include <CollisionInfo.h>
+#include <Ragdoll.h>    // for VerletPoint
 #include <ShapeCollider.h>
 #include <SharedUtil.h>
 #include <VerletCapsuleShape.h>
@@ -33,16 +34,16 @@ static const glm::vec3 zAxis(0.0f, 0.0f, 1.0f);
 void VerletShapeTests::setSpherePosition() {
     float radius = 1.0f;
     glm::vec3 offset(1.23f, 4.56f, 7.89f);
-    glm::vec3 point;
+    VerletPoint point;
     VerletSphereShape sphere(radius, &point);
 
-    point = glm::vec3(0.f);
+    point._position = glm::vec3(0.f);
     float d = glm::distance(glm::vec3(0.0f), sphere.getTranslation());
     if (d != 0.0f) {
         std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should be at origin" << std::endl;
     }
 
-    point = offset;
+    point._position = offset;
     d = glm::distance(glm::vec3(0.0f), sphere.getTranslation());
     if (d != glm::length(offset)) {
         std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should be at offset" << std::endl;
@@ -60,15 +61,15 @@ void VerletShapeTests::sphereMissesSphere() {
     float offsetDistance = alpha * radiusA + beta * radiusB;
 
     // create points for the sphere centers
-    glm::vec3 points[2];
+    VerletPoint points[2];
 
     // give pointers to the spheres
     VerletSphereShape sphereA(radiusA, (points + 0));
     VerletSphereShape sphereB(radiusB, (points + 1));
 
     // set the positions of the spheres by slamming the points directly
-    points[0] = origin;
+    points[0]._position = origin;
-    points[1] = offsetDistance * offsetDirection;
+    points[1]._position = offsetDistance * offsetDirection;
 
     CollisionList collisions(16);
 
@@ -118,15 +119,15 @@ void VerletShapeTests::sphereTouchesSphere() {
     glm::vec3 expectedPenetration = expectedPenetrationDistance * offsetDirection;
 
     // create two points for the sphere centers
-    glm::vec3 points[2];
+    VerletPoint points[2];
 
     // give pointers to the spheres
     VerletSphereShape sphereA(radiusA, points+0);
     VerletSphereShape sphereB(radiusB, points+1);
 
     // set the positions of the spheres by slamming the points directly
-    points[0] = origin;
+    points[0]._position = origin;
-    points[1] = offsetDistance * offsetDirection;
+    points[1]._position = offsetDistance * offsetDirection;
 
     CollisionList collisions(16);
     int numCollisions = 0;
@@ -213,9 +214,9 @@ void VerletShapeTests::sphereMissesCapsule() {
     float radialOffset = 1.2f * radiusA + 1.3f * radiusB;
     
     // create points for the sphere + capsule
-    glm::vec3 points[3];
+    VerletPoint points[3];
     for (int i = 0; i < 3; ++i) {
-        points[i] = glm::vec3(0.0f);
+        points[i]._position = glm::vec3(0.0f);
     }
 
     // give the points to the shapes
@@ -277,9 +278,9 @@ void VerletShapeTests::sphereTouchesCapsule() {
     float radialOffset = alpha * radiusA + beta * radiusB;
     
     // create points for the sphere + capsule
-    glm::vec3 points[3];
+    VerletPoint points[3];
     for (int i = 0; i < 3; ++i) {
-        points[i] = glm::vec3(0.0f);
+        points[i]._position = glm::vec3(0.0f);
     }
 
     // give the points to the shapes
@@ -496,9 +497,9 @@ void VerletShapeTests::capsuleMissesCapsule() {
     float totalHalfLength = totalRadius + halfHeightA + halfHeightB;
 
     // create points for the shapes
-    glm::vec3 points[4];
+    VerletPoint points[4];
     for (int i = 0; i < 4; ++i) {
-        points[i] = glm::vec3(0.0f);
+        points[i]._position = glm::vec3(0.0f);
     }
 
     // give the points to the shapes
@@ -574,9 +575,9 @@ void VerletShapeTests::capsuleTouchesCapsule() {
     float totalHalfLength = totalRadius + halfHeightA + halfHeightB;
 
     // create points for the shapes
-    glm::vec3 points[4];
+    VerletPoint points[4];
     for (int i = 0; i < 4; ++i) {
-        points[i] = glm::vec3(0.0f);
+        points[i]._position = glm::vec3(0.0f);
     }
 
     // give the points to the shapes