diff --git a/interface/src/avatar/MuscleConstraint.cpp b/interface/src/avatar/MuscleConstraint.cpp
index f4ba7975d0..31da56d3d3 100644
--- a/interface/src/avatar/MuscleConstraint.cpp
+++ b/interface/src/avatar/MuscleConstraint.cpp
@@ -16,8 +16,7 @@
 
 const float DEFAULT_MUSCLE_STRENGTH = 0.5f * MAX_MUSCLE_STRENGTH;
 
-MuscleConstraint::MuscleConstraint(VerletPoint* parent, VerletPoint* child)
-        : _rootPoint(parent), _childPoint(child), 
+MuscleConstraint::MuscleConstraint(VerletPoint* parent, VerletPoint* child) : _rootPoint(parent), _childPoint(child), 
         _parentIndex(-1), _childndex(-1), _strength(DEFAULT_MUSCLE_STRENGTH) {
     _childOffset = child->_position - parent->_position;
 }
diff --git a/interface/src/avatar/MuscleConstraint.h b/interface/src/avatar/MuscleConstraint.h
index 882b351b80..b2387a33f0 100644
--- a/interface/src/avatar/MuscleConstraint.h
+++ b/interface/src/avatar/MuscleConstraint.h
@@ -17,7 +17,7 @@
 // MuscleConstraint is a simple constraint that pushes the child toward an offset relative to the parent.
 // It does NOT push the parent.
 
