From 1819b186d60376a546722d99edd686e876b81d9f Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Sun, 20 Nov 2022 21:10:32 +0200
Subject: [PATCH] Add rotation to physics simulation

There's no friction yet, so colliding objects don't start rotating.
---
 examples/bassteroids/source/physics.cpp   | 7 +++++++
 examples/bassteroids/source/physics.h     | 2 ++
 examples/bassteroids/source/rigidbody.cpp | 5 +++++
 examples/bassteroids/source/rigidbody.h   | 5 +++++
 4 files changed, 19 insertions(+)

diff --git a/examples/bassteroids/source/physics.cpp b/examples/bassteroids/source/physics.cpp
index db8e494..a8d49ec 100644
--- a/examples/bassteroids/source/physics.cpp
+++ b/examples/bassteroids/source/physics.cpp
@@ -75,7 +75,9 @@ void Physics::copy_in(SimulatedEntity &entity)
 	{
 		Game::Handle<RigidBody> body = entity.entity->get_body();
 		entity.inverse_mass = 1.0f/body->get_mass();
+		entity.moment_of_inertia = body->get_moment_of_inertia();
 		entity.velocity = body->get_velocity();
+		entity.angular_velocity = body->get_angular_velocity();
 	}
 }
 
@@ -90,6 +92,7 @@ void Physics::copy_out(SimulatedEntity &entity)
 	{
 		Game::Handle<RigidBody> body = entity.entity->get_body();
 		body->set_velocity(entity.velocity);
+		body->set_angular_velocity(entity.angular_velocity);
 	}
 }
 
@@ -102,6 +105,10 @@ void Physics::step(float dt_secs)
 		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;
+
+		Geometry::Angle<float> new_angular_velocity = entity.angular_velocity+Geometry::Angle<float>::from_radians(entity.external_torque*(dt_secs/entity.moment_of_inertia));
+		entity.rotation = wrap_positive(entity.rotation+(entity.angular_velocity+new_angular_velocity)*(dt_secs/2));
+		entity.angular_velocity = new_angular_velocity;
 	}
 }
 
diff --git a/examples/bassteroids/source/physics.h b/examples/bassteroids/source/physics.h
index fdeb8b2..3d9d708 100644
--- a/examples/bassteroids/source/physics.h
+++ b/examples/bassteroids/source/physics.h
@@ -17,11 +17,13 @@ private:
 	{
 		Msp::Game::Handle<PhysicalEntity> entity;
 		float inverse_mass = 1.0f;
+		float moment_of_inertia = 1.0f;
 		Msp::LinAl::Vector<float, 2> external_force;
 
 		Msp::LinAl::Vector<float, 2> position;
 		Msp::Geometry::Angle<float> rotation;
 		Msp::LinAl::Vector<float, 2> velocity;
+		Msp::Geometry::Angle<float> angular_velocity;
 
 		unsigned collision_count;
 		Msp::LinAl::Vector<float, 2> position_adjust;
diff --git a/examples/bassteroids/source/rigidbody.cpp b/examples/bassteroids/source/rigidbody.cpp
index c1d096e..509e3a9 100644
--- a/examples/bassteroids/source/rigidbody.cpp
+++ b/examples/bassteroids/source/rigidbody.cpp
@@ -11,3 +11,8 @@ void RigidBody::set_velocity(const LinAl::Vector<float, 2> &v)
 {
 	velocity = v;
 }
+
+void RigidBody::set_angular_velocity(Geometry::Angle<float> as)
+{
+	angular_velocity = as;
+}
diff --git a/examples/bassteroids/source/rigidbody.h b/examples/bassteroids/source/rigidbody.h
index 8a7a5b3..6225500 100644
--- a/examples/bassteroids/source/rigidbody.h
+++ b/examples/bassteroids/source/rigidbody.h
@@ -8,6 +8,7 @@
 struct RigidBodySetup
 {
 	float mass = 1.0f;
+	float moment_of_inertia = 0.5f;
 };
 
 class RigidBody: public Msp::Game::Component
@@ -18,13 +19,17 @@ public:
 private:
 	const Setup &setup;
 	Msp::LinAl::Vector<float, 2> velocity;
+	Msp::Geometry::Angle<float> angular_velocity;
 
 public:
 	RigidBody(Msp::Game::Handle<Msp::Game::Entity>, const Setup &);
 
 	float get_mass() const { return setup.mass; }
+	float get_moment_of_inertia() const { return setup.moment_of_inertia; }
 	void set_velocity(const Msp::LinAl::Vector<float, 2> &);
+	void set_angular_velocity(Msp::Geometry::Angle<float>);
 	const Msp::LinAl::Vector<float, 2> &get_velocity() const { return velocity; }
+	Msp::Geometry::Angle<float> get_angular_velocity() const { return angular_velocity; }
 };
 
 #endif
-- 
2.45.2