diff --git a/libraries/shared/src/GeometryUtil.cpp b/libraries/shared/src/GeometryUtil.cpp index 96c4d3bdea..92fe138021 100644 --- a/libraries/shared/src/GeometryUtil.cpp +++ b/libraries/shared/src/GeometryUtil.cpp @@ -564,3 +564,17 @@ void swingTwistDecomposition(const glm::quat& rotation, // rotation = swing * twist --> swing = rotation * invTwist swing = rotation * glm::inverse(twist); } + +// calculate the minimum angle between a point and a sphere. +float coneSphereAngle(const glm::vec3& coneCenter, const glm::vec3& coneDirection, const glm::vec3& sphereCenter, float sphereRadius) { + glm::vec3 d = sphereCenter - coneCenter; + float dLen = glm::length(d); + + // theta is the angle between the coneDirection normal and the center of the sphere. + float theta = acosf(glm::dot(d, coneDirection) / dLen); + + // phi is the deflection angle from the center of the sphere to a point tangent to the sphere. + float phi = atanf(sphereRadius / dLen); + + return glm::max(0.0f, theta - phi); +} diff --git a/libraries/shared/src/GeometryUtil.h b/libraries/shared/src/GeometryUtil.h index 7224aaacff..a66b16b0fd 100644 --- a/libraries/shared/src/GeometryUtil.h +++ b/libraries/shared/src/GeometryUtil.h @@ -110,6 +110,8 @@ bool doLineSegmentsIntersect(glm::vec2 r1p1, glm::vec2 r1p2, glm::vec2 r2p1, glm bool isOnSegment(float xi, float yi, float xj, float yj, float xk, float yk); int computeDirection(float xi, float yi, float xj, float yj, float xk, float yk); +// calculate the angle between a point on a sphere that is closest to the cone. +float coneSphereAngle(const glm::vec3& coneCenter, const glm::vec3& coneDirection, const glm::vec3& sphereCenter, float sphereRadius); typedef glm::vec2 LineSegment2[2]; diff --git a/tests/shared/src/GeometryUtilTests.cpp b/tests/shared/src/GeometryUtilTests.cpp index 289ac9a0a9..79472a1128 100644 --- a/tests/shared/src/GeometryUtilTests.cpp +++ b/tests/shared/src/GeometryUtilTests.cpp @@ -14,6 +14,7 @@ #include "GeometryUtilTests.h" #include +#include #include #include @@ -22,6 +23,63 @@ QTEST_MAIN(GeometryUtilTests) +static void testSphereVsCone(const glm::vec3 coneNormal, const glm::vec3 coneBiNormal, float coneAngle, float sphereRadius, float sphereDistance) { + + glm::vec3 u, v, w; + generateBasisVectors(coneNormal, coneBiNormal, u, v, w); + glm::vec3 coneEdge = u * cosf(coneAngle) + v * sinf(coneAngle); + glm::vec3 coneCenter = glm::vec3(0.0f, 0.0f, 0.0f); + glm::vec3 sphereCenter = coneCenter + coneEdge * sphereDistance; + float result = coneSphereAngle(coneCenter, u, sphereCenter, sphereRadius); + QCOMPARE(isnan(result), false); + QCOMPARE(result < coneAngle, true); + + // push sphere outward from edge so it is tangent to the cone. + glm::vec3 sphereOffset = glm::angleAxis(PI / 2.0f, w) * coneEdge; + sphereCenter += sphereOffset * sphereRadius; + result = coneSphereAngle(coneCenter, u, sphereCenter, sphereRadius); + QCOMPARE(isnan(result), false); + QCOMPARE_WITH_ABS_ERROR(result, coneAngle, 0.001f); + + // push sphere outward from edge a bit further, so it is outside of the cone. + sphereCenter += 0.1f * sphereOffset; + result = coneSphereAngle(coneCenter, u, sphereCenter, sphereRadius); + QCOMPARE(isnan(result), false); + QCOMPARE(result > coneAngle, true); +} + +void GeometryUtilTests::testConeSphereAngle() { + + // start with a 45 degree cone. + testSphereVsCone(glm::vec3(1.0f, 0.0f, 0.0f), glm::vec3(0.0f, 1.0f, 0.0f), PI / 4.0f, 1.0f, 10.0f); + + // test 30 degree cone. + testSphereVsCone(glm::vec3(1.0f, 0.0f, 0.0f), glm::vec3(0.0f, 1.0f, 0.0f), PI / 6.0f, 1.0f, 10.0f); + + // test 60 degree cone. + testSphereVsCone(glm::vec3(1.0f, 0.0f, 0.0f), glm::vec3(0.0f, 1.0f, 0.0f), PI / 3.0f, 1.0f, 10.0f); + + // test 120 degree cone. + testSphereVsCone(glm::vec3(1.0f, 0.0f, 0.0f), glm::vec3(0.0f, 1.0f, 0.0f), 2 * PI / 3.0f, 1.0f, 10.0f); + + // test skinny cone. + testSphereVsCone(glm::vec3(1.0f, 0.0f, 0.0f), glm::vec3(0.0f, 1.0f, 0.0f), 0.0001f, 1.0f, 10.0f); + + // start again with a 45 off axis cone. + testSphereVsCone(glm::vec3(1.0f, 1.0f, 1.0f), glm::vec3(0.0f, 1.0f, 0.0f), PI / 4.0f, 1.0f, 10.0f); + + // test 30 degree off axis cone + testSphereVsCone(glm::vec3(1.0f, 1.0f, 1.0f), glm::vec3(0.0f, 1.0f, 0.0f), PI / 6.0f, 1.0f, 10.0f); + + // test 60 degree cone off axis cone + testSphereVsCone(glm::vec3(1.0f, 1.0f, 1.0f), glm::vec3(0.0f, 1.0f, 0.0f), PI / 3.0f, 1.0f, 10.0f); + + // test 120 degree off axis cone. + testSphereVsCone(glm::vec3(1.0f, 1.0f, 1.0f), glm::vec3(0.0f, 1.0f, 0.0f), 2 * PI / 3.0f, 1.0f, 10.0f); + + // test skinny off-axis cone. + testSphereVsCone(glm::vec3(1.0f, 1.0f, 1.0f), glm::vec3(0.0f, 1.0f, 0.0f), 0.0001f, 1.0f, 10.0f); +} void GeometryUtilTests::testLocalRayRectangleIntersection() { glm::vec3 xAxis(1.0f, 0.0f, 0.0f); diff --git a/tests/shared/src/GeometryUtilTests.h b/tests/shared/src/GeometryUtilTests.h index b6340cd8cf..3b5ad3da51 100644 --- a/tests/shared/src/GeometryUtilTests.h +++ b/tests/shared/src/GeometryUtilTests.h @@ -18,6 +18,7 @@ class GeometryUtilTests : public QObject { Q_OBJECT private slots: + void testConeSphereAngle(); void testLocalRayRectangleIntersection(); void testWorldRayRectangleIntersection(); void testTwistSwingDecomposition();