BansheeEngine/Source/BansheeUtility/Math/BsMatrix3.h

//********************************** Banshee Engine (www.banshee3d.com) **************************************************//
//**************** Copyright (c) 2016 Marko Pintera ([email protected]). All rights reserved. **********************//
#pragma once

#include "Prerequisites/BsPrerequisitesUtil.h"
#include "Math/BsVector3.h"

namespace bs
{
	/** @addtogroup Math
	 *  @{
	 */

	/** 
	 * A 3x3 matrix. Can be used for non-homogenous transformations of three dimensional vectors and points. In row major
	 * format. 
	 */
	class BS_UTILITY_EXPORT Matrix3
	{
	private:
		struct EulerAngleOrderData
		{
			int a, b, c;
			float sign;
		};

	public:
		Matrix3() {}

		Matrix3(BS_ZERO zero)
			:Matrix3(Matrix3::ZERO)
		{ }

		Matrix3(BS_IDENTITY identity)
			:Matrix3(Matrix3::IDENTITY)
		{ }

		Matrix3(const Matrix3& mat)
		{
			memcpy(m, mat.m, 9*sizeof(float));
		}

		Matrix3(float m00, float m01, float m02,
				float m10, float m11, float m12,
				float m20, float m21, float m22)
		{
			m[0][0] = m00;
			m[0][1] = m01;
			m[0][2] = m02;
			m[1][0] = m10;
			m[1][1] = m11;
			m[1][2] = m12;
			m[2][0] = m20;
			m[2][1] = m21;
			m[2][2] = m22;
		}

		/** Construct a matrix from a quaternion. */
		explicit Matrix3(const Quaternion& rotation)
		{
			fromQuaternion(rotation);
		}

		/** Construct a matrix that performs rotation and scale. */
		explicit Matrix3(const Quaternion& rotation, const Vector3& scale)
		{
			fromQuaternion(rotation);
			
			for (int row = 0; row < 3; row++)
			{
				for (int col = 0; col < 3; col++)
					m[row][col] = scale[row]*m[row][col];
			}
		}

		/** Construct a matrix from an angle/axis pair. */
		explicit Matrix3(const Vector3& axis, const Radian& angle)
		{
			fromAxisAngle(axis, angle);
		}

		/** Construct a matrix from 3 orthonormal local axes. */
		explicit Matrix3(const Vector3& xaxis, const Vector3& yaxis, const Vector3& zaxis)
		{
			fromAxes(xaxis, yaxis, zaxis);
		}

		/** 
		 * Construct a matrix from euler angles, YXZ ordering.
		 * 			
		 * @see		Matrix3::fromEulerAngles
		 */
		explicit Matrix3(const Radian& xAngle, const Radian& yAngle, const Radian& zAngle)
		{
			fromEulerAngles(xAngle, yAngle, zAngle);
		}

		/**
		 * Construct a matrix from euler angles, custom ordering.
		 * 			
		 * @see		Matrix3::fromEulerAngles
		 */
		explicit Matrix3(const Radian& xAngle, const Radian& yAngle, const Radian& zAngle, EulerAngleOrder order)
		{
			fromEulerAngles(xAngle, yAngle, zAngle, order);
		}

		/** Swaps the contents of this matrix with another. */
		void swap(Matrix3& other)
		{
			std::swap(m[0][0], other.m[0][0]);
			std::swap(m[0][1], other.m[0][1]);
			std::swap(m[0][2], other.m[0][2]);
			std::swap(m[1][0], other.m[1][0]);
			std::swap(m[1][1], other.m[1][1]);
			std::swap(m[1][2], other.m[1][2]);
			std::swap(m[2][0], other.m[2][0]);
			std::swap(m[2][1], other.m[2][1]);
			std::swap(m[2][2], other.m[2][2]);
		}

		/** Returns a row of the matrix. */
		float* operator[] (UINT32 row) const
		{
			assert(row < 3);

			return (float*)m[row];
		}

