From 2286eae64accf245382c2613199321b82bd66a69 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 8 Aug 2014 10:53:38 -0700 Subject: [PATCH 1/8] head connects to neck during ragdoll simulation --- interface/src/avatar/SkeletonModel.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 5e593526be..24acd8d2e3 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -432,6 +432,9 @@ bool SkeletonModel::getHeadPosition(glm::vec3& headPosition) const { } bool SkeletonModel::getNeckPosition(glm::vec3& neckPosition) const { + if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) { + return isActive() && getVisibleJointPositionInWorldFrame(_geometry->getFBXGeometry().neckJointIndex, neckPosition); + } return isActive() && getJointPositionInWorldFrame(_geometry->getFBXGeometry().neckJointIndex, neckPosition); } @@ -445,11 +448,16 @@ bool SkeletonModel::getNeckParentRotationFromDefaultOrientation(glm::quat& neckP } int parentIndex = geometry.joints.at(geometry.neckJointIndex).parentIndex; glm::quat worldFrameRotation; - if (getJointRotationInWorldFrame(parentIndex, worldFrameRotation)) { - neckParentRotation = worldFrameRotation * _jointStates[parentIndex].getFBXJoint().inverseDefaultRotation; - return true; + bool success = false; + if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) { + success = getVisibleJointRotationInWorldFrame(parentIndex, worldFrameRotation); + } else { + success = getJointRotationInWorldFrame(parentIndex, worldFrameRotation); } - return false; + if (success) { + neckParentRotation = worldFrameRotation * _jointStates[parentIndex].getFBXJoint().inverseDefaultRotation; + } + return success; } bool SkeletonModel::getEyePositions(glm::vec3& firstEyePosition, glm::vec3& secondEyePosition) const { From 2918feddf0c3a79b696f5dedb5228efd63b91901 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 11 Aug 2014 14:51:42 -0700 Subject: [PATCH 2/8] reduce acceptable error for ragdoll simulation --- interface/src/avatar/MyAvatar.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index f59064732c..7481aa9ef8 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -206,7 +206,7 @@ void MyAvatar::simulate(float deltaTime) { { PerformanceTimer perfTimer("ragdoll"); if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) { - const float minError = 0.01f; + const float minError = 0.00001f; const float maxIterations = 10; const quint64 maxUsec = 2000; _physicsSimulation.setTranslation(_position); From 4de43b7cbe3a164521856f23c6182741286d4987 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 13 Aug 2014 11:19:32 -0700 Subject: [PATCH 3/8] cleanup and reordering of code --- libraries/shared/src/ContactPoint.cpp | 80 ++++++++++++++------------- 1 file changed, 41 insertions(+), 39 deletions(-) diff --git a/libraries/shared/src/ContactPoint.cpp b/libraries/shared/src/ContactPoint.cpp index c34d248035..fcdd32e278 100644 --- a/libraries/shared/src/ContactPoint.cpp +++ b/libraries/shared/src/ContactPoint.cpp @@ -13,33 +13,37 @@ #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() : + _lastFrame(0), _shapeA(NULL), _shapeB(NULL), + _offsetA(0.0f), _offsetB(0.0f), + _numPointsA(0), _numPoints(0), _normal(0.0f) { } -ContactPoint::ContactPoint(const CollisionInfo& collision, quint32 frame) : _lastFrame(frame), - _shapeA(collision.getShapeA()), _shapeB(collision.getShapeB()), _offsetA(0.0f), _offsetB(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(); + glm::vec3 pointA = collision._contactPoint; + glm::vec3 pointB = collision._contactPoint - collision._penetration; + 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; + pointA = pointB; + pointB = collision._contactPoint; } + _offsetA = pointA - _shapeA->getTranslation(); + _offsetB = pointB - _shapeB->getTranslation(); + _contactPoint = 0.5f * (pointA + pointB); + _shapeA->getVerletPoints(_points); _numPointsA = _points.size(); _shapeB->getVerletPoints(_points); @@ -61,8 +65,7 @@ ContactPoint::ContactPoint(const CollisionInfo& collision, quint32 frame) : _las // virtual float ContactPoint::enforce() { - int numPoints = _points.size(); - for (int i = 0; i < numPoints; ++i) { + 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); @@ -81,9 +84,9 @@ float ContactPoint::enforce() { 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); + glm::vec3 penetration = pointB - pointA; + float pDotN = glm::dot(penetration, _normal); + bool constraintViolation = (pDotN < 0.0f); // the contact point will be the average of the two points on the shapes _contactPoint = 0.5f * (pointA + pointB); @@ -96,29 +99,28 @@ void ContactPoint::buildConstraints() { // 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; + if (constraintViolation) { + for (int i = 0; i < _numPoints; ++i) { + VerletPoint* point = _points[i]; + glm::vec3 offset = _offsets[i]; - glm::vec3 targetPosition = point->_position + delta; - _distances[i] = glm::distance(_contactPoint, targetPosition); - if (actuallyMovePoints) { + // 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); point->_position += HACK_STRENGTH * delta; } } From bc432aa8e83f72ca60c89c3f3c42228c94ff4110 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 13 Aug 2014 11:29:54 -0700 Subject: [PATCH 4/8] use relative masses for ContactPoint contsrtaint --- libraries/shared/src/ContactPoint.cpp | 44 +++++++++++++++++++++------ libraries/shared/src/ContactPoint.h | 2 ++ 2 files changed, 37 insertions(+), 9 deletions(-) diff --git a/libraries/shared/src/ContactPoint.cpp b/libraries/shared/src/ContactPoint.cpp index fcdd32e278..84a1032565 100644 --- a/libraries/shared/src/ContactPoint.cpp +++ b/libraries/shared/src/ContactPoint.cpp @@ -16,12 +16,14 @@ ContactPoint::ContactPoint() : _lastFrame(0), _shapeA(NULL), _shapeB(NULL), _offsetA(0.0f), _offsetB(0.0f), + _relativeMassA(0.5f), _relativeMassB(0.5f), _numPointsA(0), _numPoints(0), _normal(0.0f) { } ContactPoint::ContactPoint(const CollisionInfo& collision, quint32 frame) : _lastFrame(frame), _shapeA(collision.getShapeA()), _shapeB(collision.getShapeB()), _offsetA(0.0f), _offsetB(0.0f), + _relativeMassA(0.5f), _relativeMassB(0.5f), _numPointsA(0), _numPoints(0), _normal(0.0f) { glm::vec3 pointA = collision._contactPoint; @@ -42,13 +44,28 @@ ContactPoint::ContactPoint(const CollisionInfo& collision, quint32 frame) : _offsetA = pointA - _shapeA->getTranslation(); _offsetB = pointB - _shapeB->getTranslation(); - _contactPoint = 0.5f * (pointA + pointB); _shapeA->getVerletPoints(_points); _numPointsA = _points.size(); _shapeB->getVerletPoints(_points); _numPoints = _points.size(); + // compute and cache relative masses + float massA = EPSILON; + for (int i = 0; i < _numPointsA; ++i) { + massA += _points[i]->getMass(); + } + float massB = EPSILON; + for (int i = _numPointsA; i < _numPoints; ++i) { + massB += _points[i]->getMass(); + } + float invTotalMass = 1.0f / (massA + massB); + _relativeMassA = massA * invTotalMass; + _relativeMassB = massB * invTotalMass; + + // _contactPoint will be the weighted average of the two + _contactPoint = _relativeMassA * pointA + _relativeMassB * pointB; + // compute offsets for shapeA for (int i = 0; i < _numPointsA; ++i) { glm::vec3 offset = _points[i]->_position - collision._contactPoint; @@ -128,9 +145,12 @@ void ContactPoint::buildConstraints() { 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(); + + // compute contact points on surface of each shape + glm::vec3 pointA = collision._contactPoint; + glm::vec3 pointB = pointA - collision._penetration; + + // compute the normal (which points from A into B) float pLength = glm::length(collision._penetration); if (pLength > EPSILON) { _normal = collision._penetration / pLength; @@ -140,19 +160,25 @@ void ContactPoint::updateContact(const CollisionInfo& collision, quint32 frame) if (collision._shapeA->getID() > collision._shapeB->getID()) { // our _shapeA always has lower ID - glm::vec3 temp = _offsetA; - _offsetA = _offsetB; - _offsetB = temp; _normal = - _normal; + pointA = pointB; + pointB = collision._contactPoint; } + // compute relative offsets to per-shape contact points + _offsetA = pointA - collision._shapeA->getTranslation(); + _offsetB = pointB - collision._shapeB->getTranslation(); + + // _contactPoint will be the weighted average of the two + _contactPoint = _relativeMassA * pointA + _relativeMassB * pointB; + // compute offsets for shapeA assert(_offsets.size() == _numPoints); for (int i = 0; i < _numPointsA; ++i) { - _offsets[i] = (_points[i]->_position - collision._contactPoint); + _offsets[i] = _points[i]->_position - pointA; } // compute offsets for shapeB for (int i = _numPointsA; i < _numPoints; ++i) { - _offsets[i] = (_points[i]->_position - collision._contactPoint + collision._penetration); + _offsets[i] = _points[i]->_position - pointB; } } diff --git a/libraries/shared/src/ContactPoint.h b/libraries/shared/src/ContactPoint.h index b7e0775bc1..5257fabee0 100644 --- a/libraries/shared/src/ContactPoint.h +++ b/libraries/shared/src/ContactPoint.h @@ -41,6 +41,8 @@ protected: 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 + float _relativeMassA; // massA / totalMass + float _relativeMassB; // massB / totalMass int _numPointsA; // number of VerletPoints that belong to _shapeA int _numPoints; // total number of VerletPoints QVector _points; // points that belong to colliding shapes From 2f1a56a87c0414c3d2c9a2c74abedf4872e4e08d Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 13 Aug 2014 13:48:04 -0700 Subject: [PATCH 5/8] ContactPoint tries to help maintain collisions so that collision events will continue to update the contact --- libraries/shared/src/ContactPoint.cpp | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/libraries/shared/src/ContactPoint.cpp b/libraries/shared/src/ContactPoint.cpp index 84a1032565..c69f2ea8f2 100644 --- a/libraries/shared/src/ContactPoint.cpp +++ b/libraries/shared/src/ContactPoint.cpp @@ -13,6 +13,8 @@ #include "Shape.h" #include "SharedUtil.h" +const float CONTACT_PENETRATION_ALLOWANCE = 0.005f; + ContactPoint::ContactPoint() : _lastFrame(0), _shapeA(NULL), _shapeB(NULL), _offsetA(0.0f), _offsetB(0.0f), @@ -42,6 +44,10 @@ ContactPoint::ContactPoint(const CollisionInfo& collision, quint32 frame) : pointB = collision._contactPoint; } + // bring the contact points inside the shapes to help maintain collision updates + pointA -= CONTACT_PENETRATION_ALLOWANCE * _normal; + pointB += CONTACT_PENETRATION_ALLOWANCE * _normal; + _offsetA = pointA - _shapeA->getTranslation(); _offsetB = pointB - _shapeB->getTranslation(); @@ -68,13 +74,13 @@ ContactPoint::ContactPoint(const CollisionInfo& collision, quint32 frame) : // compute offsets for shapeA for (int i = 0; i < _numPointsA; ++i) { - glm::vec3 offset = _points[i]->_position - collision._contactPoint; + glm::vec3 offset = _points[i]->_position - pointA; _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; + glm::vec3 offset = _points[i]->_position - pointB; _offsets.push_back(offset); _distances.push_back(glm::length(offset)); } @@ -101,9 +107,9 @@ float ContactPoint::enforce() { void ContactPoint::buildConstraints() { glm::vec3 pointA = _shapeA->getTranslation() + _offsetA; glm::vec3 pointB = _shapeB->getTranslation() + _offsetB; - glm::vec3 penetration = pointB - pointA; + glm::vec3 penetration = pointA - pointB; float pDotN = glm::dot(penetration, _normal); - bool constraintViolation = (pDotN < 0.0f); + bool constraintViolation = (pDotN > CONTACT_PENETRATION_ALLOWANCE); // the contact point will be the average of the two points on the shapes _contactPoint = 0.5f * (pointA + pointB); @@ -164,6 +170,10 @@ void ContactPoint::updateContact(const CollisionInfo& collision, quint32 frame) pointA = pointB; pointB = collision._contactPoint; } + + // bring the contact points inside the shapes to help maintain collision updates + pointA -= CONTACT_PENETRATION_ALLOWANCE * _normal; + pointB += CONTACT_PENETRATION_ALLOWANCE * _normal; // compute relative offsets to per-shape contact points _offsetA = pointA - collision._shapeA->getTranslation(); From b177686f74e455874217a1b2a6e31463402cc4ed Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 13 Aug 2014 14:50:25 -0700 Subject: [PATCH 6/8] minor tweak to how ContactPoint is updated --- libraries/shared/src/ContactPoint.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/libraries/shared/src/ContactPoint.cpp b/libraries/shared/src/ContactPoint.cpp index c69f2ea8f2..2d55da9265 100644 --- a/libraries/shared/src/ContactPoint.cpp +++ b/libraries/shared/src/ContactPoint.cpp @@ -133,8 +133,8 @@ void ContactPoint::buildConstraints() { 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 + // perpendicular < parallel ==> static friction ==> perpFactor = 1.0 + // perpendicular > parallel ==> dynamic friction ==> 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; @@ -146,6 +146,10 @@ void ContactPoint::buildConstraints() { _distances[i] = glm::distance(_contactPoint, targetPosition); point->_position += HACK_STRENGTH * delta; } + } else { + for (int i = 0; i < _numPoints; ++i) { + _distances[i] = glm::length(glm::length(_offsets[i])); + } } } @@ -179,9 +183,6 @@ void ContactPoint::updateContact(const CollisionInfo& collision, quint32 frame) _offsetA = pointA - collision._shapeA->getTranslation(); _offsetB = pointB - collision._shapeB->getTranslation(); - // _contactPoint will be the weighted average of the two - _contactPoint = _relativeMassA * pointA + _relativeMassB * pointB; - // compute offsets for shapeA assert(_offsets.size() == _numPoints); for (int i = 0; i < _numPointsA; ++i) { From 3d107cc7e5f3e5f81dfbedfd047dac8cb8ea0bf6 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 13 Aug 2014 14:51:16 -0700 Subject: [PATCH 7/8] less variability in verlet simulation loop count --- interface/src/avatar/MyAvatar.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 7481aa9ef8..71052bf6ea 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -207,8 +207,8 @@ void MyAvatar::simulate(float deltaTime) { PerformanceTimer perfTimer("ragdoll"); if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) { const float minError = 0.00001f; - const float maxIterations = 10; - const quint64 maxUsec = 2000; + const float maxIterations = 3; + const quint64 maxUsec = 4000; _physicsSimulation.setTranslation(_position); _physicsSimulation.stepForward(deltaTime, minError, maxIterations, maxUsec); } else { From f66398d3666b693caef40bf4f54a3ab71221a578 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 13 Aug 2014 15:06:34 -0700 Subject: [PATCH 8/8] adding comment about parameter's purpose --- libraries/shared/src/ContactPoint.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/shared/src/ContactPoint.cpp b/libraries/shared/src/ContactPoint.cpp index 2d55da9265..4c1cf7b842 100644 --- a/libraries/shared/src/ContactPoint.cpp +++ b/libraries/shared/src/ContactPoint.cpp @@ -13,6 +13,8 @@ #include "Shape.h" #include "SharedUtil.h" +// This parameter helps keep the actual point of contact slightly inside each shape +// which allows the collisions to happen almost every frame for more frequent updates. const float CONTACT_PENETRATION_ALLOWANCE = 0.005f; ContactPoint::ContactPoint() :