From 338031de484ee117762d80449d9ca472a6918c92 Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Sun, 27 Nov 2022 23:48:39 +0200
Subject: [PATCH] Remove destroyed entities from the physics simulation

---
 examples/bassteroids/source/physics.cpp | 24 +++++++++++++++++++++++-
 examples/bassteroids/source/physics.h   |  1 +
 2 files changed, 24 insertions(+), 1 deletion(-)

diff --git a/examples/bassteroids/source/physics.cpp b/examples/bassteroids/source/physics.cpp
index 3c43502..73d8582 100644
--- a/examples/bassteroids/source/physics.cpp
+++ b/examples/bassteroids/source/physics.cpp
@@ -1,5 +1,5 @@
 #include "physics.h"
-#include <algorithm>
+#include <msp/core/algorithm.h>
 #include <msp/game/transform.h>
 #include "physicalentity.h"
 
@@ -12,6 +12,7 @@ Physics::Physics(Game::Stage &s):
 	observer(stage.get_event_bus())
 {
 	observer.observe<Game::Events::EntityCreated>([this](auto &e){ entity_added(e); });
+	observer.observe<Game::Events::EntityDestroyed>([this](auto &e){ entity_removed(e); });
 
 	stage.synthesize_initial_events(observer);
 }
@@ -36,6 +37,27 @@ void Physics::entity_added(const Game::Events::EntityCreated &e)
 	}
 }
 
+void Physics::entity_removed(const Game::Events::EntityDestroyed &e)
+{
+	if(Game::Handle<PhysicalEntity> physical = dynamic_handle_cast<PhysicalEntity>(e.entity))
+	{
+		auto i = find_member(entities, physical, &SimulatedEntity::entity);
+		if(i!=entities.end())
+		{
+			size_t index = distance(entities.begin(), i);
+			if(index<fixture_count)
+			{
+				if(index+1!=fixture_count)
+					*i = std::move(entities[fixture_count-1]);
+				entities[fixture_count-1] = std::move(entities.back());
+			}
+			else
+				*i = std::move(entities.back());
+			entities.pop_back();
+		}
+	}
+}
+
 void Physics::tick(Time::TimeDelta dt)
 {
 	float dt_secs = dt/Time::sec;
diff --git a/examples/bassteroids/source/physics.h b/examples/bassteroids/source/physics.h
index fcf4b0d..1e7bec3 100644
--- a/examples/bassteroids/source/physics.h
+++ b/examples/bassteroids/source/physics.h
@@ -55,6 +55,7 @@ public:
 
 private:
 	void entity_added(const Msp::Game::Events::EntityCreated &);
+	void entity_removed(const Msp::Game::Events::EntityDestroyed &);
 
 public:
 	void tick(Msp::Time::TimeDelta) override;
-- 
2.43.0