Working collision system

16 files changed, 281 insertions, 120 deletions

diff --git a/src/bullets.c b/src/bullets.c
index ea913c7..1619111 100644
--- a/src/bullets.c
+++ b/src/bullets.c
@@ -30,71 +30,16 @@ BulletHitInfo handleBulletHit(Entity * entity, Bullet bullet) {
 }
 
 BulletHitInfo shootBullet(World * world, Bullet bullet) {
-	int i, j;
-	RayCollision collision;
-	Entity * currentEntity;
-	Ray ray;
+	EntityId hitId = traceRayToEntityInWorld(world, bullet.ray, bullet.fromFingerprint, true);
+	Entity * hitEntity = getEntityFromWorld(*world, hitId);
 
-	// Set direction.
-	ray.direction = bullet.ray.direction;
+	// We hit a fucker.
+	if (hitEntity != NULL)
+		return handleBulletHit(hitEntity, bullet);
 
-	// Stores all the hits so we can find closest one.
-	int hits[world->entitiesCount];
-	size_t hitsSize = 0;
-
-	// Loop through entities.
-	for (i = 0; i < world->entitiesCount; ++i) {
-		currentEntity = &world->entities[i];
-
-		// This was the entity that shot it.
-		if (currentEntity->fingerprint == bullet.fromFingerprint)
-			continue;
-		else if (currentEntity->model == NULL) // Null model indeed.
-			continue;
-
-		// Set position relative to entity.
-		ray.position = Vector3Subtract(bullet.ray.position, currentEntity->position);
-
-		// Loop through meshes.
-		for (j = 0; j < currentEntity->model->meshCount; ++j) {
-			collision = GetRayCollisionMesh(
-				ray,
-				currentEntity->model->meshes[j],
-				currentEntity->model->transform
-			);
-
-			// Did hit.
-			if (collision.hit) {
-				hits[hitsSize] = i;
-				++hitsSize;
-				break;
-			}
-		}
-	}
-
-	// No hits.
-	if (hitsSize == 0)
-		return (BulletHitInfo){
-			.hit = false,
-			.killed = false,
-			.hitId = ENTITY_NONE,
-		};
-
-	float dis = Vector3Distance(world->entities[hits[0]].position, bullet.ray.position);
-	float closest = dis;
-	int closestNum = 0;
-
-	// Find closest.
-	for (i = 0; i < hitsSize; ++i) {
-		dis = Vector3Distance(world->entities[hits[i]].position, bullet.ray.position);
-
-		// This fucker is closer.
-		if (dis < closest) {
-			closest = dis;
-			closestNum = i;
-		}
-	}
-
-	// Handle closest bullet.
-	return handleBulletHit(&world->entities[hits[closestNum]], bullet);
+	return (BulletHitInfo){
+		.hit = false,
+		.killed = false,
+		.hitId = ENTITY_NONE,
+	};
 }
