resolve conflicts on merge with upstream master

interface/src/avatar/MyAvatar.cpp

@@ -85,13 +85,13 @@ MyAvatar::MyAvatar() :
     }
     _skeletonModel.setEnableShapes(true);
     // The skeleton is both a PhysicsEntity and Ragdoll, so we add it to the simulation once for each type.
-    _physicsSimulation.addEntity(&_skeletonModel);
+    _physicsSimulation.setEntity(&_skeletonModel);
-    _physicsSimulation.addRagdoll(&_skeletonModel);
+    _physicsSimulation.setRagdoll(&_skeletonModel);
 }
 
 MyAvatar::~MyAvatar() {
-    _physicsSimulation.removeEntity(&_skeletonModel);
+    _physicsSimulation.setRagdoll(NULL);
-    _physicsSimulation.removeRagdoll(&_skeletonModel);
+    _physicsSimulation.setEntity(NULL);
     _lookAtTargetAvatar.clear();
 }
 
@@ -206,9 +206,10 @@ void MyAvatar::simulate(float deltaTime) {
     {
         PerformanceTimer perfTimer("ragdoll");
         if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
-            const int minError = 0.01f;
+            const float minError = 0.01f;
             const float maxIterations = 10;
             const quint64 maxUsec = 2000;
+            _physicsSimulation.setTranslation(_position);
             _physicsSimulation.stepForward(deltaTime, minError, maxIterations, maxUsec);
         } else {
             _skeletonModel.moveShapesTowardJoints(1.0f);
@@ -235,12 +236,10 @@ void MyAvatar::simulate(float deltaTime) {
         } else {
             _trapDuration = 0.0f;
         }
-    /* TODO: Andrew to make this work
         if (_collisionGroups & COLLISION_GROUP_AVATARS) {
             PerformanceTimer perfTimer("avatars");
             updateCollisionWithAvatars(deltaTime);
         }
-    */
     }
 
     // consider updating our billboard
@@ -1551,8 +1550,6 @@ bool findAvatarAvatarPenetration(const glm::vec3 positionA, float radiusA, float
     return false;
 }
 
-const float BODY_COLLISION_RESOLUTION_TIMESCALE = 0.5f; // seconds
-
 void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
     //  Reset detector for nearest avatar
     _distanceToNearestAvatar = std::numeric_limits<float>::max();
@@ -1563,41 +1560,51 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
     }
     float myBoundingRadius = getBoundingRadius();
 
