From 48d34439f3d5bc8bebabe9f0ee35970359e61bfa Mon Sep 17 00:00:00 2001
From: Daniel Karbach <daniel.karbach@localhorst.tv>
Date: Fri, 2 Dec 2016 15:01:35 +0100
Subject: [PATCH] sphere/sphere intersection test

---
 src/geometry/geometry.cpp         | 25 +++++++++++++++
 src/geometry/primitive.hpp        | 21 ++++++++++++
 tst/geometry/IntersectionTest.cpp | 53 +++++++++++++++++++++++++++++++
 tst/geometry/IntersectionTest.hpp |  2 ++
 4 files changed, 101 insertions(+)

diff --git a/src/geometry/geometry.cpp b/src/geometry/geometry.cpp
index 002c68f..1d7beeb 100644
--- a/src/geometry/geometry.cpp
+++ b/src/geometry/geometry.cpp
@@ -302,6 +302,31 @@ std::ostream &operator <<(std::ostream &out, const Sphere &s) {
 	return out << "Sphere(" << s.origin << ", " << s.radius << ')';
 }
 
+bool Intersection(
+	const Sphere &a,
+	const Sphere &b,
+	float &dist,
+	glm::vec3 &norm
+) noexcept {
+	glm::vec3 diff(b.origin - a.origin);
+	float dist2 = glm::length2(diff);
+	if (dist2 < std::numeric_limits<float>::epsilon()) {
+		// origins coincide, use smaller of the diameters for
+		// depth and pick arbitrary normal
+		dist = 2.0f * std::min(a.radius, b.radius);
+		norm = glm::vec3(1.0f, 0.0f, 0.0f);
+		return true;
+	}
+	if (dist2 < (a.radius + b.radius) * (a.radius + b.radius)) {
+		dist = std::sqrt(dist2);
+		norm = diff / dist;
+		dist = a.radius - (dist - b.radius);
+		return true;
+	} else {
+		return false;
+	}
+}
+
 bool Intersection(
 	const Sphere &sphere,
 	const Plane &plane,
diff --git a/src/geometry/primitive.hpp b/src/geometry/primitive.hpp
index c922506..e785c63 100644
--- a/src/geometry/primitive.hpp
+++ b/src/geometry/primitive.hpp
@@ -31,6 +31,11 @@ struct AABB {
 		return glm::length(high);
 	}
 
+	void Position(const glm::vec3 &center) noexcept {
+		const glm::vec3 halfsize((max - min) * 0.5f);
+		min = center - halfsize;
+		max = center + halfsize;
+	}
 	void Move(const glm::vec3 &delta) noexcept {
 		min += delta;
 		max += delta;
@@ -170,12 +175,28 @@ bool CullTest(const AABB &box, const glm::mat4 &) noexcept;
 bool CullTest(const AABB &box, const Frustum &) noexcept;
 
 struct Sphere {
+
 	glm::vec3 origin;
 	float radius;
+
+	void Position(const glm::vec3 &center) noexcept {
+		origin = center;
+	}
+	void Move(const glm::vec3 &delta) noexcept {
+		origin += delta;
+	}
+
 };
 
 std::ostream &operator <<(std::ostream &, const Sphere &);
 
+/// Two spheres intersection test.
+bool Intersection(
+	const Sphere &,
+	const Sphere &,
+	float &dist,
+	glm::vec3 &norm) noexcept;
+
 /// Test for intersection of sphere with double sided infinite plane.
 /// If true, dist will hold the smaller interpenetration depth and norm
 /// the respective contact normal.
diff --git a/tst/geometry/IntersectionTest.cpp b/tst/geometry/IntersectionTest.cpp
index 36cacdf..550c0f2 100644
--- a/tst/geometry/IntersectionTest.cpp
+++ b/tst/geometry/IntersectionTest.cpp
@@ -325,6 +325,59 @@ void IntersectionTest::testBoxBox() {
 	);
 }
 
+void IntersectionTest::testSphereSphere() {
+	const float delta = std::numeric_limits<float>::epsilon();
+
+	Sphere a{{ 0.0f, 0.0f, 0.0f }, 1.0f};
+	Sphere b{{ 0.0f, 0.0f, 0.0f }, 1.0f};
+	float depth;
+	glm::vec3 normal;
+
+	CPPUNIT_ASSERT_MESSAGE(
+		"coincidental spheres should intersect",
+		Intersection(a, b, depth, normal)
+	);
+	CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
+		"bad intersection distance",
+		2.0f, depth, delta
+	);
+	// normal can be just about anything
+
+	b.Move({ 1, 0, 0 });
+	CPPUNIT_ASSERT_MESSAGE(
+		"spheres should intersect",
+		Intersection(a, b, depth, normal)
+	);
+	CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
+		"bad intersection distance",
+		1.0f, depth, delta
+	);
+	CPPUNIT_ASSERT_EQUAL_MESSAGE(
+		"bad intersection normal",
+		glm::vec3(1, 0, 0), normal
+	);
+
+	b.Position({ -1.5, 0, 0 });
+	CPPUNIT_ASSERT_MESSAGE(
+		"spheres should intersect",
+		Intersection(a, b, depth, normal)
+	);
+	CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
+		"bad intersection distance",
+		0.5f, depth, delta
+	);
+	CPPUNIT_ASSERT_EQUAL_MESSAGE(
+		"bad intersection normal",
+		glm::vec3(-1, 0, 0), normal
+	);
+
+	b.Move({ -1, 0, 0 });
+	CPPUNIT_ASSERT_MESSAGE(
+		"spheres should not intersect",
+		!Intersection(a, b, depth, normal)
+	);
+}
+
 void IntersectionTest::testSpherePlane() {
 	const float delta = std::numeric_limits<float>::epsilon();
 
diff --git a/tst/geometry/IntersectionTest.hpp b/tst/geometry/IntersectionTest.hpp
index 4865c88..680d4ca 100644
--- a/tst/geometry/IntersectionTest.hpp
+++ b/tst/geometry/IntersectionTest.hpp
@@ -17,6 +17,7 @@ CPPUNIT_TEST(testAABB);
 CPPUNIT_TEST(testSimpleRayBox);
 CPPUNIT_TEST(testRayBox);
 CPPUNIT_TEST(testBoxBox);
+CPPUNIT_TEST(testSphereSphere);
 CPPUNIT_TEST(testSpherePlane);
 CPPUNIT_TEST(testSphereHalfSpace);
 
@@ -30,6 +31,7 @@ public:
 	void testSimpleRayBox();
 	void testRayBox();
 	void testBoxBox();
+	void testSphereSphere();
 	void testSpherePlane();
 	void testSphereHalfSpace();
 
-- 
2.39.5