diff --git a/src/entities/antifaShip.c b/src/entities/antifaShip.c
index 1b90ea7..6fe4b66 100644
--- a/src/entities/antifaShip.c
+++ b/src/entities/antifaShip.c
@@ -5,6 +5,7 @@
 
 void initAntifaShip(Entity * entity, Game * game) {
 	entity->model = &game->assets.models[ANTIFA_SHIP_ASSET];
+	setEntityRadius(entity);
 
 	// Acceleration stuff.
 	entity->useAcceleration = true;
diff --git a/src/entities/caporale.c b/src/entities/caporale.c
index bf95235..bd6fcd5 100644
--- a/src/entities/caporale.c
+++ b/src/entities/caporale.c
@@ -4,6 +4,7 @@
 
 void initCaporale(Entity * entity, Game * game) {
 	entity->model = &game->assets.models[CAPORATE_ASSET];
+	setEntityRadius(entity);
 	entity->velocity.angularVelocity = (AxisAngle){(Vector3){1.0, 1.0, 1.0}, 1.0};
 }
 
diff --git a/src/entities/generale.c b/src/entities/generale.c
index 8e86e3f..017f213 100644
--- a/src/entities/generale.c
+++ b/src/entities/generale.c
@@ -4,6 +4,7 @@
 
 void initGenerale(Entity * entity, Game * game) {
 	entity->model = &game->assets.models[GENERALE_ASSET];
+	setEntityRadius(entity);
 	entity->velocity.angularVelocity = (AxisAngle){(Vector3){1.0, 1.0, 1.0}, 1.0};
 }
 
diff --git a/src/entities/maresciallo.c b/src/entities/maresciallo.c
index 568d056..6127786 100644
--- a/src/entities/maresciallo.c
+++ b/src/entities/maresciallo.c
@@ -4,6 +4,7 @@
 
 void initMaresciallo(Entity * entity, Game * game) {
 	entity->model = &game->assets.models[MARESCIALLO_ASSET];
+	setEntityRadius(entity);
 	entity->velocity.angularVelocity = (AxisAngle){(Vector3){1.0, 1.0, 1.0}, 1.0};
 }
 
diff --git a/src/entities/mussolini.c b/src/entities/mussolini.c
index 9ea54c7..ed6d182 100644
--- a/src/entities/mussolini.c
+++ b/src/entities/mussolini.c
@@ -5,6 +5,7 @@
 
 void initMussolini(Entity * entity, Game * game) {
 	entity->model = &game->assets.models[MUSSOLINI_ASSET];
+	setEntityRadius(entity);
 
 	// PID configs.
 	PIDConfig stickPIDConfig = {
diff --git a/src/entities/sergente.c b/src/entities/sergente.c
index 4a821dc..2614ec8 100644
--- a/src/entities/sergente.c
+++ b/src/entities/sergente.c
@@ -4,6 +4,7 @@
 
 void initSergente(Entity * entity, Game * game) {
 	entity->model = &game->assets.models[SERGENTE_ASSET];
+	setEntityRadius(entity);
 	entity->velocity.angularVelocity = (AxisAngle){(Vector3){1.0, 1.0, 1.0}, 1.0};
 }
 
diff --git a/src/entities/soldato.c b/src/entities/soldato.c
index beec40a..74544aa 100644
--- a/src/entities/soldato.c
+++ b/src/entities/soldato.c
@@ -3,6 +3,7 @@
 
 void initSoldato(Entity * entity, Game * game) {
 	entity->model = &game->assets.models[SOLDATO_ASSET];
+	setEntityRadius(entity);
 
 	// Acceleration.
 	entity->useAcceleration = true;
diff --git a/src/entity.c b/src/entity.c
index 9c6e5e3..e65f5d4 100644
--- a/src/entity.c
+++ b/src/entity.c
@@ -53,6 +53,7 @@ Entity createEntity(EntityType type, Game * game) {
 	Entity entity = (Entity){
 		.type = type,
 		.model = NULL,
+		.radius = 0.0,
 		.position = Vector3Zero(),
 		.rotation = QuaternionIdentity(),
 		.velocity = entityVelocityIdentity(),
@@ -75,25 +76,135 @@ void closeEntity(Entity * entity) {
 	entityTypeInfo[entity->type].closeCb(entity);
 }
 
-bool entitiesCollide(Entity entity1, Entity entity2) {
+void setEntityRadius(Entity * entity) {
+	int i, j;
+	Mesh mesh;
+	Vector3 v;
+	float farthest = 0.0;
+
+	// Loop through meshes.
+	for (i = 0; i < entity->model->meshCount; ++i) {
+		mesh = entity->model->meshes[i];
+
+		// Loop though vertices.
+		for (j = 0; j < mesh.vertexCount; ++j) {
+			v = (Vector3){
+				mesh.vertices[j * 3],
+				mesh.vertices[j * 3 + 1],
+				mesh.vertices[j * 3 + 2]
+			};
+
+			farthest = fmaxf(farthest, Vector3Length(v));
+		}
+	}
+
+	entity->radius = farthest;
+}
+
+// Little triangle helper for checkEntityMeshCollision.
+void getTriangleFromMeshAndTransform(int num, Mesh mesh, Vector3 * triangle, Quaternion rotation, Vector3 position) {
 	int i;
-	Vector3 triangle[3];
+	int triangleLocation = num * 9;
+	int vertexLocation;
+
+	for (i = 0; i < 3; ++i) {
+		vertexLocation = (i * 3) + triangleLocation;
+
+		// Get vertex.
+		triangle[i] = (Vector3){
+			mesh.vertices[vertexLocation],
+			mesh.vertices[vertexLocation + 1],
+			mesh.vertices[vertexLocation + 2],
+		};
+
+		// Transform vertex.
+		triangle[i] = Vector3RotateByQuaternion(triangle[i], rotation);
+		triangle[i] = Vector3Add(triangle[i], position);
+	}
+}
+
+// Big mesh helper for checkEntityCollision.
+bool checkEntityMeshCollision(Entity entity1, Entity entity2, int entity1MeshNum, int entity2MeshNum) {
+	int triangle1Num;
+	int triangle2Num;
+	bool collided;
+
+	Vector3 triangle1[3];
+	Vector3 triangle2[3];
 
-	/*
-	for (i = 0; i < mesh.triangleCount; ++i) {
+	Mesh mesh1 = entity1.model->meshes[entity1MeshNum];
+	Mesh mesh2 = entity2.model->meshes[entity2MeshNum];
 
-		for (j = 0; j < 3; ++j)
-			triangle[j] = (Vector3){
-				mesh.vertices[(i * 9) + (j * 3)],
-				mesh.vertices[(i * 9) + (j * 3) + 1],
-				mesh.vertices[(i * 9) + (j * 3) + 2],
+	Vector3 mesh2Triangles[mesh2.triangleCount][3];
+
+	// Get mesh2Triangles.
+	for (triangle2Num = 0; triangle2Num < mesh2.triangleCount; ++triangle2Num) {
+		getTriangleFromMeshAndTransform(
+			triangle2Num,
+			mesh2,
+			mesh2Triangles[triangle2Num],
+			entity2.rotation,
+			entity2.position
+		);
+	}
+
+	// Test every triangle for collision.
+	for (triangle1Num = 0; triangle1Num < mesh1.triangleCount; ++triangle1Num) {
+		getTriangleFromMeshAndTransform(
+			triangle1Num,
+			mesh1,
+			triangle1,
+			entity1.rotation,
+			entity1.position
+		);
+
+		for (triangle2Num = 0; triangle2Num < mesh2.triangleCount; ++triangle2Num) {
+			triangle2[0] = mesh2Triangles[triangle2Num][0];
+			triangle2[1] = mesh2Triangles[triangle2Num][1];
+			triangle2[2] = mesh2Triangles[triangle2Num][2];
+
+			// Get normals.
+			Vector3 normal1 = (Vector3){
+				mesh1.normals[triangle1Num * 3],
+				mesh1.normals[triangle1Num * 3 + 1],
+				mesh1.normals[triangle1Num * 3 + 2]
+			};
+			
+			Vector3 normal2 = (Vector3){
+				mesh2.normals[triangle2Num * 3],
+				mesh2.normals[triangle2Num * 3 + 1],
+				mesh2.normals[triangle2Num * 3 + 2]
 			};
 
-		DrawLine3D(triangle[0], triangle[1], BLUE);
-		DrawLine3D(triangle[1], triangle[2], BLUE);
-		DrawLine3D(triangle[2], triangle[0], BLUE);
+			// Check collision.
+			collided = checkTriangleCollision3D(
+				triangle1,
+				triangle2,
+				normal1,
+				normal2
+			);
+
+			if (collided)
+				return true;
+		}
 	}
-	*/
+
+	return false;
+}
+
+bool checkEntityCollision(Entity entity1, Entity entity2) {
+	int i, j;
+
+	// Failed quick check.
+	if (Vector3Distance(entity1.position, entity2.position) > entity1.radius + entity2.radius)
+		return false;
+
+	// Loop through every mesh and check.
+	for (i = 0; i < entity1.model->meshCount; ++i)
+		for (j = 0; j < entity2.model->meshCount; ++j)
+			if (checkEntityMeshCollision(entity1, entity2, i, j))
+				return true;
+
 	return false;
 }
 
diff --git a/src/entity.h b/src/entity.h
index 844386c..1ac3422 100644
--- a/src/entity.h
+++ b/src/entity.h
@@ -63,6 +63,7 @@ typedef struct Entity {
 	EntityType type;
 
 	Model * model;
+	float radius; // Used for quick collision detection.
 
 	Vector3 position;
 	Quaternion rotation;
@@ -102,7 +103,8 @@ const extern EntityTypeInfo entityTypeInfo[ENTITY_TYPE_COUNT];
 Entity createEntity(EntityType type, Game * game);
 void closeEntity(Entity * entity);
 
-bool entitiesCollide(Entity entity1, Entity entity2);
+void setEntityRadius(Entity * entity); // Uses model to set radius;
+bool checkEntityCollision(Entity entity1, Entity entity2);
 
 // Helper functions for updating and drawing.
 void entityDraw(Entity * entity);
diff --git a/src/game.c b/src/game.c
index 5141270..f27449f 100644
--- a/src/game.c
+++ b/src/game.c
@@ -29,7 +29,7 @@ void initGame(Game * game) {
 	};
 
 	for (int i = 1; i < 101; ++i) {
-		entries[i] = (WorldEntry){ENTITY_CAPORALE, (Vector3){0.0, 0.0, i*10}, QuaternionIdentity()};
+		entries[i] = (WorldEntry){ENTITY_SOLDATO, (Vector3){0.0, 0.0, i*10}, QuaternionIdentity()};
 	}
 
 	addEntriesToWorld(
diff --git a/src/settings.c b/src/settings.c
index 831ccc3..5b91f65 100644
--- a/src/settings.c
+++ b/src/settings.c
@@ -10,8 +10,8 @@ void initSettings(Settings * settings) {
 		.yawStick = 0,
 		.rollStick = 2,
 		.speedStick = 3,
-		.joystickSensitivity = 1.0,
-		.fps = 60,
+		.joystickSensitivity = 0.5,
+		.fps = 0,
 		.drawFps = true
 	};
 }
diff --git a/src/util.c b/src/util.c
index e39478c..8ef4018 100644
--- a/src/util.c
+++ b/src/util.c
@@ -16,3 +16,59 @@ float signum(float n) {
 void printVector3(Vector3 v) {
 	printf("%f %f %f\n", v.x, v.y, v.z);
 }
+
+
+// Warning. Mostly chatgpt written.
+
+static void projectTriangleOntoAxis(const Vector3 triangle[3], const Vector3 axis, float* min, float* max) {
+    float dot1 = Vector3DotProduct(triangle[0], axis);
+    float dot2 = Vector3DotProduct(triangle[1], axis);
+    float dot3 = Vector3DotProduct(triangle[2], axis);
+
+    *min = fminf(fminf(dot1, dot2), dot3);
+    *max = fmaxf(fmaxf(dot1, dot2), dot3);
+}
+
+static bool trianglesIntersectOnAxis(const Vector3 triangleA[3], const Vector3 triangleB[3], Vector3 axis) {
+    float minA, maxA;
+    float minB, maxB;
+
+    projectTriangleOntoAxis(triangleA, axis, &minA, &maxA);
+    projectTriangleOntoAxis(triangleB, axis, &minB, &maxB);
+
+    return (minA <= maxB && maxA >= minB);
+}
+
+bool checkTriangleCollision3D(const Vector3 triangleA[3], const Vector3 triangleB[3], Vector3 normalA, Vector3 normalB) {
+    // Test triangle normals
+    if (!trianglesIntersectOnAxis(triangleA, triangleB, normalA))
+        return false;
+
+    if (!trianglesIntersectOnAxis(triangleA, triangleB, normalB))
+        return false;
+
+    // Test cross products of triangle edges
+    Vector3 edgesA[3] = {
+        Vector3Subtract(triangleA[1], triangleA[0]),
+        Vector3Subtract(triangleA[2], triangleA[1]),
+        Vector3Subtract(triangleA[0], triangleA[2])
+    };
+
+    Vector3 edgesB[3] = {
+        Vector3Subtract(triangleB[1], triangleB[0]),
+        Vector3Subtract(triangleB[2], triangleB[1]),
+        Vector3Subtract(triangleB[0], triangleB[2])
+    };
+
+    for (int i = 0; i < 3; i++) {
+        Vector3 axis = Vector3CrossProduct(normalA, edgesA[i]);
+        if (!trianglesIntersectOnAxis(triangleA, triangleB, axis))
+            return false;
+
+        axis = Vector3CrossProduct(normalB, edgesB[i]);
+        if (!trianglesIntersectOnAxis(triangleA, triangleB, axis))
+            return false;
+    }
+
+    return true;
+}
diff --git a/src/util.h b/src/util.h
index 817d472..b7630c5 100644
--- a/src/util.h
+++ b/src/util.h
@@ -23,4 +23,13 @@ float signum(float n);
 // Debugging stuff.
 void printVector3(Vector3 v);
 
+typedef struct Triangle3D {
+    Vector3 v1;
+    Vector3 v2;
+    Vector3 v3;
+} Triangle3D;
+
+// Thank you chatgpt.
+bool checkTriangleCollision3D(const Vector3 triangleA[3], const Vector3 triangleB[3], Vector3 normalA, Vector3 normalB);
+
 #endif
diff --git a/src/world.c b/src/world.c
index 553f2ed..e6f24bb 100644
--- a/src/world.c
+++ b/src/world.c
@@ -225,47 +225,19 @@ KfError removeEntityFromWorld(World * world, EntityId id) {
 	return KFSUCCESS;
 }
 
-void checkCollisionWithWorld(World * world, Entity * entity) {
-	int i, j;
-	Entity * entity2;
-	BoundingBox box1, box2;
-
-	Mesh mesh = entity->model->meshes[0];
-
-	Vector3 triangle[3];
-
-	for (i = 0; i < mesh.triangleCount; ++i) {
-
-		for (j = 0; j < 3; ++j)
-			triangle[j] = (Vector3){
-				mesh.vertices[(i * 9) + (j * 3)],
-				mesh.vertices[(i * 9) + (j * 3) + 1],
-				mesh.vertices[(i * 9) + (j * 3) + 2],
-			};
-
-		DrawLine3D(triangle[0], triangle[1], BLUE);
-		DrawLine3D(triangle[1], triangle[2], BLUE);
-		DrawLine3D(triangle[2], triangle[0], BLUE);
-	}
-
-	// Check for collision.
-	//for (i = 0; i < world->entitiesCount; ++i) {
-	//	entity2 = &world->entities[i];
+void handleCollisionInWorld(Entity * entity1, Entity * entity2) {
+	if (entity1->id != 0)
+		entity1->health = 0.0;
+	if (entity2->id != 0)
+		entity2->health = 0.0;
 
-	//	if (entity->fingerprint == entity2->fingerprint)
-	//		continue;
-
-	//	// These fuckers collided.
-	//	if (CheckCollisionBoxes(box1, box2)) {
-	//		printf("hi %ld\n", clock());
-	//		break;
-	//	}
-	//}
+	puts("cc");
 }
 
 void updateWorld(World * world, Game * game) {
-	int i;
+	int i, j;
 	Entity * entity;
+	Entity * entity2;
 
 	// People are fucking dying.
 	EntityId kills[world->entitiesCount];
@@ -278,6 +250,15 @@ void updateWorld(World * world, Game * game) {
 		if (entity->updateCb != NULL)
 			entity->updateCb(game, entity);
 
+		// Check for collision.
+		for (j = 0; j < i; ++j) {
+			entity2 = &world->entities[j];
+
+			// Collided.
+			if (checkEntityCollision(*entity, *entity2))
+				handleCollisionInWorld(entity, entity2);
+		}
+
 		// It fucking died.
 		if (entity->health <= 0.0) {
 			kills[killCount] = entity->id;
@@ -297,14 +278,60 @@ void drawWorld(World * world, Game * game) {
 	for (i = 0; i < world->entitiesCount; ++i) {
 		entity = &world->entities[i];
 
-		//checkCollisionWithWorld(world, entity);
-
 		// Call draw callback.
 		if (entity->drawCb != NULL)
 			entity->drawCb(game, entity);
 	}
 }
 
+
+EntityId traceRayToEntityInWorld(World * world, Ray ray, EntityFingerprint from, bool useFrom) {
+	int i, j;
+	RayCollision collision;
+	Entity * currentEntity;
+	Ray transformedRay;
+
+	// Set direction.
+	transformedRay.direction = ray.direction;
+
+	// Used for finding closest.
+	float closest = -1.0; // -1.0 means ray haven't hit anything.
+	EntityId closestId = ENTITY_NONE;
+
+	// Loop through entities.
+	for (i = 0; i < world->entitiesCount; ++i) {
+		currentEntity = &world->entities[i];
+
+		if (currentEntity->fingerprint == from && useFrom)
+			continue;
+		else if (currentEntity->model == NULL) // Null model indeed.
+			continue;
+
+		// Set position relative to entity.
+		transformedRay.position = Vector3Subtract(ray.position, currentEntity->position);
+
+		// Loop through meshes.
+		for (j = 0; j < currentEntity->model->meshCount; ++j) {
+			collision = GetRayCollisionMesh(
+				transformedRay,
+				currentEntity->model->meshes[j],
+				currentEntity->model->transform
+			);
+
+			// Did hit.
+			if (collision.hit) {
+				// Find closest.
+				if (collision.distance < closest || closest == -1.0) {
+					closest = collision.distance;
+					closestId = currentEntity->id;
+				}
+			}
+		}
+	}
+
+	return closestId;
+}
+
 KfError addEntryToWorld(World * world, Game * game, WorldEntry entry) {
 	// Create entity.
 	Entity entity = createEntity(entry.type, game);
diff --git a/src/world.h b/src/world.h
index b3a7d69..9e36dc7 100644
--- a/src/world.h
+++ b/src/world.h
@@ -35,6 +35,10 @@ KfError removeEntityFromWorld(World * world, EntityId id);
 void updateWorld(World * world, Game * game);
 void drawWorld(World * world, Game * game);
 
+// Returns the closest entity that the ray hits or ENTITY_NONE;
+// From can be used to stop it from hitting the entity the ray is coming from...
+EntityId traceRayToEntityInWorld(World * world, Ray ray, EntityFingerprint from, bool useFrom);
+
 // Used for creating worlds.
 typedef struct WorldEntry {
 	EntityType type;