#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;
	
	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) / det;
	
	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;
}