/*
 *			GPAC - MPEG-4 Systems C Development Kit
 *
 *			Copyright (c) Jean Le Feuvre 2000-2005 
 *					All rights reserved
 *
 *  This file is part of GPAC / Scene Rendering sub-project
 *
 *  GPAC 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, or (at your option)
 *  any later version.
 *   
 *  GPAC 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 GNU Make; see the file COPYING.  If not, write to
 *  the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA. 
 *
 */

#include <gpac/m4_math.h>

#ifdef M4_FIXED_POINT

/* check if dimension is larger than FIX_ONE*/
#define IS_HIGH_DIM(_v)	((_v > FIX_ONE) || (_v < (s32)0xFFFF0000))
/* check if any vector dimension is larger than FIX_ONE*/
#define VEC_HIGH_MAG(_v)	(IS_HIGH_DIM(_v.x) || IS_HIGH_DIM(_v.y) || IS_HIGH_DIM(_v.z) )

Fixed m4_vec_len(M4Vec v)
{
#if 1
	/*high-magnitude vector, use low precision on frac part to avoid overflow*/
	if (VEC_HIGH_MAG(v)) {
		v.x>>=8;
		v.y>>=8;
		v.z>>=8;
		return m4_sqrt( m4_mulfix(v.x, v.x) + m4_mulfix(v.y, v.y) + m4_mulfix(v.z, v.z) ) << 8;
	}
	/*low-res vector*/
	else {
		return m4_sqrt( m4_mulfix(v.x, v.x) + m4_mulfix(v.y, v.y) + m4_mulfix(v.z, v.z) );
	}
#else
	Float x, y, z;
	x = FIX2FLT(v.x);
	y = FIX2FLT(v.y);
	z = FIX2FLT(v.z);
	return FLT2FIX(sqrt(x*x + y*y + z*z));
#endif
}

Fixed m4_vec_lensq(M4Vec v)
{
	/*high-magnitude vector, use low precision on frac part to avoid overflow*/
	if (VEC_HIGH_MAG(v)) {
		v.x>>=8;
		v.y>>=8;
		v.z>>=8;
		return ( m4_mulfix(v.x, v.x) + m4_mulfix(v.y, v.y) + m4_mulfix(v.z, v.z) ) << 16;
	} else {
		return m4_mulfix(v.x, v.x) + m4_mulfix(v.y, v.y) + m4_mulfix(v.z, v.z);
	}
}

Fixed m4_vec_dot(M4Vec v1, M4Vec v2)
{
#if 1
	/*both high-magnitude vectors, use low precision on frac part to avoid overflow
	if only one is, the dot product should still be in proper range*/
	if (VEC_HIGH_MAG(v1) && VEC_HIGH_MAG(v2)) {
		v1.x>>=8; v1.y>>=8; v1.z>>=8;
		v2.x>>=8; v2.y>>=8; v2.z>>=8;
		return ( m4_mulfix(v1.x, v2.x) + m4_mulfix(v1.y, v2.y) + m4_mulfix(v1.z, v2.z) ) << 16;
	} else {
		return m4_mulfix(v1.x, v2.x) + m4_mulfix(v1.y, v2.y) + m4_mulfix(v1.z, v2.z);
	}
#else
	Float v1x, v1y, v1z, v2x, v2y, v2z;
	v1x = FIX2FLT(v1.x);
	v1y = FIX2FLT(v1.y);
	v1z = FIX2FLT(v1.z);
	v2x = FIX2FLT(v2.x);
	v2y = FIX2FLT(v2.y);
	v2z = FIX2FLT(v2.z);
	return FLT2FIX(v1x*v2x + v1y*v2y + v1z*v2z);
#endif
}

void m4_vec_norm(M4Vec *v)
{
	Fixed __res = m4_vec_len(*v);
	v->x = m4_divfix(v->x, __res); 
	v->y = m4_divfix(v->y, __res);
	v->z = m4_divfix(v->z, __res);
}

M4Vec m4_vec_scale(M4Vec v, Fixed f)
{
	M4Vec res;
	res.x = m4_mulfix(v.x, f); 
	res.y = m4_mulfix(v.y, f); 
	res.z = m4_mulfix(v.z, f);
	return res;
}

M4Vec m4_vec_cross(M4Vec v1, M4Vec v2)
{
	M4Vec res;
#if 1
	/*both high-magnitude vectors, use low precision on frac part to avoid overflow
	if only one is, the cross product should still be in proper range*/
	if (VEC_HIGH_MAG(v1) && VEC_HIGH_MAG(v2)) {
		v1.x>>=8; v1.y>>=8; v1.z>>=8;
		v2.x>>=8; v2.y>>=8; v2.z>>=8;
		res.x = m4_mulfix(v1.y, v2.z) - m4_mulfix(v2.y, v1.z);
		res.y = m4_mulfix(v2.x, v1.z) - m4_mulfix(v1.x, v2.z);
		res.z = m4_mulfix(v1.x, v2.y) - m4_mulfix(v2.x, v1.y);
		res.x<<=16;
		res.y<<=16;
		res.z<<=16;
	} else {
		res.x = m4_mulfix(v1.y, v2.z) - m4_mulfix(v2.y, v1.z);
		res.y = m4_mulfix(v2.x, v1.z) - m4_mulfix(v1.x, v2.z);
		res.z = m4_mulfix(v1.x, v2.y) - m4_mulfix(v2.x, v1.y);
	}
#else
	Float v1x, v1y, v1z, v2x, v2y, v2z;
	v1x = FIX2FLT(v1.x);
	v1y = FIX2FLT(v1.y);
	v1z = FIX2FLT(v1.z);
	v2x = FIX2FLT(v2.x);
	v2y = FIX2FLT(v2.y);
	v2z = FIX2FLT(v2.z);
	res.x = FLT2FIX( v1y*v2z - v2y*v1z);
	res.y = FLT2FIX( v2x*v1z - v1x*v2z);
	res.z = FLT2FIX( v1x*v2y - v2x*v1y);
#endif
	return res;
}

#else

Fixed m4_vec_len(M4Vec v) { return m4_sqrt(v.x*v.x + v.y*v.y + v.z*v.z); }
Fixed m4_vec_lensq(M4Vec v) { return v.x*v.x + v.y*v.y + v.z*v.z; }
Fixed m4_vec_dot(M4Vec v1, M4Vec v2) { return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z; }
void m4_vec_norm(M4Vec *v)
{
	Fixed __res = m4_vec_len(*v);
	if (__res ) __res = 1.0f/__res ;
	v->x *= __res; 
	v->y *= __res;
	v->z *= __res;
}

M4Vec m4_vec_scale(M4Vec v, Fixed f)
{
	M4Vec res = v;
	res.x *= f; 
	res.y *= f; 
	res.z *= f;
	return res;
}

M4Vec m4_vec_cross(M4Vec v1, M4Vec v2)
{
	M4Vec res;
	res.x = v1.y*v2.z - v2.y*v1.z;
	res.y = v2.x*v1.z - v1.x*v2.z;
	res.z = v1.x*v2.y - v2.x*v1.y;
	return res;
}

#endif


void m4_mx2d_from_mx(M4Matrix2D *mat2D, M4Matrix *mat)
{
	m4_mx2d_init(*mat2D);
	mat2D->m[0] = mat->m[0];
	mat2D->m[1] = mat->m[4];
	mat2D->m[2] = mat->m[12];
	mat2D->m[3] = mat->m[1];
	mat2D->m[4] = mat->m[5];
	mat2D->m[5] = mat->m[13];
}

