rotation.hpp

//
// Copyright (c) 2019-2020 CNRS INRIA
//

#ifndef __pinocchio_math_rotation_hpp__
#define __pinocchio_math_rotation_hpp__

#include "pinocchio/fwd.hpp"
#include "pinocchio/math/matrix.hpp"
#include "pinocchio/math/sincos.hpp"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>

namespace pinocchio
{

 template<typename Vector3, typename Scalar, typename Matrix3>
 void toRotationMatrix(
 const Eigen::MatrixBase<Vector3> & axis,
 const Scalar & cos_value,
 const Scalar & sin_value,
 const Eigen::MatrixBase<Matrix3> & res)
 {
 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3, 3);
 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3, 3, 3);

 assert(isUnitary(axis) && "The axis is not unitary.");

 Matrix3 & res_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, res);
 Vector3 sin_axis = sin_value * axis;
 Vector3 cos1_axis = (Scalar(1) - cos_value) * axis;

 Scalar tmp;
 tmp = cos1_axis.x() * axis.y();
 res_.coeffRef(0, 1) = tmp - sin_axis.z();
 res_.coeffRef(1, 0) = tmp + sin_axis.z();

 tmp = cos1_axis.x() * axis.z();
 res_.coeffRef(0, 2) = tmp + sin_axis.y();
 res_.coeffRef(2, 0) = tmp - sin_axis.y();

 tmp = cos1_axis.y() * axis.z();
 res_.coeffRef(1, 2) = tmp - sin_axis.x();
 res_.coeffRef(2, 1) = tmp + sin_axis.x();

 res_.diagonal() = (cos1_axis.cwiseProduct(axis)).array() + cos_value;
 }

 template<typename Vector3, typename Scalar, typename Matrix3>
 void toRotationMatrix(
 const Eigen::MatrixBase<Vector3> & axis,
 const Scalar & angle,
 const Eigen::MatrixBase<Matrix3> & res)
 {
 Scalar sa, ca;
 SINCOS(angle, &sa, &ca);
 toRotationMatrix(axis, ca, sa, PINOCCHIO_EIGEN_CONST_CAST(Matrix3, res));
 }

 template<typename Matrix3>
 void normalizeRotation(const Eigen::MatrixBase<Matrix3> & rot)
 {
 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3, 3, 3);
 Matrix3 & rot_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, rot);

 typedef typename Matrix3::Scalar Scalar;
 enum
 {
 Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)::Options
 };
 typedef Eigen::Quaternion<Scalar, Options> Quaternion;
 Quaternion quat(rot);
 normalize(quat.coeffs());
 rot_ = quat.toRotationMatrix();
 }

 template<typename Matrix3>
 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)
 orthogonalProjection(const Eigen::MatrixBase<Matrix3> & mat)
 {
 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3, 3, 3);
 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3) ReturnType;

 typedef Eigen::JacobiSVD<Matrix3> SVD;
 const SVD svd(mat, Eigen::ComputeFullU | Eigen::ComputeFullV);

 ReturnType res;
 res.template leftCols<2>().noalias() =
 svd.matrixU() * svd.matrixV().transpose().template leftCols<2>();
 res.col(2).noalias() = res.col(0).cross(res.col(1));
 return res;
 }
} // namespace pinocchio

#endif // #ifndef __pinocchio_math_rotation_hpp__