2 #include <msp/core/algorithm.h>
3 #include <msp/game/transform.h>
4 #include "physicalentity.h"
9 Physics::Physics(Game::Stage &s):
11 event_source(stage.get_event_bus()),
12 observer(stage.get_event_bus())
14 declare_dependency<Game::Transform>(UPDATE);
15 declare_dependency<RigidBody>(CHAINED_UPDATE);
16 declare_dependency<Collider>(READ_OLD);
18 observer.observe<Game::Events::EntityCreated>([this](auto &e){ entity_added(e); });
19 observer.observe<Game::Events::EntityDestroyed>([this](auto &e){ entity_removed(e); });
21 stage.synthesize_initial_events(observer);
24 void Physics::entity_added(const Game::Events::EntityCreated &e)
26 if(Game::Handle<PhysicalEntity> physical = dynamic_handle_cast<PhysicalEntity>(e.entity))
28 for(Game::Handle<Game::Entity> p=e.entity->get_parent(); p; p=p->get_parent())
29 if(p->get_transform())
32 SimulatedEntity sim_body;
33 sim_body.entity = physical;
34 if(physical->is_fixture())
36 entities.insert(entities.begin()+fixture_count, sim_body);
40 entities.push_back(sim_body);
44 void Physics::entity_removed(const Game::Events::EntityDestroyed &e)
46 if(Game::Handle<PhysicalEntity> physical = dynamic_handle_cast<PhysicalEntity>(e.entity))
48 auto i = find_member(entities, physical, &SimulatedEntity::entity);
51 size_t index = distance(entities.begin(), i);
52 if(index<fixture_count)
54 if(index+1!=fixture_count)
55 *i = std::move(entities[fixture_count-1]);
56 entities[fixture_count-1] = std::move(entities.back());
59 *i = std::move(entities.back());
65 void Physics::tick(Time::TimeDelta dt)
67 float dt_secs = dt/Time::sec;
69 for(unsigned i=0; i<fixture_count; ++i)
70 copy_in<true>(entities[i]);
71 for(unsigned i=fixture_count; i<entities.size(); ++i)
72 copy_in<false>(entities[i]);
77 for(unsigned i=0; i<10; ++i)
85 for(unsigned i=0; i<fixture_count; ++i)
86 copy_out<true>(entities[i]);
87 for(unsigned i=fixture_count; i<entities.size(); ++i)
88 copy_out<false>(entities[i]);
90 for(const Collision &c: collisions)
91 event_source.emit<Events::Collision>(entities[c.body1].entity->get_collider(), entities[c.body2].entity->get_collider());
94 template<bool is_fixture>
95 void Physics::copy_in(SimulatedEntity &entity)
97 Game::Handle<Game::Transform> transform = entity.entity->get_transform();
98 entity.position = transform->get_position().slice<2>(0);
99 const Geometry::Quaternion<float> &r = transform->get_rotation();
100 entity.rotation = Geometry::atan2<float>(2*(r.a*r.d+r.b*r.c), 1-2*(r.c*r.c+r.d*r.d));
102 if constexpr(is_fixture)
104 entity.inverse_mass = 0.0f;
105 entity.inverse_momi = 0.0f;
109 Game::Handle<RigidBody> body = entity.entity->get_body();
110 entity.inverse_mass = 1.0f/body->get_mass();
111 entity.inverse_momi = 1.0f/body->get_moment_of_inertia();
112 entity.external_force = body->get_force();
113 entity.external_torque = body->get_torque();
114 entity.velocity = body->get_velocity();
115 entity.angular_velocity = body->get_angular_velocity();
119 template<bool is_fixture>
120 void Physics::copy_out(SimulatedEntity &entity)
122 Game::Handle<Game::Transform> transform = entity.entity->get_transform();
123 transform->set_position(compose(entity.position, 0.0f));
124 transform->set_rotation(Geometry::Quaternion<float>::rotation(entity.rotation, LinAl::Vector<float, 3>(0, 0, 1)));
126 if constexpr(!is_fixture)
128 Game::Handle<RigidBody> body = entity.entity->get_body();
129 body->set_velocity(entity.velocity);
130 body->set_angular_velocity(entity.angular_velocity);
131 body->clear_forces();
135 void Physics::step(float dt_secs)
137 for(unsigned i=fixture_count; i<entities.size(); ++i)
139 SimulatedEntity &entity = entities[i];
141 LinAl::Vector<float, 2> new_velocity = entity.velocity+entity.external_force*(dt_secs*entity.inverse_mass);
142 entity.position += (entity.velocity+new_velocity)*(dt_secs/2);
143 entity.velocity = new_velocity;
145 Geometry::Angle<float> new_angular_velocity = entity.angular_velocity+Geometry::Angle<float>::from_radians(entity.external_torque*dt_secs*entity.inverse_momi);
146 entity.rotation = wrap_positive(entity.rotation+(entity.angular_velocity+new_angular_velocity)*(dt_secs/2));
147 entity.angular_velocity = new_angular_velocity;
151 void Physics::detect_collisions()
153 for(auto &c: collisions)
156 for(unsigned i=fixture_count; i<entities.size(); ++i)
158 Game::Handle<PhysicalEntity> entity1 = entities[i].entity;
159 ColliderType type1 = entity1->get_collider()->get_type();
160 for(unsigned j=0; j<i; ++j)
162 Game::Handle<PhysicalEntity> entity2 = entities[j].entity;
163 ColliderType type2 = entity2->get_collider()->get_type();
164 if(type1==ColliderType::CIRCLE && type2==ColliderType::CIRCLE)
165 collide_circle_circle(i, j);
166 else if(type1==ColliderType::CIRCLE && type2==ColliderType::BOX)
167 collide_circle_box(i, j);
168 else if(type1==ColliderType::BOX && type2==ColliderType::CIRCLE)
169 collide_circle_box(j, i);
174 void Physics::solve_collisions()
176 for(auto &e: entities)
178 e.position_adjust = LinAl::Vector<float, 2>();
179 e.collision_count = 0;
182 for(const auto &c: collisions)
187 SimulatedEntity &entity1 = entities[c.body1];
188 SimulatedEntity &entity2 = entities[c.body2];
189 float inv_mass_sum = 1.0f/(entity1.inverse_mass+entity2.inverse_mass);
190 LinAl::Vector<float, 2> delta = c.normal*c.depth*inv_mass_sum;
191 if(c.body1>=fixture_count)
193 entity1.position_adjust += delta*entity1.inverse_mass;
194 ++entity1.collision_count;
196 if(c.body2>=fixture_count)
198 entity2.position_adjust -= delta*entity1.inverse_mass;
199 ++entity2.collision_count;
203 for(auto &e: entities)
204 if(e.collision_count)
205 e.position += e.position_adjust/static_cast<float>(e.collision_count);
208 void Physics::apply_impulses()
210 for(const auto &c: collisions)
212 SimulatedEntity &entity1 = entities[c.body1];
213 SimulatedEntity &entity2 = entities[c.body2];
214 LinAl::Vector<float, 2> r1 = c.point-entity1.position;
215 LinAl::Vector<float, 2> r2 = c.point-entity2.position;
216 LinAl::Vector<float, 2> v_p1 = entity1.velocity+LinAl::Vector<float, 2>(-r1.y, r1.x)*entity1.angular_velocity.radians();
217 LinAl::Vector<float, 2> v_p2 = entity2.velocity+LinAl::Vector<float, 2>(-r2.y, r2.x)*entity2.angular_velocity.radians();
218 LinAl::Vector<float, 2> v_rel = v_p2-v_p1;
219 LinAl::Vector<float, 2> tangent = v_rel-c.normal*inner_product(v_rel, c.normal);
220 float v_tan = tangent.norm();
221 tangent = (v_tan>1e-5 ? normalize(tangent) : LinAl::Vector<float, 2>(-c.normal.y, c.normal.x));
222 float restitution = 1.0f;
223 float friction_coeff = 0.1f;
224 float inv_mass_sum = entity1.inverse_mass+entity2.inverse_mass;
225 float reaction = (1+restitution)*inner_product(v_rel, c.normal)/inv_mass_sum;
226 float friction = min(reaction*friction_coeff, v_tan/inv_mass_sum);
227 LinAl::Vector<float, 2> impulse = c.normal*reaction+tangent*friction;
228 entity1.velocity += impulse*entity1.inverse_mass;
229 entity2.velocity -= impulse*entity2.inverse_mass;
230 entity1.angular_velocity += Geometry::Angle<float>::from_radians(entity1.inverse_momi*(r1.x*impulse.y-r1.y*impulse.x));
231 entity2.angular_velocity -= Geometry::Angle<float>::from_radians(entity2.inverse_momi*(r2.x*impulse.y-r2.y*impulse.x));
235 Physics::Collision &Physics::get_collision(unsigned i, unsigned j)
237 for(auto &c: collisions)
238 if((c.body1==i && c.body2==j) || (c.body1==j && c.body2==i))
241 Collision &c = collisions.emplace_back();
247 void Physics::collide_circle_circle(unsigned i, unsigned j)
249 const LinAl::Vector<float, 2> &pos1 = entities[i].position;
250 const LinAl::Vector<float, 2> &pos2 = entities[j].position;
251 float r1 = entities[i].entity->get_collider()->get_radius();
252 float r2 = entities[j].entity->get_collider()->get_radius();
254 /* Points in the direction the first body needs to move in to clear the
256 LinAl::Vector<float, 2> delta = pos1-pos2;
257 float d_sq = inner_product(delta, delta);
261 Collision &collision = get_collision(i, j);
262 collision.normal = normalize(delta);
263 collision.depth = r1+r2-sqrt(d_sq);
264 collision.point = pos1-collision.normal*(r1-collision.depth/2);
265 if(collision.body1!=i)
266 collision.normal = -collision.normal;
270 void Physics::collide_circle_box(unsigned i, unsigned j)
272 const LinAl::Vector<float, 2> &pos1 = entities[i].position;
273 const LinAl::Vector<float, 2> &pos2 = entities[j].position;
274 float radius = entities[i].entity->get_collider()->get_radius();
275 LinAl::Vector<float, 2> half_size = entities[j].entity->get_collider()->get_size()/2.0f;
277 LinAl::Vector<float, 2> delta = pos1-pos2;
278 float c = cos(entities[j].rotation);
279 float s = sin(entities[j].rotation);
280 LinAl::Vector<float, 2> local_delta(c*delta.x+s*delta.y, c*delta.y-s*delta.x);
281 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));
282 LinAl::Vector<float, 2> local_cdelta = local_delta-local_closest;
283 float d_sq = inner_product(local_cdelta, local_cdelta);
285 if(d_sq<radius*radius)
287 Collision &collision = get_collision(i, j);
290 collision.normal = normalize(LinAl::Vector<float, 2>(c*local_cdelta.x-s*local_cdelta.y, c*local_cdelta.y+s*local_cdelta.x));
291 collision.depth = radius-sqrt(d_sq);
295 LinAl::Vector<float, 2> inside_dist(half_size.x-abs(local_delta.x), half_size.y-abs(local_delta.y));
296 if(inside_dist.x<inside_dist.y)
298 collision.normal = LinAl::Vector<float, 2>(c, s) * (local_delta.x<0 ? -1.0f : 1.0f);
299 collision.depth = radius+inside_dist.x;
303 collision.normal = LinAl::Vector<float, 2>(-s, c) * (local_delta.y<0 ? -1.0f : 1.0f);
304 collision.depth = radius+inside_dist.y;
307 collision.point = pos1-collision.normal*(radius-collision.depth/2);
308 if(collision.body1!=i)
309 collision.normal = -collision.normal;