path: root/src/math/matrix.h

// * This file is part of the COLOBOT source code
// * Copyright (C) 2012, Polish Portal of Colobot (PPC)
// *
// * 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, either version 3 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 General Public License for more details.
// *
// * You should have received a copy of the GNU General Public License
// * along with this program. If not, see  http://www.gnu.org/licenses/.

/** @defgroup MathMatrixModule math/matrix.h
   Contains the Matrix struct and related functions.
 */

#pragma once

#include "const.h"
#include "func.h"
#include "vector.h"

#include <cmath>
#include <cassert>


// Math module namespace
namespace Math
{

/* @{ */ // start of group

/** \struct Matrix math/matrix.h
    \brief 4x4 matrix

  Represents an universal 4x4 matrix that can be used in OpenGL and DirectX engines.
  Contains the required methods for operating on matrices (inverting, multiplying, etc.).

  The internal representation is a 16-value table in column-major order, thus:

  \verbatim
m[0 ] m[4 ] m[8 ] m[12]
m[1 ] m[5 ] m[9 ] m[13]
m[2 ] m[6 ] m[10] m[14]
m[3 ] m[7 ] m[11] m[15] \endverbatim

  This representation is native to OpenGL; DirectX requires transposing the matrix.

  The order of multiplication of matrix and vector is also OpenGL-native
  (see the function MatrixVectorMultiply).

  All methods are made inline to maximize optimization.

  Unit tests for the structure and related functions are in module: math/test/matrix_test.cpp.

 **/
struct Matrix
{
  //! Matrix values in column-major order
  float m[16];

  //! Creates the indentity matrix
  inline Matrix()
  {
    LoadIdentity();
  }

  //! Creates the matrix from 1D array
  /** \a m matrix values in column-major order */
  inline explicit Matrix(const float (&m)[16])
  {
    for (int i = 0; i < 16; ++i)
      this->m[i] = m[i];
  }

  //! Creates the matrix from 2D array
  /** The array's first index is row, second is column.
      \a m array with values */
  inline explicit Matrix(const float (&m)[4][4])
  {
    for (int c = 0; c < 4; ++c)
    {
      for (int r = 0; r < 4; ++r)
      {
        this->m[4*c+r] = m[r][c];
      }
    }
  }

  //! Loads the zero matrix
  inline void LoadZero()
  {
    for (int i = 0; i < 16; ++i)
      m[i] = 0.0f;
  }

  //! Loads the identity matrix
  inline void LoadIdentity()
  {
    LoadZero();
    /* (1,1) */ m[0 ] = 1.0f;
    /* (2,2) */ m[5 ] = 1.0f;
    /* (3,3) */ m[10] = 1.0f;
    /* (4,4) */ m[15] = 1.0f;
  }

  //! Transposes the matrix
  inline void Transpose()
  {
    /* (2,1) <-> (1,2) */ Swap(m[1 ], m[4 ]);
    /* (3,1) <-> (1,3) */ Swap(m[2 ], m[8 ]);
    /* (4,1) <-> (1,4) */ Swap(m[3 ], m[12]);
    /* (3,2) <-> (2,3) */ Swap(m[6 ], m[9 ]);
    /* (4,2) <-> (2,4) */ Swap(m[7 ], m[13]);
    /* (4,3) <-> (3,4) */ Swap(m[11], m[14]);
  }

  //! Calculates the determinant of the matrix
  /** \returns the determinant */
  inline float Det() const
  {
    float result = 0.0f;
    for (int i = 0; i < 4; ++i)
    {
      result += m[i] * Cofactor(i, 0);
    }
    return result;
  }

