cloudcompare/libs/qCC_db/include/ccGLMatrixTpl.h

#pragma once

//##########################################################################
//#                                                                        #
//#                              CLOUDCOMPARE                              #
//#                                                                        #
//#  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; version 2 or later of the License.      #
//#                                                                        #
//#  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.                          #
//#                                                                        #
//#          COPYRIGHT: EDF R&D / TELECOM ParisTech (ENST-TSI)             #
//#                                                                        #
//##########################################################################

//Local
#include "ccSerializableObject.h"

//CCCoreLib
#include <CCGeom.h>
#include <CCMath.h>

//Qt
#include <QTextStream>


//! Model view matrix size (OpenGL)
static const unsigned OPENGL_MATRIX_SIZE = 16;

//Matrix element shortcuts in (line,column) order
#define CC_MAT_R11 m_mat[0]
#define CC_MAT_R21 m_mat[1]
#define CC_MAT_R31 m_mat[2]
#define CC_MAT_R41 m_mat[3]

#define CC_MAT_R12 m_mat[4]
#define CC_MAT_R22 m_mat[5]
#define CC_MAT_R32 m_mat[6]
#define CC_MAT_R42 m_mat[7]

#define CC_MAT_R13 m_mat[8]
#define CC_MAT_R23 m_mat[9]
#define CC_MAT_R33 m_mat[10]
#define CC_MAT_R43 m_mat[11]

#define CC_MAT_R14 m_mat[12]
#define CC_MAT_R24 m_mat[13]
#define CC_MAT_R34 m_mat[14]
#define CC_MAT_R44 m_mat[15]

