From 34628aef3b231b31659b63b2ae2890f3fd5faf4d Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 29 Jul 2014 16:47:54 -0700 Subject: [PATCH 01/15] separate muscle constraints from joint constraints --- interface/src/avatar/SkeletonModel.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 3caaad1391..9a3094df12 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -567,8 +567,8 @@ void SkeletonModel::buildRagdollConstraints() { ++itr; } - float MAX_STRENGTH = 0.3f; - float MIN_STRENGTH = 0.005f; + float MAX_STRENGTH = 0.6f; + float MIN_STRENGTH = 0.05f; // each joint gets a MuscleConstraint to its parent for (int i = 1; i < numPoints; ++i) { const JointState& state = _jointStates.at(i); @@ -577,7 +577,6 @@ void SkeletonModel::buildRagdollConstraints() { continue; } MuscleConstraint* constraint = new MuscleConstraint(&(_ragdollPoints[p]), &(_ragdollPoints[i])); - _ragdollConstraints.push_back(constraint); _muscleConstraints.push_back(constraint); // Short joints are more susceptible to wiggle so we modulate the strength based on the joint's length: @@ -643,6 +642,10 @@ void SkeletonModel::updateVisibleJointStates() { void SkeletonModel::stepRagdollForward(float deltaTime) { Ragdoll::stepRagdollForward(deltaTime); updateMuscles(); + int numConstraints = _muscleConstraints.size(); + for (int i = 0; i < numConstraints; ++i) { + _muscleConstraints[i]->enforce(); + } } float DENSITY_OF_WATER = 1000.0f; // kg/m^3 @@ -742,13 +745,8 @@ void SkeletonModel::updateMuscles() { for (int i = 0; i < numConstraints; ++i) { MuscleConstraint* constraint = _muscleConstraints[i]; int j = constraint->getParentIndex(); - if (j == -1) { - continue; - } int k = constraint->getChildIndex(); - if (k == -1) { - continue; - } + assert(j != -1 && k != -1); constraint->setChildOffset(_jointStates.at(k).getPosition() - _jointStates.at(j).getPosition()); } } From 67b9c185eba71a5072bead7a7b7f62f38aae6330 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 29 Jul 2014 16:48:34 -0700 Subject: [PATCH 02/15] formatting and minor tweaks to constraint strengths --- interface/src/avatar/MuscleConstraint.cpp | 3 +-- interface/src/avatar/MuscleConstraint.h | 2 +- libraries/shared/src/VerletPoint.cpp | 4 +++- libraries/shared/src/VerletPoint.h | 4 ---- 4 files changed, 5 insertions(+), 8 deletions(-) diff --git a/interface/src/avatar/MuscleConstraint.cpp b/interface/src/avatar/MuscleConstraint.cpp index f4ba7975d0..31da56d3d3 100644 --- a/interface/src/avatar/MuscleConstraint.cpp +++ b/interface/src/avatar/MuscleConstraint.cpp @@ -16,8 +16,7 @@ const float DEFAULT_MUSCLE_STRENGTH = 0.5f * MAX_MUSCLE_STRENGTH; -MuscleConstraint::MuscleConstraint(VerletPoint* parent, VerletPoint* child) - : _rootPoint(parent), _childPoint(child), +MuscleConstraint::MuscleConstraint(VerletPoint* parent, VerletPoint* child) : _rootPoint(parent), _childPoint(child), _parentIndex(-1), _childndex(-1), _strength(DEFAULT_MUSCLE_STRENGTH) { _childOffset = child->_position - parent->_position; } diff --git a/interface/src/avatar/MuscleConstraint.h b/interface/src/avatar/MuscleConstraint.h index 882b351b80..b2387a33f0 100644 --- a/interface/src/avatar/MuscleConstraint.h +++ b/interface/src/avatar/MuscleConstraint.h @@ -17,7 +17,7 @@ // MuscleConstraint is a simple constraint that pushes the child toward an offset relative to the parent. // It does NOT push the parent. -const float MAX_MUSCLE_STRENGTH = 0.5f; +const float MAX_MUSCLE_STRENGTH = 0.75f; class MuscleConstraint : public Constraint { public: diff --git a/libraries/shared/src/VerletPoint.cpp b/libraries/shared/src/VerletPoint.cpp index 641ac39341..654a74d7ac 100644 --- a/libraries/shared/src/VerletPoint.cpp +++ b/libraries/shared/src/VerletPoint.cpp @@ -11,9 +11,11 @@ #include "VerletPoint.h" +const float INTEGRATION_FRICTION_FACTOR = 0.6f; + void VerletPoint::integrateForward() { glm::vec3 oldPosition = _position; - _position += 0.6f * (_position - _lastPosition); + _position += INTEGRATION_FRICTION_FACTOR * (_position - _lastPosition); _lastPosition = oldPosition; } diff --git a/libraries/shared/src/VerletPoint.h b/libraries/shared/src/VerletPoint.h index 076a624776..f59afb16c5 100644 --- a/libraries/shared/src/VerletPoint.h +++ b/libraries/shared/src/VerletPoint.h @@ -23,10 +23,6 @@ public: void accumulateDelta(const glm::vec3& delta); void applyAccumulatedDelta(); - glm::vec3 getAccumulatedDelta() const { - return (_numDeltas > 0) ? _accumulatedDelta / (float)_numDeltas : glm::vec3(0.0f); - } - glm::vec3 _position; glm::vec3 _lastPosition; float _mass; From 81ba686fdf35fa01ce99509401e03eabb4e1b19c Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 30 Jul 2014 11:02:47 -0700 Subject: [PATCH 03/15] more perfstats for ragdoll simulation --- libraries/shared/src/PhysicsSimulation.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/libraries/shared/src/PhysicsSimulation.cpp b/libraries/shared/src/PhysicsSimulation.cpp index ca9d3d303a..33d88f0726 100644 --- a/libraries/shared/src/PhysicsSimulation.cpp +++ b/libraries/shared/src/PhysicsSimulation.cpp @@ -14,6 +14,7 @@ #include "PhysicsSimulation.h" +#include "PerfStat.h" #include "PhysicsEntity.h" #include "Ragdoll.h" #include "SharedUtil.h" @@ -142,10 +143,12 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter computeCollisions(); processCollisions(); - // enforce constraints - error = 0.0f; - for (int i = 0; i < numDolls; ++i) { - error = glm::max(error, _dolls[i]->enforceRagdollConstraints()); + { // enforce constraints + PerformanceTimer perfTimer("4-enforce"); + error = 0.0f; + for (int i = 0; i < numDolls; ++i) { + error = glm::max(error, _dolls[i]->enforceRagdollConstraints()); + } } ++iterations; @@ -166,6 +169,7 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter } void PhysicsSimulation::moveRagdolls(float deltaTime) { + PerformanceTimer perfTimer("1-integrate"); int numDolls = _dolls.size(); for (int i = 0; i < numDolls; ++i) { _dolls.at(i)->stepRagdollForward(deltaTime); @@ -173,6 +177,7 @@ void PhysicsSimulation::moveRagdolls(float deltaTime) { } void PhysicsSimulation::computeCollisions() { + PerformanceTimer perfTimer("2-collide"); _collisionList.clear(); // TODO: keep track of QSet collidedEntities; int numEntities = _entities.size(); @@ -204,6 +209,7 @@ void PhysicsSimulation::computeCollisions() { } void PhysicsSimulation::processCollisions() { + PerformanceTimer perfTimer("3-resolve"); // walk all collisions, accumulate movement on shapes, and build a list of affected shapes QSet shapes; int numCollisions = _collisionList.size(); From dde0cbddcce3c1f4cc44dcb1bab298050f7632af Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 30 Jul 2014 11:09:19 -0700 Subject: [PATCH 04/15] add Shape::_id --- libraries/shared/src/Shape.h | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/shared/src/Shape.h b/libraries/shared/src/Shape.h index 09ed30a116..a322e6defa 100644 --- a/libraries/shared/src/Shape.h +++ b/libraries/shared/src/Shape.h @@ -14,6 +14,7 @@ #include #include +#include class PhysicsEntity; @@ -21,6 +22,7 @@ const float MAX_SHAPE_MASS = 1.0e18f; // something less than sqrt(FLT_MAX) class Shape { public: + static quint32 getNextID() { static quint32 nextID = 0; return ++nextID; } enum Type{ UNKNOWN_SHAPE = 0, @@ -31,9 +33,10 @@ public: }; Shape() : _type(UNKNOWN_SHAPE), _owningEntity(NULL), _boundingRadius(0.f), _translation(0.f), _rotation(), _mass(MAX_SHAPE_MASS) { } - virtual ~Shape() {} + virtual ~Shape() { _shapeID = getNextID(); } int getType() const { return _type; } + quint32 getID() const { return _shapeID; } void setEntity(PhysicsEntity* entity) { _owningEntity = entity; } PhysicsEntity* getEntity() const { return _owningEntity; } @@ -80,6 +83,7 @@ protected: void setBoundingRadius(float radius) { _boundingRadius = radius; } int _type; + unsigned int _shapeID; PhysicsEntity* _owningEntity; float _boundingRadius; glm::vec3 _translation; From 917833d92a36437835a97ed7052e7e0f7c207ac6 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 30 Jul 2014 11:29:07 -0700 Subject: [PATCH 05/15] namechange for Shape::_id --- libraries/shared/src/Shape.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/shared/src/Shape.h b/libraries/shared/src/Shape.h index a322e6defa..197bc912aa 100644 --- a/libraries/shared/src/Shape.h +++ b/libraries/shared/src/Shape.h @@ -33,10 +33,10 @@ public: }; Shape() : _type(UNKNOWN_SHAPE), _owningEntity(NULL), _boundingRadius(0.f), _translation(0.f), _rotation(), _mass(MAX_SHAPE_MASS) { } - virtual ~Shape() { _shapeID = getNextID(); } + virtual ~Shape() { _id = getNextID(); } int getType() const { return _type; } - quint32 getID() const { return _shapeID; } + quint32 getID() const { return _id; } void setEntity(PhysicsEntity* entity) { _owningEntity = entity; } PhysicsEntity* getEntity() const { return _owningEntity; } @@ -83,7 +83,7 @@ protected: void setBoundingRadius(float radius) { _boundingRadius = radius; } int _type; - unsigned int _shapeID; + unsigned int _id; PhysicsEntity* _owningEntity; float _boundingRadius; glm::vec3 _translation; From 53db6dec136217d0b58d622761cfa187e2737376 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 30 Jul 2014 11:29:43 -0700 Subject: [PATCH 06/15] fix old copy-n-paste typo --- libraries/shared/src/VerletCapsuleShape.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/shared/src/VerletCapsuleShape.cpp b/libraries/shared/src/VerletCapsuleShape.cpp index 3ac4899682..6a145b4924 100644 --- a/libraries/shared/src/VerletCapsuleShape.cpp +++ b/libraries/shared/src/VerletCapsuleShape.cpp @@ -91,7 +91,7 @@ float VerletCapsuleShape::computeEffectiveMass(const glm::vec3& penetration, con _startLagrangeCoef = startCoef / maxCoef; _endLagrangeCoef = endCoef / maxCoef; assert(!glm::isnan(_startLagrangeCoef)); - assert(!glm::isnan(_startLagrangeCoef)); + assert(!glm::isnan(_endLagrangeCoef)); } else { // The coefficients are the same --> the collision will move both equally // as if the object were solid. From c0689f37840ff02fb75e737d326ca2ecabd739ed Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 30 Jul 2014 11:30:08 -0700 Subject: [PATCH 07/15] fix old bug: rotation of CapsuleShape not updated when setting endpoints directly --- libraries/shared/src/CapsuleShape.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/shared/src/CapsuleShape.cpp b/libraries/shared/src/CapsuleShape.cpp index 12ab6ba479..03bc48bd94 100644 --- a/libraries/shared/src/CapsuleShape.cpp +++ b/libraries/shared/src/CapsuleShape.cpp @@ -73,7 +73,7 @@ void CapsuleShape::setEndPoints(const glm::vec3& startPoint, const glm::vec3& en if (height > EPSILON) { _halfHeight = 0.5f * height; axis /= height; - computeNewRotation(axis); + _rotation = computeNewRotation(axis); } updateBoundingRadius(); } From 7b76753daf1c45edebdab9605e3fd1b784041107 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 30 Jul 2014 11:45:53 -0700 Subject: [PATCH 08/15] add PhysicsSimulation::_frame --- libraries/shared/src/PhysicsSimulation.cpp | 3 ++- libraries/shared/src/PhysicsSimulation.h | 2 ++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/shared/src/PhysicsSimulation.cpp b/libraries/shared/src/PhysicsSimulation.cpp index 33d88f0726..ad52948670 100644 --- a/libraries/shared/src/PhysicsSimulation.cpp +++ b/libraries/shared/src/PhysicsSimulation.cpp @@ -26,7 +26,7 @@ int MAX_COLLISIONS_PER_SIMULATION = 256; PhysicsSimulation::PhysicsSimulation() : _collisionList(MAX_COLLISIONS_PER_SIMULATION), - _numIterations(0), _numCollisions(0), _constraintError(0.0f), _stepTime(0) { + _frame(0), _numIterations(0), _numCollisions(0), _constraintError(0.0f), _stepTime(0) { } PhysicsSimulation::~PhysicsSimulation() { @@ -129,6 +129,7 @@ void PhysicsSimulation::removeRagdoll(Ragdoll* doll) { } void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIterations, quint64 maxUsec) { + ++_frame; quint64 now = usecTimestampNow(); quint64 startTime = now; quint64 expiry = startTime + maxUsec; diff --git a/libraries/shared/src/PhysicsSimulation.h b/libraries/shared/src/PhysicsSimulation.h index c611e06870..efd4c543c2 100644 --- a/libraries/shared/src/PhysicsSimulation.h +++ b/libraries/shared/src/PhysicsSimulation.h @@ -12,6 +12,7 @@ #ifndef hifi_PhysicsSimulation #define hifi_PhysicsSimulation +#include #include #include "CollisionInfo.h" @@ -51,6 +52,7 @@ private: QVector _dolls; // some stats + quint32 _frame; int _numIterations; int _numCollisions; float _constraintError; From 176d8d157dacb2c1a8707ad6e5599f8619d49aeb Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 30 Jul 2014 13:07:32 -0700 Subject: [PATCH 09/15] remove unused Constraint::_type --- libraries/shared/src/Constraint.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/libraries/shared/src/Constraint.h b/libraries/shared/src/Constraint.h index 422675b85d..9bbdc185e1 100644 --- a/libraries/shared/src/Constraint.h +++ b/libraries/shared/src/Constraint.h @@ -20,9 +20,6 @@ public: /// Enforce contraint by moving relevant points. /// \return max distance of point movement virtual float enforce() = 0; - -protected: - int _type; }; #endif // hifi_Constraint_h From f579d2a0e22df758ae8278c03a07843a87c4562b Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 30 Jul 2014 13:11:45 -0700 Subject: [PATCH 10/15] add CollisionInfo::getShapePairKey() --- libraries/shared/src/CollisionInfo.cpp | 10 ++++++++++ libraries/shared/src/CollisionInfo.h | 4 ++++ 2 files changed, 14 insertions(+) diff --git a/libraries/shared/src/CollisionInfo.cpp b/libraries/shared/src/CollisionInfo.cpp index e862a22f4a..9dc321fa44 100644 --- a/libraries/shared/src/CollisionInfo.cpp +++ b/libraries/shared/src/CollisionInfo.cpp @@ -26,6 +26,16 @@ CollisionInfo::CollisionInfo() : _addedVelocity(0.f) { } +quint64 CollisionInfo::getShapePairKey() const { + if (_shapeB == NULL || _shapeA == NULL) { + // zero is an invalid key + return 0; + } + quint32 idA = _shapeA->getID(); + quint32 idB = _shapeB->getID(); + return idA < idB ? ((quint64)idA << 32) + (quint64)idB : ((quint64)idB << 32) + (quint64)idA; +} + CollisionList::CollisionList(int maxSize) : _maxSize(maxSize), _size(0) { diff --git a/libraries/shared/src/CollisionInfo.h b/libraries/shared/src/CollisionInfo.h index 1ab06e2ef5..6e70654d15 100644 --- a/libraries/shared/src/CollisionInfo.h +++ b/libraries/shared/src/CollisionInfo.h @@ -15,6 +15,7 @@ #include #include +#include #include class Shape; @@ -47,6 +48,9 @@ public: Shape* getShapeA() const { return const_cast(_shapeA); } Shape* getShapeB() const { return const_cast(_shapeB); } + /// \return unique key for shape pair + quint64 getShapePairKey() const; + const Shape* _shapeA; // pointer to shapeA in this collision const Shape* _shapeB; // pointer to shapeB in this collision From 0b9f246661878b68681b0a3cf622eec0669b0496 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 31 Jul 2014 13:08:27 -0700 Subject: [PATCH 11/15] set the shape ID in the ctor's NOT the dtor --- libraries/shared/src/Shape.h | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/libraries/shared/src/Shape.h b/libraries/shared/src/Shape.h index 197bc912aa..b1efe6d9ce 100644 --- a/libraries/shared/src/Shape.h +++ b/libraries/shared/src/Shape.h @@ -32,8 +32,11 @@ public: LIST_SHAPE }; - Shape() : _type(UNKNOWN_SHAPE), _owningEntity(NULL), _boundingRadius(0.f), _translation(0.f), _rotation(), _mass(MAX_SHAPE_MASS) { } - virtual ~Shape() { _id = getNextID(); } + Shape() : _type(UNKNOWN_SHAPE), _owningEntity(NULL), _boundingRadius(0.f), + _translation(0.f), _rotation(), _mass(MAX_SHAPE_MASS) { + _id = getNextID(); + } + virtual ~Shape() { } int getType() const { return _type; } quint32 getID() const { return _id; } @@ -72,13 +75,19 @@ public: protected: // these ctors are protected (used by derived classes only) - Shape(Type type) : _type(type), _owningEntity(NULL), _boundingRadius(0.f), _translation(0.f), _rotation() {} + Shape(Type type) : _type(type), _owningEntity(NULL), _boundingRadius(0.f), _translation(0.f), _rotation() { + _id = getNextID(); + } Shape(Type type, const glm::vec3& position) - : _type(type), _owningEntity(NULL), _boundingRadius(0.f), _translation(position), _rotation() {} + : _type(type), _owningEntity(NULL), _boundingRadius(0.f), _translation(position), _rotation() { + _id = getNextID(); + } Shape(Type type, const glm::vec3& position, const glm::quat& rotation) - : _type(type), _owningEntity(NULL), _boundingRadius(0.f), _translation(position), _rotation(rotation) {} + : _type(type), _owningEntity(NULL), _boundingRadius(0.f), _translation(position), _rotation(rotation) { + _id = getNextID(); + } void setBoundingRadius(float radius) { _boundingRadius = radius; } From 0f82236f2c981af25f8286f0619f76aa9cf25758 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 31 Jul 2014 13:09:10 -0700 Subject: [PATCH 12/15] minor cleanup --- libraries/shared/src/VerletCapsuleShape.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/libraries/shared/src/VerletCapsuleShape.cpp b/libraries/shared/src/VerletCapsuleShape.cpp index 6a145b4924..ab956264b5 100644 --- a/libraries/shared/src/VerletCapsuleShape.cpp +++ b/libraries/shared/src/VerletCapsuleShape.cpp @@ -90,11 +90,9 @@ float VerletCapsuleShape::computeEffectiveMass(const glm::vec3& penetration, con // one endpoint will move the full amount while the other will move less. _startLagrangeCoef = startCoef / maxCoef; _endLagrangeCoef = endCoef / maxCoef; - assert(!glm::isnan(_startLagrangeCoef)); - assert(!glm::isnan(_endLagrangeCoef)); } else { // The coefficients are the same --> the collision will move both equally - // as if the object were solid. + // as if the contact were at the center of mass. _startLagrangeCoef = 1.0f; _endLagrangeCoef = 1.0f; } @@ -104,8 +102,8 @@ float VerletCapsuleShape::computeEffectiveMass(const glm::vec3& penetration, con void VerletCapsuleShape::accumulateDelta(float relativeMassFactor, const glm::vec3& penetration) { assert(!glm::isnan(relativeMassFactor)); - _startPoint->accumulateDelta(relativeMassFactor * _startLagrangeCoef * penetration); - _endPoint->accumulateDelta(relativeMassFactor * _endLagrangeCoef * penetration); + _startPoint->accumulateDelta((relativeMassFactor * _startLagrangeCoef) * penetration); + _endPoint->accumulateDelta((relativeMassFactor * _endLagrangeCoef) * penetration); } void VerletCapsuleShape::applyAccumulatedDelta() { From fc1d805d54dec0bd16bb6fae49e9725b193ef14b Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 31 Jul 2014 13:16:06 -0700 Subject: [PATCH 13/15] add ContactConstraint and use to impede muscles --- libraries/shared/src/ContactConstraint.cpp | 83 ++++++++++++++++ libraries/shared/src/ContactConstraint.h | 44 +++++++++ libraries/shared/src/PhysicsSimulation.cpp | 104 +++++++++++++++++---- libraries/shared/src/PhysicsSimulation.h | 23 +++-- 4 files changed, 225 insertions(+), 29 deletions(-) create mode 100644 libraries/shared/src/ContactConstraint.cpp create mode 100644 libraries/shared/src/ContactConstraint.h diff --git a/libraries/shared/src/ContactConstraint.cpp b/libraries/shared/src/ContactConstraint.cpp new file mode 100644 index 0000000000..65633788be --- /dev/null +++ b/libraries/shared/src/ContactConstraint.cpp @@ -0,0 +1,83 @@ +// +// 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; + if (glm::dot(penetration, _normal) > EPSILON) { + // 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(0.99f * massB / totalMass, -penetration); + _shapeB->accumulateDelta(0.99f * massA / totalMass, penetration); + return glm::length(penetration); + } + return 0.0f; +} + +void ContactConstraint::updateContact(const CollisionInfo& collision, quint32 frame) { + _lastFrame = frame; + _offsetA = collision._contactPoint - collision._shapeA->getTranslation(); + _offsetB = collision._contactPoint - collision._penetration - collision._shapeB->getTranslation(); + float pLength = glm::length(collision._penetration); + if (pLength > EPSILON) { + _normal = collision._penetration / pLength; + } else { + _normal = glm::vec3(0.0f); + } + if (collision._shapeA->getID() > collision._shapeB->getID()) { + // our _shapeA always has lower ID + glm::vec3 temp = _offsetA; + _offsetA = _offsetB; + _offsetB = temp; + _normal = - _normal; + } +} diff --git a/libraries/shared/src/ContactConstraint.h b/libraries/shared/src/ContactConstraint.h new file mode 100644 index 0000000000..1c8b7d1b57 --- /dev/null +++ b/libraries/shared/src/ContactConstraint.h @@ -0,0 +1,44 @@ +// +// ContactConstraint.h +// libraries/shared/src +// +// Created by Andrew Meadows 2014.07.30 +// Copyright 2014 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_ContactConstraint_h +#define hifi_ContactConstraint_h + +#include +#include + +#include "CollisionInfo.h" + +class Shape; + +class ContactConstraint { +public: + ContactConstraint(); + ContactConstraint(const CollisionInfo& collision, quint32 frame); + + virtual float enforce(); + + void updateContact(const CollisionInfo& collision, quint32 frame); + quint32 getLastFrame() const { return _lastFrame; } + + Shape* getShapeA() const { return _shapeA; } + Shape* getShapeB() const { return _shapeB; } + +protected: + quint32 _lastFrame; // frame count of last update + Shape* _shapeA; + Shape* _shapeB; + glm::vec3 _offsetA; // contact point relative to A's center + glm::vec3 _offsetB; // contact point relative to B's center + glm::vec3 _normal; // (points from A toward B) +}; + +#endif // hifi_ContactConstraint_h diff --git a/libraries/shared/src/PhysicsSimulation.cpp b/libraries/shared/src/PhysicsSimulation.cpp index ad52948670..93b5b2eef2 100644 --- a/libraries/shared/src/PhysicsSimulation.cpp +++ b/libraries/shared/src/PhysicsSimulation.cpp @@ -25,8 +25,7 @@ int MAX_ENTITIES_PER_SIMULATION = 64; int MAX_COLLISIONS_PER_SIMULATION = 256; -PhysicsSimulation::PhysicsSimulation() : _collisionList(MAX_COLLISIONS_PER_SIMULATION), - _frame(0), _numIterations(0), _numCollisions(0), _constraintError(0.0f), _stepTime(0) { +PhysicsSimulation::PhysicsSimulation() : _frame(0), _collisions(MAX_COLLISIONS_PER_SIMULATION) { } PhysicsSimulation::~PhysicsSimulation() { @@ -88,6 +87,15 @@ void PhysicsSimulation::removeEntity(PhysicsEntity* entity) { break; } } + // remove corresponding contacts + QMap::iterator itr = _contacts.begin(); + while (itr != _contacts.end()) { + if (entity == itr.value().getShapeA()->getEntity() || entity == itr.value().getShapeB()->getEntity()) { + itr = _contacts.erase(itr); + } else { + ++itr; + } + } } bool PhysicsSimulation::addRagdoll(Ragdoll* doll) { @@ -135,14 +143,19 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter quint64 expiry = startTime + maxUsec; moveRagdolls(deltaTime); - + computeCollisions(); + enforceContacts(); int numDolls = _dolls.size(); - _numCollisions = 0; + for (int i = 0; i < numDolls; ++i) { + _dolls[i]->enforceRagdollConstraints(); + } + int iterations = 0; float error = 0.0f; do { computeCollisions(); - processCollisions(); + updateContacts(); + resolveCollisions(); { // enforce constraints PerformanceTimer perfTimer("4-enforce"); @@ -154,19 +167,17 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter ++iterations; now = usecTimestampNow(); - } while (_numCollisions != 0 && (iterations < maxIterations) && (error > minError) && (now < expiry)); + } while (_collisions.size() != 0 && (iterations < maxIterations) && (error > minError) && (now < expiry)); - _numIterations = iterations; - _constraintError = error; - _stepTime = usecTimestampNow()- startTime; #ifdef ANDREW_DEBUG + quint64 stepTime = usecTimestampNow()- startTime; // temporary debug info for watching simulation performance - static int adebug = 0; ++adebug; - if (0 == (adebug % 100)) { - std::cout << "adebug Ni = " << _numIterations << " E = " << error << " t = " << _stepTime << std::endl; // adebug + if (0 == (_frame % 100)) { + std::cout << "Ni = " << iterations << " E = " << error << " t = " << stepTime << std::endl; } #endif // ANDREW_DEBUG + pruneContacts(); } void PhysicsSimulation::moveRagdolls(float deltaTime) { @@ -179,7 +190,7 @@ void PhysicsSimulation::moveRagdolls(float deltaTime) { void PhysicsSimulation::computeCollisions() { PerformanceTimer perfTimer("2-collide"); - _collisionList.clear(); + _collisions.clear(); // TODO: keep track of QSet collidedEntities; int numEntities = _entities.size(); for (int i = 0; i < numEntities; ++i) { @@ -195,7 +206,7 @@ void PhysicsSimulation::computeCollisions() { for (int k = j+1; k < numShapes; ++k) { const Shape* otherShape = shapes.at(k); if (otherShape && entity->collisionsAreEnabled(j, k)) { - ShapeCollider::collideShapes(shape, otherShape, _collisionList); + ShapeCollider::collideShapes(shape, otherShape, _collisions); } } } @@ -203,19 +214,18 @@ void PhysicsSimulation::computeCollisions() { // collide with others for (int j = i+1; j < numEntities; ++j) { const QVector otherShapes = _entities.at(j)->getShapes(); - ShapeCollider::collideShapesWithShapes(shapes, otherShapes, _collisionList); + ShapeCollider::collideShapesWithShapes(shapes, otherShapes, _collisions); } } - _numCollisions = _collisionList.size(); } -void PhysicsSimulation::processCollisions() { +void PhysicsSimulation::resolveCollisions() { PerformanceTimer perfTimer("3-resolve"); // walk all collisions, accumulate movement on shapes, and build a list of affected shapes QSet shapes; - int numCollisions = _collisionList.size(); + int numCollisions = _collisions.size(); for (int i = 0; i < numCollisions; ++i) { - CollisionInfo* collision = _collisionList.getCollision(i); + CollisionInfo* collision = _collisions.getCollision(i); collision->apply(); // there is always a shapeA shapes.insert(collision->getShapeA()); @@ -231,3 +241,59 @@ void PhysicsSimulation::processCollisions() { ++shapeItr; } } + +void PhysicsSimulation::enforceContacts() { + QSet 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::iterator itr = _contacts.find(key); + if (itr != _contacts.end()) { + if (itr.value().enforce() > 0.0f) { + shapes.insert(collision->getShapeA()); + shapes.insert(collision->getShapeB()); + } + } + } + // walk all affected shapes and apply accumulated movement + QSet::const_iterator shapeItr = shapes.constBegin(); + while (shapeItr != shapes.constEnd()) { + (*shapeItr)->applyAccumulatedDelta(); + ++shapeItr; + } +} + +void PhysicsSimulation::updateContacts() { + PerformanceTimer perfTimer("3.5-updateContacts"); + int numCollisions = _collisions.size(); + for (int i = 0; i < numCollisions; ++i) { + CollisionInfo* collision = _collisions.getCollision(i); + quint64 key = collision->getShapePairKey(); + if (key == 0) { + continue; + } + QMap::iterator itr = _contacts.find(key); + if (itr == _contacts.end()) { + _contacts.insert(key, ContactConstraint(*collision, _frame)); + } else { + itr.value().updateContact(*collision, _frame); + } + } +} + +const quint32 MAX_CONTACT_FRAME_LIFETIME = 2; + +void PhysicsSimulation::pruneContacts() { + QMap::iterator itr = _contacts.begin(); + while (itr != _contacts.end()) { + if (_frame - itr.value().getLastFrame() > MAX_CONTACT_FRAME_LIFETIME) { + itr = _contacts.erase(itr); + } else { + ++itr; + } + } +} diff --git a/libraries/shared/src/PhysicsSimulation.h b/libraries/shared/src/PhysicsSimulation.h index efd4c543c2..6e69e72219 100644 --- a/libraries/shared/src/PhysicsSimulation.h +++ b/libraries/shared/src/PhysicsSimulation.h @@ -13,9 +13,11 @@ #define hifi_PhysicsSimulation #include +#include #include #include "CollisionInfo.h" +#include "ContactConstraint.h" class PhysicsEntity; class Ragdoll; @@ -42,21 +44,22 @@ public: /// \return distance of largest movement void stepForward(float deltaTime, float minError, int maxIterations, quint64 maxUsec); +protected: void moveRagdolls(float deltaTime); void computeCollisions(); - void processCollisions(); + void resolveCollisions(); + + void enforceContacts(); + void updateContacts(); + void pruneContacts(); private: - CollisionList _collisionList; - QVector _entities; - QVector _dolls; - - // some stats quint32 _frame; - int _numIterations; - int _numCollisions; - float _constraintError; - quint64 _stepTime; + + QVector _dolls; + QVector _entities; + CollisionList _collisions; + QMap _contacts; }; #endif // hifi_PhysicsSimulation From 9d2744296e1627f3178f9e1ddddfa7b71371e18f Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 31 Jul 2014 13:33:46 -0700 Subject: [PATCH 14/15] update physics stats names --- libraries/shared/src/PhysicsSimulation.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/shared/src/PhysicsSimulation.cpp b/libraries/shared/src/PhysicsSimulation.cpp index 93b5b2eef2..3e3529be10 100644 --- a/libraries/shared/src/PhysicsSimulation.cpp +++ b/libraries/shared/src/PhysicsSimulation.cpp @@ -158,7 +158,7 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter resolveCollisions(); { // enforce constraints - PerformanceTimer perfTimer("4-enforce"); + PerformanceTimer perfTimer("5-enforce"); error = 0.0f; for (int i = 0; i < numDolls; ++i) { error = glm::max(error, _dolls[i]->enforceRagdollConstraints()); @@ -220,7 +220,7 @@ void PhysicsSimulation::computeCollisions() { } void PhysicsSimulation::resolveCollisions() { - PerformanceTimer perfTimer("3-resolve"); + PerformanceTimer perfTimer("4-resolve"); // walk all collisions, accumulate movement on shapes, and build a list of affected shapes QSet shapes; int numCollisions = _collisions.size(); @@ -268,7 +268,7 @@ void PhysicsSimulation::enforceContacts() { } void PhysicsSimulation::updateContacts() { - PerformanceTimer perfTimer("3.5-updateContacts"); + PerformanceTimer perfTimer("3-updateContacts"); int numCollisions = _collisions.size(); for (int i = 0; i < numCollisions; ++i) { CollisionInfo* collision = _collisions.getCollision(i); From 789eeed760eae8c5d4768f0d9378891cc47351c2 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 1 Aug 2014 11:11:41 -0700 Subject: [PATCH 15/15] limit action of ContactConstraint to penetration --- libraries/shared/src/ContactConstraint.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/libraries/shared/src/ContactConstraint.cpp b/libraries/shared/src/ContactConstraint.cpp index 65633788be..d1d12fa771 100644 --- a/libraries/shared/src/ContactConstraint.cpp +++ b/libraries/shared/src/ContactConstraint.cpp @@ -44,7 +44,9 @@ float ContactConstraint::enforce() { glm::vec3 pointA = _shapeA->getTranslation() + _offsetA; glm::vec3 pointB = _shapeB->getTranslation() + _offsetB; glm::vec3 penetration = pointA - pointB; - if (glm::dot(penetration, _normal) > EPSILON) { + 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); @@ -56,9 +58,9 @@ float ContactConstraint::enforce() { } // NOTE: Shape::accumulateDelta() uses the coefficients from previous call to Shape::computeEffectiveMass() // and remember that penetration points from A into B - _shapeA->accumulateDelta(0.99f * massB / totalMass, -penetration); - _shapeB->accumulateDelta(0.99f * massA / totalMass, penetration); - return glm::length(penetration); + _shapeA->accumulateDelta(massB / totalMass, -penetration); + _shapeB->accumulateDelta(massA / totalMass, penetration); + return pDotN; } return 0.0f; }