#include <math.h>

#include "math.h"
#include "math-vector.h"
#include "math-quat.h"
#include "math-affine.h"

const vec2 vec2::zero = { 0, 0 };
const vec3 vec3::zero = { 0, 0, 0 };
const vec4 vec4::zero = { 0, 0, 0, 0 };

const quat quat::identity = { 0, 0, 0, 1 };

const affine affine::identity =
{ {
	{ 1, 0, 0 },
	{ 0, 1, 0 },
	{ 0, 0, 1 },
	{ 0, 0, 0 }
} };

quat quat::operator * ( const quat &q ) const
{
	quat ret;

	ret.i = i * q.w + w * q.i + j * q.k - k * q.j;
	ret.j = j * q.w + w * q.j + k * q.i - i * q.k;
	ret.k = k * q.w + w * q.k + i * q.j - j * q.i;
	ret.w = w * q.w - i * q.i - j * q.j - k * q.k;

	return ret;
}

quat& quat::operator *= ( const quat &q )
{
	quat tmp = operator * ( q );
	return (*this = tmp);
}

affine affine::operator * ( const affine &a ) const
{
	affine ret;

	ret.c[0][0] = c[0][0] * a.c[0][0] + c[1][0] * a.c[0][1] + c[2][0] * a.c[0][2];
	ret.c[0][1] = c[0][1] * a.c[0][0] + c[1][1] * a.c[0][1] + c[2][1] * a.c[0][2];
	ret.c[0][2] = c[0][2] * a.c[0][0] + c[1][2] * a.c[0][1] + c[2][2] * a.c[0][2];

	ret.c[1][0] = c[0][0] * a.c[1][0] + c[1][0] * a.c[1][1] + c[2][0] * a.c[1][2];
	ret.c[1][1] = c[0][1] * a.c[1][0] + c[1][1] * a.c[1][1] + c[2][1] * a.c[1][2];
	ret.c[1][2] = c[0][2] * a.c[1][0] + c[1][2] * a.c[1][1] + c[2][2] * a.c[1][2];

	ret.c[2][0] = c[0][0] * a.c[2][0] + c[1][0] * a.c[2][1] + c[2][0] * a.c[2][2];
	ret.c[2][1] = c[0][1] * a.c[2][0] + c[1][1] * a.c[2][1] + c[2][1] * a.c[2][2];
	ret.c[2][2] = c[0][2] * a.c[2][0] + c[1][2] * a.c[2][1] + c[2][2] * a.c[2][2];

	ret.c[3][0] = c[0][0] * a.c[3][0] + c[1][0] * a.c[3][1] + c[2][0] * a.c[3][2] + c[3][0];
	ret.c[3][1] = c[0][1] * a.c[3][0] + c[1][1] * a.c[3][1] + c[2][1] * a.c[3][2] + c[3][1];
	ret.c[3][2] = c[0][2] * a.c[3][0] + c[1][2] * a.c[3][1] + c[2][2] * a.c[3][2] + c[3][2];

	return ret;
}

affine& affine::operator *= ( const affine &a ) 
{
	affine tmp = operator * ( a );
	return (*this = tmp);
}

const vec3& affine::col( int i ) const
{
	return *reinterpret_cast< const vec3* >( c[i] );
}

vec3& affine::col( int i )
{
	return *reinterpret_cast< vec3* >( c[i] );
}

affine quat_to_affine( const quat &q )
{
	affine ret;

	float xx = q.i * q.i;
	float yy = q.j * q.j;
	float zz = q.k * q.k;
	
	float xy = q.i * q.j;
	float xz = q.i * q.k;
	float xw = q.i * q.w;
	float yz = q.j * q.k;
	float yw = q.j * q.w;
	float zw = q.k * q.w;

	ret.c[0][0] = 1.0F - 2.0F * (yy + zz);	ret.c[1][0] = 2.0F * (xy - zw);			ret.c[2][0] = 2.0F * (xz + yw);
	ret.c[0][1] = 2.0F * (xy + zw);			ret.c[1][1] = 1.0F - 2.0F * (xx + zz);	ret.c[2][1] = 2.0F * (yz - xw);
	ret.c[0][2] = 2.0F * (xz - yw);			ret.c[1][2] = 2.0F * (yz + xw);			ret.c[2][2] = 1.0F - 2.0F * (xx + yy);

	ret.c[3][0] = 0.0F;
	ret.c[3][1] = 0.0F;
	ret.c[3][2] = 0.0F;

	return ret;
}