Add a new implementation for cone/ray intersect to simplify code and fix issue with current implementation (#3902)

* add a new implementation for cone/ray intersect to simplify and fix existing issue Signed-off-by: hultonha <hultonha@amazon.co.uk> * move new ray/cone intersection function to AzToolsFramework - repond to PR comments Signed-off-by: hultonha <hultonha@amazon.co.uk> * updates following PR feedback Signed-off-by: hultonha <hultonha@amazon.co.uk> * add additional comment to give more context to the intersection function Signed-off-by: hultonha <hultonha@amazon.co.uk> * update google test expect usage Signed-off-by: hultonha <hultonha@amazon.co.uk>
2021-09-06 11:36:17 +01:00
parent 15e889a2f6
commit 6cc9a33845
6 changed files with 161 additions and 7 deletions
@@ -16,6 +16,79 @@ namespace AzToolsFramework
 {
    namespace Picking
    {
+        // this intersection algorithm is adapted from 'Capped Cone' by Inigo Quilez
+        // ref: https://www.iquilezles.org/www/articles/intersectors/intersectors.htm and https://www.shadertoy.com/view/llcfRf
+        // all algorithms/code snippets are kindly made available under the MIT License - https://www.iquilezles.org/www/index.htm
+        bool IntersectRayCone(
+            const AZ::Vector3& rayOrigin,
+            const AZ::Vector3& rayDirection,
+            const AZ::Vector3& coneApex,
+            const AZ::Vector3& coneAxis,
+            float coneHeight,
+            float coneBaseRadius,
+            float& t)
+        {
+            const AZ::Vector3& pa = coneApex;
+            const AZ::Vector3& pb = coneApex + coneHeight * coneAxis;
+            const AZ::Vector3& ro = rayOrigin;
+            const AZ::Vector3& rd = rayDirection;
+            float rb = coneBaseRadius;
+
+            AZ::Vector3 ba = pb - pa;
+            AZ::Vector3 oa = ro - pa;
+            AZ::Vector3 ob = ro - pb;
+
+            float m0 = ba.Dot(ba);
+            float m1 = oa.Dot(ba);
+            float m2 = ob.Dot(ba);
+            float m3 = rd.Dot(ba);
+
+            auto dot2 = [](const AZ::Vector3& v)
+            {
+                return v.Dot(v);
+            };
+
+            // cap
+            if (m2 > 0.0f)
+            {
+                if (dot2(ob * m3 - rd * m2) < (rb * rb * m3 * m3))
+                {
+                    t = -m2 / m3;
+                    return true;
+                }
+            }
+
+            // body
+            float m4 = rd.Dot(oa);
+            float m5 = oa.Dot(oa);
+            float hy = m0 + rb * rb;
+
+            float k2 = m0 * m0 - m3 * m3 * hy;
+            float k1 = m0 * m0 * m4 - m1 * m3 * hy;
+            float k0 = m0 * m0 * m5 - m1 * m1 * hy;
+
+            // note: solving for simultaneously being on the sloping surface of the cone and being on the ray boils down
+            // to a quadratic equation - the discriminant of the quadratic determines if there are 1, 2 or no solutions
+            //
+            // if the discriminant is less than 0 the ray is not intersecting the cone, if it is equal to 0 then the ray
+            // is intersecting the cone once and if it is greater than 0 the ray is intersecting the cone twice
+            float discriminant = k1 * k1 - k2 * k0;
+            if (discriminant < 0.0f)
+            {
+                return false;
+            }
+
+            float tt = (-k1 - AZ::Sqrt(discriminant)) / k2;
+            float y = m1 + tt * m3;
+            if (y >= 0.0f && y < m0)
+            {
+                t = tt;
+                return true;
+            }
+
+            return false;
+        }
+
        bool ManipulatorBoundSphere::IntersectRay(
            const AZ::Vector3& rayOrigin, const AZ::Vector3& rayDirection, float& rayIntersectionDistance)
        {
@@ -86,11 +159,10 @@ namespace AzToolsFramework
        bool ManipulatorBoundCone::IntersectRay(
            const AZ::Vector3& rayOrigin, const AZ::Vector3& rayDirection, float& rayIntersectionDistance)
        {
-            float t1 = std::numeric_limits<float>::max();
-            float t2 = std::numeric_limits<float>::max();
-            if (AZ::Intersect::IntersectRayCone(rayOrigin, rayDirection, m_apexPosition, m_dir, m_height, m_radius, t1, t2) > 0)
+            float t = std::numeric_limits<float>::max();
+            if (IntersectRayCone(rayOrigin, rayDirection, m_apexPosition, m_dir, m_height, m_radius, t))
            {
-                rayIntersectionDistance = AZStd::GetMin(t1, t2);
+                rayIntersectionDistance = t;
                return true;
            }

@@ -23,6 +23,26 @@ namespace AzToolsFramework
 {
    namespace Picking
    {
+        //! Custom ray/cone intersect function for manipulator bounds to workaround a bug in the current AZ::Intersect implementation.
+        //! @note Does not give reliable results if the ray origin is inside the cone.
+        //! @param      rayOrigin      The origin of the ray to test.
+        //! @param      rayDirection   The direction of the ray to test, it must be unit length.
+        //! @param      coneApex       The apex of the cone.
+        //! @param      coneAxis       The unit-length axis (direction) from the apex to the base.
+        //! @param      coneHeight     The height of the cone, from the apex to the base.
+        //! @param      coneBaseRadius The radius of the cone base.
+        //! @param[out] t              A possible coefficient in the ray's explicit equation from which an intersecting point is calculated
+        //!                            as "rayOrigin + t * rayDirection". 't' is the closest intersecting point to the ray origin.
+        //! @return                    true for intersecting, false for not intersecting.
+        bool IntersectRayCone(
+            const AZ::Vector3& rayOrigin,
+            const AZ::Vector3& rayDirection,
+            const AZ::Vector3& coneApex,
+            const AZ::Vector3& coneAxis,
+            float coneHeight,
+            float coneBaseRadius,
+            float& t);
+
        class ManipulatorBoundSphere : public BoundShapeInterface
        {
        public: