Merge pull request #3253 from AndrewMeadows/ragdoll

Ragdoll Part 8: improved contact points

interface/src/avatar/MuscleConstraint.cpp

@@ -22,7 +22,7 @@ MuscleConstraint::MuscleConstraint(VerletPoint* parent, VerletPoint* child) : _r
 }
 
 float MuscleConstraint::enforce() {
-    _childPoint->_position = (1.0f - _strength) * _childPoint->_position + _strength * (_rootPoint->_position + _childOffset);
+    _childPoint->_position += _strength * (_rootPoint->_position + _childOffset - _childPoint->_position);
     return 0.0f;
 }
 

libraries/shared/src/ContactConstraint.cpp

@@ -1,85 +0,0 @@
-//
-//  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;
-    }
-}

libraries/shared/src/ContactPoint.cpp

@@ -0,0 +1,156 @@
+//
+//  ContactPoint.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 "ContactPoint.h"
+#include "Shape.h"
+#include "SharedUtil.h"
+
+ContactPoint::ContactPoint() : _lastFrame(0), _shapeA(NULL), _shapeB(NULL), 
+        _offsetA(0.0f), _offsetB(0.0f), _normal(0.0f) {
+}
+
+ContactPoint::ContactPoint(const CollisionInfo& collision, quint32 frame) : _lastFrame(frame),
+        _shapeA(collision.getShapeA()), _shapeB(collision.getShapeB()), _offsetA(0.0f), _offsetB(0.0f), 
+        _numPointsA(0), _numPoints(0), _normal(0.0f) {
+
+    _contactPoint = collision._contactPoint - 0.5f * collision._penetration;
+    _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;
+    }
+
+    _shapeA->getVerletPoints(_points);
+    _numPointsA = _points.size();
+    _shapeB->getVerletPoints(_points);
+    _numPoints = _points.size();
+
+    // compute offsets for shapeA
+    for (int i = 0; i < _numPointsA; ++i) {
+        glm::vec3 offset = _points[i]->_position - collision._contactPoint;
+        _offsets.push_back(offset);
+        _distances.push_back(glm::length(offset));
+    }
+    // compute offsets for shapeB
+    for (int i = _numPointsA; i < _numPoints; ++i) {
+        glm::vec3 offset = _points[i]->_position - collision._contactPoint + collision._penetration;
+        _offsets.push_back(offset);
+        _distances.push_back(glm::length(offset));
+    }
+}
+
+// virtual 
+float ContactPoint::enforce() {
+    int numPoints = _points.size();
+    for (int i = 0; i < numPoints; ++i) {
+        glm::vec3& position = _points[i]->_position;
+        // TODO: use a fast distance approximation
+        float newDistance = glm::distance(_contactPoint, position);
+        float constrainedDistance = _distances[i];
+        // NOTE: these "distance" constraints only push OUT, don't pull IN.
+        if (newDistance > EPSILON && newDistance < constrainedDistance) {
+            glm::vec3 direction = (_contactPoint - position) / newDistance;
+            glm::vec3 center = 0.5f * (_contactPoint + position);
+            _contactPoint = center + (0.5f * constrainedDistance) * direction;
+            position = center - (0.5f * constrainedDistance) * direction;
+        }
+    }
+    return 0.0f;
+}
+
+void ContactPoint::buildConstraints() {
+    glm::vec3 pointA = _shapeA->getTranslation() + _offsetA;
+    glm::vec3 pointB = _shapeB->getTranslation() + _offsetB;
+    glm::vec3 penetration = pointA - pointB;
+    float pDotN = glm::dot(penetration, _normal);
+    bool actuallyMovePoints = (pDotN > EPSILON);
+
+    // the contact point will be the average of the two points on the shapes
+    _contactPoint = 0.5f * (pointA + pointB);
+
+    // TODO: Andrew to compute more correct lagrangian weights that provide a more realistic response.
+    //
+    // HACK: since the weights are naively equal for all points (which is what the above TODO is about) we 
+    // don't want to use the full-strength delta because otherwise there can be annoying oscillations.  We 
+    // reduce this problem by in the short-term by attenuating the delta that is applied, the tradeoff is
+    // that this makes it easier for limbs to tunnel through during collisions.
+    const float HACK_STRENGTH = 0.5f;
+
+    int numPoints = _points.size();
+    for (int i = 0; i < numPoints; ++i) {
+        VerletPoint* point = _points[i];
+        glm::vec3 offset = _offsets[i];
+
+        // split delta into parallel and perpendicular components
+        glm::vec3 delta = _contactPoint + offset - point->_position;
+        glm::vec3 paraDelta = glm::dot(delta, _normal) * _normal;
+        glm::vec3 perpDelta = delta - paraDelta;
+        
+        // use the relative sizes of the components to decide how much perpenducular delta to use
+        // perpendicular < parallel ==> static friciton ==> perpFactor = 1.0
+        // perpendicular > parallel ==> dynamic friciton ==> cap to length of paraDelta ==> perpFactor < 1.0
+        float paraLength = glm::length(paraDelta);
+        float perpLength = glm::length(perpDelta);
+        float perpFactor = (perpLength > paraLength && perpLength > EPSILON) ? (paraLength / perpLength) : 1.0f;
+
+        // recombine the two components to get the final delta
+        delta = paraDelta + perpFactor * perpDelta;
+    
+        glm::vec3 targetPosition = point->_position + delta;
+        _distances[i] = glm::distance(_contactPoint, targetPosition);
+        if (actuallyMovePoints) {
+            point->_position += HACK_STRENGTH * delta;
+        }
+    }
+}
+
+void ContactPoint::updateContact(const CollisionInfo& collision, quint32 frame) {
+    _lastFrame = frame;
+    _contactPoint = collision._contactPoint - 0.5f * collision._penetration;
+    _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;
+    }
+
+    // compute offsets for shapeA
+    assert(_offsets.size() == _numPoints);
+    for (int i = 0; i < _numPointsA; ++i) {
+        _offsets[i] = (_points[i]->_position - collision._contactPoint);
+    }
+    // compute offsets for shapeB
+    for (int i = _numPointsA; i < _numPoints; ++i) {
+        _offsets[i] = (_points[i]->_position - collision._contactPoint + collision._penetration);
+    }
+}

