#include "physics.h"
-#include <algorithm>
+#include <msp/core/algorithm.h>
#include <msp/game/transform.h>
#include "physicalentity.h"
Physics::Physics(Game::Stage &s):
System(s),
+ event_source(stage.get_event_bus()),
observer(stage.get_event_bus())
{
+ declare_dependency<Game::Transform>(UPDATE);
+ declare_dependency<RigidBody>(CHAINED_UPDATE);
+ declare_dependency<Collider>(READ_OLD);
+
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);
}
}
}
+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;
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>
entity.rotation = Geometry::atan2<float>(2*(r.a*r.d+r.b*r.c), 1-2*(r.c*r.c+r.d*r.d));
if constexpr(is_fixture)
+ {
entity.inverse_mass = 0.0f;
+ entity.inverse_momi = 0.0f;
+ }
else
{
Game::Handle<RigidBody> body = entity.entity->get_body();
entity.inverse_mass = 1.0f/body->get_mass();
- entity.moment_of_inertia = body->get_moment_of_inertia();
+ entity.inverse_momi = 1.0f/body->get_moment_of_inertia();
+ entity.external_force = body->get_force();
+ entity.external_torque = body->get_torque();
entity.velocity = body->get_velocity();
entity.angular_velocity = body->get_angular_velocity();
}
Game::Handle<RigidBody> body = entity.entity->get_body();
body->set_velocity(entity.velocity);
body->set_angular_velocity(entity.angular_velocity);
+ body->clear_forces();
}
}
{
SimulatedEntity &entity = entities[i];
- LinAl::Vector<float, 2> new_velocity = entity.velocity+entity.external_force*dt_secs*entity.inverse_mass;
+ LinAl::Vector<float, 2> new_velocity = entity.velocity+entity.external_force*(dt_secs*entity.inverse_mass);
entity.position += (entity.velocity+new_velocity)*(dt_secs/2);
entity.velocity = new_velocity;
- Geometry::Angle<float> new_angular_velocity = entity.angular_velocity+Geometry::Angle<float>::from_radians(entity.external_torque*(dt_secs/entity.moment_of_inertia));
+ Geometry::Angle<float> new_angular_velocity = entity.angular_velocity+Geometry::Angle<float>::from_radians(entity.external_torque*dt_secs*entity.inverse_momi);
entity.rotation = wrap_positive(entity.rotation+(entity.angular_velocity+new_angular_velocity)*(dt_secs/2));
entity.angular_velocity = new_angular_velocity;
}
{
SimulatedEntity &entity1 = entities[c.body1];
SimulatedEntity &entity2 = entities[c.body2];
- LinAl::Vector<float, 2> v_rel = entity2.velocity-entity1.velocity;
+ LinAl::Vector<float, 2> r1 = c.point-entity1.position;
+ LinAl::Vector<float, 2> r2 = c.point-entity2.position;
+ 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 = 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 impulse = (1+restitution)*inner_product(v_rel, c.normal)/inv_mass_sum;
- entity1.velocity += c.normal*(impulse*entity1.inverse_mass);
- entity2.velocity -= c.normal*(impulse*entity2.inverse_mass);
+ float reaction = (1+restitution)*inner_product(v_rel, c.normal)/inv_mass_sum;
+ 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_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));
}
}
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;