Remove destroyed entities from the physics simulation

[libs/game.git] / examples / bassteroids / source / physics.cpp

diff --git a/examples/bassteroids/source/physics.cpp b/examples/bassteroids/source/physics.cpp
index 2453fd90ec82be2ed213eaec91da41c642b1b21f..73d858295db7df316f55a7e8396c9312e09c5c81 100644 (file)
--- a/examples/bassteroids/source/physics.cpp
+++ b/examples/bassteroids/source/physics.cpp
@@ -1,5 +1,5 @@
 #include "physics.h"
-#include <algorithm>
+#include <msp/core/algorithm.h>
 #include <msp/game/transform.h>
 #include "physicalentity.h"
 
@@ -8,9 +8,11 @@ using namespace Msp;
 
 Physics::Physics(Game::Stage &s):
        System(s),
+       event_source(stage.get_event_bus()),
        observer(stage.get_event_bus())
 {
        observer.observe<Game::Events::EntityCreated>([this](auto &e){ entity_added(e); });
+       observer.observe<Game::Events::EntityDestroyed>([this](auto &e){ entity_removed(e); });
 
        stage.synthesize_initial_events(observer);
 }
@@ -35,6 +37,27 @@ void Physics::entity_added(const Game::Events::EntityCreated &e)
        }
 }
 
+void Physics::entity_removed(const Game::Events::EntityDestroyed &e)
+{
+       if(Game::Handle<PhysicalEntity> physical = dynamic_handle_cast<PhysicalEntity>(e.entity))
+       {
+               auto i = find_member(entities, physical, &SimulatedEntity::entity);
+               if(i!=entities.end())
+               {
+                       size_t index = distance(entities.begin(), i);
+                       if(index<fixture_count)
+                       {
+                               if(index+1!=fixture_count)
+                                       *i = std::move(entities[fixture_count-1]);
+                               entities[fixture_count-1] = std::move(entities.back());
+                       }
+                       else
+                               *i = std::move(entities.back());
+                       entities.pop_back();
+               }
+       }
+}
+
 void Physics::tick(Time::TimeDelta dt)
 {
        float dt_secs = dt/Time::sec;
@@ -59,6 +82,9 @@ void Physics::tick(Time::TimeDelta dt)
                copy_out<true>(entities[i]);
        for(unsigned i=fixture_count; i<entities.size(); ++i)
                copy_out<false>(entities[i]);
+
+       for(const Collision &c: collisions)
+               event_source.emit<Events::Collision>(entities[c.body1].entity->get_collider(), entities[c.body2].entity->get_collider());
 }
 
 template<bool is_fixture>
@@ -186,15 +212,17 @@ void Physics::apply_impulses()
                LinAl::Vector<float, 2> v_p1 = entity1.velocity+LinAl::Vector<float, 2>(-r1.y, r1.x)*entity1.angular_velocity.radians();
                LinAl::Vector<float, 2> v_p2 = entity2.velocity+LinAl::Vector<float, 2>(-r2.y, r2.x)*entity2.angular_velocity.radians();
                LinAl::Vector<float, 2> v_rel = v_p2-v_p1;
-               LinAl::Vector<float, 2> tangent = normalize(v_rel-c.normal*inner_product(v_rel, c.normal));
+               LinAl::Vector<float, 2> tangent = v_rel-c.normal*inner_product(v_rel, c.normal);
+               float v_tan = tangent.norm();
+               tangent = (v_tan>1e-5 ? normalize(tangent) : LinAl::Vector<float, 2>(-c.normal.y, c.normal.x));
                float restitution = 1.0f;
                float friction_coeff = 0.1f;
                float inv_mass_sum = entity1.inverse_mass+entity2.inverse_mass;
                float reaction = (1+restitution)*inner_product(v_rel, c.normal)/inv_mass_sum;
-               float friction = reaction*friction_coeff;
+               float friction = min(reaction*friction_coeff, v_tan/inv_mass_sum);
                LinAl::Vector<float, 2> impulse = c.normal*reaction+tangent*friction;
                entity1.velocity += impulse*entity1.inverse_mass;
-               entity2.velocity -= impulse*entity2.inverse_momi;
+               entity2.velocity -= impulse*entity2.inverse_mass;
                entity1.angular_velocity += Geometry::Angle<float>::from_radians(entity1.inverse_momi*(r1.x*impulse.y-r1.y*impulse.x));
                entity2.angular_velocity -= Geometry::Angle<float>::from_radians(entity2.inverse_momi*(r2.x*impulse.y-r2.y*impulse.x));
        }
@@ -249,11 +277,29 @@ void Physics::collide_circle_box(unsigned i, unsigned j)
        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));
        LinAl::Vector<float, 2> local_cdelta = local_delta-local_closest;
        float d_sq = inner_product(local_cdelta, local_cdelta);
+
        if(d_sq<radius*radius)
        {
                Collision &collision = get_collision(i, j);
-               collision.normal = normalize(LinAl::Vector<float, 2>(c*local_cdelta.x-s*local_cdelta.y, c*local_cdelta.y+s*local_cdelta.x));
-               collision.depth = radius-sqrt(d_sq);
+               if(d_sq>1e-10)
+               {
+                       collision.normal = normalize(LinAl::Vector<float, 2>(c*local_cdelta.x-s*local_cdelta.y, c*local_cdelta.y+s*local_cdelta.x));
+                       collision.depth = radius-sqrt(d_sq);
+               }
+               else
+               {
+                       LinAl::Vector<float, 2> inside_dist(half_size.x-abs(local_delta.x), half_size.y-abs(local_delta.y));
+                       if(inside_dist.x<inside_dist.y)
+                       {
+                               collision.normal = LinAl::Vector<float, 2>(c, s) * (local_delta.x<0 ? -1.0f : 1.0f);
+                               collision.depth = radius+inside_dist.x;
+                       }
+                       else
+                       {
+                               collision.normal = LinAl::Vector<float, 2>(-s, c) * (local_delta.y<0 ? -1.0f : 1.0f);
+                               collision.depth = radius+inside_dist.y;
+                       }
+               }
                collision.point = pos1-collision.normal*(radius-collision.depth/2);
                if(collision.body1!=i)
                        collision.normal = -collision.normal;