Conversions.h

Go to the documentation of this file.

/*
 * Copyright 2012-2020 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include "SpaceVecAlg"
 
namespace sva
{
 
namespace conversions
{
constexpr bool RightHanded = true;
constexpr bool LeftHanded = false;
 
template<typename Derived>
PTransform<typename Derived::Scalar> fromHomogeneous(const Eigen::MatrixBase<Derived> & m,
                                                     bool rightHandedness = RightHanded)
{
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 4, 4);
  if(rightHandedness)
  {
    return PTransform<typename Derived::Scalar>(m.template block<3, 3>(0, 0).transpose(), m.template block<3, 1>(0, 3));
  }
  else
  {
    return PTransform<typename Derived::Scalar>(m.template block<3, 3>(0, 0), m.template block<3, 1>(0, 3));
  }
}
 
template<typename T>
Eigen::Matrix<T, 4, 4> toHomogeneous(const PTransform<T> & pt, bool rightHandedness = RightHanded)
{
  Eigen::Matrix<T, 4, 4> res = Eigen::Matrix<T, 4, 4>::Zero();
  res(3, 3) = 1.0;
 
  res.template block<3, 1>(0, 3) = pt.translation();
 
  if(rightHandedness)
  {
    res.template block<3, 3>(0, 0) = pt.rotation().transpose();
  }
  else
  {
    res.template block<3, 3>(0, 0) = pt.rotation();
  }
  return res;
}
 
template<typename T>
using affine3_t = Eigen::Transform<T, 3, Eigen::TransformTraits::Affine>;
 
template<typename T>
PTransform<T> fromAffine(const affine3_t<T> & a, bool rightHandedness = RightHanded)
{
  if(rightHandedness)
  {
    return PTransform<T>(a.rotation().transpose(), a.translation());
  }
  else
  {
    return PTransform<T>(a.rotation(), a.translation());
  }
}
 
template<typename T>
affine3_t<T> toAffine(const PTransform<T> & pt, bool rightHandedness = RightHanded)
{
  affine3_t<T> ret;
  ret.setIdentity();
  ret.translation() = pt.translation();
  if(rightHandedness)
  {
    ret.matrix().template block<3, 3>(0, 0) = pt.rotation().transpose();
  }
  else
  {
    ret.matrix().template block<3, 3>(0, 0) = pt.rotation();
  }
  return ret;
}
} // namespace conversions
 
} // namespace sva