  //! Calculates the cofactor of the matrix
  /** \a r row (0 to 3)
      \a c column (0 to 3)
      \returns the cofactor */
  inline float Cofactor(int r, int c) const
  {
    assert(r >= 0 && r <= 3);
    assert(c >= 0 && c <= 3);

    float result = 0.0f;

    /* That looks horrible, I know. But it's fast :) */

    switch (4*r + c)
    {
      // r=0, c=0
      /* 05 09 13
         06 10 14
         07 11 15 */
      case 0:
        result = + m[5 ] * (m[10] * m[15] - m[14] * m[11])
                 - m[9 ] * (m[6 ] * m[15] - m[14] * m[7 ])
                 + m[13] * (m[6 ] * m[11] - m[10] * m[7 ]);
        break;

      // r=0, c=1
      /* 01 09 13
         02 10 14
         03 11 15 */
      case 1:
        result = - m[1 ] * (m[10] * m[15] - m[14] * m[11])
                 + m[9 ] * (m[2 ] * m[15] - m[14] * m[3 ])
                 - m[13] * (m[2 ] * m[11] - m[10] * m[3 ]);
        break;

      // r=0, c=2
      /* 01 05 13
         02 06 14
         03 07 15 */
      case 2:
        result = + m[1 ] * (m[6 ] * m[15] - m[14] * m[7 ])
                 - m[5 ] * (m[2 ] * m[15] - m[14] * m[3 ])
                 + m[13] * (m[2 ] * m[7 ] - m[6 ] * m[3 ]);
        break;

      // r=0, c=3
      /* 01 05 09
         02 06 10
         03 07 11 */
      case 3:
        result = - m[1 ] * (m[6 ] * m[11] - m[10] * m[7 ])
                 + m[5 ] * (m[2 ] * m[11] - m[10] * m[3 ])
                 - m[9 ] * (m[2 ] * m[7 ] - m[6 ] * m[3 ]);
        break;

      // r=1, c=0
      /* 04 08 12
         06 10 14
         07 11 15 */
      case 4:
        result = - m[4 ] * (m[10] * m[15] - m[14] * m[11])
                 + m[8 ] * (m[6 ] * m[15] - m[14] * m[7 ])
                 - m[12] * (m[6 ] * m[11] - m[10] * m[7 ]);
        break;

      // r=1, c=1
      /* 00 08 12
         02 10 14
         03 11 15 */
      case 5:
        result = + m[0 ] * (m[10] * m[15] - m[14] * m[11])
                 - m[8 ] * (m[2 ] * m[15] - m[14] * m[3 ])
                 + m[12] * (m[2 ] * m[11] - m[10] * m[3 ]);
        break;

      // r=1, c=2
      /* 00 04 12
         02 06 14
         03 07 15 */
      case 6:
        result = - m[0 ] * (m[6 ] * m[15] - m[14] * m[7 ])
                 + m[4 ] * (m[2 ] * m[15] - m[14] * m[3 ])
                 - m[12] * (m[2 ] * m[7 ] - m[6 ] * m[3 ]);
        break;

      // r=1, c=3
      /* 00 04 08
         02 06 10
         03 07 11 */
      case 7:
        result = + m[0 ] * (m[6 ] * m[11] - m[10] * m[7 ])
                 - m[4 ] * (m[2 ] * m[11] - m[10] * m[3 ])
                 + m[8 ] * (m[2 ] * m[7 ] - m[6 ] * m[3 ]);
        break;

      // r=2, c=0
      /* 04 08 12
         05 09 13
         07 11 15 */
      case 8:
        result = + m[4 ] * (m[9 ] * m[15] - m[13] * m[11])
                 - m[8 ] * (m[5 ] * m[15] - m[13] * m[7 ])
                 + m[12] * (m[5 ] * m[11] - m[9 ] * m[7 ]);
        break;

      // r=2, c=1
      /* 00 08 12
         01 09 13
         03 11 15 */
      case 9:
        result = - m[0 ] * (m[9 ] * m[15] - m[13] * m[11])
                 + m[8 ] * (m[1 ] * m[15] - m[13] * m[3 ])
                 - m[12] * (m[1 ] * m[11] - m[9 ] * m[3 ]);
        break;

      // r=2, c=2
      /* 00 04 12
         01 05 13
         03 07 15 */
      case 10:
        result = + m[0 ] * (m[5 ] * m[15] - m[13] * m[7 ])
                 - m[4 ] * (m[1 ] * m[15] - m[13] * m[3 ])
                 + m[12] * (m[1 ] * m[7 ] - m[5 ] * m[3 ]);
        break;

      // r=2, c=3
      /* 00 04 08
         01 05 09
         03 07 11 */
      case 11:
        result = - m[0 ] * (m[5 ] * m[11] - m[9 ] * m[7 ])
                 + m[4 ] * (m[1 ] * m[11] - m[9 ] * m[3 ])
                 - m[8 ] * (m[1 ] * m[7 ] - m[5 ] * m[3 ]);
        break;

      // r=3, c=0
      /* 04 08 12
         05 09 13
         06 10 14 */
      case 12:
        result = - m[4 ] * (m[9 ] * m[14] - m[13] * m[10])
                 + m[8 ] * (m[5 ] * m[14] - m[13] * m[6 ])
                 - m[12] * (m[5 ] * m[10] - m[9 ] * m[6 ]);
        break;

      // r=3, c=1
      /* 00 08 12
         01 09 13
         02 10 14 */
      case 13:
        result = + m[0 ] * (m[9 ] * m[14] - m[13] * m[10])
                 - m[8 ] * (m[1 ] * m[14] - m[13] * m[2 ])
                 + m[12] * (m[1 ] * m[10] - m[9 ] * m[2 ]);
        break;

      // r=3, c=2
      /* 00 04 12
         01 05 13
         02 06 14 */
      case 14:
        result = - m[0 ] * (m[5 ] * m[14] - m[13] * m[6 ])
                 + m[4 ] * (m[1 ] * m[14] - m[13] * m[2 ])
                 - m[12] * (m[1 ] * m[6 ] - m[5 ] * m[2 ]);
        break;

      // r=3, c=3
      /* 00 04 08
         01 05 09
         02 06 10 */
      case 15:
        result = + m[0 ] * (m[5 ] * m[10] - m[9 ] * m[6 ])
                 - m[4 ] * (m[1 ] * m[10] - m[9 ] * m[2 ])
                 + m[8 ] * (m[1 ] * m[6 ] - m[5 ] * m[2 ]);
        break;

      default:
        break;
    }

    return result;
  }

