/// <summary>
/// This class stores a single triangular face in a CollisionMesh. This class performs the
/// actual mathematics of detecting and describing collisions between points or boxes and a triangle. 
/// </summary>
public class CollisionTriangle : CollideableElement
{
	/*
	 * CONSTANTS
	 */
	/// <summary>
	/// cosine of 135 degrees
	/// </summary>
	public const float COS_135 = -0.707106781f;
	/// <summary>
	/// cosine of 45 degrees
	/// </summary>
	public const float COS_45 = 0.707106781f;
	/// <summary>
	/// the buffer added to the triangle's bounding box
	/// </summary>
	public const float TRIANGLE_BOUNDS_BUFFER = 0.001f;

	/*
	 * CLASS PROPERTIES
	 */
	/// <summary>
	/// index of the vertices in the accesible mesh structure
	/// </summary>
	private int[] vertexIndeces;
	/// <summary>
	/// the positions of the vertices in world coordinates
	/// </summary>
	private Vector3[] pos;
	/// <summary>
	/// the area of the triangle
	/// </summary>
	public float Area;
	/// <summary>
	/// double the area of the triangle
	/// </summary>
	public float DoubleArea;
	/// <summary>
	/// the normal of the triangle surface
	/// </summary>
	public Vector3 FaceNormal;
	/// <summary>
	/// the centroid of the face
	/// </summary>
	public Vector3 Centroid;
	/// <summary>
	/// the accessible mesh structure this traingle is associated with
	/// </summary>
	private CollisionMesh Mesh;
	/// <summary>
	/// the array of 3 collideable edges of this triangle
	/// </summary>
	private CollisionEdge[] Edges;

	/*
	 * CONSTRUCTOR
	 */
	/// <summary>
	/// constructor for a new mesh triangles
	/// </summary>
	/// <param name="newIndex1">the index in the mesh object of the first vertex of the triangle</param>
	/// <param name="newIndex2">the index in the mesh object of the second vertex of the triangle</param>
	/// <param name="newIndex3">the index in the mesh object of the third vertex of the triangle</param>
	/// <param name="newMesh">the accessible mesh object to which this triangle belongs</param>
	/// <remarks>vertices should be defined in counter-clockwise order</remarks>
	public CollisionTriangle(TwelveCylinderGame newGame, int newIndex1, int newIndex2, int newIndex3, CollisionMesh newMesh)
		: base(newGame)
	{
		Mesh = newMesh;
		pos = new Vector3[3];
		vertexIndeces = new int[3] { newIndex1, newIndex2, newIndex3 };

		updateVertexPositions();
		updateFaceNormal();
		updateCollisionBox();

		//edge definitions
		Edges = new CollisionEdge[3];
		Edges[0] = new CollisionEdge(Mesh, pos[0], pos[1]);
		Edges[1] = new CollisionEdge(Mesh, pos[1], pos[2]);
		Edges[2] = new CollisionEdge(Mesh, pos[2], pos[0]);
	}

	/*
	 * PARAMETER UPDATES
	 */
	/// <summary>
	/// updates vertex positions, face normal
	/// and collision box definitions
	/// </summary>
	public void updateAll()
	{
		updateVertexPositions();
		updateEdges();
		updateFaceNormal();
		updateCollisionBox();
	}

	/// <summary>
	/// updates the vertex world positions
	/// from the collision mesh object
	/// </summary>
	public void updateVertexPositions()
	{
		pos[0] = Mesh.getWorldPosition(vertexIndeces[0]);
		pos[1] = Mesh.getWorldPosition(vertexIndeces[1]);
		pos[2] = Mesh.getWorldPosition(vertexIndeces[2]);
	}

	public void updateEdges()
	{
		Edges[0].WorldPosition1 = pos[0];
		Edges[0].WorldPosition2 = pos[1];
		Edges[0].updateEdge();

		Edges[1].WorldPosition1 = pos[1];
		Edges[1].WorldPosition2 = pos[2];
		Edges[1].updateEdge();

		Edges[2].WorldPosition1 = pos[2];
		Edges[2].WorldPosition2 = pos[0];
		Edges[2].updateEdge();
	}

	/// <summary>
	/// updates the triangles face normal direction
	/// based on its vertex positions
	/// </summary>
	public void updateFaceNormal()
	{
		Vector3 v2 = (pos[2] - pos[0]);
		Vector3 v1 = (pos[1] - pos[0]);

		Vector3 cross = Vector3.Cross(v2, v1);
		DoubleArea = cross.Length();
		Area = DoubleArea / 2.0f;
		Centroid = (pos[0] + pos[1] + pos[2]) / 3.0f;

		FaceNormal = cross / DoubleArea;
	}

