Merge pull request #3214 from AndrewMeadows/ragdoll

Ragdoll Part 6: muscle constraints for driving Ragdoll and also damped velocity integration for verlet simulation

interface/src/avatar/MuscleConstraint.cpp

@@ -0,0 +1,34 @@
+//
+//  MuscleConstraint.cpp
+//  interface/src/avatar
+//
+//  Created by Andrew Meadows 2014.07.24
+//  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 <SharedUtil.h>
+#include <VerletPoint.h>
+
+#include "MuscleConstraint.h"
+
+const float DEFAULT_MUSCLE_STRENGTH = 0.5f * MAX_MUSCLE_STRENGTH;
+
+MuscleConstraint::MuscleConstraint(VerletPoint* parent, VerletPoint* child)
+        : _rootPoint(parent), _childPoint(child), 
+        _parentIndex(-1), _childndex(-1), _strength(DEFAULT_MUSCLE_STRENGTH) {
+    _childOffset = child->_position - parent->_position;
+}
+
+float MuscleConstraint::enforce() {
+    _childPoint->_position = (1.0f - _strength) * _childPoint->_position + _strength * (_rootPoint->_position + _childOffset);
+    return 0.0f;
+}
+
+void MuscleConstraint::setIndices(int parent, int child) {
+    _parentIndex = parent;
+    _childndex = child;
+}
+

interface/src/avatar/MuscleConstraint.h

@@ -0,0 +1,43 @@
+//
+//  MuscleConstraint.h
+//  interface/src/avatar
+//
+//  Created by Andrew Meadows 2014.07.24
+//  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_MuscleConstraint_h
+#define hifi_MuscleConstraint_h
+
+#include <Constraint.h>
+
+// 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;
+
+class MuscleConstraint : public Constraint {
+public:
+    MuscleConstraint(VerletPoint* parent, VerletPoint* child);
+    MuscleConstraint(const MuscleConstraint& other);
+    float enforce();
+
+    void setIndices(int parent, int child);
+    int getParentIndex() const { return _parentIndex; }
+    int getChildIndex() const { return _childndex; }
+    void setChildOffset(const glm::vec3& offset) { _childOffset = offset; }
+    void setStrength(float strength) { _strength = glm::clamp(strength, 0.0f, MAX_MUSCLE_STRENGTH); }
+    float getStrength() const { return _strength; }
+private:
+    VerletPoint* _rootPoint;
+    VerletPoint* _childPoint;
+    int _parentIndex;
+    int _childndex;
+    glm::vec3 _childOffset;
+    float _strength;    // a value in range [0,MAX_MUSCLE_STRENGTH]
+};
+
+#endif // hifi_MuscleConstraint_h

interface/src/avatar/SkeletonModel.cpp

@@ -14,11 +14,14 @@
 
 #include <VerletCapsuleShape.h>
 #include <VerletSphereShape.h>
+#include <DistanceConstraint.h>
+#include <FixedConstraint.h>
 
 #include "Application.h"
 #include "Avatar.h"
 #include "Hand.h"
 #include "Menu.h"
+#include "MuscleConstraint.h"
 #include "SkeletonModel.h"
 
 SkeletonModel::SkeletonModel(Avatar* owningAvatar, QObject* parent) : 
