//////////////////////////////////////////////////////////////////////
//
//                             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.cpp
//  Classes				:	-
//  Description			:	This file implements some basic algebra operations
//
////////////////////////////////////////////////////////////////////////
#include <math.h>

#include "algebra.h"


// First some misc functions used in matrix inversion



///////////////////////////////////////////////////////////////////////
// Function				:	det2x2
// Description			:	2 by 2 determinant computation
// Return Value			:	Determinant
// Comments				:
// Date last edited		:	6/20/2002
// Matrix:
// | a b |
// | c d |
static	inline double det2x2( const double a,const double  b,const double  c,const double  d) {
	return  a * d - b * c;
}

///////////////////////////////////////////////////////////////////////
// Function				:	det3x3
// Description			:	3 by 3 determinant computation
// Return Value			:	Determinant
// Comments				:
// Date last edited		:	6/20/2002
// Matrix:
// | a1 a2 a3 |
// | b1 b2 b3 |
// | c1 c2 c3 |
static	inline double det3x3(const double  a1,const double  a2,const double  a3,const double  b1,const double  b2,const double  b3,const double  c1,const double  c2,const double  c3 ) {
    return	  a1 * det2x2( b2, b3, c2, c3 )
			- b1 * det2x2( a2, a3, c2, c3 )
			+ c1 * det2x2( a2, a3, b2, b3 );
}

///////////////////////////////////////////////////////////////////////
// Function				:	det4x4
// Description			:	4 by 4 determinant computation
// Return Value			:	Determinant
// Comments				:
// Date last edited		:	6/20/2002
// Matrix:
// | a1 a2 a3 a4 |
// | b1 b2 b3 b4 |
// | c1 c2 c3 c4 |
// | d1 d2 d3 d4 |
static	inline double det4x4( const float *m ) {
    double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;

	a1 = m[element(0,0)]; b1 = m[element(0,1)]; 
	c1 = m[element(0,2)]; d1 = m[element(0,3)];

	a2 = m[element(1,0)]; b2 = m[element(1,1)]; 
	c2 = m[element(1,2)]; d2 = m[element(1,3)];

	a3 = m[element(2,0)]; b3 = m[element(2,1)]; 
	c3 = m[element(2,2)]; d3 = m[element(2,3)];

	a4 = m[element(3,0)]; b4 = m[element(3,1)]; 
	c4 = m[element(3,2)]; d4 = m[element(3,3)];

    return	  a1 * det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4)
			- b1 * det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4)
			+ c1 * det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4)
			- d1 * det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
}

///////////////////////////////////////////////////////////////////////
// Function				:	adjoint
// Description			:	Compute the adjoint of a 4 by 4 matrix
// Return Value			:	-
// Comments				:
// Date last edited		:	6/20/2002
// Construct the adjoint matrix
// | a1 a2 a3 a4 |
// | b1 b2 b3 b4 |
// | c1 c2 c3 c4 |
// | d1 d2 d3 d4 |

static	inline void adjoint( float *out,const float *in) {
    float a1, a2, a3, a4, b1, b2, b3, b4;
    float c1, c2, c3, c4, d1, d2, d3, d4;

	a1 = in[element(0,0)]; b1 = in[element(0,1)]; 
	c1 = in[element(0,2)]; d1 = in[element(0,3)];

	a2 = in[element(1,0)]; b2 = in[element(1,1)]; 
	c2 = in[element(1,2)]; d2 = in[element(1,3)];

	a3 = in[element(2,0)]; b3 = in[element(2,1)];
	c3 = in[element(2,2)]; d3 = in[element(2,3)];

	a4 = in[element(3,0)]; b4 = in[element(3,1)]; 
	c4 = in[element(3,2)]; d4 = in[element(3,3)];

    out[element(0,0)]  = (float)  det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4);
    out[element(1,0)]  = (float) -det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4);
    out[element(2,0)]  = (float)  det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4);
    out[element(3,0)]  = (float) -det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
        
    out[element(0,1)]  = (float) -det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4);
    out[element(1,1)]  = (float)  det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4);
    out[element(2,1)]  = (float) -det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4);
    out[element(3,1)]  = (float)  det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4);
        
    out[element(0,2)]  = (float)  det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4);
    out[element(1,2)]  = (float) -det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4);
    out[element(2,2)]  = (float)  det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4);
    out[element(3,2)]  = (float) -det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4);
        
    out[element(0,3)]  = (float) -det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3);
    out[element(1,3)]  = (float)  det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3);
    out[element(2,3)]  = (float) -det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3);
    out[element(3,3)]  = (float)  det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3);
}

