--- /dev/null
+#include "physics.h"
+#include <msp/game/transform.h>
+#include "physicalentity.h"
+
+using namespace Msp;
+
+Physics::Physics(Game::Stage &s):
+ System(s),
+ observer(stage.get_event_bus())
+{
+ observer.observe<Game::Events::EntityCreated>([this](auto &e){ entity_added(e); });
+}
+
+void Physics::entity_added(const Game::Events::EntityCreated &e)
+{
+ if(Game::Handle<PhysicalEntity> physical = dynamic_handle_cast<PhysicalEntity>(e.entity))
+ {
+ for(Game::Handle<Game::Entity> p=e.entity->get_parent(); p; p=p->get_parent())
+ if(p->get_transform())
+ return;
+
+ SimulatedEntity sim_body;
+ sim_body.entity = physical;
+ if(physical->is_fixture())
+ {
+ entities.insert(entities.begin()+fixture_count, sim_body);
+ ++fixture_count;
+ }
+ else
+ entities.push_back(sim_body);
+ }
+}
+
+void Physics::tick(Time::TimeDelta dt)
+{
+ float dt_secs = dt/Time::sec;
+
+ for(unsigned i=0; i<fixture_count; ++i)
+ copy_in<true>(entities[i]);
+ for(unsigned i=fixture_count; i<entities.size(); ++i)
+ copy_in<false>(entities[i]);
+
+ step(dt_secs);
+
+ collisions.clear();
+ for(unsigned i=0; i<10; ++i)
+ {
+ detect_collisions();
+ solve_collisions();
+ }
+
+ apply_impulses();
+
+ for(unsigned i=0; i<fixture_count; ++i)
+ copy_out<true>(entities[i]);
+ for(unsigned i=fixture_count; i<entities.size(); ++i)
+ copy_out<false>(entities[i]);
+}
+
+template<bool is_fixture>
+void Physics::copy_in(SimulatedEntity &entity)
+{
+ Game::Handle<Game::Transform> transform = entity.entity->get_transform();
+ entity.position = transform->get_position().slice<2>(0);
+
+ if constexpr(is_fixture)
+ entity.inverse_mass = 0.0f;
+ else
+ {
+ Game::Handle<RigidBody> body = entity.entity->get_body();
+ entity.inverse_mass = 1.0f/body->get_mass();
+ entity.velocity = body->get_velocity();
+ }
+}
+
+template<bool is_fixture>
+void Physics::copy_out(SimulatedEntity &entity)
+{
+ Game::Handle<Game::Transform> transform = entity.entity->get_transform();
+ transform->set_position(compose(entity.position, 0.0f));
+
+ if constexpr(!is_fixture)
+ {
+ Game::Handle<RigidBody> body = entity.entity->get_body();
+ body->set_velocity(entity.velocity);
+ }
+}
+
+void Physics::step(float dt_secs)
+{
+ for(unsigned i=fixture_count; i<entities.size(); ++i)
+ {
+ SimulatedEntity &entity = entities[i];
+
+ 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;
+ }
+}
+
+void Physics::detect_collisions()
+{
+ for(auto &c: collisions)
+ c.depth = 0.0f;
+
+ for(unsigned i=fixture_count; i<entities.size(); ++i)
+ {
+ Game::Handle<PhysicalEntity> entity1 = entities[i].entity;
+ ColliderType type1 = entity1->get_collider()->get_type();
+ for(unsigned j=0; j<i; ++j)
+ {
+ Game::Handle<PhysicalEntity> entity2 = entities[j].entity;
+ ColliderType type2 = entity2->get_collider()->get_type();
+ if(type1==ColliderType::CIRCLE && type2==ColliderType::CIRCLE)
+ collide_circle_circle(i, j);
+ }
+ }
+}
+
+void Physics::solve_collisions()
+{
+ for(auto &e: entities)
+ {
+ e.position_adjust = LinAl::Vector<float, 2>();
+ e.collision_count = 0;
+ }
+
+ for(const auto &c: collisions)
+ {
+ if(!c.depth)
+ continue;
+
+ SimulatedEntity &entity1 = entities[c.body1];
+ SimulatedEntity &entity2 = entities[c.body2];
+ float inv_mass_sum = 1.0f/(entity1.inverse_mass+entity2.inverse_mass);
+ LinAl::Vector<float, 2> delta = c.normal*c.depth*inv_mass_sum;
+ if(c.body1>=fixture_count)
+ {
+ entity1.position_adjust += delta*entity1.inverse_mass;
+ ++entity1.collision_count;
+ }
+ if(c.body2>=fixture_count)
+ {
+ entity2.position_adjust -= delta*entity1.inverse_mass;
+ ++entity2.collision_count;
+ }
+ }
+
+ for(auto &e: entities)
+ if(e.collision_count)
+ e.position += e.position_adjust/static_cast<float>(e.collision_count);
+}
+
+void Physics::apply_impulses()
+{
+ for(const auto &c: collisions)
+ {
+ SimulatedEntity &entity1 = entities[c.body1];
+ SimulatedEntity &entity2 = entities[c.body2];
+ LinAl::Vector<float, 2> v_rel = entity2.velocity-entity1.velocity;
+ float restitution = 1.0f;
+ 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);
+ }
+}
+
+Physics::Collision &Physics::get_collision(unsigned i, unsigned j)
+{
+ for(auto &c: collisions)
+ if((c.body1==i && c.body2==j) || (c.body1==j && c.body2==i))
+ return c;
+
+ Collision &c = collisions.emplace_back();
+ c.body1 = i;
+ c.body2 = j;
+ return c;
+}
+
+void Physics::collide_circle_circle(unsigned i, unsigned j)
+{
+ const LinAl::Vector<float, 2> &pos1 = entities[i].position;
+ const LinAl::Vector<float, 2> &pos2 = entities[j].position;
+ float r1 = entities[i].entity->get_collider()->get_radius();
+ float r2 = entities[j].entity->get_collider()->get_radius();
+
+ /* Points in the direction the first body needs to move in to clear the
+ penetration */
+ LinAl::Vector<float, 2> delta = pos1-pos2;
+ float d_sq = inner_product(delta, delta);
+ float r_sum = r1+r2;
+ if(d_sq<r_sum*r_sum)
+ {
+ Collision &collision = get_collision(i, j);
+ float d = sqrt(d_sq);
+ collision.normal = normalize(delta);
+ collision.depth = r1+r2-d;
+ collision.point = pos1-collision.normal*(r1-collision.depth/2);
+ if(collision.body1!=i)
+ collision.normal = -collision.normal;
+ }
+}
--- /dev/null
+#ifndef PHYSICS_H_
+#define PHYSICS_H_
+
+#include <msp/game/eventobserver.h>
+#include <msp/game/events.h>
+#include <msp/game/handle.h>
+#include <msp/game/system.h>
+#include <msp/geometry/angle.h>
+#include <msp/linal/vector.h>
+
+class PhysicalEntity;
+
+class Physics: public Msp::Game::System
+{
+private:
+ struct SimulatedEntity
+ {
+ Msp::Game::Handle<PhysicalEntity> entity;
+ float inverse_mass = 1.0f;
+ Msp::LinAl::Vector<float, 2> external_force;
+
+ Msp::LinAl::Vector<float, 2> position;
+ Msp::LinAl::Vector<float, 2> velocity;
+
+ unsigned collision_count;
+ Msp::LinAl::Vector<float, 2> position_adjust;
+ };
+
+ struct Collision
+ {
+ std::uint16_t body1 = 0;
+ std::uint16_t body2 = 0;
+ Msp::LinAl::Vector<float, 2> point;
+ float depth = 0.0f;
+ Msp::LinAl::Vector<float, 2> normal;
+ };
+
+ Msp::Game::EventObserver observer;
+ std::vector<SimulatedEntity> entities;
+ unsigned fixture_count = 0;
+ std::vector<Collision> collisions;
+
+public:
+ Physics(Msp::Game::Stage &);
+
+private:
+ void entity_added(const Msp::Game::Events::EntityCreated &);
+
+public:
+ void tick(Msp::Time::TimeDelta) override;
+
+private:
+ template<bool>
+ void copy_in(SimulatedEntity &);
+
+ template<bool>
+ void copy_out(SimulatedEntity &);
+
+ void step(float);
+ void detect_collisions();
+ void solve_collisions();
+ void apply_impulses();
+
+ Collision &get_collision(unsigned, unsigned);
+ void collide_circle_circle(unsigned, unsigned);
+};
+
+#endif
projects / libs / game.git / commitdiff