//#####################################################################
// Copyright 2002-2004, Silvia Salinas-Blemker, Robert Bridson, Ronald Fedkiw, Eran Guendelman, Geoffrey Irving, Neil Molino, Igor Neverov, Joseph Teran, Rachel Weinstein.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
// Class MATRIX_3X3
//#####################################################################
#ifndef __MATRIX_3X3__
#define __MATRIX_3X3__

#include <assert.h>
#include "VECTOR_3D.h"
#include "../Math_Tools/sqr.h"
#include "../Math_Tools/constants.h"
#include "../Math_Tools/exchange.h"
#include "UPPER_TRIANGULAR_MATRIX_3X3.h"
#include "SYMMETRIC_MATRIX_3X3.h"
#include "MATRIX_NXN.h"
#include "MATRIX_MXN.h"
namespace PhysBAM
{

template<class T>
class MATRIX_3X3
{
public:
	T x[9];

	MATRIX_3X3()
	{
		for (int i = 0; i < 9; i++) x[i] = 0;
	}

	template<class T2>
	MATRIX_3X3 (const MATRIX_3X3<T2>& matrix_input)
	{
		for (int i = 0; i < 9; i++) x[i] = (T) matrix_input.x[i];
	}

	MATRIX_3X3 (const DIAGONAL_MATRIX_3X3<T>& matrix_input)
	{
		x[0] = matrix_input.x11;
		x[4] = matrix_input.x22;
		x[8] = matrix_input.x33;
		x[1] = x[2] = x[3] = x[5] = x[6] = x[7] = 0;
	}

	MATRIX_3X3 (const SYMMETRIC_MATRIX_3X3<T>& matrix_input)
	{
		x[0] = matrix_input.x11;
		x[1] = x[3] = matrix_input.x21;
		x[2] = x[6] = matrix_input.x31;
		x[4] = matrix_input.x22;
		x[5] = x[7] = matrix_input.x32;
		x[8] = matrix_input.x33;
	}

	MATRIX_3X3 (const UPPER_TRIANGULAR_MATRIX_3X3<T>& matrix_input)
	{
		x[0] = matrix_input.x11;
		x[3] = matrix_input.x12;
		x[4] = matrix_input.x22;
		x[6] = matrix_input.x13;
		x[7] = matrix_input.x23;
		x[8] = matrix_input.x33;
		x[1] = x[2] = x[5] = 0;
	}

	MATRIX_3X3 (const T x11, const T x21, const T x31, const T x12, const T x22, const T x32, const T x13, const T x23, const T x33)
	{
		x[0] = x11;
		x[1] = x21;
		x[2] = x31;
		x[3] = x12;
		x[4] = x22;
		x[5] = x32;
		x[6] = x13;
		x[7] = x23;
		x[8] = x33;
	}

	MATRIX_3X3 (const VECTOR_3D<T>& column1, const VECTOR_3D<T>& column2, const VECTOR_3D<T>& column3)
	{
		x[0] = column1.x;
		x[1] = column1.y;
		x[2] = column1.z;
		x[3] = column2.x;
		x[4] = column2.y;
		x[5] = column2.z;
		x[6] = column3.x;
		x[7] = column3.y;
		x[8] = column3.z;
	}

	MATRIX_3X3 (const MATRIX_NXN<T>& matrix_input)
	{
		assert (matrix_input.n == 3);

		for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) x[i + 3 * j] = (T) matrix_input (i + 1, j + 1);
	}

