3 #include <msp/game/transform.h>
4 #include "physicalentity.h"
9 Physics::Physics(Game::Stage &s):
11 observer(stage.get_event_bus())
13 observer.observe<Game::Events::EntityCreated>([this](auto &e){ entity_added(e); });
16 void Physics::entity_added(const Game::Events::EntityCreated &e)
18 if(Game::Handle<PhysicalEntity> physical = dynamic_handle_cast<PhysicalEntity>(e.entity))
20 for(Game::Handle<Game::Entity> p=e.entity->get_parent(); p; p=p->get_parent())
21 if(p->get_transform())
24 SimulatedEntity sim_body;
25 sim_body.entity = physical;
26 if(physical->is_fixture())
28 entities.insert(entities.begin()+fixture_count, sim_body);
32 entities.push_back(sim_body);
36 void Physics::tick(Time::TimeDelta dt)
38 float dt_secs = dt/Time::sec;
40 for(unsigned i=0; i<fixture_count; ++i)
41 copy_in<true>(entities[i]);
42 for(unsigned i=fixture_count; i<entities.size(); ++i)
43 copy_in<false>(entities[i]);
48 for(unsigned i=0; i<10; ++i)
56 for(unsigned i=0; i<fixture_count; ++i)
57 copy_out<true>(entities[i]);
58 for(unsigned i=fixture_count; i<entities.size(); ++i)
59 copy_out<false>(entities[i]);
62 template<bool is_fixture>
63 void Physics::copy_in(SimulatedEntity &entity)
65 Game::Handle<Game::Transform> transform = entity.entity->get_transform();
66 entity.position = transform->get_position().slice<2>(0);
67 const Geometry::Quaternion<float> &r = transform->get_rotation();
68 entity.rotation = Geometry::atan2<float>(2*(r.a*r.d+r.b*r.c), 1-2*(r.c*r.c+r.d*r.d));
70 if constexpr(is_fixture)
71 entity.inverse_mass = 0.0f;
74 Game::Handle<RigidBody> body = entity.entity->get_body();
75 entity.inverse_mass = 1.0f/body->get_mass();
76 entity.velocity = body->get_velocity();
80 template<bool is_fixture>
81 void Physics::copy_out(SimulatedEntity &entity)
83 Game::Handle<Game::Transform> transform = entity.entity->get_transform();
84 transform->set_position(compose(entity.position, 0.0f));
85 transform->set_rotation(Geometry::Quaternion<float>::rotation(entity.rotation, LinAl::Vector<float, 3>(0, 0, 1)));
87 if constexpr(!is_fixture)
89 Game::Handle<RigidBody> body = entity.entity->get_body();
90 body->set_velocity(entity.velocity);
94 void Physics::step(float dt_secs)
96 for(unsigned i=fixture_count; i<entities.size(); ++i)
98 SimulatedEntity &entity = entities[i];
100 LinAl::Vector<float, 2> new_velocity = entity.velocity+entity.external_force*dt_secs*entity.inverse_mass;
101 entity.position += (entity.velocity+new_velocity)*(dt_secs/2);
102 entity.velocity = new_velocity;
106 void Physics::detect_collisions()
108 for(auto &c: collisions)
111 for(unsigned i=fixture_count; i<entities.size(); ++i)
113 Game::Handle<PhysicalEntity> entity1 = entities[i].entity;
114 ColliderType type1 = entity1->get_collider()->get_type();
115 for(unsigned j=0; j<i; ++j)
117 Game::Handle<PhysicalEntity> entity2 = entities[j].entity;
118 ColliderType type2 = entity2->get_collider()->get_type();
119 if(type1==ColliderType::CIRCLE && type2==ColliderType::CIRCLE)
120 collide_circle_circle(i, j);
121 else if(type1==ColliderType::CIRCLE && type2==ColliderType::BOX)
122 collide_circle_box(i, j);
123 else if(type1==ColliderType::BOX && type2==ColliderType::CIRCLE)
124 collide_circle_box(j, i);
129 void Physics::solve_collisions()
131 for(auto &e: entities)
133 e.position_adjust = LinAl::Vector<float, 2>();
134 e.collision_count = 0;
137 for(const auto &c: collisions)
142 SimulatedEntity &entity1 = entities[c.body1];
143 SimulatedEntity &entity2 = entities[c.body2];
144 float inv_mass_sum = 1.0f/(entity1.inverse_mass+entity2.inverse_mass);
145 LinAl::Vector<float, 2> delta = c.normal*c.depth*inv_mass_sum;
146 if(c.body1>=fixture_count)
148 entity1.position_adjust += delta*entity1.inverse_mass;
149 ++entity1.collision_count;
151 if(c.body2>=fixture_count)
153 entity2.position_adjust -= delta*entity1.inverse_mass;
154 ++entity2.collision_count;
158 for(auto &e: entities)
159 if(e.collision_count)
160 e.position += e.position_adjust/static_cast<float>(e.collision_count);
163 void Physics::apply_impulses()
165 for(const auto &c: collisions)
167 SimulatedEntity &entity1 = entities[c.body1];
168 SimulatedEntity &entity2 = entities[c.body2];
169 LinAl::Vector<float, 2> v_rel = entity2.velocity-entity1.velocity;
170 float restitution = 1.0f;
171 float inv_mass_sum = entity1.inverse_mass+entity2.inverse_mass;
172 float impulse = (1+restitution)*inner_product(v_rel, c.normal)/inv_mass_sum;
173 entity1.velocity += c.normal*(impulse*entity1.inverse_mass);
174 entity2.velocity -= c.normal*(impulse*entity2.inverse_mass);
178 Physics::Collision &Physics::get_collision(unsigned i, unsigned j)
180 for(auto &c: collisions)
181 if((c.body1==i && c.body2==j) || (c.body1==j && c.body2==i))
184 Collision &c = collisions.emplace_back();
190 void Physics::collide_circle_circle(unsigned i, unsigned j)
192 const LinAl::Vector<float, 2> &pos1 = entities[i].position;
193 const LinAl::Vector<float, 2> &pos2 = entities[j].position;
194 float r1 = entities[i].entity->get_collider()->get_radius();
195 float r2 = entities[j].entity->get_collider()->get_radius();
197 /* Points in the direction the first body needs to move in to clear the
199 LinAl::Vector<float, 2> delta = pos1-pos2;
200 float d_sq = inner_product(delta, delta);
204 Collision &collision = get_collision(i, j);
205 collision.normal = normalize(delta);
206 collision.depth = r1+r2-sqrt(d_sq);
207 collision.point = pos1-collision.normal*(r1-collision.depth/2);
208 if(collision.body1!=i)
209 collision.normal = -collision.normal;
213 void Physics::collide_circle_box(unsigned i, unsigned j)
215 const LinAl::Vector<float, 2> &pos1 = entities[i].position;
216 const LinAl::Vector<float, 2> &pos2 = entities[j].position;
217 float radius = entities[i].entity->get_collider()->get_radius();
218 LinAl::Vector<float, 2> half_size = entities[j].entity->get_collider()->get_size()/2.0f;
220 LinAl::Vector<float, 2> delta = pos1-pos2;
221 float c = cos(entities[j].rotation);
222 float s = sin(entities[j].rotation);
223 LinAl::Vector<float, 2> local_delta(c*delta.x+s*delta.y, c*delta.y-s*delta.x);
224 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));
225 LinAl::Vector<float, 2> local_cdelta = local_delta-local_closest;
226 float d_sq = inner_product(local_cdelta, local_cdelta);
227 if(d_sq<radius*radius)
229 Collision &collision = get_collision(i, j);
230 collision.normal = normalize(LinAl::Vector<float, 2>(c*local_cdelta.x-s*local_cdelta.y, c*local_cdelta.y+s*local_cdelta.x));
231 collision.depth = radius-sqrt(d_sq);
232 collision.point = pos1-collision.normal*(radius-collision.depth/2);
233 if(collision.body1!=i)
234 collision.normal = -collision.normal;