62 seq.length() ==
static_cast<size_t>(rot.rows()),
63 "Rotation and sequence of rotation must be the same length");
69 for (
size_t i = 0; i < seq.length(); ++i) {
70 utils::Scalar cosVi(std::cos(rot[
static_cast<unsigned int>(i)]));
71 utils::Scalar sinVi(std::sin(rot[
static_cast<unsigned int>(i)]));
75 else if (seq.
tolower()[i] ==
'y')
78 else if (seq.
tolower()[i] ==
'z')
84 out.block(0, 0, 3, 3) = out.block(0, 0, 3, 3) * tp;
90 const std::pair<rigidbody::NodeSegment, rigidbody::NodeSegment>&
92 const std::pair<rigidbody::NodeSegment, rigidbody::NodeSegment>&
94 const std::pair<utils::String, utils::String>& axesNames,
96 if (!axesNames.first.compare(
"") || !axesNames.second.compare(
"")) {
98 "axesNames must be defined with a pair of \"x\", \"y\" or \"z\"");
102 std::vector<size_t> map(3);
103 std::vector<size_t> toMultiply(2);
104 if (!axesNames.first.tolower().compare(
"x")) {
106 if (!axesNames.second.tolower().compare(
"y")) {
117 }
else if (!axesNames.first.tolower().compare(
"y")) {
119 if (!axesNames.second.tolower().compare(
"x")) {
130 }
else if (!axesNames.first.tolower().compare(
"z")) {
132 if (!axesNames.second.tolower().compare(
"x")) {
146 std::vector<utils::Vector3d> axes(3);
147 axes[map[0]] = axis1markers.second - axis1markers.first;
148 axes[map[1]] = axis2markers.second - axis2markers.first;
149 axes[map[2]] = axes[toMultiply[0]].cross(axes[toMultiply[1]]);
152 if (!axisToRecalculate.
tolower().compare(
"x")) {
153 axes[0] = axes[1].cross(axes[2]);
154 }
else if (!axisToRecalculate.
tolower().compare(
"y")) {
155 axes[1] = axes[2].cross(axes[0]);
156 }
else if (!axisToRecalculate.
tolower().compare(
"z")) {
157 axes[2] = axes[0].cross(axes[1]);
162 for (
size_t i = 0; i < 3; ++i) {
163 r_out.block(0,
static_cast<unsigned int>(i), 3, 1) = axes[i];
191 if (!seq.compare(
"zyzz")) {
199 if (!seq.compare(
"x")) {
200 v[0] = std::asin(r(2, 1));
201 }
else if (!seq.compare(
"y")) {
202 v[0] = std::asin(r(0, 2));
203 }
else if (!seq.compare(
"z")) {
204 v[0] = std::asin(r(1, 0));
205 }
else if (!seq.compare(
"xy")) {
206 v[0] = std::asin(r(2, 1));
207 v[1] = std::asin(r(0, 2));
208 }
else if (!seq.compare(
"xz")) {
209 v[0] = -std::asin(r(1, 2));
210 v[1] = -std::asin(r(0, 1));
211 }
else if (!seq.compare(
"yx")) {
212 v[0] = -std::asin(r(2, 0));
213 v[1] = -std::asin(r(1, 2));
214 }
else if (!seq.compare(
"yz")) {
215 v[0] = std::asin(r(0, 2));
216 v[1] = std::asin(r(1, 0));
217 }
else if (!seq.compare(
"zx")) {
218 v[0] = std::asin(r(1, 0));
219 v[1] = std::asin(r(2, 1));
220 }
else if (!seq.compare(
"zy")) {
221 v[0] = -std::asin(r(0, 1));
222 v[1] = -std::asin(r(2, 0));
223 }
else if (!seq.compare(
"xyz")) {
224 v[0] = std::atan2(-r(1, 2), r(2, 2));
225 v[1] = std::asin(r(0, 2));
226 v[2] = std::atan2(-r(0, 1), r(0, 0));
227 }
else if (!seq.compare(
"xzy")) {
228 v[0] = std::atan2(r(2, 1), r(1, 1));
229 v[1] = std::asin(-r(0, 1));
230 v[2] = std::atan2(r(0, 2), r(0, 0));
231 }
else if (!seq.compare(
"yxy")) {
232 v[0] = std::atan2(r(0, 1), r(2, 1));
233 v[1] = std::acos(r(1, 1));
234 v[2] = std::atan2(r(1, 0), -r(1, 2));
235 }
else if (!seq.compare(
"yxz")) {
236 v[0] = std::atan2(r(0, 2), r(2, 2));
237 v[1] = std::asin(-r(1, 2));
238 v[2] = std::atan2(r(1, 0), r(1, 1));
239 }
else if (!seq.compare(
"yzx")) {
240 v[0] = std::atan2(-r(2, 0), r(0, 0));
241 v[1] = std::asin(r(1, 0));
242 v[2] = std::atan2(-r(1, 2), r(1, 1));
243 }
else if (!seq.compare(
"yzy")) {
244 v[0] = std::atan2(r(2, 1), -r(0, 1));
245 v[1] = std::acos(r(1, 1));
246 v[2] = std::atan2(r(1, 2), r(1, 0));
247 }
else if (!seq.compare(
"zxy")) {
248 v[0] = std::atan2(-r(0, 1), r(1, 1));
249 v[1] = std::asin(r(2, 1));
250 v[2] = std::atan2(-r(2, 0), r(2, 2));
251 }
else if (!seq.compare(
"zyx")) {
252 v[0] = std::atan2(r(1, 0), r(0, 0));
253 v[1] = std::asin(-r(2, 0));
254 v[2] = std::atan2(r(2, 1), r(2, 2));
255 }
else if (!seq.compare(
"zxz")) {
256 v[0] = std::atan2(r(0, 2), -r(1, 2));
257 v[1] = std::acos(r(2, 2));
258 v[2] = std::atan2(r(2, 0), r(2, 1));
259 }
else if (!seq.compare(
"zyz")) {
260 v[0] = std::atan2(r(1, 2), r(0, 2));
261 v[1] = std::acos(r(2, 2));
262 v[2] = std::atan2(r(2, 1), -r(2, 0));
263 }
else if (!seq.compare(
"zyzz")) {
264 v[0] = std::atan2(r(1, 2), r(0, 2));
265 v[1] = std::acos(r(2, 2));
266 v[2] = std::atan2(r(2, 1), -r(2, 0)) + v[0];