-const float MAX_MUSCLE_STRENGTH = 0.5f;
+const float MAX_MUSCLE_STRENGTH = 0.75f;
 
 class MuscleConstraint : public Constraint {
 public:
diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index dc6a309e70..c8e7451fc1 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -568,8 +568,8 @@ void SkeletonModel::buildRagdollConstraints() {
         ++itr;
     }
 
-    float MAX_STRENGTH = 0.3f;
-    float MIN_STRENGTH = 0.005f;
+    float MAX_STRENGTH = 0.6f;
+    float MIN_STRENGTH = 0.05f;
     // each joint gets a MuscleConstraint to its parent
     for (int i = 1; i < numPoints; ++i) {
         const JointState& state = _jointStates.at(i);
@@ -578,7 +578,6 @@ void SkeletonModel::buildRagdollConstraints() {
             continue;
         }
         MuscleConstraint* constraint = new MuscleConstraint(&(_ragdollPoints[p]), &(_ragdollPoints[i]));
-        _ragdollConstraints.push_back(constraint);
         _muscleConstraints.push_back(constraint);
 
         // Short joints are more susceptible to wiggle so we modulate the strength based on the joint's length: 
@@ -644,6 +643,10 @@ void SkeletonModel::updateVisibleJointStates() {
 void SkeletonModel::stepRagdollForward(float deltaTime) {
     Ragdoll::stepRagdollForward(deltaTime);
     updateMuscles();
+    int numConstraints = _muscleConstraints.size();
+    for (int i = 0; i < numConstraints; ++i) {
+        _muscleConstraints[i]->enforce();
+    }
 }
 
 float DENSITY_OF_WATER = 1000.0f; // kg/m^3
@@ -743,13 +746,8 @@ void SkeletonModel::updateMuscles() {
     for (int i = 0; i < numConstraints; ++i) {
         MuscleConstraint* constraint = _muscleConstraints[i];
         int j = constraint->getParentIndex();
-        if (j == -1) {
-            continue;
-        }
         int k = constraint->getChildIndex();
-        if (k == -1) {
-            continue;
-        }
+        assert(j != -1 && k != -1);
         constraint->setChildOffset(_jointStates.at(k).getPosition() - _jointStates.at(j).getPosition());
     }
 }
diff --git a/libraries/shared/src/CapsuleShape.cpp b/libraries/shared/src/CapsuleShape.cpp
index 12ab6ba479..03bc48bd94 100644
--- a/libraries/shared/src/CapsuleShape.cpp
+++ b/libraries/shared/src/CapsuleShape.cpp
@@ -73,7 +73,7 @@ void CapsuleShape::setEndPoints(const glm::vec3& startPoint, const glm::vec3& en
     if (height > EPSILON) {
         _halfHeight = 0.5f * height;
         axis /= height;
-        computeNewRotation(axis);
+        _rotation = computeNewRotation(axis);
     }
     updateBoundingRadius();
 }
diff --git a/libraries/shared/src/CollisionInfo.cpp b/libraries/shared/src/CollisionInfo.cpp
index e862a22f4a..9dc321fa44 100644
--- a/libraries/shared/src/CollisionInfo.cpp
+++ b/libraries/shared/src/CollisionInfo.cpp
@@ -26,6 +26,16 @@ CollisionInfo::CollisionInfo() :
         _addedVelocity(0.f) {
 }
 
+quint64 CollisionInfo::getShapePairKey() const {
+    if (_shapeB == NULL || _shapeA == NULL) {
+        // zero is an invalid key
+        return 0;
+    }
+    quint32 idA = _shapeA->getID();
+    quint32 idB = _shapeB->getID();
+    return idA < idB ? ((quint64)idA << 32) + (quint64)idB : ((quint64)idB << 32) + (quint64)idA;
+}
+
 CollisionList::CollisionList(int maxSize) :
     _maxSize(maxSize),
     _size(0) {
diff --git a/libraries/shared/src/CollisionInfo.h b/libraries/shared/src/CollisionInfo.h
index 1ab06e2ef5..6e70654d15 100644
--- a/libraries/shared/src/CollisionInfo.h
+++ b/libraries/shared/src/CollisionInfo.h
@@ -15,6 +15,7 @@
 #include <glm/glm.hpp>
 #include <glm/gtc/quaternion.hpp>
 
+#include <QtGlobal>
 #include <QVector>
 
 class Shape;
@@ -47,6 +48,9 @@ public:
     Shape* getShapeA() const { return const_cast<Shape*>(_shapeA); }
     Shape* getShapeB() const { return const_cast<Shape*>(_shapeB); }
 
+    /// \return unique key for shape pair
+    quint64 getShapePairKey() const;
+
     const Shape* _shapeA;  // pointer to shapeA in this collision
     const Shape* _shapeB;  // pointer to shapeB in this collision
 
diff --git a/libraries/shared/src/Constraint.h b/libraries/shared/src/Constraint.h
index 422675b85d..9bbdc185e1 100644
--- a/libraries/shared/src/Constraint.h
+++ b/libraries/shared/src/Constraint.h
@@ -20,9 +20,6 @@ public:
     /// Enforce contraint by moving relevant points.
     /// \return max distance of point movement
     virtual float enforce() = 0;
-
-protected:
-    int _type;
 };
 
 #endif // hifi_Constraint_h
diff --git a/libraries/shared/src/ContactConstraint.cpp b/libraries/shared/src/ContactConstraint.cpp
new file mode 100644
index 0000000000..d1d12fa771
--- /dev/null
+++ b/libraries/shared/src/ContactConstraint.cpp
@@ -0,0 +1,85 @@
+//
+//  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;
+    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;
+        }
+        // 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 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;
+    }
+}
diff --git a/libraries/shared/src/ContactConstraint.h b/libraries/shared/src/ContactConstraint.h
new file mode 100644
index 0000000000..1c8b7d1b57
--- /dev/null
+++ b/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
diff --git a/libraries/shared/src/PhysicsSimulation.cpp b/libraries/shared/src/PhysicsSimulation.cpp
index ca9d3d303a..3e3529be10 100644
--- a/libraries/shared/src/PhysicsSimulation.cpp
+++ b/libraries/shared/src/PhysicsSimulation.cpp
@@ -14,6 +14,7 @@
 
 #include "PhysicsSimulation.h"
 
+#include "PerfStat.h"
 #include "PhysicsEntity.h"
 #include "Ragdoll.h"
 #include "SharedUtil.h"
@@ -24,8 +25,7 @@ int MAX_ENTITIES_PER_SIMULATION = 64;
 int MAX_COLLISIONS_PER_SIMULATION = 256;
 
 
