From da195e0fa114b82708d7c2bbd6297590f34f79a4 Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Sun, 20 Nov 2022 21:22:40 +0200
Subject: [PATCH] Allow external forces and torques to be added on rigid bodies

---
 examples/bassteroids/source/physics.cpp   |  3 +++
 examples/bassteroids/source/physics.h     |  1 +
 examples/bassteroids/source/rigidbody.cpp | 25 +++++++++++++++++++++++
 examples/bassteroids/source/rigidbody.h   |  8 ++++++++
 4 files changed, 37 insertions(+)

diff --git a/examples/bassteroids/source/physics.cpp b/examples/bassteroids/source/physics.cpp
index a8d49ec..0fb4326 100644
--- a/examples/bassteroids/source/physics.cpp
+++ b/examples/bassteroids/source/physics.cpp
@@ -76,6 +76,8 @@ 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.external_force = body->get_force();
+		entity.external_torque = body->get_torque();
 		entity.velocity = body->get_velocity();
 		entity.angular_velocity = body->get_angular_velocity();
 	}
@@ -93,6 +95,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);
+		body->clear_forces();
 	}
 }
 
diff --git a/examples/bassteroids/source/physics.h b/examples/bassteroids/source/physics.h
index 3d9d708..078d2e6 100644
--- a/examples/bassteroids/source/physics.h
+++ b/examples/bassteroids/source/physics.h
@@ -19,6 +19,7 @@ private:
 		float inverse_mass = 1.0f;
 		float moment_of_inertia = 1.0f;
 		Msp::LinAl::Vector<float, 2> external_force;
+		float external_torque = 0.0f;
 
 		Msp::LinAl::Vector<float, 2> position;
 		Msp::Geometry::Angle<float> rotation;
diff --git a/examples/bassteroids/source/rigidbody.cpp b/examples/bassteroids/source/rigidbody.cpp
index 509e3a9..391707a 100644
--- a/examples/bassteroids/source/rigidbody.cpp
+++ b/examples/bassteroids/source/rigidbody.cpp
@@ -1,4 +1,6 @@
 #include "rigidbody.h"
+#include <msp/game/entity.h>
+#include <msp/game/transform.h>
 
 using namespace Msp;
 
@@ -16,3 +18,26 @@ void RigidBody::set_angular_velocity(Geometry::Angle<float> as)
 {
 	angular_velocity = as;
 }
+
+void RigidBody::add_force(const LinAl::Vector<float, 2> &f)
+{
+	force += f;
+}
+
+void RigidBody::add_force(const LinAl::Vector<float, 2> &f, const LinAl::Vector<float, 2> &p)
+{
+	force += f;
+	LinAl::Vector<float, 2> r = p-entity->get_transform()->get_position().slice<2>(0);
+	torque += r.x*f.y-r.y*f.x;
+}
+
+void RigidBody::add_torque(float t)
+{
+	torque += t;
+}
+
+void RigidBody::clear_forces()
+{
+	force = LinAl::Vector<float, 2>();
+	torque = 0.0f;
+}
diff --git a/examples/bassteroids/source/rigidbody.h b/examples/bassteroids/source/rigidbody.h
index 6225500..aaffdc0 100644
--- a/examples/bassteroids/source/rigidbody.h
+++ b/examples/bassteroids/source/rigidbody.h
@@ -20,6 +20,8 @@ private:
 	const Setup &setup;
 	Msp::LinAl::Vector<float, 2> velocity;
 	Msp::Geometry::Angle<float> angular_velocity;
+	Msp::LinAl::Vector<float, 2> force;
+	float torque = 0.0f;
 
 public:
 	RigidBody(Msp::Game::Handle<Msp::Game::Entity>, const Setup &);
@@ -28,8 +30,14 @@ public:
 	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>);
+	void add_force(const Msp::LinAl::Vector<float, 2> &);
+	void add_force(const Msp::LinAl::Vector<float, 2> &, const Msp::LinAl::Vector<float, 2> &);
+	void add_torque(float);
+	void clear_forces();
 	const Msp::LinAl::Vector<float, 2> &get_velocity() const { return velocity; }
 	Msp::Geometry::Angle<float> get_angular_velocity() const { return angular_velocity; }
+	const Msp::LinAl::Vector<float, 2> &get_force() const { return force; }
+	float get_torque() const { return torque; }
 };
 
 #endif
-- 
2.43.0