diff --git a/libraries/shared/src/CapsuleShape.cpp b/libraries/shared/src/CapsuleShape.cpp index 8e887107dc..5416ff92a6 100644 --- a/libraries/shared/src/CapsuleShape.cpp +++ b/libraries/shared/src/CapsuleShape.cpp @@ -13,6 +13,8 @@ #include #include "CapsuleShape.h" + +#include "GeometryUtil.h" #include "SharedUtil.h" @@ -84,3 +86,11 @@ void CapsuleShape::setEndPoints(const glm::vec3& startPoint, const glm::vec3& en updateBoundingRadius(); } +bool CapsuleShape::findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const { + glm::vec3 capsuleStart, capsuleEnd; + getStartPoint(capsuleStart); + getEndPoint(capsuleEnd); + // NOTE: findRayCapsuleIntersection returns 'true' with distance = 0 when rayStart is inside capsule. + // TODO: implement the raycast to return inside surface intersection for the internal rayStart. + return findRayCapsuleIntersection(rayStart, rayDirection, capsuleStart, capsuleEnd, _radius, distance); +} diff --git a/libraries/shared/src/CapsuleShape.h b/libraries/shared/src/CapsuleShape.h index 756ae18911..fdd6c3eda6 100644 --- a/libraries/shared/src/CapsuleShape.h +++ b/libraries/shared/src/CapsuleShape.h @@ -39,6 +39,8 @@ public: void setRadiusAndHalfHeight(float radius, float height); void setEndPoints(const glm::vec3& startPoint, const glm::vec3& endPoint); + bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const; + protected: void updateBoundingRadius() { _boundingRadius = _radius + _halfHeight; } diff --git a/libraries/shared/src/ListShape.h b/libraries/shared/src/ListShape.h index 7ba2410a23..17e7d7b2b6 100644 --- a/libraries/shared/src/ListShape.h +++ b/libraries/shared/src/ListShape.h @@ -55,6 +55,9 @@ public: void setShapes(QVector& shapes); + // TODO: either implement this or remove ListShape altogether + bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const { return false; } + protected: void clear(); void computeBoundingRadius(); diff --git a/libraries/shared/src/PlaneShape.cpp b/libraries/shared/src/PlaneShape.cpp index a8b4468c93..e9563c6d8b 100644 --- a/libraries/shared/src/PlaneShape.cpp +++ b/libraries/shared/src/PlaneShape.cpp @@ -30,7 +30,28 @@ PlaneShape::PlaneShape(const glm::vec4& coefficients) : } } +glm::vec3 PlaneShape::getNormal() const { + return _rotation * UNROTATED_NORMAL; +} + glm::vec4 PlaneShape::getCoefficients() const { glm::vec3 normal = _rotation * UNROTATED_NORMAL; return glm::vec4(normal.x, normal.y, normal.z, -glm::dot(normal, _position)); } + +bool PlaneShape::findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const { + glm::vec3 n = getNormal(); + float denominator = glm::dot(n, rayDirection); + if (fabsf(denominator) < EPSILON) { + // line is parallel to plane + return glm::dot(_position - rayStart, n) < EPSILON; + } else { + float d = glm::dot(_position - rayStart, n) / denominator; + if (d > 0.0f) { + // ray points toward plane + distance = d; + return true; + } + } + return false; +} diff --git a/libraries/shared/src/PlaneShape.h b/libraries/shared/src/PlaneShape.h index 524d53ec73..b8a93324b7 100644 --- a/libraries/shared/src/PlaneShape.h +++ b/libraries/shared/src/PlaneShape.h @@ -18,7 +18,10 @@ class PlaneShape : public Shape { public: PlaneShape(const glm::vec4& coefficients = glm::vec4(0.0f, 1.0f, 0.0f, 0.0f)); + glm::vec3 getNormal() const; glm::vec4 getCoefficients() const; + + bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const; }; #endif // hifi_PlaneShape_h diff --git a/libraries/shared/src/Shape.h b/libraries/shared/src/Shape.h index 87b84ea73b..3926f6cd07 100644 --- a/libraries/shared/src/Shape.h +++ b/libraries/shared/src/Shape.h @@ -38,6 +38,8 @@ public: virtual void setPosition(const glm::vec3& position) { _position = position; } virtual void setRotation(const glm::quat& rotation) { _rotation = rotation; } + virtual bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const = 0; + protected: // these ctors are protected (used by derived classes only) Shape(Type type) : _type(type), _boundingRadius(0.f), _position(0.f), _rotation() {} diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index 7c29fbae00..bbedeb401d 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -765,5 +765,24 @@ bool capsuleAACube(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, fl return sphereAACube(nearestApproach, capsuleA->getRadius(), cubeCenter, cubeSide, collisions); } +bool findRayIntersectionWithShapes(const QVector shapes, const glm::vec3& rayStart, const glm::vec3& rayDirection, float& minDistance) { + float hitDistance = FLT_MAX; + int numShapes = shapes.size(); + for (int i = 0; i < numShapes; ++i) { + Shape* shape = shapes.at(i); + if (shape) { + float distance; + if (shape->findRayIntersection(rayStart, rayDirection, distance)) { + if (distance < hitDistance) { + hitDistance = distance; + } + } + } + } + if (hitDistance < FLT_MAX) { + minDistance = hitDistance; + } + return false; +} } // namespace ShapeCollider diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h index 9e83e31571..8261aceaf3 100644 --- a/libraries/shared/src/ShapeCollider.h +++ b/libraries/shared/src/ShapeCollider.h @@ -21,8 +21,8 @@ namespace ShapeCollider { - /// \param shapeA pointer to first shape - /// \param shapeB pointer to second shape + /// \param shapeA pointer to first shape (cannot be NULL) + /// \param shapeB pointer to second shape (cannot be NULL) /// \param collisions[out] collision details /// \return true if shapes collide bool collideShapes(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions); @@ -33,123 +33,130 @@ namespace ShapeCollider { /// \return true if any shapes collide bool collideShapesCoarse(const QVector& shapesA, const QVector& shapesB, CollisionInfo& collision); - /// \param shapeA a pointer to a shape + /// \param shapeA a pointer to a shape (cannot be NULL) /// \param cubeCenter center of cube /// \param cubeSide lenght of side of cube /// \param collisions[out] average collision details /// \return true if shapeA collides with axis aligned cube bool collideShapeWithAACube(const Shape* shapeA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions); - /// \param sphereA pointer to first shape - /// \param sphereB pointer to second shape + /// \param sphereA pointer to first shape (cannot be NULL) + /// \param sphereB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionList& collisions); - /// \param sphereA pointer to first shape - /// \param capsuleB pointer to second shape + /// \param sphereA pointer to first shape (cannot be NULL) + /// \param capsuleB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionList& collisions); - /// \param sphereA pointer to first shape - /// \param planeB pointer to second shape + /// \param sphereA pointer to first shape (cannot be NULL) + /// \param planeB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide bool spherePlane(const SphereShape* sphereA, const PlaneShape* planeB, CollisionList& collisions); - /// \param capsuleA pointer to first shape - /// \param sphereB pointer to second shape + /// \param capsuleA pointer to first shape (cannot be NULL) + /// \param sphereB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionList& collisions); - /// \param capsuleA pointer to first shape - /// \param capsuleB pointer to second shape + /// \param capsuleA pointer to first shape (cannot be NULL) + /// \param capsuleB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionList& collisions); - /// \param capsuleA pointer to first shape - /// \param planeB pointer to second shape + /// \param capsuleA pointer to first shape (cannot be NULL) + /// \param planeB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide bool capsulePlane(const CapsuleShape* capsuleA, const PlaneShape* planeB, CollisionList& collisions); - /// \param planeA pointer to first shape - /// \param sphereB pointer to second shape + /// \param planeA pointer to first shape (cannot be NULL) + /// \param sphereB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide bool planeSphere(const PlaneShape* planeA, const SphereShape* sphereB, CollisionList& collisions); - /// \param planeA pointer to first shape - /// \param capsuleB pointer to second shape + /// \param planeA pointer to first shape (cannot be NULL) + /// \param capsuleB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide bool planeCapsule(const PlaneShape* planeA, const CapsuleShape* capsuleB, CollisionList& collisions); - /// \param planeA pointer to first shape - /// \param planeB pointer to second shape + /// \param planeA pointer to first shape (cannot be NULL) + /// \param planeB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide bool planePlane(const PlaneShape* planeA, const PlaneShape* planeB, CollisionList& collisions); - /// \param sphereA pointer to first shape - /// \param listB pointer to second shape + /// \param sphereA pointer to first shape (cannot be NULL) + /// \param listB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide bool sphereList(const SphereShape* sphereA, const ListShape* listB, CollisionList& collisions); - /// \param capuleA pointer to first shape - /// \param listB pointer to second shape + /// \param capuleA pointer to first shape (cannot be NULL) + /// \param listB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide bool capsuleList(const CapsuleShape* capsuleA, const ListShape* listB, CollisionList& collisions); - /// \param planeA pointer to first shape - /// \param listB pointer to second shape + /// \param planeA pointer to first shape (cannot be NULL) + /// \param listB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide bool planeList(const PlaneShape* planeA, const ListShape* listB, CollisionList& collisions); - /// \param listA pointer to first shape - /// \param sphereB pointer to second shape + /// \param listA pointer to first shape (cannot be NULL) + /// \param sphereB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide bool listSphere(const ListShape* listA, const SphereShape* sphereB, CollisionList& collisions); - /// \param listA pointer to first shape - /// \param capsuleB pointer to second shape + /// \param listA pointer to first shape (cannot be NULL) + /// \param capsuleB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide bool listCapsule(const ListShape* listA, const CapsuleShape* capsuleB, CollisionList& collisions); - /// \param listA pointer to first shape - /// \param planeB pointer to second shape + /// \param listA pointer to first shape (cannot be NULL) + /// \param planeB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide bool listPlane(const ListShape* listA, const PlaneShape* planeB, CollisionList& collisions); - /// \param listA pointer to first shape - /// \param capsuleB pointer to second shape + /// \param listA pointer to first shape (cannot be NULL) + /// \param capsuleB pointer to second shape (cannot be NULL) /// \param[out] collisions where to append collision details /// \return true if shapes collide bool listList(const ListShape* listA, const ListShape* listB, CollisionList& collisions); - /// \param sphereA pointer to sphere + /// \param sphereA pointer to sphere (cannot be NULL) /// \param cubeCenter center of cube /// \param cubeSide lenght of side of cube /// \param[out] collisions where to append collision details /// \return true if sphereA collides with axis aligned cube bool sphereAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions); - /// \param capsuleA pointer to capsule + /// \param capsuleA pointer to capsule (cannot be NULL) /// \param cubeCenter center of cube /// \param cubeSide lenght of side of cube /// \param[out] collisions where to append collision details /// \return true if capsuleA collides with axis aligned cube bool capsuleAACube(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions); + /// \param shapes list of pointers to shapes (shape pointers may be NULL) + /// \param startPoint beginning of ray + /// \param direction direction of ray + /// \param minDistance[out] shortest distance to intersection of ray with a shapes + /// \return true if ray hits any shape in shapes + bool findRayIntersectionWithShapes(const QVector shapes, const glm::vec3& startPoint, const glm::vec3& direction, float& minDistance); + } // namespace ShapeCollider #endif // hifi_ShapeCollider_h diff --git a/libraries/shared/src/SphereShape.cpp b/libraries/shared/src/SphereShape.cpp new file mode 100644 index 0000000000..49137fac43 --- /dev/null +++ b/libraries/shared/src/SphereShape.cpp @@ -0,0 +1,44 @@ +// +// SphereShape.cpp +// libraries/shared/src +// +// Created by Andrew Meadows on 2014.06.17 +// Copyright 2014 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include + +#include "SphereShape.h" + +bool SphereShape::findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const { + float r2 = _boundingRadius * _boundingRadius; + + // compute closest approach (CA) + float a = glm::dot(_position - rayStart, rayDirection); // a = distance from ray-start to CA + float b2 = glm::distance2(_position, rayStart + a * rayDirection); // b2 = squared distance from sphere-center to CA + if (b2 > r2) { + // ray does not hit sphere + return false; + } + float c = sqrtf(r2 - b2); // c = distance from CA to sphere surface along rayDirection + float d2 = glm::distance2(rayStart, _position); // d2 = squared distance from sphere-center to ray-start + if (a < 0.0f) { + // ray points away from sphere-center + if (d2 > r2) { + // ray starts outside sphere + return false; + } + // ray starts inside sphere + distance = c + a; + } else if (d2 > r2) { + // ray starts outside sphere + distance = a - c; + } else { + // ray starts inside sphere + distance = a + c; + } + return true; +} diff --git a/libraries/shared/src/SphereShape.h b/libraries/shared/src/SphereShape.h index 62783ab340..e87b8acab1 100644 --- a/libraries/shared/src/SphereShape.h +++ b/libraries/shared/src/SphereShape.h @@ -29,6 +29,8 @@ public: float getRadius() const { return _boundingRadius; } void setRadius(float radius) { _boundingRadius = radius; } + + bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const; }; #endif // hifi_SphereShape_h diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index 7b3d956065..608e012998 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -11,6 +11,7 @@ //#include #include +#include #include #include @@ -897,6 +898,405 @@ void ShapeColliderTests::sphereMissesAACube() { } } +void ShapeColliderTests::rayHitsSphere() { + float startDistance = 3.0f; + glm::vec3 rayStart(-startDistance, 0.0f, 0.0f); + glm::vec3 rayDirection(1.0f, 0.0f, 0.0f); + + float radius = 1.0f; + glm::vec3 center(0.0f); + + SphereShape sphere(radius, center); + + // very simple ray along xAxis + { + float distance = FLT_MAX; + if (!sphere.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should intersect sphere" << std::endl; + } + + float expectedDistance = startDistance - radius; + float relativeError = fabsf(distance - expectedDistance) / startDistance; + if (relativeError > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray sphere intersection distance error = " << relativeError << std::endl; + } + } + + // ray along a diagonal axis + { + rayStart = glm::vec3(startDistance, startDistance, 0.0f); + rayDirection = - glm::normalize(rayStart); + + float distance = FLT_MAX; + if (!sphere.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should intersect sphere" << std::endl; + } + + float expectedDistance = SQUARE_ROOT_OF_2 * startDistance - radius; + float relativeError = fabsf(distance - expectedDistance) / startDistance; + if (relativeError > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray sphere intersection distance error = " << relativeError << std::endl; + } + } + + // rotated and displaced ray and sphere + { + startDistance = 7.41f; + radius = 3.917f; + + glm::vec3 axis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); + glm::quat rotation = glm::angleAxis(0.987654321f, axis); + glm::vec3 translation(35.7f, 2.46f, -1.97f); + + glm::vec3 unrotatedRayDirection(-1.0f, 0.0f, 0.0f); + glm::vec3 untransformedRayStart(startDistance, 0.0f, 0.0f); + + rayStart = rotation * (untransformedRayStart + translation); + rayDirection = rotation * unrotatedRayDirection; + + sphere.setRadius(radius); + sphere.setPosition(rotation * translation); + + float distance = FLT_MAX; + if (!sphere.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should intersect sphere" << std::endl; + } + + float expectedDistance = startDistance - radius; + float relativeError = fabsf(distance - expectedDistance) / startDistance; + if (relativeError > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray sphere intersection distance error = " << relativeError << std::endl; + } + } +} + +void ShapeColliderTests::rayBarelyHitsSphere() { + float radius = 1.0f; + glm::vec3 center(0.0f); + float delta = 2.0f * EPSILON; + + float startDistance = 3.0f; + glm::vec3 rayStart(-startDistance, radius - delta, 0.0f); + glm::vec3 rayDirection(1.0f, 0.0f, 0.0f); + + SphereShape sphere(radius, center); + + // very simple ray along xAxis + float distance = FLT_MAX; + if (!sphere.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely hit sphere" << std::endl; + } + + // translate and rotate the whole system... + glm::vec3 axis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); + glm::quat rotation = glm::angleAxis(0.987654321f, axis); + glm::vec3 translation(35.7f, 2.46f, -1.97f); + + rayStart = rotation * (rayStart + translation); + rayDirection = rotation * rayDirection; + sphere.setPosition(rotation * translation); + + // ...and test again + distance = FLT_MAX; + if (!sphere.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely hit sphere" << std::endl; + } +} + + +void ShapeColliderTests::rayBarelyMissesSphere() { + // same as the barely-hits case, but this time we move the ray away from sphere + float radius = 1.0f; + glm::vec3 center(0.0f); + float delta = 2.0f * EPSILON; + + float startDistance = 3.0f; + glm::vec3 rayStart(-startDistance, radius + delta, 0.0f); + glm::vec3 rayDirection(1.0f, 0.0f, 0.0f); + + SphereShape sphere(radius, center); + + // very simple ray along xAxis + float distance = FLT_MAX; + if (sphere.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely miss sphere" << std::endl; + } + if (distance != FLT_MAX) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl; + } + + // translate and rotate the whole system... + glm::vec3 axis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); + glm::quat rotation = glm::angleAxis(0.987654321f, axis); + glm::vec3 translation(35.7f, 2.46f, -1.97f); + + rayStart = rotation * (rayStart + translation); + rayDirection = rotation * rayDirection; + sphere.setPosition(rotation * translation); + + // ...and test again + distance = FLT_MAX; + if (sphere.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely miss sphere" << std::endl; + } + if (distance != FLT_MAX) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl; + } +} + +void ShapeColliderTests::rayHitsCapsule() { + float startDistance = 3.0f; + float radius = 1.0f; + float halfHeight = 2.0f; + glm::vec3 center(0.0f); + CapsuleShape capsule(radius, halfHeight); + + { // simple test along xAxis + // toward capsule center + glm::vec3 rayStart(startDistance, 0.0f, 0.0f); + glm::vec3 rayDirection(-1.0f, 0.0f, 0.0f); + float distance = FLT_MAX; + if (!capsule.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl; + } + float expectedDistance = startDistance - radius; + float relativeError = fabsf(distance - expectedDistance) / startDistance; + if (relativeError > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " << relativeError << std::endl; + } + + // toward top of cylindrical wall + rayStart.y = halfHeight; + distance = FLT_MAX; + if (!capsule.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl; + } + relativeError = fabsf(distance - expectedDistance) / startDistance; + if (relativeError > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " << relativeError << std::endl; + } + + // toward top cap + float delta = 2.0f * EPSILON; + rayStart.y = halfHeight + delta; + distance = FLT_MAX; + if (!capsule.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl; + } + relativeError = fabsf(distance - expectedDistance) / startDistance; + if (relativeError > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " << relativeError << std::endl; + } + + const float EDGE_CASE_SLOP_FACTOR = 20.0f; + + // toward tip of top cap + rayStart.y = halfHeight + radius - delta; + distance = FLT_MAX; + if (!capsule.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl; + } + expectedDistance = startDistance - radius * sqrtf(2.0f * delta); // using small angle approximation of cosine + relativeError = fabsf(distance - expectedDistance) / startDistance; + // for edge cases we allow a LOT of error + if (relativeError > EDGE_CASE_SLOP_FACTOR * EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " << relativeError << std::endl; + } + + // toward tip of bottom cap + rayStart.y = - halfHeight - radius + delta; + distance = FLT_MAX; + if (!capsule.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl; + } + expectedDistance = startDistance - radius * sqrtf(2.0f * delta); // using small angle approximation of cosine + relativeError = fabsf(distance - expectedDistance) / startDistance; + // for edge cases we allow a LOT of error + if (relativeError > EDGE_CASE_SLOP_FACTOR * EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " << relativeError << std::endl; + } + + // toward edge of capsule cylindrical face + rayStart.y = 0.0f; + rayStart.z = radius - delta; + distance = FLT_MAX; + if (!capsule.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl; + } + expectedDistance = startDistance - radius * sqrtf(2.0f * delta); // using small angle approximation of cosine + relativeError = fabsf(distance - expectedDistance) / startDistance; + // for edge cases we allow a LOT of error + if (relativeError > EDGE_CASE_SLOP_FACTOR * EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " << relativeError << std::endl; + } + } + // TODO: test at steep angles near cylinder/cap junction +} + +void ShapeColliderTests::rayMissesCapsule() { + // same as edge case hit tests, but shifted in the opposite direction + float startDistance = 3.0f; + float radius = 1.0f; + float halfHeight = 2.0f; + glm::vec3 center(0.0f); + CapsuleShape capsule(radius, halfHeight); + + { // simple test along xAxis + // toward capsule center + glm::vec3 rayStart(startDistance, 0.0f, 0.0f); + glm::vec3 rayDirection(-1.0f, 0.0f, 0.0f); + float delta = 2.0f * EPSILON; + + // over top cap + rayStart.y = halfHeight + radius + delta; + float distance = FLT_MAX; + if (capsule.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss capsule" << std::endl; + } + if (distance != FLT_MAX) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl; + } + + // below bottom cap + rayStart.y = - halfHeight - radius - delta; + distance = FLT_MAX; + if (capsule.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss capsule" << std::endl; + } + if (distance != FLT_MAX) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl; + } + + // past edge of capsule cylindrical face + rayStart.y = 0.0f; + rayStart.z = radius + delta; + distance = FLT_MAX; + if (capsule.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss capsule" << std::endl; + } + if (distance != FLT_MAX) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl; + } + } + // TODO: test at steep angles near edge +} + +void ShapeColliderTests::rayHitsPlane() { + // make a simple plane + float planeDistanceFromOrigin = 3.579; + glm::vec3 planePosition(0.0f, planeDistanceFromOrigin, 0.0f); + PlaneShape plane; + plane.setPosition(planePosition); + + // make a simple ray + float startDistance = 1.234f; + glm::vec3 rayStart(-startDistance, 0.0f, 0.0f); + glm::vec3 rayDirection = glm::normalize(glm::vec3(1.0f, 1.0f, 1.0f)); + + float distance = FLT_MAX; + if (!plane.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit plane" << std::endl; + } + + float expectedDistance = SQUARE_ROOT_OF_3 * planeDistanceFromOrigin; + float relativeError = fabsf(distance - expectedDistance) / planeDistanceFromOrigin; + if (relativeError > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray plane intersection distance error = " << relativeError << std::endl; + } + + // rotate the whole system and try again + float angle = 37.8f; + glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) ); + glm::quat rotation = glm::angleAxis(angle, axis); + + plane.setPosition(rotation * planePosition); + plane.setRotation(rotation); + rayStart = rotation * rayStart; + rayDirection = rotation * rayDirection; + + distance = FLT_MAX; + if (!plane.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit plane" << std::endl; + } + + expectedDistance = SQUARE_ROOT_OF_3 * planeDistanceFromOrigin; + relativeError = fabsf(distance - expectedDistance) / planeDistanceFromOrigin; + if (relativeError > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray plane intersection distance error = " << relativeError << std::endl; + } +} + +void ShapeColliderTests::rayMissesPlane() { + // make a simple plane + float planeDistanceFromOrigin = 3.579; + glm::vec3 planePosition(0.0f, planeDistanceFromOrigin, 0.0f); + PlaneShape plane; + plane.setPosition(planePosition); + + { // parallel rays should miss + float startDistance = 1.234f; + glm::vec3 rayStart(-startDistance, 0.0f, 0.0f); + glm::vec3 rayDirection = glm::normalize(glm::vec3(-1.0f, 0.0f, -1.0f)); + + float distance = FLT_MAX; + if (plane.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl; + } + if (distance != FLT_MAX) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl; + } + + // rotate the whole system and try again + float angle = 37.8f; + glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) ); + glm::quat rotation = glm::angleAxis(angle, axis); + + plane.setPosition(rotation * planePosition); + plane.setRotation(rotation); + rayStart = rotation * rayStart; + rayDirection = rotation * rayDirection; + + distance = FLT_MAX; + if (plane.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl; + } + if (distance != FLT_MAX) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl; + } + } + + { // make a simple ray that points away from plane + float startDistance = 1.234f; + glm::vec3 rayStart(-startDistance, 0.0f, 0.0f); + glm::vec3 rayDirection = glm::normalize(glm::vec3(-1.0f, -1.0f, -1.0f)); + + float distance = FLT_MAX; + if (plane.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl; + } + if (distance != FLT_MAX) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl; + } + + // rotate the whole system and try again + float angle = 37.8f; + glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) ); + glm::quat rotation = glm::angleAxis(angle, axis); + + plane.setPosition(rotation * planePosition); + plane.setRotation(rotation); + rayStart = rotation * rayStart; + rayDirection = rotation * rayDirection; + + distance = FLT_MAX; + if (plane.findRayIntersection(rayStart, rayDirection, distance)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl; + } + if (distance != FLT_MAX) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" << std::endl; + } + } +} void ShapeColliderTests::runAllTests() { sphereMissesSphere(); @@ -911,4 +1311,12 @@ void ShapeColliderTests::runAllTests() { sphereTouchesAACubeFaces(); sphereTouchesAACubeEdges(); sphereMissesAACube(); + + rayHitsSphere(); + rayBarelyHitsSphere(); + rayBarelyMissesSphere(); + rayHitsCapsule(); + rayMissesCapsule(); + rayHitsPlane(); + rayMissesPlane(); } diff --git a/tests/physics/src/ShapeColliderTests.h b/tests/physics/src/ShapeColliderTests.h index b51c48a61e..fd9f1f9706 100644 --- a/tests/physics/src/ShapeColliderTests.h +++ b/tests/physics/src/ShapeColliderTests.h @@ -27,6 +27,14 @@ namespace ShapeColliderTests { void sphereTouchesAACubeEdges(); void sphereMissesAACube(); + void rayHitsSphere(); + void rayBarelyHitsSphere(); + void rayBarelyMissesSphere(); + void rayHitsCapsule(); + void rayMissesCapsule(); + void rayHitsPlane(); + void rayMissesPlane(); + void runAllTests(); }