///////////////////////////////////////////////////////////////////////
// Function				:	invert
// Description			:	invert a 4 by 4 matrix
// Return Value			:	Determinant
// Comments				:
// Date last edited		:	6/20/2002
int			invertm(float *d,const float *s)	{
	int		i;
    double	det;

    adjoint(d,s);

    det = det4x4(s);

	if (det == 0) {
		identitym(d);
        return TRUE;
	}

    for (i=0; i<16; i++)
		d[i]		=	(float) (d[i] / det);

	return FALSE;
}

///////////////////////////////////////////////////////////////////////
// Function				:	skewsymm
// Description			:	compute the skewed symmetic matrix
// Return Value			:	-
// Comments				:
// Date last edited		:	6/20/2002
void	skewsymm(float *dest,const float *src) {
	identitym(dest);

	dest[element(0,0)]	=	0;
	dest[element(1,1)]	=	0;
	dest[element(2,2)]	=	0;
	dest[element(0,1)]	=	-src[COMP_Z];
	dest[element(0,2)]	=	src[COMP_Y];
	dest[element(1,2)]	=	-src[COMP_X];
	dest[element(1,0)]	=	src[COMP_Z];
	dest[element(2,0)]	=	-src[COMP_Y];
	dest[element(2,1)]	=	src[COMP_X];
}

///////////////////////////////////////////////////////////////////////
// Function				:	determinantm
// Description			:	4 by 4 determinant computation
// Return Value			:	Determinant
// Comments				:
// Date last edited		:	6/20/2002
float	determinantm(const float *r) {
	return (float) det4x4(r);
}

///////////////////////////////////////////////////////////////////////
// Function				:	identitym
// Description			:	create identity matrix
// Return Value			:	-
// Comments				:
// Date last edited		:	6/20/2002
void	identitym(float *r) {
	int i;

	for (i=0;i<16;i++) r[i]	=	0;

	r[element(0,0)]	=	1;
	r[element(1,1)]	=	1;
	r[element(2,2)]	=	1;
	r[element(3,3)]	=	1;
}

///////////////////////////////////////////////////////////////////////
// Function				:	translatem
// Description			:	create translation matrix
// Return Value			:	-
// Comments				:
// Date last edited		:	6/20/2002
void	translatem(float *r,const float tx,const float ty,const float tz) {
	identitym(r);

	r[element(0,3)]	=	tx;
	r[element(1,3)]	=	ty;
	r[element(2,3)]	=	tz;
}

///////////////////////////////////////////////////////////////////////
// Function				:	scalem
// Description			:	create scale matrix
// Return Value			:	Determinant
// Comments				:
// Date last edited		:	6/20/2002
void	scalem(float *r,const float sx,const float sy,const float sz) {
	identitym(r);

	r[element(0,0)]	=	sx;
	r[element(1,1)]	=	sy;
	r[element(2,2)]	=	sz;
}

///////////////////////////////////////////////////////////////////////
// Function				:	rotatem
// Description			:	create a rotation matrix
// Return Value			:	-
// Comments				:	the angle is in degrees
// Date last edited		:	6/20/2002
void	rotatem(float *r,const float *v,const float angle) {
	rotatem(r,v[COMP_X],v[COMP_Y],v[COMP_Z],angle);
}

///////////////////////////////////////////////////////////////////////
// Function				:	rotatem
// Description			:	create a rotation matrix
// Return Value			:	-
// Comments				:	the angle is in degrees
// Date last edited		:	6/20/2002
void	rotatem(float *r,float x,float y,float z,const float angle) {
	double	s	=	cos(radians(angle/2));
	double	sp	=	sin(radians(angle/2));
	double	l	=	sqrt(x*x + y*y + z*z);
	double	a	=	x*sp/l;
	double	b	=	y*sp/l;
	double	c	=	z*sp/l;
	l			=	sqrt(a*a + b*b + c*c + s*s);
	a			/=	l;
	b			/=	l;
	c			/=	l;
	s			/=	l;

	r[element(0,0)] = (float) (1-2*b*b-2*c*c);
	r[element(0,1)] = (float) (2*a*b-2*s*c);
	r[element(0,2)] = (float) (2*a*c+2*s*b);
	r[element(0,3)] = 0;
	r[element(1,0)] = (float) (2*a*b+2*s*c);
	r[element(1,1)] = (float) (1-2*a*a-2*c*c);
	r[element(1,2)] = (float) (2*b*c-2*s*a);
	r[element(1,3)] = 0;
	r[element(2,0)] = (float) (2*a*c-2*s*b);
	r[element(2,1)] = (float) (2*b*c+2*s*a);
	r[element(2,2)] = (float) (1-2*a*a-2*b*b);
	r[element(2,3)] = 0;
	r[element(3,0)] = 0;
	r[element(3,1)] = 0;
	r[element(3,2)] = 0;
	r[element(3,3)] = 1;
}

