|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
"""Homogeneous Transformation Matrices and Quaternions. |
|
|
|
A library for calculating 4x4 matrices for translating, rotating, reflecting, |
|
scaling, shearing, projecting, orthogonalizing, and superimposing arrays of |
|
3D homogeneous coordinates as well as for converting between rotation matrices, |
|
Euler angles, and quaternions. Also includes an Arcball control object and |
|
functions to decompose transformation matrices. |
|
|
|
:Author: |
|
`Christoph Gohlke <http://www.lfd.uci.edu/~gohlke/>`_ |
|
|
|
:Organization: |
|
Laboratory for Fluorescence Dynamics, University of California, Irvine |
|
|
|
:Version: 2015.07.18 |
|
|
|
Requirements |
|
------------ |
|
* `CPython 2.7 or 3.4 <http://www.python.org>`_ |
|
* `Numpy 1.9 <http://www.numpy.org>`_ |
|
* `Transformations.c 2015.07.18 <http://www.lfd.uci.edu/~gohlke/>`_ |
|
(recommended for speedup of some functions) |
|
|
|
Notes |
|
----- |
|
The API is not stable yet and is expected to change between revisions. |
|
|
|
This Python code is not optimized for speed. Refer to the transformations.c |
|
module for a faster implementation of some functions. |
|
|
|
Documentation in HTML format can be generated with epydoc. |
|
|
|
Matrices (M) can be inverted using numpy.linalg.inv(M), be concatenated using |
|
numpy.dot(M0, M1), or transform homogeneous coordinate arrays (v) using |
|
numpy.dot(M, v) for shape (4, \*) column vectors, respectively |
|
numpy.dot(v, M.T) for shape (\*, 4) row vectors ("array of points"). |
|
|
|
This module follows the "column vectors on the right" and "row major storage" |
|
(C contiguous) conventions. The translation components are in the right column |
|
of the transformation matrix, i.e. M[:3, 3]. |
|
The transpose of the transformation matrices may have to be used to interface |
|
with other graphics systems, e.g. with OpenGL's glMultMatrixd(). See also [16]. |
|
|
|
Calculations are carried out with numpy.float64 precision. |
|
|
|
Vector, point, quaternion, and matrix function arguments are expected to be |
|
"array like", i.e. tuple, list, or numpy arrays. |
|
|
|
Return types are numpy arrays unless specified otherwise. |
|
|
|
Angles are in radians unless specified otherwise. |
|
|
|
Quaternions w+ix+jy+kz are represented as [w, x, y, z]. |
|
|
|
A triple of Euler angles can be applied/interpreted in 24 ways, which can |
|
be specified using a 4 character string or encoded 4-tuple: |
|
|
|
*Axes 4-string*: e.g. 'sxyz' or 'ryxy' |
|
|
|
- first character : rotations are applied to 's'tatic or 'r'otating frame |
|
- remaining characters : successive rotation axis 'x', 'y', or 'z' |
|
|
|
*Axes 4-tuple*: e.g. (0, 0, 0, 0) or (1, 1, 1, 1) |
|
|
|
- inner axis: code of axis ('x':0, 'y':1, 'z':2) of rightmost matrix. |
|
- parity : even (0) if inner axis 'x' is followed by 'y', 'y' is followed |
|
by 'z', or 'z' is followed by 'x'. Otherwise odd (1). |
|
- repetition : first and last axis are same (1) or different (0). |
|
- frame : rotations are applied to static (0) or rotating (1) frame. |
|
|
|
Other Python packages and modules for 3D transformations and quaternions: |
|
|
|
* `Transforms3d <https://pypi.python.org/pypi/transforms3d>`_ |
|
includes most code of this module. |
|
* `Blender.mathutils <http://www.blender.org/api/blender_python_api>`_ |
|
* `numpy-dtypes <https://github.com/numpy/numpy-dtypes>`_ |
|
|
|
References |
|
---------- |
|
(1) Matrices and transformations. Ronald Goldman. |
|
In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990. |
|
(2) More matrices and transformations: shear and pseudo-perspective. |
|
Ronald Goldman. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. |
|
(3) Decomposing a matrix into simple transformations. Spencer Thomas. |
|
In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. |
|
(4) Recovering the data from the transformation matrix. Ronald Goldman. |
|
In "Graphics Gems II", pp 324-331. Morgan Kaufmann, 1991. |
|
(5) Euler angle conversion. Ken Shoemake. |
|
In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994. |
|
(6) Arcball rotation control. Ken Shoemake. |
|
In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994. |
|
(7) Representing attitude: Euler angles, unit quaternions, and rotation |
|
vectors. James Diebel. 2006. |
|
(8) A discussion of the solution for the best rotation to relate two sets |
|
of vectors. W Kabsch. Acta Cryst. 1978. A34, 827-828. |
|
(9) Closed-form solution of absolute orientation using unit quaternions. |
|
BKP Horn. J Opt Soc Am A. 1987. 4(4):629-642. |
|
(10) Quaternions. Ken Shoemake. |
|
http://www.sfu.ca/~jwa3/cmpt461/files/quatut.pdf |
|
(11) From quaternion to matrix and back. JMP van Waveren. 2005. |
|
http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm |
|
(12) Uniform random rotations. Ken Shoemake. |
|
In "Graphics Gems III", pp 124-132. Morgan Kaufmann, 1992. |
|
(13) Quaternion in molecular modeling. CFF Karney. |
|
J Mol Graph Mod, 25(5):595-604 |
|
(14) New method for extracting the quaternion from a rotation matrix. |
|
Itzhack Y Bar-Itzhack, J Guid Contr Dynam. 2000. 23(6): 1085-1087. |
|
(15) Multiple View Geometry in Computer Vision. Hartley and Zissermann. |
|
Cambridge University Press; 2nd Ed. 2004. Chapter 4, Algorithm 4.7, p 130. |
|
(16) Column Vectors vs. Row Vectors. |
|
http://steve.hollasch.net/cgindex/math/matrix/column-vec.html |
|
|
|
Examples |
|
-------- |
|
>>> alpha, beta, gamma = 0.123, -1.234, 2.345 |
|
>>> origin, xaxis, yaxis, zaxis = [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1] |
|
>>> I = identity_matrix() |
|
>>> Rx = rotation_matrix(alpha, xaxis) |
|
>>> Ry = rotation_matrix(beta, yaxis) |
|
>>> Rz = rotation_matrix(gamma, zaxis) |
|
>>> R = concatenate_matrices(Rx, Ry, Rz) |
|
>>> euler = euler_from_matrix(R, 'rxyz') |
|
>>> numpy.allclose([alpha, beta, gamma], euler) |
|
True |
|
>>> Re = euler_matrix(alpha, beta, gamma, 'rxyz') |
|
>>> is_same_transform(R, Re) |
|
True |
|
>>> al, be, ga = euler_from_matrix(Re, 'rxyz') |
|
>>> is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz')) |
|
True |
|
>>> qx = quaternion_about_axis(alpha, xaxis) |
|
>>> qy = quaternion_about_axis(beta, yaxis) |
|
>>> qz = quaternion_about_axis(gamma, zaxis) |
|
>>> q = quaternion_multiply(qx, qy) |
|
>>> q = quaternion_multiply(q, qz) |
|
>>> Rq = quaternion_matrix(q) |
|
>>> is_same_transform(R, Rq) |
|
True |
|
>>> S = scale_matrix(1.23, origin) |
|
>>> T = translation_matrix([1, 2, 3]) |
|
>>> Z = shear_matrix(beta, xaxis, origin, zaxis) |
|
>>> R = random_rotation_matrix(numpy.random.rand(3)) |
|
>>> M = concatenate_matrices(T, R, Z, S) |
|
>>> scale, shear, angles, trans, persp = decompose_matrix(M) |
|
>>> numpy.allclose(scale, 1.23) |
|
True |
|
>>> numpy.allclose(trans, [1, 2, 3]) |
|
True |
|
>>> numpy.allclose(shear, [0, math.tan(beta), 0]) |
|
True |
|
>>> is_same_transform(R, euler_matrix(axes='sxyz', *angles)) |
|
True |
|
>>> M1 = compose_matrix(scale, shear, angles, trans, persp) |
|
>>> is_same_transform(M, M1) |
|
True |
|
>>> v0, v1 = random_vector(3), random_vector(3) |
|
>>> M = rotation_matrix(angle_between_vectors(v0, v1), vector_product(v0, v1)) |
|
>>> v2 = numpy.dot(v0, M[:3,:3].T) |
|
>>> numpy.allclose(unit_vector(v1), unit_vector(v2)) |
|
True |
|
|
|
""" |
|
|
|
from __future__ import division, print_function |
|
|
|
import math |
|
|
|
import numpy |
|
|
|
__version__ = '2015.07.18' |
|
__docformat__ = 'restructuredtext en' |
|
__all__ = () |
|
|
|
|
|
def identity_matrix(): |
|
"""Return 4x4 identity/unit matrix. |
|
|
|
>>> I = identity_matrix() |
|
>>> numpy.allclose(I, numpy.dot(I, I)) |
|
True |
|
>>> numpy.sum(I), numpy.trace(I) |
|
(4.0, 4.0) |
|
>>> numpy.allclose(I, numpy.identity(4)) |
|
True |
|
|
|
""" |
|
return numpy.identity(4) |
|
|
|
|
|
def translation_matrix(direction): |
|
"""Return matrix to translate by direction vector. |
|
|
|
>>> v = numpy.random.random(3) - 0.5 |
|
>>> numpy.allclose(v, translation_matrix(v)[:3, 3]) |
|
True |
|
|
|
""" |
|
M = numpy.identity(4) |
|
M[:3, 3] = direction[:3] |
|
return M |
|
|
|
|
|
def translation_from_matrix(matrix): |
|
"""Return translation vector from translation matrix. |
|
|
|
>>> v0 = numpy.random.random(3) - 0.5 |
|
>>> v1 = translation_from_matrix(translation_matrix(v0)) |
|
>>> numpy.allclose(v0, v1) |
|
True |
|
|
|
""" |
|
return numpy.array(matrix, copy=False)[:3, 3].copy() |
|
|
|
|
|
def reflection_matrix(point, normal): |
|
"""Return matrix to mirror at plane defined by point and normal vector. |
|
|
|
>>> v0 = numpy.random.random(4) - 0.5 |
|
>>> v0[3] = 1. |
|
>>> v1 = numpy.random.random(3) - 0.5 |
|
>>> R = reflection_matrix(v0, v1) |
|
>>> numpy.allclose(2, numpy.trace(R)) |
|
True |
|
>>> numpy.allclose(v0, numpy.dot(R, v0)) |
|
True |
|
>>> v2 = v0.copy() |
|
>>> v2[:3] += v1 |
|
>>> v3 = v0.copy() |
|
>>> v2[:3] -= v1 |
|
>>> numpy.allclose(v2, numpy.dot(R, v3)) |
|
True |
|
|
|
""" |
|
normal = unit_vector(normal[:3]) |
|
M = numpy.identity(4) |
|
M[:3, :3] -= 2.0 * numpy.outer(normal, normal) |
|
M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal |
|
return M |
|
|
|
|
|
def reflection_from_matrix(matrix): |
|
"""Return mirror plane point and normal vector from reflection matrix. |
|
|
|
>>> v0 = numpy.random.random(3) - 0.5 |
|
>>> v1 = numpy.random.random(3) - 0.5 |
|
>>> M0 = reflection_matrix(v0, v1) |
|
>>> point, normal = reflection_from_matrix(M0) |
|
>>> M1 = reflection_matrix(point, normal) |
|
>>> is_same_transform(M0, M1) |
|
True |
|
|
|
""" |
|
M = numpy.array(matrix, dtype=numpy.float64, copy=False) |
|
|
|
w, V = numpy.linalg.eig(M[:3, :3]) |
|
i = numpy.where(abs(numpy.real(w) + 1.0) < 1e-8)[0] |
|
if not len(i): |
|
raise ValueError("no unit eigenvector corresponding to eigenvalue -1") |
|
normal = numpy.real(V[:, i[0]]).squeeze() |
|
|
|
w, V = numpy.linalg.eig(M) |
|
i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] |
|
if not len(i): |
|
raise ValueError("no unit eigenvector corresponding to eigenvalue 1") |
|
point = numpy.real(V[:, i[-1]]).squeeze() |
|
point /= point[3] |
|
return point, normal |
|
|
|
|
|
def rotation_matrix(angle, direction, point=None): |
|
"""Return matrix to rotate about axis defined by point and direction. |
|
|
|
>>> R = rotation_matrix(math.pi/2, [0, 0, 1], [1, 0, 0]) |
|
>>> numpy.allclose(numpy.dot(R, [0, 0, 0, 1]), [1, -1, 0, 1]) |
|
True |
|
>>> angle = (random.random() - 0.5) * (2*math.pi) |
|
>>> direc = numpy.random.random(3) - 0.5 |
|
>>> point = numpy.random.random(3) - 0.5 |
|
>>> R0 = rotation_matrix(angle, direc, point) |
|
>>> R1 = rotation_matrix(angle-2*math.pi, direc, point) |
|
>>> is_same_transform(R0, R1) |
|
True |
|
>>> R0 = rotation_matrix(angle, direc, point) |
|
>>> R1 = rotation_matrix(-angle, -direc, point) |
|
>>> is_same_transform(R0, R1) |
|
True |
|
>>> I = numpy.identity(4, numpy.float64) |
|
>>> numpy.allclose(I, rotation_matrix(math.pi*2, direc)) |
|
True |
|
>>> numpy.allclose(2, numpy.trace(rotation_matrix(math.pi/2, |
|
... direc, point))) |
|
True |
|
|
|
""" |
|
sina = math.sin(angle) |
|
cosa = math.cos(angle) |
|
direction = unit_vector(direction[:3]) |
|
|
|
R = numpy.diag([cosa, cosa, cosa]) |
|
R += numpy.outer(direction, direction) * (1.0 - cosa) |
|
direction *= sina |
|
R += numpy.array([[ 0.0, -direction[2], direction[1]], |
|
[ direction[2], 0.0, -direction[0]], |
|
[-direction[1], direction[0], 0.0]]) |
|
M = numpy.identity(4) |
|
M[:3, :3] = R |
|
if point is not None: |
|
|
|
point = numpy.array(point[:3], dtype=numpy.float64, copy=False) |
|
M[:3, 3] = point - numpy.dot(R, point) |
|
return M |
|
|
|
|
|
def rotation_from_matrix(matrix): |
|
"""Return rotation angle and axis from rotation matrix. |
|
|
|
>>> angle = (random.random() - 0.5) * (2*math.pi) |
|
>>> direc = numpy.random.random(3) - 0.5 |
|
>>> point = numpy.random.random(3) - 0.5 |
|
>>> R0 = rotation_matrix(angle, direc, point) |
|
>>> angle, direc, point = rotation_from_matrix(R0) |
|
>>> R1 = rotation_matrix(angle, direc, point) |
|
>>> is_same_transform(R0, R1) |
|
True |
|
|
|
""" |
|
R = numpy.array(matrix, dtype=numpy.float64, copy=False) |
|
R33 = R[:3, :3] |
|
|
|
w, W = numpy.linalg.eig(R33.T) |
|
i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] |
|
if not len(i): |
|
raise ValueError("no unit eigenvector corresponding to eigenvalue 1") |
|
direction = numpy.real(W[:, i[-1]]).squeeze() |
|
|
|
w, Q = numpy.linalg.eig(R) |
|
i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] |
|
if not len(i): |
|
raise ValueError("no unit eigenvector corresponding to eigenvalue 1") |
|
point = numpy.real(Q[:, i[-1]]).squeeze() |
|
point /= point[3] |
|
|
|
cosa = (numpy.trace(R33) - 1.0) / 2.0 |
|
if abs(direction[2]) > 1e-8: |
|
sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2] |
|
elif abs(direction[1]) > 1e-8: |
|
sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1] |
|
else: |
|
sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0] |
|
angle = math.atan2(sina, cosa) |
|
return angle, direction, point |
|
|
|
|
|
def scale_matrix(factor, origin=None, direction=None): |
|
"""Return matrix to scale by factor around origin in direction. |
|
|
|
Use factor -1 for point symmetry. |
|
|
|
>>> v = (numpy.random.rand(4, 5) - 0.5) * 20 |
|
>>> v[3] = 1 |
|
>>> S = scale_matrix(-1.234) |
|
>>> numpy.allclose(numpy.dot(S, v)[:3], -1.234*v[:3]) |
|
True |
|
>>> factor = random.random() * 10 - 5 |
|
>>> origin = numpy.random.random(3) - 0.5 |
|
>>> direct = numpy.random.random(3) - 0.5 |
|
>>> S = scale_matrix(factor, origin) |
|
>>> S = scale_matrix(factor, origin, direct) |
|
|
|
""" |
|
if direction is None: |
|
|
|
M = numpy.diag([factor, factor, factor, 1.0]) |
|
if origin is not None: |
|
M[:3, 3] = origin[:3] |
|
M[:3, 3] *= 1.0 - factor |
|
else: |
|
|
|
direction = unit_vector(direction[:3]) |
|
factor = 1.0 - factor |
|
M = numpy.identity(4) |
|
M[:3, :3] -= factor * numpy.outer(direction, direction) |
|
if origin is not None: |
|
M[:3, 3] = (factor * numpy.dot(origin[:3], direction)) * direction |
|
return M |
|
|
|
|
|
def scale_from_matrix(matrix): |
|
"""Return scaling factor, origin and direction from scaling matrix. |
|
|
|
>>> factor = random.random() * 10 - 5 |
|
>>> origin = numpy.random.random(3) - 0.5 |
|
>>> direct = numpy.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 |
|
|
|
""" |
|
M = numpy.array(matrix, dtype=numpy.float64, copy=False) |
|
M33 = M[:3, :3] |
|
factor = numpy.trace(M33) - 2.0 |
|
try: |
|
|
|
w, V = numpy.linalg.eig(M33) |
|
i = numpy.where(abs(numpy.real(w) - factor) < 1e-8)[0][0] |
|
direction = numpy.real(V[:, i]).squeeze() |
|
direction /= vector_norm(direction) |
|
except IndexError: |
|
|
|
factor = (factor + 2.0) / 3.0 |
|
direction = None |
|
|
|
w, V = numpy.linalg.eig(M) |
|
i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] |
|
if not len(i): |
|
raise ValueError("no eigenvector corresponding to eigenvalue 1") |
|
origin = numpy.real(V[:, i[-1]]).squeeze() |
|
origin /= origin[3] |
|
return factor, origin, direction |
|
|
|
|
|
def projection_matrix(point, normal, direction=None, |
|
perspective=None, pseudo=False): |
|
"""Return matrix to project onto plane defined by point and normal. |
|
|
|
Using either perspective point, projection direction, or none of both. |
|
|
|
If pseudo is True, perspective projections will preserve relative depth |
|
such that Perspective = dot(Orthogonal, PseudoPerspective). |
|
|
|
>>> P = projection_matrix([0, 0, 0], [1, 0, 0]) |
|
>>> numpy.allclose(P[1:, 1:], numpy.identity(4)[1:, 1:]) |
|
True |
|
>>> point = numpy.random.random(3) - 0.5 |
|
>>> normal = numpy.random.random(3) - 0.5 |
|
>>> direct = numpy.random.random(3) - 0.5 |
|
>>> persp = numpy.random.random(3) - 0.5 |
|
>>> P0 = projection_matrix(point, normal) |
|
>>> P1 = projection_matrix(point, normal, direction=direct) |
|
>>> P2 = projection_matrix(point, normal, perspective=persp) |
|
>>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True) |
|
>>> is_same_transform(P2, numpy.dot(P0, P3)) |
|
True |
|
>>> P = projection_matrix([3, 0, 0], [1, 1, 0], [1, 0, 0]) |
|
>>> v0 = (numpy.random.rand(4, 5) - 0.5) * 20 |
|
>>> v0[3] = 1 |
|
>>> v1 = numpy.dot(P, v0) |
|
>>> numpy.allclose(v1[1], v0[1]) |
|
True |
|
>>> numpy.allclose(v1[0], 3-v1[1]) |
|
True |
|
|
|
""" |
|
M = numpy.identity(4) |
|
point = numpy.array(point[:3], dtype=numpy.float64, copy=False) |
|
normal = unit_vector(normal[:3]) |
|
if perspective is not None: |
|
|
|
perspective = numpy.array(perspective[:3], dtype=numpy.float64, |
|
copy=False) |
|
M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective-point, normal) |
|
M[:3, :3] -= numpy.outer(perspective, normal) |
|
if pseudo: |
|
|
|
M[:3, :3] -= numpy.outer(normal, normal) |
|
M[:3, 3] = numpy.dot(point, normal) * (perspective+normal) |
|
else: |
|
M[:3, 3] = numpy.dot(point, normal) * perspective |
|
M[3, :3] = -normal |
|
M[3, 3] = numpy.dot(perspective, normal) |
|
elif direction is not None: |
|
|
|
direction = numpy.array(direction[:3], dtype=numpy.float64, copy=False) |
|
scale = numpy.dot(direction, normal) |
|
M[:3, :3] -= numpy.outer(direction, normal) / scale |
|
M[:3, 3] = direction * (numpy.dot(point, normal) / scale) |
|
else: |
|
|
|
M[:3, :3] -= numpy.outer(normal, normal) |
|
M[:3, 3] = numpy.dot(point, normal) * normal |
|
return M |
|
|
|
|
|
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 = numpy.random.random(3) - 0.5 |
|
>>> normal = numpy.random.random(3) - 0.5 |
|
>>> direct = numpy.random.random(3) - 0.5 |
|
>>> persp = numpy.random.random(3) - 0.5 |
|
>>> P0 = projection_matrix(point, normal) |
|
>>> result = projection_from_matrix(P0) |
|
>>> P1 = projection_matrix(*result) |
|
>>> is_same_transform(P0, P1) |
|
True |
|
>>> P0 = projection_matrix(point, normal, direct) |
|
>>> result = projection_from_matrix(P0) |
|
>>> P1 = projection_matrix(*result) |
|
>>> is_same_transform(P0, P1) |
|
True |
|
>>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False) |
|
>>> result = projection_from_matrix(P0, pseudo=False) |
|
>>> P1 = projection_matrix(*result) |
|
>>> is_same_transform(P0, P1) |
|
True |
|
>>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True) |
|
>>> result = projection_from_matrix(P0, pseudo=True) |
|
>>> P1 = projection_matrix(*result) |
|
>>> is_same_transform(P0, P1) |
|
True |
|
|
|
""" |
|
M = numpy.array(matrix, dtype=numpy.float64, copy=False) |
|
M33 = M[:3, :3] |
|
w, V = numpy.linalg.eig(M) |
|
i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] |
|
if not pseudo and len(i): |
|
|
|
point = numpy.real(V[:, i[-1]]).squeeze() |
|
point /= point[3] |
|
|
|
w, V = numpy.linalg.eig(M33) |
|
i = numpy.where(abs(numpy.real(w)) < 1e-8)[0] |
|
if not len(i): |
|
raise ValueError("no eigenvector corresponding to eigenvalue 0") |
|
direction = numpy.real(V[:, i[0]]).squeeze() |
|
direction /= vector_norm(direction) |
|
|
|
w, V = numpy.linalg.eig(M33.T) |
|
i = numpy.where(abs(numpy.real(w)) < 1e-8)[0] |
|
if len(i): |
|
|
|
normal = numpy.real(V[:, i[0]]).squeeze() |
|
normal /= vector_norm(normal) |
|
return point, normal, direction, None, False |
|
else: |
|
|
|
return point, direction, None, None, False |
|
else: |
|
|
|
i = numpy.where(abs(numpy.real(w)) > 1e-8)[0] |
|
if not len(i): |
|
raise ValueError( |
|
"no eigenvector not corresponding to eigenvalue 0") |
|
point = numpy.real(V[:, i[-1]]).squeeze() |
|
point /= point[3] |
|
normal = - M[3, :3] |
|
perspective = M[:3, 3] / numpy.dot(point[:3], normal) |
|
if pseudo: |
|
perspective -= normal |
|
return point, normal, None, perspective, pseudo |
|
|
|
|
|
def clip_matrix(left, right, bottom, top, near, far, perspective=False): |
|
"""Return matrix to obtain normalized device coordinates from frustum. |
|
|
|
The frustum bounds are axis-aligned along x (left, right), |
|
y (bottom, top) and z (near, far). |
|
|
|
Normalized device coordinates are in range [-1, 1] if coordinates are |
|
inside the frustum. |
|
|
|
If perspective is True the frustum is a truncated pyramid with the |
|
perspective point at origin and direction along z axis, otherwise an |
|
orthographic canonical view volume (a box). |
|
|
|
Homogeneous coordinates transformed by the perspective clip matrix |
|
need to be dehomogenized (divided by w coordinate). |
|
|
|
>>> frustum = numpy.random.rand(6) |
|
>>> frustum[1] += frustum[0] |
|
>>> frustum[3] += frustum[2] |
|
>>> frustum[5] += frustum[4] |
|
>>> M = clip_matrix(perspective=False, *frustum) |
|
>>> numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1]) |
|
array([-1., -1., -1., 1.]) |
|
>>> numpy.dot(M, [frustum[1], frustum[3], frustum[5], 1]) |
|
array([ 1., 1., 1., 1.]) |
|
>>> M = clip_matrix(perspective=True, *frustum) |
|
>>> v = numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1]) |
|
>>> v / v[3] |
|
array([-1., -1., -1., 1.]) |
|
>>> v = numpy.dot(M, [frustum[1], frustum[3], frustum[4], 1]) |
|
>>> v / v[3] |
|
array([ 1., 1., -1., 1.]) |
|
|
|
""" |
|
if left >= right or bottom >= top or near >= far: |
|
raise ValueError("invalid frustum") |
|
if perspective: |
|
if near <= _EPS: |
|
raise ValueError("invalid frustum: near <= 0") |
|
t = 2.0 * near |
|
M = [[t/(left-right), 0.0, (right+left)/(right-left), 0.0], |
|
[0.0, t/(bottom-top), (top+bottom)/(top-bottom), 0.0], |
|
[0.0, 0.0, (far+near)/(near-far), t*far/(far-near)], |
|
[0.0, 0.0, -1.0, 0.0]] |
|
else: |
|
M = [[2.0/(right-left), 0.0, 0.0, (right+left)/(left-right)], |
|
[0.0, 2.0/(top-bottom), 0.0, (top+bottom)/(bottom-top)], |
|
[0.0, 0.0, 2.0/(far-near), (far+near)/(near-far)], |
|
[0.0, 0.0, 0.0, 1.0]] |
|
return numpy.array(M) |
|
|
|
|
|
def shear_matrix(angle, direction, point, normal): |
|
"""Return matrix to shear by angle along direction vector on shear plane. |
|
|
|
The shear plane is defined by a point and normal vector. The direction |
|
vector must be orthogonal to the plane's normal vector. |
|
|
|
A point P is transformed by the shear matrix into P" such that |
|
the vector P-P" is parallel to the direction vector and its extent is |
|
given by the angle of P-P'-P", where P' is the orthogonal projection |
|
of P onto the shear plane. |
|
|
|
>>> angle = (random.random() - 0.5) * 4*math.pi |
|
>>> direct = numpy.random.random(3) - 0.5 |
|
>>> point = numpy.random.random(3) - 0.5 |
|
>>> normal = numpy.cross(direct, numpy.random.random(3)) |
|
>>> S = shear_matrix(angle, direct, point, normal) |
|
>>> numpy.allclose(1, numpy.linalg.det(S)) |
|
True |
|
|
|
""" |
|
normal = unit_vector(normal[:3]) |
|
direction = unit_vector(direction[:3]) |
|
if abs(numpy.dot(normal, direction)) > 1e-6: |
|
raise ValueError("direction and normal vectors are not orthogonal") |
|
angle = math.tan(angle) |
|
M = numpy.identity(4) |
|
M[:3, :3] += angle * numpy.outer(direction, normal) |
|
M[:3, 3] = -angle * numpy.dot(point[:3], normal) * direction |
|
return M |
|
|
|
|
|
def shear_from_matrix(matrix): |
|
"""Return shear angle, direction and plane from shear matrix. |
|
|
|
>>> angle = (random.random() - 0.5) * 4*math.pi |
|
>>> direct = numpy.random.random(3) - 0.5 |
|
>>> point = numpy.random.random(3) - 0.5 |
|
>>> normal = numpy.cross(direct, numpy.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) |
|
True |
|
|
|
""" |
|
M = numpy.array(matrix, dtype=numpy.float64, copy=False) |
|
M33 = M[:3, :3] |
|
|
|
w, V = numpy.linalg.eig(M33) |
|
i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-4)[0] |
|
if len(i) < 2: |
|
raise ValueError("no two linear independent eigenvectors found %s" % w) |
|
V = numpy.real(V[:, i]).squeeze().T |
|
lenorm = -1.0 |
|
for i0, i1 in ((0, 1), (0, 2), (1, 2)): |
|
n = numpy.cross(V[i0], V[i1]) |
|
w = vector_norm(n) |
|
if w > lenorm: |
|
lenorm = w |
|
normal = n |
|
normal /= lenorm |
|
|
|
direction = numpy.dot(M33 - numpy.identity(3), normal) |
|
angle = vector_norm(direction) |
|
direction /= angle |
|
angle = math.atan(angle) |
|
|
|
w, V = numpy.linalg.eig(M) |
|
i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] |
|
if not len(i): |
|
raise ValueError("no eigenvector corresponding to eigenvalue 1") |
|
point = numpy.real(V[:, i[-1]]).squeeze() |
|
point /= point[3] |
|
return angle, direction, point, normal |
|
|
|
|
|
def decompose_matrix(matrix): |
|
"""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([1, 2, 3]) |
|
>>> scale, shear, angles, trans, persp = decompose_matrix(T0) |
|
>>> T1 = translation_matrix(trans) |
|
>>> numpy.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) |
|
>>> numpy.allclose(R0, R1) |
|
True |
|
|
|
""" |
|
M = numpy.array(matrix, dtype=numpy.float64, copy=True).T |
|
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 numpy.linalg.det(P): |
|
raise ValueError("matrix is singular") |
|
|
|
scale = numpy.zeros((3, )) |
|
shear = [0.0, 0.0, 0.0] |
|
angles = [0.0, 0.0, 0.0] |
|
|
|
if any(abs(M[:3, 3]) > _EPS): |
|
perspective = numpy.dot(M[:, 3], numpy.linalg.inv(P.T)) |
|
M[:, 3] = 0.0, 0.0, 0.0, 1.0 |
|
else: |
|
perspective = numpy.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] = vector_norm(row[0]) |
|
row[0] /= scale[0] |
|
shear[0] = numpy.dot(row[0], row[1]) |
|
row[1] -= row[0] * shear[0] |
|
scale[1] = vector_norm(row[1]) |
|
row[1] /= scale[1] |
|
shear[0] /= scale[1] |
|
shear[1] = numpy.dot(row[0], row[2]) |
|
row[2] -= row[0] * shear[1] |
|
shear[2] = numpy.dot(row[1], row[2]) |
|
row[2] -= row[1] * shear[2] |
|
scale[2] = vector_norm(row[2]) |
|
row[2] /= scale[2] |
|
shear[1:] /= scale[2] |
|
|
|
if numpy.dot(row[0], numpy.cross(row[1], row[2])) < 0: |
|
numpy.negative(scale, scale) |
|
numpy.negative(row, row) |
|
|
|
angles[1] = math.asin(-row[0, 2]) |
|
if math.cos(angles[1]): |
|
angles[0] = math.atan2(row[1, 2], row[2, 2]) |
|
angles[2] = math.atan2(row[0, 1], row[0, 0]) |
|
else: |
|
|
|
angles[0] = math.atan2(-row[2, 1], row[1, 1]) |
|
angles[2] = 0.0 |
|
|
|
return scale, shear, angles, translate, perspective |
|
|
|
|
|
def compose_matrix(scale=None, shear=None, angles=None, translate=None, |
|
perspective=None): |
|
"""Return transformation matrix from sequence of transformations. |
|
|
|
This is the inverse of the decompose_matrix function. |
|
|
|
Sequence of transformations: |
|
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 |
|
|
|
>>> scale = numpy.random.random(3) - 0.5 |
|
>>> shear = numpy.random.random(3) - 0.5 |
|
>>> angles = (numpy.random.random(3) - 0.5) * (2*math.pi) |
|
>>> trans = numpy.random.random(3) - 0.5 |
|
>>> persp = numpy.random.random(4) - 0.5 |
|
>>> M0 = compose_matrix(scale, shear, angles, trans, persp) |
|
>>> result = decompose_matrix(M0) |
|
>>> M1 = compose_matrix(*result) |
|
>>> is_same_transform(M0, M1) |
|
True |
|
|
|
""" |
|
M = numpy.identity(4) |
|
if perspective is not None: |
|
P = numpy.identity(4) |
|
P[3, :] = perspective[:4] |
|
M = numpy.dot(M, P) |
|
if translate is not None: |
|
T = numpy.identity(4) |
|
T[:3, 3] = translate[:3] |
|
M = numpy.dot(M, T) |
|
if angles is not None: |
|
R = euler_matrix(angles[0], angles[1], angles[2], 'sxyz') |
|
M = numpy.dot(M, R) |
|
if shear is not None: |
|
Z = numpy.identity(4) |
|
Z[1, 2] = shear[2] |
|
Z[0, 2] = shear[1] |
|
Z[0, 1] = shear[0] |
|
M = numpy.dot(M, Z) |
|
if scale is not None: |
|
S = numpy.identity(4) |
|
S[0, 0] = scale[0] |
|
S[1, 1] = scale[1] |
|
S[2, 2] = scale[2] |
|
M = numpy.dot(M, S) |
|
M /= M[3, 3] |
|
return M |
|
|
|
|
|
def orthogonalization_matrix(lengths, angles): |
|
"""Return orthogonalization matrix for crystallographic cell coordinates. |
|
|
|
Angles are expected in degrees. |
|
|
|
The de-orthogonalization matrix is the inverse. |
|
|
|
>>> O = orthogonalization_matrix([10, 10, 10], [90, 90, 90]) |
|
>>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10) |
|
True |
|
>>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7]) |
|
>>> numpy.allclose(numpy.sum(O), 43.063229) |
|
True |
|
|
|
""" |
|
a, b, c = lengths |
|
angles = numpy.radians(angles) |
|
sina, sinb, _ = numpy.sin(angles) |
|
cosa, cosb, cosg = numpy.cos(angles) |
|
co = (cosa * cosb - cosg) / (sina * sinb) |
|
return numpy.array([ |
|
[ a*sinb*math.sqrt(1.0-co*co), 0.0, 0.0, 0.0], |
|
[-a*sinb*co, b*sina, 0.0, 0.0], |
|
[ a*cosb, b*cosa, c, 0.0], |
|
[ 0.0, 0.0, 0.0, 1.0]]) |
|
|
|
|
|
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([[ 0.14549, 0.00062, 675.50008], |
|
[ 0.00048, 0.14094, 53.24971], |
|
[ 0. , 0. , 1. ]]) |
|
>>> T = translation_matrix(numpy.random.random(3)-0.5) |
|
>>> R = random_rotation_matrix(numpy.random.random(3)) |
|
>>> S = scale_matrix(random.random()) |
|
>>> M = concatenate_matrices(T, R, S) |
|
>>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20 |
|
>>> v0[3] = 1 |
|
>>> v1 = numpy.dot(M, v0) |
|
>>> v0[:3] += numpy.random.normal(0, 1e-8, 300).reshape(3, -1) |
|
>>> M = affine_matrix_from_points(v0[:3], v1[:3]) |
|
>>> numpy.allclose(v1, numpy.dot(M, v0)) |
|
True |
|
|
|
More examples in superimposition_matrix() |
|
|
|
""" |
|
v0 = numpy.array(v0, dtype=numpy.float64, copy=True) |
|
v1 = numpy.array(v1, dtype=numpy.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") |
|
|
|
|
|
t0 = -numpy.mean(v0, axis=1) |
|
M0 = numpy.identity(ndims+1) |
|
M0[:ndims, ndims] = t0 |
|
v0 += t0.reshape(ndims, 1) |
|
t1 = -numpy.mean(v1, axis=1) |
|
M1 = numpy.identity(ndims+1) |
|
M1[:ndims, ndims] = t1 |
|
v1 += t1.reshape(ndims, 1) |
|
|
|
if shear: |
|
|
|
A = numpy.concatenate((v0, v1), axis=0) |
|
u, s, vh = numpy.linalg.svd(A.T) |
|
vh = vh[:ndims].T |
|
B = vh[:ndims] |
|
C = vh[ndims:2*ndims] |
|
t = numpy.dot(C, numpy.linalg.pinv(B)) |
|
t = numpy.concatenate((t, numpy.zeros((ndims, 1))), axis=1) |
|
M = numpy.vstack((t, ((0.0,)*ndims) + (1.0,))) |
|
elif usesvd or ndims != 3: |
|
|
|
u, s, vh = numpy.linalg.svd(numpy.dot(v1, v0.T)) |
|
|
|
R = numpy.dot(u, vh) |
|
if numpy.linalg.det(R) < 0.0: |
|
|
|
R -= numpy.outer(u[:, ndims-1], vh[ndims-1, :]*2.0) |
|
s[-1] *= -1.0 |
|
|
|
M = numpy.identity(ndims+1) |
|
M[:ndims, :ndims] = R |
|
else: |
|
|
|
|
|
xx, yy, zz = numpy.sum(v0 * v1, axis=1) |
|
xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1) |
|
xz, yx, zy = numpy.sum(v0 * numpy.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]] |
|
|
|
w, V = numpy.linalg.eigh(N) |
|
q = V[:, numpy.argmax(w)] |
|
q /= vector_norm(q) |
|
|
|
M = quaternion_matrix(q) |
|
|
|
if scale and not shear: |
|
|
|
v0 *= v0 |
|
v1 *= v1 |
|
M[:ndims, :ndims] *= math.sqrt(numpy.sum(v1) / numpy.sum(v0)) |
|
|
|
|
|
M = numpy.dot(numpy.linalg.inv(M1), numpy.dot(M, M0)) |
|
M /= M[ndims, ndims] |
|
return M |
|
|
|
|
|
def superimposition_matrix(v0, v1, scale=False, usesvd=True): |
|
"""Return matrix to transform given 3D point set into second point set. |
|
|
|
v0 and v1 are shape (3, \*) or (4, \*) arrays of at least 3 points. |
|
|
|
The parameters scale and usesvd are explained in the more general |
|
affine_matrix_from_points function. |
|
|
|
The returned matrix is a similarity or Euclidean transformation matrix. |
|
This function has a fast C implementation in transformations.c. |
|
|
|
>>> v0 = numpy.random.rand(3, 10) |
|
>>> M = superimposition_matrix(v0, v0) |
|
>>> numpy.allclose(M, numpy.identity(4)) |
|
True |
|
>>> R = random_rotation_matrix(numpy.random.random(3)) |
|
>>> v0 = [[1,0,0], [0,1,0], [0,0,1], [1,1,1]] |
|
>>> v1 = numpy.dot(R, v0) |
|
>>> M = superimposition_matrix(v0, v1) |
|
>>> numpy.allclose(v1, numpy.dot(M, v0)) |
|
True |
|
>>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20 |
|
>>> v0[3] = 1 |
|
>>> v1 = numpy.dot(R, v0) |
|
>>> M = superimposition_matrix(v0, v1) |
|
>>> numpy.allclose(v1, numpy.dot(M, v0)) |
|
True |
|
>>> S = scale_matrix(random.random()) |
|
>>> T = translation_matrix(numpy.random.random(3)-0.5) |
|
>>> M = concatenate_matrices(T, R, S) |
|
>>> v1 = numpy.dot(M, v0) |
|
>>> v0[:3] += numpy.random.normal(0, 1e-9, 300).reshape(3, -1) |
|
>>> M = superimposition_matrix(v0, v1, scale=True) |
|
>>> numpy.allclose(v1, numpy.dot(M, v0)) |
|
True |
|
>>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False) |
|
>>> numpy.allclose(v1, numpy.dot(M, v0)) |
|
True |
|
>>> v = numpy.empty((4, 100, 3)) |
|
>>> v[:, :, 0] = v0 |
|
>>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False) |
|
>>> numpy.allclose(v1, numpy.dot(M, v[:, :, 0])) |
|
True |
|
|
|
""" |
|
v0 = numpy.array(v0, dtype=numpy.float64, copy=False)[:3] |
|
v1 = numpy.array(v1, dtype=numpy.float64, copy=False)[:3] |
|
return affine_matrix_from_points(v0, v1, shear=False, |
|
scale=scale, usesvd=usesvd) |
|
|
|
|
|
def euler_matrix(ai, aj, ak, axes='sxyz'): |
|
"""Return homogeneous rotation matrix from Euler angles and axis sequence. |
|
|
|
ai, aj, ak : Euler's roll, pitch and yaw angles |
|
axes : One of 24 axis sequences as string or encoded tuple |
|
|
|
>>> R = euler_matrix(1, 2, 3, 'syxz') |
|
>>> numpy.allclose(numpy.sum(R[0]), -1.34786452) |
|
True |
|
>>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1)) |
|
>>> numpy.allclose(numpy.sum(R[0]), -0.383436184) |
|
True |
|
>>> ai, aj, ak = (4*math.pi) * (numpy.random.random(3) - 0.5) |
|
>>> for axes in _AXES2TUPLE.keys(): |
|
... R = euler_matrix(ai, aj, ak, axes) |
|
>>> for axes in _TUPLE2AXES.keys(): |
|
... R = euler_matrix(ai, aj, ak, axes) |
|
|
|
""" |
|
try: |
|
firstaxis, parity, repetition, frame = _AXES2TUPLE[axes] |
|
except (AttributeError, KeyError): |
|
_TUPLE2AXES[axes] |
|
firstaxis, parity, repetition, frame = axes |
|
|
|
i = firstaxis |
|
j = _NEXT_AXIS[i+parity] |
|
k = _NEXT_AXIS[i-parity+1] |
|
|
|
if frame: |
|
ai, ak = ak, ai |
|
if parity: |
|
ai, aj, ak = -ai, -aj, -ak |
|
|
|
si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak) |
|
ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak) |
|
cc, cs = ci*ck, ci*sk |
|
sc, ss = si*ck, si*sk |
|
|
|
M = numpy.identity(4) |
|
if repetition: |
|
M[i, i] = cj |
|
M[i, j] = sj*si |
|
M[i, k] = sj*ci |
|
M[j, i] = sj*sk |
|
M[j, j] = -cj*ss+cc |
|
M[j, k] = -cj*cs-sc |
|
M[k, i] = -sj*ck |
|
M[k, j] = cj*sc+cs |
|
M[k, k] = cj*cc-ss |
|
else: |
|
M[i, i] = cj*ck |
|
M[i, j] = sj*sc-cs |
|
M[i, k] = sj*cc+ss |
|
M[j, i] = cj*sk |
|
M[j, j] = sj*ss+cc |
|
M[j, k] = sj*cs-sc |
|
M[k, i] = -sj |
|
M[k, j] = cj*si |
|
M[k, k] = cj*ci |
|
return M |
|
|
|
|
|
def euler_from_matrix(matrix, axes='sxyz'): |
|
"""Return Euler angles from rotation matrix for specified axis sequence. |
|
|
|
axes : One of 24 axis sequences as string or encoded tuple |
|
|
|
Note that many Euler angle triplets can describe one matrix. |
|
|
|
>>> R0 = euler_matrix(1, 2, 3, 'syxz') |
|
>>> al, be, ga = euler_from_matrix(R0, 'syxz') |
|
>>> R1 = euler_matrix(al, be, ga, 'syxz') |
|
>>> numpy.allclose(R0, R1) |
|
True |
|
>>> angles = (4*math.pi) * (numpy.random.random(3) - 0.5) |
|
>>> for axes in _AXES2TUPLE.keys(): |
|
... R0 = euler_matrix(axes=axes, *angles) |
|
... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes)) |
|
... if not numpy.allclose(R0, R1): print(axes, "failed") |
|
|
|
""" |
|
try: |
|
firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] |
|
except (AttributeError, KeyError): |
|
_TUPLE2AXES[axes] |
|
firstaxis, parity, repetition, frame = axes |
|
|
|
i = firstaxis |
|
j = _NEXT_AXIS[i+parity] |
|
k = _NEXT_AXIS[i-parity+1] |
|
|
|
M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3] |
|
if repetition: |
|
sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k]) |
|
if sy > _EPS: |
|
ax = math.atan2( M[i, j], M[i, k]) |
|
ay = math.atan2( sy, M[i, i]) |
|
az = math.atan2( M[j, i], -M[k, i]) |
|
else: |
|
ax = math.atan2(-M[j, k], M[j, j]) |
|
ay = math.atan2( sy, M[i, i]) |
|
az = 0.0 |
|
else: |
|
cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i]) |
|
if cy > _EPS: |
|
ax = math.atan2( M[k, j], M[k, k]) |
|
ay = math.atan2(-M[k, i], cy) |
|
az = math.atan2( M[j, i], M[i, i]) |
|
else: |
|
ax = math.atan2(-M[j, k], M[j, j]) |
|
ay = math.atan2(-M[k, i], cy) |
|
az = 0.0 |
|
|
|
if parity: |
|
ax, ay, az = -ax, -ay, -az |
|
if frame: |
|
ax, az = az, ax |
|
return ax, ay, az |
|
|
|
|
|
def euler_from_quaternion(quaternion, axes='sxyz'): |
|
"""Return Euler angles from quaternion for specified axis sequence. |
|
|
|
>>> angles = euler_from_quaternion([0.99810947, 0.06146124, 0, 0]) |
|
>>> numpy.allclose(angles, [0.123, 0, 0]) |
|
True |
|
|
|
""" |
|
return euler_from_matrix(quaternion_matrix(quaternion), axes) |
|
|
|
|
|
def quaternion_from_euler(ai, aj, ak, axes='sxyz'): |
|
"""Return quaternion from Euler angles and axis sequence. |
|
|
|
ai, aj, ak : Euler's roll, pitch and yaw angles |
|
axes : One of 24 axis sequences as string or encoded tuple |
|
|
|
>>> q = quaternion_from_euler(1, 2, 3, 'ryxz') |
|
>>> numpy.allclose(q, [0.435953, 0.310622, -0.718287, 0.444435]) |
|
True |
|
|
|
""" |
|
try: |
|
firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] |
|
except (AttributeError, KeyError): |
|
_TUPLE2AXES[axes] |
|
firstaxis, parity, repetition, frame = axes |
|
|
|
i = firstaxis + 1 |
|
j = _NEXT_AXIS[i+parity-1] + 1 |
|
k = _NEXT_AXIS[i-parity] + 1 |
|
|
|
if frame: |
|
ai, ak = ak, ai |
|
if parity: |
|
aj = -aj |
|
|
|
ai /= 2.0 |
|
aj /= 2.0 |
|
ak /= 2.0 |
|
ci = math.cos(ai) |
|
si = math.sin(ai) |
|
cj = math.cos(aj) |
|
sj = math.sin(aj) |
|
ck = math.cos(ak) |
|
sk = math.sin(ak) |
|
cc = ci*ck |
|
cs = ci*sk |
|
sc = si*ck |
|
ss = si*sk |
|
|
|
q = numpy.empty((4, )) |
|
if repetition: |
|
q[0] = cj*(cc - ss) |
|
q[i] = cj*(cs + sc) |
|
q[j] = sj*(cc + ss) |
|
q[k] = sj*(cs - sc) |
|
else: |
|
q[0] = cj*cc + sj*ss |
|
q[i] = cj*sc - sj*cs |
|
q[j] = cj*ss + sj*cc |
|
q[k] = cj*cs - sj*sc |
|
if parity: |
|
q[j] *= -1.0 |
|
|
|
return q |
|
|
|
|
|
def quaternion_about_axis(angle, axis): |
|
"""Return quaternion for rotation about axis. |
|
|
|
>>> q = quaternion_about_axis(0.123, [1, 0, 0]) |
|
>>> numpy.allclose(q, [0.99810947, 0.06146124, 0, 0]) |
|
True |
|
|
|
""" |
|
q = numpy.array([0.0, axis[0], axis[1], axis[2]]) |
|
qlen = vector_norm(q) |
|
if qlen > _EPS: |
|
q *= math.sin(angle/2.0) / qlen |
|
q[0] = math.cos(angle/2.0) |
|
return q |
|
|
|
|
|
def quaternion_matrix(quaternion): |
|
"""Return homogeneous rotation matrix from quaternion. |
|
|
|
>>> M = quaternion_matrix([0.99810947, 0.06146124, 0, 0]) |
|
>>> numpy.allclose(M, rotation_matrix(0.123, [1, 0, 0])) |
|
True |
|
>>> M = quaternion_matrix([1, 0, 0, 0]) |
|
>>> numpy.allclose(M, numpy.identity(4)) |
|
True |
|
>>> M = quaternion_matrix([0, 1, 0, 0]) |
|
>>> numpy.allclose(M, numpy.diag([1, -1, -1, 1])) |
|
True |
|
|
|
""" |
|
q = numpy.array(quaternion, dtype=numpy.float64, copy=True) |
|
n = numpy.dot(q, q) |
|
if n < _EPS: |
|
return numpy.identity(4) |
|
q *= math.sqrt(2.0 / n) |
|
q = numpy.outer(q, q) |
|
return numpy.array([ |
|
[1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0], |
|
[ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0], |
|
[ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0], |
|
[ 0.0, 0.0, 0.0, 1.0]]) |
|
|
|
|
|
def quaternion_from_matrix(matrix, isprecise=False): |
|
"""Return quaternion from rotation matrix. |
|
|
|
If isprecise is True, the input matrix is assumed to be a precise rotation |
|
matrix and a faster algorithm is used. |
|
|
|
>>> q = quaternion_from_matrix(numpy.identity(4), True) |
|
>>> numpy.allclose(q, [1, 0, 0, 0]) |
|
True |
|
>>> q = quaternion_from_matrix(numpy.diag([1, -1, -1, 1])) |
|
>>> numpy.allclose(q, [0, 1, 0, 0]) or numpy.allclose(q, [0, -1, 0, 0]) |
|
True |
|
>>> R = rotation_matrix(0.123, (1, 2, 3)) |
|
>>> q = quaternion_from_matrix(R, True) |
|
>>> numpy.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786]) |
|
True |
|
>>> R = [[-0.545, 0.797, 0.260, 0], [0.733, 0.603, -0.313, 0], |
|
... [-0.407, 0.021, -0.913, 0], [0, 0, 0, 1]] |
|
>>> q = quaternion_from_matrix(R) |
|
>>> numpy.allclose(q, [0.19069, 0.43736, 0.87485, -0.083611]) |
|
True |
|
>>> R = [[0.395, 0.362, 0.843, 0], [-0.626, 0.796, -0.056, 0], |
|
... [-0.677, -0.498, 0.529, 0], [0, 0, 0, 1]] |
|
>>> q = quaternion_from_matrix(R) |
|
>>> numpy.allclose(q, [0.82336615, -0.13610694, 0.46344705, -0.29792603]) |
|
True |
|
>>> R = random_rotation_matrix() |
|
>>> q = quaternion_from_matrix(R) |
|
>>> is_same_transform(R, quaternion_matrix(q)) |
|
True |
|
>>> R = euler_matrix(0.0, 0.0, numpy.pi/2.0) |
|
>>> numpy.allclose(quaternion_from_matrix(R, isprecise=False), |
|
... quaternion_from_matrix(R, isprecise=True)) |
|
True |
|
|
|
""" |
|
M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4] |
|
if isprecise: |
|
q = numpy.empty((4, )) |
|
t = numpy.trace(M) |
|
if t > M[3, 3]: |
|
q[0] = t |
|
q[3] = M[1, 0] - M[0, 1] |
|
q[2] = M[0, 2] - M[2, 0] |
|
q[1] = M[2, 1] - M[1, 2] |
|
else: |
|
i, j, k = 1, 2, 3 |
|
if M[1, 1] > M[0, 0]: |
|
i, j, k = 2, 3, 1 |
|
if M[2, 2] > M[i, i]: |
|
i, j, k = 3, 1, 2 |
|
t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3] |
|
q[i] = t |
|
q[j] = M[i, j] + M[j, i] |
|
q[k] = M[k, i] + M[i, k] |
|
q[3] = M[k, j] - M[j, k] |
|
q *= 0.5 / math.sqrt(t * M[3, 3]) |
|
else: |
|
m00 = M[0, 0] |
|
m01 = M[0, 1] |
|
m02 = M[0, 2] |
|
m10 = M[1, 0] |
|
m11 = M[1, 1] |
|
m12 = M[1, 2] |
|
m20 = M[2, 0] |
|
m21 = M[2, 1] |
|
m22 = M[2, 2] |
|
|
|
K = numpy.array([[m00-m11-m22, 0.0, 0.0, 0.0], |
|
[m01+m10, m11-m00-m22, 0.0, 0.0], |
|
[m02+m20, m12+m21, m22-m00-m11, 0.0], |
|
[m21-m12, m02-m20, m10-m01, m00+m11+m22]]) |
|
K /= 3.0 |
|
|
|
w, V = numpy.linalg.eigh(K) |
|
q = V[[3, 0, 1, 2], numpy.argmax(w)] |
|
if q[0] < 0.0: |
|
numpy.negative(q, q) |
|
return q |
|
|
|
|
|
def quaternion_multiply(quaternion1, quaternion0): |
|
"""Return multiplication of two quaternions. |
|
|
|
>>> q = quaternion_multiply([4, 1, -2, 3], [8, -5, 6, 7]) |
|
>>> numpy.allclose(q, [28, -44, -14, 48]) |
|
True |
|
|
|
""" |
|
w0, x0, y0, z0 = quaternion0 |
|
w1, x1, y1, z1 = quaternion1 |
|
return numpy.array([-x1*x0 - y1*y0 - z1*z0 + w1*w0, |
|
x1*w0 + y1*z0 - z1*y0 + w1*x0, |
|
-x1*z0 + y1*w0 + z1*x0 + w1*y0, |
|
x1*y0 - y1*x0 + z1*w0 + w1*z0], dtype=numpy.float64) |
|
|
|
|
|
def quaternion_conjugate(quaternion): |
|
"""Return conjugate of quaternion. |
|
|
|
>>> q0 = random_quaternion() |
|
>>> q1 = quaternion_conjugate(q0) |
|
>>> q1[0] == q0[0] and all(q1[1:] == -q0[1:]) |
|
True |
|
|
|
""" |
|
q = numpy.array(quaternion, dtype=numpy.float64, copy=True) |
|
numpy.negative(q[1:], q[1:]) |
|
return q |
|
|
|
|
|
def quaternion_inverse(quaternion): |
|
"""Return inverse of quaternion. |
|
|
|
>>> q0 = random_quaternion() |
|
>>> q1 = quaternion_inverse(q0) |
|
>>> numpy.allclose(quaternion_multiply(q0, q1), [1, 0, 0, 0]) |
|
True |
|
|
|
""" |
|
q = numpy.array(quaternion, dtype=numpy.float64, copy=True) |
|
numpy.negative(q[1:], q[1:]) |
|
return q / numpy.dot(q, q) |
|
|
|
|
|
def quaternion_real(quaternion): |
|
"""Return real part of quaternion. |
|
|
|
>>> quaternion_real([3, 0, 1, 2]) |
|
3.0 |
|
|
|
""" |
|
return float(quaternion[0]) |
|
|
|
|
|
def quaternion_imag(quaternion): |
|
"""Return imaginary part of quaternion. |
|
|
|
>>> quaternion_imag([3, 0, 1, 2]) |
|
array([ 0., 1., 2.]) |
|
|
|
""" |
|
return numpy.array(quaternion[1:4], dtype=numpy.float64, copy=True) |
|
|
|
|
|
def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True): |
|
"""Return spherical linear interpolation between two quaternions. |
|
|
|
>>> q0 = random_quaternion() |
|
>>> q1 = random_quaternion() |
|
>>> q = quaternion_slerp(q0, q1, 0) |
|
>>> numpy.allclose(q, q0) |
|
True |
|
>>> q = quaternion_slerp(q0, q1, 1, 1) |
|
>>> numpy.allclose(q, q1) |
|
True |
|
>>> q = quaternion_slerp(q0, q1, 0.5) |
|
>>> angle = math.acos(numpy.dot(q0, q)) |
|
>>> numpy.allclose(2, math.acos(numpy.dot(q0, q1)) / angle) or \ |
|
numpy.allclose(2, math.acos(-numpy.dot(q0, q1)) / angle) |
|
True |
|
|
|
""" |
|
q0 = unit_vector(quat0[:4]) |
|
q1 = unit_vector(quat1[:4]) |
|
if fraction == 0.0: |
|
return q0 |
|
elif fraction == 1.0: |
|
return q1 |
|
d = numpy.dot(q0, q1) |
|
if abs(abs(d) - 1.0) < _EPS: |
|
return q0 |
|
if shortestpath and d < 0.0: |
|
|
|
d = -d |
|
numpy.negative(q1, q1) |
|
angle = math.acos(d) + spin * math.pi |
|
if abs(angle) < _EPS: |
|
return q0 |
|
isin = 1.0 / math.sin(angle) |
|
q0 *= math.sin((1.0 - fraction) * angle) * isin |
|
q1 *= math.sin(fraction * angle) * isin |
|
q0 += q1 |
|
return q0 |
|
|
|
|
|
def random_quaternion(rand=None): |
|
"""Return uniform random unit quaternion. |
|
|
|
rand: array like or None |
|
Three independent random variables that are uniformly distributed |
|
between 0 and 1. |
|
|
|
>>> q = random_quaternion() |
|
>>> numpy.allclose(1, vector_norm(q)) |
|
True |
|
>>> q = random_quaternion(numpy.random.random(3)) |
|
>>> len(q.shape), q.shape[0]==4 |
|
(1, True) |
|
|
|
""" |
|
if rand is None: |
|
rand = numpy.random.rand(3) |
|
else: |
|
assert len(rand) == 3 |
|
r1 = numpy.sqrt(1.0 - rand[0]) |
|
r2 = numpy.sqrt(rand[0]) |
|
pi2 = math.pi * 2.0 |
|
t1 = pi2 * rand[1] |
|
t2 = pi2 * rand[2] |
|
return numpy.array([numpy.cos(t2)*r2, numpy.sin(t1)*r1, |
|
numpy.cos(t1)*r1, numpy.sin(t2)*r2]) |
|
|
|
|
|
def random_rotation_matrix(rand=None): |
|
"""Return uniform random rotation matrix. |
|
|
|
rand: array like |
|
Three independent random variables that are uniformly distributed |
|
between 0 and 1 for each returned quaternion. |
|
|
|
>>> R = random_rotation_matrix() |
|
>>> numpy.allclose(numpy.dot(R.T, R), numpy.identity(4)) |
|
True |
|
|
|
""" |
|
return quaternion_matrix(random_quaternion(rand)) |
|
|
|
|
|
class Arcball(object): |
|
"""Virtual Trackball Control. |
|
|
|
>>> ball = Arcball() |
|
>>> ball = Arcball(initial=numpy.identity(4)) |
|
>>> ball.place([320, 320], 320) |
|
>>> ball.down([500, 250]) |
|
>>> ball.drag([475, 275]) |
|
>>> R = ball.matrix() |
|
>>> numpy.allclose(numpy.sum(R), 3.90583455) |
|
True |
|
>>> ball = Arcball(initial=[1, 0, 0, 0]) |
|
>>> ball.place([320, 320], 320) |
|
>>> ball.setaxes([1, 1, 0], [-1, 1, 0]) |
|
>>> ball.constrain = True |
|
>>> ball.down([400, 200]) |
|
>>> ball.drag([200, 400]) |
|
>>> R = ball.matrix() |
|
>>> numpy.allclose(numpy.sum(R), 0.2055924) |
|
True |
|
>>> ball.next() |
|
|
|
""" |
|
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 = numpy.array([0.0, 0.0, 1.0]) |
|
self._constrain = False |
|
if initial is None: |
|
self._qdown = numpy.array([1.0, 0.0, 0.0, 0.0]) |
|
else: |
|
initial = numpy.array(initial, dtype=numpy.float64) |
|
if initial.shape == (4, 4): |
|
self._qdown = quaternion_from_matrix(initial) |
|
elif initial.shape == (4, ): |
|
initial /= vector_norm(initial) |
|
self._qdown = initial |
|
else: |
|
raise ValueError("initial not a quaternion or matrix") |
|
self._qnow = self._qpre = self._qdown |
|
|
|
def place(self, center, radius): |
|
"""Place Arcball, e.g. when window size changes. |
|
|
|
center : sequence[2] |
|
Window coordinates of trackball center. |
|
radius : float |
|
Radius of trackball in window coordinates. |
|
|
|
""" |
|
self._radius = float(radius) |
|
self._center[0] = center[0] |
|
self._center[1] = center[1] |
|
|
|
def setaxes(self, *axes): |
|
"""Set axes to constrain rotations.""" |
|
if axes is None: |
|
self._axes = None |
|
else: |
|
self._axes = [unit_vector(axis) for axis in axes] |
|
|
|
@property |
|
def constrain(self): |
|
"""Return state of constrain to axis mode.""" |
|
return self._constrain |
|
|
|
@constrain.setter |
|
def constrain(self, value): |
|
"""Set state of constrain to axis mode.""" |
|
self._constrain = bool(value) |
|
|
|
def down(self, point): |
|
"""Set initial cursor window coordinates and pick constrain-axis.""" |
|
self._vdown = arcball_map_to_sphere(point, self._center, self._radius) |
|
self._qdown = self._qpre = self._qnow |
|
if self._constrain and self._axes is not None: |
|
self._axis = arcball_nearest_axis(self._vdown, self._axes) |
|
self._vdown = arcball_constrain_to_axis(self._vdown, self._axis) |
|
else: |
|
self._axis = None |
|
|
|
def drag(self, point): |
|
"""Update current cursor window coordinates.""" |
|
vnow = arcball_map_to_sphere(point, self._center, self._radius) |
|
if self._axis is not None: |
|
vnow = arcball_constrain_to_axis(vnow, self._axis) |
|
self._qpre = self._qnow |
|
t = numpy.cross(self._vdown, vnow) |
|
if numpy.dot(t, t) < _EPS: |
|
self._qnow = self._qdown |
|
else: |
|
q = [numpy.dot(self._vdown, vnow), t[0], t[1], t[2]] |
|
self._qnow = quaternion_multiply(q, self._qdown) |
|
|
|
def next(self, acceleration=0.0): |
|
"""Continue rotation in direction of last drag.""" |
|
q = quaternion_slerp(self._qpre, self._qnow, 2.0+acceleration, False) |
|
self._qpre, self._qnow = self._qnow, q |
|
|
|
def matrix(self): |
|
"""Return homogeneous rotation matrix.""" |
|
return quaternion_matrix(self._qnow) |
|
|
|
|
|
def arcball_map_to_sphere(point, center, radius): |
|
"""Return unit sphere coordinates from window coordinates.""" |
|
v0 = (point[0] - center[0]) / radius |
|
v1 = (center[1] - point[1]) / radius |
|
n = v0*v0 + v1*v1 |
|
if n > 1.0: |
|
|
|
n = math.sqrt(n) |
|
return numpy.array([v0/n, v1/n, 0.0]) |
|
else: |
|
return numpy.array([v0, v1, math.sqrt(1.0 - n)]) |
|
|
|
|
|
def arcball_constrain_to_axis(point, axis): |
|
"""Return sphere point perpendicular to axis.""" |
|
v = numpy.array(point, dtype=numpy.float64, copy=True) |
|
a = numpy.array(axis, dtype=numpy.float64, copy=True) |
|
v -= a * numpy.dot(a, v) |
|
n = vector_norm(v) |
|
if n > _EPS: |
|
if v[2] < 0.0: |
|
numpy.negative(v, v) |
|
v /= n |
|
return v |
|
if a[2] == 1.0: |
|
return numpy.array([1.0, 0.0, 0.0]) |
|
return unit_vector([-a[1], a[0], 0.0]) |
|
|
|
|
|
def arcball_nearest_axis(point, axes): |
|
"""Return axis, which arc is nearest to point.""" |
|
point = numpy.array(point, dtype=numpy.float64, copy=False) |
|
nearest = None |
|
mx = -1.0 |
|
for axis in axes: |
|
t = numpy.dot(arcball_constrain_to_axis(point, axis), point) |
|
if t > mx: |
|
nearest = axis |
|
mx = t |
|
return nearest |
|
|
|
|
|
|
|
_EPS = numpy.finfo(float).eps * 4.0 |
|
|
|
|
|
_NEXT_AXIS = [1, 2, 0, 1] |
|
|
|
|
|
_AXES2TUPLE = { |
|
'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0), |
|
'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0), |
|
'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0), |
|
'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0), |
|
'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1), |
|
'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1), |
|
'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1), |
|
'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)} |
|
|
|
_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items()) |
|
|
|
|
|
def vector_norm(data, axis=None, out=None): |
|
"""Return length, i.e. Euclidean norm, of ndarray along axis. |
|
|
|
>>> v = numpy.random.random(3) |
|
>>> n = vector_norm(v) |
|
>>> numpy.allclose(n, numpy.linalg.norm(v)) |
|
True |
|
>>> v = numpy.random.rand(6, 5, 3) |
|
>>> n = vector_norm(v, axis=-1) |
|
>>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2))) |
|
True |
|
>>> n = vector_norm(v, axis=1) |
|
>>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) |
|
True |
|
>>> v = numpy.random.rand(5, 4, 3) |
|
>>> n = numpy.empty((5, 3)) |
|
>>> vector_norm(v, axis=1, out=n) |
|
>>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) |
|
True |
|
>>> vector_norm([]) |
|
0.0 |
|
>>> vector_norm([1]) |
|
1.0 |
|
|
|
""" |
|
data = numpy.array(data, dtype=numpy.float64, copy=True) |
|
if out is None: |
|
if data.ndim == 1: |
|
return math.sqrt(numpy.dot(data, data)) |
|
data *= data |
|
out = numpy.atleast_1d(numpy.sum(data, axis=axis)) |
|
numpy.sqrt(out, out) |
|
return out |
|
else: |
|
data *= data |
|
numpy.sum(data, axis=axis, out=out) |
|
numpy.sqrt(out, out) |
|
|
|
|
|
def unit_vector(data, axis=None, out=None): |
|
"""Return ndarray normalized by length, i.e. Euclidean norm, along axis. |
|
|
|
>>> v0 = numpy.random.random(3) |
|
>>> v1 = unit_vector(v0) |
|
>>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0)) |
|
True |
|
>>> v0 = numpy.random.rand(5, 4, 3) |
|
>>> v1 = unit_vector(v0, axis=-1) |
|
>>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2) |
|
>>> numpy.allclose(v1, v2) |
|
True |
|
>>> v1 = unit_vector(v0, axis=1) |
|
>>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1) |
|
>>> numpy.allclose(v1, v2) |
|
True |
|
>>> v1 = numpy.empty((5, 4, 3)) |
|
>>> unit_vector(v0, axis=1, out=v1) |
|
>>> numpy.allclose(v1, v2) |
|
True |
|
>>> list(unit_vector([])) |
|
[] |
|
>>> list(unit_vector([1])) |
|
[1.0] |
|
|
|
""" |
|
if out is None: |
|
data = numpy.array(data, dtype=numpy.float64, copy=True) |
|
if data.ndim == 1: |
|
data /= math.sqrt(numpy.dot(data, data)) |
|
return data |
|
else: |
|
if out is not data: |
|
out[:] = numpy.array(data, copy=False) |
|
data = out |
|
length = numpy.atleast_1d(numpy.sum(data*data, axis)) |
|
numpy.sqrt(length, length) |
|
if axis is not None: |
|
length = numpy.expand_dims(length, axis) |
|
data /= length |
|
if out is None: |
|
return data |
|
|
|
|
|
def random_vector(size): |
|
"""Return array of random doubles in the half-open interval [0.0, 1.0). |
|
|
|
>>> v = random_vector(10000) |
|
>>> numpy.all(v >= 0) and numpy.all(v < 1) |
|
True |
|
>>> v0 = random_vector(10) |
|
>>> v1 = random_vector(10) |
|
>>> numpy.any(v0 == v1) |
|
False |
|
|
|
""" |
|
return numpy.random.random(size) |
|
|
|
|
|
def vector_product(v0, v1, axis=0): |
|
"""Return vector perpendicular to vectors. |
|
|
|
>>> v = vector_product([2, 0, 0], [0, 3, 0]) |
|
>>> numpy.allclose(v, [0, 0, 6]) |
|
True |
|
>>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]] |
|
>>> v1 = [[3], [0], [0]] |
|
>>> v = vector_product(v0, v1) |
|
>>> numpy.allclose(v, [[0, 0, 0, 0], [0, 0, 6, 6], [0, -6, 0, -6]]) |
|
True |
|
>>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]] |
|
>>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]] |
|
>>> v = vector_product(v0, v1, axis=1) |
|
>>> numpy.allclose(v, [[0, 0, 6], [0, -6, 0], [6, 0, 0], [0, -6, 6]]) |
|
True |
|
|
|
""" |
|
return numpy.cross(v0, v1, axis=axis) |
|
|
|
|
|
def angle_between_vectors(v0, v1, directed=True, axis=0): |
|
"""Return angle between vectors. |
|
|
|
If directed is False, the input vectors are interpreted as undirected axes, |
|
i.e. the maximum angle is pi/2. |
|
|
|
>>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3]) |
|
>>> numpy.allclose(a, math.pi) |
|
True |
|
>>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3], directed=False) |
|
>>> numpy.allclose(a, 0) |
|
True |
|
>>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]] |
|
>>> v1 = [[3], [0], [0]] |
|
>>> a = angle_between_vectors(v0, v1) |
|
>>> numpy.allclose(a, [0, 1.5708, 1.5708, 0.95532]) |
|
True |
|
>>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]] |
|
>>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]] |
|
>>> a = angle_between_vectors(v0, v1, axis=1) |
|
>>> numpy.allclose(a, [1.5708, 1.5708, 1.5708, 0.95532]) |
|
True |
|
|
|
""" |
|
v0 = numpy.array(v0, dtype=numpy.float64, copy=False) |
|
v1 = numpy.array(v1, dtype=numpy.float64, copy=False) |
|
dot = numpy.sum(v0 * v1, axis=axis) |
|
dot /= vector_norm(v0, axis=axis) * vector_norm(v1, axis=axis) |
|
return numpy.arccos(dot if directed else numpy.fabs(dot)) |
|
|
|
|
|
def inverse_matrix(matrix): |
|
"""Return inverse of square transformation matrix. |
|
|
|
>>> M0 = random_rotation_matrix() |
|
>>> M1 = inverse_matrix(M0.T) |
|
>>> numpy.allclose(M1, numpy.linalg.inv(M0.T)) |
|
True |
|
>>> for size in range(1, 7): |
|
... M0 = numpy.random.rand(size, size) |
|
... M1 = inverse_matrix(M0) |
|
... if not numpy.allclose(M1, numpy.linalg.inv(M0)): print(size) |
|
|
|
""" |
|
return numpy.linalg.inv(matrix) |
|
|
|
|
|
def concatenate_matrices(*matrices): |
|
"""Return concatenation of series of transformation matrices. |
|
|
|
>>> M = numpy.random.rand(16).reshape((4, 4)) - 0.5 |
|
>>> numpy.allclose(M, concatenate_matrices(M)) |
|
True |
|
>>> numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T)) |
|
True |
|
|
|
""" |
|
M = numpy.identity(4) |
|
for i in matrices: |
|
M = numpy.dot(M, i) |
|
return M |
|
|
|
|
|
def is_same_transform(matrix0, matrix1): |
|
"""Return True if two matrices perform same transformation. |
|
|
|
>>> is_same_transform(numpy.identity(4), numpy.identity(4)) |
|
True |
|
>>> is_same_transform(numpy.identity(4), random_rotation_matrix()) |
|
False |
|
|
|
""" |
|
matrix0 = numpy.array(matrix0, dtype=numpy.float64, copy=True) |
|
matrix0 /= matrix0[3, 3] |
|
matrix1 = numpy.array(matrix1, dtype=numpy.float64, copy=True) |
|
matrix1 /= matrix1[3, 3] |
|
return numpy.allclose(matrix0, matrix1) |
|
|
|
|
|
def _import_module(name, package=None, warn=True, prefix='_py_', ignore='_'): |
|
"""Try import all public attributes from module into global namespace. |
|
|
|
Existing attributes with name clashes are renamed with prefix. |
|
Attributes starting with underscore are ignored by default. |
|
|
|
Return True on successful import. |
|
|
|
""" |
|
import warnings |
|
from importlib import import_module |
|
try: |
|
if not package: |
|
module = import_module(name) |
|
else: |
|
module = import_module('.' + name, package=package) |
|
except ImportError: |
|
if warn: |
|
|
|
pass |
|
else: |
|
for attr in dir(module): |
|
if ignore and attr.startswith(ignore): |
|
continue |
|
if prefix: |
|
if attr in globals(): |
|
globals()[prefix + attr] = globals()[attr] |
|
elif warn: |
|
warnings.warn("no Python implementation of " + attr) |
|
globals()[attr] = getattr(module, attr) |
|
return True |
|
|
|
|
|
_import_module('_transformations') |
|
|
|
if __name__ == "__main__": |
|
import doctest |
|
import random |
|
numpy.set_printoptions(suppress=True, precision=5) |
|
doctest.testmod() |
|
|
|
|