ecvGLMatrixTpl.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
// -                        CloudViewer: www.cloudViewer.org                  -
// ----------------------------------------------------------------------------
// Copyright (c) 2018-2024 www.cloudViewer.org
// SPDX-License-Identifier: MIT
// ----------------------------------------------------------------------------
 
#pragma once
 
// Local
#include "ecvSerializableObject.h"
 
// cloudViewer
#include <CVConst.h>
#include <CVGeom.h>
#include <CVMath.h>
 
#include <Eigen/Geometry>
 
// Qt5/Qt6 Compatibility
#include <QtCompat.h>
 
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]
 
 
template <typename T>
class ccGLMatrixTpl : public ccSerializableObject {
public:
    static inline ccGLMatrixTpl<T> FromEigenMatrix(
            const Eigen::Matrix<double, 4, 4>& mat) {
        return ccGLMatrixTpl<T>(mat.data());
    }
 
    static inline ccGLMatrixTpl<T> FromEigenMatrix3(
            const Eigen::Matrix<T, 3, 3>& mat) {
        ccGLMatrixTpl<T> rotMat;
        rotMat.clearTranslation();
        rotMat.setRotation(mat.data());
        return rotMat;
    }
 
    static inline Eigen::Matrix<T, 4, 4> ToEigenMatrix4(
            const ccGLMatrixTpl<float>& mat) {
        Eigen::Matrix4f matrix = Eigen::Matrix<float, 4, 4>(mat.data());
        return matrix.cast<T>();
    }
    static inline Eigen::Matrix<T, 4, 4> ToEigenMatrix4(
            const ccGLMatrixTpl<double>& mat) {
        Eigen::Matrix4d matrix = Eigen::Matrix<double, 4, 4>(mat.data());
        return matrix.cast<T>();
    }
 
    static inline Eigen::Matrix<T, 3, 3> ToEigenMatrix3(
            const ccGLMatrixTpl<float>& mat) {
        Eigen::Matrix<T, 3, 3> m = Eigen::Matrix<T, 3, 3>::Zero();
        const float* data = mat.data();
 
        m << data[0], data[4], data[8], data[1], data[5], data[9], data[2],
                data[6], data[10];
        return m;
    }
    static inline Eigen::Matrix<T, 3, 3> ToEigenMatrix3(
            const ccGLMatrixTpl<double>& mat) {
        Eigen::Matrix<T, 3, 3> m = Eigen::Matrix<T, 3, 3>::Zero();
        const double* data = mat.data();
 
        m << data[0], data[4], data[8], data[1], data[5], data[9], data[2],
                data[6], data[10];
        return m;
    }
 
    inline ccGLMatrixTpl(const Eigen::Matrix<float, 4, 4>& mat) { *this = mat; }
    inline ccGLMatrixTpl(const Eigen::Matrix<double, 4, 4>& mat) {
        *this = mat;
    }
 
    inline ccGLMatrixTpl(const Eigen::Matrix<float, 3, 3>& mat) { *this = mat; }
    inline ccGLMatrixTpl(const Eigen::Matrix<double, 3, 3>& mat) {
        *this = mat;
    }
 
    inline ccGLMatrixTpl<T>& operator=(const Eigen::Matrix<float, 4, 4>& mat) {
        const float* data = mat.data();
        for (unsigned i = 0; i < OPENGL_MATRIX_SIZE; ++i)
            m_mat[i] = static_cast<T>(data[i]);
        return *this;
    }
    inline ccGLMatrixTpl<T>& operator=(const Eigen::Matrix<double, 4, 4>& mat) {
        const double* data = mat.data();
        for (unsigned i = 0; i < OPENGL_MATRIX_SIZE; ++i)
            m_mat[i] = static_cast<T>(data[i]);
        return *this;
    }
 
    inline ccGLMatrixTpl<T>& operator=(const Eigen::Matrix<float, 3, 3>& mat) {
        setRotation(mat.data());
        return *this;
    }
    inline ccGLMatrixTpl<T>& operator=(const Eigen::Matrix<double, 3, 3>& mat) {
        setRotation(mat.data());
        return *this;
    }
 
 
    ccGLMatrixTpl() { toIdentity(); }
 
    ccGLMatrixTpl(const ccGLMatrixTpl<T>& mat) {
        for (unsigned i = 0; i < OPENGL_MATRIX_SIZE; ++i)
            m_mat[i] = mat.m_mat[i];
    }
 
