Add a bounding sphere class

Shapes don't produce bounding spheres yet, but it can be used for other
purposes.

diff --git a/source/geometry/boundingsphere.h b/source/geometry/boundingsphere.h
new file mode 100644 (file)
index 0000000..40826ca
--- /dev/null
+++ b/source/geometry/boundingsphere.h
@@ -0,0 +1,105 @@
+#ifndef MSP_GEOMETRY_BOUNDINGSPHERE_H_
+#define MSP_GEOMETRY_BOUNDINGSPHERE_H_
+
+#include <cmath>
+#include <msp/linal/vector.h>
+
+namespace Msp {
+namespace Geometry {
+
+template<typename T, unsigned N>
+class BoundingSphere
+{
+private:
+       LinAl::Vector<T, N> center;
+       T radius;
+
+public:
+       BoundingSphere(): radius(0) { }
+       BoundingSphere(const LinAl::Vector<T, N> &, T);
+
+       template<typename Iter>
+       static BoundingSphere from_point_cloud(const Iter &, const Iter &);
+
+       const LinAl::Vector<T, N> get_center() const { return center; }
+       T get_radius() const { return radius; }
+};
+
+template<typename T, unsigned N>
+BoundingSphere<T, N>::BoundingSphere(const LinAl::Vector<T, N> &c, T r):
+       center(c),
+       radius(r)
+{ }
+
+template<typename T, unsigned N>
+template<typename Iter>
+BoundingSphere<T, N> BoundingSphere<T, N>::from_point_cloud(const Iter &begin, const Iter &end)
+{
+       using std::sqrt;
+
+       LinAl::Vector<T, N> p1;
+       T sqdist = 0;
+       for(Iter i=begin; i!=end; ++i)
+       {
+               T d = inner_product(*i, *i);
+               if(d>sqdist)
+               {
+                       p1 = *i;
+                       sqdist = d;
+               }
+       }
+
+       LinAl::Vector<T, N> p2;
+       sqdist = 0;
+       for(Iter i=begin; i!=end; ++i)
+       {
+               LinAl::Vector<T, N> v = *i-p1;
+               T d = inner_product(v, v);
+               if(d>sqdist)
+               {
+                       p2 = *i;
+                       sqdist = d;
+               }
+       }
+
+       sqdist /= 4;
+       BoundingSphere<T, N> bsphere((p1+p2)/T(2), sqrt(sqdist));
+       for(Iter i=begin; i!=end; ++i)
+       {
+               LinAl::Vector<T, N> v = *i-bsphere.center;
+               T d = inner_product(v, v);
+               if(d>sqdist)
+               {
+                       d = sqrt(d);
+                       bsphere.center += v*(1-bsphere.radius/d);
+                       bsphere.radius += d/2;
+                       sqdist = bsphere.radius*bsphere.radius;
+               }
+       }
+
+       return bsphere;
+}
+
+template<typename T, unsigned N>
+BoundingSphere<T, N> operator|(const BoundingSphere<T, N> &bs1, const BoundingSphere<T, N> &bs2)
+{
+       LinAl::Vector<T, N> v = bs2.get_center()-bs1.get_center();
+       T d = v.norm();
+       if(d+bs2.get_radius()<bs1.get_radius())
+               return bs1;
+       else if(d+bs1.get_radius()<bs2.get_radius())
+               return bs2;
+       else
+       {
+               T r = (bs1.get_radius()+d+bs2.get_radius())/T(2);
+               if(bs1.get_radius()>bs2.get_radius())
+                       return BoundingSphere<T, N>(bs1.get_center()+v*((r-bs1.get_radius())/d), r);
+               else
+                       return BoundingSphere<T, N>(bs2.get_center()-v*((r-bs2.get_radius())/d), r);
+       }
+}
+
+} // namespace Geometry
+} // namespace Msp
+
+#endif