	MATRIX_3X3 (const MATRIX_MXN<T>& matrix_input)
	{
		assert (matrix_input.n == 3 && matrix_input.m == 3);

		for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) x[i + 3 * j] = (T) matrix_input (i + 1, j + 1);
	}

	void Initialize_With_Column_Vectors (const VECTOR_3D<T>& column1, const VECTOR_3D<T>& column2, const VECTOR_3D<T>& column3)
	{
		x[0] = column1.x;
		x[1] = column1.y;
		x[2] = column1.z;
		x[3] = column2.x;
		x[4] = column2.y;
		x[5] = column2.z;
		x[6] = column3.x;
		x[7] = column3.y;
		x[8] = column3.z;
	}

	T& operator() (const int i, const int j)
	{
		assert (i >= 1 && i <= 3);
		assert (j >= 1 && j <= 3);
		return x[i - 1 + 3 * (j - 1)];
	}

	const T& operator() (const int i, const int j) const
	{
		assert (i >= 1 && i <= 3);
		assert (j >= 1 && j <= 3);
		return x[i - 1 + 3 * (j - 1)];
	}

	VECTOR_3D<T>& Column (const int j)
	{
		assert (1 <= j && j <= 3);
		return * (VECTOR_3D<T>*) (x + 3 * (j - 1));
	}

	const VECTOR_3D<T>& Column (const int j) const
	{
		assert (1 <= j && j <= 3);
		return * (VECTOR_3D<T>*) (x + 3 * (j - 1));
	}

	MATRIX_3X3<T> operator-() const
	{
		return MATRIX_3X3<T> (-x[0], -x[1], -x[2], -x[3], -x[4], -x[5], -x[6], -x[7], -x[8]);
	}

	MATRIX_3X3<T>& operator+= (const MATRIX_3X3<T>& A)
	{
		for (int i = 0; i < 9; i++) x[i] += A.x[i];

		return *this;
	}

	MATRIX_3X3<T>& operator+= (const T& a)
	{
		x[0] += a;
		x[4] += a;
		x[8] += a;
		return *this;
	}

	MATRIX_3X3<T>& operator-= (const MATRIX_3X3<T>& A)
	{
		for (int i = 0; i < 9; i++) x[i] -= A.x[i];

		return *this;
	}

	MATRIX_3X3<T>& operator-= (const T& a)
	{
		x[0] -= a;
		x[4] -= a;
		x[8] -= a;
		return *this;
	}

	MATRIX_3X3<T>& operator*= (const MATRIX_3X3<T>& A)
	{
		return *this = *this * A;
	}

	MATRIX_3X3<T>& operator*= (const T a)
	{
		for (int i = 0; i < 9; i++) x[i] *= a;

		return *this;
	}

	MATRIX_3X3<T>& operator/= (const T a)
	{
		assert (a != 0);
		T s = 1 / a;

		for (int i = 0; i < 9; i++) x[i] *= s;

		return *this;
	}

	MATRIX_3X3<T> operator+ (const MATRIX_3X3<T>& A) const
	{
		return MATRIX_3X3<T> (x[0] + A.x[0], x[1] + A.x[1], x[2] + A.x[2], x[3] + A.x[3], x[4] + A.x[4], x[5] + A.x[5], x[6] + A.x[6], x[7] + A.x[7], x[8] + A.x[8]);
	}

	MATRIX_3X3<T> operator+ (const T a) const
	{
		return MATRIX_3X3<T> (x[0] + a, x[1], x[2], x[3], x[4] + a, x[5], x[6], x[7], x[8] + a);
	}

	MATRIX_3X3<T> operator- (const MATRIX_3X3<T>& A) const
	{
		return MATRIX_3X3<T> (x[0] - A.x[0], x[1] - A.x[1], x[2] - A.x[2], x[3] - A.x[3], x[4] - A.x[4], x[5] - A.x[5], x[6] - A.x[6], x[7] - A.x[7], x[8] - A.x[8]);
	}

	MATRIX_3X3<T> operator- (const T a) const
	{
		return MATRIX_3X3<T> (x[0] - a, x[1], x[2], x[3], x[4] - a, x[5], x[6], x[7], x[8] - a);
	}

	MATRIX_3X3<T> operator* (const MATRIX_3X3<T>& A) const // 27 mults, 18 adds
	{
		return MATRIX_3X3<T> (x[0] * A.x[0] + x[3] * A.x[1] + x[6] * A.x[2], x[1] * A.x[0] + x[4] * A.x[1] + x[7] * A.x[2], x[2] * A.x[0] + x[5] * A.x[1] + x[8] * A.x[2],
				      x[0] * A.x[3] + x[3] * A.x[4] + x[6] * A.x[5], x[1] * A.x[3] + x[4] * A.x[4] + x[7] * A.x[5], x[2] * A.x[3] + x[5] * A.x[4] + x[8] * A.x[5],
				      x[0] * A.x[6] + x[3] * A.x[7] + x[6] * A.x[8], x[1] * A.x[6] + x[4] * A.x[7] + x[7] * A.x[8], x[2] * A.x[6] + x[5] * A.x[7] + x[8] * A.x[8]);
	}

	MATRIX_3X3<T> operator* (const T a) const
	{
		return MATRIX_3X3<T> (a * x[0], a * x[1], a * x[2], a * x[3], a * x[4], a * x[5], a * x[6], a * x[7], a * x[8]);
	}

	MATRIX_3X3<T> operator/ (const T a) const
	{
		assert (a != 0);
		T s = 1 / a;
		return MATRIX_3X3<T> (s * x[0], s * x[1], s * x[2], s * x[3], s * x[4], s * x[5], s * x[6], s * x[7], s * x[8]);
	}

	VECTOR_3D<T> operator* (const VECTOR_3D<T>& v) const // 9 mults, 6 adds
	{
		return VECTOR_3D<T> (x[0] * v.x + x[3] * v.y + x[6] * v.z, x[1] * v.x + x[4] * v.y + x[7] * v.z, x[2] * v.x + x[5] * v.y + x[8] * v.z);
	}

	VECTOR_2D<T> operator* (const VECTOR_2D<T>& v) const // assumes w=1 is the 3rd coordinate of v
	{
		T w = x[2] * v.x + x[5] * v.y + x[8];
		assert (w != 0);

		if (w == 1) return VECTOR_2D<T> (x[0] * v.x + x[3] * v.y + x[6], x[1] * v.x + x[4] * v.y + x[7]);
		else
		{
			T s = 1 / w;        // rescale so w=1
			return VECTOR_2D<T> (s * (x[0] * v.x + x[3] * v.y + x[6]), s * (x[1] * v.x + x[4] * v.y + x[7]));
		}
	}

	VECTOR_2D<T> Transform_2X2 (const VECTOR_2D<T>& v) const // multiplies vector by upper 2x2 of matrix only
	{
		return VECTOR_2D<T> (x[0] * v.x + x[3] * v.y, x[1] * v.x + x[4] * v.y);
	}

	T Determinant() const
	{
		return x[0] * (x[4] * x[8] - x[7] * x[5]) + x[3] * (x[7] * x[2] - x[1] * x[8]) + x[6] * (x[1] * x[5] - x[4] * x[2]);
	}

	void Invert()
	{
		*this = Inverse();
	}

	MATRIX_3X3<T> Inverse() const
	{
		T cofactor11 = x[4] * x[8] - x[7] * x[5], cofactor12 = x[7] * x[2] - x[1] * x[8], cofactor13 = x[1] * x[5] - x[4] * x[2];
		T determinant = x[0] * cofactor11 + x[3] * cofactor12 + x[6] * cofactor13;
		assert (determinant != 0);
		T s = 1 / determinant;
		return s * MATRIX_3X3<T> (cofactor11, cofactor12, cofactor13, x[6] * x[5] - x[3] * x[8], x[0] * x[8] - x[6] * x[2], x[3] * x[2] - x[0] * x[5], x[3] * x[7] - x[6] * x[4], x[6] * x[1] - x[0] * x[7], x[0] * x[4] - x[3] * x[1]);
	}

	MATRIX_3X3<T> Inverse_Transpose() const
	{
		return Inverse().Transposed();
	}

	MATRIX_3X3<T> Deviatoric() const
	{
		T one_third_trace = (T) one_third * (x[0] + x[4] + x[8]);
		return MATRIX_3X3<T> (x[0] - one_third_trace, x[1], x[2], x[3], x[4] - one_third_trace, x[5], x[6], x[7], x[8] - one_third_trace);
	}

	MATRIX_3X3<T> Dilational() const
	{
		T one_third_trace = (T) one_third * (x[0] + x[4] + x[8]);
		return MATRIX_3X3<T> (one_third_trace, 0, 0, 0, one_third_trace, 0, 0, 0, one_third_trace);
	}

	VECTOR_3D<T> Solve_Linear_System (const VECTOR_3D<T> b) const // 33 mults, 17 adds, 1 div
	{
		T cofactor11 = x[4] * x[8] - x[7] * x[5], cofactor12 = x[7] * x[2] - x[1] * x[8], cofactor13 = x[1] * x[5] - x[4] * x[2];
		T determinant = x[0] * cofactor11 + x[3] * cofactor12 + x[6] * cofactor13;
		assert (determinant != 0);
		return MATRIX_3X3<T> (cofactor11, cofactor12, cofactor13, (x[6] * x[5] - x[3] * x[8]), (x[0] * x[8] - x[6] * x[2]), (x[3] * x[2] - x[0] * x[5]), (x[3] * x[7] - x[6] * x[4]), (x[6] * x[1] - x[0] * x[7]), (x[0] * x[4] - x[3] * x[1])) * b / determinant;
	}

	void Transpose()
	{
		exchange (x[1], x[3]);
		exchange (x[2], x[6]);
		exchange (x[5], x[7]);
	}

	MATRIX_3X3<T> Transposed() const
	{
		return MATRIX_3X3<T> (x[0], x[3], x[6], x[1], x[4], x[7], x[2], x[5], x[8]);
	}

	T Trace() const
	{
		return x[0] + x[4] + x[8];
	}

	MATRIX_3X3<T> Q_From_Gram_Schmidt_QR_Factorization() const
	{
		int k;
		MATRIX_3X3<T> Q = *this;
		T one_over_r11 = 1 / sqrt ( (sqr (Q.x[0]) + sqr (Q.x[1]) + sqr (Q.x[2])));

		for (k = 0; k <= 2; k++) Q.x[k] = one_over_r11 * Q.x[k];

		T r12 = Q.x[0] * Q.x[3] + Q.x[1] * Q.x[4] + Q.x[2] * Q.x[5];
		Q.x[3] -= r12 * Q.x[0];
		Q.x[4] -= r12 * Q.x[1];
		Q.x[5] -= r12 * Q.x[2];
		T r13 = Q.x[0] * Q.x[6] + Q.x[1] * Q.x[7] + Q.x[2] * Q.x[8];
		Q.x[6] -= r13 * Q.x[0];
		Q.x[7] -= r13 * Q.x[1];
		Q.x[8] -= r13 * Q.x[2];
		T one_over_r22 = 1 / sqrt ( (sqr (Q.x[3]) + sqr (Q.x[4]) + sqr (Q.x[5])));

		for (k = 3; k <= 5; k++) Q.x[k] = one_over_r22 * Q.x[k];

		T r23 = Q.x[3] * Q.x[6] + Q.x[4] * Q.x[7] + Q.x[5] * Q.x[8];
		Q.x[6] -= r23 * Q.x[3];
		Q.x[7] -= r23 * Q.x[4];
		Q.x[8] -= r23 * Q.x[5];
		T one_over_r33 = 1 / sqrt ( (sqr (Q.x[6]) + sqr (Q.x[7]) + sqr (Q.x[8])));

		for (k = 6; k <= 8; k++) Q.x[k] = one_over_r33 * Q.x[k];

		return Q;
	}

	UPPER_TRIANGULAR_MATRIX_3X3<T> R_From_Gram_Schmidt_QR_Factorization() const
	{
		int k;
		MATRIX_3X3<T> Q = *this;
		UPPER_TRIANGULAR_MATRIX_3X3<T> R;
		R.x11 = sqrt ( (sqr (Q.x[0]) + sqr (Q.x[1]) + sqr (Q.x[2])));
		T one_over_r11 = 1 / R.x11;

		for (k = 0; k <= 2; k++) Q.x[k] = one_over_r11 * Q.x[k];

		R.x12 = Q.x[0] * Q.x[3] + Q.x[1] * Q.x[4] + Q.x[2] * Q.x[5];
		Q.x[3] -= R.x12 * Q.x[0];
		Q.x[4] -= R.x12 * Q.x[1];
		Q.x[5] -= R.x12 * Q.x[2];
		R.x13 = Q.x[0] * Q.x[6] + Q.x[1] * Q.x[7] + Q.x[2] * Q.x[8];
		Q.x[6] -= R.x13 * Q.x[0];
		Q.x[7] -= R.x13 * Q.x[1];
		Q.x[8] -= R.x13 * Q.x[2];
		R.x22 = sqrt ( (sqr (Q.x[3]) + sqr (Q.x[4]) + sqr (Q.x[5])));
		T one_over_r22 = 1 / R.x22;

		for (k = 3; k <= 5; k++) Q.x[k] = one_over_r22 * Q.x[k];

		R.x23 = Q.x[3] * Q.x[6] + Q.x[4] * Q.x[7] + Q.x[5] * Q.x[8];
		Q.x[6] -= R.x23 * Q.x[3];
		Q.x[7] -= R.x23 * Q.x[4];
		Q.x[8] -= R.x23 * Q.x[5];
		R.x33 = sqrt ( (sqr (Q.x[6]) + sqr (Q.x[7]) + sqr (Q.x[8])));
		return R;
	}

	MATRIX_3X3<T> Higham_Iterate (const T tolerance = 1e-5, const int max_iterations = 20, const bool exit_on_max_iterations = false) const
	{
		MATRIX_3X3<T> X = *this, Y;
		int iterations = 0;

		for (;;)
		{
			Y = (T).5 * (X + X.Inverse_Transpose());

			if ( (X - Y).Max_Abs_Element() < tolerance) return Y;

			X = Y;

			if (++iterations >= max_iterations)
			{
				if (exit_on_max_iterations)
				{
					std::cout << "Failed in Higham iteration!" << std::endl;
					exit (1);
				}
				else return X;
			}
		}
	}

	MATRIX_3X3<T> Cofactor_Matrix() const
	{
		return MATRIX_3X3<T> (x[4] * x[8] - x[5] * x[7], -x[3] * x[8] + x[5] * x[6], x[3] * x[7] - x[4] * x[6],
				      -x[1] * x[8] + x[2] * x[7], x[0] * x[8] - x[2] * x[6], -x[0] * x[7] + x[1] * x[6],
				      x[1] * x[5] - x[2] * x[4], -x[0] * x[5] + x[2] * x[3], x[0] * x[4] - x[1] * x[3]);
	}

	SYMMETRIC_MATRIX_3X3<T> Outer_Product_Matrix() const
	{
		return SYMMETRIC_MATRIX_3X3<T> (x[0] * x[0] + x[3] * x[3] + x[6] * x[6], x[1] * x[0] + x[4] * x[3] + x[7] * x[6], x[2] * x[0] + x[5] * x[3] + x[8] * x[6],
						x[1] * x[1] + x[4] * x[4] + x[7] * x[7], x[2] * x[1] + x[5] * x[4] + x[8] * x[7], x[2] * x[2] + x[5] * x[5] + x[8] * x[8]);
	}

	SYMMETRIC_MATRIX_3X3<T> Normal_Equations_Matrix() const // 18 mults, 12 adds
	{
		return SYMMETRIC_MATRIX_3X3<T> (x[0] * x[0] + x[1] * x[1] + x[2] * x[2], x[3] * x[0] + x[4] * x[1] + x[5] * x[2], x[6] * x[0] + x[7] * x[1] + x[8] * x[2],
						x[3] * x[3] + x[4] * x[4] + x[5] * x[5], x[6] * x[3] + x[7] * x[4] + x[8] * x[5], x[6] * x[6] + x[7] * x[7] + x[8] * x[8]);
	}

	SYMMETRIC_MATRIX_3X3<T> Symmetric_Part() const
	{
		return SYMMETRIC_MATRIX_3X3<T> (x[0], (T).5 * (x[1] + x[3]), (T).5 * (x[2] + x[6]), x[4], (T).5 * (x[5] + x[7]), x[8]);
	}

	SYMMETRIC_MATRIX_3X3<T> Twice_Symmetric_Part() const // 3 mults, 3 adds
	{
		return SYMMETRIC_MATRIX_3X3<T> (2 * x[0], x[1] + x[3], x[2] + x[6], 2 * x[4], x[5] + x[7], 2 * x[8]);
	}

	DIAGONAL_MATRIX_3X3<T> Diagonal_Part() const
	{
		return DIAGONAL_MATRIX_3X3<T> (x[0], x[4], x[8]);
	}

	void Normalize_Columns()
	{
		T magnitude = sqrt (sqr (x[0]) + sqr (x[1]) + sqr (x[2]));
		assert (magnitude != 0);
		T s = 1 / magnitude;
		x[0] *= s;
		x[1] *= s;
		x[2] *= s;
		magnitude = sqrt (sqr (x[3]) + sqr (x[4]) + sqr (x[5]));
		assert (magnitude != 0);
		s = 1 / magnitude;
		x[3] *= s;
		x[4] *= s;
		x[5] *= s;
		magnitude = sqrt (sqr (x[6]) + sqr (x[7]) + sqr (x[8]));
		assert (magnitude != 0);
		s = 1 / magnitude;
		x[6] *= s;
		x[7] *= s;
		x[8] *= s;
	}

	VECTOR_3D<T> Largest_Normalized_Column() const
	{
		T scale1 = sqr (x[0]) + sqr (x[1]) + sqr (x[2]), scale2 = sqr (x[3]) + sqr (x[4]) + sqr (x[5]), scale3 = sqr (x[6]) + sqr (x[7]) + sqr (x[8]);

		if (scale1 > scale2)
		{
			if (scale1 > scale3) return VECTOR_3D<T> (x[0], x[1], x[2]) / sqrt (scale1);
		}
		else if (scale2 > scale3) return VECTOR_3D<T> (x[3], x[4], x[5]) / sqrt (scale2);

		return VECTOR_3D<T> (x[6], x[7], x[8]) / sqrt (scale3);
	}

	static MATRIX_3X3<T> Transpose (const MATRIX_3X3<T>& A)
	{
		return MATRIX_3X3<T> (A.x[0], A.x[3], A.x[6], A.x[1], A.x[4], A.x[7], A.x[2], A.x[5], A.x[8]);
	}

	static MATRIX_3X3<T> Translation_Matrix (const VECTOR_2D<T>& translation) // treating the 3x3 matrix as a homogeneous transformation on 2d vectors
	{
		return MATRIX_3X3<T> (1, 0, 0, 0, 1, 0, translation.x, translation.y, 1);
	}

	static MATRIX_3X3<T> Identity_Matrix()
	{
		return MATRIX_3X3<T> (1, 0, 0, 0, 1, 0, 0, 0, 1);
	}

	static MATRIX_3X3<T> Diagonal_Matrix (const T d)
	{
		return MATRIX_3X3<T> (d, 0, 0, 0, d, 0, 0, 0, d);
	}

	static MATRIX_3X3<T> Rotation_Matrix_X_Axis (const T radians)
	{
		T c = cos (radians), s = sin (radians);
		return MATRIX_3X3<T> (1, 0, 0, 0, c, s, 0, -s, c);
	}

	static MATRIX_3X3<T> Rotation_Matrix_Y_Axis (const T radians)
	{
		T c = cos (radians), s = sin (radians);
		return MATRIX_3X3<T> (c, 0, -s, 0, 1, 0, s, 0, c);
	}

	static MATRIX_3X3<T> Rotation_Matrix_Z_Axis (const T radians)
	{
		T c = cos (radians), s = sin (radians);
		return MATRIX_3X3<T> (c, s, 0, -s, c, 0, 0, 0, 1);
	}

	static MATRIX_3X3<T> Rotation_Matrix (const VECTOR_3D<T>& axis, const T radians)
	{
		T c = cos (radians), s = sin (radians);
		return MATRIX_3X3<T> (sqr (axis.x) + (1 - sqr (axis.x)) * c, axis.x * axis.y * (1 - c) + axis.z * s, axis.x * axis.z * (1 - c) - axis.y * s,
				      axis.x * axis.y * (1 - c) - axis.z * s, sqr (axis.y) + (1 - sqr (axis.y)) * c, axis.y * axis.z * (1 - c) + axis.x * s,
				      axis.x * axis.z * (1 - c) + axis.y * s, axis.y * axis.z * (1 - c) - axis.x * s, sqr (axis.z) + (1 - sqr (axis.z)) * c);
	}

	static MATRIX_3X3<T> Rotation_Matrix (const VECTOR_3D<T>& rotation)
	{
		T angle = rotation.Magnitude();
		return angle ? Rotation_Matrix (rotation / angle, angle) : Identity_Matrix();
	}

	static MATRIX_3X3<T> Rotation_Matrix (const VECTOR_3D<T>& x_final, const VECTOR_3D<T>& y_final, const VECTOR_3D<T>& z_final)
	{
		return MATRIX_3X3<T> (x_final.x, y_final.x, z_final.x, x_final.y, y_final.y, z_final.y, x_final.z, y_final.z, z_final.z);
	}

	static MATRIX_3X3<T> Rotation_Matrix (const VECTOR_3D<T>& initial_vector, const VECTOR_3D<T>& final_vector)
	{
		VECTOR_3D<T> initial_unit = initial_vector / initial_vector.Magnitude(), final_unit = final_vector / final_vector.Magnitude();
		T cos_theta = VECTOR_3D<T>::Dot_Product (initial_unit, final_unit);

		if (cos_theta > 1 - 1e-14) return Identity_Matrix();

		if (cos_theta < -1 + 1e-14) return MATRIX_3X3<T> (-1, 0, 0, 0, -1, 0, 0, 0, -1); // note-this is actually a reflection

		VECTOR_3D<T> axis = VECTOR_3D<T>::Cross_Product (initial_unit, final_unit);
		axis.Normalize();
		return Rotation_Matrix (axis, acos (cos_theta));
	}

	static MATRIX_3X3<T> Outer_Product (const VECTOR_3D<T>& u, const VECTOR_3D<T>& v)
	{
		return MATRIX_3X3<T> (u.x * v.x, u.y * v.x, u.z * v.x, u.x * v.y, u.y * v.y, u.z * v.y, u.x * v.z, u.y * v.z, u.z * v.z);
	}

	static T Inner_Product (const MATRIX_3X3<T>& A, const MATRIX_3X3<T>& B)
	{
		return A.x[0] * B.x[0] + A.x[1] * B.x[1] + A.x[2] * B.x[2] + A.x[3] * B.x[3] + A.x[4] * B.x[4] + A.x[5] * B.x[5] + A.x[6] * B.x[6] + A.x[7] * B.x[7] + A.x[8] * B.x[8];
	}

	static MATRIX_3X3<T> Cross_Product_Matrix (const VECTOR_3D<T>& v)
	{
		return MATRIX_3X3<T> (0, v.z, -v.y, -v.z, 0, v.x, v.y, -v.x, 0);
	}

	VECTOR_3D<T> Antisymmetric_Part_Cross_Product_Vector() const
	{
		return (T).5 * VECTOR_3D<T> (x[5] - x[7], x[6] - x[2], x[1] - x[3]);
	}

	static SYMMETRIC_MATRIX_3X3<T> Right_Multiply_With_Symmetric_Result (const MATRIX_3X3<T>& A, const DIAGONAL_MATRIX_3X3<T>& B)
	{
		return SYMMETRIC_MATRIX_3X3<T> (B.x11 * A.x[0], B.x11 * A.x[1], B.x11 * A.x[2], B.x22 * A.x[4], B.x22 * A.x[5], B.x33 * A.x[8]);
	}

	T Max_Abs_Element() const
	{
		return max (fabs (x[0]), fabs (x[1]), fabs (x[2]), fabs (x[3]), fabs (x[4]), fabs (x[5]), fabs (x[6]), fabs (x[7]), fabs (x[8]));
	}

	T Frobenius_Norm() const
	{
		return sqrt (sqr (x[0]) + sqr (x[1]) + sqr (x[2]) + sqr (x[3]) + sqr (x[4]) + sqr (x[5]) + sqr (x[6]) + sqr (x[7]) + sqr (x[8]));
	}

	MATRIX_3X3<T> operator* (const DIAGONAL_MATRIX_3X3<T>& A) const // 9 mults
	{
		return MATRIX_3X3<T> (x[0] * A.x11, x[1] * A.x11, x[2] * A.x11, x[3] * A.x22, x[4] * A.x22, x[5] * A.x22, x[6] * A.x33, x[7] * A.x33, x[8] * A.x33);
	}

	MATRIX_3X3<T> operator* (const UPPER_TRIANGULAR_MATRIX_3X3<T>& A) const // 18 mults, 8 adds
	{
		return MATRIX_3X3<T> (x[0] * A.x11, x[1] * A.x11, x[2] * A.x11, x[0] * A.x12 + x[3] * A.x22, x[1] * A.x12 + x[4] * A.x22, x[2] * A.x12 + x[5] * A.x22,
				      x[0] * A.x13 + x[3] * A.x23 + x[6] * A.x33, x[1] * A.x13 + x[4] * A.x23 + x[7] * A.x33, x[2] * A.x13 + x[5] * A.x23 + x[8] * A.x33);
	}

	MATRIX_3X3<T> operator* (const SYMMETRIC_MATRIX_3X3<T>& A) const // 27 mults, 18 adds
	{
		return MATRIX_3X3<T> (x[0] * A.x11 + x[3] * A.x21 + x[6] * A.x31, x[1] * A.x11 + x[4] * A.x21 + x[7] * A.x31, x[2] * A.x11 + x[5] * A.x21 + x[8] * A.x31,
				      x[0] * A.x21 + x[3] * A.x22 + x[6] * A.x32, x[1] * A.x21 + x[4] * A.x22 + x[7] * A.x32, x[2] * A.x21 + x[5] * A.x22 + x[8] * A.x32,
				      x[0] * A.x31 + x[3] * A.x32 + x[6] * A.x33, x[1] * A.x31 + x[4] * A.x32 + x[7] * A.x33, x[2] * A.x31 + x[5] * A.x32 + x[8] * A.x33);
	}

	MATRIX_3X3<T> Multiply_With_Transpose (const UPPER_TRIANGULAR_MATRIX_3X3<T>& A) const
	{
		return MATRIX_3X3<T> (x[0] * A.x11 + x[3] * A.x12 + x[6] * A.x13, x[1] * A.x11 + x[4] * A.x12 + x[7] * A.x13, x[2] * A.x11 + x[5] * A.x12 + x[8] * A.x13,
				      x[3] * A.x22 + x[6] * A.x23, x[4] * A.x22 + x[7] * A.x23, x[5] * A.x22 + x[8] * A.x23, x[6] * A.x33, x[7] * A.x33, x[8] * A.x33);
	}

	MATRIX_3X3<T> Multiply_With_Transpose (const MATRIX_3X3<T>& A) const
	{
		return MATRIX_3X3<T> (x[0] * A.x[0] + x[3] * A.x[3] + x[6] * A.x[6], x[1] * A.x[0] + x[4] * A.x[3] + x[7] * A.x[6], x[2] * A.x[0] + x[5] * A.x[3] + x[8] * A.x[6],
				      x[0] * A.x[1] + x[3] * A.x[4] + x[6] * A.x[7], x[1] * A.x[1] + x[4] * A.x[4] + x[7] * A.x[7], x[2] * A.x[1] + x[5] * A.x[4] + x[8] * A.x[7],
				      x[0] * A.x[2] + x[3] * A.x[5] + x[6] * A.x[8], x[1] * A.x[2] + x[4] * A.x[5] + x[7] * A.x[8], x[2] * A.x[2] + x[5] * A.x[5] + x[8] * A.x[8]);
	}

	MATRIX_3X3<T> Transpose_Times (const MATRIX_3X3<T>& A) const
	{
		return MATRIX_3X3<T> (x[0] * A.x[0] + x[1] * A.x[1] + x[2] * A.x[2], x[3] * A.x[0] + x[4] * A.x[1] + x[5] * A.x[2], x[6] * A.x[0] + x[7] * A.x[1] + x[8] * A.x[2],
				      x[0] * A.x[3] + x[1] * A.x[4] + x[2] * A.x[5], x[3] * A.x[3] + x[4] * A.x[4] + x[5] * A.x[5], x[6] * A.x[3] + x[7] * A.x[4] + x[8] * A.x[5],
				      x[0] * A.x[6] + x[1] * A.x[7] + x[2] * A.x[8], x[3] * A.x[6] + x[4] * A.x[7] + x[5] * A.x[8], x[6] * A.x[6] + x[7] * A.x[7] + x[8] * A.x[8]);
	}

	VECTOR_3D<T> Transpose_Times (const VECTOR_3D<T>& v) const
	{
		return VECTOR_3D<T> (x[0] * v.x + x[1] * v.y + x[2] * v.z, x[3] * v.x + x[4] * v.y + x[5] * v.z, x[6] * v.x + x[7] * v.y + x[8] * v.z);
	}

	static MATRIX_3X3<T> Left_Procrustes_Rotation (const MATRIX_3X3<T>& A, const MATRIX_3X3<T>& B)
	{
		MATRIX_3X3<T> U, V;
		DIAGONAL_MATRIX_3X3<T> D;
		A.Multiply_With_Transpose (B).Fast_Singular_Value_Decomposition (U, D, V);
		return U.Multiply_With_Transpose (V);
	}

	void Fast_Singular_Value_Decomposition (MATRIX_3X3<T>& U, DIAGONAL_MATRIX_3X3<T>& singular_values, MATRIX_3X3<T>& V) const
	{
		MATRIX_3X3<double> U_double, V_double;
		DIAGONAL_MATRIX_3X3<double> singular_values_double;
		Fast_Singular_Value_Decomposition_Double (U_double, singular_values_double, V_double);
		U = U_double;
		singular_values = singular_values_double;
		V = V_double;
	}

	template<class RW>
	void Read (std::istream &input_stream)
	{
		Read_Binary<RW> (input_stream, x[0], x[1], x[2], x[3], x[4], x[5], x[6], x[7], x[8]);
	}

	template<class RW>
	void Write (std::ostream &output_stream) const
	{
		Write_Binary<RW> (output_stream, x[0], x[1], x[2], x[3], x[4], x[5], x[6], x[7], x[8]);
	}

