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