From: Mikko Rasa <tdb@tdb.fi>
Date: Sat, 12 Nov 2022 11:35:41 +0000 (+0200)
Subject: Implement a simple physics system in Bassteroids
X-Git-Url: http://git.tdb.fi/?p=libs%2Fgame.git;a=commitdiff_plain;h=0636566dd84ca185d3e9a6fae02459569c42d220

Implement a simple physics system in Bassteroids

A more comprehensive physics system will be implemented in the game
engine later.
---

diff --git a/examples/bassteroids/source/asteroid.cpp b/examples/bassteroids/source/asteroid.cpp
index 4a89cdb..7e5bcd5 100644
--- a/examples/bassteroids/source/asteroid.cpp
+++ b/examples/bassteroids/source/asteroid.cpp
@@ -1,10 +1,9 @@
 #include "asteroid.h"
-#include <msp/game/transform.h>
 
 using namespace Msp;
 
 Asteroid::Asteroid(Game::Handle<Entity> p, const AsteroidSetup &s):
-	Entity(p, Game::TransformValues()),
+	PhysicalEntity(p, s.physical),
 	setup(s),
 	mesh(*this, setup.mesh)
 { }
diff --git a/examples/bassteroids/source/asteroid.h b/examples/bassteroids/source/asteroid.h
index 0e3232f..dcb64c3 100644
--- a/examples/bassteroids/source/asteroid.h
+++ b/examples/bassteroids/source/asteroid.h
@@ -3,13 +3,15 @@
 
 #include <msp/game/entity.h>
 #include <msp/game/meshsource.h>
+#include "physicalentity.h"
 
 struct AsteroidSetup
 {
+	PhysicalSetup physical;
 	Msp::Game::MeshSourceSetup mesh;
 };
 
-class Asteroid: public Msp::Game::Entity
+class Asteroid: public PhysicalEntity
 {
 public:
 	using Setup = AsteroidSetup;
diff --git a/examples/bassteroids/source/bassteroids.cpp b/examples/bassteroids/source/bassteroids.cpp
index 2a6d1dd..1c0ab66 100644
--- a/examples/bassteroids/source/bassteroids.cpp
+++ b/examples/bassteroids/source/bassteroids.cpp
@@ -15,6 +15,7 @@ Bassteroids::Bassteroids(int, char **):
 	camera(const_cast<const Game::Owned<Game::Entity> &>(cam_entity), cam_setup)
 {
 	game_stage.add_system<GameController>();
+	game_stage.add_system<Physics>();
 	game_stage.add_system<Game::TransformPropagator>();
 
 	director.activate_stage(game_stage);
diff --git a/examples/bassteroids/source/collider.cpp b/examples/bassteroids/source/collider.cpp
new file mode 100644
index 0000000..224af7a
--- /dev/null
+++ b/examples/bassteroids/source/collider.cpp
@@ -0,0 +1,8 @@
+#include "collider.h"
+
+using namespace Msp;
+
+Collider::Collider(Game::Handle<Game::Entity> e, const Setup &s):
+	Component(e),
+	setup(s)
+{ }
diff --git a/examples/bassteroids/source/collider.h b/examples/bassteroids/source/collider.h
new file mode 100644
index 0000000..cd5cffe
--- /dev/null
+++ b/examples/bassteroids/source/collider.h
@@ -0,0 +1,40 @@
+#ifndef COLLIDER_H_
+#define COLLIDER_H_
+
+#include <msp/game/component.h>
+#include <msp/linal/vector.h>
+
+enum class ColliderType
+{
+	CIRCLE,
+	BOX
+};
+
+struct ColliderSetup
+{
+	ColliderType type = ColliderType::CIRCLE;
+	union
+	{
+		float radius = 1.0f;
+		Msp::LinAl::Vector<float, 2> size;
+	};
+};
+
+
+class Collider: public Msp::Game::Component
+{
+public:
+	using Setup = ColliderSetup;
+
+private:
+	Setup setup;
+
+public:
+	Collider(Msp::Game::Handle<Msp::Game::Entity>, const Setup &);
+
+	ColliderType get_type() const { return setup.type; }
+	float get_radius() const { return (setup.type==ColliderType::CIRCLE ? setup.radius : 0.0f); }
+	Msp::LinAl::Vector<float, 2> get_size() const { return (setup.type==ColliderType::BOX ? setup.size : Msp::LinAl::Vector<float, 2>()); }
+};
+
+#endif
diff --git a/examples/bassteroids/source/gamecontroller.cpp b/examples/bassteroids/source/gamecontroller.cpp
index f32f04b..05586b0 100644
--- a/examples/bassteroids/source/gamecontroller.cpp
+++ b/examples/bassteroids/source/gamecontroller.cpp
@@ -8,7 +8,8 @@ using namespace Msp;
 
 GameController::GameController(Game::Stage &s):
 	System(s),
-	asteroid_setup{ .mesh = { .object_name = "Asteroid 1.object" }}
+	asteroid_setup{ .physical = { .body = { .mass = 200 }, .collider = { .type = ColliderType::CIRCLE, .radius = 3.0f } },
+		.mesh = { .object_name = "Asteroid 1.object" }}
 { }
 
 void GameController::tick(Time::TimeDelta)
@@ -22,6 +23,7 @@ void GameController::tick(Time::TimeDelta)
 			{
 				asteroids.emplace_back(stage.get_root(), asteroid_setup);
 				asteroids.back()->get_transform()->set_position({ sdist(rng)*32, sdist(rng)*18, 0.0f });
+				asteroids.back()->get_body()->set_velocity(asteroids.back()->get_transform()->get_position().slice<2>(0)*-0.1f);
 			}
 			state = PLAYING;
 		});
