//
// Math and Linear Algebra stuff.
//

#include "defines.h"
#include "algebra.h"

// ============================================================================
// 4x4 Matrix class.

void Matrix44::CreateLookAt(const Vector3& Eye, const Vector3& Target, const Vector3& Up)
{
	Vector3 x, y, z;

	// Z = Eye - Target
	z = Eye - Target;

  // X = Y Cross Z
	x = Cross3(Up, z);

  // Y = Z Cross X
	y = Cross3(z, x);

	// Normalize everything.
	x.Normalize();
	y.Normalize();
	z.Normalize();

	m_Rows[0] = Vector4(x[0], y[0], z[0], 0.0f);
	m_Rows[1] = Vector4(x[1], y[1], z[1], 0.0f);
	m_Rows[2] = Vector4(x[2], y[2], z[2], 0.0f);
	m_Rows[3] = m_Rows[0]*-Eye[0] + m_Rows[1]*-Eye[1] + m_Rows[2]*-Eye[2];
	m_Rows[3][3] = 1.0f;
}

void Matrix44::CreatePerspective(float FoVy, float Aspect, float Near, float Far)
{
	float Left, Right, Bottom, Top;

	Top = Near * (float)tan(FoVy * LC_PI / 360.0f);
	Bottom = -Top;

	Left = Bottom * Aspect;
	Right = Top * Aspect;

	if ((Near <= 0.0f) || (Far <= 0.0f) || (Near == Far) || (Left == Right) || (Top == Bottom))
		return;

	float x, y, a, b, c, d;

	x = (2.0f * Near) / (Right - Left);
	y = (2.0f * Near) / (Top - Bottom);
	a = (Right + Left) / (Right - Left);
	b = (Top + Bottom) / (Top - Bottom);
	c = -(Far + Near) / (Far - Near);
	d = -(2.0f * Far * Near) / (Far - Near);

	m_Rows[0] = Vector4(x, 0, 0,  0);
	m_Rows[1] = Vector4(0, y, 0,  0);
	m_Rows[2] = Vector4(a, b, c, -1);
	m_Rows[3] = Vector4(0, 0, d,  0);
}

// Inverse code from the GLU library.
Matrix44 Inverse(const Matrix44& m)
{
#define SWAP_ROWS(a, b) { float *_tmp = a; (a)=(b); (b)=_tmp; }
#define MAT(m,c,r) m.m_Rows[r][c]

	float wtmp[4][8];
	float m0, m1, m2, m3, s;
	float *r0, *r1, *r2, *r3;

	r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];

	r0[0] = MAT(m,0,0), r0[1] = MAT(m,0,1),
	r0[2] = MAT(m,0,2), r0[3] = MAT(m,0,3),
	r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0,

	r1[0] = MAT(m,1,0), r1[1] = MAT(m,1,1),
	r1[2] = MAT(m,1,2), r1[3] = MAT(m,1,3),
	r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0,

	r2[0] = MAT(m,2,0), r2[1] = MAT(m,2,1),
	r2[2] = MAT(m,2,2), r2[3] = MAT(m,2,3),
	r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0,

	r3[0] = MAT(m,3,0), r3[1] = MAT(m,3,1),
	r3[2] = MAT(m,3,2), r3[3] = MAT(m,3,3),
	r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0;

	// choose pivot - or die
	if (fabs(r3[0])>fabs(r2[0])) SWAP_ROWS(r3, r2);
	if (fabs(r2[0])>fabs(r1[0])) SWAP_ROWS(r2, r1);
	if (fabs(r1[0])>fabs(r0[0])) SWAP_ROWS(r1, r0);
//	if (0.0 == r0[0])  return GL_FALSE;

	// eliminate first variable
	m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0];
	s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s;
	s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s;
	s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s;
	s = r0[4];
	if (s != 0.0) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; }
	s = r0[5];
	if (s != 0.0) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; }
	s = r0[6];
	if (s != 0.0) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; }
	s = r0[7];
	if (s != 0.0) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; }

	// choose pivot - or die
	if (fabs(r3[1])>fabs(r2[1])) SWAP_ROWS(r3, r2);
	if (fabs(r2[1])>fabs(r1[1])) SWAP_ROWS(r2, r1);
//	if (0.0 == r1[1])  return GL_FALSE;

	// eliminate second variable
	m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1];
	r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2];
	r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3];
	s = r1[4]; if (0.0 != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; }
	s = r1[5]; if (0.0 != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; }
	s = r1[6]; if (0.0 != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; }
	s = r1[7]; if (0.0 != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; }

	// choose pivot - or die
	if (fabs(r3[2])>fabs(r2[2])) SWAP_ROWS(r3, r2);
