//////////////////////////////////////////////////////////////////////
//
//                             Pixie
//
// Copyright © 1999 - 2003, Okan Arikan
//
// Contact: okan@cs.berkeley.edu
//
// This library 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 library 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 library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
//
//  File				:	algebra,h
//  Description			:	This file defines vector and matrix types and
//							their misc operations
//
////////////////////////////////////////////////////////////////////////
#ifndef ALGEBRA_H
#define ALGEBRA_H

#include "global.h"


// Low precision routines are not available on all  platforms
#ifndef sqrtf
#define	sqrtf	(float) sqrt
#endif

#ifndef cosf
#define	cosf	(float) cos
#endif

#ifndef sinf
#define	sinf	(float) sin
#endif

#ifndef tanf
#define	tanf	(float) tan
#endif

#ifndef atan2f
#define	atan2f	(float) atan2
#endif

#ifndef powf
#define	powf	(float) pow
#endif

#ifndef logf
#define	logf	(float) log
#endif

#ifndef fmodf
#define	fmodf	(float) fmod
#endif


////////////////////////////////////////////////////////////////////////////
//
//
//
//	Matrix - vector	stuff
//
//
//
////////////////////////////////////////////////////////////////////////////




// Component indices
const	int		COMP_X						=	0;
const	int		COMP_Y						=	1;
const	int		COMP_Z						=	2;

const	int		COMP_R						=	0;
const	int		COMP_G						=	1;
const	int		COMP_B						=	2;

// Row major matrix element order (compatible with openGL)
#define	element(row,column)	(row+(column<<2))

typedef float	vector[3];									// an array of 3 floats
typedef float	htpoint[4];									// an array of 4 floats
typedef float	matrix[16];									// an array of 16 floats

// Fast inverse square root
inline	float isqrtf(float number) {
	long		i;
	float		x2, y;
	const float threehalfs = 1.5F;

	x2 = number * 0.5F;
	y  = number;
	i  = * ( long * ) &y;
	i  = 0x5f3759df - ( i >> 1 );
	y  = * ( float * ) &i;
	y  = y * ( threehalfs - ( x2 * y * y ) );
	//	y  = y * ( threehalfs - ( x2 * y * y ) );			// Da second iteration

	return y;
}


// Fast floating point absolute value
inline	float	absf(float f) {
	int tmp = (*(int*)&f) & 0x7FFFFFFF;
	return *(float*)&tmp;
}

inline	void	initv(float *r,const float x) {
	r[COMP_X]	=	x;
	r[COMP_Y]	=	x;
	r[COMP_Z]	=	x;
}

inline	void	initv(float *r,const float x,const float y,const float z) {
	r[COMP_X]	=	x;
	r[COMP_Y]	=	y;
	r[COMP_Z]	=	z;
}

inline	void	addvv(float *result,const float *s1,const float *s2) {
	result[0]	=	s1[0] + s2[0];
	result[1]	=	s1[1] + s2[1];
	result[2]	=	s1[2] + s2[2];
}

inline	void	addvv(float *result,const float *s1) {
	result[0]	+=	s1[0];
	result[1]	+=	s1[1];
	result[2]	+=	s1[2];
}

inline	void	addvf(float *result,const float s1) {
	result[0]	+=	s1;
	result[1]	+=	s1;
	result[2]	+=	s1;
}

inline	void	subvf(float *result,const float s1) {
	result[0]	-=	s1;
	result[1]	-=	s1;
	result[2]	-=	s1;
}

inline	void	addvf(float *result,const float *s1,const float s2) {
	result[0]	=	s1[0] + s2;
	result[1]	=	s1[1] + s2;
	result[2]	=	s1[2] + s2;
}

inline	void	subvf(float *result,const float *s1,const float s2) {
	result[0]	=	s1[0] - s2;
	result[1]	=	s1[1] - s2;
	result[2]	=	s1[2] - s2;
}

