diff --git a/src/Magnum/Math/Geometry/Intersection.h b/src/Magnum/Math/Geometry/Intersection.h index 91ca5eeca..02d008007 100644 --- a/src/Magnum/Math/Geometry/Intersection.h +++ b/src/Magnum/Math/Geometry/Intersection.h @@ -5,7 +5,7 @@ Copyright © 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018 Vladimír Vondruš - Copyright © 2016 Jonathan Hale + Copyright © 2016, 2018 Jonathan Hale Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), @@ -33,7 +33,9 @@ #include "Magnum/Math/Frustum.h" #include "Magnum/Math/Geometry/Distance.h" #include "Magnum/Math/Range.h" +#include "Magnum/Math/Vector2.h" #include "Magnum/Math/Vector3.h" +#include "Magnum/Math/Matrix4.h" namespace Magnum { namespace Math { namespace Geometry { namespace Intersection { @@ -143,7 +145,7 @@ template inline T planeLine(const Vector3& planePosition, const Vect } /** -@brief Intersection of a point and a camera frustum +@brief Intersection of a point and a frustum @param point Point @param frustum Frustum planes with normals pointing outwards @@ -155,19 +157,259 @@ points distance from the plane is negative) using @ref Distance::pointPlaneScale template bool pointFrustum(const Vector3& point, const Frustum& frustum); /** -@brief Intersection of an axis-aligned box and a camera frustum +@brief Intersection of a range and a frustum +@param range Range +@param frustum Frustum planes with normals pointing outwards + +Returns @cpp true @ce if the box intersects with the frustum. + +Uses the "p/n-vertex" approach: First converts the @ref Range3D into a representation +using center and extent which allows using the following condition for whether the +plane is intersecting the box: @f[ + \begin{array}{rcl} + d & = & \boldsymbol c \cdot \boldsymbol n \\ + r & = & \boldsymbol c \cdot \text{abs}(\boldsymbol n) \\ + d + r & < & -w + \end{array} +@f] + +for plane normal @f$ \boldsymbol n @f$ and determinant @f$ w @f$. + +@see @ref aabbFrustum() +*/ +template bool rangeFrustum(const Range3D& range, const Frustum& frustum); + +#ifdef MAGNUM_BUILD_DEPRECATED +/** +@brief @copybrief rangeFrustum() +@deprecated Use @ref rangeFrustum() instead. +*/ +template CORRADE_DEPRECATED("use rangeFrustum() instead") bool boxFrustum(const Range3D& box, const Frustum& frustum) { + return rangeFrustum(box, frustum); +} +#endif + +/** +@brief Intersection of an axis-aligned box and a frustum @param box Axis-aligned box @param frustum Frustum planes with normals pointing outwards -Returns @cpp true @ce if the box intersects with the camera frustum. +Returns @cpp true @ce if the box intersects with the frustum. + +Uses the same method as @ref rangeFrustum() "rangeFrustum()", but does not need to +convert to center/extents representation. +*/ +template bool aabbFrustum(const Vector3& center, const Vector3& extents, const Frustum& frustum); + +/** +@brief Intersection of a sphere and a frustum +@param center Sphere center +@param radius Sphere radius +@param frustum Frustum planes with normals pointing outwards + +Returns @cpp true @ce if the sphere intersects the frustum. + +Checks for each plane of the frustum whether the sphere is behind the plane (the +points distance larger than the sphere's radius) using @ref Distance::pointPlaneScaled(). +*/ +template bool sphereFrustum(const Vector3& center, T radius, const Frustum& frustum); + +/** +@brief Intersection of a point and a cone +@param p The point +@param origin Cone origin +@param normal Cone normal +@param angle Apex angle of the cone + +Returns @cpp true @ce if the point is inside the cone. + +Precomputes a portion of the intersection equation from @p angle and calls +@ref pointCone(const Vector3&, const Vector3&, const Vector3&, T) +*/ +template bool pointCone(const Vector3& p, const Vector3& origin, const Vector3& normal, Rad angle); + +/** +@brief Intersection of a point and a cone using precomputed values +@param p The point +@param coneOrigin Cone origin +@param coneNormal Cone normal +@param tanAngleSqPlusOne Precomputed portion of the cone intersection equation: + @cpp T(1) + Math::pow(Math::tan(angle/T(2)), T(2)) @ce + +Returns @cpp true @ce if the point is inside the cone. +*/ +template bool pointCone(const Vector3& p, const Vector3& coneOrigin, const Vector3& coneNormal, T tanAngleSqPlusOne); + +/** +@brief Intersection of a point and a double cone +@param p The point +@param coneOrigin Cone origin +@param coneNormal Cone normal +@param coneAngle Apex angle of the cone + +Returns @cpp true @ce if the point is inside the double cone. + +Precomputes a portion of the intersection equation from @p angle and calls +@ref pointDoubleCone(const Vector3&, const Vector3&, const Vector3&, T) +*/ +template bool pointDoubleCone(const Vector3& p, const Vector3& coneOrigin, const Vector3& coneNormal, Rad coneAngle); + +/** +@brief Intersection of a point and a double cone using precomputed values +@param p The point +@param coneOrigin Cone origin +@param coneNormal Cone normal +@param tanAngleSqPlusOne Precomputed portion of the cone intersection equation: + @cpp T(1) + Math::pow(Math::tan(angle/T(2)), T(2)) @ce + +Returns @cpp true @ce if the point is inside the double cone. + +Uses the result of precomputing @f$ x = \tan^2{\theta} + 1 @f$. +*/ +template bool pointDoubleCone(const Vector3& p, const Vector3& coneOrigin, const Vector3& coneNormal, T tanAngleSqPlusOne); + +/** +@brief Intersection of a sphere and a cone view +@param sphereCenter Center of the sphere +@param radius Radius of the sphere +@param coneView View matrix with translation and rotation of the cone +@param coneAngle Cone opening angle + +Returns @cpp true @ce if the sphere intersects the cone. + +Transforms the sphere center into cone space (using the cone view matrix) and +performs sphere-cone intersection with the zero-origin -Z axis-aligned cone. + +Precomputes a portion of the intersection equation from @p angle and calls +@ref sphereConeView(const Vector3&, T, const Matrix4&, T, T, T) +*/ +template bool sphereConeView(const Vector3& sphereCenter, T radius, const Matrix4& coneView, Rad coneAngle); + +/** +@brief Intersection of a sphere and a cone view +@param sphereCenter Sphere center +@param radius Sphere radius +@param coneView View matrix with translation and rotation of the cone +@param sinAngle Precomputed sine of half the cone's opening angle: @cpp Math::sin(angle/T(2)) @ce +@param cosAngle Precomputed cosine of half the cone's opening angle: @cpp Math::cos(angle/T(2)) @ce +@param tanAngle Precomputed tangens of half the cone's opening angle: @cpp Math::tan(angle/T(2)) @ce + +Returns @cpp true @ce if the sphere intersects the cone. +*/ +template bool sphereConeView(const Vector3& sphereCenter, T radius, const Matrix4& coneView, T sinAngle, T cosAngle, T tanAngle); + +/** +@brief Intersection of a sphere and a cone +@param sphereCenter Sphere center +@param radius Sphere Sphere radius +@param coneOrigin Cone origin +@param coneNormal Cone normal +@param coneAngle Cone opening angle (@f$ 0 < \text{angle} < \pi @f$). + +Returns @cpp true @ce if the sphere intersects with the cone. + +Offsets the cone plane by @f$ -r\sin{\theta} \cdot \boldsymbol n @f$ (with @f$ \theta @f$ +the cone's half-angle) which separates two half-spaces: +In front of the plane, in which the sphere cone intersection test is equivalent +to testing the sphere's center against a similarly offset cone (which is equivalent +the cone with surface expanded by @f$ r @f$ in surface normal direction), and +behind the plane, where the test is equivalent to testing whether the origin of +the original cone intersects the sphere. + +Precomputes a portion of the intersection equation from @p angle and calls +@ref sphereCone(const Vector3& sphereCenter, T, const Vector3&, const Vector3&, T, T) +*/ +template bool sphereCone(const Vector3& sphereCenter, T radius, const Vector3& coneOrigin, const Vector3& coneNormal, Rad coneAngle); + +/** +@brief Intersection of a sphere and a cone using precomputed values +@param sphereCenter Sphere center +@param radius Sphere radius +@param coneOrigin Cone origin +@param coneNormal Cone normal +@param sinAngle Precomputed sine of half the cone's opening angle: + @cpp Math::sin(angle/T(2)) @ce +@param tanAngleSqPlusOne Precomputed portion of the cone intersection equation: + @cpp Math::pow(Math::tan(angle/T(2)), 2) + 1 @ce + +Returns @cpp true @ce if the sphere intersects with the cone. +*/ +template bool sphereCone(const Vector3& sphereCenter, T radius, const Vector3& coneOrigin, const Vector3& coneNormal, T sinAngle, T tanAngleSqPlusOne); + +/** +@brief Intersection of an axis aligned bounding box and a cone +@param center Center of the AABB +@param extents (Half-)extents of the AABB +@param coneOrigin Cone origin +@param coneNormal Cone normal +@param coneAngle Cone opening angle + +Returns @cpp true @ce if the axis aligned bounding box intersects the cone. + +On each axis finds the intersection points of the cone's axis with infinite planes +obtained by extending the two faces of the box that are perpendicular to that axis. + +The intersection points on the planes perpendicular to axis @f$ a \in {0, 1, 2} @f$ +are given by @f[ + \boldsymbol i = \boldsymbol n \cdot \frac{(\boldsymbol c_a - \boldsymbol o_a) \pm \boldsymbol e_a}{\boldsymbol n_a} +@f] + +with normal @f$ n @f$, cone origin @f$ o @f$, box center @f$ x @f$ and box extents @f$ e @f$. + +The points on the faces that are closest to this intersection point are the closest +to the cone's axis and are tested for intersection with the cone using +@ref pointCone(const Vector3&, const Vector3&, const Vector3&, const T) "pointCone()". + +As soon as an intersecting point is found, the function returns @cpp true @ce. +If all points lie outside of the cone, it will return @cpp false @ce. + +Precomputes a portion of the intersection equation from @p angle and calls +@ref aabbCone(const Vector3&, const Vector3&, const Vector3&, const Vector3&, T) +*/ +template bool aabbCone(const Vector3& center, const Vector3& extents, const Vector3& origin, const Vector3& normal, Rad angle); + +/** +@brief Intersection of an axis aligned bounding box and a cone using precomputed values +@param center Center of the AABB +@param extents (Half-)extents of the AABB +@param origin Cone origin +@param normal Cone normal +@param tanAngleSqPlusOne Precomputed portion of the cone intersection equation: + @cpp T(1) + Math::pow(Math::tan(angle/T(2)), T(2)) @ce + +Returns @cpp true @ce if the axis aligned bounding box intersects the cone. +*/ +template bool aabbCone(const Vector3& center, const Vector3& extents, const Vector3& origin, const Vector3& normal, T tanAngleSqPlusOne); + +/** +@brief Intersection of a range and a cone + +@param range Range +@param coneOrigin Cone origin +@param coneNormal Cone normal +@param coneAngle Cone opening angle + +Returns @cpp true @ce if the range intersects the cone. + +Converts the range into center/extents representation and passes it on to +@ref aabbCone(const Vector3&, const Vector3&, const Vector3&, const Vector3&, T) "aabbCone()". +*/ +template bool rangeCone(const Range3D& range, const Vector3& coneOrigin, const Vector3& coneNormal, const Rad angle); + +/** +@brief Intersection of a range and a cone using precomputed values +@param range Range +@param coneOrigin Cone origin +@param coneNormal Cone normal +@param tanAngleSqPlusOne Precomputed portion of the cone intersection equation: + @cpp T(1) + Math::pow(Math::tan(angle/T(2)), T(2)) @ce + +Returns @cpp true @ce if the range intersects the cone. -Counts for each plane of the frustum how many points of the box lie in front of -the plane (outside of the frustum). If none, the box must lie entirely outside -of the frustum and there is no intersection. Else, the box is considered as -intersecting, even if it is merely corners of the box overlapping with corners -of the frustum, since checking the corners is less efficient. +Converts the range into center/extents representation and passes it on to +@ref aabbCone(const Vector3&, const Vector3&, const Vector3&, const Vector3&, T) "aabbCone()". */ -template bool boxFrustum(const Range3D& box, const Frustum& frustum); +template bool rangeCone(const Range3D& range, const Vector3& coneOrigin, const Vector3& coneNormal, const T tanAngleSqPlusOne); template bool pointFrustum(const Vector3& point, const Frustum& frustum) { for(const Vector4& plane: frustum.planes()) { @@ -180,28 +422,207 @@ template bool pointFrustum(const Vector3& point, const Frustum& f return true; } -template bool boxFrustum(const Range3D& box, const Frustum& frustum) { - for(const Vector4& plane: frustum.planes()) { - bool cornerHit = 0; +template bool rangeFrustum(const Range3D& box, const Frustum& frustum) { + /* Convert to center/extent, avoiding division by 2 and instead comparing + to 2*-plane.w() later */ + const Vector3 center = box.min() + box.max(); + const Vector3 extent = box.max() - box.min(); - for(UnsignedByte c = 0; c != 8; ++c) { - const Vector3 corner = Math::lerp(box.min(), box.max(), Math::BoolVector<3>{c}); + for(const Vector4& plane: frustum.planes()) { + const Vector3 absPlaneNormal = Math::abs(plane.xyz()); - if(Distance::pointPlaneScaled(corner, plane) >= T(0)) { - cornerHit = true; - break; - } + const Float d = Math::dot(center, plane.xyz()); + const Float r = Math::dot(extent, absPlaneNormal); + if(d + r < -T(2)*plane.w()) { + return false; } + } - /* All corners are outside this plane */ - if(!cornerHit) return false; + return true; +} + +template bool aabbFrustum( + const Vector3& center, const Vector3& extents, const Frustum& frustum) +{ + for(const Vector4& plane: frustum.planes()) { + const Vector3 absPlaneNormal = Math::abs(plane.xyz()); + + const Float d = Math::dot(center, plane.xyz()); + const Float r = Math::dot(extents, absPlaneNormal); + if(d + r < -plane.w()) { + return false; + } } - /** @todo potentially check corners here to avoid false positives */ + return true; +} + +template bool sphereFrustum(const Vector3& center, const T radius, const Frustum& frustum) { + const T radiusSq = radius*radius; + + for(const Vector4& plane: frustum.planes()) { + /* The sphere is in front of one of the frustum planes (normals point + outwards) */ + if(Distance::pointPlaneScaled(center, plane) < -radiusSq) + return false; + } return true; } + +template bool pointCone(const Vector3& p, const Vector3& origin, const Vector3& normal, const Rad angle) { + const T x = T(1) + Math::pow(Math::tan(angle/T(2)), T(2)); + + return pointCone(p, origin, normal, x); +} + +template bool pointCone(const Vector3& p, const Vector3& origin, const Vector3& normal, const T tanAngleSqPlusOne) { + const Vector3 c = p - origin; + const T lenA = dot(c, normal); + + return lenA >= 0 && c.dot() <= lenA*lenA*tanAngleSqPlusOne; +} + +template bool pointDoubleCone(const Vector3& p, const Vector3& origin, const Vector3& normal, const Rad angle) { + const T x = T(1) + Math::pow(Math::tan(angle/T(2)), T(2)); + + return pointDoubleCone(p, origin, normal, x); +} + +template bool pointDoubleCone(const Vector3& p, const Vector3& origin, const Vector3& normal, const T tanAngleSqPlusOne) { + const Vector3 c = p - origin; + const T lenA = dot(c, normal); + + return c.dot() <= lenA*lenA*tanAngleSqPlusOne; +} + +template bool sphereConeView(const Vector3& sphereCenter, const T radius, const Matrix4& coneView, const Rad angle) { + const T sinAngle = Math::sin(angle/T(2)); + const T cosAngle = Math::cos(angle/T(2)); + const T tanAngle = Math::tan(angle/T(2)); + + return sphereConeView(sphereCenter, radius, coneView, sinAngle, cosAngle, tanAngle); +} + +template bool sphereConeView( + const Vector3& sphereCenter, const T radius, const Matrix4& coneView, + const T sinAngle, const T cosAngle, const T tanAngle) +{ + CORRADE_ASSERT(coneView.isRigidTransformation(), "Math::Geometry::Intersection::sphereConeView(): coneView must be rigid", false); + + /* Transform the sphere so that we can test against Z axis aligned origin cone instead */ + const Vector3 center = coneView.transformPoint(sphereCenter); + + /* Test against plane which determines whether to test against shifted cone or center-sphere */ + if (-center.z() > -radius*sinAngle) { + /* Point - axis aligned cone test, shifted so that the cone's surface is extended by the radius of the sphere */ + const T coneRadius = tanAngle*(center.z() - radius/sinAngle); + return center.xy().dot() <= coneRadius*coneRadius; + } else { + /* Simple sphere point check */ + return center.dot() <= radius*radius; + } + + return false; +} + +template bool sphereCone( + const Vector3& sCenter, const T radius, + const Vector3& origin, const Vector3& normal, const Rad angle) +{ + const T sinAngle = Math::sin(angle/T(2)); + const T tanAngleSqPlusOne = T(1) + Math::pow(Math::tan(angle/T(2)), T(2)); + + return sphereCone(sCenter, radius, origin, normal, sinAngle, tanAngleSqPlusOne); +} + +template bool sphereCone( + const Vector3& sCenter, const T radius, + const Vector3& origin, const Vector3& normal, + const T sinAngle, const T tanAngleSqPlusOne) +{ + const Vector3 diff = sCenter - origin; + + if (dot(diff - radius*sinAngle*normal, normal) > T(0)) { + /* point - cone test */ + const Vector3 c = sinAngle*diff + normal*radius; + const T lenA = dot(c, normal); + + return c.dot() <= lenA*lenA*(tanAngleSqPlusOne); + } else { + /* Simple sphere point check */ + return diff.dot() <= radius*radius; + } +} + +template bool aabbCone( + const Vector3& center, const Vector3& extents, const Vector3& origin, + const Vector3& normal, const Rad angle) +{ + const T x = T(1) + Math::pow(Math::tan(angle/T(2)), T(2)); + return aabbCone(center, extents, origin, normal, x); +} + +template bool aabbCone( + const Vector3& center, const Vector3& extents, + const Vector3& coneOrigin, const Vector3& coneNormal, const T tanAngleSqPlusOne) +{ + const Vector3 c = center - coneOrigin; + + for (const Int axis: {0, 1, 2}) { + const Int z = axis; + const Int x = (axis + 1) % 3; + const Int y = (axis + 2) % 3; + if(coneNormal[z] != T(0)) { + Float t0 = ((c[z] - extents[z])/coneNormal[z]); + Float t1 = ((c[z] + extents[z])/coneNormal[z]); + + const Vector3 i0 = coneNormal*t0; + const Vector3 i1 = coneNormal*t1; + + for(const auto& i : {i0, i1}) { + Vector3 closestPoint = i; + + if(i[x] - c[x] > extents[x]) { + closestPoint[x] = c[x] + extents[x]; + } else if(i[x] - c[x] < -extents[x]) { + closestPoint[x] = c[x] - extents[x]; + } + /* Else: normal intersects within x bounds */ + + if(i[y] - c[y] > extents[y]) { + closestPoint[y] = c[y] + extents[y]; + } else if(i[y] - c[y] < -extents[y]) { + closestPoint[y] = c[y] - extents[y]; + } + /* Else: normal intersects within Y bounds */ + + if (pointCone(closestPoint, {}, coneNormal, tanAngleSqPlusOne)) { + /* Found a point in cone and aabb */ + return true; + } + } + } + /* else: normal will intersect one of the other planes */ + } + + return false; +} + +template bool rangeCone(const Range3D& range, const Vector3& coneOrigin, const Vector3& coneNormal, const Rad angle) { + const Vector3 center = (range.min() + range.max())/T(2); + const Vector3 extents = (range.max() - range.min())/T(2); + const T x = T(1) + Math::pow(Math::tan(angle/T(2)), T(2)); + return aabbCone(center, extents, coneOrigin, coneNormal, x); +} + +template bool rangeCone(const Range3D& range, const Vector3& coneOrigin, const Vector3& coneNormal, const T tanAngleSqPlusOne) { + const Vector3 center = (range.min() + range.max())/T(2); + const Vector3 extents = (range.max() - range.min())/T(2); + return aabbCone(center, extents, coneOrigin, coneNormal, tanAngleSqPlusOne); +} + }}}} #endif diff --git a/src/Magnum/Math/Geometry/Test/CMakeLists.txt b/src/Magnum/Math/Geometry/Test/CMakeLists.txt index d6bd90d57..d4d8d42eb 100644 --- a/src/Magnum/Math/Geometry/Test/CMakeLists.txt +++ b/src/Magnum/Math/Geometry/Test/CMakeLists.txt @@ -27,8 +27,14 @@ corrade_add_test(MathGeometryDistanceTest DistanceTest.cpp LIBRARIES MagnumMathT target_compile_definitions(MathGeometryDistanceTest PRIVATE "CORRADE_GRACEFUL_ASSERT") corrade_add_test(MathGeometryIntersectionTest IntersectionTest.cpp LIBRARIES MagnumMathTestLib) +corrade_add_test(MathGeometryIntersectionBenchmark IntersectionBenchmark.cpp LIBRARIES MagnumMathTestLib) set_target_properties( MathGeometryDistanceTest MathGeometryIntersectionTest + MathGeometryIntersectionBenchmark PROPERTIES FOLDER "Magnum/Math/Geometry/Test") + +set_property(TARGET + MathGeometryIntersectionTest + APPEND PROPERTY COMPILE_DEFINITIONS "CORRADE_GRACEFUL_ASSERT") diff --git a/src/Magnum/Math/Geometry/Test/IntersectionBenchmark.cpp b/src/Magnum/Math/Geometry/Test/IntersectionBenchmark.cpp new file mode 100644 index 000000000..432371e5e --- /dev/null +++ b/src/Magnum/Math/Geometry/Test/IntersectionBenchmark.cpp @@ -0,0 +1,222 @@ +/* + This file is part of Magnum. + + Copyright © 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018 + Vladimír Vondruš + Copyright © 2018 Jonathan Hale + + Permission is hereby granted, free of charge, to any person obtaining a + copy of this software and associated documentation files (the "Software"), + to deal in the Software without restriction, including without limitation + the rights to use, copy, modify, merge, publish, distribute, sublicense, + and/or sell copies of the Software, and to permit persons to whom the + Software is furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included + in all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + DEALINGS IN THE SOFTWARE. +*/ + +#include + +#include +#include +#include + +#include "Magnum/Math/Geometry/Intersection.h" +#include "Magnum/Math/Angle.h" +#include "Magnum/Math/Matrix4.h" + +namespace Magnum { namespace Math { namespace Geometry { namespace Test { + +template bool rangeFrustumNaive(const Math::Range3D& box, const Math::Frustum& frustum) { + for(const Math::Vector4& plane: frustum.planes()) { + bool cornerHit = 0; + + for(UnsignedByte c = 0; c != 8; ++c) { + const Math::Vector3 corner = Math::lerp(box.min(), box.max(), Math::BoolVector<3>{c}); + + if(Distance::pointPlaneScaled(corner, plane) >= T(0)) { + cornerHit = true; + break; + } + } + + /* All corners are outside this plane */ + if(!cornerHit) return false; + } + + return true; +} + +/* @brief Ground truth, slow sphere cone intersection - calculating exact distances, + * no optimizations, no precomputations + * @param sphereCenter Sphere center + * @param radius Sphere radius + * @param origin Origin of the cone + * @param normal Cone normal + * @param angle Cone opening angle (0 < angle < pi). + * + * Returns `true` if the sphere intersects with the cone. */ +template bool sphereConeGT( + const Math::Vector3& sphereCenter, const T radius, + const Math::Vector3& origin, const Math::Vector3& normal, const Math::Rad angle) { + const Math::Vector3 diff = sphereCenter - origin; + const Math::Vector3 dir = diff.normalized(); + const Math::Rad halfAngle = angle/T(2); + + /* Compute angle between normal and point */ + const Math::Rad actual = Math::acos(dot(normal, dir)); + + /* Distance from cone surface */ + const T distanceFromCone = Math::sin(actual - halfAngle)*diff.length(); + + /* Either the sphere center lies in cone, or cone is max radius away from the cone */ + return actual <= halfAngle || distanceFromCone <= radius; +} + +template +Math::Matrix4 coneViewFromCone(const Math::Vector3& origin, const Math::Vector3& normal) { + return Math::Matrix4::lookAt(origin, origin + normal, Math::Vector3::yAxis()).inverted(); +} + +typedef Math::Vector2 Vector2; +typedef Math::Vector3 Vector3; +typedef Math::Vector4 Vector4; +typedef Math::Matrix4 Matrix4; +typedef Math::Frustum Frustum; +typedef Math::Range3D Range3D; +typedef Math::Deg Deg; +typedef Math::Rad Rad; + +struct IntersectionBenchmark: Corrade::TestSuite::Tester { + explicit IntersectionBenchmark(); + + void rangeFrustumNaive(); + void rangeFrustum(); + + void rangeCone(); + + void sphereFrustum(); + + void sphereConeNaive(); + void sphereCone(); + void sphereConeView(); + + Frustum _frustum; + std::tuple _cone; + Matrix4 _coneView; + + std::vector _boxes; + std::vector _spheres; +}; + +IntersectionBenchmark::IntersectionBenchmark() { + addBenchmarks({&IntersectionBenchmark::rangeFrustumNaive, + &IntersectionBenchmark::rangeFrustum, + + &IntersectionBenchmark::rangeCone, + + &IntersectionBenchmark::sphereFrustum, + + &IntersectionBenchmark::sphereConeNaive, + &IntersectionBenchmark::sphereCone, + &IntersectionBenchmark::sphereConeView}, 25); + + /* Generate random data for the benchmarks */ + std::random_device rnd; + std::mt19937 g(rnd()); + /* Position distribution */ + std::uniform_real_distribution pd(-10.0f, 10.0f); + /* Cone angle distribution */ + std::uniform_real_distribution ad(1.0f, 179.0f); + + _cone = std::make_tuple(Vector3{pd(g), pd(g), pd(g)}, + Vector3{pd(g), pd(g), pd(g)}.normalized(), + Rad(Deg(ad(g)))); + _coneView = coneViewFromCone(std::get<0>(_cone), std::get<1>(_cone)); + _frustum = Frustum::fromMatrix(_coneView*Matrix4::perspectiveProjection(std::get<2>(_cone), 1.0f, 0.001f, 100.0f)); + + _boxes.reserve(512); + _spheres.reserve(512); + for(int i = 0; i < 512; ++i) { + Vector3 center{pd(g), pd(g), pd(g)}; + Vector3 extents{pd(g), pd(g), pd(g)}; + _boxes.emplace_back(center - extents, center + extents); + _spheres.emplace_back(center, extents.length()); + } +} + +void IntersectionBenchmark::rangeFrustumNaive() { + volatile bool b = false; + CORRADE_BENCHMARK(50) for(auto& box: _boxes) { + b = b ^ Test::rangeFrustumNaive(box, _frustum); + } +} + +void IntersectionBenchmark::rangeFrustum() { + volatile bool b = false; + CORRADE_BENCHMARK(50) for(auto& box: _boxes) { + b = b ^ Intersection::rangeFrustum(box, _frustum); + } +} + +void IntersectionBenchmark::rangeCone() { + volatile bool b = false; + CORRADE_BENCHMARK(50) { + const Float tanAngle = Math::tan(std::get<2>(_cone)); + const Float tanAngleSqPlusOne = tanAngle*tanAngle + 1.0f; + for(auto& box: _boxes) { + b = b ^ Intersection::rangeCone(box, std::get<0>(_cone), std::get<1>(_cone), tanAngleSqPlusOne); + } + } +} + +void IntersectionBenchmark::sphereFrustum() { + volatile bool b = false; + CORRADE_BENCHMARK(50) for(auto& sphere: _spheres) { + b = b ^ Intersection::sphereFrustum(sphere.xyz(), sphere.w(), _frustum); + } +} + +void IntersectionBenchmark::sphereConeNaive() { + volatile bool b = false; + CORRADE_BENCHMARK(50) for(auto& sphere: _spheres) { + b = b ^ sphereConeGT(sphere.xyz(), sphere.w(), std::get<0>(_cone), std::get<1>(_cone), std::get<2>(_cone)); + } +} + +void IntersectionBenchmark::sphereCone() { + volatile bool b = false; + CORRADE_BENCHMARK(50) { + const Float sinAngle = Math::sin(std::get<2>(_cone)); + const Float tanAngle = Math::tan(std::get<2>(_cone)); + const Float tanAngleSqPlusOne = tanAngle*tanAngle + 1.0f; + for(auto& sphere: _spheres) { + b = b ^ Intersection::sphereCone(sphere.xyz(), sphere.w(), std::get<0>(_cone), std::get<1>(_cone), sinAngle, tanAngleSqPlusOne); + } + } +} + +void IntersectionBenchmark::sphereConeView() { + volatile bool b = false; + CORRADE_BENCHMARK(50) { + const Float sinAngle = Math::sin(std::get<2>(_cone)); + const Float cosAngle = Math::cos(std::get<2>(_cone)); + const Float tanAngle = Math::tan(std::get<2>(_cone)); + for(auto& sphere: _spheres) { + b = b ^ Intersection::sphereConeView(sphere.xyz(), sphere.w(), _coneView, sinAngle, cosAngle, tanAngle); + } + } +} + +}}}} + +CORRADE_TEST_MAIN(Magnum::Math::Geometry::Test::IntersectionBenchmark) diff --git a/src/Magnum/Math/Geometry/Test/IntersectionTest.cpp b/src/Magnum/Math/Geometry/Test/IntersectionTest.cpp index 94944ee1d..014532fe0 100644 --- a/src/Magnum/Math/Geometry/Test/IntersectionTest.cpp +++ b/src/Magnum/Math/Geometry/Test/IntersectionTest.cpp @@ -24,12 +24,16 @@ DEALINGS IN THE SOFTWARE. */ +#include #include #include "Magnum/Math/Geometry/Intersection.h" +#include "Magnum/Math/Angle.h" namespace Magnum { namespace Math { namespace Geometry { namespace Test { +using namespace Literals; + struct IntersectionTest: Corrade::TestSuite::Tester { explicit IntersectionTest(); @@ -37,22 +41,45 @@ struct IntersectionTest: Corrade::TestSuite::Tester { void lineLine(); void pointFrustum(); - void boxFrustum(); + void rangeFrustum(); + void aabbFrustum(); + void sphereFrustum(); + + void pointCone(); + void pointDoubleCone(); + void sphereCone(); + void sphereConeView(); + void rangeCone(); + void aabbCone(); }; typedef Math::Vector2 Vector2; typedef Math::Vector3 Vector3; +typedef Math::Vector3 Vector3d; typedef Math::Vector4 Vector4; +typedef Math::Matrix4 Matrix4; +typedef Math::Matrix4 Matrix4d; typedef Math::Frustum Frustum; typedef Math::Constants Constants; typedef Math::Range3D Range3D; +typedef Math::Rad Rad; +typedef Math::Rad Radd; IntersectionTest::IntersectionTest() { addTests({&IntersectionTest::planeLine, &IntersectionTest::lineLine, &IntersectionTest::pointFrustum, - &IntersectionTest::boxFrustum}); + &IntersectionTest::rangeFrustum, + &IntersectionTest::aabbFrustum, + &IntersectionTest::sphereFrustum, + + &IntersectionTest::pointCone, + &IntersectionTest::pointDoubleCone, + &IntersectionTest::sphereCone, + &IntersectionTest::sphereConeView, + &IntersectionTest::rangeCone, + &IntersectionTest::aabbCone}); } void IntersectionTest::planeLine() { @@ -124,20 +151,277 @@ void IntersectionTest::pointFrustum() { CORRADE_VERIFY(!Intersection::pointFrustum({0.0f, 0.0f, 100.0f}, frustum)); } -void IntersectionTest::boxFrustum() { +void IntersectionTest::rangeFrustum() { const Frustum frustum{ {1.0f, 0.0f, 0.0f, 0.0f}, - {-1.0f, 0.0f, 0.0f, 10.0f}, + {-1.0f, 0.0f, 0.0f, 5.0f}, {0.0f, 1.0f, 0.0f, 0.0f}, - {0.0f, -1.0f, 0.0f, 10.0f}, + {0.0f, -1.0f, 0.0f, 1.0f}, + {0.0f, 0.0f, 1.0f, 0.0f}, + {0.0f, 0.0f, -1.0f, 10.0f}}; + + /* Fully inside */ + CORRADE_VERIFY(Intersection::rangeFrustum(Range3D{Vector3{1.0f}, Vector3{2.0f}}, frustum)); + /* Intersects with exactly one plane each */ + CORRADE_VERIFY(Intersection::rangeFrustum(Range3D::fromSize({2.4f, -0.1f, 4.9f}, Vector3{0.2f}), frustum)); + CORRADE_VERIFY(Intersection::rangeFrustum(Range3D::fromSize({2.4f, 0.9f, 4.9f}, Vector3{0.2f}), frustum)); + CORRADE_VERIFY(Intersection::rangeFrustum(Range3D::fromSize({-0.1f, 0.4f, 4.9f}, Vector3{0.2f}), frustum)); + CORRADE_VERIFY(Intersection::rangeFrustum(Range3D::fromSize({4.9f, 0.4f, 4.9f}, Vector3{0.2f}), frustum)); + CORRADE_VERIFY(Intersection::rangeFrustum(Range3D::fromSize({2.4f, 0.4f, -0.1f}, Vector3{0.2f}), frustum)); + CORRADE_VERIFY(Intersection::rangeFrustum(Range3D::fromSize({2.4f, 0.4f, 9.9f}, Vector3{0.2f}), frustum)); + /* Bigger than frustum, but still intersects */ + CORRADE_VERIFY(Intersection::rangeFrustum(Range3D{Vector3{-100.0f}, Vector3{100.0f}}, frustum)); + /* Outside of frustum */ + CORRADE_VERIFY(!Intersection::rangeFrustum(Range3D{Vector3{-10.0f}, Vector3{-5.0f}}, frustum)); +} + +void IntersectionTest::aabbFrustum() { + const Frustum frustum{ + {1.0f, 0.0f, 0.0f, 0.0f}, + {-1.0f, 0.0f, 0.0f, 5.0f}, + {0.0f, 1.0f, 0.0f, 0.0f}, + {0.0f, -1.0f, 0.0f, 1.0f}, {0.0f, 0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, -1.0f, 10.0f}}; - CORRADE_VERIFY(Intersection::boxFrustum({Vector3{1.0f}, Vector3{2.0f}}, frustum)); + /* Fully inside */ + CORRADE_VERIFY(Intersection::aabbFrustum(Vector3{0.0f}, Vector3{1.0f}, frustum)); + /* Intersects with exactly one plane each */ + CORRADE_VERIFY(Intersection::aabbFrustum(Vector3{2.5f, 0.0f, 5.0f}, Vector3{0.1f}, frustum)); + CORRADE_VERIFY(Intersection::aabbFrustum(Vector3{2.5f, 1.0f, 5.0f}, Vector3{0.1f}, frustum)); + CORRADE_VERIFY(Intersection::aabbFrustum(Vector3{0.0f, 0.5f, 5.0f}, Vector3{0.1f}, frustum)); + CORRADE_VERIFY(Intersection::aabbFrustum(Vector3{5.0f, 0.5f, 5.0f}, Vector3{0.1f}, frustum)); + CORRADE_VERIFY(Intersection::aabbFrustum(Vector3{2.5f, 0.5f, 0.0f}, Vector3{0.1f}, frustum)); + CORRADE_VERIFY(Intersection::aabbFrustum(Vector3{2.5f, 0.5f, 10.0f}, Vector3{0.1f}, frustum)); /* Bigger than frustum, but still intersects */ - CORRADE_VERIFY(Intersection::boxFrustum(Range3D{Vector3{-100.0f}, Vector3{100.0f}}, frustum)); + CORRADE_VERIFY(Intersection::aabbFrustum(Vector3{0.0f}, Vector3{100.0f}, frustum)); /* Outside of frustum */ - CORRADE_VERIFY(!Intersection::boxFrustum(Range3D{Vector3{-10.0f}, Vector3{-5.0f}}, frustum)); + CORRADE_VERIFY(!Intersection::aabbFrustum(Vector3{-7.5f}, Vector3{2.5f}, frustum)); +} + +void IntersectionTest::sphereFrustum() { + const Frustum frustum{ + {1.0f, 0.0f, 0.0f, 0.0f}, + {-1.0f, 0.0f, 0.0f, 10.0f}, + {0.0f, 1.0f, 0.0f, 0.0f}, + {0.0f, -1.0f, 0.0f, 10.0f}, + {0.0f, 0.0f, 1.0f, 0.0f}, + {0.0f, 0.0f, -1.0f, 10.0f}}; + + /* Sphere on edge */ + CORRADE_VERIFY(Intersection::sphereFrustum({0.0f, 0.0f, -1.0f}, 1.5f, frustum)); + /* Sphere inside */ + CORRADE_VERIFY(Intersection::sphereFrustum({5.5f, 5.5f, 5.5f}, 1.5f, frustum)); + /* Sphere outside */ + CORRADE_VERIFY(!Intersection::sphereFrustum({0.0f, 0.0f, 100.0f}, 0.5f, frustum)); +} + +void IntersectionTest::pointCone() { + const Vector3 center{0.1f, 0.2f, 0.3f}; + const Vector3 normal{Vector3{0.5f, 1.0f, 2.0f}.normalized()}; + const Rad angle{72.0_degf}; + + const Vector3d centerDouble{1.0, -2.0, 1.3}; + const Vector3d normalDouble{Vector3{0.5, 1.0, 2.0}.normalized()}; + const Radd angleDouble{72.0_deg}; + + /* Some vector along the surface of the cone */ + auto axis = Math::cross(Vector3::yAxis(), normal).normalized(); + auto surface = Matrix4::rotation(0.5f*angle, axis).transformVector(normal); + /* Normal on the curved surface */ + auto sNormal = Matrix4::rotation(90.0_degf, axis).transformVector(surface); + + /* Point on edge */ + CORRADE_VERIFY(Intersection::pointCone(center, center, normal, angle)); + /* Point inside */ + CORRADE_VERIFY(Intersection::pointCone(center + normal, center, normal, angle)); + /* Point outside */ + CORRADE_VERIFY(!Intersection::pointCone(Vector3{}, center, normal, angle)); + CORRADE_VERIFY(!Intersection::pointCone(center + 5.0f*surface + 0.01f*sNormal, center, normal, angle)); + /* Point behind the cone plane */ + CORRADE_VERIFY(!Intersection::pointCone(-normal, center, normal, angle)); +} + +void IntersectionTest::pointDoubleCone() { + const Vector3 center{0.1f, 0.2f, 0.3f}; + const Vector3 normal{Vector3{0.5f, 1.0f, 2.0f}.normalized()}; + const Rad angle{72.0_degf}; + + /* Some vector along the surface of the cone */ + auto axis = Math::cross(Vector3::yAxis(), normal).normalized(); + auto surface = Matrix4::rotation(0.5f*angle, axis).transformVector(normal); + /* Normal on the curved surface */ + auto sNormal = Matrix4::rotation(90.0_degf, axis).transformVector(surface); + + /* Point on edge */ + CORRADE_VERIFY(Intersection::pointDoubleCone(center, center, normal, angle)); + /* Point inside */ + CORRADE_VERIFY(Intersection::pointDoubleCone(center + normal, center, normal, angle)); + CORRADE_VERIFY(Intersection::pointDoubleCone(center - normal, center, normal, angle)); + /* Point outside */ + CORRADE_VERIFY(!Intersection::pointDoubleCone(center + sNormal, center, normal, angle)); +} + +void IntersectionTest::sphereCone() { + const Vector3 center{1.0f, -2.0f, 1.3f}; + const Vector3 normal{Vector3{0.5f, 1.0f, 2.0f}.normalized()}; + const Rad angle(72.0_degf); + + /* Same for Double precision */ + const Vector3d centerDouble{1.0, -2.0, 1.3}; + const Vector3d normalDouble{Vector3{0.5, 1.0, 2.0}.normalized()}; + const Radd angleDouble(72.0_deg); + + /* Some vector along the surface of the cone */ + auto axis = Math::cross(Vector3::yAxis(), normal).normalized(); + auto surface = Matrix4::rotation(0.5f*angle, axis).transformVector(normal); + /* Normal on the curved surface */ + auto sNormal = Matrix4::rotation(90.0_degf, axis).transformVector(surface); + + /* Same for Double precision */ + auto axisDouble = Math::cross(Vector3d::yAxis(), normalDouble).normalized(); + auto surfaceDouble = Matrix4d::rotation(0.5*angleDouble, axisDouble).transformVector(normalDouble); + auto sNormalDouble = Matrix4d::rotation(90.0_deg, axisDouble).transformVector(surfaceDouble); + + /* Sphere fully contained in cone */ + CORRADE_VERIFY(Intersection::sphereCone(center + normal*5.0f, 0.8f, center, normal, angle)); + /* Sphere fully contained in double side of cone */ + CORRADE_VERIFY(!Intersection::sphereCone(center + normal*-5.0f, 0.75f, center, normal, angle)); + /* Sphere fully outside of the cone */ + CORRADE_VERIFY(!Intersection::sphereCone(center + surface + sNormal*5.0f, 0.75f, center, normal, angle)); + + /* Sphere intersecting apex with sphere center behind the cone plane */ + CORRADE_VERIFY(Intersection::sphereCone(center - normal*0.1f, 0.55f, center, normal, angle)); + /* Sphere intersecting apex with sphere center in front of the cone plane */ + CORRADE_VERIFY(Intersection::sphereCone(center + normal*0.1f, 0.55f, center, normal, angle)); + + /* Sphere barely touching the surface of the cone, from inside and outside the cone */ + { + #ifndef CORRADE_TARGET_EMSCRIPTEN + CORRADE_EXPECT_FAIL("Cone touching from the outside fails, possibly because of precision."); + #endif + CORRADE_VERIFY(Intersection::sphereCone(centerDouble + 4.0*surfaceDouble + sNormalDouble*0.5, 0.5, centerDouble, normalDouble, angleDouble)); + } + CORRADE_VERIFY(Intersection::sphereCone(centerDouble + 4.0*surfaceDouble - sNormalDouble*0.5, 0.5, centerDouble, normalDouble, angleDouble)); + + /* Same on double side of the cone */ + CORRADE_VERIFY(!Intersection::sphereCone(center - 4.0f*surface + sNormal*0.5f, 0.5f, center, normal, angle)); + CORRADE_VERIFY(!Intersection::sphereCone(center - 4.0f*surface - sNormal*0.5f, 0.5f, center, normal, angle)); + + /* Sphere clearly, but not fully intersecting the cone */ + CORRADE_VERIFY(Intersection::sphereCone(center + surface + sNormal*0.25f, 0.5f, center, normal, angle)); + /* Sphere with center on the cone's surface */ + CORRADE_VERIFY(Intersection::sphereCone(center + 4.0f*surface, 0.5f, center, normal, angle)); + + /* Same as above on double side of the cone */ + CORRADE_VERIFY(!Intersection::sphereCone(center - surface + sNormal*0.25f, 0.5f, center, normal, angle)); + CORRADE_VERIFY(!Intersection::sphereCone(center - 4.0f*surface, 0.5f, center, normal, angle)); +} + +void IntersectionTest::sphereConeView() { + const Vector3 center{1.0f, -2.0f, 1.3f}; + const Vector3 normal{Vector3{0.5f, 1.0f, 2.0f}.normalized()}; + const Matrix4 coneView = Matrix4::lookAt(center, center + normal, Vector3::yAxis()).invertedRigid(); + + const Vector3d centerDouble{1.0, -2.0, 1.3}; + const Vector3d normalDouble{Vector3{0.5, 1.0, 2.0}.normalized()}; + const Matrix4d coneViewDouble = Matrix4d::lookAt(centerDouble, centerDouble + normalDouble, Vector3d::yAxis()).invertedRigid(); + + const Rad angle(72.0_degf); + const Radd angleDouble(72.0_deg); + + /* Some vector along the surface of the cone */ + auto axis = Math::cross(Vector3::yAxis(), normal).normalized(); + auto surface = Matrix4::rotation(0.5f*angle, axis).transformVector(normal); + /* Normal on the curved surface */ + auto sNormal = Matrix4::rotation(90.0_degf, axis).transformVector(surface); + + /* Sphere fully contained in cone */ + CORRADE_VERIFY(Intersection::sphereConeView(center + normal*5.0f, 0.8f, coneView, angle)); + /* Sphere fully contained in double side of cone */ + CORRADE_VERIFY(!Intersection::sphereConeView(center + normal*-5.0f, 0.75f, coneView, angle)); + /* Sphere fully outside of the cone */ + CORRADE_VERIFY(!Intersection::sphereConeView(center + surface + sNormal*5.0f, 0.75f, coneView, angle)); + + /* Sphere intersecting apex with sphere center behind the cone plane */ + CORRADE_VERIFY(Intersection::sphereConeView(center - normal*0.1f, 0.55f, coneView, angle)); + /* Sphere intersecting apex with sphere center in front of the cone plane */ + CORRADE_VERIFY(Intersection::sphereConeView(center + normal*0.1f, 0.55f, coneView, angle)); + + /* Sphere barely touching the surface of the cone, from inside and outside the cone + Note: behaviour differs from sphereCone! */ + CORRADE_VERIFY(Intersection::sphereConeView(centerDouble + 4.0*Vector3d(surface) + Vector3d(sNormal)*0.5, 0.5, coneViewDouble, angleDouble)); + CORRADE_VERIFY(Intersection::sphereConeView(centerDouble + 4.0*Vector3d(surface) - Vector3d(sNormal)*0.5, 0.5, coneViewDouble, angleDouble)); + /* Same on double side of the cone */ + CORRADE_VERIFY(!Intersection::sphereConeView(center - 4.0f*surface + sNormal*0.5f, 0.5f, coneView, angle)); + CORRADE_VERIFY(!Intersection::sphereConeView(center - 4.0f*surface - sNormal*0.5f, 0.5f, coneView, angle)); + + /* Sphere clearly, but not fully intersecting the cone */ + CORRADE_VERIFY(Intersection::sphereConeView(center + surface + sNormal*0.25f, 0.5f, coneView, angle)); + /* Sphere with center on the cone's surface */ + CORRADE_VERIFY(Intersection::sphereConeView(center + 4.0f*surface, 0.5f, coneView, angle)); + + /* Same as above on double side of the cone */ + CORRADE_VERIFY(!Intersection::sphereConeView(center - surface + sNormal*0.25f, 0.5f, coneView, angle)); + CORRADE_VERIFY(!Intersection::sphereConeView(center - 4.0f*surface, 0.5f, coneView, angle)); + + std::ostringstream out; + Error redirectError{&out}; + + CORRADE_VERIFY(!Intersection::sphereConeView(center, 1.0f, Matrix4{ZeroInit}, angle)); + CORRADE_COMPARE(out.str(), "Math::Geometry::Intersection::sphereConeView(): coneView must be rigid\n"); +} + +void IntersectionTest::rangeCone() { + const Vector3 center{1.0f, -2.0f, 1.3f}; + const Vector3 normal{0.453154f, 0.422618f, 0.784886f}; + const Rad angle{72.0_degf}; + + /* Box fully inside cone */ + CORRADE_VERIFY(Intersection::rangeCone(Range3D::fromSize(15.0f*normal - Vector3{1.0f}, Vector3{2.0f}), + center, normal, angle)); + /* Box intersecting cone */ + CORRADE_VERIFY(Intersection::rangeCone(Range3D::fromSize(5.0f*normal - Vector3{10.0f, 10.0f, 0.5f}, Vector3{20.0f, 20.0f, 1.0f}), + center, normal, angle)); + CORRADE_VERIFY(Intersection::rangeCone(Range3D{{-1.0f, -2.0f, -3.0f}, {1.0f, 2.0f, 3.0f}}, + center, normal, angle)); + /* Cone inside large box */ + CORRADE_VERIFY(Intersection::rangeCone(Range3D::fromSize(12.0f*normal - Vector3{20.0f}, Vector3{40.0f}), + center, normal, angle)); + /* Same corner chosen on all intersecting faces */ + CORRADE_VERIFY(Intersection::rangeCone(Range3D{{2.0f, -0.1f, -1.5f}, {3.0f, 0.1f, 1.5f}}, + center, {0.353553f, 0.707107f, 0.612372f}, angle)); + + /* Boxes outside cone */ + CORRADE_VERIFY(!Intersection::rangeCone(Range3D{Vector3{2.0f, 2.0f, -2.0f}, Vector3{8.0f, 7.0f, 2.0f}}, + center, normal, angle)); + CORRADE_VERIFY(!Intersection::rangeCone(Range3D{Vector3{6.0f, 5.0f, -7.0f}, Vector3{5.0f, 9.0f, -3.0f}}, + center, normal, angle)); + /* Box fully contained in double cone */ + CORRADE_VERIFY(!Intersection::rangeCone(Range3D::fromSize(-15.0f*normal - Vector3{1.0f}, Vector3{2.0f}), + center, normal, angle)); +} + +void IntersectionTest::aabbCone() { + const Vector3 center{1.0f, -2.0f, 1.3f}; + const Vector3 normal{0.453154f, 0.422618f, 0.784886f}; + const Rad angle{72.0_degf}; + + /* Box fully inside cone */ + CORRADE_VERIFY(Intersection::aabbCone(15.0f*normal, Vector3{1.0f}, center, normal, angle)); + /* Box intersecting cone */ + CORRADE_VERIFY(Intersection::aabbCone(5.0f*normal, {10.0f, 10.0f, 0.5f}, center, normal, angle)); + CORRADE_VERIFY(Intersection::aabbCone({}, { 1.0f, 2.0f, 3.0f }, center, normal, angle)); + /* Cone inside large box */ + CORRADE_VERIFY(Intersection::aabbCone(12.0f*normal, {20.0f, 20.0f, 20.0f}, center, normal, angle)); + /* Same corner chosen on all intersecting faces */ + CORRADE_VERIFY(Intersection::aabbCone({2.5f, 0.0f, 0.0f}, {0.5f, 0.1f, 1.5f}, center, {0.353553f, 0.707107f, 0.612372f}, angle)); + + /* Boxes outside cone */ + CORRADE_VERIFY(!Intersection::aabbCone({5.0f, 5.0f, 0.0f}, {3.0f, 2.0f, 2.0f}, center, normal, angle)); + CORRADE_VERIFY(!Intersection::aabbCone({8.0f, 7.0f, -5.0f}, {2.0f, 2.0f, 2.0f}, center, normal, angle)); + /* Box fully contained in double cone */ + CORRADE_VERIFY(!Intersection::aabbCone(-15.0f*normal, Vector3{1.0f}, center, normal, angle)); } }}}}