void m4_mx_apply_rect(M4Matrix *mat, M4Rect *rc)
{
	M4Matrix2D mat2D;
	m4_mx2d_from_mx(&mat2D, mat);
	m4_mx2d_apply_rect(&mat2D, rc);
}

void m4_mx_add_matrix(M4Matrix *mat, M4Matrix *mul)
{
    M4Matrix tmp;
	m4_mx_init(tmp);

    tmp.m[0] = m4_mulfix(mat->m[0],mul->m[0]) + m4_mulfix(mat->m[4],mul->m[1]) + m4_mulfix(mat->m[8],mul->m[2]);
    tmp.m[4] = m4_mulfix(mat->m[0],mul->m[4]) + m4_mulfix(mat->m[4],mul->m[5]) + m4_mulfix(mat->m[8],mul->m[6]);
    tmp.m[8] = m4_mulfix(mat->m[0],mul->m[8]) + m4_mulfix(mat->m[4],mul->m[9]) + m4_mulfix(mat->m[8],mul->m[10]);
    tmp.m[12]= m4_mulfix(mat->m[0],mul->m[12]) + m4_mulfix(mat->m[4],mul->m[13]) + m4_mulfix(mat->m[8],mul->m[14]) + mat->m[12];
    tmp.m[1] = m4_mulfix(mat->m[1],mul->m[0]) + m4_mulfix(mat->m[5],mul->m[1]) + m4_mulfix(mat->m[9],mul->m[2]);
    tmp.m[5] = m4_mulfix(mat->m[1],mul->m[4]) + m4_mulfix(mat->m[5],mul->m[5]) + m4_mulfix(mat->m[9],mul->m[6]);
    tmp.m[9] = m4_mulfix(mat->m[1],mul->m[8]) + m4_mulfix(mat->m[5],mul->m[9]) + m4_mulfix(mat->m[9],mul->m[10]);
    tmp.m[13]= m4_mulfix(mat->m[1],mul->m[12]) + m4_mulfix(mat->m[5],mul->m[13]) + m4_mulfix(mat->m[9],mul->m[14]) + mat->m[13];
    tmp.m[2] = m4_mulfix(mat->m[2],mul->m[0]) + m4_mulfix(mat->m[6],mul->m[1]) + m4_mulfix(mat->m[10],mul->m[2]);
    tmp.m[6] = m4_mulfix(mat->m[2],mul->m[4]) + m4_mulfix(mat->m[6],mul->m[5]) + m4_mulfix(mat->m[10],mul->m[6]);
    tmp.m[10]= m4_mulfix(mat->m[2],mul->m[8]) + m4_mulfix(mat->m[6],mul->m[9]) + m4_mulfix(mat->m[10],mul->m[10]);
    tmp.m[14]= m4_mulfix(mat->m[2],mul->m[12]) + m4_mulfix(mat->m[6],mul->m[13]) + m4_mulfix(mat->m[10],mul->m[14]) + mat->m[14];
	memcpy(mat->m, tmp.m, sizeof(Fixed)*16);
}

void m4_mx_add_translation(M4Matrix *mat, Fixed tx, Fixed ty, Fixed tz)
{
	Fixed tmp[3];
	u32 i;
	tmp[0] = mat->m[12];
	tmp[1] = mat->m[13];
	tmp[2] = mat->m[14];
	for (i=0; i<3; i++)
		tmp[i] += (m4_mulfix(tx,mat->m[i]) + m4_mulfix(ty,mat->m[i+4]) + m4_mulfix(tz, mat->m[i + 8]));
	mat->m[12] = tmp[0];
	mat->m[13] = tmp[1];
	mat->m[14] = tmp[2];
}

void m4_mx_add_scale(M4Matrix *mat, Fixed sx, Fixed sy, Fixed sz)
{
	Fixed tmp[3];
	u32 i, j;

	tmp[0] = sx; 
	tmp[1] = sy; 
	tmp[2] = sz; 

	for (i=0; i<3; i++) {
		for (j=0; j<3; j++) {
			mat->m[i*4 + j] = m4_mulfix(mat->m[j+4 * i], tmp[i]);
		}
	}
}

void m4_mx_add_rotation(M4Matrix *mat, Fixed angle, Fixed x, Fixed y, Fixed z)
{
	M4Matrix tmp;
	Fixed xx, yy, zz, xy, xz, yz;
	Fixed nor = m4_sqrt(m4_mulfix(x,x) + m4_mulfix(y,y) + m4_mulfix(z,z));
	Fixed cos_a = m4_cos(angle);
	Fixed sin_a = m4_sin(angle);
	Fixed icos_a = FIX_ONE - cos_a;

	if (nor && (nor!=FIX_ONE)) { 
		x = m4_divfix(x, nor); 
		y = m4_divfix(y, nor); 
		z = m4_divfix(z, nor); 
	}
	xx = m4_mulfix(x,x); yy = m4_mulfix(y,y); zz = m4_mulfix(z,z); xy = m4_mulfix(x,y); xz = m4_mulfix(x,z); yz = m4_mulfix(y,z);
	m4_mx_init(tmp);
    tmp.m[0] = m4_mulfix(icos_a,xx) + cos_a;
    tmp.m[1] = m4_mulfix(xy,icos_a) + m4_mulfix(z,sin_a);
    tmp.m[2] = m4_mulfix(xz,icos_a) - m4_mulfix(y,sin_a);
    
	tmp.m[4] = m4_mulfix(xy,icos_a) - m4_mulfix(z,sin_a);
    tmp.m[5] = m4_mulfix(icos_a,yy) + cos_a;
    tmp.m[6] = m4_mulfix(yz,icos_a) + m4_mulfix(x,sin_a);

	tmp.m[8] = m4_mulfix(xz,icos_a) + m4_mulfix(y,sin_a);
    tmp.m[9] = m4_mulfix(yz,icos_a) - m4_mulfix(x,sin_a);
    tmp.m[10]= m4_mulfix(icos_a,zz) + cos_a;

	m4_mx_add_matrix(mat, &tmp);
}

void m4_mx_from_mx2d(M4Matrix *mat, M4Matrix2D *mat2D)
{
	m4_mx_init(*mat);
	mat->m[0] = mat2D->m[0];
	mat->m[4] = mat2D->m[1];
	mat->m[12] = mat2D->m[2];
	mat->m[1] = mat2D->m[3];
	mat->m[5] = mat2D->m[4];
	mat->m[13] = mat2D->m[5];
}

Bool m4_mx_equal(M4Matrix *mx1, M4Matrix *mx2)
{
	if (mx1->m[0] != mx2->m[0]) return 0;
	if (mx1->m[1] != mx2->m[1]) return 0;
	if (mx1->m[2] != mx2->m[2]) return 0;
	if (mx1->m[4] != mx2->m[4]) return 0;
	if (mx1->m[5] != mx2->m[5]) return 0;
	if (mx1->m[6] != mx2->m[6]) return 0;
	if (mx1->m[8] != mx2->m[8]) return 0;
	if (mx1->m[9] != mx2->m[9]) return 0;
	if (mx1->m[10] != mx2->m[10]) return 0;
	if (mx1->m[12] != mx2->m[12]) return 0;
	if (mx1->m[13] != mx2->m[13]) return 0;
	if (mx1->m[14] != mx2->m[14]) return 0;
	return 1;
}


