Multiply quaternion(s) q with quaternion(s) r. Expects two equally-sized tensors of shape (*, 4), where * denotes any number of dimensions. Returns q*r as a tensor of shape (*, 4).
(q, r)
| 33 | |
| 34 | |
| 35 | def qmul(q, r): |
| 36 | """ |
| 37 | Multiply quaternion(s) q with quaternion(s) r. |
| 38 | Expects two equally-sized tensors of shape (*, 4), |
| 39 | where * denotes any number of dimensions. |
| 40 | Returns q*r as a tensor of shape (*, 4). |
| 41 | """ |
| 42 | assert q.shape[-1] == 4 |
| 43 | assert r.shape[-1] == 4 |
| 44 | |
| 45 | original_shape = q.shape |
| 46 | |
| 47 | # Compute outer product |
| 48 | terms = torch.bmm(r.view(-1, 4, 1), q.view(-1, 1, 4)) |
| 49 | |
| 50 | w = terms[:, 0, 0] - terms[:, 1, 1] - terms[:, 2, 2] - terms[:, 3, 3] |
| 51 | x = terms[:, 0, 1] + terms[:, 1, 0] - terms[:, 2, 3] + terms[:, 3, 2] |
| 52 | y = terms[:, 0, 2] + terms[:, 1, 3] + terms[:, 2, 0] - terms[:, 3, 1] |
| 53 | z = terms[:, 0, 3] - terms[:, 1, 2] + terms[:, 2, 1] + terms[:, 3, 0] |
| 54 | return torch.stack((w, x, y, z), dim=1).view(original_shape) |
| 55 | |
| 56 | |
| 57 | def qrot(q, v): |
no outgoing calls
no test coverage detected