Program Listing for File rotation.hpp
↰ Return to documentation for file (include/pinocchio/math/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__