void m4_mx_inverse(M4Matrix *mx)
{
    Fixed det;
	M4Matrix rev;
	m4_mx_init(rev);

	assert(! ((mx->m[3] != 0) || (mx->m[7] != 0) || (mx->m[11] != 0) || (mx->m[15] != FIX_ONE)) );

	det = m4_mulfix(m4_mulfix(mx->m[0], mx->m[5]) , mx->m[10]);
	det += m4_mulfix(m4_mulfix(mx->m[1], mx->m[6]) , mx->m[8]);
	det += m4_mulfix(m4_mulfix(mx->m[2], mx->m[4]) , mx->m[9]);
	det -= m4_mulfix(m4_mulfix(mx->m[2], mx->m[5]) , mx->m[8]);
	det -= m4_mulfix(m4_mulfix(mx->m[1], mx->m[4]) , mx->m[10]);
	det -= m4_mulfix(m4_mulfix(mx->m[0], mx->m[6]) , mx->m[9]);

//	if (m4_mulfix(det) < FIX_EPSILON) return;

	/* Calculate inverse(A) = adj(A) / det(A) */
	rev.m[0] =  m4_muldiv(mx->m[5], mx->m[10], det) - m4_muldiv(mx->m[6], mx->m[9], det);
	rev.m[4] = -m4_muldiv(mx->m[4], mx->m[10], det) + m4_muldiv(mx->m[6], mx->m[8], det);
	rev.m[8] =  m4_muldiv(mx->m[4], mx->m[9], det) - m4_muldiv(mx->m[5], mx->m[8], det);
	rev.m[1] = -m4_muldiv(mx->m[1], mx->m[10], det) + m4_muldiv(mx->m[2], mx->m[9], det);
	rev.m[5] =  m4_muldiv(mx->m[0], mx->m[10], det) - m4_muldiv(mx->m[2], mx->m[8], det);
	rev.m[9] = -m4_muldiv(mx->m[0], mx->m[9], det) + m4_muldiv(mx->m[1], mx->m[8], det);
	rev.m[2] =  m4_muldiv(mx->m[1], mx->m[6], det) - m4_muldiv(mx->m[2], mx->m[5], det);
	rev.m[6] = -m4_muldiv(mx->m[0], mx->m[6], det) + m4_muldiv(mx->m[2], mx->m[4], det);
	rev.m[10] = m4_muldiv(mx->m[0], mx->m[5], det) - m4_muldiv(mx->m[1], mx->m[4], det);

	/* do translation part*/
	rev.m[12] = -( m4_mulfix(mx->m[12], rev.m[0]) + m4_mulfix(mx->m[13], rev.m[4]) + m4_mulfix(mx->m[14], rev.m[8]) );
	rev.m[13] = -( m4_mulfix(mx->m[12], rev.m[1]) + m4_mulfix(mx->m[13], rev.m[5]) + m4_mulfix(mx->m[14], rev.m[9]) );
	rev.m[14] = -( m4_mulfix(mx->m[12], rev.m[2]) + m4_mulfix(mx->m[13], rev.m[6]) + m4_mulfix(mx->m[14], rev.m[10]) );
	m4_mx_copy(*mx, rev);
}


void m4_mx_apply_vec(M4Matrix *mx, M4Vec *pt)
{
	M4Vec res;
	res.x = m4_mulfix(pt->x, mx->m[0]) + m4_mulfix(pt->y, mx->m[4]) + m4_mulfix(pt->z, mx->m[8]) + mx->m[12];
	res.y = m4_mulfix(pt->x, mx->m[1]) + m4_mulfix(pt->y, mx->m[5]) + m4_mulfix(pt->z, mx->m[9]) + mx->m[13];
	res.z = m4_mulfix(pt->x, mx->m[2]) + m4_mulfix(pt->y, mx->m[6]) + m4_mulfix(pt->z, mx->m[10]) + mx->m[14];
	*pt = res;
}

void m4_mx_ortho(M4Matrix *mx, Fixed left, Fixed right, Fixed bottom, Fixed top, Fixed z_near, Fixed z_far)
{
	m4_mx_init(*mx);
	mx->m[0] = m4_divfix(2*FIX_ONE, right-left);
	mx->m[5] = m4_divfix(2*FIX_ONE, top-bottom);
	mx->m[10] = m4_divfix(-2*FIX_ONE, z_far-z_near);
	mx->m[12] = m4_divfix(right+left, right-left);
	mx->m[13] = m4_divfix(top+bottom, top-bottom);
	mx->m[14] = m4_divfix(z_far+z_near, z_far-z_near);
	mx->m[15] = FIX_ONE;
}

void m4_mx_perspective(M4Matrix *mx, Fixed fieldOfView, Fixed aspectRatio, Fixed z_near, Fixed z_far)
{
	Fixed f = m4_divfix(m4_cos(fieldOfView/2), m4_sin(fieldOfView/2));
	m4_mx_init(*mx);
	mx->m[0] = m4_divfix(f, aspectRatio);
	mx->m[5] = f;
	mx->m[10] = m4_divfix(z_far+z_near, z_near-z_far);
	mx->m[11] = -FIX_ONE;
	mx->m[14] = 2*m4_muldiv(z_near, z_far, z_near-z_far);
	mx->m[15] = 0;
}

void m4_mx_lookat(M4Matrix *mx, M4Vec eye, M4Vec center, M4Vec upVector)
{
	M4Vec f, s, u;
	
	m4_vec_diff(f, center, eye);
	m4_vec_norm(&f);
	m4_vec_norm(&upVector);

	s = m4_vec_cross(f, upVector);
	u = m4_vec_cross(s, f);
	m4_mx_init(*mx);
	
	mx->m[0] = s.x;
	mx->m[1] = u.x;
	mx->m[2] = -f.x;
	mx->m[4] = s.y;
	mx->m[5] = u.y;
	mx->m[6] = -f.y;
	mx->m[8] = s.z;
	mx->m[9] = u.z;
	mx->m[10] = -f.z;

	m4_mx_add_translation(mx, -eye.x, -eye.y, -eye.z);
}

void m4_mx_decompose(M4Matrix *mx, M4Vec *translate, M4Vec *scale, M4Vec4 *rotate, M4Vec *shear)
{
	u32 i, j;
	M4Vec4 quat;
	Fixed locmat[16];
	M4Matrix tmp;
	M4Vec row0, row1, row2;
	Fixed shear_xy, shear_xz, shear_yz; 
	assert(mx->m[15]);

	memcpy(locmat, mx->m, sizeof(Fixed)*16);
	/*no perspective*/
    locmat[3] = locmat[7] = locmat[11] = 0;
	/*normalize*/
    for (i=0; i<4; i++) {
        for (j=0; j<4; j++) {
            locmat[4*i+j] = m4_divfix(locmat[4*i+j], locmat[15]);
        }
    }
    translate->x = locmat[12];
    translate->y = locmat[13];
    translate->z = locmat[14];
    locmat[12] = locmat[13] = locmat[14] = 0;
    row0.x = locmat[0]; row0.y = locmat[1]; row0.z = locmat[2];
    row1.x = locmat[4]; row1.y = locmat[5]; row1.z = locmat[6];
    row2.x = locmat[8]; row2.y = locmat[9]; row2.z = locmat[10];

    scale->x = m4_vec_len(row0);
    m4_vec_norm(&row0);
    shear_xy = m4_vec_dot(row0, row1);
    row1.x -= m4_mulfix(row0.x, shear_xy);
    row1.y -= m4_mulfix(row0.y, shear_xy);
    row1.z -= m4_mulfix(row0.z, shear_xy);

    scale->y = m4_vec_len(row1);
    m4_vec_norm(&row1);
    shear->x = m4_divfix(shear_xy, scale->y);

    shear_xz = m4_vec_dot(row0, row2);
    row2.x -= m4_mulfix(row0.x, shear_xz);
    row2.y -= m4_mulfix(row0.y, shear_xz);
    row2.z -= m4_mulfix(row0.z, shear_xz);
    shear_yz = m4_vec_dot(row1, row2);
    row2.x -= m4_mulfix(row1.x, shear_yz);
    row2.y -= m4_mulfix(row1.y, shear_yz);
    row2.z -= m4_mulfix(row1.z, shear_yz);

    scale->z = m4_vec_len(row2);
    m4_vec_norm(&row2);
    shear->y = m4_divfix(shear_xz, scale->z);
    shear->z = m4_divfix(shear_yz, scale->z);

	locmat[0] = row0.x; locmat[4] = row1.x; locmat[8] = row2.x;
	locmat[1] = row0.y; locmat[5] = row1.y; locmat[9] = row2.y;
	locmat[2] = row0.z; locmat[6] = row1.z; locmat[10] = row2.z;

	memcpy(tmp.m, locmat, sizeof(Fixed)*16);
	quat = m4_quat_from_matrix(&tmp);
	*rotate = m4_quat_to_rotation(&quat);
}

