KeosMatrix4.h

Go to the documentation of this file.

/*
 * This source file is part of KEOS (Free 3D Engine)
 * For the latest info, see http://www.keosengine.org/
 * E-mails : thierry.vouriot@keosengine.org, yeri@keosengine.org
 *
 * This program is free software; you can redistribute it and/or modify it under
 * the terms of the GNU Lesser General Public License as published by the Free Software
 * Foundation; either version 2 of the License, or (at your option) any later
 * version.
 *
 * 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 Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License along with
 * this program; if not, write to the Free Software Foundation, Inc., 59 Temple
 * Place - Suite 330, Boston, MA 02111-1307, USA, or go to
 * http://www.gnu.org/copyleft/lesser.txt.
 *
 */

#ifndef KEOS_MATRIX4_H
#define KEOS_MATRIX4_H

#include "KeosPrerequisites.h"

namespace Keos
{
  //-----------------------------------------------------------------------
  class CMatrix4
  {
  public :

    CMatrix4(Real m11 = 1.0f, Real m12 = 0.0f, Real m13 = 0.0f, Real m14 = 0.0f,
             Real m21 = 0.0f, Real m22 = 1.0f, Real m23 = 0.0f, Real m24 = 0.0f,
             Real m31 = 0.0f, Real m32 = 0.0f, Real m33 = 1.0f, Real m34 = 0.0f,
             Real m41 = 0.0f, Real m42 = 0.0f, Real m43 = 0.0f, Real m44 = 1.0f);

    void Identity();

    Real Determinant() const;

    CMatrix4 Transpose() const;

    CMatrix4 Inverse() const;

    void SetTranslation(Real x, Real y, Real z);

    void SetTranslation(const TVector3F& Trans);

    void SetScaling(Real x, Real y, Real z);

    void SetRotationX(Real Angle);

    void SetRotationY(Real Angle);

    void SetRotationZ(Real Angle);

    void SetRotationX(Real Angle, const TVector3F& Center);

    void SetRotationY(Real Angle, const TVector3F& Center);

    void SetRotationZ(Real Angle, const TVector3F& Center);

    TVector3F GetTranslation() const;

    TVector3F Transform(const TVector3F& v, Real w = 1.0f) const;

    TVector4F Transform(const TVector4F& v) const;

    void OrthoOffCenter(Real Left, Real Top, Real Right, Real Bottom);

    void PerspectiveFOV(Real Fov, Real Ratio, Real Near, Real Far);
    //void PerspectiveFOV2(Real Fov, Real Ratio, Real Near, Real Far);

    void LookAt(const TVector3F& From, const TVector3F& To, const TVector3F& Up = TVector3F(0, 1, 0));

    // Operators
    CMatrix4 operator +() const;
    CMatrix4 operator -() const;
    CMatrix4 operator +(const CMatrix4& m) const;
    CMatrix4 operator -(const CMatrix4& m) const;
    const CMatrix4& operator +=(const CMatrix4& m);
    const CMatrix4& operator -=(const CMatrix4& m);
    CMatrix4 operator *(const CMatrix4& m) const;
    const CMatrix4& operator *=(const CMatrix4& m);
    const CMatrix4& operator *=(Real t);
    const CMatrix4& operator /=(Real t);
    bool operator ==(const CMatrix4& m) const;
    bool operator !=(const CMatrix4& m) const;

    Real& operator ()(size_t i, size_t j);

    const Real& operator ()(size_t i, size_t j) const;

    operator Real*();

    operator const Real*() const;

    Real a11, a21, a31, a41; // Matrix first line
    Real a12, a22, a32, a42; // Matrix second line
    Real a13, a23, a33, a43; // Matrix third line
    Real a14, a24, a34, a44; // Matrix fourth line
  };

  // Globals functions
  CMatrix4      operator * (const CMatrix4& m, Real t);
  CMatrix4      operator * (Real t, const CMatrix4& m);
  CMatrix4      operator / (const CMatrix4& m, Real t);
  std::istream& operator >>(std::istream& Stream, CMatrix4& Mat);
  std::ostream& operator <<(std::ostream& Stream, const CMatrix4& Mat);


  //=======================================================================
  // CMatrix4 inline functions
  //=======================================================================

