fphysics/main.c

#define _POSIX_C_SOURCE 200809L

#include "objects.h"
#include "vec_util.h"
#include <bits/time.h>
#include <math.h>
#include <pthread.h>
#include <raylib.h>
#include <stdio.h>
#include <stdlib.h>
#include <threads.h>
#include <time.h>
#include <unistd.h>

static const long TIMESTEP = 1000000;
static const long simx = 2;

int center_x_text(const char *text, int width, int size) {
  return (width - MeasureText(text, size)) / 2;
}

static inline int center_y_text(int height, int size) {
  return (height - size) / 2;
}

Vector2 body_collision(mass_body_t *a, mass_body_t *b) {
  Vector2 diff = vec2_diff(&a->position, &b->position);
  float len = vec2_len(diff);
  if (len > a->radius + b->radius) {
    return NULL_VEC2;
  }
  if (len == 0) {
    return (Vector2){1, 0};
  }
  Vector2 result = vec2_normalize(diff);
  return result;
}

Vector2 edge_collision(mass_body_t *b) {
  float width = GetRenderWidth();
  float height = GetRenderHeight();
  Vector2 result = {0, 0};
  if (b->position.x + b->radius > width / 2) {
    result.x = -1;
  } else if (b->position.x - b->radius < -width / 2) {
    result.x = 1;
  }

  if (b->position.y + b->radius > height / 2) {
    result.y = -1;
  } else if (b->position.y - b->radius < -height / 2) {
    result.y = 1;
  }

  if (result.x == 0 && result.y == 0) {
    return NULL_VEC2;
  }

  return vec2_normalize(result);
}

void collide(mass_body_t *a, mass_body_t *b, const Vector2 *normal,
             float resitution) {
  float inverse_a = 1.0f / a->mass;
  float inverse_b = 1.0f / b->mass;
  float inverse_total = inverse_a + inverse_b;
  float overlap =
      a->radius + b->radius - vec2_len(vec2_diff(&a->position, &b->position));
  Vector2 correction = vec2_scale(normal, overlap / inverse_total);
  Vector2 a_move = vec2_scale(&correction, inverse_a);
  Vector2 b_move = vec2_scale(&correction, inverse_b);
  a->position = vec2_sum(&a->position, &a_move);
  b->position = vec2_diff(&b->position, &b_move);

  Vector2 relative = vec2_diff(&b->speed, &a->speed);
  float v_normal = vec2_dot(&relative, normal);
  if (v_normal <= 0) {
    return;
  }

  float factor = -(1 + resitution);
  float j = factor * v_normal;

  j /= inverse_total;

  Vector2 final = vec2_scale(normal, j);

  Vector2 a_diff = vec2_scale(&final, inverse_a);
  a->speed = vec2_diff(&a->speed, &a_diff);
  Vector2 b_diff = vec2_scale(&final, inverse_b);
  b->speed = vec2_sum(&b->speed, &b_diff);
}

void collide_rigid(mass_body_t *a, const Vector2 *normal, float restitution) {

  float v_normal = vec2_dot(&a->speed, normal);

  Vector2 delta_v = vec2_scale(normal, -(1.0 + restitution) * v_normal);
  a->speed = vec2_sum(&a->speed, &delta_v);
}

bool vec2_is_null(Vector2 v) {
  if (v.x == 0 && v.y == 0) {
    return true;
  }
  return false;
}

long long timespec_diff_ns(struct timespec end, struct timespec start) {
  return (end.tv_sec - start.tv_sec) * 1000000000LL +
         (end.tv_nsec - start.tv_nsec);
}

void *physics(void *args) {
  universe_t *uni = (universe_t *)args;
  mass_body_t body_1;
  mass_body_t body_2;
  Vector2 collision;
  Vector2 scaled_speed;
  struct timespec ot, ct;
  while (true) {
    clock_gettime(CLOCK_MONOTONIC, &ct);
    for (unsigned int i = 0; i < uni->num_bodies; i++) {
      body_1 = uni->bodies[i];
      scaled_speed = vec2_scale(&body_1.speed, 1.0 / simx);
      body_1.position = vec2_sum(&body_1.position, &scaled_speed);
      if (i < uni->num_bodies - 1) {
        for (unsigned int y = i + 1; y < uni->num_bodies; y++) {
          body_2 = uni->bodies[y];
          collision = body_collision(&body_1, &body_2);
          if (!vec2_is_null(collision)) {
            collide(&body_1, &body_2, &collision, 0.99);
          }
          uni->bodies[y] = body_2;
        }
      }
      collision = edge_collision(&body_1);
      if (!vec2_is_null(collision)) {
        collide_rigid(&body_1, &collision, 1.0005);
      }
      uni->bodies[i] = body_1;
    }
    ot = ct;
    clock_gettime(CLOCK_MONOTONIC, &ct);
    long long elapsed = timespec_diff_ns(ct, ot);
    long long remaining = TIMESTEP / simx - elapsed;

    if (remaining > 0) {
      long long sleep_time = remaining * 9 / 10;

      struct timespec sleep_time_spec = {0, sleep_time};
      thrd_sleep(&sleep_time_spec, NULL);
    }

    do {
      clock_gettime(CLOCK_MONOTONIC, &ct);
    } while (timespec_diff_ns(ct, ot) < TIMESTEP / simx);
  }
}

int main(void) {
  pthread_t physics_thread;
  const char *welcome_text = "Welcome to Fucked Physics!";
  universe_t universe = {0, NULL};
  universe.bodies = malloc(1024 * sizeof(mass_body_t));
  for (int i = 0; i < 100; i++) {
    float radius = 12.0f + 8.0f / (i + 1);
    universe.bodies[i] = (mass_body_t){
        radius,
        radius,
        {-360.0f + 80.0f * (i % 10), -202.5f + 45.0f * (i / 10)},
        {-0.1f, 0.8f - i * 0.005f},
    };
  }
  universe.num_bodies = 100;
  SetConfigFlags(FLAG_WINDOW_RESIZABLE);
  InitWindow(800, 450, "Fucked Physics");
  SetTargetFPS(120);
  pthread_create(&physics_thread, NULL, physics, (void *)&universe);

  while (!WindowShouldClose()) {

    BeginDrawing();
    ClearBackground(RAYWHITE);
    for (unsigned int i = 0; i < universe.num_bodies; i++) {
      DrawCircleV(transform_to_center(universe.bodies[i].position),
                  universe.bodies[i].radius,
                  (Color){((256 / universe.num_bodies) * i) % 256,
                          255 * (i % 2), 255 * (i % 2) * (i % 3), 255});
    }

    DrawText(welcome_text, center_x_text(welcome_text, GetRenderWidth(), 20),
             center_y_text(GetRenderHeight(), 20), 20, BLACK);
    EndDrawing();
  }

  CloseWindow();
  return EXIT_SUCCESS;
}