//! A 4x4 'transformation' matrix (column major order)
/** Transformation (M) is composed by a rotation (R) and a translation (T):
	M*X = R*X + T
**/
template <typename T> class ccGLMatrixTpl : public ccSerializableObject
{
public:

	//! Default constructor
	/** Matrix is set to identity (see toIdentity) by default.
	**/
	ccGLMatrixTpl() { toIdentity(); }

	//! Copy constructor
	ccGLMatrixTpl(const ccGLMatrixTpl<T>& mat) { for (unsigned i = 0; i < OPENGL_MATRIX_SIZE; ++i) m_mat[i] = mat.m_mat[i]; }

	//! Constructor from a float GL matrix array
	/** \param mat16f a 16 elements array (column major order)
	**/
	explicit ccGLMatrixTpl(const float* mat16f) { for (unsigned i = 0; i < OPENGL_MATRIX_SIZE; ++i) m_mat[i] = static_cast<T>(mat16f[i]); internalRescale(); }

	//! Constructor from a double GL matrix array
	/** \warning Will implicitly cast the elements to float!
		\param mat16d a 16 elements array (column major order)
	**/
	explicit ccGLMatrixTpl(const double* mat16d) { for (unsigned i = 0; i < OPENGL_MATRIX_SIZE; ++i) m_mat[i] = static_cast<T>(mat16d[i]); internalRescale(); }

	//! Constructor from 4 columns (X,Y,Z,T)
	/** \param X 3 first elements of the 1st column (last one is 0)
		\param Y 3 first elements of the 2nd column (last one is 0)
		\param Z 3 first elements of the 3rd column (last one is 0)
		\param Tr 3 first elements of the last column (last one is 1)
	**/
	ccGLMatrixTpl(const Vector3Tpl<T>& X, const Vector3Tpl<T>& Y, const Vector3Tpl<T>& Z, const Vector3Tpl<T>& Tr)
	{
		CC_MAT_R11 = X.x;  CC_MAT_R21 = X.y;  CC_MAT_R31 = X.z;  CC_MAT_R41 = 0;
		CC_MAT_R12 = Y.x;  CC_MAT_R22 = Y.y;  CC_MAT_R32 = Y.z;  CC_MAT_R42 = 0;
		CC_MAT_R13 = Z.x;  CC_MAT_R23 = Z.y;  CC_MAT_R33 = Z.z;  CC_MAT_R43 = 0;
		CC_MAT_R14 = Tr.x; CC_MAT_R24 = Tr.y; CC_MAT_R34 = Tr.z; CC_MAT_R44 = static_cast<T>(1);
	}

	//! Interpolates two matrices at relative position 'coef'
	/** \param coef interpolation position (should be between 0 and 1).
		\param glMat1 'left' matrix
		\param glMat2 'right' matrix
	**/
	static ccGLMatrixTpl Interpolate(T coef, const ccGLMatrixTpl<T>& glMat1, const ccGLMatrixTpl<T>& glMat2)
	{
		//we compute the transformation matrix between glMat1 and glMat2
		ccGLMatrixTpl<T> invTrans1 = glMat1.inverse();
		ccGLMatrixTpl<T> m12 = invTrans1 * glMat2;

		Vector3Tpl<T> axis,Tr;
		T alpha;
		m12.getParameters(alpha, axis, Tr);

		//we only have to interpolate the angle value
		alpha *= coef;
		//and the translation
		Tr *= coef;

		//we build-up the resulting matrix
		m12.initFromParameters(alpha, axis, Tr);

		//eventually we build-up resulting transformation
		return glMat1 * m12;
	}

	//! Creates a transformation matrix that rotates a vector to another
	/** Adapted from  "Efficiently Building a Matrix to Rotate One Vector to Another"
		By Tomas Möller, John Hughes, Journal of Graphics Tools, 4(4):1-4, 1999
		\param from normalized non-zero source vector
		\param to normalized non-zero destination vector
	**/
	static ccGLMatrixTpl<T> FromToRotation(const Vector3Tpl<T>& from, const Vector3Tpl<T>& to)
	{
		T c = from.dot(to);
		T f = (c < 0 ? -c : c);
		ccGLMatrixTpl<T> result;

		if ( CCCoreLib::LessThanEpsilon( 1.0 - f ) ) //"from" and "to"-vector almost parallel
		{
			// "to" vector most nearly orthogonal to "from"
			Vector3Tpl<T> x(0, 0, 0);
			if (std::abs(from.x) < std::abs(from.y))
			{
				if (std::abs(from.x) < std::abs(from.z))
					x.x = static_cast<T>(1);
				else
					x.z = static_cast<T>(1);
			}
			else
			{
				if (std::abs(from.y) < std::abs(from.z))
					x.y = static_cast<T>(1);
				else
					x.z = static_cast<T>(1);
			}

			Vector3Tpl<T> u = x - from;
			Vector3Tpl<T> v = x - to;

			T c1 = 2 / u.dot(u);
			T c2 = 2 / v.dot(v);
			T c3 = c1 * c2  * u.dot(v);

			T* mat = result.data();
			for (unsigned i = 0; i < 3; i++)
			{
				for (unsigned j = 0; j < 3; j++)
				{
					mat[i * 4 + j] = c3 * v.u[i] * u.u[j]
								   - c2 * v.u[i] * v.u[j]
								   - c1 * u.u[i] * u.u[j];
				}
				mat[i * 4 + i] += static_cast<T>(1);
			}
		}
		else  // the most common case, unless "from"="to", or "from"=-"to"
		{
			//see Efficiently Building a Matrix to Rotate One Vector to Another
			//T. Moller and J.F. Hugues (1999)
			Vector3Tpl<T> v = from.cross(to);
			T h = 1 / (1 + c);
			T hvx = h * v.x;
			T hvz = h * v.z;
			T hvxy = hvx * v.y;
			T hvxz = hvx * v.z;
			T hvyz = hvz * v.y;

			T* mat = result.data();
			mat[0] = c + hvx * v.x;
			mat[1] = hvxy + v.z;
			mat[2] = hvxz - v.y;

			mat[4] = hvxy - v.z;
			mat[5] = c + h * v.y * v.y;
			mat[6] = hvyz + v.x;

			mat[8] = hvxz + v.y;
			mat[9] = hvyz - v.x;
			mat[10] = c + hvz * v.z;
		}

		return result;
	}

	//! Converts a quaternion to a rotation matrix
	/** \param q quaternion (4 values: w,x,y,z)
		\return corresponding rotation matrix
	**/
	template<class Tq> static ccGLMatrixTpl<T> FromQuaternion(const Tq q[])
	{
		assert(q);

		ccGLMatrixTpl<T> rotMat;
		T* mat = rotMat.data();

		//diagonal
		{
			Tq q00 = q[0] * q[0];
			Tq q11 = q[1] * q[1];
			Tq q22 = q[2] * q[2];
			Tq q33 = q[3] * q[3];

			mat[0]	= static_cast<T>(q00 + q11 - q22 - q33);
			mat[5]	= static_cast<T>(q00 - q11 + q22 - q33);
			mat[10]	= static_cast<T>(q00 - q11 - q22 + q33);
			mat[15]	= static_cast<T>(1);
		}

		//non-diagonal elements
		{
			Tq q03 = q[0] * q[3];
			Tq q13 = q[1] * q[3];
			Tq q23 = q[2] * q[3];
			Tq q02 = q[0] * q[2];
			Tq q12 = q[1] * q[2];
			Tq q01 = q[0] * q[1];

			mat[1] = static_cast<T>((q12 + q03) * 2);
			mat[2] = static_cast<T>((q13 - q02) * 2);
			mat[4] = static_cast<T>((q12 - q03) * 2);
			mat[6] = static_cast<T>((q23 + q01) * 2);
			mat[8] = static_cast<T>((q13 + q02) * 2);
			mat[9] = static_cast<T>((q23 - q01) * 2);
		}

		return rotMat;
	}

	//! Generates a 'viewing' matrix from a looking vector and a 'up' direction
	/** \warning No translation is applied (pure rotation matrix)
		\param forward forward 'view' vector
		\param up up vector
		\return corresponding rotation matrix
	**/
	static ccGLMatrixTpl<T> FromViewDirAndUpDir(const Vector3Tpl<T>& forward, const Vector3Tpl<T>& up)
	{
		//normalize forward
		Vector3Tpl<T> uForward = forward;
		uForward.normalize();
		
		//side = forward x up
		Vector3Tpl<T> uSide = uForward.cross(up);
		uSide.normalize();
		
		//recompute 'up' as: up = side x forward
		Vector3Tpl<T> uUp = uSide.cross(uForward);
		uUp.normalize();

		ccGLMatrixTpl<T> matrix;
		{
			T* mat = matrix.data();
			mat[ 0] =  uSide.x;
			mat[ 4] =  uSide.y;
			mat[ 8] =  uSide.z;
			mat[12] =  0;
			mat[ 1] =  uUp.x;
			mat[ 5] =  uUp.y;
			mat[ 9] =  uUp.z;
			mat[13] =  0 ;
			mat[ 2] = -uForward.x;
			mat[ 6] = -uForward.y;
			mat[10] = -uForward.z;
			mat[14] =  0 ;
			mat[ 3] =  0;
			mat[ 7] =  0;
			mat[11] =  0;
			mat[15] =  static_cast<T>(1) ;
		}
		return matrix;
	}

	//! Converts a 'text' matrix to a ccGLMatrix
	/** \param[in] matText matrix in text format
		\param[out] success whether input matrix text is valid or not
	**/
	static ccGLMatrixTpl<T> FromString(const QString& matText, bool& success)
	{
		QStringList valuesStr = matText.simplified().split(QChar(' '), QString::SkipEmptyParts);
		if (valuesStr.size() != OPENGL_MATRIX_SIZE)
		{
			success = false;
			return ccGLMatrixTpl<T>();
		}

		ccGLMatrixTpl<T> matrix;
		T* matValues = matrix.data();
		for (unsigned i = 0; i < OPENGL_MATRIX_SIZE; ++i)
		{
			matValues[i] = static_cast<T>(valuesStr[(i % 4) * 4 + (i >> 2)].toDouble(&success));
			if (!success)
				return ccGLMatrixTpl<T>();
		}

		matrix.internalRescale();

		success = true;
		return matrix;
	}

	//! Returns matrix as a string
	/** \param precision numerical precision
		\param separator separator
		\return string
	**/
	QString toString(int precision = 12, QChar separator = ' ') const
	{
		QString str;
		for (unsigned l = 0; l < 4; ++l) //lines
		{
			for (unsigned c = 0; c < 4; ++c) //columns
			{
				str.append(QString::number(m_mat[c * 4 + l], 'f', precision));
				if (c != 3)
					str.append(separator);
			}
			if (l != 3)
				str.append("\n");
		}
		return str;
	}

	//! Saves matrix to an ASCII file
	/** \param filename output file name
		\param precision output digits precision
	***/
	virtual bool toAsciiFile(QString filename, int precision = 12) const
	{
		QFile fp(filename);
		if (!fp.open(QFile::WriteOnly | QFile::Text))
			return false;

		QTextStream stream(&fp);
		stream.setRealNumberPrecision(precision);
		stream.setRealNumberNotation(QTextStream::FixedNotation);
		for (unsigned i = 0; i < 4; ++i)
		{
			stream << m_mat[i] << " " << m_mat[i + 4] << " " << m_mat[i + 8] << " " << m_mat[i + 12] << endl;
		}

		return (fp.error() == QFile::NoError);
	}

	//! Loads matrix from an ASCII file
	/** \param filename input file name
	***/
	virtual bool fromAsciiFile(QString filename)
	{
		QFile fp(filename);
		if (!fp.open(QFile::ReadOnly | QFile::Text))
			return false;

		QTextStream stream(&fp);

		for (unsigned i = 0; i < 4; ++i)
		{
			stream >> m_mat[i     ];
			stream >> m_mat[i +  4];
			stream >> m_mat[i +  8];
			stream >> m_mat[i + 12];
		}

		internalRescale();

		return (fp.error() == QFile::NoError);
	}

	//! Returns the rotation component around X only
	ccGLMatrixTpl<T> xRotation() const
	{
		ccGLMatrixTpl<T> newRotMat;
		newRotMat.toIdentity();

		//we use a specific Euler angles convention here
		if (CC_MAT_R13 >= 1)
		{
			//simpler/faster to ignore this (very) specific case!
			return newRotMat;
		}
		T phi = -asin(CC_MAT_R13);
		T cos_phi = cos(phi);
		T theta = atan2(CC_MAT_R23 / cos_phi, CC_MAT_R33 / cos_phi);

		newRotMat.CC_MAT_R22 = newRotMat.CC_MAT_R33 = cos(theta);
		newRotMat.CC_MAT_R32 = newRotMat.CC_MAT_R23 = sin(theta);
		newRotMat.CC_MAT_R23 *= static_cast<T>(-1);

		newRotMat.setTranslation(getTranslation());

		return newRotMat;
	}

	//! Returns the rotation component around Y only
	ccGLMatrixTpl<T> yRotation() const
	{
		ccGLMatrixTpl<T> newRotMat;
		newRotMat.toIdentity();

		//we use a specific Euler angles convention here
		if (CC_MAT_R32 >= 1)
		{
			//simpler/faster to ignore this (very) specific case!
			return newRotMat;
		}
		T theta = asin(CC_MAT_R32);
		T cos_theta = cos(theta);
		T phi = atan2(-CC_MAT_R31 / cos_theta, CC_MAT_R33 / cos_theta);

		newRotMat.CC_MAT_R11 = newRotMat.CC_MAT_R33 = cos(phi);
		newRotMat.CC_MAT_R31 = newRotMat.CC_MAT_R13 = sin(phi);
		newRotMat.CC_MAT_R31 *= static_cast<T>(-1);

		newRotMat.setTranslation(getTranslation());

		return newRotMat;
	}

	//! Returns the rotation component around Z only
	ccGLMatrixTpl<T> zRotation() const
	{
		//we can use the standard Euler angles convention here
		T phi, theta, psi;
		Vector3Tpl<T> Tr;
		getParameters(phi, theta, psi, Tr);
		assert(Tr.norm2() == 0);

		ccGLMatrixTpl<T> newRotMat;
		newRotMat.initFromParameters(phi, 0, 0, Tr);

		return newRotMat;
	}

	//! Clears matrix
	/** Matrix is set to 0.
	**/
	inline virtual void toZero() { memset(m_mat, 0, OPENGL_MATRIX_SIZE * sizeof(T)); }

	//! Sets matrix to identity
	inline virtual void toIdentity() { toZero(); CC_MAT_R11 = CC_MAT_R22 = CC_MAT_R33 = CC_MAT_R44 = static_cast<T>(1); }

	//! Returns whether this matrix is equal to identity
	inline virtual bool isIdentity() const
	{
		for (unsigned l = 0; l < 4; ++l) //lines
		{
			for (unsigned c = 0; c < 4; ++c) //columns
			{
				if (m_mat[c * 4 + l] != static_cast<T>(l == c ? 1 : 0))
					return false;
			}
		}

		return true;
	}

	//! Clears translation
	/** Translation is set to (0,0,0).
	**/
	inline void clearTranslation() { CC_MAT_R14 = CC_MAT_R24 = CC_MAT_R34 = 0; }

	//! Inits transformation from a rotation axis, an angle and a translation
	/** \param[in] alpha_rad rotation angle (in radians)
		\param[in] axis3D rotation axis
		\param[in] t3D translation
	**/
	void initFromParameters(T alpha_rad,
							const Vector3Tpl<T>& axis3D,
							const Vector3Tpl<T>& t3D)
	{
		T cos_t = cos(alpha_rad);
		T sin_t = sin(alpha_rad);
		T inv_cos_t = static_cast<T>(1) - cos_t;

		//normalize rotation axis
		Vector3Tpl<T> uAxis3D = axis3D;
		uAxis3D.normalize();

		const T& l1 = uAxis3D.x;
		const T& l2 = uAxis3D.y;
		const T& l3 = uAxis3D.z;

		T l1_inv_cos_t = l1 * inv_cos_t;
		T l3_inv_cos_t = l3 * inv_cos_t;

		//1st column
		CC_MAT_R11 = cos_t + l1 * l1_inv_cos_t;
		CC_MAT_R21 = l2 * l1_inv_cos_t + l3 * sin_t;
		CC_MAT_R31 = l3 * l1_inv_cos_t - l2 * sin_t;
		CC_MAT_R41 = 0;

		//2nd column
		CC_MAT_R12 = l2 * l1_inv_cos_t - l3 * sin_t;
		CC_MAT_R22 = cos_t + l2 * l2*inv_cos_t;
		CC_MAT_R32 = l2 * l3_inv_cos_t + l1 * sin_t;
		CC_MAT_R42 = 0;

		//3rd column
		CC_MAT_R13 = l3 * l1_inv_cos_t + l2 * sin_t;
		CC_MAT_R23 = l2 * l3_inv_cos_t - l1 * sin_t;
		CC_MAT_R33 = cos_t + l3 * l3_inv_cos_t;
		CC_MAT_R43 = 0;

		//4th column
		CC_MAT_R14 = t3D.x;
		CC_MAT_R24 = t3D.y;
		CC_MAT_R34 = t3D.z;
		CC_MAT_R44 = static_cast<T>(1);
	}


	//! Inits transformation from 3 rotation angles and a translation
	/** See http://en.wikipedia.org/wiki/Euler_angles (Tait-Bryan Z1Y2X3)
		\param[in] phi_rad Phi angle (in radians)
		\param[in] theta_rad Theta angle (in radians)
		\param[in] psi_rad Psi angle (in radians)
		\param[in] t3D translation
		\param[in] scale rotation scale
	**/
	void initFromParameters(	T phi_rad,
								T theta_rad,
								T psi_rad,
								const Vector3Tpl<T>& t3D)
	{
		T c1 = cos(phi_rad);
		T c2 = cos(theta_rad);
		T c3 = cos(psi_rad);

		T s1 = sin(phi_rad);
		T s2 = sin(theta_rad);
		T s3 = sin(psi_rad);

		//1st column
		CC_MAT_R11 = c2 * c1;
		CC_MAT_R21 = c2 * s1;
		CC_MAT_R31 = -s2;
		CC_MAT_R41 = 0;

		//2nd column
		CC_MAT_R12 = s3 * s2*c1 - c3 * s1;
		CC_MAT_R22 = s3 * s2*s1 + c3 * c1;
		CC_MAT_R32 = s3 * c2;
		CC_MAT_R42 = 0;

		//3rd column
		CC_MAT_R13 = c3 * s2*c1 + s3 * s1;
		CC_MAT_R23 = c3 * s2*s1 - s3 * c1;
		CC_MAT_R33 = c3 * c2;
		CC_MAT_R43 = 0;

		//4th column
		CC_MAT_R14 = t3D.x;
		CC_MAT_R24 = t3D.y;
		CC_MAT_R34 = t3D.z;
		CC_MAT_R44 = static_cast<T>(1);
	}

	//! Returns equivalent parameters: a rotation axis, an angle and a translation
	/** \param[out] alpha_rad rotation angle in radians (in [0;pi])
		\param[out] axis3D unit rotation axis
		\param[out] t3D translation
		\param[out] scale [optional] scale
	**/
	void getParameters(	T& alpha_rad,
						Vector3Tpl<T>& axis3D,
						Vector3Tpl<T>& t3D,
						T* scale = nullptr) const
	{
		T trace = CC_MAT_R11 + CC_MAT_R22 + CC_MAT_R33;

		// we arbitrarily consider the first column to read the scale
		Vector3Tpl<T> rotX = getColumnAsVec3D(0);
		T _scale = rotX.norm();
		if (scale)
		{
			*scale = _scale;
		}
		if (_scale != static_cast<T>(1) && CCCoreLib::GreaterThanEpsilon(_scale))
		{
			trace /= _scale;
		}

		T cos_t = (trace - 1) / 2;
		if (std::abs(cos_t) <= 1)
		{
			alpha_rad = acos(cos_t); //result in [0;pi]
		}
		else
		{
			alpha_rad = 0;
		}

		axis3D.x = CC_MAT_R32 - CC_MAT_R23;
		axis3D.y = CC_MAT_R13 - CC_MAT_R31;
		axis3D.z = CC_MAT_R21 - CC_MAT_R12;

		//normalize axis
		T n2 = axis3D.norm2();
		if (CCCoreLib::GreaterThanEpsilon(n2))
		{
			axis3D /= sqrt(n2);
		}
		else
		{
			//axis is too small!
			axis3D = Vector3Tpl<T>(0, 0, 1);
		}

		t3D.x = CC_MAT_R14;
		t3D.y = CC_MAT_R24;
		t3D.z = CC_MAT_R34;
	}

	//! Returns equivalent parameters: 3 rotation angles and a translation
	/** See http://en.wikipedia.org/wiki/Euler_angles (Tait-Bryan Z1Y2X3)
		\param[out] phi_rad Phi angle (in radians)
		\param[out] theta_rad Theta angle (in radians)
		\param[out] psi_rad Psi angle (in radians)
		\param[out] t3D translation
		\param[out] scale [optional] scale
	**/
	void getParameters(	T &phi_rad,
						T &theta_rad,
						T &psi_rad,
						Vector3Tpl<T>& t3D,
						T* scale = nullptr) const
	{
		Vector3Tpl<T> rotX = getColumnAsVec3D(0);
		Vector3Tpl<T> rotY = getColumnAsVec3D(1);
		Vector3Tpl<T> rotZ = getColumnAsVec3D(2);

		// we arbitrarily consider the first column to read the scale
		T _scale = rotX.norm();
		if (scale)
		{
			*scale = _scale;
		}
		if (_scale != static_cast<T>(1) && CCCoreLib::GreaterThanEpsilon(_scale))
		{
			rotX /= _scale;
			rotY /= _scale;
			rotZ /= _scale;
		}

		if (std::abs(rotX.z) != 1)
		{
			theta_rad = -asin(rotX.z);
			T cos_theta = cos(theta_rad);
			psi_rad = atan2(rotY.z / cos_theta, rotZ.z / cos_theta);
			phi_rad = atan2(rotX.y / cos_theta, rotX.x / cos_theta);

			//Other solution
			//theta = M_PI + asin(rotX.z);
			//T cos_theta = cos(theta);
			//psi = atan2(rotY.z / cos_theta, rotZ.z / cos_theta);
			//phi = atan2(rotX.y / cos_theta, rotX.x / cos_theta);
		}
		else
		{
			phi_rad = 0;

			if (rotX.z == -1)
			{
				theta_rad = static_cast<T>(M_PI_2);
				psi_rad = atan2(rotY.x, rotZ.x);
			}
			else
			{
				theta_rad = -static_cast<T>(M_PI_2);
				psi_rad = -atan2(rotY.x, rotZ.x);
			}
		}

		t3D.x = CC_MAT_R14;
		t3D.y = CC_MAT_R24;
		t3D.z = CC_MAT_R34;
	}

	//! Returns a pointer to internal data
	inline T* data() { return m_mat; }

	//! Returns a const pointer to internal data
	inline const T* data() const { return m_mat; }

	//! Returns a pointer to internal translation
	/** Translation corresponds to the beginning of the
		third column of the matrix.
	**/
	inline T* getTranslation() { return m_mat+12; }

	//! Returns a const pointer to internal translation
	/** Translation corresponds to the beginning of the
		third column of the matrix.
	**/
	inline const T* getTranslation() const { return m_mat+12; }

	//! Returns a copy of the translation as a CCVector3
	/** \return translation vector
	**/
	inline Vector3Tpl<T> getTranslationAsVec3D() const { return getColumnAsVec3D(3); }

	//! Sets translation (float version)
	/** \param Tr 3D vector **/
	inline void setTranslation(const Vector3Tpl<float>& Tr) { CC_MAT_R14 = static_cast<T>(Tr.x); CC_MAT_R24 = static_cast<T>(Tr.y); CC_MAT_R34 = static_cast<T>(Tr.z); }
	//! Sets translation (double version)
	/** \param Tr 3D vector **/
	inline void setTranslation(const Vector3Tpl<double>& Tr) { CC_MAT_R14 = static_cast<T>(Tr.x); CC_MAT_R24 = static_cast<T>(Tr.y); CC_MAT_R34 = static_cast<T>(Tr.z); }

	//! Sets translation from a float array
	/** \param Tr 3D vector as a float array
	**/
	void setTranslation(const float Tr[3]) { CC_MAT_R14 = static_cast<T>(Tr[0]); CC_MAT_R24 = static_cast<T>(Tr[1]); CC_MAT_R34 = static_cast<T>(Tr[2]); }
	//! Sets translation from a double array
	/** \param Tr 3D vector as a double array
	**/
	void setTranslation(const double Tr[3]) { CC_MAT_R14 = static_cast<T>(Tr[0]); CC_MAT_R24 = static_cast<T>(Tr[1]); CC_MAT_R34 = static_cast<T>(Tr[2]); }

	//! Returns a pointer to a given column
	/** \param index column index (between 0 and 3)
		\return pointer to the first element of the corresponding column
	**/
	inline T* getColumn(unsigned index) { return m_mat + (index << 2); }

	//! Returns a const pointer to a given column
	/** \param index column index (between 0 and 3)
		\return pointer to the first element of the corresponding column
	**/
	inline const T* getColumn(unsigned index) const { return m_mat + (index << 2); }

	//! Returns a copy of a given column as a CCVector3
	/** 4th value is ignored.
		\param index column index (between 0 and 3)
		\return copy of the three first elements of the corresponding column
	**/
	inline Vector3Tpl<T> getColumnAsVec3D(unsigned index) const { return Vector3Tpl<T>::fromArray(getColumn(index)); }

	//! Sets the content of a given column
	/** \param index column index (between 0 and 3)
		\param v new column values
	**/
	inline void setColumn(unsigned index, const Vector3Tpl<T>& v) { T* col = m_mat + (index << 2); col[0] = v.x; col[1] = v.y; col[2] = v.z; }

	//! Sets the content of a given column
	/** \param index column index (between 0 and 3)
		\param v new column values
	**/
	inline void setColumn(unsigned index, const Tuple4Tpl<T>& v) { T* col = m_mat + (index << 2); col[0] = v.x; col[1] = v.y; col[2] = v.z; col[3] = v.w; }

	//! Multiplication by a matrix operator
	ccGLMatrixTpl<T> operator * (const ccGLMatrixTpl<T>& mat) const
	{
		ccGLMatrixTpl<T> result;

		const T* A = m_mat;
		const T* B = mat.m_mat;
		T* C = result.m_mat;

		for (unsigned j = 0; j < 4; ++j, B += 4)
			for (unsigned i = 0; i < 4; ++i)
				*C++ = A[i] * B[0] + A[i + 4] * B[1] + A[i + 8] * B[2] + A[i + 12] * B[3];

		return result;
	}

	//! Multiplication by a vector operator (float version)
	inline Vector3Tpl<float> operator * (const Vector3Tpl<float>& vec) const { return Vector3Tpl<float>(applyX(vec), applyY(vec), applyZ(vec)); }
	//! Multiplication by a vector operator (double version)
	inline Vector3Tpl<double> operator * (const Vector3Tpl<double>& vec) const { return Vector3Tpl<double>(applyX(vec), applyY(vec), applyZ(vec)); }

	//! Multiplication by a 4D vector operator (float version)
	inline Tuple4Tpl<float> operator * (const Tuple4Tpl<float>& vec) const { return Tuple4Tpl<float>(applyX(vec), applyY(vec), applyZ(vec), applyW(vec)); }
	//! Multiplication by a 4D vector operator (double version)
	inline Tuple4Tpl<double> operator * (const Tuple4Tpl<double>& vec) const { return Tuple4Tpl<double>(applyX(vec), applyY(vec), applyZ(vec), applyW(vec)); }

	//! (in place) Addition operator
	ccGLMatrixTpl<T>& operator += (const ccGLMatrixTpl<T>& mat)
	{
		for (unsigned i = 0; i < OPENGL_MATRIX_SIZE; ++i)
			m_mat[i] += mat.m_mat[i];
		return (*this);
	}

	//! (in place) Difference operator
	ccGLMatrixTpl<T>& operator -= (const ccGLMatrixTpl<T>& mat)
	{
		for (unsigned i = 0; i < OPENGL_MATRIX_SIZE; ++i)
			m_mat[i] -= mat.m_mat[i];
		return (*this);
	}

	//! (in place) Multiplication operator
	ccGLMatrixTpl<T>& operator *= (const ccGLMatrixTpl<T>& mat)
	{
		(*this) = (*this) * mat;
		return (*this);
	}

	//! (in place) Forward translation operator (float version)
	ccGLMatrixTpl<T>& operator += (const Vector3Tpl<float>& Tr)
	{
		CC_MAT_R14 += static_cast<T>(Tr.x);
		CC_MAT_R24 += static_cast<T>(Tr.y);
		CC_MAT_R34 += static_cast<T>(Tr.z);
		return (*this);
	}
	//! (in place) Forward translation operator (double version)
	ccGLMatrixTpl<T>& operator += (const Vector3Tpl<double>& Tr)
	{
		CC_MAT_R14 += static_cast<T>(Tr.x);
		CC_MAT_R24 += static_cast<T>(Tr.y);
		CC_MAT_R34 += static_cast<T>(Tr.z);
		return (*this);
	}
	//! (in place) Backward translation operator (float version)
	ccGLMatrixTpl<T>& operator -= (const Vector3Tpl<float>& Tr)
	{
		CC_MAT_R14 -= static_cast<T>(Tr.x);
		CC_MAT_R24 -= static_cast<T>(Tr.y);
		CC_MAT_R34 -= static_cast<T>(Tr.z);
		return (*this);
	}
	//! (in place) Backward translation operator (double version)
	ccGLMatrixTpl<T>& operator -= (const Vector3Tpl<double>& Tr)
	{
		CC_MAT_R14 -= static_cast<T>(Tr.x);
		CC_MAT_R24 -= static_cast<T>(Tr.y);
		CC_MAT_R34 -= static_cast<T>(Tr.z);
		return (*this);
	}

	//! Returns the value at a given position
	T operator () (unsigned row, unsigned col) const
	{
		return m_mat[(col << 2) + row];
	}

	//! Applies transformation to a 3D vector (in place) - float version
	/** Input vector is directly modified after calling this method
	**/
	inline void apply(Vector3Tpl<float>& vec) const { vec = (*this)*vec; }
	//! Applies transformation to a 3D vector (in place) - double version
	/** Input vector is directly modified after calling this method
	**/
	inline void apply(Vector3Tpl<double>& vec) const { vec = (*this)*vec; }

	//! Applies transformation to a 4D vector (in place) - float version
	/** Input vector is directly modified after calling this method
	**/
	inline void apply(Tuple4Tpl<float>& vec) const { vec = (*this)*vec; }
	//! Applies transformation to a 3D vector (in place) - double version
	/** Input vector is directly modified after calling this method
	**/
	inline void apply(Tuple4Tpl<double>& vec) const { vec = (*this)*vec; }

	//! Get the resulting transformation along X dimension (float version)
	inline float applyX(const Vector3Tpl<float>& vec) const
	{
		return    static_cast<float>(CC_MAT_R11) * vec.x
				+ static_cast<float>(CC_MAT_R12) * vec.y
				+ static_cast<float>(CC_MAT_R13) * vec.z
				+ static_cast<float>(CC_MAT_R14);
	}
	//! Get the resulting transformation along X dimension (double version)
	inline double applyX(const Vector3Tpl<double>& vec) const
	{
		return    static_cast<double>(CC_MAT_R11) * vec.x
				+ static_cast<double>(CC_MAT_R12) * vec.y
				+ static_cast<double>(CC_MAT_R13) * vec.z
				+ static_cast<double>(CC_MAT_R14);
	}

	//! Get the resulting transformation along Y dimension (float version)
	inline float applyY(const Vector3Tpl<float>& vec) const
	{
		return    static_cast<float>(CC_MAT_R21) * vec.x
				+ static_cast<float>(CC_MAT_R22) * vec.y
				+ static_cast<float>(CC_MAT_R23) * vec.z
				+ static_cast<float>(CC_MAT_R24);
	}
	//! Get the resulting transformation along Y dimension (double version)
	inline double applyY(const Vector3Tpl<double>& vec) const
	{
		return    static_cast<double>(CC_MAT_R21) * vec.x
				+ static_cast<double>(CC_MAT_R22) * vec.y
				+ static_cast<double>(CC_MAT_R23) * vec.z
				+ static_cast<double>(CC_MAT_R24);
	}

	//! Get the resulting transformation along Z dimension (float version)
	inline float applyZ(const Vector3Tpl<float>& vec) const
	{
		return    static_cast<float>(CC_MAT_R31) * vec.x
				+ static_cast<float>(CC_MAT_R32) * vec.y
				+ static_cast<float>(CC_MAT_R33) * vec.z
				+ static_cast<float>(CC_MAT_R34);
	}
	//! Get the resulting transformation along Z dimension (double version)
	inline double applyZ(const Vector3Tpl<double>& vec) const
	{
		return    static_cast<double>(CC_MAT_R31) * vec.x
				+ static_cast<double>(CC_MAT_R32) * vec.y
				+ static_cast<double>(CC_MAT_R33) * vec.z
				+ static_cast<double>(CC_MAT_R34);
	}

	//! Get the resulting transformation along X dimension (float version)
	inline float applyX(const Tuple4Tpl<float>& vec) const
	{
		return    static_cast<float>(CC_MAT_R11) * vec.x
				+ static_cast<float>(CC_MAT_R12) * vec.y
				+ static_cast<float>(CC_MAT_R13) * vec.z
				+ static_cast<float>(CC_MAT_R14) * vec.w;
	}
	//! Get the resulting transformation along X dimension (double version)
	inline double applyX(const Tuple4Tpl<double>& vec) const
	{
		return    static_cast<double>(CC_MAT_R11) * vec.x
				+ static_cast<double>(CC_MAT_R12) * vec.y
				+ static_cast<double>(CC_MAT_R13) * vec.z
				+ static_cast<double>(CC_MAT_R14) * vec.w;
	}

	//! Get the resulting transformation along Y dimension (float version)
	inline float applyY(const Tuple4Tpl<float>& vec) const
	{
		return    static_cast<float>(CC_MAT_R21) * vec.x
				+ static_cast<float>(CC_MAT_R22) * vec.y
				+ static_cast<float>(CC_MAT_R23) * vec.z
				+ static_cast<float>(CC_MAT_R24) * vec.w;
	}
	//! Get the resulting transformation along Y dimension (double version)
	inline double applyY(const Tuple4Tpl<double>& vec) const
	{
		return    static_cast<double>(CC_MAT_R21) * vec.x
				+ static_cast<double>(CC_MAT_R22) * vec.y
				+ static_cast<double>(CC_MAT_R23) * vec.z
				+ static_cast<double>(CC_MAT_R24) * vec.w;
	}

	//! Get the resulting transformation along Z dimension (float version)
	inline float applyZ(const Tuple4Tpl<float>& vec) const
	{
		return    static_cast<float>(CC_MAT_R31) * vec.x
				+ static_cast<float>(CC_MAT_R32) * vec.y
				+ static_cast<float>(CC_MAT_R33) * vec.z
				+ static_cast<float>(CC_MAT_R34) * vec.w;
	}
	//! Get the resulting transformation along Z dimension (double version)
	inline double applyZ(const Tuple4Tpl<double>& vec) const
	{
		return    static_cast<double>(CC_MAT_R31) * vec.x
				+ static_cast<double>(CC_MAT_R32) * vec.y
				+ static_cast<double>(CC_MAT_R33) * vec.z
				+ static_cast<double>(CC_MAT_R34) * vec.w;
	}

	//! Get the resulting transformation along the 4th dimension (float version)
	inline float applyW(const Tuple4Tpl<float>& vec) const
	{
		return    static_cast<float>(CC_MAT_R41) * vec.x
				+ static_cast<float>(CC_MAT_R42) * vec.y
				+ static_cast<float>(CC_MAT_R43) * vec.z
				+ static_cast<float>(CC_MAT_R44) * vec.w;
	}
	//! Get the resulting transformation along the 4th dimension (double version)
	inline double applyW(const Tuple4Tpl<double>& vec) const
	{
		return    static_cast<double>(CC_MAT_R41) * vec.x
				+ static_cast<double>(CC_MAT_R42) * vec.y
				+ static_cast<double>(CC_MAT_R43) * vec.z
				+ static_cast<double>(CC_MAT_R44) * vec.w;
	}

	//! Applies rotation only to a 3D vector (in place) - float version
	/** Input vector is directly modified after calling this method
	**/
	inline void applyRotation(Vector3Tpl<float>& vec) const
	{
		float vx = vec.x;
		float vy = vec.y;
		float vz = vec.z;

		vec.x =   static_cast<float>(CC_MAT_R11) * vx
				+ static_cast<float>(CC_MAT_R12) * vy
				+ static_cast<float>(CC_MAT_R13) * vz;

		vec.y =   static_cast<float>(CC_MAT_R21) * vx
				+ static_cast<float>(CC_MAT_R22) * vy
				+ static_cast<float>(CC_MAT_R23) * vz;

		vec.z =   static_cast<float>(CC_MAT_R31) * vx
				+ static_cast<float>(CC_MAT_R32) * vy
				+ static_cast<float>(CC_MAT_R33) * vz;
	}
	//! Applies rotation only to a 3D vector (in place) - double version
	/** Input vector is directly modified after calling this method
	**/
	inline void applyRotation(Vector3Tpl<double>& vec) const
	{
		double vx = vec.x;
		double vy = vec.y;
		double vz = vec.z;

		vec.x =   static_cast<double>(CC_MAT_R11) * vx
				+ static_cast<double>(CC_MAT_R12) * vy
				+ static_cast<double>(CC_MAT_R13) * vz;

		vec.y =   static_cast<double>(CC_MAT_R21) * vx
				+ static_cast<double>(CC_MAT_R22) * vy
				+ static_cast<double>(CC_MAT_R23) * vz;

		vec.z =   static_cast<double>(CC_MAT_R31) * vx
				+ static_cast<double>(CC_MAT_R32) * vy
				+ static_cast<double>(CC_MAT_R33) * vz;
	}

	//! Applies rotation only to a 3D vector (in place) - float version
	/** Input array is directly modified after calling this method
	**/
	inline void applyRotation(float vec[3]) const
	{
		float vx = vec[0];
		float vy = vec[1];
		float vz = vec[2];

		vec[0] =  static_cast<float>(CC_MAT_R11) * vx
				+ static_cast<float>(CC_MAT_R12) * vy
				+ static_cast<float>(CC_MAT_R13) * vz;

		vec[1] =  static_cast<float>(CC_MAT_R21) * vx
				+ static_cast<float>(CC_MAT_R22) * vy
				+ static_cast<float>(CC_MAT_R23) * vz;

		vec[2] =  static_cast<float>(CC_MAT_R31) * vx
				+ static_cast<float>(CC_MAT_R32) * vy
				+ static_cast<float>(CC_MAT_R33) * vz;
	}
	//! Applies rotation only to a 3D vector (in place) - float version
	/** Input array is directly modified after calling this method
	**/
	inline void applyRotation(double vec[3]) const
	{
		double vx = vec[0];
		double vy = vec[1];
		double vz = vec[2];

		vec[0] =  static_cast<double>(CC_MAT_R11) * vx
				+ static_cast<double>(CC_MAT_R12) * vy
				+ static_cast<double>(CC_MAT_R13) * vz;

		vec[1] =  static_cast<double>(CC_MAT_R21) * vx
				+ static_cast<double>(CC_MAT_R22) * vy
				+ static_cast<double>(CC_MAT_R23) * vz;

		vec[2] =  static_cast<double>(CC_MAT_R31) * vx
				+ static_cast<double>(CC_MAT_R32) * vy
				+ static_cast<double>(CC_MAT_R33) * vz;
	}

	//! Shifts rotation center
	/** Warning, this method only applies a shift (i.e. relatively to the
		current rotation center). This is not a way to set an
		absolute rotation center 'directly'.
	**/
	void shiftRotationCenter(const Vector3Tpl<T>& vec)
	{
		//R(X-vec)+T+vec = R(X)+T + vec-R(vec)
		Vector3Tpl<T> Rvec = vec;
		applyRotation(Rvec);
		*this += (vec - Rvec);
	}

	//! Transposes matrix (in place)
	void transpose()
	{
		std::swap(CC_MAT_R21, CC_MAT_R12);
		std::swap(CC_MAT_R31, CC_MAT_R13);
		std::swap(CC_MAT_R41, CC_MAT_R14);
		std::swap(CC_MAT_R32, CC_MAT_R23);
		std::swap(CC_MAT_R42, CC_MAT_R24);
		std::swap(CC_MAT_R43, CC_MAT_R34);
	}

	//! Returns transposed matrix
	ccGLMatrixTpl<T> transposed() const
	{
		ccGLMatrixTpl<T> t(*this);
		t.transpose();
	
		return t;
	}

	//! Inverts transformation
	void invert()
	{
		//inverse scale as well!
		//we use the columns norm (considering is should be 1 for an 'unscaled' matrix)
		T sx2 = getColumnAsVec3D(0).norm2();
		T sy2 = getColumnAsVec3D(1).norm2();
		T sz2 = getColumnAsVec3D(2).norm2();

		//we invert rotation
		std::swap(CC_MAT_R21, CC_MAT_R12);
		std::swap(CC_MAT_R31, CC_MAT_R13);
		std::swap(CC_MAT_R32, CC_MAT_R23);

		if (std::abs(sx2) > std::numeric_limits<T>::epsilon() && sx2 != 1)
		{
			CC_MAT_R11 /= sx2;
			CC_MAT_R21 /= sx2;
			CC_MAT_R31 /= sx2;
		}
		if (std::abs(sy2) > std::numeric_limits<T>::epsilon() && sy2 != 1)
		{
			CC_MAT_R12 /= sy2;
			CC_MAT_R22 /= sy2;
			CC_MAT_R32 /= sy2;
		}
		if (std::abs(sz2) > std::numeric_limits<T>::epsilon() && sz2 != 1)
		{
			CC_MAT_R13 /= sz2;
			CC_MAT_R23 /= sz2;
			CC_MAT_R33 /= sz2;
		}

		//eventually we invert translation
		applyRotation(m_mat + 12);
		double w = CC_MAT_R44;
		if (std::abs(w) > std::numeric_limits<T>::epsilon())
		{
			CC_MAT_R14 = -CC_MAT_R14 / w;
			CC_MAT_R24 = -CC_MAT_R24 / w;
			CC_MAT_R34 = -CC_MAT_R34 / w;
			CC_MAT_R44 = 1.0 / w;
		}
		else
		{
			assert(false);
			CC_MAT_R14 = -CC_MAT_R14;
			CC_MAT_R24 = -CC_MAT_R24;
			CC_MAT_R34 = -CC_MAT_R34;
		}
	}

	//! Returns inverse transformation
	ccGLMatrixTpl<T> inverse() const
	{
		ccGLMatrixTpl<T> t(*this);
		t.invert();

		return t;
	}

	//! Scales the rotation and the translation (but not the the last row)
	/**	\param coef scaling coef.
	**/
	inline void scale(T coef)
	{
		CC_MAT_R11 *= coef; CC_MAT_R12 *= coef; CC_MAT_R13 *= coef; CC_MAT_R14 *= coef;
		CC_MAT_R21 *= coef; CC_MAT_R22 *= coef; CC_MAT_R23 *= coef; CC_MAT_R24 *= coef;
		CC_MAT_R31 *= coef; CC_MAT_R32 *= coef; CC_MAT_R33 *= coef; CC_MAT_R34 *= coef;
	}

	//! Scales the rotation part of the matrix
	/**	\param coef scaling coef.
	**/
	inline void scaleRotation(T coef)
	{
		CC_MAT_R11 *= coef; CC_MAT_R12 *= coef; CC_MAT_R13 *= coef;
		CC_MAT_R21 *= coef; CC_MAT_R22 *= coef; CC_MAT_R23 *= coef;
		CC_MAT_R31 *= coef; CC_MAT_R32 *= coef; CC_MAT_R33 *= coef;
	}

	//! Scales one row of the matrix
	/** \param rowIndex tow index (0-3)
		\param coef scaling coef.
	**/
	void scaleRow(unsigned rowIndex, T coef)
	{
		assert(rowIndex < 4);
		m_mat[   rowIndex] *= coef;
		m_mat[ 4+rowIndex] *= coef;
		m_mat[ 8+rowIndex] *= coef;
		m_mat[12+rowIndex] *= coef;
	}

	//! Scales one column of the matrix
	/** \warning The last row is never scaled
		\param colIndex column index (0-3)
		\param coef scaling coef.
	**/
	void scaleColumn(unsigned colIndex, T coef)
	{
		assert(colIndex < 4);
		T* col = getColumn(colIndex);
		col[0] *= coef;
		col[1] *= coef;
		col[2] *= coef;
		//col[3] *= coef;
	}

	//inherited from ccSerializableObject
	bool isSerializable() const override
	{
		return true;
	}

	//inherited from ccSerializableObject
	bool toFile(QFile& out, short dataVersion) const override
	{
		assert(out.isOpen() && (out.openMode() & QIODevice::WriteOnly));
		if (dataVersion < 20)
		{
			assert(false);
			return false;
		}

		//data (dataVersion>=20)
		if (out.write(reinterpret_cast<const char*>(m_mat), sizeof(T)*OPENGL_MATRIX_SIZE) < 0)
		{
			return WriteError();
		}

		return true;
	}

	//inherited from ccSerializableObject
	bool fromFile(QFile& in, short dataVersion, int flags, LoadedIDMap& oldToNewIDMap) override
	{
		assert(in.isOpen() && (in.openMode() & QIODevice::ReadOnly));

		if (dataVersion < 20)
			return CorruptError();

		//data (dataVersion>=20)
		if (in.read(reinterpret_cast<char*>(m_mat), sizeof(T)*OPENGL_MATRIX_SIZE) < 0)
			return ReadError();

		return true;
	}

	//inherited from ccSerializableObject
	short minimumFileVersion() const override
	{
		return 20;
	}

protected:

	//! Transfers the homogenous coordinate (scale) to the whole matrix
	void internalRescale()
	{
		if (CC_MAT_R44 != static_cast<T>(1) && CCCoreLib::GreaterThanEpsilon(CC_MAT_R44))
		{
			scaleRotation(1.0 / CC_MAT_R44);
			CC_MAT_R44 = static_cast<T>(1);
		}
	}

	//! Internal 4x4 GL-style matrix data
	T m_mat[OPENGL_MATRIX_SIZE];
};