Sonic fast collision

1 files changed, 187 insertions, 53 deletions

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();