Matrix3.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2012 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_MATH_MATRIX3_HH_
#define IGNITION_MATH_MATRIX3_HH_

#include <algorithm>
#include <cstring>
#include <ignition/math/Helpers.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Quaternion.hh>
#include <ignition/math/config.hh>

namespace ignition
{
  namespace math
  {
    // Inline bracket to help doxygen filtering.
    inline namespace IGNITION_MATH_VERSION_NAMESPACE {
    //
    template <typename T> class Quaternion;

    template<typename T>
    class Matrix3
    {
      public: static const Matrix3<T> Identity;

      public: static const Matrix3<T> Zero;

      public: Matrix3()
      {
        std::memset(this->data, 0, sizeof(this->data[0][0])*9);
      }

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

      public: Matrix3(T _v00, T _v01, T _v02,
                      T _v10, T _v11, T _v12,
                      T _v20, T _v21, T _v22)
      {
        this->data[0][0] = _v00;
        this->data[0][1] = _v01;
        this->data[0][2] = _v02;
        this->data[1][0] = _v10;
        this->data[1][1] = _v11;
        this->data[1][2] = _v12;
        this->data[2][0] = _v20;
        this->data[2][1] = _v21;
        this->data[2][2] = _v22;
      }

      public: explicit Matrix3(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(),
                  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(),
                  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());
      }

      public: virtual ~Matrix3() {}

      public: void Set(T _v00, T _v01, T _v02,
                       T _v10, T _v11, T _v12,
                       T _v20, T _v21, T _v22)
      {
        this->data[0][0] = _v00;
        this->data[0][1] = _v01;
        this->data[0][2] = _v02;
        this->data[1][0] = _v10;
        this->data[1][1] = _v11;
        this->data[1][2] = _v12;
        this->data[2][0] = _v20;
        this->data[2][1] = _v21;
        this->data[2][2] = _v22;
      }

      public: void Axes(const Vector3<T> &_xAxis,
                        const Vector3<T> &_yAxis,
                        const Vector3<T> &_zAxis)
      {
        this->Col(0, _xAxis);
        this->Col(1, _yAxis);
        this->Col(2, _zAxis);
      }

      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 From2Axes(const Vector3<T> &_v1, const Vector3<T> &_v2)
      {
        const T _v1LengthSquared = _v1.SquaredLength();
        if (_v1LengthSquared <= 0.0)
        {
          // zero vector - we can't handle this
          this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
          return;
        }

        const T _v2LengthSquared = _v2.SquaredLength();
        if (_v2LengthSquared <= 0.0)
        {
          // zero vector - we can't handle this
          this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
          return;
        }

        const T dot = _v1.Dot(_v2) / sqrt(_v1LengthSquared * _v2LengthSquared);
        if (fabs(dot - 1.0) <= 1e-6)
        {
          // the vectors are parallel
          this->Set(1, 0, 0, 0, 1, 0, 0, 0, 1);
          return;
        }
        else if (fabs(dot + 1.0) <= 1e-6)
        {
          // the vectors are opposite
          this->Set(-1, 0, 0, 0, -1, 0, 0, 0, -1);
          return;
        }

        const Vector3<T> cross = _v1.Cross(_v2).Normalize();

        this->Axis(cross, acos(dot));
      }

      public: void Col(unsigned int _c, const Vector3<T> &_v)
      {
        unsigned int c = clamp(_c, 0u, 2u);

        this->data[0][c] = _v.X();
        this->data[1][c] = _v.Y();
        this->data[2][c] = _v.Z();
      }

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

      public: Matrix3<T> operator-(const Matrix3<T> &_m) const
      {
        return Matrix3<T>(
            this->data[0][0] - _m(0, 0),
            this->data[0][1] - _m(0, 1),
            this->data[0][2] - _m(0, 2),
            this->data[1][0] - _m(1, 0),
            this->data[1][1] - _m(1, 1),
            this->data[1][2] - _m(1, 2),
            this->data[2][0] - _m(2, 0),
            this->data[2][1] - _m(2, 1),
            this->data[2][2] - _m(2, 2));
      }

      public: Matrix3<T> operator+(const Matrix3<T> &_m) const
      {
        return Matrix3<T>(
            this->data[0][0]+_m(0, 0),
            this->data[0][1]+_m(0, 1),
            this->data[0][2]+_m(0, 2),
            this->data[1][0]+_m(1, 0),
            this->data[1][1]+_m(1, 1),
            this->data[1][2]+_m(1, 2),
            this->data[2][0]+_m(2, 0),
            this->data[2][1]+_m(2, 1),
            this->data[2][2]+_m(2, 2));
      }

      public: Matrix3<T> operator*(const T &_s) const
      {
        return Matrix3<T>(
          _s * this->data[0][0], _s * this->data[0][1], _s * this->data[0][2],
          _s * this->data[1][0], _s * this->data[1][1], _s * this->data[1][2],
          _s * this->data[2][0], _s * this->data[2][1], _s * this->data[2][2]);
      }

      public: Matrix3<T> operator*(const Matrix3<T> &_m) const
      {
        return Matrix3<T>(
            // first row
            this->data[0][0]*_m(0, 0)+
            this->data[0][1]*_m(1, 0)+
            this->data[0][2]*_m(2, 0),

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

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

            // second row
            this->data[1][0]*_m(0, 0)+
            this->data[1][1]*_m(1, 0)+
            this->data[1][2]*_m(2, 0),

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

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

            // third row
            this->data[2][0]*_m(0, 0)+
            this->data[2][1]*_m(1, 0)+
            this->data[2][2]*_m(2, 0),

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

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

      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[1][0]*_vec.X() + this->data[1][1]*_vec.Y() +
            this->data[1][2]*_vec.Z(),
            this->data[2][0]*_vec.X() + this->data[2][1]*_vec.Y() +
            this->data[2][2]*_vec.Z());
      }

      public: friend inline Matrix3<T> operator*(T _s, const Matrix3<T> &_m)
      {
        return _m * _s;
      }

      public: friend inline Vector3<T> operator*(const Vector3<T> &_v,
                                                 const Matrix3<T> &_m)
      {
        return Vector3<T>(
            _m(0, 0)*_v.X() + _m(1, 0)*_v.Y() + _m(2, 0)*_v.Z(),
            _m(0, 1)*_v.X() + _m(1, 1)*_v.Y() + _m(2, 1)*_v.Z(),
            _m(0, 2)*_v.X() + _m(1, 2)*_v.Y() + _m(2, 2)*_v.Z());
      }

      public: bool Equal(const Matrix3 &_m, const T &_tol) const
      {
        return equal<T>(this->data[0][0], _m(0, 0), _tol)
            && equal<T>(this->data[0][1], _m(0, 1), _tol)
            && equal<T>(this->data[0][2], _m(0, 2), _tol)
            && equal<T>(this->data[1][0], _m(1, 0), _tol)
            && equal<T>(this->data[1][1], _m(1, 1), _tol)
            && equal<T>(this->data[1][2], _m(1, 2), _tol)
            && equal<T>(this->data[2][0], _m(2, 0), _tol)
            && equal<T>(this->data[2][1], _m(2, 1), _tol)
            && equal<T>(this->data[2][2], _m(2, 2), _tol);
      }

      public: bool operator==(const Matrix3<T> &_m) const
      {
        return this->Equal(_m, static_cast<T>(1e-6));
      }

      public: Matrix3<T> &operator=(const Quaternion<T> &_q)
      {
        return *this = Matrix3<T>(_q);
      }

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

      public: inline const T &operator()(size_t _row, size_t _col) const
      {
        return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]
                         [clamp(_col, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)];
      }