libraries/shared/src/ContactPoint.h

@@ -1,5 +1,5 @@
 //
-//  ContactConstraint.h
+//  ContactPoint.h
 //  libraries/shared/src
 //
 //  Created by Andrew Meadows 2014.07.30
@@ -9,23 +9,25 @@
 //  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
 //
 
-#ifndef hifi_ContactConstraint_h
-#define hifi_ContactConstraint_h
+#ifndef hifi_ContactPoint_h
+#define hifi_ContactPoint_h
 
 #include <QtGlobal>
 #include <glm/glm.hpp>
 
 #include "CollisionInfo.h"
+#include "VerletPoint.h"
 
 class Shape;
 
-class ContactConstraint {
+class ContactPoint {
 public:
-    ContactConstraint();
-    ContactConstraint(const CollisionInfo& collision, quint32 frame);
+    ContactPoint();
+    ContactPoint(const CollisionInfo& collision, quint32 frame);
 
     virtual float enforce();
    
+    void buildConstraints();
     void updateContact(const CollisionInfo& collision, quint32 frame);
     quint32 getLastFrame() const { return _lastFrame; }
 
@@ -38,7 +40,13 @@ protected:
     Shape* _shapeB;
     glm::vec3 _offsetA; // contact point relative to A's center
     glm::vec3 _offsetB; // contact point relative to B's center
+    glm::vec3 _contactPoint; // a "virtual" point that is added to the simulation
+    int _numPointsA; // number of VerletPoints that belong to _shapeA
+    int _numPoints; // total number of VerletPoints
+    QVector<VerletPoint*> _points; // points that belong to colliding shapes
+    QVector<glm::vec3> _offsets;  // offsets to _points from contactPoint
+    QVector<float> _distances; // distances to _points from contactPoint (during enforcement stage)
     glm::vec3 _normal; // (points from A toward B)
 };
 
-#endif // hifi_ContactConstraint_h
+#endif // hifi_ContactPoint_h

libraries/shared/src/PhysicsSimulation.cpp

@@ -10,7 +10,6 @@
 //
 
 #include <glm/glm.hpp>
-#include <iostream>
 
 #include "PhysicsSimulation.h"
 
@@ -88,7 +87,7 @@ void PhysicsSimulation::removeEntity(PhysicsEntity* entity) {
         }
     }
     // remove corresponding contacts
