// ===================================================
// MATH.CPP         -=- Focus3 -=-     By Jacco Bikker
// Integer math 3D engine for mobile devices  ><> 2003 
// ===================================================

#include "maths.h"
#include "string.h"

namespace Overloaded {

void Vector::Between( Vector& a_V1, Vector& a_V2, int a_Distance )
{	
	x = a_V1.x + ((a_V2.x - a_V1.x) >> 8) * (a_Distance >> 8);
	y = a_V1.y + ((a_V2.y - a_V1.y) >> 8) * (a_Distance >> 8);
	z = a_V1.z + ((a_V2.z - a_V1.z) >> 8) * (a_Distance >> 8);
}

int Vector::Distance( Plane& p, bool oldmethod )
{	
	if (oldmethod) return Dot( p.GetNormal() ) - p.GetD();
			  else return Dot( p.GetNormal() ) + p.GetD();
}

int Vector::Distance( Vector& v )
{	
	Vector t = Vector( x, y, z ) - v;
	return t.Length();
}

Vector Vector::ClosestPoint( Vector& a_C1, Vector& a_C2 )
{
	Vector v1 = *this - a_C1, v2 = a_C2 - a_C1;
	v2.Normalize();
	int d = a_C1.Distance( a_C2 );
	int t = v1.Dot( v2 );
	if (t <= 0) return a_C1; else if (t >= d) return a_C2; else return a_C1 + v2 * t;
}

void Vector::Normalize() 
{
	double l = (double)Length() / 65536.0f; 
	x = (int)((float)x / l);
	y = (int)((float)y / l);
	z = (int)((float)z / l);
}

int Vector::Length() 
{
	double fx = (double)x, fy = (double)y, fz = (double)z;
	return (int)sqrt( fx * fx + fy * fy + fz * fz );
}

void Vector::Transform( Matrix& m )
{	
	int ox = x, oy = y, oz = z;
	x = FPMUL( ox, m.Get( 0 ) ) + FPMUL( oy, m.Get( 1 ) ) + FPMUL( oz, m.Get( 2 ) ) + m.Get( 3 );
	y = FPMUL( ox, m.Get( 4 ) ) + FPMUL( oy, m.Get( 5 ) ) + FPMUL( oz, m.Get( 6 ) ) + m.Get( 7 );
	z = FPMUL( ox, m.Get( 8 ) ) + FPMUL( oy, m.Get( 9 ) ) + FPMUL( oz, m.Get( 10 ) ) + m.Get( 11 );
}

void Vector::InvTransform( Matrix& m )
{	
	int ox = x + m.Get( 3 ), oy = y + m.Get( 7 ), oz = z + m.Get( 11 );
	x = FPMUL( ox, m.Get( 0 ) ) + FPMUL( oy, m.Get( 1 ) ) + FPMUL( oz, m.Get( 2 ) );
	y = FPMUL( ox, m.Get( 4 ) ) + FPMUL( oy, m.Get( 5 ) ) + FPMUL( oz, m.Get( 6 ) );
	z = FPMUL( ox, m.Get( 8 ) ) + FPMUL( oy, m.Get( 9 ) ) + FPMUL( oz, m.Get( 10 ) );
}

void Matrix::Identity()
{	
	m_Cell[1] = m_Cell[2] = m_Cell[TX] = m_Cell[4] = m_Cell[6] = m_Cell[TY] =
	m_Cell[8] = m_Cell[9] = m_Cell[TZ] = m_Cell[12] = m_Cell[13] = m_Cell[14] =0;
	m_Cell[0] = m_Cell[5] = m_Cell[10] = m_Cell[15] = FPONE;
	m_Virgin = true;
}

void Matrix::LookAt( Vector& a_Eye, Vector& a_At, Vector& a_Up )
{
	Vector dir = a_At - a_Eye;
	dir.Normalize();
	Vector r = dir.Cross( a_Up );
	Vector u = r.Cross( dir );
	m_Cell[0] = r.x;
	m_Cell[1] = r.y;
	m_Cell[2] = r.z;
	m_Cell[3] = 0;
	m_Cell[4] = u.x;
	m_Cell[5] = u.y;
	m_Cell[6] = u.z;
	m_Cell[7] = 0;
	m_Cell[8] = -dir.x;
	m_Cell[9] = -dir.y;
	m_Cell[10] = -dir.z;
	m_Cell[11] = 0;
	m_Virgin = false;
}

void Matrix::Concatenate( Matrix& m2 )
{	
	Matrix res;
	if (m2.m_Virgin)
	{
		Vector V = m2.GetTranslation();
		m_Cell[3] = m_Cell[3] + FPMUL( V.x, m_Cell[0] ) + FPMUL( V.y, m_Cell[1] ) + FPMUL( V.z, m_Cell[2] );
		m_Cell[7] = m_Cell[7] + FPMUL( V.x, m_Cell[4] ) + FPMUL( V.y, m_Cell[5] ) + FPMUL( V.z, m_Cell[6] );
		m_Cell[11] = m_Cell[11] + FPMUL( V.x, m_Cell[8] ) + FPMUL( V.y, m_Cell[9] ) + FPMUL( V.z, m_Cell[10] );
	}
	else
	{
		for( int i = 0; i < 3; i++ )
		{
			for( int j = 0; j < 3; j++ )
			{
				res.m_Cell[i * 4 + j] = 0;
				res.m_Cell[i * 4 + j] += ((m_Cell[i * 4 + 0] >> 1) * (m2.m_Cell[0 * 4 + j] >> 1)) >> 14;
				res.m_Cell[i * 4 + j] += ((m_Cell[i * 4 + 1] >> 1) * (m2.m_Cell[1 * 4 + j] >> 1)) >> 14;
				res.m_Cell[i * 4 + j] += ((m_Cell[i * 4 + 2] >> 1) * (m2.m_Cell[2 * 4 + j] >> 1)) >> 14;
				res.m_Cell[i * 4 + j] += ((m_Cell[i * 4 + 3] >> 1) * (m2.m_Cell[3 * 4 + j] >> 1)) >> 14;
			}
			res.m_Cell[i * 4 + 3] = 0;
			res.m_Cell[i * 4 + 3] += ((m_Cell[i * 4 + 0] >> 4) * (m2.m_Cell[0 * 4 + 3] >> 4)) >> 8;
			res.m_Cell[i * 4 + 3] += ((m_Cell[i * 4 + 1] >> 4) * (m2.m_Cell[1 * 4 + 3] >> 4)) >> 8;
			res.m_Cell[i * 4 + 3] += ((m_Cell[i * 4 + 2] >> 4) * (m2.m_Cell[2 * 4 + 3] >> 4)) >> 8;
			res.m_Cell[i * 4 + 3] += ((m_Cell[i * 4 + 3] >> 6) * (m2.m_Cell[3 * 4 + 3] >> 6)) >> 4;
		}
		for( int j = 0; j < 4; j++ )
		{
			res.m_Cell[12 + j] = 0;
			res.m_Cell[12 + j] += ((m_Cell[12] >> 4) * (m2.m_Cell[0 + j] >> 4)) >> 8;
			res.m_Cell[12 + j] += ((m_Cell[13] >> 4) * (m2.m_Cell[4 + j] >> 4)) >> 8;
			res.m_Cell[12 + j] += ((m_Cell[14] >> 4) * (m2.m_Cell[8 + j] >> 4)) >> 8;
			res.m_Cell[12 + j] += ((m_Cell[15] >> 4) * (m2.m_Cell[12 + j] >> 4)) >> 8;
		}
		memcpy( m_Cell, res.m_Cell, 4 * 16 );
	}
	m_Virgin = false;
}

void Matrix::RotateX( int a_RX, int* a_Sin, int* a_Cos )
{	
	int sx = a_Sin[a_RX];
	int cx = a_Cos[a_RX];
	Identity();
	m_Cell[5] = cx;
	m_Cell[6] = sx;
	m_Cell[9] = -sx;
	m_Cell[10] = cx;
	m_Virgin = false;
}

void Matrix::RotateY( int a_RY, int* a_Sin, int* a_Cos  )
{	
	int sy = a_Sin[a_RY];
	int cy = a_Cos[a_RY];
	Identity();
	m_Cell[0] = cy;
	m_Cell[2] = -sy;
	m_Cell[8] = sy;
	m_Cell[10] = cy;
	m_Virgin = false;
}

void Matrix::RotateZ( int a_RZ, int* a_Sin, int* a_Cos  )
{	
	int sz = a_Sin[a_RZ];
	int cz = a_Cos[a_RZ];
	Identity();
	m_Cell[0] = cz;
	m_Cell[1] = sz;
	m_Cell[4] = -sz;
	m_Cell[5] = cz;
	m_Virgin = false;
}

void Matrix::Rotate( Vector a_Trans, int a_RX, int a_RY, int a_RZ, int* a_Sin, int* a_Cos  )
{	
	Matrix t;
	t.RotateX( a_RZ, a_Sin, a_Cos );
	RotateY( a_RY, a_Sin, a_Cos );
	Concatenate( t );
	t.RotateZ( a_RX, a_Sin, a_Cos );
	Concatenate( t );
	Translate( a_Trans );
}

void Matrix::Invert()
{	
	Matrix t;
	int tx = -m_Cell[3];
	int ty = -m_Cell[7];
	int tz = -m_Cell[11];
	for ( int h = 0; h < 3; h++ ) for ( int v = 0; v < 3; v++ ) t.m_Cell[h + v * 4] = m_Cell[v + h * 4];
	for ( int i = 0; i < 16; i++ ) m_Cell[i] = t.m_Cell[i];
	m_Cell[3] = (((tx >> 4) * (m_Cell[0] >> 4)) >> 8) + (((ty >> 4) * (m_Cell[1] >> 4)) >> 8) + (((tz >> 4) * (m_Cell[2] >> 4)) >> 8);
	m_Cell[7] = (((tx >> 4) * (m_Cell[4] >> 4)) >> 8) + (((ty >> 4) * (m_Cell[5] >> 4)) >> 8) + (((tz >> 4) * (m_Cell[6] >> 4)) >> 8);
	m_Cell[11] = (((tx >> 4) * (m_Cell[8] >> 4)) >> 8) + (((ty >> 4) * (m_Cell[9] >> 4)) >> 8) + (((tz >> 4) * (m_Cell[10] >> 4)) >> 8);
}

Matrix Matrix::GetTransformMatrix()
{
	Matrix retval = *this;
	retval.m_Cell[TX] = retval.m_Cell[TY] = retval.m_Cell[TZ] = 0;
	return retval;
}

Matrix Matrix::GetTranslationMatrix()
{
	Matrix retval;
	retval.Translate( Vector( m_Cell[TX], m_Cell[TY], m_Cell[TZ] ) );
	return retval;
}

Vector Matrix::GetTranslation()
{
	return Vector( m_Cell[TX], m_Cell[TY], m_Cell[TZ] );
}

void Matrix::CheckIdentity()
{
	m_Virgin = ((m_Cell[0] != 1) && (m_Cell[1] != 0) && (m_Cell[2] != 0) &&
				(m_Cell[4] != 0) && (m_Cell[5] != 1) && (m_Cell[6] != 0) &&
				(m_Cell[8] != 0) && (m_Cell[9] != 0) && (m_Cell[10] != 1));
}

void Plane::ABCD( Vector& v1, Vector& v2, Vector& v3)
{	
	Vector a = v2 - v1, b = v3 - v1;
	m_Normal.x = FPMUL( a.y, b.z ) - FPMUL( b.y, a.z );
	m_Normal.y = FPMUL( a.z, b.x ) - FPMUL( b.z, a.x );
	m_Normal.z = FPMUL( a.x, b.y ) - FPMUL( b.x, a.y );
	m_Normal.Normalize();
	m_D = m_Normal.Dot( v2 );
}

Frustum::Frustum()
{
	m_Planes = 0;
	for ( int i = 0; i < 10; i++ ) m_RotPlane[i] = new Plane();
}

Frustum::~Frustum()
{
	for (int i = 0; i < m_Planes; i++ ) delete m_Plane[i];
}

void Frustum::Transform( Matrix& m )
{
	for ( int i = 0; i < m_Planes; i++ )
	{
		Vector V, N = m_Plane[i]->GetNormal();
		Vector T = m.GetTranslation();
		V.x = FPMUL( N.x, m.Get( 0 ) ) + FPMUL( N.y, m.Get( 1 ) ) + FPMUL( N.z, m.Get( 2 ) );
		V.y = FPMUL( N.x, m.Get( 4 ) ) + FPMUL( N.y, m.Get( 5 ) ) + FPMUL( N.z, m.Get( 6 ) );
		V.z = FPMUL( N.x, m.Get( 8 ) ) + FPMUL( N.y, m.Get( 9 ) ) + FPMUL( N.z, m.Get( 10 ) );
		m_RotPlane[i]->SetNormal( V );
		m_RotPlane[i]->SetD( T.Dot( V ) );
	}
}

void Frustum::Reset()
{
	for (int i = 0; i < m_Planes; i++ ) delete m_Plane[i];
	m_Planes = 0;
}

}; // namespace Overloaded

// ===================================================
// EOF