Implement box colliders in Bassteroids' physics system

diff --git a/examples/bassteroids/source/physics.cpp b/examples/bassteroids/source/physics.cpp
index 89feeed47eebb6a5beb6f4b5ba1c4eb09e2db364..5d4c242233da509062fecc093b14aca7d0494003 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):
@@ -62,6 +64,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;
@@ -78,6 +82,7 @@ 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)
        {
@@ -113,6 +118,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);
                }
        }
 }
@@ -200,3 +209,28 @@ void Physics::collide_circle_circle(unsigned i, unsigned j)
                        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;
+       }
+}

diff --git a/examples/bassteroids/source/physics.h b/examples/bassteroids/source/physics.h
index a611789ebd1cfa9b16d2c15f1d841ec7b181de27..fdeb8b2c82167577c217d835584872ea037a8cdc 100644 (file)
--- a/examples/bassteroids/source/physics.h
+++ b/examples/bassteroids/source/physics.h
@@ -20,6 +20,7 @@ private:
                Msp::LinAl::Vector<float, 2> external_force;
 
                Msp::LinAl::Vector<float, 2> position;
+               Msp::Geometry::Angle<float> rotation;
                Msp::LinAl::Vector<float, 2> velocity;
 
                unsigned collision_count;
@@ -63,6 +64,7 @@ private:
 
        Collision &get_collision(unsigned, unsigned);
        void collide_circle_circle(unsigned, unsigned);
+       void collide_circle_box(unsigned, unsigned);
 };
 
 #endif