/******************************************************************************
	libx42 - skinned vertex animation library
	Copyright (C) 2007 HermitWorks Entertainment Corporation

	This program is free software; you can redistribute it and/or modify it
	under the terms of the GNU General Public License as published by the Free
	Software Foundation; either version 2 of the License, or (at your option)
	any later version.

	This program is distributed in the hope that it will be useful, but
	WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
	or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
	for more details.

	You should have received a copy of the GNU General Public License along
	with this program; if not, write to the
	
		Free Software Foundation, Inc.
		51 Franklin Street, Fifth Floor
		Boston, MA  02110-1301, USA.
******************************************************************************/

#ifndef INC_MATH_UTIL_H
#define INC_MATH_UTIL_H

NOGLOBALALIAS ALWAYS_INLINE float rsqrt( float x )
{
	long i;
	float y, r;

	y = x * 0.5F;

	i = *(long*)&x;
	i = 0x5F375A86 - (i >> 1);
	r = *(float*)&i;
	
	//need at least two steps, things get twitchy with just one
	r = r * (1.5F - r * r * y);
	r = r * (1.5F - r * r * y);
	
	return r;
}

NOGLOBALALIAS void quat_mul( float *o, const float *a, const float *b );
NOGLOBALALIAS void quat_interp( float *o, const float *a, const float *b, float t );

NOGLOBALALIAS ALWAYS_INLINE void quat_cpy( float *o, const float *q )
{
	o[0] = q[0];
	o[1] = q[1];
	o[2] = q[2];
	o[3] = q[3];
}

NOGLOBALALIAS ALWAYS_INLINE void vec3_add( float *o, const float *a, const float *b )
{
	o[0] = a[0] + b[0];
	o[1] = a[1] + b[1];
	o[2] = a[2] + b[2];
}

NOGLOBALALIAS ALWAYS_INLINE void vec3_mul( float *o, const float *a, const float *b )
{
	o[0] = a[0] * b[0];
	o[1] = a[1] * b[1];
	o[2] = a[2] * b[2];
}

NOGLOBALALIAS ALWAYS_INLINE void vec3_scale( float *o, const float *v, float s )
{
	o[0] = v[0] * s;
	o[1] = v[1] * s;
	o[2] = v[2] * s;
}

NOGLOBALALIAS ALWAYS_INLINE void vec3_lerp( float *o, const float *a, const float *b, float t )
{
	float it = 1.0F - t;

	o[0] = a[0] * it + b[0] * t;
	o[1] = a[1] * it + b[1] * t;
	o[2] = a[2] * it + b[2] * t;
}

NOGLOBALALIAS ALWAYS_INLINE void vec3_cpy( float *o, const float *v )
{
	o[0] = v[0];
	o[1] = v[1];
	o[2] = v[2];
}

NOGLOBALALIAS ALWAYS_INLINE void vec3_cross( float *o, const float *a, const float *b )
{
	o[0] = a[1] * b[2] - a[2] * b[1];
	o[1] = a[2] * b[0] - a[0] * b[2];
	o[2] = a[0] * b[1] - a[1] * b[0];
}

NOGLOBALALIAS ALWAYS_INLINE float vec3_lensq( float *v )
{
	return v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
}

NOGLOBALALIAS ALWAYS_INLINE void vec4_lerp( float *o, const float *a, const float *b, float t )
{
	float it = 1.0F - t;

	o[0] = a[0] * it + b[0] * t;
	o[1] = a[1] * it + b[1] * t;
	o[2] = a[2] * it + b[2] * t;
	o[3] = a[3] * it + b[3] * t;
}

NOGLOBALALIAS void affine_mul( affine_t *o, const affine_t *a, const affine_t *b );
NOGLOBALALIAS void quat_to_affine( affine_t *o, const quat_t q );

NOGLOBALALIAS void affine_scale( affine_t *o, const affine_t *a, float s );
NOGLOBALALIAS void affine_mad( affine_t *o, const affine_t *m, float s, const affine_t *a );
NOGLOBALALIAS void affine_mul_vec( vec3_t o, const affine_t *m, const vec3_t v );
NOGLOBALALIAS void affine_mul_pos( vec3_t o, const affine_t *m, const vec3_t p );

NOGLOBALALIAS ALWAYS_INLINE void affine_cpy( affine_t *o, const affine_t *i )
{
	memcpy( o, i, sizeof( affine_t ) );
}

#endif