2 #include <msp/game/transform.h>
3 #include "physicalentity.h"
7 Physics::Physics(Game::Stage &s):
9 observer(stage.get_event_bus())
11 observer.observe<Game::Events::EntityCreated>([this](auto &e){ entity_added(e); });
14 void Physics::entity_added(const Game::Events::EntityCreated &e)
16 if(Game::Handle<PhysicalEntity> physical = dynamic_handle_cast<PhysicalEntity>(e.entity))
18 for(Game::Handle<Game::Entity> p=e.entity->get_parent(); p; p=p->get_parent())
19 if(p->get_transform())
22 SimulatedEntity sim_body;
23 sim_body.entity = physical;
24 if(physical->is_fixture())
26 entities.insert(entities.begin()+fixture_count, sim_body);
30 entities.push_back(sim_body);
34 void Physics::tick(Time::TimeDelta dt)
36 float dt_secs = dt/Time::sec;
38 for(unsigned i=0; i<fixture_count; ++i)
39 copy_in<true>(entities[i]);
40 for(unsigned i=fixture_count; i<entities.size(); ++i)
41 copy_in<false>(entities[i]);
46 for(unsigned i=0; i<10; ++i)
54 for(unsigned i=0; i<fixture_count; ++i)
55 copy_out<true>(entities[i]);
56 for(unsigned i=fixture_count; i<entities.size(); ++i)
57 copy_out<false>(entities[i]);
60 template<bool is_fixture>
61 void Physics::copy_in(SimulatedEntity &entity)
63 Game::Handle<Game::Transform> transform = entity.entity->get_transform();
64 entity.position = transform->get_position().slice<2>(0);
66 if constexpr(is_fixture)
67 entity.inverse_mass = 0.0f;
70 Game::Handle<RigidBody> body = entity.entity->get_body();
71 entity.inverse_mass = 1.0f/body->get_mass();
72 entity.velocity = body->get_velocity();
76 template<bool is_fixture>
77 void Physics::copy_out(SimulatedEntity &entity)
79 Game::Handle<Game::Transform> transform = entity.entity->get_transform();
80 transform->set_position(compose(entity.position, 0.0f));
82 if constexpr(!is_fixture)
84 Game::Handle<RigidBody> body = entity.entity->get_body();
85 body->set_velocity(entity.velocity);
89 void Physics::step(float dt_secs)
91 for(unsigned i=fixture_count; i<entities.size(); ++i)
93 SimulatedEntity &entity = entities[i];
95 LinAl::Vector<float, 2> new_velocity = entity.velocity+entity.external_force*dt_secs*entity.inverse_mass;
96 entity.position += (entity.velocity+new_velocity)*(dt_secs/2);
97 entity.velocity = new_velocity;
101 void Physics::detect_collisions()
103 for(auto &c: collisions)
106 for(unsigned i=fixture_count; i<entities.size(); ++i)
108 Game::Handle<PhysicalEntity> entity1 = entities[i].entity;
109 ColliderType type1 = entity1->get_collider()->get_type();
110 for(unsigned j=0; j<i; ++j)
112 Game::Handle<PhysicalEntity> entity2 = entities[j].entity;
113 ColliderType type2 = entity2->get_collider()->get_type();
114 if(type1==ColliderType::CIRCLE && type2==ColliderType::CIRCLE)
115 collide_circle_circle(i, j);
120 void Physics::solve_collisions()
122 for(auto &e: entities)
124 e.position_adjust = LinAl::Vector<float, 2>();
125 e.collision_count = 0;
128 for(const auto &c: collisions)
133 SimulatedEntity &entity1 = entities[c.body1];
134 SimulatedEntity &entity2 = entities[c.body2];
135 float inv_mass_sum = 1.0f/(entity1.inverse_mass+entity2.inverse_mass);
136 LinAl::Vector<float, 2> delta = c.normal*c.depth*inv_mass_sum;
137 if(c.body1>=fixture_count)
139 entity1.position_adjust += delta*entity1.inverse_mass;
140 ++entity1.collision_count;
142 if(c.body2>=fixture_count)
144 entity2.position_adjust -= delta*entity1.inverse_mass;
145 ++entity2.collision_count;
149 for(auto &e: entities)
150 if(e.collision_count)
151 e.position += e.position_adjust/static_cast<float>(e.collision_count);
154 void Physics::apply_impulses()
156 for(const auto &c: collisions)
158 SimulatedEntity &entity1 = entities[c.body1];
159 SimulatedEntity &entity2 = entities[c.body2];
160 LinAl::Vector<float, 2> v_rel = entity2.velocity-entity1.velocity;
161 float restitution = 1.0f;
162 float inv_mass_sum = entity1.inverse_mass+entity2.inverse_mass;
163 float impulse = (1+restitution)*inner_product(v_rel, c.normal)/inv_mass_sum;
164 entity1.velocity += c.normal*(impulse*entity1.inverse_mass);
165 entity2.velocity -= c.normal*(impulse*entity2.inverse_mass);
169 Physics::Collision &Physics::get_collision(unsigned i, unsigned j)
171 for(auto &c: collisions)
172 if((c.body1==i && c.body2==j) || (c.body1==j && c.body2==i))
175 Collision &c = collisions.emplace_back();
181 void Physics::collide_circle_circle(unsigned i, unsigned j)
183 const LinAl::Vector<float, 2> &pos1 = entities[i].position;
184 const LinAl::Vector<float, 2> &pos2 = entities[j].position;
185 float r1 = entities[i].entity->get_collider()->get_radius();
186 float r2 = entities[j].entity->get_collider()->get_radius();
188 /* Points in the direction the first body needs to move in to clear the
190 LinAl::Vector<float, 2> delta = pos1-pos2;
191 float d_sq = inner_product(delta, delta);
195 Collision &collision = get_collision(i, j);
196 float d = sqrt(d_sq);
197 collision.normal = normalize(delta);
198 collision.depth = r1+r2-d;
199 collision.point = pos1-collision.normal*(r1-collision.depth/2);
200 if(collision.body1!=i)
201 collision.normal = -collision.normal;