Rotate vector(s) v about the rotation described by quaternion(s) q. Expects a tensor of shape (*, 4) for q and a tensor of shape (*, 3) for v, where * denotes any number of dimensions. Returns a tensor of shape (*, 3).
(q, v)
| 45 | |
| 46 | |
| 47 | def qrot(q, v): |
| 48 | """ |
| 49 | Rotate vector(s) v about the rotation described by quaternion(s) q. |
| 50 | Expects a tensor of shape (*, 4) for q and a tensor of shape (*, 3) for v, |
| 51 | where * denotes any number of dimensions. |
| 52 | Returns a tensor of shape (*, 3). |
| 53 | """ |
| 54 | assert q.shape[-1] == 4 |
| 55 | assert v.shape[-1] == 3 |
| 56 | assert q.shape[:-1] == v.shape[:-1] |
| 57 | |
| 58 | original_shape = list(v.shape) |
| 59 | # print(q.shape) |
| 60 | q = q.contiguous().view(-1, 4) |
| 61 | v = v.contiguous().view(-1, 3) |
| 62 | |
| 63 | qvec = q[:, 1:] |
| 64 | uv = torch.cross(qvec, v, dim=1) |
| 65 | uuv = torch.cross(qvec, uv, dim=1) |
| 66 | return (v + 2 * (q[:, :1] * uv + uuv)).view(original_shape) |
| 67 | |
| 68 | |
| 69 | def recover_root_rot_pos(data): |
no outgoing calls
no test coverage detected