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