  //! Calculates the inverse matrix
  /** The determinant of the matrix must not be zero.
      \returns the inverted matrix */
  inline Matrix Inverse() const
  {
    float d = Det();
    assert(! IsZero(d));

    float result[16] = { 0.0f };

    for (int r = 0; r < 4; ++r)
    {
      for (int c = 0; c < 4; ++c)
      {
        // Already transposed!
        result[4*r+c] = (1.0f / d) * Cofactor(r, c);
      }
    }

    return Matrix(result);
  }

  //! Calculates the multiplication of this matrix * given matrix
  /** \a right right-hand matrix
      \returns multiplication result */
  inline Matrix Multiply(const Matrix &right) const
  {
    float result[16] = { 0.0f };

    for (int c = 0; c < 4; ++c)
    {
      for (int r = 0; r < 4; ++r)
      {
        result[4*c+r] = 0.0f;
        for (int i = 0; i < 4; ++i)
        {
          result[4*c+r] += m[4*i+r] * right.m[4*c+i];
        }
      }
    }

    return Matrix(result);
  }

  //! Loads view matrix from the given vectors
  /** \a from origin
      \a at view direction
      \a worldUp up vector */
  inline void LoadView(const Vector &from, const Vector &at, const Vector &worldUp)
  {
    // Get the z basis vector, which points straight ahead. This is the
    // difference from the eyepoint to the lookat point.
    Vector view = at - from;

    float length = view.Length();
    assert(! IsZero(length) );

    // Normalize the z basis vector
    view /= length;

    // Get the dot product, and calculate the projection of the z basis
    // vector onto the up vector. The projection is the y basis vector.
    float dotProduct = DotProduct(worldUp, view);

    Vector up = worldUp - dotProduct * view;

    // If this vector has near-zero length because the input specified a
    // bogus up vector, let's try a default up vector
    if ( IsZero(length = up.Length()) )
    {
      up = Vector(0.0f, 1.0f, 0.0f) - view.y * view;

      // If we still have near-zero length, resort to a different axis.
      if ( IsZero(length = up.Length()) )
      {
        up = Vector(0.0f, 0.0f, 1.0f) - view.z * view;

        assert(! IsZero(up.Length()) );
      }
    }

    // Normalize the y basis vector
    up /= length;

    // The x basis vector is found simply with the cross product of the y
    // and z basis vectors
    Vector right = CrossProduct(up, view);

    // Start building the matrix. The first three rows contains the basis
    // vectors used to rotate the view to point at the lookat point
    LoadIdentity();

    /* (1,1) */ m[0 ] = right.x;
    /* (2,1) */ m[1 ] = up.x;
    /* (3,1) */ m[2 ] = view.x;
    /* (1,2) */ m[4 ] = right.y;
    /* (2,2) */ m[5 ] = up.y;
    /* (3,2) */ m[6 ] = view.y;
    /* (1,3) */ m[8 ] = right.z;
    /* (2,3) */ m[9 ] = up.z;
    /* (3,3) */ m[10] = view.z;

    // Do the translation values (rotations are still about the eyepoint)
    /* (1,4) */ m[12] = -DotProduct(from, right);
    /* (2,4) */ m[13] = -DotProduct(from, up);
    /* (3,4) */ m[14] = -DotProduct(from, view);
  }

