/*
===========================================================================
maya2q3 - export .md3 files from maya
Copyright (C) 2005 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.
===========================================================================
*/

#include "common.h"

/*
Calculate the eigenvalues and eigenvectors for a 3x3 symmetric matrix.
Code based on that in Mathematics for 3D Game Programming & Computer Graphics,
Second Edition by Eric Lengyel.
*/
#define EigenSys_Epsilon 1.0E-10F
#define EigenSys_MaxSweeps 32

void CalculateEigensystem3x3s(
							  //in, the matrix
							  float m11,
							  float m21, float m22,
							  float m31, float m32, float m33,

							  //out, the eigenvectors (must be float[3])
							  float *er, float *es, float *et,
							  //out, the eigenvalues corresponding to r, s, t (must be float[3])
							  float *el
							  )
{
	assert( er && es && et && el, "can't output to null values" );

    er[0] = 1.0F; er[1] = 0.0F; er[2] = 0.0F;
	es[0] = 0.0f; es[1] = 1.0F; es[2] = 0.0F;
	et[0] = 0.0f; et[1] = 0.0F; et[2] = 1.0F;

	for( int a = 0; a < EigenSys_MaxSweeps; a++ )
	{
		//exit if off-diagonal entries small enough
		if( fabsf( m21 ) < EigenSys_Epsilon &&
			fabsf( m31 ) < EigenSys_Epsilon &&
			fabsf( m32 ) < EigenSys_Epsilon )
			break;

        //annihilate 21 entry
		if( m21 != 0.0F )
		{
			float u = (m22 - m11) * 0.5F / m21;
			float u2 = u * u;
			float u2p1 = u2 + 1.0F;
			float t = (u2p1 != u2) ? ((u < 0.0F) ? -1.0F : 1.0F) * (sqrtf( u2p1 ) - fabsf( u )) : 0.5F / u;
			float c = 1.0F / sqrtf( t * t + 1.0F);
			float s = c * t;
			float temp;

			m11 -= t * m21;
			m22 += t * m21;
			m21 = 0.0F;

			temp = c * m31 - s * m32;
			m32 = s * m31 + c * m32;
			m31 = temp;

			temp = c * er[0] - s * es[0];
			es[0] = s * er[0] + c * es[0];
			er[0] = temp;

			temp = c * er[1] - s * es[1];
			es[1] = s * er[1] + c * es[1];
			er[1] = temp;

			temp = c * er[2] - s * es[2];
			es[2] = s * er[2] + c * es[2];
			er[2] = temp;
		}

		//annihilate 31 entry
		if( m31 != 0.0F )
		{
			float u = (m33 - m11) * 0.5F / m31;
			float u2 = u * u;
			float u2p1 = u2 + 1.0F;
			float t = (u2p1 != u2) ? ((u < 0.0F) ? -1.0F : 1.0F) * (sqrtf( u2p1 ) - fabsf( u )) : 0.5F / u;
			float c = 1.0F / sqrtf( t * t + 1.0F);
			float s = c * t;
			float temp;

			m11 -= t * m31;
			m22 += t * m31;
			m31 = 0.0F;

			temp = c * m21 - s * m32;
			m32 = s * m21 + c * m32;
			m21 = temp;

			temp = c * er[0] - s * et[0];
			et[0] = s * er[0] + c * et[0];
			er[0] = temp;

			temp = c * er[1] - s * et[1];
			et[1] = s * er[1] + c * et[1];
			er[1] = temp;

			temp = c * er[2] - s * et[2];
			et[2] = s * er[2] + c * et[2];
			er[2] = temp;
		}

		//annihilate 32 entry
		if( m32 != 0.0F )
		{
			float u = (m33 - m22) * 0.5F / m32;
			float u2 = u * u;
			float u2p1 = u2 + 1.0F;
			float t = (u2p1 != u2) ? ((u < 0.0F) ? -1.0F : 1.0F) * (sqrtf( u2p1 ) - fabsf( u )) : 0.5F / u;
			float c = 1.0F / sqrtf( t * t + 1.0F);
			float s = c * t;
			float temp;

			m22 -= t * m32;
			m33 += t * m32;
			m32 = 0.0F;

			temp = c * m21 - s * m31;
			m31 = s * m21 + c * m31;
			m21 = temp;

			temp = c * es[0] - s * et[0];
			et[0] = s * es[0] + c * et[0];
			es[0] = temp;

			temp = c * es[1] - s * et[1];
			et[1] = s * es[1] + c * et[1];
			es[1] = temp;

			temp = c * es[2] - s * et[2];
			et[2] = s * es[2] + c * et[2];
			es[2] = temp;
		}
	}

	el[0] = m11;
	el[1] = m22;
	el[2] = m33;
}