examples/bassteroids/source/physics.cpp

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