geom/plane: add furthest point finder

geom/plane.hpp

@@ -94,4 +94,27 @@ namespace util::geom {
     {
         return -distance (q, p);
     }
+
+
+    ///////////////////////////////////////////////////////////////////////////
+    template <size_t S, typename T>
+    auto
+    furthest (plane<S,T> p, const std::vector<util::point<S,T>> &cloud)
+    {
+        T maxd = -INFINITY;
+        auto best = cloud.begin ();
+
+        for (auto q = cloud.begin (), last = cloud.end (); q != last; ++q) {
+            if (auto d = distance (p, *q); d > maxd) {
+                maxd = d;
+                best = q;
+            }
+        }
+
+        return std::make_tuple (best, maxd);
+    }
+
+    template <size_t S, typename T>
+    auto
+    furthest (plane<S,T>, std::vector<util::point<S,T>> &&) = delete;
 }