X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=examples%2Fbassteroids%2Fsource%2Fphysics.cpp;fp=examples%2Fbassteroids%2Fsource%2Fphysics.cpp;h=429715aa99b5d365e26f436e170bd468119f6403;hb=0636566dd84ca185d3e9a6fae02459569c42d220;hp=0000000000000000000000000000000000000000;hpb=29aa52b23d74d390c677b728776022a9b267d9e6;p=libs%2Fgame.git

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;
+	}
+}