├── tests ├── __init__.py ├── test_pylinalg.py ├── test_misc.py ├── test_quaternion.py ├── conftest.py ├── test_matrix.py └── test_vectors.py ├── docs ├── _static │ └── style.css ├── index.rst ├── reference.rst ├── Makefile ├── make.bat └── conf.py ├── .github ├── CODEOWNERS └── workflows │ └── ci.yml ├── .gitignore ├── pylinalg ├── __init__.py ├── misc.py ├── quaternion.py ├── vector.py └── matrix.py ├── .readthedocs.yaml ├── README.md ├── LICENSE ├── pyproject.toml └── CONVENTIONS.md /tests/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /docs/_static/style.css: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /.github/CODEOWNERS: -------------------------------------------------------------------------------- 1 | * @Korijn 2 | -------------------------------------------------------------------------------- /docs/index.rst: -------------------------------------------------------------------------------- 1 | Pylinalg 2 | -------- 3 | 4 | .. toctree:: 5 | :maxdepth: 2 6 | 7 | reference 8 | -------------------------------------------------------------------------------- /docs/reference.rst: -------------------------------------------------------------------------------- 1 | Pylinalg API reference 2 | ---------------------- 3 | 4 | .. automodule:: pylinalg 5 | :members: 6 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.egg-info/ 2 | __pycache__ 3 | .vscode/ 4 | .coverage 5 | dist 6 | playground.py 7 | .hypothesis/ 8 | .venv/ 9 | docs/_build/ 10 | -------------------------------------------------------------------------------- /pylinalg/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | pylinalg 3 | 4 | Linear algebra utilities for Python. 5 | """ 6 | 7 | from importlib.metadata import version 8 | 9 | from .matrix import * 10 | from .misc import * 11 | from .quaternion import * 12 | from .vector import * 13 | 14 | __version__ = version("pylinalg") 15 | version_info = tuple(map(int, __version__.split("."))) 16 | 17 | del version 18 | 19 | 20 | __all__ = [ 21 | name for name in globals() if name.startswith(("vec_", "mat_", "quat_", "aabb_")) 22 | ] 23 | -------------------------------------------------------------------------------- /.readthedocs.yaml: -------------------------------------------------------------------------------- 1 | # Read the Docs configuration file 2 | # See https://docs.readthedocs.io/en/stable/config-file/v2.html for details 3 | 4 | version: 2 5 | 6 | build: 7 | os: ubuntu-22.04 8 | tools: 9 | python: "3.12" 10 | 11 | sphinx: 12 | configuration: docs/conf.py 13 | fail_on_warning: true 14 | 15 | # Optionally declare the Python requirements required to build your docs 16 | python: 17 | install: 18 | - method: pip 19 | path: . 20 | extra_requirements: 21 | - docs 22 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line, and also 5 | # from the environment for the first two. 6 | SPHINXOPTS ?= 7 | SPHINXBUILD ?= sphinx-build 8 | SOURCEDIR = . 9 | BUILDDIR = _build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | clean : 16 | -rm -r _build 17 | -rm -r _autosummary 18 | 19 | .PHONY: help Makefile 20 | 21 | # Catch-all target: route all unknown targets to Sphinx using the new 22 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 23 | %: Makefile 24 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 25 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # pylinalg 2 | 3 | [![CI](https://github.com/pygfx/pylinalg/actions/workflows/ci.yml/badge.svg)](https://github.com/pygfx/pylinalg/actions/workflows/ci.yml) 4 | [![Documentation Status 5 | ](https://readthedocs.org/projects/pylinalg/badge/?version=latest) 6 | ](https://pylinalg.readthedocs.io/en/latest/?badge=latest) 7 | [![PyPI version ](https://badge.fury.io/py/pylinalg.svg) 8 | ](https://badge.fury.io/py/pylinalg) 9 | 10 | Linear algebra utilities for Python. 11 | 12 |

13 | [API Reference] 14 |

15 | 16 | ## Installation 17 | 18 | ```bash 19 | pip install pylinalg 20 | ``` 21 | 22 | ### Development Install 23 | To get a working dev install of pylinalg you can use the following steps: 24 | 25 | ```bash 26 | # Click the Fork button on GitHub and navigate to your fork 27 | git clone 28 | cd pylinalg 29 | # if you use a venv, create and activate it 30 | pip install -e ".[dev,docs,examples]" 31 | pytest tests 32 | ``` 33 | -------------------------------------------------------------------------------- /docs/make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | pushd %~dp0 4 | 5 | REM Command file for Sphinx documentation 6 | 7 | if "%SPHINXBUILD%" == "" ( 8 | set SPHINXBUILD=sphinx-build 9 | ) 10 | set SOURCEDIR=. 11 | set BUILDDIR=_build 12 | 13 | if "%1" == "" goto help 14 | 15 | if "%1" == "clean" goto clean 16 | 17 | %SPHINXBUILD% >NUL 2>NUL 18 | if errorlevel 9009 ( 19 | echo. 20 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 21 | echo.installed, then set the SPHINXBUILD environment variable to point 22 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 23 | echo.may add the Sphinx directory to PATH. 24 | echo. 25 | echo.If you don't have Sphinx installed, grab it from 26 | echo.http://sphinx-doc.org/ 27 | exit /b 1 28 | ) 29 | 30 | %SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 31 | goto end 32 | 33 | :help 34 | %SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 35 | goto end 36 | 37 | :clean 38 | rmdir /s/q "_build" 39 | rmdir /s/q "_autosummary" 40 | goto end 41 | 42 | :end 43 | popd 44 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright © 2019-2022 pylinalg authors 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. 22 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | # ===== Project info 2 | 3 | [project] 4 | name = "pylinalg" 5 | version = "0.6.8" 6 | description = "Linear algebra utilities for Python" 7 | readme = "README.md" 8 | license = { file = "LICENSE" } 9 | authors = [{ name = "Almar Klein" }, { name = "Korijn van Golen" }] 10 | keywords = [ 11 | "graphics", 12 | "3d", 13 | "linear algebra", 14 | ] 15 | requires-python = ">= 3.9" 16 | dependencies = [ 17 | "numpy>=1.20.0", 18 | ] 19 | 20 | [project.optional-dependencies] 21 | lint = ["ruff"] 22 | docs = [ 23 | "sphinx>7.2", 24 | "sphinx_rtd_theme", 25 | ] 26 | tests = [ 27 | "pytest", 28 | "pytest-cov", 29 | "pytest-watcher", 30 | "hypothesis[numpy]~=6.61.0", 31 | "packaging", 32 | "twine", 33 | "scipy", 34 | ] 35 | dev = ["pylinalg[lint,tests,docs]"] 36 | 37 | [project.urls] 38 | Repository = "https://github.com/pygfx/pylinalg" 39 | 40 | # ===== Building 41 | 42 | # Flit is great solution for simple pure-Python projects. 43 | [build-system] 44 | requires = ["flit_core >=3.2,<4"] 45 | build-backend = "flit_core.buildapi" 46 | 47 | # ===== Tooling 48 | 49 | [tool.ruff] 50 | line-length = 88 51 | 52 | [tool.ruff.lint] 53 | select = ["F", "E", "W", "N", "B", "RUF"] 54 | ignore = [ 55 | "E501", # Line too long 56 | "E731", # Do not assign a `lambda` expression, use a `def` 57 | "B019", # Use of `functools.lru_cache` or `functools.cache` on methods can lead to memory leaks 58 | "RUF012", # Mutable class attributes should be annotated with `typing.ClassVar`" 59 | ] 60 | 61 | [tool.ruff.lint.per-file-ignores] 62 | "__init__.py" = ["F401", "F403", "RUF048"] 63 | "tests/conftest.py" = ["B008"] -------------------------------------------------------------------------------- /docs/conf.py: -------------------------------------------------------------------------------- 1 | """Configuration script for Sphinx.""" 2 | 3 | import os 4 | import shutil 5 | import sys 6 | 7 | ROOT_DIR = os.path.abspath(os.path.join(__file__, "..", "..")) 8 | sys.path.insert(0, ROOT_DIR) 9 | 10 | # -- Project information ----------------------------------------------------- 11 | 12 | project = "pylinalg" 13 | copyright = "2021-2025, Almar Klein, Korijn van Golen" 14 | author = "Almar Klein, Korijn van Golen" 15 | 16 | # The full version, including alpha/beta/rc tags 17 | # release = '0.1.0' 18 | 19 | # -- General configuration --------------------------------------------------- 20 | 21 | # Add any Sphinx extension module names here, as strings. They can be 22 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 23 | # ones. 24 | extensions = [ 25 | "sphinx.ext.autodoc", 26 | "sphinx.ext.napoleon", 27 | "sphinx.ext.autosummary", 28 | "sphinx_rtd_theme", 29 | ] 30 | 31 | html_theme = "sphinx_rtd_theme" 32 | 33 | # Add any paths that contain templates here, relative to this directory. 34 | templates_path = [] 35 | 36 | # Just let autosummary produce a new version each time 37 | shutil.rmtree(os.path.join(os.path.dirname(__file__), "_autosummary"), True) 38 | 39 | # List of patterns, relative to source directory, that match files and 40 | # directories to ignore when looking for source files. 41 | # This pattern also affects html_static_path and html_extra_path. 42 | exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] 43 | 44 | # -- Options for HTML output ------------------------------------------------- 45 | 46 | # The theme to use for HTML and HTML Help pages. See the documentation for 47 | # a list of builtin themes. 48 | # 49 | # html_theme = 'alabaster' 50 | 51 | # Add any paths that contain custom static files (such as style sheets) here, 52 | # relative to this directory. They are copied after the builtin static files, 53 | # so a file named "default.css" will overwrite the builtin "default.css". 54 | html_static_path = ["_static"] 55 | 56 | html_theme_options = {} 57 | -------------------------------------------------------------------------------- /tests/test_pylinalg.py: -------------------------------------------------------------------------------- 1 | import importlib 2 | import inspect 3 | 4 | import packaging.version 5 | 6 | import pylinalg as la 7 | 8 | 9 | def test_api(): 10 | api = dir(la) 11 | 12 | def popattr(key): 13 | val = getattr(la, key) 14 | api.remove(key) 15 | return val 16 | 17 | # assert that we expose a valid version 18 | __version__ = popattr("__version__") 19 | packaging.version.parse(__version__) 20 | 21 | # we don't want a runtime dependency on `packaging` 22 | # so parsing of version_info is a little dumb in 23 | # that it always expects integer components 24 | # so we can just test for that 25 | version_info = popattr("version_info") 26 | assert isinstance(version_info, tuple) 27 | assert all(isinstance(x, int) for x in version_info) 28 | 29 | # assert that we expose __all__ for tools like sphinx 30 | __all__ = popattr("__all__") 31 | 32 | # assert that all the remaining elements of the 33 | # public api are either builtins, submodules/packages, 34 | # or callables with legal prefixes 35 | legal_prefixes = ("vec_", "mat_", "quat_", "aabb_") 36 | callables = [] 37 | for key in api: 38 | if key.startswith("__") and key.endswith("__"): 39 | # builtins are OK 40 | continue 41 | if inspect.ismodule(getattr(la, key)): 42 | # submodule/package 43 | try: 44 | importlib.import_module(f".{key}", "pylinalg") 45 | except ImportError: 46 | # should be `del`eted 47 | raise AssertionError( 48 | f"API includes module '{key}' which is not a submodule/package" 49 | ) from None 50 | # actual pylinalg submodules/packages are OK 51 | continue 52 | # otherwise it should be a callable 53 | # with a legal prefix 54 | assert key.startswith(legal_prefixes) 55 | assert callable(getattr(la, key)) 56 | callables.append(key) 57 | 58 | # confirm the signature of each callable 59 | sig = inspect.signature(getattr(la, key)) 60 | 61 | assert sig.return_annotation is not inspect.Signature.empty, ( 62 | f"Missing return annotation on {key}" 63 | ) 64 | if sig.return_annotation is bool: 65 | key_parts = key.split("_") 66 | assert key_parts[1] in ("is", "has") 67 | else: 68 | has_out, has_dtype = False, False 69 | for param in sig.parameters.values(): 70 | # all arguments are either positional-only, or keyword-only 71 | assert param.kind in (param.POSITIONAL_ONLY, param.KEYWORD_ONLY) 72 | # every function has out & dtype keyword-only arguments 73 | if param.name == "dtype": 74 | assert param.KEYWORD_ONLY 75 | has_dtype = True 76 | elif param.name == "out": 77 | assert param.KEYWORD_ONLY 78 | has_out = True 79 | assert has_out, f"Function {key} does not have 'out' keyword-only argument" 80 | assert has_dtype, ( 81 | f"Function {key} does not have 'dtype' keyword-only argument" 82 | ) 83 | 84 | # assert that all callables are available in __all__ 85 | assert set(__all__) == set(callables) 86 | -------------------------------------------------------------------------------- /tests/test_misc.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from hypothesis import assume, given 3 | from hypothesis.strategies import floats 4 | 5 | import pylinalg as la 6 | 7 | from . import conftest as ct 8 | 9 | 10 | @given( 11 | ct.test_unit_vector, 12 | ct.test_unit_vector, 13 | floats(-1e6, 1e6, allow_nan=False, allow_infinity=False), 14 | ) 15 | def test_aabb_to_sphere(point, offset, scale): 16 | assume(abs(scale) > 1e-16) 17 | assume(np.linalg.norm(point - (0, 0, 1)) > 1e-16) 18 | assume(np.linalg.norm(point - (0, 1, 0)) > 1e-16) 19 | assume(np.linalg.norm(point - (1, 0, 0)) > 1e-16) 20 | 21 | candidate = np.stack( 22 | ( 23 | scale * point + offset, 24 | -scale * point + offset, 25 | ) 26 | ) 27 | 28 | aabb = np.empty_like(candidate) 29 | aabb[0, :] = np.min(candidate, axis=0) 30 | aabb[1, :] = np.max(candidate, axis=0) 31 | 32 | sphere = la.aabb_to_sphere(aabb) 33 | 34 | assert np.allclose(sphere[:3], offset, atol=1e-10) 35 | assert np.allclose(sphere[-1], abs(scale), rtol=1e-10) 36 | 37 | 38 | @given(ct.test_unit_vector, ct.test_vector, ct.test_scaling) 39 | def test_aabb_transform(point, translation, scale): 40 | candidate = np.stack((point, -point)) 41 | aabb = np.empty_like(candidate) 42 | aabb[0, :] = np.min(candidate, axis=0) 43 | aabb[1, :] = np.max(candidate, axis=0) 44 | 45 | translation_matrix = la.mat_from_translation(translation) 46 | result = la.aabb_transform(aabb, translation_matrix) 47 | assert np.allclose(result, aabb + translation, atol=1e-10) 48 | 49 | scale_matrix = la.mat_from_scale(scale) 50 | result = la.aabb_transform(aabb, scale_matrix) 51 | assert np.allclose(result, np.sort(aabb * scale, axis=0), atol=1e-10) 52 | 53 | 54 | def test_aabb_transform_single(): 55 | """Test single transform.""" 56 | aabb = np.array([[-1, -1, -1], [1, 1, 1]]) 57 | translation = np.array([1, 0, 0]) 58 | translation_matrix = la.mat_from_translation(translation) 59 | 60 | expected = aabb + translation 61 | result = la.aabb_transform(aabb, translation_matrix) 62 | assert np.allclose(result, expected, atol=1e-10) 63 | 64 | 65 | def test_aabb_transform_broadcasting(): 66 | """Test pairwise broadcasting of AABBs and matrices.""" 67 | aabbs = np.array( 68 | [ 69 | [[-1, -1, -1], [1, 1, 1]], 70 | [[-2, -2, -2], [2, 2, 2]], 71 | ] 72 | ) 73 | translations = np.array( 74 | [ 75 | [1, 0, 0], 76 | [0, 1, 0], 77 | ] 78 | ) 79 | translation_matrices = la.mat_from_translation(translations) 80 | 81 | expected = aabbs + translations[:, np.newaxis, :] 82 | result = la.aabb_transform(aabbs, translation_matrices) 83 | assert np.allclose(result, expected, atol=1e-10) 84 | 85 | 86 | def test_aabb_transform_broadcasting_2(): 87 | """Test broadcasting many matrices and one AABB.""" 88 | aabb = np.array([[-1, -1, -1], [1, 1, 1]]) 89 | translations = np.array( 90 | [ 91 | [1, 0, 0], 92 | [0, 1, 0], 93 | ] 94 | ) 95 | translation_matrices = la.mat_from_translation(translations) 96 | 97 | expected = aabb[np.newaxis, ...] + translations[:, np.newaxis, :] 98 | result = la.aabb_transform(aabb, translation_matrices) 99 | assert np.allclose(result, expected, atol=1e-10) 100 | -------------------------------------------------------------------------------- /pylinalg/misc.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def aabb_to_sphere(aabb, /, *, out=None, dtype=None) -> np.ndarray: 5 | """A sphere that envelops an Axis-Aligned Bounding Box. 6 | 7 | Parameters 8 | ---------- 9 | aabb : ndarray, [2, 3] 10 | The axis-aligned bounding box. 11 | out : ndarray, optional 12 | A location into which the result is stored. If provided, it 13 | must have a shape that the inputs broadcast to. If not provided or 14 | None, a freshly-allocated array is returned. A tuple must have 15 | length equal to the number of outputs. 16 | dtype : data-type, optional 17 | Overrides the data type of the result. 18 | 19 | Returns 20 | ------- 21 | sphere : ndarray, [4] 22 | A sphere (x, y, z, radius). 23 | 24 | """ 25 | 26 | aabb = np.asarray(aabb, dtype=float) 27 | 28 | if out is None: 29 | out = np.empty((*aabb.shape[:-2], 4), dtype=dtype) 30 | 31 | out[..., :3] = np.sum(aabb, axis=-2) / 2 32 | out[..., 3] = np.linalg.norm(np.diff(aabb, axis=-2), axis=-1) / 2 33 | 34 | return out 35 | 36 | 37 | def aabb_transform(aabb, matrix, /, *, out=None, dtype=None) -> np.ndarray: 38 | """Apply an affine transformation to an axis-aligned bounding box. 39 | 40 | Parameters 41 | ---------- 42 | aabb : ndarray, [2, 3] 43 | The axis-aligned bounding box. 44 | homogeneous_matrix : [4, 4] 45 | The homogeneous transformation to apply. 46 | out : ndarray, optional 47 | A location into which the result is stored. If provided, it 48 | must have a shape that the inputs broadcast to. If not provided or 49 | None, a freshly-allocated array is returned. A tuple must have 50 | length equal to the number of outputs. 51 | dtype : data-type, optional 52 | Overrides the data type of the result. 53 | 54 | Returns 55 | ------- 56 | aabb : ndarray, [2, 3] 57 | The transformed axis-aligned bounding box. 58 | 59 | Notes 60 | ----- 61 | This function preserves the alignment axes of the bounding box. This means 62 | the returned bounding box has the same alignment axes as the input bounding 63 | box, but contains the transformed object. In other words, the box will grow 64 | or shrink depending on how the contained object is transformed, but its 65 | alignment axis stay the same. 66 | 67 | """ 68 | 69 | aabb = np.asarray(aabb, dtype=float) 70 | matrix = np.asarray(matrix, dtype=float) 71 | 72 | # transpose last two dimensions 73 | axes = list(range(matrix.ndim)) 74 | axes[-2:] = axes[-1], axes[-2] 75 | matrix = matrix.transpose(axes) 76 | 77 | if out is None: 78 | # Compute output shape by broadcasting aabb and matrix shapes (excluding last 2 dims) 79 | aabb_shape = aabb.shape[:-2] 80 | matrix_shape = matrix.shape[:-2] 81 | broadcast_shape = np.broadcast_shapes(aabb_shape, matrix_shape) 82 | out = np.empty((*broadcast_shape, *aabb.shape[-2:]), dtype=dtype) 83 | 84 | corners = np.full( 85 | (*aabb.shape[:-2], 8, 4), 86 | # Fill value of 1 is used for homogeneous coordinates. 87 | fill_value=1.0, 88 | dtype=float, 89 | ) 90 | 91 | # x 92 | corners[..., 0::2, 0] = aabb[..., 0, 0, np.newaxis] 93 | corners[..., 1::2, 0] = aabb[..., 1, 0, np.newaxis] 94 | 95 | # y 96 | corners[..., 0::4, 1] = aabb[..., 0, 1, np.newaxis] 97 | corners[..., 1::4, 1] = aabb[..., 0, 1, np.newaxis] 98 | corners[..., 2::4, 1] = aabb[..., 1, 1, np.newaxis] 99 | corners[..., 3::4, 1] = aabb[..., 1, 1, np.newaxis] 100 | 101 | # z 102 | corners[..., 0:4, 2] = aabb[..., 0, 2, np.newaxis] 103 | corners[..., 4:8, 2] = aabb[..., 1, 2, np.newaxis] 104 | 105 | corners = corners @ matrix 106 | out[..., 0, :] = np.min(corners[..., :-1], axis=-2) 107 | out[..., 1, :] = np.max(corners[..., :-1], axis=-2) 108 | 109 | return out 110 | 111 | 112 | def quat_to_axis_angle(quaternion, /, *, out=None, dtype=None) -> np.ndarray: 113 | """Convert a quaternion to axis-angle representation. 114 | 115 | Parameters 116 | ---------- 117 | quaternion : ndarray, [4] 118 | A quaternion describing the rotation. 119 | out : Tuple[ndarray, ...], optional 120 | A location into which the result is stored. If provided, it 121 | must have a shape that the inputs broadcast to. If not provided or 122 | None, a freshly-allocated array is returned. A tuple must have 123 | length equal to the number of outputs. 124 | dtype : data-type, optional 125 | Overrides the data type of the result. 126 | 127 | Returns 128 | ------- 129 | axis : ndarray, [3] 130 | The axis around which the quaternion rotates in euclidean coordinates. 131 | angle : ndarray, [1] 132 | The angle (in rad) by which the quaternion rotates. 133 | 134 | Notes 135 | ----- 136 | To use `out` with a single quaternion you need to provide a ndarray of shape 137 | ``(1,)`` for angle. 138 | 139 | """ 140 | 141 | quaternion = np.asarray(quaternion) 142 | 143 | if out is None: 144 | if dtype is not None: 145 | quaternion = quaternion.astype(dtype, copy=False) 146 | out = ( 147 | quaternion[..., :3] / np.sqrt(1 - quaternion[..., 3] ** 2), 148 | 2 * np.arccos(quaternion[..., 3]), 149 | ) 150 | else: 151 | out[0][:] = quaternion[..., :3] / np.sqrt(1 - quaternion[..., 3] ** 2) 152 | out[1][:] = 2 * np.arccos(quaternion[..., 3]) 153 | 154 | return out 155 | 156 | 157 | __all__ = [ 158 | name for name in globals() if name.startswith(("vec_", "mat_", "quat_", "aabb_")) 159 | ] 160 | -------------------------------------------------------------------------------- /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: 4 | push: 5 | branches: 6 | - main 7 | tags: 8 | - 'v*' 9 | pull_request: 10 | branches: 11 | - main 12 | 13 | concurrency: 14 | group: CI-${{ github.ref }} 15 | cancel-in-progress: true 16 | 17 | jobs: 18 | 19 | lint-build: 20 | name: Linting 21 | runs-on: ubuntu-latest 22 | strategy: 23 | fail-fast: false 24 | steps: 25 | - uses: actions/checkout@v4 26 | - name: Set up Python 27 | uses: actions/setup-python@v5 28 | with: 29 | python-version: 3.12 30 | - name: Install dependencies 31 | run: | 32 | python -m pip install --upgrade pip 33 | pip install ruff 34 | - name: Ruff lint 35 | run: | 36 | ruff check --output-format=github . 37 | - name: Ruff format 38 | run: | 39 | ruff format --check . 40 | 41 | docs-build: 42 | name: Docs 43 | runs-on: ubuntu-latest 44 | strategy: 45 | fail-fast: false 46 | steps: 47 | - uses: actions/checkout@v4 48 | - name: Set up Python 49 | uses: actions/setup-python@v5 50 | with: 51 | python-version: 3.12 52 | - name: Install dev dependencies 53 | run: | 54 | python -m pip install --upgrade pip 55 | pip install -U -e .[docs] 56 | - name: Build docs 57 | run: | 58 | cd docs 59 | make html SPHINXOPTS="-W --keep-going" 60 | 61 | test-builds: 62 | name: ${{ matrix.name }} 63 | runs-on: ${{ matrix.os }} 64 | strategy: 65 | fail-fast: false 66 | matrix: 67 | include: 68 | - name: Test py39 69 | os: ubuntu-latest 70 | pyversion: '3.9' 71 | - name: Test py310 72 | os: ubuntu-latest 73 | pyversion: '3.10' 74 | - name: Test py311 75 | os: ubuntu-latest 76 | pyversion: '3.11' 77 | - name: Test py312 78 | os: ubuntu-latest 79 | pyversion: '3.12' 80 | - name: Test py313 81 | os: ubuntu-latest 82 | pyversion: '3.13' 83 | steps: 84 | - uses: actions/checkout@v4 85 | - name: Set up Python ${{ matrix.pyversion }} 86 | uses: actions/setup-python@v5 87 | with: 88 | python-version: ${{ matrix.pyversion }} 89 | - name: Install package and dev dependencies 90 | run: | 91 | python -m pip install --upgrade pip 92 | pip install .[tests] 93 | rm -r pylinalg 94 | - name: Unit tests 95 | run: | 96 | pytest -v tests --cov=pylinalg --cov-report=term-missing 97 | 98 | test-pygfx-builds: 99 | name: ${{ matrix.name }} 100 | runs-on: ${{ matrix.os }} 101 | strategy: 102 | fail-fast: false 103 | matrix: 104 | include: 105 | - name: Test pygfx release 106 | os: ubuntu-latest 107 | ref: refs/tags/v0.7.0 108 | pyversion: '3.12' 109 | - name: Test pygfx main 110 | os: ubuntu-latest 111 | ref: refs/heads/main 112 | pyversion: '3.12' 113 | steps: 114 | - uses: actions/checkout@v4 115 | with: 116 | path: pylinalg 117 | - uses: actions/checkout@v4 118 | with: 119 | repository: pygfx/pygfx 120 | path: pygfx 121 | ref: ${{ matrix.ref }} 122 | - name: Set up Python ${{ matrix.pyversion }} 123 | uses: actions/setup-python@v5 124 | with: 125 | python-version: ${{ matrix.pyversion }} 126 | - name: Install llvmpipe and lavapipe for offscreen canvas 127 | if: matrix.os == 'ubuntu-latest' 128 | run: | 129 | sudo apt-get update -y -qq 130 | sudo apt install -y libegl1-mesa-dev libgl1-mesa-dri libxcb-xfixes0-dev mesa-vulkan-drivers 131 | - name: Install package and dev dependencies 132 | run: | 133 | python -m pip install --upgrade pip 134 | cd pygfx 135 | pip install .[tests] 136 | rm -r pygfx 137 | cd ../pylinalg 138 | pip install . 139 | rm -r pylinalg 140 | - name: Unit tests 141 | run: | 142 | cd pygfx 143 | pytest -v --ignore=tests/utils/test_load.py tests 144 | 145 | release-build: 146 | name: Build release on ubuntu-latest 147 | runs-on: ubuntu-latest 148 | strategy: 149 | fail-fast: false 150 | steps: 151 | - uses: actions/checkout@v4 152 | - name: Set up Python 153 | uses: actions/setup-python@v5 154 | with: 155 | python-version: 3.12 156 | - name: Install dev dependencies 157 | run: | 158 | python -m pip install --upgrade pip 159 | pip install -U flit build twine 160 | - name: Create source distribution 161 | run: | 162 | python -m build -n -s 163 | - name: Build wheel 164 | run: | 165 | python -m build -n -w 166 | - name: Test sdist 167 | shell: bash 168 | run: | 169 | rm -rf ./pylinalg 170 | pushd $HOME 171 | pip install $GITHUB_WORKSPACE/dist/*.tar.gz 172 | python -c "import pylinalg; print(pylinalg.__version__)" 173 | popd 174 | # don't run tests, we just want to know if the sdist can be installed 175 | pip uninstall -y pylinalg 176 | git reset --hard HEAD 177 | - name: Twine check 178 | run: | 179 | twine check dist/* 180 | - name: Upload distributions 181 | uses: actions/upload-artifact@v4 182 | with: 183 | path: dist 184 | name: dist 185 | 186 | publish: 187 | name: Publish release to Github and Pypi 188 | runs-on: ubuntu-latest 189 | needs: [test-builds, release-build] 190 | if: success() && startsWith(github.ref, 'refs/tags/v') 191 | steps: 192 | - uses: actions/checkout@v4 193 | - name: Set up Python 194 | uses: actions/setup-python@v5 195 | with: 196 | python-version: 3.12 197 | - name: Download assets 198 | uses: actions/download-artifact@v4 199 | with: 200 | name: dist 201 | path: dist 202 | - name: Release 203 | uses: softprops/action-gh-release@v2 204 | with: 205 | token: ${{ secrets.GITHUB_TOKEN }} 206 | files: | 207 | dist/*.tar.gz 208 | dist/*.whl 209 | draft: true 210 | prerelease: false 211 | - name: Publish to PyPI 212 | uses: pypa/gh-action-pypi-publish@release/v1 213 | with: 214 | user: __token__ 215 | password: ${{ secrets.PYPI_PASSWORD }} 216 | -------------------------------------------------------------------------------- /tests/test_quaternion.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import numpy.testing as npt 3 | import pytest 4 | from hypothesis import assume, given 5 | from hypothesis.strategies import text 6 | 7 | import pylinalg as la 8 | 9 | from . import conftest as ct 10 | 11 | 12 | @pytest.mark.parametrize( 13 | "expected,quaternion,dtype", 14 | [ 15 | # case a 16 | ([0, 0, np.pi / 2], [0, 0, np.sqrt(2) / 2, np.sqrt(2) / 2], "f8"), 17 | # case a, two ordered rotations 18 | ([0, -np.pi / 2, np.pi / 2], [0.5, -0.5, 0.5, 0.5], "f8"), 19 | # non-default dtype 20 | ([0, -np.pi / 2, np.pi / 2], [0.5, -0.5, 0.5, 0.5], "f4"), 21 | # case b (contrived example for code coverage) 22 | ( 23 | [0, np.pi * 0.51, np.pi * 0.51], 24 | [0.515705, -0.499753, -0.499753, -0.484295], 25 | "f8", 26 | ), 27 | # case c (contrived example for code coverage) 28 | ( 29 | [np.pi * 1.2, np.pi * 1.8, np.pi], 30 | [-0.095492, 0.904508, -0.293893, -0.293893], 31 | "f8", 32 | ), 33 | # case d (contrived example for code coverage) 34 | ( 35 | [np.pi * 0.45, np.pi * 1.8, np.pi], 36 | [0.234978, 0.617662, 0.723189, -0.20069], 37 | "f8", 38 | ), 39 | ], 40 | ) 41 | def test_mat_from_quat(expected, quaternion, dtype): 42 | matrix = la.mat_from_quat(quaternion, dtype=dtype) 43 | 44 | expected_matrix = la.mat_from_euler(expected, dtype=dtype) 45 | npt.assert_array_almost_equal( 46 | matrix, 47 | expected_matrix, 48 | decimal=5, 49 | ) 50 | assert matrix.dtype == dtype 51 | 52 | 53 | def test_quat_mul_quaternion(): 54 | # quaternion corresponding to 90 degree rotation about z-axis 55 | a = np.array([0, 0, np.sqrt(2) / 2, np.sqrt(2) / 2]) 56 | b = np.array([0, 0, 0, 1]) 57 | c = la.quat_mul(a, b) 58 | # multiplying by the identity quaternion 59 | npt.assert_array_equal(c, a) 60 | 61 | d = la.quat_mul(a, a) 62 | # should be 180 degree rotation about z-axis 63 | npt.assert_array_almost_equal(d, [0, 0, 1, 0]) 64 | 65 | 66 | def test_quaternion_norm(): 67 | a = np.array([0, 0, np.sqrt(2) / 2, np.sqrt(2) / 2]) 68 | b = np.array([0, 0, 0, 1]) 69 | c = np.array([0, 0, 1, 0]) 70 | assert np.linalg.norm(a) == 1 71 | assert np.linalg.norm(b) == 1 72 | assert np.linalg.norm(c) == 1 73 | 74 | 75 | def test_quaternion_norm_vectorized(): 76 | a = np.array([[0, 0, np.sqrt(2) / 2, np.sqrt(2) / 2]]) 77 | npt.assert_array_equal(np.linalg.norm(a, axis=-1), [1]) 78 | 79 | 80 | @given(ct.test_unit_vector, ct.test_unit_vector, ct.legal_positive_number) 81 | def test_quaternion_from_unit_vectors( 82 | source_direction, target_direction, source_length 83 | ): 84 | assume(abs(source_length) > 1e-8) 85 | 86 | # Note: the length of the cross product of two large vectors can overflow 87 | # and become Inf. to avoid this, we only scale source. 88 | source = source_length * source_direction 89 | target = target_direction 90 | 91 | rotation = la.quat_from_vecs(source, target) 92 | actual = la.vec_transform_quat(source_direction, rotation) 93 | 94 | assert np.allclose(actual, target_direction) 95 | 96 | 97 | def test_quat_inv(): 98 | a = np.array([0, 0, np.sqrt(2) / 2, np.sqrt(2) / 2]) 99 | ai = la.quat_inv(a) 100 | 101 | npt.assert_array_equal(a[:3], -ai[:3]) 102 | npt.assert_array_equal(a[3], ai[3]) 103 | 104 | # broadcasting over multiple quaternions 105 | b = np.array( 106 | [ 107 | [0, 0, np.sqrt(2) / 2, np.sqrt(2) / 2], 108 | [0, 0, 0, 1], 109 | [0, 0, 1, 0], 110 | ] 111 | ) 112 | bi = la.quat_inv(b) 113 | 114 | npt.assert_array_equal(b[..., :3], -bi[..., :3]) 115 | npt.assert_array_equal(b[..., 3], bi[..., 3]) 116 | 117 | 118 | @given(ct.legal_positive_number) 119 | def test_quaternion_from_axis_angle(length): 120 | assume(abs(length) > 1e-10) 121 | 122 | axis = np.array([1, 0, 0], dtype="f4") 123 | angle = np.pi / 2 124 | q = la.quat_from_axis_angle(length * axis, angle) 125 | 126 | npt.assert_array_almost_equal(q, [np.sqrt(2) / 2, 0, 0, np.sqrt(2) / 2]) 127 | 128 | 129 | def test_quaternion_from_axis_angle_broadcasting(): 130 | actual = la.quat_from_axis_angle( 131 | [ 132 | [1, 0, 0], 133 | [0, 1, 0], 134 | [0, 0, 1], 135 | [0, 0, 1], 136 | ], 137 | [ 138 | np.pi, 139 | np.pi * 2, 140 | np.pi / 2, 141 | np.pi * 1.5, 142 | ], 143 | ) 144 | 145 | expected = np.array( 146 | [ 147 | [1, 0, 0, 0], 148 | [0, 0, 0, -1], 149 | [0, 0, np.sqrt(2) / 2, np.sqrt(2) / 2], 150 | [0, 0, np.sqrt(2) / 2, -np.sqrt(2) / 2], 151 | ] 152 | ) 153 | 154 | npt.assert_array_almost_equal(actual, expected) 155 | 156 | 157 | @given(ct.test_unit_vector, ct.legal_angle) 158 | def test_quaternion_from_axis_angle_roundtrip(true_axis, true_angle): 159 | assume(abs(true_angle) > 1e-6) 160 | assume(abs(true_angle) < 2 * np.pi - 1e-6) 161 | 162 | quaternion = la.quat_from_axis_angle(true_axis, true_angle) 163 | axis, angle = la.quat_to_axis_angle(quaternion) 164 | 165 | assert np.allclose(angle, true_angle) 166 | 167 | # Note: We loose the scaling of the axis, but can (roughly) reconstruct the 168 | # direction 169 | actual_dot = np.dot(axis, true_axis) 170 | assert np.allclose(actual_dot, 1, atol=1e-4) 171 | 172 | 173 | @given(ct.legal_positive_number) 174 | def test_quaternion_from_axis_angle_scaling(axis_scaling): 175 | assume(abs(axis_scaling) > 1e-6) 176 | 177 | true_axis = np.array((0, 1, 0)) 178 | quaternion = la.quat_from_axis_angle(axis_scaling * true_axis, np.pi / 2) 179 | axis, angle = la.quat_to_axis_angle(quaternion) 180 | 181 | assert np.allclose(angle, np.pi / 2) 182 | assert np.allclose(axis, true_axis) 183 | 184 | 185 | @given(ct.test_angles_rad, text("xyz", min_size=1, max_size=3)) 186 | def test_quat_from_euler(angles, order): 187 | angles = np.squeeze(angles[: len(order)]) 188 | result = la.quat_from_euler(angles, order=order) 189 | actual = la.mat_from_quat(result) 190 | 191 | expected = la.mat_from_euler(angles, order=order) 192 | assert np.allclose(actual, expected) 193 | 194 | 195 | def test_quat_from_axis_angle_input_mutation(): 196 | """(Regression) test that the input arguments are not mutated in-place.""" 197 | axis = np.array((0.0, 2.0, 0.0)) 198 | backup = axis.copy() 199 | la.quat_from_axis_angle(axis, np.pi / 2) 200 | npt.assert_array_equal(axis, backup) 201 | -------------------------------------------------------------------------------- /tests/conftest.py: -------------------------------------------------------------------------------- 1 | import platform 2 | import subprocess 3 | from math import cos, sin 4 | 5 | import hypothesis as hp 6 | import hypothesis.strategies as st 7 | import numpy as np 8 | from hypothesis.extra.numpy import arrays, from_dtype 9 | 10 | import pylinalg as la 11 | 12 | 13 | def pytest_report_header(config): 14 | # report the CPU model to allow detecting platform-specific problems 15 | if platform.system() == "Windows": 16 | try: 17 | name = ( 18 | subprocess.check_output(["wmic", "cpu", "get", "name"]) 19 | .decode() 20 | .strip() 21 | .split("\n")[1] 22 | ) 23 | cpu_info = " ".join([name]) 24 | except Exception: 25 | cpu_info = "Unknown CPU (wmic not available)" 26 | elif platform.system() == "Linux": 27 | info_string = subprocess.check_output(["lscpu"]).decode() 28 | for line in info_string.split("\n"): 29 | if line.startswith("Model name"): 30 | cpu_info = line[33:] 31 | break 32 | else: 33 | cpu_info = platform.processor() 34 | 35 | return "CPU: " + cpu_info 36 | 37 | 38 | # Hypothesis related logic 39 | # ------------------------ 40 | 41 | # upper bound on approximation error 42 | EPS = 1e-6 43 | 44 | 45 | @st.composite 46 | def generate_spherical_vector( 47 | draw, 48 | radius=st.floats(min_value=0, max_value=360, allow_infinity=False, allow_nan=False), 49 | theta=st.floats( 50 | min_value=EPS, max_value=np.pi - EPS, allow_infinity=False, allow_nan=False 51 | ), 52 | phi=st.floats( 53 | min_value=EPS, max_value=2 * np.pi - EPS, allow_infinity=False, allow_nan=False 54 | ), 55 | ): 56 | return np.array((draw(radius), draw(theta), draw(phi))) 57 | 58 | 59 | @st.composite 60 | def generate_quaternion( 61 | draw, 62 | elements=st.floats( 63 | min_value=0, max_value=360, allow_infinity=False, allow_nan=False 64 | ), 65 | snap_precision=0, 66 | ): 67 | """ 68 | Generate a valid quaternion 69 | 70 | This function generates a quaternion from three angles. It first generates a 71 | point on the unit-sphere (in polar coordinates) that represents the rotation 72 | vector. It then generates an angle to rotate by, and finally constructs a 73 | valid quaternion from this axis-angle representation. 74 | 75 | Parameters 76 | ---------- 77 | draw : Any 78 | Mandatory input from Hypothesis to track when elements are drawn from 79 | strategies to allow test-case simplification on failure. 80 | elements : strategy 81 | A strategy that creates valid elements. Defaults to any degree in [0, 360]. 82 | snap_precision : int 83 | The precision to which to round ("snap") angles to. 84 | 85 | Returns 86 | ------- 87 | quaternion : ndarray, [4] 88 | The generated quaternion. 89 | 90 | """ 91 | theta, phi, angle = draw(elements), draw(elements), draw(elements) 92 | theta, phi, angle = ( 93 | round(theta, snap_precision), 94 | round(phi, snap_precision), 95 | round(angle, snap_precision), 96 | ) 97 | hp.assume(theta <= 180) 98 | 99 | theta, phi, angle = ( 100 | 2 * np.pi * theta / 360, 101 | 2 * np.pi * phi / 360, 102 | 2 * np.pi * angle / 360, 103 | ) 104 | 105 | # spherical to euclidean (r = 1) 106 | x = cos(theta) * sin(phi) 107 | y = sin(theta) * sin(phi) 108 | z = cos(phi) 109 | 110 | # axis-angle to quaternion 111 | qx = x * sin(angle / 2) 112 | qy = y * sin(angle / 2) 113 | qz = z * sin(angle / 2) 114 | qw = cos(angle / 2) 115 | 116 | quaternion = np.array((qx, qy, qz, qw)) 117 | 118 | # reject samples that are not precise 119 | hp.assume(np.linalg.norm(quaternion) - 1 < EPS) 120 | 121 | return quaternion 122 | 123 | 124 | @st.composite 125 | def dtype_string(draw): 126 | letter = draw(st.sampled_from("?iuf")) 127 | 128 | if letter == "?": 129 | code = letter 130 | elif letter == "f": 131 | code = letter + draw(st.sampled_from("48")) 132 | else: 133 | code = letter + draw(st.sampled_from("1248")) 134 | 135 | return code 136 | 137 | 138 | def rotation_matrix(axis, angle): 139 | """Rotation by angle around the given cardinal axis. 140 | 141 | Parameters 142 | ---------- 143 | axis : str 144 | One of "x", "y", or "z". 145 | angle : float 146 | The angle to rotate by (in rad). 147 | 148 | """ 149 | axis_idx = {"x": 0, "y": 1, "z": 2}[axis] 150 | 151 | matrix = np.array([[cos(angle), -sin(angle)], [sin(angle), cos(angle)]]) 152 | if axis_idx == 1: 153 | matrix = matrix.T 154 | 155 | matrix = np.insert(matrix, axis_idx, 0, axis=0) 156 | matrix = np.insert(matrix, axis_idx, 0, axis=1) 157 | matrix[axis_idx, axis_idx] = 1 158 | 159 | return matrix 160 | 161 | 162 | @st.composite 163 | def unit_vector( 164 | draw, 165 | elements=st.floats( 166 | allow_infinity=False, allow_nan=False, min_value=0, max_value=2 * np.pi 167 | ), 168 | ): 169 | """ 170 | Generate a unit vector using a point on the unit-sphere 171 | (essentially spherical coordinates to euclidean coordinates) 172 | """ 173 | theta, phi = draw(elements), draw(elements) 174 | 175 | # spherical to euclidean (r = 1) 176 | x = cos(theta) * sin(phi) 177 | y = sin(theta) * sin(phi) 178 | z = cos(phi) 179 | 180 | return np.array((x, y, z)) 181 | 182 | 183 | @st.composite 184 | def perspecitve_matrix( 185 | draw, elements=st.floats(allow_infinity=False, allow_nan=False, min_value=1e-16) 186 | ): 187 | top, bottom = draw(elements), draw(elements) 188 | hp.assume(top != bottom) 189 | 190 | left, right = draw(elements), draw(elements) 191 | hp.assume(left != right) 192 | 193 | near, far = draw(elements), draw(elements) 194 | hp.assume(near != far) 195 | hp.assume(0 < near) 196 | hp.assume(near < far) 197 | 198 | matrix = la.mat_perspective(left, right, top, bottom, near, far) 199 | hp.assume(not (np.any(np.isinf(matrix) | np.isnan(matrix)))) 200 | 201 | try: 202 | np.linalg.inv(matrix) 203 | except np.linalg.LinAlgError: 204 | # only stable/invertible matrices 205 | hp.assume(False) 206 | 207 | return matrix 208 | 209 | 210 | @st.composite 211 | def orthographic_matrix( 212 | draw, elements=st.floats(allow_infinity=False, allow_nan=False) 213 | ): 214 | top, bottom = draw(elements), draw(elements) 215 | hp.assume(top != bottom) 216 | 217 | left, right = draw(elements), draw(elements) 218 | hp.assume(left != right) 219 | 220 | near, far = draw(elements), draw(elements) 221 | hp.assume(near != far) 222 | hp.assume(0 < near) 223 | hp.assume(near < far) 224 | 225 | matrix = la.mat_orthographic(left, right, top, bottom, near, far) 226 | hp.assume(not (np.any(np.isinf(matrix) | np.isnan(matrix)))) 227 | 228 | try: 229 | np.linalg.inv(matrix) 230 | except np.linalg.LinAlgError: 231 | # only stable/invertible matrices 232 | hp.assume(False) 233 | 234 | return matrix 235 | 236 | 237 | def nonzero_scale(scale): 238 | return np.where(np.abs(scale) < EPS, 1, scale) 239 | 240 | 241 | # Hypthesis testing strategies 242 | # Note: components where abs(x[i]) > 1e150 can cause overflow (inf) when 243 | # squared, which affects kernels using np.linalg.norm 244 | legal_numbers = from_dtype( 245 | np.dtype(float), 246 | allow_infinity=False, 247 | allow_nan=False, 248 | min_value=-1e150, 249 | max_value=1e150, 250 | ) 251 | legal_positive_number = from_dtype( 252 | np.dtype(float), 253 | allow_infinity=False, 254 | allow_nan=False, 255 | min_value=0, 256 | max_value=1e150, 257 | ) 258 | legal_angle = from_dtype( 259 | np.dtype(float), 260 | allow_infinity=False, 261 | allow_nan=False, 262 | min_value=0, 263 | max_value=2 * np.pi, 264 | ) 265 | test_vector = arrays(float, (3,), elements=legal_numbers) 266 | test_quaternion = generate_quaternion() 267 | test_matrix_affine = arrays(float, (4, 4), elements=legal_numbers) 268 | test_scaling = arrays(float, (3,), elements=legal_numbers).map(nonzero_scale) 269 | test_dtype = dtype_string() 270 | test_angles_rad = arrays(float, (3,), elements=legal_angle) 271 | test_spherical = generate_spherical_vector() 272 | test_unit_vector = unit_vector() 273 | test_projection = perspecitve_matrix() | orthographic_matrix() 274 | -------------------------------------------------------------------------------- /pylinalg/quaternion.py: -------------------------------------------------------------------------------- 1 | """Note that we assume unit quaternions for faster implementations""" 2 | 3 | import numpy as np 4 | from numpy.lib.stride_tricks import as_strided 5 | 6 | 7 | def mat_from_quat(quaternion, /, *, out=None, dtype=None) -> np.ndarray: 8 | """ 9 | Make a rotation matrix given a quaternion. 10 | 11 | Parameters 12 | ---------- 13 | quaternion : ndarray, [4] 14 | Quaternion. 15 | out : ndarray, optional 16 | A location into which the result is stored. If provided, it 17 | must have a shape that the inputs broadcast to. If not provided or 18 | None, a freshly-allocated array is returned. A tuple must have 19 | length equal to the number of outputs. 20 | dtype : data-type, optional 21 | Overrides the data type of the result. 22 | 23 | Returns 24 | ------- 25 | ndarray, [4, 4] 26 | rotation matrix. 27 | """ 28 | quaternion = np.asarray(quaternion) 29 | result_shape = (*quaternion.shape[:-1], 4, 4) 30 | 31 | if out is None: 32 | out = np.empty(result_shape, dtype=dtype) 33 | 34 | # view into the diagonal of the result 35 | n_matrices = np.prod(result_shape[:-2], dtype=int) 36 | itemsize = out.itemsize 37 | diagonal = as_strided( 38 | out, shape=(n_matrices, 4), strides=(16 * itemsize, 5 * itemsize) 39 | ) 40 | 41 | out[:] = 0 42 | diagonal[:] = 1 43 | 44 | x, y, z, w = ( 45 | quaternion[..., 0], 46 | quaternion[..., 1], 47 | quaternion[..., 2], 48 | quaternion[..., 3], 49 | ) 50 | 51 | x2 = x * 2 52 | y2 = y * 2 53 | z2 = z * 2 54 | xx = x * x2 55 | xy = x * y2 56 | xz = x * z2 57 | yy = y * y2 58 | yz = y * z2 59 | zz = z * z2 60 | wx = w * x2 61 | wy = w * y2 62 | wz = w * z2 63 | 64 | out[..., 0, 0] = 1 - (yy + zz) 65 | out[..., 1, 0] = xy + wz 66 | out[..., 2, 0] = xz - wy 67 | out[..., 0, 1] = xy - wz 68 | out[..., 1, 1] = 1 - (xx + zz) 69 | out[..., 2, 1] = yz + wx 70 | out[..., 0, 2] = xz + wy 71 | out[..., 1, 2] = yz - wx 72 | out[..., 2, 2] = 1 - (xx + yy) 73 | 74 | return out 75 | 76 | 77 | def quat_mul(a, b, /, *, out=None, dtype=None) -> np.ndarray: 78 | """ 79 | Multiply two quaternions 80 | 81 | Parameters 82 | ---------- 83 | a : ndarray, [4] 84 | Left-hand quaternion 85 | b : ndarray, [4] 86 | Right-hand quaternion 87 | out : ndarray, optional 88 | A location into which the result is stored. If provided, it 89 | must have a shape that the inputs broadcast to. If not provided or 90 | None, a freshly-allocated array is returned. A tuple must have 91 | length equal to the number of outputs. 92 | dtype : data-type, optional 93 | Overrides the data type of the result. 94 | 95 | Returns 96 | ------- 97 | ndarray, [4] 98 | Quaternion. 99 | """ 100 | a = np.asarray(a) 101 | b = np.asarray(b) 102 | 103 | if out is None: 104 | out = np.empty(4, dtype=dtype) 105 | 106 | xyz = a[3] * b[:3] + b[3] * a[:3] + np.cross(a[:3], b[:3]) 107 | w = a[3] * b[3] - a[:3].dot(b[:3]) 108 | 109 | out[:3] = xyz 110 | out[3] = w 111 | 112 | return out 113 | 114 | 115 | def quat_from_vecs(source, target, /, *, out=None, dtype=None) -> np.ndarray: 116 | """Rotate one vector onto another. 117 | 118 | Create a quaternion that rotates ``source`` onto ``target``. 119 | 120 | Parameters 121 | ---------- 122 | source : ndarray, [3] 123 | The vector that should be rotated. 124 | target : ndarray, [3] 125 | The vector that will be rotated onto. 126 | out : ndarray, optional 127 | A location into which the result is stored. If provided, it 128 | must have a shape that the inputs broadcast to. If not provided or 129 | None, a freshly-allocated array is returned. A tuple must have 130 | length equal to the number of outputs. 131 | dtype : data-type, optional 132 | Overrides the data type of the result. 133 | 134 | Returns 135 | ------- 136 | ndarray, [4] 137 | Quaternion. 138 | 139 | Notes 140 | ----- 141 | Among all the possible rotations that send ``source`` onto ``target`` this 142 | function always chooses the right-hand rotation around the origin. In cases 143 | where more than one right-hand rotation around the origin exists (``source`` 144 | and ``target`` are parallel), an arbitrary one is returned. 145 | 146 | While this function is intended to be used with unit vectors, it also works 147 | on non-unit vectors in which case the returned rotation will point 148 | ``source`` in the direction of ``target``. 149 | 150 | """ 151 | 152 | source = np.asarray(source, dtype=float) 153 | target = np.asarray(target, dtype=float) 154 | 155 | if out is None: 156 | result_shape = np.broadcast_shapes(source.shape, target.shape)[:-1] 157 | out = np.empty((*result_shape, 4), dtype=dtype) 158 | 159 | axis = np.cross(source, target) 160 | angle = np.arctan2(np.linalg.norm(axis), np.dot(source, target)) 161 | 162 | # if source and target are parallel, axis will be 0. In this case, we 163 | # need to choose a replacement axis, which is any vector that is orthogonal 164 | # to source (and/or target). 165 | use_fallback = np.linalg.norm(axis, axis=-1) == 0 166 | if np.any(use_fallback): 167 | fallback = np.empty((*use_fallback.shape, 3), dtype=float) 168 | fallback = np.atleast_2d(fallback) 169 | 170 | template = source[use_fallback] 171 | y_zero = template[..., 1] == 0 172 | z_zero = template[..., 2] == 0 173 | both_nonzero = ~(y_zero | z_zero) 174 | 175 | # if any axis is zero, we can use that axis 176 | fallback[y_zero, :] = (0, 1, 0) 177 | fallback[z_zero, :] = (0, 0, 1) 178 | 179 | # if two axes are non-zero we can use those 180 | if np.any(both_nonzero): 181 | fallback[both_nonzero, :] = (0, -1, 1) * template[both_nonzero, [0, 2, 1]] 182 | 183 | axis[use_fallback] = np.squeeze(fallback) 184 | 185 | return quat_from_axis_angle(axis, angle, out=out) 186 | 187 | 188 | def quat_inv(quaternion, /, *, out=None, dtype=None) -> np.ndarray: 189 | """ 190 | Inverse of a given quaternion 191 | 192 | Parameters 193 | ---------- 194 | a : ndarray, [3] 195 | First unit vector 196 | out : ndarray, optional 197 | A location into which the result is stored. If provided, it 198 | must have a shape that the inputs broadcast to. If not provided or 199 | None, a freshly-allocated array is returned. A tuple must have 200 | length equal to the number of outputs. 201 | dtype : data-type, optional 202 | Overrides the data type of the result. 203 | 204 | Returns 205 | ------- 206 | ndarray, [4] 207 | Quaternion. 208 | """ 209 | quaternion = np.asarray(quaternion) 210 | 211 | if out is None: 212 | out = np.empty_like(quaternion, dtype=dtype) 213 | 214 | out[:] = quaternion 215 | out[..., :3] *= -1 216 | 217 | return out 218 | 219 | 220 | def quat_from_axis_angle(axis, angle, /, *, out=None, dtype=None) -> np.ndarray: 221 | """Quaternion from axis-angle pair. 222 | 223 | Create a quaternion representing the rotation of an given angle 224 | about a given unit vector 225 | 226 | Parameters 227 | ---------- 228 | axis : ndarray, [3] 229 | Unit vector 230 | angle : number 231 | The angle (in radians) to rotate about axis 232 | out : ndarray, optional 233 | A location into which the result is stored. If provided, it 234 | must have a shape that the inputs broadcast to. If not provided or 235 | None, a freshly-allocated array is returned. A tuple must have 236 | length equal to the number of outputs. 237 | dtype : data-type, optional 238 | Overrides the data type of the result. 239 | 240 | Returns 241 | ------- 242 | ndarray, [4] 243 | Quaternion. 244 | """ 245 | 246 | axis = np.asarray(axis, dtype=float) 247 | angle = np.asarray(angle, dtype=float) 248 | 249 | if out is None: 250 | out_shape = np.broadcast_shapes(axis.shape[:-1], angle.shape) 251 | out = np.empty((*out_shape, 4), dtype=dtype) 252 | 253 | # result should be independent of the length of the given axis 254 | lengths_shape = (*axis.shape[:-1], 1) 255 | axis = axis / np.linalg.norm(axis, axis=-1).reshape(lengths_shape) 256 | 257 | out[..., :3] = axis * np.sin(angle / 2).reshape(lengths_shape) 258 | out[..., 3] = np.cos(angle / 2) 259 | 260 | return out 261 | 262 | 263 | def quat_from_euler(angles, /, *, order="xyz", out=None, dtype=None) -> np.ndarray: 264 | """ 265 | Create a quaternion from euler angles. 266 | 267 | Parameters 268 | ---------- 269 | angles : ndarray, [3] 270 | A set of XYZ euler angles. 271 | order : string, optional 272 | The rotation order as a string. Can include 'X', 'Y', 'Z' for intrinsic 273 | rotation (uppercase) or 'x', 'y', 'z' for extrinsic rotation (lowercase). 274 | Default is "xyz". 275 | out : ndarray, optional 276 | A location into which the result is stored. If provided, it 277 | must have a shape that the inputs broadcast to. If not provided or 278 | None, a freshly-allocated array is returned. A tuple must have 279 | length equal to the number of outputs. 280 | dtype : data-type, optional 281 | Overrides the data type of the result. 282 | 283 | Returns 284 | ------- 285 | quaternion : ndarray, [4] 286 | The rotation expressed as a quaternion. 287 | 288 | """ 289 | 290 | angles = np.asarray(angles, dtype=float) 291 | batch_shape = angles.shape[:-1] if len(order) > 1 else angles.shape 292 | 293 | if out is None: 294 | out = np.empty((*batch_shape, 4), dtype=dtype) 295 | 296 | # work out the sequence in which to apply the rotations 297 | is_extrinsic = [x.islower() for x in order] 298 | basis_index = {"x": 0, "y": 1, "z": 2} 299 | order = [basis_index[x] for x in order.lower()] 300 | 301 | # convert each euler matrix into a quaternion 302 | quaternions = np.zeros((len(order), *batch_shape, 4), dtype=float) 303 | quaternions[:, ..., -1] = np.cos(angles / 2) 304 | quaternions[np.arange(len(order)), ..., order] = np.sin(angles / 2) 305 | 306 | # multiple euler-angle quaternions respecting 307 | out[:] = quaternions[0] 308 | for idx in range(1, len(quaternions)): 309 | if is_extrinsic[idx]: 310 | quat_mul(quaternions[idx], out, out=out) 311 | else: 312 | quat_mul(out, quaternions[idx], out=out) 313 | 314 | return out 315 | 316 | 317 | __all__ = [ 318 | name for name in globals() if name.startswith(("vec_", "mat_", "quat_", "aabb_")) 319 | ] 320 | -------------------------------------------------------------------------------- /CONVENTIONS.md: -------------------------------------------------------------------------------- 1 | # Style conventions 2 | 3 | ## Docstrings 4 | 5 | Docstrings shall be written in NumpyDoc format. 6 | 7 | At least the following sections shall be provided: 8 | 9 | * the short summary (one sentence at the top) 10 | * the parameters section (if the function has input arguments) 11 | * the returns section (if the function returns anything other than `None`) 12 | 13 | ## Type annotations 14 | 15 | Until Numpy version 1.22+ becomes generally adopted this library will not 16 | provide type annotations. 17 | 18 | ## Linting 19 | 20 | Linting shall be performed with flake8, flake8-isort, flake8-black and 21 | pep8-naming. 22 | 23 | The default configurations for these linting tools shall be upheld, which 24 | includes pep8 style and naming conventions. 25 | 26 | Automated formatting shall be performed with black and isort. 27 | 28 | Black is left at defaults, flake8 and isort are configured to adhere to black. 29 | 30 | ## Testing 31 | 32 | All functions need to be covered by unit tests. 33 | 34 | # Coordinate frame conventions 35 | 36 | The primary purpose of pylinalg as a library is to support linear algebra 37 | operations in pygfx and applications based on pygfx. As such, pylinalg shares 38 | its coordinate frame conventions with pygfx, which in turn follows [gITF's 39 | conventions 40 | ](https://registry.khronos.org/glTF/specs/2.0/glTF-2.0.html#coordinate-system-and-units). 41 | 42 | ## Coordinate Systems 43 | 44 | Unless stated otherwise, frames use 3-dimensional euclidean coordinates and a 45 | right-handed coordinate frame. What differs depending on the type are the 46 | semantics used to describe each axis. Distances are measured in `m` (meters) and 47 | and angles in `rad` (radians) unless stated otherwise. 48 | 49 | * **Object Coordinates** are represented using `(x, y, z)` vectors and are used 50 | to describe objects in a scene. They use the following semantics: 51 | * The *negative* X axis is called *right*. 52 | * The positive Y axis is called *up*. 53 | * The positive Z axis is called *forward*. 54 | * **Camera Coordinates** are represented using `(x, y, z)` vectors and are used 55 | to describe cameras and lights. They use the following semantics: 56 | * The positive X axis is called *right*. 57 | * The positive Y axis is called *up*. 58 | * The *negative* Z axis is called *forward*. 59 | * **Normalized Device Coordinates (NDC)** are left-handed and represented using 60 | `(x, y, z)` vectors. They are used to describe points to render/plot. Points 61 | inside the unit (half) box are rendered, others are not. NDC uses the 62 | following semantics: 63 | * The positive X axis is called *right* and the box extent is `[-1, 1]`. 64 | * The positive Y axis is called *up* and the box extent is `[-1, 1]`. 65 | * The positive Z axis is the viewing direction and the box extent is `[0, 1]`. 66 | * **Spherical Coordinates** are represented using `(radius, theta, phi)` vectors 67 | and use the following semantics: 68 | * The *radius* measures the distance between a point and the origin and lies 69 | between `[0, inf)`. 70 | * The *phi* angle measures the counter-clockwise (CCW) rotation around the 71 | positive y-axis. It is measured from the positive Z-axis and lies between 72 | `[0, pi)`. 73 | * The *theta* angle measures the counter-clockwise (CCW) rotation around the 74 | negative X-axis. It is measured from the positive Y-axis and lies between 75 | `[0, 2*pi)`. 76 | * **Homogeneous Coordinates** are represented using `(x, y, z, 1)` vectors and 77 | use the same semantics as their cartesian dual, i.e., if they represent an 78 | object, they use the semantics of object coordinates and when they represent a 79 | camera they use the semantics of camera coordinates. 80 | * **Quaternion Coordinates** are represented using `(x, y, z, w)` vectors and 81 | have no explicit semantics. 82 | 83 | ## Named Coordinate Frames 84 | 85 | We identify three important coordinate frames that we use as named references: 86 | 87 | 1. **world**: The world frame is the (global) inertial reference frame. All 88 | other coordinate frames are positioned relative to *world*; either 89 | explicitly, or implicitly via a sequence of frames that were previously 90 | placed relative to *world*. This creates a graph of coordinate frames 91 | (called the scene graph), with *world* at its root and it allows finding a 92 | transformation matrix between pairs of coordinate frames. *World* uses 93 | object coordinates. 94 | 2. **local**: The local frame is the coordinate frame in which an object's 95 | vertices are expressed. For example, when inspecting the position of a 96 | cube's corners their numerical values are given in the cube's *local frame*. 97 | 3. **parent**: The parent frame is the coordinate frame in which an object's 98 | pose (position + orientation) is expressed. *Parent* can either be the 99 | inertial reference frame (world) or it can be another object's local frame. 100 | An object's position is always relative to its parent; for example, if a 101 | lightbulb has a lamp's local frame as it's parent, then the lightbulb's 102 | position expressed in world coordinates will change whenever the position of 103 | lamp changes, i.e., if the lamp moves, so does the lightbulb. 104 | 105 | Further, when talking about transformation matrices and coordinate transforms, 106 | we use two additional frames to avoid confusion: 107 | 108 | * **source**: The source frame is the coordinate frame in which 109 | to-be-transformed vectors are expressed, i.e., it is the reference frame of 110 | the input vectors. *Source* uses homogeneous coordinates. 111 | * **target**: The target frame is the coordinate frame in which transformed 112 | vectors are expressed, i.e., it is the reference frame of the output vectors. 113 | *Target* uses homogeneous coordinates. 114 | 115 | ## Example using the named frames 116 | 117 | To make this concrete, imagine a scene with a space shuttle that is about to 118 | lift off. The *world* frame is a frame that is anchored to the surface of the 119 | planet, the *local* frame is a frame attached to the space shuttle, and the 120 | space shuttle's *parent* frame is the *local* frame of the rocket to which the 121 | shuttle is attached to. 122 | 123 | In the above example, gravity points along the negative y-axis in *world* (Y is 124 | up) and along the positive z-axis in *local* (-Z is forward). During take-off, 125 | the rocket will generate thrust and move in the direction of *parent*'s negative 126 | Z (forward). From the perspective of *world*, however, the rocket launches in 127 | the direction of the positive y-axis (up, as it should). The space shuttle, 128 | being attached to the rocket, will be dragged along for the ride. It's position 129 | relative to the rocket doesn't change; however, from the perspective of *world* 130 | it, too, will accelerate along the positive y-axis. 131 | 132 | ## Rendering 133 | 134 | To render an object we have to express its vertices in a camera's NDC. Recall 135 | that vertices are expressed in the object's *local* frame, which means we need 136 | to work out the transformation from object *local* to camera NDC. We can do this 137 | by following the chain of transformations in the scene graph from object *local* 138 | via *world* to camera *local* and from camera *local* into NDC. Since this chain 139 | is very important for rendering, it's parts have special names: 140 | 141 | * **World Matrix**: The transformation from object's *local* into *world*. 142 | * **View Matrix**: The transformation from *world* to camera's *local*. This is 143 | the inverse of the camera's world matrix. 144 | * **Projection Matrix**: The transformation from camera's *local* into NDC. 145 | 146 | # Memory layout conventions 147 | 148 | Row-major can mean two things: 149 | 150 | * Memory layout; are rows or columns contiguous in memory 151 | * Are vectors columns or rows 152 | 153 | Pylinalg's kernels are written assuming a row-major (C-contigous) layout. If a 154 | kernel supports batch processing of vetors, it assumes that the last dimension 155 | contains the relevant vector data and that all other dimensions are batch/loop 156 | dimensions. As such, you can think of vectors being row vectors. 157 | 158 | # API conventions 159 | 160 | This API is for power-users that want to 161 | vectorize operations on large sets of things. 162 | 163 | Performance is prioritized over extensive input validation. 164 | 165 | The source for this API resides in the `pylinalg` package and is 166 | organized by type. 167 | 168 | ## Function naming 169 | 170 | Since all functions are exposed on the root `pylinalg` module object, 171 | a simple naming scheme is put in place: 172 | 173 | * Functions are organized by type. For example, functions that work on 174 | matrices, or create matrices, go into the `pylinalg/matrix.py` module 175 | and their function names are prefixed by `mat_`. 176 | 177 | ## Function signatures 178 | 179 | We strive to align closely with numpy conventions in order to be 180 | least-surprising for users accustomed to numpy. 181 | 182 | * Data arguments feeding into computation are positional-only. 183 | * Optional arguments affecting the result of computation are keyword-only. 184 | * The `dtype` and `out` arguments are available whenever possible, and they 185 | work as they do in numpy: 186 | * The `out` argument can be provided to write the results to an existing array, 187 | instead of a new array. 188 | * The `dtype` argument can be provided to override the data type of the result. 189 | * If there are multiple outputs, `out` is expected to be a tuple 190 | with matching number of elements. 191 | * If `out` and `dtype` are both provided, the `dtype` argument is ignored. 192 | 193 | Here is an example of a function that complies with the conventions posed in 194 | this document: 195 | 196 | ```python 197 | def vec_transform(vectors, matrix, /, *, w=1, out=None, dtype=None): 198 | """ 199 | Transform vectors by a transformation matrix. 200 | 201 | Parameters 202 | ---------- 203 | vectors : ndarray, [..., 3] 204 | Array of vectors 205 | matrix : ndarray, [4, 4] 206 | Transformation matrix 207 | w : number, optional, default 1 208 | The value for the homogeneous dimensionality. 209 | this affects the result of translation transforms. use 0 (vectors) 210 | if the translation component should not be applied, 1 (positions) 211 | otherwise. 212 | out : ndarray, optional 213 | A location into which the result is stored. If provided, it 214 | must have a shape that the inputs broadcast to. If not provided or 215 | None, a freshly-allocated array is returned. A tuple must have 216 | length equal to the number of outputs. 217 | dtype : data-type, optional 218 | Overrides the data type of the result. 219 | 220 | Returns 221 | ------- 222 | ndarray, [..., 3] 223 | transformed vectors 224 | """ 225 | vectors = vec_homogeneous(vectors, w=w) 226 | # usually when applying a transformation matrix to a vector 227 | # the vector is a column, so if you were to have an array of vectors 228 | # it would have shape (ndim, nvectors). 229 | # however, we instead have the convention (nvectors, ndim) where 230 | # vectors are rows. 231 | # therefore it is necessary to transpose the transformation matrix 232 | # additionally we slice off the last row of the matrix, since we are not interested 233 | # in the resulting w coordinate 234 | transform = matrix[:-1, :].T 235 | if out is not None: 236 | try: 237 | # if `out` is exactly compatible, that is the most performant 238 | return np.dot(vectors, transform, out=out) 239 | except ValueError: 240 | # otherwise we need a temporary array and cast 241 | out[:] = np.dot(vectors, transform) 242 | return out 243 | # otherwise just return whatever dot computes 244 | out = np.dot(vectors, transform) 245 | # cast if requested 246 | if dtype is not None: 247 | out = out.astype(dtype, copy=False) 248 | return out 249 | ``` 250 | 251 | ## Note on linear algebra operations already provided by numpy 252 | 253 | Since the conventions align with those of numpy, in some cases, it just does not 254 | make sense to add the function this library and incur all the overhead of 255 | maintenance, documentation and testing. For example, a function to perform 256 | vector addition would be exactly equal to the `np.add` function, and as such, it 257 | is not necessary to add them to pylinalg. 258 | -------------------------------------------------------------------------------- /tests/test_matrix.py: -------------------------------------------------------------------------------- 1 | import hypothesis.strategies as st 2 | import numpy as np 3 | import numpy.testing as npt 4 | import pytest 5 | from hypothesis import assume, example, given 6 | 7 | import pylinalg as la 8 | 9 | from . import conftest as ct 10 | 11 | 12 | @given(ct.legal_numbers | ct.test_vector, st.none() | ct.test_dtype) 13 | def test_mat_from_translation(position, dtype): 14 | result = la.mat_from_translation(position, dtype=dtype) 15 | 16 | expected = np.eye(4, dtype=dtype) 17 | expected[:3, 3] = np.asarray(position) 18 | 19 | npt.assert_array_almost_equal(result, expected) 20 | 21 | 22 | @given(ct.legal_numbers | ct.test_scaling, st.none() | ct.test_dtype) 23 | def test_mat_from_scale(scale, dtype): 24 | result = la.mat_from_scale(scale, dtype=dtype) 25 | 26 | scaling = np.ones(4, dtype=dtype) 27 | scaling[:3] = np.asarray(scale, dtype=dtype) 28 | 29 | expected = np.identity(4, dtype=dtype) 30 | np.fill_diagonal(expected, scaling) 31 | 32 | npt.assert_array_almost_equal(result, expected) 33 | assert result.dtype == dtype 34 | 35 | 36 | @given(ct.test_angles_rad, st.permutations("xyz"), ct.test_dtype) 37 | @example((np.pi, -np.pi / 2, 0), "zyx", "f8") 38 | @example((0, np.pi / 2, 0), "xyz", "f8") 39 | def test_mat_from_euler(angles, order, dtype): 40 | result = la.mat_from_euler(angles, order="".join(order), dtype=dtype) 41 | 42 | expected = np.eye(4, dtype=dtype) 43 | for axis, angle in zip(order, angles): 44 | matrix = np.eye(4, dtype=dtype) 45 | matrix[:3, :3] = ct.rotation_matrix(axis, angle) 46 | expected = matrix @ expected 47 | 48 | npt.assert_array_almost_equal(result, expected) 49 | assert result.dtype == dtype 50 | 51 | 52 | def test_mat_from_axis_angle_direction(): 53 | """Test that a positive pi/2 rotation about the z-axis results 54 | in counter clockwise rotation, in accordance with the unit circle.""" 55 | result = la.mat_from_axis_angle([0, 0, 1], np.pi / 2) 56 | npt.assert_array_almost_equal( 57 | result, 58 | [ 59 | [0, -1, 0, 0], 60 | [1, 0, 0, 0], 61 | [0, 0, 1, 0], 62 | [0, 0, 0, 1], 63 | ], 64 | ) 65 | 66 | 67 | def test_mat_from_axis_angle_xy(): 68 | """Test that a negative pi rotation about the diagonal of the x-y plane 69 | flips the x and y coordinates, and negates the z coordinate.""" 70 | axis = np.array([1, 1, 0], dtype="f8") 71 | axis /= np.linalg.norm(axis) 72 | result = la.mat_from_axis_angle(axis, -np.pi) 73 | npt.assert_array_almost_equal( 74 | result, 75 | [ 76 | [0, 1, 0, 0], 77 | [1, 0, 0, 0], 78 | [0, 0, -1, 0], 79 | [0, 0, 0, 1], 80 | ], 81 | ) 82 | 83 | 84 | @pytest.mark.parametrize( 85 | "angles,expected,dtype", 86 | [ 87 | # case a 88 | ([0, 0, np.pi / 2], [0, 0, np.sqrt(2) / 2, np.sqrt(2) / 2], "f8"), 89 | # case a, two ordered rotations 90 | ([0, -np.pi / 2, np.pi / 2], [0.5, -0.5, 0.5, 0.5], "f8"), 91 | # non-default dtype 92 | ([0, -np.pi / 2, np.pi / 2], [0.5, -0.5, 0.5, 0.5], "f4"), 93 | # case b (contrived example for code coverage) 94 | ( 95 | [0, np.pi * 0.51, np.pi * 0.51], 96 | [0.515705, -0.499753, -0.499753, -0.484295], 97 | "f8", 98 | ), 99 | # case c (contrived example for code coverage) 100 | ( 101 | [np.pi * 1.2, np.pi * 1.8, np.pi], 102 | [-0.095492, 0.904508, -0.293893, -0.293893], 103 | "f8", 104 | ), 105 | # case d (contrived example for code coverage) 106 | ( 107 | [np.pi * 0.45, np.pi * 1.8, np.pi], 108 | [0.234978, 0.617662, 0.723189, -0.20069], 109 | "f8", 110 | ), 111 | ], 112 | ) 113 | def test_quat_from_mat(angles, expected, dtype): 114 | matrix = la.mat_from_euler(angles, dtype=dtype) 115 | quaternion = la.quat_from_mat(matrix, dtype=dtype) 116 | npt.assert_array_almost_equal( 117 | quaternion, 118 | expected, 119 | ) 120 | assert matrix.dtype == dtype 121 | assert quaternion.dtype == dtype 122 | 123 | 124 | def test_mat_combine(): 125 | """Test that the matrices are combined in the expected order.""" 126 | # non-uniform scaling such that the test would fail if rotation/scaling are 127 | # applied in the incorrect order 128 | scaling = la.mat_from_scale([1, 2, 1]) 129 | rotation = la.mat_from_euler([0, np.pi / 4, np.pi / 2]) 130 | translation = la.mat_from_translation(2) 131 | # apply the standard SRT ordering 132 | result = la.mat_combine([translation, rotation, scaling]) 133 | # therefore translation should be unaffected in the combined matrix 134 | npt.assert_array_almost_equal( 135 | result, 136 | [ 137 | [0, -2, 0, 2], 138 | [np.sqrt(2) / 2, 0, np.sqrt(2) / 2, 2], 139 | [-np.sqrt(2) / 2, 0, np.sqrt(2) / 2, 2], 140 | [0, 0, 0, 1], 141 | ], 142 | ) 143 | 144 | with pytest.raises(TypeError): 145 | la.mat_combine() 146 | 147 | result = la.mat_combine([translation, translation], dtype="f4") 148 | npt.assert_array_almost_equal( 149 | result, 150 | [ 151 | [1, 0, 0, 4], 152 | [0, 1, 0, 4], 153 | [0, 0, 1, 4], 154 | [0, 0, 0, 1], 155 | ], 156 | ) 157 | assert result.dtype == "f4" 158 | 159 | 160 | def test_mat_compose(): 161 | """Test that the matrices are composed correctly in SRT order.""" 162 | # non-uniform scaling such that the test would fail if rotation/scaling are 163 | # applied in the incorrect order 164 | scaling = [1, 2, 1] 165 | # quaternion corresponding to 90 degree rotation about z-axis 166 | rotation = [0, 0, np.sqrt(2) / 2, np.sqrt(2) / 2] 167 | translation = [2, 2, 2] 168 | # compose the transform 169 | result = la.mat_compose(translation, rotation, scaling) 170 | npt.assert_array_almost_equal( 171 | result, 172 | [ 173 | [0, -2, 0, 2], 174 | [1, 0, 0, 2], 175 | [0, 0, 1, 2], 176 | [0, 0, 0, 1], 177 | ], 178 | ) 179 | 180 | 181 | def test_mat_decompose(): 182 | """Test that the matrices are decomposed correctly.""" 183 | matrix = [ 184 | [0, -2, 0, 2], 185 | [1, 0, 0, 2], 186 | [0, 0, 1, 2], 187 | [0, 0, 0, 1], 188 | ] 189 | translation, rotation, scaling = la.mat_decompose(matrix) 190 | npt.assert_array_equal(translation, [2, 2, 2]) 191 | npt.assert_array_equal(scaling, [1, 2, 1]) 192 | npt.assert_array_almost_equal(rotation, [0, 0, np.sqrt(2) / 2, np.sqrt(2) / 2]) 193 | 194 | 195 | def test_mat_decompose_scaling_0(): 196 | """Test that the matrices are decomposed correctly when scaling is 0.""" 197 | 198 | scaling = [0, 0, 2] 199 | rotation = [0, 0, np.sqrt(2) / 2, np.sqrt(2) / 2] 200 | translation = [2, 2, 2] 201 | 202 | matrix = la.mat_compose(translation, rotation, scaling) 203 | translation_, rotation_, scaling_ = la.mat_decompose(matrix) 204 | 205 | npt.assert_array_almost_equal(translation_, translation) 206 | npt.assert_array_almost_equal(scaling_, scaling) 207 | # rotation is not uniquely defined when scaling is 0, but it should not be NaN 208 | assert not np.isnan(rotation_).any() 209 | 210 | 211 | @pytest.mark.parametrize( 212 | "signs", 213 | [ 214 | # enumerate all combinations of signs 215 | [1, 1, 1], 216 | [-1, 1, 1], 217 | [1, -1, 1], 218 | [-1, -1, 1], 219 | [1, 1, -1], 220 | [-1, 1, -1], 221 | [1, -1, -1], 222 | [-1, -1, -1], 223 | ], 224 | ids=str, 225 | ) 226 | def test_mat_compose_roundtrip(signs): 227 | """Test that transform components survive a matrix 228 | compose -> decompose roundtrip.""" 229 | scaling = np.array([1, 2, 3]) * signs 230 | rotation = [0, 0, np.sqrt(2) / 2, np.sqrt(2) / 2] 231 | translation = [-100, -6, 5] 232 | matrix = la.mat_compose(translation, rotation, scaling) 233 | 234 | # decompose cannot reconstruct original scaling 235 | # so this is expected to fail 236 | translation2, rotation2, scaling2 = la.mat_decompose(matrix) 237 | npt.assert_array_equal(translation, translation2) 238 | if signs in ([1, 1, 1], [-1, 1, 1]): 239 | # if there are no flips, or if the flip happens to be the first axis 240 | # then we can correctly reconstruct the scaling without 241 | # prior knowledge 242 | npt.assert_array_almost_equal(scaling, scaling2) 243 | npt.assert_array_almost_equal(rotation, rotation2) 244 | else: 245 | with pytest.raises(AssertionError): 246 | npt.assert_array_almost_equal(scaling, scaling2) 247 | with pytest.raises(AssertionError): 248 | npt.assert_array_almost_equal(rotation, rotation2) 249 | 250 | # now inform decompose of the original scaling 251 | translation3, rotation3, scaling3 = la.mat_decompose( 252 | matrix, scaling_signs=np.sign(scaling) 253 | ) 254 | npt.assert_array_equal(translation, translation3) 255 | npt.assert_array_almost_equal(scaling, scaling3) 256 | npt.assert_array_almost_equal(rotation, rotation3) 257 | 258 | 259 | def test_mat_compose_validation(): 260 | """Test that decompose validates consistency of scaling signs.""" 261 | signs = [-1, -1, -1] 262 | scaling = np.array([1, 2, -3]) * signs 263 | rotation = [0, 0, np.sqrt(2) / 2, np.sqrt(2) / 2] 264 | translation = [-100, -6, 5] 265 | matrix = la.mat_compose(translation, rotation, scaling) 266 | 267 | with pytest.raises(ValueError): 268 | la.mat_decompose(matrix, scaling_signs=signs) 269 | 270 | 271 | def naive_mat_compose(translation, rotation, scaling, /, *, out=None, dtype=None): 272 | return la.mat_combine( 273 | [ 274 | la.mat_from_translation(translation), 275 | la.mat_from_quat(rotation), 276 | la.mat_from_scale(scaling), 277 | ], 278 | out=out, 279 | dtype=dtype, 280 | ) 281 | 282 | 283 | def test_mat_compose_naive(): 284 | """Compare the direct composition with the naive composition.""" 285 | npt.assert_equal( 286 | la.mat_compose([1, 2, 3], [np.pi, np.pi / 4, 0, 1], [1, -2, 9]), 287 | naive_mat_compose([1, 2, 3], [np.pi, np.pi / 4, 0, 1], [1, -2, 9]), 288 | ) 289 | 290 | 291 | def test_mat_compose_scalar_scaling(): 292 | """Check that a scaler scaling argument is supported in mat_compose.""" 293 | npt.assert_equal( 294 | la.mat_compose([1, 2, 3], [np.pi, np.pi / 4, 0, 1], [1.25, 1.25, 1.25]), 295 | la.mat_compose([1, 2, 3], [np.pi, np.pi / 4, 0, 1], 1.25), 296 | ) 297 | 298 | npt.assert_equal( 299 | la.mat_compose([1, 2, 3], [np.pi, np.pi / 4, 0, 1], [1.25, 1.25, 1.25]), 300 | la.mat_compose([1, 2, 3], [np.pi, np.pi / 4, 0, 1], [1.25]), 301 | ) 302 | 303 | 304 | def test_mat_perspective(): 305 | a = la.mat_perspective(-1, 1, -1, 1, 1, 100) 306 | npt.assert_array_almost_equal( 307 | a, 308 | [ 309 | [1, 0, 0, 0], 310 | [0, -1, 0, 0], 311 | [0, 0, -101 / 99, -200 / 99], 312 | [0, 0, -1, 0], 313 | ], 314 | ) 315 | 316 | 317 | def test_mat_orthographic(): 318 | a = la.mat_orthographic(-1, 1, -1, 1, 1, 100) 319 | npt.assert_array_almost_equal( 320 | a, 321 | [ 322 | [1, 0, 0, 0], 323 | [0, -1, 0, 0], 324 | [0, 0, -2 / 99, -101 / 99], 325 | [0, 0, 0, 1], 326 | ], 327 | ) 328 | 329 | 330 | @given(ct.test_unit_vector, ct.test_unit_vector, ct.test_unit_vector) 331 | def test_mat_look_at(eye, target, up_reference): 332 | # Note: to run this test, we need to choose 2 independent vectors (eye, 333 | # target) and one arbitrary vector. Scale doesn't matter, so doing this on 334 | # the unit-sphere will (almost) always succeed. 335 | independence_matrix = np.stack((eye, target), axis=0) 336 | assume(np.linalg.matrix_rank(independence_matrix) == 2) 337 | assume(np.linalg.norm(up_reference - (target - eye)) > 1e-10) 338 | 339 | rotation = la.mat_look_at(eye, target, up_reference) 340 | 341 | inverse_rotation = np.eye(4) 342 | inverse_rotation[:3, :3] = rotation[:3, :3].T 343 | 344 | # ensure matrix is inverted by its transpose 345 | identity = rotation @ inverse_rotation 346 | assert np.allclose(identity, np.eye(4), rtol=1e-10) 347 | 348 | # ensure z_new is along target - eye 349 | target_pointer = target - eye 350 | target_pointer = target_pointer / np.linalg.norm(target_pointer) 351 | target_pointer = la.vec_homogeneous(target_pointer) 352 | result = rotation @ (0, 0, 1, 1) 353 | assert np.allclose(result, target_pointer, rtol=1e-16) 354 | 355 | # ensure y_new, z_new, and up_reference roughly align 356 | # (map up_reference from target to source space and check if it's in the YZ-plane) 357 | new_reference = rotation.T @ la.vec_homogeneous(up_reference) 358 | assert np.abs(new_reference[0]) < 1e-10 359 | 360 | 361 | def test_mat_euler_vs_scipy(): 362 | """Compare our implementation with scipy's.""" 363 | from scipy.spatial.transform import Rotation as R # noqa: N817 364 | 365 | cases = [ 366 | ("XYZ", [np.pi / 2, np.pi / 180, 0]), 367 | ("xyz", [np.pi / 2, np.pi / 180, 0]), 368 | ("ZXY", [np.pi, np.pi / 180, -np.pi / 180]), 369 | ("zxy", [np.pi, np.pi / 180, -np.pi / 180]), 370 | ("ZYX", [0, np.pi / 2, np.pi / 2]), 371 | ("zyx", [0, np.pi / 2, np.pi / 2]), 372 | ] 373 | 374 | for order, angles in cases: 375 | scipy_mat = np.identity(4) 376 | scipy_mat[:3, :3] = R.from_euler(order, angles).as_matrix() 377 | 378 | npt.assert_allclose( 379 | la.mat_from_euler(angles, order=order), 380 | scipy_mat, 381 | atol=1e-15, 382 | ) 383 | 384 | 385 | def test_mat_inverse(): 386 | a = la.mat_from_translation([1, 2, 3]) 387 | np_inv = la.mat_inverse(a, method="numpy") 388 | python_inv = la.mat_inverse(a, method="python") 389 | npt.assert_array_equal(np_inv, python_inv) 390 | 391 | # test for singular matrix 392 | b = la.mat_from_scale([0, 2, 3]) 393 | np_inv = la.mat_inverse(b, method="numpy") 394 | python_inv = la.mat_inverse(b, method="python") 395 | npt.assert_array_equal(np_inv, np.zeros((4, 4))) 396 | npt.assert_array_equal(python_inv, np.zeros((4, 4))) 397 | 398 | 399 | def test_mat_has_shear(): 400 | translate = la.mat_from_translation([1, 2, 3]) 401 | assert not la.mat_has_shear(translate) 402 | 403 | rot_z = la.mat_from_euler([0, 0, 0.5]) 404 | assert not la.mat_has_shear(rot_z) 405 | 406 | scale_y = la.mat_from_scale([0, 1.5, 0]) 407 | assert not la.mat_has_shear(scale_y) 408 | 409 | transform_no_shear = la.mat_combine([rot_z, scale_y]) 410 | assert not la.mat_has_shear(transform_no_shear) 411 | 412 | transform_shear = la.mat_combine([scale_y, rot_z]) 413 | assert la.mat_has_shear(transform_shear) 414 | -------------------------------------------------------------------------------- /tests/test_vectors.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import numpy.testing as npt 3 | import pytest 4 | from hypothesis import HealthCheck, assume, example, given, settings 5 | from hypothesis.strategies import none 6 | 7 | import pylinalg as la 8 | 9 | from . import conftest as ct 10 | 11 | 12 | def test_vec_normalize(): 13 | vectors = [ 14 | [2, 0, 0], 15 | [1, 1, 1], 16 | [-1.0, -1.0, -1.0], 17 | [1.0, 0.0, 0.0], 18 | ] 19 | expected = [ 20 | [1, 0, 0], 21 | [1 / np.sqrt(3), 1 / np.sqrt(3), 1 / np.sqrt(3)], 22 | [-1 / np.sqrt(3), -1 / np.sqrt(3), -1 / np.sqrt(3)], 23 | [1, 0, 0], 24 | ] 25 | 26 | # Test individuals 27 | for vec, e_vec in zip(vectors, expected): 28 | npt.assert_array_almost_equal( 29 | la.vec_normalize(vec), 30 | e_vec, 31 | ) 32 | 33 | # Test the batch 34 | npt.assert_array_almost_equal( 35 | la.vec_normalize(vectors), 36 | expected, 37 | ) 38 | 39 | 40 | ## 41 | @pytest.mark.parametrize( 42 | "vectors,value,expected", 43 | [ 44 | ([1, 1, 1], 1, [1, 1, 1, 1]), 45 | ([1, 1, 1], 0, [1, 1, 1, 0]), 46 | ([[1, 1, 1]], 1, [[1, 1, 1, 1]]), 47 | ([[1, 1, 1], [2, 2, 2]], 1, [[1, 1, 1, 1], [2, 2, 2, 1]]), 48 | ([[1, 1, 1], [2, 2, 2]], 0, [[1, 1, 1, 0], [2, 2, 2, 0]]), 49 | ], 50 | ) 51 | def test_vec_homogeneous(vectors, value, expected): 52 | vectors = np.asarray(vectors) 53 | expected = np.asarray(expected) 54 | result = la.vec_homogeneous(vectors, w=value) 55 | npt.assert_array_equal(result, expected) 56 | 57 | 58 | @given(ct.test_vector) 59 | def test_mat_decompose_translation(translation): 60 | matrix = la.mat_from_translation(translation) 61 | result = la.mat_decompose_translation(matrix) 62 | 63 | npt.assert_equal(result, translation) 64 | 65 | 66 | def test_vector_apply_translation(): 67 | vectors = np.array([[1, 0, 0]]) 68 | expected = np.array([[0, 2, 2]]) 69 | matrix = la.mat_from_translation([-1, 2, 2]) 70 | result = la.vec_transform(vectors, matrix, projection=False) 71 | 72 | npt.assert_array_almost_equal( 73 | result, 74 | expected, 75 | ) 76 | 77 | 78 | @given(ct.test_vector) 79 | def test_vec_spherical_safe(vector): 80 | result = la.vec_spherical_safe(vector) 81 | 82 | assert np.all((0 <= result[..., 1]) & (result[..., 1] < np.pi)) 83 | assert np.all((0 <= result[..., 2]) & (result[..., 2] < 2 * np.pi)) 84 | 85 | 86 | def test_vec_transform_out(): 87 | vectors = np.array([[1, 0, 0]], dtype="f4") 88 | out = np.empty_like(vectors, dtype="i4") 89 | matrix = la.mat_from_translation([-1, 2, 2]) 90 | result = la.vec_transform(vectors, matrix, out=out) 91 | 92 | assert result is out 93 | 94 | 95 | def test_vec_transform_projection_flag(): 96 | vectors = np.array( 97 | [ 98 | [1, 0, 0], 99 | [1, 2, 3], 100 | [1, 1, 1], 101 | [0, 0, 0], 102 | [7, 8, -9], 103 | ], 104 | dtype="f8", 105 | ) 106 | translation = np.array([-1, 2, 2], dtype="f8") 107 | expected = vectors + translation[None, :] 108 | 109 | matrix = la.mat_from_translation(translation) 110 | 111 | for projection in [True, False]: 112 | for batch in [True, False]: 113 | if batch: 114 | vectors_in = vectors 115 | expected_out = expected 116 | else: 117 | vectors_in = vectors[0] 118 | expected_out = expected[0] 119 | result = la.vec_transform(vectors_in, matrix, projection=projection) 120 | npt.assert_array_equal(result, expected_out) 121 | 122 | 123 | def test_vec_transform_ndim(): 124 | vectors_2d = np.array( 125 | [ 126 | [1, 0, 0], 127 | [1, 2, 3], 128 | [1, 1, 1], 129 | [1, 1, -1], 130 | [0, 0, 0], 131 | [7, 8, -9], 132 | ], 133 | dtype="f8", 134 | ) 135 | translation = np.array([-1, 2, 2], dtype="f8") 136 | 137 | vectors_3d = vectors_2d.reshape((3, 2, 3)) 138 | vectors_4d = vectors_2d.reshape((6, 1, 1, 3)) 139 | 140 | expected_3d = vectors_3d + translation[None, None, :] 141 | expected_4d = vectors_4d + translation[None, None, None, :] 142 | 143 | matrix = la.mat_from_translation(translation) 144 | 145 | for projection in [True, False]: 146 | result = la.vec_transform(vectors_3d, matrix, projection=projection) 147 | npt.assert_array_equal(result, expected_3d) 148 | 149 | result = la.vec_transform(vectors_4d, matrix, projection=projection) 150 | npt.assert_array_equal(result, expected_4d) 151 | 152 | 153 | @given(ct.test_spherical, none()) 154 | @example((1, 0, np.pi / 2), (0, 0, 1)) 155 | @example((1, np.pi / 2, np.pi / 2), (1, 0, 0)) 156 | @example((1, 0, 0), (0, 1, 0)) 157 | def test_vec_euclidean_to_spherical(expected, vector): 158 | if vector is None: 159 | assume(abs(expected[0]) > 1e-10) 160 | vector = la.vec_spherical_to_euclidean(expected) 161 | else: 162 | expected = np.asarray(expected) 163 | vector = np.asarray(vector) 164 | 165 | actual = la.vec_euclidean_to_spherical(vector) 166 | 167 | assert np.allclose(actual, expected, rtol=1e-10) 168 | 169 | 170 | def test_vec_transform_out_performant(): 171 | vectors = np.array([[1, 0, 0]], dtype="f4") 172 | out = np.empty_like(vectors, dtype="f8") 173 | matrix = la.mat_from_translation([-1, 2, 2]) 174 | result = la.vec_transform(vectors, matrix, out=out) 175 | 176 | assert result is out 177 | 178 | 179 | def test_vec_transform_dtype(): 180 | vectors = np.array([[1, 0, 0]], dtype="f4") 181 | matrix = la.mat_from_translation([-1, 2, 2]) 182 | result = la.vec_transform(vectors, matrix, dtype="i2") 183 | 184 | assert result.dtype == "i2" 185 | 186 | 187 | @given(ct.test_vector, ct.test_vector) 188 | def test_vec_dist(vector_a, vector_b): 189 | expected = np.linalg.norm(vector_a - vector_b) 190 | result = la.vec_dist(vector_a, vector_b) 191 | 192 | assert np.allclose(result, expected, rtol=1e-10) 193 | 194 | 195 | def test_vec_dist_exceptions(): 196 | tmp = np.array(0) 197 | with pytest.raises(IndexError): 198 | la.vec_dist((0, 0, 0), (0, 1, 0), out=tmp) 199 | 200 | 201 | def test_vec_angle(): 202 | cases = [ 203 | ((0, 0, 1), (0, 1, 0), 90), 204 | ((0, 0, 1), (0, 2, 0), 90), 205 | ((1, 0, 0), (0, 3, 0), 90), 206 | ((2, 0, 0), (3, 0, 0), 0), 207 | ((2, 2, 0), (3, 3, 0), 0), 208 | ((2, 2, 2), (3, 3, 3), 0), 209 | ((1, 0, 0), (-2, 0, 0), 180), 210 | ((0, 1, 2), (0, -3, -6), 180), 211 | ((1, 0, 0), (2, 2, 0), 45), 212 | ((1, 0, 1), (0, 0, 80), 45), 213 | ] 214 | for v1, v2, deg in cases: 215 | rad = deg * np.pi / 180 216 | result = la.vec_angle(v1, v2) 217 | assert np.abs(result - rad) < 0.0001 218 | 219 | v1 = np.array([case[0] for case in cases]) 220 | v2 = np.array([case[1] for case in cases]) 221 | expected = np.array([case[2] for case in cases]) * np.pi / 180 222 | result = la.vec_angle(v1, v2) 223 | 224 | assert np.allclose(result, expected, rtol=1e-8) 225 | 226 | 227 | def test_vec_transform_out_dtype(): 228 | vectors = np.array([[1, 0, 0]], dtype="f4") 229 | matrix = la.mat_from_translation([-1, 2, 2]) 230 | out = np.empty_like(vectors, dtype="i4") 231 | result = la.vec_transform(vectors, matrix, out=out, dtype="i2") 232 | 233 | assert result is out 234 | assert result.dtype == "i4" 235 | 236 | 237 | @given(ct.test_spherical) 238 | def test_vec_spherical_to_euclidean(spherical): 239 | # accuracy of trigonometric ops close to 0, 90, 180, 270, 360 deg dependes a 240 | # lot on the underlying hardware. Let's avoid it here. 241 | angles = spherical[..., [1, 2]] 242 | assume(np.all(np.abs(angles - 0) > 1e-100)) 243 | assume(np.all(np.abs(angles - np.pi / 2) > 1e-100)) 244 | assume(np.all(np.abs(angles - np.pi) > 1e-100)) 245 | assume(np.all(np.abs(angles - 2 * np.pi) > 1e-100)) 246 | 247 | # same for really short vectors (can produce 0) 248 | assume(np.all(np.abs(spherical[0] - 0) > 1e-200)) 249 | 250 | # we can't do a simple round trip test. Instead we ensure that we are 251 | # rotating in the right direction and that the radius/length match 252 | result = la.vec_spherical_to_euclidean(spherical) 253 | 254 | # ensure azimuth rotates CCW 255 | expected_sign = np.where(spherical[1] < np.pi / 2, 1, -1) 256 | actual_sign = np.prod(np.sign(result[..., [0, 2]])) 257 | assert np.all(expected_sign == actual_sign) 258 | 259 | # ensure inclination is measured from positive y 260 | expected_sign = np.where(spherical[2] < np.pi / 2, 1, -1) 261 | expected_sign = np.where(spherical[2] > 3 / 2 * np.pi, 1, expected_sign) 262 | actual_sign = np.sign(result[..., 1]) 263 | assert np.all(expected_sign == actual_sign) 264 | 265 | # ensure length is what we expect 266 | length = np.linalg.norm(result) 267 | assert np.allclose(length, spherical[0], rtol=1e-16, atol=np.inf) 268 | 269 | 270 | def test_vec_spherical_to_euclidean_refs(): 271 | # ensure that the reference axes get transformed as expected 272 | result = la.vec_spherical_to_euclidean((1, 0, 0)) 273 | assert np.allclose(result, (0, 1, 0)) 274 | 275 | result = la.vec_spherical_to_euclidean((1, 0, np.pi / 2)) 276 | assert np.allclose(result, (0, 0, 1)) 277 | 278 | 279 | def test_vector_apply_rotation_about_z_matrix(): 280 | """Test that a positive pi/2 rotation about the z-axis results 281 | in counter clockwise rotation, in accordance with the unit circle.""" 282 | vectors = np.array( 283 | [1, 0, 0], 284 | ) 285 | expected = np.array( 286 | [0, 1, 0], 287 | ) 288 | matrix = la.mat_from_euler([0, 0, np.pi / 2]) 289 | result = la.vec_transform(vectors, matrix) 290 | 291 | npt.assert_array_almost_equal( 292 | result, 293 | expected, 294 | ) 295 | 296 | 297 | @settings(suppress_health_check=(HealthCheck.filter_too_much,)) 298 | @given(ct.test_vector, ct.test_projection) 299 | def test_vec_unproject(expected, projection_matrix): 300 | expected_2d = la.vec_transform(expected, projection_matrix) 301 | 302 | depth = expected_2d[..., 2] 303 | vector = expected_2d[..., [0, 1]] 304 | 305 | actual = la.vec_unproject(vector, projection_matrix, depth=depth) 306 | 307 | # only test stable results 308 | assume(not np.any(np.isnan(actual) | np.isinf(actual))) 309 | assert np.allclose(actual, expected, rtol=1e-16, atol=np.inf) 310 | 311 | 312 | def test_unproject_explicitly(): 313 | # see https://github.com/pygfx/pylinalg/pull/60#discussion_r1159522602 314 | # and the following comments 315 | 316 | # cube at origin with side length 10 317 | cube_corners = np.array( 318 | [ 319 | [-5, -5, -5], 320 | [5, -5, -5], 321 | [5, 5, -5], 322 | [-5, 5, -5], 323 | [-5, -5, 5], 324 | [5, -5, 5], 325 | [5, 5, 5], 326 | [-5, 5, 5], 327 | ] 328 | ) 329 | cube_world_matrix = np.eye(4) 330 | 331 | # camera 10 units away from cube origin 332 | camera_pos = (0, 0, 10) 333 | cam_world_matrix = la.mat_from_translation(camera_pos) 334 | view_matrix = np.linalg.inv(cam_world_matrix) 335 | 336 | # Scenario 1: near=4, far=16 337 | projection_matrix = la.mat_orthographic(-10, 10, 10, -10, 4, 16, depth_range=(0, 1)) 338 | cube_local_to_cam_ndc = projection_matrix @ view_matrix @ cube_world_matrix 339 | corners_ndc = la.vec_transform(cube_corners, cube_local_to_cam_ndc) 340 | corner_in_view = np.all( 341 | ((-1, -1, 0) < corners_ndc) & (corners_ndc < (1, 1, 1)), axis=-1 342 | ) 343 | assert np.sum(corner_in_view) == 8 344 | 345 | # Scenario 2: near=6, far=14 346 | projection_matrix = la.mat_orthographic(-10, 10, 10, -10, 6, 14, depth_range=(0, 1)) 347 | cube_local_to_cam_ndc = projection_matrix @ view_matrix @ cube_world_matrix 348 | corners_ndc = la.vec_transform(cube_corners, cube_local_to_cam_ndc) 349 | corner_in_view = np.all( 350 | ((-1, -1, 0) < corners_ndc) & (corners_ndc < (1, 1, 1)), axis=-1 351 | ) 352 | assert np.sum(corner_in_view) == 0 353 | 354 | 355 | def test_vec_unproject_exceptions(): 356 | vector = np.ones(2) 357 | matrix = np.eye(4) 358 | matrix[1, 1] = 0 359 | 360 | with pytest.raises(ValueError): 361 | la.vec_unproject(vector, matrix) 362 | 363 | 364 | def test_vec_unproject_is_inverse(): 365 | a = la.mat_perspective(-1, 1, -1, 1, 1, 100) 366 | a_inv = la.mat_inverse(a) 367 | vecs = np.array([[1, 2], [4, 5], [7, 8]]) 368 | 369 | expected = la.vec_unproject(vecs, a) 370 | actual = la.vec_unproject(vecs, a_inv, matrix_is_inv=True) 371 | npt.assert_array_equal(expected, actual) 372 | 373 | 374 | def test_vector_apply_rotation_ordered(): 375 | """Test that a positive pi/2 rotation about the z-axis and then the y-axis 376 | results in a different output then in standard rotation ordering.""" 377 | vectors = np.array( 378 | [1, 0, 0], 379 | ) 380 | expected = np.array( 381 | [0, 1, 0], 382 | ) 383 | matrix = la.mat_from_euler([0, np.pi / 2, np.pi / 2], order="zyx") 384 | result = la.vec_transform(vectors, matrix) 385 | 386 | npt.assert_array_almost_equal( 387 | result, 388 | expected, 389 | ) 390 | 391 | 392 | @given(ct.test_vector, ct.test_quaternion) 393 | def test_vec_transform_quat(vector, quaternion): 394 | scale = np.linalg.norm(vector) 395 | if scale > 1e100: 396 | vector = vector / scale * 1e100 397 | 398 | actual = la.vec_transform_quat(vector, quaternion) 399 | 400 | # reference implementation 401 | matrix = la.mat_from_quat(quaternion) 402 | vector = la.vec_homogeneous(vector) 403 | expected = (matrix @ vector)[..., :-1] 404 | 405 | # assert relative proximity only 406 | assert np.allclose(actual, expected, rtol=1e-10, atol=np.inf) 407 | 408 | 409 | @given(ct.test_quaternion) 410 | def test_matrix_vs_quaternion_apply(quaternion): 411 | basis = np.eye(3) 412 | matrix = la.mat_from_quat(quaternion) 413 | 414 | expected = la.vec_transform(basis, matrix) 415 | actual = la.vec_transform_quat(basis, quaternion) 416 | 417 | assert np.allclose(actual, expected) 418 | 419 | 420 | @given(ct.test_vector, ct.test_quaternion) 421 | def test_vec_transform_quat_identity(vector, quaternion): 422 | scale = np.linalg.norm(vector) 423 | if scale > 1e100: 424 | vector = vector / scale * 1e100 425 | 426 | inv_quaternion = la.quat_inv(quaternion) 427 | tmp = la.vec_transform_quat(vector, quaternion) 428 | actual = la.vec_transform_quat(tmp, inv_quaternion) 429 | 430 | # assert relative proximity only 431 | assert np.allclose(actual, vector, rtol=1e-10, atol=np.inf) 432 | 433 | 434 | def test_vec_transform__perspective(): 435 | # Test for OpenGL, wgpu, and arbitrary depth ranges 436 | depth_ranges = (-1, 1), (0, 1), (-2, 9) 437 | 438 | for depth_range in depth_ranges: 439 | m = la.mat_perspective(-1, 1, -1, 1, 1, 17, depth_range=depth_range) 440 | 441 | # Check the depth range 442 | vec2 = la.vec_transform((0, 0, -1), m) 443 | assert vec2[2] == depth_range[0] 444 | vec2 = la.vec_transform((0, 0, -17), m) 445 | assert vec2[2] == depth_range[1] 446 | # vec2 = la.vec_transform((0, 0, -9), m) -> skip: halfway is not 0.5 ndc 447 | 448 | cases = [ 449 | [(1, 0, -2), 0.5], 450 | [(1, 0, -4), 0.25], 451 | [(0, 0, -4), 0.0], 452 | [(-1, 0, -4), -0.25], 453 | [(-1, 0, -2), -0.5], 454 | ] 455 | 456 | # Check cases one by one 457 | for vec1, expected in cases: 458 | vec2 = la.vec_transform(vec1, m) 459 | assert vec2[0] == expected 460 | 461 | # Check cases batched 462 | vectors1 = np.vstack([v for v, _ in cases]) 463 | vectors2 = la.vec_transform(vectors1, m) 464 | assert vectors2[0][0] == cases[0][1] 465 | assert vectors2[1][0] == cases[1][1] 466 | assert vectors2[2][0] == cases[2][1] 467 | 468 | # Check cases batched, via out 469 | vectors2 = la.vec_transform(vectors1, m, out=vectors2) 470 | assert vectors2[0][0] == cases[0][1] 471 | assert vectors2[1][0] == cases[1][1] 472 | assert vectors2[2][0] == cases[2][1] 473 | 474 | 475 | def test_vec_transform_orthographic(): 476 | # Test for OpenGL, wgpu, and arbitrary depth ranges 477 | depth_ranges = (-1, 1), (0, 1), (-2, 9) 478 | 479 | for depth_range in depth_ranges: 480 | m = la.mat_orthographic(-1, 1, -1, 1, 1, 17, depth_range=depth_range) 481 | 482 | # Check the depth range 483 | vec2 = la.vec_transform((0, 0, -1), m) 484 | assert vec2[2] == depth_range[0] 485 | vec2 = la.vec_transform((0, 0, -17), m) 486 | assert vec2[2] == depth_range[1] 487 | vec2 = la.vec_transform((0, 0, -9), m) 488 | assert vec2[2] == (depth_range[0] + depth_range[1]) / 2 489 | 490 | # This point would be at the edge of NDC 491 | vec2 = la.vec_transform((1, 0, -2), m) 492 | assert vec2[0] == 1 493 | 494 | # This point would be at the egde of NDC 495 | vec2 = la.vec_transform((1, 0, -4), m) 496 | assert vec2[0] == 1 497 | 498 | 499 | @given(ct.test_angles_rad) 500 | def test_quat_to_euler(angles): 501 | """ 502 | Test that we can recover a rotation in euler angles from a given quaternion. 503 | 504 | This test applies the recovered rotation to a vector. 505 | """ 506 | order = "xyz" 507 | quaternion = la.quat_from_euler(angles, order=order) 508 | matrix = la.mat_from_euler(angles, order=order) 509 | 510 | angles_reconstructed = la.quat_to_euler(quaternion, order=order) 511 | matrix_reconstructed = la.mat_from_euler(angles_reconstructed) 512 | 513 | expected = la.vec_transform([1, 2, 3], matrix) 514 | actual = la.vec_transform([1, 2, 3], matrix_reconstructed) 515 | 516 | assert np.allclose(actual, expected) 517 | 518 | 519 | @given(ct.test_angles_rad) 520 | def test_quat_to_euler_roundtrip(angles): 521 | """ 522 | Test that we can recover a rotation in euler angles from a given quaternion. 523 | 524 | This test creates another quaternion with the recovered angles and 525 | test for equality. 526 | """ 527 | order = "xyz" 528 | quaternion = la.quat_from_euler(angles, order=order) 529 | 530 | angles_reconstructed = la.quat_to_euler(quaternion, order=order) 531 | quaternion_reconstructed = la.quat_from_euler(angles_reconstructed, order=order) 532 | 533 | assert np.allclose(quaternion, quaternion_reconstructed) or np.allclose( 534 | quaternion, -quaternion_reconstructed 535 | ) 536 | 537 | 538 | def test_quat_from_euler_upper_case_order(): 539 | order = "XYZ" 540 | angles = np.array([np.pi / 2, np.pi / 180, 0]) 541 | quat = la.quat_from_euler(angles, order=order) 542 | actual = la.quat_to_euler(quat, order=order) 543 | 544 | npt.assert_allclose(actual, angles) 545 | 546 | 547 | def test_quat_from_euler_lower_case_order(): 548 | order = "xyz" 549 | angles = np.array([np.pi / 2, np.pi / 180, 0]) 550 | quat = la.quat_from_euler(angles, order=order) 551 | actual = la.quat_to_euler(quat, order=order) 552 | 553 | npt.assert_allclose(actual, angles) 554 | 555 | 556 | def test_quat_euler_vs_scipy(): 557 | """Compare our implementation with scipy's.""" 558 | from scipy.spatial.transform import Rotation as R # noqa: N817 559 | 560 | cases = [ 561 | ("xyz", [np.pi / 2, np.pi / 180, 0]), 562 | ("XYZ", [np.pi / 2, np.pi / 180, 0]), 563 | ("zxy", [np.pi, np.pi / 180, -np.pi / 180]), 564 | ("ZXY", [np.pi, np.pi / 180, -np.pi / 180]), 565 | ] 566 | 567 | for order, angles in cases: 568 | npt.assert_allclose( 569 | la.quat_from_euler(angles, order=order), 570 | R.from_euler(order, angles).as_quat(), 571 | ) 572 | 573 | cases = [(order, la.quat_from_euler(euler, order=order)) for order, euler in cases] 574 | 575 | for order, quat in cases: 576 | npt.assert_allclose( 577 | la.quat_to_euler(quat, order=order), 578 | R.from_quat(quat).as_euler(order), 579 | ) 580 | 581 | 582 | def test_quat_to_euler_broadcasting(): 583 | """ 584 | Test that quat_to_euler supports broadcasting. 585 | """ 586 | quaternions = la.quat_from_axis_angle( 587 | [ 588 | [1, 0, 0], 589 | [0, 1, 0], 590 | [0, 0, 1], 591 | [0, 0, 1], 592 | ], 593 | [ 594 | np.pi, 595 | np.pi * 2, 596 | np.pi / 2, 597 | np.pi * 1.5, 598 | ], 599 | ) 600 | 601 | expected = np.array( 602 | [ 603 | [np.pi, 0, 0], 604 | [0, 0, 0], 605 | [0, 0, np.pi / 2], 606 | [0, 0, -np.pi / 2], 607 | ] 608 | ) 609 | actual = la.quat_to_euler(quaternions) 610 | 611 | npt.assert_array_almost_equal(actual, expected) 612 | -------------------------------------------------------------------------------- /pylinalg/vector.py: -------------------------------------------------------------------------------- 1 | import logging 2 | 3 | import numpy as np 4 | 5 | logger = logging.getLogger() 6 | 7 | 8 | def vec_normalize(vectors, /, *, out=None, dtype=None) -> np.ndarray: 9 | """ 10 | Normalize an array of vectors. 11 | 12 | Parameters 13 | ---------- 14 | vectors : array_like, [..., 3] 15 | array of vectors 16 | out : ndarray, optional 17 | A location into which the result is stored. If provided, it 18 | must have a shape that the inputs broadcast to. If not provided or 19 | None, a freshly-allocated array is returned. A tuple must have 20 | length equal to the number of outputs. 21 | dtype : data-type, optional 22 | Overrides the data type of the result. 23 | 24 | Returns 25 | ------- 26 | ndarray, [..., 3] 27 | array of normalized vectors. 28 | """ 29 | vectors = np.asarray(vectors, dtype=float) 30 | if out is None: 31 | out = np.empty_like(vectors, dtype=dtype) 32 | 33 | lengths_shape = (*vectors.shape[:-1], 1) 34 | lengths = np.linalg.norm(vectors, axis=-1).reshape(lengths_shape) 35 | return np.divide(vectors, lengths, out=out) 36 | 37 | 38 | def vec_homogeneous(vectors, /, *, w=1, out=None, dtype=None) -> np.ndarray: 39 | """ 40 | Append homogeneous coordinates to vectors. 41 | 42 | Parameters 43 | ---------- 44 | vectors : array_like, [..., 3] 45 | array of vectors 46 | w : number, optional, default is 1 47 | the value for the homogeneous dimensionality. 48 | this affects the result of translation transforms. use 0 (vectors) 49 | if the translation component should not be applied, 1 (positions) 50 | otherwise. 51 | out : ndarray, optional 52 | A location into which the result is stored. If provided, it 53 | must have a shape that the inputs broadcast to. If not provided or 54 | None, a freshly-allocated array is returned. A tuple must have 55 | length equal to the number of outputs. 56 | dtype : data-type, optional 57 | Overrides the data type of the result. 58 | 59 | Returns 60 | ------- 61 | ndarray, [..., 4] 62 | The list of vectors with appended homogeneous value. 63 | """ 64 | 65 | if out is None: 66 | vectors = np.asarray(vectors) 67 | shape = list(vectors.shape) 68 | shape[-1] += 1 69 | out = np.empty(shape, dtype=dtype) 70 | out[..., -1] = w 71 | out[..., :-1] = vectors 72 | return out 73 | 74 | 75 | def vec_transform( 76 | vectors, matrix, /, *, w=1, projection=True, out=None, dtype=None 77 | ) -> np.ndarray: 78 | """ 79 | Apply a transformation matrix to a vector. 80 | 81 | Parameters 82 | ---------- 83 | vectors : ndarray, [3] 84 | Array of vectors 85 | matrix : ndarray, [4, 4] 86 | Transformation matrix 87 | w : ndarray, [1], optional 88 | The value of the scale component of the homogeneous coordinate. This 89 | affects the result of translation transforms. use 0 (vectors) if the 90 | translation component should not be applied, 1 (positions) otherwise. 91 | projection : bool, optional 92 | If False, the matrix is assumed to be purely affine 93 | and the homogeneous component is not applied. Default is True. 94 | out : ndarray, optional 95 | A location into which the result is stored. If provided, it must have a 96 | shape that the inputs broadcast to. If not provided or None, a 97 | freshly-allocated array is returned. A tuple must have length equal to 98 | the number of outputs. 99 | dtype : data-type, optional 100 | Overrides the data type of the result. 101 | 102 | Returns 103 | ------- 104 | ndarray, [3] 105 | transformed vectors 106 | """ 107 | 108 | matrix = np.asarray(matrix) 109 | vectors = vec_homogeneous(vectors, w=w) 110 | 111 | # yes, the ndim > 2 version can also handle ndim=1 112 | # and ndim=2, but it's slower 113 | if vectors.ndim == 1: 114 | vectors = matrix @ vectors 115 | if projection: 116 | vectors = vectors[:-1] / vectors[-1] 117 | else: 118 | vectors = vectors[:-1] 119 | elif vectors.ndim == 2: 120 | # transposing the vectors array performs better 121 | # than transposing the matrix 122 | vectors = (matrix @ vectors.T).T 123 | if projection: 124 | vectors = vectors[:, :-1] / vectors[:, -1, None] 125 | else: 126 | vectors = vectors[:, :-1] 127 | else: 128 | vectors = matrix @ vectors[..., None] 129 | if projection: 130 | vectors = vectors[..., :-1, 0] / vectors[..., -1, :] 131 | else: 132 | vectors = vectors[..., :-1, 0] 133 | 134 | if out is None: 135 | out = vectors 136 | if dtype is not None: 137 | out = out.astype(dtype, copy=False) 138 | else: 139 | out[:] = vectors 140 | 141 | return out 142 | 143 | 144 | def vec_unproject( 145 | vector, matrix, /, *, matrix_is_inv=False, depth=0, out=None, dtype=None 146 | ) -> np.ndarray: 147 | """ 148 | Un-project a vector from 2D space to 3D space. 149 | 150 | Find a ``vectorB`` in 3D euclidean space such that the projection 151 | ``matrix @ vectorB`` yields the provided vector (in 2D euclidean 152 | space). Since the solution to the above is a 1D subspace of 3D space (a 153 | line), ``depth`` is used to select a single vector within. 154 | 155 | Parameters 156 | ---------- 157 | vector : ndarray, [2] 158 | The vector to be un-projected. 159 | matrix: ndarray, [4, 4] 160 | The camera's intrinsic matrix. 161 | matrix_is_inv: bool, optional 162 | Default is False. If True, the provided matrix is assumed to be the 163 | inverse of the camera's intrinsic matrix. 164 | depth : number, optional 165 | The distance of the unprojected vector from the camera. 166 | out : ndarray, optional 167 | A location into which the result is stored. If provided, it 168 | must have a shape that the inputs broadcast to. If not provided or 169 | None, a freshly-allocated array is returned. A tuple must have 170 | length equal to the number of outputs. 171 | dtype : data-type, optional 172 | Overrides the data type of the result. 173 | 174 | Returns 175 | ------- 176 | projected_vector : ndarray, [3] 177 | The unprojected vector in 3D space 178 | 179 | Notes 180 | ----- 181 | The source frame of this operation is the XY-plane of the camera's NDC frame 182 | and the target frame is the camera's local frame. 183 | """ 184 | 185 | vector = np.asarray(vector, dtype=float) 186 | matrix = np.asarray(matrix, dtype=float) 187 | depth = np.asarray(depth, dtype=float) 188 | 189 | result_shape = np.broadcast_shapes( 190 | vector.shape[:-1], matrix.shape[:-2], depth.shape 191 | ) 192 | 193 | if out is None: 194 | out = np.empty((*result_shape, 3), dtype=dtype) 195 | 196 | if matrix_is_inv: 197 | inverse_projection = matrix 198 | else: 199 | from .matrix import mat_inverse 200 | 201 | inverse_projection = mat_inverse(matrix, raise_err=True) 202 | 203 | vector_hom = np.empty((*result_shape, 4), dtype=dtype) 204 | vector_hom[..., 2] = depth 205 | vector_hom[..., [0, 1]] = vector 206 | vector_hom[..., 3] = 1 207 | 208 | out_hom = vector_hom @ inverse_projection.T 209 | scale = out_hom[..., -1][..., None] 210 | out[:] = (out_hom / scale)[..., :-1] 211 | 212 | return out 213 | 214 | 215 | def vec_transform_quat(vector, quaternion, /, *, out=None, dtype=None) -> np.ndarray: 216 | """Rotate a vector using a quaternion. 217 | 218 | Parameters 219 | ---------- 220 | vector : ndarray, [3] 221 | The vector to be rotated. 222 | quaternion : ndarray, [4] 223 | The quaternion to apply (in xyzw format). 224 | out : ndarray, optional 225 | A location into which the result is stored. If provided, it 226 | must have a shape that the inputs broadcast to. If not provided or 227 | None, a freshly-allocated array is returned. A tuple must have 228 | length equal to the number of outputs. 229 | dtype : data-type, optional 230 | Overrides the data type of the result. 231 | 232 | Returns 233 | ------- 234 | rotated_vector : ndarray, [3] 235 | The rotated vector. 236 | 237 | """ 238 | 239 | vector = np.asarray(vector, dtype=float) 240 | quaternion = np.asarray(quaternion, dtype=float) 241 | 242 | if out is None: 243 | out = np.zeros_like(vector, dtype=dtype) 244 | 245 | # based on https://gamedev.stackexchange.com/a/50545 246 | # (more readable than my attempt at doing the same) 247 | 248 | quat_vector = quaternion[..., :-1] 249 | quat_scalar = quaternion[..., -1] 250 | 251 | out += 2 * np.sum(quat_vector * vector, axis=-1, keepdims=True) * quat_vector 252 | out += ( 253 | quat_scalar**2 - np.sum(quat_vector * quat_vector, axis=-1, keepdims=True) 254 | ) * vector 255 | out += 2 * quat_scalar * np.cross(quat_vector, vector) 256 | 257 | return out 258 | 259 | 260 | def vec_spherical_to_euclidean(spherical, /, *, out=None, dtype=None) -> np.ndarray: 261 | """Convert spherical -> euclidean coordinates. 262 | 263 | Parameters 264 | ---------- 265 | spherical : ndarray, [3] 266 | A vector in spherical coordinates (r, phi, theta). Phi and theta are 267 | measured in radians. 268 | out : ndarray, optional 269 | A location into which the result is stored. If provided, it 270 | must have a shape that the inputs broadcast to. If not provided or 271 | None, a freshly-allocated array is returned. A tuple must have 272 | length equal to the number of outputs. 273 | dtype : data-type, optional 274 | Overrides the data type of the result. 275 | 276 | Returns 277 | ------- 278 | euclidean : ndarray, [3] 279 | A vector in euclidean coordinates. 280 | 281 | Notes 282 | ----- 283 | This implementation follows pygfx's coordinate conventions. This means that 284 | the positive y-axis is the zenith reference and the positive z-axis is the 285 | azimuth reference. Angles are measured counter-clockwise. 286 | 287 | """ 288 | 289 | spherical = np.asarray(spherical, dtype=float) 290 | 291 | if out is None: 292 | out = np.empty_like(spherical, dtype=dtype) 293 | 294 | r, theta, phi = np.split(spherical, 3, axis=-1) 295 | out[..., 0] = r * np.sin(phi) * np.sin(theta) 296 | out[..., 1] = r * np.cos(phi) 297 | out[..., 2] = r * np.sin(phi) * np.cos(theta) 298 | 299 | return out 300 | 301 | 302 | def vec_dist(vector_a, vector_b, /, *, out=None, dtype=None) -> np.ndarray: 303 | """The distance between two vectors 304 | 305 | Parameters 306 | ---------- 307 | vector_a : ndarray, [3] 308 | The first vector. 309 | vector_b : ndarray, [3] 310 | The second vector. 311 | out : ndarray, optional 312 | A location into which the result is stored. If provided, it 313 | must have a shape that the inputs broadcast to. If not provided or 314 | None, a freshly-allocated array is returned. A tuple must have 315 | length equal to the number of outputs. 316 | dtype : data-type, optional 317 | Overrides the data type of the result. 318 | 319 | Returns 320 | ------- 321 | distance : ndarray 322 | The distance between both vectors. 323 | 324 | """ 325 | 326 | vector_a = np.asarray(vector_a, dtype=float) 327 | vector_b = np.asarray(vector_b, dtype=float) 328 | 329 | shape = vector_a.shape[:-1] 330 | if out is None: 331 | out = np.linalg.norm(vector_a - vector_b, axis=-1) 332 | if dtype is not None: 333 | out = out.astype(dtype, copy=False) 334 | elif len(shape) >= 0: 335 | out[:] = np.linalg.norm(vector_a - vector_b, axis=-1) 336 | else: 337 | raise ValueError("Can't use `out` with scalar output.") 338 | 339 | return out 340 | 341 | 342 | def vec_angle(vector_a, vector_b, /, *, out=None, dtype=None) -> np.ndarray: 343 | """The angle between two vectors 344 | 345 | Parameters 346 | ---------- 347 | vector_a : ndarray, [3] 348 | The first vector. 349 | vector_b : ndarray, [3] 350 | The second vector. 351 | out : ndarray, optional 352 | A location into which the result is stored. If provided, it 353 | must have a shape that the inputs broadcast to. If not provided or 354 | None, a freshly-allocated array is returned. A tuple must have 355 | length equal to the number of outputs. 356 | dtype : data-type, optional 357 | Overrides the data type of the result. 358 | 359 | Returns 360 | ------- 361 | angle : ndarray 362 | The angle between both vectors. 363 | 364 | """ 365 | 366 | vector_a = np.asarray(vector_a, dtype=float) 367 | vector_b = np.asarray(vector_b, dtype=float) 368 | 369 | shape = vector_a.shape[:-1] 370 | 371 | # Cannot broadcast np.dot(), so just write it out 372 | dot_prod = sum( 373 | [ 374 | vector_a[..., 0] * vector_b[..., 0], 375 | vector_a[..., 1] * vector_b[..., 1], 376 | vector_a[..., 2] * vector_b[..., 2], 377 | ] 378 | ) 379 | the_cos = ( 380 | dot_prod / np.linalg.norm(vector_a, axis=-1) / np.linalg.norm(vector_b, axis=-1) 381 | ) 382 | 383 | if out is None: 384 | out = np.arccos(the_cos) 385 | if dtype is not None: 386 | out = out.astype(dtype, copy=False) 387 | elif len(shape) >= 0: 388 | out[:] = np.arccos(the_cos) 389 | else: 390 | raise ValueError("Can't use `out` with scalar output.") 391 | 392 | return out 393 | 394 | 395 | def mat_decompose_translation( 396 | homogeneous_matrix, /, *, out=None, dtype=None 397 | ) -> np.ndarray: 398 | """Position component of a homogeneous matrix. 399 | 400 | Parameters 401 | ---------- 402 | homogeneous_matrix : ndarray, [4, 4] 403 | The matrix of which the position/translation component will be 404 | extracted. 405 | out : ndarray, optional 406 | A location into which the result is stored. If provided, it 407 | must have a shape that the inputs broadcast to. If not provided or 408 | None, a freshly-allocated array is returned. A tuple must have 409 | length equal to the number of outputs. 410 | dtype : data-type, optional 411 | Overrides the data type of the result. 412 | 413 | Returns 414 | ------- 415 | position : ndarray, [3] 416 | The position/translation component. 417 | 418 | """ 419 | 420 | homogeneous_matrix = np.asarray(homogeneous_matrix) 421 | 422 | if out is None: 423 | out = homogeneous_matrix[..., :-1, -1] 424 | if dtype is not None: 425 | out = out.astype(dtype, copy=False) 426 | else: 427 | out[:] = homogeneous_matrix[..., :-1, -1] 428 | 429 | return out 430 | 431 | 432 | def vec_euclidean_to_spherical(euclidean, /, *, out=None, dtype=None) -> np.ndarray: 433 | """Convert euclidean -> spherical coordinates 434 | 435 | Parameters 436 | ---------- 437 | euclidean : ndarray, [3] 438 | A vector in euclidean coordinates. 439 | out : ndarray, optional 440 | A location into which the result is stored. If provided, it 441 | must have a shape that the inputs broadcast to. If not provided or 442 | None, a freshly-allocated array is returned. A tuple must have 443 | length equal to the number of outputs. 444 | dtype : data-type, optional 445 | Overrides the data type of the result. 446 | 447 | Returns 448 | ------- 449 | spherical : ndarray, [3] 450 | A vector in spherical coordinates (r, phi, theta). 451 | 452 | """ 453 | 454 | euclidean = np.asarray(euclidean, dtype=float) 455 | 456 | if out is None: 457 | out = np.zeros_like(euclidean, dtype=dtype) 458 | else: 459 | out[:] = 0 460 | 461 | out[..., 0] = np.sqrt(np.sum(euclidean**2, axis=-1)) 462 | 463 | # flags to handle all cases 464 | needs_flip = np.sign(euclidean[..., 0]) < 0 465 | len_xz = np.sum(euclidean[..., [0, 2]] ** 2, axis=-1) 466 | xz_nonzero = ~np.all(len_xz == 0, axis=-1) 467 | r_nonzero = ~np.all(out[..., [0]] == 0, axis=-1) 468 | 469 | # chooses phi = 0 if vector runs along y-axis 470 | out[..., 1] = np.divide(euclidean[..., 2], np.sqrt(len_xz), where=xz_nonzero) 471 | out[..., 1] = np.arccos(out[..., 1], where=xz_nonzero) 472 | out[..., 1] = np.where(needs_flip, np.abs(out[..., 1] - np.pi), out[..., 1]) 473 | 474 | # chooses theta = 0 at the origin (0, 0, 0) 475 | out[..., 2] = np.divide(euclidean[..., 1], out[..., 0], where=r_nonzero) 476 | out[..., 2] = np.arccos(out[..., 2], where=r_nonzero) 477 | out[..., 2] = np.where(needs_flip, 2 * np.pi - out[..., 2], out[..., 2]) 478 | 479 | return out 480 | 481 | 482 | def vec_spherical_safe(vector, /, *, out=None, dtype=None) -> np.ndarray: 483 | """Normalize sperhical coordinates. 484 | 485 | Normalizes a vector of spherical coordinates to restrict phi to [0, pi) and 486 | theta to [0, 2pi). 487 | 488 | Parameters 489 | ---------- 490 | vector : ndarray, [3] 491 | A vector in spherical coordinates. 492 | out : ndarray, optional 493 | A location into which the result is stored. If provided, it 494 | must have a shape that the inputs broadcast to. If not provided or 495 | None, a freshly-allocated array is returned. A tuple must have 496 | length equal to the number of outputs. 497 | dtype : data-type, optional 498 | Overrides the data type of the result. 499 | 500 | Returns 501 | ------- 502 | normalized_vector : ndarray, [3] 503 | A vector in spherical coordinates with restricted angle values. 504 | 505 | """ 506 | 507 | vector = np.asarray(vector, dtype=float) 508 | 509 | if out is None: 510 | out = np.zeros_like(vector, dtype=dtype) 511 | 512 | is_flipped = vector[..., 1] % (2 * np.pi) >= np.pi 513 | out[..., 2] = np.where(is_flipped, -vector[..., 2], vector[..., 2]) 514 | 515 | out[..., 0] = vector[..., 0] 516 | out[..., 1] = vector[..., 1] % np.pi 517 | out[..., 2] = vector[..., 1] % (2 * np.pi) 518 | 519 | out[..., 1] = np.where(out[..., 1] == np.pi, 0, out[..., 1]) 520 | out[..., 2] = np.where(out[..., 2] == 2 * np.pi, 0, out[..., 2]) 521 | 522 | return out 523 | 524 | 525 | def quat_to_euler( 526 | quaternion, /, *, order="xyz", epsilon=1e-7, out=None, dtype=None 527 | ) -> np.ndarray: 528 | """Convert quaternions to Euler angles with specified rotation order. 529 | 530 | Parameters 531 | ---------- 532 | quaternion : ndarray, [4] 533 | The quaternion to convert (in xyzw format). 534 | order : str, optional 535 | The rotation order as a string. Can include 'X', 'Y', 'Z' for intrinsic 536 | rotation (uppercase) or 'x', 'y', 'z' for extrinsic rotation (lowercase). 537 | Default is "xyz". 538 | epsilon : float, optional 539 | The floating point error margin. Default is 1e-7. 540 | out : ndarray, optional 541 | A location into which the result is stored. If provided, it 542 | must have a shape that the inputs broadcast to. If not provided or 543 | None, a freshly-allocated array is returned. A tuple must have 544 | length equal to the number of outputs. 545 | dtype : data-type, optional 546 | Overrides the data type of the result. 547 | 548 | Returns 549 | ------- 550 | out : ndarray, [3] 551 | The Euler angles in the specified order. 552 | 553 | References 554 | ---------- 555 | This implementation is based on the method described in the following paper: 556 | Bernardes E, Viollet S (2022) Quaternion to Euler angles conversion: A direct, general and computationally efficient method. 557 | PLoS ONE 17(11): e0276302. https://doi.org/10.1371/journal.pone.0276302 558 | """ 559 | quaternion = np.asarray(quaternion, dtype=float) 560 | quat = np.atleast_2d(quaternion) 561 | num_rotations = quat.shape[0] 562 | 563 | if out is None: 564 | out = np.empty((*quaternion.shape[:-1], 3), dtype=dtype) 565 | 566 | extrinsic = order.islower() 567 | order = order.lower() 568 | if not extrinsic: 569 | order = order[::-1] 570 | 571 | basis_index = {"x": 0, "y": 1, "z": 2} 572 | i = basis_index[order[0]] 573 | j = basis_index[order[1]] 574 | k = basis_index[order[2]] 575 | 576 | is_proper = i == k 577 | 578 | if is_proper: 579 | k = 3 - i - j # get third axis 580 | 581 | # Step 0 582 | # Check if permutation is even (+1) or odd (-1) 583 | sign = int((i - j) * (j - k) * (k - i) / 2) 584 | 585 | for ind in range(num_rotations): 586 | if num_rotations == 1 and out.ndim == 1: 587 | _angles = out 588 | else: 589 | _angles = out[ind, :] 590 | 591 | if is_proper: 592 | a = quat[ind, 3] 593 | b = quat[ind, i] 594 | c = quat[ind, j] 595 | d = quat[ind, k] * sign 596 | else: 597 | a = quat[ind, 3] - quat[ind, j] 598 | b = quat[ind, i] + quat[ind, k] * sign 599 | c = quat[ind, j] + quat[ind, 3] 600 | d = quat[ind, k] * sign - quat[ind, i] 601 | 602 | n2 = a**2 + b**2 + c**2 + d**2 603 | 604 | # Step 3 605 | # Compute second angle... 606 | # _angles[1] = 2*np.arccos(np.sqrt((a**2 + b**2) / n2)) 607 | _angles[1] = np.arccos(2 * (a**2 + b**2) / n2 - 1) 608 | 609 | # ... and check if it is equal to 0 or pi, causing a singularity 610 | safe1 = np.abs(_angles[1]) >= epsilon 611 | safe2 = np.abs(_angles[1] - np.pi) >= epsilon 612 | safe = safe1 and safe2 613 | 614 | # Step 4 615 | # compute first and third angles, according to case 616 | if safe: 617 | half_sum = np.arctan2(b, a) # == (alpha+gamma)/2 618 | half_diff = np.arctan2(-d, c) # == (alpha-gamma)/2 619 | 620 | _angles[0] = half_sum + half_diff 621 | _angles[2] = half_sum - half_diff 622 | 623 | else: 624 | # _angles[0] = 0 625 | 626 | if not extrinsic: 627 | # For intrinsic, set first angle to zero so that after reversal we 628 | # ensure that third angle is zero 629 | # 6a 630 | if not safe: 631 | _angles[0] = 0 632 | if not safe1: 633 | half_sum = np.arctan2(b, a) 634 | _angles[2] = 2 * half_sum 635 | # 6c 636 | if not safe2: 637 | half_diff = np.arctan2(-d, c) 638 | _angles[2] = -2 * half_diff 639 | else: 640 | # For extrinsic, set third angle to zero 641 | # 6b 642 | if not safe: 643 | _angles[2] = 0 644 | if not safe1: 645 | half_sum = np.arctan2(b, a) 646 | _angles[0] = 2 * half_sum 647 | # 6c 648 | if not safe2: 649 | half_diff = np.arctan2(-d, c) 650 | _angles[0] = 2 * half_diff 651 | 652 | for i_ in range(3): 653 | if _angles[i_] < -np.pi: 654 | _angles[i_] += 2 * np.pi 655 | elif _angles[i_] > np.pi: 656 | _angles[i_] -= 2 * np.pi 657 | 658 | # for Tait-Bryan angles 659 | if not is_proper: 660 | _angles[2] *= sign 661 | _angles[1] -= np.pi / 2 662 | 663 | if not extrinsic: 664 | # reversal 665 | _angles[0], _angles[2] = _angles[2], _angles[0] 666 | 667 | # Step 8 668 | if not safe: 669 | logger.warning( 670 | "Gimbal lock detected. Setting third angle to zero " 671 | "since it is not possible to uniquely determine " 672 | "all angles." 673 | ) 674 | 675 | return out 676 | 677 | 678 | _warned_about_eucledian = False 679 | 680 | 681 | def _warn_about_eucledian(): 682 | global _warned_about_eucledian 683 | if not _warned_about_eucledian: 684 | _warned_about_eucledian = True 685 | m = "methods vec_spherical_to_euclidian() and vec_euclidian_to_spherical()" 686 | m += " have been renamed, because its eucledEan, not eucledIan." 687 | logger.warning("pylinalg deprecation warning: " + m) 688 | 689 | 690 | def vec_euclidian_to_spherical(euclidean, /, *, out=None, dtype=None) -> np.ndarray: 691 | _warn_about_eucledian() 692 | return vec_euclidean_to_spherical(euclidean, out=out, dtype=dtype) 693 | 694 | 695 | def vec_spherical_to_euclidian(spherical, /, *, out=None, dtype=None) -> np.ndarray: 696 | _warn_about_eucledian() 697 | return vec_spherical_to_euclidean(spherical, out=out, dtype=dtype) 698 | 699 | 700 | __all__ = [ 701 | name for name in globals() if name.startswith(("vec_", "mat_", "quat_", "aabb_")) 702 | ] 703 | -------------------------------------------------------------------------------- /pylinalg/matrix.py: -------------------------------------------------------------------------------- 1 | from typing import Tuple 2 | import numpy as np 3 | from numpy.lib.stride_tricks import as_strided 4 | 5 | 6 | def mat_combine(matrices, /, *, out=None, dtype=None) -> np.ndarray: 7 | """ 8 | Combine a list of affine matrices by multiplying them. 9 | 10 | Note that by matrix multiplication rules, the output matrix will applied the 11 | given transformations in reverse order. For example, passing a scaling, 12 | rotation and translation matrix (in that order), will lead to a combined 13 | transformation matrix that applies translation first, then rotation and finally 14 | scaling. 15 | 16 | Parameters 17 | ---------- 18 | matrices : list of ndarray, [4, 4] 19 | List of affine matrices to combine. 20 | out : ndarray, optional 21 | A location into which the result is stored. If provided, it 22 | must have a shape that the inputs broadcast to. If not provided or 23 | None, a freshly-allocated array is returned. A tuple must have 24 | length equal to the number of outputs. 25 | dtype : data-type, optional 26 | Overrides the data type of the result. 27 | 28 | Returns 29 | ------- 30 | ndarray, [4, 4] 31 | Combined transformation matrix. 32 | """ 33 | 34 | matrices = [np.asarray(matrix) for matrix in matrices] 35 | result_shape = np.broadcast_shapes(*[matrix.shape for matrix in matrices]) 36 | 37 | if out is None: 38 | out = np.empty(result_shape, dtype=dtype) 39 | 40 | out[:] = matrices[0] 41 | for matrix in matrices[1:]: 42 | np.matmul(out, matrix, out=out) 43 | 44 | return out 45 | 46 | 47 | def mat_from_translation(vector, /, *, out=None, dtype=None) -> np.ndarray: 48 | """ 49 | Make a translationmatrix given a translation vector. 50 | 51 | Parameters 52 | ---------- 53 | vector : number or ndarray, [3] 54 | translation vector 55 | out : ndarray, optional 56 | A location into which the result is stored. If provided, it 57 | must have a shape that the inputs broadcast to. If not provided or 58 | None, a freshly-allocated array is returned. A tuple must have 59 | length equal to the number of outputs. 60 | dtype : data-type, optional 61 | Overrides the data type of the result. 62 | 63 | Returns 64 | ------- 65 | ndarray, [4, 4] 66 | Translation matrix. 67 | """ 68 | vector = np.asarray(vector) 69 | result_shape = (*vector.shape[:-1], 4, 4) 70 | 71 | if out is None: 72 | out = np.empty(result_shape, dtype=dtype) 73 | 74 | # view into the diagonal of the result 75 | n_matrices = np.prod(result_shape[:-2], dtype=int) 76 | itemsize = out.itemsize 77 | diagonal = as_strided( 78 | out, shape=(n_matrices, 4), strides=(16 * itemsize, 5 * itemsize) 79 | ) 80 | 81 | out[:] = 0 82 | diagonal[:] = 1 83 | out[..., :-1, -1] = vector 84 | 85 | return out 86 | 87 | 88 | def mat_from_scale(factors, /, *, out=None, dtype=None) -> np.ndarray: 89 | """ 90 | Make a scaling matrix given scaling factors per axis, or a 91 | single uniform scaling factor. 92 | 93 | Parameters 94 | ---------- 95 | factor : number or ndarray, [3] 96 | scaling factor(s) 97 | out : ndarray, optional 98 | A location into which the result is stored. If provided, it 99 | must have a shape that the inputs broadcast to. If not provided or 100 | None, a freshly-allocated array is returned. A tuple must have 101 | length equal to the number of outputs. 102 | dtype : data-type, optional 103 | Overrides the data type of the result. 104 | 105 | Returns 106 | ------- 107 | ndarray, [4, 4] 108 | Scaling matrix. 109 | """ 110 | factors = np.asarray(factors, dtype=dtype) 111 | 112 | matrix = np.identity(4, dtype=dtype) 113 | matrix[np.diag_indices(3)] = factors 114 | 115 | if out is not None: 116 | out[:] = matrix 117 | return out 118 | 119 | return matrix 120 | 121 | 122 | def mat_from_euler(angles, /, *, order="xyz", out=None, dtype=None) -> np.ndarray: 123 | """ 124 | Make a matrix given euler angles (in radians) per axis. 125 | 126 | Parameters 127 | ---------- 128 | angles : ndarray, [3] 129 | The euler angles. 130 | order : string, optional 131 | The rotation order as a string. Can include 'X', 'Y', 'Z' for intrinsic 132 | rotation (uppercase) or 'x', 'y', 'z' for extrinsic rotation (lowercase). 133 | Default is "xyz". 134 | out : ndarray, optional 135 | A location into which the result is stored. If provided, it 136 | must have a shape that the inputs broadcast to. If not provided or 137 | None, a freshly-allocated array is returned. A tuple must have 138 | length equal to the number of outputs. 139 | dtype : data-type, optional 140 | Overrides the data type of the result. 141 | 142 | Returns 143 | ------- 144 | ndarray, [4, 4] 145 | Rotation matrix. 146 | 147 | Notes 148 | ----- 149 | If you are familiar with TreeJS note that this function uses ``order`` to 150 | denote both the order in which rotations are applied *and* the order in 151 | which angles are provided in ``angles``. I.e., 152 | ``mat_from_euler([np.pi, np.pi, 0], order="zyx")`` 153 | will first rotate 180° ccw (counter-clockwise) around the z-axis, then 180° 154 | ccw around the y-axis, and finally 0° around the x axis. 155 | 156 | """ 157 | fill_out_first = out is not None 158 | angles = np.atleast_1d(np.asarray(angles)) 159 | extrinsic = order.islower() 160 | order = order.upper() 161 | axis_lookup = {"X": 0, "Y": 1, "Z": 2} 162 | if extrinsic: 163 | angles = reversed(angles) 164 | order = reversed(order) 165 | for angle, axis in zip(angles, order): 166 | axis_idx = axis_lookup[axis] 167 | affine_matrix = np.identity(4, dtype=dtype) 168 | 169 | if axis_idx == 0: 170 | affine_matrix[1, 1] = np.cos(angle) 171 | affine_matrix[1, 2] = -np.sin(angle) 172 | affine_matrix[2, 1] = np.sin(angle) 173 | affine_matrix[2, 2] = np.cos(angle) 174 | elif axis_idx == 1: 175 | affine_matrix[0, 0] = np.cos(angle) 176 | affine_matrix[0, 2] = np.sin(angle) 177 | affine_matrix[2, 0] = -np.sin(angle) 178 | affine_matrix[2, 2] = np.cos(angle) 179 | elif axis_idx == 2: 180 | affine_matrix[0, 0] = np.cos(angle) 181 | affine_matrix[0, 1] = -np.sin(angle) 182 | affine_matrix[1, 0] = np.sin(angle) 183 | affine_matrix[1, 1] = np.cos(angle) 184 | 185 | if fill_out_first: 186 | out[:] = affine_matrix 187 | fill_out_first = False 188 | elif out is None: 189 | out = affine_matrix 190 | else: 191 | out @= affine_matrix 192 | return out 193 | 194 | 195 | def mat_from_axis_angle(axis, angle, /, *, out=None, dtype=None) -> np.ndarray: 196 | """ 197 | Make a rotation matrix given a rotation axis and an angle (in radians). 198 | 199 | Parameters 200 | ---------- 201 | axis : ndarray, [3] 202 | The rotation axis. 203 | angle : number 204 | The angle (in radians) to rotate about the axis. 205 | out : ndarray, optional 206 | A location into which the result is stored. If provided, it 207 | must have a shape that the inputs broadcast to. If not provided or 208 | None, a freshly-allocated array is returned. A tuple must have 209 | length equal to the number of outputs. 210 | dtype : data-type, optional 211 | Overrides the data type of the result. 212 | 213 | Returns 214 | ------- 215 | ndarray, [4, 4] 216 | Rotation matrix. 217 | """ 218 | axis = np.asarray(axis) 219 | 220 | if out is None: 221 | out = np.identity(4, dtype=dtype) 222 | else: 223 | out[:] = np.identity(4) 224 | 225 | eye = out[:3, :3] 226 | rotation = np.cos(angle) * eye 227 | # the second component here is the "cross product matrix" of axis 228 | rotation += np.sin(angle) * np.cross(axis, eye * -1) 229 | rotation += (1 - np.cos(angle)) * (np.outer(axis, axis)) 230 | out[:3, :3] = rotation 231 | 232 | return out 233 | 234 | 235 | def quat_from_mat(matrix, /, *, out=None, dtype=None) -> np.ndarray: 236 | """ 237 | Make a quaternion given a rotation matrix. 238 | 239 | Parameters 240 | ---------- 241 | matrix : ndarray, [3] 242 | The rotation matrix. 243 | out : ndarray, optional 244 | A location into which the result is stored. If provided, it 245 | must have a shape that the inputs broadcast to. If not provided or 246 | None, a freshly-allocated array is returned. A tuple must have 247 | length equal to the number of outputs. 248 | dtype : data-type, optional 249 | Overrides the data type of the result. 250 | 251 | Returns 252 | ------- 253 | ndarray, [4] 254 | Quaternion. 255 | """ 256 | m = matrix[:3, :3] 257 | t = np.trace(m) 258 | 259 | if t > 0: 260 | s = 0.5 / np.sqrt(t + 1) 261 | x = (m[2, 1] - m[1, 2]) * s 262 | y = (m[0, 2] - m[2, 0]) * s 263 | z = (m[1, 0] - m[0, 1]) * s 264 | w = 0.25 / s 265 | 266 | elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]: 267 | s = 2 * np.sqrt(1 + m[0, 0] - m[1, 1] - m[2, 2]) 268 | x = 0.25 * s 269 | y = (m[0, 1] + m[1, 0]) / s 270 | z = (m[0, 2] + m[2, 0]) / s 271 | w = (m[2, 1] - m[1, 2]) / s 272 | 273 | elif m[1, 1] > m[2, 2]: 274 | s = 2 * np.sqrt(1 + m[1, 1] - m[0, 0] - m[2, 2]) 275 | x = (m[0, 1] + m[1, 0]) / s 276 | y = 0.25 * s 277 | z = (m[1, 2] + m[2, 1]) / s 278 | w = (m[0, 2] - m[2, 0]) / s 279 | 280 | else: 281 | s = 2 * np.sqrt(1 + m[2, 2] - m[0, 0] - m[1, 1]) 282 | x = (m[0, 2] + m[2, 0]) / s 283 | y = (m[1, 2] + m[2, 1]) / s 284 | z = 0.25 * s 285 | w = (m[1, 0] - m[0, 1]) / s 286 | 287 | if out is None: 288 | out = np.array([x, y, z, w], dtype=dtype) 289 | else: 290 | out[:] = [x, y, z, w] 291 | return out 292 | 293 | 294 | def mat_compose( 295 | translation, rotation, scaling, /, *, out=None, dtype=None 296 | ) -> np.ndarray: 297 | """ 298 | Compose a transformation matrix given a translation vector, a 299 | quaternion and a scaling vector. 300 | 301 | Parameters 302 | ---------- 303 | translation : number or ndarray, [3] 304 | translation vector 305 | rotation : ndarray, [4] 306 | quaternion 307 | scaling : number or ndarray, [3] 308 | scaling factor(s) 309 | out : ndarray, optional 310 | A location into which the result is stored. If provided, it 311 | must have a shape that the inputs broadcast to. If not provided or 312 | None, a freshly-allocated array is returned. A tuple must have 313 | length equal to the number of outputs. 314 | dtype : data-type, optional 315 | Overrides the data type of the result. 316 | 317 | Returns 318 | ------- 319 | ndarray, [4, 4] 320 | Transformation matrix 321 | """ 322 | if out is None: 323 | out = np.empty((4, 4), dtype=dtype) 324 | 325 | x, y, z, w = rotation 326 | x2 = x + x 327 | y2 = y + y 328 | z2 = z + z 329 | xx = x * x2 330 | xy = x * y2 331 | xz = x * z2 332 | yy = y * y2 333 | yz = y * z2 334 | zz = z * z2 335 | wx = w * x2 336 | wy = w * y2 337 | wz = w * z2 338 | 339 | scaling = np.asarray(scaling) 340 | if scaling.size == 1: 341 | scaling = np.broadcast_to(scaling, (3,)) 342 | sx, sy, sz = scaling 343 | 344 | out[0, 0] = (1 - (yy + zz)) * sx 345 | out[1, 0] = (xy + wz) * sx 346 | out[2, 0] = (xz - wy) * sx 347 | out[3, 0:3] = 0 348 | 349 | out[0, 1] = (xy - wz) * sy 350 | out[1, 1] = (1 - (xx + zz)) * sy 351 | out[2, 1] = (yz + wx) * sy 352 | 353 | out[0, 2] = (xz + wy) * sz 354 | out[1, 2] = (yz - wx) * sz 355 | out[2, 2] = (1 - (xx + yy)) * sz 356 | 357 | out[0:3, 3] = translation 358 | out[3, 3] = 1 359 | 360 | return out 361 | 362 | 363 | def mat_decompose( 364 | matrix, /, *, scaling_signs=None, dtype=None, out=None 365 | ) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: 366 | """ 367 | Decompose a transformation matrix into a translation vector, a 368 | quaternion and a scaling vector. 369 | 370 | Parameters 371 | ---------- 372 | matrix : ndarray, [4, 4] 373 | transformation matrix 374 | scaling_signs : ndarray, [3], optional 375 | scaling factor signs. If you wish to preserve the original scaling 376 | factors through a compose-decompose roundtrip, you should 377 | provide the original scaling factors here, or alternatively just 378 | the signs. 379 | out : ndarray, optional 380 | A location into which the result is stored. If provided, it 381 | must have a shape that the inputs broadcast to. If not provided or 382 | None, a freshly-allocated array is returned. A tuple must have 383 | length equal to the number of outputs. 384 | dtype : data-type, optional 385 | Overrides the data type of the result. 386 | 387 | Returns 388 | ------- 389 | translation : ndarray, [3] 390 | translation vector 391 | rotation : ndarray, [4] 392 | quaternion 393 | scaling : ndarray, [3] 394 | scaling factors 395 | """ 396 | matrix = np.asarray(matrix) 397 | 398 | if out is not None: 399 | translation = out[0] 400 | else: 401 | translation = np.empty((3,), dtype=dtype) 402 | translation[:] = matrix[:-1, -1] 403 | 404 | flip = 1 if np.linalg.det(matrix) >= 0 else -1 405 | if scaling_signs is not None: 406 | # if the user provides the scaling signs, always use them 407 | scaling_signs = np.sign(scaling_signs) 408 | if np.prod(scaling_signs) != flip: 409 | raise ValueError( 410 | "Number of negative signs is inconsistent with the determinant" 411 | ) 412 | else: 413 | # if not, detect if a flip is needed to reconstruct the transform 414 | # and apply it to the first axis arbitrarily 415 | scaling_signs = np.array([flip, 1, 1]) 416 | 417 | if out is not None: 418 | scaling = out[2] 419 | else: 420 | scaling = np.empty((3,), dtype=dtype) 421 | scaling[:] = np.linalg.norm(matrix[:-1, :-1], axis=0) 422 | scaling *= scaling_signs 423 | 424 | rotation = out[1] if out is not None else None 425 | 426 | rotation_matrix = matrix[:-1, :-1].astype(float, copy=True) 427 | mask = scaling != 0 428 | rotation_matrix[:, mask] /= scaling[mask][None, :] 429 | rotation_matrix[:, ~mask] = 0.0 430 | rotation = quat_from_mat(rotation_matrix, out=rotation, dtype=dtype) 431 | 432 | return translation, rotation, scaling 433 | 434 | 435 | def mat_perspective( 436 | left, right, top, bottom, near, far, /, *, depth_range=(-1, 1), out=None, dtype=None 437 | ) -> np.ndarray: 438 | """ 439 | Create a perspective projection matrix. 440 | 441 | Parameters 442 | ---------- 443 | left : number 444 | distance between the left frustum plane and the origin 445 | right : number 446 | distance between the right frustum plane and the origin 447 | top : number 448 | distance between the top frustum plane and the origin 449 | bottom : number 450 | distance between the bottom frustum plane and the origin 451 | near : number 452 | distance between the near frustum plane and the origin 453 | far : number 454 | distance between the far frustum plane and the origin 455 | depth_range : Tuple[float, float] 456 | The interval along the z-axis in NDC that shall correspond to the region 457 | inside the viewing frustum. 458 | out : ndarray, optional 459 | A location into which the result is stored. If provided, it 460 | must have a shape that the inputs broadcast to. If not provided or 461 | None, a freshly-allocated array is returned. A tuple must have 462 | length equal to the number of outputs. 463 | dtype : data-type, optional 464 | Overrides the data type of the result. 465 | 466 | Returns 467 | ------- 468 | matrix : ndarray, [4, 4] 469 | perspective projection matrix 470 | """ 471 | if out is None: 472 | out = np.zeros((4, 4), dtype=dtype) 473 | else: 474 | out[:] = 0.0 475 | 476 | x = 2 * near / (right - left) 477 | y = 2 * near / (top - bottom) 478 | 479 | near_d = near * depth_range[0] 480 | far_d = far * depth_range[1] 481 | depth_diff = depth_range[1] - depth_range[0] 482 | 483 | a = (right + left) / (right - left) 484 | b = (top + bottom) / (top - bottom) 485 | c = -(far_d - near_d) / (far - near) 486 | d = -(far * near * depth_diff) / (far - near) 487 | 488 | out[0, 0] = x 489 | out[0, 2] = a 490 | out[1, 1] = y 491 | out[1, 2] = b 492 | out[2, 2] = c 493 | out[2, 3] = d 494 | out[3, 2] = -1 495 | 496 | return out 497 | 498 | 499 | def mat_orthographic( 500 | left, right, top, bottom, near, far, /, *, depth_range=(-1, 1), out=None, dtype=None 501 | ) -> np.ndarray: 502 | """Create an orthographic projection matrix. 503 | 504 | The result projects points from local space into NDC (normalized device 505 | coordinates). Elements inside the viewing frustum defind by left, right, 506 | top, bottom, near, far, are projected into the unit cube centered at the 507 | origin (default) or a cuboid (custom `depth_range`). The frustum is centered 508 | around the local frame's origin. 509 | 510 | Parameters 511 | ---------- 512 | left : ndarray, [1] 513 | Distance between the left frustum plane and the origin 514 | right : ndarray, [1] 515 | Distance between the right frustum plane and the origin 516 | top : ndarray, [1] 517 | Distance between the top frustum plane and the origin 518 | bottom : ndarray, [1] 519 | Distance between the bottom frustum plane and the origin 520 | near : ndarray, [1] 521 | Distance between the near frustum plane and the origin 522 | far : ndarray, [1] 523 | Distance between the far frustum plane and the origin 524 | depth_range : ndarray, [2] 525 | The interval along the z-axis in NDC that shall correspond to the region 526 | inside the viewing frustum. 527 | out : ndarray, optional 528 | A location into which the result is stored. If provided, it must have a 529 | shape that the inputs broadcast to. If not provided or None, a 530 | freshly-allocated array is returned. A tuple must have length equal to 531 | the number of outputs. 532 | dtype : data-type, optional 533 | Overrides the data type of the result. 534 | 535 | Returns 536 | ------- 537 | matrix : ndarray, [4, 4] 538 | orthographic projection matrix 539 | 540 | Notes 541 | ----- 542 | The parameters to this function are given in a left-handed frame that is 543 | obtained by mirroring source's Z-axis at the origin. In other words, if the 544 | returned matrix represents a camera's projection matrix then this function's 545 | parameters are given in a frame that is like the camera's local frame except 546 | that it's Z-axis is inverted. This means that positive values for `near` and 547 | `far` refer to a negative Z values in camera local. 548 | 549 | """ 550 | 551 | left = np.asarray(left, dtype=float) 552 | right = np.asarray(right, dtype=float) 553 | top = np.asarray(top, dtype=float) 554 | bottom = np.asarray(bottom, dtype=float) 555 | far = np.asarray(far, dtype=float) 556 | near = np.asarray(near, dtype=float) 557 | depth_range = np.asarray(depth_range, dtype=float) 558 | 559 | if out is None: 560 | batch_shape = np.broadcast_shapes( 561 | left.shape[:-1], 562 | right.shape[:-1], 563 | top.shape[:-1], 564 | bottom.shape[:-1], 565 | far.shape[:-1], 566 | near.shape[:-1], 567 | depth_range.shape[:-1], 568 | ) 569 | out = np.zeros((*batch_shape, 4, 4), dtype=dtype) 570 | else: 571 | out[:] = 0 572 | 573 | # desired cuboid dimensions 574 | out[..., 0, 0] = 2 575 | out[..., 1, 1] = 2 576 | out[..., 2, 2] = -np.diff(depth_range, axis=-1) 577 | out[..., 3, 3] = 1 578 | 579 | # translation to cuboid origin 580 | out[..., 0, 3] = -(right + left) 581 | out[..., 1, 3] = -(top + bottom) 582 | out[..., 2, 3] = far * depth_range[..., 0] - near * depth_range[..., 1] 583 | 584 | # frustum-based scaling 585 | out[..., 0, :] /= right - left 586 | out[..., 1, :] /= top - bottom 587 | out[..., 2, :] /= far - near 588 | 589 | return out 590 | 591 | 592 | def mat_look_at(eye, target, up_reference, /, *, out=None, dtype=None) -> np.ndarray: 593 | """ 594 | Rotation that aligns two vectors. 595 | 596 | Given an entity at position `eye` looking at position `target`, this 597 | function computes a rotation matrix that makes the local frame "look at" the 598 | same direction, i.e., the matrix will rotate the local frame's z-axes 599 | (forward) to point in direction ``target - eye``. 600 | 601 | This rotation matrix is not unique (yet), as the above doesn't specify the 602 | desired rotation around the new z-axis. The rotation around this axis is 603 | controlled by ``up_reference``, which indicates the direction of the y-axis 604 | (up) of a reference frame of choice expressed in local coordinates. The 605 | rotation around the new z-axis will then align `up_reference`, the new 606 | y-axis, and the new z-axis in the same plane. 607 | 608 | In many cases, a natural choice for ``up_reference`` is the world frame's 609 | y-axis, i.e., ``up_reference`` would be the world's y-axis expressed in 610 | local coordinates. This can be thought of as "gravity pulling on the 611 | rotation" (opposite direction of world frame's up) and will create a result 612 | with a level attitude. 613 | 614 | Parameters 615 | ---------- 616 | eye : ndarray, [3] 617 | A vector indicating the direction that should be aligned. 618 | target : ndarray, [3] 619 | A vector indicating the direction to align on. 620 | up : ndarray, [3] 621 | The direction of the camera's up axis. 622 | out : ndarray, optional 623 | A location into which the result is stored. If provided, it 624 | must have a shape that the inputs broadcast to. If not provided or 625 | None, a freshly-allocated array is returned. A tuple must have 626 | length equal to the number of outputs. 627 | dtype : data-type, optional 628 | Overrides the data type of the result. 629 | 630 | Returns 631 | ------- 632 | rotation_matrix : ndarray, [4, 4] 633 | A homogeneous matrix describing the rotation. 634 | 635 | Notes 636 | ----- 637 | If the new z-axis (``target - eye``) aligns with the chosen ``up_reference`` 638 | then we can't compute the angle of rotation around the new z-axis. In this 639 | case, we will default to a rotation of 0, which may result in surprising 640 | behavior for some use-cases. It is the user's responsibility to ensure that 641 | these two directions don't align. 642 | 643 | """ 644 | 645 | eye = np.asarray(eye, dtype=float) 646 | target = np.asarray(target, dtype=float) 647 | up_reference = np.asarray(up_reference, dtype=float) 648 | 649 | new_z = target - eye 650 | up_reference = up_reference / np.linalg.norm(up_reference, axis=-1) 651 | 652 | result_shape = np.broadcast_shapes(eye.shape, target.shape, up_reference.shape) 653 | if out is None: 654 | out = np.zeros((*result_shape[:-1], 4, 4), dtype=dtype) 655 | else: 656 | out[:] = 0 657 | 658 | # Note: The below is equivalent to np.fill_diagonal(out, 1, axes=(-2, -1)), 659 | # i.e., treat the last two axes as a matrix and fill its diagonal with 1. 660 | # Currently numpy doesn't support axes on fill_diagonal, so we do it 661 | # ourselves to support input batches and mimic the `np.linalg` API. 662 | n_matrices = np.prod(result_shape[:-1], dtype=int) 663 | itemsize = out.itemsize 664 | view = as_strided(out, shape=(n_matrices, 4), strides=(16 * itemsize, 5 * itemsize)) 665 | view[:] = 1 666 | 667 | out[..., :-1, 2] = new_z / np.linalg.norm(new_z, axis=-1) 668 | out[..., :-1, 0] = np.cross( 669 | up_reference, out[..., :-1, 2], axisa=-1, axisb=-1, axisc=-1 670 | ) 671 | out[..., :-1, 1] = np.cross( 672 | out[..., :-1, 2], out[..., :-1, 0], axisa=-1, axisb=-1, axisc=-1 673 | ) 674 | out /= np.linalg.norm(out, axis=-2)[..., None, :] 675 | 676 | return out 677 | 678 | 679 | def _mat_inv(m) -> np.ndarray: 680 | # Reference: 681 | # https://github.com/mrdoob/three.js/blob/dev/src/math/Matrix4.js 682 | # based on: 683 | # http://www.euclideanspace.com/maths/algebra/matrix/functions/inverse/fourD/index.htm 684 | out = np.empty((4, 4), dtype=float) 685 | me = m.flat 686 | 687 | n11 = me[0] 688 | n21 = me[4] 689 | n31 = me[8] 690 | n41 = me[12] 691 | n12 = me[1] 692 | n22 = me[5] 693 | n32 = me[9] 694 | n42 = me[13] 695 | n13 = me[2] 696 | n23 = me[6] 697 | n33 = me[10] 698 | n43 = me[14] 699 | n14 = me[3] 700 | n24 = me[7] 701 | n34 = me[11] 702 | n44 = me[15] 703 | 704 | t11 = ( 705 | n23 * n34 * n42 706 | - n24 * n33 * n42 707 | + n24 * n32 * n43 708 | - n22 * n34 * n43 709 | - n23 * n32 * n44 710 | + n22 * n33 * n44 711 | ) 712 | t12 = ( 713 | n14 * n33 * n42 714 | - n13 * n34 * n42 715 | - n14 * n32 * n43 716 | + n12 * n34 * n43 717 | + n13 * n32 * n44 718 | - n12 * n33 * n44 719 | ) 720 | t13 = ( 721 | n13 * n24 * n42 722 | - n14 * n23 * n42 723 | + n14 * n22 * n43 724 | - n12 * n24 * n43 725 | - n13 * n22 * n44 726 | + n12 * n23 * n44 727 | ) 728 | t14 = ( 729 | n14 * n23 * n32 730 | - n13 * n24 * n32 731 | - n14 * n22 * n33 732 | + n12 * n24 * n33 733 | + n13 * n22 * n34 734 | - n12 * n23 * n34 735 | ) 736 | 737 | det = n11 * t11 + n21 * t12 + n31 * t13 + n41 * t14 738 | 739 | if det == 0: 740 | raise np.linalg.LinAlgError("Singular matrix") 741 | 742 | det_inv = 1 / det 743 | 744 | oe = out.flat 745 | oe[0] = t11 * det_inv 746 | oe[4] = ( 747 | n24 * n33 * n41 748 | - n23 * n34 * n41 749 | - n24 * n31 * n43 750 | + n21 * n34 * n43 751 | + n23 * n31 * n44 752 | - n21 * n33 * n44 753 | ) * det_inv 754 | oe[8] = ( 755 | n22 * n34 * n41 756 | - n24 * n32 * n41 757 | + n24 * n31 * n42 758 | - n21 * n34 * n42 759 | - n22 * n31 * n44 760 | + n21 * n32 * n44 761 | ) * det_inv 762 | oe[12] = ( 763 | n23 * n32 * n41 764 | - n22 * n33 * n41 765 | - n23 * n31 * n42 766 | + n21 * n33 * n42 767 | + n22 * n31 * n43 768 | - n21 * n32 * n43 769 | ) * det_inv 770 | 771 | oe[1] = t12 * det_inv 772 | oe[5] = ( 773 | n13 * n34 * n41 774 | - n14 * n33 * n41 775 | + n14 * n31 * n43 776 | - n11 * n34 * n43 777 | - n13 * n31 * n44 778 | + n11 * n33 * n44 779 | ) * det_inv 780 | oe[9] = ( 781 | n14 * n32 * n41 782 | - n12 * n34 * n41 783 | - n14 * n31 * n42 784 | + n11 * n34 * n42 785 | + n12 * n31 * n44 786 | - n11 * n32 * n44 787 | ) * det_inv 788 | oe[13] = ( 789 | n12 * n33 * n41 790 | - n13 * n32 * n41 791 | + n13 * n31 * n42 792 | - n11 * n33 * n42 793 | - n12 * n31 * n43 794 | + n11 * n32 * n43 795 | ) * det_inv 796 | 797 | oe[2] = t13 * det_inv 798 | oe[6] = ( 799 | n14 * n23 * n41 800 | - n13 * n24 * n41 801 | - n14 * n21 * n43 802 | + n11 * n24 * n43 803 | + n13 * n21 * n44 804 | - n11 * n23 * n44 805 | ) * det_inv 806 | oe[10] = ( 807 | n12 * n24 * n41 808 | - n14 * n22 * n41 809 | + n14 * n21 * n42 810 | - n11 * n24 * n42 811 | - n12 * n21 * n44 812 | + n11 * n22 * n44 813 | ) * det_inv 814 | oe[14] = ( 815 | n13 * n22 * n41 816 | - n12 * n23 * n41 817 | - n13 * n21 * n42 818 | + n11 * n23 * n42 819 | + n12 * n21 * n43 820 | - n11 * n22 * n43 821 | ) * det_inv 822 | 823 | oe[3] = t14 * det_inv 824 | oe[7] = ( 825 | n13 * n24 * n31 826 | - n14 * n23 * n31 827 | + n14 * n21 * n33 828 | - n11 * n24 * n33 829 | - n13 * n21 * n34 830 | + n11 * n23 * n34 831 | ) * det_inv 832 | oe[11] = ( 833 | n14 * n22 * n31 834 | - n12 * n24 * n31 835 | - n14 * n21 * n32 836 | + n11 * n24 * n32 837 | + n12 * n21 * n34 838 | - n11 * n22 * n34 839 | ) * det_inv 840 | oe[15] = ( 841 | n12 * n23 * n31 842 | - n13 * n22 * n31 843 | + n13 * n21 * n32 844 | - n11 * n23 * n32 845 | - n12 * n21 * n33 846 | + n11 * n22 * n33 847 | ) * det_inv 848 | 849 | return out 850 | 851 | 852 | if int(np.__version__.split(".")[0]) >= 2: 853 | _default_mat_inv_method = "numpy" 854 | else: 855 | _default_mat_inv_method = "python" 856 | 857 | 858 | def mat_inverse( 859 | matrix, /, *, method=_default_mat_inv_method, raise_err=False, dtype=None, out=None 860 | ) -> np.ndarray: 861 | """ 862 | Compute the inverse of a matrix. 863 | 864 | Parameters 865 | ---------- 866 | matrix : ndarray, [4, 4] 867 | The matrix to invert. 868 | method : str, optional 869 | The method to use for inversion. The default is "numpy" when 870 | numpy version is 2.0.0 or newer, otherwise "python". 871 | raise_err : bool, optional 872 | Raise a ValueError if the matrix is singular. Default is False. 873 | dtype : data-type, optional 874 | Overrides the data type of the result. 875 | out : ndarray, optional 876 | A location into which the result is stored. If provided, it must have a 877 | shape that the inputs broadcast to. If not provided or None, a 878 | freshly-allocated array is returned. A tuple must have length equal to 879 | the number of outputs. 880 | 881 | Returns 882 | ------- 883 | out : ndarray, [4, 4] 884 | The inverse of the input matrix. 885 | 886 | Notes 887 | ----- 888 | The default method is "numpy" when numpy version >= 2.0.0, 889 | which uses the `numpy.linalg.inv` function. 890 | The alternative method is "python", which uses a pure python implementation of 891 | the inversion algorithm. The python method is used to avoid a performance 892 | issue with `numpy.linalg.inv` on some platforms when numpy version < 2.0.0. 893 | See: https://github.com/pygfx/pygfx/issues/763 894 | The python method is slower than the numpy method, but it is guaranteed to work. 895 | 896 | When the matrix is singular, it will return a matrix filled with zeros, 897 | This is a common behavior in real-time graphics applications. 898 | 899 | """ 900 | 901 | fn = { 902 | "numpy": np.linalg.inv, 903 | "python": _mat_inv, 904 | }[method] 905 | 906 | matrix = np.asarray(matrix) 907 | try: 908 | inverse = fn(matrix) 909 | except np.linalg.LinAlgError as err: 910 | if raise_err: 911 | raise ValueError("The provided matrix is not invertible.") from err 912 | inverse = np.zeros_like(matrix, dtype=dtype) 913 | if out is None: 914 | return np.asarray(inverse, dtype=dtype) 915 | else: 916 | out[:] = inverse 917 | return out 918 | 919 | 920 | def mat_has_shear(matrix, epsilon=1e-7) -> bool: 921 | """ 922 | Check if a matrix has shear by checking the orthogonality of its basis vectors. 923 | 924 | Parameters 925 | ---------- 926 | matrix : ndarray, [4, 4] 927 | The matrix to check. 928 | epsilon : float, optional 929 | The floating point error margin. Default is 1e-7. 930 | 931 | Returns 932 | ------- 933 | out : bool 934 | True if the matrix has a shearing component, False otherwise. 935 | """ 936 | v1, v2, v3 = matrix[:3, :3].T 937 | for pair in ((v1, v2), (v1, v3), (v2, v3)): 938 | if np.abs(np.dot(*pair)) > epsilon: 939 | return True 940 | return False 941 | 942 | 943 | __all__ = [ 944 | name for name in globals() if name.startswith(("vec_", "mat_", "quat_", "aabb_")) 945 | ] 946 | --------------------------------------------------------------------------------