//	if (0.0 == r2[2])  return GL_FALSE;

	// eliminate third variable
	m3 = r3[2]/r2[2];
	r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4],
	r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6],
	r3[7] -= m3 * r2[7];

	// last check
//	if (0.0 == r3[3]) return GL_FALSE;

	s = 1.0f/r3[3];              // now back substitute row 3
	r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s;

	m2 = r2[3];                 // now back substitute row 2
	s  = 1.0f/r2[2];
	r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2),
	r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2);
	m1 = r1[3];
	r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1,
	r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
	m0 = r0[3];
	r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0,
	r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;

	m1 = r1[2];                 // now back substitute row 1
	s  = 1.0f/r1[1];
	r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1),
	r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1);
	m0 = r0[2];
	r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0,
	r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;

	m0 = r0[1];                 // now back substitute row 0
	s  = 1.0f/r0[0];
	r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0),
	r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0);

	Vector4 Row0(r0[4], r1[4], r2[4], r3[4]);
	Vector4 Row1(r0[5], r1[5], r2[5], r3[5]);
	Vector4 Row2(r0[6], r1[6], r2[6], r3[6]);
	Vector4 Row3(r0[7], r1[7], r2[7], r3[7]);
	
	Matrix44 out(Row0, Row1, Row2, Row3);
	
	return out;

#undef MAT
#undef SWAP_ROWS
}

// ============================================================================
// Project/Unproject a point.

// Convert world coordinates to screen coordinates.
Vector3 ProjectPoint(const Vector3& Pt, const Matrix44& ModelView, const Matrix44& Projection, const int Viewport[4])
{
	Vector4 Tmp;

	Tmp = Mul4(Vector4(Pt[0], Pt[1], Pt[2], 1.0f), ModelView);
	Tmp = Mul4(Tmp, Projection);

	// Normalize.
	Tmp /= Tmp[3];

	// Screen coordinates.
	return Vector3(Viewport[0]+(1+Tmp[0])*Viewport[2]/2, Viewport[1]+(1+Tmp[1])*Viewport[3]/2, (1+Tmp[2])/2);
}

void ProjectPoints(Vector3* Points, int NumPoints, const Matrix44& ModelView, const Matrix44& Projection, const int Viewport[4])
{
	for (int i = 0; i < NumPoints; i++)
	{
		Vector4 Tmp;

		Tmp = Mul4(Vector4(Points[i][0], Points[i][1], Points[i][2], 1.0f), ModelView);
		Tmp = Mul4(Tmp, Projection);

		// Normalize.
		Tmp /= Tmp[3];

		// Screen coordinates.
		Points[i] = Vector3(Viewport[0]+(1+Tmp[0])*Viewport[2]/2, Viewport[1]+(1+Tmp[1])*Viewport[3]/2, (1+Tmp[2])/2);
	}
}

// Convert screen coordinates to world coordinates.
Vector3 UnprojectPoint(const Vector3& Point, const Matrix44& ModelView, const Matrix44& Projection, const int Viewport[4])
{
	Vector3 Tmp = Point;
	UnprojectPoints(&Tmp, 1, ModelView, Projection, Viewport);
	return Tmp;
}

void UnprojectPoints(Vector3* Points, int NumPoints, const Matrix44& ModelView, const Matrix44& Projection, const int Viewport[4])
{
	// Calculate the screen to model transform.
	Matrix44 Transform = Inverse(Mul(ModelView, Projection));

	for (int i = 0; i < NumPoints; i++)
	{
		Vector4 Tmp;

		// Convert the point to homogeneous coordinates.
		Tmp[0] = (Points[i][0] - Viewport[0]) * 2.0f / Viewport[2] - 1.0f;
		Tmp[1] = (Points[i][1] - Viewport[1]) * 2.0f / Viewport[3] - 1.0f;
		Tmp[2] = Points[i][2] * 2.0f - 1.0f;
		Tmp[3] = 1.0f;

		Tmp = Mul4(Tmp, Transform);

		if (Tmp[3] != 0.0f)
			Tmp /= Tmp[3];

		Points[i] = Vector3(Tmp[0], Tmp[1], Tmp[2]);
	}
}

// ============================================================================
// Geometry functions.