void m4_mx_apply_bbox(M4Matrix *mx, M4BBox *b)
{
	Fixed var;
	m4_mx_apply_vec(mx, &b->min_edge);
	m4_mx_apply_vec(mx, &b->max_edge);

	if (b->min_edge.x > b->max_edge.x) 
	{
		var = b->min_edge.x; b->min_edge.x = b->max_edge.x; b->max_edge.x = var;
	}
	if (b->min_edge.y > b->max_edge.y) 
	{
		var = b->min_edge.y; b->min_edge.y = b->max_edge.y; b->max_edge.y = var;
	}
	if (b->min_edge.z > b->max_edge.z) 
	{
		var = b->min_edge.z; b->min_edge.z = b->max_edge.z; b->max_edge.z = var;
	}
	m4_bbox_refresh(b);
}


// Apply the rotation portion of a matrix to a vector.
void m4_mx_rotate_vector(M4Matrix *mx, M4Vec *pt)
{
	M4Vec res;
	Fixed den;
	res.x = m4_mulfix(pt->x, mx->m[0]) + m4_mulfix(pt->y, mx->m[4]) + m4_mulfix(pt->z, mx->m[8]);
	res.y = m4_mulfix(pt->x, mx->m[1]) + m4_mulfix(pt->y, mx->m[5]) + m4_mulfix(pt->z, mx->m[9]);
	res.z = m4_mulfix(pt->x, mx->m[2]) + m4_mulfix(pt->y, mx->m[6]) + m4_mulfix(pt->z, mx->m[10]);
	den = m4_mulfix(pt->x, mx->m[3]) + m4_mulfix(pt->y, mx->m[7]) + m4_mulfix(pt->z, mx->m[11]) + mx->m[15];
	if (den == 0) return;
	res.x = m4_divfix(res.x, den);
	res.y = m4_divfix(res.y, den);
	res.z = m4_divfix(res.z, den);
	*pt = res;
}

void m4_mx_rotation_matrix_from_vectors(M4Matrix *mx, M4Vec x, M4Vec y, M4Vec z)
{
    mx->m[0] = x.x; mx->m[1] = y.x; mx->m[2] = z.x; mx->m[3] = 0;
    mx->m[4] = x.y; mx->m[5] = y.y; mx->m[6] = z.y; mx->m[7] = 0;
    mx->m[8] = x.z; mx->m[9] = y.z; mx->m[10] = z.z; mx->m[11] = 0;
    mx->m[12] = 0; mx->m[13] = 0; mx->m[14] = 0; mx->m[15] = FIX_ONE;
}


/*we should only need a full matrix product for frustrum setup*/
void m4_mx_add_matrix_4x4(M4Matrix *mat, M4Matrix *mul)
{
    M4Matrix tmp;
	m4_mx_init(tmp);
    tmp.m[0] = m4_mulfix(mat->m[0],mul->m[0]) + m4_mulfix(mat->m[4],mul->m[1]) + m4_mulfix(mat->m[8],mul->m[2]) + m4_mulfix(mat->m[12],mul->m[3]);
    tmp.m[1] = m4_mulfix(mat->m[1],mul->m[0]) + m4_mulfix(mat->m[5],mul->m[1]) + m4_mulfix(mat->m[9],mul->m[2]) + m4_mulfix(mat->m[13],mul->m[3]);
    tmp.m[2] = m4_mulfix(mat->m[2],mul->m[0]) + m4_mulfix(mat->m[6],mul->m[1]) + m4_mulfix(mat->m[10],mul->m[2]) + m4_mulfix(mat->m[14],mul->m[3]);
    tmp.m[3] = m4_mulfix(mat->m[3],mul->m[0]) + m4_mulfix(mat->m[7],mul->m[1]) + m4_mulfix(mat->m[11],mul->m[2]) + m4_mulfix(mat->m[15],mul->m[3]);
    tmp.m[4] = m4_mulfix(mat->m[0],mul->m[4]) + m4_mulfix(mat->m[4],mul->m[5]) + m4_mulfix(mat->m[8],mul->m[6]) + m4_mulfix(mat->m[12],mul->m[7]);
    tmp.m[5] = m4_mulfix(mat->m[1],mul->m[4]) + m4_mulfix(mat->m[5],mul->m[5]) + m4_mulfix(mat->m[9],mul->m[6]) + m4_mulfix(mat->m[13],mul->m[7]);
    tmp.m[6] = m4_mulfix(mat->m[2],mul->m[4]) + m4_mulfix(mat->m[6],mul->m[5]) + m4_mulfix(mat->m[10],mul->m[6]) + m4_mulfix(mat->m[14],mul->m[7]);
    tmp.m[7] = m4_mulfix(mat->m[3],mul->m[4]) + m4_mulfix(mat->m[7],mul->m[5]) + m4_mulfix(mat->m[11],mul->m[6]) + m4_mulfix(mat->m[15],mul->m[7]);
    tmp.m[8] = m4_mulfix(mat->m[0],mul->m[8]) + m4_mulfix(mat->m[4],mul->m[9]) + m4_mulfix(mat->m[8],mul->m[10]) + m4_mulfix(mat->m[12],mul->m[11]);
    tmp.m[9] = m4_mulfix(mat->m[1],mul->m[8]) + m4_mulfix(mat->m[5],mul->m[9]) + m4_mulfix(mat->m[9],mul->m[10]) + m4_mulfix(mat->m[13],mul->m[11]);
    tmp.m[10] = m4_mulfix(mat->m[2],mul->m[8]) + m4_mulfix(mat->m[6],mul->m[9]) + m4_mulfix(mat->m[10],mul->m[10]) + m4_mulfix(mat->m[14],mul->m[11]);
    tmp.m[11] = m4_mulfix(mat->m[3],mul->m[8]) + m4_mulfix(mat->m[7],mul->m[9]) + m4_mulfix(mat->m[11],mul->m[10]) + m4_mulfix(mat->m[15],mul->m[11]);
    tmp.m[12] = m4_mulfix(mat->m[0],mul->m[12]) + m4_mulfix(mat->m[4],mul->m[13]) + m4_mulfix(mat->m[8],mul->m[14]) + m4_mulfix(mat->m[12],mul->m[15]);
    tmp.m[13] = m4_mulfix(mat->m[1],mul->m[12]) + m4_mulfix(mat->m[5],mul->m[13]) + m4_mulfix(mat->m[9],mul->m[14]) + m4_mulfix(mat->m[13],mul->m[15]);
    tmp.m[14] = m4_mulfix(mat->m[2],mul->m[12]) + m4_mulfix(mat->m[6],mul->m[13]) + m4_mulfix(mat->m[10],mul->m[14]) + m4_mulfix(mat->m[14],mul->m[15]);
    tmp.m[15] = m4_mulfix(mat->m[3],mul->m[12]) + m4_mulfix(mat->m[7],mul->m[13]) + m4_mulfix(mat->m[11],mul->m[14]) + m4_mulfix(mat->m[15],mul->m[15]);
	memcpy(mat->m, tmp.m, sizeof(Fixed)*16);
}