    ccGLMatrixTpl<T>& operator=(const ccGLMatrixTpl<T>& mat) {
        if (this != &mat) {
            for (unsigned i = 0; i < OPENGL_MATRIX_SIZE; ++i)
                m_mat[i] = mat.m_mat[i];
        }
        return *this;
    }
 
 
    explicit ccGLMatrixTpl(const float* mat16f) {
        for (unsigned i = 0; i < OPENGL_MATRIX_SIZE; ++i)
            m_mat[i] = static_cast<T>(mat16f[i]);
    }
 
 
    explicit ccGLMatrixTpl(const double* mat16d) {
        for (unsigned i = 0; i < OPENGL_MATRIX_SIZE; ++i)
            m_mat[i] = static_cast<T>(mat16d[i]);
    }
 
 
    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);
    }
 
 
    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;
    }
 
 
    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 (cloudViewer::LessThanEpsilon(
                    1.0 - f))  //"from" and "to"-vector almost parallel
        {
            // "to" vector most nearly orthogonal to "from"
            Vector3Tpl<T> x(0, 0, 0);
            if (fabs(from.x) < fabs(from.y)) {
                if (fabs(from.x) < fabs(from.z))
                    x.x = static_cast<T>(1);
                else
                    x.z = static_cast<T>(1);
            } else {
                if (fabs(from.y) < fabs(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;
    }
 
 
    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;
    }
 
 
    static Eigen::Matrix<T, 3, 3> QuaternionToRotationMatrix(
            const Eigen::Matrix<T, 4, 1>& qvec) {
        const Eigen::Matrix<T, 4, 1> normalized_qvec =
                NormalizeQuaternion(qvec);
        const Eigen::Quaternion<T> quat(normalized_qvec(0), normalized_qvec(1),
                                        normalized_qvec(2), normalized_qvec(3));
        return quat.toRotationMatrix();
    }
 
 
    static ccGLMatrixTpl<T> QuaternionToRotationMatrix(
            const Tuple4Tpl<T>& qvec) {
        Eigen::Matrix<T, 4, 1> t_qvec;
        t_qvec(0) = qvec.w;
        t_qvec(1) = qvec.x;
        t_qvec(2) = qvec.y;
        t_qvec(3) = qvec.z;
        const Eigen::Matrix<T, 4, 1> normalized_qvec =
                NormalizeQuaternion(t_qvec);
        const Eigen::Quaternion<T> quat(normalized_qvec(0), normalized_qvec(1),
                                        normalized_qvec(2), normalized_qvec(3));
        return FromEigenMatrix3(quat.toRotationMatrix());
    }
 
 
    static Eigen::Matrix<T, 4, 1> NormalizeQuaternion(
            const Eigen::Matrix<T, 4, 1>& qvec) {
        const T norm = qvec.norm();
        if (norm == 0) {
            // We do not just use (1, 0, 0, 0) because that is a constant and
            // when used for automatic differentiation that would lead to a zero
            // derivative.
            return Eigen::Matrix<T, 4, 1>(static_cast<T>(1.0), qvec(1), qvec(2),
                                          qvec(3));
        } else {
            return qvec / norm;
        }
    }
 
 
    void toQuaternion(T q[]) const {
        Eigen::Quaternion<T> t_q = Eigen::Quaternion<T>(ToEigenMatrix3(*this));
        //        t_q.normalize();
        q[0] = t_q.w();
        q[1] = t_q.x();
        q[2] = t_q.y();
        q[3] = t_q.z();
    }
 
 
    void toAngleAxis(T& alpha_rad, Vector3Tpl<T>& axis3D) const {
        Eigen::AngleAxis<T> v = Eigen::AngleAxis<T>(ToEigenMatrix3(*this));
        alpha_rad = v.angle();
        axis3D.x = v.axis().x();
        axis3D.y = v.axis().y();
        axis3D.z = v.axis().z();
    }
 
 
    void toEulerAngle(T& rz, T& ry, T& rx) const {
        Eigen::Matrix<T, 3, 1> euler_angles =
                ToEigenMatrix3(*this).eulerAngles(2, 1, 0);
        rz = euler_angles.z();
        ry = euler_angles.y();
        rx = euler_angles.x();
    }
 
 
    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;
    }
 
 
    static ccGLMatrixTpl<T> FromString(const QString& matText, bool& success) {
        QStringList valuesStr =
                qtCompatSplitRegex(matText, "\\s+", QtCompat::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>();
        }
 
        success = true;
        return matrix;
    }
 
 
    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;
    }
 
 
    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] << QtCompat::endl;
        }
 
        return (fp.error() == QFile::NoError);
    }
 
 
    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];
        }
 
        return (fp.error() == QFile::NoError);
    }
 
    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;
    }
 
    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;
    }
 
    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;
    }
 
 
    inline virtual void toZero() {
        memset(m_mat, 0, OPENGL_MATRIX_SIZE * sizeof(T));
    }
 
    inline virtual void toIdentity() {
        toZero();
        CC_MAT_R11 = CC_MAT_R22 = CC_MAT_R33 = CC_MAT_R44 = static_cast<T>(1);
    }
 
    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;
    }
 
 
    inline void clearTranslation() { CC_MAT_R14 = CC_MAT_R24 = CC_MAT_R34 = 0; }
 
    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);
    }
 
 
    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.0);
    }
 
 
    void getParameters(T& alpha_rad,
                       Vector3Tpl<T>& axis3D,
                       Vector3Tpl<T>& t3D) const {
        T trace = CC_MAT_R11 + CC_MAT_R22 + CC_MAT_R33;
        T cos_t = (trace - 1) / 2;
        if (fabs(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 (cloudViewer::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;
    }
 
 
    void getParameters(T& phi_rad,
                       T& theta_rad,
                       T& psi_rad,
                       Vector3Tpl<T>& t3D) const {
        if (fabs(CC_MAT_R31) != 1) {
            theta_rad = -asin(CC_MAT_R31);
            T cos_theta = cos(theta_rad);
            psi_rad = atan2(CC_MAT_R32 / cos_theta, CC_MAT_R33 / cos_theta);
            phi_rad = atan2(CC_MAT_R21 / cos_theta, CC_MAT_R11 / cos_theta);
 
            // Other solution
            // theta = M_PI + asin(CC_MAT_R31);
            // T cos_theta = cos(theta);
            // psi = atan2(CC_MAT_R32 / cos_theta, CC_MAT_R33 / cos_theta);
            // phi = atan2(CC_MAT_R21 / cos_theta, CC_MAT_R11 / cos_theta);
        } else {
            phi_rad = 0;
 
            if (CC_MAT_R31 == -1) {
                theta_rad = static_cast<T>(M_PI_2);
                psi_rad = atan2(CC_MAT_R12, CC_MAT_R13);
            } else {
                theta_rad = -static_cast<T>(M_PI_2);
                psi_rad = -atan2(CC_MAT_R12, CC_MAT_R13);
            }
        }
 
        t3D.x = CC_MAT_R14;
        t3D.y = CC_MAT_R24;
        t3D.z = CC_MAT_R34;
    }
 
    inline T* data() { return m_mat; }
 
    inline const T* data() const { return m_mat; }
 
 
    inline T* getTranslation() { return m_mat + 12; }
 
 
    inline const T* getTranslation() const { return m_mat + 12; }
 
 
    inline Vector3Tpl<T> getTranslationAsVec3D() const {
        return getColumnAsVec3D(3);
    }
 
 
    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);
    }
 
    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);
    }
 
 
    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]);
    }
 
    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]);
    }
 
 
    void setRotation(const float Rt[9]) {
        CC_MAT_R11 = static_cast<T>(Rt[0]);
        CC_MAT_R21 = static_cast<T>(Rt[1]);
        CC_MAT_R31 = static_cast<T>(Rt[2]);
 
        CC_MAT_R12 = static_cast<T>(Rt[3]);
        CC_MAT_R22 = static_cast<T>(Rt[4]);
        CC_MAT_R32 = static_cast<T>(Rt[5]);
 
        CC_MAT_R13 = static_cast<T>(Rt[6]);
        CC_MAT_R23 = static_cast<T>(Rt[7]);
        CC_MAT_R33 = static_cast<T>(Rt[8]);
    }
 
 
    void setRotation(const double Rt[9]) {
        CC_MAT_R11 = static_cast<T>(Rt[0]);
        CC_MAT_R21 = static_cast<T>(Rt[1]);
        CC_MAT_R31 = static_cast<T>(Rt[2]);
 
        CC_MAT_R12 = static_cast<T>(Rt[3]);
        CC_MAT_R22 = static_cast<T>(Rt[4]);
        CC_MAT_R32 = static_cast<T>(Rt[5]);
 
        CC_MAT_R13 = static_cast<T>(Rt[6]);
        CC_MAT_R23 = static_cast<T>(Rt[7]);
        CC_MAT_R33 = static_cast<T>(Rt[8]);
    }
 
 
    inline T* getColumn(unsigned index) { return m_mat + (index << 2); }
 
 
    inline const T* getColumn(unsigned index) const {
        return m_mat + (index << 2);
    }
 
 
    inline Vector3Tpl<T> getColumnAsVec3D(unsigned index) const {
        return Vector3Tpl<T>::fromArray(getColumn(index));
    }
 
 
    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;
    }
 
 
    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;
    }
 
    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;
    }
 
    inline Vector3Tpl<float> operator*(const Vector3Tpl<float>& vec) const {
        return Vector3Tpl<float>(applyX(vec), applyY(vec), applyZ(vec));
    }
    inline Vector3Tpl<double> operator*(const Vector3Tpl<double>& vec) const {
        return Vector3Tpl<double>(applyX(vec), applyY(vec), applyZ(vec));
    }
 
    inline Tuple4Tpl<float> operator*(const Tuple4Tpl<float>& vec) const {
        return Tuple4Tpl<float>(applyX(vec), applyY(vec), applyZ(vec),
                                applyW(vec));
    }
    inline Tuple4Tpl<double> operator*(const Tuple4Tpl<double>& vec) const {
        return Tuple4Tpl<double>(applyX(vec), applyY(vec), applyZ(vec),
                                 applyW(vec));
    }
 
    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);
    }
 
    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);
    }
 
    ccGLMatrixTpl<T>& operator*=(const ccGLMatrixTpl<T>& mat) {
        (*this) = (*this) * mat;
        return (*this);
    }
 
    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);
    }
    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);
    }
    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);
    }
    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);
    }
 
    T operator()(unsigned row, unsigned col) const {
        return m_mat[(col << 2) + row];
    }
 
 
    inline void apply(float vec[3]) const {
        CCVector3 internal_vec(vec[0], vec[1], vec[2]);
        apply(internal_vec);
        vec[0] = internal_vec[0];
        vec[1] = internal_vec[1];
        vec[2] = internal_vec[2];
    }
 
    inline void apply(double vec[3]) const {
        CCVector3d internal_vec(vec[0], vec[1], vec[2]);
        apply(internal_vec);
        vec[0] = internal_vec[0];
        vec[1] = internal_vec[1];
        vec[2] = internal_vec[2];
    }
 
 
    inline void apply(Vector3Tpl<float>& vec) const { vec = (*this) * vec; }
 
    inline void apply(Vector3Tpl<double>& vec) const { vec = (*this) * vec; }
 
 
    inline void apply(Tuple4Tpl<float>& vec) const { vec = (*this) * vec; }
 
    inline void apply(Tuple4Tpl<double>& vec) const { vec = (*this) * vec; }
 
    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);
    }
    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);
    }
 
    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);
    }
    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);
    }
 
    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);
    }
    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);
    }
 
    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;
    }
    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;
    }
 
    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;
    }
    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;
    }
 
    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;
    }
    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;
    }
 
    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;
    }
    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;
    }
 
 
    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;
    }
 
    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;
    }
 
 
    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;
    }
 
    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;
    }
 
 
    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);
    }
 
    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);
    }
 
    ccGLMatrixTpl<T> transposed() const {
        ccGLMatrixTpl<T> t(*this);
        t.transpose();
 
        return t;
    }
 
    void invert() {
        // inverse scale as well!
        // we use the first column == X (its norm should be 1 for an 'unscaled'
        // matrix ;)
        T s2 = getColumnAsVec3D(0).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 (s2 != 0 && s2 != 1) {
            CC_MAT_R11 /= s2;
            CC_MAT_R12 /= s2;
            CC_MAT_R13 /= s2;
            CC_MAT_R21 /= s2;
            CC_MAT_R22 /= s2;
            CC_MAT_R23 /= s2;
            CC_MAT_R31 /= s2;
            CC_MAT_R32 /= s2;
            CC_MAT_R33 /= s2;
        }
 
        // eventually we invert translation
        applyRotation(m_mat + 12);
        CC_MAT_R14 = -CC_MAT_R14;
        CC_MAT_R24 = -CC_MAT_R24;
        CC_MAT_R34 = -CC_MAT_R34;
    }
 
    ccGLMatrixTpl<T> inverse() const {
        ccGLMatrixTpl<T> t(*this);
        t.invert();
 
        return t;
    }
 
 
    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;
    }
 
 
    void scaleLine(unsigned lineIndex, T coef) {
        assert(lineIndex < 4);
        m_mat[lineIndex] *= coef;
        m_mat[4 + lineIndex] *= coef;
        m_mat[8 + lineIndex] *= coef;
        m_mat[12 + lineIndex] *= 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;
    }
 
 
    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;
    }
 
 
    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; }
 
    bool toFile(QFile& out, short dataVersion) const override {
        assert(out.isOpen() && (out.openMode() & QIODevice::WriteOnly));
 
        // Version validation
        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;
    }
 
    short minimumFileVersion() const override {
        // ccGLMatrix data structure has been stable since version 20
        return 20;
    }
 
    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;
    }
 
protected:
    T m_mat[OPENGL_MATRIX_SIZE];
};