inline	void	subvv(float *result,const float *s1,const float *s2) {
	result[0]	=	s1[0] - s2[0];
	result[1]	=	s1[1] - s2[1];
	result[2]	=	s1[2] - s2[2];
}

inline	void	subvv(float *result,const float *s1) {
	result[0]	-=	s1[0];
	result[1]	-=	s1[1];
	result[2]	-=	s1[2];
}

inline	void	mulvf(float *result,const float *v,const float m) {
	result[0]	=	v[0]*m;
	result[1]	=	v[1]*m;
	result[2]	=	v[2]*m;
}

inline	void	mulvf(float *result,const float m) {
	result[0]	*=	m;
	result[1]	*=	m;
	result[2]	*=	m;
}

inline	void	mulvv(float	*result,const float *s1,const float *s2) {
	result[0]	=	s1[0]*s2[0];
	result[1]	=	s1[1]*s2[1];
	result[2]	=	s1[2]*s2[2];
}

inline	void	mulvv(float *result,const float *s1) {
	result[0]	*=	s1[0];
	result[1]	*=	s1[1];
	result[2]	*=	s1[2];
}

inline	void	divvv(float *result,const float *s1,const float *s2) {
	result[0]	=	s1[0] / s2[0];
	result[1]	=	s1[1] / s2[1];
	result[2]	=	s1[2] / s2[2];
}

inline	void	divvv(float *result,const float *s1) {
	result[0]	/=	s1[0];
	result[1]	/=	s1[1];
	result[2]	/=	s1[2];
}

inline	void	crossvv(float *result,const float *s1,const float *s2) {
	result[COMP_X]	=	(s1[COMP_Y]*s2[COMP_Z] - s1[COMP_Z]*s2[COMP_Y]);
	result[COMP_Y]	=	(s1[COMP_Z]*s2[COMP_X] - s1[COMP_X]*s2[COMP_Z]);
	result[COMP_Z]	=	(s1[COMP_X]*s2[COMP_Y] - s1[COMP_Y]*s2[COMP_X]);
}

inline	float	dotvv(const float *s1,const float *s2) {
	return (float) (s1[COMP_X]*s2[COMP_X] + s1[COMP_Y]*s2[COMP_Y] + s1[COMP_Z]*s2[COMP_Z]);
}

inline	void	interpolatev(float *result,const float *s1,const float *s2,const float alpha) {
	result[0]	=	s1[0]*(1-alpha) + s2[0]*alpha;
	result[1]	=	s1[1]*(1-alpha) + s2[1]*alpha;
	result[2]	=	s1[2]*(1-alpha) + s2[2]*alpha;
}

inline	void	movvv(float *dest,const float *src) {
	dest[0]		=	src[0];
	dest[1]		=	src[1];
	dest[2]		=	src[2];
}

inline	float	lengthv(const float *v) {
	return sqrtf(dotvv(v,v));
}

inline	void	normalizev(float *r,const float *v) {
	const double	l	=	1 / sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);

	r[0]	=	(float) (v[0]*l);
	r[1]	=	(float) (v[1]*l);
	r[2]	=	(float) (v[2]*l);
}

inline	void	normalizevf(float *r,const float *v) {
	const float	l	=	isqrtf(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);

	r[0]	=	(float) (v[0]*l);
	r[1]	=	(float) (v[1]*l);
	r[2]	=	(float) (v[2]*l);
}


inline	void	normalizev(float *v) {
	const double	l	=	1 / sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);

	v[0]	=	(float) (v[0]*l);
	v[1]	=	(float) (v[1]*l);
	v[2]	=	(float) (v[2]*l);
}

inline	void	normalizevf(float *v) {
	const float	l	=	isqrtf(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);

	v[0]	=	(float) (v[0]*l);
	v[1]	=	(float) (v[1]*l);
	v[2]	=	(float) (v[2]*l);
}