  //! Loads a perspective projection matrix
  /** \a fov field of view in radians
      \a aspect aspect ratio (width / height)
      \a nearPlane distance to near cut plane
      \a farPlane distance to far cut plane */
  inline void LoadProjection(float fov = 1.570795f, float aspect = 1.0f,
                             float nearPlane = 1.0f, float farPlane = 1000.0f)
  {
    assert(fabs(farPlane - nearPlane) >= 0.01f);
    assert(fabs(sin(fov / 2)) >= 0.01f);

    float w = aspect * (cosf(fov / 2) / sinf(fov / 2));
    float h = 1.0f   * (cosf(fov / 2) / sinf(fov / 2));
    float q = farPlane / (farPlane - nearPlane);

    LoadZero();

    /* (1,1) */ m[0 ] = w;
    /* (2,2) */ m[5 ] = h;
    /* (3,3) */ m[10] = q;
    /* (3,4) */ m[14] = 1.0f;
    /* (4,3) */ m[11] = -q * nearPlane;
  }

  //! Loads a translation matrix from given vector
  /** \a trans vector of translation*/
  inline void LoadTranslation(const Vector &trans)
  {
    LoadIdentity();
    /* (1,4) */ m[12] = trans.x;
    /* (2,4) */ m[13] = trans.y;
    /* (3,4) */ m[14] = trans.z;
  }

  //! Loads a scaling matrix fom given vector
  /** \a scale vector with scaling factors for X, Y, Z */
  inline void LoadScale(const Vector &scale)
  {
    LoadIdentity();
    /* (1,1) */ m[0 ] = scale.x;
    /* (2,2) */ m[5 ] = scale.y;
    /* (3,3) */ m[10] = scale.z;
  }

  //! Loads a rotation matrix along the X axis
  /** \a angle angle in radians */
  inline void LoadRotationX(float angle)
  {
    LoadIdentity();
    /* (2,2) */ m[5 ] =  cosf(angle);
    /* (3,2) */ m[6 ] =  sinf(angle);
    /* (2,3) */ m[9 ] = -sinf(angle);
    /* (3,3) */ m[10] =  cosf(angle);
  }

  //! Loads a rotation matrix along the Y axis
  /** \a angle angle in radians */
  inline void LoadRotationY(float angle)
  {
    LoadIdentity();
    /* (1,1) */ m[0 ] =  cosf(angle);
    /* (3,1) */ m[2 ] = -sinf(angle);
    /* (1,3) */ m[8 ] =  sinf(angle);
    /* (3,3) */ m[10] =  cosf(angle);
  }

  //! Loads a rotation matrix along the Z axis
  /** \a angle angle in radians */
  inline void LoadRotationZ(float angle)
  {
    LoadIdentity();
    /* (1,1) */ m[0 ] =  cosf(angle);
    /* (2,1) */ m[1 ] =  sinf(angle);
    /* (1,2) */ m[4 ] = -sinf(angle);
    /* (2,2) */ m[5 ] =  cosf(angle);
  }

