From 69a0220ce9c71ebc92a4f9e77baf53780fee2206 Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Wed, 11 Sep 2013 10:15:02 +0300
Subject: [PATCH] Add a bounding sphere class

Shapes don't produce bounding spheres yet, but it can be used for other
purposes.
---
 source/geometry/boundingsphere.h | 105 +++++++++++++++++++++++++++++++
 1 file changed, 105 insertions(+)
 create mode 100644 source/geometry/boundingsphere.h

diff --git a/source/geometry/boundingsphere.h b/source/geometry/boundingsphere.h
new file mode 100644
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
-- 
2.43.0