  //-----------------------------------------------------------------------
  inline CMatrix4::CMatrix4(Real m11, Real m12, Real m13, Real m14,
                            Real m21, Real m22, Real m23, Real m24,
                            Real m31, Real m32, Real m33, Real m34,
                            Real m41, Real m42, Real m43, Real m44) :
      a11(m11), a12(m12), a13(m13), a14(m14),
      a21(m21), a22(m22), a23(m23), a24(m24),
      a31(m31), a32(m32), a33(m33), a34(m34),
      a41(m41), a42(m42), a43(m43), a44(m44)
  {}

  //-----------------------------------------------------------------------
  inline void CMatrix4::Identity()
  {
    a11 = 1.0f;
    a12 = 0.0f;
    a13 = 0.0f;
    a14 = 0.0f;
    a21 = 0.0f;
    a22 = 1.0f;
    a23 = 0.0f;
    a24 = 0.0f;
    a31 = 0.0f;
    a32 = 0.0f;
    a33 = 1.0f;
    a34 = 0.0f;
    a41 = 0.0f;
    a42 = 0.0f;
    a43 = 0.0f;
    a44 = 1.0f;
  }

  //-----------------------------------------------------------------------
  inline Real CMatrix4::Determinant() const
  {
    Real A = a22 * (a33 * a44 - a43 * a34) - a32 * (a23 * a44 - a43 * a24) + a42 * (a23 * a34 - a33 * a24);
    Real B = a12 * (a33 * a44 - a43 * a34) - a32 * (a13 * a44 - a43 * a14) + a42 * (a13 * a34 - a33 * a14);
    Real C = a12 * (a23 * a44 - a43 * a24) - a22 * (a13 * a44 - a43 * a14) + a42 * (a13 * a24 - a23 * a14);
    Real D = a12 * (a23 * a34 - a33 * a24) - a22 * (a13 * a34 - a33 * a14) + a32 * (a13 * a24 - a23 * a14);

    return a11 * A - a21 * B + a31 * C - a41 * D;
  }

  //-----------------------------------------------------------------------
  inline CMatrix4 CMatrix4::Transpose() const
  {
    return CMatrix4(a11, a21, a31, a41,
                    a12, a22, a32, a42,
                    a13, a23, a33, a43,
                    a14, a24, a34, a44);
  }

  //-----------------------------------------------------------------------
  inline CMatrix4 CMatrix4::Inverse() const
  {
    CMatrix4 Ret;
    Real Det = Determinant();

    if (std::fabs(Det) > std::numeric_limits<Real>::epsilon())
    {
      Ret.a11 =  (a22 * (a33 * a44 - a34 * a43) - a32 * (a23 * a44 - a43 * a24) + a42 * (a23 * a34 - a33 *  a24)) / Det;
      Ret.a12 = -(a12 * (a33 * a44 - a43 * a34) - a32 * (a13 * a44 - a43 * a14) + a42 * (a13 * a34 - a33 *  a14)) / Det;
      Ret.a13 =  (a12 * (a23 * a44 - a43 * a24) - a22 * (a13 * a44 - a43 * a14) + a42 * (a13 * a24 - a23 *  a14)) / Det;
      Ret.a14 = -(a12 * (a23 * a34 - a33 * a24) - a22 * (a13 * a34 - a33 * a14) + a32 * (a13 * a24 - a23 *  a14)) / Det;

      Ret.a21 = -(a21 * (a33 * a44 - a34 * a43) - a23 * (a31 * a44 - a34 * a41) + a24 * (a31 * a43 - a33 *  a41)) / Det;
      Ret.a22 =  (a11 * (a33 * a44 - a34 * a43) - a13 * (a31 * a44 - a34 * a41) + a14 * (a31 * a43 - a33 *  a41)) / Det;
      Ret.a23 = -(a11 * (a23 * a44 - a24 * a43) - a13 * (a21 * a44 - a24 * a41) + a14 * (a21 * a43 - a23 *  a41)) / Det;
      Ret.a24 =  (a11 * (a23 * a34 - a24 * a33) - a13 * (a21 * a34 - a24 * a31) + a14 * (a21 * a33 - a23 *  a31)) / Det;

      Ret.a31 =  (a21 * (a32 * a44 - a34 * a42) - a22 * (a31 * a44 - a34 * a41) + a24 * (a31 * a42 - a32 *  a41)) / Det;
      Ret.a32 = -(a11 * (a32 * a44 - a34 * a42) - a12 * (a31 * a44 - a34 * a41) + a14 * (a31 * a42 - a32 *  a41)) / Det;
      Ret.a33 =  (a11 * (a22 * a44 - a24 * a42) - a12 * (a21 * a44 - a24 * a41) + a14 * (a21 * a42 - a22 *  a41)) / Det;
      Ret.a34 = -(a11 * (a22 * a34 - a24 * a32) - a12 * (a21 * a34 - a24 * a31) + a14 * (a21 * a32 - a22 *  a31)) / Det;

      Ret.a41 = -(a21 * (a32 * a43 - a33 * a42) - a22 * (a31 * a43 - a33 * a41) + a23 * (a31 * a42 - a32 *  a41)) / Det;
      Ret.a42 =  (a11 * (a32 * a43 - a33 * a42) - a12 * (a31 * a43 - a33 * a41) + a13 * (a31 * a42 - a32 *  a41)) / Det;
      Ret.a43 = -(a11 * (a22 * a43 - a23 * a42) - a12 * (a21 * a43 - a23 * a41) + a13 * (a21 * a42 - a22 *  a41)) / Det;
      Ret.a44 =  (a11 * (a22 * a33 - a23 * a32) - a12 * (a21 * a33 - a23 * a31) + a13 * (a21 * a32 - a22 *  a31)) / Det;
    }

    return Ret;
  }

