Math: Add Distance::pointPlane* and two frustum intersection functions

`Intersection::pointFrustum` and `Intersection::boxFrustum`

Signed-off-by: Squareys <Squareys@googlemail.com>

src/Magnum/Math/Geometry/Distance.h

@@ -31,6 +31,7 @@
 
 #include "Magnum/Math/Functions.h"
 #include "Magnum/Math/Vector3.h"
+#include "Magnum/Math/Vector4.h"
 
 namespace Magnum { namespace Math { namespace Geometry {
 
@@ -162,6 +163,61 @@ class Distance {
          * the square root.
          */
         template<class T> static T lineSegmentPointSquared(const Vector3<T>& a, const Vector3<T>& b, const Vector3<T>& point);
+
+        /**
+         * @brief Distance of point from plane
+         *
+         * The distance **d** is computed from point **p** and plane with normal
+         * **n** and **w** using: @f[
+         *      d = \frac{\sum_i^3 (p \cdot n) + w}{\left| n \right|}
+         * @f]
+         * The distance is negative if the point lies behind the plane.
+         *
+         * In cases where the planes normal is a unit vector, @ref pointPlaneUnnormalized()
+         * is more efficient.
+         *
+         * If merely the sign of the distance is of interest, @ref pointPlaneScaled()
+         * is more efficient.
+         */
+        template<class T> static T pointPlane(const Vector3<T>& point, const Vector4<T>& plane) {
+            return pointPlaneScaled<T>(point, plane)/plane.xyz().length();
+        }
+
+        /**
+         * @brief Distance of point from plane, scaled by the length of the planes normal
+         *
+         * The distance **d** is computed from point **p** and plane with normal
+         * **n** and **w** using: @f[
+         *      d = \sum_i^3 (p \cdot n) + w
+         * @f]
+         * The distance is negative if the point lies behind the plane.
+         *
+         * More efficient than @ref pointPlane() when merely the sign of the distance is
+         * of interest, for example when testing on which half space of the plane the
+         * point lies.
+         */
+        template<class T> static T pointPlaneScaled(const Vector3<T>& point, const Vector4<T>& plane) {
+            return (plane.xyz()*point).sum() + plane.w();
+        }
+
+        /**
+         * @brief Distance of point from plane with normalized normal
+         *
+         * The distance **d** is computed from point **p** and plane with normal
+         * **n** and **w** using: @f[
+         *      d = \sum_i^3 (p \cdot n) + w
+         * @f]
+         * The distance is negative if the point lies behind the plane.
+         *
+         * More efficient than @ref pointPlane() in cases where the planes normal is
+         * normalized.
+         */
+        template<class T> static T pointPlaneNormalized(const Vector3<T>& point, const Vector4<T>& plane) {
+            CORRADE_ASSERT(plane.xyz().isNormalized(),
+                    "Math::Geometry::Distance::pointPlaneNormalized(): the planes normal is not a unit vector", {});
+            return pointPlaneScaled<T>(point, plane);
+        }
+
 };
 
 template<class T> T Distance::lineSegmentPoint(const Vector2<T>& a, const Vector2<T>& b, const Vector2<T>& point) {

src/Magnum/Math/Geometry/Intersection.h

@@ -29,6 +29,9 @@
  * @brief Class @ref Magnum::Math::Geometry::Intersection
  */
 
+#include "Magnum/Math/Frustum.h"
+#include "Magnum/Math/Geometry/Distance.h"
+#include "Magnum/Math/Range.h"
 #include "Magnum/Math/Vector3.h"
 
 namespace Magnum { namespace Math { namespace Geometry {
@@ -123,8 +126,83 @@ class Intersection {
             const T f = dot(planePosition, planeNormal);
             return (f-dot(planeNormal, p))/dot(planeNormal, r);
         }
+
+        /**
+         * @brief Intersection of a point and a camera frustum
+         * @param point Point
+         * @param frustum Frustum planes with normals pointing outwards
+         * @return `true` if the point is on or inside the frustum.
+         *
+         * Checks for each plane of the frustum whether the point is behind the plane
+         * (the points distance from the plane is negative) using
+         * @ref Distance::pointPlaneScaled().
+         */
+        template<class T> static bool pointFrustum(const Vector3<T>& point, const Frustum<T>& frustum);
+
+        /**
+         * @brief Intersection of a range and a camera frustum
+         * @return `true` if the box intersects with the camera frustum.
+         *
+         * 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.
+         */
+        template<class T> static bool boxFrustum(const Range3D<T>& box, const Frustum<T>& frustum);
+
 };
 
+template<class T> bool Intersection::pointFrustum(const Vector3<T>& point, const Frustum<T>& frustum) {
+    for(const Vector4<T>& f : frustum.planes()) {
+        if(Distance::pointPlaneScaled<T>(point, f) < T(0)) {
+            /* the point is in front of one of the frustum planes (normals point outwards) */
+            return false;
+        }
+    }
+
+    return true;
+}
+
+template<class T> bool Intersection::boxFrustum(const Range3D<T>& box, const Frustum<T>& frustum) {
+    /*
+     * Create the 8 vertices of the box from the 2 given vertices min and max
+     * Check for each corner of an octant whether it is inside the frustum.
+     * If only some of the corners are inside, the octant requires further checks.
+     */
+    int planes = 0;
+
+    for(const Vector4<T>& plane : frustum.planes()) {
+        int corners = 0;
+
+        for(UnsignedByte c = 0; c < 8; ++c) {
+            const Vector3<T> corner = Math::lerp(box.min(), box.max(), Math::BoolVector<3>{c});
+
+            if(Distance::pointPlaneScaled<T>(corner, plane) >= T(0)) {
+                ++corners;
+            }
+        }
+
+        if(corners == 0) {
+            /* all corners are outside this plane */
+            return false;
+        }
+
+        if(corners == 8) {
+            ++planes;
+        }
+    }
+
+    if(planes == 6) {
+        return true;
+    }
+
+    // potentially check corners here to avoid false positives!
+
+    return true;
+}
+
 }}}
 
 #endif

src/Magnum/Math/Geometry/Test/CMakeLists.txt

@@ -25,3 +25,7 @@
 
 corrade_add_test(MathGeometryDistanceTest DistanceTest.cpp LIBRARIES MagnumMathTestLib)
 corrade_add_test(MathGeometryIntersectionTest IntersectionTest.cpp LIBRARIES MagnumMathTestLib)
+
+set_property(TARGET
+    MathGeometryDistanceTest
+    APPEND PROPERTY COMPILE_DEFINITIONS "CORRADE_GRACEFUL_ASSERT")

src/Magnum/Math/Geometry/Test/DistanceTest.cpp

@@ -23,6 +23,7 @@
     DEALINGS IN THE SOFTWARE.
 */
 
+#include <sstream>
 #include <Corrade/TestSuite/Tester.h>
 
 #include "Magnum/Math/Constants.h"
@@ -37,17 +38,24 @@ struct DistanceTest: Corrade::TestSuite::Tester {
     void linePoint3D();
     void lineSegmentPoint2D();
     void lineSegmentPoint3D();
+    void pointPlane();
+    void pointPlaneScaled();
+    void pointPlaneNormalized();
 };
 
 typedef Math::Vector2<Float> Vector2;
 typedef Math::Vector3<Float> Vector3;
+typedef Math::Vector4<Float> Vector4;
 typedef Math::Constants<Float> Constants;
 
 DistanceTest::DistanceTest() {
     addTests({&DistanceTest::linePoint2D,
               &DistanceTest::linePoint3D,
               &DistanceTest::lineSegmentPoint2D,
-              &DistanceTest::lineSegmentPoint3D});
+              &DistanceTest::lineSegmentPoint3D,
+              &DistanceTest::pointPlane,
+              &DistanceTest::pointPlaneScaled,
+              &DistanceTest::pointPlaneNormalized});
 }
 
 void DistanceTest::linePoint2D() {
@@ -156,6 +164,33 @@ void DistanceTest::lineSegmentPoint3D() {
                     Constants::sqrt2());
 }
 
+void DistanceTest::pointPlane() {
+    Vector3 point{0.0f, 0.0f, 0.0f};
+    Vector4 plane{3.0f, 0.0f, 4.0f, 5.0f};
+
+    CORRADE_COMPARE(Distance::pointPlane(point, plane), 1.0f);
+}
+
+void DistanceTest::pointPlaneScaled() {
+    Vector3 point{1.0f, 1.0f, 1.0f};
+    Vector4 plane{2.0f, 2.0f, 2.0f, 0.0f};
+
+    CORRADE_COMPARE(Distance::pointPlaneScaled(point, plane), 6.0f);
+}
+
+void DistanceTest::pointPlaneNormalized() {
+    Vector3 point{1.0f, 2.0f, 3.0f};
+    Vector4 invalidPlane{2.0f, 2.0f, 2.0f, 0.0f};
+
+    const Vector4 plane{0.0f, 1.0f, 0.0f, 1.0f};
+    CORRADE_COMPARE(Distance::pointPlaneNormalized(point, plane), 3.0f);
+
+    std::ostringstream o;
+    Error redirectError{&o};
+    Distance::pointPlaneNormalized(point, invalidPlane);
+    CORRADE_COMPARE(o.str(), "Math::Geometry::Distance::pointPlaneNormalized(): the planes normal is not a unit vector\n");
+}
+
 }}}}
 
 CORRADE_TEST_MAIN(Magnum::Math::Geometry::Test::DistanceTest)

