| 745 | } |
| 746 | |
| 747 | void |
| 748 | quat_to_angle_axis (const Eigen::Quaternionf &qx, double &theta, double axis[3]) |
| 749 | { |
| 750 | double q[4]; |
| 751 | q[0] = qx.w(); |
| 752 | q[1] = qx.x(); |
| 753 | q[2] = qx.y(); |
| 754 | q[3] = qx.z(); |
| 755 | |
| 756 | double halftheta = std::acos (q[0]); |
| 757 | theta = halftheta * 2; |
| 758 | double sinhalftheta = sin (halftheta); |
| 759 | if (feq (halftheta, 0)) { |
| 760 | axis[0] = 0; |
| 761 | axis[1] = 0; |
| 762 | axis[2] = 1; |
| 763 | theta = 0; |
| 764 | } else { |
| 765 | axis[0] = q[1] / sinhalftheta; |
| 766 | axis[1] = q[2] / sinhalftheta; |
| 767 | axis[2] = q[3] / sinhalftheta; |
| 768 | } |
| 769 | } |
| 770 | |
| 771 | |
| 772 | ///////////////////////////////////////////////////////////////////////////////////////////// |
no test coverage detected