  //-----------------------------------------------------------------------
  inline void CMatrix4::SetTranslation(Real x, Real y, Real z)
  {
    a11 = 1.0f;
    a12 = 0.0f;
    a13 = 0.0f;
    a14 = x;
    a21 = 0.0f;
    a22 = 1.0f;
    a23 = 0.0f;
    a24 = y;
    a31 = 0.0f;
    a32 = 0.0f;
    a33 = 1.0f;
    a34 = z;
    a41 = 0.0f;
    a42 = 0.0f;
    a43 = 0.0f;
    a44 = 1.0f;
  }

  //-----------------------------------------------------------------------
  inline void CMatrix4::SetTranslation(const TVector3F& Trans)
  {
    SetTranslation(Trans.x, Trans.y, Trans.z);
  }

  //-----------------------------------------------------------------------
  inline void CMatrix4::SetScaling(Real x, Real y, Real z)
  {
    a11 = x;
    a12 = 0.0f;
    a13 = 0.0f;
    a14 = 0.0f;
    a21 = 0.0f;
    a22 = y;
    a23 = 0.0f;
    a24 = 0.0f;
    a31 = 0.0f;
    a32 = 0.0f;
    a33 = z;
    a34 = 0.0f;
    a41 = 0.0f;
    a42 = 0.0f;
    a43 = 0.0f;
    a44 = 1.0f;
  }

  //-----------------------------------------------------------------------
  inline void CMatrix4::SetRotationX(Real Angle)
  {
    Real Cos = std::cos(Angle);
    Real Sin = std::sin(Angle);

    a11 = 1.0f;
    a12 = 0.0f;
    a13 = 0.0f;
    a14 = 0.0f;
    a21 = 0.0f;
    a22 = Cos;
    a23 = Sin;
    a24 = 0.0f;
    a31 = 0.0f;
    a32 = -Sin;
    a33 = Cos;
    a34 = 0.0f;
    a41 = 0.0f;
    a42 = 0.0f;
    a43 = 0.0f;
    a44 = 1.0f;
  }

  //-----------------------------------------------------------------------
  inline void CMatrix4::SetRotationY(Real Angle)
  {
    Real Cos = std::cos(Angle);
    Real Sin = std::sin(Angle);

    a11 = Cos;
    a12 = 0.0f;
    a13 = -Sin;
    a14 = 0.0f;
    a21 = 0.0f;
    a22 = 1.0f;
    a23 = 0.0f;
    a24 = 0.0f;
    a31 = Sin;
    a32 = 0.0f;
    a33 = Cos;
    a34 = 0.0f;
    a41 = 0.0f;
    a42 = 0.0f;
    a43 = 0.0f;
    a44 = 1.0f;
  }

