feat: COLISIONS

include/particle.h

@@ -1,12 +1,13 @@
 #ifndef PARTICLE_H
 #define PARTICLE_H
 
+#include <math.h>
 #include <pthread.h>
+#include <stdatomic.h>
 #include <stdlib.h>
 #include <unistd.h>
-#include <stdatomic.h>
 
-#define G_CONSTANT 0.207f
+#define G_CONSTANT 0.65f
 
 typedef struct {
 	float x, y, z;
@@ -23,7 +24,7 @@ typedef struct {
 
 	float dampening;
 	float life;
-	float size;
+	float radius;
 } Particle;
 
 typedef struct {
@@ -36,7 +37,8 @@ typedef struct {
 
 	pthread_mutex_t *mutex;
 	pthread_cond_t *cond;
-	int *barrier;
+	pthread_barrier_t *barrier;
+	volatile int run;
 	atomic_int exit;
 } Physics_params;
 
@@ -47,14 +49,23 @@ void vec_add(Vec3 &out, Vec3 adder) {
 	out.z += adder.z;
 }
 
+float vec_length(Vec3 v) {
+	return sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
+}
+
+float vec_dot(Vec3 a, Vec3 b) {
+	return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+
 Particle particle_init(Vec3 position, Color color) {
 	Particle p;
 	p.position = position;
 	p.color = color;
 	p.velocity = {0.0f, 0.0f, 0.0f};
 
-	p.life = 10;
-	p.dampening = 0.97f;
+	p.life = -2.0f; // INFO: Imorta
+	p.dampening = 0.9f;
+	p.radius = 0.05f;
 	return p;
 }
 
@@ -89,16 +100,62 @@ int particle_update(Particle *particles, unsigned long long size, float time) {
 	return alive_count;
 }
 
+void collision_update(Particle *particles, unsigned long long count, float restitution) {
+	for (unsigned long long i = 0; i < count; i++) {
+		for (unsigned long long j = i + 1; j < count; j++) {
+			Particle &p1 = particles[i];
+			Particle &p2 = particles[j];
+
+			if (p1.life == 0 || p2.life == 0) {
+				continue;
+			}
+
+			Vec3 diff = {p2.position.x - p1.position.x, p2.position.y - p1.position.y, p2.position.z - p1.position.z};
+			float dist = vec_length(diff);
+
+			float overlap = p1.radius + p2.radius - dist;
+			Vec3 normal;
+			if (overlap > 0 && dist > 0.00001f) {
+				normal = {diff.x / dist, diff.y / dist, diff.z / dist};
+				p1.position.x -= normal.x * overlap * 0.5f;
+				p1.position.y -= normal.y * overlap * 0.5f;
+				p1.position.z -= normal.z * overlap * 0.5f;
+
+				p2.position.x += normal.x * overlap * 0.5f;
+				p2.position.y += normal.y * overlap * 0.5f;
+				p2.position.z += normal.z * overlap * 0.5f;
+
+				float velocity_relative_x = p1.velocity.x - p2.velocity.x;
+				float velocity_relative_y = p1.velocity.y - p2.velocity.y;
+				float velocity_relative_z = p1.velocity.z - p2.velocity.z;
+
+                float v_rel = vec_dot({velocity_relative_x, velocity_relative_y, velocity_relative_z}, normal);
+				if (v_rel < 0) {
+                    float impulse = -(1 + restitution) * v_rel / 2.0f;
+
+					p1.velocity.x -= impulse * normal.x;
+					p1.velocity.y -= impulse * normal.y;
+					p1.velocity.z -= impulse * normal.z;
+
+					p2.velocity.x += impulse * normal.x;
+					p2.velocity.y += impulse * normal.y;
+					p2.velocity.z += impulse * normal.z;
+				}
+			}
+		}
+	}
+}
+
 void *t_update(void *args) {
 	Physics_params *params = (Physics_params *)args;
 	while (params->exit != 1) {
-        pthread_mutex_lock(params->mutex);
-		if (params->(missing) == 0) {
-			sleep(100);
+		pthread_mutex_lock(params->mutex);
+		while (params->run == 0) {
+			pthread_cond_wait(params->cond, params->mutex); // sleeps here
 		}
-        pthread_mutex_unlock(params->mutex);
+		pthread_mutex_unlock(params->mutex);
 
-        params->alive_count = 0;
+		params->alive_count = 0;
 		for (unsigned long long i = params->interval_start; i < params->interval_end; i++) {
 			Particle p = params->particle[i];
 
@@ -123,6 +180,7 @@ void *t_update(void *args) {
 				p.life = 0;
 
 			// DEBUG
+			/*
 			if (params->thread_id == 0) {
 				p.color = {1.0f, 1.0f, 1.0f, 1.0f};
 			} else if (params->thread_id == 1) {
@@ -135,10 +193,13 @@ void *t_update(void *args) {
 				p.color = {0.7f, 0.7f, 0.7f, 1.0f};
 				;
 			}
+			*/
 
 			params->particle[i] = p;
 			params->alive_count++;
 		}
+		params->run = 0;
+		pthread_barrier_wait(params->barrier);
 	}
 
 	return NULL;