diff --git a/libraries/shared/src/CapsuleShape.cpp b/libraries/shared/src/CapsuleShape.cpp index 68e0acf561..c71f03aa47 100644 --- a/libraries/shared/src/CapsuleShape.cpp +++ b/libraries/shared/src/CapsuleShape.cpp @@ -6,6 +6,7 @@ // Copyright (c) 2014 High Fidelity, Inc. All rights reserved. // +#include #include #include "CapsuleShape.h" @@ -13,6 +14,7 @@ // default axis of CapsuleShape is Y-axis +const glm::vec3 localAxis(0.f, 1.f, 0.f); CapsuleShape::CapsuleShape() : Shape(Shape::CAPSULE_SHAPE) {} @@ -43,6 +45,21 @@ CapsuleShape::CapsuleShape(float radius, const glm::vec3& startPoint, const glm: updateBoundingRadius(); } +/// \param[out] startPoint is the center of start cap +void CapsuleShape::getStartPoint(glm::vec3& startPoint) const { + startPoint = getPosition() - _halfHeight * (_rotation * glm::vec3(0.f, 1.f, 0.f)); +} + +/// \param[out] endPoint is the center of the end cap +void CapsuleShape::getEndPoint(glm::vec3& endPoint) const { + endPoint = getPosition() + _halfHeight * (_rotation * glm::vec3(0.f, 1.f, 0.f)); +} + +void CapsuleShape::computeNormalizedAxis(glm::vec3& axis) const { + // default axis of a capsule is along the yAxis + axis = _rotation * glm::vec3(0.f, 1.f, 0.f); +} + void CapsuleShape::setRadius(float radius) { _radius = radius; updateBoundingRadius(); diff --git a/libraries/shared/src/CapsuleShape.h b/libraries/shared/src/CapsuleShape.h index 726d880c37..6d7e0a50be 100644 --- a/libraries/shared/src/CapsuleShape.h +++ b/libraries/shared/src/CapsuleShape.h @@ -11,6 +11,7 @@ #include "Shape.h" +// adebug bookmark TODO: convert to new world-frame approach // default axis of CapsuleShape is Y-axis class CapsuleShape : public Shape { @@ -23,6 +24,14 @@ public: float getRadius() const { return _radius; } float getHalfHeight() const { return _halfHeight; } + /// \param[out] startPoint is the center of start cap + void getStartPoint(glm::vec3& startPoint) const; + + /// \param[out] endPoint is the center of the end cap + void getEndPoint(glm::vec3& endPoint) const; + + void computeNormalizedAxis(glm::vec3& axis) const; + void setRadius(float radius); void setHalfHeight(float height); void setRadiusAndHalfHeight(float radius, float height); diff --git a/libraries/shared/src/CollisionInfo.cpp b/libraries/shared/src/CollisionInfo.cpp index de233d9420..9e76ca59f6 100644 --- a/libraries/shared/src/CollisionInfo.cpp +++ b/libraries/shared/src/CollisionInfo.cpp @@ -8,17 +8,6 @@ #include "CollisionInfo.h" -void CollisionInfo::rotateThenTranslate(const glm::quat& rotation, const glm::vec3& translation) { - _contactPoint = translation + rotation * _contactPoint; - _penetration = rotation * _penetration; - _addedVelocity = rotation * _addedVelocity; -} - -void CollisionInfo::translateThenRotate(const glm::vec3& translation, const glm::quat& rotation) { - _contactPoint = rotation * (_contactPoint + translation); - _penetration = rotation * _penetration; - _addedVelocity = rotation * _addedVelocity; -} CollisionList::CollisionList(int maxSize) : _maxSize(maxSize), diff --git a/libraries/shared/src/CollisionInfo.h b/libraries/shared/src/CollisionInfo.h index b71d67e532..629cb6b8f2 100644 --- a/libraries/shared/src/CollisionInfo.h +++ b/libraries/shared/src/CollisionInfo.h @@ -56,12 +56,6 @@ public: ~CollisionInfo() {} - /// Rotate and translate the details. - void rotateThenTranslate(const glm::quat& rotation, const glm::vec3& translation); - - /// Translate then rotate the details - void translateThenRotate(const glm::vec3& translation, const glm::quat& rotation); - qint32 _type; // type of Collision (will determine what is supposed to be in _data and _flags) void* _data; // pointer to user supplied data quint32 _flags; // 32 bits for whatever diff --git a/libraries/shared/src/Shape.h b/libraries/shared/src/Shape.h index 0e6d17c059..5273c69007 100644 --- a/libraries/shared/src/Shape.h +++ b/libraries/shared/src/Shape.h @@ -1,6 +1,5 @@ // // Shape.h -// hifi // // Created by Andrew Meadows on 2014.02.20 // Copyright (c) 2014 High Fidelity, Inc. All rights reserved. @@ -20,6 +19,7 @@ public: SPHERE_SHAPE, CAPSULE_SHAPE, BOX_SHAPE, + LIST_SHAPE }; Shape() : _type(UNKNOWN_SHAPE), _boundingRadius(0.f), _position(0.f), _rotation() { } @@ -33,8 +33,12 @@ public: void setRotation(const glm::quat& rotation) { _rotation = rotation; } protected: + // these ctors are protected (used by derived classes only) Shape(Type type) : _type(type), _boundingRadius(0.f), _position(0.f), _rotation() {} + Shape(Type type, const glm::vec3& position) + : _type(type), _boundingRadius(0.f), _position(position), _rotation() {} + Shape(Type type, const glm::vec3& position, const glm::quat& rotation) : _type(type), _boundingRadius(0.f), _position(position), _rotation(rotation) {} diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index f3598ebf39..c5d497694e 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -6,42 +6,37 @@ // Copyright (c) 2014 High Fidelity, Inc. All rights reserved. // +#include + +#include + #include "ShapeCollider.h" namespace ShapeCollider { -bool shapeShape(const Shape* shapeA, const Shape* shapeB, - const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) { +bool shapeShape(const Shape* shapeA, const Shape* shapeB, CollisionInfo& collision) { // 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), - rotationAB, offsetA, collision); + return sphereSphere(sphereA, static_cast(shapeB), collision); } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) { - return sphereCapsule(sphereA, static_cast(shapeB), - rotationAB, offsetA, collision); + return sphereCapsule(sphereA, static_cast(shapeB), collision); } } 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), - rotationAB, offsetA, collision); + return capsuleSphere(capsuleA, static_cast(shapeB), collision); } else if (shapeB->getType() == Shape::CAPSULE_SHAPE) { - return capsuleCapsule(capsuleA, static_cast(shapeB), - rotationAB, offsetA, collision); + return capsuleCapsule(capsuleA, static_cast(shapeB), collision); } } return false; } -bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, - const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) { - // A in B's frame - glm::vec3 A = rotationAB * sphereA->getPosition() + offsetA; - // BA = B - A = from A to B, in B's frame - glm::vec3 BA = sphereB->getPosition() - A; +bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionInfo& collision) { + glm::vec3 BA = sphereB->getPosition() - sphereA->getPosition(); float distanceSquared = glm::dot(BA, BA); float totalRadius = sphereA->getRadius() + sphereB->getRadius(); if (distanceSquared < totalRadius * totalRadius) { @@ -53,103 +48,124 @@ bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, } else { BA /= distance; } - // store the collision details in B's frame + // penetration points from A into B collision._penetration = BA * (totalRadius - distance); - collision._contactPoint = A + sphereA->getRadius() * BA; + // contactPoint is on surface of A + collision._contactPoint = sphereA->getPosition() + sphereA->getRadius() * BA; return true; } return false; } -// everything in the capsule's natural frame (where its axis is along yAxis) -bool sphereCapsuleHelper(float sphereRadius, const glm::vec3 sphereCenter, - const CapsuleShape* capsule, CollisionInfo& collision) { - float xzSquared = sphereCenter.x * sphereCenter.x + sphereCenter.y * sphereCenter.y; - float totalRadius = sphereRadius + capsule->getRadius(); - if (xzSquared < totalRadius * totalRadius) { - float fabsY = fabs(sphereCenter.y); - float halfHeight = capsule->getHalfHeight(); - if (fabsY < halfHeight) { - // sphere collides with cylindrical wall - glm::vec3 BA = -sphereCenter; - BA.y = 0.f; - float distance = sqrtf(xzSquared); - if (distance < EPSILON) { - // for now we don't handle this singular case +bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionInfo& collision) { + // find sphereA's closest approach to axis of capsuleB + glm::vec3 BA = capsuleB->getPosition() - sphereA->getPosition(); + glm::vec3 capsuleAxis; + capsuleB->computeNormalizedAxis(capsuleAxis); + float axialDistance = - glm::dot(BA, capsuleAxis); + float absAxialDistance = fabs(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 + float radialDistance2 = glm::length2(radialAxis); + if (radialDistance2 > totalRadius * totalRadius) { + // sphere is too far from capsule axis + return false; + } + if (absAxialDistance > capsuleB->getHalfHeight()) { + // sphere hits capsule on a cap --> recompute radialAxis to point from spherA to cap center + float sign = (axialDistance > 0.f) ? 1.f : -1.f; + radialAxis = BA + (sign * capsuleB->getHalfHeight()) * capsuleAxis; + radialDistance2 = glm::length2(radialAxis); + } + if (radialDistance2 > EPSILON * EPSILON) { + // 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 + // contactPoint is on surface of sphereA + 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; } - BA /= distance; - collision._penetration = BA * (totalRadius - distance); - collision._contactPoint = BA * (sphereRadius - totalRadius + distance); - collision._contactPoint.y = fabsY; - if (sphereCenter.y < 0.f) { - // negate the y elements of the collision info - collision._penetration.y *= -1.f; - collision._contactPoint.y *= -1.f; + // ... but still defined for the cap case + if (axialDistance < 0.f) { + // we're hitting the start cap, so we negate the capsuleAxis + capsuleAxis *= -1; } - return true; + // penetration points from A into B + float sign = (axialDistance > 0.f) ? -1.f : 1.f; + collision._penetration = (sign * (totalRadius + capsuleB->getHalfHeight() - absAxialDistance)) * capsuleAxis; + // contactPoint is on surface of sphereA + collision._contactPoint = sphereA->getPosition() + (sign * sphereA->getRadius()) * capsuleAxis; + } + return true; + } + return false; +} + +bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionInfo& collision) { + // find sphereB's closest approach to axis of capsuleA + glm::vec3 AB = capsuleA->getPosition() - sphereB->getPosition(); + glm::vec3 capsuleAxis; + capsuleA->computeNormalizedAxis(capsuleAxis); + float axialDistance = - glm::dot(AB, capsuleAxis); + float absAxialDistance = fabs(axialDistance); + float totalRadius = sphereB->getRadius() + capsuleA->getRadius(); + if (absAxialDistance < totalRadius + capsuleA->getHalfHeight()) { + glm::vec3 radialAxis = AB + axialDistance * capsuleAxis; // from sphereB to axis of capsuleA + float radialDistance2 = glm::length2(radialAxis); + if (radialDistance2 > totalRadius * totalRadius) { + // sphere is too far from capsule axis + return false; + } + + // closestApproach = point on capsuleA's axis that is closest to sphereB's center + glm::vec3 closestApproach = capsuleA->getPosition() + axialDistance * capsuleAxis; + + if (absAxialDistance > capsuleA->getHalfHeight()) { + // sphere hits capsule on a cap + // --> recompute radialAxis and closestApproach + float sign = (axialDistance > 0.f) ? 1.f : -1.f; + closestApproach = capsuleA->getPosition() + (sign * capsuleA->getHalfHeight()) * capsuleAxis; + radialAxis = closestApproach - sphereB->getPosition(); + radialDistance2 = glm::length2(radialAxis); + } + if (radialDistance2 > EPSILON * EPSILON) { + // 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 + // contactPoint is on surface of capsuleA + collision._contactPoint = closestApproach - capsuleA->getRadius() * radialAxis; } else { - // tansform into frame where cap is at origin - float newY = fabsY - halfHeight; - float distance = sqrtf(newY * newY + xzSquared); - if (distance < totalRadius) { - // sphere collides with cap - - // BA points from sphere to cap - glm::vec3 BA(-sphereCenter.x, newY, -sphereCenter.z); - if (distance < EPSILON) { - // for now we don't handle this singular case - return false; + // 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 { + // ... but still defined for the cap case + if (axialDistance < 0.f) { + // we're hitting the start cap, so we negate the capsuleAxis + capsuleAxis *= -1; } - BA /= distance; - collision._penetration = BA * (totalRadius - distance); - collision._contactPoint = BA * (sphereRadius - totalRadius + distance); - collision._contactPoint.y += halfHeight; - - if (sphereCenter.y < 0.f) { - // negate the y elements of the collision info - collision._penetration.y *= -1.f; - collision._contactPoint.y *= -1.f; - } - return true; + float sign = (axialDistance > 0.f) ? 1.f : -1.f; + collision._penetration = (sign * (totalRadius + capsuleA->getHalfHeight() - absAxialDistance)) * capsuleAxis; + // contactPoint is on surface of sphereA + collision._contactPoint = closestApproach + (sign * capsuleA->getRadius()) * capsuleAxis; } } - } - return false; -} - -bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, - const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) { - // transform sphereA all the way to capsuleB's natural frame - glm::quat rotationB = capsuleB->getRotation(); - glm::vec3 sphereCenter = rotationB * (offsetA + rotationAB * sphereA->getPosition() - capsuleB->getPosition()); - if (sphereCapsuleHelper(sphereA->getRadius(), sphereCenter, capsuleB, collision)) { - // need to transform collision details back into B's offset frame - collision.rotateThenTranslate(glm::inverse(rotationB), capsuleB->getPosition()); return true; } return false; } -bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, - const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) { - // transform sphereB all the way to capsuleA's natural frame - glm::quat rotationBA = glm::inverse(rotationAB); - glm::quat rotationA = capsuleA->getRotation(); - glm::vec3 offsetB = rotationBA * (-offsetA); - glm::vec3 sphereCenter = rotationA * (offsetB + rotationBA * sphereB->getPosition() - capsuleA->getPosition()); - if (sphereCapsuleHelper(sphereB->getRadius(), sphereCenter, capsuleA, collision)) { - // need to transform collision details back into B's offset frame - // BUG: these back translations are probably not right - collision.rotateThenTranslate(glm::inverse(rotationA), capsuleA->getPosition()); - collision.rotateThenTranslate(glm::inverse(rotationAB), -offsetA); - return true; - } - return false; -} - -bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, - const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision) { +bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionInfo& collision) { return false; } diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h index 4cef6f943f..0eaf746595 100644 --- a/libraries/shared/src/ShapeCollider.h +++ b/libraries/shared/src/ShapeCollider.h @@ -16,50 +16,35 @@ namespace ShapeCollider { -/// \param shapeA pointer to first shape -/// \param shapeB pointer to second shape -/// \param rotationAB rotation from A into reference frame of B -/// \param offsetA offset of A (in B's frame) -/// \param[out] collision where to store collision details -/// \return true of shapes collide -bool shapeShape(const Shape* shapeA, const Shape* shapeB, - const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision); + /// \param shapeA pointer to first shape + /// \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); -/// \param sphereA pointer to first shape (sphere) -/// \param sphereB pointer to second shape (sphere) -/// \param rotationAB rotation from A into reference frame of B -/// \param offsetA offset of A (in B's frame) -/// \param[out] collision where to store collision details -/// \return true of shapes collide -bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, - const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision); + /// \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); -/// \param sphereA pointer to first shape (sphere) -/// \param capsuleB pointer to second shape (capsule) -/// \param rotationAB rotation from A into reference frame of B -/// \param offsetA offset of A (in B's frame) -/// \param[out] collision where to store collision details -/// \return true of shapes collide -bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, - const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision); + /// \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); -/// \param capsuleA pointer to first shape (capsule) -/// \param sphereB pointer to second shape (sphere) -/// \param rotationAB rotation from A into reference frame of B -/// \param offsetA offset of A (in B's frame) -/// \param[out] collision where to store collision details -/// \return true of shapes collide -bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, - const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision); + /// \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); -/// \param capsuleA pointer to first shape (capsule) -/// \param capsuleB pointer to second shape (capsule) -/// \param rotationAB rotation from A into reference frame of B -/// \param offsetA offset of A (in B's frame) -/// \param[out] collision where to store collision details -/// \return true of shapes collide -bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, - const glm::quat& rotationAB, const glm::vec3& offsetA, CollisionInfo& collision); + /// \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); } // namespace ShapeCollider diff --git a/libraries/shared/src/SphereShape.h b/libraries/shared/src/SphereShape.h index a1028956ee..d720dd2289 100644 --- a/libraries/shared/src/SphereShape.h +++ b/libraries/shared/src/SphereShape.h @@ -15,9 +15,12 @@ class SphereShape : public Shape { public: SphereShape() : Shape(Shape::SPHERE_SHAPE) {} - SphereShape(float radius, const glm::vec3& position) : Shape(Shape::SPHERE_SHAPE) { + SphereShape(float radius) : Shape(Shape::SPHERE_SHAPE) { + _boundingRadius = radius; + } + + SphereShape(float radius, const glm::vec3& position) : Shape(Shape::SPHERE_SHAPE, position) { _boundingRadius = radius; - setPosition(position); } float getRadius() const { return _boundingRadius; } diff --git a/tests/physics/src/CollisionInfoTests.cpp b/tests/physics/src/CollisionInfoTests.cpp index 0355e2d27b..813100944b 100644 --- a/tests/physics/src/CollisionInfoTests.cpp +++ b/tests/physics/src/CollisionInfoTests.cpp @@ -18,6 +18,7 @@ #include "PhysicsTestUtil.h" +/* void CollisionInfoTests::rotateThenTranslate() { CollisionInfo collision; collision._penetration = xAxis; @@ -95,8 +96,9 @@ void CollisionInfoTests::translateThenRotate() { << std::endl; } } +*/ void CollisionInfoTests::runAllTests() { - CollisionInfoTests::rotateThenTranslate(); - CollisionInfoTests::translateThenRotate(); +// CollisionInfoTests::rotateThenTranslate(); +// CollisionInfoTests::translateThenRotate(); } diff --git a/tests/physics/src/CollisionInfoTests.h b/tests/physics/src/CollisionInfoTests.h index cc0ef337a2..51579e8f11 100644 --- a/tests/physics/src/CollisionInfoTests.h +++ b/tests/physics/src/CollisionInfoTests.h @@ -11,8 +11,8 @@ namespace CollisionInfoTests { - void rotateThenTranslate(); - void translateThenRotate(); +// void rotateThenTranslate(); +// void translateThenRotate(); void runAllTests(); } diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index b964e2f6de..8380bcf0a7 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -22,33 +22,384 @@ const glm::vec3 origin(0.f); -void ShapeColliderTests::sphereSphere() { +void ShapeColliderTests::sphereMissesSphere() { + // non-overlapping spheres of unequal size + float radiusA = 7.f; + float radiusB = 3.f; + float alpha = 1.2f; + float beta = 1.3f; + glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.f, 2.f, 3.f)); + float offsetDistance = alpha * radiusA + beta * radiusB; + float expectedPenetrationDistance = 0.f; + + SphereShape sphereA(radiusA, origin); + SphereShape sphereB(radiusB, offsetDistance * offsetDirection); CollisionInfo collision; - float radius = 2.f; - SphereShape sphereA(radius, origin); - SphereShape sphereB(radius, origin); - - glm::vec3 translation(0.5f * radius, 0.f, 0.f); - glm::quat rotation; - - // these spheres should be touching - bool touching = ShapeCollider::sphereSphere(&sphereA, &sphereB, rotation, translation, collision); - if (!touching) { - std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: sphereA and sphereB do not touch" << std::endl; + // collide A to B... + { + bool touching = ShapeCollider::shapeShape(&sphereA, &sphereB, collision); + if (touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB should NOT touch" << std::endl; + } } - // TODO: verify the collision info is good... - // penetration should point from A into B + // collide B to A... + { + bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collision); + if (touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB should NOT touch" << std::endl; + } + } + + // also test shapeShape + { + bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collision); + if (touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB should NOT touch" << std::endl; + } + } } -/* -void ShapeColliderTests::test2() { +void ShapeColliderTests::sphereTouchesSphere() { + // overlapping spheres of unequal size + float radiusA = 7.f; + float radiusB = 3.f; + float alpha = 0.2f; + float beta = 0.3f; + glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.f, 2.f, 3.f)); + float offsetDistance = alpha * radiusA + beta * radiusB; + float expectedPenetrationDistance = (1.f - alpha) * radiusA + (1.f - beta) * radiusB; + glm::vec3 expectedPenetration = expectedPenetrationDistance * offsetDirection; + + SphereShape sphereA(radiusA, origin); + SphereShape sphereB(radiusB, offsetDistance * offsetDirection); + CollisionInfo collision; + + // collide A to B... + { + bool touching = ShapeCollider::shapeShape(&sphereA, &sphereB, collision); + if (!touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB should touch" << std::endl; + } + + // penetration points from sphereA into sphereB + float inaccuracy = glm::length(collision._penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " 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); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision._contactPoint + << std::endl; + } + } + + // collide B to A... + { + bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collision); + if (!touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB should touch" << std::endl; + } + + // penetration points from sphereA into sphereB + float inaccuracy = glm::length(collision._penetration + expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " 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); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision._contactPoint + << std::endl; + } + } +} + +void ShapeColliderTests::sphereMissesCapsule() { + // non-overlapping sphere and capsule + float radiusA = 1.5f; + float radiusB = 2.3f; + float totalRadius = radiusA + radiusB; + float halfHeightB = 1.7f; + float axialOffset = totalRadius + 1.1f * halfHeightB; + float radialOffset = 1.2f * radiusA + 1.3f * radiusB; + + SphereShape sphereA(radiusA); + CapsuleShape capsuleB(radiusB, halfHeightB); + + // give the capsule some arbirary transform + float angle = 37.8; + glm::vec3 axis = glm::normalize( glm::vec3(-7.f, 2.8f, 9.3f) ); + glm::quat rotation = glm::angleAxis(angle, axis); + glm::vec3 translation(15.1f, -27.1f, -38.6f); + capsuleB.setRotation(rotation); + capsuleB.setPosition(translation); + + // walk sphereA along the local yAxis next to, but not touching, capsuleB + glm::vec3 localStartPosition(radialOffset, axialOffset, 0.f); + int numberOfSteps = 10; + float delta = 1.3f * (totalRadius + halfHeightB) / (numberOfSteps - 1); + for (int i = 0; i < numberOfSteps; ++i) { + // translate sphereA into world-frame + glm::vec3 localPosition = localStartPosition + (float(i) * delta) * yAxis; + sphereA.setPosition(rotation * localPosition + translation); + + CollisionInfo collision; + // sphereA agains capsuleB + if (ShapeCollider::shapeShape(&sphereA, &capsuleB, collision)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphere and capsule should NOT touch" + << std::endl; + } + + // capsuleB against sphereA + if (ShapeCollider::shapeShape(&capsuleB, &sphereA, collision)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphere and capsule should NOT touch" + << std::endl; + } + } +} + +void ShapeColliderTests::sphereTouchesCapsule() { + // overlapping sphere and capsule + float radiusA = 2.f; + float radiusB = 1.f; + float totalRadius = radiusA + radiusB; + float halfHeightB = 2.f; + float alpha = 0.5f; + float beta = 0.5f; + float axialOffset = 0.f; + float radialOffset = alpha * radiusA + beta * radiusB; + + SphereShape sphereA(radiusA); + CapsuleShape capsuleB(radiusB, halfHeightB); + + CollisionInfo collision; + { // sphereA collides with capsuleB's cylindrical wall + sphereA.setPosition(radialOffset * xAxis); + + if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collision)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphere and capsule should touch" + << std::endl; + } + + // penetration points from sphereA into capsuleB + glm::vec3 expectedPenetration = (radialOffset - totalRadius) * xAxis; + float inaccuracy = glm::length(collision._penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision._penetration + << std::endl; + } + + // contactPoint is on surface of sphereA + glm::vec3 expectedContactPoint = sphereA.getPosition() - radiusA * xAxis; + inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision._contactPoint + << std::endl; + } + + // capsuleB collides with sphereA + if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collision)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and sphere should touch" + << std::endl; + } + + // penetration points from sphereA into capsuleB + expectedPenetration = - (radialOffset - totalRadius) * xAxis; + inaccuracy = glm::length(collision._penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision._penetration + << std::endl; + } + + // contactPoint is on surface of capsuleB + 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); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision._contactPoint + << std::endl; + } + } + { // sphereA hits end cap at axis + glm::vec3 axialOffset = (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis; + sphereA.setPosition(axialOffset * yAxis); + + if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collision)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphere and capsule should touch" + << std::endl; + } + + // penetration points from sphereA into capsuleB + glm::vec3 expectedPenetration = - ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis; + float inaccuracy = glm::length(collision._penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision._penetration + << std::endl; + } + + // contactPoint is on surface of sphereA + glm::vec3 expectedContactPoint = sphereA.getPosition() - radiusA * yAxis; + inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision._contactPoint + << std::endl; + } + + // capsuleB collides with sphereA + if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collision)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and sphere should touch" + << std::endl; + } + + // penetration points from sphereA into capsuleB + expectedPenetration = ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis; + inaccuracy = glm::length(collision._penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision._penetration + << std::endl; + } + + // contactPoint is on surface of capsuleB + glm::vec3 endPoint; + capsuleB.getEndPoint(endPoint); + expectedContactPoint = endPoint + radiusB * yAxis; + inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision._contactPoint + << std::endl; + } + } + { // sphereA hits start cap at axis + glm::vec3 axialOffset = - (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis; + sphereA.setPosition(axialOffset * yAxis); + + if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collision)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphere and capsule should touch" + << std::endl; + } + + // penetration points from sphereA into capsuleB + glm::vec3 expectedPenetration = ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis; + float inaccuracy = glm::length(collision._penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision._penetration + << std::endl; + } + + // contactPoint is on surface of sphereA + glm::vec3 expectedContactPoint = sphereA.getPosition() + radiusA * yAxis; + inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision._contactPoint + << std::endl; + } + + // capsuleB collides with sphereA + if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collision)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and sphere should touch" + << std::endl; + } + + // penetration points from sphereA into capsuleB + expectedPenetration = - ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis; + inaccuracy = glm::length(collision._penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision._penetration + << std::endl; + } + + // contactPoint is on surface of capsuleB + glm::vec3 startPoint; + capsuleB.getStartPoint(startPoint); + expectedContactPoint = startPoint - radiusB * yAxis; + inaccuracy = glm::length(collision._contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision._contactPoint + << std::endl; + } + } +} + +void ShapeColliderTests::capsuleMissesCapsule() { + // TODO: implement this +} + +void ShapeColliderTests::capsuleTouchesCapsule() { + // TODO: implement this } -*/ void ShapeColliderTests::runAllTests() { - ShapeColliderTests::sphereSphere(); -// ShapeColliderTests::test2(); + sphereMissesSphere(); + sphereTouchesSphere(); + + sphereMissesCapsule(); + sphereTouchesCapsule(); + + capsuleMissesCapsule(); + capsuleTouchesCapsule(); } diff --git a/tests/physics/src/ShapeColliderTests.h b/tests/physics/src/ShapeColliderTests.h index a3a19805f8..ecd4a7f045 100644 --- a/tests/physics/src/ShapeColliderTests.h +++ b/tests/physics/src/ShapeColliderTests.h @@ -11,8 +11,14 @@ namespace ShapeColliderTests { - void sphereSphere(); - //void test2(); + void sphereMissesSphere(); + void sphereTouchesSphere(); + + void sphereMissesCapsule(); + void sphereTouchesCapsule(); + + void capsuleMissesCapsule(); + void capsuleTouchesCapsule(); void runAllTests(); } diff --git a/tests/physics/src/main.cpp b/tests/physics/src/main.cpp index 3ae5a0a6fe..b0a7adde4e 100644 --- a/tests/physics/src/main.cpp +++ b/tests/physics/src/main.cpp @@ -3,13 +3,9 @@ // physics-tests // -//#include - #include "ShapeColliderTests.h" -#include "CollisionInfoTests.h" int main(int argc, char** argv) { - CollisionInfoTests::runAllTests(); ShapeColliderTests::runAllTests(); return 0; }