	/// <summary>
	/// updates the object oriented
	/// bounding box of this triangle
	/// </summary>
	public override void updateCollisionBox()
	{
		box = Bounds.FromPoints(curGame, CollisionTriangle.TRIANGLE_BOUNDS_BUFFER, pos[0], pos[1], pos[2]);
	}

	/*
	 * EDGE-EDGE INTERSECTION
	 */
	/// <summary>
	/// intersect BoxEdge moving in direction (dir) colliding with edge (p3,p4) 
	/// return true on a collision with collision distance (dist) and intersection point (ip)
	/// </summary>
	/// <param name="boxEdge">the moving box edge definition</param>
	/// <param name="faceEdge">the static edge of the triangle</param>
	/// <param name="paramT">the output distance from the box start position at which the collision occured</param>
	/// <param name="intersectPos">the output position at which the intersection occured</param>
	/// <returns>true if a collision occured, false otherwise</returns>
	public static bool EdgeIntersect(BoxEdge boxEdge, CollisionEdge faceEdge,
											out float paramT, out Vector3 intersectPos)
	{
		paramT = 0;
		intersectPos = Vector3.Zero;

		// if colliding edge (p3,p4) does not cross plane return no collision
		// same as if p3 and p4 on same side of plane return 0
		float temp = (Vector3.Dot(boxEdge.PlaneDir, faceEdge.WorldPosition1) - boxEdge.PlaneW) 
						* (Vector3.Dot(boxEdge.PlaneDir, faceEdge.WorldPosition2) - boxEdge.PlaneW);
		if (temp > 0)
			return false;

		// if colliding edge (p3,p4) and plane are paralell return no collision
		//v2.Normalize();
		temp = Vector3.Dot(boxEdge.PlaneDir, faceEdge.EdgeDir);
		if (temp == 0)
			return false;

		// compute intersection point of plane and colliding edge (p3,p4)
		intersectPos = faceEdge.WorldPosition1 + (faceEdge.EdgeDir *
					((boxEdge.PlaneW - Vector3.Dot(boxEdge.PlaneDir, faceEdge.WorldPosition1)) / temp));

		uint i = XNAHelper.GetLargestAbsComponent(boxEdge.PlaneDir);

		// remove a component from each vector to simply to a 2D problem
		Vector2 p12d = XNAHelper.Vector3RemoveComponent(boxEdge.WorldPosition1, i);
		Vector2 v12d = XNAHelper.Vector3RemoveComponent(boxEdge.EdgeVector, i);
		Vector2 ip2d = XNAHelper.Vector3RemoveComponent(intersectPos, i);
		Vector2 dir2d = XNAHelper.Vector3RemoveComponent(boxEdge.EdgeMotion, i);

		// compute distance of intersection from line to line
		paramT = (v12d.X * (ip2d.Y - p12d.Y) - v12d.Y * (ip2d.X - p12d.X)) / (v12d.X * dir2d.Y - v12d.Y * dir2d.X);
		if (paramT < 0)
			return false;

		// compute intesection point along edge path
		intersectPos -= paramT * boxEdge.EdgeMotion;

		// check if intersection point is between egde  vertices
		temp = Vector3.Dot(boxEdge.WorldPosition1 - intersectPos, boxEdge.WorldPosition2 - intersectPos);
		if (temp >= 0)
			return false; // no collision

		return true; // collision found!
	}

	/*
	 * RAY-TRIANGLE INTERSECT
	 */
	/// <summary>
	/// ray-triangle intersect from
	/// http://www.graphics.cornell.edu/pubs/1997/MT97.pdf 
	/// </summary>
	/// <param name="rayOrigin"> origin of the ray</param>
	/// <param name="rayDirection">direction of the ray</param>
	/// <param name="dist">output distance to intersection</param>
	/// <returns></returns>
	public bool RayTriangleIntersect(Vector3 rayOrigin, Vector3 rayDirection, out float dist)
	{
		Vector3 edge1 = pos[1] - pos[0];
		Vector3 edge2 = pos[2] - pos[0];

		Vector3 tVector, pVector, qVector;
		float det, invDet;
		dist = 0; //parametric distance
		pVector = Vector3.Cross(rayDirection, edge2);

		det = Vector3.Dot(edge1, pVector);

		if (det > -0.00001f)
			//ray is parallel to triangle
			return false

		invDet = 1.0f / det;

		tVector = rayOrigin - pos[0];

		float u = Vector3.Dot(tVector, pVector) * invDet;
		if (u < -0.0001f || u > 1.0001f)
			//intersection with plane of triangle is off the face
			return false;

		qVector = Vector3.Cross(tVector, edge1);

		float v = Vector3.Dot(rayDirection, qVector) * invDet;
		if (v < -0.0001f || u + v > 1.0001f)
			//intersection with plane of triangle is off the face
			return false;

		//intersect distance
		dist = Vector3.Dot(edge2, qVector) * invDet;

		if (dist <= 0)
			//intersection is behind ray origin
			return false;

		return true; //intersection occured
	}