-PhysicsSimulation::PhysicsSimulation() : _collisionList(MAX_COLLISIONS_PER_SIMULATION), 
-        _numIterations(0), _numCollisions(0), _constraintError(0.0f), _stepTime(0) {
+PhysicsSimulation::PhysicsSimulation() : _frame(0), _collisions(MAX_COLLISIONS_PER_SIMULATION) {
 }
 
 PhysicsSimulation::~PhysicsSimulation() {
@@ -87,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) {
@@ -128,44 +137,51 @@ void PhysicsSimulation::removeRagdoll(Ragdoll* doll) {
 }
 
 void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIterations, quint64 maxUsec) {
+    ++_frame;
     quint64 now = usecTimestampNow();
     quint64 startTime = now;
     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
-        error = 0.0f;
-        for (int i = 0; i < numDolls; ++i) {
-            error = glm::max(error, _dolls[i]->enforceRagdollConstraints());
+        { // enforce constraints
+            PerformanceTimer perfTimer("5-enforce");
+            error = 0.0f;
+            for (int i = 0; i < numDolls; ++i) {
+                error = glm::max(error, _dolls[i]->enforceRagdollConstraints());
+            }
         }
         ++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) {
+    PerformanceTimer perfTimer("1-integrate");
     int numDolls = _dolls.size();
     for (int i = 0; i < numDolls; ++i) {
         _dolls.at(i)->stepRagdollForward(deltaTime);
@@ -173,7 +189,8 @@ void PhysicsSimulation::moveRagdolls(float deltaTime) {
 }
 
 void PhysicsSimulation::computeCollisions() {
-    _collisionList.clear();
+    PerformanceTimer perfTimer("2-collide");
+    _collisions.clear();
     // TODO: keep track of QSet<PhysicsEntity*> collidedEntities;
     int numEntities = _entities.size();
     for (int i = 0; i < numEntities; ++i) {
@@ -189,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);
                 }
             }
         }
@@ -197,18 +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("4-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());
@@ -224,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-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;
+        }
+    }
+}
diff --git a/libraries/shared/src/PhysicsSimulation.h b/libraries/shared/src/PhysicsSimulation.h
index c611e06870..6e69e72219 100644
--- a/libraries/shared/src/PhysicsSimulation.h
+++ b/libraries/shared/src/PhysicsSimulation.h
@@ -12,9 +12,12 @@
 #ifndef hifi_PhysicsSimulation
 #define hifi_PhysicsSimulation
 
+#include <QtGlobal>
+#include <QMap>
 #include <QVector>
 
 #include "CollisionInfo.h"
+#include "ContactConstraint.h"
 
 class PhysicsEntity;
 class Ragdoll;
@@ -41,20 +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;
+    quint32 _frame;
 
-    // some stats
-    int _numIterations;
-    int _numCollisions;
-    float _constraintError;
-    quint64 _stepTime;
+    QVector<Ragdoll*> _dolls;
+    QVector<PhysicsEntity*> _entities;
+    CollisionList _collisions;
+    QMap<quint64, ContactConstraint> _contacts;
 };
 
 #endif // hifi_PhysicsSimulation
diff --git a/libraries/shared/src/Shape.h b/libraries/shared/src/Shape.h
index 09ed30a116..b1efe6d9ce 100644
--- a/libraries/shared/src/Shape.h
+++ b/libraries/shared/src/Shape.h
@@ -14,6 +14,7 @@
 
 #include <glm/glm.hpp>
 #include <glm/gtc/quaternion.hpp>
+#include <QtGlobal>
 
 class PhysicsEntity;
 