-    QMap<quint64, ContactConstraint>::iterator itr = _contacts.begin();
+    QMap<quint64, ContactPoint>::iterator itr = _contacts.begin();
     while (itr != _contacts.end()) {
         if (entity == itr.value().getShapeA()->getEntity() || entity == itr.value().getShapeB()->getEntity()) {
             itr = _contacts.erase(itr);
@@ -143,11 +142,13 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter
     quint64 expiry = startTime + maxUsec;
 
     moveRagdolls(deltaTime);
-    computeCollisions();
-    enforceContacts();
+    buildContactConstraints();
     int numDolls = _dolls.size();
-    for (int i = 0; i < numDolls; ++i) {
-        _dolls[i]->enforceRagdollConstraints();
+    {
+        PerformanceTimer perfTimer("enforce");
+        for (int i = 0; i < numDolls; ++i) {
+            _dolls[i]->enforceRagdollConstraints();
+        }
     }
 
     int iterations = 0;
@@ -158,30 +159,23 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter
         resolveCollisions();
 
         { // enforce constraints
-            PerformanceTimer perfTimer("5-enforce");
+            PerformanceTimer perfTimer("enforce");
             error = 0.0f;
             for (int i = 0; i < numDolls; ++i) {
                 error = glm::max(error, _dolls[i]->enforceRagdollConstraints());
             }
         }
+        enforceContactConstraints();
         ++iterations;
 
         now = usecTimestampNow();
     } while (_collisions.size() != 0 && (iterations < maxIterations) && (error > minError) && (now < expiry));
 
-
-#ifdef ANDREW_DEBUG
-    quint64 stepTime = usecTimestampNow()- startTime;
-    // temporary debug info for watching simulation performance
-    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");
+    PerformanceTimer perfTimer("integrate");
     int numDolls = _dolls.size();
     for (int i = 0; i < numDolls; ++i) {
         _dolls.at(i)->stepRagdollForward(deltaTime);
@@ -189,7 +183,7 @@ void PhysicsSimulation::moveRagdolls(float deltaTime) {
 }
 
 void PhysicsSimulation::computeCollisions() {
-    PerformanceTimer perfTimer("2-collide");
+    PerformanceTimer perfTimer("collide");
     _collisions.clear();
     // TODO: keep track of QSet<PhysicsEntity*> collidedEntities;
     int numEntities = _entities.size();
@@ -220,7 +214,7 @@ void PhysicsSimulation::computeCollisions() {
 }
 
 void PhysicsSimulation::resolveCollisions() {
-    PerformanceTimer perfTimer("4-resolve");
+    PerformanceTimer perfTimer("resolve");
     // walk all collisions, accumulate movement on shapes, and build a list of affected shapes
     QSet<Shape*> shapes;
     int numCollisions = _collisions.size();
@@ -242,33 +236,26 @@ void PhysicsSimulation::resolveCollisions() {
     }
 }
 
-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());
-            }
-        }
+void PhysicsSimulation::buildContactConstraints() {
+    PerformanceTimer perfTimer("contacts");
+    QMap<quint64, ContactPoint>::iterator itr = _contacts.begin();
+    while (itr != _contacts.end()) {
+        itr.value().buildConstraints();
+        ++itr;
     }
-    // walk all affected shapes and apply accumulated movement
-    QSet<Shape*>::const_iterator shapeItr = shapes.constBegin();
-    while (shapeItr != shapes.constEnd()) {
-        (*shapeItr)->applyAccumulatedDelta();
-        ++shapeItr;
+}
+
+void PhysicsSimulation::enforceContactConstraints() {
+    PerformanceTimer perfTimer("contacts");
+    QMap<quint64, ContactPoint>::iterator itr = _contacts.begin();
+    while (itr != _contacts.end()) {
+        itr.value().enforce();
+        ++itr;
     }
 }
 
 void PhysicsSimulation::updateContacts() {
-    PerformanceTimer perfTimer("3-updateContacts");
+    PerformanceTimer perfTimer("contacts");
     int numCollisions = _collisions.size();
     for (int i = 0; i < numCollisions; ++i) {
         CollisionInfo* collision = _collisions.getCollision(i);
@@ -276,9 +263,9 @@ void PhysicsSimulation::updateContacts() {
         if (key == 0) {
             continue;
         }
-        QMap<quint64, ContactConstraint>::iterator itr = _contacts.find(key);
+        QMap<quint64, ContactPoint>::iterator itr = _contacts.find(key);
         if (itr == _contacts.end()) {
-            _contacts.insert(key, ContactConstraint(*collision, _frame));
+            _contacts.insert(key, ContactPoint(*collision, _frame));
         } else {
             itr.value().updateContact(*collision, _frame);
         }
@@ -288,7 +275,7 @@ void PhysicsSimulation::updateContacts() {
 const quint32 MAX_CONTACT_FRAME_LIFETIME = 2;
 
 void PhysicsSimulation::pruneContacts() {
-    QMap<quint64, ContactConstraint>::iterator itr = _contacts.begin();
+    QMap<quint64, ContactPoint>::iterator itr = _contacts.begin();
     while (itr != _contacts.end()) {
         if (_frame - itr.value().getLastFrame() > MAX_CONTACT_FRAME_LIFETIME) {
             itr = _contacts.erase(itr);

libraries/shared/src/PhysicsSimulation.h

@@ -17,7 +17,7 @@
 #include <QVector>
 
 #include "CollisionInfo.h"
-#include "ContactConstraint.h"
+#include "ContactPoint.h"
 
 class PhysicsEntity;
 class Ragdoll;
@@ -49,7 +49,8 @@ protected:
     void computeCollisions();
     void resolveCollisions();
 
-    void enforceContacts();
+    void buildContactConstraints();
+    void enforceContactConstraints();
     void updateContacts();
     void pruneContacts();
 
@@ -59,7 +60,7 @@ private:
     QVector<Ragdoll*> _dolls;
     QVector<PhysicsEntity*> _entities;
     CollisionList _collisions;
-    QMap<quint64, ContactConstraint> _contacts;
+    QMap<quint64, ContactPoint> _contacts;
 };
 
 #endif // hifi_PhysicsSimulation

libraries/shared/src/Shape.h

@@ -15,8 +15,10 @@
 #include <glm/glm.hpp>
 #include <glm/gtc/quaternion.hpp>
 #include <QtGlobal>
+#include <QVector>
 
 class PhysicsEntity;
+class VerletPoint;
 
 const float MAX_SHAPE_MASS = 1.0e18f; // something less than sqrt(FLT_MAX)
 
@@ -73,6 +75,8 @@ public:
     /// \return volume of shape in cubic meters
     virtual float getVolume() const { return 1.0; }
 
+    virtual void getVerletPoints(QVector<VerletPoint*>& points) {}
+
 protected:
     // these ctors are protected (used by derived classes only)
     Shape(Type type) : _type(type), _owningEntity(NULL), _boundingRadius(0.f), _translation(0.f), _rotation() {

libraries/shared/src/ShapeCollider.cpp

@@ -189,7 +189,7 @@ bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, Col
     glm::vec3 capsuleAxis; 
     capsuleB->computeNormalizedAxis(capsuleAxis);
     float axialDistance = - glm::dot(BA, capsuleAxis);
-    float absAxialDistance = fabs(axialDistance);
+    float absAxialDistance = fabsf(axialDistance);
     float totalRadius = sphereA->getRadius() + capsuleB->getRadius();
     if (absAxialDistance < totalRadius + capsuleB->getHalfHeight()) {
         glm::vec3 radialAxis = BA + axialDistance * capsuleAxis; // points from A to axis of B
@@ -274,7 +274,7 @@ bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, Col
     glm::vec3 capsuleAxis;
     capsuleA->computeNormalizedAxis(capsuleAxis);
     float axialDistance = - glm::dot(AB, capsuleAxis);
-    float absAxialDistance = fabs(axialDistance);
+    float absAxialDistance = fabsf(axialDistance);
     float totalRadius = sphereB->getRadius() + capsuleA->getRadius();
     if (absAxialDistance < totalRadius + capsuleA->getHalfHeight()) {
         glm::vec3 radialAxis = AB + axialDistance * capsuleAxis; // from sphereB to axis of capsuleA
@@ -501,7 +501,7 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB,
         // capsules are approximiately parallel but might still collide
         glm::vec3 BA = centerB - centerA;
         float axialDistance = glm::dot(BA, axisB);
-        if (axialDistance > totalRadius + capsuleA->getHalfHeight() + capsuleB->getHalfHeight()) {
+        if (fabsf(axialDistance) > totalRadius + capsuleA->getHalfHeight() + capsuleB->getHalfHeight()) {
             return false;
         }
         BA = BA - axialDistance * axisB;     // BA now points from centerA to axisB (perp to axis)
@@ -847,7 +847,7 @@ bool sphereAACube_StarkAngles(const glm::vec3& sphereCenter, float sphereRadius,
         // compute the nearest point on sphere
         glm::vec3 surfaceA = sphereCenter + sphereRadius * BA;
         // compute the nearest point on cube
-        float maxBA = glm::max(glm::max(fabs(BA.x), fabs(BA.y)), fabs(BA.z));
+        float maxBA = glm::max(glm::max(fabsf(BA.x), fabsf(BA.y)), fabsf(BA.z));
         glm::vec3 surfaceB = cubeCenter - (0.5f * cubeSide / maxBA) * BA;
         // collision happens when "vector to surfaceA from surfaceB" dots with BA to produce a positive value
         glm::vec3 surfaceAB = surfaceA - surfaceB;

libraries/shared/src/VerletCapsuleShape.cpp

@@ -111,6 +111,11 @@ void VerletCapsuleShape::applyAccumulatedDelta() {
     _endPoint->applyAccumulatedDelta();
 }
 
+void VerletCapsuleShape::getVerletPoints(QVector<VerletPoint*>& points) {
+    points.push_back(_startPoint);
+    points.push_back(_endPoint);
+}
+
 // virtual
 float VerletCapsuleShape::getHalfHeight() const {
     return 0.5f * glm::distance(_startPoint->_position, _endPoint->_position);

libraries/shared/src/VerletCapsuleShape.h

@@ -47,6 +47,7 @@ public:
     float computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint);
     void accumulateDelta(float relativeMassFactor, const glm::vec3& penetration);
     void applyAccumulatedDelta();
+    virtual void getVerletPoints(QVector<VerletPoint*>& points);
 
     //float getRadius() const { return _radius; }
     virtual float getHalfHeight() const;

libraries/shared/src/VerletSphereShape.cpp

@@ -48,3 +48,7 @@ void VerletSphereShape::accumulateDelta(float relativeMassFactor, const glm::vec
 void VerletSphereShape::applyAccumulatedDelta() {
     _point->applyAccumulatedDelta();
 }
+
+void VerletSphereShape::getVerletPoints(QVector<VerletPoint*>& points) {
+    points.push_back(_point);
+}

libraries/shared/src/VerletSphereShape.h

@@ -38,6 +38,8 @@ public:
     float computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint);
     void accumulateDelta(float relativeMassFactor, const glm::vec3& penetration);
     void applyAccumulatedDelta();
+    void getVerletPoints(QVector<VerletPoint*>& points);
+
 
 protected:
     // NOTE: VerletSphereShape does NOT own its _point