From 374e89817bf86b66df0371065e89cf6ecd4ad5cf Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 1 Aug 2014 14:46:28 -0700 Subject: [PATCH 1/6] minor optimization of MuscleConstraint::enforce() --- interface/src/avatar/MuscleConstraint.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/interface/src/avatar/MuscleConstraint.cpp b/interface/src/avatar/MuscleConstraint.cpp index 31da56d3d3..76f30fdbc4 100644 --- a/interface/src/avatar/MuscleConstraint.cpp +++ b/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; } From 1ec6ee05f39d93b5efa6ae7f9d3a1b47ce838811 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 1 Aug 2014 15:08:21 -0700 Subject: [PATCH 2/6] Rename ContactConstraint to ContactPoint --- .../{ContactConstraint.cpp => ContactPoint.cpp} | 12 ++++++------ .../src/{ContactConstraint.h => ContactPoint.h} | 14 +++++++------- libraries/shared/src/PhysicsSimulation.cpp | 10 +++++----- libraries/shared/src/PhysicsSimulation.h | 4 ++-- 4 files changed, 20 insertions(+), 20 deletions(-) rename libraries/shared/src/{ContactConstraint.cpp => ContactPoint.cpp} (88%) rename libraries/shared/src/{ContactConstraint.h => ContactPoint.h} (78%) diff --git a/libraries/shared/src/ContactConstraint.cpp b/libraries/shared/src/ContactPoint.cpp similarity index 88% rename from libraries/shared/src/ContactConstraint.cpp rename to libraries/shared/src/ContactPoint.cpp index d1d12fa771..1f6dbcdb11 100644 --- a/libraries/shared/src/ContactConstraint.cpp +++ b/libraries/shared/src/ContactPoint.cpp @@ -1,5 +1,5 @@ // -// ContactConstraint.cpp +// ContactPoint.cpp // libraries/shared/src // // Created by Andrew Meadows 2014.07.30 @@ -9,15 +9,15 @@ // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html // -#include "ContactConstraint.h" +#include "ContactPoint.h" #include "Shape.h" #include "SharedUtil.h" -ContactConstraint::ContactConstraint() : _lastFrame(0), _shapeA(NULL), _shapeB(NULL), +ContactPoint::ContactPoint() : _lastFrame(0), _shapeA(NULL), _shapeB(NULL), _offsetA(0.0f), _offsetB(0.0f), _normal(0.0f) { } -ContactConstraint::ContactConstraint(const CollisionInfo& collision, quint32 frame) : _lastFrame(frame), +ContactPoint::ContactPoint(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(); @@ -40,7 +40,7 @@ ContactConstraint::ContactConstraint(const CollisionInfo& collision, quint32 fra } // virtual -float ContactConstraint::enforce() { +float ContactPoint::enforce() { glm::vec3 pointA = _shapeA->getTranslation() + _offsetA; glm::vec3 pointB = _shapeB->getTranslation() + _offsetB; glm::vec3 penetration = pointA - pointB; @@ -65,7 +65,7 @@ float ContactConstraint::enforce() { return 0.0f; } -void ContactConstraint::updateContact(const CollisionInfo& collision, quint32 frame) { +void ContactPoint::updateContact(const CollisionInfo& collision, quint32 frame) { _lastFrame = frame; _offsetA = collision._contactPoint - collision._shapeA->getTranslation(); _offsetB = collision._contactPoint - collision._penetration - collision._shapeB->getTranslation(); diff --git a/libraries/shared/src/ContactConstraint.h b/libraries/shared/src/ContactPoint.h similarity index 78% rename from libraries/shared/src/ContactConstraint.h rename to libraries/shared/src/ContactPoint.h index 1c8b7d1b57..84e15a85ee 100644 --- a/libraries/shared/src/ContactConstraint.h +++ b/libraries/shared/src/ContactPoint.h @@ -1,5 +1,5 @@ // -// ContactConstraint.h +// ContactPoint.h // libraries/shared/src // // Created by Andrew Meadows 2014.07.30 @@ -9,8 +9,8 @@ // 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 #include @@ -19,10 +19,10 @@ class Shape; -class ContactConstraint { +class ContactPoint { public: - ContactConstraint(); - ContactConstraint(const CollisionInfo& collision, quint32 frame); + ContactPoint(); + ContactPoint(const CollisionInfo& collision, quint32 frame); virtual float enforce(); @@ -41,4 +41,4 @@ protected: glm::vec3 _normal; // (points from A toward B) }; -#endif // hifi_ContactConstraint_h +#endif // hifi_ContactPoint_h diff --git a/libraries/shared/src/PhysicsSimulation.cpp b/libraries/shared/src/PhysicsSimulation.cpp index 3e3529be10..8bcbb7255a 100644 --- a/libraries/shared/src/PhysicsSimulation.cpp +++ b/libraries/shared/src/PhysicsSimulation.cpp @@ -88,7 +88,7 @@ void PhysicsSimulation::removeEntity(PhysicsEntity* entity) { } } // remove corresponding contacts - QMap::iterator itr = _contacts.begin(); + QMap::iterator itr = _contacts.begin(); while (itr != _contacts.end()) { if (entity == itr.value().getShapeA()->getEntity() || entity == itr.value().getShapeB()->getEntity()) { itr = _contacts.erase(itr); @@ -251,7 +251,7 @@ void PhysicsSimulation::enforceContacts() { if (key == 0) { continue; } - QMap::iterator itr = _contacts.find(key); + QMap::iterator itr = _contacts.find(key); if (itr != _contacts.end()) { if (itr.value().enforce() > 0.0f) { shapes.insert(collision->getShapeA()); @@ -276,9 +276,9 @@ void PhysicsSimulation::updateContacts() { if (key == 0) { continue; } - QMap::iterator itr = _contacts.find(key); + QMap::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 +288,7 @@ void PhysicsSimulation::updateContacts() { const quint32 MAX_CONTACT_FRAME_LIFETIME = 2; void PhysicsSimulation::pruneContacts() { - QMap::iterator itr = _contacts.begin(); + QMap::iterator itr = _contacts.begin(); while (itr != _contacts.end()) { if (_frame - itr.value().getLastFrame() > MAX_CONTACT_FRAME_LIFETIME) { itr = _contacts.erase(itr); diff --git a/libraries/shared/src/PhysicsSimulation.h b/libraries/shared/src/PhysicsSimulation.h index 6e69e72219..8cbc39b9d3 100644 --- a/libraries/shared/src/PhysicsSimulation.h +++ b/libraries/shared/src/PhysicsSimulation.h @@ -17,7 +17,7 @@ #include #include "CollisionInfo.h" -#include "ContactConstraint.h" +#include "ContactPoint.h" class PhysicsEntity; class Ragdoll; @@ -59,7 +59,7 @@ private: QVector _dolls; QVector _entities; CollisionList _collisions; - QMap _contacts; + QMap _contacts; }; #endif // hifi_PhysicsSimulation From d1b2ba4a430418c97f3b6cf1cede02a0a7c6d039 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 4 Aug 2014 17:30:23 -0700 Subject: [PATCH 3/6] fix bug: false positive capsuleCapsule() collision for capsules with parallel axes and negative pojection of center-to-center separation also replace fabs() with fabsf() --- libraries/shared/src/ShapeCollider.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index 5e4eff67ec..805e7f30f6 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/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; From 4beee3fecfbd51bb9ac0f4ef666c5545a9e4d122 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 4 Aug 2014 17:33:24 -0700 Subject: [PATCH 4/6] add Shape::getVerletPoints() --- libraries/shared/src/Shape.h | 4 ++++ libraries/shared/src/VerletCapsuleShape.cpp | 5 +++++ libraries/shared/src/VerletCapsuleShape.h | 1 + libraries/shared/src/VerletSphereShape.cpp | 4 ++++ libraries/shared/src/VerletSphereShape.h | 2 ++ 5 files changed, 16 insertions(+) diff --git a/libraries/shared/src/Shape.h b/libraries/shared/src/Shape.h index b1efe6d9ce..2efa5b824f 100644 --- a/libraries/shared/src/Shape.h +++ b/libraries/shared/src/Shape.h @@ -15,8 +15,10 @@ #include #include #include +#include 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& 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() { diff --git a/libraries/shared/src/VerletCapsuleShape.cpp b/libraries/shared/src/VerletCapsuleShape.cpp index ab956264b5..6f547d2048 100644 --- a/libraries/shared/src/VerletCapsuleShape.cpp +++ b/libraries/shared/src/VerletCapsuleShape.cpp @@ -111,6 +111,11 @@ void VerletCapsuleShape::applyAccumulatedDelta() { _endPoint->applyAccumulatedDelta(); } +void VerletCapsuleShape::getVerletPoints(QVector& points) { + points.push_back(_startPoint); + points.push_back(_endPoint); +} + // virtual float VerletCapsuleShape::getHalfHeight() const { return 0.5f * glm::distance(_startPoint->_position, _endPoint->_position); diff --git a/libraries/shared/src/VerletCapsuleShape.h b/libraries/shared/src/VerletCapsuleShape.h index 1fd84f5b1e..828e5def6c 100644 --- a/libraries/shared/src/VerletCapsuleShape.h +++ b/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& points); //float getRadius() const { return _radius; } virtual float getHalfHeight() const; diff --git a/libraries/shared/src/VerletSphereShape.cpp b/libraries/shared/src/VerletSphereShape.cpp index 10c40c6611..e24465fd89 100644 --- a/libraries/shared/src/VerletSphereShape.cpp +++ b/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& points) { + points.push_back(_point); +} diff --git a/libraries/shared/src/VerletSphereShape.h b/libraries/shared/src/VerletSphereShape.h index 65da3b2597..c9a23faef2 100644 --- a/libraries/shared/src/VerletSphereShape.h +++ b/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& points); + protected: // NOTE: VerletSphereShape does NOT own its _point From f126ce299a6a56f7b7e9e2f03c5c02f21a6f4564 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 4 Aug 2014 17:34:24 -0700 Subject: [PATCH 5/6] ContactPoint = set of assymetric distance consraints --- libraries/shared/src/ContactPoint.cpp | 111 +++++++++++++++++---- libraries/shared/src/ContactPoint.h | 8 ++ libraries/shared/src/PhysicsSimulation.cpp | 65 +++++------- libraries/shared/src/PhysicsSimulation.h | 3 +- 4 files changed, 127 insertions(+), 60 deletions(-) diff --git a/libraries/shared/src/ContactPoint.cpp b/libraries/shared/src/ContactPoint.cpp index 1f6dbcdb11..c34d248035 100644 --- a/libraries/shared/src/ContactPoint.cpp +++ b/libraries/shared/src/ContactPoint.cpp @@ -18,8 +18,10 @@ ContactPoint::ContactPoint() : _lastFrame(0), _shapeA(NULL), _shapeB(NULL), } ContactPoint::ContactPoint(const CollisionInfo& collision, quint32 frame) : _lastFrame(frame), - _shapeA(collision.getShapeA()), _shapeB(collision.getShapeB()), _offsetA(0.0f), _offsetB(0.0f), _normal(0.0f) { + _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); @@ -37,36 +39,94 @@ ContactPoint::ContactPoint(const CollisionInfo& collision, quint32 frame) : _las _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() { - 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; + 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; } - // 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 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); @@ -75,6 +135,7 @@ void ContactPoint::updateContact(const CollisionInfo& collision, quint32 frame) } else { _normal = glm::vec3(0.0f); } + if (collision._shapeA->getID() > collision._shapeB->getID()) { // our _shapeA always has lower ID glm::vec3 temp = _offsetA; @@ -82,4 +143,14 @@ void ContactPoint::updateContact(const CollisionInfo& collision, quint32 frame) _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); + } } diff --git a/libraries/shared/src/ContactPoint.h b/libraries/shared/src/ContactPoint.h index 84e15a85ee..b7e0775bc1 100644 --- a/libraries/shared/src/ContactPoint.h +++ b/libraries/shared/src/ContactPoint.h @@ -16,6 +16,7 @@ #include #include "CollisionInfo.h" +#include "VerletPoint.h" class Shape; @@ -26,6 +27,7 @@ public: virtual float enforce(); + void buildConstraints(); void updateContact(const CollisionInfo& collision, quint32 frame); quint32 getLastFrame() const { return _lastFrame; } @@ -38,6 +40,12 @@ 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 _points; // points that belong to colliding shapes + QVector _offsets; // offsets to _points from contactPoint + QVector _distances; // distances to _points from contactPoint (during enforcement stage) glm::vec3 _normal; // (points from A toward B) }; diff --git a/libraries/shared/src/PhysicsSimulation.cpp b/libraries/shared/src/PhysicsSimulation.cpp index 8bcbb7255a..bac4f2ad77 100644 --- a/libraries/shared/src/PhysicsSimulation.cpp +++ b/libraries/shared/src/PhysicsSimulation.cpp @@ -10,7 +10,6 @@ // #include -#include #include "PhysicsSimulation.h" @@ -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 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 shapes; int numCollisions = _collisions.size(); @@ -242,33 +236,26 @@ void PhysicsSimulation::resolveCollisions() { } } -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()); - } - } +void PhysicsSimulation::buildContactConstraints() { + PerformanceTimer perfTimer("contacts"); + QMap::iterator itr = _contacts.begin(); + while (itr != _contacts.end()) { + itr.value().buildConstraints(); + ++itr; } - // walk all affected shapes and apply accumulated movement - QSet::const_iterator shapeItr = shapes.constBegin(); - while (shapeItr != shapes.constEnd()) { - (*shapeItr)->applyAccumulatedDelta(); - ++shapeItr; +} + +void PhysicsSimulation::enforceContactConstraints() { + PerformanceTimer perfTimer("contacts"); + QMap::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); diff --git a/libraries/shared/src/PhysicsSimulation.h b/libraries/shared/src/PhysicsSimulation.h index 8cbc39b9d3..fc6d518d62 100644 --- a/libraries/shared/src/PhysicsSimulation.h +++ b/libraries/shared/src/PhysicsSimulation.h @@ -49,7 +49,8 @@ protected: void computeCollisions(); void resolveCollisions(); - void enforceContacts(); + void buildContactConstraints(); + void enforceContactConstraints(); void updateContacts(); void pruneContacts(); From a1fccdb17711897aa37385e471482b3e2e342a68 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 5 Aug 2014 08:32:14 -0700 Subject: [PATCH 6/6] remove dangling shape pointers from PhysicsSimulation --- libraries/shared/src/PhysicsEntity.cpp | 3 +++ libraries/shared/src/PhysicsSimulation.cpp | 6 +++++- libraries/shared/src/PhysicsSimulation.h | 1 + 3 files changed, 9 insertions(+), 1 deletion(-) diff --git a/libraries/shared/src/PhysicsEntity.cpp b/libraries/shared/src/PhysicsEntity.cpp index 37d1a88d67..09b00c201c 100644 --- a/libraries/shared/src/PhysicsEntity.cpp +++ b/libraries/shared/src/PhysicsEntity.cpp @@ -65,6 +65,9 @@ void PhysicsEntity::setEnableShapes(bool enable) { } void PhysicsEntity::clearShapes() { + if (_simulation) { + _simulation->removeShapes(this); + } for (int i = 0; i < _shapes.size(); ++i) { delete _shapes[i]; } diff --git a/libraries/shared/src/PhysicsSimulation.cpp b/libraries/shared/src/PhysicsSimulation.cpp index bac4f2ad77..c2a852c32b 100644 --- a/libraries/shared/src/PhysicsSimulation.cpp +++ b/libraries/shared/src/PhysicsSimulation.cpp @@ -70,6 +70,7 @@ void PhysicsSimulation::removeEntity(PhysicsEntity* entity) { if (!entity || !entity->_simulation || !(entity->_simulation == this)) { return; } + removeShapes(entity); int numEntities = _entities.size(); for (int i = 0; i < numEntities; ++i) { if (entity == _entities.at(i)) { @@ -86,7 +87,10 @@ void PhysicsSimulation::removeEntity(PhysicsEntity* entity) { break; } } - // remove corresponding contacts +} + +void PhysicsSimulation::removeShapes(const PhysicsEntity* entity) { + // remove data structures with pointers to entity's shapes QMap::iterator itr = _contacts.begin(); while (itr != _contacts.end()) { if (entity == itr.value().getShapeA()->getEntity() || entity == itr.value().getShapeB()->getEntity()) { diff --git a/libraries/shared/src/PhysicsSimulation.h b/libraries/shared/src/PhysicsSimulation.h index fc6d518d62..506b37533a 100644 --- a/libraries/shared/src/PhysicsSimulation.h +++ b/libraries/shared/src/PhysicsSimulation.h @@ -32,6 +32,7 @@ public: bool addEntity(PhysicsEntity* entity); void removeEntity(PhysicsEntity* entity); + void removeShapes(const PhysicsEntity* entity); /// \return true if doll was added to or is already in the list bool addRagdoll(Ragdoll* doll);