examples/bassteroids/source/physics.cpp

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