From f18864bd72019f7b69a8d13b8aaa2d505237f721 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 17 Jun 2014 12:36:36 -0700 Subject: [PATCH] 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) {