Matrix4.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2012-2014 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#ifndef _IGNITION_MATRIX4_HH_
#define _IGNITION_MATRIX4_HH_

#include <ignition/math/Helpers.hh>
#include <ignition/math/AffineException.hh>
#include <ignition/math/Matrix3.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Pose3.hh>

namespace ignition
{
  namespace math
  {
    template<typename T>
    class Matrix4
    {
      public: static const Matrix4<T> Identity;

      public: static const Matrix4<T> Zero;

      public: Matrix4()
      {
        memset(this->data, 0, sizeof(this->data[0][0])*16);
      }

      public: Matrix4(const Matrix4<T> &_m)
      {
        memcpy(this->data, _m.data, sizeof(this->data[0][0])*16);
      }

      public: Matrix4(T _v00, T _v01, T _v02, T _v03,
                      T _v10, T _v11, T _v12, T _v13,
                      T _v20, T _v21, T _v22, T _v23,
                      T _v30, T _v31, T _v32, T _v33)
      {
        this->Set(_v00, _v01, _v02, _v03,
                  _v10, _v11, _v12, _v13,
                  _v20, _v21, _v22, _v23,
                  _v30, _v31, _v32, _v33);
      }

      public: Matrix4(const Quaternion<T> &_q)
      {
        Quaternion<T> qt = _q;
        qt.Normalize();
        this->Set(1 - 2*qt.Y()*qt.Y() - 2 *qt.Z()*qt.Z(),
                  2 * qt.X()*qt.Y() - 2*qt.Z()*qt.W(),
                  2 * qt.X() * qt.Z() + 2 * qt.Y() * qt.W(),
                  0,

                  2 * qt.X() * qt.Y() + 2 * qt.Z() * qt.W(),
                  1 - 2*qt.X()*qt.X() - 2 * qt.Z()*qt.Z(),
                  2 * qt.Y() * qt.Z() - 2 * qt.X() * qt.W(),
                  0,

                  2 * qt.X() * qt.Z() - 2 * qt.Y() * qt.W(),
                  2 * qt.Y() * qt.Z() + 2 * qt.X() * qt.W(),
                  1 - 2 * qt.X()*qt.X() - 2 * qt.Y()*qt.Y(),
                  0,

                  0, 0, 0, 1);
      }

      public: virtual ~Matrix4() {}

      public: void Set(
            T _v00, T _v01, T _v02, T _v03,
            T _v10, T _v11, T _v12, T _v13,
            T _v20, T _v21, T _v22, T _v23,
            T _v30, T _v31, T _v32, T _v33)
      {
        this->data[0][0] = _v00;
        this->data[0][1] = _v01;
        this->data[0][2] = _v02;
        this->data[0][3] = _v03;

        this->data[1][0] = _v10;
        this->data[1][1] = _v11;
        this->data[1][2] = _v12;
        this->data[1][3] = _v13;

        this->data[2][0] = _v20;
        this->data[2][1] = _v21;
        this->data[2][2] = _v22;
        this->data[2][3] = _v23;

        this->data[3][0] = _v30;
        this->data[3][1] = _v31;
        this->data[3][2] = _v32;
        this->data[3][3] = _v33;
      }

      public: void Axis(const Vector3<T> &_axis, T _angle)
      {
        T c = cos(_angle);
        T s = sin(_angle);
        T C = 1-c;

        this->data[0][0] = _axis.X()*_axis.X()*C + c;
        this->data[0][1] = _axis.X()*_axis.Y()*C - _axis.Z()*s;
        this->data[0][2] = _axis.X()*_axis.Z()*C + _axis.Y()*s;

        this->data[1][0] = _axis.Y()*_axis.X()*C + _axis.Z()*s;
        this->data[1][1] = _axis.Y()*_axis.Y()*C + c;
        this->data[1][2] = _axis.Y()*_axis.Z()*C - _axis.X()*s;

        this->data[2][0] = _axis.Z()*_axis.X()*C - _axis.Y()*s;
        this->data[2][1] = _axis.Z()*_axis.Y()*C + _axis.X()*s;
        this->data[2][2] = _axis.Z()*_axis.Z()*C + c;
      }

      public: void Translate(const Vector3<T> &_t)
      {
        this->data[0][3] = _t.X();
        this->data[1][3] = _t.Y();
        this->data[2][3] = _t.Z();
      }

      public: void Translate(T _x, T _y, T _z)
      {
        this->data[0][3] = _x;
        this->data[1][3] = _y;
        this->data[2][3] = _z;
      }

