X-Git-Url: https://git.lizzy.rs/?a=blobdiff_plain;f=src%2Fgame%2Flevel%2Frigid_bodies.c;h=667916e87fc7b5cf9e31688f1454b001e0fe6555;hb=41e24206d6b0211307641bef85a252890bfc7339;hp=4b8e248def1af9f9f0296b40952d5cc0c55b81f3;hpb=2c998148337607b628c2b659d27833d1e06b7e38;p=nothing.git

diff --git a/src/game/level/rigid_bodies.c b/src/game/level/rigid_bodies.c
index 4b8e248d..667916e8 100644
--- a/src/game/level/rigid_bodies.c
+++ b/src/game/level/rigid_bodies.c
@@ -6,6 +6,10 @@
 #include "system/lt.h"
 #include "system/nth_alloc.h"
 #include "system/stacktrace.h"
+#include "system/line_stream.h"
+#include "system/str.h"
+#include "system/log.h"
+#include "hashset.h"
 
 #include "./rigid_bodies.h"
 
@@ -15,46 +19,18 @@ struct RigidBodies
     size_t capacity;
     size_t count;
 
-    Vec *positions;
-    Vec *velocities;
-    Vec *movements;
-    Vec *sizes;
-    Color *colors;
+    Rect *bodies;
+    Vec2f *velocities;
+    Vec2f *movements;
     bool *grounded;
-    Vec *forces;
+    Vec2f *forces;
+    bool *deleted;
+    bool *disabled;
 };
 
