-
Notifications
You must be signed in to change notification settings - Fork 22
/
Conversions.h
140 lines (124 loc) · 3.75 KB
/
Conversions.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
/*
* Copyright 2012-2020 CNRS-UM LIRMM, CNRS-AIST JRL
*/
#pragma once
#include "SpaceVecAlg"
namespace sva
{
/**
* \addtogroup Conversions Convert to and from sva types
*
* @{
*/
/**
* @brief Generic functions to convert to/from sva types
*
* Each function in the conversions namespace allows to convert to and from
* another type. An important point to note is that sva internally uses a
* left-handed convention. Hence, all functions will assume that the non-sva
* objects use a right-handed convention and convert accordingly. This is
* overridable by the "rightHandedness" argument.
*/
namespace conversions
{
//! Alias for right handedness (default)
constexpr bool RightHanded = true;
//! Alias for left handedness
constexpr bool LeftHanded = false;
/**
* @brief Convert an homogeneous matrix into a Plucker Transform
*
* @return Plucker Transform equivalent to the input homogeneous matrix
* @param m A 4x4 Eigen Matrix
* @param RightHandedness Handedness of the input homogeneous matrix.
* Defaults to right handedness.
*/
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));
}
}
/**
* @brief Convert a Plucker Transform into an homogeneous matrix
*
* @return An Eigen 4x4 homogeneous matrix equivalent to the input Plucker Transform
* @param pt Plucker Transform
* @param RightHandedness Handedness of the output homogeneous matrix.
* Defaults to right handedness.
*/
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;
}
//! Define an Eigen::Affine3<T>
template<typename T>
using affine3_t = Eigen::Transform<T, 3, Eigen::TransformTraits::Affine>;
/**
* @brief Convert an Eigen::Affine3<T> into a Plucker Transform
*
* @return A Plucker Transform of the same type as the input Transform
* @param a Input Eigen Transform. Only rotation and translation components
* are considered.
* @param RightHandedness Handedness of the input Eigen Transform.
* Defaults to right handedness.
*/
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());
}
}
/**
* @brief Convert a Plucker Transform into an Eigen::Affine3<T>
*
* @return An Eigen::Transform of dimension 3, and same scalar type as the input PTransform.
* @param pt Input Plucker Transform.
* @param RightHandedness Handedness of the output Eigen Transform.
* Defaults to right handedness.
*/
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