| 1 |
"""Module implementing point transformations and their matrices.""" |
| 2 |
|
| 3 |
from __future__ import annotations |
| 4 |
|
| 5 |
from typing import TYPE_CHECKING |
| 6 |
from typing import Literal |
| 7 |
from typing import TypeAlias |
| 8 |
from typing import overload |
| 9 |
|
| 10 |
import numpy as np |
| 11 |
|
| 12 |
from pyvista._deprecate_positional_args import _deprecate_positional_args |
| 13 |
from pyvista.core import _validation |
| 14 |
from pyvista.core.utilities.misc import _reciprocal |
| 15 |
|
| 16 |
if TYPE_CHECKING: |
| 17 |
from pyvista.core._typing_core import NumpyArray |
| 18 |
from pyvista.core._typing_core import TransformLike |
| 19 |
from pyvista.core._typing_core import VectorLike |
| 20 |
|
| 21 |
_FiveArrays: TypeAlias = tuple[ |
| 22 |
NumpyArray[float], |
| 23 |
NumpyArray[float], |
| 24 |
NumpyArray[float], |
| 25 |
NumpyArray[float], |
| 26 |
NumpyArray[float], |
| 27 |
] |
| 28 |
|
| 29 |
|
| 30 |
@_deprecate_positional_args(allowed=['axis', 'angle']) |
| 31 |
def axis_angle_rotation( # noqa: PLR0917 |
| 32 |
axis: VectorLike[float], |
| 33 |
angle: float, |
| 34 |
point: VectorLike[float] | None = None, |
| 35 |
deg: bool = True, # noqa: FBT001, FBT002 |
| 36 |
) -> NumpyArray[float]: |
| 37 |
r"""Return a 4x4 matrix for rotation about any axis by given angle. |
| 38 |
|
| 39 |
Rotations around an axis that contains the origin can easily be |
| 40 |
computed using Rodrigues' rotation formula. The key quantity is |
| 41 |
the ``K`` cross product matrix for the unit vector ``n`` defining |
| 42 |
the axis of the rotation: |
| 43 |
|
| 44 |
/ 0 -nz ny \ |
| 45 |
K = | nz 0 -nx | |
| 46 |
\ -ny nx 0 / |
| 47 |
|
| 48 |
For a rotation angle ``phi`` around the vector ``n`` the rotation |
| 49 |
matrix is given by |
| 50 |
|
| 51 |
R = I + sin(phi) K + (1 - cos(phi)) K^2 |
| 52 |
|
| 53 |
where ``I`` is the 3-by-3 unit matrix and ``K^2`` denotes the matrix |
| 54 |
square of ``K``. |
| 55 |
|
| 56 |
If the rotation axis doesn't contain the origin, we have to first |
| 57 |
shift real space to transform the axis' ``p0`` reference point into |
| 58 |
the origin, then shift the points back after rotation: |
| 59 |
|
| 60 |
p' = R @ (p - p0) + p0 = R @ p + (p0 - R @ p0) |
| 61 |
|
| 62 |
This means that the rotation in general consists of a 3-by-3 |
| 63 |
rotation matrix ``R``, and a translation given by |
| 64 |
``b = p0 - R @ p0``. These can be encoded in a 4-by-4 transformation |
| 65 |
matrix by filling the 3-by-3 leading principal submatrix with ``R``, |
| 66 |
and filling the top 3 values in the last column with ``b``. |
| 67 |
|
| 68 |
Parameters |
| 69 |
---------- |
| 70 |
axis : sequence[float] |
| 71 |
The direction vector of the rotation axis. It need not be a |
| 72 |
unit vector, but it must not be a zero vector. |
| 73 |
|
| 74 |
angle : float |
| 75 |
Angle of rotation around the axis. The angle is defined as a |
| 76 |
counterclockwise rotation when facing the normal vector of the |
| 77 |
rotation axis. Passed either in degrees or radians depending on |
| 78 |
the value of ``deg``. |
| 79 |
|
| 80 |
point : sequence[float], optional |
| 81 |
The origin of the rotation (a reference point through which the |
| 82 |
rotation axis passes). By default the rotation axis contains the |
| 83 |
origin. |
| 84 |
|
| 85 |
deg : bool, default: True |
| 86 |
Whether the angle is specified in degrees. ``False`` implies |
| 87 |
radians. |
| 88 |
|
| 89 |
Returns |
| 90 |
------- |
| 91 |
numpy.ndarray |
| 92 |
The ``(4, 4)`` rotation matrix. |
| 93 |
|
| 94 |
Examples |
| 95 |
-------- |
| 96 |
Generate a transformation matrix for rotation around a cube's body |
| 97 |
diagonal by 120 degrees. |
| 98 |
|
| 99 |
>>> import numpy as np |
| 100 |
>>> from pyvista import transformations |
| 101 |
>>> trans = transformations.axis_angle_rotation([1, 1, 1], 120) |
| 102 |
|
| 103 |
Check that the transformation cycles the cube's three corners. |
| 104 |
|
| 105 |
>>> corners = np.array( |
| 106 |
... [ |
| 107 |
... [1, 0, 0], |
| 108 |
... [0, 1, 0], |
| 109 |
... [0, 0, 1], |
| 110 |
... ] |
| 111 |
... ) |
| 112 |
>>> rotated = transformations.apply_transformation_to_points( |
| 113 |
... trans, corners |
| 114 |
... ) |
| 115 |
>>> np.allclose(rotated, corners[[1, 2, 0], :]) |
| 116 |
True |
| 117 |
|
| 118 |
""" |
| 119 |
if deg: |
| 120 |
# convert to radians |
| 121 |
angle *= np.pi / 180 |
| 122 |
|
| 123 |
# return early for no rotation; play it safe and check only exact equality |
| 124 |
if angle % (2 * np.pi) == 0: |
| 125 |
return np.eye(4) |
| 126 |
|
| 127 |
axis_ = _validation.validate_array3(axis, dtype_out=float, name='axis') |
| 128 |
if point is not None: |
| 129 |
point_ = _validation.validate_array3(point, dtype_out=float, name='point') |
| 130 |
|
| 131 |
# check and normalize |
| 132 |
axis_norm = np.linalg.norm(axis_) |
| 133 |
if np.isclose(axis_norm, 0): |
| 134 |
msg = 'Cannot rotate around zero vector axis.' |
| 135 |
raise ValueError(msg) |
| 136 |
if not np.isclose(axis_norm, 1): |
| 137 |
axis_ = axis_ / axis_norm |
| 138 |
|
| 139 |
# build Rodrigues' rotation matrix |
| 140 |
K = np.zeros((3, 3)) |
| 141 |
K[[2, 0, 1], [1, 2, 0]] = axis_ |
| 142 |
K += -K.T |
| 143 |
|
| 144 |
# the cos and sin functions can introduce some numerical error |
| 145 |
# round the elements to exact values for special cases where we know |
| 146 |
# sin/cos should evaluate exactly to 0 or 1 |
| 147 |
sin_angle = np.sin(angle) |
| 148 |
cos_angle = np.cos(angle) |
| 149 |
if angle % (np.pi / 2) == 0: |
| 150 |
cos_angle = round(cos_angle) |
| 151 |
sin_angle = round(sin_angle) |
| 152 |
|
| 153 |
R = np.eye(3) + sin_angle * K + (1 - cos_angle) * K @ K |
| 154 |
|
| 155 |
augmented = np.eye(4) |
| 156 |
augmented[:-1, :-1] = R |
| 157 |
|
| 158 |
if point is not None: |
| 159 |
# rotation of point p would be R @ (p - point) + point |
| 160 |
# which is R @ p + (point - R @ point) |
| 161 |
augmented[:-1, -1] = point_ - R @ point_ |
| 162 |
|
| 163 |
return augmented |
| 164 |
|
| 165 |
|
| 166 |
def reflection( |
| 167 |
normal: VectorLike[float], point: VectorLike[float] | None = None |
| 168 |
) -> NumpyArray[float]: |
| 169 |
"""Return a 4x4 matrix for reflection across a normal about a point. |
| 170 |
|
| 171 |
Projection to a unit vector ``n`` can be computed using the dyadic |
| 172 |
product (or outer product) ``P`` of ``n`` with itself, which is a |
| 173 |
3-by-3 symmetric matrix. |
| 174 |
|
| 175 |
Reflection across a plane that contains the origin amounts to |
| 176 |
reversing the components of real space points that are perpendicular |
| 177 |
to the reflection plane. This gives us the transformation ``R`` |
| 178 |
acting on a point ``p`` as |
| 179 |
|
| 180 |
p' = R @ p = p - 2 P @ p = (I - 2 P) @ p |
| 181 |
|
| 182 |
so the reflection's transformation matrix is the unit matrix minus |
| 183 |
twice the dyadic product ``P``. |
| 184 |
|
| 185 |
If additionally we want to compute a reflection to a plane that does |
| 186 |
not contain the origin, we can we can first shift every point in |
| 187 |
real space by ``-p0`` (if ``p0`` is a point that lies on the plane) |
| 188 |
|
| 189 |
p' = R @ (p - p0) + p0 = R @ p + (p0 - R @ p0) |
| 190 |
|
| 191 |
This means that the reflection in general consists of a 3-by-3 |
| 192 |
reflection matrix ``R``, and a translation given by |
| 193 |
``b = p0 - R @ p0``. These can be encoded in a 4-by-4 transformation |
| 194 |
matrix by filling the 3-by-3 leading principal submatrix with ``R``, |
| 195 |
and filling the top 3 values in the last column with ``b``. |
| 196 |
|
| 197 |
Parameters |
| 198 |
---------- |
| 199 |
normal : sequence[float] |
| 200 |
The normal vector of the reflection plane. It need not be a unit |
| 201 |
vector, but it must not be a zero vector. |
| 202 |
|
| 203 |
point : sequence[float], optional |
| 204 |
The origin of the reflection (a reference point through which |
| 205 |
the reflection plane passes). By default the reflection plane |
| 206 |
contains the origin. |
| 207 |
|
| 208 |
Returns |
| 209 |
------- |
| 210 |
ndarray |
| 211 |
A ``(4, 4)`` transformation matrix for reflecting points across the |
| 212 |
plane defined by the given normal and point. |
| 213 |
|
| 214 |
Examples |
| 215 |
-------- |
| 216 |
Generate a transformation matrix for reflection over the XZ plane. |
| 217 |
|
| 218 |
>>> import numpy as np |
| 219 |
>>> from pyvista import transformations |
| 220 |
>>> trans = transformations.reflection([0, 1, 0]) |
| 221 |
|
| 222 |
Check that the reflection transforms corners of a cube among one |
| 223 |
another. |
| 224 |
|
| 225 |
>>> verts = np.array( |
| 226 |
... [ |
| 227 |
... [1, -1, 1], |
| 228 |
... [-1, -1, 1], |
| 229 |
... [-1, -1, -1], |
| 230 |
... [-1, -1, 1], |
| 231 |
... [1, 1, 1], |
| 232 |
... [-1, 1, 1], |
| 233 |
... [-1, 1, -1], |
| 234 |
... [-1, 1, 1], |
| 235 |
... ] |
| 236 |
... ) |
| 237 |
>>> mirrored = transformations.apply_transformation_to_points(trans, verts) |
| 238 |
>>> np.allclose(mirrored, verts[[np.r_[4:8, 0:4]], :]) |
| 239 |
True |
| 240 |
|
| 241 |
""" |
| 242 |
normal = np.asarray(normal, dtype='float64') |
| 243 |
if normal.shape != (3,): |
| 244 |
msg = 'Normal must be a 3-length array-like.' |
| 245 |
raise ValueError(msg) |
| 246 |
if point is not None: |
| 247 |
point = np.asarray(point) |
| 248 |
if point.shape != (3,): |
| 249 |
msg = 'Plane reference point must be a 3-length array-like.' |
| 250 |
raise ValueError(msg) |
| 251 |
|
| 252 |
# check and normalize |
| 253 |
normal_norm = np.linalg.norm(normal) |
| 254 |
if np.isclose(normal_norm, 0): |
| 255 |
msg = 'Plane normal cannot be zero.' |
| 256 |
raise ValueError(msg) |
| 257 |
if not np.isclose(normal_norm, 1): |
| 258 |
normal = normal / normal_norm |
| 259 |
|
| 260 |
# build reflection matrix |
| 261 |
projection = np.outer(normal, normal) |
| 262 |
R = np.eye(3) - 2 * projection |
| 263 |
augmented = np.eye(4) |
| 264 |
augmented[:-1, :-1] = R |
| 265 |
|
| 266 |
if point is not None: |
| 267 |
# reflection of point p would be R @ (p - point) + point |
| 268 |
# which is R @ p + (point - R @ point) |
| 269 |
augmented[:-1, -1] = point - R @ point |
| 270 |
|
| 271 |
return augmented |
| 272 |
|
| 273 |
|
| 274 |
@overload |
| 275 |
def apply_transformation_to_points( |
| 276 |
transformation: NumpyArray[float], |
| 277 |
points: NumpyArray[float], |
| 278 |
inplace: Literal[True] = True, # noqa: FBT002 |
| 279 |
) -> None: ... |
| 280 |
@overload |
| 281 |
def apply_transformation_to_points( |
| 282 |
transformation: NumpyArray[float], |
| 283 |
points: NumpyArray[float], |
| 284 |
inplace: Literal[False] = False, # noqa: FBT002 |
| 285 |
) -> NumpyArray[float]: ... |
| 286 |
@overload |
| 287 |
def apply_transformation_to_points( |
| 288 |
transformation: NumpyArray[float], |
| 289 |
points: NumpyArray[float], |
| 290 |
inplace: bool = ..., # noqa: FBT001 |
| 291 |
) -> NumpyArray[float] | None: ... |
| 292 |
@_deprecate_positional_args(allowed=['transformation', 'points']) |
| 293 |
def apply_transformation_to_points( |
| 294 |
transformation: NumpyArray[float], |
| 295 |
points: NumpyArray[float], |
| 296 |
inplace: Literal[True, False] = False, # noqa: FBT002 |
| 297 |
) -> NumpyArray[float] | None: |
| 298 |
"""Apply a given transformation matrix (3x3 or 4x4) to a set of points. |
| 299 |
|
| 300 |
Parameters |
| 301 |
---------- |
| 302 |
transformation : np.ndarray |
| 303 |
Transformation matrix of shape (3, 3) or (4, 4). |
| 304 |
|
| 305 |
points : np.ndarray |
| 306 |
Array of points to be transformed of shape (N, 3). |
| 307 |
|
| 308 |
inplace : bool, default: False |
| 309 |
Updates points in-place while returning nothing. |
| 310 |
|
| 311 |
Returns |
| 312 |
------- |
| 313 |
numpy.ndarray |
| 314 |
Transformed points. |
| 315 |
|
| 316 |
Examples |
| 317 |
-------- |
| 318 |
Scale a set of points in-place. |
| 319 |
|
| 320 |
>>> import numpy as np |
| 321 |
>>> import pyvista as pv |
| 322 |
>>> from pyvista import examples |
| 323 |
>>> points = examples.load_airplane().points |
| 324 |
>>> points_orig = points.copy() |
| 325 |
>>> scale_factor = 2 |
| 326 |
>>> tf = scale_factor * np.eye(4) |
| 327 |
>>> tf[3, 3] = 1 |
| 328 |
>>> pv.core.utilities.transformations.apply_transformation_to_points( |
| 329 |
... tf, points, inplace=True |
| 330 |
... ) |
| 331 |
>>> assert np.all(np.isclose(points, scale_factor * points_orig)) |
| 332 |
|
| 333 |
""" |
| 334 |
transformation_shape = transformation.shape |
| 335 |
if transformation_shape not in ((3, 3), (4, 4)): |
| 336 |
msg = '`transformation` must be of shape (3, 3) or (4, 4).' |
| 337 |
raise ValueError(msg) |
| 338 |
|
| 339 |
if points.shape[1] != 3: |
| 340 |
msg = '`points` must be of shape (N, 3).' |
| 341 |
raise ValueError(msg) |
| 342 |
|
| 343 |
if transformation_shape[0] == 4: # type: ignore[comparison-overlap] |
| 344 |
# Divide by scale factor when homogeneous |
| 345 |
transformation /= transformation[3, 3] |
| 346 |
|
| 347 |
# Add the homogeneous coordinate |
| 348 |
# `points_2` is a copy of the data, not a view |
| 349 |
points_2 = np.empty((len(points), 4)) |
| 350 |
points_2[:, :-1] = points |
| 351 |
points_2[:, -1] = 1 |
| 352 |
else: |
| 353 |
points_2 = points # type: ignore[assignment] |
| 354 |
|
| 355 |
# Paged matrix multiplication. For arrays with ndim > 2, matmul assumes |
| 356 |
# that the matrices to be multiplied lie in the last two dimensions. |
| 357 |
points_2 = (transformation[np.newaxis, :, :] @ points_2.T)[0, :3, :].T |
| 358 |
|
| 359 |
# If inplace, set the points |
| 360 |
if inplace: |
| 361 |
points[:] = points_2 |
| 362 |
return None |
| 363 |
else: |
| 364 |
# otherwise return the new points |
| 365 |
return points_2 |
| 366 |
|
| 367 |
|
| 368 |
def decomposition(transformation: TransformLike, *, homogeneous: bool = False) -> _FiveArrays: |
| 369 |
"""Decompose a transformation into its components. |
| 370 |
|
| 371 |
The transformation matrix ``M`` is decomposed into five components: |
| 372 |
|
| 373 |
- translation ``T`` |
| 374 |
- rotation ``R`` |
| 375 |
- reflection ``N`` |
| 376 |
- scaling ``S`` |
| 377 |
- shearing ``K`` |
| 378 |
|
| 379 |
such that, when represented as 4x4 matrices, ``M = TRNSK``. The decomposition is |
| 380 |
unique and is computed with polar matrix decomposition. |
| 381 |
|
| 382 |
By default, compact representations of the transformations are returned (e.g. as a |
| 383 |
3-element vector or a 3x3 matrix). Optionally, 4x4 matrices may be returned instead. |
| 384 |
|
| 385 |
.. note:: |
| 386 |
|
| 387 |
- The rotation is orthonormal and right-handed with positive determinant. |
| 388 |
- The scaling factors are positive. |
| 389 |
- The reflection is either ``1`` (no reflection) or ``-1`` (has reflection) |
| 390 |
and can be used like a scaling factor. |
| 391 |
|
| 392 |
Parameters |
| 393 |
---------- |
| 394 |
transformation : TransformLike |
| 395 |
Array or transform to decompose. |
| 396 |
|
| 397 |
homogeneous : bool, default: False |
| 398 |
If ``True``, return the components (translation, rotation, etc.) as 4x4 |
| 399 |
homogeneous matrices. By default, reflection is a scalar, translation and |
| 400 |
scaling are length-3 vectors, and rotation and shear are 3x3 matrices. |
| 401 |
|
| 402 |
Returns |
| 403 |
------- |
| 404 |
numpy.ndarray |
| 405 |
Translation component ``T``. Returned as a 3-element vector (or a 4x4 |
| 406 |
translation matrix if ``homogeneous`` is ``True``). |
| 407 |
|
| 408 |
numpy.ndarray |
| 409 |
Rotation component ``R``. Returned as a 3x3 orthonormal rotation matrix of row |
| 410 |
vectors (or a 4x4 rotation matrix if ``homogeneous`` is ``True``). |
| 411 |
|
| 412 |
numpy.ndarray |
| 413 |
Reflection component ``N``. Returned as a NumPy scalar (or a 4x4 reflection |
| 414 |
matrix if ``homogeneous`` is ``True``). |
| 415 |
|
| 416 |
numpy.ndarray |
| 417 |
Scaling component ``S``. Returned as a 3-element vector (or a 4x4 scaling matrix |
| 418 |
if ``homogeneous`` is ``True``). |
| 419 |
|
| 420 |
numpy.ndarray |
| 421 |
Shear component ``K``. Returned as a 3x3 matrix with ones on the diagonal and |
| 422 |
shear values in the off-diagonals (or as a 4x4 shearing matrix if ``homogeneous`` |
| 423 |
is ``True``). |
| 424 |
|
| 425 |
Examples |
| 426 |
-------- |
| 427 |
Decompose a transformation matrix which has scaling, rotation, and translation. |
| 428 |
|
| 429 |
>>> import pyvista as pv |
| 430 |
>>> matrix = [ |
| 431 |
... [0.0, -2.0, 0.0, 4.0], |
| 432 |
... [1.0, 0.0, 0.0, 5.0], |
| 433 |
... [0.0, 0.0, 3.0, 6.0], |
| 434 |
... [0.0, 0.0, 0.0, 1.0], |
| 435 |
... ] |
| 436 |
>>> T, R, N, S, K = pv.transformations.decomposition(matrix) |
| 437 |
|
| 438 |
Since the input has no shear, this component is the identity matrix. |
| 439 |
|
| 440 |
>>> K # shear |
| 441 |
array([[1., 0., 0.], |
| 442 |
[0., 1., 0.], |
| 443 |
[0., 0., 1.]]) |
| 444 |
|
| 445 |
>>> S # scale |
| 446 |
array([1., 2., 3.]) |
| 447 |
|
| 448 |
There is no reflection so this component is ``1``. |
| 449 |
|
| 450 |
>>> N # reflection |
| 451 |
array(1.) |
| 452 |
|
| 453 |
>>> R # rotation |
| 454 |
array([[ 0., -1., 0.], |
| 455 |
[ 1., 0., 0.], |
| 456 |
[ 0., 0., 1.]]) |
| 457 |
|
| 458 |
>>> T # translation |
| 459 |
array([4., 5., 6.]) |
| 460 |
|
| 461 |
Repeat the example, but this time with a small shear component of 0.1. Note how the |
| 462 |
presence of shear also affects the values of the scaling and rotation components. |
| 463 |
|
| 464 |
>>> matrix = [ |
| 465 |
... [0.0, -2.0, 0.0, 4.0], |
| 466 |
... [1.0, 0.1, 0.0, 5.0], |
| 467 |
... [0.0, 0.0, 3.0, 6.0], |
| 468 |
... [0.0, 0.0, 0.0, 1.0], |
| 469 |
... ] |
| 470 |
>>> T, R, N, S, K = pv.transformations.decomposition(matrix) |
| 471 |
|
| 472 |
>>> K # shear |
| 473 |
array([[1. , 0.03333333, 0. ], |
| 474 |
[0.01663894, 1. , 0. ], |
| 475 |
[0. , 0. , 1. ]]) |
| 476 |
|
| 477 |
>>> S # scale |
| 478 |
array([0.99944491, 2.0022213 , 3. ]) |
| 479 |
|
| 480 |
>>> N # reflection |
| 481 |
array(1.) |
| 482 |
|
| 483 |
>>> R # rotation |
| 484 |
array([[ 0.03331483, -0.99944491, 0. ], |
| 485 |
[ 0.99944491, 0.03331483, 0. ], |
| 486 |
[ 0. , 0. , 1. ]]) |
| 487 |
|
| 488 |
>>> T # translation |
| 489 |
array([4., 5., 6.]) |
| 490 |
|
| 491 |
""" |
| 492 |
matrix4x4 = _validation.validate_transform4x4(transformation) |
| 493 |
|
| 494 |
dtype_out = matrix4x4.dtype |
| 495 |
I3 = np.eye(3, dtype=dtype_out) |
| 496 |
matrix3x3 = matrix4x4[:3, :3] |
| 497 |
|
| 498 |
T = matrix4x4[:3, 3] |
| 499 |
RN, SK = _polar_decomposition(matrix3x3) |
| 500 |
|
| 501 |
# Get scale from diagonals and shear from off-diagonals |
| 502 |
S = np.diagonal(SK).copy() # Copy since it's read only |
| 503 |
|
| 504 |
# Avoid division by zero for cases with rank < 3 |
| 505 |
inv_S = _reciprocal(S, tol=1e-12, value_if_division_by_zero=0.0) |
| 506 |
K = (SK * (I3 == 0.0)) * inv_S[:, np.newaxis] + I3 |
| 507 |
|
| 508 |
# Get reflection and ensure rotation is right-handed |
| 509 |
if np.linalg.det(RN) < 0: |
| 510 |
# Reflections are present |
| 511 |
R = RN * -1 |
| 512 |
N = np.array(-1, dtype=dtype_out) |
| 513 |
else: |
| 514 |
R = RN |
| 515 |
N = np.array(1, dtype=dtype_out) |
| 516 |
|
| 517 |
if homogeneous: |
| 518 |
return _decomposition_as_homogeneous(T, R, N, S, K) |
| 519 |
return T, R, N, S, K |
| 520 |
|
| 521 |
|
| 522 |
def _decomposition_as_homogeneous( # noqa: PLR0917 |
| 523 |
T: NumpyArray[float], # noqa: N803 |
| 524 |
R: NumpyArray[float], # noqa: N803 |
| 525 |
N: NumpyArray[float], # noqa: N803 |
| 526 |
S: NumpyArray[float], # noqa: N803 |
| 527 |
K: NumpyArray[float], # noqa: N803 |
| 528 |
) -> _FiveArrays: |
| 529 |
"""Return TRNSK decomposition as homogeneous matrices.""" |
| 530 |
dtype_out = T.dtype # Assume all inputs have the same dtype |
| 531 |
I3 = np.eye(3, dtype=dtype_out) |
| 532 |
I4 = np.eye(4, dtype=dtype_out) |
| 533 |
|
| 534 |
T4 = I4.copy() |
| 535 |
T4[:3, 3] = T |
| 536 |
|
| 537 |
R4 = I4.copy() |
| 538 |
R4[:3, :3] = R |
| 539 |
|
| 540 |
N4 = I4.copy() |
| 541 |
N4[:3, :3] = I3 * N |
| 542 |
|
| 543 |
S4 = I4.copy() |
| 544 |
S4[:3, :3] = I3 * S |
| 545 |
|
| 546 |
K4 = I4.copy() |
| 547 |
K4[:3, :3] = K |
| 548 |
|
| 549 |
return T4, R4, N4, S4, K4 |
| 550 |
|
| 551 |
|
| 552 |
def _polar_decomposition(a: NumpyArray[float]) -> tuple[NumpyArray[float], NumpyArray[float]]: |
| 553 |
# Decompose `a=up` where u is orthonormal and p is positive semi-definite |
| 554 |
# See scipy.linalg.polar for details |
| 555 |
w, s, vh = np.linalg.svd(a, full_matrices=False) |
| 556 |
u = w.dot(vh) |
| 557 |
p = (vh.T.conj() * s).dot(vh) |
| 558 |
return u, p |