128 Eigen::Quaternion<Real> & q)
131 if ((1 + O[0][0] + O[1][1] + O[2][2]) > 0)
133 q4 = sqrt(1 + O[0][0] + O[1][1] + O[2][2]) / 2;
135 q.x() = (O[2][1] - O[1][2]) / (4 * q4);
136 q.y() = (O[0][2] - O[2][0]) / (4 * q4);
137 q.z() = (O[1][0] - O[0][1]) / (4 * q4);
139 else if ((1 + O[0][0] - O[1][1] - O[2][2]) > 0)
141 q4 = sqrt(1 + O[0][0] - O[1][1] - O[2][2]) / 2;
142 q.w() = (O[2][1] - O[1][2]) / (4 * q4);
144 q.y() = (O[1][0] + O[0][1]) / (4 * q4);
145 q.z() = (O[0][2] + O[2][0]) / (4 * q4);
147 else if ((1 - O[0][0] + O[1][1] - O[2][2]) > 0)
149 q4 = sqrt(1 - O[0][0] + O[1][1] - O[2][2]) / 2;
150 q.w() = (O[0][2] - O[2][0]) / (4 * q4);
151 q.x() = (O[1][0] + O[0][1]) / (4 * q4);
153 q.z() = (O[2][1] + O[1][2]) / (4 * q4);
155 else if ((1 - O[0][0] - O[1][1] + O[2][2]) > 0)
157 q4 = sqrt(1 - O[0][0] - O[1][1] + O[2][2]) / 2;
158 q.w() = (O[1][0] - O[0][1]) / (4 * q4);
159 q.x() = (O[0][2] + O[2][0]) / (4 * q4);
160 q.y() = (O[2][1] + O[1][2]) / (4 * q4);
173 double sym_rotation[24][3][3] = {
174 {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}, {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}},
175 {{1, 0, 0}, {0, 0, -1}, {0, 1, 0}}, {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}},
176 {{-1, 0, 0}, {0, 1, 0}, {0, 0, -1}}, {{-1, 0, 0}, {0, -1, 0}, {0, 0, 1}},
177 {{-1, 0, 0}, {0, 0, -1}, {0, -1, 0}}, {{-1, 0, 0}, {0, 0, 1}, {0, 1, 0}},
178 {{0, 1, 0}, {-1, 0, 0}, {0, 0, 1}}, {{0, 1, 0}, {0, 0, -1}, {-1, 0, 0}},
179 {{0, 1, 0}, {1, 0, 0}, {0, 0, -1}}, {{0, 1, 0}, {0, 0, 1}, {1, 0, 0}},
180 {{0, -1, 0}, {1, 0, 0}, {0, 0, 1}}, {{0, -1, 0}, {0, 0, -1}, {1, 0, 0}},
181 {{0, -1, 0}, {-1, 0, 0}, {0, 0, -1}}, {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}},
182 {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}}, {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}},
183 {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}, {{0, 0, 1}, {-1, 0, 0}, {0, -1, 0}},
184 {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}, {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}},
185 {{0, 0, -1}, {0, -1, 0}, {-1, 0, 0}}, {{0, 0, -1}, {1, 0, 0}, {0, -1, 0}}};
188 for (
int o = 0; o <
_o_sym; o++)