///////////////////////////////////////////////////////////////////////
// Function				:	skewm
// Description			:	create a skew matrix
// Return Value			:	-
// Comments				:	the angle is in degrees
// Date last edited		:	6/20/2002
void	skewm(float *r,const float angle,const float dx1,const float dy1,const float dz1,const float dx2,const float dy2,const float dz2) {
	vector	v1,v2,d;
	matrix	R,S,tmp,tmp2;
	float	costheta,sintheta,skew;

	v1[COMP_X]	=	dx1;
	v1[COMP_Y]	=	dy1;
	v1[COMP_Z]	=	dz1;
	normalizev(v1);
	
	v2[COMP_X]	=	dx2;
	v2[COMP_Y]	=	dy2;
	v2[COMP_Z]	=	dz2;
	normalizev(v2);

	crossvv(d,v1,v2);
	normalizev(d);

	costheta	=	dotvv(v1,v2);
	sintheta	=	sqrtf(1 - costheta*costheta);
	skew		=	(float) tan(radians(angle) + acos(sintheta))*sintheta - costheta;

	crossvv(v1,d,v2);
	normalizev(v1);

	R[element(0,0)]	=	d[COMP_X];
	R[element(1,0)]	=	d[COMP_Y];
	R[element(2,0)]	=	d[COMP_Z];
	R[element(3,0)]	=	0;
	R[element(0,1)]	=	v1[COMP_X];
	R[element(1,1)]	=	v1[COMP_Y];
	R[element(2,1)]	=	v1[COMP_Z];
	R[element(3,1)]	=	0;
	R[element(0,2)]	=	v2[COMP_X];
	R[element(1,2)]	=	v2[COMP_Y];
	R[element(2,2)]	=	v2[COMP_Z];
	R[element(3,2)]	=	0;
	R[element(0,3)]	=	0;
	R[element(1,3)]	=	0;
	R[element(2,3)]	=	0;
	R[element(3,3)]	=	1;

	identitym(S);
	S[element(2,1)]	=	-skew;

	transposem(tmp,R);
	mulmm(tmp2,S,tmp);
	mulmm(r,R,tmp2);
}


#define	S		0
#define	VX		1
#define	VY		2
#define	VZ		3

///////////////////////////////////////////////////////////////////////
// Function				:	rotatem
// Description			:	create a rotation matrix
// Return Value			:	-
// Comments				:
// Date last edited		:	6/20/2002
void	rotatem(float *to,const float *from) {
	to[element(0,0)]	=	1-2*from[VY]*from[VY] - 2*from[VZ]*from[VZ];
	to[element(0,1)]	=	2*from[VX]*from[VY] - 2*from[S]*from[VZ];
	to[element(0,2)]	=	2*from[VX]*from[VZ] + 2*from[S]*from[VY];
	to[element(1,0)]	=	2*from[VX]*from[VY] + 2*from[S]*from[VZ];
	to[element(1,1)]	=	1-2*from[VX]*from[VX] - 2*from[VZ]*from[VZ];
	to[element(1,2)]	=	2*from[VY]*from[VZ] - 2*from[S]*from[VX];
	to[element(2,0)]	=	2*from[VX]*from[VZ] - 2*from[S]*from[VY];
	to[element(2,1)]	=	2*from[VY]*from[VZ] + 2*from[S]*from[VX];
	to[element(2,2)]	=	1-2*from[VX]*from[VX] - 2*from[VY]*from[VY];
	to[element(0,3)]	=	0;
	to[element(1,3)]	=	0;
	to[element(2,3)]	=	0;
	to[element(3,3)]	=	1;
	to[element(3,0)]	=	0;
	to[element(3,1)]	=	0;
	to[element(3,2)]	=	0;
}

#undef S
#undef VX
#undef VY
#undef VZ