diff --git a/examples/bassteroids/source/physicalentity.cpp b/examples/bassteroids/source/physicalentity.cpp
new file mode 100644
index 0000000..585eb34
--- /dev/null
+++ b/examples/bassteroids/source/physicalentity.cpp
@@ -0,0 +1,12 @@
+#include "physicalentity.h"
+#include <msp/game/transform.h>
+
+using namespace Msp;
+
+PhysicalEntity::PhysicalEntity(Game::Handle<Game::Entity> p, const Setup &s):
+	Entity(p, Game::TransformValues()),
+	collider(*this, s.collider)
+{
+	if(!s.fixture)
+		body = Game::Owned<RigidBody>(*this, s.body);
+}
diff --git a/examples/bassteroids/source/physicalentity.h b/examples/bassteroids/source/physicalentity.h
new file mode 100644
index 0000000..a0cca37
--- /dev/null
+++ b/examples/bassteroids/source/physicalentity.h
@@ -0,0 +1,32 @@
+#ifndef PHYSICALENTITY_H_
+#define PHYSICALENTITY_H_
+
+#include <msp/game/entity.h>
+#include "rigidbody.h"
+#include "collider.h"
+
+struct PhysicalSetup
+{
+	bool fixture = false;
+	RigidBodySetup body;
+	ColliderSetup collider;
+};
+
+class PhysicalEntity: public Msp::Game::Entity
+{
+public:
+	using Setup = PhysicalSetup;
+
+protected:
+	Msp::Game::Owned<RigidBody> body;
+	Msp::Game::Owned<Collider> collider;
+
+public:
+	PhysicalEntity(Msp::Game::Handle<Msp::Game::Entity>, const Setup &);
+
+	bool is_fixture() const { return !body; }
+	Msp::Game::Handle<RigidBody> get_body() const { return body; }
+	Msp::Game::Handle<Collider> get_collider() const { return collider; }
+};
+
+#endif
diff --git a/examples/bassteroids/source/physics.cpp b/examples/bassteroids/source/physics.cpp
new file mode 100644
index 0000000..429715a
--- /dev/null
+++ b/examples/bassteroids/source/physics.cpp
@@ -0,0 +1,203 @@
+#include "physics.h"
+#include <msp/game/transform.h>
+#include "physicalentity.h"
+
+using namespace Msp;
+
+Physics::Physics(Game::Stage &s):
+	System(s),
+	observer(stage.get_event_bus())
+{
+	observer.observe<Game::Events::EntityCreated>([this](auto &e){ entity_added(e); });
+}
+
+void Physics::entity_added(const Game::Events::EntityCreated &e)
+{
+	if(Game::Handle<PhysicalEntity> physical = dynamic_handle_cast<PhysicalEntity>(e.entity))
+	{
+		for(Game::Handle<Game::Entity> p=e.entity->get_parent(); p; p=p->get_parent())
+			if(p->get_transform())
+				return;
+
+		SimulatedEntity sim_body;
+		sim_body.entity = physical;
+		if(physical->is_fixture())
+		{
+			entities.insert(entities.begin()+fixture_count, sim_body);
+			++fixture_count;
+		}
+		else
+			entities.push_back(sim_body);
+	}
+}
+
+void Physics::tick(Time::TimeDelta dt)
+{
+	float dt_secs = dt/Time::sec;
+
+	for(unsigned i=0; i<fixture_count; ++i)
+		copy_in<true>(entities[i]);
+	for(unsigned i=fixture_count; i<entities.size(); ++i)
+		copy_in<false>(entities[i]);
+
+	step(dt_secs);
+
+	collisions.clear();
+	for(unsigned i=0; i<10; ++i)
+	{
+		detect_collisions();
+		solve_collisions();
+	}
+
+	apply_impulses();
+
+	for(unsigned i=0; i<fixture_count; ++i)
+		copy_out<true>(entities[i]);
+	for(unsigned i=fixture_count; i<entities.size(); ++i)
+		copy_out<false>(entities[i]);
+}
+
+template<bool is_fixture>
+void Physics::copy_in(SimulatedEntity &entity)
+{
+	Game::Handle<Game::Transform> transform = entity.entity->get_transform();
+	entity.position = transform->get_position().slice<2>(0);
+
+	if constexpr(is_fixture)
+		entity.inverse_mass = 0.0f;
+	else
+	{
+		Game::Handle<RigidBody> body = entity.entity->get_body();
+		entity.inverse_mass = 1.0f/body->get_mass();
+		entity.velocity = body->get_velocity();
+	}
+}
+
+template<bool is_fixture>
+void Physics::copy_out(SimulatedEntity &entity)
+{
+	Game::Handle<Game::Transform> transform = entity.entity->get_transform();
+	transform->set_position(compose(entity.position, 0.0f));
+
+	if constexpr(!is_fixture)
+	{
+		Game::Handle<RigidBody> body = entity.entity->get_body();
+		body->set_velocity(entity.velocity);
+	}
+}
+
+void Physics::step(float dt_secs)
+{
+	for(unsigned i=fixture_count; i<entities.size(); ++i)
+	{
+		SimulatedEntity &entity = entities[i];
+
+		LinAl::Vector<float, 2> new_velocity = entity.velocity+entity.external_force*dt_secs*entity.inverse_mass;
+		entity.position += (entity.velocity+new_velocity)*(dt_secs/2);
+		entity.velocity = new_velocity;
+	}
+}
+
+void Physics::detect_collisions()
+{
+	for(auto &c: collisions)
+		c.depth = 0.0f;
+
+	for(unsigned i=fixture_count; i<entities.size(); ++i)
+	{
+		Game::Handle<PhysicalEntity> entity1 = entities[i].entity;
+		ColliderType type1 = entity1->get_collider()->get_type();
+		for(unsigned j=0; j<i; ++j)
+		{
+			Game::Handle<PhysicalEntity> entity2 = entities[j].entity;
+			ColliderType type2 = entity2->get_collider()->get_type();
+			if(type1==ColliderType::CIRCLE && type2==ColliderType::CIRCLE)
+				collide_circle_circle(i, j);
+		}
+	}
+}
+
+void Physics::solve_collisions()
+{
+	for(auto &e: entities)
+	{
+		e.position_adjust = LinAl::Vector<float, 2>();
+		e.collision_count = 0;
+	}
+
+	for(const auto &c: collisions)
+	{
+		if(!c.depth)
+			continue;
+
+		SimulatedEntity &entity1 = entities[c.body1];
+		SimulatedEntity &entity2 = entities[c.body2];
+		float inv_mass_sum = 1.0f/(entity1.inverse_mass+entity2.inverse_mass);
+		LinAl::Vector<float, 2> delta = c.normal*c.depth*inv_mass_sum;
+		if(c.body1>=fixture_count)
+		{
+			entity1.position_adjust += delta*entity1.inverse_mass;
+			++entity1.collision_count;
+		}
+		if(c.body2>=fixture_count)
+		{
+			entity2.position_adjust -= delta*entity1.inverse_mass;
+			++entity2.collision_count;
+		}
+	}
+
+	for(auto &e: entities)
+		if(e.collision_count)
+			e.position += e.position_adjust/static_cast<float>(e.collision_count);
+}
+
+void Physics::apply_impulses()
+{
+	for(const auto &c: collisions)
+	{
+		SimulatedEntity &entity1 = entities[c.body1];
+		SimulatedEntity &entity2 = entities[c.body2];
+		LinAl::Vector<float, 2> v_rel = entity2.velocity-entity1.velocity;
+		float restitution = 1.0f;
+		float inv_mass_sum = entity1.inverse_mass+entity2.inverse_mass;
+		float impulse = (1+restitution)*inner_product(v_rel, c.normal)/inv_mass_sum;
+		entity1.velocity += c.normal*(impulse*entity1.inverse_mass);
+		entity2.velocity -= c.normal*(impulse*entity2.inverse_mass);
+	}
+}
+
+Physics::Collision &Physics::get_collision(unsigned i, unsigned j)
+{
+	for(auto &c: collisions)
+		if((c.body1==i && c.body2==j) || (c.body1==j && c.body2==i))
+			return c;
+
+	Collision &c = collisions.emplace_back();
+	c.body1 = i;
+	c.body2 = j;
+	return c;
+}
+
+void Physics::collide_circle_circle(unsigned i, unsigned j)
+{
+	const LinAl::Vector<float, 2> &pos1 = entities[i].position;
+	const LinAl::Vector<float, 2> &pos2 = entities[j].position;
+	float r1 = entities[i].entity->get_collider()->get_radius();
+	float r2 = entities[j].entity->get_collider()->get_radius();
+
+	/* Points in the direction the first body needs to move in to clear the
+	penetration */
+	LinAl::Vector<float, 2> delta = pos1-pos2;
+	float d_sq = inner_product(delta, delta);
+	float r_sum = r1+r2;
+	if(d_sq<r_sum*r_sum)
+	{
+		Collision &collision = get_collision(i, j);
+		float d = sqrt(d_sq);
+		collision.normal = normalize(delta);
+		collision.depth = r1+r2-d;
+		collision.point = pos1-collision.normal*(r1-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
new file mode 100644
index 0000000..a611789
--- /dev/null
+++ b/examples/bassteroids/source/physics.h
@@ -0,0 +1,68 @@
+#ifndef PHYSICS_H_
+#define PHYSICS_H_
+
+#include <msp/game/eventobserver.h>
+#include <msp/game/events.h>
+#include <msp/game/handle.h>
+#include <msp/game/system.h>
+#include <msp/geometry/angle.h>
+#include <msp/linal/vector.h>
+
+class PhysicalEntity;
+
+class Physics: public Msp::Game::System
+{
+private:
+	struct SimulatedEntity
+	{
+		Msp::Game::Handle<PhysicalEntity> entity;
+		float inverse_mass = 1.0f;
+		Msp::LinAl::Vector<float, 2> external_force;
+
+		Msp::LinAl::Vector<float, 2> position;
+		Msp::LinAl::Vector<float, 2> velocity;
+
+		unsigned collision_count;
+		Msp::LinAl::Vector<float, 2> position_adjust;
+	};
+
+	struct Collision
+	{
+		std::uint16_t body1 = 0;
+		std::uint16_t body2 = 0;
+		Msp::LinAl::Vector<float, 2> point;
+		float depth = 0.0f;
+		Msp::LinAl::Vector<float, 2> normal;
+	};
+
+	Msp::Game::EventObserver observer;
+	std::vector<SimulatedEntity> entities;
+	unsigned fixture_count = 0;
+	std::vector<Collision> collisions;
+
+public:
+	Physics(Msp::Game::Stage &);
+
+private:
+	void entity_added(const Msp::Game::Events::EntityCreated &);
+
+public:
+	void tick(Msp::Time::TimeDelta) override;
+
+private:
+	template<bool>
+	void copy_in(SimulatedEntity &);
+
+	template<bool>
+	void copy_out(SimulatedEntity &);
+
+	void step(float);
+	void detect_collisions();
+	void solve_collisions();
+	void apply_impulses();
+
+	Collision &get_collision(unsigned, unsigned);
+	void collide_circle_circle(unsigned, unsigned);
+};
+
+#endif
diff --git a/examples/bassteroids/source/rigidbody.cpp b/examples/bassteroids/source/rigidbody.cpp
new file mode 100644
index 0000000..c1d096e
--- /dev/null
+++ b/examples/bassteroids/source/rigidbody.cpp
@@ -0,0 +1,13 @@
+#include "rigidbody.h"
+
+using namespace Msp;
+
+RigidBody::RigidBody(Game::Handle<Game::Entity> e, const Setup &s):
+	Component(e),
+	setup(s)
+{ }
+
+void RigidBody::set_velocity(const LinAl::Vector<float, 2> &v)
+{
+	velocity = v;
+}
diff --git a/examples/bassteroids/source/rigidbody.h b/examples/bassteroids/source/rigidbody.h
new file mode 100644
index 0000000..8a7a5b3
--- /dev/null
+++ b/examples/bassteroids/source/rigidbody.h
@@ -0,0 +1,30 @@
+#ifndef RIGIDBODY_H_
+#define RIGIDBODY_H_
+
+#include <msp/game/component.h>
+#include <msp/geometry/angle.h>
+#include <msp/linal/vector.h>
+
+struct RigidBodySetup
+{
+	float mass = 1.0f;
+};
+
+class RigidBody: public Msp::Game::Component
+{
+public:
+	using Setup = RigidBodySetup;
+
+private:
+	const Setup &setup;
+	Msp::LinAl::Vector<float, 2> velocity;
+
+public:
+	RigidBody(Msp::Game::Handle<Msp::Game::Entity>, const Setup &);
+
+	float get_mass() const { return setup.mass; }
+	void set_velocity(const Msp::LinAl::Vector<float, 2> &);
+	const Msp::LinAl::Vector<float, 2> &get_velocity() const { return velocity; }
+};
+
+#endif