Throttled loop, x and y properly fucked up

main.c

@@ -1,11 +1,17 @@
+#define _POSIX_C_SOURCE 200809L
+
+#include <bits/time.h>
 #include <math.h>
-#include <time.h>
 #include <pthread.h>
 #include <raylib.h>
 #include <stdio.h>
 #include <stdlib.h>
+#include <threads.h>
+#include <time.h>
+#include <unistd.h>
 
 static const Vector2 NULL_VEC2 = {0, 0};
+static const long TIMESTEP = 1000000;
 
 typedef struct {
   float mass;
@@ -18,7 +24,9 @@ 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; }
+static inline int center_y_text(int height, int size) {
+  return (height - size) / 2;
+}
 
 Vector2 transform_to_center(Vector2 v) {
   int width = GetRenderWidth();
@@ -40,6 +48,9 @@ float vec2_len(Vector2 v) { return sqrtf(v.x * v.x + v.y * v.y); }
 
 Vector2 vec2_normalize(const Vector2 v) {
   float len = vec2_len(v);
+  if (len == 0.0) {
+    return NULL_VEC2;
+  }
   Vector2 result = {v.x / len, v.y / len};
   return result;
 }
@@ -124,31 +135,53 @@ typedef struct universe {
   mass_body_t *bodies;
 } universe_t;
 
+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;
+  universe_t *uni = (universe_t *)args;
   mass_body_t body_1;
   mass_body_t body_2;
   Vector2 collision;
-  while(true){
-      for(unsigned int i=0;i<uni->num_bodies;i++){
-          body_1=uni->bodies[i];
-          printf("body %d pos is : %f %f \n speed is : %f %f\n",i,uni->bodies[i].position.x,uni->bodies[i].position.y,uni->bodies[i].speed.x,uni->bodies[i].speed.x);
-          body_1.position = vec2_sum(&body_1.position, &body_1.speed);
-          printf("body %d pos is : %f %f \n speed is : %f %f\n",i,uni->bodies[i].position.x,uni->bodies[i].position.y,uni->bodies[i].speed.x,uni->bodies[i].speed.x);
-          for(unsigned int y=0;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,1.0);
-              }
-              uni->bodies[y]=body_2;
+  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];
+      body_1.position = vec2_sum(&body_1.position, &body_1.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, 1.0);
           }
-        collision = edge_collision(&body_1);
-        if (!vec2_is_null(collision)) {
-            collide_rigid(&body_1, &collision, 1.0);
+          uni->bodies[y] = body_2;
         }
-        uni->bodies[i]=body_1;
       }
+      collision = edge_collision(&body_1);
+      if (!vec2_is_null(collision)) {
+        collide_rigid(&body_1, &collision, 1.0);
+      }
+      uni->bodies[i] = body_1;
+    }
+    ot = ct;
+    clock_gettime(CLOCK_MONOTONIC, &ct);
+    long long elapsed = timespec_diff_ns(ct, ot);
+    long long remaining = TIMESTEP - 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);
   }
 }
 
@@ -156,14 +189,14 @@ int main(void) {
   pthread_t physics_thread;
   const char *welcome_text = "Welcome to Fucked Physics!";
   universe_t universe = {0, NULL};
-  pthread_create(&physics_thread,NULL,physics,(void*)&universe);
+  pthread_create(&physics_thread, NULL, physics, (void *)&universe);
   universe.bodies = malloc(1024 * sizeof(mass_body_t));
 
-  mass_body_t body_1 = {100, 50, {-200, -100}, {0.5, 0.1}};
-  mass_body_t body_2 = {100, 50, {200, 0}, {-0.5, 0}};
-  universe.bodies[0]=body_1;
-  universe.bodies[1]=body_2;
-  universe.num_bodies=2;
+  mass_body_t body_1 = {100, 50, {-200, -100}, {0.1, 0.00000000001}};
+  mass_body_t body_2 = {100, 50, {200, 0}, {0.1, 0.000000000001}};
+  universe.bodies[0] = body_1;
+  universe.bodies[1] = body_2;
+  universe.num_bodies = 2;
   SetConfigFlags(FLAG_WINDOW_RESIZABLE);
   InitWindow(800, 450, "Fucked Physics");
   SetTargetFPS(120);
@@ -172,15 +205,15 @@ int main(void) {
 
     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});
+    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);
+             center_y_text(GetRenderHeight(), 20), 20, BLACK);
     EndDrawing();
   }