geom/plane: add point-plane distance

geom/plane.hpp

@@ -71,8 +71,27 @@ namespace util::geom {
     }
 
 
-    typedef plane<2,float> plane2f;
-    typedef plane<3,float> plane3f;
+    ///////////////////////////////////////////////////////////////////////////
+    /// calculates the distance from a plane to a point.
+    ///
+    /// positive distances are in front of the plane, negative is behind;
+    template <size_t S, typename T>
+    T
+    distance (plane<S,T> p, point<S,T> q)
+    {
+        auto d = dot (
+            p.coefficients,
+            q.template redim<S+1> (0)
+        );
+
+        return d / norm (normal (p));
     }
 
-#endif
+
+    template <size_t S, typename T>
+    T
+    distance (point<S,T> p, plane<S,T> q)
+    {
+        return -distance (q, p);
+    }
+}