src/Magnum/Math/Geometry/Test/IntersectionTest.cpp

@@ -3,6 +3,7 @@
 
     Copyright © 2010, 2011, 2012, 2013, 2014, 2015, 2016
               Vladimír Vondruš <mosra@centrum.cz>
+    Copyright © 2016 Jonathan Hale <squareys@googlemail.com>
 
     Permission is hereby granted, free of charge, to any person obtaining a
     copy of this software and associated documentation files (the "Software"),
@@ -34,15 +35,22 @@ struct IntersectionTest: Corrade::TestSuite::Tester {
 
     void planeLine();
     void lineLine();
+    void pointFrustum();
+    void boxFrustum();
 };
 
 typedef Math::Vector2<Float> Vector2;
 typedef Math::Vector3<Float> Vector3;
+typedef Math::Vector4<Float> Vector4;
+typedef Math::Frustum<Float> Frustum;
 typedef Math::Constants<Float> Constants;
+typedef Math::Range3D<Float> Range3D;
 
 IntersectionTest::IntersectionTest() {
     addTests({&IntersectionTest::planeLine,
-              &IntersectionTest::lineLine});
+              &IntersectionTest::lineLine,
+              &IntersectionTest::pointFrustum,
+              &IntersectionTest::boxFrustum});
 }
 
 void IntersectionTest::planeLine() {
@@ -97,6 +105,39 @@ void IntersectionTest::lineLine() {
         {0.0f, 0.0f}, {1.0f, 2.0f}), Constants::inf());
 }
 
