16 #ifndef SURGSIM_MATH_MATHCONVERT_INL_H
17 #define SURGSIM_MATH_MATHCONVERT_INL_H
25 const std::string rotationPropertyName =
"Quaternion";
26 const std::string translationPropertyName =
"Translation";
31 template <
class Type,
int Rows,
int Cols,
int MOpt>
32 YAML::Node YAML::convert<typename Eigen::Matrix<Type, Rows, Cols, MOpt>>::encode(
33 const typename Eigen::Matrix<Type, Rows, Cols, MOpt>& rhs)
35 typedef typename Eigen::Matrix<Type, Rows, Cols, MOpt>::Index Index;
37 node.SetStyle(YAML::EmitterStyle::Flow);
40 for (Index i = 0; i < rhs.size(); ++i)
42 node.push_back(rhs(i, 0));
47 for (
size_t row = 0; row < Rows; ++row)
50 for (
size_t col = 0; col < Cols; ++col)
52 rowNode.push_back(rhs.row(row)[col]);
54 node.push_back(rowNode);
62 template <
class Type,
int Rows,
int Cols,
int MOpt>
63 bool YAML::convert<typename Eigen::Matrix<Type, Rows, Cols, MOpt>>::decode(
65 typename Eigen::Matrix<Type, Rows, Cols, MOpt>& rhs)
67 if (! node.IsSequence() || node.size() != Rows)
74 for (
size_t i = 0; i < node.size(); ++i)
78 rhs(i, 0) = node[i].as<Type>();
80 catch (YAML::RepresentationException)
82 rhs(i, 0) = std::numeric_limits<Type>::quiet_NaN();
85 SURGSIM_LOG(logger, WARNING) <<
"Bad conversion, using #NaN value. For node: " << node;
91 for (
size_t row = 0; row < node.size(); ++row)
93 YAML::Node rowNode = node[row];
94 if (!rowNode.IsSequence() || node.size() != Cols)
98 for (
size_t col = 0; col < rowNode.size(); ++col)
102 rhs.row(row)[col] = rowNode[col].as<Type>();
104 catch (YAML::RepresentationException)
106 rhs.row(row)[col] = std::numeric_limits<Type>::quiet_NaN();
108 SURGSIM_LOG(logger, WARNING) <<
"Bad conversion, using #NaN value. For node: " << node;
117 template <
class Type,
int QOpt>
118 YAML::Node YAML::convert<typename Eigen::Quaternion<Type, QOpt>>::encode(
119 const typename Eigen::Quaternion<Type, QOpt>& rhs)
121 return Node(
convert<
typename Eigen::Matrix<Type, 4, 1, QOpt>>::encode(rhs.coeffs()));
125 template <
class Type,
int QOpt>
126 bool YAML::convert<typename Eigen::Quaternion<Type, QOpt>>::decode(
128 typename Eigen::Quaternion<Type, QOpt>& rhs)
131 if (node.IsSequence() && node.size() == 4)
133 result = convert<typename Eigen::Matrix<Type, 4, 1, QOpt>>::decode(node, rhs.coeffs());
137 Eigen::AngleAxis<Type> angleAxis;
138 result = convert<typename Eigen::AngleAxis<Type>>::decode(node, angleAxis);
148 template <
class Type,
int Dim,
int TMode,
int TOptions>
149 YAML::Node YAML::convert<typename Eigen::Transform<Type, Dim, TMode, TOptions>>::encode(
150 const typename Eigen::Transform<Type, Dim, TMode, TOptions>& rhs)
152 typedef typename Eigen::Transform<Type, Dim, TMode, TOptions>::LinearMatrixType LinearMatrixType;
153 LinearMatrixType linear(rhs.linear());
154 Eigen::Quaternion<Type, TOptions> quaternion(linear);
155 Eigen::Matrix<Type, Dim, 1, TOptions> translation(rhs.translation());
158 node[rotationPropertyName] = quaternion;
159 node[translationPropertyName] = translation;
164 template <
class Type,
int Dim,
int TMode,
int TOptions>
165 bool YAML::convert<typename Eigen::Transform<Type, Dim, TMode, TOptions>>::decode(
167 typename Eigen::Transform<Type, Dim, TMode, TOptions>& rhs)
174 Eigen::Quaternion<Type, TOptions> rotation(Eigen::Quaternion<Type, TOptions>::Identity());
175 Eigen::Matrix<Type, Dim, 1, TOptions> translation(Eigen::Matrix<Type, Dim, 1, TOptions>::Zero());
176 if (node[rotationPropertyName].IsDefined())
178 rotation = node[rotationPropertyName].as<Eigen::Quaternion<Type, TOptions>>();
181 if (node[translationPropertyName].IsDefined())
183 translation = node[translationPropertyName].as<Eigen::Matrix<Type, Dim, 1, TOptions>>();
187 rhs.linear() = rotation.matrix();
188 rhs.translation() = translation;
194 template <
class Type>
195 YAML::Node YAML::convert<typename Eigen::AngleAxis<Type>>::encode(
196 const typename Eigen::AngleAxis<Type>& rhs)
199 node.SetStyle(EmitterStyle::Flow);
200 node[
"Angle"] = rhs.angle();
201 node[
"Axis"] = convert<typename Eigen::Matrix<Type, 3, 1>>::encode(rhs.axis());
206 template <
class Type>
207 bool YAML::convert<typename Eigen::AngleAxis<Type>>::decode(
209 typename Eigen::AngleAxis<Type>& rhs)
212 if (node.IsMap() && node[
"Angle"].IsDefined() && node[
"Axis"].IsDefined())
216 rhs.angle() = node[
"Angle"].as<Type>();
218 catch (RepresentationException)
220 rhs.angle() = std::numeric_limits<Type>::quiet_NaN();
222 SURGSIM_LOG(logger, WARNING) <<
"Bad conversion, using #NaN value. For node: " << node;
224 result = convert<typename Eigen::Matrix<Type, 3, 1>>::decode(node[
"Axis"], rhs.axis());
229 #endif // SURGSIM_MATH_MATHCONVERT_INL_H