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
  10 #include "./rigid_bodies.h"
  11
  12 struct RigidBodies
  13 {
  14     Lt *lt;
  15     size_t capacity;
  16     size_t count;
  17
  18     Vec *positions;
  19     Vec *velocities;
  20     Vec *movements;
  21     Vec *sizes;
  22     Color *colors;
  23     bool *grounded;
  24     Vec *forces;
  25 };
  26
  27 static const Vec opposing_rect_side_forces[RECT_SIDE_N] = {
  28     { .x = 1.0f,  .y =  0.0f  },  /* RECT_SIDE_LEFT = 0, */
  29     { .x = -1.0f, .y =  0.0f  },  /* RECT_SIDE_RIGHT, */
  30     { .x = 0.0f,  .y =  1.0f, },  /* RECT_SIDE_TOP, */
  31     { .x = 0.0f,  .y = -1.0f, }   /* RECT_SIDE_BOTTOM, */
  32 };
  33
  34 static Vec opposing_force_by_sides(int sides[RECT_SIDE_N])
  35 {
  36     Vec opposing_force = {
  37         .x = 0.0f,
  38         .y = 0.0f
  39     };
  40
  41     for (Rect_side side = 0; side < RECT_SIDE_N; ++side) {
  42         if (sides[side]) {
  43             vec_add(
  44                 &opposing_force,
  45                 opposing_rect_side_forces[side]);
  46         }
  47     }
  48
  49     return opposing_force;
  50 }
  51
  52 RigidBodies *create_rigid_bodies(size_t capacity)
  53 {
  54     Lt *lt = create_lt();
  55     if (lt == NULL) {
  56         return NULL;
  57     }
  58
  59     RigidBodies *rigid_bodies = PUSH_LT(lt, nth_calloc(1, sizeof(RigidBodies)), free);
  60     if (rigid_bodies == NULL) {
  61         RETURN_LT(lt, NULL);
  62     }
  63     rigid_bodies->lt = lt;
  64
  65     rigid_bodies->capacity = capacity;
  66     rigid_bodies->count = 0;
  67
  68     rigid_bodies->positions = PUSH_LT(lt, nth_calloc(capacity, sizeof(Vec)), free);
  69     if (rigid_bodies->positions == NULL) {
  70         RETURN_LT(lt, NULL);
  71     }
  72
  73     rigid_bodies->velocities = PUSH_LT(lt, nth_calloc(capacity, sizeof(Vec)), free);
  74     if (rigid_bodies->velocities == NULL) {
  75         RETURN_LT(lt, NULL);
  76     }
  77
  78     rigid_bodies->movements = PUSH_LT(lt, nth_calloc(capacity, sizeof(Vec)), free);
  79     if (rigid_bodies->movements == NULL) {
  80         RETURN_LT(lt, NULL);
  81     }
  82
  83     rigid_bodies->sizes = PUSH_LT(lt, nth_calloc(capacity, sizeof(Vec)), free);
  84     if (rigid_bodies->sizes == NULL) {
  85         RETURN_LT(lt, NULL);
  86     }
  87
  88     rigid_bodies->colors = PUSH_LT(lt, nth_calloc(capacity, sizeof(Color)), free);
  89     if (rigid_bodies->colors == NULL) {
  90         RETURN_LT(lt, NULL);
  91     }
  92
  93     rigid_bodies->grounded = PUSH_LT(lt, nth_calloc(capacity, sizeof(bool)), free);
  94     if (rigid_bodies->grounded == NULL) {
  95         RETURN_LT(lt, NULL);
  96     }
  97
  98     rigid_bodies->forces = PUSH_LT(lt, nth_calloc(capacity, sizeof(Vec)), free);
  99     if (rigid_bodies->forces == 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     for (size_t i1 = 0; i1 < rigid_bodies->count - 1; ++i1) {
 121         for (size_t i2 = i1 + 1; i2 < rigid_bodies->count; ++i2) {
 122             // TODO(#653): Rigid Bodies perform too many conversions between rect and two vecs representation
 123             //   Maybe it's just better to represent the bodies as rects all the time?
 124             // TODO(#654): Rigid Bodies don't exchange forces with each other
 125
 126             Rect r1 = rect_from_vecs(rigid_bodies->positions[i1], rigid_bodies->sizes[i1]);
 127             Rect r2 = rect_from_vecs(rigid_bodies->positions[i2], rigid_bodies->sizes[i2]);
 128
 129             if (!rects_overlap(r1, r2)) {
 130                 continue;
 131             }
 132
 133             rect_impulse(&r1, &r2);
 134
 135             rigid_bodies->positions[i1] = vec(r1.x, r1.y);
 136             rigid_bodies->sizes[i1] = vec(r1.w, r1.h);
 137
 138             rigid_bodies->positions[i2] = vec(r2.x, r2.y);
 139             rigid_bodies->sizes[i2] = vec(r2.w, r2.h);
 140         }
 141     }
 142
 143     return 0;
 144 }
 145
 146 static int rigid_bodies_collide_with_platforms(
 147     RigidBodies *rigid_bodies,
 148     const Platforms *platforms)
 149 {
 150     trace_assert(rigid_bodies);
 151     trace_assert(platforms);
 152
 153     int sides[RECT_SIDE_N] = { 0, 0, 0, 0 };
 154
 155     for (size_t i = 0; i < rigid_bodies->count; ++i) {
 156         memset(sides, 0, sizeof(int) * RECT_SIDE_N);
 157
 158         Rect hitbox = rigid_bodies_hitbox(rigid_bodies, i);
 159
 160         platforms_touches_rect_sides(platforms, hitbox, sides);
 161
 162         if (sides[RECT_SIDE_BOTTOM]) {
 163             rigid_bodies->grounded[i] = true;
 164         }
 165
 166         /* TODO: Opposing force notion in Rigid Bodies seems redundant */
 167         Vec opforce_direction = opposing_force_by_sides(sides);
 168
 169         if (fabs(opforce_direction.x) > 1e-6 && (opforce_direction.x < 0.0f) != ((rigid_bodies->velocities[i].x + rigid_bodies->movements[i].x) < 0.0f)) {
 170             rigid_bodies->velocities[i].x = 0.0f;
 171             rigid_bodies->movements[i].x = 0.0f;
 172         }
 173
 174         if (fabs(opforce_direction.y) > 1e-6 && (opforce_direction.y < 0.0f) != ((rigid_bodies->velocities[i].y + rigid_bodies->movements[i].y) < 0.0f)) {
 175             rigid_bodies->velocities[i].y = 0.0f;
 176             rigid_bodies->movements[i].y = 0.0f;
 177
 178             if (vec_length(rigid_bodies->velocities[i]) > 1e-6) {
 179                 rigid_bodies_apply_force(
 180                     rigid_bodies, i,
 181                     vec_scala_mult(
 182                         vec_neg(rigid_bodies->velocities[i]),
 183                         16.0f));
 184             }
 185         }
 186
 187         hitbox = platforms_snap_rect(platforms, hitbox);
 188         rigid_bodies->positions[i].x = hitbox.x;
 189         rigid_bodies->positions[i].y = hitbox.y;
 190     }
 191
 192     return 0;
 193 }
 194
 195 int rigid_bodies_collide(RigidBodies *rigid_bodies,
 196                          const Platforms *platforms)
 197 {
 198     if (rigid_bodies_collide_with_itself(rigid_bodies) < 0) {
 199         return -1;
 200     }
 201
 202     if (rigid_bodies_collide_with_platforms(rigid_bodies, platforms) < 0) {
 203         return -1;
 204     }
 205
 206     return 0;
 207 }
 208
 209 int rigid_bodies_update(RigidBodies *rigid_bodies,
 210                         float delta_time)
 211 {
 212     trace_assert(rigid_bodies);
 213
 214     memset(rigid_bodies->grounded, 0,
 215            sizeof(bool) * rigid_bodies->count);
 216
 217     for (size_t i = 0; i < rigid_bodies->count; ++i) {
 218         rigid_bodies->velocities[i] = vec_sum(
 219             rigid_bodies->velocities[i],
 220             vec_scala_mult(
 221                 rigid_bodies->forces[i],
 222                 delta_time));
 223     }
 224
 225     for (size_t i = 0; i < rigid_bodies->count; ++i) {
 226         rigid_bodies->positions[i] = vec_sum(
 227             rigid_bodies->positions[i],
 228             vec_scala_mult(
 229                 vec_sum(
 230                     rigid_bodies->velocities[i],
 231                     rigid_bodies->movements[i]),
 232                 delta_time));
 233     }
 234
 235     memset(rigid_bodies->forces, 0,
 236            sizeof(Vec) * rigid_bodies->count);
 237
 238     return 0;
 239 }
 240
 241 int rigid_bodies_render(RigidBodies *rigid_bodies,
 242                         Camera *camera)
 243 {
 244     trace_assert(rigid_bodies);
 245     trace_assert(camera);
 246
 247     /* TODO: Rigid Bodies don't render their ids in the debug mode */
 248
 249     for (size_t i = 0; i < rigid_bodies->count; ++i) {
 250         if (camera_fill_rect(
 251                 camera,
 252                 rect_from_vecs(
 253                     rigid_bodies->positions[i],
 254                     rigid_bodies->sizes[i]),
 255                 rigid_bodies->colors[i]) < 0) {
 256             return -1;
 257         }
 258     }
 259
 260     return 0;
 261 }
 262
 263 RigidBodyId rigid_bodies_add(RigidBodies *rigid_bodies,
 264                              Rect rect,
 265                              Color color)
 266 {
 267     trace_assert(rigid_bodies);
 268     trace_assert(rigid_bodies->count < rigid_bodies->capacity);
 269
 270     RigidBodyId id = rigid_bodies->count++;
 271     rigid_bodies->positions[id] = vec(rect.x, rect.y);
 272     rigid_bodies->sizes[id] = vec(rect.w, rect.h);
 273     rigid_bodies->colors[id] = color;
 274
 275     return id;
 276 }
 277
 278 Rect rigid_bodies_hitbox(const RigidBodies *rigid_bodies,
 279                          RigidBodyId id)
 280 {
 281     trace_assert(rigid_bodies);
 282     trace_assert(id < rigid_bodies->count);
 283
 284     return rect_from_vecs(rigid_bodies->positions[id],
 285                           rigid_bodies->sizes[id]);
 286 }
 287
 288 void rigid_bodies_move(RigidBodies *rigid_bodies,
 289                        RigidBodyId id,
 290                        Vec movement)
 291 {
 292     trace_assert(rigid_bodies);
 293     trace_assert(id < rigid_bodies->count);
 294
 295     rigid_bodies->movements[id] = movement;
 296 }
 297
 298 int rigid_bodies_touches_ground(const RigidBodies *rigid_bodies,
 299                                 RigidBodyId id)
 300 {
 301     trace_assert(rigid_bodies);
 302     trace_assert(id < rigid_bodies->count);
 303
 304     return rigid_bodies->grounded[id];
 305 }
 306
 307 void rigid_bodies_apply_omniforce(RigidBodies *rigid_bodies,
 308                                   Vec force)
 309 {
 310     for (size_t i = 0; i < rigid_bodies->count; ++i) {
 311         rigid_bodies_apply_force(rigid_bodies, i, force);
 312     }
 313 }
 314
 315 void rigid_bodies_apply_force(RigidBodies * rigid_bodies,
 316                               RigidBodyId id,
 317                               Vec force)
 318 {
 319     trace_assert(rigid_bodies);
 320     trace_assert(id < rigid_bodies->count);
 321
 322     rigid_bodies->forces[id] = vec_sum(rigid_bodies->forces[id], force);
 323 }
 324
 325 void rigid_bodies_transform_velocity(RigidBodies *rigid_bodies,
 326                                      RigidBodyId id,
 327                                      mat3x3 trans_mat)
 328 {
 329     trace_assert(rigid_bodies);
 330     trace_assert(id < rigid_bodies->count);
 331
 332     rigid_bodies->velocities[id] = point_mat3x3_product(
 333         rigid_bodies->velocities[id],
 334         trans_mat);
 335 }
 336
 337 void rigid_bodies_teleport_to(RigidBodies *rigid_bodies,
 338                               RigidBodyId id,
 339                               Vec position)
 340 {
 341     trace_assert(rigid_bodies);
 342     trace_assert(id < rigid_bodies->count);
 343
 344     rigid_bodies->positions[id] = position;
 345 }
 346
 347 void rigid_bodies_damper(RigidBodies *rigid_bodies,
 348                          RigidBodyId id,
 349                          Vec v)
 350 {
 351     trace_assert(rigid_bodies);
 352     trace_assert(id < rigid_bodies->count);
 353
 354     rigid_bodies_apply_force(
 355         rigid_bodies, id,
 356         vec(
 357             rigid_bodies->velocities[id].x * v.x,
 358             rigid_bodies->velocities[id].y * v.y));
 359 }