		Vector3 getColumn(UINT32 col) const;
		void setColumn(UINT32 col, const Vector3& vec);

		Matrix3& operator= (const Matrix3& rhs)
		{
			memcpy(m, rhs.m, 9*sizeof(float));
			return *this;
		}
		bool operator== (const Matrix3& rhs) const;
		bool operator!= (const Matrix3& rhs) const;

		Matrix3 operator+ (const Matrix3& rhs) const;
		Matrix3 operator- (const Matrix3& rhs) const;
		Matrix3 operator* (const Matrix3& rhs) const;
		Matrix3 operator- () const;
		Matrix3 operator* (float rhs) const;

		friend Matrix3 operator* (float lhs, const Matrix3& rhs);

		/** Transforms the given vector by this matrix and returns the newly transformed vector. */
		Vector3 transform(const Vector3& vec) const;

		/** Returns a transpose of the matrix (switched columns and rows). */
		Matrix3 transpose () const;

		/**
		 * Calculates an inverse of the matrix if it exists.
		 *
		 * @param[out]	mat			Resulting matrix inverse.
		 * @param[in]	fTolerance 	(optional) Tolerance to use when checking if determinant is zero (or near zero in this case).
		 * 							Zero determinant means inverse doesn't exist.
		 * @return					True if inverse exists, false otherwise.
		 */
		bool inverse(Matrix3& mat, float fTolerance = 1e-06f) const;

		/**
		 * Calculates an inverse of the matrix if it exists.
		 *
		 * @param[in]	fTolerance 	(optional) Tolerance to use when checking if determinant is zero (or near zero in this case).
		 * 							Zero determinant means inverse doesn't exist.
		 *
		 * @return					Resulting matrix inverse if it exists, otherwise a zero matrix.
		 */
		Matrix3 inverse(float fTolerance = 1e-06f) const;

		/** Calculates the matrix determinant. */
		float determinant() const;

		/**
		 * Decompose a Matrix3 to rotation and scale.
		 *
		 * @note	
		 * Matrix must consist only of rotation and uniform scale transformations, otherwise accurate results are not 
		 * guaranteed. Applying non-uniform scale guarantees rotation portion will not be accurate.
		 */
		void decomposition(Quaternion& rotation, Vector3& scale) const;

		/**
		 * Decomposes the matrix into various useful values.
		 *
		 * @param[out]	matL	Unitary matrix. Columns form orthonormal bases. If your matrix is affine and
		 * 						doesn't use non-uniform scaling this matrix will be a conjugate transpose of the rotation part of the matrix.
		 * @param[out]	matS	Singular values of the matrix. If your matrix is affine these will be scaling factors of the matrix.
		 * @param[out]	matR	Unitary matrix. Columns form orthonormal bases. If your matrix is affine and
		 * 						doesn't use non-uniform scaling this matrix will be the rotation part of the matrix.
		 */
		void singularValueDecomposition(Matrix3& matL, Vector3& matS, Matrix3& matR) const;

		/**
		 * Decomposes the matrix into a set of values.
		 *
		 * @param[out]	matQ	Columns form orthonormal bases. If your matrix is affine and
		 * 						doesn't use non-uniform scaling this matrix will be the rotation part of the matrix.
		 * @param[out]	vecD	If the matrix is affine these will be scaling factors of the matrix.
		 * @param[out]	vecU	If the matrix is affine these will be shear factors of the matrix.
		 */
		void QDUDecomposition(Matrix3& matQ, Vector3& vecD, Vector3& vecU) const;

		/** Gram-Schmidt orthonormalization (applied to columns of rotation matrix) */
		void orthonormalize();

		/**
		 * Converts an orthonormal matrix to axis angle representation.
		 *
		 * @note	Matrix must be orthonormal.
		 */
		void toAxisAngle(Vector3& axis, Radian& angle) const;

