From e75340f8ceb2b966e5ae7c1b8acf346ea6490d3a Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 17 Jun 2014 10:48:29 -0700 Subject: [PATCH 1/5] Add PlaneShape::getNormal() method. --- libraries/shared/src/PlaneShape.cpp | 4 ++++ libraries/shared/src/PlaneShape.h | 1 + 2 files changed, 5 insertions(+) diff --git a/libraries/shared/src/PlaneShape.cpp b/libraries/shared/src/PlaneShape.cpp index a8b4468c93..0617feb863 100644 --- a/libraries/shared/src/PlaneShape.cpp +++ b/libraries/shared/src/PlaneShape.cpp @@ -30,6 +30,10 @@ 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)); diff --git a/libraries/shared/src/PlaneShape.h b/libraries/shared/src/PlaneShape.h index 524d53ec73..24a3f1a2bc 100644 --- a/libraries/shared/src/PlaneShape.h +++ b/libraries/shared/src/PlaneShape.h @@ -18,6 +18,7 @@ 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; }; From ab3d582d79ca08001ef87cdf486ef3d28e63b5e5 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 17 Jun 2014 10:49:14 -0700 Subject: [PATCH 2/5] Add ray intersection tests against most shapes. --- libraries/shared/src/ShapeCollider.cpp | 81 ++++++++++++++++++++++++ libraries/shared/src/ShapeCollider.h | 88 +++++++++++++++----------- 2 files changed, 132 insertions(+), 37 deletions(-) diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index 7c29fbae00..24d7fdb01a 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -765,5 +765,86 @@ 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 (findRayIntersectionWithShape(shape, rayStart, rayDirection, distance)) { + if (distance < hitDistance) { + hitDistance = distance; + } + } + } + } + if (hitDistance < FLT_MAX) { + minDistance = hitDistance; + } + return false; +} + +bool findRayIntersectionWithShape(const Shape* shape, const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) { + // NOTE: rayDirection is assumed to be normalized + int typeA = shape->getType(); + if (typeA == Shape::SPHERE_SHAPE) { + const SphereShape* sphere = static_cast(shape); + glm::vec3 sphereCenter = sphere->getPosition(); + float r2 = sphere->getRadius() * sphere->getRadius(); // r2 = radius^2 + + // compute closest approach (CA) + float a = glm::dot(sphere->getPosition() - rayStart, rayDirection); // a = distance from ray-start to CA + float b2 = glm::distance2(sphereCenter, 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, sphereCenter); // 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; + } else if (typeA == Shape::CAPSULE_SHAPE) { + const CapsuleShape* capsule = static_cast(shape); + float radius = capsule->getRadius(); + glm::vec3 capsuleStart, capsuleEnd; + capsule->getStartPoint(capsuleStart); + capsule->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); + } else if (typeA == Shape::PLANE_SHAPE) { + const PlaneShape* plane = static_cast(shape); + glm::vec3 n = plane->getNormal(); + glm::vec3 P = plane->getPosition(); + float denominator = glm::dot(n, rayDirection); + if (fabsf(denominator) < EPSILON) { + // line is parallel to plane + return glm::dot(P - rayStart, n) < EPSILON; + } else { + float d = glm::dot(P - rayStart, n) / denominator; + if (d > 0.0f) { + // ray points toward plane + distance = d; + return true; + } + } + } + return false; +} } // namespace ShapeCollider diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h index 9e83e31571..308a8cf10b 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,137 @@ 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); + + /// \param shapeA pointer to shape (cannot be NULL) + /// \param startPoint beginning of ray + /// \param direction direction of ray + /// \param distance[out] distance to intersection of shape and ray + /// \return true if ray hits shapeA + bool findRayIntersectionWithShape(const Shape* shapeA, const glm::vec3& startPoint, const glm::vec3& direction, float& distance); + } // namespace ShapeCollider #endif // hifi_ShapeCollider_h From fa6aed3e01a9d004cca5de4fa12c7538f37c35e9 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 17 Jun 2014 10:49:53 -0700 Subject: [PATCH 3/5] Add unit tests for ray intersections with shapes. --- tests/physics/src/ShapeColliderTests.cpp | 408 +++++++++++++++++++++++ tests/physics/src/ShapeColliderTests.h | 8 + 2 files changed, 416 insertions(+) diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index 7b3d956065..3387ba6aba 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 (!ShapeCollider::findRayIntersectionWithShape(&sphere, 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 (!ShapeCollider::findRayIntersectionWithShape(&sphere, 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 (!ShapeCollider::findRayIntersectionWithShape(&sphere, 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 (!ShapeCollider::findRayIntersectionWithShape(&sphere, 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 (!ShapeCollider::findRayIntersectionWithShape(&sphere, 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 (ShapeCollider::findRayIntersectionWithShape(&sphere, 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 (ShapeCollider::findRayIntersectionWithShape(&sphere, 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 (!ShapeCollider::findRayIntersectionWithShape(&capsule, 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 (!ShapeCollider::findRayIntersectionWithShape(&capsule, 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 (!ShapeCollider::findRayIntersectionWithShape(&capsule, 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 (!ShapeCollider::findRayIntersectionWithShape(&capsule, 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 (!ShapeCollider::findRayIntersectionWithShape(&capsule, 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 (!ShapeCollider::findRayIntersectionWithShape(&capsule, 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 (ShapeCollider::findRayIntersectionWithShape(&capsule, 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 (ShapeCollider::findRayIntersectionWithShape(&capsule, 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 (ShapeCollider::findRayIntersectionWithShape(&capsule, 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 (!ShapeCollider::findRayIntersectionWithShape(&plane, 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 (!ShapeCollider::findRayIntersectionWithShape(&plane, 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 (ShapeCollider::findRayIntersectionWithShape(&plane, 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 (ShapeCollider::findRayIntersectionWithShape(&plane, 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 (ShapeCollider::findRayIntersectionWithShape(&plane, 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 (ShapeCollider::findRayIntersectionWithShape(&plane, 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(); } From f18864bd72019f7b69a8d13b8aaa2d505237f721 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 17 Jun 2014 12:36:36 -0700 Subject: [PATCH 4/5] Moved findRayIntersection() to the Shape classes --- libraries/shared/src/CapsuleShape.cpp | 10 ++++ libraries/shared/src/CapsuleShape.h | 2 + libraries/shared/src/ListShape.h | 3 ++ libraries/shared/src/PlaneShape.cpp | 17 +++++++ libraries/shared/src/PlaneShape.h | 2 + libraries/shared/src/Shape.h | 2 + libraries/shared/src/ShapeCollider.cpp | 64 +----------------------- libraries/shared/src/ShapeCollider.h | 7 --- libraries/shared/src/SphereShape.h | 2 + tests/physics/src/ShapeColliderTests.cpp | 44 ++++++++-------- 10 files changed, 61 insertions(+), 92 deletions(-) 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 0617feb863..e9563c6d8b 100644 --- a/libraries/shared/src/PlaneShape.cpp +++ b/libraries/shared/src/PlaneShape.cpp @@ -38,3 +38,20 @@ 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 24a3f1a2bc..b8a93324b7 100644 --- a/libraries/shared/src/PlaneShape.h +++ b/libraries/shared/src/PlaneShape.h @@ -20,6 +20,8 @@ public: 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 24d7fdb01a..bbedeb401d 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -772,7 +772,7 @@ bool findRayIntersectionWithShapes(const QVector shapes, const glm::vec3 Shape* shape = shapes.at(i); if (shape) { float distance; - if (findRayIntersectionWithShape(shape, rayStart, rayDirection, distance)) { + if (shape->findRayIntersection(rayStart, rayDirection, distance)) { if (distance < hitDistance) { hitDistance = distance; } @@ -785,66 +785,4 @@ bool findRayIntersectionWithShapes(const QVector shapes, const glm::vec3 return false; } -bool findRayIntersectionWithShape(const Shape* shape, const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) { - // NOTE: rayDirection is assumed to be normalized - int typeA = shape->getType(); - if (typeA == Shape::SPHERE_SHAPE) { - const SphereShape* sphere = static_cast(shape); - glm::vec3 sphereCenter = sphere->getPosition(); - float r2 = sphere->getRadius() * sphere->getRadius(); // r2 = radius^2 - - // compute closest approach (CA) - float a = glm::dot(sphere->getPosition() - rayStart, rayDirection); // a = distance from ray-start to CA - float b2 = glm::distance2(sphereCenter, 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, sphereCenter); // 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; - } else if (typeA == Shape::CAPSULE_SHAPE) { - const CapsuleShape* capsule = static_cast(shape); - float radius = capsule->getRadius(); - glm::vec3 capsuleStart, capsuleEnd; - capsule->getStartPoint(capsuleStart); - capsule->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); - } else if (typeA == Shape::PLANE_SHAPE) { - const PlaneShape* plane = static_cast(shape); - glm::vec3 n = plane->getNormal(); - glm::vec3 P = plane->getPosition(); - float denominator = glm::dot(n, rayDirection); - if (fabsf(denominator) < EPSILON) { - // line is parallel to plane - return glm::dot(P - rayStart, n) < EPSILON; - } else { - float d = glm::dot(P - rayStart, n) / denominator; - if (d > 0.0f) { - // ray points toward plane - distance = d; - return true; - } - } - } - return false; -} - } // namespace ShapeCollider diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h index 308a8cf10b..8261aceaf3 100644 --- a/libraries/shared/src/ShapeCollider.h +++ b/libraries/shared/src/ShapeCollider.h @@ -157,13 +157,6 @@ namespace ShapeCollider { /// \return true if ray hits any shape in shapes bool findRayIntersectionWithShapes(const QVector shapes, const glm::vec3& startPoint, const glm::vec3& direction, float& minDistance); - /// \param shapeA pointer to shape (cannot be NULL) - /// \param startPoint beginning of ray - /// \param direction direction of ray - /// \param distance[out] distance to intersection of shape and ray - /// \return true if ray hits shapeA - bool findRayIntersectionWithShape(const Shape* shapeA, const glm::vec3& startPoint, const glm::vec3& direction, float& distance); - } // namespace ShapeCollider #endif // hifi_ShapeCollider_h 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 3387ba6aba..608e012998 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -911,7 +911,7 @@ void ShapeColliderTests::rayHitsSphere() { // very simple ray along xAxis { float distance = FLT_MAX; - if (!ShapeCollider::findRayIntersectionWithShape(&sphere, rayStart, rayDirection, distance)) { + if (!sphere.findRayIntersection(rayStart, rayDirection, distance)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should intersect sphere" << std::endl; } @@ -928,7 +928,7 @@ void ShapeColliderTests::rayHitsSphere() { rayDirection = - glm::normalize(rayStart); float distance = FLT_MAX; - if (!ShapeCollider::findRayIntersectionWithShape(&sphere, rayStart, rayDirection, distance)) { + if (!sphere.findRayIntersection(rayStart, rayDirection, distance)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should intersect sphere" << std::endl; } @@ -958,7 +958,7 @@ void ShapeColliderTests::rayHitsSphere() { sphere.setPosition(rotation * translation); float distance = FLT_MAX; - if (!ShapeCollider::findRayIntersectionWithShape(&sphere, rayStart, rayDirection, distance)) { + if (!sphere.findRayIntersection(rayStart, rayDirection, distance)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should intersect sphere" << std::endl; } @@ -983,7 +983,7 @@ void ShapeColliderTests::rayBarelyHitsSphere() { // very simple ray along xAxis float distance = FLT_MAX; - if (!ShapeCollider::findRayIntersectionWithShape(&sphere, rayStart, rayDirection, distance)) { + if (!sphere.findRayIntersection(rayStart, rayDirection, distance)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely hit sphere" << std::endl; } @@ -998,7 +998,7 @@ void ShapeColliderTests::rayBarelyHitsSphere() { // ...and test again distance = FLT_MAX; - if (!ShapeCollider::findRayIntersectionWithShape(&sphere, rayStart, rayDirection, distance)) { + if (!sphere.findRayIntersection(rayStart, rayDirection, distance)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely hit sphere" << std::endl; } } @@ -1018,7 +1018,7 @@ void ShapeColliderTests::rayBarelyMissesSphere() { // very simple ray along xAxis float distance = FLT_MAX; - if (ShapeCollider::findRayIntersectionWithShape(&sphere, rayStart, rayDirection, distance)) { + if (sphere.findRayIntersection(rayStart, rayDirection, distance)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely miss sphere" << std::endl; } if (distance != FLT_MAX) { @@ -1036,7 +1036,7 @@ void ShapeColliderTests::rayBarelyMissesSphere() { // ...and test again distance = FLT_MAX; - if (ShapeCollider::findRayIntersectionWithShape(&sphere, rayStart, rayDirection, distance)) { + if (sphere.findRayIntersection(rayStart, rayDirection, distance)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely miss sphere" << std::endl; } if (distance != FLT_MAX) { @@ -1056,7 +1056,7 @@ void ShapeColliderTests::rayHitsCapsule() { glm::vec3 rayStart(startDistance, 0.0f, 0.0f); glm::vec3 rayDirection(-1.0f, 0.0f, 0.0f); float distance = FLT_MAX; - if (!ShapeCollider::findRayIntersectionWithShape(&capsule, rayStart, rayDirection, distance)) { + if (!capsule.findRayIntersection(rayStart, rayDirection, distance)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl; } float expectedDistance = startDistance - radius; @@ -1068,7 +1068,7 @@ void ShapeColliderTests::rayHitsCapsule() { // toward top of cylindrical wall rayStart.y = halfHeight; distance = FLT_MAX; - if (!ShapeCollider::findRayIntersectionWithShape(&capsule, rayStart, rayDirection, distance)) { + if (!capsule.findRayIntersection(rayStart, rayDirection, distance)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl; } relativeError = fabsf(distance - expectedDistance) / startDistance; @@ -1080,7 +1080,7 @@ void ShapeColliderTests::rayHitsCapsule() { float delta = 2.0f * EPSILON; rayStart.y = halfHeight + delta; distance = FLT_MAX; - if (!ShapeCollider::findRayIntersectionWithShape(&capsule, rayStart, rayDirection, distance)) { + if (!capsule.findRayIntersection(rayStart, rayDirection, distance)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl; } relativeError = fabsf(distance - expectedDistance) / startDistance; @@ -1093,7 +1093,7 @@ void ShapeColliderTests::rayHitsCapsule() { // toward tip of top cap rayStart.y = halfHeight + radius - delta; distance = FLT_MAX; - if (!ShapeCollider::findRayIntersectionWithShape(&capsule, rayStart, rayDirection, distance)) { + 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 @@ -1106,7 +1106,7 @@ void ShapeColliderTests::rayHitsCapsule() { // toward tip of bottom cap rayStart.y = - halfHeight - radius + delta; distance = FLT_MAX; - if (!ShapeCollider::findRayIntersectionWithShape(&capsule, rayStart, rayDirection, distance)) { + 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 @@ -1120,7 +1120,7 @@ void ShapeColliderTests::rayHitsCapsule() { rayStart.y = 0.0f; rayStart.z = radius - delta; distance = FLT_MAX; - if (!ShapeCollider::findRayIntersectionWithShape(&capsule, rayStart, rayDirection, distance)) { + 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 @@ -1150,7 +1150,7 @@ void ShapeColliderTests::rayMissesCapsule() { // over top cap rayStart.y = halfHeight + radius + delta; float distance = FLT_MAX; - if (ShapeCollider::findRayIntersectionWithShape(&capsule, rayStart, rayDirection, distance)) { + if (capsule.findRayIntersection(rayStart, rayDirection, distance)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss capsule" << std::endl; } if (distance != FLT_MAX) { @@ -1160,7 +1160,7 @@ void ShapeColliderTests::rayMissesCapsule() { // below bottom cap rayStart.y = - halfHeight - radius - delta; distance = FLT_MAX; - if (ShapeCollider::findRayIntersectionWithShape(&capsule, rayStart, rayDirection, distance)) { + if (capsule.findRayIntersection(rayStart, rayDirection, distance)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss capsule" << std::endl; } if (distance != FLT_MAX) { @@ -1171,7 +1171,7 @@ void ShapeColliderTests::rayMissesCapsule() { rayStart.y = 0.0f; rayStart.z = radius + delta; distance = FLT_MAX; - if (ShapeCollider::findRayIntersectionWithShape(&capsule, rayStart, rayDirection, distance)) { + if (capsule.findRayIntersection(rayStart, rayDirection, distance)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss capsule" << std::endl; } if (distance != FLT_MAX) { @@ -1194,7 +1194,7 @@ void ShapeColliderTests::rayHitsPlane() { glm::vec3 rayDirection = glm::normalize(glm::vec3(1.0f, 1.0f, 1.0f)); float distance = FLT_MAX; - if (!ShapeCollider::findRayIntersectionWithShape(&plane, rayStart, rayDirection, distance)) { + if (!plane.findRayIntersection(rayStart, rayDirection, distance)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit plane" << std::endl; } @@ -1215,7 +1215,7 @@ void ShapeColliderTests::rayHitsPlane() { rayDirection = rotation * rayDirection; distance = FLT_MAX; - if (!ShapeCollider::findRayIntersectionWithShape(&plane, rayStart, rayDirection, distance)) { + if (!plane.findRayIntersection(rayStart, rayDirection, distance)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit plane" << std::endl; } @@ -1239,7 +1239,7 @@ void ShapeColliderTests::rayMissesPlane() { glm::vec3 rayDirection = glm::normalize(glm::vec3(-1.0f, 0.0f, -1.0f)); float distance = FLT_MAX; - if (ShapeCollider::findRayIntersectionWithShape(&plane, rayStart, rayDirection, distance)) { + if (plane.findRayIntersection(rayStart, rayDirection, distance)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl; } if (distance != FLT_MAX) { @@ -1257,7 +1257,7 @@ void ShapeColliderTests::rayMissesPlane() { rayDirection = rotation * rayDirection; distance = FLT_MAX; - if (ShapeCollider::findRayIntersectionWithShape(&plane, rayStart, rayDirection, distance)) { + if (plane.findRayIntersection(rayStart, rayDirection, distance)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl; } if (distance != FLT_MAX) { @@ -1271,7 +1271,7 @@ void ShapeColliderTests::rayMissesPlane() { glm::vec3 rayDirection = glm::normalize(glm::vec3(-1.0f, -1.0f, -1.0f)); float distance = FLT_MAX; - if (ShapeCollider::findRayIntersectionWithShape(&plane, rayStart, rayDirection, distance)) { + if (plane.findRayIntersection(rayStart, rayDirection, distance)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl; } if (distance != FLT_MAX) { @@ -1289,7 +1289,7 @@ void ShapeColliderTests::rayMissesPlane() { rayDirection = rotation * rayDirection; distance = FLT_MAX; - if (ShapeCollider::findRayIntersectionWithShape(&plane, rayStart, rayDirection, distance)) { + if (plane.findRayIntersection(rayStart, rayDirection, distance)) { std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl; } if (distance != FLT_MAX) { From b6cecf3cfd4ef651f1fcc8120dae7880265b37e7 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 17 Jun 2014 12:50:49 -0700 Subject: [PATCH 5/5] Add SphereShape.cpp to project --- libraries/shared/src/SphereShape.cpp | 44 ++++++++++++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 libraries/shared/src/SphereShape.cpp 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; +}