Spaces:
Building
Building
File size: 8,064 Bytes
d49f7bc |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 |
# Copyright (c) Meta Platforms, Inc. and affiliates.
# This source code is licensed under the MIT license found in the
# LICENSE file in the root directory of this source tree.
from __future__ import annotations # so we can refer to class Type inside class
import numpy as np
import numpy.typing as npt
import logging
from typing import Union, Iterable, List, Tuple
from animated_drawings.model.vectors import Vectors
import math
from animated_drawings.utils import TOLERANCE
from functools import reduce
class Quaternions:
"""
Wrapper class around ndarray interpreted as one or more quaternions. Quaternion order is [w, x, y, z]
When passing in existing Quaternions, new Quaternions object will share the underlying nparray, so be careful.
Strongly influenced by Daniel Holden's excellent Quaternions class.
"""
def __init__(self, qs: Union[Iterable[Union[int, float]], npt.NDArray[np.float32], Quaternions]) -> None:
self.qs: npt.NDArray[np.float32]
if isinstance(qs, np.ndarray):
if not qs.shape[-1] == 4:
msg = f'Final dimension passed to Quaternions must be 4. Found {qs.shape[-1]}'
logging.critical(msg)
assert False, msg
if len(qs.shape) == 1:
qs = np.expand_dims(qs, axis=0)
self.qs = qs
elif isinstance(qs, tuple) or isinstance(qs, list):
try:
qs = np.array(qs)
assert qs.shape[-1] == 4
except Exception:
msg = 'Could not convert quaternion data to ndarray with shape[-1] == 4'
logging.critical(msg)
assert False, msg
if len(qs.shape) == 1:
qs = np.expand_dims(qs, axis=0)
self.qs = qs
elif isinstance(qs, Quaternions):
self.qs = qs.qs
else:
msg = 'Quaternions must be constructed from Quaternions or numpy array'
logging.critical(msg)
assert False, msg
self.normalize()
def normalize(self) -> None:
self.qs = self.qs / np.expand_dims(np.sum(self.qs ** 2.0, axis=-1) ** 0.5, axis=-1)
def to_rotation_matrix(self) -> npt.NDArray[np.float32]:
"""
From Ken Shoemake
https://www.ljll.math.upmc.fr/~frey/papers/scientific%20visualisation/Shoemake%20K.,%20Quaternions.pdf
:return: 4x4 rotation matrix representation of quaternions
"""
w = self.qs[..., 0].squeeze()
x = self.qs[..., 1].squeeze()
y = self.qs[..., 2].squeeze()
z = self.qs[..., 3].squeeze()
xx, yy, zz = x**2, y**2, z**2
wx, wy, wz = w*x, w*y, w*z
xy, xz = x*y, x*z # no
yz = y*z
# Row 1
r00 = 1 - 2 * (yy + zz)
r01 = 2 * (xy - wz)
r02 = 2 * (xz + wy)
# Row 2
r10 = 2 * (xy + wz)
r11 = 1 - 2 * (xx + zz)
r12 = 2 * (yz - wx)
# Row 3
r20 = 2 * (xz - wy)
r21 = 2 * (yz + wx)
r22 = 1 - 2 * (xx + yy)
return np.array([[r00, r01, r02, 0.0],
[r10, r11, r12, 0.0],
[r20, r21, r22, 0.0],
[0.0, 0.0, 0.0, 1.0]], dtype=np.float32)
@classmethod
def rotate_between_vectors(cls, v1: Vectors, v2: Vectors) -> Quaternions:
""" Computes quaternion rotating from v1 to v2. """
xyz: List[float] = v1.cross(v2).vs.squeeze().tolist()
w: float = math.sqrt((v1.length**2) * (v2.length**2)) + np.dot(v1.vs.squeeze(), v2.vs.squeeze())
ret_q = Quaternions([w, *xyz])
ret_q.normalize()
return ret_q
@classmethod
def from_angle_axis(cls, angles: npt.NDArray[np.float32], axes: Vectors) -> Quaternions:
axes.norm()
if len(angles.shape) == 1:
angles = np.expand_dims(angles, axis=0)
ss = np.sin(angles / 2.0)
cs = np.cos(angles / 2.0)
return Quaternions(np.concatenate([cs, axes.vs * ss], axis=-1))
@classmethod
def identity(cls, ret_shape: Tuple[int]) -> Quaternions:
qs = np.broadcast_to(np.array([1.0, 0.0, 0.0, 0.0]), [*ret_shape, 4])
return Quaternions(qs)
@classmethod
def from_euler_angles(cls, order: str, angles: npt.NDArray[np.float32]) -> Quaternions:
"""
Applies a series of euler angle rotations. Angles applied from right to left
:param order: string comprised of x, y, and/or z
:param angles: angles in degrees
"""
if len(angles.shape) == 1:
angles = np.expand_dims(angles, axis=0)
if len(order) != angles.shape[-1]:
msg = 'length of orders and angles does not match'
logging.critical(msg)
assert False, msg
_quats = [Quaternions.identity(angles.shape[:-1])]
for axis_char, pos in zip(order, range(len(order))):
angle = angles[..., pos] * np.pi / 180
angle = np.expand_dims(angle, axis=1)
axis_char = axis_char.lower()
if axis_char not in 'xyz':
msg = f'order contained unsupported char:{axis_char}'
logging.critical(msg)
assert False, msg
axis = np.zeros([*angles.shape[:-1], 3])
axis[..., ord(axis_char) - ord('x')] = 1.0
_quats.insert(0, Quaternions.from_angle_axis(angle, Vectors(axis)))
ret_q = reduce(lambda a, b: b * a, _quats)
return ret_q
@classmethod
def from_rotation_matrix(cls, M: npt.NDArray[np.float32]) -> Quaternions:
"""
As described here: https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2015/01/matrix-to-quat.pdf
"""
is_orthogonal = np.isclose(M @ M.T, np.identity(4), atol=TOLERANCE)
if not is_orthogonal.all():
msg = "attempted to create quaternion from non-orthogonal rotation matrix"
logging.critical(msg)
assert False, msg
if not np.isclose(np.linalg.det(M), 1.0):
msg = "attempted to create quaternion from rotation matrix with det != 1"
logging.critical(msg)
assert False, msg
# Note: Mike Day's article uses row vectors, whereas we used column, so here use transpose of matrix
MT = M.T
m00, m01, m02 = MT[0, 0], MT[0, 1], MT[0, 2]
m10, m11, m12 = MT[1, 0], MT[1, 1], MT[1, 2]
m20, m21, m22 = MT[2, 0], MT[2, 1], MT[2, 2]
if m22 < 0:
if m00 > m11:
t = 1 + m00 - m11 - m22
q = np.array([m12-m21, t, m01+m10, m20+m02])
else:
t = 1 - m00 + m11 - m22
q = np.array([m20-m02, m01+m10, t, m12+m21])
else:
if m00 < -m11:
t = 1 - m00 - m11 + m22
q = np.array([m01-m10, m20+m02, m12+m21, t])
else:
t = 1 + m00 + m11 + m22
q = np.array([ t, m12-m21, m20-m02, m01-m10])
q *= (0.5 / math.sqrt(t))
ret_q = Quaternions(q)
ret_q.normalize()
return ret_q
def __mul__(self, other: Quaternions):
"""
From https://danceswithcode.net/engineeringnotes/quaternions/quaternions.html
"""
s0 = self.qs[..., 0]
s1 = self.qs[..., 1]
s2 = self.qs[..., 2]
s3 = self.qs[..., 3]
r0 = other.qs[..., 0]
r1 = other.qs[..., 1]
r2 = other.qs[..., 2]
r3 = other.qs[..., 3]
t = np.empty(self.qs.shape)
t[..., 0] = r0*s0 - r1*s1 - r2*s2 - r3*s3
t[..., 1] = r0*s1 + r1*s0 - r2*s3 + r3*s2
t[..., 2] = r0*s2 + r1*s3 + r2*s0 - r3*s1
t[..., 3] = r0*s3 - r1*s2 + r2*s1 + r3*s0
return Quaternions(t)
def __neg__(self):
return Quaternions(self.qs * np.array([1, -1, -1, -1]))
def __str__(self):
return f"Quaternions({str(self.qs)})"
def __repr__(self):
return f"Quaternions({str(self.qs)})"
|