-    const float BODY_COLLISION_RESOLUTION_FACTOR = glm::max(1.0f, deltaTime / BODY_COLLISION_RESOLUTION_TIMESCALE);
+    // find nearest avatar
-
+    float nearestDistance2 = std::numeric_limits<float>::max();
+    Avatar* nearestAvatar = NULL;
     foreach (const AvatarSharedPointer& avatarPointer, avatars) {
         Avatar* avatar = static_cast<Avatar*>(avatarPointer.data());
         if (static_cast<Avatar*>(this) == avatar) {
             // don't collide with ourselves
             continue;
         }
-        float distance = glm::length(_position - avatar->getPosition());        
+        float distance2 = glm::distance2(_position, avatar->getPosition());        
-        if (_distanceToNearestAvatar > distance) {
+        if (nearestDistance2 > distance2) {
-            _distanceToNearestAvatar = distance;
+            nearestDistance2 = distance2;
+            nearestAvatar = avatar;
         }
-        float theirBoundingRadius = avatar->getBoundingRadius();
+    }
-        if (distance < myBoundingRadius + theirBoundingRadius) {
+    _distanceToNearestAvatar = glm::sqrt(nearestDistance2);
-            // collide our body against theirs
-            QVector<const Shape*> myShapes;
-            _skeletonModel.getBodyShapes(myShapes);
-            QVector<const Shape*> theirShapes;
-            avatar->getSkeletonModel().getBodyShapes(theirShapes);
 
-            CollisionInfo collision;
+    if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
-            if (ShapeCollider::collideShapesCoarse(myShapes, theirShapes, collision)) {
+        if (nearestAvatar != NULL) {
-                float penetrationDepth = glm::length(collision._penetration);
+            if (_distanceToNearestAvatar > myBoundingRadius + nearestAvatar->getBoundingRadius()) {
-                if (penetrationDepth > myBoundingRadius) {
+                // they aren't close enough to put into the _physicsSimulation
-                    qDebug() << "WARNING: ignoring avatar-avatar penetration depth " << penetrationDepth;
+                // so we clear the pointer
-                }
+                nearestAvatar = NULL;
-                else if (penetrationDepth > EPSILON) {
+            }
-                    setPosition(getPosition() - BODY_COLLISION_RESOLUTION_FACTOR * collision._penetration);
+        }
-                    _lastBodyPenetration += collision._penetration;
+    
-                    emit collisionWithAvatar(getSessionUUID(), avatar->getSessionUUID(), collision);
+        foreach (const AvatarSharedPointer& avatarPointer, avatars) {
-                }
+            Avatar* avatar = static_cast<Avatar*>(avatarPointer.data());
+            if (static_cast<Avatar*>(this) == avatar) {
+                // don't collide with ourselves
+                continue;
+            }
+            SkeletonModel* skeleton = &(avatar->getSkeletonModel());
+            PhysicsSimulation* simulation = skeleton->getSimulation();
+            if (avatar == nearestAvatar) {
+                if (simulation != &(_physicsSimulation)) {
+                    skeleton->setEnableShapes(true);
+                    _physicsSimulation.addEntity(skeleton);
+                    _physicsSimulation.addRagdoll(skeleton);
+                }
+            } else if (simulation == &(_physicsSimulation)) {
+                _physicsSimulation.removeRagdoll(skeleton);
+                _physicsSimulation.removeEntity(skeleton);
+                skeleton->setEnableShapes(false);
             }
-
-            // collide their hands against us
-            avatar->getHand()->collideAgainstAvatar(this, false);
         }
     }
 }

interface/src/avatar/SkeletonModel.cpp

@@ -490,11 +490,13 @@ void SkeletonModel::renderRagdoll() {
     float alpha = 0.3f;
     float radius1 = 0.008f;
     float radius2 = 0.01f;
+    glm::vec3 simulationTranslation = getTranslationInSimulationFrame();
     for (int i = 0; i < numPoints; ++i) {
         glPushMatrix();
-        // draw each point as a yellow hexagon with black border
+        // NOTE: ragdollPoints are in simulation-frame but we want them to be model-relative
-        glm::vec3 position = _rotation * _ragdollPoints[i]._position;
+        glm::vec3 position = _ragdollPoints[i]._position - simulationTranslation;
         glTranslatef(position.x, position.y, position.z);
+        // draw each point as a yellow hexagon with black border
         glColor4f(0.0f, 0.0f, 0.0f, alpha);
         glutSolidSphere(radius2, BALL_SUBDIVISIONS, BALL_SUBDIVISIONS);
         glColor4f(1.0f, 1.0f, 0.0f, alpha);
@@ -511,13 +513,14 @@ void SkeletonModel::initRagdollPoints() {
     clearRagdollConstraintsAndPoints();
     _muscleConstraints.clear();
 
+    initRagdollTransform();
     // one point for each joint
-    int numJoints = _jointStates.size();
+    int numStates = _jointStates.size();
-    _ragdollPoints.fill(VerletPoint(), numJoints);
+    _ragdollPoints.fill(VerletPoint(), numStates);
-    for (int i = 0; i < numJoints; ++i) {
+    for (int i = 0; i < numStates; ++i) {
         const JointState& state = _jointStates.at(i);
-        glm::vec3 position = state.getPosition();
+        // _ragdollPoints start in model-frame
-        _ragdollPoints[i].initPosition(position);
+        _ragdollPoints[i].initPosition(state.getPosition());
     }
 }
 
@@ -534,12 +537,12 @@ void SkeletonModel::buildRagdollConstraints() {
         const JointState& state = _jointStates.at(i);
         int parentIndex = state.getParentIndex();
         if (parentIndex == -1) {
-            FixedConstraint* anchor = new FixedConstraint(&(_ragdollPoints[i]), glm::vec3(0.0f));
+            FixedConstraint* anchor = new FixedConstraint(&_translationInSimulationFrame, &(_ragdollPoints[i]));
-            _ragdollConstraints.push_back(anchor);
+            _fixedConstraints.push_back(anchor);
         } else { 
             DistanceConstraint* bone = new DistanceConstraint(&(_ragdollPoints[i]), &(_ragdollPoints[parentIndex]));
             bone->setDistance(state.getDistanceToParent());
-            _ragdollConstraints.push_back(bone);
+            _boneConstraints.push_back(bone);
             families.insert(parentIndex, i);
         }
         float boneLength = glm::length(state.getPositionInParentFrame());
@@ -558,11 +561,11 @@ void SkeletonModel::buildRagdollConstraints() {
         if (numChildren > 1) {
             for (int i = 1; i < numChildren; ++i) {
                 DistanceConstraint* bone = new DistanceConstraint(&(_ragdollPoints[children[i-1]]), &(_ragdollPoints[children[i]]));
-                _ragdollConstraints.push_back(bone);
+                _boneConstraints.push_back(bone);
             }
             if (numChildren > 2) {
                 DistanceConstraint* bone = new DistanceConstraint(&(_ragdollPoints[children[numChildren-1]]), &(_ragdollPoints[children[0]]));
-                _ragdollConstraints.push_back(bone);
+                _boneConstraints.push_back(bone);
             }
         }
         ++itr;
@@ -604,6 +607,7 @@ void SkeletonModel::updateVisibleJointStates() {
     }
     QVector<glm::vec3> points;
     points.reserve(_jointStates.size());
+    glm::quat invRotation = glm::inverse(_rotation);
     for (int i = 0; i < _jointStates.size(); i++) {
         JointState& state = _jointStates[i];
         points.push_back(_ragdollPoints[i]._position);
@@ -628,8 +632,9 @@ void SkeletonModel::updateVisibleJointStates() {
 
         // we're looking for the rotation that moves visible bone parallel to ragdoll bone
         // rotationBetween(jointTip - jointPivot, shapeTip - shapePivot)
+        // NOTE: points are in simulation-frame so rotate line segment into model-frame
         glm::quat delta = rotationBetween(state.getVisiblePosition() - extractTranslation(parentTransform), 
-                points[i] - points[parentIndex]);
+                invRotation * (points[i] - points[parentIndex]));
 
         // apply
         parentState.mixVisibleRotationDelta(delta, 0.01f);
@@ -641,6 +646,7 @@ void SkeletonModel::updateVisibleJointStates() {
 
 // virtual 
 void SkeletonModel::stepRagdollForward(float deltaTime) {
+    setRagdollTransform(_translation, _rotation);
     Ragdoll::stepRagdollForward(deltaTime);
     updateMuscles();
     int numConstraints = _muscleConstraints.size();
@@ -665,6 +671,7 @@ void SkeletonModel::buildShapes() {
     }
 
     initRagdollPoints();
+    float massScale = getMassScale();
 
     float uniformScale = extractUniformScale(_scale);
     const int numStates = _jointStates.size();
@@ -678,25 +685,30 @@ void SkeletonModel::buildShapes() {
             // this shape is forced to be a sphere
             type = Shape::SPHERE_SHAPE;
         }
+        if (radius < EPSILON) {
+            type = Shape::UNKNOWN_SHAPE;
+        }
         Shape* shape = NULL;
         int parentIndex = joint.parentIndex;
         if (type == Shape::SPHERE_SHAPE) {
             shape = new VerletSphereShape(radius, &(_ragdollPoints[i]));
             shape->setEntity(this);
-            _ragdollPoints[i]._mass = glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume());
+            _ragdollPoints[i].setMass(massScale * glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume()));
         } else if (type == Shape::CAPSULE_SHAPE) {
             assert(parentIndex != -1);
             shape = new VerletCapsuleShape(radius, &(_ragdollPoints[parentIndex]), &(_ragdollPoints[i]));
             shape->setEntity(this);
-            _ragdollPoints[i]._mass = glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume());
+            _ragdollPoints[i].setMass(massScale * glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume()));
         } 
         if (parentIndex != -1) {
             // always disable collisions between joint and its parent
-            disableCollisions(i, parentIndex);
+            if (shape) {
+                disableCollisions(i, parentIndex);
+            }
         } else {
             // give the base joint a very large mass since it doesn't actually move
             // in the local-frame simulation (it defines the origin)
-            _ragdollPoints[i]._mass = VERY_BIG_MASS;
+            _ragdollPoints[i].setMass(VERY_BIG_MASS);
         }
         _shapes.push_back(shape);
     }
@@ -712,8 +724,9 @@ void SkeletonModel::buildShapes() {
 
     // ... then move shapes back to current joint positions
     if (_ragdollPoints.size() == numStates) {
-        int numJoints = _jointStates.size();
+        int numStates = _jointStates.size();
-        for (int i = 0; i < numJoints; ++i) {
+        for (int i = 0; i < numStates; ++i) {
+            // ragdollPoints start in model-frame
             _ragdollPoints[i].initPosition(_jointStates.at(i).getPosition());
         }
     }
@@ -735,9 +748,11 @@ void SkeletonModel::moveShapesTowardJoints(float deltaTime) {
     float fraction = glm::clamp(deltaTime / RAGDOLL_FOLLOWS_JOINTS_TIMESCALE, 0.0f, 1.0f);
 
     float oneMinusFraction = 1.0f - fraction; 
+    glm::vec3 simulationTranslation = getTranslationInSimulationFrame();
     for (int i = 0; i < numStates; ++i) {
+        // ragdollPoints are in simulation-frame but jointStates are in model-frame
         _ragdollPoints[i].initPosition(oneMinusFraction * _ragdollPoints[i]._position + 
-                fraction * _jointStates.at(i).getPosition());
+                fraction * (simulationTranslation + _rotation * (_jointStates.at(i).getPosition())));
     }
 }
 
@@ -748,23 +763,22 @@ void SkeletonModel::updateMuscles() {
         int j = constraint->getParentIndex();
         int k = constraint->getChildIndex();
         assert(j != -1 && k != -1);
-        constraint->setChildOffset(_jointStates.at(k).getPosition() - _jointStates.at(j).getPosition());
+        // ragdollPoints are in simulation-frame but jointStates are in model-frame
+        constraint->setChildOffset(_rotation * (_jointStates.at(k).getPosition() - _jointStates.at(j).getPosition()));
     }
 }
 
 void SkeletonModel::computeBoundingShape(const FBXGeometry& geometry) {
     // compute default joint transforms
-    int numJoints = geometry.joints.size();
+    int numStates = _jointStates.size();
-    if (numJoints != _ragdollPoints.size()) {
-        return;
-    }
     QVector<glm::mat4> transforms;
-    transforms.fill(glm::mat4(), numJoints);
+    transforms.fill(glm::mat4(), numStates);
 
     // compute the default transforms and slam the ragdoll positions accordingly
     // (which puts the shapes where we want them)
-    for (int i = 0; i < numJoints; i++) {
+    for (int i = 0; i < numStates; i++) {
-        const FBXJoint& joint = geometry.joints.at(i);
+        JointState& state = _jointStates[i];
+        const FBXJoint& joint = state.getFBXJoint();
         int parentIndex = joint.parentIndex;
         if (parentIndex == -1) {
             transforms[i] = _jointStates[i].getTransform();
@@ -836,7 +850,7 @@ void SkeletonModel::resetShapePositionsToDefaultPose() {
     }
     
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
-    if (geometry.joints.isEmpty() || _shapes.size() != geometry.joints.size()) {
+    if (geometry.joints.isEmpty()) {
         return;
     }
 
@@ -891,3 +905,54 @@ void SkeletonModel::renderBoundingCollisionShapes(float alpha) {
     glPopMatrix();
 }
 
+const int BALL_SUBDIVISIONS = 10;
+
+// virtual
+void SkeletonModel::renderJointCollisionShapes(float alpha) { 
+    glPushMatrix();
+    Application::getInstance()->loadTranslatedViewMatrix(_translation);
+    glm::vec3 simulationTranslation = getTranslationInSimulationFrame();
+    for (int i = 0; i < _shapes.size(); i++) { 
+        Shape* shape = _shapes[i];
+        if (!shape) { 
+            continue;
+        } 
+
+        glPushMatrix();
+        // shapes are stored in simulation-frame but we want position to be model-relative
+        if (shape->getType() == Shape::SPHERE_SHAPE) { 
+            glm::vec3 position = shape->getTranslation() - simulationTranslation;
+            glTranslatef(position.x, position.y, position.z);
+            // draw a grey sphere at shape position
+            glColor4f(0.75f, 0.75f, 0.75f, alpha);
+            glutSolidSphere(shape->getBoundingRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS);
+        } else if (shape->getType() == Shape::CAPSULE_SHAPE) {
+            CapsuleShape* capsule = static_cast<CapsuleShape*>(shape);
+
+            // draw a blue sphere at the capsule endpoint                                         
+            glm::vec3 endPoint;
+            capsule->getEndPoint(endPoint);
+            endPoint = endPoint - simulationTranslation;
+            glTranslatef(endPoint.x, endPoint.y, endPoint.z);                                     
+            glColor4f(0.6f, 0.6f, 0.8f, alpha);
+            glutSolidSphere(capsule->getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS);          
+
+            // draw a yellow sphere at the capsule startpoint
+            glm::vec3 startPoint;
+            capsule->getStartPoint(startPoint);
+            startPoint = startPoint - simulationTranslation;
+            glm::vec3 axis = endPoint - startPoint;
+            glTranslatef(-axis.x, -axis.y, -axis.z);
+            glColor4f(0.8f, 0.8f, 0.6f, alpha);
+            glutSolidSphere(capsule->getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS);
+
+            // draw a green cylinder between the two points
+            glm::vec3 origin(0.0f);
+            glColor4f(0.6f, 0.8f, 0.6f, alpha);
+            Avatar::renderJointConnectingCone( origin, axis, capsule->getRadius(), capsule->getRadius());
+        }
+        glPopMatrix();
+    }
+    glPopMatrix();
+}
+

interface/src/avatar/SkeletonModel.h

@@ -105,6 +105,7 @@ public:
 
     void computeBoundingShape(const FBXGeometry& geometry);
     void renderBoundingCollisionShapes(float alpha);
+    void renderJointCollisionShapes(float alpha);
     float getBoundingShapeRadius() const { return _boundingShape.getRadius(); }
     const CapsuleShape& getBoundingShape() const { return _boundingShape; }
 

interface/src/renderer/Model.cpp

@@ -188,10 +188,12 @@ void Model::initSkinProgram(ProgramObject& program, Model::SkinLocations& locati
 
 QVector<JointState> Model::createJointStates(const FBXGeometry& geometry) {
     QVector<JointState> jointStates;
-    foreach (const FBXJoint& joint, geometry.joints) {
+    for (int i = 0; i < geometry.joints.size(); ++i) {
-        // NOTE: the state keeps a pointer to an FBXJoint
+        const FBXJoint& joint = geometry.joints[i];
+        // store a pointer to the FBXJoint in the JointState
         JointState state;
         state.setFBXJoint(&joint);
+
         jointStates.append(state);
     }
     return jointStates;
@@ -199,8 +201,8 @@ QVector<JointState> Model::createJointStates(const FBXGeometry& geometry) {
 
 void Model::initJointTransforms() {
     // compute model transforms
-    int numJoints = _jointStates.size();
+    int numStates = _jointStates.size();
-    for (int i = 0; i < numJoints; ++i) {
+    for (int i = 0; i < numStates; ++i) {
         JointState& state = _jointStates[i];
         const FBXJoint& joint = state.getFBXJoint();
         int parentIndex = joint.parentIndex;
@@ -538,9 +540,9 @@ void Model::setJointStates(QVector<JointState> states) {
     _jointStates = states;
     initJointTransforms();
 
-    int numJoints = _jointStates.size();
+    int numStates = _jointStates.size();
     float radius = 0.0f;
-    for (int i = 0; i < numJoints; ++i) {
+    for (int i = 0; i < numStates; ++i) {
         float distance = glm::length(_jointStates[i].getPosition());
         if (distance > radius) {
             radius = distance;
@@ -1243,55 +1245,7 @@ float Model::getLimbLength(int jointIndex) const {
 const int BALL_SUBDIVISIONS = 10;
 
 void Model::renderJointCollisionShapes(float alpha) {
-    glPushMatrix();
+    // implement this when we have shapes for regular models
-    Application::getInstance()->loadTranslatedViewMatrix(_translation);
-    for (int i = 0; i < _shapes.size(); i++) {
-        Shape* shape = _shapes[i];
-        if (!shape) {
-            continue;
-        }
-
-        glPushMatrix();
-        // NOTE: the shapes are in the avatar local-frame
-        if (shape->getType() == Shape::SPHERE_SHAPE) {
-            // shapes are stored in world-frame, so we have to transform into model frame
-            glm::vec3 position = _rotation * shape->getTranslation();
-            glTranslatef(position.x, position.y, position.z);
-            const glm::quat& rotation = shape->getRotation();
-            glm::vec3 axis = glm::axis(rotation);
-            glRotatef(glm::degrees(glm::angle(rotation)), axis.x, axis.y, axis.z);
-
-            // draw a grey sphere at shape position
-            glColor4f(0.75f, 0.75f, 0.75f, alpha);
-            glutSolidSphere(shape->getBoundingRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS);
-        } else if (shape->getType() == Shape::CAPSULE_SHAPE) {
-            CapsuleShape* capsule = static_cast<CapsuleShape*>(shape);
-
-            // draw a blue sphere at the capsule endpoint
-            glm::vec3 endPoint;
-            capsule->getEndPoint(endPoint);
-            endPoint = _rotation * endPoint;
-            glTranslatef(endPoint.x, endPoint.y, endPoint.z);
-            glColor4f(0.6f, 0.6f, 0.8f, alpha);
-            glutSolidSphere(capsule->getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS);
-
-            // draw a yellow sphere at the capsule startpoint
-            glm::vec3 startPoint;
-            capsule->getStartPoint(startPoint);
-            startPoint = _rotation * startPoint;
-            glm::vec3 axis = endPoint - startPoint;
-            glTranslatef(-axis.x, -axis.y, -axis.z);
-            glColor4f(0.8f, 0.8f, 0.6f, alpha);
-            glutSolidSphere(capsule->getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS);
-            
-            // draw a green cylinder between the two points
-            glm::vec3 origin(0.0f);
-            glColor4f(0.6f, 0.8f, 0.6f, alpha);
-            Avatar::renderJointConnectingCone( origin, axis, capsule->getRadius(), capsule->getRadius());
-        }
-        glPopMatrix();
-    }
-    glPopMatrix();
 }
 
 void Model::setBlendedVertices(const QVector<glm::vec3>& vertices, const QVector<glm::vec3>& normals) {

interface/src/renderer/Model.h

@@ -149,7 +149,7 @@ public:
     virtual void buildShapes();
     virtual void updateShapePositions();
 
-    void renderJointCollisionShapes(float alpha);
+    virtual void renderJointCollisionShapes(float alpha);
     
     /// Sets blended vertices computed in a separate thread.
     void setBlendedVertices(const QVector<glm::vec3>& vertices, const QVector<glm::vec3>& normals);

libraries/shared/src/FixedConstraint.cpp

@@ -10,26 +10,26 @@
 //
 
 #include "FixedConstraint.h"
-#include "Shape.h"  // for MAX_SHAPE_MASS
 #include "VerletPoint.h"
 
-FixedConstraint::FixedConstraint(VerletPoint* point, const glm::vec3& anchor) : _point(point), _anchor(anchor) {
+FixedConstraint::FixedConstraint(glm::vec3* anchor, VerletPoint* point) : _anchor(anchor), _point(point) {
+    assert(anchor);
+    assert(point);
 }
 
 float FixedConstraint::enforce() {
     assert(_point != NULL);
-    // TODO: use fast approximate sqrt here
+    _point->_position = *_anchor;
-    float distance = glm::distance(_anchor, _point->_position);
+    _point->_lastPosition = *_anchor;
-    _point->_position = _anchor;
+    return 0.0f;
-    return distance;
+}
+
+void FixedConstraint::setAnchor(glm::vec3* anchor) { 
+    assert(anchor);
+    _anchor = anchor; 
 }
 
 void FixedConstraint::setPoint(VerletPoint* point) {
     assert(point);
     _point = point;
-    _point->_mass = MAX_SHAPE_MASS;
-}
-
-void FixedConstraint::setAnchor(const glm::vec3& anchor) {
-    _anchor = anchor;
 }

libraries/shared/src/FixedConstraint.h

@@ -18,15 +18,19 @@
 
 class VerletPoint;
 
+// FixedConstraint takes pointers to a glm::vec3 and a VerletPoint.  
+// The enforcement will copy the value of the vec3 into the VerletPoint.
+
 class FixedConstraint : public Constraint {
 public:
-    FixedConstraint(VerletPoint* point, const glm::vec3& anchor);
+    FixedConstraint(glm::vec3* anchor, VerletPoint* point);
+    ~FixedConstraint() {}
     float enforce();
+    void setAnchor(glm::vec3* anchor);
     void setPoint(VerletPoint* point);
-    void setAnchor(const glm::vec3& anchor);
 private:
+    glm::vec3* _anchor;
     VerletPoint* _point;
-    glm::vec3 _anchor;
 };
 
 #endif // hifi_FixedConstraint_h

libraries/shared/src/PhysicsSimulation.cpp

@@ -24,19 +24,51 @@ int MAX_ENTITIES_PER_SIMULATION = 64;
 int MAX_COLLISIONS_PER_SIMULATION = 256;
 
 
-PhysicsSimulation::PhysicsSimulation() : _frame(0), _collisions(MAX_COLLISIONS_PER_SIMULATION) {
+PhysicsSimulation::PhysicsSimulation() : _translation(0.0f), _frameCount(0), _entity(NULL), _ragdoll(NULL), 
+        _collisions(MAX_COLLISIONS_PER_SIMULATION) {
 }
 
 PhysicsSimulation::~PhysicsSimulation() {
     // entities have a backpointer to this simulator that must be cleaned up
-    int numEntities = _entities.size();
+    int numEntities = _otherEntities.size();
     for (int i = 0; i < numEntities; ++i) {
-        _entities[i]->_simulation = NULL;
+        _otherEntities[i]->_simulation = NULL;
+    }
+    _otherEntities.clear();
+    if (_entity) {
+        _entity->_simulation = NULL;
     }
-    _entities.clear();
 
     // but Ragdolls do not
-    _dolls.clear();
+    _ragdoll = NULL;
+    _otherRagdolls.clear();
+}
+
+void PhysicsSimulation::setRagdoll(Ragdoll* ragdoll) { 
+    if (_ragdoll != ragdoll) {
+        if (_ragdoll) {
+            _ragdoll->_ragdollSimulation = NULL;
+        }
+        _ragdoll = ragdoll;
+        if (_ragdoll) {
+            assert(!(_ragdoll->_ragdollSimulation));
+            _ragdoll->_ragdollSimulation = this;
+        }
+    }
+}
+
+void PhysicsSimulation::setEntity(PhysicsEntity* entity) {
+    if (_entity != entity) {
+        if (_entity) {
+            assert(_entity->_simulation == this);
+            _entity->_simulation = NULL;
+        }
+        _entity = entity;
+        if (_entity) {
+            assert(!(_entity->_simulation));
+            _entity->_simulation = this;
+        }
+    }
 }
 
 bool PhysicsSimulation::addEntity(PhysicsEntity* entity) {
@@ -44,25 +76,25 @@ bool PhysicsSimulation::addEntity(PhysicsEntity* entity) {
         return false;
     }
     if (entity->_simulation == this) {
-        int numEntities = _entities.size();
+        int numEntities = _otherEntities.size();
         for (int i = 0; i < numEntities; ++i) {
-            if (entity == _entities.at(i)) {
+            if (entity == _otherEntities.at(i)) {
                 // already in list
-                assert(entity->_simulation == this);
                 return true;
             }
         }
         // belongs to some other simulation
         return false;
     }
-    int numEntities = _entities.size();
+    int numEntities = _otherEntities.size();
     if (numEntities > MAX_ENTITIES_PER_SIMULATION) {
         // list is full
         return false;
     }
     // add to list
+    assert(!(entity->_simulation));
     entity->_simulation = this;
-    _entities.push_back(entity);
+    _otherEntities.push_back(entity);
     return true;
 }
 
@@ -71,17 +103,17 @@ void PhysicsSimulation::removeEntity(PhysicsEntity* entity) {
         return;
     }
     removeShapes(entity);
-    int numEntities = _entities.size();
+    int numEntities = _otherEntities.size();
     for (int i = 0; i < numEntities; ++i) {
-        if (entity == _entities.at(i)) {
+        if (entity == _otherEntities.at(i)) {
             if (i == numEntities - 1) {
                 // remove it
-                _entities.pop_back();
+                _otherEntities.pop_back();
             } else {
                 // swap the last for this one
-                PhysicsEntity* lastEntity = _entities[numEntities - 1];
+                PhysicsEntity* lastEntity = _otherEntities[numEntities - 1];
-                _entities.pop_back();
+                _otherEntities.pop_back();
-                _entities[i] = lastEntity;
+                _otherEntities[i] = lastEntity;
             }
             entity->_simulation = NULL;
             break;
@@ -101,57 +133,75 @@ void PhysicsSimulation::removeShapes(const PhysicsEntity* entity) {
     }
 }
 
+const float OTHER_RAGDOLL_MASS_SCALE = 10.0f;
+
 bool PhysicsSimulation::addRagdoll(Ragdoll* doll) {
     if (!doll) {
         return false;
     }
-    int numDolls = _dolls.size();
+    int numDolls = _otherRagdolls.size();
     if (numDolls > MAX_DOLLS_PER_SIMULATION) {
         // list is full
         return false;
     }
-    for (int i = 0; i < numDolls; ++i) {
+    if (doll->_ragdollSimulation == this) {
-        if (doll == _dolls[i]) {
+        for (int i = 0; i < numDolls; ++i) {
-            // already in list
+            if (doll == _otherRagdolls[i]) {
-            return true;
+                // already in list
+                return true;
+            }
         }
     }
     // add to list
-    _dolls.push_back(doll);
+    assert(!(doll->_ragdollSimulation));
+    doll->_ragdollSimulation = this;
+    _otherRagdolls.push_back(doll);
+
+    // set the massScale of otherRagdolls artificially high
+    doll->setMassScale(OTHER_RAGDOLL_MASS_SCALE);
     return true;
 }
 
 void PhysicsSimulation::removeRagdoll(Ragdoll* doll) {
-    int numDolls = _dolls.size();
+    int numDolls = _otherRagdolls.size();
+    if (doll->_ragdollSimulation != this) {
+        return;
+    }
     for (int i = 0; i < numDolls; ++i) {
-        if (doll == _dolls[i]) {
+        if (doll == _otherRagdolls[i]) {
             if (i == numDolls - 1) {
                 // remove it
-                _dolls.pop_back();
+                _otherRagdolls.pop_back();
             } else {
                 // swap the last for this one
-                Ragdoll* lastDoll = _dolls[numDolls - 1];
+                Ragdoll* lastDoll = _otherRagdolls[numDolls - 1];
-                _dolls.pop_back();
+                _otherRagdolls.pop_back();
-                _dolls[i] = lastDoll;
+                _otherRagdolls[i] = lastDoll;
             }
+            doll->_ragdollSimulation = NULL;
+            doll->setMassScale(1.0f);
             break;
         }
     }
 }
 
 void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIterations, quint64 maxUsec) {
-    ++_frame;
+    ++_frameCount;
+    if (!_ragdoll) {
+        return;
+    }
     quint64 now = usecTimestampNow();
     quint64 startTime = now;
     quint64 expiry = startTime + maxUsec;
 
     moveRagdolls(deltaTime);
     buildContactConstraints();
-    int numDolls = _dolls.size();
+    int numDolls = _otherRagdolls.size();
     {
         PerformanceTimer perfTimer("enforce");
+        _ragdoll->enforceRagdollConstraints();
         for (int i = 0; i < numDolls; ++i) {
-            _dolls[i]->enforceRagdollConstraints();
+            _otherRagdolls[i]->enforceRagdollConstraints();
         }
     }
 
@@ -164,9 +214,9 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter
 
         { // enforce constraints
             PerformanceTimer perfTimer("enforce");
-            error = 0.0f;
+            error = _ragdoll->enforceRagdollConstraints();
             for (int i = 0; i < numDolls; ++i) {
-                error = glm::max(error, _dolls[i]->enforceRagdollConstraints());
+                error = glm::max(error, _otherRagdolls[i]->enforceRagdollConstraints());
             }
         }
         enforceContactConstraints();
@@ -180,40 +230,38 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter
 
 void PhysicsSimulation::moveRagdolls(float deltaTime) {
     PerformanceTimer perfTimer("integrate");
-    int numDolls = _dolls.size();
+    _ragdoll->stepRagdollForward(deltaTime);
+    int numDolls = _otherRagdolls.size();
     for (int i = 0; i < numDolls; ++i) {
-        _dolls.at(i)->stepRagdollForward(deltaTime);
+        _otherRagdolls[i]->stepRagdollForward(deltaTime);
     }
 }
 
 void PhysicsSimulation::computeCollisions() {
     PerformanceTimer perfTimer("collide");
     _collisions.clear();
-    // TODO: keep track of QSet<PhysicsEntity*> collidedEntities;
-    int numEntities = _entities.size();
-    for (int i = 0; i < numEntities; ++i) {
-        PhysicsEntity* entity = _entities.at(i);
-        const QVector<Shape*> shapes = entity->getShapes();
-        int numShapes = shapes.size();
-        // collide with self
-        for (int j = 0; j < numShapes; ++j) {
-            const Shape* shape = shapes.at(j);
-            if (!shape) {
-                continue;
-            }
-            for (int k = j+1; k < numShapes; ++k) {
-                const Shape* otherShape = shapes.at(k);
-                if (otherShape && entity->collisionsAreEnabled(j, k)) {
-                    ShapeCollider::collideShapes(shape, otherShape, _collisions);
-                }
-            }
-        }
 
-        // collide with others
+    const QVector<Shape*> shapes = _entity->getShapes();
-        for (int j = i+1; j < numEntities; ++j) {
+    int numShapes = shapes.size();
-            const QVector<Shape*> otherShapes = _entities.at(j)->getShapes();
+    // collide main ragdoll with self
-            ShapeCollider::collideShapesWithShapes(shapes, otherShapes, _collisions);
+    for (int i = 0; i < numShapes; ++i) {
+        const Shape* shape = shapes.at(i);
+        if (!shape) {
+            continue;
         }
+        for (int j = i+1; j < numShapes; ++j) {
+            const Shape* otherShape = shapes.at(j);
+            if (otherShape && _entity->collisionsAreEnabled(i, j)) {
+                ShapeCollider::collideShapes(shape, otherShape, _collisions);
+            }
+        }
+    }
+
+    // collide main ragdoll with others
+    int numEntities = _otherEntities.size();
+    for (int i = 0; i < numEntities; ++i) {
+        const QVector<Shape*> otherShapes = _otherEntities.at(i)->getShapes();
+        ShapeCollider::collideShapesWithShapes(shapes, otherShapes, _collisions);
     }
 }
 
@@ -269,9 +317,9 @@ void PhysicsSimulation::updateContacts() {
         }
         QMap<quint64, ContactPoint>::iterator itr = _contacts.find(key);
         if (itr == _contacts.end()) {
-            _contacts.insert(key, ContactPoint(*collision, _frame));
+            _contacts.insert(key, ContactPoint(*collision, _frameCount));
         } else {
-            itr.value().updateContact(*collision, _frame);
+            itr.value().updateContact(*collision, _frameCount);
         }
     }
 }
@@ -281,7 +329,7 @@ const quint32 MAX_CONTACT_FRAME_LIFETIME = 2;
 void PhysicsSimulation::pruneContacts() {
     QMap<quint64, ContactPoint>::iterator itr = _contacts.begin();
     while (itr != _contacts.end()) {
-        if (_frame - itr.value().getLastFrame() > MAX_CONTACT_FRAME_LIFETIME) {
+        if (_frameCount - itr.value().getLastFrame() > MAX_CONTACT_FRAME_LIFETIME) {
             itr = _contacts.erase(itr);
         } else {
             ++itr;

libraries/shared/src/PhysicsSimulation.h

@@ -28,6 +28,12 @@ public:
     PhysicsSimulation();
     ~PhysicsSimulation();
 
+    void setTranslation(const glm::vec3& translation) { _translation = translation; }
+    const glm::vec3& getTranslation() const { return _translation; }
+
+    void setRagdoll(Ragdoll* ragdoll);
+    void setEntity(PhysicsEntity* entity);
+
     /// \return true if entity was added to or is already in the list
     bool addEntity(PhysicsEntity* entity);
 
@@ -56,10 +62,15 @@ protected:
     void pruneContacts();
 
 private:
-    quint32 _frame;
+    glm::vec3 _translation; // origin of simulation in world-frame
 
-    QVector<Ragdoll*> _dolls;
+    quint32 _frameCount;
-    QVector<PhysicsEntity*> _entities;
+
+    PhysicsEntity* _entity;
+    Ragdoll* _ragdoll;
+
+    QVector<Ragdoll*> _otherRagdolls;
+    QVector<PhysicsEntity*> _otherEntities;
     CollisionList _collisions;
     QMap<quint64, ContactPoint> _contacts;
 };

libraries/shared/src/Ragdoll.cpp

@@ -9,13 +9,17 @@
 //  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
 //
 
+#include <glm/gtx/norm.hpp>
+
 #include "Ragdoll.h"
 
 #include "Constraint.h"
 #include "DistanceConstraint.h"
 #include "FixedConstraint.h"
+#include "PhysicsSimulation.h"
+#include "SharedUtil.h" // for EPSILON
 
-Ragdoll::Ragdoll() {
+Ragdoll::Ragdoll() : _massScale(1.0f), _ragdollTranslation(0.0f), _translationInSimulationFrame(0.0f), _ragdollSimulation(NULL) {
 }
 
 Ragdoll::~Ragdoll() {
@@ -23,6 +27,9 @@ Ragdoll::~Ragdoll() {
 }
 
 void Ragdoll::stepRagdollForward(float deltaTime) {
+    if (_ragdollSimulation) {
+        updateSimulationTransforms(_ragdollTranslation - _ragdollSimulation->getTranslation(), _ragdollRotation);
+    }
     int numPoints = _ragdollPoints.size();
     for (int i = 0; i < numPoints; ++i) {
         _ragdollPoints[i].integrateForward();
@@ -30,21 +37,79 @@ void Ragdoll::stepRagdollForward(float deltaTime) {
 }
     
 void Ragdoll::clearRagdollConstraintsAndPoints() {
-    int numConstraints = _ragdollConstraints.size();
+    int numConstraints = _boneConstraints.size();
     for (int i = 0; i < numConstraints; ++i) {
-        delete _ragdollConstraints[i];
+        delete _boneConstraints[i];
     }
-    _ragdollConstraints.clear();
+    _boneConstraints.clear();
+    numConstraints = _fixedConstraints.size();
+    for (int i = 0; i < numConstraints; ++i) {
+        delete _fixedConstraints[i];
+    }
+    _fixedConstraints.clear();
     _ragdollPoints.clear();
 }
 
 float Ragdoll::enforceRagdollConstraints() {
     float maxDistance = 0.0f;
-    const int numConstraints = _ragdollConstraints.size();
+    // enforce the bone constraints first
+    int numConstraints = _boneConstraints.size();
     for (int i = 0; i < numConstraints; ++i) {
-        DistanceConstraint* c = static_cast<DistanceConstraint*>(_ragdollConstraints[i]);
+        maxDistance = glm::max(maxDistance, _boneConstraints[i]->enforce());
-        //maxDistance = glm::max(maxDistance, _ragdollConstraints[i]->enforce());
+    }
-        maxDistance = glm::max(maxDistance, c->enforce());
+    // enforce FixedConstraints second
+    numConstraints = _fixedConstraints.size();
+    for (int i = 0; i < _fixedConstraints.size(); ++i) {
+        maxDistance = glm::max(maxDistance, _fixedConstraints[i]->enforce());
     }
     return maxDistance;
 }
+
+void Ragdoll::initRagdollTransform() {
+    _ragdollTranslation = glm::vec3(0.0f);
+    _ragdollRotation = glm::quat();
+    _translationInSimulationFrame = glm::vec3(0.0f);
+    _rotationInSimulationFrame = glm::quat();
+}
+
+void Ragdoll::setRagdollTransform(const glm::vec3& translation, const glm::quat& rotation) {
+    _ragdollTranslation = translation;
+    _ragdollRotation = rotation;
+}
+
+void Ragdoll::updateSimulationTransforms(const glm::vec3& translation, const glm::quat& rotation) {
+    const float EPSILON2 = EPSILON * EPSILON;
+    if (glm::distance2(translation, _translationInSimulationFrame) < EPSILON2 && 
+            glm::abs(1.0f - glm::abs(glm::dot(rotation, _rotationInSimulationFrame))) < EPSILON2) {
+        // nothing to do
+        return;
+    }
+
+    // compute linear and angular deltas
+    glm::vec3 deltaPosition = translation - _translationInSimulationFrame;
+    glm::quat deltaRotation = rotation * glm::inverse(_rotationInSimulationFrame);
+
+    // apply the deltas to all ragdollPoints
+    int numPoints = _ragdollPoints.size();
+    for (int i = 0; i < numPoints; ++i) {
+        _ragdollPoints[i].move(deltaPosition, deltaRotation, _translationInSimulationFrame);
+    }
+
+    // remember the current transform
+    _translationInSimulationFrame = translation;
+    _rotationInSimulationFrame = rotation;
+}
+
+void Ragdoll::setMassScale(float scale) {
+    const float MIN_SCALE = 1.0e-2f;
+    const float MAX_SCALE = 1.0e6f;
+    scale = glm::clamp(glm::abs(scale), MIN_SCALE, MAX_SCALE);
+    if (scale != _massScale) {
+        float rescale = scale / _massScale;
+        int numPoints = _ragdollPoints.size();
+        for (int i = 0; i < numPoints; ++i) {
+            _ragdollPoints[i].setMass(rescale * _ragdollPoints[i].getMass());
+        }
+        _massScale = scale;
+    }
+}

libraries/shared/src/Ragdoll.h

@@ -18,7 +18,14 @@
 
 #include <QVector>
 
-class Constraint;
+//#include "PhysicsSimulation.h"
+
+class DistanceConstraint;
+class FixedConstraint;
+class PhysicsSimulation;
+
+// TODO: don't derive SkeletonModel from Ragdoll so we can clean up the Ragdoll API
+// (==> won't need to worry about namespace conflicts between Entity and Ragdoll).
 
 class Ragdoll {
 public:
@@ -35,13 +42,35 @@ public:
     const QVector<VerletPoint>& getRagdollPoints() const { return _ragdollPoints; }
     QVector<VerletPoint>& getRagdollPoints() { return _ragdollPoints; }
 
+    void initRagdollTransform();
+
+    /// set the translation and rotation of the Ragdoll and adjust all VerletPoints.
+    void setRagdollTransform(const glm::vec3& translation, const glm::quat& rotation);
+
+    const glm::vec3& getTranslationInSimulationFrame() const { return _translationInSimulationFrame; }
+
+    void setMassScale(float scale);
+    float getMassScale() const { return _massScale; }
+
 protected:
     void clearRagdollConstraintsAndPoints();
     virtual void initRagdollPoints() = 0;
     virtual void buildRagdollConstraints() = 0;
 
+    float _massScale;
+    glm::vec3 _ragdollTranslation;  // world-frame
+    glm::quat _ragdollRotation; // world-frame
+    glm::vec3 _translationInSimulationFrame;
+    glm::quat _rotationInSimulationFrame;
+
     QVector<VerletPoint> _ragdollPoints;
-    QVector<Constraint*> _ragdollConstraints;
+    QVector<DistanceConstraint*> _boneConstraints;
+    QVector<FixedConstraint*> _fixedConstraints;
+private:
+    void updateSimulationTransforms(const glm::vec3& translation, const glm::quat& rotation);
+
+    friend class PhysicsSimulation;
+    PhysicsSimulation* _ragdollSimulation;
 };
 
 #endif // hifi_Ragdoll_h

libraries/shared/src/SharedUtil.h

@@ -122,4 +122,5 @@ bool isNaN(float value);
 
 QString formatUsecTime(float usecs, int prec = 3);
 
+
 #endif // hifi_SharedUtil_h

libraries/shared/src/VerletCapsuleShape.cpp

@@ -97,7 +97,7 @@ float VerletCapsuleShape::computeEffectiveMass(const glm::vec3& penetration, con
         _endLagrangeCoef = 1.0f;
     }
     // the effective mass is the weighted sum of the two endpoints
-    return _startLagrangeCoef * _startPoint->_mass + _endLagrangeCoef * _endPoint->_mass;
+    return _startLagrangeCoef * _startPoint->getMass() + _endLagrangeCoef * _endPoint->getMass();
 }
 
 void VerletCapsuleShape::accumulateDelta(float relativeMassFactor, const glm::vec3& penetration) {

libraries/shared/src/VerletPoint.cpp

@@ -31,3 +31,19 @@ void VerletPoint::applyAccumulatedDelta() {
         _numDeltas = 0;
     }
 }
+
+void VerletPoint::move(const glm::vec3& deltaPosition, const glm::quat& deltaRotation, const glm::vec3& oldPivot) {
+    glm::vec3 arm = _position - oldPivot;
+    _position += deltaPosition + (deltaRotation * arm - arm);
+    arm = _lastPosition - oldPivot;
+    _lastPosition += deltaPosition + (deltaRotation * arm - arm);
+}
+
+void VerletPoint::setMass(float mass) {
+    const float MIN_MASS = 1.0e-6f;
+    const float MAX_MASS = 1.0e18f;
+    if (glm::isnan(mass)) {
+        mass = MIN_MASS;
+    }
+    _mass = glm::clamp(glm::abs(mass), MIN_MASS, MAX_MASS);
+}

libraries/shared/src/VerletPoint.h

@@ -13,6 +13,8 @@
 #define hifi_VerletPoint_h
 
 #include <glm/glm.hpp>
+#include <glm/gtc/quaternion.hpp>
+
 
 class VerletPoint {
 public:
@@ -22,12 +24,16 @@ public:
     void integrateForward();
     void accumulateDelta(const glm::vec3& delta);
     void applyAccumulatedDelta();
+    void move(const glm::vec3& deltaPosition, const glm::quat& deltaRotation, const glm::vec3& oldPivot);
+
+    void setMass(float mass);
+    float getMass() const { return _mass; }
 
     glm::vec3 _position;
     glm::vec3 _lastPosition;
-    float _mass;
 
 private:
+    float _mass;
     glm::vec3 _accumulatedDelta;
     int _numDeltas;
 };

libraries/shared/src/VerletSphereShape.cpp

@@ -36,7 +36,7 @@ const glm::vec3& VerletSphereShape::getTranslation() const {
 
 // virtual
 float VerletSphereShape::computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint) {
-    return _point->_mass;
+    return _point->getMass();
 }
 
 // virtual