#ifndef RIVET_MATH_MATRIX3
#define RIVET_MATH_MATRIX3

#include "Rivet/Math/MathConstants.hh"
#include "Rivet/Math/MathUtils.hh"
#include "Rivet/Math/MatrixN.hh"
#include "Rivet/Math/Vector3.hh"

namespace Rivet {


  /// @brief Specialisation of MatrixN to aid 3 dimensional rotations.
  class Matrix3 : public Matrix<3> {
  public:
    Matrix3() = default;

    Matrix3(const Matrix<3>& m3) :  Matrix<3>::Matrix(m3) { }

    Matrix3(const Vector3& axis, const double angle) {
      const Vector3 normaxis = axis.unit();
      _matrix = RivetEigen::AngleAxis<double>(angle, normaxis._vec);
    }

    Matrix3(const Vector3& from, const Vector3& to) {
      setAsRotation(from, to);
    }

    static Matrix3 mkXRotation(const double angle) {
      return Matrix3(Vector3(1,0,0), angle);
    }

    static Matrix3 mkYRotation(const double angle) {
      return Matrix3(Vector3(0,1,0), angle);
    }

    static Matrix3 mkZRotation(const double angle) {
      return Matrix3(Vector3(0,0,1), angle);
    }
    
    Matrix3& setAsRotation(const Vector3& from, const Vector3& to) {
      const double theta = angle(from, to);
      if (Rivet::isZero(theta)) {
        _matrix = EMatrix::Identity();
      } else {
        const Vector3 normaxis = cross(from, to).unit();
        _matrix = RivetEigen::AngleAxis<double>(theta, normaxis._vec);
      }
      return *this;
    }
    
    static Matrix3 mkRotation(const Vector3& from, const Vector3& to) {
      Matrix3 rtn;
      rtn.setAsRotation(from, to);
      return rtn;
    }

  };


}

#endif