      public: inline T &operator()(size_t _row, size_t _col)
      {
        return this->data[clamp(_row, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)]
                         [clamp(_col, IGN_ZERO_SIZE_T, IGN_TWO_SIZE_T)];
      }

      public: T Determinant() const
      {
        T t0 = this->data[2][2]*this->data[1][1]
             - this->data[2][1]*this->data[1][2];

        T t1 = -(this->data[2][2]*this->data[1][0]
                -this->data[2][0]*this->data[1][2]);

        T t2 = this->data[2][1]*this->data[1][0]
             - this->data[2][0]*this->data[1][1];

        return t0 * this->data[0][0]
             + t1 * this->data[0][1]
             + t2 * this->data[0][2];
      }

      public: Matrix3<T> Inverse() const
      {
        T t0 = this->data[2][2]*this->data[1][1] -
                    this->data[2][1]*this->data[1][2];

        T t1 = -(this->data[2][2]*this->data[1][0] -
                      this->data[2][0]*this->data[1][2]);

        T t2 = this->data[2][1]*this->data[1][0] -
                    this->data[2][0]*this->data[1][1];

        T invDet = 1.0 / (t0 * this->data[0][0] +
                               t1 * this->data[0][1] +
                               t2 * this->data[0][2]);

        return invDet * Matrix3<T>(
          t0,
          - (this->data[2][2] * this->data[0][1] -
             this->data[2][1] * this->data[0][2]),
          + (this->data[1][2] * this->data[0][1] -
             this->data[1][1] * this->data[0][2]),
          t1,
          + (this->data[2][2] * this->data[0][0] -
             this->data[2][0] * this->data[0][2]),
          - (this->data[1][2] * this->data[0][0] -
             this->data[1][0] * this->data[0][2]),
          t2,
          - (this->data[2][1] * this->data[0][0] -
             this->data[2][0] * this->data[0][1]),
          + (this->data[1][1] * this->data[0][0] -
             this->data[1][0] * this->data[0][1]));
      }

      public: void Transpose()
      {
        std::swap(this->data[0][1], this->data[1][0]);
        std::swap(this->data[0][2], this->data[2][0]);
        std::swap(this->data[1][2], this->data[2][1]);
      }

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

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

        return _out;
      }
      public: friend std::istream &operator>>(
                  std::istream &_in, ignition::math::Matrix3<T> &_m)
      {
        // Skip white spaces
        _in.setf(std::ios_base::skipws);
        T d[9];
        _in >> d[0] >> d[1] >> d[2]
            >> d[3] >> d[4] >> d[5]
            >> d[6] >> d[7] >> d[8];

        _m.Set(d[0], d[1], d[2],
               d[3], d[4], d[5],
               d[6], d[7], d[8]);
        return _in;
      }

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

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

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

    typedef Matrix3<int> Matrix3i;
    typedef Matrix3<double> Matrix3d;
    typedef Matrix3<float> Matrix3f;
    }
  }
}

#endif