examples/bassteroids/source/physics.cpp

   1 #include "physics.h"
   2 #include <msp/game/transform.h>
   3 #include "physicalentity.h"
   4
   5 using namespace Msp;
   6
   7 Physics::Physics(Game::Stage &s):
   8         System(s),
   9         observer(stage.get_event_bus())
  10 {
  11         observer.observe<Game::Events::EntityCreated>([this](auto &e){ entity_added(e); });
  12 }
  13
  14 void Physics::entity_added(const Game::Events::EntityCreated &e)
  15 {
  16         if(Game::Handle<PhysicalEntity> physical = dynamic_handle_cast<PhysicalEntity>(e.entity))
  17         {
  18                 for(Game::Handle<Game::Entity> p=e.entity->get_parent(); p; p=p->get_parent())
  19                         if(p->get_transform())
  20                                 return;
  21
  22                 SimulatedEntity sim_body;
  23                 sim_body.entity = physical;
  24                 if(physical->is_fixture())
  25                 {
  26                         entities.insert(entities.begin()+fixture_count, sim_body);
  27                         ++fixture_count;
  28                 }
  29                 else
  30                         entities.push_back(sim_body);
  31         }
  32 }
  33
  34 void Physics::tick(Time::TimeDelta dt)
  35 {
  36         float dt_secs = dt/Time::sec;
  37
  38         for(unsigned i=0; i<fixture_count; ++i)
  39                 copy_in<true>(entities[i]);
  40         for(unsigned i=fixture_count; i<entities.size(); ++i)
  41                 copy_in<false>(entities[i]);
  42
  43         step(dt_secs);
  44
  45         collisions.clear();
  46         for(unsigned i=0; i<10; ++i)
  47         {
  48                 detect_collisions();
  49                 solve_collisions();
  50         }
  51
  52         apply_impulses();
  53
  54         for(unsigned i=0; i<fixture_count; ++i)
  55                 copy_out<true>(entities[i]);
  56         for(unsigned i=fixture_count; i<entities.size(); ++i)
  57                 copy_out<false>(entities[i]);
  58 }
  59
  60 template<bool is_fixture>
  61 void Physics::copy_in(SimulatedEntity &entity)
  62 {
  63         Game::Handle<Game::Transform> transform = entity.entity->get_transform();
  64         entity.position = transform->get_position().slice<2>(0);
  65
  66         if constexpr(is_fixture)
  67                 entity.inverse_mass = 0.0f;
  68         else
  69         {
  70                 Game::Handle<RigidBody> body = entity.entity->get_body();
  71                 entity.inverse_mass = 1.0f/body->get_mass();
  72                 entity.velocity = body->get_velocity();
  73         }
  74 }
  75
  76 template<bool is_fixture>
  77 void Physics::copy_out(SimulatedEntity &entity)
  78 {
  79         Game::Handle<Game::Transform> transform = entity.entity->get_transform();
  80         transform->set_position(compose(entity.position, 0.0f));
  81
  82         if constexpr(!is_fixture)
  83         {
  84                 Game::Handle<RigidBody> body = entity.entity->get_body();
  85                 body->set_velocity(entity.velocity);
  86         }
  87 }
  88
  89 void Physics::step(float dt_secs)
  90 {
  91         for(unsigned i=fixture_count; i<entities.size(); ++i)
  92         {
  93                 SimulatedEntity &entity = entities[i];
  94
  95                 LinAl::Vector<float, 2> new_velocity = entity.velocity+entity.external_force*dt_secs*entity.inverse_mass;
  96                 entity.position += (entity.velocity+new_velocity)*(dt_secs/2);
  97                 entity.velocity = new_velocity;
  98         }
  99 }
 100
 101 void Physics::detect_collisions()
 102 {
 103         for(auto &c: collisions)
 104                 c.depth = 0.0f;
 105
 106         for(unsigned i=fixture_count; i<entities.size(); ++i)
 107         {
 108                 Game::Handle<PhysicalEntity> entity1 = entities[i].entity;
 109                 ColliderType type1 = entity1->get_collider()->get_type();
 110                 for(unsigned j=0; j<i; ++j)
 111                 {
 112                         Game::Handle<PhysicalEntity> entity2 = entities[j].entity;
 113                         ColliderType type2 = entity2->get_collider()->get_type();
 114                         if(type1==ColliderType::CIRCLE && type2==ColliderType::CIRCLE)
 115                                 collide_circle_circle(i, j);
 116                 }
 117         }
 118 }
 119
 120 void Physics::solve_collisions()
 121 {
 122         for(auto &e: entities)
 123         {
 124                 e.position_adjust = LinAl::Vector<float, 2>();
 125                 e.collision_count = 0;
 126         }
 127
 128         for(const auto &c: collisions)
 129         {
 130                 if(!c.depth)
 131                         continue;
 132
 133                 SimulatedEntity &entity1 = entities[c.body1];
 134                 SimulatedEntity &entity2 = entities[c.body2];
 135                 float inv_mass_sum = 1.0f/(entity1.inverse_mass+entity2.inverse_mass);
 136                 LinAl::Vector<float, 2> delta = c.normal*c.depth*inv_mass_sum;
 137                 if(c.body1>=fixture_count)
 138                 {
 139                         entity1.position_adjust += delta*entity1.inverse_mass;
 140                         ++entity1.collision_count;
 141                 }
 142                 if(c.body2>=fixture_count)
 143                 {
 144                         entity2.position_adjust -= delta*entity1.inverse_mass;
 145                         ++entity2.collision_count;
 146                 }
 147         }
 148
 149         for(auto &e: entities)
 150                 if(e.collision_count)
 151                         e.position += e.position_adjust/static_cast<float>(e.collision_count);
 152 }
 153
 154 void Physics::apply_impulses()
 155 {
 156         for(const auto &c: collisions)
 157         {
 158                 SimulatedEntity &entity1 = entities[c.body1];
 159                 SimulatedEntity &entity2 = entities[c.body2];
 160                 LinAl::Vector<float, 2> v_rel = entity2.velocity-entity1.velocity;
 161                 float restitution = 1.0f;
 162                 float inv_mass_sum = entity1.inverse_mass+entity2.inverse_mass;
 163                 float impulse = (1+restitution)*inner_product(v_rel, c.normal)/inv_mass_sum;
 164                 entity1.velocity += c.normal*(impulse*entity1.inverse_mass);
 165                 entity2.velocity -= c.normal*(impulse*entity2.inverse_mass);
 166         }
 167 }
 168
 169 Physics::Collision &Physics::get_collision(unsigned i, unsigned j)
 170 {
 171         for(auto &c: collisions)
 172                 if((c.body1==i && c.body2==j) || (c.body1==j && c.body2==i))
 173                         return c;
 174
 175         Collision &c = collisions.emplace_back();
 176         c.body1 = i;
 177         c.body2 = j;
 178         return c;
 179 }
 180
 181 void Physics::collide_circle_circle(unsigned i, unsigned j)
 182 {
 183         const LinAl::Vector<float, 2> &pos1 = entities[i].position;
 184         const LinAl::Vector<float, 2> &pos2 = entities[j].position;
 185         float r1 = entities[i].entity->get_collider()->get_radius();
 186         float r2 = entities[j].entity->get_collider()->get_radius();
 187
 188         /* Points in the direction the first body needs to move in to clear the
 189         penetration */
 190         LinAl::Vector<float, 2> delta = pos1-pos2;
 191         float d_sq = inner_product(delta, delta);
 192         float r_sum = r1+r2;
 193         if(d_sq<r_sum*r_sum)
 194         {
 195                 Collision &collision = get_collision(i, j);
 196                 collision.normal = normalize(delta);
 197                 collision.depth = r1+r2-sqrt(d_sq);
 198                 collision.point = pos1-collision.normal*(r1-collision.depth/2);
 199                 if(collision.body1!=i)
 200                         collision.normal = -collision.normal;
 201         }
 202 }