Sonic fast collision

8 files changed, 251 insertions, 66 deletions

diff --git a/src/entities/antifaShip.c b/src/entities/antifaShip.c
index 6fe4b66..94fc8c8 100644
--- a/src/entities/antifaShip.c
+++ b/src/entities/antifaShip.c
@@ -5,6 +5,8 @@
 
 void initAntifaShip(Entity * entity, Game * game) {
 	entity->model = &game->assets.models[ANTIFA_SHIP_ASSET];
+	entity->collisionModel = entityCreateCollisionModel(*entity->model);
+	entity->transformedCollisionModel = entityCreateCollisionModel(*entity->model);
 	setEntityRadius(entity);
 
 	// Acceleration stuff.
@@ -31,6 +33,9 @@ void initAntifaShip(Entity * entity, Game * game) {
 void closeAntifaShip(Entity * entity) {
 	if (entity->data != NULL)
 		KF_FREE(entity->data);
+
+	entityFreeCollisionModel(entity->collisionModel);
+	entityFreeCollisionModel(entity->transformedCollisionModel);
 }
 
 void controlAntifaShipJoystick(Game * game, Entity * entity) {
@@ -102,6 +107,8 @@ void controlAntifaShipKeyboardAndMouse(Game * game, Entity * entity) {
 }
 
 void updateAntifaShip(Game * game, Entity * entity) {
+	entityUpdateLastValues(entity);
+
 	switch (game->settings.controlMode) {
 		case JOYSTICK_CONTROL:
 			controlAntifaShipJoystick(game, entity);
@@ -112,6 +119,8 @@ void updateAntifaShip(Game * game, Entity * entity) {
 		default:
 			break;
 	}
+
+	entityCheckTransformedCollisionModel(entity);
 }
 
 void drawAntifaShip(Game * game, Entity * entity) {
diff --git a/src/entities/soldato.c b/src/entities/soldato.c
index 74544aa..2cc0e07 100644
--- a/src/entities/soldato.c
+++ b/src/entities/soldato.c
@@ -3,6 +3,8 @@
 
 void initSoldato(Entity * entity, Game * game) {
 	entity->model = &game->assets.models[SOLDATO_ASSET];
+	entity->collisionModel = entityCreateCollisionModel(*entity->model);
+	entity->transformedCollisionModel = entityCreateCollisionModel(*entity->model);
 	setEntityRadius(entity);
 
 	// Acceleration.
@@ -46,9 +48,14 @@ void initSoldato(Entity * entity, Game * game) {
 void closeSoldato(Entity * entity) {
 	if (entity->data != NULL)
 		KF_FREE(entity->data);
+
+	entityFreeCollisionModel(entity->collisionModel);
+	entityFreeCollisionModel(entity->transformedCollisionModel);
 }
 
 void updateSoldato(Game * game, Entity * entity) {
+	entityUpdateLastValues(entity);
+
 	Entity * player = getEntityFromWorld(game->world, 0);
 	Soldato * data = (Soldato*)entity->data;
 
@@ -57,6 +64,8 @@ void updateSoldato(Game * game, Entity * entity) {
 		player->position,
 		&data->flyToPoint
 	);
+
+	entityCheckTransformedCollisionModel(entity);
 }
 
 void drawSoldato(Game * game, Entity * entity) {
diff --git a/src/entity.c b/src/entity.c
index e65f5d4..e5afbcd 100644
--- a/src/entity.c
+++ b/src/entity.c
@@ -54,8 +54,13 @@ Entity createEntity(EntityType type, Game * game) {
 		.type = type,
 		.model = NULL,
 		.radius = 0.0,
+		.collisionModel = (EntityCollisionModel){0, NULL},
+		.transformedCollisionModel = (EntityCollisionModel){0, NULL},
+		.collisionModelTransformed = false,
 		.position = Vector3Zero(),
 		.rotation = QuaternionIdentity(),
+		.lastPosition = Vector3Zero(),
+		.lastRotation = QuaternionIdentity(),
 		.velocity = entityVelocityIdentity(),
 		.lastVelocity = entityVelocityIdentity(),
 		.useAcceleration = false,
@@ -102,7 +107,7 @@ void setEntityRadius(Entity * entity) {
 }
 
 // Little triangle helper for checkEntityMeshCollision.
-void getTriangleFromMeshAndTransform(int num, Mesh mesh, Vector3 * triangle, Quaternion rotation, Vector3 position) {
+void getTriangleFromMeshAndTransform(int num, Mesh mesh, Triangle3D triangle, Quaternion rotation, Vector3 position) {
 	int i;
 	int triangleLocation = num * 9;
 	int vertexLocation;
@@ -123,65 +128,178 @@ void getTriangleFromMeshAndTransform(int num, Mesh mesh, Vector3 * triangle, Qua
 	}
 }
 
+// Little normals helper for checkEntityMeshCollision. num is not triangle number for fast fast reasons.
+Vector3 getNormalsFromMeshAndTransform(int num, Mesh mesh, Quaternion rotation) {
+	// Get normals.
+	Vector3 normals = (Vector3){
+		mesh.normals[num],
+		mesh.normals[num + 1],
+		mesh.normals[num + 2]
+	};
+
+	// Transform.
+	normals = Vector3RotateByQuaternion(normals, rotation);
+
+	return normals;
+}
+
+EntityCollisionMesh createCollisionMesh(Mesh mesh) {
+	int i, j;
+	EntityCollisionMesh collisionMesh;
+	collisionMesh.triangleCount = mesh.triangleCount;
+
+	// Allocate.
+	collisionMesh.triangles = (Triangle3D*)KF_CALLOC(collisionMesh.triangleCount, sizeof(Triangle3D));
+	collisionMesh.normals = (Vector3*)KF_CALLOC(collisionMesh.triangleCount, sizeof(Vector3));
+
+	if (collisionMesh.triangles == NULL || collisionMesh.normals == NULL) {
+		ALLOCATION_ERROR;
+		return (EntityCollisionMesh){0, NULL, NULL};
+	}
+
+	int triangleLocation;
+	int vertexLocation;
+	int normalLocation;
+
+	// Copy triangles and normals over.
+	for (i = 0; i < collisionMesh.triangleCount; ++i) {
+		triangleLocation = i * 9;
+		normalLocation = i * 3;
+
+		// Get triangle.
+		for (j = 0; j < 3; ++j) {
+			vertexLocation = triangleLocation + (j * 3);
+   
+			collisionMesh.triangles[i][j] = (Vector3){
+				mesh.vertices[vertexLocation],
+				mesh.vertices[vertexLocation + 1],
+				mesh.vertices[vertexLocation + 2]
+			};
+		}
+
+		// Get normal.
+		collisionMesh.normals[i] = (Vector3){
+			mesh.normals[normalLocation],
+			mesh.normals[normalLocation + 1],
+			mesh.normals[normalLocation + 2],
+		};
+	}
+
+	return collisionMesh;
+}
+
+void freeCollisionMesh(EntityCollisionMesh mesh) {
+	if (mesh.triangles == NULL)
+		return;
+
+	KF_FREE(mesh.triangles);
+	KF_FREE(mesh.normals);
+}
+
+EntityCollisionModel entityCreateCollisionModel(Model model) {
+	int i;
+	EntityCollisionModel collisionModel;      
+	collisionModel.meshCount = model.meshCount;
+
+	// Allocate.
+	collisionModel.meshes = (EntityCollisionMesh*)KF_CALLOC(collisionModel.meshCount, sizeof(EntityCollisionMesh));
+
+	if (collisionModel.meshes == NULL) {
+		ALLOCATION_ERROR;
+		return (EntityCollisionModel){0, NULL};
+	}
+
+	// Create meshes.
+	for (i = 0; i < collisionModel.meshCount; ++i)
+		collisionModel.meshes[i] = createCollisionMesh(model.meshes[i]);
+
+	return collisionModel;
+}
+
+void entityFreeCollisionModel(EntityCollisionModel model) {
+	int i;
+
+	if (model.meshes == NULL)
+		return;
+
+	for (i = 0; i < model.meshCount; ++i)
+		freeCollisionMesh(model.meshes[i]);
+
+	KF_FREE(model.meshes);
+}
+
+void transformCollisionMesh(Entity * entity, int num) {
+	int i, j;
+	EntityCollisionMesh mesh1 = entity->collisionModel.meshes[num];
+	EntityCollisionMesh * mesh2 = &entity->transformedCollisionModel.meshes[num];
+	Vector3 vertex;
+
+	Matrix m = QuaternionToMatrix(QuaternionInvert(entity->rotation));
+
+	for (i = 0; i < mesh1.triangleCount; ++i) {
+
+		// Transform triangle.
+		for (j = 0; j < 3; ++j) {
+			vertex = mesh1.triangles[i][j];
+
+			// Rotate.
+			mesh2->triangles[i][j] = (Vector3){
+				m.m0 * vertex.x + m.m1 * vertex.y + m.m2 * vertex.z,
+				m.m4 * vertex.x + m.m5 * vertex.y + m.m6 * vertex.z,
+				m.m8 * vertex.x + m.m9 * vertex.y + m.m10 * vertex.z
+			};
+
+			// Move to position.
+			mesh2->triangles[i][j] = Vector3Add(entity->position, mesh2->triangles[i][j]);
+		}
+
+		// Transform normals.
+		vertex = mesh1.normals[i];
+
+		// Rotate.
+		mesh2->normals[i] = (Vector3){
+			m.m0 * vertex.x + m.m1 * vertex.y + m.m2 * vertex.z,
+			m.m4 * vertex.x + m.m5 * vertex.y + m.m6 * vertex.z,
+			m.m8 * vertex.x + m.m9 * vertex.y + m.m10 * vertex.z
+		};
+
+	}
+}
+
+void entityTransformCollisionModel(Entity * entity) {
+	int i;
+
+	for (i = 0; i < entity->collisionModel.meshCount; ++i)
+		transformCollisionMesh(entity, i);
+}
+
+void entityCheckTransformedCollisionModel(Entity * entity) {
+	bool moved = !Vector3Equals(entity->lastPosition, entity->position);
+	moved |= !QuaternionEquals(entity->lastRotation, entity->rotation);
+
+	if (moved)
+		entity->collisionModelTransformed = false;
+}
+
 // 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];
-
-	Mesh mesh1 = entity1.model->meshes[entity1MeshNum];
-	Mesh mesh2 = entity2.model->meshes[entity2MeshNum];
-
-	Vector3 mesh2Triangles[mesh2.triangleCount][3];
-
-	// Get mesh2Triangles.
-	for (triangle2Num = 0; triangle2Num < mesh2.triangleCount; ++triangle2Num) {
-		getTriangleFromMeshAndTransform(
-			triangle2Num,
-			mesh2,
-			mesh2Triangles[triangle2Num],
-			entity2.rotation,
-			entity2.position
-		);
-	}
+	EntityCollisionMesh mesh1 = entity1.transformedCollisionModel.meshes[entity1MeshNum];
+	EntityCollisionMesh mesh2 = entity2.transformedCollisionModel.meshes[entity2MeshNum];
 
 	// 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]
-			};
 
-			// Check collision.
+			// Check for collision.
 			collided = checkTriangleCollision3D(
-				triangle1,
-				triangle2,
-				normal1,
-				normal2
+				mesh1.triangles[triangle1Num],
+				mesh2.triangles[triangle2Num],
+				mesh1.normals[triangle1Num],
+				mesh2.normals[triangle2Num]
 			);
 
 			if (collided)
@@ -192,17 +310,28 @@ bool checkEntityMeshCollision(Entity entity1, Entity entity2, int entity1MeshNum
 	return false;
 }
 
-bool checkEntityCollision(Entity entity1, Entity entity2) {
+bool checkEntityCollision(Entity * entity1, Entity * entity2) {
 	int i, j;
 
 	// Failed quick check.
-	if (Vector3Distance(entity1.position, entity2.position) > entity1.radius + entity2.radius)
+	if (Vector3Distance(entity1->position, entity2->position) > entity1->radius + entity2->radius)
 		return false;
 
+	// Transform collision model.
+	if (!entity1->collisionModelTransformed) {
+		entityTransformCollisionModel(entity1);
+		entity1->collisionModelTransformed = true;
+	}
+
+	if (!entity2->collisionModelTransformed) {
+		entityTransformCollisionModel(entity2);
+		entity2->collisionModelTransformed = true;
+	}
+
 	// 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))
+	for (i = 0; i < entity1->collisionModel.meshCount; ++i)
+		for (j = 0; j < entity2->collisionModel.meshCount; ++j)
+			if (checkEntityMeshCollision(*entity1, *entity2, i, j))
 				return true;
 
 	return false;
@@ -289,6 +418,11 @@ void entityJoystickControl(Entity * entity, Vector3 stick, float speed) {
 	entityUpdatePosition(entity);
 }
 
+void entityUpdateLastValues(Entity * entity) {
+	entity->lastPosition = entity->position;
+	entity->lastRotation = entity->rotation;
+}
+
 void entityFlyToPoint(Entity * entity, Vector3 point, EntityFlyToPointInfo * info) {
 	float t = GetFrameTime();
 
diff --git a/src/entity.h b/src/entity.h
index 1ac3422..6246fdd 100644
--- a/src/entity.h
+++ b/src/entity.h
@@ -45,6 +45,26 @@ typedef struct EntityVelocity {
 
 EntityVelocity entityVelocityIdentity();
 
+typedef struct EntityCollisionMesh {
+	size_t triangleCount;
+	Triangle3D * triangles;
+	Vector3 * normals;
+} EntityCollisionMesh;
+
+typedef struct EntityCollisionModel {
+	size_t meshCount;
+	EntityCollisionMesh * meshes;
+} EntityCollisionModel;
+
+EntityCollisionModel entityCreateCollisionModel(Model model);
+void entityFreeCollisionModel(EntityCollisionModel model);
+
+// Only use if both collision models came from the same model.
+void entityTransformCollisionModel(Entity * entity);
+
+// Checks if collision model will need to be transformed.
+void entityCheckTransformedCollisionModel(Entity * entity);
+
 // This fucker hit something.
 typedef struct EntityCollision {
 	bool hit;
@@ -64,10 +84,18 @@ typedef struct Entity {
 
 	Model * model;
 	float radius; // Used for quick collision detection.
+	
+	EntityCollisionModel collisionModel;
+	EntityCollisionModel transformedCollisionModel;
+	// Use for checking if model been transformed indeed.
+	bool collisionModelTransformed;
 
 	Vector3 position;
 	Quaternion rotation;
 
+	Vector3 lastPosition;
+	Quaternion lastRotation;
+
 	EntityVelocity velocity;
 	EntityVelocity lastVelocity;
 
@@ -104,12 +132,13 @@ Entity createEntity(EntityType type, Game * game);
 void closeEntity(Entity * entity);
 
 void setEntityRadius(Entity * entity); // Uses model to set radius;
-bool checkEntityCollision(Entity entity1, Entity entity2);
+bool checkEntityCollision(Entity * entity1, Entity * entity2);
 
-// Helper functions for updating and drawing.
+// Helper functions for updating, drawing...
 void entityDraw(Entity * entity);
 void entityUpdatePosition(Entity * entity);
 void entityUpdateRotation(Entity * entity);
+void entityUpdateLastValues(Entity * entity); // Should be at top of update function.
 
 void entityJoystickControl(Entity * entity, Vector3 stick, float speed);
 
diff --git a/src/gameCommon.h b/src/gameCommon.h
index 4c22664..d1d78ee 100644
--- a/src/gameCommon.h
+++ b/src/gameCommon.h
@@ -34,6 +34,8 @@ typedef struct Entity Entity;
 
 #define ALLOCATION_ERROR TraceLog(LOG_ERROR, "Allocation error in %s:%d", __FILE__, __LINE__)
 
+typedef Vector3 Triangle3D[3];
+
 // Errors.
 typedef enum KfError {
 	KFERROR = -1,
diff --git a/src/util.c b/src/util.c
index 8ef4018..7f6d2ce 100644
--- a/src/util.c
+++ b/src/util.c
@@ -20,7 +20,7 @@ void printVector3(Vector3 v) {
 
 // Warning. Mostly chatgpt written.
 
-static void projectTriangleOntoAxis(const Vector3 triangle[3], const Vector3 axis, float* min, float* max) {
+static void projectTriangleOntoAxis(const Triangle3D triangle, 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);
@@ -29,7 +29,7 @@ static void projectTriangleOntoAxis(const Vector3 triangle[3], const Vector3 axi
     *max = fmaxf(fmaxf(dot1, dot2), dot3);
 }
 
-static bool trianglesIntersectOnAxis(const Vector3 triangleA[3], const Vector3 triangleB[3], Vector3 axis) {
+static bool trianglesIntersectOnAxis(const Triangle3D triangleA, const Triangle3D triangleB, Vector3 axis) {
     float minA, maxA;
     float minB, maxB;
 
@@ -39,7 +39,7 @@ static bool trianglesIntersectOnAxis(const Vector3 triangleA[3], const Vector3 t
     return (minA <= maxB && maxA >= minB);
 }
 
-bool checkTriangleCollision3D(const Vector3 triangleA[3], const Vector3 triangleB[3], Vector3 normalA, Vector3 normalB) {
+bool checkTriangleCollision3D(const Triangle3D triangleA, const Triangle3D triangleB, Vector3 normalA, Vector3 normalB) {
     // Test triangle normals
     if (!trianglesIntersectOnAxis(triangleA, triangleB, normalA))
         return false;
@@ -72,3 +72,9 @@ bool checkTriangleCollision3D(const Vector3 triangleA[3], const Vector3 triangle
 
     return true;
 }
+
+void copyTriangle3D(Triangle3D a, const Triangle3D b) {
+	a[0] = b[0];
+	a[1] = b[1];
+	a[2] = b[2];
+}
diff --git a/src/util.h b/src/util.h
index b7630c5..bc8c07f 100644
--- a/src/util.h
+++ b/src/util.h
@@ -23,13 +23,9 @@ 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);
+bool checkTriangleCollision3D(const Triangle3D triangleA, const Triangle3D triangleB, Vector3 normalA, Vector3 normalB);
+
+void copyTriangle3D(Triangle3D a, const Triangle3D b);
 
 #endif
diff --git a/src/world.c b/src/world.c
index e6f24bb..1a3edde 100644
--- a/src/world.c
+++ b/src/world.c
@@ -255,7 +255,7 @@ void updateWorld(World * world, Game * game) {
 			entity2 = &world->entities[j];
 
 			// Collided.
-			if (checkEntityCollision(*entity, *entity2))
+			if (checkEntityCollision(entity, entity2))
 				handleCollisionInWorld(entity, entity2);
 		}