	/*
	 * COLLIDEABLE ELEMENT INTERSECTION TESTS
	 */
	/// <summary>
	/// ray intersect face and return
	/// intersection distance, point and normal 
	/// </summary>
	/// <param name="pointData">point movement data that
	/// describes movement of point to test for intersection</param>
	public override void PointIntersect(ref MovingPointData pointData)
	{
		float dist;
		if (this.RayTriangleIntersect(pointData.StartPosition, pointData.MoveDirection, out dist))
		{
			if (dist < pointData.CollisionDistance)
				pointData.CollisionOccured(this, pointData.StartPosition +
					(pointData.MoveDirection * dist), this.FaceNormal, dist);
		}
	}

	/// <summary>
	/// box intersect face and return intersection distance, point and normal 
	/// </summary>
	/// <param name="boxData"></param>
	/// <param name="collisionDistance">output distance to intersection</param>
	/// <param name="collisionPosition">output position of intersection</param>
	/// <param name="collisionNormal">output normal of intersection</param>
	/// <returns>true if an intersection occured</returns>
	public override bool BoxIntersect(MovingBoxData boxData, out float collisionDistance,
		out Vector3 collisionPosition, out Vector3 collisionNormal)
	{
		collisionDistance = float.MaxValue;
		collisionPosition = boxData.WorldStart;
		collisionNormal = Vector3.Zero;
		bool collided = false;

		//cull triangle if we're moving in the same direction as the triangle's normal
		if (Vector3.Dot(this.FaceNormal, boxData.OffsetDirection) > 0)
			return false;

		uint i, j;
		float distance;
		Vector3 position;
		float tnear, tfar;

		/*
		 * EDGE TO EDGE
		 */
		for (i = 0; i < 12; i++)
		{
			//for each edge of the box

			// skip edges with normal more than 135 degrees away from moving direction
			if (Vector3.Dot(Bounds.edgeNormals[i], boxData.OffsetDirection)
													< CollisionTriangle.COS_135)
				continue;

			for (j = 0; j < Edges.Length; j++)
			{
				//for each edge of the triangle test for intersection with the current box edge
				if (CollisionTriangle.EdgeIntersect(boxData.BoxEdges[i], Edges[j],
															out distance, out position))
				{
					//intersection occured
					if (distance < collisionDistance)
					{
						//intersection is closer than previous distance
						collisionDistance = distance;
						collisionPosition = position;
						collisionNormal = Vector3.Normalize(Vector3.Cross(boxData.BoxEdges[i].WorldPosition2 -
							boxData.BoxEdges[i].WorldPosition1, -Edges[j].EdgeVector));

						if (Vector3.Dot(boxData.OffsetDirection, collisionNormal) > 0)
							collisionNormal = -collisionNormal;
						collided = true;
					}
				}
			}
		}

		/*
		 * TRIANGLE VERTICES WITH BOX
		 */
		BoxFace boxFace;
		for (i = 0; i < 3; i++)
		{
			//for each triangle vertex...
			//intersect with collision box
			boxFace = boxData.WorldBox.RayIntersect(new OptimizedRay(pos[i], -boxData.OffsetDirection),
												out tnear, out tfar);
			if (boxFace != BoxFace.None)
			{
				//ray hits the bounds
				if (tnear < collisionDistance)
				{
					//closest distance
					collisionDistance = tnear;
					collisionPosition = pos[i];
					collisionNormal = -Bounds.faceNormals[(int)boxFace];
					collided = true;
				}
			}
		}

		/*
		 * BOX VERTICES WITH TRIANGLE
		 */
		float dist;
		for (i = 0; i < 8; i++)
		{
			// cull vertices with normal more than 135 degree from moving direction
			if (Vector3.Dot(Bounds.vertexNormals[i], boxData.OffsetDirection) < CollisionTriangle.COS_135)
				continue;

			if (this.RayTriangleIntersect(boxData.WorldBoxVerts[i], 
									boxData.OffsetDirection, out dist))
			{
				if (dist < collisionDistance)
				{
					collisionDistance = dist;
					collisionPosition = (boxData.OffsetDirection * dist) +
											boxData.WorldBoxVerts[i];
					collisionNormal = this.FaceNormal; //normal of triangle
					collided = true;
				}
			}
		}

		return collided;
	}
}