  //-----------------------------------------------------------------------
  inline void CMatrix4::SetRotationZ(Real Angle)
  {
    Real Cos = std::cos(Angle);
    Real Sin = std::sin(Angle);

    a11 = Cos;
    a12 = Sin;
    a13 = 0.0f;
    a14 = 0.0f;
    a21 = -Sin;
    a22 = Cos;
    a23 = 0.0f;
    a24 = 0.0f;
    a31 = 0.0f;
    a32 = 0.0f;
    a33 = 1.0f;
    a34 = 0.0f;
    a41 = 0.0f;
    a42 = 0.0f;
    a43 = 0.0f;
    a44 = 1.0f;
  }

  //-----------------------------------------------------------------------
  inline void CMatrix4::SetRotationX(Real Angle, const TVector3F& Center)
  {
    CMatrix4 Tr1, Tr2, Rot;

    Tr1.SetTranslation(Center.x, Center.y, Center.z);
    Tr2.SetTranslation(-Center.x, -Center.y, -Center.z);
    Rot.SetRotationX(Angle);

    *this = Tr1 * Rot * Tr2;
  }

  //-----------------------------------------------------------------------
  inline void CMatrix4::SetRotationY(Real Angle, const TVector3F& Center)
  {
    CMatrix4 Tr1, Tr2, Rot;

    Tr1.SetTranslation(Center.x, Center.y, Center.z);
    Tr2.SetTranslation(-Center.x, -Center.y, -Center.z);
    Rot.SetRotationY(Angle);

    *this = Tr1 * Rot * Tr2;
  }

  //-----------------------------------------------------------------------
  inline void CMatrix4::SetRotationZ(Real Angle, const TVector3F& Center)
  {
    CMatrix4 Tr1, Tr2, Rot;

    Tr1.SetTranslation(Center.x, Center.y, Center.z);
    Tr2.SetTranslation(-Center.x, -Center.y, -Center.z);
    Rot.SetRotationZ(Angle);

    *this = Tr1 * Rot * Tr2;
  }

  //-----------------------------------------------------------------------
  inline TVector3F CMatrix4::GetTranslation() const
  {
    return TVector3F(a14, a24, a34);
  }

  //-----------------------------------------------------------------------
  inline TVector3F CMatrix4::Transform(const TVector3F& v, Real w) const
  {
    return TVector3F(v.x * a11 + v.y * a21 + v.z * a31 + w * a41,
                     v.x * a12 + v.y * a22 + v.z * a32 + w * a42,
                     v.x * a13 + v.y * a23 + v.z * a33 + w * a43);
  }

  //-----------------------------------------------------------------------
  inline TVector4F CMatrix4::Transform(const TVector4F& v) const
  {
    return TVector4F(v.x * a11 + v.y * a21 + v.z * a31 + v.w * a41,
                     v.x * a12 + v.y * a22 + v.z * a32 + v.w * a42,
                     v.x * a13 + v.y * a23 + v.z * a33 + v.w * a43,
                     v.x * a14 + v.y * a24 + v.z * a34 + v.w * a44);
  }

  //-----------------------------------------------------------------------
  inline void CMatrix4::OrthoOffCenter(Real Left, Real Top, Real Right, Real Bottom)
  {
    a11 = 2 / (Right - Left);
    a12 = 0.0f;
    a13 = 0.0f;
    a14 = (Left + Right) / (Left - Right);
    a21 = 0.0f;
    a22 = 2 / (Top - Bottom);
    a23 = 0.0f;
    a24 = (Bottom + Top) / (Bottom - Top);
    a31 = 0.0f;
    a32 = 0.0f;
    a33 = 1.0f;
    a34 = 0.0f;
    a41 = 0.0f;
    a42 = 0.0f;
    a43 = 0.0f;
    a44 = 1.0f;
  }

  //-----------------------------------------------------------------------
  inline void CMatrix4::PerspectiveFOV(Real Fov, Real Ratio, Real Near, Real Far)
  {
    Real YScale = 1.0f / std::tan(Fov / 2);
    Real XScale = YScale / Ratio;
    Real Coeff  = Far / (Far - Near);

    a11 = XScale;
    a12 = 0.0f;
    a13 = 0.0f;
    a14 = 0.0f;
    a21 = 0.0f;
    a22 = YScale;
    a23 = 0.0f;
    a24 = 0.0f;
    a31 = 0.0f;
    a32 = 0.0f;
    a33 = Coeff;
    a34 = Near * -Coeff;
    a41 = 0.0f;
    a42 = 0.0f;
    a43 = 1.0f;
    a44 = 0.0f;
  }

