├── pyproject.toml ├── .gitattributes ├── transformations ├── __init__.py ├── conftest.py └── transformations.py ├── MANIFEST.in ├── .github └── workflows │ └── wheel.yml ├── LICENSE ├── .gitignore ├── setup.py └── README.rst /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["setuptools", "numpy>2"] 3 | build-backend = "setuptools.build_meta" 4 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | *.ppm binary 4 | *.pbm binary 5 | *.pgm binary 6 | *.pnm binary 7 | *.pam binary 8 | *.container binary 9 | -------------------------------------------------------------------------------- /transformations/__init__.py: -------------------------------------------------------------------------------- 1 | # transformations/__init__.py 2 | 3 | from .transformations import * 4 | from .transformations import __all__, __doc__, __version__ 5 | 6 | 7 | def _set_module() -> None: 8 | """Set __module__ attribute for all public objects.""" 9 | globs = globals() 10 | module = globs['__name__'] 11 | for item in __all__: 12 | obj = globs[item] 13 | if hasattr(obj, '__module__'): 14 | obj.__module__ = module 15 | 16 | 17 | _set_module() 18 | -------------------------------------------------------------------------------- /transformations/conftest.py: -------------------------------------------------------------------------------- 1 | """Pytest configuration.""" 2 | 3 | import math 4 | import random 5 | 6 | import numpy 7 | import pytest 8 | 9 | numpy.set_printoptions(legacy='1.21') 10 | 11 | 12 | @pytest.fixture(autouse=True) 13 | def doctest_config(doctest_namespace): 14 | """Add random and numpy to doctest namespace.""" 15 | numpy.set_printoptions(suppress=True, precision=5) 16 | doctest_namespace['math'] = math 17 | doctest_namespace['numpy'] = numpy 18 | doctest_namespace['random'] = random 19 | -------------------------------------------------------------------------------- /MANIFEST.in: -------------------------------------------------------------------------------- 1 | include LICENSE 2 | include README.rst 3 | include CHANGES.rst 4 | include pyproject.toml 5 | 6 | include transformations/py.typed 7 | include transformations/transformations.c 8 | 9 | include .github/workflows/wheel.yml 10 | 11 | exclude *.cmd 12 | recursive-exclude doc * 13 | recursive-exclude docs * 14 | recursive-exclude test * 15 | recursive-exclude tests * 16 | 17 | recursive-exclude * __pycache__ 18 | recursive-exclude * *.py[co] 19 | recursive-exclude * *Copy* 20 | recursive-exclude * *- 21 | recursive-exclude * *.html 22 | -------------------------------------------------------------------------------- /.github/workflows/wheel.yml: -------------------------------------------------------------------------------- 1 | name: Build wheels 2 | 3 | on: 4 | workflow_dispatch: 5 | 6 | jobs: 7 | build: 8 | name: Build wheels on ${{ matrix.os }} 9 | runs-on: ${{ matrix.os }} 10 | strategy: 11 | matrix: 12 | os: [ubuntu-latest, macos-latest, windows-latest] 13 | steps: 14 | - uses: actions/checkout@v4 15 | - uses: pypa/cibuildwheel@v3.1.2 16 | env: 17 | # CIBW_ENVIRONMENT: "PIP_PRE=1" 18 | CIBW_BUILD_VERBOSITY: 2 19 | CIBW_SKIP: "pp* cp37* cp38* cp39* cp310* *musllinux* *i686 *ppc64le *s390x" 20 | # CIBW_ARCHS_LINUX: auto aarch64 21 | CIBW_ARCHS_LINUX: auto 22 | CIBW_ARCHS_MACOS: x86_64 arm64 23 | CIBW_ARCHS_WINDOWS: AMD64 ARM64 x86 24 | CIBW_TEST_REQUIRES: numpy pytest 25 | CIBW_TEST_COMMAND: python -m pytest --doctest-modules --pyargs transformations 26 | - uses: actions/upload-artifact@v4 27 | with: 28 | path: ./wheelhouse/*.whl 29 | name: wheels-${{ matrix.os }} 30 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD-3-Clause license 2 | 3 | Copyright (c) 2006-2025, Christoph Gohlke 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, 10 | this list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | _*.c 2 | *.wpr 3 | *.wpu 4 | .idea 5 | # Byte-compiled / optimized / DLL files 6 | __pycache__/ 7 | *.py[cod] 8 | *$py.class 9 | 10 | # C extensions 11 | *.so 12 | 13 | # Distribution / packaging 14 | .Python 15 | build/ 16 | develop-eggs/ 17 | dist/ 18 | downloads/ 19 | eggs/ 20 | .eggs/ 21 | lib/ 22 | lib64/ 23 | parts/ 24 | sdist/ 25 | var/ 26 | wheels/ 27 | pip-wheel-metadata/ 28 | share/python-wheels/ 29 | *.egg-info/ 30 | .installed.cfg 31 | *.egg 32 | MANIFEST 33 | setup.cfg 34 | PKG-INFO 35 | mypy.ini 36 | # PyInstaller 37 | # Usually these files are written by a python script from a template 38 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 39 | *.manifest 40 | *.spec 41 | 42 | # Installer logs 43 | pip-log.txt 44 | pip-delete-this-directory.txt 45 | 46 | # Unit test / coverage reports 47 | htmlcov/ 48 | .tox/ 49 | .nox/ 50 | .coverage 51 | .coverage.* 52 | .cache 53 | nosetests.xml 54 | coverage.xml 55 | *.cover 56 | *.py,cover 57 | .hypothesis/ 58 | .pytest_cache/ 59 | 60 | # Translations 61 | *.mo 62 | *.pot 63 | 64 | # Django stuff: 65 | *.log 66 | local_settings.py 67 | db.sqlite3 68 | db.sqlite3-journal 69 | 70 | # Flask stuff: 71 | instance/ 72 | .webassets-cache 73 | 74 | # Scrapy stuff: 75 | .scrapy 76 | 77 | # Sphinx documentation 78 | docs/_build/ 79 | 80 | # PyBuilder 81 | target/ 82 | 83 | # Jupyter Notebook 84 | .ipynb_checkpoints 85 | 86 | # IPython 87 | profile_default/ 88 | ipython_config.py 89 | 90 | # pyenv 91 | .python-version 92 | 93 | # pipenv 94 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 95 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 96 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 97 | # install all needed dependencies. 98 | #Pipfile.lock 99 | 100 | # celery beat schedule file 101 | celerybeat-schedule 102 | 103 | # SageMath parsed files 104 | *.sage.py 105 | 106 | # Environments 107 | .env 108 | .venv 109 | env/ 110 | venv/ 111 | ENV/ 112 | env.bak/ 113 | venv.bak/ 114 | 115 | # Spyder project settings 116 | .spyderproject 117 | .spyproject 118 | 119 | # Rope project settings 120 | .ropeproject 121 | 122 | # mkdocs documentation 123 | /site 124 | 125 | # mypy 126 | .mypy_cache/ 127 | .dmypy.json 128 | dmypy.json 129 | 130 | # Pyre type checker 131 | .pyre/ 132 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | # transformations/setup.py 2 | 3 | """Transformations package Setuptools script.""" 4 | 5 | import re 6 | import sys 7 | 8 | import numpy 9 | from setuptools import Extension, setup 10 | 11 | buildnumber = '' 12 | 13 | 14 | def search(pattern: str, string: str, flags: int = 0) -> str: 15 | """Return first match of pattern in string.""" 16 | match = re.search(pattern, string, flags) 17 | if match is None: 18 | raise ValueError(f'{pattern!r} not found') 19 | return match.groups()[0] 20 | 21 | 22 | def fix_docstring_examples(docstring: str) -> str: 23 | """Return docstring with examples fixed for GitHub.""" 24 | start = True 25 | indent = False 26 | lines = ['..', ' This file is generated by setup.py', ''] 27 | for line in docstring.splitlines(): 28 | if not line.strip(): 29 | start = True 30 | indent = False 31 | if line.startswith('>>> '): 32 | indent = True 33 | if start: 34 | lines.extend(['.. code-block:: python', '']) 35 | start = False 36 | lines.append((' ' if indent else '') + line) 37 | return '\n'.join(lines) 38 | 39 | 40 | with open('transformations/transformations.py', encoding='utf-8') as fh: 41 | code = fh.read() 42 | 43 | version = search(r"__version__ = '(.*?)'", code).replace('.x.x', '.dev0') 44 | version += ('.' + buildnumber) if buildnumber else '' 45 | 46 | description = search(r'"""(.*)\.(?:\r\n|\r|\n)', code) 47 | 48 | readme = search( 49 | r'(?:\r\n|\r|\n){2}"""(.*)"""(?:\r\n|\r|\n){2}from __future__', 50 | code, 51 | re.MULTILINE | re.DOTALL, 52 | ) 53 | readme = '\n'.join( 54 | [description, '=' * len(description)] + readme.splitlines()[1:] 55 | ) 56 | 57 | if 'sdist' in sys.argv: 58 | # update README, LICENSE, and CHANGES files 59 | 60 | with open('README.rst', 'w', encoding='utf-8') as fh: 61 | fh.write(fix_docstring_examples(readme)) 62 | 63 | license = search( 64 | r'(# Copyright.*?(?:\r\n|\r|\n))(?:\r\n|\r|\n)+""', 65 | code, 66 | re.MULTILINE | re.DOTALL, 67 | ) 68 | license = license.replace('# ', '').replace('#', '') 69 | 70 | with open('LICENSE', 'w', encoding='utf-8') as fh: 71 | fh.write('BSD-3-Clause license\n\n') 72 | fh.write(license) 73 | 74 | setup( 75 | name='transformations', 76 | version=version, 77 | license='BSD-3-Clause', 78 | description=description, 79 | long_description=readme, 80 | long_description_content_type='text/x-rst', 81 | author='Christoph Gohlke', 82 | author_email='cgohlke@cgohlke.com', 83 | url='https://www.cgohlke.com', 84 | project_urls={ 85 | 'Bug Tracker': 'https://github.com/cgohlke/transformations/issues', 86 | 'Source Code': 'https://github.com/cgohlke/transformations', 87 | # 'Documentation': 'https://', 88 | }, 89 | python_requires='>=3.11', 90 | install_requires=['numpy'], 91 | packages=['transformations'], 92 | # package_data={'akima': ['py.typed']}, 93 | ext_modules=[ 94 | Extension( 95 | 'transformations._transformations', 96 | ['transformations/transformations.c'], 97 | include_dirs=[numpy.get_include()], 98 | ) 99 | ], 100 | zip_safe=False, 101 | platforms=['any'], 102 | classifiers=[ 103 | 'Development Status :: 7 - Inactive', 104 | 'Intended Audience :: Science/Research', 105 | 'Intended Audience :: Developers', 106 | 'Operating System :: OS Independent', 107 | 'Programming Language :: C', 108 | 'Programming Language :: Python :: 3 :: Only', 109 | 'Programming Language :: Python :: 3.11', 110 | 'Programming Language :: Python :: 3.12', 111 | 'Programming Language :: Python :: 3.13', 112 | 'Programming Language :: Python :: 3.14', 113 | ], 114 | ) 115 | -------------------------------------------------------------------------------- /README.rst: -------------------------------------------------------------------------------- 1 | .. 2 | This file is generated by setup.py 3 | 4 | Homogeneous Transformation Matrices and Quaternions 5 | =================================================== 6 | 7 | Transformations is a Python library for calculating 4x4 matrices for 8 | translating, rotating, reflecting, scaling, shearing, projecting, 9 | orthogonalizing, and superimposing arrays of 3D homogeneous coordinates 10 | as well as for converting between rotation matrices, Euler angles, 11 | and quaternions. Also includes an Arcball control object and 12 | functions to decompose transformation matrices. 13 | 14 | The transformations library is no longer actively developed. 15 | 16 | :Author: `Christoph Gohlke `_ 17 | :License: BSD-3-Clause 18 | :Version: 2025.8.1 19 | 20 | Quickstart 21 | ---------- 22 | 23 | Install the transformations package and all dependencies from the 24 | `Python Package Index `_:: 25 | 26 | python -m pip install -U transformations 27 | 28 | See `Examples`_ for using the programming interface. 29 | 30 | Source code and support are available on 31 | `GitHub `_. 32 | 33 | Requirements 34 | ------------ 35 | 36 | This revision was tested with the following requirements and dependencies 37 | (other versions may work): 38 | 39 | - `CPython `_ 3.11.9, 3.12.10, 3.13.5, 3.14.0rc 64-bit 40 | - `NumPy `_ 2.3.2 41 | 42 | Revisions 43 | --------- 44 | 45 | 2025.8.1 46 | 47 | - Drop support for Python 3.10, support Python 3.14. 48 | 49 | 2025.1.1 50 | 51 | - Drop support for Python 3.9, support Python 3.13. 52 | 53 | 2024.5.24 54 | 55 | - Fix docstring examples not correctly rendered on GitHub. 56 | 57 | 2024.4.24 58 | 59 | - Support NumPy 2. 60 | 61 | 2024.1.6 62 | 63 | - Remove support for Python 3.8 and numpy 1.22 (NEP 29). 64 | 65 | 2022.9.26 66 | 67 | - Add dimension check on superimposition_matrix (#2). 68 | 69 | 2022.8.26 70 | 71 | - Update metadata 72 | - Remove support for Python 3.7 (NEP 29). 73 | 74 | 2021.6.6 75 | 76 | - Remove support for Python 3.6 (NEP 29). 77 | 78 | 2020.1.1 79 | 80 | - Remove support for Python 2.7 and 3.5. 81 | 82 | 2019.4.22 83 | 84 | - Fix setup requirements. 85 | 86 | Notes 87 | ----- 88 | 89 | Transformations.py is no longer actively developed and has a few known issues 90 | and numerical instabilities. The module is mostly superseded by other modules 91 | for 3D transformations and quaternions: 92 | 93 | - `Pytransform3d `_ 94 | - `Scipy.spatial.transform 95 | `_ 96 | - `Transforms3d `_ 97 | (includes most code of this module) 98 | - `Numpy-quaternion `_ 99 | - `Blender.mathutils `_ 100 | 101 | The API is not stable yet and is expected to change between revisions. 102 | 103 | This Python code is not optimized for speed. Refer to the transformations.c 104 | module for a faster implementation of some functions. 105 | 106 | Documentation in HTML format can be generated with epydoc. 107 | 108 | Matrices (M) can be inverted using numpy.linalg.inv(M), be concatenated using 109 | numpy.dot(M0, M1), or transform homogeneous coordinate arrays (v) using 110 | numpy.dot(M, v) for shape (4, -1) column vectors, respectively 111 | numpy.dot(v, M.T) for shape (-1, 4) row vectors ("array of points"). 112 | 113 | This module follows the "column vectors on the right" and "row major storage" 114 | (C contiguous) conventions. The translation components are in the right column 115 | of the transformation matrix, i.e. M[:3, 3]. 116 | The transpose of the transformation matrices may have to be used to interface 117 | with other graphics systems, e.g. OpenGL's glMultMatrixd(). See also [16]. 118 | 119 | Calculations are carried out with numpy.float64 precision. 120 | 121 | Vector, point, quaternion, and matrix function arguments are expected to be 122 | "array like", i.e. tuple, list, or numpy arrays. 123 | 124 | Return types are numpy arrays unless specified otherwise. 125 | 126 | Angles are in radians unless specified otherwise. 127 | 128 | Quaternions w+ix+jy+kz are represented as [w, x, y, z]. 129 | 130 | A triple of Euler angles can be applied/interpreted in 24 ways, which can 131 | be specified using a 4 character string or encoded 4-tuple: 132 | 133 | *Axes 4-string*: e.g. 'sxyz' or 'ryxy' 134 | 135 | - first character : rotations are applied to 's'tatic or 'r'otating frame 136 | - remaining characters : successive rotation axis 'x', 'y', or 'z' 137 | 138 | *Axes 4-tuple*: e.g. (0, 0, 0, 0) or (1, 1, 1, 1) 139 | 140 | - inner axis: code of axis ('x':0, 'y':1, 'z':2) of rightmost matrix. 141 | - parity : even (0) if inner axis 'x' is followed by 'y', 'y' is followed 142 | by 'z', or 'z' is followed by 'x'. Otherwise odd (1). 143 | - repetition : first and last axis are same (1) or different (0). 144 | - frame : rotations are applied to static (0) or rotating (1) frame. 145 | 146 | References 147 | ---------- 148 | 149 | 1. Matrices and transformations. Ronald Goldman. 150 | In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990. 151 | 2. More matrices and transformations: shear and pseudo-perspective. 152 | Ronald Goldman. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. 153 | 3. Decomposing a matrix into simple transformations. Spencer Thomas. 154 | In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. 155 | 4. Recovering the data from the transformation matrix. Ronald Goldman. 156 | In "Graphics Gems II", pp 324-331. Morgan Kaufmann, 1991. 157 | 5. Euler angle conversion. Ken Shoemake. 158 | In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994. 159 | 6. Arcball rotation control. Ken Shoemake. 160 | In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994. 161 | 7. Representing attitude: Euler angles, unit quaternions, and rotation 162 | vectors. James Diebel. 2006. 163 | 8. A discussion of the solution for the best rotation to relate two sets 164 | of vectors. W Kabsch. Acta Cryst. 1978. A34, 827-828. 165 | 9. Closed-form solution of absolute orientation using unit quaternions. 166 | BKP Horn. J Opt Soc Am A. 1987. 4(4):629-642. 167 | 10. Quaternions. Ken Shoemake. 168 | http://www.sfu.ca/~jwa3/cmpt461/files/quatut.pdf 169 | 11. From quaternion to matrix and back. JMP van Waveren. 2005. 170 | http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm 171 | 12. Uniform random rotations. Ken Shoemake. 172 | In "Graphics Gems III", pp 124-132. Morgan Kaufmann, 1992. 173 | 13. Quaternion in molecular modeling. CFF Karney. 174 | J Mol Graph Mod, 25(5):595-604 175 | 14. New method for extracting the quaternion from a rotation matrix. 176 | Itzhack Y Bar-Itzhack, J Guid Contr Dynam. 2000. 23(6): 1085-1087. 177 | 15. Multiple View Geometry in Computer Vision. Hartley and Zissermann. 178 | Cambridge University Press; 2nd Ed. 2004. Chapter 4, Algorithm 4.7, p 130. 179 | 16. Column Vectors vs. Row Vectors. 180 | http://steve.hollasch.net/cgindex/math/matrix/column-vec.html 181 | 182 | Examples 183 | -------- 184 | 185 | .. code-block:: python 186 | 187 | >>> alpha, beta, gamma = 0.123, -1.234, 2.345 188 | >>> origin, xaxis, yaxis, zaxis = [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1] 189 | >>> I = identity_matrix() 190 | >>> Rx = rotation_matrix(alpha, xaxis) 191 | >>> Ry = rotation_matrix(beta, yaxis) 192 | >>> Rz = rotation_matrix(gamma, zaxis) 193 | >>> R = concatenate_matrices(Rx, Ry, Rz) 194 | >>> euler = euler_from_matrix(R, 'rxyz') 195 | >>> numpy.allclose([alpha, beta, gamma], euler) 196 | True 197 | >>> Re = euler_matrix(alpha, beta, gamma, 'rxyz') 198 | >>> is_same_transform(R, Re) 199 | True 200 | >>> al, be, ga = euler_from_matrix(Re, 'rxyz') 201 | >>> is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz')) 202 | True 203 | >>> qx = quaternion_about_axis(alpha, xaxis) 204 | >>> qy = quaternion_about_axis(beta, yaxis) 205 | >>> qz = quaternion_about_axis(gamma, zaxis) 206 | >>> q = quaternion_multiply(qx, qy) 207 | >>> q = quaternion_multiply(q, qz) 208 | >>> Rq = quaternion_matrix(q) 209 | >>> is_same_transform(R, Rq) 210 | True 211 | >>> S = scale_matrix(1.23, origin) 212 | >>> T = translation_matrix([1, 2, 3]) 213 | >>> Z = shear_matrix(beta, xaxis, origin, zaxis) 214 | >>> R = random_rotation_matrix(numpy.random.rand(3)) 215 | >>> M = concatenate_matrices(T, R, Z, S) 216 | >>> scale, shear, angles, trans, persp = decompose_matrix(M) 217 | >>> numpy.allclose(scale, 1.23) 218 | True 219 | >>> numpy.allclose(trans, [1, 2, 3]) 220 | True 221 | >>> numpy.allclose(shear, [0, math.tan(beta), 0]) 222 | True 223 | >>> is_same_transform(R, euler_matrix(axes='sxyz', *angles)) 224 | True 225 | >>> M1 = compose_matrix(scale, shear, angles, trans, persp) 226 | >>> is_same_transform(M, M1) 227 | True 228 | >>> v0, v1 = random_vector(3), random_vector(3) 229 | >>> M = rotation_matrix(angle_between_vectors(v0, v1), vector_product(v0, v1)) 230 | >>> v2 = numpy.dot(v0, M[:3, :3].T) 231 | >>> numpy.allclose(unit_vector(v1), unit_vector(v2)) 232 | True -------------------------------------------------------------------------------- /transformations/transformations.py: -------------------------------------------------------------------------------- 1 | # transformations.py 2 | 3 | # Copyright (c) 2006-2025, Christoph Gohlke 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | # 9 | # 1. Redistributions of source code must retain the above copyright notice, 10 | # this list of conditions and the following disclaimer. 11 | # 12 | # 2. Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # 16 | # 3. Neither the name of the copyright holder nor the names of its 17 | # contributors may be used to endorse or promote products derived from 18 | # this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | # POSSIBILITY OF SUCH DAMAGE. 31 | 32 | """Homogeneous Transformation Matrices and Quaternions. 33 | 34 | Transformations is a Python library for calculating 4x4 matrices for 35 | translating, rotating, reflecting, scaling, shearing, projecting, 36 | orthogonalizing, and superimposing arrays of 3D homogeneous coordinates 37 | as well as for converting between rotation matrices, Euler angles, 38 | and quaternions. Also includes an Arcball control object and 39 | functions to decompose transformation matrices. 40 | 41 | The transformations library is no longer actively developed. 42 | 43 | :Author: `Christoph Gohlke `_ 44 | :License: BSD-3-Clause 45 | :Version: 2025.8.1 46 | 47 | Quickstart 48 | ---------- 49 | 50 | Install the transformations package and all dependencies from the 51 | `Python Package Index `_:: 52 | 53 | python -m pip install -U transformations 54 | 55 | See `Examples`_ for using the programming interface. 56 | 57 | Source code and support are available on 58 | `GitHub `_. 59 | 60 | Requirements 61 | ------------ 62 | 63 | This revision was tested with the following requirements and dependencies 64 | (other versions may work): 65 | 66 | - `CPython `_ 3.11.9, 3.12.10, 3.13.5, 3.14.0rc 64-bit 67 | - `NumPy `_ 2.3.2 68 | 69 | Revisions 70 | --------- 71 | 72 | 2025.8.1 73 | 74 | - Drop support for Python 3.10, support Python 3.14. 75 | 76 | 2025.1.1 77 | 78 | - Drop support for Python 3.9, support Python 3.13. 79 | 80 | 2024.5.24 81 | 82 | - Fix docstring examples not correctly rendered on GitHub. 83 | 84 | 2024.4.24 85 | 86 | - Support NumPy 2. 87 | 88 | 2024.1.6 89 | 90 | - Remove support for Python 3.8 and numpy 1.22 (NEP 29). 91 | 92 | 2022.9.26 93 | 94 | - Add dimension check on superimposition_matrix (#2). 95 | 96 | 2022.8.26 97 | 98 | - Update metadata 99 | - Remove support for Python 3.7 (NEP 29). 100 | 101 | 2021.6.6 102 | 103 | - Remove support for Python 3.6 (NEP 29). 104 | 105 | 2020.1.1 106 | 107 | - Remove support for Python 2.7 and 3.5. 108 | 109 | 2019.4.22 110 | 111 | - Fix setup requirements. 112 | 113 | Notes 114 | ----- 115 | 116 | Transformations.py is no longer actively developed and has a few known issues 117 | and numerical instabilities. The module is mostly superseded by other modules 118 | for 3D transformations and quaternions: 119 | 120 | - `Pytransform3d `_ 121 | - `Scipy.spatial.transform 122 | `_ 123 | - `Transforms3d `_ 124 | (includes most code of this module) 125 | - `Numpy-quaternion `_ 126 | - `Blender.mathutils `_ 127 | 128 | The API is not stable yet and is expected to change between revisions. 129 | 130 | This Python code is not optimized for speed. Refer to the transformations.c 131 | module for a faster implementation of some functions. 132 | 133 | Documentation in HTML format can be generated with epydoc. 134 | 135 | Matrices (M) can be inverted using numpy.linalg.inv(M), be concatenated using 136 | numpy.dot(M0, M1), or transform homogeneous coordinate arrays (v) using 137 | numpy.dot(M, v) for shape (4, -1) column vectors, respectively 138 | numpy.dot(v, M.T) for shape (-1, 4) row vectors ("array of points"). 139 | 140 | This module follows the "column vectors on the right" and "row major storage" 141 | (C contiguous) conventions. The translation components are in the right column 142 | of the transformation matrix, i.e. M[:3, 3]. 143 | The transpose of the transformation matrices may have to be used to interface 144 | with other graphics systems, e.g. OpenGL's glMultMatrixd(). See also [16]. 145 | 146 | Calculations are carried out with numpy.float64 precision. 147 | 148 | Vector, point, quaternion, and matrix function arguments are expected to be 149 | "array like", i.e. tuple, list, or numpy arrays. 150 | 151 | Return types are numpy arrays unless specified otherwise. 152 | 153 | Angles are in radians unless specified otherwise. 154 | 155 | Quaternions w+ix+jy+kz are represented as [w, x, y, z]. 156 | 157 | A triple of Euler angles can be applied/interpreted in 24 ways, which can 158 | be specified using a 4 character string or encoded 4-tuple: 159 | 160 | *Axes 4-string*: e.g. 'sxyz' or 'ryxy' 161 | 162 | - first character : rotations are applied to 's'tatic or 'r'otating frame 163 | - remaining characters : successive rotation axis 'x', 'y', or 'z' 164 | 165 | *Axes 4-tuple*: e.g. (0, 0, 0, 0) or (1, 1, 1, 1) 166 | 167 | - inner axis: code of axis ('x':0, 'y':1, 'z':2) of rightmost matrix. 168 | - parity : even (0) if inner axis 'x' is followed by 'y', 'y' is followed 169 | by 'z', or 'z' is followed by 'x'. Otherwise odd (1). 170 | - repetition : first and last axis are same (1) or different (0). 171 | - frame : rotations are applied to static (0) or rotating (1) frame. 172 | 173 | References 174 | ---------- 175 | 176 | 1. Matrices and transformations. Ronald Goldman. 177 | In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990. 178 | 2. More matrices and transformations: shear and pseudo-perspective. 179 | Ronald Goldman. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. 180 | 3. Decomposing a matrix into simple transformations. Spencer Thomas. 181 | In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. 182 | 4. Recovering the data from the transformation matrix. Ronald Goldman. 183 | In "Graphics Gems II", pp 324-331. Morgan Kaufmann, 1991. 184 | 5. Euler angle conversion. Ken Shoemake. 185 | In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994. 186 | 6. Arcball rotation control. Ken Shoemake. 187 | In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994. 188 | 7. Representing attitude: Euler angles, unit quaternions, and rotation 189 | vectors. James Diebel. 2006. 190 | 8. A discussion of the solution for the best rotation to relate two sets 191 | of vectors. W Kabsch. Acta Cryst. 1978. A34, 827-828. 192 | 9. Closed-form solution of absolute orientation using unit quaternions. 193 | BKP Horn. J Opt Soc Am A. 1987. 4(4):629-642. 194 | 10. Quaternions. Ken Shoemake. 195 | http://www.sfu.ca/~jwa3/cmpt461/files/quatut.pdf 196 | 11. From quaternion to matrix and back. JMP van Waveren. 2005. 197 | http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm 198 | 12. Uniform random rotations. Ken Shoemake. 199 | In "Graphics Gems III", pp 124-132. Morgan Kaufmann, 1992. 200 | 13. Quaternion in molecular modeling. CFF Karney. 201 | J Mol Graph Mod, 25(5):595-604 202 | 14. New method for extracting the quaternion from a rotation matrix. 203 | Itzhack Y Bar-Itzhack, J Guid Contr Dynam. 2000. 23(6): 1085-1087. 204 | 15. Multiple View Geometry in Computer Vision. Hartley and Zissermann. 205 | Cambridge University Press; 2nd Ed. 2004. Chapter 4, Algorithm 4.7, p 130. 206 | 16. Column Vectors vs. Row Vectors. 207 | http://steve.hollasch.net/cgindex/math/matrix/column-vec.html 208 | 209 | Examples 210 | -------- 211 | 212 | >>> alpha, beta, gamma = 0.123, -1.234, 2.345 213 | >>> origin, xaxis, yaxis, zaxis = [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1] 214 | >>> I = identity_matrix() 215 | >>> Rx = rotation_matrix(alpha, xaxis) 216 | >>> Ry = rotation_matrix(beta, yaxis) 217 | >>> Rz = rotation_matrix(gamma, zaxis) 218 | >>> R = concatenate_matrices(Rx, Ry, Rz) 219 | >>> euler = euler_from_matrix(R, 'rxyz') 220 | >>> numpy.allclose([alpha, beta, gamma], euler) 221 | True 222 | >>> Re = euler_matrix(alpha, beta, gamma, 'rxyz') 223 | >>> is_same_transform(R, Re) 224 | True 225 | >>> al, be, ga = euler_from_matrix(Re, 'rxyz') 226 | >>> is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz')) 227 | True 228 | >>> qx = quaternion_about_axis(alpha, xaxis) 229 | >>> qy = quaternion_about_axis(beta, yaxis) 230 | >>> qz = quaternion_about_axis(gamma, zaxis) 231 | >>> q = quaternion_multiply(qx, qy) 232 | >>> q = quaternion_multiply(q, qz) 233 | >>> Rq = quaternion_matrix(q) 234 | >>> is_same_transform(R, Rq) 235 | True 236 | >>> S = scale_matrix(1.23, origin) 237 | >>> T = translation_matrix([1, 2, 3]) 238 | >>> Z = shear_matrix(beta, xaxis, origin, zaxis) 239 | >>> R = random_rotation_matrix(numpy.random.rand(3)) 240 | >>> M = concatenate_matrices(T, R, Z, S) 241 | >>> scale, shear, angles, trans, persp = decompose_matrix(M) 242 | >>> numpy.allclose(scale, 1.23) 243 | True 244 | >>> numpy.allclose(trans, [1, 2, 3]) 245 | True 246 | >>> numpy.allclose(shear, [0, math.tan(beta), 0]) 247 | True 248 | >>> is_same_transform(R, euler_matrix(axes='sxyz', *angles)) 249 | True 250 | >>> M1 = compose_matrix(scale, shear, angles, trans, persp) 251 | >>> is_same_transform(M, M1) 252 | True 253 | >>> v0, v1 = random_vector(3), random_vector(3) 254 | >>> M = rotation_matrix(angle_between_vectors(v0, v1), vector_product(v0, v1)) 255 | >>> v2 = numpy.dot(v0, M[:3, :3].T) 256 | >>> numpy.allclose(unit_vector(v1), unit_vector(v2)) 257 | True 258 | 259 | """ 260 | 261 | from __future__ import annotations 262 | 263 | __version__ = '2025.8.1' 264 | 265 | __all__ = [ 266 | '__version__', 267 | 'affine_matrix_from_points', 268 | 'angle_between_vectors', 269 | 'arcball_constrain_to_axis', 270 | 'arcball_map_to_sphere', 271 | 'arcball_nearest_axis', 272 | 'clip_matrix', 273 | 'compose_matrix', 274 | 'concatenate_matrices', 275 | 'decompose_matrix', 276 | 'euler_from_matrix', 277 | 'euler_from_quaternion', 278 | 'euler_matrix', 279 | 'identity_matrix', 280 | 'inverse_matrix', 281 | 'is_same_quaternion', 282 | 'is_same_transform', 283 | 'orthogonalization_matrix', 284 | 'projection_from_matrix', 285 | 'projection_matrix', 286 | 'quaternion_about_axis', 287 | 'quaternion_conjugate', 288 | 'quaternion_from_euler', 289 | 'quaternion_from_matrix', 290 | 'quaternion_imag', 291 | 'quaternion_inverse', 292 | 'quaternion_matrix', 293 | 'quaternion_multiply', 294 | 'quaternion_real', 295 | 'quaternion_slerp', 296 | 'random_quaternion', 297 | 'random_rotation_matrix', 298 | 'random_vector', 299 | 'reflection_from_matrix', 300 | 'reflection_matrix', 301 | 'rotation_from_matrix', 302 | 'rotation_matrix', 303 | 'scale_from_matrix', 304 | 'scale_matrix', 305 | 'shear_from_matrix', 306 | 'shear_matrix', 307 | 'superimposition_matrix', 308 | 'translation_from_matrix', 309 | 'translation_matrix', 310 | 'unit_vector', 311 | 'vector_norm', 312 | 'vector_product', 313 | '_AXES2TUPLE', 314 | '_TUPLE2AXES', 315 | ] 316 | 317 | import math 318 | 319 | import numpy 320 | 321 | 322 | def identity_matrix(): 323 | """Return 4x4 identity/unit matrix. 324 | 325 | >>> I = identity_matrix() 326 | >>> numpy.allclose(I, numpy.dot(I, I)) 327 | True 328 | >>> float(numpy.sum(I)), float(numpy.trace(I)) 329 | (4.0, 4.0) 330 | >>> numpy.allclose(I, numpy.identity(4)) 331 | True 332 | 333 | """ 334 | return numpy.identity(4) 335 | 336 | 337 | def translation_matrix(direction): 338 | """Return matrix to translate by direction vector. 339 | 340 | >>> v = numpy.random.random(3) - 0.5 341 | >>> numpy.allclose(v, translation_matrix(v)[:3, 3]) 342 | True 343 | 344 | """ 345 | M = numpy.identity(4) 346 | M[:3, 3] = direction[:3] 347 | return M 348 | 349 | 350 | def translation_from_matrix(matrix): 351 | """Return translation vector from translation matrix. 352 | 353 | >>> v0 = numpy.random.random(3) - 0.5 354 | >>> v1 = translation_from_matrix(translation_matrix(v0)) 355 | >>> numpy.allclose(v0, v1) 356 | True 357 | 358 | """ 359 | return numpy.asarray(matrix)[:3, 3].copy() 360 | 361 | 362 | def reflection_matrix(point, normal): 363 | """Return matrix to mirror at plane defined by point and normal vector. 364 | 365 | >>> v0 = numpy.random.random(4) - 0.5 366 | >>> v0[3] = 1.0 367 | >>> v1 = numpy.random.random(3) - 0.5 368 | >>> R = reflection_matrix(v0, v1) 369 | >>> numpy.allclose(2, numpy.trace(R)) 370 | True 371 | >>> numpy.allclose(v0, numpy.dot(R, v0)) 372 | True 373 | >>> v2 = v0.copy() 374 | >>> v2[:3] += v1 375 | >>> v3 = v0.copy() 376 | >>> v2[:3] -= v1 377 | >>> numpy.allclose(v2, numpy.dot(R, v3)) 378 | True 379 | 380 | """ 381 | normal = unit_vector(normal[:3]) 382 | M = numpy.identity(4) 383 | M[:3, :3] -= 2.0 * numpy.outer(normal, normal) 384 | M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal 385 | return M 386 | 387 | 388 | def reflection_from_matrix(matrix): 389 | """Return mirror plane point and normal vector from reflection matrix. 390 | 391 | >>> v0 = numpy.random.random(3) - 0.5 392 | >>> v1 = numpy.random.random(3) - 0.5 393 | >>> M0 = reflection_matrix(v0, v1) 394 | >>> point, normal = reflection_from_matrix(M0) 395 | >>> M1 = reflection_matrix(point, normal) 396 | >>> is_same_transform(M0, M1) 397 | True 398 | 399 | """ 400 | M = numpy.asarray(matrix, dtype=numpy.float64) 401 | # normal: unit eigenvector corresponding to eigenvalue -1 402 | w, V = numpy.linalg.eig(M[:3, :3]) 403 | i = numpy.where(abs(numpy.real(w) + 1.0) < 1e-8)[0] 404 | if len(i) == 0: 405 | raise ValueError('no unit eigenvector corresponding to eigenvalue -1') 406 | normal = numpy.real(V[:, i[0]]).squeeze() 407 | # point: any unit eigenvector corresponding to eigenvalue 1 408 | w, V = numpy.linalg.eig(M) 409 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 410 | if len(i) == 0: 411 | raise ValueError('no unit eigenvector corresponding to eigenvalue 1') 412 | point = numpy.real(V[:, i[-1]]).squeeze() 413 | point /= point[3] 414 | return point, normal 415 | 416 | 417 | def rotation_matrix(angle, direction, point=None): 418 | """Return matrix to rotate about axis defined by point and direction. 419 | 420 | >>> R = rotation_matrix(math.pi / 2, [0, 0, 1], [1, 0, 0]) 421 | >>> numpy.allclose(numpy.dot(R, [0, 0, 0, 1]), [1, -1, 0, 1]) 422 | True 423 | >>> angle = (random.random() - 0.5) * (2 * math.pi) 424 | >>> direc = numpy.random.random(3) - 0.5 425 | >>> point = numpy.random.random(3) - 0.5 426 | >>> R0 = rotation_matrix(angle, direc, point) 427 | >>> R1 = rotation_matrix(angle - 2 * math.pi, direc, point) 428 | >>> is_same_transform(R0, R1) 429 | True 430 | >>> R0 = rotation_matrix(angle, direc, point) 431 | >>> R1 = rotation_matrix(-angle, -direc, point) 432 | >>> is_same_transform(R0, R1) 433 | True 434 | >>> I = numpy.identity(4, numpy.float64) 435 | >>> numpy.allclose(I, rotation_matrix(math.pi * 2, direc)) 436 | True 437 | >>> numpy.allclose( 438 | ... 2, numpy.trace(rotation_matrix(math.pi / 2, direc, point)) 439 | ... ) 440 | True 441 | 442 | """ 443 | sina = math.sin(angle) 444 | cosa = math.cos(angle) 445 | direction = unit_vector(direction[:3]) 446 | # rotation matrix around unit vector 447 | R = numpy.diag([cosa, cosa, cosa]) 448 | R += numpy.outer(direction, direction) * (1.0 - cosa) 449 | direction *= sina 450 | R += numpy.array( 451 | [ 452 | [0.0, -direction[2], direction[1]], 453 | [direction[2], 0.0, -direction[0]], 454 | [-direction[1], direction[0], 0.0], 455 | ] 456 | ) 457 | M = numpy.identity(4) 458 | M[:3, :3] = R 459 | if point is not None: 460 | # rotation not around origin 461 | point = numpy.asarray(point[:3], dtype=numpy.float64) 462 | M[:3, 3] = point - numpy.dot(R, point) 463 | return M 464 | 465 | 466 | def rotation_from_matrix(matrix): 467 | """Return rotation angle and axis from rotation matrix. 468 | 469 | >>> angle = (random.random() - 0.5) * (2 * math.pi) 470 | >>> direc = numpy.random.random(3) - 0.5 471 | >>> point = numpy.random.random(3) - 0.5 472 | >>> R0 = rotation_matrix(angle, direc, point) 473 | >>> angle, direc, point = rotation_from_matrix(R0) 474 | >>> R1 = rotation_matrix(angle, direc, point) 475 | >>> is_same_transform(R0, R1) 476 | True 477 | 478 | """ 479 | R = numpy.asarray(matrix, dtype=numpy.float64) 480 | R33 = R[:3, :3] 481 | # direction: unit eigenvector of R33 corresponding to eigenvalue of 1 482 | w, W = numpy.linalg.eig(R33.T) 483 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 484 | if len(i) == 0: 485 | raise ValueError('no unit eigenvector corresponding to eigenvalue 1') 486 | direction = numpy.real(W[:, i[-1]]).squeeze() 487 | # point: unit eigenvector of R33 corresponding to eigenvalue of 1 488 | w, Q = numpy.linalg.eig(R) 489 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 490 | if len(i) == 0: 491 | raise ValueError('no unit eigenvector corresponding to eigenvalue 1') 492 | point = numpy.real(Q[:, i[-1]]).squeeze() 493 | point /= point[3] 494 | # rotation angle depending on direction 495 | cosa = (numpy.trace(R33) - 1.0) / 2.0 496 | if abs(direction[2]) > 1e-8: 497 | sina = ( 498 | R[1, 0] + (cosa - 1.0) * direction[0] * direction[1] 499 | ) / direction[2] 500 | elif abs(direction[1]) > 1e-8: 501 | sina = ( 502 | R[0, 2] + (cosa - 1.0) * direction[0] * direction[2] 503 | ) / direction[1] 504 | else: 505 | sina = ( 506 | R[2, 1] + (cosa - 1.0) * direction[1] * direction[2] 507 | ) / direction[0] 508 | angle = math.atan2(sina, cosa) 509 | return angle, direction, point 510 | 511 | 512 | def scale_matrix(factor, origin=None, direction=None): 513 | """Return matrix to scale by factor around origin in direction. 514 | 515 | Use factor -1 for point symmetry. 516 | 517 | >>> v = (numpy.random.rand(4, 5) - 0.5) * 20 518 | >>> v[3] = 1 519 | >>> S = scale_matrix(-1.234) 520 | >>> numpy.allclose(numpy.dot(S, v)[:3], -1.234 * v[:3]) 521 | True 522 | >>> factor = random.random() * 10 - 5 523 | >>> origin = numpy.random.random(3) - 0.5 524 | >>> direct = numpy.random.random(3) - 0.5 525 | >>> S = scale_matrix(factor, origin) 526 | >>> S = scale_matrix(factor, origin, direct) 527 | 528 | """ 529 | if direction is None: 530 | # uniform scaling 531 | M = numpy.diag([factor, factor, factor, 1.0]) 532 | if origin is not None: 533 | M[:3, 3] = origin[:3] 534 | M[:3, 3] *= 1.0 - factor 535 | else: 536 | # nonuniform scaling 537 | direction = unit_vector(direction[:3]) 538 | factor = 1.0 - factor 539 | M = numpy.identity(4) 540 | M[:3, :3] -= factor * numpy.outer(direction, direction) 541 | if origin is not None: 542 | M[:3, 3] = (factor * numpy.dot(origin[:3], direction)) * direction 543 | return M 544 | 545 | 546 | def scale_from_matrix(matrix): 547 | """Return scaling factor, origin and direction from scaling matrix. 548 | 549 | >>> factor = random.random() * 10 - 5 550 | >>> origin = numpy.random.random(3) - 0.5 551 | >>> direct = numpy.random.random(3) - 0.5 552 | >>> S0 = scale_matrix(factor, origin) 553 | >>> factor, origin, direction = scale_from_matrix(S0) 554 | >>> S1 = scale_matrix(factor, origin, direction) 555 | >>> is_same_transform(S0, S1) 556 | True 557 | >>> S0 = scale_matrix(factor, origin, direct) 558 | >>> factor, origin, direction = scale_from_matrix(S0) 559 | >>> S1 = scale_matrix(factor, origin, direction) 560 | >>> is_same_transform(S0, S1) 561 | True 562 | 563 | """ 564 | M = numpy.asarray(matrix, dtype=numpy.float64) 565 | M33 = M[:3, :3] 566 | factor = numpy.trace(M33) - 2.0 567 | try: 568 | # direction: unit eigenvector corresponding to eigenvalue factor 569 | w, V = numpy.linalg.eig(M33) 570 | i = numpy.where(abs(numpy.real(w) - factor) < 1e-8)[0][0] 571 | direction = numpy.real(V[:, i]).squeeze() 572 | direction /= vector_norm(direction) 573 | except IndexError: 574 | # uniform scaling 575 | factor = (factor + 2.0) / 3.0 576 | direction = None 577 | # origin: any eigenvector corresponding to eigenvalue 1 578 | w, V = numpy.linalg.eig(M) 579 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 580 | if len(i) == 0: 581 | raise ValueError('no eigenvector corresponding to eigenvalue 1') 582 | origin = numpy.real(V[:, i[-1]]).squeeze() 583 | origin /= origin[3] 584 | return factor, origin, direction 585 | 586 | 587 | def projection_matrix( 588 | point, normal, direction=None, perspective=None, pseudo=False 589 | ): 590 | """Return matrix to project onto plane defined by point and normal. 591 | 592 | Using either perspective point, projection direction, or none of both. 593 | 594 | If pseudo is True, perspective projections will preserve relative depth 595 | such that Perspective = dot(Orthogonal, PseudoPerspective). 596 | 597 | >>> P = projection_matrix([0, 0, 0], [1, 0, 0]) 598 | >>> numpy.allclose(P[1:, 1:], numpy.identity(4)[1:, 1:]) 599 | True 600 | >>> point = numpy.random.random(3) - 0.5 601 | >>> normal = numpy.random.random(3) - 0.5 602 | >>> direct = numpy.random.random(3) - 0.5 603 | >>> persp = numpy.random.random(3) - 0.5 604 | >>> P0 = projection_matrix(point, normal) 605 | >>> P1 = projection_matrix(point, normal, direction=direct) 606 | >>> P2 = projection_matrix(point, normal, perspective=persp) 607 | >>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True) 608 | >>> is_same_transform(P2, numpy.dot(P0, P3)) 609 | True 610 | >>> P = projection_matrix([3, 0, 0], [1, 1, 0], [1, 0, 0]) 611 | >>> v0 = (numpy.random.rand(4, 5) - 0.5) * 20 612 | >>> v0[3] = 1 613 | >>> v1 = numpy.dot(P, v0) 614 | >>> numpy.allclose(v1[1], v0[1]) 615 | True 616 | >>> numpy.allclose(v1[0], 3 - v1[1]) 617 | True 618 | 619 | """ 620 | M = numpy.identity(4) 621 | point = numpy.asarray(point[:3], dtype=numpy.float64) 622 | normal = unit_vector(normal[:3]) 623 | if perspective is not None: 624 | # perspective projection 625 | perspective = numpy.asarray(perspective[:3], dtype=numpy.float64) 626 | M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective - point, normal) 627 | M[:3, :3] -= numpy.outer(perspective, normal) 628 | if pseudo: 629 | # preserve relative depth 630 | M[:3, :3] -= numpy.outer(normal, normal) 631 | M[:3, 3] = numpy.dot(point, normal) * (perspective + normal) 632 | else: 633 | M[:3, 3] = numpy.dot(point, normal) * perspective 634 | M[3, :3] = -normal 635 | M[3, 3] = numpy.dot(perspective, normal) 636 | elif direction is not None: 637 | # parallel projection 638 | direction = numpy.asarray(direction[:3], dtype=numpy.float64) 639 | scale = numpy.dot(direction, normal) 640 | M[:3, :3] -= numpy.outer(direction, normal) / scale 641 | M[:3, 3] = direction * (numpy.dot(point, normal) / scale) 642 | else: 643 | # orthogonal projection 644 | M[:3, :3] -= numpy.outer(normal, normal) 645 | M[:3, 3] = numpy.dot(point, normal) * normal 646 | return M 647 | 648 | 649 | def projection_from_matrix(matrix, pseudo=False): 650 | """Return projection plane and perspective point from projection matrix. 651 | 652 | Return values are same as arguments for projection_matrix function: 653 | point, normal, direction, perspective, and pseudo. 654 | 655 | >>> point = numpy.random.random(3) - 0.5 656 | >>> normal = numpy.random.random(3) - 0.5 657 | >>> direct = numpy.random.random(3) - 0.5 658 | >>> persp = numpy.random.random(3) - 0.5 659 | >>> P0 = projection_matrix(point, normal) 660 | >>> result = projection_from_matrix(P0) 661 | >>> P1 = projection_matrix(*result) 662 | >>> is_same_transform(P0, P1) 663 | True 664 | >>> P0 = projection_matrix(point, normal, direct) 665 | >>> result = projection_from_matrix(P0) 666 | >>> P1 = projection_matrix(*result) 667 | >>> is_same_transform(P0, P1) 668 | True 669 | >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False) 670 | >>> result = projection_from_matrix(P0, pseudo=False) 671 | >>> P1 = projection_matrix(*result) 672 | >>> is_same_transform(P0, P1) 673 | True 674 | >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True) 675 | >>> result = projection_from_matrix(P0, pseudo=True) 676 | >>> P1 = projection_matrix(*result) 677 | >>> is_same_transform(P0, P1) 678 | True 679 | 680 | """ 681 | M = numpy.asarray(matrix, dtype=numpy.float64) 682 | M33 = M[:3, :3] 683 | w, V = numpy.linalg.eig(M) 684 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 685 | if not pseudo and len(i) > 0: 686 | # point: any eigenvector corresponding to eigenvalue 1 687 | point = numpy.real(V[:, i[-1]]).squeeze() 688 | point /= point[3] 689 | # direction: unit eigenvector corresponding to eigenvalue 0 690 | w, V = numpy.linalg.eig(M33) 691 | i = numpy.where(abs(numpy.real(w)) < 1e-8)[0] 692 | if len(i) == 0: 693 | raise ValueError('no eigenvector corresponding to eigenvalue 0') 694 | direction = numpy.real(V[:, i[0]]).squeeze() 695 | direction /= vector_norm(direction) 696 | # normal: unit eigenvector of M33.T corresponding to eigenvalue 0 697 | w, V = numpy.linalg.eig(M33.T) 698 | i = numpy.where(abs(numpy.real(w)) < 1e-8)[0] 699 | if len(i) > 0: 700 | # parallel projection 701 | normal = numpy.real(V[:, i[0]]).squeeze() 702 | normal /= vector_norm(normal) 703 | return point, normal, direction, None, False 704 | # orthogonal projection, where normal equals direction vector 705 | return point, direction, None, None, False 706 | 707 | # perspective projection 708 | i = numpy.where(abs(numpy.real(w)) > 1e-8)[0] 709 | if len(i) == 0: 710 | raise ValueError('no eigenvector not corresponding to eigenvalue 0') 711 | point = numpy.real(V[:, i[-1]]).squeeze() 712 | point /= point[3] 713 | normal = -M[3, :3] 714 | perspective = M[:3, 3] / numpy.dot(point[:3], normal) 715 | if pseudo: 716 | perspective -= normal 717 | return point, normal, None, perspective, pseudo 718 | 719 | 720 | def clip_matrix(left, right, bottom, top, near, far, perspective=False): 721 | """Return matrix to obtain normalized device coordinates from frustum. 722 | 723 | The frustum bounds are axis-aligned along x (left, right), 724 | y (bottom, top) and z (near, far). 725 | 726 | Normalized device coordinates are in range [-1, 1] if coordinates are 727 | inside the frustum. 728 | 729 | If perspective is True the frustum is a truncated pyramid with the 730 | perspective point at origin and direction along z axis, otherwise an 731 | orthographic canonical view volume (a box). 732 | 733 | Homogeneous coordinates transformed by the perspective clip matrix 734 | need to be dehomogenized (divided by w coordinate). 735 | 736 | >>> frustum = numpy.random.rand(6) 737 | >>> frustum[1] += frustum[0] 738 | >>> frustum[3] += frustum[2] 739 | >>> frustum[5] += frustum[4] 740 | >>> M = clip_matrix(perspective=False, *frustum) 741 | >>> numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1]) 742 | array([-1., -1., -1., 1.]) 743 | >>> numpy.dot(M, [frustum[1], frustum[3], frustum[5], 1]) 744 | array([1., 1., 1., 1.]) 745 | >>> M = clip_matrix(perspective=True, *frustum) 746 | >>> v = numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1]) 747 | >>> v / v[3] 748 | array([-1., -1., -1., 1.]) 749 | >>> v = numpy.dot(M, [frustum[1], frustum[3], frustum[4], 1]) 750 | >>> v / v[3] 751 | array([ 1., 1., -1., 1.]) 752 | 753 | """ 754 | if left >= right or bottom >= top or near >= far: 755 | raise ValueError('invalid frustum') 756 | if perspective: 757 | if near <= _EPS: 758 | raise ValueError('invalid frustum: near <= 0') 759 | t = 2.0 * near 760 | M = [ 761 | [t / (left - right), 0.0, (right + left) / (right - left), 0.0], 762 | [0.0, t / (bottom - top), (top + bottom) / (top - bottom), 0.0], 763 | [0.0, 0.0, (far + near) / (near - far), t * far / (far - near)], 764 | [0.0, 0.0, -1.0, 0.0], 765 | ] 766 | else: 767 | M = [ 768 | [2.0 / (right - left), 0.0, 0.0, (right + left) / (left - right)], 769 | [0.0, 2.0 / (top - bottom), 0.0, (top + bottom) / (bottom - top)], 770 | [0.0, 0.0, 2.0 / (far - near), (far + near) / (near - far)], 771 | [0.0, 0.0, 0.0, 1.0], 772 | ] 773 | return numpy.array(M) 774 | 775 | 776 | def shear_matrix(angle, direction, point, normal): 777 | """Return matrix to shear by angle along direction vector on shear plane. 778 | 779 | The shear plane is defined by a point and normal vector. The direction 780 | vector must be orthogonal to the plane's normal vector. 781 | 782 | A point P is transformed by the shear matrix into P" such that 783 | the vector P-P" is parallel to the direction vector and its extent is 784 | given by the angle of P-P'-P", where P' is the orthogonal projection 785 | of P onto the shear plane. 786 | 787 | >>> angle = (random.random() - 0.5) * 4 * math.pi 788 | >>> direct = numpy.random.random(3) - 0.5 789 | >>> point = numpy.random.random(3) - 0.5 790 | >>> normal = numpy.cross(direct, numpy.random.random(3)) 791 | >>> S = shear_matrix(angle, direct, point, normal) 792 | >>> numpy.allclose(1, numpy.linalg.det(S)) 793 | True 794 | 795 | """ 796 | normal = unit_vector(normal[:3]) 797 | direction = unit_vector(direction[:3]) 798 | if abs(numpy.dot(normal, direction)) > 1e-6: 799 | raise ValueError('direction and normal vectors are not orthogonal') 800 | angle = math.tan(angle) 801 | M = numpy.identity(4) 802 | M[:3, :3] += angle * numpy.outer(direction, normal) 803 | M[:3, 3] = -angle * numpy.dot(point[:3], normal) * direction 804 | return M 805 | 806 | 807 | def shear_from_matrix(matrix): 808 | """Return shear angle, direction and plane from shear matrix. 809 | 810 | >>> angle = (random.random() - 0.5) * 4 * math.pi 811 | >>> direct = numpy.random.random(3) - 0.5 812 | >>> point = numpy.random.random(3) - 0.5 813 | >>> normal = numpy.cross(direct, numpy.random.random(3)) 814 | >>> S0 = shear_matrix(angle, direct, point, normal) 815 | >>> angle, direct, point, normal = shear_from_matrix(S0) 816 | >>> S1 = shear_matrix(angle, direct, point, normal) 817 | >>> is_same_transform(S0, S1) 818 | True 819 | 820 | """ 821 | M = numpy.asarray(matrix, dtype=numpy.float64) 822 | M33 = M[:3, :3] 823 | # normal: cross independent eigenvectors corresponding to the eigenvalue 1 824 | w, V = numpy.linalg.eig(M33) 825 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-4)[0] 826 | if len(i) < 2: 827 | raise ValueError(f'no two linear independent eigenvectors found {w}') 828 | V = numpy.real(V[:, i]).squeeze().T 829 | lenorm = -1.0 830 | for i0, i1 in ((0, 1), (0, 2), (1, 2)): 831 | n = numpy.cross(V[i0], V[i1]) 832 | w = vector_norm(n) 833 | if w > lenorm: 834 | lenorm = w # type: ignore[assignment] 835 | normal = n 836 | normal /= lenorm 837 | # direction and angle 838 | direction = numpy.dot(M33 - numpy.identity(3), normal) 839 | angle = vector_norm(direction) 840 | direction /= angle 841 | angle = math.atan(angle) 842 | # point: eigenvector corresponding to eigenvalue 1 843 | w, V = numpy.linalg.eig(M) 844 | i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] 845 | if len(i) == 0: 846 | raise ValueError('no eigenvector corresponding to eigenvalue 1') 847 | point = numpy.real(V[:, i[-1]]).squeeze() 848 | point /= point[3] 849 | return angle, direction, point, normal 850 | 851 | 852 | def decompose_matrix(matrix): 853 | """Return sequence of transformations from transformation matrix. 854 | 855 | matrix : array_like 856 | Non-degenerative homogeneous transformation matrix 857 | 858 | Return tuple of: 859 | scale : vector of 3 scaling factors 860 | shear : list of shear factors for x-y, x-z, y-z axes 861 | angles : list of Euler angles about static x, y, z axes 862 | translate : translation vector along x, y, z axes 863 | perspective : perspective partition of matrix 864 | 865 | Raise ValueError if matrix is of wrong type or degenerative. 866 | 867 | >>> T0 = translation_matrix([1, 2, 3]) 868 | >>> scale, shear, angles, trans, persp = decompose_matrix(T0) 869 | >>> T1 = translation_matrix(trans) 870 | >>> numpy.allclose(T0, T1) 871 | True 872 | >>> S = scale_matrix(0.123) 873 | >>> scale, shear, angles, trans, persp = decompose_matrix(S) 874 | >>> float(scale[0]) 875 | 0.123 876 | >>> R0 = euler_matrix(1, 2, 3) 877 | >>> scale, shear, angles, trans, persp = decompose_matrix(R0) 878 | >>> R1 = euler_matrix(*angles) 879 | >>> numpy.allclose(R0, R1) 880 | True 881 | 882 | """ 883 | M = numpy.array(matrix, dtype=numpy.float64, copy=True).T 884 | if abs(M[3, 3]) < _EPS: 885 | raise ValueError('M[3, 3] is zero') 886 | M /= M[3, 3] 887 | P = M.copy() 888 | P[:, 3] = 0.0, 0.0, 0.0, 1.0 889 | if not numpy.linalg.det(P): 890 | raise ValueError('matrix is singular') 891 | 892 | scale = numpy.zeros((3,)) 893 | shear = [0.0, 0.0, 0.0] 894 | angles = [0.0, 0.0, 0.0] 895 | 896 | if any(abs(M[:3, 3]) > _EPS): 897 | perspective = numpy.dot(M[:, 3], numpy.linalg.inv(P.T)) 898 | M[:, 3] = 0.0, 0.0, 0.0, 1.0 899 | else: 900 | perspective = numpy.array([0.0, 0.0, 0.0, 1.0]) 901 | 902 | translate = M[3, :3].copy() 903 | M[3, :3] = 0.0 904 | 905 | row = M[:3, :3].copy() 906 | scale[0] = vector_norm(row[0]) 907 | row[0] /= scale[0] 908 | shear[0] = numpy.dot(row[0], row[1]) 909 | row[1] -= row[0] * shear[0] 910 | scale[1] = vector_norm(row[1]) 911 | row[1] /= scale[1] 912 | shear[0] /= scale[1] 913 | shear[1] = numpy.dot(row[0], row[2]) 914 | row[2] -= row[0] * shear[1] 915 | shear[2] = numpy.dot(row[1], row[2]) 916 | row[2] -= row[1] * shear[2] 917 | scale[2] = vector_norm(row[2]) 918 | row[2] /= scale[2] 919 | shear[1:] /= scale[2] 920 | 921 | if numpy.dot(row[0], numpy.cross(row[1], row[2])) < 0: 922 | numpy.negative(scale, scale) 923 | numpy.negative(row, row) 924 | 925 | angles[1] = math.asin(-row[0, 2]) 926 | if math.cos(angles[1]): 927 | angles[0] = math.atan2(row[1, 2], row[2, 2]) 928 | angles[2] = math.atan2(row[0, 1], row[0, 0]) 929 | else: 930 | # angles[0] = math.atan2(row[1, 0], row[1, 1]) 931 | angles[0] = math.atan2(-row[2, 1], row[1, 1]) 932 | angles[2] = 0.0 933 | 934 | return scale, shear, angles, translate, perspective 935 | 936 | 937 | def compose_matrix( 938 | scale=None, shear=None, angles=None, translate=None, perspective=None 939 | ): 940 | """Return transformation matrix from sequence of transformations. 941 | 942 | This is the inverse of the decompose_matrix function. 943 | 944 | Sequence of transformations: 945 | scale : vector of 3 scaling factors 946 | shear : list of shear factors for x-y, x-z, y-z axes 947 | angles : list of Euler angles about static x, y, z axes 948 | translate : translation vector along x, y, z axes 949 | perspective : perspective partition of matrix 950 | 951 | >>> scale = numpy.random.random(3) - 0.5 952 | >>> shear = numpy.random.random(3) - 0.5 953 | >>> angles = (numpy.random.random(3) - 0.5) * (2 * math.pi) 954 | >>> trans = numpy.random.random(3) - 0.5 955 | >>> persp = numpy.random.random(4) - 0.5 956 | >>> M0 = compose_matrix(scale, shear, angles, trans, persp) 957 | >>> result = decompose_matrix(M0) 958 | >>> M1 = compose_matrix(*result) 959 | >>> is_same_transform(M0, M1) 960 | True 961 | 962 | """ 963 | M = numpy.identity(4) 964 | if perspective is not None: 965 | P = numpy.identity(4) 966 | P[3, :] = perspective[:4] 967 | M = numpy.dot(M, P) 968 | if translate is not None: 969 | T = numpy.identity(4) 970 | T[:3, 3] = translate[:3] 971 | M = numpy.dot(M, T) 972 | if angles is not None: 973 | R = euler_matrix(angles[0], angles[1], angles[2], 'sxyz') 974 | M = numpy.dot(M, R) 975 | if shear is not None: 976 | Z = numpy.identity(4) 977 | Z[1, 2] = shear[2] 978 | Z[0, 2] = shear[1] 979 | Z[0, 1] = shear[0] 980 | M = numpy.dot(M, Z) 981 | if scale is not None: 982 | S = numpy.identity(4) 983 | S[0, 0] = scale[0] 984 | S[1, 1] = scale[1] 985 | S[2, 2] = scale[2] 986 | M = numpy.dot(M, S) 987 | M /= M[3, 3] 988 | return M 989 | 990 | 991 | def orthogonalization_matrix(lengths, angles): 992 | """Return orthogonalization matrix for crystallographic cell coordinates. 993 | 994 | Angles are expected in degrees. 995 | 996 | The de-orthogonalization matrix is the inverse. 997 | 998 | >>> O = orthogonalization_matrix([10, 10, 10], [90, 90, 90]) 999 | >>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10) 1000 | True 1001 | >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7]) 1002 | >>> numpy.allclose(numpy.sum(O), 43.063229) 1003 | True 1004 | 1005 | """ 1006 | a, b, c = lengths 1007 | angles = numpy.radians(angles) 1008 | sina, sinb, _ = numpy.sin(angles) 1009 | cosa, cosb, cosg = numpy.cos(angles) 1010 | co = (cosa * cosb - cosg) / (sina * sinb) 1011 | return numpy.array( 1012 | [ 1013 | [a * sinb * math.sqrt(1.0 - co * co), 0.0, 0.0, 0.0], 1014 | [-a * sinb * co, b * sina, 0.0, 0.0], 1015 | [a * cosb, b * cosa, c, 0.0], 1016 | [0.0, 0.0, 0.0, 1.0], 1017 | ] 1018 | ) 1019 | 1020 | 1021 | def affine_matrix_from_points(v0, v1, shear=True, scale=True, usesvd=True): 1022 | """Return affine transform matrix to register two point sets. 1023 | 1024 | v0 and v1 are shape (ndims, -1) arrays of at least ndims non-homogeneous 1025 | coordinates, where ndims is the dimensionality of the coordinate space. 1026 | 1027 | If shear is False, a similarity transformation matrix is returned. 1028 | If also scale is False, a rigid/Euclidean transformation matrix 1029 | is returned. 1030 | 1031 | By default the algorithm by Hartley and Zissermann [15] is used. 1032 | If usesvd is True, similarity and Euclidean transformation matrices 1033 | are calculated by minimizing the weighted sum of squared deviations 1034 | (RMSD) according to the algorithm by Kabsch [8]. 1035 | Otherwise, and if ndims is 3, the quaternion based algorithm by Horn [9] 1036 | is used, which is slower when using this Python implementation. 1037 | 1038 | The returned matrix performs rotation, translation and uniform scaling 1039 | (if specified). 1040 | 1041 | >>> v0 = [[0, 1031, 1031, 0], [0, 0, 1600, 1600]] 1042 | >>> v1 = [[675, 826, 826, 677], [55, 52, 281, 277]] 1043 | >>> affine_matrix_from_points(v0, v1) 1044 | array([[ 0.14549, 0.00062, 675.50008], 1045 | [ 0.00048, 0.14094, 53.24971], 1046 | [ 0. , 0. , 1. ]]) 1047 | >>> T = translation_matrix(numpy.random.random(3) - 0.5) 1048 | >>> R = random_rotation_matrix(numpy.random.random(3)) 1049 | >>> S = scale_matrix(random.random()) 1050 | >>> M = concatenate_matrices(T, R, S) 1051 | >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20 1052 | >>> v0[3] = 1 1053 | >>> v1 = numpy.dot(M, v0) 1054 | >>> v0[:3] += numpy.random.normal(0, 1e-8, 300).reshape(3, -1) 1055 | >>> M = affine_matrix_from_points(v0[:3], v1[:3]) 1056 | >>> numpy.allclose(v1, numpy.dot(M, v0)) 1057 | True 1058 | 1059 | More examples in superimposition_matrix() 1060 | 1061 | """ 1062 | v0 = numpy.array(v0, dtype=numpy.float64, copy=True) 1063 | v1 = numpy.array(v1, dtype=numpy.float64, copy=True) 1064 | 1065 | ndims = v0.shape[0] 1066 | if ndims < 2 or v0.shape[1] < ndims or v0.shape != v1.shape: 1067 | raise ValueError('input arrays are of wrong shape or type') 1068 | 1069 | # move centroids to origin 1070 | t0 = -numpy.mean(v0, axis=1) 1071 | M0 = numpy.identity(ndims + 1) 1072 | M0[:ndims, ndims] = t0 1073 | v0 += t0.reshape(ndims, 1) 1074 | t1 = -numpy.mean(v1, axis=1) 1075 | M1 = numpy.identity(ndims + 1) 1076 | M1[:ndims, ndims] = t1 1077 | v1 += t1.reshape(ndims, 1) 1078 | 1079 | if shear: 1080 | # Affine transformation 1081 | A = numpy.concatenate((v0, v1), axis=0) 1082 | u, s, vh = numpy.linalg.svd(A.T) 1083 | vh = vh[:ndims].T 1084 | B = vh[:ndims] 1085 | C = vh[ndims : 2 * ndims] 1086 | t = numpy.dot(C, numpy.linalg.pinv(B)) 1087 | t = numpy.concatenate((t, numpy.zeros((ndims, 1))), axis=1) 1088 | M = numpy.vstack((t, ((0.0,) * ndims) + (1.0,))) 1089 | elif usesvd or ndims != 3: 1090 | # Rigid transformation via SVD of covariance matrix 1091 | u, s, vh = numpy.linalg.svd(numpy.dot(v1, v0.T)) 1092 | # rotation matrix from SVD orthonormal bases 1093 | R = numpy.dot(u, vh) 1094 | if numpy.linalg.det(R) < 0.0: 1095 | # R does not constitute right handed system 1096 | R -= numpy.outer(u[:, ndims - 1], vh[ndims - 1, :] * 2.0) 1097 | s[-1] *= -1.0 1098 | # homogeneous transformation matrix 1099 | M = numpy.identity(ndims + 1) 1100 | M[:ndims, :ndims] = R 1101 | else: 1102 | # Rigid transformation matrix via quaternion 1103 | # compute symmetric matrix N 1104 | xx, yy, zz = numpy.sum(v0 * v1, axis=1) 1105 | xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1) 1106 | xz, yx, zy = numpy.sum(v0 * numpy.roll(v1, -2, axis=0), axis=1) 1107 | N = [ 1108 | [xx + yy + zz, 0.0, 0.0, 0.0], 1109 | [yz - zy, xx - yy - zz, 0.0, 0.0], 1110 | [zx - xz, xy + yx, yy - xx - zz, 0.0], 1111 | [xy - yx, zx + xz, yz + zy, zz - xx - yy], 1112 | ] 1113 | # quaternion: eigenvector corresponding to most positive eigenvalue 1114 | w, V = numpy.linalg.eigh(N) 1115 | q = V[:, numpy.argmax(w)] 1116 | q /= vector_norm(q) # unit quaternion 1117 | # homogeneous transformation matrix 1118 | M = quaternion_matrix(q) 1119 | 1120 | if scale and not shear: 1121 | # Affine transformation; scale is ratio of RMS deviations from centroid 1122 | v0 *= v0 1123 | v1 *= v1 1124 | M[:ndims, :ndims] *= math.sqrt(numpy.sum(v1) / numpy.sum(v0)) 1125 | 1126 | # move centroids back 1127 | M = numpy.dot(numpy.linalg.inv(M1), numpy.dot(M, M0)) 1128 | M /= M[ndims, ndims] 1129 | return M 1130 | 1131 | 1132 | def superimposition_matrix(v0, v1, scale=False, usesvd=True): 1133 | """Return matrix to transform given 3D point set into second point set. 1134 | 1135 | v0 and v1 are shape (3, -1) or (4, -1) arrays of at least 3 points. 1136 | 1137 | The parameters scale and usesvd are explained in the more general 1138 | affine_matrix_from_points function. 1139 | 1140 | The returned matrix is a similarity or Euclidean transformation matrix. 1141 | This function has a fast C implementation in transformations.c. 1142 | 1143 | >>> v0 = numpy.random.rand(3, 10) 1144 | >>> M = superimposition_matrix(v0, v0) 1145 | >>> numpy.allclose(M, numpy.identity(4)) 1146 | True 1147 | >>> R = random_rotation_matrix(numpy.random.random(3)) 1148 | >>> v0 = [[1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 1, 1]] 1149 | >>> v1 = numpy.dot(R, v0) 1150 | >>> M = superimposition_matrix(v0, v1) 1151 | >>> numpy.allclose(v1, numpy.dot(M, v0)) 1152 | True 1153 | >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20 1154 | >>> v0[3] = 1 1155 | >>> v1 = numpy.dot(R, v0) 1156 | >>> M = superimposition_matrix(v0, v1) 1157 | >>> numpy.allclose(v1, numpy.dot(M, v0)) 1158 | True 1159 | >>> S = scale_matrix(random.random()) 1160 | >>> T = translation_matrix(numpy.random.random(3) - 0.5) 1161 | >>> M = concatenate_matrices(T, R, S) 1162 | >>> v1 = numpy.dot(M, v0) 1163 | >>> v0[:3] += numpy.random.normal(0, 1e-9, 300).reshape(3, -1) 1164 | >>> M = superimposition_matrix(v0, v1, scale=True) 1165 | >>> numpy.allclose(v1, numpy.dot(M, v0)) 1166 | True 1167 | >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False) 1168 | >>> numpy.allclose(v1, numpy.dot(M, v0)) 1169 | True 1170 | >>> v = numpy.empty((4, 100, 3)) 1171 | >>> v[:, :, 0] = v0 1172 | >>> M = superimposition_matrix(v0, v1, scale=True, usesvd=False) 1173 | >>> numpy.allclose(v1, numpy.dot(M, v[:, :, 0])) 1174 | True 1175 | 1176 | """ 1177 | v0 = numpy.asarray(v0, dtype=numpy.float64) 1178 | v1 = numpy.asarray(v1, dtype=numpy.float64) 1179 | if ( 1180 | v0.shape != v1.shape 1181 | or v0.ndim != 2 1182 | or v0.shape[0] not in (3, 4) 1183 | or v0.shape[1] < 3 1184 | ): 1185 | raise ValueError('invalid input shapes') 1186 | return affine_matrix_from_points( 1187 | v0[:3], v1[:3], shear=False, scale=scale, usesvd=usesvd 1188 | ) 1189 | 1190 | 1191 | def euler_matrix(ai, aj, ak, axes='sxyz'): 1192 | """Return homogeneous rotation matrix from Euler angles and axis sequence. 1193 | 1194 | ai, aj, ak : Euler's roll, pitch and yaw angles 1195 | axes : One of 24 axis sequences as string or encoded tuple 1196 | 1197 | >>> R = euler_matrix(1, 2, 3, 'syxz') 1198 | >>> numpy.allclose(numpy.sum(R[0]), -1.34786452) 1199 | True 1200 | >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1)) 1201 | >>> numpy.allclose(numpy.sum(R[0]), -0.383436184) 1202 | True 1203 | >>> ai, aj, ak = (4 * math.pi) * (numpy.random.random(3) - 0.5) 1204 | >>> for axes in _AXES2TUPLE.keys(): 1205 | ... R = euler_matrix(ai, aj, ak, axes) 1206 | ... 1207 | >>> for axes in _TUPLE2AXES.keys(): 1208 | ... R = euler_matrix(ai, aj, ak, axes) 1209 | ... 1210 | 1211 | """ 1212 | try: 1213 | firstaxis, parity, repetition, frame = _AXES2TUPLE[axes] 1214 | except (AttributeError, KeyError): 1215 | _TUPLE2AXES[axes] # noqa: validation 1216 | firstaxis, parity, repetition, frame = axes 1217 | 1218 | i = firstaxis 1219 | j = _NEXT_AXIS[i + parity] 1220 | k = _NEXT_AXIS[i - parity + 1] 1221 | 1222 | if frame: 1223 | ai, ak = ak, ai 1224 | if parity: 1225 | ai, aj, ak = -ai, -aj, -ak 1226 | 1227 | si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak) 1228 | ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak) 1229 | cc, cs = ci * ck, ci * sk 1230 | sc, ss = si * ck, si * sk 1231 | 1232 | M = numpy.identity(4) 1233 | if repetition: 1234 | M[i, i] = cj 1235 | M[i, j] = sj * si 1236 | M[i, k] = sj * ci 1237 | M[j, i] = sj * sk 1238 | M[j, j] = -cj * ss + cc 1239 | M[j, k] = -cj * cs - sc 1240 | M[k, i] = -sj * ck 1241 | M[k, j] = cj * sc + cs 1242 | M[k, k] = cj * cc - ss 1243 | else: 1244 | M[i, i] = cj * ck 1245 | M[i, j] = sj * sc - cs 1246 | M[i, k] = sj * cc + ss 1247 | M[j, i] = cj * sk 1248 | M[j, j] = sj * ss + cc 1249 | M[j, k] = sj * cs - sc 1250 | M[k, i] = -sj 1251 | M[k, j] = cj * si 1252 | M[k, k] = cj * ci 1253 | return M 1254 | 1255 | 1256 | def euler_from_matrix(matrix, axes='sxyz'): 1257 | """Return Euler angles from rotation matrix for specified axis sequence. 1258 | 1259 | axes : One of 24 axis sequences as string or encoded tuple 1260 | 1261 | Note that many Euler angle triplets can describe one matrix. 1262 | 1263 | >>> R0 = euler_matrix(1, 2, 3, 'syxz') 1264 | >>> al, be, ga = euler_from_matrix(R0, 'syxz') 1265 | >>> R1 = euler_matrix(al, be, ga, 'syxz') 1266 | >>> numpy.allclose(R0, R1) 1267 | True 1268 | >>> angles = (4 * math.pi) * (numpy.random.random(3) - 0.5) 1269 | >>> for axes in _AXES2TUPLE.keys(): 1270 | ... R0 = euler_matrix(axes=axes, *angles) 1271 | ... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes)) 1272 | ... if not numpy.allclose(R0, R1): 1273 | ... print(axes, "failed") 1274 | ... 1275 | 1276 | """ 1277 | try: 1278 | firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] 1279 | except (AttributeError, KeyError): 1280 | _TUPLE2AXES[axes] # noqa: validation 1281 | firstaxis, parity, repetition, frame = axes 1282 | 1283 | i = firstaxis 1284 | j = _NEXT_AXIS[i + parity] 1285 | k = _NEXT_AXIS[i - parity + 1] 1286 | 1287 | M = numpy.asarray(matrix, dtype=numpy.float64)[:3, :3] 1288 | if repetition: 1289 | sy = math.sqrt(M[i, j] * M[i, j] + M[i, k] * M[i, k]) 1290 | if sy > _EPS: 1291 | ax = math.atan2(M[i, j], M[i, k]) 1292 | ay = math.atan2(sy, M[i, i]) 1293 | az = math.atan2(M[j, i], -M[k, i]) 1294 | else: 1295 | ax = math.atan2(-M[j, k], M[j, j]) 1296 | ay = math.atan2(sy, M[i, i]) 1297 | az = 0.0 1298 | else: 1299 | cy = math.sqrt(M[i, i] * M[i, i] + M[j, i] * M[j, i]) 1300 | if cy > _EPS: 1301 | ax = math.atan2(M[k, j], M[k, k]) 1302 | ay = math.atan2(-M[k, i], cy) 1303 | az = math.atan2(M[j, i], M[i, i]) 1304 | else: 1305 | ax = math.atan2(-M[j, k], M[j, j]) 1306 | ay = math.atan2(-M[k, i], cy) 1307 | az = 0.0 1308 | 1309 | if parity: 1310 | ax, ay, az = -ax, -ay, -az 1311 | if frame: 1312 | ax, az = az, ax 1313 | return ax, ay, az 1314 | 1315 | 1316 | def euler_from_quaternion(quaternion, axes='sxyz'): 1317 | """Return Euler angles from quaternion for specified axis sequence. 1318 | 1319 | >>> angles = euler_from_quaternion([0.99810947, 0.06146124, 0, 0]) 1320 | >>> numpy.allclose(angles, [0.123, 0, 0]) 1321 | True 1322 | 1323 | """ 1324 | return euler_from_matrix(quaternion_matrix(quaternion), axes) 1325 | 1326 | 1327 | def quaternion_from_euler(ai, aj, ak, axes='sxyz'): 1328 | """Return quaternion from Euler angles and axis sequence. 1329 | 1330 | ai, aj, ak : Euler's roll, pitch and yaw angles 1331 | axes : One of 24 axis sequences as string or encoded tuple 1332 | 1333 | >>> q = quaternion_from_euler(1, 2, 3, 'ryxz') 1334 | >>> numpy.allclose(q, [0.435953, 0.310622, -0.718287, 0.444435]) 1335 | True 1336 | 1337 | """ 1338 | try: 1339 | firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] 1340 | except (AttributeError, KeyError): 1341 | _TUPLE2AXES[axes] # noqa: validation 1342 | firstaxis, parity, repetition, frame = axes 1343 | 1344 | i = firstaxis + 1 1345 | j = _NEXT_AXIS[i + parity - 1] + 1 1346 | k = _NEXT_AXIS[i - parity] + 1 1347 | 1348 | if frame: 1349 | ai, ak = ak, ai 1350 | if parity: 1351 | aj = -aj 1352 | 1353 | ai /= 2.0 1354 | aj /= 2.0 1355 | ak /= 2.0 1356 | ci = math.cos(ai) 1357 | si = math.sin(ai) 1358 | cj = math.cos(aj) 1359 | sj = math.sin(aj) 1360 | ck = math.cos(ak) 1361 | sk = math.sin(ak) 1362 | cc = ci * ck 1363 | cs = ci * sk 1364 | sc = si * ck 1365 | ss = si * sk 1366 | 1367 | q = numpy.empty((4,)) 1368 | if repetition: 1369 | q[0] = cj * (cc - ss) 1370 | q[i] = cj * (cs + sc) 1371 | q[j] = sj * (cc + ss) 1372 | q[k] = sj * (cs - sc) 1373 | else: 1374 | q[0] = cj * cc + sj * ss 1375 | q[i] = cj * sc - sj * cs 1376 | q[j] = cj * ss + sj * cc 1377 | q[k] = cj * cs - sj * sc 1378 | if parity: 1379 | q[j] *= -1.0 1380 | 1381 | return q 1382 | 1383 | 1384 | def quaternion_about_axis(angle, axis): 1385 | """Return quaternion for rotation about axis. 1386 | 1387 | >>> q = quaternion_about_axis(0.123, [1, 0, 0]) 1388 | >>> numpy.allclose(q, [0.99810947, 0.06146124, 0, 0]) 1389 | True 1390 | 1391 | """ 1392 | q = numpy.array([0.0, axis[0], axis[1], axis[2]]) 1393 | qlen = vector_norm(q) 1394 | if qlen > _EPS: 1395 | q *= math.sin(angle / 2.0) / qlen 1396 | q[0] = math.cos(angle / 2.0) 1397 | return q 1398 | 1399 | 1400 | def quaternion_matrix(quaternion): 1401 | """Return homogeneous rotation matrix from quaternion. 1402 | 1403 | >>> M = quaternion_matrix([0.99810947, 0.06146124, 0, 0]) 1404 | >>> numpy.allclose(M, rotation_matrix(0.123, [1, 0, 0])) 1405 | True 1406 | >>> M = quaternion_matrix([1, 0, 0, 0]) 1407 | >>> numpy.allclose(M, numpy.identity(4)) 1408 | True 1409 | >>> M = quaternion_matrix([0, 1, 0, 0]) 1410 | >>> numpy.allclose(M, numpy.diag([1, -1, -1, 1])) 1411 | True 1412 | 1413 | """ 1414 | q = numpy.array(quaternion, dtype=numpy.float64, copy=True) 1415 | n = numpy.dot(q, q) 1416 | if n < _EPS: 1417 | return numpy.identity(4) 1418 | q *= math.sqrt(2.0 / n) 1419 | q = numpy.outer(q, q) 1420 | return numpy.array( 1421 | [ 1422 | [ 1423 | 1.0 - q[2, 2] - q[3, 3], 1424 | q[1, 2] - q[3, 0], 1425 | q[1, 3] + q[2, 0], 1426 | 0.0, 1427 | ], 1428 | [ 1429 | q[1, 2] + q[3, 0], 1430 | 1.0 - q[1, 1] - q[3, 3], 1431 | q[2, 3] - q[1, 0], 1432 | 0.0, 1433 | ], 1434 | [ 1435 | q[1, 3] - q[2, 0], 1436 | q[2, 3] + q[1, 0], 1437 | 1.0 - q[1, 1] - q[2, 2], 1438 | 0.0, 1439 | ], 1440 | [0.0, 0.0, 0.0, 1.0], 1441 | ] 1442 | ) 1443 | 1444 | 1445 | def quaternion_from_matrix(matrix, isprecise=False): 1446 | """Return quaternion from rotation matrix. 1447 | 1448 | If isprecise is True, the input matrix is assumed to be a precise rotation 1449 | matrix and a faster algorithm is used. 1450 | 1451 | >>> q = quaternion_from_matrix(numpy.identity(4), True) 1452 | >>> numpy.allclose(q, [1, 0, 0, 0]) 1453 | True 1454 | >>> q = quaternion_from_matrix(numpy.diag([1, -1, -1, 1])) 1455 | >>> numpy.allclose(q, [0, 1, 0, 0]) or numpy.allclose(q, [0, -1, 0, 0]) 1456 | True 1457 | >>> R = rotation_matrix(0.123, (1, 2, 3)) 1458 | >>> q = quaternion_from_matrix(R, True) 1459 | >>> numpy.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786]) 1460 | True 1461 | >>> R = [ 1462 | ... [-0.545, 0.797, 0.260, 0], 1463 | ... [0.733, 0.603, -0.313, 0], 1464 | ... [-0.407, 0.021, -0.913, 0], 1465 | ... [0, 0, 0, 1], 1466 | ... ] 1467 | >>> q = quaternion_from_matrix(R) 1468 | >>> numpy.allclose(q, [0.19069, 0.43736, 0.87485, -0.083611]) 1469 | True 1470 | >>> R = [ 1471 | ... [0.395, 0.362, 0.843, 0], 1472 | ... [-0.626, 0.796, -0.056, 0], 1473 | ... [-0.677, -0.498, 0.529, 0], 1474 | ... [0, 0, 0, 1], 1475 | ... ] 1476 | >>> q = quaternion_from_matrix(R) 1477 | >>> numpy.allclose(q, [0.82336615, -0.13610694, 0.46344705, -0.29792603]) 1478 | True 1479 | >>> R = random_rotation_matrix() 1480 | >>> q = quaternion_from_matrix(R) 1481 | >>> is_same_transform(R, quaternion_matrix(q)) 1482 | True 1483 | >>> is_same_quaternion( 1484 | ... quaternion_from_matrix(R, isprecise=False), 1485 | ... quaternion_from_matrix(R, isprecise=True), 1486 | ... ) 1487 | True 1488 | >>> R = euler_matrix(0.0, 0.0, numpy.pi / 2.0) 1489 | >>> is_same_quaternion( 1490 | ... quaternion_from_matrix(R, isprecise=False), 1491 | ... quaternion_from_matrix(R, isprecise=True), 1492 | ... ) 1493 | True 1494 | 1495 | """ 1496 | M = numpy.asarray(matrix, dtype=numpy.float64)[:4, :4] 1497 | if isprecise: 1498 | q = numpy.empty((4,)) 1499 | t = numpy.trace(M) 1500 | if t > M[3, 3]: 1501 | q[0] = t 1502 | q[3] = M[1, 0] - M[0, 1] 1503 | q[2] = M[0, 2] - M[2, 0] 1504 | q[1] = M[2, 1] - M[1, 2] 1505 | else: 1506 | i, j, k = 0, 1, 2 1507 | if M[1, 1] > M[0, 0]: 1508 | i, j, k = 1, 2, 0 1509 | if M[2, 2] > M[i, i]: 1510 | i, j, k = 2, 0, 1 1511 | t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3] 1512 | q[i] = t 1513 | q[j] = M[i, j] + M[j, i] 1514 | q[k] = M[k, i] + M[i, k] 1515 | q[3] = M[k, j] - M[j, k] 1516 | q = q[[3, 0, 1, 2]] 1517 | q *= 0.5 / math.sqrt(t * M[3, 3]) 1518 | else: 1519 | m00 = M[0, 0] 1520 | m01 = M[0, 1] 1521 | m02 = M[0, 2] 1522 | m10 = M[1, 0] 1523 | m11 = M[1, 1] 1524 | m12 = M[1, 2] 1525 | m20 = M[2, 0] 1526 | m21 = M[2, 1] 1527 | m22 = M[2, 2] 1528 | # symmetric matrix K 1529 | K = numpy.array( 1530 | [ 1531 | [m00 - m11 - m22, 0.0, 0.0, 0.0], 1532 | [m01 + m10, m11 - m00 - m22, 0.0, 0.0], 1533 | [m02 + m20, m12 + m21, m22 - m00 - m11, 0.0], 1534 | [m21 - m12, m02 - m20, m10 - m01, m00 + m11 + m22], 1535 | ] 1536 | ) 1537 | K /= 3.0 1538 | # quaternion is eigenvector of K that corresponds to largest eigenvalue 1539 | w, V = numpy.linalg.eigh(K) 1540 | q = V[[3, 0, 1, 2], numpy.argmax(w)] 1541 | if q[0] < 0.0: 1542 | numpy.negative(q, q) 1543 | return q 1544 | 1545 | 1546 | def quaternion_multiply(quaternion1, quaternion0): 1547 | """Return multiplication of two quaternions. 1548 | 1549 | >>> q = quaternion_multiply([4, 1, -2, 3], [8, -5, 6, 7]) 1550 | >>> numpy.allclose(q, [28, -44, -14, 48]) 1551 | True 1552 | 1553 | """ 1554 | w0, x0, y0, z0 = quaternion0 1555 | w1, x1, y1, z1 = quaternion1 1556 | return numpy.array( 1557 | [ 1558 | -x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0, 1559 | x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0, 1560 | -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0, 1561 | x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0, 1562 | ], 1563 | dtype=numpy.float64, 1564 | ) 1565 | 1566 | 1567 | def quaternion_conjugate(quaternion): 1568 | """Return conjugate of quaternion. 1569 | 1570 | >>> q0 = random_quaternion() 1571 | >>> q1 = quaternion_conjugate(q0) 1572 | >>> q1[0] == q0[0] and all(q1[1:] == -q0[1:]) 1573 | True 1574 | 1575 | """ 1576 | q = numpy.array(quaternion, dtype=numpy.float64, copy=True) 1577 | numpy.negative(q[1:], q[1:]) 1578 | return q 1579 | 1580 | 1581 | def quaternion_inverse(quaternion): 1582 | """Return inverse of quaternion. 1583 | 1584 | >>> q0 = random_quaternion() 1585 | >>> q1 = quaternion_inverse(q0) 1586 | >>> numpy.allclose(quaternion_multiply(q0, q1), [1, 0, 0, 0]) 1587 | True 1588 | 1589 | """ 1590 | q = numpy.array(quaternion, dtype=numpy.float64, copy=True) 1591 | numpy.negative(q[1:], q[1:]) 1592 | return q / numpy.dot(q, q) 1593 | 1594 | 1595 | def quaternion_real(quaternion): 1596 | """Return real part of quaternion. 1597 | 1598 | >>> quaternion_real([3, 0, 1, 2]) 1599 | 3.0 1600 | 1601 | """ 1602 | return float(quaternion[0]) 1603 | 1604 | 1605 | def quaternion_imag(quaternion): 1606 | """Return imaginary part of quaternion. 1607 | 1608 | >>> quaternion_imag([3, 0, 1, 2]) 1609 | array([0., 1., 2.]) 1610 | 1611 | """ 1612 | return numpy.array(quaternion[1:4], dtype=numpy.float64, copy=True) 1613 | 1614 | 1615 | def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True): 1616 | """Return spherical linear interpolation between two quaternions. 1617 | 1618 | >>> q0 = random_quaternion() 1619 | >>> q1 = random_quaternion() 1620 | >>> q = quaternion_slerp(q0, q1, 0) 1621 | >>> numpy.allclose(q, q0) 1622 | True 1623 | >>> q = quaternion_slerp(q0, q1, 1, 1) 1624 | >>> numpy.allclose(q, q1) 1625 | True 1626 | >>> q = quaternion_slerp(q0, q1, 0.5) 1627 | >>> angle = math.acos(numpy.dot(q0, q)) 1628 | >>> ( 1629 | ... numpy.allclose(2, math.acos(numpy.dot(q0, q1)) / angle) 1630 | ... or numpy.allclose(2, math.acos(-numpy.dot(q0, q1)) / angle) 1631 | ... ) 1632 | True 1633 | 1634 | """ 1635 | q0 = unit_vector(quat0[:4]) 1636 | q1 = unit_vector(quat1[:4]) 1637 | if fraction == 0.0: 1638 | return q0 1639 | if fraction == 1.0: 1640 | return q1 1641 | d = numpy.dot(q0, q1) 1642 | if abs(abs(d) - 1.0) < _EPS: 1643 | return q0 1644 | if shortestpath and d < 0.0: 1645 | # invert rotation 1646 | d = -d 1647 | numpy.negative(q1, q1) 1648 | angle = math.acos(d) + spin * math.pi 1649 | if abs(angle) < _EPS: 1650 | return q0 1651 | isin = 1.0 / math.sin(angle) 1652 | q0 *= math.sin((1.0 - fraction) * angle) * isin 1653 | q1 *= math.sin(fraction * angle) * isin 1654 | q0 += q1 1655 | return q0 1656 | 1657 | 1658 | def random_quaternion(rand=None): 1659 | """Return uniform random unit quaternion. 1660 | 1661 | rand: array like or None 1662 | Three independent random variables that are uniformly distributed 1663 | between 0 and 1. 1664 | 1665 | >>> q = random_quaternion() 1666 | >>> numpy.allclose(1, vector_norm(q)) 1667 | True 1668 | >>> q = random_quaternion(numpy.random.random(3)) 1669 | >>> len(q.shape), q.shape[0] == 4 1670 | (1, True) 1671 | 1672 | """ 1673 | if rand is None: 1674 | rand = numpy.random.rand(3) 1675 | else: 1676 | assert len(rand) == 3 1677 | r1 = numpy.sqrt(1.0 - rand[0]) 1678 | r2 = numpy.sqrt(rand[0]) 1679 | pi2 = math.pi * 2.0 1680 | t1 = pi2 * rand[1] 1681 | t2 = pi2 * rand[2] 1682 | return numpy.array( 1683 | [ 1684 | numpy.cos(t2) * r2, 1685 | numpy.sin(t1) * r1, 1686 | numpy.cos(t1) * r1, 1687 | numpy.sin(t2) * r2, 1688 | ] 1689 | ) 1690 | 1691 | 1692 | def random_rotation_matrix(rand=None): 1693 | """Return uniform random rotation matrix. 1694 | 1695 | rand: array like 1696 | Three independent random variables that are uniformly distributed 1697 | between 0 and 1 for each returned quaternion. 1698 | 1699 | >>> R = random_rotation_matrix() 1700 | >>> numpy.allclose(numpy.dot(R.T, R), numpy.identity(4)) 1701 | True 1702 | 1703 | """ 1704 | return quaternion_matrix(random_quaternion(rand)) 1705 | 1706 | 1707 | class Arcball: 1708 | """Virtual Trackball Control. 1709 | 1710 | >>> ball = Arcball() 1711 | >>> ball = Arcball(initial=numpy.identity(4)) 1712 | >>> ball.place([320, 320], 320) 1713 | >>> ball.down([500, 250]) 1714 | >>> ball.drag([475, 275]) 1715 | >>> R = ball.matrix() 1716 | >>> numpy.allclose(numpy.sum(R), 3.90583455) 1717 | True 1718 | >>> ball = Arcball(initial=[1, 0, 0, 0]) 1719 | >>> ball.place([320, 320], 320) 1720 | >>> ball.setaxes([1, 1, 0], [-1, 1, 0]) 1721 | >>> ball.constrain = True 1722 | >>> ball.down([400, 200]) 1723 | >>> ball.drag([200, 400]) 1724 | >>> R = ball.matrix() 1725 | >>> numpy.allclose(numpy.sum(R), 0.2055924) 1726 | True 1727 | >>> ball.next() 1728 | 1729 | """ 1730 | 1731 | def __init__(self, initial=None): 1732 | """Initialize virtual trackball control. 1733 | 1734 | initial : quaternion or rotation matrix 1735 | 1736 | """ 1737 | self._axis = None 1738 | self._axes = None 1739 | self._radius = 1.0 1740 | self._center = [0.0, 0.0] 1741 | self._vdown = numpy.array([0.0, 0.0, 1.0]) 1742 | self._constrain = False 1743 | if initial is None: 1744 | self._qdown = numpy.array([1.0, 0.0, 0.0, 0.0]) 1745 | else: 1746 | initial = numpy.array(initial, dtype=numpy.float64) 1747 | if initial.shape == (4, 4): 1748 | self._qdown = quaternion_from_matrix(initial) 1749 | elif initial.shape == (4,): 1750 | initial /= vector_norm(initial) 1751 | self._qdown = initial 1752 | else: 1753 | raise ValueError("initial not a quaternion or matrix") 1754 | self._qnow = self._qpre = self._qdown 1755 | 1756 | def place(self, center, radius): 1757 | """Place Arcball, e.g. when window size changes. 1758 | 1759 | center : sequence[2] 1760 | Window coordinates of trackball center. 1761 | radius : float 1762 | Radius of trackball in window coordinates. 1763 | 1764 | """ 1765 | self._radius = float(radius) 1766 | self._center[0] = center[0] 1767 | self._center[1] = center[1] 1768 | 1769 | def setaxes(self, *axes): 1770 | """Set axes to constrain rotations.""" 1771 | if len(axes) == 0: 1772 | self._axes = None 1773 | else: 1774 | self._axes = [unit_vector(axis) for axis in axes] 1775 | 1776 | @property 1777 | def constrain(self): 1778 | """Return state of constrain to axis mode.""" 1779 | return self._constrain 1780 | 1781 | @constrain.setter 1782 | def constrain(self, value): 1783 | """Set state of constrain to axis mode.""" 1784 | self._constrain = bool(value) 1785 | 1786 | def down(self, point): 1787 | """Set initial cursor window coordinates and pick constrain-axis.""" 1788 | self._vdown = arcball_map_to_sphere(point, self._center, self._radius) 1789 | self._qdown = self._qpre = self._qnow 1790 | if self._constrain and self._axes is not None: 1791 | self._axis = arcball_nearest_axis(self._vdown, self._axes) 1792 | self._vdown = arcball_constrain_to_axis(self._vdown, self._axis) 1793 | else: 1794 | self._axis = None 1795 | 1796 | def drag(self, point): 1797 | """Update current cursor window coordinates.""" 1798 | vnow = arcball_map_to_sphere(point, self._center, self._radius) 1799 | if self._axis is not None: 1800 | vnow = arcball_constrain_to_axis(vnow, self._axis) 1801 | self._qpre = self._qnow 1802 | t = numpy.cross(self._vdown, vnow) 1803 | if numpy.dot(t, t) < _EPS: 1804 | self._qnow = self._qdown 1805 | else: 1806 | q = [numpy.dot(self._vdown, vnow), t[0], t[1], t[2]] 1807 | self._qnow = quaternion_multiply(q, self._qdown) 1808 | 1809 | def next(self, acceleration=0.0): 1810 | """Continue rotation in direction of last drag.""" 1811 | q = quaternion_slerp(self._qpre, self._qnow, 2.0 + acceleration, False) 1812 | self._qpre, self._qnow = self._qnow, q 1813 | 1814 | def matrix(self): 1815 | """Return homogeneous rotation matrix.""" 1816 | return quaternion_matrix(self._qnow) 1817 | 1818 | 1819 | def arcball_map_to_sphere(point, center, radius): 1820 | """Return unit sphere coordinates from window coordinates.""" 1821 | v0 = (point[0] - center[0]) / radius 1822 | v1 = (center[1] - point[1]) / radius 1823 | n = v0 * v0 + v1 * v1 1824 | if n > 1.0: 1825 | # position outside of sphere 1826 | n = math.sqrt(n) 1827 | return numpy.array([v0 / n, v1 / n, 0.0]) 1828 | return numpy.array([v0, v1, math.sqrt(1.0 - n)]) 1829 | 1830 | 1831 | def arcball_constrain_to_axis(point, axis): 1832 | """Return sphere point perpendicular to axis.""" 1833 | v = numpy.array(point, dtype=numpy.float64, copy=True) 1834 | a = numpy.array(axis, dtype=numpy.float64, copy=True) 1835 | v -= a * numpy.dot(a, v) # on plane 1836 | n = vector_norm(v) 1837 | if n > _EPS: 1838 | if v[2] < 0.0: 1839 | numpy.negative(v, v) 1840 | v /= n 1841 | return v 1842 | if a[2] == 1.0: 1843 | return numpy.array([1.0, 0.0, 0.0]) 1844 | return unit_vector([-a[1], a[0], 0.0]) 1845 | 1846 | 1847 | def arcball_nearest_axis(point, axes): 1848 | """Return axis, which arc is nearest to point.""" 1849 | point = numpy.asarray(point, dtype=numpy.float64) 1850 | nearest = None 1851 | mx = -1.0 1852 | for axis in axes: 1853 | t = numpy.dot(arcball_constrain_to_axis(point, axis), point) 1854 | if t > mx: 1855 | nearest = axis 1856 | mx = t 1857 | return nearest 1858 | 1859 | 1860 | # epsilon for testing whether a number is close to zero 1861 | _EPS = numpy.finfo(float).eps * 4.0 1862 | 1863 | # axis sequences for Euler angles 1864 | _NEXT_AXIS = [1, 2, 0, 1] 1865 | 1866 | # map axes strings to/from tuples of inner axis, parity, repetition, frame 1867 | _AXES2TUPLE = { 1868 | 'sxyz': (0, 0, 0, 0), 1869 | 'sxyx': (0, 0, 1, 0), 1870 | 'sxzy': (0, 1, 0, 0), 1871 | 'sxzx': (0, 1, 1, 0), 1872 | 'syzx': (1, 0, 0, 0), 1873 | 'syzy': (1, 0, 1, 0), 1874 | 'syxz': (1, 1, 0, 0), 1875 | 'syxy': (1, 1, 1, 0), 1876 | 'szxy': (2, 0, 0, 0), 1877 | 'szxz': (2, 0, 1, 0), 1878 | 'szyx': (2, 1, 0, 0), 1879 | 'szyz': (2, 1, 1, 0), 1880 | 'rzyx': (0, 0, 0, 1), 1881 | 'rxyx': (0, 0, 1, 1), 1882 | 'ryzx': (0, 1, 0, 1), 1883 | 'rxzx': (0, 1, 1, 1), 1884 | 'rxzy': (1, 0, 0, 1), 1885 | 'ryzy': (1, 0, 1, 1), 1886 | 'rzxy': (1, 1, 0, 1), 1887 | 'ryxy': (1, 1, 1, 1), 1888 | 'ryxz': (2, 0, 0, 1), 1889 | 'rzxz': (2, 0, 1, 1), 1890 | 'rxyz': (2, 1, 0, 1), 1891 | 'rzyz': (2, 1, 1, 1), 1892 | } 1893 | 1894 | _TUPLE2AXES = {v: k for k, v in _AXES2TUPLE.items()} 1895 | 1896 | 1897 | def vector_norm(data, axis=None, out=None): 1898 | """Return length, i.e. Euclidean norm, of ndarray along axis. 1899 | 1900 | >>> v = numpy.random.random(3) 1901 | >>> n = vector_norm(v) 1902 | >>> numpy.allclose(n, numpy.linalg.norm(v)) 1903 | True 1904 | >>> v = numpy.random.rand(6, 5, 3) 1905 | >>> n = vector_norm(v, axis=-1) 1906 | >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v * v, axis=2))) 1907 | True 1908 | >>> n = vector_norm(v, axis=1) 1909 | >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v * v, axis=1))) 1910 | True 1911 | >>> v = numpy.random.rand(5, 4, 3) 1912 | >>> n = numpy.empty((5, 3)) 1913 | >>> vector_norm(v, axis=1, out=n) 1914 | >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v * v, axis=1))) 1915 | True 1916 | >>> vector_norm([]) 1917 | 0.0 1918 | >>> vector_norm([1]) 1919 | 1.0 1920 | 1921 | """ 1922 | data = numpy.array(data, dtype=numpy.float64, copy=True) 1923 | if out is None: 1924 | if data.ndim == 1: 1925 | return math.sqrt(numpy.dot(data, data)) 1926 | data *= data 1927 | out = numpy.atleast_1d(numpy.sum(data, axis=axis)) 1928 | numpy.sqrt(out, out=out) 1929 | return out 1930 | data *= data 1931 | numpy.sum(data, axis=axis, out=out) 1932 | numpy.sqrt(out, out=out) 1933 | return None 1934 | 1935 | 1936 | def unit_vector(data, axis=None, out=None): 1937 | """Return ndarray normalized by length, i.e. Euclidean norm, along axis. 1938 | 1939 | >>> v0 = numpy.random.random(3) 1940 | >>> v1 = unit_vector(v0) 1941 | >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0)) 1942 | True 1943 | >>> v0 = numpy.random.rand(5, 4, 3) 1944 | >>> v1 = unit_vector(v0, axis=-1) 1945 | >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0 * v0, axis=2)), 2) 1946 | >>> numpy.allclose(v1, v2) 1947 | True 1948 | >>> v1 = unit_vector(v0, axis=1) 1949 | >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0 * v0, axis=1)), 1) 1950 | >>> numpy.allclose(v1, v2) 1951 | True 1952 | >>> v1 = numpy.empty((5, 4, 3)) 1953 | >>> unit_vector(v0, axis=1, out=v1) 1954 | >>> numpy.allclose(v1, v2) 1955 | True 1956 | >>> list(unit_vector([])) 1957 | [] 1958 | >>> list(unit_vector([1])) 1959 | [...1.0...] 1960 | 1961 | """ 1962 | if out is None: 1963 | data = numpy.array(data, dtype=numpy.float64, copy=True) 1964 | if data.ndim == 1: 1965 | data /= math.sqrt(numpy.dot(data, data)) 1966 | return data 1967 | else: 1968 | if out is not data: 1969 | out[:] = numpy.asarray(data) 1970 | data = out 1971 | length = numpy.atleast_1d(numpy.sum(data * data, axis)) 1972 | numpy.sqrt(length, length) 1973 | if axis is not None: 1974 | length = numpy.expand_dims(length, axis) 1975 | data /= length 1976 | if out is None: 1977 | return data 1978 | return None 1979 | 1980 | 1981 | def random_vector(size): 1982 | """Return array of random doubles in the half-open interval [0.0, 1.0). 1983 | 1984 | >>> v = random_vector(10000) 1985 | >>> bool(numpy.all(v >= 0) and numpy.all(v < 1)) 1986 | True 1987 | >>> v0 = random_vector(10) 1988 | >>> v1 = random_vector(10) 1989 | >>> bool(numpy.any(v0 == v1)) 1990 | False 1991 | 1992 | """ 1993 | return numpy.random.random(size) 1994 | 1995 | 1996 | def vector_product(v0, v1, axis=0): 1997 | """Return vector perpendicular to vectors. 1998 | 1999 | >>> v = vector_product([2, 0, 0], [0, 3, 0]) 2000 | >>> numpy.allclose(v, [0, 0, 6]) 2001 | True 2002 | >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]] 2003 | >>> v1 = [[3], [0], [0]] 2004 | >>> v = vector_product(v0, v1) 2005 | >>> numpy.allclose(v, [[0, 0, 0, 0], [0, 0, 6, 6], [0, -6, 0, -6]]) 2006 | True 2007 | >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]] 2008 | >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]] 2009 | >>> v = vector_product(v0, v1, axis=1) 2010 | >>> numpy.allclose(v, [[0, 0, 6], [0, -6, 0], [6, 0, 0], [0, -6, 6]]) 2011 | True 2012 | 2013 | """ 2014 | return numpy.cross(v0, v1, axis=axis) 2015 | 2016 | 2017 | def angle_between_vectors(v0, v1, directed=True, axis=0): 2018 | """Return angle between vectors. 2019 | 2020 | If directed is False, the input vectors are interpreted as undirected axes, 2021 | i.e. the maximum angle is pi/2. 2022 | 2023 | >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3]) 2024 | >>> numpy.allclose(a, math.pi) 2025 | True 2026 | >>> a = angle_between_vectors([1, -2, 3], [-1, 2, -3], directed=False) 2027 | >>> numpy.allclose(a, 0) 2028 | True 2029 | >>> v0 = [[2, 0, 0, 2], [0, 2, 0, 2], [0, 0, 2, 2]] 2030 | >>> v1 = [[3], [0], [0]] 2031 | >>> a = angle_between_vectors(v0, v1) 2032 | >>> numpy.allclose(a, [0, 1.5708, 1.5708, 0.95532]) 2033 | True 2034 | >>> v0 = [[2, 0, 0], [2, 0, 0], [0, 2, 0], [2, 0, 0]] 2035 | >>> v1 = [[0, 3, 0], [0, 0, 3], [0, 0, 3], [3, 3, 3]] 2036 | >>> a = angle_between_vectors(v0, v1, axis=1) 2037 | >>> numpy.allclose(a, [1.5708, 1.5708, 1.5708, 0.95532]) 2038 | True 2039 | 2040 | """ 2041 | v0 = numpy.asarray(v0, dtype=numpy.float64) 2042 | v1 = numpy.asarray(v1, dtype=numpy.float64) 2043 | dot = numpy.sum(v0 * v1, axis=axis) 2044 | dot /= vector_norm(v0, axis=axis) * vector_norm(v1, axis=axis) 2045 | dot = numpy.clip(dot, -1.0, 1.0) 2046 | return numpy.arccos(dot if directed else numpy.fabs(dot)) 2047 | 2048 | 2049 | def inverse_matrix(matrix): 2050 | """Return inverse of square transformation matrix. 2051 | 2052 | >>> M0 = random_rotation_matrix() 2053 | >>> M1 = inverse_matrix(M0.T) 2054 | >>> numpy.allclose(M1, numpy.linalg.inv(M0.T)) 2055 | True 2056 | >>> for size in range(1, 7): 2057 | ... M0 = numpy.random.rand(size, size) 2058 | ... M1 = inverse_matrix(M0) 2059 | ... if not numpy.allclose(M1, numpy.linalg.inv(M0)): 2060 | ... print(size) 2061 | ... 2062 | 2063 | """ 2064 | return numpy.linalg.inv(matrix) 2065 | 2066 | 2067 | def concatenate_matrices(*matrices): 2068 | """Return concatenation of series of transformation matrices. 2069 | 2070 | >>> M = numpy.random.rand(16).reshape((4, 4)) - 0.5 2071 | >>> numpy.allclose(M, concatenate_matrices(M)) 2072 | True 2073 | >>> numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T)) 2074 | True 2075 | 2076 | """ 2077 | M = numpy.identity(4) 2078 | for i in matrices: 2079 | M = numpy.dot(M, i) 2080 | return M 2081 | 2082 | 2083 | def is_same_transform(matrix0, matrix1): 2084 | """Return True if two matrices perform same transformation. 2085 | 2086 | >>> is_same_transform(numpy.identity(4), numpy.identity(4)) 2087 | True 2088 | >>> is_same_transform(numpy.identity(4), random_rotation_matrix()) 2089 | False 2090 | 2091 | """ 2092 | matrix0 = numpy.array(matrix0, dtype=numpy.float64, copy=True) 2093 | matrix0 /= matrix0[3, 3] 2094 | matrix1 = numpy.array(matrix1, dtype=numpy.float64, copy=True) 2095 | matrix1 /= matrix1[3, 3] 2096 | return numpy.allclose(matrix0, matrix1) 2097 | 2098 | 2099 | def is_same_quaternion(q0, q1): 2100 | """Return True if two quaternions are equal.""" 2101 | q0 = numpy.array(q0) 2102 | q1 = numpy.array(q1) 2103 | return numpy.allclose(q0, q1) or numpy.allclose(q0, -q1) 2104 | 2105 | 2106 | def _import_module(name, package=None, warn=True, postfix='_py', ignore='_'): 2107 | """Try import all public attributes from module into global namespace. 2108 | 2109 | Existing attributes with name clashes are renamed with prefix. 2110 | Attributes starting with underscore are ignored by default. 2111 | 2112 | Return True on successful import. 2113 | 2114 | """ 2115 | import warnings 2116 | from importlib import import_module 2117 | 2118 | try: 2119 | if not package: 2120 | module = import_module(name) 2121 | else: 2122 | module = import_module('.' + name, package=package) 2123 | except ImportError as exc: 2124 | if warn: 2125 | warnings.warn(str(exc)) 2126 | else: 2127 | for attr in dir(module): 2128 | if ignore and attr.startswith(ignore): 2129 | continue 2130 | if postfix: 2131 | if attr in globals(): 2132 | globals()[attr + postfix] = globals()[attr] 2133 | __all__.append(attr + postfix) 2134 | elif warn: 2135 | warnings.warn('no Python implementation of ' + attr) 2136 | globals()[attr] = getattr(module, attr) 2137 | return True 2138 | return None 2139 | 2140 | 2141 | _import_module('_transformations', __package__) 2142 | 2143 | 2144 | if __name__ == '__main__': 2145 | import doctest 2146 | import random # noqa: used in doctests 2147 | 2148 | numpy.set_printoptions(suppress=True, precision=5) 2149 | doctest.testmod(optionflags=doctest.ELLIPSIS) 2150 | 2151 | # mypy: allow-untyped-defs, allow-untyped-calls 2152 | --------------------------------------------------------------------------------