]> git.tdb.fi Git - libs/game.git/blobdiff - examples/bassteroids/source/physics.cpp
Implement a simple physics system in Bassteroids
[libs/game.git] / examples / bassteroids / source / physics.cpp
diff --git a/examples/bassteroids/source/physics.cpp b/examples/bassteroids/source/physics.cpp
new file mode 100644 (file)
index 0000000..429715a
--- /dev/null
@@ -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;
+       }
+}