From ab3d582d79ca08001ef87cdf486ef3d28e63b5e5 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 17 Jun 2014 10:49:14 -0700 Subject: [PATCH] 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