le palle si scontrano

objects.c

@@ -0,0 +1,101 @@
+#include "objects.h"
+#include <math.h>
+#include <raymath.h>
+
+#define min(a, b) (a>b?b:a);
+#define max(a, b) (a>b?a:b);
+
+object *newobj(Vector3 pos, double r, Color c, double rest, double m)
+{
+  object *obj = (object*)malloc(sizeof(object));
+  obj->r = r;
+  obj->pos = pos;
+  obj->color = c;
+  obj->vel = (Vector3){0, 0, 0};
+  obj->restitution = rest;
+	obj->mass = m;
+  return obj;
+}
+
+Vector3 gravitation_force(object *obj1, object *obj2)
+{
+	const double GRAVITY_FACTOR = 1;
+	Vector3 force;
+	double dist, mass, total_g;
+	mass = obj1->mass * obj2->mass;
+	force = Vector3Subtract(obj2->pos, obj1->pos);
+
+	dist = Vector3Length(force);
+	force = Vector3Normalize(force);
+
+	dist = max(dist, obj1->r+obj2->r);
+
+	total_g = GRAVITY_FACTOR * mass / (dist * dist);
+	force = Vector3Scale(force, total_g);
+
+	return force;
+}
+
+void integrate_g(object *obj1, object *obj2, Vector3 force)
+{
+	Vector3 acc;
+	double f, a;
+
+	f = Vector3Length(force);
+	//acc = Vector3Scale(Vector3Subtract(obj1->pos, obj2->pos), f);
+	acc = force;
+
+	a = f / obj1->mass;
+	acc = Vector3Scale(Vector3Normalize(acc), a*GetFrameTime());
+	obj1->vel = Vector3Add(obj1->vel, acc);
+
+	a = f / obj2->mass;
+	acc = Vector3Scale(Vector3Normalize(acc), a*GetFrameTime());
+	obj2->vel = Vector3Add(obj2->vel, acc);
+}
+
+void resolve_collision(object *obj1, object *obj2)
+{
+	Vector3 normal = Vector3Subtract(obj1->pos, obj2->pos);
+	float distsq = Vector3LengthSqr(normal);
+	float sumrad = obj1->r+obj2->r;
+	float mindistsq = sumrad*sumrad;
+
+	if (distsq < mindistsq) {
+		float dist = sqrtf(distsq);
+
+		if (dist == 0) {
+			normal = Vector3RotateByAxisAngle(Vector3One(), (Vector3){0, 1, 1}, rand()*360);
+			dist = sumrad;
+		} else {
+			normal = Vector3Scale(normal, 1.0f / dist);
+		}
+
+		float overlap = sumrad - dist;
+		Vector3 separation = Vector3Scale(normal, overlap / 2.0f);
+
+		obj1->pos = Vector3Subtract(obj1->pos, separation);
+		obj2->pos = Vector3Add(obj2->pos, separation);
+
+	} else {
+		normal = Vector3Normalize(normal);
+	}
+
+	float vel1_normal = Vector3DotProduct(obj1->vel, normal);
+	float vel2_normal = Vector3DotProduct(obj2->vel, normal);
+
+	if (vel1_normal - vel2_normal > 0) return;
+
+	float totalmass = obj1->mass + obj2->mass;
+	float vel1_normal_final = (obj1->mass * vel1_normal + obj2->mass * vel2_normal - obj2->mass * obj2->restitution * (vel1_normal - vel2_normal)) / totalmass;
+	float vel2_normal_final = (obj1->mass * vel1_normal + obj2->mass * vel2_normal - obj2->mass * obj1->restitution * (vel2_normal - vel1_normal)) / totalmass;
+
+	Vector3 vel1_tang = Vector3Subtract(obj1->vel, Vector3Scale(normal, vel1_normal));
+	Vector3 vel2_tang = Vector3Subtract(obj2->vel, Vector3Scale(normal, vel2_normal));
+
+	obj1->vel = Vector3Add(Vector3Scale(normal, vel1_normal_final), vel1_tang);
+	obj2->vel = Vector3Add(Vector3Scale(normal, vel2_normal_final), vel2_tang);
+
+}
+
+