Allow external forces and torques to be added on rigid bodies

[libs/game.git] / examples / bassteroids / source / physics.cpp

diff --git a/examples/bassteroids/source/physics.cpp b/examples/bassteroids/source/physics.cpp
index 429715aa99b5d365e26f436e170bd468119f6403..0fb43260f4cff7b82f57e39eb6fed6aa72b3b954 100644 (file)
--- a/examples/bassteroids/source/physics.cpp
+++ b/examples/bassteroids/source/physics.cpp
@@ -1,7 +1,9 @@
 #include "physics.h"
+#include <algorithm>
 #include <msp/game/transform.h>
 #include "physicalentity.h"
 
+using namespace std;
 using namespace Msp;
 
 Physics::Physics(Game::Stage &s):
@@ -9,6 +11,8 @@ Physics::Physics(Game::Stage &s):
        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)
@@ -62,6 +66,8 @@ 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;
@@ -69,7 +75,11 @@ void Physics::copy_in(SimulatedEntity &entity)
        {
                Game::Handle<RigidBody> body = entity.entity->get_body();
                entity.inverse_mass = 1.0f/body->get_mass();
+               entity.moment_of_inertia = 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 +88,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();
        }
 }
 
@@ -95,6 +108,10 @@ void Physics::step(float dt_secs)
                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.moment_of_inertia));
+               entity.rotation = wrap_positive(entity.rotation+(entity.angular_velocity+new_angular_velocity)*(dt_secs/2));
+               entity.angular_velocity = new_angular_velocity;
        }
 }
 
@@ -113,6 +130,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);
                }
        }
 }
@@ -193,11 +214,35 @@ 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);
+               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);
+               collision.point = pos1-collision.normal*(radius-collision.depth/2);
+               if(collision.body1!=i)
+                       collision.normal = -collision.normal;
+       }
+}