  //! Loads a rotation matrix along the given axis
  /** \a dir axis of rotation
      \a angle angle in radians */
  inline void LoadRotation(const Vector &dir, float angle)
  {
    float cos = cosf(angle);
    float sin = sinf(angle);
    Vector v = Normalize(dir);

    LoadIdentity();

    /* (1,1) */ m[0 ] = (v.x * v.x) * (1.0f - cos) + cos;
    /* (2,1) */ m[1 ] = (v.x * v.y) * (1.0f - cos) - (v.z * sin);
    /* (3,1) */ m[2 ] = (v.x * v.z) * (1.0f - cos) + (v.y * sin);

    /* (1,2) */ m[4 ] = (v.y * v.x) * (1.0f - cos) + (v.z * sin);
    /* (2,2) */ m[5 ] = (v.y * v.y) * (1.0f - cos) + cos ;
    /* (3,2) */ m[6 ] = (v.y * v.z) * (1.0f - cos) - (v.x * sin);

    /* (1,3) */ m[8 ] = (v.z * v.x) * (1.0f - cos) - (v.y * sin);
    /* (2,3) */ m[9 ] = (v.z * v.y) * (1.0f - cos) + (v.x * sin);
    /* (3,3) */ m[10] = (v.z * v.z) * (1.0f - cos) + cos;
  }

  //! Calculates the matrix to make three rotations in the order X, Z and Y
  inline void RotateXZY(const Vector &angle)
  {
    this->LoadRotationX(angle.x);

    Matrix temp;
    temp.LoadRotationZ(angle.z);
    this->Multiply(temp);

    temp.LoadRotationY(angle.y);
    this->Multiply(temp);
  }

  //! Calculates the matrix to make three rotations in the order Z, X and Y
  inline void RotateZXY(const Vector &angle)
  {
    this->LoadRotationZ(angle.z);

    Matrix temp;
    temp.LoadRotationX(angle.x);
    this->Multiply(temp);

    temp.LoadRotationY(angle.y);
    this->Multiply(temp);
  }

}; // struct Matrix

//! Checks if two matrices are equal within given \a tolerance
inline bool MatricesEqual(const Matrix &m1, const Matrix &m2,
                                  float tolerance = TOLERANCE)
{
  for (int i = 0; i < 16; ++i)
  {
    if (! IsEqual(m1.m[i], m2.m[i], tolerance))
      return false;
  }

  return true;
}

//! Convenience function for getting transposed matrix
inline Matrix Transpose(const Matrix &m)
{
  Matrix result = m;
  result.Transpose();
  return result;
}

//! Convenience function for multiplying a matrix
/** \a left left-hand matrix
    \a right right-hand matrix
    \returns multiplied matrices */
inline Matrix MultiplyMatrices(const Matrix &left, const Matrix &right)
{
  return left.Multiply(right);
}

//! Calculates the result of multiplying m * v
/** The multiplication is performed thus:
\verbatim [  m.m[0 ] m.m[4 ] m.m[8 ] m.m[12]  ]   [ v.x ]
[  m.m[1 ] m.m[5 ] m.m[9 ] m.m[13]  ]   [ v.y ]
[  m.m[2 ] m.m[6 ] m.m[10] m.m[14]  ] * [ v.z ]
[  m.m[3 ] m.m[7 ] m.m[11] m.m[15]  ]   [  1  ] \endverbatim

    The result, a 4x1 vector is then converted to 3x1 by dividing
    x,y,z coords by the fourth coord (w). */
inline Vector MatrixVectorMultiply(const Matrix &m, const Vector &v)
{
  float x = v.x * m.m[0 ] + v.y * m.m[4 ] + v.z * m.m[8 ] + m.m[12];
  float y = v.x * m.m[1 ] + v.y * m.m[5 ] + v.z * m.m[9 ] + m.m[13];
  float z = v.x * m.m[2 ] + v.y * m.m[6 ] + v.z * m.m[10] + m.m[14];
  float w = v.x * m.m[3 ] + v.y * m.m[7 ] + v.z * m.m[11] + m.m[15];

  if (IsZero(w))
    return Vector(x, y, z);

  x /= w;
  y /= w;
  z /= w;

  return Vector(x, y, z);
}

//! Calculation point of view to look at a center two angles and a distance
inline Vector RotateView(const Vector &center, float angleH, float angleV, float dist)
{
  Matrix mat1, mat2, mat;

  mat1.LoadRotationZ(-angleV);
  mat2.LoadRotationY(-angleH);
  mat = MultiplyMatrices(mat1, mat2);

  Vector eye;
  eye.x = 0.0f+dist;
  eye.y = 0.0f;
  eye.z = 0.0f;
  eye = MatrixVectorMultiply(mat, eye);

  return eye + center;
}

/* @} */ // end of group

}; // namespace Math