inline	void	mulmm(float *result,const float *s1,const float *s2) {
	int i,j,k;

	for (i=0;i<4;i++)
		for (j=0;j<4;j++) {
				double		dest		=	0;

				for (k=0;k<4;k++) dest	+=	s1[element(i,k)]*s2[element(k,j)];

				result[element(i,j)]	=	(float) dest;
		}
}

inline	void	mulmp(float *result,const float *s1,const float *s2) {
	const	float	x		=	s1[element(0,0)]*s2[0]+s1[element(0,1)]*s2[1]+s1[element(0,2)]*s2[2]+s1[element(0,3)];
	const	float	y		=	s1[element(1,0)]*s2[0]+s1[element(1,1)]*s2[1]+s1[element(1,2)]*s2[2]+s1[element(1,3)];
	const	float	z		=	s1[element(2,0)]*s2[0]+s1[element(2,1)]*s2[1]+s1[element(2,2)]*s2[2]+s1[element(2,3)];
	const	float	w		= 	s1[element(3,0)]*s2[0]+s1[element(3,1)]*s2[1]+s1[element(3,2)]*s2[2]+s1[element(3,3)];
	

	if (w != 1) {
		const double	l	=	1 / (double) w;

		result[0]	=	(float) (x*l);
		result[1]	=	(float) (y*l);
		result[2]	=	(float) (z*l);
	} else {
		result[0]	=	x;
		result[1]	=	y;
		result[2]	=	z;
	}
}

inline	void	mulmv(float *result,const float *s1,const float *s2) {
	const	float	x		=	s1[element(0,0)]*s2[0]+s1[element(0,1)]*s2[1]+s1[element(0,2)]*s2[2];
	const	float	y		=	s1[element(1,0)]*s2[0]+s1[element(1,1)]*s2[1]+s1[element(1,2)]*s2[2];
	const	float	z		=	s1[element(2,0)]*s2[0]+s1[element(2,1)]*s2[1]+s1[element(2,2)]*s2[2];
	
	result[0]	=	x;
	result[1]	=	y;
	result[2]	=	z;
}

inline	void	mulmn(float *result,const float *s1,const float *s2) {
	const	float	x		=	s1[element(0,0)]*s2[0]+s1[element(1,0)]*s2[1]+s1[element(2,0)]*s2[2];
	const	float	y		=	s1[element(0,1)]*s2[0]+s1[element(1,1)]*s2[1]+s1[element(2,1)]*s2[2];
	const	float	z		=	s1[element(0,2)]*s2[0]+s1[element(1,2)]*s2[1]+s1[element(2,2)]*s2[2];

	result[0]	=	x;
	result[1]	=	y;
	result[2]	=	z;
}

inline	void	mulmp4(float *result,const float *s1,const float *s2) {
	const	float	x		=	s1[element(0,0)]*s2[0]+s1[element(0,1)]*s2[1]+s1[element(0,2)]*s2[2]+s1[element(0,3)]*s2[3];
	const	float	y		=	s1[element(1,0)]*s2[0]+s1[element(1,1)]*s2[1]+s1[element(1,2)]*s2[2]+s1[element(1,3)]*s2[3];
	const	float	z		=	s1[element(2,0)]*s2[0]+s1[element(2,1)]*s2[1]+s1[element(2,2)]*s2[2]+s1[element(2,3)]*s2[3];
	const	float	w		= 	s1[element(3,0)]*s2[0]+s1[element(3,1)]*s2[1]+s1[element(3,2)]*s2[2]+s1[element(3,3)]*s2[3];
	

	result[0]	=	x;
	result[1]	=	y;
	result[2]	=	z;
	result[3]	=	w;
}


inline	void	mulmv4(float *result,const float *s1,const float *s2) {
	const	float	x		=	s1[element(0,0)]*s2[0]+s1[element(0,1)]*s2[1]+s1[element(0,2)]*s2[2];
	const	float	y		=	s1[element(1,0)]*s2[0]+s1[element(1,1)]*s2[1]+s1[element(1,2)]*s2[2];
	const	float	z		=	s1[element(2,0)]*s2[0]+s1[element(2,1)]*s2[1]+s1[element(2,2)]*s2[2];
	const	float	w		= 	s1[element(3,0)]*s2[0]+s1[element(3,1)]*s2[1]+s1[element(3,2)]*s2[2];
	
	result[0]	=	x;
	result[1]	=	y;
	result[2]	=	z;
	result[3]	=	w;
}

