1 files changed, 116 insertions, 0 deletions
diff --git a/collision.c b/collision.c
new file mode 100644
index 0000000..27e00b7
--- /dev/null
+++ b/collision.c
@@ -0,0 +1,116 @@
+#include <neofx/collision.h>
+#include <neofx/math.h>
+
+static VERTEX TriangleCollisionPoint(TRIANGLE, VERTEX);
+static VERTEX LineCollisionPoint(VERTEX, VERTEX, VERTEX);
+
+int CollisionPointTriangle(VERTEX p, TRIANGLE t) {
+	VECTOR v1, v2;
+	TRIANGLE triangle = t;
+	
+	if(VectorDot(VectorSub(p, triangle.vertices[0]), triangle.normal) > 0.0) return 0;
+	
+	v1 = VectorCross(VectorSub(triangle.vertices[2], triangle.vertices[1]), VectorSub(p, triangle.vertices[1]));
+	v2 = VectorCross(VectorSub(triangle.vertices[2], triangle.vertices[1]), VectorSub(triangle.vertices[0], triangle.vertices[1]));
+	if(VectorDot(v1, v2) < 0.0) return 0;
+	
+	v1 = VectorCross(VectorSub(triangle.vertices[2], triangle.vertices[0]), VectorSub(p, triangle.vertices[0]));
+	v2 = VectorCross(VectorSub(triangle.vertices[2], triangle.vertices[0]), VectorSub(triangle.vertices[1], triangle.vertices[0]));
+	if(VectorDot(v1, v2) < 0.0) return 0;
+	
+	v1 = VectorCross(VectorSub(triangle.vertices[1], triangle.vertices[0]), VectorSub(p, triangle.vertices[0]));
+	v2 = VectorCross(VectorSub(triangle.vertices[1], triangle.vertices[0]), VectorSub(triangle.vertices[2], triangle.vertices[0]));
+	if(VectorDot(v1, v2) < 0.0) return 0;
+	
+	return 1;
+}
+
+int CollisionRayTriangle(VERTEX p, VECTOR vec,  TRIANGLE t, float* f) {
+	VECTOR pvec, tvec, qvec, edge1, edge2;
+	float det, u, v, invDet;
+	
+	edge1 = VectorSub(t.vertices[1], t.vertices[0]);
+	edge2 = VectorSub(t.vertices[2], t.vertices[0]);
+	
+	pvec = VectorCross(vec, edge2);
+	
+	det = VectorDot(edge1, pvec);
+	
+	if(det < 0.0001) return 0;
+	
+	tvec = VectorSub(p, t.vertices[0]);
+	u = VectorDot(tvec, pvec);
+	if(u < 0.0 || u > det) return 0;
+	
+	qvec = VectorCross(tvec, edge1);
+	v = VectorDot(vec, qvec);
+	if(v < 0.0 || u+v > det) return 0;
+	
+	if(f) {
+		*f = VectorDot(edge2, qvec);
+		invDet = 1.0 / det;
+		*f *= invDet;
+	}
+	
+	return 1;
+}
+
+static VERTEX LineCollisionPoint(VERTEX v1, VERTEX v2, VERTEX p) {
+	VECTOR c;
+	VECTOR V;
+	float d;
+	float t;
+	
+	c = VectorSub(p, v1);
+	V = VectorSub(v2, v1);
+	d = VectorLengthSq(V);
+	t = VectorDot(V, c);
+	
+	if(t < 0) return v1;
+	if(t > d) return v2;
+	
+	return VectorAdd(v1, VectorMul(V, t/d));
+}
+
+static VERTEX TriangleCollisionPoint(TRIANGLE t, VERTEX p) {
+	VERTEX v1, v2, v3;
+	
+	v1 = LineCollisionPoint(t.vertices[0], t.vertices[1], p);
+	v2 = LineCollisionPoint(t.vertices[1], t.vertices[2], p);
+	v3 = LineCollisionPoint(t.vertices[2], t.vertices[0], p);
+	
+	if(VectorLengthSq(VectorSub(v1, p)) < VectorLengthSq(VectorSub(v2, p)) &&
+		VectorLengthSq(VectorSub(v1, p)) < VectorLengthSq(VectorSub(v3, p))) return v1;
+	if(VectorLengthSq(VectorSub(v2, p)) < VectorLengthSq(VectorSub(v3, p))) return v2;
+	return v3;
+}
+
+int CollisionSphereTriangle(VERTEX p, float r, TRIANGLE t) {
+	float f;
+	VERTEX v;
+	
+	/*if(CollisionRayTriangle(VectorAdd(p, VectorMul(VectorNeg(t.normal), r)), m, t, &f)) {*/
+	if(CollisionRayTriangle(p, VectorNeg(t.normal), t, &f)) {
+		if(f < r) return 1;
+		return 0; 
+	}
+	
+	v = TriangleCollisionPoint(t, p);
+	if(VectorLengthSq(VectorSub(p, v)) < r*r) return 1;
+	return 0;
+}
+
+int CollisionMovingSphereTriangle(VERTEX p, float r, VECTOR m, float l, TRIANGLE t) {
+	float f;
+	VERTEX v;
+	
+	if(CollisionRayTriangle(VectorAdd(p, VectorMul(VectorNeg(t.normal), r)), m, t, &f)) {
+		if(f < l) return 1;
+		return 0; 
+	}
+	
+	v = TriangleCollisionPoint(t, p);
+	v = LineCollisionPoint(v, VectorSub(v, VectorMul(m, l)), p);
+	if(VectorLengthSq(VectorSub(p, v)) < r*r) return 1;
+	return 0;
+}