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