Working collision system

1 files changed, 56 insertions, 0 deletions

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