  /*inline void CMatrix4::PerspectiveFOV(Real Fov, Real Ratio, Real Near, Real Far)
     {
   Real h = std::cos(Fov/2) / std::sin(Fov/2);
         Real w = h / Ratio;
         Real zn  = Near;
   Real zf  = Far;

         a11 = 2*zn/w; a21 = 0.0f;   a31 = 0.0f;    a41 = 0.0f;
         a12 = 0.0f;   a22 = 2*zn/h; a32 = 0.0f;    a42 = 0.0f;
         a13 = 0.0f;   a23 = 0.0f;   a33 = zf/(zf-zn);  a43 = 1.0f;
         a14 = 0.0f;   a24 = 0.0f;   a34 = zn*zf/(zn-zf); a44 = 0.0f;
     }*/

  //-----------------------------------------------------------------------
  inline void CMatrix4::LookAt(const TVector3F& From, const TVector3F& To, const TVector3F& Up)
  {
    // TODO : gérer le cas où (To - From) et Up sont colinéaires

    TVector3F ZAxis = To - From;
    ZAxis.Normalize();
    TVector3F XAxis = VectorCross(Up, ZAxis);
    XAxis.Normalize();
    TVector3F YAxis = VectorCross(ZAxis, XAxis);

    a11 = XAxis.x;
    a12 = XAxis.y;
    a13 = XAxis.z;
    a14 = -VectorDot(XAxis, From);
    a21 = YAxis.x;
    a22 = YAxis.y;
    a23 = YAxis.z;
    a24 = -VectorDot(YAxis, From);
    a31 = ZAxis.x;
    a32 = ZAxis.y;
    a33 = ZAxis.z;
    a34 = -VectorDot(ZAxis, From);
    a41 = 0.0f;
    a42 = 0.0f;
    a43 = 0.0f;
    a44 = 1.0f;
  }

  //-----------------------------------------------------------------------
  inline CMatrix4 CMatrix4::operator +() const
  {
    return *this;
  }

  //-----------------------------------------------------------------------
  inline CMatrix4 CMatrix4::operator -() const
  {
    return CMatrix4(-a11, -a12, -a13, -a14,
                    -a21, -a22, -a23, -a24,
                    -a31, -a32, -a33, -a34,
                    -a41, -a42, -a43, -a44);
  }

  //-----------------------------------------------------------------------
  inline CMatrix4 CMatrix4::operator +(const CMatrix4& m) const
  {
    return CMatrix4(a11 + m.a11, a12 + m.a12, a13 + m.a13, a14 + m.a14,
                    a21 + m.a21, a22 + m.a22, a23 + m.a23, a24 + m.a24,
                    a31 + m.a31, a32 + m.a32, a33 + m.a33, a34 + m.a34,
                    a41 + m.a41, a42 + m.a42, a43 + m.a43, a44 + m.a44);
  }

  //-----------------------------------------------------------------------
  inline CMatrix4 CMatrix4::operator -(const CMatrix4& m) const
  {
    return CMatrix4(a11 - m.a11, a12 - m.a12, a13 - m.a13, a14 - m.a14,
                    a21 - m.a21, a22 - m.a22, a23 - m.a23, a24 - m.a24,
                    a31 - m.a31, a32 - m.a32, a33 - m.a33, a34 - m.a34,
                    a41 - m.a41, a42 - m.a42, a43 - m.a43, a44 - m.a44);
  }

  //-----------------------------------------------------------------------
  inline const CMatrix4& CMatrix4::operator +=(const CMatrix4& m)
  {
    a11 += m.a11;
    a12 += m.a12;
    a13 += m.a13;
    a14 += m.a14;
    a21 += m.a21;
    a22 += m.a22;
    a23 += m.a23;
    a24 += m.a24;
    a31 += m.a31;
    a32 += m.a32;
    a33 += m.a33;
    a34 += m.a34;
    a41 += m.a41;
    a42 += m.a42;
    a43 += m.a43;
    a44 += m.a44;

    return *this;
  }

