336 typename MatType::value_type eps =
static_cast<typename MatType::value_type
>(1.0e-8))
338 using ValueType =
typename MatType::value_type;
340 ValueType phi, theta, psi;
342 switch(rotationOrder)
346 theta = ValueType(math::pi<double>() / 2.0);
347 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
350 theta = ValueType(-math::pi<double>() / 2.0);
351 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
354 psi = ValueType(atan2(-mat[1][0],mat[0][0]));
355 phi = ValueType(atan2(-mat[2][1],mat[2][2]));
356 theta = ValueType(atan2(mat[2][0],
357 sqrt( mat[2][1]*mat[2][1] +
358 mat[2][2]*mat[2][2])));
360 return V(phi, theta, psi);
363 theta = ValueType(math::pi<double>() / 2.0);
364 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
367 theta = ValueType(-math::pi<double>() / 2.0);
368 phi = ValueType(0.5 * atan2(mat[0][1],mat[2][1]));
371 psi = ValueType(atan2(-mat[0][2], mat[2][2]));
372 phi = ValueType(atan2(-mat[1][0], mat[1][1]));
373 theta = ValueType(atan2(mat[1][2],
374 sqrt(mat[0][2] * mat[0][2] +
375 mat[2][2] * mat[2][2])));
377 return V(theta, psi, phi);
381 theta = ValueType(math::pi<double>() / 2.0);
382 phi = ValueType(0.5 * atan2(mat[2][0], mat[2][2]));
385 theta = ValueType(-math::pi<double>() / 2.0);
386 phi = ValueType(0.5 * atan2(mat[2][0], mat[1][0]));
389 psi = ValueType(atan2(-mat[2][1], mat[1][1]));
390 phi = ValueType(atan2(-mat[0][2], mat[0][0]));
391 theta = ValueType(atan2(mat[0][1],
392 sqrt(mat[0][0] * mat[0][0] +
393 mat[0][2] * mat[0][2])));
395 return V(psi, phi, theta);
400 theta = ValueType(0.0);
401 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
404 theta = ValueType(math::pi<double>());
405 psi = ValueType(0.5 * atan2(mat[2][1], -mat[1][1]));
408 psi = ValueType(atan2(mat[2][0], -mat[1][0]));
409 phi = ValueType(atan2(mat[0][2], mat[0][1]));
410 theta = ValueType(atan2(sqrt(mat[0][1] * mat[0][1] +
411 mat[0][2] * mat[0][2]),
414 return V(phi, psi, theta);
419 theta = ValueType(0.0);
420 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
423 theta = ValueType(math::pi<double>());
424 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
427 psi = ValueType(atan2(mat[0][2], mat[1][2]));
428 phi = ValueType(atan2(mat[2][0], -mat[2][1]));
429 theta = ValueType(atan2(sqrt(mat[0][2] * mat[0][2] +
430 mat[1][2] * mat[1][2]),
433 return V(theta, psi, phi);
438 theta = ValueType(-math::pi<double>() / 2.0);
439 phi = ValueType(0.5 * atan2(-mat[1][0], mat[0][0]));
442 theta = ValueType(math::pi<double>() / 2.0);
443 phi = ValueType(0.5 * atan2(mat[1][0], mat[0][0]));
446 psi = ValueType(atan2(mat[0][1], mat[1][1]));
447 phi = ValueType(atan2(mat[2][0], mat[2][2]));
448 theta = ValueType(atan2(-mat[2][1],
449 sqrt(mat[0][1] * mat[0][1] +
450 mat[1][1] * mat[1][1])));
452 return V(theta, phi, psi);
457 theta = ValueType(-math::pi<double>() / 2.0);
458 phi = ValueType(0.5 * atan2(-mat[1][0], mat[1][1]));
461 theta = ValueType(math::pi<double>() / 2.0);
462 phi = ValueType(0.5 * atan2(mat[2][1], mat[2][0]));
465 psi = ValueType(atan2(mat[1][2], mat[2][2]));
466 phi = ValueType(atan2(mat[0][1], mat[0][0]));
467 theta = ValueType(atan2(-mat[0][2],
468 sqrt(mat[0][1] * mat[0][1] +
469 mat[0][0] * mat[0][0])));
471 return V(psi, theta, phi);
476 theta = ValueType(math::pi<double>() / 2.0);
477 psi = ValueType(0.5 * atan2(mat[2][1], mat[2][2]));
480 theta = ValueType(-math::pi<double>() / 2.0);
481 psi = ValueType(0.5 * atan2(- mat[2][1], mat[2][2]));
484 psi = ValueType(atan2(mat[2][0], mat[0][0]));
485 phi = ValueType(atan2(mat[1][2], mat[1][1]));
486 theta = ValueType(atan2(- mat[1][0],
487 sqrt(mat[1][1] * mat[1][1] +
488 mat[1][2] * mat[1][2])));
490 return V(phi, psi, theta);