inline	void	mulmn4(float *result,const float *s1,const float *s2) {
	const	float	x		=	s1[element(0,0)]*s2[0]+s1[element(1,0)]*s2[1]+s1[element(2,0)]*s2[2];
	const	float	y		=	s1[element(0,1)]*s2[0]+s1[element(1,1)]*s2[1]+s1[element(2,1)]*s2[2];
	const	float	z		=	s1[element(0,2)]*s2[0]+s1[element(1,2)]*s2[1]+s1[element(2,2)]*s2[2];
	const	float	w		= 	s1[element(0,3)]*s2[0]+s1[element(1,3)]*s2[1]+s1[element(2,3)]*s2[2];

	result[0]	=	x;
	result[1]	=	y;
	result[2]	=	z;
	result[3]	=	w;
}

inline	void	mulpm(float  *result,const float *s1,const float *s2) {
	const	float	x		=	s1[0]*s2[element(0,0)] + s1[1]*s2[element(1,0)] + s1[2]*s2[element(2,0)] + s2[element(3,0)];
	const	float	y		=	s1[0]*s2[element(0,1)] + s1[1]*s2[element(1,1)] + s1[2]*s2[element(2,1)] + s2[element(3,1)];
	const	float	z		=	s1[0]*s2[element(0,2)] + s1[1]*s2[element(1,2)] + s1[2]*s2[element(2,2)] + s2[element(3,2)];
	const	float	w		=	s1[0]*s2[element(0,3)] + s1[1]*s2[element(1,3)] + s1[2]*s2[element(2,3)] + s2[element(3,3)];

	if (w != 1) {
		const double	l	=	1 / (double) w;

		result[0]	=	(float) (x*l);
		result[1]	=	(float) (y*l);
		result[2]	=	(float) (z*l);
	} else {
		result[0]	=	x;
		result[1]	=	y;
		result[2]	=	z;
	}
}

inline	void	mulpm4(float *result,const float *s1,const float *s2) {
	const	float	x		=	s1[0]*s2[element(0,0)] + s1[1]*s2[element(1,0)] + s1[2]*s2[element(2,0)] + s1[3]*s2[element(3,0)];
	const	float	y		=	s1[0]*s2[element(0,1)] + s1[1]*s2[element(1,1)] + s1[2]*s2[element(2,1)] + s1[3]*s2[element(3,1)];
	const	float	z		=	s1[0]*s2[element(0,2)] + s1[1]*s2[element(1,2)] + s1[2]*s2[element(2,2)] + s1[3]*s2[element(3,2)];
	const	float	w		=	s1[0]*s2[element(0,3)] + s1[1]*s2[element(1,3)] + s1[2]*s2[element(2,3)] + s1[3]*s2[element(3,3)];
	

	result[0]	=	x;
	result[1]	=	y;
	result[2]	=	z;
	result[3]	=	w;
}


inline	void	mulvm(float  *result,const float *s1,const float *s2) {
	const	float	x		=	s1[0]*s2[element(0,0)] + s1[1]*s2[element(1,0)] + s1[2]*s2[element(2,0)];
	const	float	y		=	s1[0]*s2[element(0,1)] + s1[1]*s2[element(1,1)] + s1[2]*s2[element(2,1)];
	const	float	z		=	s1[0]*s2[element(0,2)] + s1[1]*s2[element(1,2)] + s1[2]*s2[element(2,2)];

	result[0]	=	x;
	result[1]	=	y;
	result[2]	=	z;
}

inline	void	mulmf(float  *result,const float *s,const float t) {
	int	i;

	for (i=0;i<16;i++)
		result[i]	=	s[i]*t;
}

