Working collision system

1 files changed, 124 insertions, 13 deletions

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;
 }