def quaternion_integrate(q, rate: np.ndarray, timestep: double):
"""Advance a time varying quaternion to its value at a time `timestep` in the future.
The Quaternion object will be modified to its future value.
It is guaranteed to remain a unit quaternion.
Params:
rate: np 3-array (or array-like) describing rotation rates about the
global x, y and z axes respectively.
timestep: interval over which to integrate into the future.
Assuming *now* is `T=0`, the integration occurs over the interval
`T=0` to `T=timestep`. Smaller intervals are more accurate when
`rate` changes over time.
Note:
The solution is closed form given the assumption that `rate` is constant
over the interval of length `timestep`.
"""
q = quaternion_normalize(q)
rotation_vector = rate * timestep
rotation_norm = norm(rotation_vector)
if rotation_norm > 0:
axis = rotation_vector / rotation_norm
angle = rotation_norm
q2 = quaternion_from_axis_angle(axis=axis, angle=angle)
return quaternion_normalize(quaternion_multiply(q, q2))
else:
raise ValueError("Rotation norm is null!")
def quaternion_axis(q, undefined=origin):
"""Get the axis or vector about which the quaternion rotation occurs
For a null rotation (a purely real quaternion), the rotation angle will
always be `0`, but the rotation axis is undefined.
It is by default assumed to be `[0, 0, 0]`.
Params:
undefined: [optional] specify the axis vector that should define a null rotation.
This is geometrically meaningless, and could be any of an infinite set of vectors,
but can be specified if the default (`[0, 0, 0]`) causes undesired behaviour.
Returns:
A Numpy unit 3-vector describing the Quaternion object's axis of rotation.
Note:
This feature only makes sense when referring to a unit quaternion.
Calling this method will implicitly normalise the Quaternion object to a unit quaternion if it is not
already one.
"""
tolerance = 1e-17
q = quaternion_normalize(q)
v = q[1:4]
n = norm(v)
if n < tolerance:
# Here there are an infinite set of possible axes, use what has been specified as an undefined axis.
return undefined
else:
return v / n
def shear_from_matrix(matrix):
"""Return shear angle, direction and plane from shear matrix.
>>> angle = (np.random.random() - 0.5) * 4*pi
>>> direct = np.random.random(3) - 0.5
>>> point = np.random.random(3) - 0.5
>>> normal = np.cross(direct, np.random.random(3))
>>> S0 = shear_matrix(angle, direct, point, normal)
>>> angle, direct, point, normal = shear_from_matrix(S0)
>>> S1 = shear_matrix(angle, direct, point, normal)
>>> is_same_transform(S0, S1, eps=1e-7)
True
"""
M = matrix
M33 = M[:3, :3]
# normal: cross independent eigenvectors corresponding to the eigenvalue 1
w, V = np.linalg.eig(M33)
i = np.where(np.abs(np.real(w) - 1.0) < 1e-4)[0]
if len(i) < 2:
raise ValueError("no two linear independent eigenvectors found")
V = np.real(V[:, i]).squeeze().T
lenorm = -1.0
normal = None
for i0, i1 in ((0, 1), (0, 2), (1, 2)):
n = np.cross(V[i0], V[i1])
w = norm(n)
if w > lenorm:
lenorm = w
normal = n
if normal is None:
raise ValueError("Can not find normal vector!")
normal /= lenorm
# direction and angle
direction = np.dot(M33 - np.identity(3), normal)
angle = norm(direction)
direction /= angle
angle = atan(angle)
# point: eigenvector corresponding to eigenvalue 1
w, V = np.linalg.eig(M)
i = np.where(abs(np.real(w) - 1.0) < 1e-8)[0]
if not len(i):
raise ValueError("no eigenvector corresponding to eigenvalue 1")
point = np.real(V[:, i[-1]]).squeeze()
point /= point[3]
return angle, direction, point, normal
def quaternion_normalize(q: quaternion) -> quaternion:
"""Object is guaranteed to be a unit quaternion after calling this
operation UNLESS the object is equivalent to Quaternion(0)
"""
n = norm(q)
if n > 0:
return q / n
else:
raise ZeroDivisionError("Can not normalize null quaternion!")
def vector_to_euler(v: np.ndarray):
"""
Return roll, pitch and yaw for given offset vector
:param v: The offset vector in cartesian coordinates
:return: the pitch and yaw angles to match vector. Roll is always 0
"""
v = v / norm(v)
yaw = np.arctan2(v[1], v[0])
pitch = np.arcsin(v[2])
return vector(0.0, pitch, yaw)
def arcball_constrain_to_axis(point, axis):
"""Return sphere point perpendicular to axis."""
v = np.array(point, dtype=np.float64, copy=True)
a = np.array(axis, dtype=np.float64, copy=True)
v -= a * np.dot(a, v) # on plane
n = norm(v)
if n > EPS:
if v[2] < 0.0:
np.negative(v, v)
v /= n
return v
if a[2] == 1.0:
return vector(1.0, 0.0, 0.0)
return unit_vector(vector(-a[1], a[0], 0.0))
def rotation_to_match(target_vector, actual_vector):
"""
Construct rotation matrix that would bring actual_vector to coincide with target_vector.
Both input vectors must be normalized!!!!
Works very reliably for all tested datasets
:param target_vector: 3-vector of where we want to appear
:param actual_vector: 3-vector of where we are
:return: 3x3 rotation matrix
"""
# TODO: rewrite with quaternions?
print(f"actual = {actual_vector}, target={target_vector}")
# Check if we even need to do anything =)
if np.allclose(target_vector, actual_vector):
return np.eye(3)
# an off-plane vector is needed for rotations, lets make it using cross product
zz = np.cross(actual_vector, target_vector)
print("zz=", zz)
# check for gimbal lock =)
if np.allclose(zz, np.zeros_like(zz)):
print("No cross product can be found!!!")
# luckily, the only case when cross product is zero corresponds to reflection
# the case of noop is taken care of previously
rmd = reflection_matrix(np.zeros(3), actual_vector)[:3, :3]
else:
# normalize the off-plane vector
zz = zz / norm(zz)
# Create a 90-degree rotation matrix using off-plane vector to construct new Y axis
rz = rotation_matrix(np.pi / 2, zz)[:3, :3]
# construct the new "y" axis using rotation of actual_vector
q1 = rz @ actual_vector
# plot_line(ax,np.zeros(3), q1, label="q1", linestyle=":", color="black")
rm0 = np.vstack((actual_vector, q1, zz)).T
# construct the new "y" axis using rotation of target_vector
q2 = rz @ target_vector
# plot_line(ax,np.zeros(3), q2, label="q2", linestyle=":", color="green")
rm1 = np.vstack((target_vector, q2, zz)).T
# Difference between rotations will be what we actually need=)
rmd = rm1 @ rm0.T
return rmd
def scale_from_matrix(matrix: np.ndarray) -> Tuple[float, np.ndarray, np.ndarray]:
"""Return scaling factor, origin and direction from scaling matrix.
>>> factor = np.random.random() * 10 - 5
>>> origin = np.random.random(3) - 0.5
>>> direct = np.random.random(3) - 0.5
>>> S0 = scale_matrix(factor, origin)
>>> factor, origin, direction = scale_from_matrix(S0)
>>> S1 = scale_matrix(factor, origin, direction)
>>> is_same_transform(S0, S1)
True
>>> S0 = scale_matrix(factor, origin, direct)
>>> factor, origin, direction = scale_from_matrix(S0)
>>> S1 = scale_matrix(factor, origin, direction)
>>> is_same_transform(S0, S1)
True
"""
M33 = matrix[:3, :3]
factor: float = np.trace(M33) - 2.0
# direction: unit eigenvector corresponding to eigenvalue factor
w, V = np.linalg.eig(M33)
q = np.where(np.abs(np.real(w) - factor) < 1e-8)[0]
if len(q):
i = q[0]
direction = np.real(V[:, i]).flatten()[0:3]
direction /= norm(direction)
else:
# uniform scaling
factor = (factor + 2.0) / 3.0
direction = _origin
# origin: any eigenvector corresponding to eigenvalue 1
w, V = np.linalg.eig(matrix)
i = np.where(np.abs(np.real(w) - 1.0) < 1e-8)[0]
if not len(i):
raise ValueError("no eigenvector corresponding to eigenvalue 1")
origin = np.real(V[:, i[-1]]).flatten()
origin = (origin / origin[3])[0:3]
return factor, origin, direction
def pos_around_hex(x1: float,
y1: float,
r_min: float,
r_max: float,
num_points: int = 1):
"""
Generate random uniform points in a hex centered around x1, y1
This can make infinite-ish loop if r_min is very close to r_max
:param x1: center location (float in meters)
:param y1: center location (float in meters)
:param r_min: minimal radial distance from hex center (float in meters, can be zero)
:param r_max: maximal radial distance from hex center (float in meters)
:param num_points: total number of points to make. Will return a generator.
:return: generator making the points.
"""
# 3 constant vectors forming possible basis pairs 120 degrees apart
basis = np.array([(-1., 0), (.5, sqrt(3.) / 2.),
(.5, -sqrt(3.) / 2.)]) * r_max
assert r_max > 0
assert r_max > r_min
if num_points == 0:
return
while num_points > 0:
# Choose random "sector" of 120 degrees
q = rng.randint(0, 3)
V = np.stack((basis[q], basis[(q + 1) % 3]))
# Choose a random point on a square
W = np.random.rand(2)
# Transform the square onto the basis formed by sector in V
val = np.dot(W, V)
# check that we are not too close to the center
if norm(val) > r_min:
num_points -= 1
yield val + (x1, y1)
def quaternion_angle(q: np.ndarray):
"""Get the angle (in radians) describing the magnitude of the quaternion rotation about its rotation axis.
This is guaranteed to be within the range (-pi:pi) with the direction of
rotation indicated by the sign.
When a particular rotation describes a 180 degree rotation about an arbitrary
axis vector `v`, the conversion to axis / angle representation may jump
discontinuously between all permutations of `(-pi, pi)` and `(-v, v)`,
each being geometrically equivalent (see Note in documentation).
Returns:
A real number in the range (-pi:pi) describing the angle of rotation
in radians about a Quaternion object's axis of rotation.
Note:
This feature only makes sense when referring to a unit quaternion.
Calling this method will implicitly normalise the Quaternion object to a unit quaternion if
it is not already one.
"""
q = quaternion_normalize(q)
n = norm(q[1:4])
return wrap_angle(2.0 * atan2(n, q[0]))
def __init__(self, initial=None):
"""Initialize virtual trackball control.
initial : quaternion or rotation matrix
"""
self._axis = None
self._axes = None
self._radius = 1.0
self._center = [0.0, 0.0]
self._vdown = np.array([0.0, 0.0, 1.0])
self._constrain = False
if initial is None:
self._qdown = quaternion_from_elements(1.0, 0.0, 0.0, 0.0)
else:
initial = np.array(initial, dtype=np.float64)
if initial.shape == (4, 4):
self._qdown = quaternion_from_matrix(initial)
elif initial.shape == (4, ):
initial /= norm(initial)
self._qdown = initial
else:
raise ValueError("initial not a quaternion or matrix")
self._qnow = self._qpre = self._qdown
def affine_matrix_from_points(v0, v1, shear=True, scale=True, usesvd=True):
"""Return affine transform matrix to register two point sets.
v0 and v1 are shape (ndims, \*) arrays of at least ndims non-homogeneous
coordinates, where ndims is the dimensionality of the coordinate space.
If shear is False, a similarity transformation matrix is returned.
If also scale is False, a rigid/Euclidean transformation matrix
is returned.
By default the algorithm by Hartley and Zissermann [15] is used.
If usesvd is True, similarity and Euclidean transformation matrices
are calculated by minimizing the weighted sum of squared deviations
(RMSD) according to the algorithm by Kabsch [8].
Otherwise, and if ndims is 3, the quaternion based algorithm by Horn [9]
is used, which is slower when using this Python implementation.
The returned matrix performs rotation, translation and uniform scaling
(if specified).
>>> v0 = [[0, 1031, 1031, 0], [0, 0, 1600, 1600]]
>>> v1 = [[675, 826, 826, 677], [55, 52, 281, 277]]
>>> affine_matrix_from_points(v0, v1)
array([[1.45489948e-01, 6.24811295e-04, 6.75500083e+02],
[4.84502149e-04, 1.40938160e-01, 5.32497108e+01],
[0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
>>> T = translation_matrix(np.random.random(3)-0.5)
>>> R = random_rotation_matrix()
>>> S = scale_matrix(np.random.random())
>>> M = concatenate_matrices(T, R, S)
>>> v0 = (np.random.rand(4, 100) - 0.5) * 20
>>> v0[3] = 1
>>> v1 = np.dot(M, v0)
>>> v0[:3] += np.random.normal(0, 1e-8, 300).reshape(3, -1)
>>> M = affine_matrix_from_points(v0[:3], v1[:3])
>>> np.allclose(v1, np.dot(M, v0))
True
More examples in superimposition_matrix()
"""
v0 = np.array(v0, dtype=np.float64, copy=True)
v1 = np.array(v1, dtype=np.float64, copy=True)
ndims = v0.shape[0]
if ndims < 2 or v0.shape[1] < ndims or v0.shape != v1.shape:
raise ValueError("input arrays are of wrong shape or type")
# move centroids to origin
t0 = -np.mean(v0, axis=1)
M0 = np.identity(ndims + 1)
M0[:ndims, ndims] = t0
v0 += t0.reshape(ndims, 1)
t1 = -np.mean(v1, axis=1)
M1 = np.identity(ndims + 1)
M1[:ndims, ndims] = t1
v1 += t1.reshape(ndims, 1)
if shear:
# Affine transformation
A = np.concatenate((v0, v1), axis=0)
u, s, vh = np.linalg.svd(A.T)
vh = vh[:ndims].T
B = vh[:ndims]
C = vh[ndims:2 * ndims]
t = np.dot(C, np.linalg.pinv(B))
t = np.concatenate((t, np.zeros((ndims, 1))), axis=1)
M = np.vstack((t, ((0.0,) * ndims) + (1.0,)))
elif usesvd or ndims != 3:
# Rigid transformation via SVD of covariance matrix
u, s, vh = np.linalg.svd(np.dot(v1, v0.T))
# rotation matrix from SVD orthonormal bases
R = np.dot(u, vh)
if np.linalg.det(R) < 0.0:
# R does not constitute right handed system
R -= np.outer(u[:, ndims - 1], vh[ndims - 1, :] * 2.0)
s[-1] *= -1.0
# homogeneous transformation matrix
M = np.identity(ndims + 1)
M[:ndims, :ndims] = R
else:
# Rigid transformation matrix via quaternion
# compute symmetric matrix N
xx, yy, zz = np.sum(v0 * v1, axis=1)
xy, yz, zx = np.sum(v0 * np.roll(v1, -1, axis=0), axis=1)
xz, yx, zy = np.sum(v0 * np.roll(v1, -2, axis=0), axis=1)
N = [[xx + yy + zz, 0.0, 0.0, 0.0],
[yz - zy, xx - yy - zz, 0.0, 0.0],
[zx - xz, xy + yx, yy - xx - zz, 0.0],
[xy - yx, zx + xz, yz + zy, zz - xx - yy]]
# quaternion: eigenvector corresponding to most positive eigenvalue
w, V = np.linalg.eigh(N)
q = V[:, np.argmax(w)]
q /= norm(q) # unit quaternion
# homogeneous transformation matrix
M = quaternion_to_transform_matrix(q)
if scale and not shear:
# Affine transformation; scale is ratio of RMS deviations from centroid
v0 *= v0
v1 *= v1
M[:ndims, :ndims] *= sqrt(np.sum(v1) / np.sum(v0))
# move centroids back
M = np.dot(np.linalg.inv(M1), np.dot(M, M0))
M /= M[ndims, ndims]
return M
def decompose_matrix(matrix: np.ndarray):
"""Return sequence of transformations from transformation matrix.
matrix : array_like
Non-degenerative homogeneous transformation matrix
Return tuple of:
scale : vector of 3 scaling factors
shear : list of shear factors for x-y, x-z, y-z axes
angles : list of Euler angles about static x, y, z axes
translate : translation vector along x, y, z axes
perspective : perspective partition of matrix
Raise ValueError if matrix is of wrong type or degenerative.
>>> T0 = translation_matrix(vector(1, 2, 3))
>>> scale, shear, angles, trans, persp = decompose_matrix(T0)
>>> T1 = translation_matrix(trans)
>>> np.allclose(T0, T1)
True
>>> S = scale_matrix(0.123)
>>> scale, shear, angles, trans, persp = decompose_matrix(S)
>>> scale[0]
0.123
>>> R0 = euler_matrix(1, 2, 3)
>>> scale, shear, angles, trans, persp = decompose_matrix(R0)
>>> R1 = euler_matrix(*angles)
>>> np.allclose(R0, R1)
True
"""
M = np.transpose(np.copy(matrix))
if abs(M[3, 3]) < EPS:
raise ValueError("M[3, 3] is zero")
M /= M[3, 3]
P = M.copy()
P[:, 3] = 0.0, 0.0, 0.0, 1.0
if not np.linalg.det(P):
raise ValueError("matrix is singular")
scale = np.zeros((3,))
shear = np.zeros(3)
angles = np.zeros(3)
if np.any(np.abs(M[:3, 3]) > EPS):
perspective = np.dot(M[:, 3], np.linalg.inv(P.T))
M[:, 3] = 0.0, 0.0, 0.0, 1.0
else:
perspective = np.array([0.0, 0.0, 0.0, 1.0])
translate = M[3, :3].copy()
M[3, :3] = 0.0
row = M[:3, :3].copy()
scale[0] = norm(row[0])
row[0] /= scale[0]
shear[0] = np.dot(row[0], row[1])
row[1] -= row[0] * shear[0]
scale[1] = norm(row[1])
row[1] /= scale[1]
shear[0] /= scale[1]
shear[1] = np.dot(row[0], row[2])
row[2] -= row[0] * shear[1]
shear[2] = np.dot(row[1], row[2])
row[2] -= row[1] * shear[2]
scale[2] = norm(row[2])
row[2] /= scale[2]
shear[1] /= scale[2]
shear[2] /= scale[2]
if np.dot(row[0], np.cross(row[1], row[2])) < 0:
np.negative(scale, scale)
np.negative(row, row)
angles[1] = asin(-row[0, 2])
if cos(angles[1]):
angles[0] = atan2(row[1, 2], row[2, 2])
angles[2] = atan2(row[0, 1], row[0, 0])
else:
# angles[0] = atan2(row[1, 0], row[1, 1])
angles[0] = atan2(-row[2, 1], row[1, 1])
angles[2] = 0.0
return scale, shear, angles, translate, perspective
def projection_from_matrix(matrix, pseudo=False):
"""Return projection plane and perspective point from projection matrix.
Return values are same as arguments for projection_matrix function:
point, normal, direction, perspective, and pseudo.
>>> point = np.random.random(3) - 0.5
>>> normal = np.random.random(3) - 0.5
>>> direct = np.random.random(3) - 0.5
>>> persp = np.random.random(3) - 0.5
>>> P0 = projection_matrix(point, normal)
>>> result = projection_from_matrix(P0)
>>> P1 = projection_matrix(*result)
>>> is_same_transform(P0, P1, eps=1e-7)
True
>>> P2 = projection_matrix(point, normal, direct)
>>> result = projection_from_matrix(P2)
>>> P3 = projection_matrix(*result)
>>> is_same_transform(P2, P3, eps=1e-6)
True
>>> P4 = projection_matrix(point, normal, perspective=persp, pseudo=False)
>>> result = projection_from_matrix(P4, pseudo=False)
>>> P5 = projection_matrix(*result)
>>> is_same_transform(P4, P5, eps=1e-5)
True
>>> P6 = projection_matrix(point, normal, perspective=persp, pseudo=True)
>>> result = projection_from_matrix(P6, pseudo=True)
>>> P7 = projection_matrix(*result)
>>> is_same_transform(P6, P7, eps=1e-7)
True
"""
M = matrix
M33 = M[:3, :3]
w, V = np.linalg.eig(M)
i = np.where(np.abs(np.real(w) - 1.0) < 1e-8)[0]
if not pseudo and len(i):
# point: any eigenvector corresponding to eigenvalue 1
point = np.real(V[:, i[-1]]).flatten()
point /= point[3]
# direction: unit eigenvector corresponding to eigenvalue 0
w, V = np.linalg.eig(M33)
i = np.where(np.abs(np.real(w)) < 1e-8)[0]
if not len(i):
raise ValueError("no eigenvector corresponding to eigenvalue 0")
direction = np.real(V[:, i[0]]).flatten()
direction /= norm(direction)
# normal: unit eigenvector of M33.T corresponding to eigenvalue 0
w, V = np.linalg.eig(M33.T)
i = np.where(np.abs(np.real(w)) < 1e-8)[0]
if len(i):
# parallel projection
normal = np.real(V[:, i[0]]).flatten()
normal /= norm(normal)
return point, normal, direction, None, False
else:
# orthogonal projection, where normal equals direction vector
return point, direction, None, None, False
else:
# perspective projection
i = np.where(np.abs(np.real(w)) > 1e-8)[0]
if not len(i):
raise ValueError("no eigenvector not corresponding to eigenvalue 0")
point = np.real(V[:, i[-1]]).flatten()
point /= point[3]
normal = - M[3, :3]
perspective = M[:3, 3] / np.dot(point[:3], normal)
if pseudo:
perspective -= normal
return point, normal, None, perspective, pseudo