├── .pre-commit-config.yaml ├── LICENSE ├── README.md ├── package.xml ├── resource └── tf_transformations ├── setup.cfg ├── setup.py ├── test ├── test_flake8.py ├── test_pep257.py └── test_transformations.py └── tf_transformations └── __init__.py /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | - repo: https://github.com/pre-commit/pre-commit-hooks 3 | hooks: 4 | - id: end-of-file-fixer 5 | - id: trailing-whitespace 6 | - id: check-merge-conflict 7 | - id: mixed-line-ending 8 | - id: check-executables-have-shebangs 9 | - id: check-shebang-scripts-are-executable 10 | - id: detect-private-key 11 | - id: destroyed-symlinks 12 | - id: check-symlinks 13 | - id: check-case-conflict 14 | - id: check-ast 15 | - id: double-quote-string-fixer 16 | - id: requirements-txt-fixer 17 | - id: check-xml 18 | rev: v4.5.0 19 | - repo: https://github.com/codespell-project/codespell 20 | hooks: 21 | - id: codespell 22 | args: 23 | - --write-changes 24 | rev: v2.2.6 25 | - repo: https://github.com/hhatto/autopep8 26 | hooks: 27 | - id: autopep8 28 | rev: v2.1.0 29 | - repo: https://github.com/PyCQA/flake8 30 | hooks: 31 | - id: flake8 32 | rev: 7.0.0 33 | ci: 34 | autoupdate_schedule: quarterly 35 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2021, David V. Lu!! 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # tf_transformations 2 | 3 | ## Context 4 | In ROS 1, the TF library provided the helpful [`transformations.py`](https://github.com/ros/geometry/blob/noetic-devel/tf/src/tf/transformations.py) module for doing various rotation-based conversions. 5 | 6 | Quoting from [the ROS wiki page](http://wiki.ros.org/geometry2/RotationMethods) 7 | > The tf package also includes the popular transformations.py module. TransformerROS uses transformations.py to perform 8 | > conversions between quaternions and matrices. transformations.py does has[sic] useful conversion on numpy matrices; 9 | > it can convert between transformations as Euler angles, quaternions, and matrices. 10 | 11 | Since TF was supported in all distros of ROS 1, the library was widely used for all sorts of angular conversions 12 | in `rospy` code. 13 | 14 | However, ROS 2 only supports [TF2](https://wiki.ros.org/tf2) (not to be confused with [TF2](https://store.steampowered.com/app/440/Team_Fortress_2/)) and TF2 does not have an equivalent library. 15 | 16 | Quoting from TF (Tully Foote) himself on [ROS Answers](https://answers.ros.org/question/373068/ros2-foxy-tftransformationsquaternion_from_euler-equivalent/), 17 | > tf.transformations is a fork of [https://github.com/cgohlke/transformations/](https://github.com/cgohlke/transformations/). This package has been deprecated "Transformations.py is no longer actively developed and has a few known issues and numerical instabilities." 18 | 19 | The recommended alternative is a package called [`transforms3d`](https://matthew-brett.github.io/transforms3d/). This was originally only available via `pip` but is now available via `apt` and other package managers [in the `rosdistro`](https://github.com/ros/rosdistro/pull/33091). 20 | 21 | However, using that library has a few obstacles that make porting ROS 1 code to ROS 2 difficult. 22 | 1. The API is different. The new API has more consistent naming, but even then, it is not a one-to-one translation. for example, `tf.transformations.quaternion_from_euler` could be replaced with `transforms3d.euler.euler2quat`, but `tf` returns the quaternion with the ordering `x, y, z, w` and `transforms3d` returns `w, x, y, z`. 23 | 2. Not all of the functions have been migrated. As noted in the [`transforms3d` Refactoring Plan](https://matthew-brett.github.io/transforms3d/devel/refactor_plan.html), some of the functions are still in the "to integrate" pile. 24 | 25 | ## Migration 26 | If you're here, its likely because you want to migrate some code easily. You have two options: 27 | 1. Use `transforms3d` by adding a dependency on `python3-transforms3d` in your `package.xml` and noting differences in API discussed above. 28 | 2. Use this library `tf_transformations`. 29 | 30 | If you wrote the following in ROS 1, 31 | 32 | from tf.transformations import euler_from_quaternion 33 | 34 | the only change you need to your code in ROS 2 is 35 | 36 | from tf_transformations import euler_from_quaternion 37 | 38 | Note that ROS 1 uses `tf` DOT `transformations` and ROS 2 uses `tf` UNDERSCORE `transformations`. 39 | 40 | You also need to add a dependency on the `tf_transformations` in your `package.xml` 41 | 42 | ## Installation 43 | 44 | This package is available in full as a ROS binary, which you can install via 45 | 46 | sudo apt install ros-$ROSDISTRO-tf-transformations 47 | 48 | In older versions, you would need to manually install the `transforms3d` library via pip. 49 | 50 | sudo pip3 install transforms3d 51 | 52 | ## Implementation 53 | Wherever possible, this library uses `transforms3d` to do the math for each of the functions. For functions that 54 | are still on the Refactoring Plan, the original implementation is left in place. It is not ideal to have to maintain 55 | the math for those functions, given the complexity of the geometry at hand, and in the future, effort should be made 56 | to fix the problems upstream and replace the implementations in this package with the upstream versions. 57 | 58 | However, the original `transformations.py` library was remarkably stable. There have only been [two commits](https://github.com/ros/geometry/commits/noetic-devel/tf/src/tf/transformations.py) in its entire Git history (the initial commit and porting to Python3, which was a one line cosmetic change). Therefore, the hope is that this package will not need too much maintenance to correct problems. 59 | 60 | ## Special Thanks 61 | This package rests on the shoulders of giants, namely the OG author [Christoph Gohlke](http://www.lfd.uci.edu/~gohlke/) [[github]](https://github.com/cgohlke/transformations) and [Matthew Brett](https://matthew-brett.github.io/transforms3d/). 62 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | tf_transformations 5 | 1.1.0 6 | 7 | Reimplementation of the tf/transformations.py library for common Python spatial operations 8 | 9 | David V. Lu!! 10 | BSD 11 | 12 | python3-transforms3d 13 | python3-numpy 14 | 15 | ament_flake8 16 | ament_pep257 17 | python3-pytest 18 | 19 | 20 | ament_python 21 | 22 | 23 | -------------------------------------------------------------------------------- /resource/tf_transformations: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DLu/tf_transformations/07d77223288c457cfe71f361257e9fdd350c6fd9/resource/tf_transformations -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/tf_transformations 3 | 4 | [install] 5 | install_scripts=$base/lib/tf_transformations 6 | 7 | [flake8] 8 | max_line_length=120 9 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | package_name = 'tf_transformations' 4 | 5 | setup( 6 | name=package_name, 7 | version='1.1.0', 8 | packages=[package_name], 9 | data_files=[ 10 | ('share/ament_index/resource_index/packages', 11 | ['resource/' + package_name]), 12 | ('share/' + package_name, ['package.xml']), 13 | ], 14 | install_requires=['setuptools'], 15 | zip_safe=True, 16 | maintainer='David V. Lu!!', 17 | maintainer_email='davidvlu@gmail.com', 18 | description='Reimplementation of the tf/transformations.py library', 19 | license='BSD', 20 | tests_require=['pytest'], 21 | ) 22 | -------------------------------------------------------------------------------- /test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /test/test_transformations.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2006, Christoph Gohlke 2 | # Copyright (c) 2006-2009, The Regents of the University of California 3 | # Copyright (c) 2021 PickNik Robotics 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | # 9 | # * Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # * Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in the 13 | # documentation and/or other materials provided with the distribution. 14 | # * Neither the name of the copyright holders nor the names of any 15 | # contributors may be used to endorse or promote products derived 16 | # from this software without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | # POSSIBILITY OF SUCH DAMAGE. 29 | 30 | """Tests for tf_transformations.""" 31 | import math 32 | import random 33 | 34 | import numpy 35 | 36 | from tf_transformations import _AXES2TUPLE, clip_matrix, euler_from_quaternion 37 | from tf_transformations import compose_matrix, concatenate_matrices 38 | from tf_transformations import decompose_matrix, euler_from_matrix 39 | from tf_transformations import euler_matrix, identity_matrix, inverse_matrix 40 | from tf_transformations import is_same_transform, orthogonalization_matrix 41 | from tf_transformations import projection_from_matrix, projection_matrix 42 | from tf_transformations import quaternion_about_axis, quaternion_conjugate 43 | from tf_transformations import quaternion_from_euler, quaternion_from_matrix 44 | from tf_transformations import quaternion_inverse, quaternion_matrix 45 | from tf_transformations import quaternion_multiply, quaternion_slerp 46 | from tf_transformations import random_quaternion, random_rotation_matrix 47 | from tf_transformations import random_vector, rotation_matrix 48 | from tf_transformations import reflection_from_matrix, reflection_matrix 49 | from tf_transformations import rotation_from_matrix, scale_from_matrix 50 | from tf_transformations import scale_matrix, shear_from_matrix, shear_matrix 51 | from tf_transformations import superimposition_matrix, translation_matrix 52 | from tf_transformations import translation_from_matrix, unit_vector 53 | from tf_transformations import vector_norm 54 | 55 | 56 | def test_standard(): 57 | alpha, beta, gamma = 0.123, -1.234, 2.345 58 | origin, xaxis, yaxis, zaxis = (0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1) 59 | Rx = rotation_matrix(alpha, xaxis) 60 | Ry = rotation_matrix(beta, yaxis) 61 | Rz = rotation_matrix(gamma, zaxis) 62 | R = concatenate_matrices(Rx, Ry, Rz) 63 | euler = euler_from_matrix(R, 'rxyz') 64 | assert numpy.allclose([alpha, beta, gamma], euler) 65 | 66 | Re = euler_matrix(alpha, beta, gamma, 'rxyz') 67 | assert is_same_transform(R, Re) 68 | 69 | al, be, ga = euler_from_matrix(Re, 'rxyz') 70 | assert is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz')) 71 | 72 | qx = quaternion_about_axis(alpha, xaxis) 73 | qy = quaternion_about_axis(beta, yaxis) 74 | qz = quaternion_about_axis(gamma, zaxis) 75 | q = quaternion_multiply(qx, qy) 76 | q = quaternion_multiply(q, qz) 77 | Rq = quaternion_matrix(q) 78 | assert is_same_transform(R, Rq) 79 | 80 | S = scale_matrix(1.23, origin) 81 | T = translation_matrix((1, 2, 3)) 82 | Z = shear_matrix(beta, xaxis, origin, zaxis) 83 | R = random_rotation_matrix(numpy.random.rand(3)) 84 | M = concatenate_matrices(T, R, Z, S) 85 | scale, shear, angles, trans, persp = decompose_matrix(M) 86 | assert numpy.allclose(scale, 1.23) 87 | 88 | assert numpy.allclose(trans, (1, 2, 3)) 89 | 90 | assert numpy.allclose(shear, (0, math.tan(beta), 0)) 91 | 92 | assert is_same_transform(R, euler_matrix(axes='sxyz', *angles)) 93 | 94 | M1 = compose_matrix(scale, shear, angles, trans, persp) 95 | assert is_same_transform(M, M1) 96 | 97 | 98 | def test_identity_matrix(): 99 | m1 = identity_matrix() 100 | assert numpy.allclose(m1, numpy.dot(m1, m1)) 101 | assert numpy.sum(m1) == 4.0 102 | assert numpy.trace(m1) == 4.0 103 | assert numpy.allclose(m1, numpy.identity(4, dtype=numpy.float64)) 104 | 105 | 106 | def test_translation_matrix(): 107 | v = numpy.random.random(3) - 0.5 108 | assert numpy.allclose(v, translation_matrix(v)[:3, 3]) 109 | 110 | 111 | def test_translation_from_matrix(): 112 | v0 = numpy.random.random(3) - 0.5 113 | v1 = translation_from_matrix(translation_matrix(v0)) 114 | assert numpy.allclose(v0, v1) 115 | 116 | 117 | def test_reflection_matrix(): 118 | v0 = numpy.random.random(4) - 0.5 119 | v0[3] = 1.0 120 | v1 = numpy.random.random(3) - 0.5 121 | R = reflection_matrix(v0, v1) 122 | assert numpy.allclose(2., numpy.trace(R)) 123 | assert numpy.allclose(v0, numpy.dot(R, v0)) 124 | v2 = v0.copy() 125 | v2[:3] += v1 126 | v3 = v0.copy() 127 | v2[:3] -= v1 128 | assert numpy.allclose(v2, numpy.dot(R, v3)) 129 | 130 | 131 | def test_reflection_from_matrix(): 132 | v0 = numpy.random.random(3) - 0.5 133 | v1 = numpy.random.random(3) - 0.5 134 | M0 = reflection_matrix(v0, v1) 135 | point, normal = reflection_from_matrix(M0) 136 | M1 = reflection_matrix(point, normal) 137 | assert is_same_transform(M0, M1) 138 | 139 | 140 | def test_rotation_matrix(): 141 | angle = (random.random() - 0.5) * (2*math.pi) 142 | direc = numpy.random.random(3) - 0.5 143 | point = numpy.random.random(3) - 0.5 144 | R0 = rotation_matrix(angle, direc, point) 145 | R1 = rotation_matrix(angle-2*math.pi, direc, point) 146 | assert is_same_transform(R0, R1) 147 | R0 = rotation_matrix(angle, direc, point) 148 | R1 = rotation_matrix(-angle, -direc, point) 149 | assert is_same_transform(R0, R1) 150 | m1 = numpy.identity(4, numpy.float64) 151 | assert numpy.allclose(m1, rotation_matrix(math.pi*2, direc)) 152 | m2 = numpy.trace(rotation_matrix(math.pi/2, direc, point)) 153 | assert numpy.allclose(2., m2) 154 | 155 | 156 | def test_rotation_from_matrix(): 157 | angle = (random.random() - 0.5) * (2*math.pi) 158 | direc = numpy.random.random(3) - 0.5 159 | point = numpy.random.random(3) - 0.5 160 | R0 = rotation_matrix(angle, direc, point) 161 | angle, direc, point = rotation_from_matrix(R0) 162 | R1 = rotation_matrix(angle, direc, point) 163 | assert is_same_transform(R0, R1) 164 | 165 | 166 | def test_scale_matrix(): 167 | v = (numpy.random.rand(4, 5) - 0.5) * 20.0 168 | v[3] = 1.0 169 | S = scale_matrix(-1.234) 170 | assert numpy.allclose(numpy.dot(S, v)[:3], -1.234*v[:3]) 171 | factor = random.random() * 10 - 5 172 | origin = numpy.random.random(3) - 0.5 173 | direct = numpy.random.random(3) - 0.5 174 | S = scale_matrix(factor, origin) 175 | S = scale_matrix(factor, origin, direct) 176 | 177 | 178 | def test_scale_from_matrix(): 179 | factor = random.random() * 10 - 5 180 | origin = numpy.random.random(3) - 0.5 181 | direct = numpy.random.random(3) - 0.5 182 | S0 = scale_matrix(factor, origin) 183 | factor, origin, direction = scale_from_matrix(S0) 184 | S1 = scale_matrix(factor, origin, direction) 185 | assert is_same_transform(S0, S1) 186 | S0 = scale_matrix(factor, origin, direct) 187 | factor, origin, direction = scale_from_matrix(S0) 188 | S1 = scale_matrix(factor, origin, direction) 189 | assert is_same_transform(S0, S1) 190 | 191 | 192 | def test_projection_matrix(): 193 | P = projection_matrix((0, 0, 0), (1, 0, 0)) 194 | assert numpy.allclose(P[1:, 1:], numpy.identity(4)[1:, 1:]) 195 | point = numpy.random.random(3) - 0.5 196 | normal = numpy.random.random(3) - 0.5 197 | # direct = numpy.random.random(3) - 0.5 198 | persp = numpy.random.random(3) - 0.5 199 | P0 = projection_matrix(point, normal) 200 | # P1 = projection_matrix(point, normal, direction=direct) 201 | P2 = projection_matrix(point, normal, perspective=persp) 202 | P3 = projection_matrix(point, normal, perspective=persp, pseudo=True) 203 | assert is_same_transform(P2, numpy.dot(P0, P3)) 204 | P = projection_matrix((3, 0, 0), (1, 1, 0), (1, 0, 0)) 205 | v0 = (numpy.random.rand(4, 5) - 0.5) * 20.0 206 | v0[3] = 1.0 207 | v1 = numpy.dot(P, v0) 208 | assert numpy.allclose(v1[1], v0[1]) 209 | assert numpy.allclose(v1[0], 3.0-v1[1]) 210 | 211 | 212 | def test_projection_from_matrix(): 213 | point = numpy.random.random(3) - 0.5 214 | normal = numpy.random.random(3) - 0.5 215 | direct = numpy.random.random(3) - 0.5 216 | persp = numpy.random.random(3) - 0.5 217 | P0 = projection_matrix(point, normal) 218 | result = projection_from_matrix(P0) 219 | P1 = projection_matrix(*result) 220 | assert is_same_transform(P0, P1) 221 | P0 = projection_matrix(point, normal, direct) 222 | result = projection_from_matrix(P0) 223 | P1 = projection_matrix(*result) 224 | assert is_same_transform(P0, P1) 225 | P0 = projection_matrix(point, normal, perspective=persp, pseudo=False) 226 | result = projection_from_matrix(P0, pseudo=False) 227 | P1 = projection_matrix(*result) 228 | assert is_same_transform(P0, P1) 229 | P0 = projection_matrix(point, normal, perspective=persp, pseudo=True) 230 | result = projection_from_matrix(P0, pseudo=True) 231 | P1 = projection_matrix(*result) 232 | assert is_same_transform(P0, P1) 233 | 234 | 235 | def test_clip_matrix(): 236 | frustum = numpy.random.rand(6) 237 | frustum[1] += frustum[0] 238 | frustum[3] += frustum[2] 239 | frustum[5] += frustum[4] 240 | M = clip_matrix(*frustum, perspective=False) 241 | assert numpy.allclose( 242 | numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1.0]), 243 | [-1., -1., -1., 1.]) 244 | assert numpy.allclose( 245 | numpy.dot(M, [frustum[1], frustum[3], frustum[5], 1.0]), 246 | [1., 1., 1., 1.]) 247 | M = clip_matrix(*frustum, perspective=True) 248 | v = numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1.0]) 249 | assert numpy.allclose(v / v[3], [-1., -1., -1., 1.]) 250 | v = numpy.dot(M, [frustum[1], frustum[3], frustum[4], 1.0]) 251 | assert numpy.allclose(v / v[3], [1., 1., -1., 1.]) 252 | 253 | 254 | def test_shear_matrix(): 255 | angle = (random.random() - 0.5) * 4*math.pi 256 | direct = numpy.random.random(3) - 0.5 257 | point = numpy.random.random(3) - 0.5 258 | normal = numpy.cross(direct, numpy.random.random(3)) 259 | S = shear_matrix(angle, direct, point, normal) 260 | assert numpy.allclose(1.0, numpy.linalg.det(S)) 261 | 262 | 263 | def test_shear_from_matrix(): 264 | angle = (random.random() - 0.5) * 4*math.pi 265 | direct = numpy.random.random(3) - 0.5 266 | point = numpy.random.random(3) - 0.5 267 | normal = numpy.cross(direct, numpy.random.random(3)) 268 | S0 = shear_matrix(angle, direct, point, normal) 269 | angle, direct, point, normal = shear_from_matrix(S0) 270 | S1 = shear_matrix(angle, direct, point, normal) 271 | assert is_same_transform(S0, S1) 272 | 273 | 274 | def test_decompose_matrix(): 275 | T0 = translation_matrix((1, 2, 3)) 276 | scale, shear, angles, trans, persp = decompose_matrix(T0) 277 | T1 = translation_matrix(trans) 278 | assert numpy.allclose(T0, T1) 279 | S = scale_matrix(0.123) 280 | scale, shear, angles, trans, persp = decompose_matrix(S) 281 | assert scale[0] == 0.123 282 | R0 = euler_matrix(1, 2, 3) 283 | scale, shear, angles, trans, persp = decompose_matrix(R0) 284 | R1 = euler_matrix(*angles) 285 | assert numpy.allclose(R0, R1) 286 | 287 | 288 | def test_compose_matrix(): 289 | scale = numpy.random.random(3) - 0.5 290 | shear = numpy.random.random(3) - 0.5 291 | angles = (numpy.random.random(3) - 0.5) * (2*math.pi) 292 | trans = numpy.random.random(3) - 0.5 293 | persp = numpy.random.random(4) - 0.5 294 | M0 = compose_matrix(scale, shear, angles, trans, persp) 295 | result = decompose_matrix(M0) 296 | M1 = compose_matrix(*result) 297 | assert is_same_transform(M0, M1) 298 | 299 | 300 | def test_orthogonalization_matrix(): 301 | om = orthogonalization_matrix((10., 10., 10.), (90., 90., 90.)) 302 | assert numpy.allclose(om[:3, :3], numpy.identity(3, float) * 10) 303 | om = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7]) 304 | assert numpy.allclose(numpy.sum(om), 43.063229) 305 | 306 | 307 | def test_superimposition_matrix(): 308 | v0 = numpy.random.rand(3, 10) 309 | M = superimposition_matrix(v0, v0) 310 | assert numpy.allclose(M, numpy.identity(4)) 311 | R = random_rotation_matrix(numpy.random.random(3)) 312 | v0 = ((1, 0, 0), (0, 1, 0), (0, 0, 1), (1, 1, 1)) 313 | v1 = numpy.dot(R, v0) 314 | M = superimposition_matrix(v0, v1) 315 | assert numpy.allclose(v1, numpy.dot(M, v0)) 316 | v0 = (numpy.random.rand(4, 100) - 0.5) * 20.0 317 | v0[3] = 1.0 318 | v1 = numpy.dot(R, v0) 319 | M = superimposition_matrix(v0, v1) 320 | assert numpy.allclose(v1, numpy.dot(M, v0)) 321 | S = scale_matrix(random.random()) 322 | T = translation_matrix(numpy.random.random(3)-0.5) 323 | M = concatenate_matrices(T, R, S) 324 | v1 = numpy.dot(M, v0) 325 | v0[:3] += numpy.random.normal(0.0, 1e-9, 300).reshape(3, -1) 326 | M = superimposition_matrix(v0, v1, scaling=True) 327 | assert numpy.allclose(v1, numpy.dot(M, v0)) 328 | M = superimposition_matrix(v0, v1, scaling=True, usesvd=False) 329 | assert numpy.allclose(v1, numpy.dot(M, v0)) 330 | v = numpy.empty((4, 100, 3), dtype=numpy.float64) 331 | v[:, :, 0] = v0 332 | M = superimposition_matrix(v0, v1, scaling=True, usesvd=False) 333 | assert numpy.allclose(v1, numpy.dot(M, v[:, :, 0])) 334 | 335 | 336 | def test_euler_matrix(): 337 | R = euler_matrix(1, 2, 3, 'syxz') 338 | assert numpy.allclose(numpy.sum(R[0]), -1.34786452) 339 | R = euler_matrix(1, 2, 3, (0, 1, 0, 1)) 340 | assert numpy.allclose(numpy.sum(R[0]), -0.383436184) 341 | 342 | 343 | def test_euler_from_matrix(): 344 | R0 = euler_matrix(1, 2, 3, 'syxz') 345 | al, be, ga = euler_from_matrix(R0, 'syxz') 346 | R1 = euler_matrix(al, be, ga, 'syxz') 347 | assert numpy.allclose(R0, R1) 348 | angles = (4.0*math.pi) * (numpy.random.random(3) - 0.5) 349 | for axes in _AXES2TUPLE.keys(): 350 | R0 = euler_matrix(axes=axes, *angles) 351 | R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes)) 352 | assert numpy.allclose(R0, R1) 353 | 354 | 355 | def test_euler_from_quaternion(): 356 | angles = euler_from_quaternion([0.06146124, 0, 0, 0.99810947]) 357 | assert numpy.allclose(angles, [0.123, 0, 0]) 358 | 359 | 360 | def test_quaternion_from_euler(): 361 | q = quaternion_from_euler(1, 2, 3, 'ryxz') 362 | assert numpy.allclose(q, [0.310622, -0.718287, 0.444435, 0.435953]) 363 | 364 | 365 | def test_quaternion_about_axis(): 366 | q = quaternion_about_axis(0.123, (1, 0, 0)) 367 | assert numpy.allclose(q, [0.06146124, 0, 0, 0.99810947]) 368 | 369 | 370 | def test_quaternion_matrix(): 371 | R = quaternion_matrix([0.06146124, 0, 0, 0.99810947]) 372 | assert numpy.allclose(R, rotation_matrix(0.123, (1, 0, 0))) 373 | 374 | 375 | def test_quaternion_from_matrix(): 376 | R = rotation_matrix(0.123, (1, 2, 3)) 377 | q = quaternion_from_matrix(R) 378 | assert numpy.allclose(q, [0.0164262, 0.0328524, 0.0492786, 0.9981095]) 379 | 380 | 381 | def test_quaternion_multiply(): 382 | q = quaternion_multiply([1, -2, 3, 4], [-5, 6, 7, 8]) 383 | assert numpy.allclose(q, [-44, -14, 48, 28]) 384 | 385 | 386 | def test_quaternion_conjugate(): 387 | q0 = random_quaternion() 388 | q1 = quaternion_conjugate(q0) 389 | assert q1[3] == q0[3] and all(q1[:3] == -q0[:3]) 390 | 391 | 392 | def test_quaternion_inverse(): 393 | q0 = random_quaternion() 394 | q1 = quaternion_inverse(q0) 395 | assert numpy.allclose(quaternion_multiply(q0, q1), [0, 0, 0, 1]) 396 | 397 | 398 | def test_quaternion_slerp(): 399 | q0 = random_quaternion() 400 | q1 = random_quaternion() 401 | q = quaternion_slerp(q0, q1, 0.0) 402 | assert numpy.allclose(q, q0) 403 | q = quaternion_slerp(q0, q1, 1.0, 1) 404 | assert numpy.allclose(q, q1) 405 | q = quaternion_slerp(q0, q1, 0.5) 406 | angle = math.acos(numpy.dot(q0, q)) 407 | assert numpy.allclose(2.0, math.acos(numpy.dot(q0, q1)) / angle) or \ 408 | numpy.allclose(2.0, math.acos(-numpy.dot(q0, q1)) / angle) 409 | 410 | 411 | def test_random_quaternion(): 412 | q = random_quaternion() 413 | assert numpy.allclose(1.0, vector_norm(q)) 414 | q = random_quaternion(numpy.random.random(3)) 415 | assert q.shape == (4,) 416 | 417 | 418 | def test_random_rotation_matrix(): 419 | R = random_rotation_matrix() 420 | assert numpy.allclose(numpy.dot(R.T, R), numpy.identity(4)) 421 | 422 | 423 | def test_vector_norm(): 424 | v = numpy.random.random(3) 425 | n = vector_norm(v) 426 | assert numpy.allclose(n, numpy.linalg.norm(v)) 427 | v = numpy.random.rand(6, 5, 3) 428 | n = vector_norm(v, axis=-1) 429 | assert numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2))) 430 | n = vector_norm(v, axis=1) 431 | assert numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) 432 | v = numpy.random.rand(5, 4, 3) 433 | n = numpy.empty((5, 3), dtype=numpy.float64) 434 | vector_norm(v, axis=1, out=n) 435 | assert numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) 436 | assert vector_norm([]) == 0.0 437 | assert vector_norm([1.0]) == 1.0 438 | 439 | 440 | def test_unit_vector(): 441 | v0 = numpy.random.random(3) 442 | v1 = unit_vector(v0) 443 | assert numpy.allclose(v1, v0 / numpy.linalg.norm(v0)) 444 | v0 = numpy.random.rand(5, 4, 3) 445 | v1 = unit_vector(v0, axis=-1) 446 | v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2) 447 | assert numpy.allclose(v1, v2) 448 | v1 = unit_vector(v0, axis=1) 449 | v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1) 450 | assert numpy.allclose(v1, v2) 451 | v1 = numpy.empty((5, 4, 3), dtype=numpy.float64) 452 | unit_vector(v0, axis=1, out=v1) 453 | assert numpy.allclose(v1, v2) 454 | assert list(unit_vector([])) == [] 455 | assert list(unit_vector([1.0])) == [1.0] 456 | 457 | 458 | def test_random_vector(): 459 | v = random_vector(10000) 460 | assert numpy.all(v >= 0.0) and numpy.all(v < 1.0) 461 | v0 = random_vector(10) 462 | v1 = random_vector(10) 463 | assert not numpy.any(v0 == v1) 464 | 465 | 466 | def test_inverse_matrix(): 467 | M0 = random_rotation_matrix() 468 | M1 = inverse_matrix(M0.T) 469 | assert numpy.allclose(M1, numpy.linalg.inv(M0.T)) 470 | for size in range(1, 7): 471 | M0 = numpy.random.rand(size, size) 472 | M1 = inverse_matrix(M0) 473 | assert numpy.allclose(M1, numpy.linalg.inv(M0)) 474 | 475 | 476 | def test_concatenate_matrices(): 477 | M = numpy.random.rand(16).reshape((4, 4)) - 0.5 478 | assert numpy.allclose(M, concatenate_matrices(M)) 479 | assert numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T)) 480 | 481 | 482 | def test_is_same_transform(): 483 | assert is_same_transform(numpy.identity(4), numpy.identity(4)) 484 | assert not is_same_transform(numpy.identity(4), random_rotation_matrix()) 485 | -------------------------------------------------------------------------------- /tf_transformations/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | This library is a reimplementation of the tf/transformations.py library. 3 | 4 | https://github.com/ros/geometry/blob/noetic-devel/tf/src/tf/transformations.py 5 | 6 | Original author: Christoph Gohlke 7 | Laboratory for Fluorescence Dynamics, University of California, Irvine 8 | 9 | Makes use of https://matthew-brett.github.io/transforms3d/ which is also 10 | a reimplementation of the Gohlke's work, but this maintains the API. 11 | """ 12 | 13 | # Copyright (c) 2006, Christoph Gohlke 14 | # Copyright (c) 2006-2009, The Regents of the University of California 15 | # Copyright (c) 2021 PickNik Robotics 16 | # All rights reserved. 17 | # 18 | # Redistribution and use in source and binary forms, with or without 19 | # modification, are permitted provided that the following conditions are met: 20 | # 21 | # * Redistributions of source code must retain the above copyright 22 | # notice, this list of conditions and the following disclaimer. 23 | # * Redistributions in binary form must reproduce the above copyright 24 | # notice, this list of conditions and the following disclaimer in the 25 | # documentation and/or other materials provided with the distribution. 26 | # * Neither the name of the copyright holders nor the names of any 27 | # contributors may be used to endorse or promote products derived 28 | # from this software without specific prior written permission. 29 | # 30 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 31 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 32 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 33 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 34 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 35 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 36 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 37 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 38 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 39 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 40 | # POSSIBILITY OF SUCH DAMAGE. 41 | 42 | import math 43 | 44 | import numpy 45 | 46 | import transforms3d 47 | 48 | TRANSLATION_IDENTITY = [0.0, 0.0, 0.0] 49 | ROTATION_IDENTITY = numpy.identity(3, dtype=numpy.float64) 50 | ZOOM_IDENTITY = [1.0, 1.0, 1.0] 51 | SHEAR_IDENTITY = TRANSLATION_IDENTITY 52 | 53 | 54 | def identity_matrix(): 55 | """ 56 | Return 4x4 identity/unit matrix. 57 | 58 | >>> I = identity_matrix() 59 | >>> numpy.allclose(I, numpy.dot(I, I)) 60 | True 61 | >>> numpy.sum(I), numpy.trace(I) 62 | (4.0, 4.0) 63 | >>> numpy.allclose(I, numpy.identity(4, dtype=numpy.float64)) 64 | True 65 | 66 | """ 67 | return transforms3d.affines.compose(TRANSLATION_IDENTITY, 68 | ROTATION_IDENTITY, 69 | ZOOM_IDENTITY) 70 | 71 | 72 | def translation_matrix(direction): 73 | """ 74 | Return matrix to translate by direction vector. 75 | 76 | >>> v = numpy.random.random(3) - 0.5 77 | >>> numpy.allclose(v, translation_matrix(v)[:3, 3]) 78 | True 79 | 80 | """ 81 | return transforms3d.affines.compose(direction, 82 | ROTATION_IDENTITY, 83 | ZOOM_IDENTITY) 84 | 85 | 86 | def translation_from_matrix(matrix): 87 | """ 88 | Return translation vector from translation matrix. 89 | 90 | >>> v0 = numpy.random.random(3) - 0.5 91 | >>> v1 = translation_from_matrix(translation_matrix(v0)) 92 | >>> numpy.allclose(v0, v1) 93 | True 94 | 95 | """ 96 | return transforms3d.affines.decompose(matrix)[0] 97 | 98 | 99 | def reflection_matrix(point, normal): 100 | """ 101 | Return matrix to mirror at plane defined by point and normal vector. 102 | 103 | >>> v0 = numpy.random.random(4) - 0.5 104 | >>> v0[3] = 1.0 105 | >>> v1 = numpy.random.random(3) - 0.5 106 | >>> R = reflection_matrix(v0, v1) 107 | >>> numpy.allclose(2., numpy.trace(R)) 108 | True 109 | >>> numpy.allclose(v0, numpy.dot(R, v0)) 110 | True 111 | >>> v2 = v0.copy() 112 | >>> v2[:3] += v1 113 | >>> v3 = v0.copy() 114 | >>> v2[:3] -= v1 115 | >>> numpy.allclose(v2, numpy.dot(R, v3)) 116 | True 117 | 118 | """ 119 | # Note: point[3] is ignored 120 | return transforms3d.reflections.rfnorm2aff(normal, point[:3]) 121 | 122 | 123 | def reflection_from_matrix(matrix): 124 | """ 125 | Return mirror plane point and normal vector from reflection matrix. 126 | 127 | >>> v0 = numpy.random.random(3) - 0.5 128 | >>> v1 = numpy.random.random(3) - 0.5 129 | >>> M0 = reflection_matrix(v0, v1) 130 | >>> point, normal = reflection_from_matrix(M0) 131 | >>> M1 = reflection_matrix(point, normal) 132 | >>> is_same_transform(M0, M1) 133 | True 134 | 135 | """ 136 | normal, point = transforms3d.reflections.aff2rfnorm(matrix) 137 | return point, normal 138 | 139 | 140 | def rotation_matrix(angle, direction, point=None): 141 | """ 142 | Return matrix to rotate about axis defined by point and direction. 143 | 144 | >>> angle = (random.random() - 0.5) * (2*math.pi) 145 | >>> direc = numpy.random.random(3) - 0.5 146 | >>> point = numpy.random.random(3) - 0.5 147 | >>> R0 = rotation_matrix(angle, direc, point) 148 | >>> R1 = rotation_matrix(angle-2*math.pi, direc, point) 149 | >>> is_same_transform(R0, R1) 150 | True 151 | >>> R0 = rotation_matrix(angle, direc, point) 152 | >>> R1 = rotation_matrix(-angle, -direc, point) 153 | >>> is_same_transform(R0, R1) 154 | True 155 | >>> I = numpy.identity(4, numpy.float64) 156 | >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc)) 157 | True 158 | >>> numpy.allclose(2., numpy.trace(rotation_matrix(math.pi/2, 159 | ... direc, point))) 160 | True 161 | 162 | """ 163 | return transforms3d.axangles.axangle2aff(direction, angle, point=point) 164 | 165 | 166 | def rotation_from_matrix(matrix): 167 | """ 168 | Return rotation angle and axis from rotation matrix. 169 | 170 | >>> angle = (random.random() - 0.5) * (2*math.pi) 171 | >>> direc = numpy.random.random(3) - 0.5 172 | >>> point = numpy.random.random(3) - 0.5 173 | >>> R0 = rotation_matrix(angle, direc, point) 174 | >>> angle, direc, point = rotation_from_matrix(R0) 175 | >>> R1 = rotation_matrix(angle, direc, point) 176 | >>> is_same_transform(R0, R1) 177 | True 178 | 179 | """ 180 | direction, angle, point = transforms3d.axangles.aff2axangle(matrix) 181 | return angle, direction, point 182 | 183 | 184 | def scale_matrix(factor, origin=None, direction=None): 185 | """ 186 | Return matrix to scale by factor around origin in direction. 187 | 188 | Use factor -1 for point symmetry. 189 | 190 | >>> v = (numpy.random.rand(4, 5) - 0.5) * 20.0 191 | >>> v[3] = 1.0 192 | >>> S = scale_matrix(-1.234) 193 | >>> numpy.allclose(numpy.dot(S, v)[:3], -1.234*v[:3]) 194 | True 195 | >>> factor = random.random() * 10 - 5 196 | >>> origin = numpy.random.random(3) - 0.5 197 | >>> direct = numpy.random.random(3) - 0.5 198 | >>> S = scale_matrix(factor, origin) 199 | >>> S = scale_matrix(factor, origin, direct) 200 | 201 | """ 202 | return transforms3d.zooms.zfdir2aff(factor, 203 | direction=direction, 204 | origin=origin) 205 | 206 | 207 | def scale_from_matrix(matrix): 208 | """ 209 | Return scaling factor, origin and direction from scaling matrix. 210 | 211 | >>> factor = random.random() * 10 - 5 212 | >>> origin = numpy.random.random(3) - 0.5 213 | >>> direct = numpy.random.random(3) - 0.5 214 | >>> S0 = scale_matrix(factor, origin) 215 | >>> factor, origin, direction = scale_from_matrix(S0) 216 | >>> S1 = scale_matrix(factor, origin, direction) 217 | >>> is_same_transform(S0, S1) 218 | True 219 | >>> S0 = scale_matrix(factor, origin, direct) 220 | >>> factor, origin, direction = scale_from_matrix(S0) 221 | >>> S1 = scale_matrix(factor, origin, direction) 222 | >>> is_same_transform(S0, S1) 223 | True 224 | 225 | """ 226 | factor, direction, origin = transforms3d.zooms.aff2zfdir(matrix) 227 | return factor, origin, direction 228 | 229 | 230 | def projection_matrix(point, normal, direction=None, 231 | perspective=None, pseudo=False): 232 | """ 233 | Return matrix to project onto plane defined by point and normal. 234 | 235 | Using either perspective point, projection direction, or none of both. 236 | 237 | If pseudo is True, perspective projections will preserve relative depth 238 | such that Perspective = dot(Orthogonal, PseudoPerspective). 239 | 240 | >>> P = projection_matrix((0, 0, 0), (1, 0, 0)) 241 | >>> numpy.allclose(P[1:, 1:], numpy.identity(4)[1:, 1:]) 242 | True 243 | >>> point = numpy.random.random(3) - 0.5 244 | >>> normal = numpy.random.random(3) - 0.5 245 | >>> direct = numpy.random.random(3) - 0.5 246 | >>> persp = numpy.random.random(3) - 0.5 247 | >>> P0 = projection_matrix(point, normal) 248 | >>> P1 = projection_matrix(point, normal, direction=direct) 249 | >>> P2 = projection_matrix(point, normal, perspective=persp) 250 | >>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True) 251 | >>> is_same_transform(P2, numpy.dot(P0, P3)) 252 | True 253 | >>> P = projection_matrix((3, 0, 0), (1, 1, 0), (1, 0, 0)) 254 | >>> v0 = (numpy.random.rand(4, 5) - 0.5) * 20.0 255 | >>> v0[3] = 1.0 256 | >>> v1 = numpy.dot(P, v0) 257 | >>> numpy.allclose(v1[1], v0[1]) 258 | True 259 | >>> numpy.allclose(v1[0], 3.0-v1[1]) 260 | True 261 | 262 | """ 263 | M = numpy.identity(4) 264 | point = numpy.array(point[:3], dtype=numpy.float64, copy=False) 265 | normal = unit_vector(normal[:3]) 266 | if perspective is not None: 267 | # perspective projection 268 | perspective = numpy.array(perspective[:3], dtype=numpy.float64, 269 | copy=False) 270 | M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective-point, normal) 271 | M[:3, :3] -= numpy.outer(perspective, normal) 272 | if pseudo: 273 | # preserve relative depth 274 | M[:3, :3] -= numpy.outer(normal, normal) 275 | M[:3, 3] = numpy.dot(point, normal) * (perspective+normal) 276 | else: 277 | M[:3, 3] = numpy.dot(point, normal) * perspective 278 | M[3, :3] = -normal 279 | M[3, 3] = numpy.dot(perspective, normal) 280 | elif direction is not None: 281 | # parallel projection 282 | direction = numpy.array(direction[:3], dtype=numpy.float64, copy=False) 283 | scale = numpy.dot(direction, normal) 284 | M[:3, :3] -= numpy.outer(direction, normal) / scale 285 | M[:3, 3] = direction * (numpy.dot(point, normal) / scale) 286 | else: 287 | # orthogonal projection 288 | M[:3, :3] -= numpy.outer(normal, normal) 289 | M[:3, 3] = numpy.dot(point, normal) * normal 290 | return M 291 | 292 | 293 | def projection_from_matrix(matrix, pseudo=False): 294 | """ 295 | Return projection plane and perspective point from projection matrix. 296 | 297 | Return values are same as arguments for projection_matrix function: 298 | point, normal, direction, perspective, and pseudo. 299 | 300 | >>> point = numpy.random.random(3) - 0.5 301 | >>> normal = numpy.random.random(3) - 0.5 302 | >>> direct = numpy.random.random(3) - 0.5 303 | >>> persp = numpy.random.random(3) - 0.5 304 | >>> P0 = projection_matrix(point, normal) 305 | >>> result = projection_from_matrix(P0) 306 | >>> P1 = projection_matrix(*result) 307 | >>> is_same_transform(P0, P1) 308 | True 309 | >>> P0 = projection_matrix(point, normal, direct) 310 | >>> result = projection_from_matrix(P0) 311 | >>> P1 = projection_matrix(*result) 312 | >>> is_same_transform(P0, P1) 313 | True 314 | >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False) 315 | >>> result = projection_from_matrix(P0, pseudo=False) 316 | >>> P1 = projection_matrix(*result) 317 | >>> is_same_transform(P0, P1) 318 | True 319 | >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True) 320 | >>> result = projection_from_matrix(P0, pseudo=True) 321 | >>> P1 = projection_matrix(*result) 322 | >>> is_same_transform(P0, P1) 323 | True 324 | 325 | """ 326 | M = numpy.array(matrix, dtype=numpy.float64, copy=False) 327 | M33 = M[:3, :3] 328 | l, V = numpy.linalg.eig(M) 329 | i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] 330 | if not pseudo and len(i): 331 | # point: any eigenvector corresponding to eigenvalue 1 332 | point = numpy.real(V[:, i[-1]]).squeeze() 333 | point /= point[3] 334 | # direction: unit eigenvector corresponding to eigenvalue 0 335 | l, V = numpy.linalg.eig(M33) 336 | i = numpy.where(abs(numpy.real(l)) < 1e-8)[0] 337 | if not len(i): 338 | raise ValueError('no eigenvector corresponding to eigenvalue 0') 339 | direction = numpy.real(V[:, i[0]]).squeeze() 340 | direction /= vector_norm(direction) 341 | # normal: unit eigenvector of M33.T corresponding to eigenvalue 0 342 | l, V = numpy.linalg.eig(M33.T) 343 | i = numpy.where(abs(numpy.real(l)) < 1e-8)[0] 344 | if len(i): 345 | # parallel projection 346 | normal = numpy.real(V[:, i[0]]).squeeze() 347 | normal /= vector_norm(normal) 348 | return point, normal, direction, None, False 349 | else: 350 | # orthogonal projection, where normal equals direction vector 351 | return point, direction, None, None, False 352 | else: 353 | # perspective projection 354 | i = numpy.where(abs(numpy.real(l)) > 1e-8)[0] 355 | if not len(i): 356 | raise ValueError( 357 | 'no eigenvector not corresponding to eigenvalue 0') 358 | point = numpy.real(V[:, i[-1]]).squeeze() 359 | point /= point[3] 360 | normal = - M[3, :3] 361 | perspective = M[:3, 3] / numpy.dot(point[:3], normal) 362 | if pseudo: 363 | perspective -= normal 364 | return point, normal, None, perspective, pseudo 365 | 366 | 367 | def clip_matrix(left, right, bottom, top, near, far, perspective=False): 368 | """ 369 | Return matrix to obtain normalized device coordinates from frustum. 370 | 371 | The frustum bounds are axis-aligned along x (left, right), 372 | y (bottom, top) and z (near, far). 373 | 374 | Normalized device coordinates are in range [-1, 1] if coordinates are 375 | inside the frustum. 376 | 377 | If perspective is True the frustum is a truncated pyramid with the 378 | perspective point at origin and direction along z axis, otherwise an 379 | orthographic canonical view volume (a box). 380 | 381 | Homogeneous coordinates transformed by the perspective clip matrix 382 | need to be dehomogenized (divided by w coordinate). 383 | 384 | >>> frustum = numpy.random.rand(6) 385 | >>> frustum[1] += frustum[0] 386 | >>> frustum[3] += frustum[2] 387 | >>> frustum[5] += frustum[4] 388 | >>> M = clip_matrix(*frustum, perspective=False) 389 | >>> numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1.0]) 390 | array([-1., -1., -1., 1.]) 391 | >>> numpy.dot(M, [frustum[1], frustum[3], frustum[5], 1.0]) 392 | array([ 1., 1., 1., 1.]) 393 | >>> M = clip_matrix(*frustum, perspective=True) 394 | >>> v = numpy.dot(M, [frustum[0], frustum[2], frustum[4], 1.0]) 395 | >>> v / v[3] 396 | array([-1., -1., -1., 1.]) 397 | >>> v = numpy.dot(M, [frustum[1], frustum[3], frustum[4], 1.0]) 398 | >>> v / v[3] 399 | array([ 1., 1., -1., 1.]) 400 | 401 | """ 402 | if left >= right or bottom >= top or near >= far: 403 | raise ValueError('invalid frustrum') 404 | if perspective: 405 | if near <= _EPS: 406 | raise ValueError('invalid frustum: near <= 0') 407 | t = 2.0 * near 408 | M = ((-t/(right-left), 0.0, (right+left)/(right-left), 0.0), 409 | (0.0, -t/(top-bottom), (top+bottom)/(top-bottom), 0.0), 410 | (0.0, 0.0, -(far+near)/(far-near), t*far/(far-near)), 411 | (0.0, 0.0, -1.0, 0.0)) 412 | else: 413 | M = ((2.0/(right-left), 0.0, 0.0, (right+left)/(left-right)), 414 | (0.0, 2.0/(top-bottom), 0.0, (top+bottom)/(bottom-top)), 415 | (0.0, 0.0, 2.0/(far-near), (far+near)/(near-far)), 416 | (0.0, 0.0, 0.0, 1.0)) 417 | return numpy.array(M, dtype=numpy.float64) 418 | 419 | 420 | def shear_matrix(angle, direction, point, normal): 421 | """ 422 | Return matrix to shear by angle along direction vector on shear plane. 423 | 424 | The shear plane is defined by a point and normal vector. The direction 425 | vector must be orthogonal to the plane's normal vector. 426 | 427 | A point P is transformed by the shear matrix into P" such that 428 | the vector P-P" is parallel to the direction vector and its extent is 429 | given by the angle of P-P'-P", where P' is the orthogonal projection 430 | of P onto the shear plane. 431 | 432 | >>> angle = (random.random() - 0.5) * 4*math.pi 433 | >>> direct = numpy.random.random(3) - 0.5 434 | >>> point = numpy.random.random(3) - 0.5 435 | >>> normal = numpy.cross(direct, numpy.random.random(3)) 436 | >>> S = shear_matrix(angle, direct, point, normal) 437 | >>> numpy.allclose(1.0, numpy.linalg.det(S)) 438 | True 439 | 440 | """ 441 | return transforms3d.shears.sadn2aff(angle, direction, normal, point) 442 | 443 | 444 | def shear_from_matrix(matrix): 445 | """ 446 | Return shear angle, direction and plane from shear matrix. 447 | 448 | >>> angle = (random.random() - 0.5) * 4*math.pi 449 | >>> direct = numpy.random.random(3) - 0.5 450 | >>> point = numpy.random.random(3) - 0.5 451 | >>> normal = numpy.cross(direct, numpy.random.random(3)) 452 | >>> S0 = shear_matrix(angle, direct, point, normal) 453 | >>> angle, direct, point, normal = shear_from_matrix(S0) 454 | >>> S1 = shear_matrix(angle, direct, point, normal) 455 | >>> is_same_transform(S0, S1) 456 | True 457 | 458 | """ 459 | angle, direction, normal, point = transforms3d.shears.aff2sadn(matrix) 460 | return angle, direction, point, normal 461 | 462 | 463 | def decompose_matrix(matrix): 464 | """ 465 | Return sequence of transformations from transformation matrix. 466 | 467 | matrix : array_like 468 | Non-degenerative homogeneous transformation matrix 469 | 470 | Return tuple of: 471 | scale : vector of 3 scaling factors 472 | shear : list of shear factors for x-y, x-z, y-z axes 473 | angles : list of Euler angles about static x, y, z axes 474 | translate : translation vector along x, y, z axes 475 | perspective : perspective partition of matrix 476 | 477 | Raise ValueError if matrix is of wrong type or degenerative. 478 | 479 | >>> T0 = translation_matrix((1, 2, 3)) 480 | >>> scale, shear, angles, trans, persp = decompose_matrix(T0) 481 | >>> T1 = translation_matrix(trans) 482 | >>> numpy.allclose(T0, T1) 483 | True 484 | >>> S = scale_matrix(0.123) 485 | >>> scale, shear, angles, trans, persp = decompose_matrix(S) 486 | >>> scale[0] 487 | 0.123 488 | >>> R0 = euler_matrix(1, 2, 3) 489 | >>> scale, shear, angles, trans, persp = decompose_matrix(R0) 490 | >>> R1 = euler_matrix(*angles) 491 | >>> numpy.allclose(R0, R1) 492 | True 493 | 494 | """ 495 | T, R, Z, S = transforms3d.affines.decompose(matrix) 496 | angles = euler_from_matrix(R) 497 | # TODO: Fill in the perspective partition of matrix 498 | return Z, S, angles, T, None 499 | 500 | 501 | def compose_matrix(scale=None, shear=None, angles=None, translate=None, 502 | perspective=None): 503 | """ 504 | Return transformation matrix from sequence of transformations. 505 | 506 | This is the inverse of the decompose_matrix function. 507 | 508 | Sequence of transformations: 509 | scale : vector of 3 scaling factors 510 | shear : list of shear factors for x-y, x-z, y-z axes 511 | angles : list of Euler angles about static x, y, z axes 512 | translate : translation vector along x, y, z axes 513 | perspective : perspective partition of matrix 514 | 515 | >>> scale = numpy.random.random(3) - 0.5 516 | >>> shear = numpy.random.random(3) - 0.5 517 | >>> angles = (numpy.random.random(3) - 0.5) * (2*math.pi) 518 | >>> trans = numpy.random.random(3) - 0.5 519 | >>> persp = numpy.random.random(4) - 0.5 520 | >>> M0 = compose_matrix(scale, shear, angles, trans, persp) 521 | >>> result = decompose_matrix(M0) 522 | >>> M1 = compose_matrix(*result) 523 | >>> is_same_transform(M0, M1) 524 | True 525 | 526 | """ 527 | T = translate if translate is not None else TRANSLATION_IDENTITY 528 | if angles is not None: 529 | R = transforms3d.euler.euler2mat(*angles) 530 | else: 531 | R = ROTATION_IDENTITY 532 | Z = scale if scale is not None else ZOOM_IDENTITY 533 | S = shear if shear is not None else SHEAR_IDENTITY 534 | M = transforms3d.affines.compose(T, R, Z, S) 535 | 536 | # TODO: Include perspective in composition 537 | return M 538 | 539 | 540 | def orthogonalization_matrix(lengths, angles): 541 | """ 542 | Return orthogonalization matrix for crystallographic cell coordinates. 543 | 544 | Angles are expected in degrees. 545 | 546 | The de-orthogonalization matrix is the inverse. 547 | 548 | >>> O = orthogonalization_matrix((10., 10., 10.), (90., 90., 90.)) 549 | >>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10) 550 | True 551 | >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7]) 552 | >>> numpy.allclose(numpy.sum(O), 43.063229) 553 | True 554 | 555 | """ 556 | a, b, c = lengths 557 | angles = numpy.radians(angles) 558 | sina, sinb, _ = numpy.sin(angles) 559 | cosa, cosb, cosg = numpy.cos(angles) 560 | co = (cosa * cosb - cosg) / (sina * sinb) 561 | return numpy.array(( 562 | (a*sinb*math.sqrt(1.0-co*co), 0.0, 0.0, 0.0), 563 | (-a*sinb*co, b*sina, 0.0, 0.0), 564 | (a*cosb, b*cosa, c, 0.0), 565 | (0.0, 0.0, 0.0, 1.0)), 566 | dtype=numpy.float64) 567 | 568 | 569 | def superimposition_matrix(v0, v1, scaling=False, usesvd=True): 570 | """ 571 | Return matrix to transform given vector set into second vector set. 572 | 573 | v0 and v1 are shape (3, *) or (4, *) arrays of at least 3 vectors. 574 | 575 | If usesvd is True, the weighted sum of squared deviations (RMSD) is 576 | minimized according to the algorithm by W. Kabsch [8]. Otherwise the 577 | quaternion based algorithm by B. Horn [9] is used (slower when using 578 | this Python implementation). 579 | 580 | The returned matrix performs rotation, translation and uniform scaling 581 | (if specified). 582 | 583 | >>> v0 = numpy.random.rand(3, 10) 584 | >>> M = superimposition_matrix(v0, v0) 585 | >>> numpy.allclose(M, numpy.identity(4)) 586 | True 587 | >>> R = random_rotation_matrix(numpy.random.random(3)) 588 | >>> v0 = ((1,0,0), (0,1,0), (0,0,1), (1,1,1)) 589 | >>> v1 = numpy.dot(R, v0) 590 | >>> M = superimposition_matrix(v0, v1) 591 | >>> numpy.allclose(v1, numpy.dot(M, v0)) 592 | True 593 | >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20.0 594 | >>> v0[3] = 1.0 595 | >>> v1 = numpy.dot(R, v0) 596 | >>> M = superimposition_matrix(v0, v1) 597 | >>> numpy.allclose(v1, numpy.dot(M, v0)) 598 | True 599 | >>> S = scale_matrix(random.random()) 600 | >>> T = translation_matrix(numpy.random.random(3)-0.5) 601 | >>> M = concatenate_matrices(T, R, S) 602 | >>> v1 = numpy.dot(M, v0) 603 | >>> v0[:3] += numpy.random.normal(0.0, 1e-9, 300).reshape(3, -1) 604 | >>> M = superimposition_matrix(v0, v1, scaling=True) 605 | >>> numpy.allclose(v1, numpy.dot(M, v0)) 606 | True 607 | >>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False) 608 | >>> numpy.allclose(v1, numpy.dot(M, v0)) 609 | True 610 | >>> v = numpy.empty((4, 100, 3), dtype=numpy.float64) 611 | >>> v[:, :, 0] = v0 612 | >>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False) 613 | >>> numpy.allclose(v1, numpy.dot(M, v[:, :, 0])) 614 | True 615 | 616 | """ 617 | v0 = numpy.array(v0, dtype=numpy.float64, copy=False)[:3] 618 | v1 = numpy.array(v1, dtype=numpy.float64, copy=False)[:3] 619 | 620 | if v0.shape != v1.shape or v0.shape[1] < 3: 621 | raise ValueError('Vector sets are of wrong shape or type.') 622 | 623 | # move centroids to origin 624 | t0 = numpy.mean(v0, axis=1) 625 | t1 = numpy.mean(v1, axis=1) 626 | v0 = v0 - t0.reshape(3, 1) 627 | v1 = v1 - t1.reshape(3, 1) 628 | 629 | if usesvd: 630 | # Singular Value Decomposition of covariance matrix 631 | u, s, vh = numpy.linalg.svd(numpy.dot(v1, v0.T)) 632 | # rotation matrix from SVD orthonormal bases 633 | R = numpy.dot(u, vh) 634 | if numpy.linalg.det(R) < 0.0: 635 | # R does not constitute right handed system 636 | R -= numpy.outer(u[:, 2], vh[2, :]*2.0) 637 | s[-1] *= -1.0 638 | # homogeneous transformation matrix 639 | M = numpy.identity(4) 640 | M[:3, :3] = R 641 | else: 642 | # compute symmetric matrix N 643 | xx, yy, zz = numpy.sum(v0 * v1, axis=1) 644 | xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1) 645 | xz, yx, zy = numpy.sum(v0 * numpy.roll(v1, -2, axis=0), axis=1) 646 | N = ((xx+yy+zz, yz-zy, zx-xz, xy-yx), 647 | (yz-zy, xx-yy-zz, xy+yx, zx+xz), 648 | (zx-xz, xy+yx, -xx+yy-zz, yz+zy), 649 | (xy-yx, zx+xz, yz+zy, -xx-yy+zz)) 650 | # quaternion: eigenvector corresponding to most positive eigenvalue 651 | l, V = numpy.linalg.eig(N) 652 | q = V[:, numpy.argmax(l)] 653 | q /= vector_norm(q) # unit quaternion 654 | q = numpy.roll(q, -1) # move w component to end 655 | # homogeneous transformation matrix 656 | M = quaternion_matrix(q) 657 | 658 | # scale: ratio of rms deviations from centroid 659 | if scaling: 660 | v0 *= v0 661 | v1 *= v1 662 | M[:3, :3] *= math.sqrt(numpy.sum(v1) / numpy.sum(v0)) 663 | 664 | # translation 665 | M[:3, 3] = t1 666 | T = numpy.identity(4) 667 | T[:3, 3] = -t0 668 | M = numpy.dot(M, T) 669 | return M 670 | 671 | 672 | def euler_matrix(ai, aj, ak, axes='sxyz'): 673 | """ 674 | Return homogeneous rotation matrix from Euler angles and axis sequence. 675 | 676 | ai, aj, ak : Euler's roll, pitch and yaw angles 677 | axes : One of 24 axis sequences as string or encoded tuple 678 | 679 | >>> R = euler_matrix(1, 2, 3, 'syxz') 680 | >>> numpy.allclose(numpy.sum(R[0]), -1.34786452) 681 | True 682 | >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1)) 683 | >>> numpy.allclose(numpy.sum(R[0]), -0.383436184) 684 | True 685 | >>> ai, aj, ak = (4.0*math.pi) * (numpy.random.random(3) - 0.5) 686 | >>> for axes in _AXES2TUPLE.keys(): 687 | ... R = euler_matrix(ai, aj, ak, axes) 688 | >>> for axes in _TUPLE2AXES.keys(): 689 | ... R = euler_matrix(ai, aj, ak, axes) 690 | 691 | """ 692 | rotation_matrix = transforms3d.euler.euler2mat(ai, aj, ak, axes=axes) 693 | return transforms3d.affines.compose(TRANSLATION_IDENTITY, 694 | rotation_matrix, 695 | ZOOM_IDENTITY) 696 | 697 | 698 | def euler_from_matrix(matrix, axes='sxyz'): 699 | """ 700 | Return Euler angles from rotation matrix for specified axis sequence. 701 | 702 | axes : One of 24 axis sequences as string or encoded tuple 703 | 704 | Note that many Euler angle triplets can describe one matrix. 705 | 706 | >>> R0 = euler_matrix(1, 2, 3, 'syxz') 707 | >>> al, be, ga = euler_from_matrix(R0, 'syxz') 708 | >>> R1 = euler_matrix(al, be, ga, 'syxz') 709 | >>> numpy.allclose(R0, R1) 710 | True 711 | >>> angles = (4.0*math.pi) * (numpy.random.random(3) - 0.5) 712 | >>> for axes in _AXES2TUPLE.keys(): 713 | ... R0 = euler_matrix(axes=axes, *angles) 714 | ... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes)) 715 | ... if not numpy.allclose(R0, R1): print axes, "failed" 716 | 717 | """ 718 | return transforms3d.euler.mat2euler(matrix, axes=axes) 719 | 720 | 721 | def euler_from_quaternion(quaternion, axes='sxyz'): 722 | """ 723 | Return Euler angles from quaternion for specified axis sequence. 724 | 725 | >>> angles = euler_from_quaternion([0.06146124, 0, 0, 0.99810947]) 726 | >>> numpy.allclose(angles, [0.123, 0, 0]) 727 | True 728 | 729 | """ 730 | return euler_from_matrix(quaternion_matrix(quaternion), axes) 731 | 732 | 733 | def _reorder_input_quaternion(quaternion): 734 | """Reorder quaternion to have w term first.""" 735 | x, y, z, w = quaternion 736 | return w, x, y, z 737 | 738 | 739 | def _reorder_output_quaternion(quaternion): 740 | """Reorder quaternion to have w term last.""" 741 | w, x, y, z = quaternion 742 | return x, y, z, w 743 | 744 | 745 | def quaternion_from_euler(ai, aj, ak, axes='sxyz'): 746 | """ 747 | Return quaternion from Euler angles and axis sequence. 748 | 749 | ai, aj, ak : Euler's roll, pitch and yaw angles 750 | axes : One of 24 axis sequences as string or encoded tuple 751 | 752 | >>> q = quaternion_from_euler(1, 2, 3, 'ryxz') 753 | >>> numpy.allclose(q, [0.310622, -0.718287, 0.444435, 0.435953]) 754 | True 755 | 756 | """ 757 | return _reorder_output_quaternion( 758 | transforms3d.euler.euler2quat(ai, aj, ak, axes=axes) 759 | ) 760 | 761 | 762 | def quaternion_about_axis(angle, axis): 763 | """ 764 | Return quaternion for rotation about axis. 765 | 766 | >>> q = quaternion_about_axis(0.123, (1, 0, 0)) 767 | >>> numpy.allclose(q, [0.06146124, 0, 0, 0.99810947]) 768 | True 769 | 770 | """ 771 | return _reorder_output_quaternion( 772 | transforms3d.quaternions.axangle2quat(axis, angle) 773 | ) 774 | 775 | 776 | def quaternion_matrix(quaternion): 777 | """ 778 | Return 4x4 homogeneous rotation matrix from quaternion. 779 | 780 | >>> R = quaternion_matrix([0.06146124, 0, 0, 0.99810947]) 781 | >>> numpy.allclose(R, rotation_matrix(0.123, (1, 0, 0))) 782 | True 783 | 784 | """ 785 | rotation_matrix = transforms3d.quaternions.quat2mat( 786 | _reorder_input_quaternion(quaternion) 787 | ) 788 | return transforms3d.affines.compose(TRANSLATION_IDENTITY, 789 | rotation_matrix, 790 | ZOOM_IDENTITY) 791 | 792 | 793 | def quaternion_from_matrix(matrix): 794 | """ 795 | Return quaternion from rotation matrix. 796 | 797 | >>> R = rotation_matrix(0.123, (1, 2, 3)) 798 | >>> q = quaternion_from_matrix(R) 799 | >>> numpy.allclose(q, [0.0164262, 0.0328524, 0.0492786, 0.9981095]) 800 | True 801 | 802 | """ 803 | rotation_matrix = transforms3d.affines.decompose(matrix)[1] 804 | return _reorder_output_quaternion( 805 | transforms3d.quaternions.mat2quat(rotation_matrix) 806 | ) 807 | 808 | 809 | def quaternion_multiply(quaternion1, quaternion0): 810 | """ 811 | Return multiplication of two quaternions. 812 | 813 | >>> q = quaternion_multiply([1, -2, 3, 4], [-5, 6, 7, 8]) 814 | >>> numpy.allclose(q, [-44, -14, 48, 28]) 815 | True 816 | 817 | """ 818 | x0, y0, z0, w0 = quaternion0 819 | x1, y1, z1, w1 = quaternion1 820 | w2, x2, y2, z2 = transforms3d.quaternions.qmult([w1, x1, y1, z1], 821 | [w0, x0, y0, z0]) 822 | return x2, y2, z2, w2 823 | 824 | 825 | def quaternion_conjugate(quaternion): 826 | """ 827 | Return conjugate of quaternion. 828 | 829 | >>> q0 = random_quaternion() 830 | >>> q1 = quaternion_conjugate(q0) 831 | >>> q1[3] == q0[3] and all(q1[:3] == -q0[:3]) 832 | True 833 | 834 | """ 835 | return numpy.array((-quaternion[0], -quaternion[1], 836 | -quaternion[2], quaternion[3]), dtype=numpy.float64) 837 | 838 | 839 | def quaternion_inverse(quaternion): 840 | """ 841 | Return inverse of quaternion. 842 | 843 | >>> q0 = random_quaternion() 844 | >>> q1 = quaternion_inverse(q0) 845 | >>> numpy.allclose(quaternion_multiply(q0, q1), [0, 0, 0, 1]) 846 | True 847 | 848 | """ 849 | return quaternion_conjugate(quaternion) / numpy.dot(quaternion, quaternion) 850 | 851 | 852 | def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True): 853 | """ 854 | Return spherical linear interpolation between two quaternions. 855 | 856 | >>> q0 = random_quaternion() 857 | >>> q1 = random_quaternion() 858 | >>> q = quaternion_slerp(q0, q1, 0.0) 859 | >>> numpy.allclose(q, q0) 860 | True 861 | >>> q = quaternion_slerp(q0, q1, 1.0, 1) 862 | >>> numpy.allclose(q, q1) 863 | True 864 | >>> q = quaternion_slerp(q0, q1, 0.5) 865 | >>> angle = math.acos(numpy.dot(q0, q)) 866 | >>> numpy.allclose(2.0, math.acos(numpy.dot(q0, q1)) / angle) or 867 | numpy.allclose(2.0, math.acos(-numpy.dot(q0, q1)) / angle) 868 | True 869 | 870 | """ 871 | q0 = unit_vector(quat0[:4]) 872 | q1 = unit_vector(quat1[:4]) 873 | if fraction == 0.0: 874 | return q0 875 | elif fraction == 1.0: 876 | return q1 877 | d = numpy.dot(q0, q1) 878 | if abs(abs(d) - 1.0) < _EPS: 879 | return q0 880 | if shortestpath and d < 0.0: 881 | # invert rotation 882 | d = -d 883 | q1 *= -1.0 884 | angle = math.acos(d) + spin * math.pi 885 | if abs(angle) < _EPS: 886 | return q0 887 | isin = 1.0 / math.sin(angle) 888 | q0 *= math.sin((1.0 - fraction) * angle) * isin 889 | q1 *= math.sin(fraction * angle) * isin 890 | q0 += q1 891 | return q0 892 | 893 | 894 | def random_quaternion(rand=None): 895 | """ 896 | Return uniform random unit quaternion. 897 | 898 | rand: array like or None 899 | Three independent random variables that are uniformly distributed 900 | between 0 and 1. 901 | 902 | >>> q = random_quaternion() 903 | >>> numpy.allclose(1.0, vector_norm(q)) 904 | True 905 | >>> q = random_quaternion(numpy.random.random(3)) 906 | >>> q.shape 907 | (4,) 908 | 909 | """ 910 | if rand is None: 911 | rand = numpy.random.rand(3) 912 | else: 913 | assert len(rand) == 3 914 | r1 = numpy.sqrt(1.0 - rand[0]) 915 | r2 = numpy.sqrt(rand[0]) 916 | pi2 = math.pi * 2.0 917 | t1 = pi2 * rand[1] 918 | t2 = pi2 * rand[2] 919 | return numpy.array((numpy.sin(t1)*r1, 920 | numpy.cos(t1)*r1, 921 | numpy.sin(t2)*r2, 922 | numpy.cos(t2)*r2), dtype=numpy.float64) 923 | 924 | 925 | def random_rotation_matrix(rand=None): 926 | """ 927 | Return uniform random rotation matrix. 928 | 929 | rnd: array like 930 | Three independent random variables that are uniformly distributed 931 | between 0 and 1 for each returned quaternion. 932 | 933 | >>> R = random_rotation_matrix() 934 | >>> numpy.allclose(numpy.dot(R.T, R), numpy.identity(4)) 935 | True 936 | 937 | """ 938 | return quaternion_matrix(random_quaternion(rand)) 939 | 940 | 941 | class Arcball(object): 942 | """ 943 | Virtual Trackball Control. 944 | 945 | >>> ball = Arcball() 946 | >>> ball = Arcball(initial=numpy.identity(4)) 947 | >>> ball.place([320, 320], 320) 948 | >>> ball.down([500, 250]) 949 | >>> ball.drag([475, 275]) 950 | >>> R = ball.matrix() 951 | >>> numpy.allclose(numpy.sum(R), 3.90583455) 952 | True 953 | >>> ball = Arcball(initial=[0, 0, 0, 1]) 954 | >>> ball.place([320, 320], 320) 955 | >>> ball.setaxes([1,1,0], [-1, 1, 0]) 956 | >>> ball.setconstrain(True) 957 | >>> ball.down([400, 200]) 958 | >>> ball.drag([200, 400]) 959 | >>> R = ball.matrix() 960 | >>> numpy.allclose(numpy.sum(R), 0.2055924) 961 | True 962 | >>> ball.next() 963 | 964 | """ 965 | 966 | def __init__(self, initial=None): 967 | """ 968 | Initialize virtual trackball control. 969 | 970 | initial : quaternion or rotation matrix 971 | 972 | """ 973 | self._axis = None 974 | self._axes = None 975 | self._radius = 1.0 976 | self._center = [0.0, 0.0] 977 | self._vdown = numpy.array([0, 0, 1], dtype=numpy.float64) 978 | self._constrain = False 979 | 980 | if initial is None: 981 | self._qdown = numpy.array([0, 0, 0, 1], dtype=numpy.float64) 982 | else: 983 | initial = numpy.array(initial, dtype=numpy.float64) 984 | if initial.shape == (4, 4): 985 | self._qdown = quaternion_from_matrix(initial) 986 | elif initial.shape == (4, ): 987 | initial /= vector_norm(initial) 988 | self._qdown = initial 989 | else: 990 | raise ValueError('initial not a quaternion or matrix.') 991 | 992 | self._qnow = self._qpre = self._qdown 993 | 994 | def place(self, center, radius): 995 | """ 996 | Place Arcball, e.g. when window size changes. 997 | 998 | center : sequence[2] 999 | Window coordinates of trackball center. 1000 | radius : float 1001 | Radius of trackball in window coordinates. 1002 | 1003 | """ 1004 | self._radius = float(radius) 1005 | self._center[0] = center[0] 1006 | self._center[1] = center[1] 1007 | 1008 | def setaxes(self, *axes): 1009 | """Set axes to constrain rotations.""" 1010 | if axes is None: 1011 | self._axes = None 1012 | else: 1013 | self._axes = [unit_vector(axis) for axis in axes] 1014 | 1015 | def setconstrain(self, constrain): 1016 | """Set state of constrain to axis mode.""" 1017 | self._constrain = constrain is True 1018 | 1019 | def getconstrain(self): 1020 | """Return state of constrain to axis mode.""" 1021 | return self._constrain 1022 | 1023 | def down(self, point): 1024 | """Set initial cursor window coordinates and pick constrain-axis.""" 1025 | self._vdown = arcball_map_to_sphere(point, self._center, self._radius) 1026 | self._qdown = self._qpre = self._qnow 1027 | 1028 | if self._constrain and self._axes is not None: 1029 | self._axis = arcball_nearest_axis(self._vdown, self._axes) 1030 | self._vdown = arcball_constrain_to_axis(self._vdown, self._axis) 1031 | else: 1032 | self._axis = None 1033 | 1034 | def drag(self, point): 1035 | """Update current cursor window coordinates.""" 1036 | vnow = arcball_map_to_sphere(point, self._center, self._radius) 1037 | 1038 | if self._axis is not None: 1039 | vnow = arcball_constrain_to_axis(vnow, self._axis) 1040 | 1041 | self._qpre = self._qnow 1042 | 1043 | t = numpy.cross(self._vdown, vnow) 1044 | if numpy.dot(t, t) < _EPS: 1045 | self._qnow = self._qdown 1046 | else: 1047 | q = [t[0], t[1], t[2], numpy.dot(self._vdown, vnow)] 1048 | self._qnow = quaternion_multiply(q, self._qdown) 1049 | 1050 | def next(self, acceleration=0.0): # noqa: A003 (for backwards compat.) 1051 | """Continue rotation in direction of last drag.""" 1052 | q = quaternion_slerp(self._qpre, self._qnow, 2.0+acceleration, False) 1053 | self._qpre, self._qnow = self._qnow, q 1054 | 1055 | def matrix(self): 1056 | """Return homogeneous rotation matrix.""" 1057 | return quaternion_matrix(self._qnow) 1058 | 1059 | 1060 | def arcball_map_to_sphere(point, center, radius): 1061 | """Return unit sphere coordinates from window coordinates.""" 1062 | v = numpy.array(((point[0] - center[0]) / radius, 1063 | (center[1] - point[1]) / radius, 1064 | 0.0), dtype=numpy.float64) 1065 | n = v[0]*v[0] + v[1]*v[1] 1066 | if n > 1.0: 1067 | v /= math.sqrt(n) # position outside of sphere 1068 | else: 1069 | v[2] = math.sqrt(1.0 - n) 1070 | return v 1071 | 1072 | 1073 | def arcball_constrain_to_axis(point, axis): 1074 | """Return sphere point perpendicular to axis.""" 1075 | v = numpy.array(point, dtype=numpy.float64, copy=True) 1076 | a = numpy.array(axis, dtype=numpy.float64, copy=True) 1077 | v -= a * numpy.dot(a, v) # on plane 1078 | n = vector_norm(v) 1079 | if n > _EPS: 1080 | if v[2] < 0.0: 1081 | v *= -1.0 1082 | v /= n 1083 | return v 1084 | if a[2] == 1.0: 1085 | return numpy.array([1, 0, 0], dtype=numpy.float64) 1086 | return unit_vector([-a[1], a[0], 0]) 1087 | 1088 | 1089 | def arcball_nearest_axis(point, axes): 1090 | """Return axis, which arc is nearest to point.""" 1091 | point = numpy.array(point, dtype=numpy.float64, copy=False) 1092 | nearest = None 1093 | mx = -1.0 1094 | for axis in axes: 1095 | t = numpy.dot(arcball_constrain_to_axis(point, axis), point) 1096 | if t > mx: 1097 | nearest = axis 1098 | mx = t 1099 | return nearest 1100 | 1101 | 1102 | # epsilon for testing whether a number is close to zero 1103 | _EPS = numpy.finfo(float).eps * 4.0 1104 | 1105 | # axis sequences for Euler angles 1106 | _NEXT_AXIS = [1, 2, 0, 1] 1107 | 1108 | # map axes strings to/from tuples of inner axis, parity, repetition, frame 1109 | _AXES2TUPLE = { 1110 | 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0), 1111 | 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0), 1112 | 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0), 1113 | 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0), 1114 | 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1), 1115 | 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1), 1116 | 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1), 1117 | 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)} 1118 | 1119 | _TUPLE2AXES = {v: k for k, v in _AXES2TUPLE.items()} 1120 | 1121 | 1122 | def vector_norm(data, axis=None, out=None): 1123 | """ 1124 | Return length, i.e. eucledian norm, of ndarray along axis. 1125 | 1126 | >>> v = numpy.random.random(3) 1127 | >>> n = vector_norm(v) 1128 | >>> numpy.allclose(n, numpy.linalg.norm(v)) 1129 | True 1130 | >>> v = numpy.random.rand(6, 5, 3) 1131 | >>> n = vector_norm(v, axis=-1) 1132 | >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2))) 1133 | True 1134 | >>> n = vector_norm(v, axis=1) 1135 | >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) 1136 | True 1137 | >>> v = numpy.random.rand(5, 4, 3) 1138 | >>> n = numpy.empty((5, 3), dtype=numpy.float64) 1139 | >>> vector_norm(v, axis=1, out=n) 1140 | >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) 1141 | True 1142 | >>> vector_norm([]) 1143 | 0.0 1144 | >>> vector_norm([1.0]) 1145 | 1.0 1146 | 1147 | """ 1148 | data = numpy.array(data, dtype=numpy.float64, copy=True) 1149 | if out is None: 1150 | if data.ndim == 1: 1151 | return math.sqrt(numpy.dot(data, data)) 1152 | data *= data 1153 | out = numpy.atleast_1d(numpy.sum(data, axis=axis)) 1154 | numpy.sqrt(out, out) 1155 | return out 1156 | else: 1157 | data *= data 1158 | numpy.sum(data, axis=axis, out=out) 1159 | numpy.sqrt(out, out) 1160 | 1161 | 1162 | def unit_vector(data, axis=None, out=None): 1163 | """ 1164 | Return ndarray normalized by length, i.e. eucledian norm, along axis. 1165 | 1166 | >>> v0 = numpy.random.random(3) 1167 | >>> v1 = unit_vector(v0) 1168 | >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0)) 1169 | True 1170 | >>> v0 = numpy.random.rand(5, 4, 3) 1171 | >>> v1 = unit_vector(v0, axis=-1) 1172 | >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2) 1173 | >>> numpy.allclose(v1, v2) 1174 | True 1175 | >>> v1 = unit_vector(v0, axis=1) 1176 | >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1) 1177 | >>> numpy.allclose(v1, v2) 1178 | True 1179 | >>> v1 = numpy.empty((5, 4, 3), dtype=numpy.float64) 1180 | >>> unit_vector(v0, axis=1, out=v1) 1181 | >>> numpy.allclose(v1, v2) 1182 | True 1183 | >>> list(unit_vector([])) 1184 | [] 1185 | >>> list(unit_vector([1.0])) 1186 | [1.0] 1187 | 1188 | """ 1189 | if out is None: 1190 | data = numpy.array(data, dtype=numpy.float64, copy=True) 1191 | if data.ndim == 1: 1192 | data /= math.sqrt(numpy.dot(data, data)) 1193 | return data 1194 | else: 1195 | if out is not data: 1196 | out[:] = numpy.array(data, copy=False) 1197 | data = out 1198 | length = numpy.atleast_1d(numpy.sum(data*data, axis)) 1199 | numpy.sqrt(length, length) 1200 | if axis is not None: 1201 | length = numpy.expand_dims(length, axis) 1202 | data /= length 1203 | if out is None: 1204 | return data 1205 | 1206 | 1207 | def random_vector(size): 1208 | """ 1209 | Return array of random doubles in the half-open interval [0.0, 1.0). 1210 | 1211 | >>> v = random_vector(10000) 1212 | >>> numpy.all(v >= 0.0) and numpy.all(v < 1.0) 1213 | True 1214 | >>> v0 = random_vector(10) 1215 | >>> v1 = random_vector(10) 1216 | >>> numpy.any(v0 == v1) 1217 | False 1218 | 1219 | """ 1220 | return numpy.random.random(size) 1221 | 1222 | 1223 | def inverse_matrix(matrix): 1224 | """ 1225 | Return inverse of square transformation matrix. 1226 | 1227 | >>> M0 = random_rotation_matrix() 1228 | >>> M1 = inverse_matrix(M0.T) 1229 | >>> numpy.allclose(M1, numpy.linalg.inv(M0.T)) 1230 | True 1231 | >>> for size in range(1, 7): 1232 | ... M0 = numpy.random.rand(size, size) 1233 | ... M1 = inverse_matrix(M0) 1234 | ... if not numpy.allclose(M1, numpy.linalg.inv(M0)): print size 1235 | 1236 | """ 1237 | return numpy.linalg.inv(matrix) 1238 | 1239 | 1240 | def concatenate_matrices(*matrices): 1241 | """ 1242 | Return concatenation of series of transformation matrices. 1243 | 1244 | >>> M = numpy.random.rand(16).reshape((4, 4)) - 0.5 1245 | >>> numpy.allclose(M, concatenate_matrices(M)) 1246 | True 1247 | >>> numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T)) 1248 | True 1249 | 1250 | """ 1251 | M = numpy.identity(4) 1252 | for i in matrices: 1253 | M = numpy.dot(M, i) 1254 | return M 1255 | 1256 | 1257 | def is_same_transform(matrix0, matrix1): 1258 | """ 1259 | Return True if two matrices perform same transformation. 1260 | 1261 | >>> is_same_transform(numpy.identity(4), numpy.identity(4)) 1262 | True 1263 | >>> is_same_transform(numpy.identity(4), random_rotation_matrix()) 1264 | False 1265 | 1266 | """ 1267 | matrix0 = numpy.array(matrix0, dtype=numpy.float64, copy=True) 1268 | matrix0 /= matrix0[3, 3] 1269 | matrix1 = numpy.array(matrix1, dtype=numpy.float64, copy=True) 1270 | matrix1 /= matrix1[3, 3] 1271 | return numpy.allclose(matrix0, matrix1) 1272 | --------------------------------------------------------------------------------