diff --git a/libraries/shared/src/CollisionInfo.cpp b/libraries/shared/src/CollisionInfo.cpp index 9e76ca59f6..e54ff12f47 100644 --- a/libraries/shared/src/CollisionInfo.cpp +++ b/libraries/shared/src/CollisionInfo.cpp @@ -17,7 +17,7 @@ CollisionList::CollisionList(int maxSize) : CollisionInfo* CollisionList::getNewCollision() { // return pointer to existing CollisionInfo, or NULL of list is full - return (_size < _maxSize) ? &(_collisions[++_size]) : NULL; + return (_size < _maxSize) ? &(_collisions[_size++]) : NULL; } CollisionInfo* CollisionList::getCollision(int index) { diff --git a/libraries/shared/src/CollisionInfo.h b/libraries/shared/src/CollisionInfo.h index 629cb6b8f2..da89870226 100644 --- a/libraries/shared/src/CollisionInfo.h +++ b/libraries/shared/src/CollisionInfo.h @@ -87,8 +87,8 @@ public: void clear(); private: - int _maxSize; - int _size; + int _maxSize; // the container cannot get larger than this + int _size; // the current number of valid collisions in the list QVector _collisions; }; diff --git a/libraries/shared/src/ListShape.cpp b/libraries/shared/src/ListShape.cpp new file mode 100644 index 0000000000..60fd964ee4 --- /dev/null +++ b/libraries/shared/src/ListShape.cpp @@ -0,0 +1,81 @@ +// +// ListShape.cpp +// +// ListShape: A collection of shapes, each with a local transform. +// +// Created by Andrew Meadows on 2014.02.20 +// Copyright (c) 2014 High Fidelity, Inc. All rights reserved. +// + +#include "ListShape.h" + +// ListShapeEntry + +void ListShapeEntry::updateTransform(const glm::vec3& rootPosition, const glm::quat& rootRotation) { + _shape->setPosition(rootPosition + rootRotation * _localPosition); + _shape->setRotation(_localRotation * rootRotation); +} + +// ListShape + +ListShape::~ListShape() { + clear(); +} + +void ListShape::setPosition(const glm::vec3& position) { + _subShapeTransformsAreDirty = true; + Shape::setPosition(position); +} + +void ListShape::setRotation(const glm::quat& rotation) { + _subShapeTransformsAreDirty = true; + Shape::setRotation(rotation); +} + +void ListShape::updateSubTransforms() { + if (_subShapeTransformsAreDirty) { + for (int i = 0; i < _subShapeEntries.size(); ++i) { + _subShapeEntries[i].updateTransform(_position, _rotation); + } + _subShapeTransformsAreDirty = false; + } +} + +void ListShape::addShape(Shape* shape, const glm::vec3& localPosition, const glm::quat& localRotation) { + if (shape) { + ListShapeEntry entry; + entry._shape = shape; + entry._localPosition = localPosition; + entry._localRotation = localRotation; + _subShapeEntries.push_back(entry); + } +} + +void ListShape::setShapes(QVector& shapes) { + clear(); + _subShapeEntries.swap(shapes); + // TODO: audit our new list of entries and delete any that have null pointers + computeBoundingRadius(); +} + +void ListShape::clear() { + // the ListShape owns its subShapes, so they need to be deleted + for (int i = 0; i < _subShapeEntries.size(); ++i) { + delete _subShapeEntries[i]._shape; + } + _subShapeEntries.clear(); + setBoundingRadius(0.f); +} + +void ListShape::computeBoundingRadius() { + float maxRadius = 0.f; + for (int i = 0; i < _subShapeEntries.size(); ++i) { + ListShapeEntry& entry = _subShapeEntries[i]; + float radius = glm::length(entry._localPosition) + entry._shape->getBoundingRadius(); + if (radius > maxRadius) { + maxRadius = radius; + } + } + setBoundingRadius(maxRadius); +} + diff --git a/libraries/shared/src/ListShape.h b/libraries/shared/src/ListShape.h new file mode 100644 index 0000000000..fe3726c2d1 --- /dev/null +++ b/libraries/shared/src/ListShape.h @@ -0,0 +1,65 @@ +// +// ListShape.h +// +// ListShape: A collection of shapes, each with a local transform. +// +// Created by Andrew Meadows on 2014.02.20 +// Copyright (c) 2014 High Fidelity, Inc. All rights reserved. +// + +#ifndef __hifi__ListShape__ +#define __hifi__ListShape__ + +#include + +#include +#include +#include + +#include "Shape.h" + + +class ListShapeEntry { +public: + void updateTransform(const glm::vec3& position, const glm::quat& rotation); + + Shape* _shape; + glm::vec3 _localPosition; + glm::quat _localRotation; +}; + +class ListShape : public Shape { +public: + + ListShape() : Shape(LIST_SHAPE), _subShapeEntries(), _subShapeTransformsAreDirty(false) {} + + ListShape(const glm::vec3& position, const glm::quat& rotation) : + Shape(LIST_SHAPE, position, rotation), _subShapeEntries(), _subShapeTransformsAreDirty(false) {} + + ~ListShape(); + + void setPosition(const glm::vec3& position); + void setRotation(const glm::quat& rotation); + + void updateSubTransforms(); + + int size() { return _subShapeEntries.size(); } + + void addShape(Shape* shape, const glm::vec3& localPosition, const glm::quat& localRotation); + + void setShapes(QVector& shapes); + + //const QVector& getSubShapes() { return _subShapeEntries; } + +protected: + void clear(); + void computeBoundingRadius(); + + QVector _subShapeEntries; + bool _subShapeTransformsAreDirty; + +private: + ListShape(const ListShape& otherList); // don't implement this +}; + +#endif // __hifi__ListShape__ diff --git a/libraries/shared/src/Shape.h b/libraries/shared/src/Shape.h index 5273c69007..5bb94d32ff 100644 --- a/libraries/shared/src/Shape.h +++ b/libraries/shared/src/Shape.h @@ -29,8 +29,8 @@ public: const glm::vec3& getPosition() const { return _position; } const glm::quat& getRotation() const { return _rotation; } - void setPosition(const glm::vec3& position) { _position = position; } - void setRotation(const glm::quat& rotation) { _rotation = rotation; } + virtual void setPosition(const glm::vec3& position) { _position = position; } + virtual void setRotation(const glm::quat& rotation) { _rotation = rotation; } protected: // these ctors are protected (used by derived classes only) diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index bef31a8e41..83b3582395 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -14,28 +14,28 @@ namespace ShapeCollider { -bool shapeShape(const Shape* shapeA, const Shape* shapeB, CollisionInfo& collision) { +bool shapeShape(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { // ATM we only have two shape types so we just check every case. // TODO: make a fast lookup for correct method if (shapeA->getType() == Shape::SPHERE_SHAPE) { const SphereShape* sphereA = static_cast(shapeA); if (shapeB->getType() == Shape::SPHERE_SHAPE) { - return sphereSphere(sphereA, static_cast(shapeB), collision); + return sphereSphere(sphereA, static_cast(shapeB), collisions); } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) { - return sphereCapsule(sphereA, static_cast(shapeB), collision); + return sphereCapsule(sphereA, static_cast(shapeB), collisions); } } else if (shapeA->getType() == Shape::CAPSULE_SHAPE) { const CapsuleShape* capsuleA = static_cast(shapeA); if (shapeB->getType() == Shape::SPHERE_SHAPE) { - return capsuleSphere(capsuleA, static_cast(shapeB), collision); + return capsuleSphere(capsuleA, static_cast(shapeB), collisions); } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) { - return capsuleCapsule(capsuleA, static_cast(shapeB), collision); + return capsuleCapsule(capsuleA, static_cast(shapeB), collisions); } } return false; } -bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionInfo& collision) { +bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionList& collisions) { glm::vec3 BA = sphereB->getPosition() - sphereA->getPosition(); float distanceSquared = glm::dot(BA, BA); float totalRadius = sphereA->getRadius() + sphereB->getRadius(); @@ -50,15 +50,18 @@ bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, Collis BA /= distance; } // penetration points from A into B - collision._penetration = BA * (totalRadius - distance); - // contactPoint is on surface of A - collision._contactPoint = sphereA->getPosition() + sphereA->getRadius() * BA; - return true; + CollisionInfo* collision = collisions.getNewCollision(); + if (collision) { + collision->_penetration = BA * (totalRadius - distance); + // contactPoint is on surface of A + collision->_contactPoint = sphereA->getPosition() + sphereA->getRadius() * BA; + return true; + } } return false; } -bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionInfo& collision) { +bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionList& collisions) { // find sphereA's closest approach to axis of capsuleB glm::vec3 BA = capsuleB->getPosition() - sphereA->getPosition(); glm::vec3 capsuleAxis; @@ -80,19 +83,29 @@ bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, Col radialDistance2 = glm::length2(radialAxis); } if (radialDistance2 > EPSILON * EPSILON) { + CollisionInfo* collision = collisions.getNewCollision(); + if (!collision) { + // collisions list is full + return false; + } // normalize the radialAxis float radialDistance = sqrtf(radialDistance2); radialAxis /= radialDistance; // penetration points from A into B - collision._penetration = (totalRadius - radialDistance) * radialAxis; // points from A into B + collision->_penetration = (totalRadius - radialDistance) * radialAxis; // points from A into B // contactPoint is on surface of sphereA - collision._contactPoint = sphereA->getPosition() + sphereA->getRadius() * radialAxis; + collision->_contactPoint = sphereA->getPosition() + sphereA->getRadius() * radialAxis; } else { // A is on B's axis, so the penetration is undefined... if (absAxialDistance > capsuleB->getHalfHeight()) { // ...for the cylinder case (for now we pretend the collision doesn't exist) return false; } + CollisionInfo* collision = collisions.getNewCollision(); + if (!collision) { + // collisions list is full + return false; + } // ... but still defined for the cap case if (axialDistance < 0.f) { // we're hitting the start cap, so we negate the capsuleAxis @@ -100,16 +113,16 @@ bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, Col } // penetration points from A into B float sign = (axialDistance > 0.f) ? -1.f : 1.f; - collision._penetration = (sign * (totalRadius + capsuleB->getHalfHeight() - absAxialDistance)) * capsuleAxis; + collision->_penetration = (sign * (totalRadius + capsuleB->getHalfHeight() - absAxialDistance)) * capsuleAxis; // contactPoint is on surface of sphereA - collision._contactPoint = sphereA->getPosition() + (sign * sphereA->getRadius()) * capsuleAxis; + collision->_contactPoint = sphereA->getPosition() + (sign * sphereA->getRadius()) * capsuleAxis; } return true; } return false; } -bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionInfo& collision) { +bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionList& collisions) { // find sphereB's closest approach to axis of capsuleA glm::vec3 AB = capsuleA->getPosition() - sphereB->getPosition(); glm::vec3 capsuleAxis; @@ -137,28 +150,38 @@ bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, Col radialDistance2 = glm::length2(radialAxis); } if (radialDistance2 > EPSILON * EPSILON) { + CollisionInfo* collision = collisions.getNewCollision(); + if (!collision) { + // collisions list is full + return false; + } // normalize the radialAxis float radialDistance = sqrtf(radialDistance2); radialAxis /= radialDistance; // penetration points from A into B - collision._penetration = (radialDistance - totalRadius) * radialAxis; // points from A into B + collision->_penetration = (radialDistance - totalRadius) * radialAxis; // points from A into B // contactPoint is on surface of capsuleA - collision._contactPoint = closestApproach - capsuleA->getRadius() * radialAxis; + collision->_contactPoint = closestApproach - capsuleA->getRadius() * radialAxis; } else { // A is on B's axis, so the penetration is undefined... if (absAxialDistance > capsuleA->getHalfHeight()) { // ...for the cylinder case (for now we pretend the collision doesn't exist) return false; } else { + CollisionInfo* collision = collisions.getNewCollision(); + if (!collision) { + // collisions list is full + return false; + } // ... but still defined for the cap case if (axialDistance < 0.f) { // we're hitting the start cap, so we negate the capsuleAxis capsuleAxis *= -1; } float sign = (axialDistance > 0.f) ? 1.f : -1.f; - collision._penetration = (sign * (totalRadius + capsuleA->getHalfHeight() - absAxialDistance)) * capsuleAxis; + collision->_penetration = (sign * (totalRadius + capsuleA->getHalfHeight() - absAxialDistance)) * capsuleAxis; // contactPoint is on surface of sphereA - collision._contactPoint = closestApproach + (sign * capsuleA->getRadius()) * capsuleAxis; + collision->_contactPoint = closestApproach + (sign * capsuleA->getRadius()) * capsuleAxis; } } return true; @@ -166,7 +189,7 @@ bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, Col return false; } -bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionInfo& collision) { +bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionList& collisions) { glm::vec3 axisA; capsuleA->computeNormalizedAxis(axisA); glm::vec3 axisB; @@ -201,6 +224,11 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, glm::vec3 BA = (centerB + distanceB * axisB) - (centerA + distanceA * axisA); float distanceSquared = glm::dot(BA, BA); if (distanceSquared < totalRadius * totalRadius) { + CollisionInfo* collision = collisions.getNewCollision(); + if (!collision) { + // collisions list is full + return false; + } // normalize BA float distance = sqrtf(distanceSquared); if (distance < EPSILON) { @@ -222,9 +250,9 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, BA /= distance; } // penetration points from A into B - collision._penetration = BA * (totalRadius - distance); + collision->_penetration = BA * (totalRadius - distance); // contactPoint is on surface of A - collision._contactPoint = centerA + distanceA * axisA + capsuleA->getRadius() * BA; + collision->_contactPoint = centerA + distanceA * axisA + capsuleA->getRadius() * BA; return true; } } else { @@ -238,6 +266,11 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, BA = BA - axialDistance * axisB; // BA now points from centerA to axisB (perp to axis) float distanceSquared = glm::length2(BA); if (distanceSquared < totalRadius * totalRadius) { + CollisionInfo* collision = collisions.getNewCollision(); + if (!collision) { + // collisions list is full + return false; + } // We have all the info we need to compute the penetration vector... // normalize BA float distance = sqrtf(distanceSquared); @@ -248,7 +281,7 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, BA /= distance; } // penetration points from A into B - collision._penetration = BA * (totalRadius - distance); + collision->_penetration = BA * (totalRadius - distance); // However we need some more world-frame info to compute the contactPoint, // which is on the surface of capsuleA... @@ -284,7 +317,7 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, } // average the internal pair, and then do the math from centerB - collision._contactPoint = centerB + (0.5f * (points[1] + points[2])) * axisB + collision->_contactPoint = centerB + (0.5f * (points[1] + points[2])) * axisB + (capsuleA->getRadius() - distance) * BA; return true; } diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h index 0eaf746595..2be804e207 100644 --- a/libraries/shared/src/ShapeCollider.h +++ b/libraries/shared/src/ShapeCollider.h @@ -20,31 +20,31 @@ namespace ShapeCollider { /// \param shapeB pointer to second shape /// \param[out] collision where to store collision details /// \return true of shapes collide - bool shapeShape(const Shape* shapeA, const Shape* shapeB, CollisionInfo& collision); + bool shapeShape(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); /// \param sphereA pointer to first shape (sphere) /// \param sphereB pointer to second shape (sphere) /// \param[out] collision where to store collision details /// \return true of shapes collide - bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionInfo& collision); + bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionList& collisions); /// \param sphereA pointer to first shape (sphere) /// \param capsuleB pointer to second shape (capsule) /// \param[out] collision where to store collision details /// \return true of shapes collide - bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionInfo& collision); + bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionList& collisions); /// \param capsuleA pointer to first shape (capsule) /// \param sphereB pointer to second shape (sphere) /// \param[out] collision where to store collision details /// \return true of shapes collide - bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionInfo& collision); + bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionList& collisions); /// \param capsuleA pointer to first shape (capsule) /// \param capsuleB pointer to second shape (capsule) /// \param[out] collision where to store collision details /// \return true of shapes collide - bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionInfo& collision); + bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionList& collisions); } // namespace ShapeCollider diff --git a/tests/physics/src/PhysicsTestUtil.cpp b/tests/physics/src/PhysicsTestUtil.cpp index f14739c07f..fb940d2043 100644 --- a/tests/physics/src/PhysicsTestUtil.cpp +++ b/tests/physics/src/PhysicsTestUtil.cpp @@ -6,6 +6,8 @@ // Copyright (c) 2014 High Fidelity, Inc. All rights reserved. // +#include + #include "PhysicsTestUtil.h" std::ostream& operator<<(std::ostream& s, const glm::vec3& v) { @@ -13,6 +15,20 @@ std::ostream& operator<<(std::ostream& s, const glm::vec3& v) { return s; } +std::ostream& operator<<(std::ostream& s, const glm::quat& q) { + s << "<" << q.x << "," << q.y << "," << q.z << "," << q.w << ">"; + return s; +} + +std::ostream& operator<<(std::ostream& s, const glm::mat4& m) { + s << "["; + for (int j = 0; j < 4; ++j) { + s << " " << m[0][j] << " " << m[1][j] << " " << m[2][j] << " " << m[3][j] << ";"; + } + s << " ]"; + return s; +} + std::ostream& operator<<(std::ostream& s, const CollisionInfo& c) { s << "[penetration=" << c._penetration << ", contactPoint=" << c._contactPoint diff --git a/tests/physics/src/PhysicsTestUtil.h b/tests/physics/src/PhysicsTestUtil.h index f63e7e5910..dcbeaed346 100644 --- a/tests/physics/src/PhysicsTestUtil.h +++ b/tests/physics/src/PhysicsTestUtil.h @@ -21,6 +21,8 @@ const glm::vec3 zAxis(0.f, 0.f, 1.f); const float rightAngle = 90.f; // degrees std::ostream& operator<<(std::ostream& s, const glm::vec3& v); +std::ostream& operator<<(std::ostream& s, const glm::quat& q); +std::ostream& operator<<(std::ostream& s, const glm::mat4& m); std::ostream& operator<<(std::ostream& s, const CollisionInfo& c); #endif // __tests__PhysicsTestUtil__ diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index ba648995c0..d5cb74dd87 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -34,11 +34,11 @@ void ShapeColliderTests::sphereMissesSphere() { SphereShape sphereA(radiusA, origin); SphereShape sphereB(radiusB, offsetDistance * offsetDirection); - CollisionInfo collision; + CollisionList collisions(16); // collide A to B... { - bool touching = ShapeCollider::shapeShape(&sphereA, &sphereB, collision); + bool touching = ShapeCollider::shapeShape(&sphereA, &sphereB, collisions); if (touching) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphereA and sphereB should NOT touch" << std::endl; @@ -47,7 +47,7 @@ void ShapeColliderTests::sphereMissesSphere() { // collide B to A... { - bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collision); + bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collisions); if (touching) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphereA and sphereB should NOT touch" << std::endl; @@ -56,12 +56,18 @@ void ShapeColliderTests::sphereMissesSphere() { // also test shapeShape { - bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collision); + bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collisions); if (touching) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphereA and sphereB should NOT touch" << std::endl; } } + + if (collisions.size() > 0) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: expected empty collision list but size is " << collisions.size() + << std::endl; + } } void ShapeColliderTests::sphereTouchesSphere() { @@ -77,62 +83,80 @@ void ShapeColliderTests::sphereTouchesSphere() { SphereShape sphereA(radiusA, origin); SphereShape sphereB(radiusB, offsetDistance * offsetDirection); - CollisionInfo collision; + CollisionList collisions(16); + int numCollisions = 0; // collide A to B... { - bool touching = ShapeCollider::shapeShape(&sphereA, &sphereB, collision); + bool touching = ShapeCollider::shapeShape(&sphereA, &sphereB, collisions); if (!touching) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphereA and sphereB should touch" << std::endl; + } else { + ++numCollisions; + } + + // verify state of collisions + if (numCollisions != collisions.size()) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: expected collisions size of " << numCollisions << " but actual size is " << collisions.size() + << std::endl; + } + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); + if (!collision) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: null collision" << std::endl; } // penetration points from sphereA into sphereB - float inaccuracy = glm::length(collision._penetration - expectedPenetration); + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } // contactPoint is on surface of sphereA glm::vec3 AtoB = sphereB.getPosition() - sphereA.getPosition(); glm::vec3 expectedContactPoint = sphereA.getPosition() + radiusA * glm::normalize(AtoB); - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } } // collide B to A... { - bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collision); + bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collisions); if (!touching) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphereA and sphereB should touch" << std::endl; + } else { + ++numCollisions; } // penetration points from sphereA into sphereB - float inaccuracy = glm::length(collision._penetration + expectedPenetration); + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); + float inaccuracy = glm::length(collision->_penetration + expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } // contactPoint is on surface of sphereA glm::vec3 BtoA = sphereA.getPosition() - sphereB.getPosition(); glm::vec3 expectedContactPoint = sphereB.getPosition() + radiusB * glm::normalize(BtoA); - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } } @@ -158,6 +182,8 @@ void ShapeColliderTests::sphereMissesCapsule() { capsuleB.setRotation(rotation); capsuleB.setPosition(translation); + CollisionList collisions(16); + // walk sphereA along the local yAxis next to, but not touching, capsuleB glm::vec3 localStartPosition(radialOffset, axialOffset, 0.f); int numberOfSteps = 10; @@ -167,9 +193,8 @@ void ShapeColliderTests::sphereMissesCapsule() { glm::vec3 localPosition = localStartPosition + (float(i) * delta) * yAxis; sphereA.setPosition(rotation * localPosition + translation); - CollisionInfo collision; // sphereA agains capsuleB - if (ShapeCollider::shapeShape(&sphereA, &capsuleB, collision)) + if (ShapeCollider::shapeShape(&sphereA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere and capsule should NOT touch" @@ -177,13 +202,19 @@ void ShapeColliderTests::sphereMissesCapsule() { } // capsuleB against sphereA - if (ShapeCollider::shapeShape(&capsuleB, &sphereA, collision)) + if (ShapeCollider::shapeShape(&capsuleB, &sphereA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere and capsule should NOT touch" << std::endl; } } + + if (collisions.size() > 0) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: expected empty collision list but size is " << collisions.size() + << std::endl; + } } void ShapeColliderTests::sphereTouchesCapsule() { @@ -200,52 +231,60 @@ void ShapeColliderTests::sphereTouchesCapsule() { SphereShape sphereA(radiusA); CapsuleShape capsuleB(radiusB, halfHeightB); - CollisionInfo collision; + CollisionList collisions(16); + int numCollisions = 0; + { // sphereA collides with capsuleB's cylindrical wall sphereA.setPosition(radialOffset * xAxis); - if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collision)) + if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere and capsule should touch" << std::endl; + } else { + ++numCollisions; } // penetration points from sphereA into capsuleB + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); glm::vec3 expectedPenetration = (radialOffset - totalRadius) * xAxis; - float inaccuracy = glm::length(collision._penetration - expectedPenetration); + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } // contactPoint is on surface of sphereA glm::vec3 expectedContactPoint = sphereA.getPosition() - radiusA * xAxis; - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } // capsuleB collides with sphereA - if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collision)) + if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and sphere should touch" << std::endl; + } else { + ++numCollisions; } // penetration points from sphereA into capsuleB + collision = collisions.getCollision(numCollisions - 1); expectedPenetration = - (radialOffset - totalRadius) * xAxis; - inaccuracy = glm::length(collision._penetration - expectedPenetration); + inaccuracy = glm::length(collision->_penetration - expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } @@ -253,11 +292,11 @@ void ShapeColliderTests::sphereTouchesCapsule() { glm::vec3 BtoA = sphereA.getPosition() - capsuleB.getPosition(); glm::vec3 closestApproach = capsuleB.getPosition() + glm::dot(BtoA, yAxis) * yAxis; expectedContactPoint = closestApproach + radiusB * glm::normalize(BtoA - closestApproach); - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } } @@ -265,48 +304,54 @@ void ShapeColliderTests::sphereTouchesCapsule() { glm::vec3 axialOffset = (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis; sphereA.setPosition(axialOffset * yAxis); - if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collision)) + if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere and capsule should touch" << std::endl; + } else { + ++numCollisions; } // penetration points from sphereA into capsuleB + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); glm::vec3 expectedPenetration = - ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis; - float inaccuracy = glm::length(collision._penetration - expectedPenetration); + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } // contactPoint is on surface of sphereA glm::vec3 expectedContactPoint = sphereA.getPosition() - radiusA * yAxis; - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } // capsuleB collides with sphereA - if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collision)) + if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and sphere should touch" << std::endl; + } else { + ++numCollisions; } // penetration points from sphereA into capsuleB + collision = collisions.getCollision(numCollisions - 1); expectedPenetration = ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis; - inaccuracy = glm::length(collision._penetration - expectedPenetration); + inaccuracy = glm::length(collision->_penetration - expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } @@ -314,11 +359,11 @@ void ShapeColliderTests::sphereTouchesCapsule() { glm::vec3 endPoint; capsuleB.getEndPoint(endPoint); expectedContactPoint = endPoint + radiusB * yAxis; - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } } @@ -326,48 +371,54 @@ void ShapeColliderTests::sphereTouchesCapsule() { glm::vec3 axialOffset = - (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis; sphereA.setPosition(axialOffset * yAxis); - if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collision)) + if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere and capsule should touch" << std::endl; + } else { + ++numCollisions; } // penetration points from sphereA into capsuleB + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); glm::vec3 expectedPenetration = ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis; - float inaccuracy = glm::length(collision._penetration - expectedPenetration); + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } // contactPoint is on surface of sphereA glm::vec3 expectedContactPoint = sphereA.getPosition() + radiusA * yAxis; - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } // capsuleB collides with sphereA - if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collision)) + if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and sphere should touch" << std::endl; + } else { + ++numCollisions; } // penetration points from sphereA into capsuleB + collision = collisions.getCollision(numCollisions - 1); expectedPenetration = - ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis; - inaccuracy = glm::length(collision._penetration - expectedPenetration); + inaccuracy = glm::length(collision->_penetration - expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } @@ -375,14 +426,19 @@ void ShapeColliderTests::sphereTouchesCapsule() { glm::vec3 startPoint; capsuleB.getStartPoint(startPoint); expectedContactPoint = startPoint - radiusB * yAxis; - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } } + if (collisions.size() != numCollisions) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: expected " << numCollisions << " collisions but actual number is " << collisions.size() + << std::endl; + } } void ShapeColliderTests::capsuleMissesCapsule() { @@ -398,16 +454,17 @@ void ShapeColliderTests::capsuleMissesCapsule() { CapsuleShape capsuleA(radiusA, halfHeightA); CapsuleShape capsuleB(radiusA, halfHeightA); + CollisionList collisions(16); + // side by side capsuleB.setPosition((1.01f * totalRadius) * xAxis); - CollisionInfo collision; - if (ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision)) + if (ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should NOT touch" << std::endl; } - if (ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision)) + if (ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should NOT touch" @@ -416,13 +473,13 @@ void ShapeColliderTests::capsuleMissesCapsule() { // end to end capsuleB.setPosition((1.01f * totalHalfLength) * xAxis); - if (ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision)) + if (ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should NOT touch" << std::endl; } - if (ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision)) + if (ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should NOT touch" @@ -433,18 +490,24 @@ void ShapeColliderTests::capsuleMissesCapsule() { glm::quat rotation = glm::angleAxis(rightAngle, zAxis); capsuleB.setRotation(rotation); capsuleB.setPosition((1.01f * (totalRadius + capsuleB.getHalfHeight())) * xAxis); - if (ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision)) + if (ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should NOT touch" << std::endl; } - if (ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision)) + if (ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should NOT touch" << std::endl; } + + if (collisions.size() > 0) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: expected empty collision list but size is " << collisions.size() + << std::endl; + } } void ShapeColliderTests::capsuleTouchesCapsule() { @@ -460,38 +523,47 @@ void ShapeColliderTests::capsuleTouchesCapsule() { CapsuleShape capsuleA(radiusA, halfHeightA); CapsuleShape capsuleB(radiusB, halfHeightB); - CollisionInfo collision; + CollisionList collisions(16); + int numCollisions = 0; { // side by side capsuleB.setPosition((0.99f * totalRadius) * xAxis); - if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision)) + if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" << std::endl; + } else { + ++numCollisions; } - if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision)) + if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" << std::endl; + } else { + ++numCollisions; } } { // end to end capsuleB.setPosition((0.99f * totalHalfLength) * yAxis); - if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision)) + if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" << std::endl; + } else { + ++numCollisions; } - if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision)) + if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" << std::endl; + } else { + ++numCollisions; } } @@ -500,17 +572,21 @@ void ShapeColliderTests::capsuleTouchesCapsule() { capsuleB.setRotation(rotation); capsuleB.setPosition((0.99f * (totalRadius + capsuleB.getHalfHeight())) * xAxis); - if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision)) + if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" << std::endl; + } else { + ++numCollisions; } - if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision)) + if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" << std::endl; + } else { + ++numCollisions; } } @@ -522,54 +598,60 @@ void ShapeColliderTests::capsuleTouchesCapsule() { capsuleB.setPosition(positionB); // capsuleA vs capsuleB - if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision)) + if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" << std::endl; + } else { + ++numCollisions; } + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); glm::vec3 expectedPenetration = overlap * xAxis; - float inaccuracy = glm::length(collision._penetration - expectedPenetration); + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } glm::vec3 expectedContactPoint = capsuleA.getPosition() + radiusA * xAxis; - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } // capsuleB vs capsuleA - if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collision)) + if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" << std::endl; + } else { + ++numCollisions; } + collision = collisions.getCollision(numCollisions - 1); expectedPenetration = - overlap * xAxis; - inaccuracy = glm::length(collision._penetration - expectedPenetration); + inaccuracy = glm::length(collision->_penetration - expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } expectedContactPoint = capsuleB.getPosition() - (radiusB + halfHeightB) * xAxis; - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } } @@ -583,28 +665,31 @@ void ShapeColliderTests::capsuleTouchesCapsule() { capsuleB.setPosition(positionB); // capsuleA vs capsuleB - if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collision)) + if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: capsule and capsule should touch" << std::endl; + } else { + ++numCollisions; } + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); glm::vec3 expectedPenetration = overlap * zAxis; - float inaccuracy = glm::length(collision._penetration - expectedPenetration); + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad penetration: expected = " << expectedPenetration - << " actual = " << collision._penetration + << " actual = " << collision->_penetration << std::endl; } glm::vec3 expectedContactPoint = capsuleA.getPosition() + radiusA * zAxis + shift * yAxis; - inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: bad contactPoint: expected = " << expectedContactPoint - << " actual = " << collision._contactPoint + << " actual = " << collision->_contactPoint << std::endl; } }