From cf97839fff9ecb0783cecc9510994fb4a13f2dbe Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Sat, 12 Nov 2022 18:06:14 +0200
Subject: [PATCH] Implement box colliders in Bassteroids' physics system

---
 examples/bassteroids/source/physics.cpp | 34 +++++++++++++++++++++++++
 examples/bassteroids/source/physics.h   |  2 ++
 2 files changed, 36 insertions(+)

diff --git a/examples/bassteroids/source/physics.cpp b/examples/bassteroids/source/physics.cpp
index 89feeed..5d4c242 100644
--- a/examples/bassteroids/source/physics.cpp
+++ b/examples/bassteroids/source/physics.cpp
@@ -1,7 +1,9 @@
 #include "physics.h"
+#include <algorithm>
 #include <msp/game/transform.h>
 #include "physicalentity.h"
 
+using namespace std;
 using namespace Msp;
 
 Physics::Physics(Game::Stage &s):
@@ -62,6 +64,8 @@ void Physics::copy_in(SimulatedEntity &entity)
 {
 	Game::Handle<Game::Transform> transform = entity.entity->get_transform();
 	entity.position = transform->get_position().slice<2>(0);
+	const Geometry::Quaternion<float> &r = transform->get_rotation();
+	entity.rotation = Geometry::atan2<float>(2*(r.a*r.d+r.b*r.c), 1-2*(r.c*r.c+r.d*r.d));
 
 	if constexpr(is_fixture)
 		entity.inverse_mass = 0.0f;
@@ -78,6 +82,7 @@ void Physics::copy_out(SimulatedEntity &entity)
 {
 	Game::Handle<Game::Transform> transform = entity.entity->get_transform();
 	transform->set_position(compose(entity.position, 0.0f));
+	transform->set_rotation(Geometry::Quaternion<float>::rotation(entity.rotation, LinAl::Vector<float, 3>(0, 0, 1)));
 
 	if constexpr(!is_fixture)
 	{
@@ -113,6 +118,10 @@ void Physics::detect_collisions()
 			ColliderType type2 = entity2->get_collider()->get_type();
 			if(type1==ColliderType::CIRCLE && type2==ColliderType::CIRCLE)
 				collide_circle_circle(i, j);
+			else if(type1==ColliderType::CIRCLE && type2==ColliderType::BOX)
+				collide_circle_box(i, j);
+			else if(type1==ColliderType::BOX && type2==ColliderType::CIRCLE)
+				collide_circle_box(j, i);
 		}
 	}
 }
@@ -200,3 +209,28 @@ void Physics::collide_circle_circle(unsigned i, unsigned j)
 			collision.normal = -collision.normal;
 	}
 }
+
+void Physics::collide_circle_box(unsigned i, unsigned j)
+{
+	const LinAl::Vector<float, 2> &pos1 = entities[i].position;
+	const LinAl::Vector<float, 2> &pos2 = entities[j].position;
+	float radius = entities[i].entity->get_collider()->get_radius();
+	LinAl::Vector<float, 2> half_size = entities[j].entity->get_collider()->get_size()/2.0f;
+
+	LinAl::Vector<float, 2> delta = pos1-pos2;
+	float c = cos(entities[j].rotation);
+	float s = sin(entities[j].rotation);
+	LinAl::Vector<float, 2> local_delta(c*delta.x+s*delta.y, c*delta.y-s*delta.x);
+	LinAl::Vector<float, 2> local_closest(clamp(local_delta.x, -half_size.x, half_size.x), clamp(local_delta.y, -half_size.y, half_size.y));
+	LinAl::Vector<float, 2> local_cdelta = local_delta-local_closest;
+	float d_sq = inner_product(local_cdelta, local_cdelta);
+	if(d_sq<radius*radius)
+	{
+		Collision &collision = get_collision(i, j);
+		collision.normal = normalize(LinAl::Vector<float, 2>(c*local_cdelta.x-s*local_cdelta.y, c*local_cdelta.y+s*local_cdelta.x));
+		collision.depth = radius-sqrt(d_sq);
+		collision.point = pos1-collision.normal*(radius-collision.depth/2);
+		if(collision.body1!=i)
+			collision.normal = -collision.normal;
+	}
+}
diff --git a/examples/bassteroids/source/physics.h b/examples/bassteroids/source/physics.h
index a611789..fdeb8b2 100644
--- a/examples/bassteroids/source/physics.h
+++ b/examples/bassteroids/source/physics.h
@@ -20,6 +20,7 @@ private:
 		Msp::LinAl::Vector<float, 2> external_force;
 
 		Msp::LinAl::Vector<float, 2> position;
+		Msp::Geometry::Angle<float> rotation;
 		Msp::LinAl::Vector<float, 2> velocity;
 
 		unsigned collision_count;
@@ -63,6 +64,7 @@ private:
 
 	Collision &get_collision(unsigned, unsigned);
 	void collide_circle_circle(unsigned, unsigned);
+	void collide_circle_box(unsigned, unsigned);
 };
 
 #endif
-- 
2.43.0