inline	void	mulmf(float  *result,const float t) {
	int	i;

	for (i=0;i<16;i++)
		result[i]	*=	t;
}

inline	void	addmm(float  *result,const float *s1,const float *s2) {
	int	i;

	for (i=0;i<16;i++)
		result[i]	=	s1[i] + s2[i];
}

inline	void	submm(float  *result,const float *s1,const float *s2) {
	int	i;

	for (i=0;i<16;i++)
		result[i]	=	s1[i] - s2[i];
}

inline	void	addmm(float  *result,const float *s) {
	int	i;

	for (i=0;i<16;i++)
		result[i]	+=	s[i];
}

inline	void	submm(float  *result,const float *s) {
	int	i;

	for (i=0;i<16;i++)
		result[i]	-=	s[i];
}


inline	void	movmm(float *dest,const float *src) {
	int i;

	for (i=0;i<16;i++)
		dest[i]	=	src[i];
}

inline void		transposem(float *dest,const float *src) {
	int i,j;

	for (i=0;i<4;i++) {
		for (j=0;j<4;j++) {
			dest[element(i,j)] = src[element(j,i)];
		}
	}
}

inline	float	area(float x1,float y1,float x2,float y2,float x3,float y3) {
	const double	acx = x1 - x3;
	const double	bcx = x2 - x3;
	const double	acy = y1 - y3;
	const double	bcy = y2 - y3;
	return (float) (acx * bcy - acy * bcx);
}

inline double	aread(double x1,double y1,double x2,double y2,double x3,double y3) {
	const double	acx = x1 - x3;
	const double	bcx = x2 - x3;
	const double	acy = y1 - y3;
	const double	bcy = y2 - y3;
	return acx * bcy - acy * bcx;
}

inline	void	mulmvv(float *dest,const float *s1,const float *s2) {
	dest[element(0,0)]	=	s1[0]*s2[0];
	dest[element(1,0)]	=	s1[1]*s2[0];
	dest[element(2,0)]	=	s1[2]*s2[0];
	dest[element(3,0)]	=	0;
	dest[element(0,1)]	=	s1[0]*s2[1];
	dest[element(1,1)]	=	s1[1]*s2[1];
	dest[element(2,1)]	=	s1[2]*s2[1];
	dest[element(3,1)]	=	0;
	dest[element(0,2)]	=	s1[0]*s2[2];
	dest[element(1,2)]	=	s1[1]*s2[2];
	dest[element(2,2)]	=	s1[2]*s2[2];
	dest[element(3,2)]	=	0;
	dest[element(0,3)]	=	0;
	dest[element(1,3)]	=	0;
	dest[element(2,3)]	=	0;
	dest[element(3,3)]	=	1;
}

inline	float	clampf(const float mi,const float l,const float ma) {
	return (l < mi ? mi : (l > ma ? ma : l));
}

inline	void	clampv(const float *mi,float *l,const float *ma) {
	l[0]	=	(l[0] < mi[0] ? mi[0] : (l[0] > ma[0] ? ma[0] : l[0]));
	l[1]	=	(l[1] < mi[1] ? mi[1] : (l[1] > ma[1] ? ma[1] : l[1]));
	l[2]	=	(l[2] < mi[2] ? mi[2] : (l[2] > ma[2] ? ma[2] : l[2]));
}





////////////////////////////////////////////////////////////////////////////
//
//
//
//	Misc box routines
//
//
//
////////////////////////////////////////////////////////////////////////////




// True if point v is inside the box (bmin,bmax)
inline	int		inBox(const float *bmin,const float *bmax,const float *v) {
	int	i;

	for (i=0;i<3;i++) {
		if ((v[i] < bmin[i]) || (v[i] > bmax[i])) return FALSE;
	}

	return TRUE;
}



// Expand the box (bmin,bmax) so that point v is inside it
inline	void	addBox(float *bmin,float *bmax,const float *v) {
	int	i;

	for (i=0;i<3;i++) {
		if (v[i] < bmin[i])	bmin[i]	=	v[i];
		if (v[i] > bmax[i]) bmax[i] =	v[i];
	}
}

