implemented velocity verlet integration

src/collision.c

@@ -79,7 +79,7 @@ int _internal_collision_get_overlap(PhysicsEntity self, PhysicsEntity other, Col
         }
 
         if(sqr_mag == 0) {
-            out->separation_force = InfinityVector;
+            out->penetration_vector = InfinityVector;
             return 0;
         }
     }
@@ -96,7 +96,7 @@ int _internal_collision_get_overlap(PhysicsEntity self, PhysicsEntity other, Col
         .normal = vnormalizedf(shortest_escape),
 
         .velocity = velocity,
-        .separation_force = shortest_escape,
+        .penetration_vector = shortest_escape,
 
         .edge_left = shape_get_point_transformed(self_shape, shortest_escape_edge, *self_transform),
         .edge_right = shape_get_point_transformed(self_shape, (1 + shortest_escape_edge) % self_point_count, *self_transform),
@@ -112,7 +112,7 @@ Collision _internal_collision_invert(Collision collision_a, PhysicsEntity a) {
         .point = collision_a.point,
         .normal = vinvf(collision_a.normal),
         .velocity = vinvf(collision_a.velocity),
-        .separation_force = vinvf(collision_a.separation_force),
+        .penetration_vector = vinvf(collision_a.penetration_vector),
         .edge_left = collision_a.edge_left,
         .edge_right = collision_a.edge_right,
     };
@@ -126,7 +126,7 @@ int collision_check(PhysicsEntity a, PhysicsEntity b, Collision* out_a, Collisio
     if(!collision_a_overlaps || !collision_b_overlaps)
         return 0;
 
-    if(vsqrmagnitudef(collision_a.separation_force) < vsqrmagnitudef(collision_b.separation_force))
+    if(vsqrmagnitudef(collision_a.penetration_vector) < vsqrmagnitudef(collision_b.penetration_vector))
         collision_b = _internal_collision_invert(collision_a, a);
     else
         collision_a = _internal_collision_invert(collision_b, b);

src/collision.h

@@ -12,7 +12,7 @@ typedef struct Collision {
     Vector normal;
 
     Vector velocity;
-    Vector separation_force;
+    Vector penetration_vector;
 
     Vector edge_left;
     Vector edge_right;

src/physics_world.c

@@ -125,7 +125,7 @@ void _internal_physics_narrow_collision() {
 
     for(PhysicsEntity* left = _world_bodies; left < end; ++left) {
         for(PhysicsEntity* right = _world_bodies; right < half_end; ++right) {
-            if(left == right) continue;
+            if(left->data == right->data) continue;
 
             if(collision_check(*left, *right, &collision_left, &collision_right)) {
                 left->tc->on_collision(left->data, collision_left);
@@ -171,7 +171,6 @@ void _internal_physics_apply() {
 
     for(PhysicsEntity* entity = _world_bodies; entity < end; ++entity) {
         body = entity->tc->get_rigidbody(entity->data);
-        rigidbody_solve_contacts(body);
         rigidbody_apply_physics(body);
     }
 }

src/player.c

@@ -55,8 +55,9 @@ void player_start(Player* self) {
 
 void player_update(Player* self, float dt) {
     Vector velocity = rigidbody_get_velocity(self->rigidbody);
-    Vector velocity_target = {directional.x, directional.y};
+    Vector velocity_target = {directional.x, velocity.y};
     rigidbody_accelerate(self->rigidbody, vmulff(vsubf(velocity_target, velocity), 20.f));
+    rigidbody_accelerate(self->rigidbody, (Vector){0.0f, 20.f});
 }
 
 void player_collision(Player* self, Collision hit) {

src/rigidbody.c

@@ -3,6 +3,7 @@
 #include "debug.h"
 #include "program.h"
 #include "collision.h"
+#include "transformable.h"
 
 typedef struct {
     Collision hit;
@@ -15,8 +16,10 @@ struct RigidBody {
 
     float mass;
 
-    Vector linear_force;
+    Vector last_linear_force;
+    Vector next_linear_force;
     Vector linear_velocity;
+    Transform internal_transform;
 
     int is_static;
 
@@ -29,7 +32,13 @@ RigidBody* rigidbody_make(Transformable transform) {
     *self = (RigidBody){
         .transformable = transform,
         .mass = 1.0f,
+
         .linear_velocity = ZeroVector,
+        .next_linear_force = ZeroVector,
+        .last_linear_force = ZeroVector,
+
+        .internal_transform = *transform.tc->get_transform(transform.data),
+
         .is_static = 0,
         .contacts = list_from_type(Contact),
     };
@@ -47,6 +56,7 @@ void rigidbody_add_contact(RigidBody* self, Collision hit) {
     list_foreach(Contact, contact, &self->contacts) {
         if(contact->hit.other.data == hit.other.data) {
             ++contact->expiry;
+            hit.velocity = contact->hit.velocity;
             contact->hit = hit;
             contact->warming += delta_time();
             return;
@@ -74,6 +84,7 @@ void _internal_rigidbody_collect_contacts(RigidBody* self) {
             if(vdotf(vnormalizedf(self->linear_velocity), contact->hit.normal) >= -0.01f || contact->expiry <= -1) {
                 list_erase(&self->contacts, i);
                 i--;
+                continue;
             }
         }
         --(contact->expiry);
@@ -96,21 +107,27 @@ void _internal_debug_draw_collision_edge(Vector left, Vector right, Vector norma
 }
 
 static  inline
-Vector _internal_calculate_contact_force(Contact* contact) {
+Vector _internal_calculate_contact_force(RigidBody* self, Contact* contact) {
     Collision hit = contact->hit;
 
-    if(vsqrmagnitudef(hit.separation_force) < powf(0.01f, 2))
+    if(vsqrmagnitudef(hit.penetration_vector) < powf(0.01f, 2))
         return ZeroVector;
 
-    const float warming = fminf(1.f, contact->warming * 100.f);
+
+    const float warming = fminf(1.f, contact->warming * 1000.f);
     const float d = 1.0f;
-    const float k = 0.001f;
+    const float k = 0.3f;
-    const float b = 1.0f * warming;
+    const float b = 0.3f;
 
     const Vector damping = vmulff(hit.normal, k * d);
-    const Vector bounce = vprojectf(vmulff(hit.normal, -b), hit.velocity);
+    const Vector bounce = vmulff(hit.normal, b * vdotf(hit.normal, hit.velocity));
 
-    return vsubf(damping, bounce);
+    float dot = -vdotf(contact->hit.normal, vaddf(self->next_linear_force, self->linear_velocity));
+    if(dot < 0)
+        dot = 0;
+    Vector counter_force = vmulff(contact->hit.normal, dot * vsqrmagnitudef(hit.penetration_vector));
+
+    return vaddf(vmulff(vsubf(damping, bounce), warming), counter_force);
 }
 
 static inline
@@ -118,20 +135,18 @@ void _internal_rigidbody_solve_contact(RigidBody* self, Contact* contact, Vector
     Collision hit = contact->hit;
     if(vdotf(hit.normal, self->linear_velocity) > 0.0)
         return;
-    // if (vsqrmagnitudef(contact->hit.separation_force) < 0.001f*0.001f)
-        // return;
 
     float normalmag = vmagnitudef(contact->hit.normal);
     ASSERT_RETURN(fabsf(normalmag - 1.f) < 0.99999f,, "Normal of collision not one");
 
-    Vector force = _internal_calculate_contact_force(contact);
+    Vector force = _internal_calculate_contact_force(self, contact);
     float dot = vdotf(vnormalizedf(*solve_forces), force);
     if (veqf(*solve_forces, ZeroVector) || dot * dot > vsqrmagnitudef(*solve_forces) || dot <= 0.0f) {
         #if !NDEBUG
             LOG_INFO("warming: %f", contact->warming);
             LOG_INFO("force: %f %f", force.x, force.y);
             LOG_INFO("dot: %f", dot);
-            LOG_INFO("mag: %f", vmagnitudef(self->linear_force));
+            LOG_INFO("mag: %f", vmagnitudef(self->next_linear_force));
         #endif
         *solve_forces = vaddf(*solve_forces, force);
         rigidbody_add_impulse(self, force);
@@ -153,11 +168,33 @@ void rigidbody_solve_contacts(RigidBody* self) {
     ASSERT_RETURN(!visnanf(self->linear_velocity),, "Velocity is NaN (1)");
 }
 
+static inline
+void _internal_rigidbody_integrate_forces(RigidBody* self) {
+    const float dt = delta_time();
+
+    Vector position = self->internal_transform.position;
+    Vector velocity = self->linear_velocity;
+    Vector acceleration = vmulff(self->last_linear_force, 0.5f * dt*dt);
+
+    position = vaddf(position, vmulff(velocity, dt));
+    position = vaddf(position, acceleration);
+
+    velocity = vaddf(velocity, acceleration);
+    acceleration = vmulff(self->next_linear_force, dt/2);
+    velocity = vaddf(velocity, acceleration);
+
+    self->linear_velocity = velocity;
+    self->internal_transform.position = position;
+    if(vsqrdistf(position, transformable_get_position(self->transformable)) > 0.001f)
+        transformable_set_position(self->transformable, position);
+
+    self->last_linear_force = self->next_linear_force;
+    self->next_linear_force = ZeroVector;
+}
+
 void rigidbody_apply_physics(RigidBody* self) {
-    Vector position = transformable_get_position(self->transformable);
+    rigidbody_solve_contacts(self);
-    Vector velocity = vmulff(self->linear_velocity, delta_time());
+    _internal_rigidbody_integrate_forces(self);
-    transformable_set_position(self->transformable, vaddf(position, velocity));
-    self->linear_force = ZeroVector;
 }
 
 float rigidbody_get_mass(const RigidBody* self) {
@@ -169,12 +206,12 @@ void rigidbody_set_mass(RigidBody* self, float mass) {
 }
 
 void rigidbody_add_impulse(RigidBody* self, Vector force) {
-    self->linear_force = vaddf(self->linear_force, force);
+    rigidbody_accelerate(self, vmulff(force, 1.0f/delta_time()));
-    self->linear_velocity = vaddf(self->linear_velocity, force);
 }
 
 void rigidbody_accelerate(RigidBody* self, Vector force) {
-    rigidbody_add_impulse(self, vmulff(force, delta_time()));
+    if(vsqrmagnitudef(force) > powf(0.01f, 2))
+        self->next_linear_force = vaddf(self->next_linear_force, force);
 }
 
 int rigidbody_is_static(const RigidBody* self) {
@@ -190,7 +227,7 @@ Vector rigidbody_get_velocity(const RigidBody* self) {
 }
 
 void rigidbody_set_velocity(RigidBody* self, Vector velocity) {
-    self->linear_force = vaddf(self->linear_force, vsubf(velocity, self->linear_force));
+    self->next_linear_force = vaddf(self->next_linear_force, vsubf(velocity, self->next_linear_force));
     self->linear_velocity = velocity;
 }
 
@@ -199,3 +236,7 @@ void rigidbody_debug_draw_contacts(RigidBody* self) {
         _internal_debug_draw_collision_edge(contact->hit.edge_left, contact->hit.edge_right, contact->hit.normal);
     }
 }
+
+Transform* rigidbody_get_transform(RigidBody* self) {
+    return &self->internal_transform;
+}

src/rigidbody.h

@@ -31,5 +31,10 @@ extern Vector rigidbody_get_velocity(const RigidBody* self);
 extern void rigidbody_set_velocity(RigidBody* self, Vector velocity);
 
 extern void rigidbody_debug_draw_contacts(RigidBody* self);
+extern Transform* rigidbody_get_transform(RigidBody* self);
+
+impl_Transformable_for(RigidBody,
+    rigidbody_get_transform
+)
 
 #endif // !_fencer_rigidbody_h