Simulate friction in collisions

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

diff --git a/examples/bassteroids/source/physics.cpp b/examples/bassteroids/source/physics.cpp
index 0fb43260f4cff7b82f57e39eb6fed6aa72b3b954..2453fd90ec82be2ed213eaec91da41c642b1b21f 100644 (file)
--- a/examples/bassteroids/source/physics.cpp
+++ b/examples/bassteroids/source/physics.cpp
@@ -70,12 +70,15 @@ void Physics::copy_in(SimulatedEntity &entity)
        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.moment_of_inertia = body->get_moment_of_inertia();
+               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();
@@ -105,11 +108,11 @@ 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.moment_of_inertia));
+               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;
        }
@@ -178,12 +181,22 @@ 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 = normalize(v_rel-c.normal*inner_product(v_rel, c.normal));
                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 = reaction*friction_coeff;
+               LinAl::Vector<float, 2> impulse = c.normal*reaction+tangent*friction;
+               entity1.velocity += impulse*entity1.inverse_mass;
+               entity2.velocity -= impulse*entity2.inverse_momi;
+               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));
        }
 }