Convert Euler angles to quaternions.
(e, order)
| 231 | |
| 232 | |
| 233 | def euler_to_quaternion(e, order): |
| 234 | """ |
| 235 | Convert Euler angles to quaternions. |
| 236 | """ |
| 237 | assert e.shape[-1] == 3 |
| 238 | |
| 239 | original_shape = list(e.shape) |
| 240 | original_shape[-1] = 4 |
| 241 | |
| 242 | e = e.reshape(-1, 3) |
| 243 | |
| 244 | x = e[:, 0] |
| 245 | y = e[:, 1] |
| 246 | z = e[:, 2] |
| 247 | |
| 248 | rx = np.stack((np.cos(x / 2), np.sin(x / 2), np.zeros_like(x), np.zeros_like(x)), axis=1) |
| 249 | ry = np.stack((np.cos(y / 2), np.zeros_like(y), np.sin(y / 2), np.zeros_like(y)), axis=1) |
| 250 | rz = np.stack((np.cos(z / 2), np.zeros_like(z), np.zeros_like(z), np.sin(z / 2)), axis=1) |
| 251 | |
| 252 | result = None |
| 253 | for coord in order: |
| 254 | if coord == 'x': |
| 255 | r = rx |
| 256 | elif coord == 'y': |
| 257 | r = ry |
| 258 | elif coord == 'z': |
| 259 | r = rz |
| 260 | else: |
| 261 | raise |
| 262 | if result is None: |
| 263 | result = r |
| 264 | else: |
| 265 | result = qmul_np(result, r) |
| 266 | |
| 267 | # Reverse antipodal representation to have a non-negative "w" |
| 268 | if order in ['xyz', 'yzx', 'zxy']: |
| 269 | result *= -1 |
| 270 | |
| 271 | return result.reshape(original_shape) |
| 272 | |
| 273 | |
| 274 | def quaternion_to_matrix(quaternions): |