@@ -21,6 +22,7 @@ const float MAX_SHAPE_MASS = 1.0e18f; // something less than sqrt(FLT_MAX)
 
 class Shape {
 public:
+    static quint32 getNextID() { static quint32 nextID = 0; return ++nextID; }
 
     enum Type{
         UNKNOWN_SHAPE = 0,
@@ -30,10 +32,14 @@ public:
         LIST_SHAPE
     };
 
-    Shape() : _type(UNKNOWN_SHAPE), _owningEntity(NULL), _boundingRadius(0.f), _translation(0.f), _rotation(), _mass(MAX_SHAPE_MASS) { }
-    virtual ~Shape() {}
+    Shape() : _type(UNKNOWN_SHAPE), _owningEntity(NULL), _boundingRadius(0.f), 
+            _translation(0.f), _rotation(), _mass(MAX_SHAPE_MASS) {
+        _id = getNextID();
+    }
+    virtual ~Shape() { }
 
     int getType() const { return _type; }
+    quint32 getID() const { return _id; }
 
     void setEntity(PhysicsEntity* entity) { _owningEntity = entity; }
     PhysicsEntity* getEntity() const { return _owningEntity; }
@@ -69,17 +75,24 @@ public:
 
 protected:
     // these ctors are protected (used by derived classes only)
-    Shape(Type type) : _type(type), _owningEntity(NULL), _boundingRadius(0.f), _translation(0.f), _rotation() {}
+    Shape(Type type) : _type(type), _owningEntity(NULL), _boundingRadius(0.f), _translation(0.f), _rotation() {
+        _id = getNextID();
+    }
 
     Shape(Type type, const glm::vec3& position) 
-        : _type(type), _owningEntity(NULL), _boundingRadius(0.f), _translation(position), _rotation() {}
+        : _type(type), _owningEntity(NULL), _boundingRadius(0.f), _translation(position), _rotation() {
+        _id = getNextID();
+    }
 
     Shape(Type type, const glm::vec3& position, const glm::quat& rotation) 
-        : _type(type), _owningEntity(NULL), _boundingRadius(0.f), _translation(position), _rotation(rotation) {}
+        : _type(type), _owningEntity(NULL), _boundingRadius(0.f), _translation(position), _rotation(rotation) {
+        _id = getNextID();
+    }
 
     void setBoundingRadius(float radius) { _boundingRadius = radius; }
 
     int _type;
+    unsigned int _id;
     PhysicsEntity* _owningEntity;
     float _boundingRadius;
     glm::vec3 _translation;
diff --git a/libraries/shared/src/VerletCapsuleShape.cpp b/libraries/shared/src/VerletCapsuleShape.cpp
index 3ac4899682..ab956264b5 100644
--- a/libraries/shared/src/VerletCapsuleShape.cpp
+++ b/libraries/shared/src/VerletCapsuleShape.cpp
@@ -90,11 +90,9 @@ float VerletCapsuleShape::computeEffectiveMass(const glm::vec3& penetration, con
         // 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.
+        // as if the contact were at the center of mass.
         _startLagrangeCoef = 1.0f;
         _endLagrangeCoef = 1.0f;
     }
@@ -104,8 +102,8 @@ float VerletCapsuleShape::computeEffectiveMass(const glm::vec3& penetration, con
 
 void VerletCapsuleShape::accumulateDelta(float relativeMassFactor, const glm::vec3& penetration) {
     assert(!glm::isnan(relativeMassFactor));
-    _startPoint->accumulateDelta(relativeMassFactor * _startLagrangeCoef * penetration);
-    _endPoint->accumulateDelta(relativeMassFactor * _endLagrangeCoef * penetration);
+    _startPoint->accumulateDelta((relativeMassFactor * _startLagrangeCoef) * penetration);
+    _endPoint->accumulateDelta((relativeMassFactor * _endLagrangeCoef) * penetration);
 }
 
 void VerletCapsuleShape::applyAccumulatedDelta() {
diff --git a/libraries/shared/src/VerletPoint.cpp b/libraries/shared/src/VerletPoint.cpp
index 641ac39341..654a74d7ac 100644
--- a/libraries/shared/src/VerletPoint.cpp
+++ b/libraries/shared/src/VerletPoint.cpp
@@ -11,9 +11,11 @@
 
 #include "VerletPoint.h"
 
+const float INTEGRATION_FRICTION_FACTOR = 0.6f;
+
 void VerletPoint::integrateForward() {
     glm::vec3 oldPosition = _position;
-    _position += 0.6f * (_position - _lastPosition);
+    _position += INTEGRATION_FRICTION_FACTOR * (_position - _lastPosition);
     _lastPosition = oldPosition;
 }
 
diff --git a/libraries/shared/src/VerletPoint.h b/libraries/shared/src/VerletPoint.h
index 076a624776..f59afb16c5 100644
--- a/libraries/shared/src/VerletPoint.h
+++ b/libraries/shared/src/VerletPoint.h
@@ -23,10 +23,6 @@ public:
     void accumulateDelta(const glm::vec3& delta);
     void applyAccumulatedDelta();
 
-    glm::vec3 getAccumulatedDelta() const { 
-        return (_numDeltas > 0) ?  _accumulatedDelta / (float)_numDeltas : glm::vec3(0.0f);
-    }
-
     glm::vec3 _position;
     glm::vec3 _lastPosition;
     float _mass;