X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=examples%2Fbassteroids%2Fsource%2Fphysics.cpp;h=3c435026e925b057b9eaf7ee3987bd08793153c5;hb=4014851ac2ed8e86b5a781dcc3056088d40465a1;hp=429715aa99b5d365e26f436e170bd468119f6403;hpb=0636566dd84ca185d3e9a6fae02459569c42d220;p=libs%2Fgame.git

diff --git a/examples/bassteroids/source/physics.cpp b/examples/bassteroids/source/physics.cpp
index 429715a..3c43502 100644
--- a/examples/bassteroids/source/physics.cpp
+++ b/examples/bassteroids/source/physics.cpp
@@ -1,14 +1,19 @@
 #include "physics.h"
+#include <algorithm>
 #include <msp/game/transform.h>
 #include "physicalentity.h"
 
+using namespace std;
 using namespace Msp;
 
 Physics::Physics(Game::Stage &s):
 	System(s),
+	event_source(stage.get_event_bus()),
 	observer(stage.get_event_bus())
 {
 	observer.observe<Game::Events::EntityCreated>([this](auto &e){ entity_added(e); });
+
+	stage.synthesize_initial_events(observer);
 }
 
 void Physics::entity_added(const Game::Events::EntityCreated &e)
@@ -55,6 +60,9 @@ void Physics::tick(Time::TimeDelta dt)
 		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>
@@ -62,14 +70,23 @@ void Physics::copy_in(SimulatedEntity &entity)
 {
 	Game::Handle<Game::Transform> transform = entity.entity->get_transform();
 	entity.position = transform->get_position().slice<2>(0);
+	const Geometry::Quaternion<float> &r = transform->get_rotation();
+	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.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();
 	}
 }
 
@@ -78,11 +95,14 @@ void Physics::copy_out(SimulatedEntity &entity)
 {
 	Game::Handle<Game::Transform> transform = entity.entity->get_transform();
 	transform->set_position(compose(entity.position, 0.0f));
+	transform->set_rotation(Geometry::Quaternion<float>::rotation(entity.rotation, LinAl::Vector<float, 3>(0, 0, 1)));
 
 	if constexpr(!is_fixture)
 	{
 		Game::Handle<RigidBody> body = entity.entity->get_body();
 		body->set_velocity(entity.velocity);
+		body->set_angular_velocity(entity.angular_velocity);
+		body->clear_forces();
 	}
 }
 
@@ -92,9 +112,13 @@ void Physics::step(float dt_secs)
 	{
 		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.inverse_momi);
+		entity.rotation = wrap_positive(entity.rotation+(entity.angular_velocity+new_angular_velocity)*(dt_secs/2));
+		entity.angular_velocity = new_angular_velocity;
 	}
 }
 
@@ -113,6 +137,10 @@ void Physics::detect_collisions()
 			ColliderType type2 = entity2->get_collider()->get_type();
 			if(type1==ColliderType::CIRCLE && type2==ColliderType::CIRCLE)
 				collide_circle_circle(i, j);
+			else if(type1==ColliderType::CIRCLE && type2==ColliderType::BOX)
+				collide_circle_box(i, j);
+			else if(type1==ColliderType::BOX && type2==ColliderType::CIRCLE)
+				collide_circle_box(j, i);
 		}
 	}
 }
@@ -157,12 +185,24 @@ void Physics::apply_impulses()
 	{
 		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));
 	}
 }
 
@@ -193,11 +233,53 @@ void Physics::collide_circle_circle(unsigned i, unsigned j)
 	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.depth = r1+r2-sqrt(d_sq);
 		collision.point = pos1-collision.normal*(r1-collision.depth/2);
 		if(collision.body1!=i)
 			collision.normal = -collision.normal;
 	}
 }
+
+void Physics::collide_circle_box(unsigned i, unsigned j)
+{
+	const LinAl::Vector<float, 2> &pos1 = entities[i].position;
+	const LinAl::Vector<float, 2> &pos2 = entities[j].position;
+	float radius = entities[i].entity->get_collider()->get_radius();
+	LinAl::Vector<float, 2> half_size = entities[j].entity->get_collider()->get_size()/2.0f;
+
+	LinAl::Vector<float, 2> delta = pos1-pos2;
+	float c = cos(entities[j].rotation);
+	float s = sin(entities[j].rotation);
+	LinAl::Vector<float, 2> local_delta(c*delta.x+s*delta.y, c*delta.y-s*delta.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);
+		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;
+	}
+}