void m4_mx_apply_vec_4x4(M4Matrix *mx, M4Vec4 *vec)
{
	M4Vec4 res;
	res.x = m4_mulfix(mx->m[0], vec->x) + m4_mulfix(mx->m[4], vec->y) + m4_mulfix(mx->m[8], vec->z) + m4_mulfix(mx->m[12], vec->q);
	res.y = m4_mulfix(mx->m[1], vec->x) + m4_mulfix(mx->m[5], vec->y) + m4_mulfix(mx->m[9], vec->z) + m4_mulfix(mx->m[13], vec->q);
	res.z = m4_mulfix(mx->m[2], vec->x) + m4_mulfix(mx->m[6], vec->y) + m4_mulfix(mx->m[10], vec->z) + m4_mulfix(mx->m[14], vec->q);
	res.q = m4_mulfix(mx->m[3], vec->x) + m4_mulfix(mx->m[7], vec->y) + m4_mulfix(mx->m[11], vec->z) + m4_mulfix(mx->m[15], vec->q);
	*vec = res;
}

/*
 *	Taken from MESA/GLU (LGPL)
 *
 * Compute inverse of 4x4 transformation matrix.
 * Code contributed by Jacques Leroy jle@star.be
 * Return 1 for success, 0 for failure (singular matrix)
 */