-static const Vec opposing_rect_side_forces[RECT_SIDE_N] = {
-    { .x = 1.0f,  .y =  0.0f  },  /* RECT_SIDE_LEFT = 0, */
-    { .x = -1.0f, .y =  0.0f  },  /* RECT_SIDE_RIGHT, */
-    { .x = 0.0f,  .y =  1.0f, },  /* RECT_SIDE_TOP, */
-    { .x = 0.0f,  .y = -1.0f, }   /* RECT_SIDE_BOTTOM, */
-};
-
-static Vec opposing_force_by_sides(int sides[RECT_SIDE_N])
-{
-    Vec opposing_force = {
-        .x = 0.0f,
-        .y = 0.0f
-    };
-
-    for (Rect_side side = 0; side < RECT_SIDE_N; ++side) {
-        if (sides[side]) {
-            vec_add(
-                &opposing_force,
-                opposing_rect_side_forces[side]);
-        }
-    }
-
-    return opposing_force;
-}
-
 RigidBodies *create_rigid_bodies(size_t capacity)
 {
     Lt *lt = create_lt();
-    if (lt == NULL) {
-        return NULL;
-    }
 
     RigidBodies *rigid_bodies = PUSH_LT(lt, nth_calloc(1, sizeof(RigidBodies)), free);
     if (rigid_bodies == NULL) {
@@ -65,38 +41,41 @@ RigidBodies *create_rigid_bodies(size_t capacity)
     rigid_bodies->capacity = capacity;
     rigid_bodies->count = 0;
 
-    rigid_bodies->positions = PUSH_LT(lt, nth_calloc(capacity, sizeof(Vec)), free);
-    if (rigid_bodies->positions == NULL) {
+    rigid_bodies->bodies = PUSH_LT(lt, nth_calloc(capacity, sizeof(Rect)), free);
+    if (rigid_bodies->bodies == NULL) {
         RETURN_LT(lt, NULL);
     }
 
-    rigid_bodies->velocities = PUSH_LT(lt, nth_calloc(capacity, sizeof(Vec)), free);
+    rigid_bodies->velocities = PUSH_LT(lt, nth_calloc(capacity, sizeof(Vec2f)), free);
     if (rigid_bodies->velocities == NULL) {
         RETURN_LT(lt, NULL);
     }
 
-    rigid_bodies->movements = PUSH_LT(lt, nth_calloc(capacity, sizeof(Vec)), free);
+    rigid_bodies->movements = PUSH_LT(lt, nth_calloc(capacity, sizeof(Vec2f)), free);
     if (rigid_bodies->movements == NULL) {
         RETURN_LT(lt, NULL);
     }
 
-    rigid_bodies->sizes = PUSH_LT(lt, nth_calloc(capacity, sizeof(Vec)), free);
-    if (rigid_bodies->sizes == NULL) {
+    rigid_bodies->grounded = PUSH_LT(lt, nth_calloc(capacity, sizeof(bool)), free);
+    if (rigid_bodies->grounded == NULL) {
         RETURN_LT(lt, NULL);
     }
 
-    rigid_bodies->colors = PUSH_LT(lt, nth_calloc(capacity, sizeof(Color)), free);
-    if (rigid_bodies->colors == NULL) {
+    rigid_bodies->forces = PUSH_LT(lt, nth_calloc(capacity, sizeof(Vec2f)), free);
+    if (rigid_bodies->forces == NULL) {
         RETURN_LT(lt, NULL);
     }
 
-    rigid_bodies->grounded = PUSH_LT(lt, nth_calloc(capacity, sizeof(bool)), free);
-    if (rigid_bodies->grounded == NULL) {
+    rigid_bodies->deleted = PUSH_LT(lt, nth_calloc(capacity, sizeof(bool)), free);
+    if (rigid_bodies->deleted == NULL) {
         RETURN_LT(lt, NULL);
     }
 
-    rigid_bodies->forces = PUSH_LT(lt, nth_calloc(capacity, sizeof(Vec)), free);
-    if (rigid_bodies->forces == NULL) {
+    rigid_bodies->disabled = PUSH_LT(
+        lt,
+        nth_calloc(capacity, sizeof(bool)),
+        free);
+    if (rigid_bodies->disabled == NULL) {
         RETURN_LT(lt, NULL);
     }
 
@@ -109,178 +88,228 @@ void destroy_rigid_bodies(RigidBodies *rigid_bodies)
     RETURN_LT0(rigid_bodies->lt);
 }
 
-static int rigid_bodies_collide_with_itself(RigidBodies *rigid_bodies)
+int rigid_bodies_collide(RigidBodies *rigid_bodies,
+                         const Platforms *platforms)
 {
-    trace_assert(rigid_bodies);
+    memset(rigid_bodies->grounded, 0, sizeof(bool) * rigid_bodies->count);
 
-    for (size_t i1 = 0; i1 < rigid_bodies->count - 1; ++i1) {
-        for (size_t i2 = i1 + 1; i2 < rigid_bodies->count; ++i2) {
-            Rect r1 = rect_from_vecs(rigid_bodies->positions[i1], rigid_bodies->sizes[i1]);
-            Rect r2 = rect_from_vecs(rigid_bodies->positions[i2], rigid_bodies->sizes[i2]);
+    if (rigid_bodies->count == 0) {
+        return 0;
+    }
 
-            if (!rects_overlap(r1, r2)) {
-                continue;
-            }
+    int sides[RECT_SIDE_N] = { 0, 0, 0, 0 };
 
-            rect_impulse(&r1, &r2);
 
-            rigid_bodies->positions[i1] = vec(r1.x, r1.y);
-            rigid_bodies->sizes[i1] = vec(r1.w, r1.h);
+    int t = 100;
+    int the_variable_that_gets_set_when_a_collision_happens_xd = 1;
+    while (t-- > 0 && the_variable_that_gets_set_when_a_collision_happens_xd) {
+        the_variable_that_gets_set_when_a_collision_happens_xd = 0;
 
-            rigid_bodies->positions[i2] = vec(r2.x, r2.y);
-            rigid_bodies->sizes[i2] = vec(r2.w, r2.h);
-        }
-    }
+        for (size_t i1 = 0; i1 < rigid_bodies->count; ++i1) {
+            if (rigid_bodies->deleted[i1] || rigid_bodies->disabled[i1]) {
+                continue;
+            }
 
-    return 0;
-}
+            // Platforms
+            memset(sides, 0, sizeof(int) * RECT_SIDE_N);
 
-static int rigid_bodies_collide_with_platforms(
-    RigidBodies *rigid_bodies,
-    const Platforms *platforms)
-{
-    trace_assert(rigid_bodies);
-    trace_assert(platforms);
+            platforms_touches_rect_sides(platforms, rigid_bodies->bodies[i1], sides);
 
-    int sides[RECT_SIDE_N] = { 0, 0, 0, 0 };
+            for (int i = 0; i < RECT_SIDE_N; ++i) {
+                if (sides[i]) {
+                    the_variable_that_gets_set_when_a_collision_happens_xd = 1;
+                }
+            }
 
-    for (size_t i = 0; i < rigid_bodies->count; ++i) {
-        memset(sides, 0, sizeof(int) * RECT_SIDE_N);
+            if (sides[RECT_SIDE_BOTTOM]) {
+                rigid_bodies->grounded[i1] = true;
+            }
+
+            Vec2f v = platforms_snap_rect(platforms, &rigid_bodies->bodies[i1]);
+            rigid_bodies->velocities[i1] = vec_entry_mult(rigid_bodies->velocities[i1], v);
+            rigid_bodies->movements[i1] = vec_entry_mult(rigid_bodies->movements[i1], v);
+            rigid_bodies_damper(rigid_bodies, i1, vec_entry_mult(v, vec(-16.0f, 0.0f)));
 
-        Rect hitbox = rigid_bodies_hitbox(rigid_bodies, i);
+            if (i1 >= rigid_bodies->count - 1) {
+                continue;
+            }
 
-        platforms_touches_rect_sides(platforms, hitbox, sides);
+            // Self-collision
+            for (size_t i2 = i1 + 1; i2 < rigid_bodies->count; ++i2) {
+                if (rigid_bodies->deleted[i2] || rigid_bodies->disabled[i1]) {
+                    continue;
+                }
 
-        if (sides[RECT_SIDE_BOTTOM]) {
-            rigid_bodies->grounded[i] = true;
-        }
+                if (!rects_overlap(rigid_bodies->bodies[i1], rigid_bodies->bodies[i2])) {
+                    continue;
+                }
 
-        Vec opforce_direction = opposing_force_by_sides(sides);
+                the_variable_that_gets_set_when_a_collision_happens_xd = 1;
 
-        if (fabs(opforce_direction.x) > 1e-6 && (opforce_direction.x < 0.0f) != ((rigid_bodies->velocities[i].x + rigid_bodies->movements[i].x) < 0.0f)) {
-            rigid_bodies->velocities[i].x = 0.0f;
-            rigid_bodies->movements[i].x = 0.0f;
-        }
+                Vec2f orient = rect_impulse(&rigid_bodies->bodies[i1], &rigid_bodies->bodies[i2]);
 
-        if (fabs(opforce_direction.y) > 1e-6 && (opforce_direction.y < 0.0f) != ((rigid_bodies->velocities[i].y + rigid_bodies->movements[i].y) < 0.0f)) {
-            rigid_bodies->velocities[i].y = 0.0f;
-            rigid_bodies->movements[i].y = 0.0f;
+                if (orient.x > orient.y) {
+                    if (rigid_bodies->bodies[i1].y < rigid_bodies->bodies[i2].y) {
+                        rigid_bodies->grounded[i1] = true;
+                    } else {
+                        rigid_bodies->grounded[i2] = true;
+                    }
+                }
 
-            if (vec_length(rigid_bodies->velocities[i]) > 1e-6) {
-                rigid_bodies_apply_force(
-                    rigid_bodies, i,
-                    vec_scala_mult(
-                        vec_neg(rigid_bodies->velocities[i]),
-                        16.0f));
+                rigid_bodies->velocities[i1] = vec(rigid_bodies->velocities[i1].x * orient.x, rigid_bodies->velocities[i1].y * orient.y);
+                rigid_bodies->velocities[i2] = vec(rigid_bodies->velocities[i2].x * orient.x, rigid_bodies->velocities[i2].y * orient.y);
+                rigid_bodies->movements[i1] = vec(rigid_bodies->movements[i1].x * orient.x, rigid_bodies->movements[i1].y * orient.y);
+                rigid_bodies->movements[i2] = vec(rigid_bodies->movements[i2].x * orient.x, rigid_bodies->movements[i2].y * orient.y);
             }
         }
-
-        hitbox = platforms_snap_rect(platforms, hitbox);
-        rigid_bodies->positions[i].x = hitbox.x;
-        rigid_bodies->positions[i].y = hitbox.y;
-    }
-
-    return 0;
-}
-
-int rigid_bodies_collide(RigidBodies *rigid_bodies,
-                         const Platforms *platforms)
-{
-    if (rigid_bodies_collide_with_itself(rigid_bodies) < 0) {
-        return -1;
-    }
-
-    if (rigid_bodies_collide_with_platforms(rigid_bodies, platforms) < 0) {
-        return -1;
     }
 
     return 0;
 }
 
 int rigid_bodies_update(RigidBodies *rigid_bodies,
+                        RigidBodyId id,
                         float delta_time)
 {
     trace_assert(rigid_bodies);
 
-    memset(rigid_bodies->grounded, 0,
-           sizeof(bool) * rigid_bodies->count);
-
-    for (size_t i = 0; i < rigid_bodies->count; ++i) {
-        rigid_bodies->velocities[i] = vec_sum(
-            rigid_bodies->velocities[i],
-            vec_scala_mult(
-                rigid_bodies->forces[i],
-                delta_time));
+    if (rigid_bodies->deleted[id] || rigid_bodies->disabled[id]) {
+        return 0;
     }
 
-    for (size_t i = 0; i < rigid_bodies->count; ++i) {
-        rigid_bodies->positions[i] = vec_sum(
-            rigid_bodies->positions[i],
+    rigid_bodies->velocities[id] = vec_sum(
+            rigid_bodies->velocities[id],
             vec_scala_mult(
-                vec_sum(
-                    rigid_bodies->velocities[i],
-                    rigid_bodies->movements[i]),
+                rigid_bodies->forces[id],
                 delta_time));
-    }
 
-    memset(rigid_bodies->forces, 0,
-           sizeof(Vec) * rigid_bodies->count);
+    Vec2f position = vec(rigid_bodies->bodies[id].x,
+                       rigid_bodies->bodies[id].y);
+
+    position = vec_sum(
+        position,
+        vec_scala_mult(
+            vec_sum(
+                rigid_bodies->velocities[id],
+                rigid_bodies->movements[id]),
+            delta_time));
+
+    rigid_bodies->bodies[id].x = position.x;
+    rigid_bodies->bodies[id].y = position.y;
+
+    rigid_bodies->forces[id] = vec(0.0f, 0.0f);
 
     return 0;
 }
 
 int rigid_bodies_render(RigidBodies *rigid_bodies,
-                        Camera *camera)
+                        RigidBodyId id,
+                        Color color,
+                        const Camera *camera)
 {
     trace_assert(rigid_bodies);
     trace_assert(camera);
 
-    for (size_t i = 0; i < rigid_bodies->count; ++i) {
-        if (camera_fill_rect(
-                camera,
-                rect_from_vecs(
-                    rigid_bodies->positions[i],
-                    rigid_bodies->sizes[i]),
-                rigid_bodies->colors[i]) < 0) {
-            return -1;
-        }
+    if (rigid_bodies->deleted[id] || rigid_bodies->disabled[id]) {
+        return 0;
+    }
+
+    char text_buffer[256];
+
+    if (camera_fill_rect(
+            camera,
+            rigid_bodies->bodies[id],
+            color) < 0) {
+        return -1;
+    }
+
+    snprintf(text_buffer, 256, "id: %zd", id);
+
+    if (camera_render_debug_text(
+            camera,
+            text_buffer,
+            vec(rigid_bodies->bodies[id].x,
+                rigid_bodies->bodies[id].y)) < 0) {
+        return -1;
+    }
+
+    snprintf(text_buffer, 256, "p:(%.2f, %.2f)",
+             rigid_bodies->bodies[id].x,
+             rigid_bodies->bodies[id].y);
+    if (camera_render_debug_text(
+            camera,
+            text_buffer,
+            vec(rigid_bodies->bodies[id].x,
+                rigid_bodies->bodies[id].y + FONT_CHAR_HEIGHT * 2.0f))) {
+        return -1;
+    }
+
+    snprintf(text_buffer, 256, "v:(%.2f, %.2f)",
+             rigid_bodies->velocities[id].x,
+             rigid_bodies->velocities[id].y);
+    if (camera_render_debug_text(
+            camera,
+            text_buffer,
+            vec(rigid_bodies->bodies[id].x,
+                rigid_bodies->bodies[id].y + FONT_CHAR_HEIGHT * 4.0f))) {
+        return -1;
+    }
+
+    snprintf(text_buffer, 256, "m:(%.2f, %.2f)",
+             rigid_bodies->movements[id].x,
+             rigid_bodies->movements[id].y);
+    if (camera_render_debug_text(
+            camera,
+            text_buffer,
+            vec(rigid_bodies->bodies[id].x,
+                rigid_bodies->bodies[id].y + FONT_CHAR_HEIGHT * 6.0f))) {
+        return -1;
     }
 
     return 0;
 }
 
 RigidBodyId rigid_bodies_add(RigidBodies *rigid_bodies,
-                             Rect rect,
-                             Color color)
+                             Rect rect)
 {
     trace_assert(rigid_bodies);
     trace_assert(rigid_bodies->count < rigid_bodies->capacity);
 
     RigidBodyId id = rigid_bodies->count++;
-    rigid_bodies->positions[id] = vec(rect.x, rect.y);
-    rigid_bodies->sizes[id] = vec(rect.w, rect.h);
-    rigid_bodies->colors[id] = color;
+    rigid_bodies->bodies[id] = rect;
 
     return id;
 }
 
+void rigid_bodies_remove(RigidBodies *rigid_bodies,
+                         RigidBodyId id)
+{
+    trace_assert(rigid_bodies);
+    trace_assert(id < rigid_bodies->capacity);
+
+    rigid_bodies->deleted[id] = true;
+}
+
 Rect rigid_bodies_hitbox(const RigidBodies *rigid_bodies,
                          RigidBodyId id)
 {
     trace_assert(rigid_bodies);
     trace_assert(id < rigid_bodies->count);
 
-    return rect_from_vecs(rigid_bodies->positions[id],
-                          rigid_bodies->sizes[id]);
+    return rigid_bodies->bodies[id];
 }
 
 void rigid_bodies_move(RigidBodies *rigid_bodies,
                        RigidBodyId id,
-                       Vec movement)
+                       Vec2f movement)
 {
     trace_assert(rigid_bodies);
     trace_assert(id < rigid_bodies->count);
 
+    if (rigid_bodies->deleted[id] || rigid_bodies->disabled[id]) {
+        return;
+    }
+
     rigid_bodies->movements[id] = movement;
 }
 
@@ -294,7 +323,7 @@ int rigid_bodies_touches_ground(const RigidBodies *rigid_bodies,
 }
 
 void rigid_bodies_apply_omniforce(RigidBodies *rigid_bodies,
-                                  Vec force)
+                                  Vec2f force)
 {
     for (size_t i = 0; i < rigid_bodies->count; ++i) {
         rigid_bodies_apply_force(rigid_bodies, i, force);
@@ -303,11 +332,15 @@ void rigid_bodies_apply_omniforce(RigidBodies *rigid_bodies,
 
 void rigid_bodies_apply_force(RigidBodies * rigid_bodies,
                               RigidBodyId id,
-                              Vec force)
+                              Vec2f force)
 {
     trace_assert(rigid_bodies);
     trace_assert(id < rigid_bodies->count);
 
+    if (rigid_bodies->deleted[id] || rigid_bodies->disabled[id]) {
+        return;
+    }
+
     rigid_bodies->forces[id] = vec_sum(rigid_bodies->forces[id], force);
 }
 
@@ -318,6 +351,10 @@ void rigid_bodies_transform_velocity(RigidBodies *rigid_bodies,
     trace_assert(rigid_bodies);
     trace_assert(id < rigid_bodies->count);
 
+    if (rigid_bodies->deleted[id] || rigid_bodies->disabled[id]) {
+        return;
+    }
+
     rigid_bodies->velocities[id] = point_mat3x3_product(
         rigid_bodies->velocities[id],
         trans_mat);
@@ -325,24 +362,43 @@ void rigid_bodies_transform_velocity(RigidBodies *rigid_bodies,
 
 void rigid_bodies_teleport_to(RigidBodies *rigid_bodies,
                               RigidBodyId id,
-                              Vec position)
+                              Vec2f position)
 {
     trace_assert(rigid_bodies);
     trace_assert(id < rigid_bodies->count);
 
-    rigid_bodies->positions[id] = position;
+    if (rigid_bodies->deleted[id] || rigid_bodies->disabled[id]) {
+        return;
+    }
+
+    rigid_bodies->bodies[id].x = position.x;
+    rigid_bodies->bodies[id].y = position.y;
 }
 
 void rigid_bodies_damper(RigidBodies *rigid_bodies,
                          RigidBodyId id,
-                         Vec v)
+                         Vec2f v)
 {
     trace_assert(rigid_bodies);
     trace_assert(id < rigid_bodies->count);
 
+    if (rigid_bodies->deleted[id] || rigid_bodies->disabled[id]) {
+        return;
+    }
+
     rigid_bodies_apply_force(
         rigid_bodies, id,
         vec(
             rigid_bodies->velocities[id].x * v.x,
             rigid_bodies->velocities[id].y * v.y));
 }
+
+void rigid_bodies_disable(RigidBodies *rigid_bodies,
+                          RigidBodyId id,
+                          bool disabled)
+{
+    trace_assert(rigid_bodies);
+    trace_assert(id < rigid_bodies->count);
+
+    rigid_bodies->disabled[id] = disabled;
+}