// True if two given boxes intersect
inline	int		intersectBox(const float *bmin1,const float *bmax1,const float *bmin2,const float *bmax2) {
	int	i;

	for (i=0;i<3;i++) {
		if ((bmin1[i] > bmax2[i]) || (bmax1[i] < bmin2[i]))	return	FALSE;
	}

	return	TRUE;
}








////////////////////////////////////////////////////////////////////////////
//
//
//
//	Misc polynomial solving
//
//
//
////////////////////////////////////////////////////////////////////////////


#define     cbrt(x)     ((x) > 0.0 ? pow(x, 1.0/3.0) : \
						((x) < 0.0 ? -pow(-x, 1.0/3.0) : 0.0))

template <class T> inline	int	solveQuadric(T a,T b,T c,T *r) {
	double	inva	=	1	/	a;
	double	nc		=	c*inva;
	double	nb		=	b*inva*0.5;
	double	delta	=	nb*nb - nc;

	if (delta < 0)	return	0;
	else {
		double	sqrtDelta	=	sqrt(delta);

		r[0]	=	(T) (-sqrtDelta - nb);
		r[1]	=	(T) (sqrtDelta - nb);

		return 2;
	}
}

template <class T> inline	int solveCubic(T c[4],T s[3]) {
    int     i, num;
    double	sub;
    double	A, B, C;
    double	sq_A, p, q;
    double	cb_p, D;
	double	sd[3];

    A		= c[2] / c[3];
    B		= c[1] / c[3];
    C		= c[0] / c[3];

    sq_A	= A * A;
    p		= (1.0/3 * (- 1.0/3 * sq_A + B));
    q		= (1.0/2 * (2.0/27 * A * sq_A - 1.0/3 * A * B + C));

    cb_p	= p * p * p;
    D		= q * q + cb_p;

    if (D == 0) {
		if (q == 0) {
			sd[0]		= 0;
			num			= 1;
		} else 	{
			double u	= (cbrt(-q));
			sd[0]		= 2 * u;
			sd[1]		= -u;
			num			= 2;
		}
    } else if (D < 0) {
		double phi		= (1.0/3 * acos(-q / sqrt(-cb_p)));
		double t		= (2 * sqrt(-p));

		sd[0]			=   t * cos(phi);
		sd[1]			= - t * cos(phi + C_PI / 3);
		sd[2]			= - t * cos(phi - C_PI / 3);
		num				= 3;
    } else {
		double sqrt_D	= sqrt(D);
		double u		= cbrt(sqrt_D - q);
		double v		= -cbrt(sqrt_D + q);

		sd[0]			= (u + v);
		num				= 1;
    }

    sub = (1.0/3 * A);

    for (i=0;i<num;++i)
		s[i] = (T) (sd[i] - sub);

    return num;
}


