GeometryUtils: coneSphereAngle test

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);
+}

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];
 

tests/shared/src/GeometryUtilTests.cpp

@@ -14,6 +14,7 @@
 #include "GeometryUtilTests.h"
 
 #include <GeometryUtil.h>
+#include <GLMHelpers.h>
 #include <NumericalConstants.h>
 #include <StreamUtils.h>
 
@@ -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);

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();