  //-----------------------------------------------------------------------
  inline const CMatrix4& CMatrix4::operator -=(const CMatrix4& m)
  {
    a11 -= m.a11;
    a12 -= m.a12;
    a13 -= m.a13;
    a14 -= m.a14;
    a21 -= m.a21;
    a22 -= m.a22;
    a23 -= m.a23;
    a24 -= m.a24;
    a31 -= m.a31;
    a32 -= m.a32;
    a33 -= m.a33;
    a34 -= m.a34;
    a41 -= m.a41;
    a42 -= m.a42;
    a43 -= m.a43;
    a44 -= m.a44;

    return *this;
  }

  //-----------------------------------------------------------------------
  inline CMatrix4 CMatrix4::operator *(const CMatrix4& m) const
  {
    return CMatrix4(a11 * m.a11 + a21 * m.a12 + a31 * m.a13 + a41 * m.a14,
                    a12 * m.a11 + a22 * m.a12 + a32 * m.a13 + a42 * m.a14,
                    a13 * m.a11 + a23 * m.a12 + a33 * m.a13 + a43 * m.a14,
                    a14 * m.a11 + a24 * m.a12 + a34 * m.a13 + a44 * m.a14,

                    a11 * m.a21 + a21 * m.a22 + a31 * m.a23 + a41 * m.a24,
                    a12 * m.a21 + a22 * m.a22 + a32 * m.a23 + a42 * m.a24,
                    a13 * m.a21 + a23 * m.a22 + a33 * m.a23 + a43 * m.a24,
                    a14 * m.a21 + a24 * m.a22 + a34 * m.a23 + a44 * m.a24,

                    a11 * m.a31 + a21 * m.a32 + a31 * m.a33 + a41 * m.a34,
                    a12 * m.a31 + a22 * m.a32 + a32 * m.a33 + a42 * m.a34,
                    a13 * m.a31 + a23 * m.a32 + a33 * m.a33 + a43 * m.a34,
                    a14 * m.a31 + a24 * m.a32 + a34 * m.a33 + a44 * m.a34,

                    a11 * m.a41 + a21 * m.a42 + a31 * m.a43 + a41 * m.a44,
                    a12 * m.a41 + a22 * m.a42 + a32 * m.a43 + a42 * m.a44,
                    a13 * m.a41 + a23 * m.a42 + a33 * m.a43 + a43 * m.a44,
                    a14 * m.a41 + a24 * m.a42 + a34 * m.a43 + a44 * m.a44);
  }

  //-----------------------------------------------------------------------
  inline const CMatrix4& CMatrix4::operator *=(const CMatrix4& m)
  {
    *this = *this * m;

    return *this;
  }

  //-----------------------------------------------------------------------
  inline const CMatrix4& CMatrix4::operator *=(Real t)
  {
    a11 *= t;
    a12 *= t;
    a13 *= t;
    a14 *= t;
    a21 *= t;
    a22 *= t;
    a23 *= t;
    a24 *= t;
    a31 *= t;
    a32 *= t;
    a33 *= t;
    a34 *= t;
    a41 *= t;
    a42 *= t;
    a43 *= t;
    a44 *= t;

    return *this;
  }

  //-----------------------------------------------------------------------
  inline const CMatrix4& CMatrix4::operator /=(Real t)
  {
    a11 /= t;
    a12 /= t;
    a13 /= t;
    a14 /= t;
    a21 /= t;
    a22 /= t;
    a23 /= t;
    a24 /= t;
    a31 /= t;
    a32 /= t;
    a33 /= t;
    a34 /= t;
    a41 /= t;
    a42 /= t;
    a43 /= t;
    a44 /= t;

    return *this;
  }