@@ -505,6 +508,7 @@ void SkeletonModel::renderRagdoll() {
 // virtual
 void SkeletonModel::initRagdollPoints() {
     clearRagdollConstraintsAndPoints();
+    _muscleConstraints.clear();
 
     // one point for each joint
     int numJoints = _jointStates.size();
@@ -512,8 +516,7 @@ void SkeletonModel::initRagdollPoints() {
     for (int i = 0; i < numJoints; ++i) {
         const JointState& state = _jointStates.at(i);
         glm::vec3 position = state.getPosition();
-        _ragdollPoints[i]._position = position;
+        _ragdollPoints[i].initPosition(position);
-        _ragdollPoints[i]._lastPosition = position;
     }
 }
 
@@ -523,11 +526,12 @@ void SkeletonModel::buildRagdollConstraints() {
     const int numPoints = _ragdollPoints.size();
     assert(numPoints == _jointStates.size());
 
+    float minBone = FLT_MAX;
+    float maxBone = -FLT_MAX;
     QMultiMap<int, int> families;
     for (int i = 0; i < numPoints; ++i) {
         const JointState& state = _jointStates.at(i);
-        const FBXJoint& joint = state.getFBXJoint();
+        int parentIndex = state.getParentIndex();
-        int parentIndex = joint.parentIndex;
         if (parentIndex == -1) {
             FixedConstraint* anchor = new FixedConstraint(&(_ragdollPoints[i]), glm::vec3(0.0f));
             _ragdollConstraints.push_back(anchor);
@@ -537,21 +541,60 @@ void SkeletonModel::buildRagdollConstraints() {
             _ragdollConstraints.push_back(bone);
             families.insert(parentIndex, i);
         }
+        float boneLength = glm::length(state.getPositionInParentFrame());
+        if (boneLength > maxBone) {
+            maxBone = boneLength;
+        } else if (boneLength < minBone) {
+            minBone = boneLength;
+        }
     }
     // Joints that have multiple children effectively have rigid constraints between the children
-    // in the parent frame, so we add constraints between children in the same family.
+    // in the parent frame, so we add DistanceConstraints between children in the same family.
     QMultiMap<int, int>::iterator itr = families.begin();
     while (itr != families.end()) {
         QList<int> children = families.values(itr.key());
-        if (children.size() > 1) {
+        int numChildren = children.size();
-            for (int i = 1; i < children.size(); ++i) {
+        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);
             }
+            if (numChildren > 2) {
+                DistanceConstraint* bone = new DistanceConstraint(&(_ragdollPoints[children[numChildren-1]]), &(_ragdollPoints[children[0]]));
+                _ragdollConstraints.push_back(bone);
+            }
         }
         ++itr;
     }
 
+    float MAX_STRENGTH = 0.3f;
+    float MIN_STRENGTH = 0.005f;
+    // each joint gets a MuscleConstraint to its parent
+    for (int i = 1; i < numPoints; ++i) {
+        const JointState& state = _jointStates.at(i);
+        int p = state.getParentIndex();
+        if (p == -1) {
+            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: 
+        // long = weak and short = strong.
+        constraint->setIndices(p, i);
+        float boneLength = glm::length(state.getPositionInParentFrame());
+
+        float strength = MIN_STRENGTH + (MAX_STRENGTH - MIN_STRENGTH) * (maxBone - boneLength) / (maxBone - minBone);
+        if (!families.contains(i)) {
+            // Although muscles only pull on the children not parents, nevertheless those joints that have
+            // parents AND children are more stable than joints at the end such as fingers.  For such joints we
+            // bestow maximum strength which helps reduce wiggle.
+            strength = MAX_MUSCLE_STRENGTH;
+        }
+        constraint->setStrength(strength);
+    }
+    
 }
 
 void SkeletonModel::updateVisibleJointStates() {
@@ -598,7 +641,8 @@ void SkeletonModel::updateVisibleJointStates() {
 
 // virtual 
 void SkeletonModel::stepRagdollForward(float deltaTime) {
-    moveShapesTowardJoints(deltaTime);
+    Ragdoll::stepRagdollForward(deltaTime);
+    updateMuscles();
 }
 
 float DENSITY_OF_WATER = 1000.0f; // kg/m^3
@@ -666,14 +710,16 @@ void SkeletonModel::buildShapes() {
     if (_ragdollPoints.size() == numStates) {
         int numJoints = _jointStates.size();
         for (int i = 0; i < numJoints; ++i) {
-            _ragdollPoints[i]._lastPosition = _ragdollPoints.at(i)._position;
+            _ragdollPoints[i].initPosition(_jointStates.at(i).getPosition());
-            _ragdollPoints[i]._position = _jointStates.at(i).getPosition();
         }
     }
     enforceRagdollConstraints();
 }
 
 void SkeletonModel::moveShapesTowardJoints(float deltaTime) {
+    // KEEP: although we don't currently use this method we may eventually need it to help
+    // unravel a skelton that has become tangled in its constraints.  So let's keep this
+    // around for a while just in case.
     const int numStates = _jointStates.size();
     assert(_jointStates.size() == _ragdollPoints.size());
     if (_ragdollPoints.size() != numStates) {
@@ -684,41 +730,26 @@ void SkeletonModel::moveShapesTowardJoints(float deltaTime) {
     const float RAGDOLL_FOLLOWS_JOINTS_TIMESCALE = 0.05f;
     float fraction = glm::clamp(deltaTime / RAGDOLL_FOLLOWS_JOINTS_TIMESCALE, 0.0f, 1.0f);
 
-    // SIMPLE LINEAR SLAVING -- KEEP this implementation for reference
+    float oneMinusFraction = 1.0f - fraction; 
-    //float oneMinusFraction = 1.0f - fraction; 
-    //for (int i = 0; i < numStates; ++i) {
-    //    _ragdollPoints[i]._lastPosition = _ragdollPoints[i]._position;
-    //    _ragdollPoints[i]._position = oneMinusFraction * _ragdollPoints[i]._position + fraction * _jointStates.at(i).getPosition();
-    //}
-    // SIMPLE LINEAR SLAVING -- KEEP
-
-    // parent-relative linear slaving
     for (int i = 0; i < numStates; ++i) {
-        JointState& state = _jointStates[i];
+        _ragdollPoints[i].initPosition(oneMinusFraction * _ragdollPoints[i]._position + 
-        _ragdollPoints[i]._lastPosition = _ragdollPoints.at(i)._position;
+                fraction * _jointStates.at(i).getPosition());
+    }
+}
 
-        int p = state.getParentIndex();
+void SkeletonModel::updateMuscles() {
-        if (p == -1) {
+    int numConstraints = _muscleConstraints.size();
-            _ragdollPoints[i]._position = glm::vec3(0.0f);
+    for (int i = 0; i < numConstraints; ++i) {
+        MuscleConstraint* constraint = _muscleConstraints[i];
+        int j = constraint->getParentIndex();
+        if (j == -1) {
             continue;
         }
-        if (state.getDistanceToParent() < EPSILON) {
+        int k = constraint->getChildIndex();
-            _ragdollPoints[i]._position = _ragdollPoints.at(p)._position;
+        if (k == -1) {
             continue;
         }
-
+        constraint->setChildOffset(_jointStates.at(k).getPosition() - _jointStates.at(j).getPosition());
-        glm::vec3 bone = _ragdollPoints.at(i)._lastPosition - _ragdollPoints.at(p)._lastPosition;
-        const JointState& parentState = _jointStates.at(p);
-        glm::vec3 targetBone = state.getPosition() - parentState.getPosition();
-
-        glm::vec3 newBone = (1.0f - fraction) * bone + fraction * targetBone;
-        float boneLength = glm::length(newBone);
-        if (boneLength > EPSILON) {
-            // slam newBone's length to that of the joint helps maintain distance constraints
-            newBone *= state.getDistanceToParent() / boneLength;
-        }
-        // set the new position relative to parent's new position
-        _ragdollPoints[i]._position = _ragdollPoints.at(p)._position + newBone;
     }
 }
 
@@ -738,8 +769,7 @@ void SkeletonModel::computeBoundingShape(const FBXGeometry& geometry) {
         int parentIndex = joint.parentIndex;
         if (parentIndex == -1) {
             transforms[i] = _jointStates[i].getTransform();
-            _ragdollPoints[i]._position = extractTranslation(transforms[i]);
+            _ragdollPoints[i].initPosition(extractTranslation(transforms[i]));
-            _ragdollPoints[i]._lastPosition = _ragdollPoints[i]._position;
             continue;
         }
         
@@ -747,8 +777,7 @@ void SkeletonModel::computeBoundingShape(const FBXGeometry& geometry) {
         transforms[i] = transforms[parentIndex] * glm::translate(joint.translation) 
             * joint.preTransform * glm::mat4_cast(modifiedRotation) * joint.postTransform;
         // setting the ragdollPoints here slams the VerletShapes into their default positions
-        _ragdollPoints[i]._position = extractTranslation(transforms[i]);
+        _ragdollPoints[i].initPosition(extractTranslation(transforms[i]));
-        _ragdollPoints[i]._lastPosition = _ragdollPoints[i]._position;
     }
 
     // compute bounding box that encloses all shapes

interface/src/avatar/SkeletonModel.h

@@ -18,6 +18,7 @@
 #include <Ragdoll.h>
 
 class Avatar;
+class MuscleConstraint;
 
 /// A skeleton loaded from a model.
 class SkeletonModel : public Model, public Ragdoll {
@@ -100,6 +101,7 @@ public:
     virtual void stepRagdollForward(float deltaTime);
 
     void moveShapesTowardJoints(float fraction);
+    void updateMuscles();
 
     void computeBoundingShape(const FBXGeometry& geometry);
     void renderBoundingCollisionShapes(float alpha);
@@ -144,6 +146,7 @@ private:
 
     CapsuleShape _boundingShape;
     glm::vec3 _boundingShapeLocalOffset;
+    QVector<MuscleConstraint*> _muscleConstraints;
 };
 
 #endif // hifi_SkeletonModel_h

libraries/shared/src/Constraint.h

@@ -0,0 +1,28 @@
+//
+//  Constraint.h
+//  libraries/shared/src
+//
+//  Created by Andrew Meadows 2014.07.24
+//  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_Constraint_h
+#define hifi_Constraint_h
+
+class Constraint {
+public:
+    Constraint() {}
+    virtual ~Constraint() {}
+
+    /// Enforce contraint by moving relevant points.
+    /// \return max distance of point movement
+    virtual float enforce() = 0;
+
+protected:
+    int _type;
+};
+
+#endif // hifi_Constraint_h

libraries/shared/src/DistanceConstraint.cpp

@@ -0,0 +1,43 @@
+//
+//  DistanceConstraint.cpp
+//  libraries/shared/src
+//
+//  Created by Andrew Meadows 2014.07.24
+//  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 "DistanceConstraint.h"
+#include "SharedUtil.h" // for EPSILON
+#include "VerletPoint.h"
+
+DistanceConstraint::DistanceConstraint(VerletPoint* startPoint, VerletPoint* endPoint) : _distance(-1.0f) {
+    _points[0] = startPoint;
+    _points[1] = endPoint;
+    _distance = glm::distance(_points[0]->_position, _points[1]->_position);
+}
+
+DistanceConstraint::DistanceConstraint(const DistanceConstraint& other) {
+    _distance = other._distance;
+    _points[0] = other._points[0];
+    _points[1] = other._points[1];
+}
+
+void DistanceConstraint::setDistance(float distance) {
+    _distance = fabsf(distance);
+}
+
+float DistanceConstraint::enforce() {
+    // TODO: use a fast distance approximation
+    float newDistance = glm::distance(_points[0]->_position, _points[1]->_position);
+    glm::vec3 direction(0.0f, 1.0f, 0.0f);
+    if (newDistance > EPSILON) {
+        direction = (_points[0]->_position - _points[1]->_position) / newDistance;
+    }
+    glm::vec3 center = 0.5f * (_points[0]->_position + _points[1]->_position);
+    _points[0]->_position = center + (0.5f * _distance) * direction;
+    _points[1]->_position = center - (0.5f * _distance) * direction;
+    return glm::abs(newDistance - _distance);
+}

libraries/shared/src/DistanceConstraint.h

@@ -0,0 +1,31 @@
+//
+//  DistanceConstraint.h
+//  libraries/shared/src
+//
+//  Created by Andrew Meadows 2014.07.24
+//  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_DistanceConstraint_h
+#define hifi_DistanceConstraint_h
+
+#include "Constraint.h"
+
+class VerletPoint;
+
+class DistanceConstraint : public Constraint {
+public:
+    DistanceConstraint(VerletPoint* startPoint, VerletPoint* endPoint);
+    DistanceConstraint(const DistanceConstraint& other);
+    float enforce();
+    void setDistance(float distance);
+    float getDistance() const { return _distance; }
+private:
+    float _distance;
+    VerletPoint* _points[2];
+};
+
+#endif // hifi_DistanceConstraint_h

libraries/shared/src/FixedConstraint.cpp

@@ -0,0 +1,35 @@
+//
+//  FixedConstraint.cpp
+//  libraries/shared/src
+//
+//  Created by Andrew Meadows 2014.07.24
+//  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 "FixedConstraint.h"
+#include "Shape.h"  // for MAX_SHAPE_MASS
+#include "VerletPoint.h"
+
+FixedConstraint::FixedConstraint(VerletPoint* point, const glm::vec3& anchor) : _point(point), _anchor(anchor) {
+}
+
+float FixedConstraint::enforce() {
+    assert(_point != NULL);
+    // TODO: use fast approximate sqrt here
+    float distance = glm::distance(_anchor, _point->_position);
+    _point->_position = _anchor;
+    return distance;
+}
+
+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

@@ -0,0 +1,32 @@
+//
+//  FixedConstraint.h
+//  libraries/shared/src
+//
+//  Created by Andrew Meadows 2014.07.24
+//  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_FixedConstraint_h
+#define hifi_FixedConstraint_h
+
+#include <glm/glm.hpp>
+
+#include "Constraint.h"
+
+class VerletPoint;
+
+class FixedConstraint : public Constraint {
+public:
+    FixedConstraint(VerletPoint* point, const glm::vec3& anchor);
+    float enforce();
+    void setPoint(VerletPoint* point);
+    void setAnchor(const glm::vec3& anchor);
+private:
+    VerletPoint* _point;
+    glm::vec3 _anchor;
+};
+
+#endif // hifi_FixedConstraint_h

libraries/shared/src/Ragdoll.cpp

@@ -11,86 +11,9 @@
 
 #include "Ragdoll.h"
 
-#include "CapsuleShape.h"
+#include "Constraint.h"
-#include "CollisionInfo.h"
+#include "DistanceConstraint.h"
-#include "SharedUtil.h"
+#include "FixedConstraint.h"
-#include "SphereShape.h"
-
-// ----------------------------------------------------------------------------
-// VerletPoint
-// ----------------------------------------------------------------------------
-void VerletPoint::accumulateDelta(const glm::vec3& delta) {
-    _accumulatedDelta += delta;
-    ++_numDeltas;
-}
-
-void VerletPoint::applyAccumulatedDelta() {
-    if (_numDeltas > 0) { 
-        _position += _accumulatedDelta / (float)_numDeltas;
-        _accumulatedDelta = glm::vec3(0.0f);
-        _numDeltas = 0;
-    }
-}
-
-// ----------------------------------------------------------------------------
-// FixedConstraint
-// ----------------------------------------------------------------------------
-FixedConstraint::FixedConstraint(VerletPoint* point, const glm::vec3& anchor) : _point(point), _anchor(anchor) {
-}
-
-float FixedConstraint::enforce() {
-    assert(_point != NULL);
-    // TODO: use fast approximate sqrt here
-    float distance = glm::distance(_anchor, _point->_position);
-    _point->_position = _anchor;
-    return distance;
-}
-
-void FixedConstraint::setPoint(VerletPoint* point) {
-    assert(point);
-    _point = point;
-    _point->_mass = MAX_SHAPE_MASS;
-}
-
-void FixedConstraint::setAnchor(const glm::vec3& anchor) {
-    _anchor = anchor;
-}
-
-// ----------------------------------------------------------------------------
-// DistanceConstraint
-// ----------------------------------------------------------------------------
-DistanceConstraint::DistanceConstraint(VerletPoint* startPoint, VerletPoint* endPoint) : _distance(-1.0f) {
-    _points[0] = startPoint;
-    _points[1] = endPoint;
-    _distance = glm::distance(_points[0]->_position, _points[1]->_position);
-}
-
-DistanceConstraint::DistanceConstraint(const DistanceConstraint& other) {
-    _distance = other._distance;
-    _points[0] = other._points[0];
-    _points[1] = other._points[1];
-}
-
-void DistanceConstraint::setDistance(float distance) {
-    _distance = fabsf(distance);
-}
-
-float DistanceConstraint::enforce() {
-    // TODO: use a fast distance approximation
-    float newDistance = glm::distance(_points[0]->_position, _points[1]->_position);
-    glm::vec3 direction(0.0f, 1.0f, 0.0f);
-    if (newDistance > EPSILON) {
-        direction = (_points[0]->_position - _points[1]->_position) / newDistance;
-    }
-    glm::vec3 center = 0.5f * (_points[0]->_position + _points[1]->_position);
-    _points[0]->_position = center + (0.5f * _distance) * direction;
-    _points[1]->_position = center - (0.5f * _distance) * direction;
-    return glm::abs(newDistance - _distance);
-}
-
-// ----------------------------------------------------------------------------
-// Ragdoll
-// ----------------------------------------------------------------------------
 
 Ragdoll::Ragdoll() {
 }
@@ -99,6 +22,13 @@ Ragdoll::~Ragdoll() {
     clearRagdollConstraintsAndPoints();
 }
 
+void Ragdoll::stepRagdollForward(float deltaTime) {
+    int numPoints = _ragdollPoints.size();
+    for (int i = 0; i < numPoints; ++i) {
+        _ragdollPoints[i].integrateForward();
+    }
+}
+    
 void Ragdoll::clearRagdollConstraintsAndPoints() {
     int numConstraints = _ragdollConstraints.size();
     for (int i = 0; i < numConstraints; ++i) {
@@ -118,4 +48,3 @@ float Ragdoll::enforceRagdollConstraints() {
     }
     return maxDistance;
 }
-

libraries/shared/src/Ragdoll.h

@@ -14,67 +14,11 @@
 
 #include <glm/glm.hpp>
 #include <glm/gtx/quaternion.hpp>
+#include "VerletPoint.h"
 
 #include <QVector>
 
-class Shape;
+class Constraint;
-
-// TODO: Andrew to move VerletPoint class to its own file
-class VerletPoint {
-public:
-    VerletPoint() : _position(0.0f), _lastPosition(0.0f), _mass(1.0f), _accumulatedDelta(0.0f), _numDeltas(0) {}
-
-    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;
-
-private:
-    glm::vec3 _accumulatedDelta;
-    int _numDeltas;
-};
-
-class Constraint {
-public:
-    Constraint() {}
-    virtual ~Constraint() {}
-
-    /// Enforce contraint by moving relevant points.
-    /// \return max distance of point movement
-    virtual float enforce() = 0;
-
-protected:
-    int _type;
-};
-
-class FixedConstraint : public Constraint {
-public:
-    FixedConstraint(VerletPoint* point, const glm::vec3& anchor);
-    float enforce();
-    void setPoint(VerletPoint* point);
-    void setAnchor(const glm::vec3& anchor);
-private:
-    VerletPoint* _point;
-    glm::vec3 _anchor;
-};
-
-class DistanceConstraint : public Constraint {
-public:
-    DistanceConstraint(VerletPoint* startPoint, VerletPoint* endPoint);
-    DistanceConstraint(const DistanceConstraint& other);
-    float enforce();
-    void setDistance(float distance);
-    float getDistance() const { return _distance; }
-private:
-    float _distance;
-    VerletPoint* _points[2];
-};
 
 class Ragdoll {
 public:
@@ -82,7 +26,7 @@ public:
     Ragdoll();
     virtual ~Ragdoll();
 
-    virtual void stepRagdollForward(float deltaTime) = 0;
+    virtual void stepRagdollForward(float deltaTime);
 
     /// \return max distance of point movement
     float enforceRagdollConstraints();

libraries/shared/src/VerletPoint.cpp

@@ -0,0 +1,31 @@
+//
+//  VerletPoint.cpp
+//  libraries/shared/src
+//
+//  Created by Andrew Meadows 2014.07.24
+//  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 "VerletPoint.h"
+
+void VerletPoint::integrateForward() {
+    glm::vec3 oldPosition = _position;
+    _position += 0.6f * (_position - _lastPosition);
+    _lastPosition = oldPosition;
+}
+
+void VerletPoint::accumulateDelta(const glm::vec3& delta) {
+    _accumulatedDelta += delta;
+    ++_numDeltas;
+}
+
+void VerletPoint::applyAccumulatedDelta() {
+    if (_numDeltas > 0) { 
+        _position += _accumulatedDelta / (float)_numDeltas;
+        _accumulatedDelta = glm::vec3(0.0f);
+        _numDeltas = 0;
+    }
+}

libraries/shared/src/VerletPoint.h

@@ -0,0 +1,39 @@
+//
+//  VerletPoint.h
+//  libraries/shared/src
+//
+//  Created by Andrew Meadows 2014.07.24
+//  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_VerletPoint_h
+#define hifi_VerletPoint_h
+
+#include <glm/glm.hpp>
+
+class VerletPoint {
+public:
+    VerletPoint() : _position(0.0f), _lastPosition(0.0f), _mass(1.0f), _accumulatedDelta(0.0f), _numDeltas(0) {}
+
+    void initPosition(const glm::vec3& position) { _position = position; _lastPosition = position; }
+    void integrateForward();
+    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;
+
+private:
+    glm::vec3 _accumulatedDelta;
+    int _numDeltas;
+};
+
+#endif // hifi_VerletPoint_h