#include <math.h>
#include "vertex3dop.h"

float	operator * ( const VERTEX3D &a, const VERTEX3D &b )
{
	return ( a.x * b.x + a.y * b.y + a.z * b.z );
}

VERTEX3D	operator + ( const VERTEX3D &a, const VERTEX3D &b )
{
	VERTEX3D result;

	result.x = a.x + b.x;
	result.y = a.y + b.y;
	result.z = a.z + b.z;

	return result;
}

VERTEX3D	operator * ( const VERTEX3D &a, const float b )
{
	VERTEX3D result;

	result.x = a.x * b;
	result.y = a.y * b;
	result.z = a.z * b;

	return result;
}

VERTEX3D	operator - ( const VERTEX3D &a, const VERTEX3D &b )
{
	VERTEX3D result;

	result.x = a.x - b.x;
	result.y = a.y - b.y;
	result.z = a.z - b.z;

	return result;
}

void	operator += ( VERTEX3D &a, const VERTEX3D &b )
{
	a.x += b.x;
	a.y += b.y;
	a.z += b.z;
}

VERTEX3D	operator ^ ( const VERTEX3D &a, const VERTEX3D &b )
{
	VERTEX3D result;

	result.x = a.y * b.z - b.y * a.z;
	result.y = a.z * b.x - b.z * a.x;
	result.z = a.x * b.y - b.x * a.y;

	return result;
}

void	operator *= ( VERTEX3D &a, const float b )
{
	a.x *= b;
	a.y *= b;
	a.z *= b;
}

void	operator ~ ( VERTEX3D &a )
{
	float faktor = a * a;

	if ( faktor > 0 ) faktor = (float)1.0 / (float)sqrt( faktor );

	a *= faktor;
}

VERTEX3D	operator * ( const MATRIX44 &matrix, const VERTEX3D &source )
{
	VERTEX3D dest;

	dest.x = source.x * matrix[ 0 + 4 * 0 ] +
             source.y * matrix[ 0 + 4 * 1 ] +
             source.z * matrix[ 0 + 4 * 2 ] + matrix[ 0 + 4 * 3 ];
	dest.y = source.x * matrix[ 1 + 4 * 0 ] +
             source.y * matrix[ 1 + 4 * 1 ] +
             source.z * matrix[ 1 + 4 * 2 ] + matrix[ 1 + 4 * 3 ];
	dest.z = source.x * matrix[ 2 + 4 * 0 ] +
             source.y * matrix[ 2 + 4 * 1 ] +
             source.z * matrix[ 2 + 4 * 2 ] + matrix[ 2 + 4 * 3 ];

	return dest;
}

void	InverseMatrixAnglePreserving( const MATRIX44 source, MATRIX44 dest )
{
    float scale;

    scale = source[ 0 * 4 + 0 ] * source[ 0 * 4 + 0 ] +
            source[ 0 * 4 + 1 ] * source[ 0 * 4 + 1 ] +
            source[ 0 * 4 + 2 ] * source[ 0 * 4 + 2 ];

	if ( scale == 0 ) return;

    scale = (float)1.0 / scale;

    dest[ 0 * 4 + 0 ] = scale * source[ 0 * 4 + 0 ];
    dest[ 1 * 4 + 0 ] = scale * source[ 0 * 4 + 1 ];
    dest[ 2 * 4 + 0 ] = scale * source[ 0 * 4 + 2 ];
    dest[ 0 * 4 + 1 ] = scale * source[ 1 * 4 + 0 ];
    dest[ 1 * 4 + 1 ] = scale * source[ 1 * 4 + 1 ];
    dest[ 2 * 4 + 1 ] = scale * source[ 1 * 4 + 2 ];
    dest[ 0 * 4 + 2 ] = scale * source[ 2 * 4 + 0 ];
    dest[ 1 * 4 + 2 ] = scale * source[ 2 * 4 + 1 ];
    dest[ 2 * 4 + 2 ] = scale * source[ 2 * 4 + 2 ];

    dest[ 0 * 4 + 3 ] = - ( dest[ 0 * 4 + 0 ] * source[ 0 * 4 + 3 ] +
                            dest[ 0 * 4 + 1 ] * source[ 1 * 4 + 3 ] +
                            dest[ 0 * 4 + 2 ] * source[ 2 * 4 + 3 ] );
    dest[ 1 * 4 + 3 ] = - ( dest[ 1 * 4 + 0 ] * source[ 0 * 4 + 3 ] +
                            dest[ 1 * 4 + 1 ] * source[ 1 * 4 + 3 ] +
                            dest[ 1 * 4 + 2 ] * source[ 2 * 4 + 3 ] );
    dest[ 2 * 4 + 3 ] = - ( dest[ 2 * 4 + 0 ] * source[ 0 * 4 + 3 ] +
                            dest[ 2 * 4 + 1 ] * source[ 1 * 4 + 3 ] +
                            dest[ 2 * 4 + 2 ] * source[ 2 * 4 + 3 ] );

    dest[ 3 * 4 + 0 ] = dest[ 3 * 4 + 1] = dest[ 3 * 4 + 2 ] = (float)0.0;
    dest[ 3 * 4 + 3 ] = (float)1.0;

};