      public: Vector3<T> Translation() const
      {
        return Vector3<T>(this->data[0][3], this->data[1][3], this->data[2][3]);
      }

      public: Vector3<T> Scale() const
      {
        return Vector3<T>(this->data[0][0], this->data[1][1], this->data[2][2]);
      }

      public: Quaternion<T> Rotation() const
      {
        Quaternion<T> q;
        T trace = this->data[0][0] + this->data[1][1] + this->data[2][2];
        T root;
        if (trace > 0)
        {
          root = sqrt(trace + 1.0);
          q.W(root / 2.0);
          root = 1.0 / (2.0 * root);
          q.X((this->data[2][1] - this->data[1][2]) * root);
          q.Y((this->data[0][2] - this->data[2][0]) * root);
          q.Z((this->data[1][0] - this->data[0][1]) * root);
        }
        else
        {
          static unsigned int s_iNext[3] = {1, 2, 0};
          unsigned int i = 0;
          if (this->data[1][1] > this->data[0][0])
            i = 1;
          if (this->data[2][2] > this->data[i][i])
            i = 2;
          unsigned int j = s_iNext[i];
          unsigned int k = s_iNext[j];

          root = sqrt(this->data[i][i] - this->data[j][j] -
                      this->data[k][k] + 1.0);

          T a, b, c;
          a = root / 2.0;
          root = 1.0 / (2.0 * root);
          b = (this->data[j][i] + this->data[i][j]) * root;
          c = (this->data[k][i] + this->data[i][k]) * root;

          switch (i)
          {
            default:
            case 0: q.X(a); break;
            case 1: q.Y(a); break;
            case 2: q.Z(a); break;
          };
          switch (j)
          {
            default:
            case 0: q.X(b); break;
            case 1: q.Y(b); break;
            case 2: q.Z(b); break;
          };
          switch (k)
          {
            default:
            case 0: q.X(c); break;
            case 1: q.Y(c); break;
            case 2: q.Z(c); break;
          };

          q.W((this->data[k][j] - this->data[j][k]) * root);
        }

        return q;
      }

      public: Vector3<T> EulerRotation(bool _firstSolution) const
      {
        Vector3<T> euler;
        Vector3<T> euler2;

        T m31 = this->data[2][0];
        T m11 = this->data[0][0];
        T m12 = this->data[0][1];
        T m13 = this->data[0][2];
        T m32 = this->data[2][1];
        T m33 = this->data[2][2];
        T m21 = this->data[1][0];

        if (std::abs(m31) >= 1.0)
        {
          euler.Z(0.0);
          euler2.Z(0.0);

          if (m31 < 0.0)
          {
            euler.Y(IGN_PI / 2.0);
            euler2.Y(IGN_PI / 2.0);
            euler.X(atan2(m12, m13));
            euler2.X(atan2(m12, m13));
          }
          else
          {
            euler.Y(-IGN_PI / 2.0);
            euler2.Y(-IGN_PI / 2.0);
            euler.X(atan2(-m12, -m13));
            euler2.X(atan2(-m12, -m13));
          }
        }
        else
        {
          euler.Y(-asin(m31));
          euler2.Y(IGN_PI - euler.Y());

          euler.X(atan2(m32 / cos(euler.Y()), m33 / cos(euler.Y())));
          euler2.X(atan2(m32 / cos(euler2.Y()), m33 / cos(euler2.Y())));

          euler.Z(atan2(m21 / cos(euler.Y()), m11 / cos(euler.Y())));
          euler2.Z(atan2(m21 / cos(euler2.Y()), m11 / cos(euler2.Y())));
        }

        if (_firstSolution)
          return euler;
        else
          return euler2;
      }

      public: Pose3<T> Pose() const
      {
        return Pose3<T>(this->Translation(), this->Rotation());
      }

      public: void Scale(const Vector3<T> &_s)
      {
        this->data[0][0] = _s.X();
        this->data[1][1] = _s.Y();
        this->data[2][2] = _s.Z();
        this->data[3][3] = 1.0;
      }

      public: void Scale(T _x, T _y, T _z)
      {
        this->data[0][0] = _x;
        this->data[1][1] = _y;
        this->data[2][2] = _z;
        this->data[3][3] = 1.0;
      }

      public: bool IsAffine() const
      {
        return equal(this->data[3][0], static_cast<T>(0)) &&
          equal(this->data[3][1], static_cast<T>(0)) &&
          equal(this->data[3][2], static_cast<T>(0)) &&
          equal(this->data[3][3], static_cast<T>(1));
      }