+void IntersectionTest::pointFrustum() {
+    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}};
+
+    /* Point on edge */
+    CORRADE_VERIFY(Intersection::pointFrustum<Float>(Vector3{}, frustum));
+    /* Point inside */
+    CORRADE_VERIFY(Intersection::pointFrustum<Float>(Vector3{5.0f, 5.0f, 5.0f}, frustum));
+    /* Point outside */
+    CORRADE_VERIFY(!Intersection::pointFrustum<Float>(Vector3{0.0f, 0.0f, 100.0f}, frustum));
+}
+
+void IntersectionTest::boxFrustum() {
+    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}};
+
+    CORRADE_VERIFY(Intersection::boxFrustum<Float>(Range3D{Vector3{1.0f}, Vector3{2.0f}}, frustum));
+    /* Bigger than frustum, but still intersects */
+    CORRADE_VERIFY(Intersection::boxFrustum<Float>(Range3D{Vector3{-100.0f}, Vector3{100.0f}}, frustum));
+    /* Outside of frustum */
+    CORRADE_VERIFY(!Intersection::boxFrustum<Float>(Range3D{Vector3{-10.0f}, Vector3{-5.0f}}, frustum));
+}
+
 }}}}
 
 CORRADE_TEST_MAIN(Magnum::Math::Geometry::Test::IntersectionTest)