src/parsec/disk-image/parsec/parsec-benchmark/pkgs/apps/facesim/src/Public_Library/Matrices_And_Vectors/MATRIX_4X4.h - public/gem5-resources - Git at Google

 //#####################################################################
 // Copyright 2002-2004, Robert Bridson, Ronald Fedkiw, Geoffrey Irving, 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_4X4
 //#####################################################################
 #ifndef __MATRIX_4X4__
 #define __MATRIX_4X4__

 #include <assert.h>
 #include "VECTOR_3D.h"
 #include "MATRIX_3X3.h"
 #include "../Math_Tools/sqr.h"
 #include "../Math_Tools/constants.h"
 #include <math.h>
 namespace PhysBAM
 {

 template<class T>
 class MATRIX_4X4
 {
 public:
 	T x[16];

 	MATRIX_4X4()
 	{
 		for (int i = 0; i < 16; i++) x[i] = 0;
 	}

 	MATRIX_4X4 (const MATRIX_4X4<T>& matrix_input)
 	{
 		for (int i = 0; i < 16; i++) x[i] = matrix_input.x[i];
 	}

 	MATRIX_4X4 (const T x11, const T x21, const T x31, const T x41, const T x12, const T x22, const T x32, const T x42, const T x13, const T x23, const T x33, const T x43, const T x14, const T x24, const T x34,
 		    const T x44)
 	{
 		x[0] = x11;
 		x[1] = x21;
 		x[2] = x31;
 		x[3] = x41;
 		x[4] = x12;
 		x[5] = x22;
 		x[6] = x32;
 		x[7] = x42;
 		x[8] = x13;
 		x[9] = x23;
 		x[10] = x33;
 		x[11] = x43;
 		x[12] = x14;
 		x[13] = x24;
 		x[14] = x34;
 		x[15] = x44;
 	}

 	explicit MATRIX_4X4 (const MATRIX_3X3<T>& matrix_input) // Create a homogeneous 4x4 matrix corresponding to 3x3 transform
 	{
 		x[0] = matrix_input.x[0];
 		x[1] = matrix_input.x[1];
 		x[2] = matrix_input.x[2];
 		x[3] = (T) 0;
 		x[4] = matrix_input.x[3];
 		x[5] = matrix_input.x[4];
 		x[6] = matrix_input.x[5];
 		x[7] = (T) 0;
 		x[8] = matrix_input.x[6];
 		x[9] = matrix_input.x[7];
 		x[10] = matrix_input.x[8];
 		x[11] = (T) 0;
 		x[12] = (T) 0;
 		x[13] = (T) 0;
 		x[14] = (T) 0;
 		x[15] = (T) 1;
 	}

 	bool operator== (const MATRIX_4X4<T>& A) const
 	{
 		for (int i = 0; i < 16; i++) if (x[i] != A.x[i]) return false;

 		return true;
 	}

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

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

 	MATRIX_4X4<T> operator-() const
 	{
 		return MATRIX_4X4<T> (-x[0], -x[1], -x[2], -x[3], -x[4], -x[5], -x[6], -x[7], -x[8], -x[9], -x[10], -x[11], -x[12], -x[13], -x[14], -x[15]);
 	}

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

 		return *this;
 	}

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

 		return *this;
 	}

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

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

 		return *this;
 	}

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

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

 		return *this;
 	}

 	MATRIX_4X4<T> operator+ (const MATRIX_4X4<T>& A) const
 	{
 		return MATRIX_4X4<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], x[9] + A.x[9], x[10] + A.x[10],
 				      x[11] + A.x[11], x[12] + A.x[12], x[13] + A.x[13], x[14] + A.x[14], x[15] + A.x[15]);
 	}

 	MATRIX_4X4<T> operator- (const MATRIX_4X4<T>& A) const
 	{
 		return MATRIX_4X4<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], x[9] - A.x[9], x[10] - A.x[10], x[11] - A.x[11],
 				      x[12] - A.x[12], x[13] - A.x[13], x[14] - A.x[14], x[15] - A.x[15]);
 	}

 	MATRIX_4X4<T> operator* (const MATRIX_4X4<T>& A) const
 	{
 		return MATRIX_4X4<T> (x[0] * A.x[0] + x[4] * A.x[1] + x[8] * A.x[2] + x[12] * A.x[3], x[1] * A.x[0] + x[5] * A.x[1] + x[9] * A.x[2] + x[13] * A.x[3],
 				      x[2] * A.x[0] + x[6] * A.x[1] + x[10] * A.x[2] + x[14] * A.x[3], x[3] * A.x[0] + x[7] * A.x[1] + x[11] * A.x[2] + x[15] * A.x[3],
 				      x[0] * A.x[4] + x[4] * A.x[5] + x[8] * A.x[6] + x[12] * A.x[7], x[1] * A.x[4] + x[5] * A.x[5] + x[9] * A.x[6] + x[13] * A.x[7],
 				      x[2] * A.x[4] + x[6] * A.x[5] + x[10] * A.x[6] + x[14] * A.x[7], x[3] * A.x[4] + x[7] * A.x[5] + x[11] * A.x[6] + x[15] * A.x[7],
 				      x[0] * A.x[8] + x[4] * A.x[9] + x[8] * A.x[10] + x[12] * A.x[11], x[1] * A.x[8] + x[5] * A.x[9] + x[9] * A.x[10] + x[13] * A.x[11],
 				      x[2] * A.x[8] + x[6] * A.x[9] + x[10] * A.x[10] + x[14] * A.x[11], x[3] * A.x[8] + x[7] * A.x[9] + x[11] * A.x[10] + x[15] * A.x[11],
 				      x[0] * A.x[12] + x[4] * A.x[13] + x[8] * A.x[14] + x[12] * A.x[15], x[1] * A.x[12] + x[5] * A.x[13] + x[9] * A.x[14] + x[13] * A.x[15],
 				      x[2] * A.x[12] + x[6] * A.x[13] + x[10] * A.x[14] + x[14] * A.x[15], x[3] * A.x[12] + x[7] * A.x[13] + x[11] * A.x[14] + x[15] * A.x[15]);
 	}

 	MATRIX_4X4<T> operator* (const T a) const
 	{
 		return MATRIX_4X4<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], a * x[9], a * x[10], a * x[11], a * x[12], a * x[13], a * x[14], a * x[15]);
 	}

 	MATRIX_4X4<T> operator/ (const T a) const
 	{
 		assert (a != 0);
 		T s = 1 / a;
 		return MATRIX_4X4<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], s * x[9], s * x[10], s * x[11], s * x[12], s * x[13], s * x[14], s * x[15]);
 	}

 	VECTOR_3D<T> operator* (const VECTOR_3D<T>& v) const // assumes w=1 is the 4th coordinate of v
 	{
 		T w = x[3] * v.x + x[7] * v.y + x[11] * v.z + x[15];
 		assert (w != 0);

 		if (w == 1) return VECTOR_3D<T> (x[0] * v.x + x[4] * v.y + x[8] * v.z + x[12], x[1] * v.x + x[5] * v.y + x[9] * v.z + x[13], x[2] * v.x + x[6] * v.y + x[10] * v.z + x[14]);
 		else
 		{
 			T s = 1 / w; // rescale so w=1
 			return VECTOR_3D<T> (s * (x[0] * v.x + x[4] * v.y + x[8] * v.z + x[12]), s * (x[1] * v.x + x[5] * v.y + x[9] * v.z + x[13]), s * (x[2] * v.x + x[6] * v.y + x[10] * v.z + x[14]));
 		}
 	}

 	VECTOR_3D<T> Transform_3X3 (const VECTOR_3D<T>& v) const // multiplies vector by upper 3x3 of matrix only
 	{
 		return VECTOR_3D<T> (x[0] * v.x + x[4] * v.y + x[8] * v.z, x[1] * v.x + x[5] * v.y + x[9] * v.z, x[2] * v.x + x[6] * v.y + x[10] * v.z);
 	}

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

 	MATRIX_4X4<T> Inverse()
 	{
 		int pivot_row;
 		T p1 = fabs (x[0]), p2 = fabs (x[1]), p3 = fabs (x[2]), p4 = fabs (x[3]);

 		if (p1 > p2)
 		{
 			if (p1 > p3)
 			{
 				if (p1 > p4) pivot_row = 1;
 				else pivot_row = 4;
 			}
 			else if (p3 > p4) pivot_row = 3;
 			else pivot_row = 4;
 		}
 		else if (p2 > p3)
 		{
 			if (p2 > p4) pivot_row = 2;
 			else pivot_row = 4;
 		}
 		else if (p3 > p4) pivot_row = 3;
 		else pivot_row = 4;

 		T a = x[pivot_row - 1];
 		assert (a != 0);
 		VECTOR_3D<T> b (x[3 + pivot_row], x[7 + pivot_row], x[11 + pivot_row]), c;
 		MATRIX_3X3<T> d;

 		switch (pivot_row)
 		{
 		case 1:
 			c = VECTOR_3D<T> (x[1], x[2], x[3]);
 			d = MATRIX_3X3<T> (x[5], x[6], x[7], x[9], x[10], x[11], x[13], x[14], x[15]);
 			break;
 		case 2:
 			c = VECTOR_3D<T> (x[0], x[2], x[3]);
 			d = MATRIX_3X3<T> (x[4], x[6], x[7], x[8], x[10], x[11], x[12], x[14], x[15]);
 			break;
 		case 3:
 			c = VECTOR_3D<T> (x[0], x[1], x[3]);
 			d = MATRIX_3X3<T> (x[4], x[5], x[7], x[8], x[9], x[11], x[12], x[13], x[15]);
 			break;
 		default:
 			c = VECTOR_3D<T> (x[0], x[1], x[2]);
 			d = MATRIX_3X3<T> (x[4], x[5], x[6], x[8], x[9], x[10], x[12], x[13], x[14]);
 		}

 		T m = -1 / a;
 		b *= m;
 		d += MATRIX_3X3<T>::Outer_Product (c, b); // find schur complement
 		MATRIX_3X3<T> h = d.Inverse();
 		VECTOR_3D<T> g = h * (c * m);
 		VECTOR_3D<T> f = b * h;
 		T e = VECTOR_3D<T>::Dot_Product (b, g) - m; // compute parts of inverse

 		switch (pivot_row)
 		{
 		case 1:
 			return MATRIX_4X4<T> (e, g.x, g.y, g.z, f.x, h (1, 1), h (2, 1), h (3, 1), f.y, h (1, 2), h (2, 2), h (3, 2), f.z, h (1, 3), h (2, 3), h (3, 3));
 			break;
 		case 2:
 			return MATRIX_4X4<T> (f.x, h (1, 1), h (2, 1), h (3, 1), e, g.x, g.y, g.z, f.y, h (1, 2), h (2, 2), h (3, 2), f.z, h (1, 3), h (2, 3), h (3, 3));
 			break;
 		case 3:
 			return MATRIX_4X4<T> (f.x, h (1, 1), h (2, 1), h (3, 1), f.y, h (1, 2), h (2, 2), h (3, 2), e, g.x, g.y, g.z, f.z, h (1, 3), h (2, 3), h (3, 3));
 			break;
 		default:
 			return MATRIX_4X4<T> (f.x, h (1, 1), h (2, 1), h (3, 1), f.y, h (1, 2), h (2, 2), h (3, 2), f.z, h (1, 3), h (2, 3), h (3, 3), e, g.x, g.y, g.z);
 		}
 	}

 	MATRIX_4X4<T> Cofactor_Matrix() const
 	{
 		T y[16];
 		y[0] = x[5] * x[10] * x[15] + x[9] * x[14] * x[7] + x[13] * x[6] * x[11] - x[5] * x[14] * x[11] - x[9] * x[6] * x[15] - x[13] * x[10] * x[7];
 		y[1] = -x[6] * x[11] * x[12] - x[10] * x[15] * x[4] - x[14] * x[7] * x[8] + x[6] * x[15] * x[8] + x[10] * x[7] * x[12] + x[14] * x[11] * x[4];
 		y[2] = x[7] * x[8] * x[13] + x[11] * x[12] * x[5] + x[15] * x[4] * x[9] - x[7] * x[12] * x[9] - x[11] * x[4] * x[13] - x[15] * x[8] * x[5];
 		y[3] = -x[4] * x[9] * x[14] - x[8] * x[13] * x[6] - x[12] * x[5] * x[10] + x[4] * x[13] * x[10] + x[8] * x[5] * x[14] + x[12] * x[9] * x[6];
 		y[4] = -x[9] * x[14] * x[3] - x[13] * x[2] * x[11] - x[1] * x[10] * x[15] + x[9] * x[2] * x[15] + x[13] * x[10] * x[3] + x[1] * x[14] * x[11];
 		y[5] = x[10] * x[15] * x[0] + x[14] * x[3] * x[8] + x[2] * x[11] * x[12] - x[10] * x[3] * x[12] - x[14] * x[11] * x[0] - x[2] * x[15] * x[8];
 		y[6] = -x[11] * x[12] * x[1] - x[15] * x[0] * x[9] - x[3] * x[8] * x[13] + x[11] * x[0] * x[13] + x[15] * x[8] * x[1] + x[3] * x[12] * x[9];
 		y[7] = x[8] * x[13] * x[2] + x[12] * x[1] * x[10] + x[0] * x[9] * x[14] - x[8] * x[1] * x[14] - x[12] * x[9] * x[2] - x[0] * x[13] * x[10];
 		y[8] = x[13] * x[2] * x[7] + x[1] * x[6] * x[15] + x[5] * x[14] * x[3] - x[13] * x[6] * x[3] - x[1] * x[14] * x[7] - x[5] * x[2] * x[15];
 		y[9] = -x[14] * x[3] * x[4] - x[2] * x[7] * x[12] - x[6] * x[15] * x[0] + x[14] * x[7] * x[0] + x[2] * x[15] * x[4] + x[6] * x[3] * x[12];
 		y[10] = x[15] * x[0] * x[5] + x[3] * x[4] * x[13] + x[7] * x[12] * x[1] - x[15] * x[4] * x[1] - x[3] * x[12] * x[5] - x[7] * x[0] * x[13];
 		y[11] = -x[12] * x[1] * x[6] - x[0] * x[5] * x[14] - x[4] * x[13] * x[2] + x[12] * x[5] * x[2] + x[0] * x[13] * x[6] + x[4] * x[1] * x[14];
 		y[12] = -x[1] * x[6] * x[11] - x[5] * x[10] * x[3] - x[9] * x[2] * x[7] + x[1] * x[10] * x[7] + x[5] * x[2] * x[11] + x[9] * x[6] * x[3];
 		y[13] = x[2] * x[7] * x[8] + x[6] * x[11] * x[0] + x[10] * x[3] * x[4] - x[2] * x[11] * x[4] - x[6] * x[3] * x[8] - x[10] * x[7] * x[0];
 		y[14] = -x[3] * x[4] * x[9] - x[7] * x[8] * x[1] - x[11] * x[0] * x[5] + x[3] * x[8] * x[5] + x[7] * x[0] * x[9] + x[11] * x[4] * x[1];
 		y[15] = x[0] * x[5] * x[10] + x[4] * x[9] * x[2] + x[8] * x[1] * x[6] - x[0] * x[9] * x[6] - x[4] * x[1] * x[10] - x[8] * x[5] * x[2];
 		return MATRIX_4X4<T> (y[0], y[1], y[2], y[3], y[4], y[5], y[6], y[7], y[8], y[9], y[10], y[11], y[12], y[13], y[14], y[15]);
 	}

 	void Invert_Rotation_And_Translation()
 	{
 		*this = Inverse_Rotation_And_Translation();
 	}

 	MATRIX_4X4<T> Inverse_Rotation_And_Translation()
 	{
 		return MATRIX_4X4<T> (x[0], x[4], x[8], 0, x[1], x[5], x[9], 0, x[2], x[6], x[10], 0, -x[0] * x[12] - x[1] * x[13] - x[2] * x[14], -x[4] * x[12] - x[5] * x[13] - x[6] * x[14], -x[8] * x[12] - x[9] * x[13] - x[10] * x[14], 1);
 	}

 	MATRIX_4X4<T> Rotation_Only() const
 	{
 		return MATRIX_4X4<T> (x[0], x[1], x[2], 0, x[4], x[5], x[6], 0, x[8], x[9], x[10], 0, 0, 0, 0, 1);
 	}

 	MATRIX_3X3<T> Upper_3X3() const
 	{
 		return MATRIX_3X3<T> (x[0], x[1], x[2], x[4], x[5], x[6], x[8], x[9], x[10]);
 	}

 	void Transpose()
 	{
 		exchange (x[1], x[4]);
 		exchange (x[2], x[8]);
 		exchange (x[3], x[12]);
 		exchange (x[6], x[9]);
 		exchange (x[7], x[13]);
 		exchange (x[11], x[14]);
 	}

 	static MATRIX_4X4<T> Transpose (const MATRIX_4X4<T>& A)
 	{
 		return MATRIX_4X4<T> (A.x[0], A.x[4], A.x[8], A.x[12], A.x[1], A.x[5], A.x[9], A.x[13], A.x[2], A.x[6], A.x[10], A.x[14], A.x[3], A.x[7], A.x[11], A.x[15]);
 	}

 	static MATRIX_4X4<T> Translation_Matrix (const VECTOR_3D<T>& translation)
 	{
 		return MATRIX_4X4<T> (1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, translation.x, translation.y, translation.z, 1);
 	}

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

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

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

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

 	static MATRIX_4X4<T> Rotation_Matrix (const VECTOR_3D<T>& axis, const T radians)
 	{
 		T c = cos (radians), s = sin (radians);
 		return MATRIX_4X4<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, 0,
 				      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, 0,
 				      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, 0,
 				      0, 0, 0, 1);
 	}

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

 	static MATRIX_4X4<T> Rotation_Matrix (const VECTOR_3D<T>& initial_vector, const VECTOR_3D<T>& final_vector, const T tolerance = 1e-14)
 	{
 		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 - tolerance) return Identity_Matrix();

 		if (cos_theta < -1 + tolerance) return MATRIX_4X4<T> (-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0, 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_4X4<T> Scale_Matrix (const VECTOR_3D<T>& scale_vector)
 	{
 		return MATRIX_4X4<T> (scale_vector.x, 0, 0, 0, 0, scale_vector.y, 0, 0, 0, 0, scale_vector.z, 0, 0, 0, 0, 1);
 	}

 	static MATRIX_4X4<T> Scale_Matrix (const T scale)
 	{
 		return MATRIX_4X4<T> (scale, 0, 0, 0, 0, scale, 0, 0, 0, 0, scale, 0, 0, 0, 0, 1);
 	}

 //#####################################################################
 	template<class S> friend MATRIX_4X4<S> operator* (const S a, const MATRIX_4X4<S>& A);
 };
 template<class T>
 inline MATRIX_4X4<T> operator* (const T a, const MATRIX_4X4<T>& A)
 {
 	return MATRIX_4X4<T> (a * A.x[0], a * A.x[1], a * A.x[2], a * A.x[3], a * A.x[4], a * A.x[5], a * A.x[6], a * A.x[7], a * A.x[8], a * A.x[9], a * A.x[10], a * A.x[11], a * A.x[12], a * A.x[13], a * A.x[14], a * A.x[15]);
 }

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

 	return input_stream;
 }

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

 		output_stream << std::endl;
 	}

 	return output_stream;
 }
 }
 #endif
	//#####################################################################
	// Copyright 2002-2004, Robert Bridson, Ronald Fedkiw, Geoffrey Irving, 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_4X4
	//#####################################################################
	#ifndef __MATRIX_4X4__
	#define __MATRIX_4X4__

	#include <assert.h>
	#include "VECTOR_3D.h"
	#include "MATRIX_3X3.h"
	#include "../Math_Tools/sqr.h"
	#include "../Math_Tools/constants.h"
	#include <math.h>
	namespace PhysBAM
	{

	template<class T>
	class MATRIX_4X4
	{
	public:
	T x[16];

	MATRIX_4X4()
	{
	for (int i = 0; i < 16; i++) x[i] = 0;
	}

	MATRIX_4X4 (const MATRIX_4X4<T>& matrix_input)
	{
	for (int i = 0; i < 16; i++) x[i] = matrix_input.x[i];
	}

	MATRIX_4X4 (const T x11, const T x21, const T x31, const T x41, const T x12, const T x22, const T x32, const T x42, const T x13, const T x23, const T x33, const T x43, const T x14, const T x24, const T x34,
	const T x44)
	{
	x[0] = x11;
	x[1] = x21;
	x[2] = x31;
	x[3] = x41;
	x[4] = x12;
	x[5] = x22;
	x[6] = x32;
	x[7] = x42;
	x[8] = x13;
	x[9] = x23;
	x[10] = x33;
	x[11] = x43;
	x[12] = x14;
	x[13] = x24;
	x[14] = x34;
	x[15] = x44;
	}

	explicit MATRIX_4X4 (const MATRIX_3X3<T>& matrix_input) // Create a homogeneous 4x4 matrix corresponding to 3x3 transform
	{
	x[0] = matrix_input.x[0];
	x[1] = matrix_input.x[1];
	x[2] = matrix_input.x[2];
	x[3] = (T) 0;
	x[4] = matrix_input.x[3];
	x[5] = matrix_input.x[4];
	x[6] = matrix_input.x[5];
	x[7] = (T) 0;
	x[8] = matrix_input.x[6];
	x[9] = matrix_input.x[7];
	x[10] = matrix_input.x[8];
	x[11] = (T) 0;
	x[12] = (T) 0;
	x[13] = (T) 0;
	x[14] = (T) 0;
	x[15] = (T) 1;
	}

	bool operator== (const MATRIX_4X4<T>& A) const
	{
	for (int i = 0; i < 16; i++) if (x[i] != A.x[i]) return false;

	return true;
	}

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

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

	MATRIX_4X4<T> operator-() const
	{
	return MATRIX_4X4<T> (-x[0], -x[1], -x[2], -x[3], -x[4], -x[5], -x[6], -x[7], -x[8], -x[9], -x[10], -x[11], -x[12], -x[13], -x[14], -x[15]);
	}

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

	return *this;
	}

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

	return *this;
	}

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

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

	return *this;
	}

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

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

	return *this;
	}

	MATRIX_4X4<T> operator+ (const MATRIX_4X4<T>& A) const
	{
	return MATRIX_4X4<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], x[9] + A.x[9], x[10] + A.x[10],
	x[11] + A.x[11], x[12] + A.x[12], x[13] + A.x[13], x[14] + A.x[14], x[15] + A.x[15]);
	}

	MATRIX_4X4<T> operator- (const MATRIX_4X4<T>& A) const
	{
	return MATRIX_4X4<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], x[9] - A.x[9], x[10] - A.x[10], x[11] - A.x[11],
	x[12] - A.x[12], x[13] - A.x[13], x[14] - A.x[14], x[15] - A.x[15]);
	}

	MATRIX_4X4<T> operator* (const MATRIX_4X4<T>& A) const
	{
	return MATRIX_4X4<T> (x[0] * A.x[0] + x[4] * A.x[1] + x[8] * A.x[2] + x[12] * A.x[3], x[1] * A.x[0] + x[5] * A.x[1] + x[9] * A.x[2] + x[13] * A.x[3],
	x[2] * A.x[0] + x[6] * A.x[1] + x[10] * A.x[2] + x[14] * A.x[3], x[3] * A.x[0] + x[7] * A.x[1] + x[11] * A.x[2] + x[15] * A.x[3],
	x[0] * A.x[4] + x[4] * A.x[5] + x[8] * A.x[6] + x[12] * A.x[7], x[1] * A.x[4] + x[5] * A.x[5] + x[9] * A.x[6] + x[13] * A.x[7],
	x[2] * A.x[4] + x[6] * A.x[5] + x[10] * A.x[6] + x[14] * A.x[7], x[3] * A.x[4] + x[7] * A.x[5] + x[11] * A.x[6] + x[15] * A.x[7],
	x[0] * A.x[8] + x[4] * A.x[9] + x[8] * A.x[10] + x[12] * A.x[11], x[1] * A.x[8] + x[5] * A.x[9] + x[9] * A.x[10] + x[13] * A.x[11],
	x[2] * A.x[8] + x[6] * A.x[9] + x[10] * A.x[10] + x[14] * A.x[11], x[3] * A.x[8] + x[7] * A.x[9] + x[11] * A.x[10] + x[15] * A.x[11],
	x[0] * A.x[12] + x[4] * A.x[13] + x[8] * A.x[14] + x[12] * A.x[15], x[1] * A.x[12] + x[5] * A.x[13] + x[9] * A.x[14] + x[13] * A.x[15],
	x[2] * A.x[12] + x[6] * A.x[13] + x[10] * A.x[14] + x[14] * A.x[15], x[3] * A.x[12] + x[7] * A.x[13] + x[11] * A.x[14] + x[15] * A.x[15]);
	}

	MATRIX_4X4<T> operator* (const T a) const
	{
	return MATRIX_4X4<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], a * x[9], a * x[10], a * x[11], a * x[12], a * x[13], a * x[14], a * x[15]);
	}

	MATRIX_4X4<T> operator/ (const T a) const
	{
	assert (a != 0);
	T s = 1 / a;
	return MATRIX_4X4<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], s * x[9], s * x[10], s * x[11], s * x[12], s * x[13], s * x[14], s * x[15]);
	}

	VECTOR_3D<T> operator* (const VECTOR_3D<T>& v) const // assumes w=1 is the 4th coordinate of v
	{
	T w = x[3] * v.x + x[7] * v.y + x[11] * v.z + x[15];
	assert (w != 0);

	if (w == 1) return VECTOR_3D<T> (x[0] * v.x + x[4] * v.y + x[8] * v.z + x[12], x[1] * v.x + x[5] * v.y + x[9] * v.z + x[13], x[2] * v.x + x[6] * v.y + x[10] * v.z + x[14]);
	else
	{
	T s = 1 / w; // rescale so w=1
	return VECTOR_3D<T> (s * (x[0] * v.x + x[4] * v.y + x[8] * v.z + x[12]), s * (x[1] * v.x + x[5] * v.y + x[9] * v.z + x[13]), s * (x[2] * v.x + x[6] * v.y + x[10] * v.z + x[14]));
	}
	}

	VECTOR_3D<T> Transform_3X3 (const VECTOR_3D<T>& v) const // multiplies vector by upper 3x3 of matrix only
	{
	return VECTOR_3D<T> (x[0] * v.x + x[4] * v.y + x[8] * v.z, x[1] * v.x + x[5] * v.y + x[9] * v.z, x[2] * v.x + x[6] * v.y + x[10] * v.z);
	}

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

	MATRIX_4X4<T> Inverse()
	{
	int pivot_row;
	T p1 = fabs (x[0]), p2 = fabs (x[1]), p3 = fabs (x[2]), p4 = fabs (x[3]);

	if (p1 > p2)
	{
	if (p1 > p3)
	{
	if (p1 > p4) pivot_row = 1;
	else pivot_row = 4;
	}
	else if (p3 > p4) pivot_row = 3;
	else pivot_row = 4;
	}
	else if (p2 > p3)
	{
	if (p2 > p4) pivot_row = 2;
	else pivot_row = 4;
	}
	else if (p3 > p4) pivot_row = 3;
	else pivot_row = 4;

	T a = x[pivot_row - 1];
	assert (a != 0);
	VECTOR_3D<T> b (x[3 + pivot_row], x[7 + pivot_row], x[11 + pivot_row]), c;
	MATRIX_3X3<T> d;

	switch (pivot_row)
	{
	case 1:
	c = VECTOR_3D<T> (x[1], x[2], x[3]);
	d = MATRIX_3X3<T> (x[5], x[6], x[7], x[9], x[10], x[11], x[13], x[14], x[15]);
	break;
	case 2:
	c = VECTOR_3D<T> (x[0], x[2], x[3]);
	d = MATRIX_3X3<T> (x[4], x[6], x[7], x[8], x[10], x[11], x[12], x[14], x[15]);
	break;
	case 3:
	c = VECTOR_3D<T> (x[0], x[1], x[3]);
	d = MATRIX_3X3<T> (x[4], x[5], x[7], x[8], x[9], x[11], x[12], x[13], x[15]);
	break;
	default:
	c = VECTOR_3D<T> (x[0], x[1], x[2]);
	d = MATRIX_3X3<T> (x[4], x[5], x[6], x[8], x[9], x[10], x[12], x[13], x[14]);
	}

	T m = -1 / a;
	b *= m;
	d += MATRIX_3X3<T>::Outer_Product (c, b); // find schur complement
	MATRIX_3X3<T> h = d.Inverse();
	VECTOR_3D<T> g = h * (c * m);
	VECTOR_3D<T> f = b * h;
	T e = VECTOR_3D<T>::Dot_Product (b, g) - m; // compute parts of inverse

	switch (pivot_row)
	{
	case 1:
	return MATRIX_4X4<T> (e, g.x, g.y, g.z, f.x, h (1, 1), h (2, 1), h (3, 1), f.y, h (1, 2), h (2, 2), h (3, 2), f.z, h (1, 3), h (2, 3), h (3, 3));
	break;
	case 2:
	return MATRIX_4X4<T> (f.x, h (1, 1), h (2, 1), h (3, 1), e, g.x, g.y, g.z, f.y, h (1, 2), h (2, 2), h (3, 2), f.z, h (1, 3), h (2, 3), h (3, 3));
	break;
	case 3:
	return MATRIX_4X4<T> (f.x, h (1, 1), h (2, 1), h (3, 1), f.y, h (1, 2), h (2, 2), h (3, 2), e, g.x, g.y, g.z, f.z, h (1, 3), h (2, 3), h (3, 3));
	break;
	default:
	return MATRIX_4X4<T> (f.x, h (1, 1), h (2, 1), h (3, 1), f.y, h (1, 2), h (2, 2), h (3, 2), f.z, h (1, 3), h (2, 3), h (3, 3), e, g.x, g.y, g.z);
	}
	}

	MATRIX_4X4<T> Cofactor_Matrix() const
	{
	T y[16];
	y[0] = x[5] * x[10] * x[15] + x[9] * x[14] * x[7] + x[13] * x[6] * x[11] - x[5] * x[14] * x[11] - x[9] * x[6] * x[15] - x[13] * x[10] * x[7];
	y[1] = -x[6] * x[11] * x[12] - x[10] * x[15] * x[4] - x[14] * x[7] * x[8] + x[6] * x[15] * x[8] + x[10] * x[7] * x[12] + x[14] * x[11] * x[4];
	y[2] = x[7] * x[8] * x[13] + x[11] * x[12] * x[5] + x[15] * x[4] * x[9] - x[7] * x[12] * x[9] - x[11] * x[4] * x[13] - x[15] * x[8] * x[5];
	y[3] = -x[4] * x[9] * x[14] - x[8] * x[13] * x[6] - x[12] * x[5] * x[10] + x[4] * x[13] * x[10] + x[8] * x[5] * x[14] + x[12] * x[9] * x[6];
	y[4] = -x[9] * x[14] * x[3] - x[13] * x[2] * x[11] - x[1] * x[10] * x[15] + x[9] * x[2] * x[15] + x[13] * x[10] * x[3] + x[1] * x[14] * x[11];
	y[5] = x[10] * x[15] * x[0] + x[14] * x[3] * x[8] + x[2] * x[11] * x[12] - x[10] * x[3] * x[12] - x[14] * x[11] * x[0] - x[2] * x[15] * x[8];
	y[6] = -x[11] * x[12] * x[1] - x[15] * x[0] * x[9] - x[3] * x[8] * x[13] + x[11] * x[0] * x[13] + x[15] * x[8] * x[1] + x[3] * x[12] * x[9];
	y[7] = x[8] * x[13] * x[2] + x[12] * x[1] * x[10] + x[0] * x[9] * x[14] - x[8] * x[1] * x[14] - x[12] * x[9] * x[2] - x[0] * x[13] * x[10];
	y[8] = x[13] * x[2] * x[7] + x[1] * x[6] * x[15] + x[5] * x[14] * x[3] - x[13] * x[6] * x[3] - x[1] * x[14] * x[7] - x[5] * x[2] * x[15];
	y[9] = -x[14] * x[3] * x[4] - x[2] * x[7] * x[12] - x[6] * x[15] * x[0] + x[14] * x[7] * x[0] + x[2] * x[15] * x[4] + x[6] * x[3] * x[12];
	y[10] = x[15] * x[0] * x[5] + x[3] * x[4] * x[13] + x[7] * x[12] * x[1] - x[15] * x[4] * x[1] - x[3] * x[12] * x[5] - x[7] * x[0] * x[13];
	y[11] = -x[12] * x[1] * x[6] - x[0] * x[5] * x[14] - x[4] * x[13] * x[2] + x[12] * x[5] * x[2] + x[0] * x[13] * x[6] + x[4] * x[1] * x[14];
	y[12] = -x[1] * x[6] * x[11] - x[5] * x[10] * x[3] - x[9] * x[2] * x[7] + x[1] * x[10] * x[7] + x[5] * x[2] * x[11] + x[9] * x[6] * x[3];
	y[13] = x[2] * x[7] * x[8] + x[6] * x[11] * x[0] + x[10] * x[3] * x[4] - x[2] * x[11] * x[4] - x[6] * x[3] * x[8] - x[10] * x[7] * x[0];
	y[14] = -x[3] * x[4] * x[9] - x[7] * x[8] * x[1] - x[11] * x[0] * x[5] + x[3] * x[8] * x[5] + x[7] * x[0] * x[9] + x[11] * x[4] * x[1];
	y[15] = x[0] * x[5] * x[10] + x[4] * x[9] * x[2] + x[8] * x[1] * x[6] - x[0] * x[9] * x[6] - x[4] * x[1] * x[10] - x[8] * x[5] * x[2];
	return MATRIX_4X4<T> (y[0], y[1], y[2], y[3], y[4], y[5], y[6], y[7], y[8], y[9], y[10], y[11], y[12], y[13], y[14], y[15]);
	}

	void Invert_Rotation_And_Translation()
	{
	*this = Inverse_Rotation_And_Translation();
	}

	MATRIX_4X4<T> Inverse_Rotation_And_Translation()
	{
	return MATRIX_4X4<T> (x[0], x[4], x[8], 0, x[1], x[5], x[9], 0, x[2], x[6], x[10], 0, -x[0] * x[12] - x[1] * x[13] - x[2] * x[14], -x[4] * x[12] - x[5] * x[13] - x[6] * x[14], -x[8] * x[12] - x[9] * x[13] - x[10] * x[14], 1);
	}

	MATRIX_4X4<T> Rotation_Only() const
	{
	return MATRIX_4X4<T> (x[0], x[1], x[2], 0, x[4], x[5], x[6], 0, x[8], x[9], x[10], 0, 0, 0, 0, 1);
	}

	MATRIX_3X3<T> Upper_3X3() const
	{
	return MATRIX_3X3<T> (x[0], x[1], x[2], x[4], x[5], x[6], x[8], x[9], x[10]);
	}

	void Transpose()
	{
	exchange (x[1], x[4]);
	exchange (x[2], x[8]);
	exchange (x[3], x[12]);
	exchange (x[6], x[9]);
	exchange (x[7], x[13]);
	exchange (x[11], x[14]);
	}

	static MATRIX_4X4<T> Transpose (const MATRIX_4X4<T>& A)
	{
	return MATRIX_4X4<T> (A.x[0], A.x[4], A.x[8], A.x[12], A.x[1], A.x[5], A.x[9], A.x[13], A.x[2], A.x[6], A.x[10], A.x[14], A.x[3], A.x[7], A.x[11], A.x[15]);
	}

	static MATRIX_4X4<T> Translation_Matrix (const VECTOR_3D<T>& translation)
	{
	return MATRIX_4X4<T> (1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, translation.x, translation.y, translation.z, 1);
	}

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

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

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

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

	static MATRIX_4X4<T> Rotation_Matrix (const VECTOR_3D<T>& axis, const T radians)
	{
	T c = cos (radians), s = sin (radians);
	return MATRIX_4X4<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, 0,
	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, 0,
	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, 0,
	0, 0, 0, 1);
	}

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

	static MATRIX_4X4<T> Rotation_Matrix (const VECTOR_3D<T>& initial_vector, const VECTOR_3D<T>& final_vector, const T tolerance = 1e-14)
	{
	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 - tolerance) return Identity_Matrix();

	if (cos_theta < -1 + tolerance) return MATRIX_4X4<T> (-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0, 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_4X4<T> Scale_Matrix (const VECTOR_3D<T>& scale_vector)
	{
	return MATRIX_4X4<T> (scale_vector.x, 0, 0, 0, 0, scale_vector.y, 0, 0, 0, 0, scale_vector.z, 0, 0, 0, 0, 1);
	}

	static MATRIX_4X4<T> Scale_Matrix (const T scale)
	{
	return MATRIX_4X4<T> (scale, 0, 0, 0, 0, scale, 0, 0, 0, 0, scale, 0, 0, 0, 0, 1);
	}

	//#####################################################################
	template<class S> friend MATRIX_4X4<S> operator* (const S a, const MATRIX_4X4<S>& A);
	};
	template<class T>
	inline MATRIX_4X4<T> operator* (const T a, const MATRIX_4X4<T>& A)
	{
	return MATRIX_4X4<T> (a * A.x[0], a * A.x[1], a * A.x[2], a * A.x[3], a * A.x[4], a * A.x[5], a * A.x[6], a * A.x[7], a * A.x[8], a * A.x[9], a * A.x[10], a * A.x[11], a * A.x[12], a * A.x[13], a * A.x[14], a * A.x[15]);
	}

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

	return input_stream;
	}

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

	output_stream << std::endl;
	}

	return output_stream;
	}
	}
	#endif