├── .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 |
--------------------------------------------------------------------------------