// Sutherland-Hodgman method of clipping a polygon to a plane.
void PolygonPlaneClip(Vector3* InPoints, int NumInPoints, Vector3* OutPoints, int* NumOutPoints, const Vector4& Plane)
{
	Vector3 *s, *p, i;

	*NumOutPoints = 0;
	s = &InPoints[NumInPoints-1];

	for (int j = 0; j < NumInPoints; j++)
	{
		p = &InPoints[j];

		if (Dot3(*p, Plane) + Plane[3] <= 0)
		{
			if (Dot3(*s, Plane) + Plane[3] <= 0)
			{
				// Both points inside.
				OutPoints[*NumOutPoints] = *p;
				*NumOutPoints = *NumOutPoints + 1;
			}
			else
			{
				// Outside, inside.
				LinePlaneIntersection(i, *s, *p, Plane);

				OutPoints[*NumOutPoints] = i;
				*NumOutPoints = *NumOutPoints + 1;
				OutPoints[*NumOutPoints] = *p;
				*NumOutPoints = *NumOutPoints + 1;
			}
		}
		else
		{
			if (Dot3(*s, Plane) + Plane[3] <= 0)
			{
				// Inside, outside.
				LinePlaneIntersection(i, *s, *p, Plane);

				OutPoints[*NumOutPoints] = i;
				*NumOutPoints = *NumOutPoints + 1;
			}
		}

		s = p;
	}
}

// Calculate the intersection of a line segment and a plane and returns false
// if they are parallel or the intersection is outside the line segment.
bool LinePlaneIntersection(Vector3& Intersection, const Vector3& Start, const Vector3& End, const Vector4& Plane)
{
	Vector3 Dir = End - Start;

	float t1 = Dot3(Plane, Start) + Plane[3];
	float t2 = Dot3(Plane, Dir);

	if (t2 == 0.0f)
		return false;

	float t = -t1 / t2;

	Intersection = Start + t * Dir;

	if ((t < 0.0f) || (t > 1.0f))
		return false;

	return true;
}

bool LineTriangleMinIntersection(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& Start, const Vector3& End, float& MinDist, Vector3& Intersection)
{
	// Calculate the polygon plane.
	Vector4 Plane;
	Plane = Cross3(p1 - p2, p3 - p2);
	Plane[3] = -Dot3(Plane, p1);

	// Check if the line is parallel to the plane.
	Vector3 Dir = End - Start;

	float t1 = Dot3(Plane, Start) + Plane[3];
	float t2 = Dot3(Plane, Dir);

	if (t2 == 0)
		return false;

	float t = -(t1 / t2);

	if (t < 0)
		return false;

	// Intersection of the plane and line segment.
	Intersection = Start - (t1 / t2) * Dir;

	float Dist = (Start - Intersection).Length();

	if (Dist > MinDist)
		return false;

	// Check if we're inside the triangle.
	Vector3 pa1, pa2, pa3;
	pa1 = (p1 - Intersection).Normalize();
	pa2 = (p2 - Intersection).Normalize();
	pa3 = (p3 - Intersection).Normalize();

	float a1, a2, a3;
	a1 = Dot3(pa1, pa2);
	a2 = Dot3(pa2, pa3);
	a3 = Dot3(pa3, pa1);

	float total = (acosf(a1) + acosf(a2) + acosf(a3)) * RTOD;

	if (fabs(total - 360) <= 0.001f)
	{
		MinDist = Dist;
		return true;
	}

	return false;
}

bool LineQuadMinIntersection(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& p4, const Vector3& Start, const Vector3& End, float& MinDist, Vector3& Intersection)
{
	// Calculate the polygon plane.
	Vector4 Plane;
	Plane = Cross3(p1 - p2, p3 - p2);
	Plane[3] = -Dot3(Plane, p1);

	// Check if the line is parallel to the plane.
	Vector3 Dir = End - Start;

	float t1 = Dot3(Plane, Start) + Plane[3];
	float t2 = Dot3(Plane, Dir);

	if (t2 == 0)
		return false;

	float t = -(t1 / t2);

	if (t < 0)
		return false;

	// Intersection of the plane and line segment.
	Intersection = Start - (t1 / t2) * Dir;

	float Dist = (Start - Intersection).Length();

	if (Dist > MinDist)
		return false;

	// Check if we're inside the triangle.
	Vector3 pa1, pa2, pa3;
	pa1 = (p1 - Intersection).Normalize();
	pa2 = (p2 - Intersection).Normalize();
	pa3 = (p3 - Intersection).Normalize();

	float a1, a2, a3;
	a1 = Dot3(pa1, pa2);
	a2 = Dot3(pa2, pa3);
	a3 = Dot3(pa3, pa1);

	float total = (acosf(a1) + acosf(a2) + acosf(a3)) * RTOD;

	if (fabs(total - 360) <= 0.001f)
	{
		MinDist = Dist;
		return true;
	}

	// Check if we're inside the second triangle.
	pa2 = (p4 - Intersection).Normalize();

	a1 = Dot3(pa1, pa2);
	a2 = Dot3(pa2, pa3);
	a3 = Dot3(pa3, pa1);

	total = (acosf(a1) + acosf(a2) + acosf(a3)) * RTOD;
			
	if (fabs(total - 360) <= 0.001f)
	{
		MinDist = Dist;
		return true;
	}

	return false;
}
