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