Add OBB::ClosestPoint Enclose Distance Contains

include/J3ML/Geometry/OBB.h

@@ -1,7 +1,7 @@
 #pragma once
 
 #include <J3ML/Geometry/Common.h>
-#include <J3ML/Geometry/AABB.h>
+#include <J3ML/Geometry/AABB.hpp>
 #include <J3ML/Geometry/Polyhedron.h>
 
 
@@ -108,5 +108,30 @@ namespace J3ML::Geometry {
         Matrix4x4 LocalToWorld() const;
 
         Matrix4x4 WorldToLocal() const;
+
+        Vector3 ClosestPoint(const Vector3 &targetPoint) const;
+
+        /// Expands this OBB to enclose the given object. The axis directions of this OBB remain intact.
+        /** This function operates in-place. This function does not necessarily result in an OBB that is an
+            optimal fit for the previous OBB and the given point. */
+        void Enclose(const Vector3 & vector3);
+
+        float Distance(const Vector3 &point) const;
+
+
+        /// Tests if the given object is fully contained inside this OBB.
+        /** This function returns true if the given object lies inside this OBB, and false otherwise.
+            @note The comparison is performed using less-or-equal, so the faces of this OBB count as being inside,
+            but due to float inaccuracies, this cannot generally be relied upon. */
+        bool Contains(const Vector3& point) const;
+        bool Contains(const LineSegment&) const;
+        bool Contains(const AABB&) const;
+        bool Contains(const OBB&) const;
+        bool Contains(const Triangle&)  const;
+        bool Contains(const Polygon&)   const;
+        bool Contains(const Frustum&)   const;
+        bool Contains(const Polyhedron&) const;
+
+
     };
 }

src/J3ML/Geometry/OBB.cpp

@@ -1,12 +1,13 @@
 #include <J3ML/Geometry/Shape.h>
-#include <J3ML/Geometry/AABB.h>
+#include <J3ML/Geometry/AABB.hpp>
 #include <J3ML/Geometry/LineSegment.h>
 #include <J3ML/Geometry/Polyhedron.h>
 #include <J3ML/Geometry/OBB.h>
 #include <J3ML/Geometry/Sphere.h>
 
-namespace J3ML::Geometry
-{
+namespace J3ML::Geometry {
+
+    using namespace J3ML::Math;
 
     Polyhedron OBB::ToPolyhedron() const {
         // Note to maintainer: This function is an exact copy of AABB::ToPolyhedron() and Frustum::ToPolyhedron()
@@ -22,12 +23,12 @@ namespace J3ML::Geometry
         // generate the 6 faces of this OBB.
         const int faces[6][4] =
                 {
-                        {0, 1, 3, 2}, // X-
-                        {4, 6, 7, 5}, // X+
-                        {0, 4, 5, 1}, // Y-
-                        {7, 6, 2, 3}, // Y+
-                        {0, 2, 6, 4}, // Z-
-                        {1, 5, 7, 3}  // Z+
+            {0, 1, 3, 2}, // X-
+            {4, 6, 7, 5}, // X+
+            {0, 4, 5, 1}, // Y-
+            {7, 6, 2, 3}, // Y+
+            {0, 2, 6, 4}, // Z-
+            {1, 5, 7, 3}  // Z+
                 };
 
         for (int f = 0; f < 6; ++f)
@@ -405,6 +406,50 @@ namespace J3ML::Geometry
         return m;
     }
 
+    Vector3 OBB::ClosestPoint(const Vector3& targetPoint) const {
+        Vector3 d = targetPoint - pos;
+        Vector3 closestPoint = pos; // Start at the center point of the OBB.
+        for (int i = 0; i < 3; ++i) // Project the target onto the OBB axes and walk towards that point.
+            closestPoint += Clamp(Vector3::Dot(d, axis[i]), -r[i], r[i]) * axis[i];
+
+
+        return closestPoint;
+    }
+
+
+    void OBB::Enclose(const Vector3& point) {
+        Vector3 p = point - pos;
+
+        for (int i = 0; i < 3; ++i) {
+            assert(Math::EqualAbs(axis[i].Length(), 1.f));
+            float dist = p.Dot(axis[i]);
+            float distanceFromOBB = Math::Abs(dist) - r[i];
+            if (distanceFromOBB > 0.f) {
+                r[i] += distanceFromOBB * 0.5f;
+                if (dist > 0.f) // TODO: Optimize out this comparison!
+                    pos += axis[i] * distanceFromOBB * 0.5f;
+                else
+                    pos -= axis[i] * distanceFromOBB * 0.5f;
+
+                p = point - pos;
+                assert(Math::EqualAbs(Math::Abs(p.Dot(axis[i])), r[i], 1e-1f));
+            }
+        }
+        assert(Distance(point) <= 1e-3f);
+    }
+
+    float OBB::Distance(const Vector3& point) const {
+        Vector3 closestPoint = ClosestPoint(point);
+        return point.Distance(closestPoint);
+    }
+
+    bool OBB::Contains(const Vector3 &point) const {
+        Vector3 pt = point - pos;
+        return Abs(Vector3::Dot(pt, axis[0])) <= r[0] &&
+               Abs(Vector3::Dot(pt, axis[1])) <= r[1] &&
+               Abs(Vector3::Dot(pt, axis[2])) <= r[2];
+    }
+
     Matrix4x4 OBB::LocalToWorld() const
     {
         // To produce a normalized local->world matrix, do the following.