      public: Vector3<T> TransformAffine(const Vector3<T> &_v) const
      {
        if (!this->IsAffine())
          throw AffineException();

        return Vector3<T>(this->data[0][0]*_v.X() + this->data[0][1]*_v.Y() +
                           this->data[0][2]*_v.Z() + this->data[0][3],
                           this->data[1][0]*_v.X() + this->data[1][1]*_v.Y() +
                           this->data[1][2]*_v.Z() + this->data[1][3],
                           this->data[2][0]*_v.X() + this->data[2][1]*_v.Y() +
                           this->data[2][2]*_v.Z() + this->data[2][3]);
      }

      public: Matrix4<T> Inverse() const
      {
        T v0, v1, v2, v3, v4, v5, t00, t10, t20, t30;
        Matrix4<T> r;

        v0 = this->data[2][0]*this->data[3][1] -
          this->data[2][1]*this->data[3][0];
        v1 = this->data[2][0]*this->data[3][2] -
          this->data[2][2]*this->data[3][0];
        v2 = this->data[2][0]*this->data[3][3] -
          this->data[2][3]*this->data[3][0];
        v3 = this->data[2][1]*this->data[3][2] -
          this->data[2][2]*this->data[3][1];
        v4 = this->data[2][1]*this->data[3][3] -
          this->data[2][3]*this->data[3][1];
        v5 = this->data[2][2]*this->data[3][3] -
          this->data[2][3]*this->data[3][2];

        t00 = +(v5*this->data[1][1] -
            v4*this->data[1][2] + v3*this->data[1][3]);
        t10 = -(v5*this->data[1][0] -
            v2*this->data[1][2] + v1*this->data[1][3]);
        t20 = +(v4*this->data[1][0] -
            v2*this->data[1][1] + v0*this->data[1][3]);
        t30 = -(v3*this->data[1][0] -
            v1*this->data[1][1] + v0*this->data[1][2]);

        T invDet = 1 / (t00 * this->data[0][0] + t10 * this->data[0][1] +
            t20 * this->data[0][2] + t30 * this->data[0][3]);

        r(0, 0) = t00 * invDet;
        r(1, 0) = t10 * invDet;
        r(2, 0) = t20 * invDet;
        r(3, 0) = t30 * invDet;

        r(0, 1) = -(v5*this->data[0][1] -
            v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
        r(1, 1) = +(v5*this->data[0][0] -
            v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
        r(2, 1) = -(v4*this->data[0][0] -
            v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
        r(3, 1) = +(v3*this->data[0][0] -
            v1*this->data[0][1] + v0*this->data[0][2]) * invDet;

        v0 = this->data[1][0]*this->data[3][1] -
          this->data[1][1]*this->data[3][0];
        v1 = this->data[1][0]*this->data[3][2] -
          this->data[1][2]*this->data[3][0];
        v2 = this->data[1][0]*this->data[3][3] -
          this->data[1][3]*this->data[3][0];
        v3 = this->data[1][1]*this->data[3][2] -
          this->data[1][2]*this->data[3][1];
        v4 = this->data[1][1]*this->data[3][3] -
          this->data[1][3]*this->data[3][1];
        v5 = this->data[1][2]*this->data[3][3] -
          this->data[1][3]*this->data[3][2];

        r(0, 2) = +(v5*this->data[0][1] -
            v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
        r(1, 2) = -(v5*this->data[0][0] -
            v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
        r(2, 2) = +(v4*this->data[0][0] -
            v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
        r(3, 2) = -(v3*this->data[0][0] -
            v1*this->data[0][1] + v0*this->data[0][2]) * invDet;

        v0 = this->data[2][1]*this->data[1][0] -
          this->data[2][0]*this->data[1][1];
        v1 = this->data[2][2]*this->data[1][0] -
          this->data[2][0]*this->data[1][2];
        v2 = this->data[2][3]*this->data[1][0] -
          this->data[2][0]*this->data[1][3];
        v3 = this->data[2][2]*this->data[1][1] -
          this->data[2][1]*this->data[1][2];
        v4 = this->data[2][3]*this->data[1][1] -
          this->data[2][1]*this->data[1][3];
        v5 = this->data[2][3]*this->data[1][2] -
          this->data[2][2]*this->data[1][3];

        r(0, 3) = -(v5*this->data[0][1] -
            v4*this->data[0][2] + v3*this->data[0][3]) * invDet;
        r(1, 3) = +(v5*this->data[0][0] -
            v2*this->data[0][2] + v1*this->data[0][3]) * invDet;
        r(2, 3) = -(v4*this->data[0][0] -
            v2*this->data[0][1] + v0*this->data[0][3]) * invDet;
        r(3, 3) = +(v3*this->data[0][0] -
            v1*this->data[0][1] + v0*this->data[0][2]) * invDet;

        return r;
      }

      public: Matrix4<T> &operator=(const Matrix4<T> &_mat)
      {
        memcpy(this->data, _mat.data, sizeof(this->data[0][0])*16);
        return *this;
      }

      public: const Matrix4<T> &operator=(const Matrix3<T> &_mat)
      {
        this->data[0][0] = _mat(0, 0);
        this->data[0][1] = _mat(0, 1);
        this->data[0][2] = _mat(0, 2);

        this->data[1][0] = _mat(1, 0);
        this->data[1][1] = _mat(1, 1);
        this->data[1][2] = _mat(1, 2);

        this->data[2][0] = _mat(2, 0);
        this->data[2][1] = _mat(2, 1);
        this->data[2][2] = _mat(2, 2);

        return *this;
      }

      public: Matrix4<T> operator*(const Matrix4<T> &_m2) const
      {
        return Matrix4<T>(
          this->data[0][0] * _m2(0, 0) +
          this->data[0][1] * _m2(1, 0) +
          this->data[0][2] * _m2(2, 0) +
          this->data[0][3] * _m2(3, 0),

          this->data[0][0] * _m2(0, 1) +
          this->data[0][1] * _m2(1, 1) +
          this->data[0][2] * _m2(2, 1) +
          this->data[0][3] * _m2(3, 1),

          this->data[0][0] * _m2(0, 2) +
          this->data[0][1] * _m2(1, 2) +
          this->data[0][2] * _m2(2, 2) +
          this->data[0][3] * _m2(3, 2),

          this->data[0][0] * _m2(0, 3) +
          this->data[0][1] * _m2(1, 3) +
          this->data[0][2] * _m2(2, 3) +
          this->data[0][3] * _m2(3, 3),

          this->data[1][0] * _m2(0, 0) +
          this->data[1][1] * _m2(1, 0) +
          this->data[1][2] * _m2(2, 0) +
          this->data[1][3] * _m2(3, 0),

          this->data[1][0] * _m2(0, 1) +
          this->data[1][1] * _m2(1, 1) +
          this->data[1][2] * _m2(2, 1) +
          this->data[1][3] * _m2(3, 1),

          this->data[1][0] * _m2(0, 2) +
          this->data[1][1] * _m2(1, 2) +
          this->data[1][2] * _m2(2, 2) +
          this->data[1][3] * _m2(3, 2),

          this->data[1][0] * _m2(0, 3) +
          this->data[1][1] * _m2(1, 3) +
          this->data[1][2] * _m2(2, 3) +
          this->data[1][3] * _m2(3, 3),

          this->data[2][0] * _m2(0, 0) +
          this->data[2][1] * _m2(1, 0) +
          this->data[2][2] * _m2(2, 0) +
          this->data[2][3] * _m2(3, 0),

          this->data[2][0] * _m2(0, 1) +
          this->data[2][1] * _m2(1, 1) +
          this->data[2][2] * _m2(2, 1) +
          this->data[2][3] * _m2(3, 1),

          this->data[2][0] * _m2(0, 2) +
          this->data[2][1] * _m2(1, 2) +
          this->data[2][2] * _m2(2, 2) +
          this->data[2][3] * _m2(3, 2),

          this->data[2][0] * _m2(0, 3) +
          this->data[2][1] * _m2(1, 3) +
          this->data[2][2] * _m2(2, 3) +
          this->data[2][3] * _m2(3, 3),

          this->data[3][0] * _m2(0, 0) +
          this->data[3][1] * _m2(1, 0) +
          this->data[3][2] * _m2(2, 0) +
          this->data[3][3] * _m2(3, 0),

          this->data[3][0] * _m2(0, 1) +
          this->data[3][1] * _m2(1, 1) +
          this->data[3][2] * _m2(2, 1) +
          this->data[3][3] * _m2(3, 1),

          this->data[3][0] * _m2(0, 2) +
          this->data[3][1] * _m2(1, 2) +
          this->data[3][2] * _m2(2, 2) +
          this->data[3][3] * _m2(3, 2),

          this->data[3][0] * _m2(0, 3) +
          this->data[3][1] * _m2(1, 3) +
          this->data[3][2] * _m2(2, 3) +
          this->data[3][3] * _m2(3, 3));
      }

      public: Vector3<T> operator*(const Vector3<T> &_vec) const
      {
        return Vector3<T>(
            this->data[0][0]*_vec.X() + this->data[0][1]*_vec.Y() +
            this->data[0][2]*_vec.Z() + this->data[0][3],
            this->data[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
            this->data[1][2]*_vec.Z() + this->data[1][3],
            this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
            this->data[2][2]*_vec.Z() + this->data[2][3]);
      }

      public: inline const T &operator()(size_t _row, size_t _col) const
      {
        if (_row >= 4 || _col >= 4)
          throw IndexException();
        return this->data[_row][_col];
      }

      public: inline T &operator()(size_t _row, size_t _col)
      {
        if (_row >= 4 || _col >= 4)
          throw IndexException();
        return this->data[_row][_col];
      }

      public: bool operator==(const Matrix4<T> &_m) const
      {
        return math::equal(this->data[0][0], _m(0, 0)) &&
               math::equal(this->data[0][1], _m(0, 1)) &&
               math::equal(this->data[0][2], _m(0, 2)) &&
               math::equal(this->data[0][3], _m(0, 3)) &&

               math::equal(this->data[1][0], _m(1, 0)) &&
               math::equal(this->data[1][1], _m(1, 1)) &&
               math::equal(this->data[1][2], _m(1, 2)) &&
               math::equal(this->data[1][3], _m(1, 3)) &&

               math::equal(this->data[2][0], _m(2, 0)) &&
               math::equal(this->data[2][1], _m(2, 1)) &&
               math::equal(this->data[2][2], _m(2, 2)) &&
               math::equal(this->data[2][3], _m(2, 3)) &&

               math::equal(this->data[3][0], _m(3, 0)) &&
               math::equal(this->data[3][1], _m(3, 1)) &&
               math::equal(this->data[3][2], _m(3, 2)) &&
               math::equal(this->data[3][3], _m(3, 3));
      }

      public: bool operator!=(const Matrix4<T> &_m) const
      {
        return !(*this == _m);
      }

      public: friend std::ostream &operator<<(
                  std::ostream &_out, const ignition::math::Matrix4<T> &_m)
      {
        _out << precision(_m(0, 0), 6) << " "
             << precision(_m(0, 1), 6) << " "
             << precision(_m(0, 2), 6) << " "
             << precision(_m(0, 3), 6) << " "
             << precision(_m(1, 0), 6) << " "
             << precision(_m(1, 1), 6) << " "
             << precision(_m(1, 2), 6) << " "
             << precision(_m(1, 3), 6) << " "
             << precision(_m(2, 0), 6) << " "
             << precision(_m(2, 1), 6) << " "
             << precision(_m(2, 2), 6) << " "
             << precision(_m(2, 3), 6) << " "
             << precision(_m(3, 0), 6) << " "
             << precision(_m(3, 1), 6) << " "
             << precision(_m(3, 2), 6) << " "
             << precision(_m(3, 3), 6);

        return _out;
      }

      public: friend std::istream &operator>>(
                  std::istream &_in, ignition::math::Matrix4<T> &_m)
      {
        // Skip white spaces
        _in.setf(std::ios_base::skipws);
        T d[16];
        _in >> d[0] >> d[1] >> d[2] >> d[3]
            >> d[4] >> d[5] >> d[6] >> d[7]
            >> d[8] >> d[9] >> d[10] >> d[11]
            >> d[12] >> d[13] >> d[14] >> d[15];

        _m.Set(d[0], d[1], d[2], d[3],
               d[4], d[5], d[6], d[7],
               d[8], d[9], d[10], d[11],
               d[12], d[13], d[14], d[15]);
        return _in;
      }

      private: T data[4][4];
    };

    template<typename T>
    const Matrix4<T> Matrix4<T>::Identity(
        1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1, 0,
        0, 0, 0, 1);

    template<typename T>
    const Matrix4<T> Matrix4<T>::Zero(
        0, 0, 0, 0,
        0, 0, 0, 0,
        0, 0, 0, 0,
        0, 0, 0, 0);

    typedef Matrix4<int> Matrix4i;
    typedef Matrix4<double> Matrix4d;
    typedef Matrix4<float> Matrix4f;
  }
}
#endif