  //-----------------------------------------------------------------------
  inline bool CMatrix4::operator ==(const CMatrix4& m) const
  {
    return ((std::fabs(a11 - m.a11) < std::numeric_limits<Real>::epsilon()) && (std::fabs(a12 - m.a12) < std::numeric_limits<Real>::epsilon()) &&
            (std::fabs(a13 - m.a13) < std::numeric_limits<Real>::epsilon()) && (std::fabs(a14 - m.a14) < std::numeric_limits<Real>::epsilon()) &&
            (std::fabs(a21 - m.a21) < std::numeric_limits<Real>::epsilon()) && (std::fabs(a22 - m.a22) < std::numeric_limits<Real>::epsilon()) &&
            (std::fabs(a23 - m.a23) < std::numeric_limits<Real>::epsilon()) && (std::fabs(a24 - m.a24) < std::numeric_limits<Real>::epsilon()) &&
            (std::fabs(a31 - m.a31) < std::numeric_limits<Real>::epsilon()) && (std::fabs(a32 - m.a32) < std::numeric_limits<Real>::epsilon()) &&
            (std::fabs(a33 - m.a33) < std::numeric_limits<Real>::epsilon()) && (std::fabs(a34 - m.a34) < std::numeric_limits<Real>::epsilon()) &&
            (std::fabs(a41 - m.a41) < std::numeric_limits<Real>::epsilon()) && (std::fabs(a42 - m.a42) < std::numeric_limits<Real>::epsilon()) &&
            (std::fabs(a43 - m.a43) < std::numeric_limits<Real>::epsilon()) && (std::fabs(a44 - m.a44) < std::numeric_limits<Real>::epsilon()));
  }

  //-----------------------------------------------------------------------
  inline bool CMatrix4::operator !=(const CMatrix4& m) const
  {
    return !(*this == m);
  }

  //-----------------------------------------------------------------------
  inline Real& CMatrix4::operator ()(size_t i, size_t j)
  {
    return operator Real*()[i + 4 * j];
  }

  //-----------------------------------------------------------------------
  inline const Real& CMatrix4::operator ()(size_t i, size_t j) const
  {
    return operator ()(i, j);
  }

  //-----------------------------------------------------------------------
  inline CMatrix4::operator const Real*() const
  {
    return &a11;
  }

  //-----------------------------------------------------------------------
  inline CMatrix4::operator Real*()
  {
    return &a11;
  }

  //-----------------------------------------------------------------------
  inline CMatrix4 operator *(const CMatrix4& m, Real t)
  {
    return CMatrix4(m.a11 * t, m.a12 * t, m.a13 * t, m.a14 * t,
                    m.a21 * t, m.a22 * t, m.a23 * t, m.a24 * t,
                    m.a31 * t, m.a32 * t, m.a33 * t, m.a34 * t,
                    m.a41 * t, m.a42 * t, m.a43 * t, m.a44 * t);
  }

  //-----------------------------------------------------------------------
  inline CMatrix4 operator *(Real t, const CMatrix4& m)
  {
    return m * t;
  }

  //-----------------------------------------------------------------------
  inline CMatrix4 operator /(const CMatrix4& m, Real t)
  {
    return CMatrix4(m.a11 / t, m.a12 / t, m.a13 / t, m.a14 / t,
                    m.a21 / t, m.a22 / t, m.a23 / t, m.a24 / t,
                    m.a31 / t, m.a32 / t, m.a33 / t, m.a34 / t,
                    m.a41 / t, m.a42 / t, m.a43 / t, m.a44 / t);
  }

  //-----------------------------------------------------------------------
  inline std::istream& operator >>(std::istream& Stream, CMatrix4& Mat)
  {
    Stream >> Mat.a11 >> Mat.a12 >> Mat.a13 >> Mat.a14;
    Stream >> Mat.a21 >> Mat.a22 >> Mat.a23 >> Mat.a24;
    Stream >> Mat.a31 >> Mat.a32 >> Mat.a33 >> Mat.a34;
    Stream >> Mat.a41 >> Mat.a42 >> Mat.a43 >> Mat.a44;

    return Stream;
  }

  //-----------------------------------------------------------------------
  inline std::ostream& operator <<(std::ostream& Stream, const CMatrix4& Mat)
  {
    Stream << Mat.a11 << " " << Mat.a12 << " " << Mat.a13 << " " << Mat.a14 << std::endl;
    Stream << Mat.a21 << " " << Mat.a22 << " " << Mat.a23 << " " << Mat.a24 << std::endl;
    Stream << Mat.a31 << " " << Mat.a32 << " " << Mat.a33 << " " << Mat.a34 << std::endl;
    Stream << Mat.a41 << " " << Mat.a42 << " " << Mat.a43 << " " << Mat.a44 << std::endl;

    return Stream;
  }

} // namespace Keos

#endif // KEOS_MATRIX4_H

---