From c17b8fdb6078a7a2bbd779132c4a49b6d29b036b Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 20 Jun 2014 16:31:39 -0700 Subject: [PATCH] collisions update shapes VerletShapes keep pointers to VerletPoints (rather than to glm::vec3's) VerletShapes compute lagrangian coefficients VerletShapes send their movement accumulations to their VerletPoints --- interface/src/avatar/SkeletonModel.cpp | 4 +- libraries/shared/src/CollisionInfo.cpp | 11 ++- libraries/shared/src/PhysicsSimulation.cpp | 16 +++- libraries/shared/src/Ragdoll.cpp | 16 ++-- libraries/shared/src/Ragdoll.h | 21 ++++-- libraries/shared/src/VerletCapsuleShape.cpp | 84 ++++++++++++++++----- libraries/shared/src/VerletCapsuleShape.h | 20 ++++- libraries/shared/src/VerletSphereShape.cpp | 26 ++++++- libraries/shared/src/VerletSphereShape.h | 11 ++- tests/physics/src/VerletShapeTests.cpp | 35 ++++----- 10 files changed, 177 insertions(+), 67 deletions(-) diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index b28dd08663..0598e2aabd 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -580,12 +580,12 @@ void SkeletonModel::buildShapes() { } Shape* shape = NULL; if (type == Shape::SPHERE_SHAPE) { - shape = new VerletSphereShape(radius, &(_ragdollPoints[i]._position)); + shape = new VerletSphereShape(radius, &(_ragdollPoints[i])); shape->setEntity(this); } else if (type == Shape::CAPSULE_SHAPE) { int parentIndex = joint.parentIndex; assert(parentIndex != -1); - shape = new VerletCapsuleShape(radius, &(_ragdollPoints[parentIndex]._position), &(_ragdollPoints[i]._position)); + shape = new VerletCapsuleShape(radius, &(_ragdollPoints[parentIndex]), &(_ragdollPoints[i])); shape->setEntity(this); } _shapes.push_back(shape); diff --git a/libraries/shared/src/CollisionInfo.cpp b/libraries/shared/src/CollisionInfo.cpp index 3dd3610edb..c76c99e747 100644 --- a/libraries/shared/src/CollisionInfo.cpp +++ b/libraries/shared/src/CollisionInfo.cpp @@ -12,6 +12,7 @@ #include "CollisionInfo.h" #include "Shape.h" +#include "SharedUtil.h" CollisionInfo::CollisionInfo() : _data(NULL), @@ -42,10 +43,16 @@ void CollisionInfo::apply() { Shape* shapeB = const_cast(_shapeB); massB = shapeB->computeEffectiveMass(-_penetration, _contactPoint - _penetration); totalMass = massA + massB; - shapeB->accumulateDelta(massA / totalMass, -_penetration); + if (totalMass < EPSILON) { + massA = massB = 1.0f; + totalMass = 2.0f; + } + // remember that _penetration points from A into B + shapeB->accumulateDelta(massA / totalMass, _penetration); } // NOTE: Shape::accumulateDelta() uses the coefficients from previous call to Shape::computeEffectiveMass() - shapeA->accumulateDelta(massB / totalMass, _penetration); + // remember that _penetration points from A into B + shapeA->accumulateDelta(massB / totalMass, -_penetration); } CollisionInfo* CollisionList::getNewCollision() { diff --git a/libraries/shared/src/PhysicsSimulation.cpp b/libraries/shared/src/PhysicsSimulation.cpp index 2891ea8853..c15d0fb541 100644 --- a/libraries/shared/src/PhysicsSimulation.cpp +++ b/libraries/shared/src/PhysicsSimulation.cpp @@ -128,7 +128,7 @@ void PhysicsSimulation::removeRagdoll(Ragdoll* doll) { } } } -// TODO: Andrew need to implement: +// TODO: Andrew to implement: // DONE (1) joints pull points (SpecialCapsuleShape would help solve this) // DONE (2) points slam shapes (SpecialCapsuleShape would help solve this) // DONE (3) detect collisions @@ -136,9 +136,16 @@ void PhysicsSimulation::removeRagdoll(Ragdoll* doll) { // DONE (5) enforce constraints // DONE (6) make sure MyAvatar creates shapes, adds to simulation with ragdoll support // DONE (7) support for pairwise collision bypass -// (8) process collisions -// (9) add and enforce angular contraints for joints +// DONE (8) process collisions +// DONE (8a) stubbery +// DONE (8b) shapes actually accumulate movement +// DONE (9) verify that avatar shapes self collide +// (10) slave rendered SkeletonModel to physical shapes +// (10a) give SkeletonModel duplicate JointState data +// (10b) figure out how to slave dupe JointStates to physical shapes +// (11) add and enforce angular contraints for joints void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIterations, quint64 maxUsec) { + static int adebug = 0; ++adebug; quint64 startTime = usecTimestampNow(); quint64 expiry = startTime + maxUsec; @@ -148,6 +155,9 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter enforceConstraints(minError, maxIterations, expiry - usecTimestampNow()); _stepTime = usecTimestampNow()- startTime; + if (0 == (adebug % 200)) { + std::cout << " adebug nC = " << _numCollisions << " i = " << _numIterations << " e = " << _constraintError << " t = " << _stepTime << std::endl; // adebug + } } void PhysicsSimulation::moveRagdolls(float deltaTime) { diff --git a/libraries/shared/src/Ragdoll.cpp b/libraries/shared/src/Ragdoll.cpp index 20828018e5..be1ee07a20 100644 --- a/libraries/shared/src/Ragdoll.cpp +++ b/libraries/shared/src/Ragdoll.cpp @@ -19,15 +19,17 @@ // ---------------------------------------------------------------------------- // VerletPoint // ---------------------------------------------------------------------------- -void VerletPoint::accumulatePush(const glm::vec3& delta) { - _accumulatedPush += delta; - ++_numPushes; +void VerletPoint::accumulateDelta(const glm::vec3& delta) { + _accumulatedDelta += delta; + ++_numDeltas; } -void VerletPoint::applyAccumulatedPush() { - _lastPosition = _position; - _position += _accumulatedPush / (float)_numPushes; - _numPushes = 0; +void VerletPoint::applyAccumulatedDelta() { + if (_numDeltas > 0) { + _position += _accumulatedDelta / (float)_numDeltas; + _accumulatedDelta = glm::vec3(0.0f); + _numDeltas = 0; + } } // ---------------------------------------------------------------------------- diff --git a/libraries/shared/src/Ragdoll.h b/libraries/shared/src/Ragdoll.h index e6622e8c85..5c9f47841b 100644 --- a/libraries/shared/src/Ragdoll.h +++ b/libraries/shared/src/Ragdoll.h @@ -19,20 +19,29 @@ class Shape; +// TODO: Andrew to move VerletPoint class to its own file class VerletPoint { public: - VerletPoint() : _position(0.0f), _lastPosition(0.0f), _mass(0.0f), _accumulatedPush(0.0f), _numPushes(0) {} + VerletPoint() : _position(0.0f), _lastPosition(0.0f), _mass(1.0f), _accumulatedDelta(0.0f), _numDeltas(0) {} - void accumulatePush(const glm::vec3& delta); - void applyAccumulatedPush(); + void accumulateDelta(const glm::vec3& delta); + void applyAccumulatedDelta(); + + glm::vec3 getAccumulatedDelta() const { + glm::vec3 foo(0.0f); + if (_numDeltas > 0) { + foo = _accumulatedDelta / (float)_numDeltas; + } + return foo; + } glm::vec3 _position; glm::vec3 _lastPosition; float _mass; -protected: - glm::vec3 _accumulatedPush; - int _numPushes; +private: + glm::vec3 _accumulatedDelta; + int _numDeltas; }; class Constraint { diff --git a/libraries/shared/src/VerletCapsuleShape.cpp b/libraries/shared/src/VerletCapsuleShape.cpp index b4117e40f7..3ac4899682 100644 --- a/libraries/shared/src/VerletCapsuleShape.cpp +++ b/libraries/shared/src/VerletCapsuleShape.cpp @@ -10,21 +10,24 @@ // #include "VerletCapsuleShape.h" + +#include "Ragdoll.h" // for VerletPoint #include "SharedUtil.h" -VerletCapsuleShape::VerletCapsuleShape(glm::vec3* startPoint, glm::vec3* endPoint) : - CapsuleShape(), _startPoint(startPoint), _endPoint(endPoint) { +VerletCapsuleShape::VerletCapsuleShape(VerletPoint* startPoint, VerletPoint* endPoint) : + CapsuleShape(), _startPoint(startPoint), _endPoint(endPoint), _startLagrangeCoef(0.5f), _endLagrangeCoef(0.5f) { assert(startPoint); assert(endPoint); - _halfHeight = 0.5f * glm::distance(*_startPoint, *_endPoint); + _halfHeight = 0.5f * glm::distance(_startPoint->_position, _endPoint->_position); updateBoundingRadius(); } -VerletCapsuleShape::VerletCapsuleShape(float radius, glm::vec3* startPoint, glm::vec3* endPoint) : - CapsuleShape(radius, 1.0f), _startPoint(startPoint), _endPoint(endPoint) { +VerletCapsuleShape::VerletCapsuleShape(float radius, VerletPoint* startPoint, VerletPoint* endPoint) : + CapsuleShape(radius, 1.0f), _startPoint(startPoint), _endPoint(endPoint), + _startLagrangeCoef(0.5f), _endLagrangeCoef(0.5f) { assert(startPoint); assert(endPoint); - _halfHeight = 0.5f * glm::distance(*_startPoint, *_endPoint); + _halfHeight = 0.5f * glm::distance(_startPoint->_position, _endPoint->_position); updateBoundingRadius(); } @@ -46,8 +49,8 @@ void VerletCapsuleShape::setRotation(const glm::quat& rotation) { glm::vec3 center = getTranslation(); float halfHeight = getHalfHeight(); glm::vec3 axis = rotation * DEFAULT_CAPSULE_AXIS; - *_startPoint = center - halfHeight * axis; - *_endPoint = center + halfHeight * axis; + _startPoint->_position = center - halfHeight * axis; + _endPoint->_position = center + halfHeight * axis; } void VerletCapsuleShape::setTranslation(const glm::vec3& position) { @@ -56,35 +59,78 @@ void VerletCapsuleShape::setTranslation(const glm::vec3& position) { // update the points such that their center is at position glm::vec3 movement = position - getTranslation(); - *_startPoint += movement; - *_endPoint += movement; + _startPoint->_position += movement; + _endPoint->_position += movement; } const glm::vec3& VerletCapsuleShape::getTranslation() const { // the "translation" of this shape must be computed on the fly VerletCapsuleShape* thisCapsule = const_cast(this); - thisCapsule->_translation = 0.5f * ((*_startPoint) + (*_endPoint)); + thisCapsule->_translation = 0.5f * (_startPoint->_position + _endPoint->_position); return _translation; } +float VerletCapsuleShape::computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint) { + glm::vec3 startLeg = _startPoint->_position - contactPoint; + glm::vec3 endLeg = _endPoint->_position - contactPoint; + + // TODO: use fast approximate distance calculations here + float startLength = glm::length(startLeg); + float endlength = glm::length(endLeg); + + // The raw coefficient is proportional to the other leg's length multiplied by the dot-product + // of the penetration and this leg direction. We don't worry about the common penetration length + // because it is normalized out later. + float startCoef = glm::abs(glm::dot(startLeg, penetration)) * endlength / (startLength + EPSILON); + float endCoef = glm::abs(glm::dot(endLeg, penetration)) * startLength / (endlength + EPSILON); + + float maxCoef = glm::max(startCoef, endCoef); + if (maxCoef > EPSILON) { + // One of these coeficients will be 1.0, the other will be less --> + // 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(_startLagrangeCoef)); + } else { + // The coefficients are the same --> the collision will move both equally + // as if the object were solid. + _startLagrangeCoef = 1.0f; + _endLagrangeCoef = 1.0f; + } + // the effective mass is the weighted sum of the two endpoints + return _startLagrangeCoef * _startPoint->_mass + _endLagrangeCoef * _endPoint->_mass; +} + +void VerletCapsuleShape::accumulateDelta(float relativeMassFactor, const glm::vec3& penetration) { + assert(!glm::isnan(relativeMassFactor)); + _startPoint->accumulateDelta(relativeMassFactor * _startLagrangeCoef * penetration); + _endPoint->accumulateDelta(relativeMassFactor * _endLagrangeCoef * penetration); +} + +void VerletCapsuleShape::applyAccumulatedDelta() { + _startPoint->applyAccumulatedDelta(); + _endPoint->applyAccumulatedDelta(); +} + // virtual float VerletCapsuleShape::getHalfHeight() const { - return 0.5f * glm::distance(*_startPoint, *_endPoint); + return 0.5f * glm::distance(_startPoint->_position, _endPoint->_position); } // virtual void VerletCapsuleShape::getStartPoint(glm::vec3& startPoint) const { - startPoint = *_startPoint; + startPoint = _startPoint->_position; } // virtual void VerletCapsuleShape::getEndPoint(glm::vec3& endPoint) const { - endPoint = *_endPoint; + endPoint = _endPoint->_position; } // virtual void VerletCapsuleShape::computeNormalizedAxis(glm::vec3& axis) const { - glm::vec3 unormalizedAxis = *_endPoint - *_startPoint; + glm::vec3 unormalizedAxis = _endPoint->_position - _startPoint->_position; float fullLength = glm::length(unormalizedAxis); if (fullLength > EPSILON) { axis = unormalizedAxis / fullLength; @@ -101,8 +147,8 @@ void VerletCapsuleShape::setHalfHeight(float halfHeight) { glm::vec3 center = getTranslation(); glm::vec3 axis; computeNormalizedAxis(axis); - *_startPoint = center - halfHeight * axis; - *_endPoint = center + halfHeight * axis; + _startPoint->_position = center - halfHeight * axis; + _endPoint->_position = center + halfHeight * axis; _boundingRadius = _radius + halfHeight; } @@ -114,7 +160,7 @@ void VerletCapsuleShape::setRadiusAndHalfHeight(float radius, float halfHeight) // virtual void VerletCapsuleShape::setEndPoints(const glm::vec3& startPoint, const glm::vec3& endPoint) { - *_startPoint = startPoint; - *_endPoint = endPoint; + _startPoint->_position = startPoint; + _endPoint->_position = endPoint; updateBoundingRadius(); } diff --git a/libraries/shared/src/VerletCapsuleShape.h b/libraries/shared/src/VerletCapsuleShape.h index 72cc8b7b0e..1fd84f5b1e 100644 --- a/libraries/shared/src/VerletCapsuleShape.h +++ b/libraries/shared/src/VerletCapsuleShape.h @@ -32,16 +32,21 @@ // down in a hot simulation loop, such as when processing collision results. Best to // just let the verlet simulation do its thing and not try to constantly force a rotation. +class VerletPoint; + class VerletCapsuleShape : public CapsuleShape { public: - VerletCapsuleShape(glm::vec3* startPoint, glm::vec3* endPoint); - VerletCapsuleShape(float radius, glm::vec3* startPoint, glm::vec3* endPoint); + VerletCapsuleShape(VerletPoint* startPoint, VerletPoint* endPoint); + VerletCapsuleShape(float radius, VerletPoint* startPoint, VerletPoint* endPoint); // virtual overrides from Shape const glm::quat& getRotation() const; void setRotation(const glm::quat& rotation); void setTranslation(const glm::vec3& position); const glm::vec3& getTranslation() const; + float computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint); + void accumulateDelta(float relativeMassFactor, const glm::vec3& penetration); + void applyAccumulatedDelta(); //float getRadius() const { return _radius; } virtual float getHalfHeight() const; @@ -64,8 +69,15 @@ public: protected: // NOTE: VerletCapsuleShape does NOT own the data in its points. - glm::vec3* _startPoint; - glm::vec3* _endPoint; + VerletPoint* _startPoint; + VerletPoint* _endPoint; + + // The LagrangeCoef's are numerical weights for distributing collision movement + // between the relevant VerletPoints associated with this shape. They are functions + // of the movement parameters and are computed (and cached) in computeEffectiveMass() + // and then used in the subsequent accumulateDelta(). + float _startLagrangeCoef; + float _endLagrangeCoef; }; #endif // hifi_VerletCapsuleShape_h diff --git a/libraries/shared/src/VerletSphereShape.cpp b/libraries/shared/src/VerletSphereShape.cpp index 2c659b8159..10c40c6611 100644 --- a/libraries/shared/src/VerletSphereShape.cpp +++ b/libraries/shared/src/VerletSphereShape.cpp @@ -11,22 +11,40 @@ #include "VerletSphereShape.h" -VerletSphereShape::VerletSphereShape(glm::vec3* centerPoint) : SphereShape() { +#include "Ragdoll.h" // for VerletPoint + +VerletSphereShape::VerletSphereShape(VerletPoint* centerPoint) : SphereShape() { assert(centerPoint); _point = centerPoint; } -VerletSphereShape::VerletSphereShape(float radius, glm::vec3* centerPoint) : SphereShape(radius) { +VerletSphereShape::VerletSphereShape(float radius, VerletPoint* centerPoint) : SphereShape(radius) { assert(centerPoint); _point = centerPoint; } // virtual from Shape class void VerletSphereShape::setTranslation(const glm::vec3& position) { - *_point = position; + _point->_position = position; + _point->_lastPosition = position; } // virtual from Shape class const glm::vec3& VerletSphereShape::getTranslation() const { - return *_point; + return _point->_position; +} + +// virtual +float VerletSphereShape::computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint) { + return _point->_mass; +} + +// virtual +void VerletSphereShape::accumulateDelta(float relativeMassFactor, const glm::vec3& penetration) { + _point->accumulateDelta(relativeMassFactor * penetration); +} + +// virtual +void VerletSphereShape::applyAccumulatedDelta() { + _point->applyAccumulatedDelta(); } diff --git a/libraries/shared/src/VerletSphereShape.h b/libraries/shared/src/VerletSphereShape.h index 395f5901e6..65da3b2597 100644 --- a/libraries/shared/src/VerletSphereShape.h +++ b/libraries/shared/src/VerletSphereShape.h @@ -24,19 +24,24 @@ // (2) A VerletShape doesn't own the points that it uses, so you must be careful not to // leave dangling pointers around. +class VerletPoint; + class VerletSphereShape : public SphereShape { public: - VerletSphereShape(glm::vec3* centerPoint); + VerletSphereShape(VerletPoint* point); - VerletSphereShape(float radius, glm::vec3* centerPoint); + VerletSphereShape(float radius, VerletPoint* centerPoint); // virtual overrides from Shape void setTranslation(const glm::vec3& position); const glm::vec3& getTranslation() const; + float computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint); + void accumulateDelta(float relativeMassFactor, const glm::vec3& penetration); + void applyAccumulatedDelta(); protected: // NOTE: VerletSphereShape does NOT own its _point - glm::vec3* _point; + VerletPoint* _point; }; #endif // hifi_VerletSphereShape_h diff --git a/tests/physics/src/VerletShapeTests.cpp b/tests/physics/src/VerletShapeTests.cpp index 6ae839392e..3a3bd43278 100644 --- a/tests/physics/src/VerletShapeTests.cpp +++ b/tests/physics/src/VerletShapeTests.cpp @@ -17,6 +17,7 @@ #include #include +#include // for VerletPoint #include #include #include @@ -33,16 +34,16 @@ static const glm::vec3 zAxis(0.0f, 0.0f, 1.0f); void VerletShapeTests::setSpherePosition() { float radius = 1.0f; glm::vec3 offset(1.23f, 4.56f, 7.89f); - glm::vec3 point; + VerletPoint point; VerletSphereShape sphere(radius, &point); - point = glm::vec3(0.f); + point._position = glm::vec3(0.f); float d = glm::distance(glm::vec3(0.0f), sphere.getTranslation()); if (d != 0.0f) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should be at origin" << std::endl; } - point = offset; + point._position = offset; d = glm::distance(glm::vec3(0.0f), sphere.getTranslation()); if (d != glm::length(offset)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should be at offset" << std::endl; @@ -60,15 +61,15 @@ void VerletShapeTests::sphereMissesSphere() { float offsetDistance = alpha * radiusA + beta * radiusB; // create points for the sphere centers - glm::vec3 points[2]; + VerletPoint points[2]; // give pointers to the spheres VerletSphereShape sphereA(radiusA, (points + 0)); VerletSphereShape sphereB(radiusB, (points + 1)); // set the positions of the spheres by slamming the points directly - points[0] = origin; - points[1] = offsetDistance * offsetDirection; + points[0]._position = origin; + points[1]._position = offsetDistance * offsetDirection; CollisionList collisions(16); @@ -118,15 +119,15 @@ void VerletShapeTests::sphereTouchesSphere() { glm::vec3 expectedPenetration = expectedPenetrationDistance * offsetDirection; // create two points for the sphere centers - glm::vec3 points[2]; + VerletPoint points[2]; // give pointers to the spheres VerletSphereShape sphereA(radiusA, points+0); VerletSphereShape sphereB(radiusB, points+1); // set the positions of the spheres by slamming the points directly - points[0] = origin; - points[1] = offsetDistance * offsetDirection; + points[0]._position = origin; + points[1]._position = offsetDistance * offsetDirection; CollisionList collisions(16); int numCollisions = 0; @@ -213,9 +214,9 @@ void VerletShapeTests::sphereMissesCapsule() { float radialOffset = 1.2f * radiusA + 1.3f * radiusB; // create points for the sphere + capsule - glm::vec3 points[3]; + VerletPoint points[3]; for (int i = 0; i < 3; ++i) { - points[i] = glm::vec3(0.0f); + points[i]._position = glm::vec3(0.0f); } // give the points to the shapes @@ -277,9 +278,9 @@ void VerletShapeTests::sphereTouchesCapsule() { float radialOffset = alpha * radiusA + beta * radiusB; // create points for the sphere + capsule - glm::vec3 points[3]; + VerletPoint points[3]; for (int i = 0; i < 3; ++i) { - points[i] = glm::vec3(0.0f); + points[i]._position = glm::vec3(0.0f); } // give the points to the shapes @@ -496,9 +497,9 @@ void VerletShapeTests::capsuleMissesCapsule() { float totalHalfLength = totalRadius + halfHeightA + halfHeightB; // create points for the shapes - glm::vec3 points[4]; + VerletPoint points[4]; for (int i = 0; i < 4; ++i) { - points[i] = glm::vec3(0.0f); + points[i]._position = glm::vec3(0.0f); } // give the points to the shapes @@ -574,9 +575,9 @@ void VerletShapeTests::capsuleTouchesCapsule() { float totalHalfLength = totalRadius + halfHeightA + halfHeightB; // create points for the shapes - glm::vec3 points[4]; + VerletPoint points[4]; for (int i = 0; i < 4; ++i) { - points[i] = glm::vec3(0.0f); + points[i]._position = glm::vec3(0.0f); } // give the points to the shapes