template <class T> inline	int solveQuartic(T c[5],T s[4]) {
    double		coeffs[4];
    double		z, u, v, sub;
    double		A, B, C, D;
    double		sq_A, p, q, r;
    int			i, num;
	double		sd[4];

    A = c[ 3 ] / c[ 4 ];
    B = c[ 2 ] / c[ 4 ];
    C = c[ 1 ] / c[ 4 ];
    D = c[ 0 ] / c[ 4 ];


    sq_A	= A * A;
    p		= (- 3.0/8 * sq_A + B);
    q		= (1.0/8 * sq_A * A - 1.0/2 * A * B + C);
    r		= (-3.0/256*sq_A*sq_A + 1.0/16*sq_A*B - 1.0/4*A*C + D);

    if (r == 0) {
		coeffs[ 0 ] = q;
		coeffs[ 1 ] = p;
		coeffs[ 2 ] = 0;
		coeffs[ 3 ] = 1;

		num			= solveCubic<double>(coeffs, sd);

		sd[num++]	= 0;
    } else {
		coeffs[ 0 ] = 1.0/2 * r * p - 1.0/8 * q * q;
		coeffs[ 1 ] = - r;
		coeffs[ 2 ] = - 1.0/2 * p;
		coeffs[ 3 ] = 1;

		solveCubic<double>(coeffs, sd);

		z = sd[ 0 ];

		u = z * z - r;
		v = 2 * z - p;

		if (u == 0)
			u = 0;
		else if (u > 0)
			u = sqrt(u);
		else
			return 0;

		if (v == 0)
			v = 0;
		else if (v > 0)
			v = sqrt(v);
		else
			return 0;

		coeffs[ 0 ] = z - u;
		coeffs[ 1 ] = q < 0 ? -v : v;
		coeffs[ 2 ] = 1;

		num = solveQuadric<double>(coeffs[2],coeffs[1],coeffs[0],sd);

		coeffs[ 0 ]= z + u;
		coeffs[ 1 ] = q < 0 ? v : -v;
		coeffs[ 2 ] = 1;

		num += solveQuadric<double>(coeffs[2],coeffs[1],coeffs[0],sd + num);
    }

    sub = 1.0/4 * A;

    for (i = 0; i < num; ++i) {
		s[i] = (T) (sd[i] - sub);
	}

    return num;
}






////////////////////////////////////////////////////////////////////////////
//
//
//
//	Reflection / Refraction
//
//
//
////////////////////////////////////////////////////////////////////////////






// Reflect I around N
inline	void	reflect(float *r,const float *I,const float *N) {
	mulvf(r,N,-2*dotvv(N,I));
	addvv(r,I);
}

// Refrect I around N
inline	void	refract(float *r,const float *I,const float *N,float eta) {
	vector	vtmp;
	float	IdotN,k;
	IdotN	=	dotvv(N,I); 
	k		=	1 - eta*eta*(1-IdotN*IdotN);
	if (k < 0) {
		initv(r,0,0,0);
	} else	{
		mulvf(vtmp,N,(eta*IdotN+sqrtf(k)));
		mulvf(r,I,eta);
		subvv(r,vtmp);
	}
}

// The fresnel function
inline	void	fresnel(const float *I,const float *N,float eta,float &Kr,float &Kt,float *R,float *T) {
	const float	e		=	1 / eta;
	const float	c		=	-dotvv(I,N);
	const float	t		=	e*e+c*c-1;
	const float	g		=	sqrtf(max(t,0));
	const float	a		=	(g - c) / (g + c);
	const float	b		=	(c*(g+c) - 1) / (c*(g-c) + 1);

	Kr			=	0.5f*a*a*(1 + b*b);
	Kr			=	min(Kr,1);
	Kr			=	max(Kr,0);
	Kt			=	1 - Kr;
	reflect(R,I,N);
	refract(T,I,N,eta);
}



////////////////////////////////////////////////////////////////////////////
//
//
//
//	Misc matrix operations that are too long to implement as inline
//	These are defined in algebra.cpp
//
//
//
////////////////////////////////////////////////////////////////////////////



void	skewsymm(float *,const float *);								// Create a skew symmetric matrix from matrix
float	determinantm(const float *);									// Take the determinant of a 4x4 matrix
void	identitym(float *);												// Construct identity matrix
void	translatem(float *,const float,const float,const float);		// Construct translate matrix
void	scalem(float *,const float,const float,const float);			// Construct scale matrix
void	rotatem(float *,const float *,const float);						// Construct rotate matrix (the second argument is the rotation vector)
void	rotatem(float *,float,float,float,const float);					// Construct rotate matrix (the arguments are: result,x,y,z coordinates of the rotation vector and angle to rotate)
void	rotatem(float *,const float *);
void	skewm(float *,const float,const float,const float,const float,const float,const float,const float);
int		invertm(float *,const float *);									// Invert a matrix. Returns 0 if the inversion is successful 1 otherwise (i.e. matrix is singular)

#endif