diff --git a/src/simplnx/Common/Ray.hpp b/src/simplnx/Common/Ray.hpp index 7ffa671216..db378eb632 100644 --- a/src/simplnx/Common/Ray.hpp +++ b/src/simplnx/Common/Ray.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include "simplnx/Common/Array.hpp" #include "simplnx/Common/EulerAngle.hpp" @@ -16,7 +17,7 @@ namespace nx::core * simplify and better describe values used in GeometryMath. The rays initial alignment * is assumed to be with the Y-axis. */ -template +template class Ray { public: @@ -41,11 +42,15 @@ class Ray * @param ang * @param length */ - Ray(const PointType& origin, const ZXZEulerType& ang, LengthType length) + Ray(const PointType& origin, ZXZEulerType ang, LengthType length) : m_Origin(origin) - , m_Angle(ang) + , m_Angle(std::move(ang)) , m_Length(length) { + if constexpr(CacheEndV) + { + m_Endpoint = calculateEndpoint(); + } } /** @@ -85,12 +90,11 @@ class Ray * @brief Returns the origin point. * @return const PointType& */ - const PointType& getOrigin() const + const PointType& getOriginRef() const { return m_Origin; } - /** * @brief Returns the Euler angle describing the Ray's direction. * @return ZXZEulerType @@ -143,24 +147,19 @@ class Ray */ PointType getEndPoint() const { - if(!m_Endpoint.toArray().empty()) + if constexpr(CacheEndV) { - const auto sin1 = std::sin(m_Angle[0]); - const auto sin2 = std::sin(m_Angle[1]); - const auto sin3 = std::sin(m_Angle[2]); - - const auto cos1 = std::cos(m_Angle[0]); - const auto cos2 = std::cos(m_Angle[1]); - const auto cos3 = std::cos(m_Angle[2]); - - // Reference: https://ntrs.nasa.gov/api/citations/19770019231/downloads/19770019231.pdf Page:23 - Vec3 localXRotationVec((-sin1 * cos2 * sin3) + (cos1 * cos3), (cos1 * cos2 * sin3) + (sin1 * cos3), sin2 * sin3); - Vec3 localYRotationVec((-sin1 * cos2 * cos3) - (cos1 * cos3), (cos1 * cos2 * cos3) - (sin1 * sin3), sin2 * cos3); - Vec3 localZRotationVec((sin1 * sin2), -cos1 * sin2, cos2); - - m_Endpoint = m_Origin + (localXRotationVec * m_Length) + (localYRotationVec * m_Length) + (localZRotationVec * m_Length); + return m_Endpoint; + } + else + { + return calculateEndpoint(); } + } + template > + const PointType& getEndPointRef() const + { return m_Endpoint; } @@ -234,5 +233,23 @@ class Ray // Optional caching for speed PointType m_Endpoint = {}; + + PointType calculateEndpoint() const + { + const auto sin1 = std::sin(m_Angle[0]); + const auto sin2 = std::sin(m_Angle[1]); + const auto sin3 = std::sin(m_Angle[2]); + + const auto cos1 = std::cos(m_Angle[0]); + const auto cos2 = std::cos(m_Angle[1]); + const auto cos3 = std::cos(m_Angle[2]); + + // Reference: https://ntrs.nasa.gov/api/citations/19770019231/downloads/19770019231.pdf Page:23 + Vec3 localXRotationVec((-sin1 * cos2 * sin3) + (cos1 * cos3), (cos1 * cos2 * sin3) + (sin1 * cos3), sin2 * sin3); + Vec3 localYRotationVec((-sin1 * cos2 * cos3) - (cos1 * cos3), (cos1 * cos2 * cos3) - (sin1 * sin3), sin2 * cos3); + Vec3 localZRotationVec((sin1 * sin2), -cos1 * sin2, cos2); + + return m_Origin + (localXRotationVec * m_Length) + (localYRotationVec * m_Length) + (localZRotationVec * m_Length); + } }; } // namespace nx::core diff --git a/src/simplnx/Utilities/Math/GeometryMath.hpp b/src/simplnx/Utilities/Math/GeometryMath.hpp index c7e95a1490..2d247c0058 100644 --- a/src/simplnx/Utilities/Math/GeometryMath.hpp +++ b/src/simplnx/Utilities/Math/GeometryMath.hpp @@ -256,11 +256,31 @@ inline bool IsPointInBox(const nx::core::Point3D& point, const nx::core::Boun template bool DoesRayIntersectBox(const nx::core::Ray& ray, const nx::core::BoundingBox3D& bounds) { - const auto& origin = ray.getOrigin(); + const auto& origin = ray.getOriginRef(); const auto& min = bounds.getMinPoint(); const auto& max = bounds.getMaxPoint(); - auto end = ray.getEndPoint(); + auto end = ray.getEndPointRef(); + + return !((min[0] > origin[0]) && (min[0] > end[0]) && (min[1] > origin[1]) && (min[1] > end[1]) && (min[2] > origin[2]) && (min[2] > end[2]) && + (max[0] < origin[0]) && (max[0] < end[0]) && (max[1] < origin[1]) && (max[1] < end[1]) && (max[2] < origin[2]) && (max[2] < end[2])); +} + +/** + * @brief [Optimized Overload] Returns true if a ray intersects the specified box. Returns false + * otherwise. + * @param ray + * @param bounds + * @return bool + */ +template +bool DoesRayIntersectBox(const nx::core::Ray& ray, const nx::core::BoundingBox3D& bounds) +{ + const auto& origin = ray.getOriginRef(); + const auto& min = bounds.getMinPoint(); + const auto& max = bounds.getMaxPoint(); + + const auto& end = ray.getEndPointRef(); return !((min[0] > origin[0]) && (min[0] > end[0]) && (min[1] > origin[1]) && (min[1] > end[1]) && (min[2] > origin[2]) && (min[2] > end[2]) && (max[0] < origin[0]) && (max[0] < end[0]) && (max[1] < origin[1]) && (max[1] < end[1]) && (max[2] < origin[2]) && (max[2] < end[2])); @@ -446,6 +466,52 @@ char RayCrossesTriangle(const Point3D& p0, const Point3D& p1, const Point3 return '0'; } +/** + * @brief [Optimized Overload] Returns true if the specified Ray crosses into or out of the triangle + * defined by its corner points. Returns false otherwise. + * + * Tangential intersections where the Ray touches the triangle but does not + * enter or leave will return false. + * @param ray + * @param p0 + * @param p1 + * @param p2 + * @return bool + */ +template +char RayCrossesTriangle(const Point3D& p0, const Point3D& p1, const Point3D& p2, const Ray& ray) +{ + T vol0 = FindTetrahedronVolume(ray.getOrigin(), p0, p1, ray.getEndPointRef()); + T vol1 = FindTetrahedronVolume(ray.getOrigin(), p1, p2, ray.getEndPointRef()); + T vol2 = FindTetrahedronVolume(ray.getOrigin(), p2, p0, ray.getEndPointRef()); + + bool hasPos = vol0 > 0 || vol1 > 0 || vol2 > 0; + bool hasNeg = vol0 < 0 || vol1 < 0 || vol2 < 0; + int32 zeroCount = !vol0 + !vol1 + !vol2; + + if(!(hasPos && hasNeg)) + { + if(zeroCount == 0) + { + return 'f'; + } + else if(zeroCount == 1) + { + return 'e'; + } + else if(zeroCount == 2) + { + return 'v'; + } + else if(zeroCount == 3) + { + return '?'; + } + } + + return '0'; +} + /** * @brief Returns true if the specified Ray intersects a plan defined by three * points along the surface. Returns false otherwise. @@ -493,6 +559,53 @@ char RayIntersectsPlane(const Ray& ray, const Point3D& p0, const Point3D +char RayIntersectsPlane(const Ray& ray, const Point3D& p0, const Point3D& p1, const Point3D& p2) +{ + Vec3 normal = FindPlaneNormalVector(p0, p1, p2); + T d = FindPlaneCoefficients(p0, normal); + + T numerator = d - ray.getOrigin().dot(normal); + + Point3D rq = ray.getEndPoint() - ray.getOrigin(); + T denominator = rq.dot(normal); + + if(denominator == 0.0) + { + if(numerator == 0.0) + { + return 'p'; + } + + return '0'; + } + + T t = numerator / denominator; + if(t > 0.0 && t < 1.0) + { + return '1'; + } + else if(numerator == 0.0) + { + return 'q'; + } + else if(numerator == denominator) + { + return 'r'; + } + + return '0'; +} + /** * @brief Checks if the specified Ray intersects a triangle defined by the * corner points. The inter parameter is updated to reflect the points at which @@ -530,6 +643,43 @@ char RayIntersectsTriangle(const Ray& ray, const nx::core::Point3D& p0, co } } +/** + * @brief [Optimized Overload] Checks if the specified Ray intersects a triangle defined by the + * corner points. The inter parameter is updated to reflect the points at which + * the ray intersects the triangle. Returns the number of points at which the + * Ray intersects the triangle. + * @param ray + * @param p0 + * @param p1 + * @param p2 + * @param inter + * @return char + */ +template +char RayIntersectsTriangle(const Ray& ray, const nx::core::Point3D& p0, const nx::core::Point3D& p1, const nx::core::Point3D& p2) +{ + char code = RayIntersectsPlane(ray, p0, p1, p2); + + // Specifically use if-else chain to try to get the compiler to make + // switch-like optimizations over the chain because 'code' is unbounded + if(code == '1') + { + return RayCrossesTriangle(p0, p1, p2, ray); + } + else if(code == 'q') + { + return IsPointInTriangle(p0, p1, p2, ray.getOrigin()); + } + else if(code == 'r') + { + return IsPointInTriangle(p0, p1, p2, ray.getEndPointRef()); + } + else + { + return code; + } +} + /** * @breif !!!Uses unseeded randomness, but only for validity checks [SHOULD not affect outcomes]!!! Determines if a point is in the polyhedron * @param faces the geometry to query @@ -579,8 +729,8 @@ char IsPointInPolyhedron(const nx::core::TriangleGeom& triangleGeomRef, const st eulerAngles[0] = w * std::cos(t); eulerAngles[1] = w * std::sin(t); - // Generate and add ray to point to find other end - Ray ray(point, ZXZEuler(eulerAngles.data()), radius); + // Generate and add ray to point to find other end [optimized version with lifetime caching] + Ray ray(point, ZXZEuler(eulerAngles.data()), radius); bool doNextCheck = false; for(usize face = 0; face < numFaces; face++)