Fixed sticking

main.c

@@ -1,6 +1,7 @@
 #define _POSIX_C_SOURCE 200809L
 
 #include "objects.h"
+#include "vec_util.h"
 #include <bits/time.h>
 #include <math.h>
 #include <pthread.h>
@@ -12,6 +13,7 @@
 #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;
@@ -27,6 +29,9 @@ Vector2 body_collision(mass_body_t *a, mass_body_t *b) {
   if (len > a->radius + b->radius) {
     return NULL_VEC2;
   }
+  if (len == 0) {
+    return (Vector2){1, 0};
+  }
   Vector2 result = vec2_normalize(diff);
   return result;
 }
@@ -56,19 +61,33 @@ Vector2 edge_collision(mass_body_t *b) {
 
 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 /= (1.0 / a->mass) + (1.0 / b->mass);
+  j /= inverse_total;
 
   Vector2 final = vec2_scale(normal, j);
 
-  Vector2 a_diff = vec2_scale(&final, 1.0 / a->mass);
+  Vector2 a_diff = vec2_scale(&final, inverse_a);
   a->speed = vec2_diff(&a->speed, &a_diff);
-  Vector2 b_diff = vec2_scale(&final, 1.0 / b->mass);
+  Vector2 b_diff = vec2_scale(&final, inverse_b);
   b->speed = vec2_sum(&b->speed, &b_diff);
 }
 
@@ -97,12 +116,14 @@ void *physics(void *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];
-      body_1.position = vec2_sum(&body_1.position, &body_1.speed);
+      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];
@@ -122,7 +143,7 @@ void *physics(void *args) {
     ot = ct;
     clock_gettime(CLOCK_MONOTONIC, &ct);
     long long elapsed = timespec_diff_ns(ct, ot);
-    long long remaining = TIMESTEP - elapsed;
+    long long remaining = TIMESTEP / simx - elapsed;
 
     if (remaining > 0) {
       long long sleep_time = remaining * 9 / 10;
@@ -133,7 +154,7 @@ void *physics(void *args) {
 
     do {
       clock_gettime(CLOCK_MONOTONIC, &ct);
-    } while (timespec_diff_ns(ct, ot) < TIMESTEP);
+    } while (timespec_diff_ns(ct, ot) < TIMESTEP / simx);
   }
 }
 
@@ -142,12 +163,14 @@ int main(void) {
   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++){
-  if(i>500){
-    universe.bodies[i] = (mass_body_t){100/(25+50/(i+1)), 50+50/(i+1), {i*10, i*200%500}, {0.1, 0.5+i*0.01}};
-  }else{
-    universe.bodies[i] = (mass_body_t){100/(25+50/(i+1)), 25+50/(i+1), {-i*10, -i*300%500}, {-0.1, 0.8-i*0.005}};
-  }
+  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);