From 5b878983a65d70daae586a0637d68a72fbe6b2bd Mon Sep 17 00:00:00 2001
From: Danny Robson <danny@nerdcruft.net>
Date: Wed, 8 Apr 2015 19:00:17 +1000
Subject: [PATCH] aabb: add contract and closest methods

---
 aabb.cpp | 57 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 aabb.hpp |  7 +++++++
 2 files changed, 64 insertions(+)

diff --git a/aabb.cpp b/aabb.cpp
index d3f24d3b..7da23dc3 100644
--- a/aabb.cpp
+++ b/aabb.cpp
@@ -58,6 +58,63 @@ AABB<S,T>::overlaps (point<S,T> p) const
 }
 
 
+//-----------------------------------------------------------------------------
+template <size_t S, typename T>
+util::point<S,T>
+AABB<S,T>::closest (point<S,T> q) const
+{
+    point<S,T> res;
+
+    for (size_t i = 0; i < S; ++i)
+        res[i] = q[i] < p0[i] ? p0[i] :
+                 q[i] > p1[i] ? p1[i] :
+                 q[i];
+
+    return res;
+}
+
+
+//-----------------------------------------------------------------------------
+template <size_t S, typename T>
+AABB<S,T>&
+AABB<S,T>::contract (util::vector<S,T> mag)
+{
+    p0 -= mag / T{2};
+    p1 += mag / T{2};
+
+    return *this;
+}
+
+
+//-----------------------------------------------------------------------------
+template <size_t S, typename T>
+AABB<S,T>&
+AABB<S,T>::contract (T mag)
+{
+    return contract (util::vector<S,T> {mag});
+}
+
+
+//-----------------------------------------------------------------------------
+template <size_t S, typename T>
+AABB<S,T>
+AABB<S,T>::contracted (util::vector<S,T> mag) const
+{
+    AABB<S,T> res = *this;
+    res.contract (mag);
+    return res;
+}
+
+
+//-----------------------------------------------------------------------------
+template <size_t S, typename T>
+AABB<S,T>
+AABB<S,T>::contracted (T mag) const
+{
+    return contracted (vector<S,T> {mag});
+}
+
+
 //-----------------------------------------------------------------------------
 template <size_t S, typename T>
 void
diff --git a/aabb.hpp b/aabb.hpp
index 3a3d56b3..9f27d0ec 100644
--- a/aabb.hpp
+++ b/aabb.hpp
@@ -37,6 +37,13 @@ namespace util {
 
         bool overlaps (point<S,T>) const;
 
+        point<S,T> closest (point<S,T>) const;
+
+        AABB<S,T>& contract (util::vector<S,T>);
+        AABB<S,T>& contract (T);
+        AABB<S,T>  contracted (util::vector<S,T>) const;
+        AABB<S,T>  contracted (T) const;
+
         void cover (point<S,T>);
 
         AABB<S,T> operator+ (vector<S,T>) const;