//#####################################################################
	void Fast_Singular_Value_Decomposition_Double (MATRIX_3X3<double>& U, DIAGONAL_MATRIX_3X3<double>& singular_values, MATRIX_3X3<double>& V, const double tolerance = 1e-7) const;
	T Tetrahedron_Minimum_Altitude() const;
//#####################################################################
};
// global functions
template<class T>
inline MATRIX_3X3<T> operator+ (const T a, const MATRIX_3X3<T>& A)
{
	return A + a;
}

template<class T>
inline MATRIX_3X3<T> operator* (const T a, const MATRIX_3X3<T>& A)
{
	return A * a;
}

template<class T>
inline VECTOR_3D<T> operator* (const VECTOR_3D<T>& v, const MATRIX_3X3<T>& A)
{
	return VECTOR_3D<T> (v.x * A.x[0] + v.y * A.x[1] + v.z * A.x[2], v.x * A.x[3] + v.y * A.x[4] + v.z * A.x[5], v.x * A.x[6] + v.y * A.x[7] + v.z * A.x[8]);
}

template<class T>
inline MATRIX_3X3<T> operator* (const DIAGONAL_MATRIX_3X3<T>& A, const MATRIX_3X3<T>& B)
{
	return MATRIX_3X3<T> (A.x11 * B.x[0], A.x22 * B.x[1], A.x33 * B.x[2], A.x11 * B.x[3], A.x22 * B.x[4], A.x33 * B.x[5], A.x11 * B.x[6], A.x22 * B.x[7], A.x33 * B.x[8]);
}

