From 6cc9a3384525b7641b0e7a79dba9b3ebcd6a0a1b Mon Sep 17 00:00:00 2001 From: hultonha <82228511+hultonha@users.noreply.github.com> Date: Mon, 6 Sep 2021 11:36:17 +0100 Subject: [PATCH] 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 * move new ray/cone intersection function to AzToolsFramework - repond to PR comments Signed-off-by: hultonha * updates following PR feedback Signed-off-by: hultonha * add additional comment to give more context to the intersection function Signed-off-by: hultonha * update google test expect usage Signed-off-by: hultonha --- .../AzCore/AzCore/Math/IntersectSegment.cpp | 4 +- .../AzCore/AzCore/Math/IntersectSegment.h | 2 +- .../AzCore/Tests/Math/IntersectionTests.cpp | 15 ++++ .../Manipulators/ManipulatorBounds.cpp | 80 ++++++++++++++++++- .../Picking/Manipulators/ManipulatorBounds.h | 20 +++++ .../Tests/ManipulatorBoundsTests.cpp | 47 +++++++++++ 6 files changed, 161 insertions(+), 7 deletions(-) diff --git a/Code/Framework/AzCore/AzCore/Math/IntersectSegment.cpp b/Code/Framework/AzCore/AzCore/Math/IntersectSegment.cpp index fa52599c4a..a7a0d5197e 100644 --- a/Code/Framework/AzCore/AzCore/Math/IntersectSegment.cpp +++ b/Code/Framework/AzCore/AzCore/Math/IntersectSegment.cpp @@ -628,7 +628,7 @@ int AZ::Intersect::IntersectRayCappedCylinder( int AZ::Intersect::IntersectRayCone( const Vector3& rayOrigin, const Vector3& rayDir, const Vector3& coneApex, const Vector3& coneDir, float coneHeight, - float coneBaseRaidus, float& t1, float& t2) + float coneBaseRadius, float& t1, float& t2) { // Q = rayOrgin, A = coneApex Vector3 AQ = rayOrigin - coneApex; @@ -646,7 +646,7 @@ int AZ::Intersect::IntersectRayCone( return 0; } - float r2 = coneBaseRaidus * coneBaseRaidus; + float r2 = coneBaseRadius * coneBaseRadius; float h2 = coneHeight * coneHeight; float m2 = m * m; diff --git a/Code/Framework/AzCore/AzCore/Math/IntersectSegment.h b/Code/Framework/AzCore/AzCore/Math/IntersectSegment.h index 3dffb90d7c..df3d5e10fb 100644 --- a/Code/Framework/AzCore/AzCore/Math/IntersectSegment.h +++ b/Code/Framework/AzCore/AzCore/Math/IntersectSegment.h @@ -240,7 +240,7 @@ namespace AZ //! @return The number of intersecting points. int IntersectRayCone( const Vector3& rayOrigin, const Vector3& rayDir, - const Vector3& coneApex, const Vector3& coneDir, float coneHeight, float coneBaseRaidus, + const Vector3& coneApex, const Vector3& coneDir, float coneHeight, float coneBaseRadius, float& t1, float& t2); //! Test intersection between a ray and a plane in 3D. diff --git a/Code/Framework/AzCore/Tests/Math/IntersectionTests.cpp b/Code/Framework/AzCore/Tests/Math/IntersectionTests.cpp index 5bebe5e4f2..e8a52f8fba 100644 --- a/Code/Framework/AzCore/Tests/Math/IntersectionTests.cpp +++ b/Code/Framework/AzCore/Tests/Math/IntersectionTests.cpp @@ -609,6 +609,21 @@ namespace UnitTest EXPECT_EQ(hits, 0); } + // replicates a scenario in the Editor using a cone and a pick ray which should have failed but passed + // note: To replicate this, select an entity so the default translation manipulator appears, move very close to the + // entity, hover the mouse over one of the manipulator linear manipulator arrows (cone part) and move the mouse away + // notice the manipulator will remain highlighted as a successful intersection is still reported + TEST(MATH_IntersectRayConeTestEditor, DISABLED_RayConeEditorScenarioTest) + { + auto rayOrigin = Vector3(0.0f, -0.808944702f, 0.0f); + auto rayDir = Vector3(0.301363617f, 0.939044654f, 0.165454566f); + float t1 = 0.0f; + float t2 = 0.0f; + int hits = Intersect::IntersectRayCone( + rayOrigin, rayDir, AZ::Vector3(0.0f, 0.0f, 0.161788940f), AZ::Vector3(0.0f, 0.0f, -1.0f), 0.0453009047, 0.0113252262, t1, t2); + EXPECT_EQ(hits, 0); + } + class MATH_IntersectRayQuadTest : public AllocatorsFixture { diff --git a/Code/Framework/AzToolsFramework/AzToolsFramework/Picking/Manipulators/ManipulatorBounds.cpp b/Code/Framework/AzToolsFramework/AzToolsFramework/Picking/Manipulators/ManipulatorBounds.cpp index 9ede35b837..3a7fffb3be 100644 --- a/Code/Framework/AzToolsFramework/AzToolsFramework/Picking/Manipulators/ManipulatorBounds.cpp +++ b/Code/Framework/AzToolsFramework/AzToolsFramework/Picking/Manipulators/ManipulatorBounds.cpp @@ -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::max(); - float t2 = std::numeric_limits::max(); - if (AZ::Intersect::IntersectRayCone(rayOrigin, rayDirection, m_apexPosition, m_dir, m_height, m_radius, t1, t2) > 0) + float t = std::numeric_limits::max(); + if (IntersectRayCone(rayOrigin, rayDirection, m_apexPosition, m_dir, m_height, m_radius, t)) { - rayIntersectionDistance = AZStd::GetMin(t1, t2); + rayIntersectionDistance = t; return true; } diff --git a/Code/Framework/AzToolsFramework/AzToolsFramework/Picking/Manipulators/ManipulatorBounds.h b/Code/Framework/AzToolsFramework/AzToolsFramework/Picking/Manipulators/ManipulatorBounds.h index 55a276f87b..59f339c129 100644 --- a/Code/Framework/AzToolsFramework/AzToolsFramework/Picking/Manipulators/ManipulatorBounds.h +++ b/Code/Framework/AzToolsFramework/AzToolsFramework/Picking/Manipulators/ManipulatorBounds.h @@ -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: diff --git a/Code/Framework/AzToolsFramework/Tests/ManipulatorBoundsTests.cpp b/Code/Framework/AzToolsFramework/Tests/ManipulatorBoundsTests.cpp index 1c3303269d..bca1624290 100644 --- a/Code/Framework/AzToolsFramework/Tests/ManipulatorBoundsTests.cpp +++ b/Code/Framework/AzToolsFramework/Tests/ManipulatorBoundsTests.cpp @@ -187,4 +187,51 @@ namespace UnitTest EXPECT_NEAR(intersectionDistance, 10.0f, g_epsilon); EXPECT_TRUE(intersection); } + + // replicates a scenario in the Editor using a cone and a pick ray which should have failed but passed with Intersect::IntersectRayCone + TEST(ManipulatorIntersectRayConeTest, RayConeEditorScenarioTest) + { + auto rayOrigin = AZ::Vector3(0.0f, -0.808944702f, 0.0f); + auto rayDir = AZ::Vector3(0.301363617f, 0.939044654f, 0.165454566f); + float t = 0.0f; + bool hit = AzToolsFramework::Picking::IntersectRayCone( + rayOrigin, rayDir, AZ::Vector3(0.0f, 0.0f, 0.161788940f), AZ::Vector3(0.0f, 0.0f, -1.0f), 0.0453009047, 0.0113252262, t); + EXPECT_FALSE(hit); + } + + // cone lying flat, ray going towards base of cone + TEST(ManipulatorIntersectRayConeTest, RayIntersectsConeBase) + { + auto rayOrigin = AZ::Vector3::CreateZero(); + auto rayDir = AZ::Vector3::CreateAxisY(); + float t = 0.0f; + bool hit = AzToolsFramework::Picking::IntersectRayCone( + rayOrigin, rayDir, AZ::Vector3::CreateAxisY(10.0f), AZ::Vector3::CreateAxisY(-1.0f), 5.0f, 1.0f, t); + EXPECT_TRUE(hit); + EXPECT_THAT(t, ::testing::FloatNear(5.0f, 0.0001f)); + } + + // cone standing up, ray going towards mid side of cone + TEST(ManipulatorIntersectRayConeTest, RayIntersectsConeSide) + { + auto rayOrigin = AZ::Vector3::CreateZero(); + auto rayDir = AZ::Vector3::CreateAxisY(); + float t = 0.0f; + bool hit = AzToolsFramework::Picking::IntersectRayCone( + rayOrigin, rayDir, AZ::Vector3(0.0f, 10.0f, 5.0f), AZ::Vector3::CreateAxisZ(-1.0f), 10.0f, 5.0f, t); + EXPECT_TRUE(hit); + EXPECT_THAT(t, ::testing::FloatNear(7.5f, 0.0001f)); + } + + // cone standing up, ray going towards mid side of cone + TEST(ManipulatorIntersectRayConeTest, RayIntersectsConeApex) + { + auto rayOrigin = AZ::Vector3::CreateZero(); + auto rayDir = AZ::Vector3::CreateAxisY(); + float t = 0.0f; + bool hit = AzToolsFramework::Picking::IntersectRayCone( + rayOrigin, rayDir, AZ::Vector3::CreateAxisY(2.5f), AZ::Vector3::CreateAxisY(1.0f), 5.0f, 1.0f, t); + EXPECT_TRUE(hit); + EXPECT_THAT(t, ::testing::FloatNear(2.5f, 0.0001f)); + } } // namespace UnitTest