src/game/level/rigid_bodies.c

   1 #include <stdlib.h>
   2 #include <stdbool.h>
   3
   4 #include "game/camera.h"
   5 #include "game/level/platforms.h"
   6 #include "system/lt.h"
   7 #include "system/nth_alloc.h"
   8 #include "system/stacktrace.h"
   9 #include "system/line_stream.h"
  10 #include "system/str.h"
  11 #include "system/log.h"
  12 #include "hashset.h"
  13
  14 #include "./rigid_bodies.h"
  15
  16 struct RigidBodies
  17 {
  18     Lt *lt;
  19     size_t capacity;
  20     size_t count;
  21
  22     Rect *bodies;
  23     Vec *velocities;
  24     Vec *movements;
  25     bool *grounded;
  26     Vec *forces;
  27     bool *deleted;
  28     HashSet *collided;
  29     bool *disabled;
  30 };
  31
  32 RigidBodies *create_rigid_bodies(size_t capacity)
  33 {
  34     Lt *lt = create_lt();
  35
  36     RigidBodies *rigid_bodies = PUSH_LT(lt, nth_calloc(1, sizeof(RigidBodies)), free);
  37     if (rigid_bodies == NULL) {
  38         RETURN_LT(lt, NULL);
  39     }
  40     rigid_bodies->lt = lt;
  41
  42     rigid_bodies->capacity = capacity;
  43     rigid_bodies->count = 0;
  44
  45     rigid_bodies->bodies = PUSH_LT(lt, nth_calloc(capacity, sizeof(Rect)), free);
  46     if (rigid_bodies->bodies == NULL) {
  47         RETURN_LT(lt, NULL);
  48     }
  49
  50     rigid_bodies->velocities = PUSH_LT(lt, nth_calloc(capacity, sizeof(Vec)), free);
  51     if (rigid_bodies->velocities == NULL) {
  52         RETURN_LT(lt, NULL);
  53     }
  54
  55     rigid_bodies->movements = PUSH_LT(lt, nth_calloc(capacity, sizeof(Vec)), free);
  56     if (rigid_bodies->movements == NULL) {
  57         RETURN_LT(lt, NULL);
  58     }
  59
  60     rigid_bodies->grounded = PUSH_LT(lt, nth_calloc(capacity, sizeof(bool)), free);
  61     if (rigid_bodies->grounded == NULL) {
  62         RETURN_LT(lt, NULL);
  63     }
  64
  65     rigid_bodies->forces = PUSH_LT(lt, nth_calloc(capacity, sizeof(Vec)), free);
  66     if (rigid_bodies->forces == NULL) {
  67         RETURN_LT(lt, NULL);
  68     }
  69
  70     rigid_bodies->deleted = PUSH_LT(lt, nth_calloc(capacity, sizeof(bool)), free);
  71     if (rigid_bodies->deleted == NULL) {
  72         RETURN_LT(lt, NULL);
  73     }
  74
  75     rigid_bodies->collided = PUSH_LT(
  76         lt,
  77         create_hashset(sizeof(size_t) * 2, capacity * 2),
  78         destroy_hashset);
  79     if (rigid_bodies->collided == NULL) {
  80         RETURN_LT(lt, NULL);
  81     }
  82
  83     rigid_bodies->disabled = PUSH_LT(
  84         lt,
  85         nth_calloc(capacity, sizeof(bool)),
  86         free);
  87     if (rigid_bodies->disabled == NULL) {
  88         RETURN_LT(lt, NULL);
  89     }
  90
  91     return rigid_bodies;
  92 }
  93
  94 void destroy_rigid_bodies(RigidBodies *rigid_bodies)
  95 {
  96     trace_assert(rigid_bodies);
  97     RETURN_LT0(rigid_bodies->lt);
  98 }
  99
 100 static int rigid_bodies_collide_with_itself(RigidBodies *rigid_bodies)
 101 {
 102     trace_assert(rigid_bodies);
 103
 104     if (rigid_bodies->count == 0) {
 105         return 0;
 106     }
 107
 108     size_t pair[2];
 109     hashset_clear(rigid_bodies->collided);
 110
 111     bool the_variable_that_gets_set_when_a_collision_happens_xd = true;
 112
 113     for (size_t i = 0; i < 1000 && the_variable_that_gets_set_when_a_collision_happens_xd; ++i) {
 114         the_variable_that_gets_set_when_a_collision_happens_xd = false;
 115         for (size_t i1 = 0; i1 < rigid_bodies->count - 1; ++i1) {
 116             if (rigid_bodies->deleted[i1] || rigid_bodies->disabled[i1]) {
 117                 continue;
 118             }
 119
 120             for (size_t i2 = i1 + 1; i2 < rigid_bodies->count; ++i2) {
 121                 if (rigid_bodies->deleted[i2] || rigid_bodies->disabled[i1]) {
 122                     continue;
 123                 }
 124
 125                 if (!rects_overlap(rigid_bodies->bodies[i1], rigid_bodies->bodies[i2])) {
 126                     continue;
 127                 }
 128
 129                 the_variable_that_gets_set_when_a_collision_happens_xd = true;
 130
 131                 pair[0] = i1;
 132                 pair[1] = i2;
 133                 hashset_insert(rigid_bodies->collided, pair);
 134
 135                 Vec orient = rect_impulse(&rigid_bodies->bodies[i1], &rigid_bodies->bodies[i2]);
 136
 137                 if (orient.x > orient.y) {
 138                     if (rigid_bodies->bodies[i1].y < rigid_bodies->bodies[i2].y) {
 139                         rigid_bodies->grounded[i1] = true;
 140                     } else {
 141                         rigid_bodies->grounded[i2] = true;
 142                     }
 143                 }
 144
 145                 rigid_bodies->velocities[i1] = vec(rigid_bodies->velocities[i1].x * orient.x, rigid_bodies->velocities[i1].y * orient.y);
 146                 rigid_bodies->velocities[i2] = vec(rigid_bodies->velocities[i2].x * orient.x, rigid_bodies->velocities[i2].y * orient.y);
 147                 rigid_bodies->movements[i1] = vec(rigid_bodies->movements[i1].x * orient.x, rigid_bodies->movements[i1].y * orient.y);
 148                 rigid_bodies->movements[i2] = vec(rigid_bodies->movements[i2].x * orient.x, rigid_bodies->movements[i2].y * orient.y);
 149
 150             }
 151         }
 152     }
 153
 154     size_t *collided = hashset_values(rigid_bodies->collided);
 155     const size_t n = hashset_count(rigid_bodies->collided);
 156     for (size_t i = 0; i < n; ++i) {
 157         const size_t i1 = *(collided + i * 2);
 158         const size_t i2 = *(collided + i * 2 + 1);
 159
 160         rigid_bodies_apply_force(
 161             rigid_bodies, i1, vec_sum(rigid_bodies->velocities[i2], rigid_bodies->movements[i2]));
 162         rigid_bodies_apply_force(
 163             rigid_bodies, i2, vec_sum(rigid_bodies->velocities[i1], rigid_bodies->movements[i1]));
 164     }
 165
 166     return 0;
 167 }
 168
 169 static int rigid_bodies_collide_with_platforms(
 170     RigidBodies *rigid_bodies,
 171     const Platforms *platforms)
 172 {
 173     trace_assert(rigid_bodies);
 174     trace_assert(platforms);
 175
 176     int sides[RECT_SIDE_N] = { 0, 0, 0, 0 };
 177
 178     for (size_t i = 0; i < rigid_bodies->count; ++i) {
 179         if (rigid_bodies->deleted[i] || rigid_bodies->disabled[i]) {
 180             continue;
 181         }
 182
 183         memset(sides, 0, sizeof(int) * RECT_SIDE_N);
 184
 185         platforms_touches_rect_sides(platforms, rigid_bodies->bodies[i], sides);
 186
 187         if (sides[RECT_SIDE_BOTTOM]) {
 188             rigid_bodies->grounded[i] = true;
 189         }
 190
 191         Vec v = platforms_snap_rect(platforms, &rigid_bodies->bodies[i]);
 192         rigid_bodies->velocities[i] = vec_entry_mult(rigid_bodies->velocities[i], v);
 193         rigid_bodies->movements[i] = vec_entry_mult(rigid_bodies->movements[i], v);
 194         rigid_bodies_damper(rigid_bodies, i, vec_entry_mult(v, vec(-16.0f, 0.0f)));
 195     }
 196
 197     return 0;
 198 }
 199
 200 int rigid_bodies_collide(RigidBodies *rigid_bodies,
 201                          const Platforms *platforms)
 202 {
 203     // TODO(#683): RigidBodies should collide only the bodies that were updated on after a previous collision
 204     memset(rigid_bodies->grounded, 0, sizeof(bool) * rigid_bodies->count);
 205
 206     if (rigid_bodies_collide_with_itself(rigid_bodies) < 0) {
 207         return -1;
 208     }
 209
 210     if (rigid_bodies_collide_with_platforms(rigid_bodies, platforms) < 0) {
 211         return -1;
 212     }
 213
 214     return 0;
 215 }
 216
 217 int rigid_bodies_update(RigidBodies *rigid_bodies,
 218                         RigidBodyId id,
 219                         float delta_time)
 220 {
 221     trace_assert(rigid_bodies);
 222
 223     if (rigid_bodies->deleted[id] || rigid_bodies->disabled[id]) {
 224         return 0;
 225     }
 226
 227     rigid_bodies->velocities[id] = vec_sum(
 228             rigid_bodies->velocities[id],
 229             vec_scala_mult(
 230                 rigid_bodies->forces[id],
 231                 delta_time));
 232
 233     Vec position = vec(rigid_bodies->bodies[id].x,
 234                        rigid_bodies->bodies[id].y);
 235
 236     position = vec_sum(
 237         position,
 238         vec_scala_mult(
 239             vec_sum(
 240                 rigid_bodies->velocities[id],
 241                 rigid_bodies->movements[id]),
 242             delta_time));
 243
 244     rigid_bodies->bodies[id].x = position.x;
 245     rigid_bodies->bodies[id].y = position.y;
 246
 247     rigid_bodies->forces[id] = vec(0.0f, 0.0f);
 248
 249     return 0;
 250 }
 251
 252 int rigid_bodies_render(RigidBodies *rigid_bodies,
 253                         RigidBodyId id,
 254                         Color color,
 255                         Camera *camera)
 256 {
 257     trace_assert(rigid_bodies);
 258     trace_assert(camera);
 259
 260     if (rigid_bodies->deleted[id] || rigid_bodies->disabled[id]) {
 261         return 0;
 262     }
 263
 264     char text_buffer[256];
 265
 266     if (camera_fill_rect(
 267             camera,
 268             rigid_bodies->bodies[id],
 269             color) < 0) {
 270         return -1;
 271     }
 272
 273     snprintf(text_buffer, 256, "id: %ld", id);
 274
 275     if (camera_render_debug_text(
 276             camera,
 277             text_buffer,
 278             vec(rigid_bodies->bodies[id].x,
 279                 rigid_bodies->bodies[id].y)) < 0) {
 280         return -1;
 281     }
 282
 283     snprintf(text_buffer, 256, "p:(%.2f, %.2f)",
 284              rigid_bodies->bodies[id].x,
 285              rigid_bodies->bodies[id].y);
 286     if (camera_render_debug_text(
 287             camera,
 288             text_buffer,
 289             vec(rigid_bodies->bodies[id].x,
 290                 rigid_bodies->bodies[id].y + FONT_CHAR_HEIGHT * 2.0f))) {
 291         return -1;
 292     }
 293
 294     snprintf(text_buffer, 256, "v:(%.2f, %.2f)",
 295              rigid_bodies->velocities[id].x,
 296              rigid_bodies->velocities[id].y);
 297     if (camera_render_debug_text(
 298             camera,
 299             text_buffer,
 300             vec(rigid_bodies->bodies[id].x,
 301                 rigid_bodies->bodies[id].y + FONT_CHAR_HEIGHT * 4.0f))) {
 302         return -1;
 303     }
 304
 305     snprintf(text_buffer, 256, "m:(%.2f, %.2f)",
 306              rigid_bodies->movements[id].x,
 307              rigid_bodies->movements[id].y);
 308     if (camera_render_debug_text(
 309             camera,
 310             text_buffer,
 311             vec(rigid_bodies->bodies[id].x,
 312                 rigid_bodies->bodies[id].y + FONT_CHAR_HEIGHT * 6.0f))) {
 313         return -1;
 314     }
 315
 316     return 0;
 317 }
 318
 319 RigidBodyId rigid_bodies_add(RigidBodies *rigid_bodies,
 320                              Rect rect)
 321 {
 322     trace_assert(rigid_bodies);
 323     trace_assert(rigid_bodies->count < rigid_bodies->capacity);
 324
 325     RigidBodyId id = rigid_bodies->count++;
 326     rigid_bodies->bodies[id] = rect;
 327
 328     return id;
 329 }
 330
 331 void rigid_bodies_remove(RigidBodies *rigid_bodies,
 332                          RigidBodyId id)
 333 {
 334     trace_assert(rigid_bodies);
 335     trace_assert(id < rigid_bodies->capacity);
 336
 337     rigid_bodies->deleted[id] = true;
 338 }
 339
 340 Rect rigid_bodies_hitbox(const RigidBodies *rigid_bodies,
 341                          RigidBodyId id)
 342 {
 343     trace_assert(rigid_bodies);
 344     trace_assert(id < rigid_bodies->count);
 345
 346     return rigid_bodies->bodies[id];
 347 }
 348
 349 void rigid_bodies_move(RigidBodies *rigid_bodies,
 350                        RigidBodyId id,
 351                        Vec movement)
 352 {
 353     trace_assert(rigid_bodies);
 354     trace_assert(id < rigid_bodies->count);
 355
 356     if (rigid_bodies->deleted[id] || rigid_bodies->disabled[id]) {
 357         return;
 358     }
 359
 360     rigid_bodies->movements[id] = movement;
 361 }
 362
 363 int rigid_bodies_touches_ground(const RigidBodies *rigid_bodies,
 364                                 RigidBodyId id)
 365 {
 366     trace_assert(rigid_bodies);
 367     trace_assert(id < rigid_bodies->count);
 368
 369     return rigid_bodies->grounded[id];
 370 }
 371
 372 void rigid_bodies_apply_omniforce(RigidBodies *rigid_bodies,
 373                                   Vec force)
 374 {
 375     for (size_t i = 0; i < rigid_bodies->count; ++i) {
 376         rigid_bodies_apply_force(rigid_bodies, i, force);
 377     }
 378 }
 379
 380 void rigid_bodies_apply_force(RigidBodies * rigid_bodies,
 381                               RigidBodyId id,
 382                               Vec force)
 383 {
 384     trace_assert(rigid_bodies);
 385     trace_assert(id < rigid_bodies->count);
 386
 387     if (rigid_bodies->deleted[id] || rigid_bodies->disabled[id]) {
 388         return;
 389     }
 390
 391     rigid_bodies->forces[id] = vec_sum(rigid_bodies->forces[id], force);
 392 }
 393
 394 void rigid_bodies_transform_velocity(RigidBodies *rigid_bodies,
 395                                      RigidBodyId id,
 396                                      mat3x3 trans_mat)
 397 {
 398     trace_assert(rigid_bodies);
 399     trace_assert(id < rigid_bodies->count);
 400
 401     if (rigid_bodies->deleted[id] || rigid_bodies->disabled[id]) {
 402         return;
 403     }
 404
 405     rigid_bodies->velocities[id] = point_mat3x3_product(
 406         rigid_bodies->velocities[id],
 407         trans_mat);
 408 }
 409
 410 void rigid_bodies_teleport_to(RigidBodies *rigid_bodies,
 411                               RigidBodyId id,
 412                               Vec position)
 413 {
 414     trace_assert(rigid_bodies);
 415     trace_assert(id < rigid_bodies->count);
 416
 417     if (rigid_bodies->deleted[id] || rigid_bodies->disabled[id]) {
 418         return;
 419     }
 420
 421     rigid_bodies->bodies[id].x = position.x;
 422     rigid_bodies->bodies[id].y = position.y;
 423 }
 424
 425 void rigid_bodies_damper(RigidBodies *rigid_bodies,
 426                          RigidBodyId id,
 427                          Vec v)
 428 {
 429     trace_assert(rigid_bodies);
 430     trace_assert(id < rigid_bodies->count);
 431
 432     if (rigid_bodies->deleted[id] || rigid_bodies->disabled[id]) {
 433         return;
 434     }
 435
 436     rigid_bodies_apply_force(
 437         rigid_bodies, id,
 438         vec(
 439             rigid_bodies->velocities[id].x * v.x,
 440             rigid_bodies->velocities[id].y * v.y));
 441 }
 442
 443 void rigid_bodies_disable(RigidBodies *rigid_bodies,
 444                           RigidBodyId id,
 445                           bool disabled)
 446 {
 447     trace_assert(rigid_bodies);
 448     trace_assert(id < rigid_bodies->count);
 449
 450     rigid_bodies->disabled[id] = disabled;
 451 }