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