Bool m4_mx_inverse_4x4(M4Matrix *mx)
{

#define SWAP_ROWS(a, b) { Fixed *_tmp = a; (a)=(b); (b)=_tmp; }
	Fixed wtmp[4][8];
	Fixed m0, m1, m2, m3, s;
	Fixed *r0, *r1, *r2, *r3;
	M4Matrix res;
	r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];
	r0[0] = mx->m[0]; r0[1] = mx->m[4]; r0[2] = mx->m[8]; r0[3] = mx->m[12]; r0[4] = FIX_ONE; r0[5] = r0[6] = r0[7] = 0;
	r1[0] = mx->m[1]; r1[1] = mx->m[5]; r1[2] = mx->m[9]; r1[3] = mx->m[13]; r1[5] = FIX_ONE; r1[4] = r1[6] = r1[7] = 0;
	r2[0] = mx->m[2]; r2[1] = mx->m[6]; r2[2] = mx->m[10]; r2[3] = mx->m[14]; r2[6] = FIX_ONE; r2[4] = r2[5] = r2[7] = 0;
	r3[0] = mx->m[3]; r3[1] = mx->m[7]; r3[2] = mx->m[11]; r3[3] = mx->m[15]; r3[7] = FIX_ONE; r3[4] = r3[5] = r3[6] = 0;

	/* choose pivot - or die */
	if (ABS(r3[0]) > ABS(r2[0])) SWAP_ROWS(r3, r2);
	if (ABS(r2[0]) > ABS(r1[0])) SWAP_ROWS(r2, r1);
	if (ABS(r1[0]) > ABS(r0[0])) SWAP_ROWS(r1, r0);
	if (r0[0]==0) return 0;
	
	/*eliminate first variable*/
	m1 = m4_divfix(r1[0], r0[0]);
	m2 = m4_divfix(r2[0], r0[0]);
	m3 = m4_divfix(r3[0], r0[0]);
	s = r0[1];
	r1[1] -= m4_mulfix(m1, s);
	r2[1] -= m4_mulfix(m2, s);
	r3[1] -= m4_mulfix(m3, s);
	s = r0[2];
	r1[2] -= m4_mulfix(m1, s);
	r2[2] -= m4_mulfix(m2, s);
	r3[2] -= m4_mulfix(m3, s);
	s = r0[3];
	r1[3] -= m4_mulfix(m1, s);
	r2[3] -= m4_mulfix(m2, s);
	r3[3] -= m4_mulfix(m3, s);
	s = r0[4];
	if (s != 0) {
		r1[4] -= m4_mulfix(m1, s);
		r2[4] -= m4_mulfix(m2, s);
		r3[4] -= m4_mulfix(m3, s);
	}
	s = r0[5];
	if (s != 0) {
		r1[5] -= m4_mulfix(m1, s);
		r2[5] -= m4_mulfix(m2, s);
		r3[5] -= m4_mulfix(m3, s);
	}
	s = r0[6];
	if (s != 0) {
		r1[6] -= m4_mulfix(m1, s);
		r2[6] -= m4_mulfix(m2, s);
		r3[6] -= m4_mulfix(m3, s);
	}
	s = r0[7];
	if (s != 0) {
		r1[7] -= m4_mulfix(m1, s);
		r2[7] -= m4_mulfix(m2, s);
		r3[7] -= m4_mulfix(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 (r1[1]==0) return 0;
	
	/* eliminate second variable */
	m2 = m4_divfix(r2[1], r1[1]);
	m3 = m4_divfix(r3[1], r1[1]);
	r2[2] -= m4_mulfix(m2, r1[2]);
	r3[2] -= m4_mulfix(m3, r1[2]);
	r2[3] -= m4_mulfix(m2, r1[3]);
	r3[3] -= m4_mulfix(m3, r1[3]);
	s = r1[4];
	if (s!=0) {
		r2[4] -= m4_mulfix(m2, s);
		r3[4] -= m4_mulfix(m3, s);
	}
	s = r1[5];
	if (s!=0) {
		r2[5] -= m4_mulfix(m2, s);
		r3[5] -= m4_mulfix(m3, s);
	}
	s = r1[6];
	if (s!=0) {
		r2[6] -= m4_mulfix(m2, s);
		r3[6] -= m4_mulfix(m3, s);
	}
	s = r1[7];
	if (s!=0) {
		r2[7] -= m4_mulfix(m2, s);
		r3[7] -= m4_mulfix(m3, s);
	}

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

	/* eliminate third variable */
	m3 = m4_divfix(r3[2], r2[2]);
	r3[3] -= m4_mulfix(m3, r2[3]); r3[4] -= m4_mulfix(m3, r2[4]); r3[5] -= m4_mulfix(m3, r2[5]);
	r3[6] -= m4_mulfix(m3, r2[6]); r3[7] -= m4_mulfix(m3, r2[7]);
	/* last check */
	if (r3[3]==0) return 0;

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

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

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

	m4_mx_init(res)
	res.m[0] = r0[4]; res.m[4] = r0[5]; res.m[8] = r0[6]; res.m[12] = r0[7];
	res.m[1] = r1[4]; res.m[5] = r1[5], res.m[9] = r1[6]; res.m[13] = r1[7];
	res.m[2] = r2[4]; res.m[6] = r2[5]; res.m[10] = r2[6]; res.m[14] = r2[7];
	res.m[3] = r3[4]; res.m[7] = r3[5]; res.m[11] = r3[6]; res.m[15] = r3[7];
	m4_mx_copy(*mx, res);
	return 1;
#undef SWAP_ROWS

}


Bool m4_plane_exists_intersection(M4Plane *plane, M4Plane *with)
{
	M4Vec cross;
	cross = m4_vec_cross(with->normal, plane->normal);
	return m4_vec_lensq(cross) > FIX_EPSILON;
}

Bool m4_plane_intersect_line(M4Plane *plane, M4Vec *linepoint, M4Vec *linevec, M4Vec *outPoint)
{
	Fixed t, t2;
	t2 = m4_vec_dot(plane->normal, *linevec);
	if (t2 == 0) return 0;
	t = - m4_divfix((m4_vec_dot(plane->normal, *linepoint) + plane->d) , t2);
	if (t<0) return 0;
	*outPoint = m4_vec_scale(*linevec, t);
	m4_vec_add(*outPoint, *linepoint, *outPoint);
	return 1;
}

Bool m4_plane_intersect_plane(M4Plane *plane, M4Plane *with, M4Vec *linepoint, M4Vec *linevec)
{
	Fixed fn00 = m4_vec_len(plane->normal);
	Fixed fn01 = m4_vec_dot(plane->normal, with->normal);
	Fixed fn11 = m4_vec_len(with->normal);
	Fixed det = m4_mulfix(fn00,fn11) - m4_mulfix(fn01,fn01);
	if (fabs(det) > FIX_EPSILON) {
		Fixed fc0, fc1;
		M4Vec v1, v2;
		fc0 = m4_divfix( m4_mulfix(fn11, -plane->d) + m4_mulfix(fn01, with->d) , det);
		fc1 = m4_divfix( m4_mulfix(fn00, -with->d) + m4_mulfix(fn01, plane->d) , det);
		*linevec = m4_vec_cross(plane->normal, with->normal);
		v1 = m4_vec_scale(plane->normal, fc0);
		v2 = m4_vec_scale(with->normal, fc1);
		m4_vec_add(*linepoint, v1, v2);
		return 1;
	}
	return 0;
}

Bool m4_plane_intersect_planes(M4Plane *plane, M4Plane *p1, M4Plane *p2, M4Vec *outPoint)
{
	M4Vec lp, lv;
	if (m4_plane_intersect_plane(plane, p1, &lp, &lv))
		return m4_plane_intersect_line(p2, &lp, &lv, outPoint);
	return 0;
}



M4Ray m4_ray(M4Vec start, M4Vec end)
{
	M4Ray r;
	r.orig = start;
	m4_vec_diff(r.dir, end, start);
	m4_vec_norm(&r.dir);
	return r;
}

void m4_mx_apply_ray(M4Matrix *mx, M4Ray *r)
{
	m4_vec_add(r->dir, r->orig, r->dir);
	m4_mx_apply_vec(mx, &r->orig);
	m4_mx_apply_vec(mx, &r->dir);
	m4_vec_diff(r->dir, r->dir, r->orig);
	m4_vec_norm(&r->dir);
}

#define XPLANE 0
#define YPLANE 1
#define ZPLANE 2


Bool m4_ray_hit_box(M4Ray *ray, M4Vec box_min, M4Vec box_max, M4Vec *outPoint)
{
	Fixed t1, t2, tNEAR=FIX_MIN, tFAR=FIX_MAX;
	Fixed temp;
	s8 xyorz, sign;
	
	if (ray->dir.x == 0) {
		if ((ray->orig.x < box_min.x) || (ray->orig.x > box_max.x))
			return 0;
	} else {
		t1 = m4_divfix(box_min.x - ray->orig.x, ray->dir.x);
		t2 = m4_divfix(box_max.x - ray->orig.x, ray->dir.x);
		if (t1 > t2) {
			temp = t1;
			t1 = t2;
			t2 = temp;
		}
		if (t1 > tNEAR) {
			tNEAR = t1;
			xyorz = XPLANE;
			sign = (ray->dir.x < 0) ? 1 : -1;
		}
		if (t2 < tFAR) tFAR = t2;
		if (tNEAR > tFAR) return 0; // box missed
		if (tFAR < 0) return 0; // box behind the ray
	}

	if (ray->dir.y == 0) {
		if ((ray->orig.y < box_min.y) || (ray->orig.y > box_max.y)) 
			return 0;
	} else {
		t1 = m4_divfix(box_min.y - ray->orig.y, ray->dir.y);
		t2 = m4_divfix(box_max.y - ray->orig.y, ray->dir.y);
		if (t1 > t2) {
			temp = t1;
			t1 = t2;
			t2 = temp;
		}
		if (t1 > tNEAR) {
			tNEAR = t1;
			xyorz = YPLANE;
			sign = (ray->dir.y < 0) ? 1 : -1;
		}
		if (t2 < tFAR) tFAR = t2;
		if (tNEAR > tFAR) return 0; // box missed
		if (tFAR < 0) return 0; // box behind the ray
	}

	// Check the Z plane
	if (ray->dir.z == 0) {
		if ((ray->orig.z < box_min.z) || (ray->orig.z > box_max.z))
			return 0;
	} else {
		t1 = m4_divfix(box_min.z - ray->orig.z, ray->dir.z);
		t2 = m4_divfix(box_max.z - ray->orig.z, ray->dir.z);
		if (t1 > t2) {
			temp = t1;
			t1 = t2;
			t2 = temp;
		}
		if (t1 > tNEAR) {
			tNEAR = t1;
			xyorz = ZPLANE;
			sign = (ray->dir.z < 0) ? 1 : -1;
		}
		if (t2 < tFAR) tFAR = t2;
		if (tNEAR>tFAR) return 0; // box missed
		if (tFAR < 0) return 0;  // box behind the ray
	}
	if (outPoint) {
		*outPoint = m4_vec_scale(ray->dir, tNEAR);
		m4_vec_add(*outPoint, *outPoint, ray->orig);
	}
	return 1;
}


Bool m4_ray_hit_sphere(M4Ray *ray, M4Vec *center, Fixed radius, M4Vec *outPoint)
{
	M4Vec radv;
	Fixed dist, center_proj, center_proj_sq, hcord;
	if (center) {
		m4_vec_diff(radv, *center, ray->orig);
	} else {
		radv = ray->orig;
		m4_vec_rev(radv);
	}
	dist = m4_vec_len(radv);
	center_proj = m4_vec_dot(radv, ray->dir);
	if (radius + ABS(center_proj) < dist ) return 0;
	
	center_proj_sq = m4_mulfix(center_proj, center_proj);
	hcord = center_proj_sq - m4_mulfix(dist, dist) + m4_mulfix(radius , radius);
    if (hcord < 0) return 0;
	if (center_proj_sq < hcord) return 0;
	if (outPoint) {
		center_proj -= m4_sqrt(hcord);
		radv = m4_vec_scale(ray->dir, center_proj);
		m4_vec_add(*outPoint, ray->orig, radv);
	}
	return 1;
}

/*
 *		Tomas Möller and Ben Trumbore.
 *	 Fast, minimum storage ray-triangle intersection. 
 *		Journal of graphics tools, 2(1):21-28, 1997
 *
 */
Bool m4_ray_hit_triangle(M4Ray *ray, M4Vec *v0, M4Vec *v1, M4Vec *v2, Fixed *dist)
{
	Fixed u, v, det;
	M4Vec edge1, edge2, tvec, pvec, qvec;
	/* find vectors for two edges sharing vert0 */
	m4_vec_diff(edge1, *v1, *v0);
	m4_vec_diff(edge2, *v2, *v0);
	/* begin calculating determinant - also used to calculate U parameter */
	pvec = m4_vec_cross(ray->dir, edge2);
	/* if determinant is near zero, ray lies in plane of triangle */
	det = m4_vec_dot(edge1, pvec);
	if (ABS(det) < FIX_EPSILON) return 0;
	/* calculate distance from vert0 to ray origin */
	m4_vec_diff(tvec, ray->orig, *v0);
	/* calculate U parameter and test bounds */
	u = m4_divfix(m4_vec_dot(tvec, pvec), det);
	if ((u < 0) || (u > FIX_ONE)) return 0;
	/* prepare to test V parameter */
	qvec = m4_vec_cross(tvec, edge1);
	/* calculate V parameter and test bounds */
	v = m4_divfix(m4_vec_dot(ray->dir, qvec), det);
	if ((v < 0) || (u + v > FIX_ONE)) return 0;
	/* calculate t, ray intersects triangle */
	*dist = m4_divfix(m4_vec_dot(edge2, qvec), det);
	return 1;
}

Bool m4_ray_hit_triangle_backcull(M4Ray *ray, M4Vec *v0, M4Vec *v1, M4Vec *v2, Fixed *dist)
{
	Fixed u, v, det;
	M4Vec edge1, edge2, tvec, pvec, qvec;
	/* find vectors for two edges sharing vert0 */
	m4_vec_diff(edge1, *v1, *v0);
	m4_vec_diff(edge2, *v2, *v0);
	/* begin calculating determinant - also used to calculate U parameter */
	pvec = m4_vec_cross(ray->dir, edge2);
	/* if determinant is near zero, ray lies in plane of triangle */
	det = m4_vec_dot(edge1, pvec);
	if (det < FIX_EPSILON) return 0;
	/* calculate distance from vert0 to ray origin */
	m4_vec_diff(tvec, ray->orig, *v0);
	/* calculate U parameter and test bounds */
	u = m4_vec_dot(tvec, pvec);
	if ((u < 0) || (u > det)) return 0;
	/* prepare to test V parameter */
	qvec = m4_vec_cross(tvec, edge1);
	/* calculate V parameter and test bounds */
	v = m4_vec_dot(ray->dir, qvec);
	if ((v < 0) || (u + v > det)) return 0;
	/* calculate t, scale parameters, ray intersects triangle */
	*dist = m4_divfix(m4_vec_dot(edge2, qvec), det);
	return 1;
}

M4Vec m4_closest_point_to_line(M4Vec line_pt, M4Vec line_vec, M4Vec pt)
{
	M4Vec c;
	Fixed t;
	m4_vec_diff(c, pt, line_pt);
	t = m4_vec_dot(line_vec, c);
	c = m4_vec_scale(line_vec, t);
	m4_vec_add(c, c, line_pt);
	return c;
}


M4Vec4 m4_quat_from_matrix(M4Matrix *mx)
{
	M4Vec4 res;
	Fixed diagonal, s;
    diagonal = mx->m[0] + mx->m[5] + mx->m[10];

    if (diagonal > 0) {
        s = m4_sqrt(diagonal + FIX_ONE);
        res.q = s / 2;
        s = m4_divfix(FIX_ONE/2, s);
        res.x = m4_mulfix(mx->m[6] - mx->m[9], s);
        res.y = m4_mulfix(mx->m[8] - mx->m[2], s);
        res.z = m4_mulfix(mx->m[1] - mx->m[4], s);
    } else {
		Fixed q[4];
        u32 i, j, k;
        static const u32 next[3] = { 1, 2, 0 };
        i = 0;
        if (mx->m[5] > mx->m[0]) { i = 1; }
        if (mx->m[10] > mx->m[4*i+i]) { i = 2; }
        j = next[i];
        k = next[j];
        s = m4_sqrt(FIX_ONE + mx->m[4*i + i] - (mx->m[4*j+j] + mx->m[4*k+k]) );
        q[i] = s / 2;
        if (s != 0) { s = m4_divfix(FIX_ONE/2, s); }
        q[3] = m4_mulfix(mx->m[4*j+k] - mx->m[4*k+j], s);
        q[j] = m4_mulfix(mx->m[4*i+j] + mx->m[4*j+i], s);
        q[k] = m4_mulfix(mx->m[4*i+k] + mx->m[4*k+i], s);
		res.x = q[0]; res.y = q[1]; res.z = q[2]; res.q = q[3];
    }
	return res;
}

M4Vec4 m4_quat_to_rotation(M4Vec4 *quat)
{
	M4Vec4 r;
    Fixed val = m4_acos(quat->q);
    if (val == 0) {
        r.x = r.y = 0;
        r.z = FIX_ONE;
		r.q = 0;
    } else {
		M4Vec axis;
        Fixed sin_val = m4_sin(val);
        axis.x = m4_divfix(quat->x, sin_val);
        axis.y = m4_divfix(quat->y, sin_val);
        axis.z = m4_divfix(quat->z, sin_val);
		m4_vec_norm(&axis);
		r.x = axis.x;
		r.y = axis.y;
		r.z = axis.z;
        r.q= 2 * val;
    }
	return r;
}

M4Vec4 m4_quat_from_rotation(M4Vec4 rot)
{
	M4Vec4 res;
	Fixed s;
	Fixed scale = m4_sqrt( m4_mulfix(rot.x, rot.x) + m4_mulfix(rot.y, rot.y) + m4_mulfix(rot.z, rot.z));

	/* no rotation - use (multiplication ???) identity quaternion */
	if (scale == 0) {
		res.q = FIX_ONE;
		res.x = 0;
		res.y = 0;
		res.z = 0;
	} else {
		s = m4_sin(rot.q/2);
		res.q = m4_cos(rot.q / 2);
		res.x = m4_muldiv(s, rot.x, scale);
		res.y = m4_muldiv(s, rot.y, scale);
		res.z = m4_muldiv(s, rot.z, scale);
		m4_quat_norm(res);
	}
	return res;
}

M4Vec4 m4_quat_from_axis_cos(M4Vec axis, Fixed cos_a)
{
	M4Vec4 r;
	if (cos_a < -FIX_ONE) cos_a = -FIX_ONE;
	else if (cos_a > FIX_ONE) cos_a = FIX_ONE;
	r.x = axis.x; r.y = axis.y; r.z = axis.z;
	r.q = m4_acos(cos_a);
	return m4_quat_from_rotation(r);
}

void m4_quat_conjugate(M4Vec4 *quat)
{
	quat->x *= -1;
	quat->y *= -1;
	quat->z *= -1;
}

M4Vec4 m4_quat_get_inv(M4Vec4 *quat)
{
	M4Vec4 ret = *quat;
	m4_quat_conjugate(&ret);
	m4_quat_norm(ret);
	return ret;
}


M4Vec4 m4_quat_multiply(M4Vec4 *q1, M4Vec4 *q2)
{
	M4Vec4 ret;
	ret.q = m4_mulfix(q1->q, q2->q) - m4_mulfix(q1->x, q2->x) - m4_mulfix(q1->y, q2->y) - m4_mulfix(q1->z, q2->z);
	ret.x = m4_mulfix(q1->q, q2->x) + m4_mulfix(q1->x, q2->q) + m4_mulfix(q1->y, q2->z) - m4_mulfix(q1->z, q2->y);
	ret.y = m4_mulfix(q1->q, q2->y) + m4_mulfix(q1->y, q2->q) - m4_mulfix(q1->x, q2->z) + m4_mulfix(q1->z, q2->x);
	ret.z = m4_mulfix(q1->q, q2->z) + m4_mulfix(q1->z, q2->q) + m4_mulfix(q1->x, q2->y) - m4_mulfix(q1->y, q2->x);
	return ret;
}

M4Vec m4_quat_rotate(M4Vec4 *quat, M4Vec *vec)
{
	M4Vec ret;
	M4Vec4 q_v, q_i, q_r1, q_r2;
	q_v.q = 0;
	q_v.x = vec->x;
	q_v.y = vec->y;
	q_v.z = vec->z;
	q_i = m4_quat_get_inv(quat);
	q_r1 = m4_quat_multiply(&q_v, &q_i);
	q_r2 = m4_quat_multiply(quat, &q_r1);
	ret.x = q_r2.x;
	ret.y = q_r2.y;
	ret.z = q_r2.z;
	return ret;
}

/*
 * Code from www.gamasutra.com/features/19980703/quaternions_01.htm,
 * Listing 5.
 *
 * SLERP(p, q, t) = [p sin((1 - t)a) + q sin(ta)] / sin(a)
 *
 * where a is the arc angle, quaternions pq = cos(q) and 0 <= t <= 1
 */
M4Vec4 m4_quat_slerp(M4Vec4 q1, M4Vec4 q2, Fixed frac)
{
	M4Vec4 res;
	Fixed omega, cosom, sinom, scale0, scale1, q2_array[4];

	cosom = m4_mulfix(q1.x, q2.x) + m4_mulfix(q1.y, q2.y) + m4_mulfix(q1.z, q2.z) + m4_mulfix(q1.q, q2.q);
	if (cosom < 0) {
		cosom = -cosom;
		q2_array[0] = -q2.x;
		q2_array[1] = -q2.y;
		q2_array[2] = -q2.z;
		q2_array[3] = -q2.q;
	} else {
		q2_array[0] = q2.x;
		q2_array[1] = q2.y;
		q2_array[2] = q2.z;
		q2_array[3] = q2.q;
	}

	/* calculate coefficients */
	if ((FIX_ONE - cosom) > FIX_EPSILON) {
		omega = m4_acos(cosom);
		sinom = m4_sin(omega);
		scale0 = m4_divfix(m4_sin( m4_mulfix(FIX_ONE - frac, omega)), sinom);
		scale1 = m4_divfix(m4_sin( m4_mulfix(frac, omega)), sinom);
	} else {
		/* q1 & q2 are very close, so do linear interpolation */
		scale0 = FIX_ONE - frac;
		scale1 = frac;
	}
	res.x = m4_mulfix(scale0, q1.x) + m4_mulfix(scale1, q2_array[0]);
	res.y = m4_mulfix(scale0, q1.y) + m4_mulfix(scale1, q2_array[1]);
	res.z = m4_mulfix(scale0, q1.z) + m4_mulfix(scale1, q2_array[2]);
	res.q = m4_mulfix(scale0, q1.q) + m4_mulfix(scale1, q2_array[3]);
	return res;
}


/*update center & radius & is_set flag*/
void m4_bbox_refresh(M4BBox *b)
{
	M4Vec v;
	m4_vec_add(v, b->min_edge, b->max_edge);
	b->center = m4_vec_scale(v, FIX_ONE / 2);
	m4_vec_diff(v, b->max_edge, b->min_edge);
	b->radius = m4_vec_len(v) / 2;
	b->is_set = 1;
}

void m4_bbox_from_rect(M4BBox *box, M4Rect *rc)
{
	box->min_edge.x = rc->x;
	box->min_edge.y = rc->y - rc->height;
	box->min_edge.z = 0;
	box->max_edge.x = rc->x + rc->width;
	box->max_edge.y = rc->y;
	box->max_edge.z = 0;
	m4_bbox_refresh(box);
}

void m4_rect_from_bbox(M4Rect *rc, M4BBox *box)
{
	rc->x = box->min_edge.x;
	rc->y = box->max_edge.y;
	rc->width = box->max_edge.x - box->min_edge.x;
	rc->height = box->max_edge.y - box->min_edge.y;
}

void m4_bbox_grow_point(M4BBox *box, M4Vec pt)
{
	if (pt.x > box->max_edge.x) box->max_edge.x = pt.x;
	if (pt.y > box->max_edge.y) box->max_edge.y = pt.y;
	if (pt.z > box->max_edge.z) box->max_edge.z = pt.z;
	if (pt.x < box->min_edge.x) box->min_edge.x = pt.x;
	if (pt.y < box->min_edge.y) box->min_edge.y = pt.y;
	if (pt.z < box->min_edge.z) box->min_edge.z = pt.z;
}

void m4_bbox_union(M4BBox *b1, M4BBox *b2)
{
	if (b2->is_set) {
		if (!b1->is_set) {
			*b1 = *b2;
		} else {
			m4_bbox_grow_point(b1, b2->min_edge);
			m4_bbox_grow_point(b1, b2->max_edge);
			m4_bbox_refresh(b1);
		}
	}
}

Bool m4_bbox_equal(M4BBox *b1, M4BBox *b2)
{
	return (m4_vec_equal(b1->min_edge, b2->min_edge) && m4_vec_equal(b1->max_edge, b2->max_edge));
}

Bool m4_bbox_point_inside(M4BBox *box, M4Vec *p)
{
	return (p->x >= box->min_edge.x && p->x <= box->max_edge.x &&
			p->y >= box->min_edge.y && p->y <= box->max_edge.y &&
			p->z >= box->min_edge.z && p->z <= box->max_edge.z);
}

/*vertices are ordered to respect p vertex indexes (vertex from bbox closer to plane)
and so that n-vertex (vertex from bbox farther from plane) is 7-p_vx_idx*/
void m4_bbox_get_vertices(M4Vec bmin, M4Vec bmax, M4Vec *vecs)
{
	vecs[0].x = vecs[1].x = vecs[2].x = vecs[3].x = bmax.x;
	vecs[4].x = vecs[5].x = vecs[6].x = vecs[7].x = bmin.x;
	vecs[0].y = vecs[1].y = vecs[4].y = vecs[5].y = bmax.y;
	vecs[2].y = vecs[3].y = vecs[6].y = vecs[7].y = bmin.y;
	vecs[0].z = vecs[2].z = vecs[4].z = vecs[6].z = bmax.z;
	vecs[1].z = vecs[3].z = vecs[5].z = vecs[7].z = bmin.z;
}


void m4_mx_apply_plane(M4Matrix *mx, M4Plane *plane)
{
	M4Vec pt, end;
	/*get pt*/
	pt = m4_vec_scale(plane->normal, -plane->d);
	m4_vec_add(end, pt, plane->normal);
	m4_mx_apply_vec(mx, &pt);
	m4_mx_apply_vec(mx, &end);
	m4_vec_diff(plane->normal, end, pt);
	m4_vec_norm(&plane->normal);
	plane->d = - m4_vec_dot(pt, plane->normal);
}

Fixed m4_plane_get_distance(M4Plane *plane, M4Vec *p)
{
	return m4_vec_dot(*p, plane->normal) + plane->d;
}


/*return p-vertex index (vertex from bbox closer to plane) - index range from 0 to 8*/
u32 m4_plane_get_p_vertex_idx(M4Plane *p)
{
	if (p->normal.x>=0) {
		if (p->normal.y>=0) return (p->normal.z>=0) ? 0 : 1;
		return (p->normal.z>=0) ? 2 : 3;
	} else {
		if (p->normal.y>=0) return (p->normal.z>=0) ? 4 : 5;
		return (p->normal.z>=0) ? 6 : 7;
	}
}


u32 m4_box_plane_relation(M4BBox *box, M4Plane *p)
{
	M4Vec nearv, farv;
	nearv = box->max_edge;
	farv = box->min_edge;
	if (p->normal.x > 0) {
		nearv.x = box->min_edge.x;
		farv.x = box->max_edge.x;
	}
	if (p->normal.y > 0) {
		nearv.y = box->min_edge.y;
		farv.y = box->max_edge.y;
	}
	if (p->normal.z > 0) {
		nearv.z = box->min_edge.z;
		farv.z = box->max_edge.z;
	}
	if (m4_vec_dot(p->normal, nearv) + p->d > 0) return M4_BOX_FRONT;
	if (m4_vec_dot(p->normal, farv) + p->d > 0) return M4_BOX_INTER;
	return M4_BOX_BACK;
}

