#include <camodocal/sparse_graph/Transform.h>

namespace camodocal {

Transform::Transform() {
  m_q.setIdentity();
  m_t.setZero();
}

Transform::Transform(const Eigen::Matrix4d& H) {
  m_q = Eigen::Quaterniond(H.block<3, 3>(0, 0));
  m_t = H.block<3, 1>(0, 3);
}

Eigen::Quaterniond& Transform::rotation(void) { return m_q; }

const Eigen::Quaterniond& Transform::rotation(void) const { return m_q; }

double* Transform::rotationData(void) { return m_q.coeffs().data(); }

const double* const Transform::rotationData(void) const { return m_q.coeffs().data(); }

Eigen::Vector3d& Transform::translation(void) { return m_t; }

const Eigen::Vector3d& Transform::translation(void) const { return m_t; }

double* Transform::translationData(void) { return m_t.data(); }

const double* const Transform::translationData(void) const { return m_t.data(); }

Eigen::Matrix4d Transform::toMatrix(void) const {
  Eigen::Matrix4d H;
  H.setIdentity();
  H.block<3, 3>(0, 0) = m_q.toRotationMatrix();
  H.block<3, 1>(0, 3) = m_t;

  return H;
}

}  // namespace camodocal