template<class T>
inline MATRIX_3X3<T> operator* (const UPPER_TRIANGULAR_MATRIX_3X3<T>& A, const MATRIX_3X3<T>& B)
{
	return MATRIX_3X3<T> (A.x11 * B.x[0] + A.x12 * B.x[1] + A.x13 * B.x[2], A.x22 * B.x[1] + A.x23 * B.x[2], A.x33 * B.x[2], A.x11 * B.x[3] + A.x12 * B.x[4] + A.x13 * B.x[5],
			      A.x22 * B.x[4] + A.x23 * B.x[5], A.x33 * B.x[5], A.x11 * B.x[6] + A.x12 * B.x[7] + A.x13 * B.x[8], A.x22 * B.x[7] + A.x23 * B.x[8], A.x33 * B.x[8]);
}

template<class T>
inline MATRIX_3X3<T> operator* (const SYMMETRIC_MATRIX_3X3<T>& A, const MATRIX_3X3<T>& B)
{
	return MATRIX_3X3<T> (A.x11 * B.x[0] + A.x21 * B.x[1] + A.x31 * B.x[2], A.x21 * B.x[0] + A.x22 * B.x[1] + A.x32 * B.x[2], A.x31 * B.x[0] + A.x32 * B.x[1] + A.x33 * B.x[2],
			      A.x11 * B.x[3] + A.x21 * B.x[4] + A.x31 * B.x[5], A.x21 * B.x[3] + A.x22 * B.x[4] + A.x32 * B.x[5], A.x31 * B.x[3] + A.x32 * B.x[4] + A.x33 * B.x[5],
			      A.x11 * B.x[6] + A.x21 * B.x[7] + A.x31 * B.x[8], A.x21 * B.x[6] + A.x22 * B.x[7] + A.x32 * B.x[8], A.x31 * B.x[6] + A.x32 * B.x[7] + A.x33 * B.x[8]);
}

template<class T>
inline std::istream& operator>> (std::istream& input_stream, MATRIX_3X3<T>& A)
{
	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) input_stream >> A.x[i + j * 3];

	return input_stream;
}

template<class T>
inline std::ostream& operator<< (std::ostream& output_stream, const MATRIX_3X3<T>& A)
{
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++) output_stream << A.x[i + j * 3] << " ";

		output_stream << std::endl;
	}

	return output_stream;
}
//#####################################################################
}
#endif