		/** Creates a rotation matrix from an axis angle representation. */
		void fromAxisAngle(const Vector3& axis, const Radian& angle);

		/**
		 * Converts an orthonormal matrix to quaternion representation.
		 *
		 * @note	Matrix must be orthonormal.
		 */
		void toQuaternion(Quaternion& quat) const;

		/** Creates a rotation matrix from a quaternion representation. */
		void fromQuaternion(const Quaternion& quat);

		/** Creates a matrix from a three axes. */
		void fromAxes(const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis);

		/**
		 * Converts an orthonormal matrix to euler angle (pitch/yaw/roll) representation.
		 *
		 * @param[in,out]	xAngle	Rotation about x axis. (AKA Pitch)
		 * @param[in,out]	yAngle  Rotation about y axis. (AKA Yaw)
		 * @param[in,out]	zAngle 	Rotation about z axis. (AKA Roll)
		 * @return					True if unique solution was found, false otherwise.
		 * 			
		 * @note	Matrix must be orthonormal.
		 */
		bool toEulerAngles(Radian& xAngle, Radian& yAngle, Radian& zAngle) const;

		/**
		 * Creates a rotation matrix from the provided Pitch/Yaw/Roll angles.
		 *
		 * @param[in]	xAngle	Rotation about x axis. (AKA Pitch)
		 * @param[in]	yAngle	Rotation about y axis. (AKA Yaw)
		 * @param[in]	zAngle	Rotation about z axis. (AKA Roll)
		 *
		 * @note	Matrix must be orthonormal.
		 * 			Since different values will be produced depending in which order are the rotations applied, this method assumes
		 * 			they are applied in YXZ order. If you need a specific order, use the overloaded "fromEulerAngles" method instead.
		 */
		void fromEulerAngles(const Radian& xAngle, const Radian& yAngle, const Radian& zAngle);

		/**
		 * Creates a rotation matrix from the provided Pitch/Yaw/Roll angles.
		 *
		 * @param[in]	xAngle	Rotation about x axis. (AKA Pitch)
		 * @param[in]	yAngle	Rotation about y axis. (AKA Yaw)
		 * @param[in]	zAngle	Rotation about z axis. (AKA Roll)
		 * @param[in]	order 	The order in which rotations will be applied. 
		 *						Different rotations can be created depending on the order.
		 *
		 * @note	Matrix must be orthonormal.
		 */
		void fromEulerAngles(const Radian& xAngle, const Radian& yAngle, const Radian& zAngle, EulerAngleOrder order);

		/**
		 * Eigensolver, matrix must be symmetric.
		 *
		 * @note	
		 * Eigenvectors are vectors which when transformed by the matrix, only change in magnitude, but not in direction. 
		 * Eigenvalue is that magnitude. In other words you will get the same result whether you multiply the vector by the 
		 * matrix or by its eigenvalue.
		 */
		void eigenSolveSymmetric(float eigenValues[3], Vector3 eigenVectors[3]) const;

		static const float EPSILON;
		static const Matrix3 ZERO;
		static const Matrix3 IDENTITY;

	protected:
		friend class Matrix4;

		// Support for eigensolver
		void tridiagonal (float diag[3], float subDiag[3]);
		bool QLAlgorithm (float diag[3], float subDiag[3]);

		// Support for singular value decomposition
		static const float SVD_EPSILON;
		static const unsigned int SVD_MAX_ITERS;
		static void bidiagonalize (Matrix3& matA, Matrix3& matL, Matrix3& matR);
		static void golubKahanStep (Matrix3& matA, Matrix3& matL, Matrix3& matR);

		// Euler angle conversions
		static const EulerAngleOrderData EA_LOOKUP[6];

		float m[3][3];
	};

	/** @} */

	/** @cond SPECIALIZATIONS */
	BS_ALLOW_MEMCPY_SERIALIZATION(Matrix3);
	/** @endcond */
}