reconnect moved files to git repo

This commit is contained in:
root
2025-08-01 04:33:03 -04:00
commit 5d3c35492d
23190 changed files with 4750716 additions and 0 deletions

View File

@ -0,0 +1,29 @@
"""
Spatial Transformations (:mod:`scipy.spatial.transform`)
========================================================
.. currentmodule:: scipy.spatial.transform
This package implements various spatial transformations. For now,
only rotations are supported.
Rotations in 3 dimensions
-------------------------
.. autosummary::
:toctree: generated/
Rotation
Slerp
RotationSpline
"""
from ._rotation import Rotation, Slerp
from ._rotation_spline import RotationSpline
# Deprecated namespaces, to be removed in v2.0.0
from . import rotation
__all__ = ['Rotation', 'Slerp', 'RotationSpline']
from scipy._lib._testutils import PytestTester
test = PytestTester(__name__)
del PytestTester

View File

@ -0,0 +1,65 @@
from __future__ import annotations
from typing import TYPE_CHECKING
from collections.abc import Sequence
import numpy as np
if TYPE_CHECKING:
import numpy.typing as npt
_IntegerType = int | np.integer
class Rotation:
def __init__(self, quat: npt.ArrayLike, normalize: bool = ..., copy: bool = ...) -> None: ...
@property
def single(self) -> bool: ...
def __len__(self) -> int: ...
@classmethod
def from_quat(cls, quat: npt.ArrayLike, *, scalar_first: bool = ...) -> Rotation: ...
@classmethod
def from_matrix(cls, matrix: npt.ArrayLike) -> Rotation: ...
@classmethod
def from_rotvec(cls, rotvec: npt.ArrayLike, degrees: bool = ...) -> Rotation: ...
@classmethod
def from_euler(cls, seq: str, angles: float | npt.ArrayLike, degrees: bool = ...) -> Rotation: ...
@classmethod
def from_davenport(cls, axes: npt.ArrayLike, order: str, angles: float | npt.ArrayLike, degrees: bool = ...) -> Rotation: ...
@classmethod
def from_mrp(cls, mrp: npt.ArrayLike) -> Rotation: ...
def as_quat(self, canonical: bool = ..., *, scalar_first: bool = ...) -> np.ndarray: ...
def as_matrix(self) -> np.ndarray: ...
def as_rotvec(self, degrees: bool = ...) -> np.ndarray: ...
def as_euler(self, seq: str, degrees: bool = ...) -> np.ndarray: ...
def as_davenport(self, axes: npt.ArrayLike, order: str, degrees: bool = ...) -> np.ndarray: ...
def as_mrp(self) -> np.ndarray: ...
@classmethod
def concatenate(cls, rotations: Sequence[Rotation]) -> Rotation: ...
def apply(self, vectors: npt.ArrayLike, inverse: bool = ...) -> np.ndarray: ...
def __mul__(self, other: Rotation) -> Rotation: ...
def __pow__(self, n: float, modulus: int | None) -> Rotation: ...
def inv(self) -> Rotation: ...
def magnitude(self) -> np.ndarray | float: ...
def approx_equal(self, other: Rotation, atol: float | None, degrees: bool = ...) -> np.ndarray | bool: ...
def mean(self, weights: npt.ArrayLike | None = ...) -> Rotation: ...
def reduce(self, left: Rotation | None = ..., right: Rotation | None = ...,
return_indices: bool = ...) -> Rotation | tuple[Rotation, np.ndarray, np.ndarray]: ...
@classmethod
def create_group(cls, group: str, axis: str = ...) -> Rotation: ...
def __getitem__(self, indexer: int | slice | npt.ArrayLike) -> Rotation: ...
@classmethod
def identity(cls, num: int | None = ...) -> Rotation: ...
@classmethod
def random(cls, num: int | None = ...,
random_state: _IntegerType | np.random.Generator | np.random.RandomState | None = ...) -> Rotation: ...
@classmethod
def align_vectors(cls, a: npt.ArrayLike, b: npt.ArrayLike,
weights: npt.ArrayLike | None = ...,
return_sensitivity: bool = ...) -> tuple[Rotation, float] | tuple[Rotation, float, np.ndarray]:...
class Slerp:
times: np.ndarray
timedelta: np.ndarray
rotations: Rotation
rotvecs: np.ndarray
def __init__(self, times: npt.ArrayLike, rotations: Rotation) -> None: ...
def __call__(self, times: npt.ArrayLike) -> Rotation: ...

View File

@ -0,0 +1,140 @@
import numpy as np
from scipy.constants import golden as phi
def icosahedral(cls):
g1 = tetrahedral(cls).as_quat()
a = 0.5
b = 0.5 / phi
c = phi / 2
g2 = np.array([[+a, +b, +c, 0],
[+a, +b, -c, 0],
[+a, +c, 0, +b],
[+a, +c, 0, -b],
[+a, -b, +c, 0],
[+a, -b, -c, 0],
[+a, -c, 0, +b],
[+a, -c, 0, -b],
[+a, 0, +b, +c],
[+a, 0, +b, -c],
[+a, 0, -b, +c],
[+a, 0, -b, -c],
[+b, +a, 0, +c],
[+b, +a, 0, -c],
[+b, +c, +a, 0],
[+b, +c, -a, 0],
[+b, -a, 0, +c],
[+b, -a, 0, -c],
[+b, -c, +a, 0],
[+b, -c, -a, 0],
[+b, 0, +c, +a],
[+b, 0, +c, -a],
[+b, 0, -c, +a],
[+b, 0, -c, -a],
[+c, +a, +b, 0],
[+c, +a, -b, 0],
[+c, +b, 0, +a],
[+c, +b, 0, -a],
[+c, -a, +b, 0],
[+c, -a, -b, 0],
[+c, -b, 0, +a],
[+c, -b, 0, -a],
[+c, 0, +a, +b],
[+c, 0, +a, -b],
[+c, 0, -a, +b],
[+c, 0, -a, -b],
[0, +a, +c, +b],
[0, +a, +c, -b],
[0, +a, -c, +b],
[0, +a, -c, -b],
[0, +b, +a, +c],
[0, +b, +a, -c],
[0, +b, -a, +c],
[0, +b, -a, -c],
[0, +c, +b, +a],
[0, +c, +b, -a],
[0, +c, -b, +a],
[0, +c, -b, -a]])
return cls.from_quat(np.concatenate((g1, g2)))
def octahedral(cls):
g1 = tetrahedral(cls).as_quat()
c = np.sqrt(2) / 2
g2 = np.array([[+c, 0, 0, +c],
[0, +c, 0, +c],
[0, 0, +c, +c],
[0, 0, -c, +c],
[0, -c, 0, +c],
[-c, 0, 0, +c],
[0, +c, +c, 0],
[0, -c, +c, 0],
[+c, 0, +c, 0],
[-c, 0, +c, 0],
[+c, +c, 0, 0],
[-c, +c, 0, 0]])
return cls.from_quat(np.concatenate((g1, g2)))
def tetrahedral(cls):
g1 = np.eye(4)
c = 0.5
g2 = np.array([[c, -c, -c, +c],
[c, -c, +c, +c],
[c, +c, -c, +c],
[c, +c, +c, +c],
[c, -c, -c, -c],
[c, -c, +c, -c],
[c, +c, -c, -c],
[c, +c, +c, -c]])
return cls.from_quat(np.concatenate((g1, g2)))
def dicyclic(cls, n, axis=2):
g1 = cyclic(cls, n, axis).as_rotvec()
thetas = np.linspace(0, np.pi, n, endpoint=False)
rv = np.pi * np.vstack([np.zeros(n), np.cos(thetas), np.sin(thetas)]).T
g2 = np.roll(rv, axis, axis=1)
return cls.from_rotvec(np.concatenate((g1, g2)))
def cyclic(cls, n, axis=2):
thetas = np.linspace(0, 2 * np.pi, n, endpoint=False)
rv = np.vstack([thetas, np.zeros(n), np.zeros(n)]).T
return cls.from_rotvec(np.roll(rv, axis, axis=1))
def create_group(cls, group, axis='Z'):
if not isinstance(group, str):
raise ValueError("`group` argument must be a string")
permitted_axes = ['x', 'y', 'z', 'X', 'Y', 'Z']
if axis not in permitted_axes:
raise ValueError("`axis` must be one of " + ", ".join(permitted_axes))
if group in ['I', 'O', 'T']:
symbol = group
order = 1
elif group[:1] in ['C', 'D'] and group[1:].isdigit():
symbol = group[:1]
order = int(group[1:])
else:
raise ValueError("`group` must be one of 'I', 'O', 'T', 'Dn', 'Cn'")
if order < 1:
raise ValueError("Group order must be positive")
axis = 'xyz'.index(axis.lower())
if symbol == 'I':
return icosahedral(cls)
elif symbol == 'O':
return octahedral(cls)
elif symbol == 'T':
return tetrahedral(cls)
elif symbol == 'D':
return dicyclic(cls, order, axis=axis)
elif symbol == 'C':
return cyclic(cls, order, axis=axis)
else:
assert False

View File

@ -0,0 +1,460 @@
import numpy as np
from scipy.linalg import solve_banded
from ._rotation import Rotation
def _create_skew_matrix(x):
"""Create skew-symmetric matrices corresponding to vectors.
Parameters
----------
x : ndarray, shape (n, 3)
Set of vectors.
Returns
-------
ndarray, shape (n, 3, 3)
"""
result = np.zeros((len(x), 3, 3))
result[:, 0, 1] = -x[:, 2]
result[:, 0, 2] = x[:, 1]
result[:, 1, 0] = x[:, 2]
result[:, 1, 2] = -x[:, 0]
result[:, 2, 0] = -x[:, 1]
result[:, 2, 1] = x[:, 0]
return result
def _matrix_vector_product_of_stacks(A, b):
"""Compute the product of stack of matrices and vectors."""
return np.einsum("ijk,ik->ij", A, b)
def _angular_rate_to_rotvec_dot_matrix(rotvecs):
"""Compute matrices to transform angular rates to rot. vector derivatives.
The matrices depend on the current attitude represented as a rotation
vector.
Parameters
----------
rotvecs : ndarray, shape (n, 3)
Set of rotation vectors.
Returns
-------
ndarray, shape (n, 3, 3)
"""
norm = np.linalg.norm(rotvecs, axis=1)
k = np.empty_like(norm)
mask = norm > 1e-4
nm = norm[mask]
k[mask] = (1 - 0.5 * nm / np.tan(0.5 * nm)) / nm**2
mask = ~mask
nm = norm[mask]
k[mask] = 1/12 + 1/720 * nm**2
skew = _create_skew_matrix(rotvecs)
result = np.empty((len(rotvecs), 3, 3))
result[:] = np.identity(3)
result[:] += 0.5 * skew
result[:] += k[:, None, None] * np.matmul(skew, skew)
return result
def _rotvec_dot_to_angular_rate_matrix(rotvecs):
"""Compute matrices to transform rot. vector derivatives to angular rates.
The matrices depend on the current attitude represented as a rotation
vector.
Parameters
----------
rotvecs : ndarray, shape (n, 3)
Set of rotation vectors.
Returns
-------
ndarray, shape (n, 3, 3)
"""
norm = np.linalg.norm(rotvecs, axis=1)
k1 = np.empty_like(norm)
k2 = np.empty_like(norm)
mask = norm > 1e-4
nm = norm[mask]
k1[mask] = (1 - np.cos(nm)) / nm ** 2
k2[mask] = (nm - np.sin(nm)) / nm ** 3
mask = ~mask
nm = norm[mask]
k1[mask] = 0.5 - nm ** 2 / 24
k2[mask] = 1 / 6 - nm ** 2 / 120
skew = _create_skew_matrix(rotvecs)
result = np.empty((len(rotvecs), 3, 3))
result[:] = np.identity(3)
result[:] -= k1[:, None, None] * skew
result[:] += k2[:, None, None] * np.matmul(skew, skew)
return result
def _angular_acceleration_nonlinear_term(rotvecs, rotvecs_dot):
"""Compute the non-linear term in angular acceleration.
The angular acceleration contains a quadratic term with respect to
the derivative of the rotation vector. This function computes that.
Parameters
----------
rotvecs : ndarray, shape (n, 3)
Set of rotation vectors.
rotvecs_dot : ndarray, shape (n, 3)
Set of rotation vector derivatives.
Returns
-------
ndarray, shape (n, 3)
"""
norm = np.linalg.norm(rotvecs, axis=1)
dp = np.sum(rotvecs * rotvecs_dot, axis=1)
cp = np.cross(rotvecs, rotvecs_dot)
ccp = np.cross(rotvecs, cp)
dccp = np.cross(rotvecs_dot, cp)
k1 = np.empty_like(norm)
k2 = np.empty_like(norm)
k3 = np.empty_like(norm)
mask = norm > 1e-4
nm = norm[mask]
k1[mask] = (-nm * np.sin(nm) - 2 * (np.cos(nm) - 1)) / nm ** 4
k2[mask] = (-2 * nm + 3 * np.sin(nm) - nm * np.cos(nm)) / nm ** 5
k3[mask] = (nm - np.sin(nm)) / nm ** 3
mask = ~mask
nm = norm[mask]
k1[mask] = 1/12 - nm ** 2 / 180
k2[mask] = -1/60 + nm ** 2 / 12604
k3[mask] = 1/6 - nm ** 2 / 120
dp = dp[:, None]
k1 = k1[:, None]
k2 = k2[:, None]
k3 = k3[:, None]
return dp * (k1 * cp + k2 * ccp) + k3 * dccp
def _compute_angular_rate(rotvecs, rotvecs_dot):
"""Compute angular rates given rotation vectors and its derivatives.
Parameters
----------
rotvecs : ndarray, shape (n, 3)
Set of rotation vectors.
rotvecs_dot : ndarray, shape (n, 3)
Set of rotation vector derivatives.
Returns
-------
ndarray, shape (n, 3)
"""
return _matrix_vector_product_of_stacks(
_rotvec_dot_to_angular_rate_matrix(rotvecs), rotvecs_dot)
def _compute_angular_acceleration(rotvecs, rotvecs_dot, rotvecs_dot_dot):
"""Compute angular acceleration given rotation vector and its derivatives.
Parameters
----------
rotvecs : ndarray, shape (n, 3)
Set of rotation vectors.
rotvecs_dot : ndarray, shape (n, 3)
Set of rotation vector derivatives.
rotvecs_dot_dot : ndarray, shape (n, 3)
Set of rotation vector second derivatives.
Returns
-------
ndarray, shape (n, 3)
"""
return (_compute_angular_rate(rotvecs, rotvecs_dot_dot) +
_angular_acceleration_nonlinear_term(rotvecs, rotvecs_dot))
def _create_block_3_diagonal_matrix(A, B, d):
"""Create a 3-diagonal block matrix as banded.
The matrix has the following structure:
DB...
ADB..
.ADB.
..ADB
...AD
The blocks A, B and D are 3-by-3 matrices. The D matrices has the form
d * I.
Parameters
----------
A : ndarray, shape (n, 3, 3)
Stack of A blocks.
B : ndarray, shape (n, 3, 3)
Stack of B blocks.
d : ndarray, shape (n + 1,)
Values for diagonal blocks.
Returns
-------
ndarray, shape (11, 3 * (n + 1))
Matrix in the banded form as used by `scipy.linalg.solve_banded`.
"""
ind = np.arange(3)
ind_blocks = np.arange(len(A))
A_i = np.empty_like(A, dtype=int)
A_i[:] = ind[:, None]
A_i += 3 * (1 + ind_blocks[:, None, None])
A_j = np.empty_like(A, dtype=int)
A_j[:] = ind
A_j += 3 * ind_blocks[:, None, None]
B_i = np.empty_like(B, dtype=int)
B_i[:] = ind[:, None]
B_i += 3 * ind_blocks[:, None, None]
B_j = np.empty_like(B, dtype=int)
B_j[:] = ind
B_j += 3 * (1 + ind_blocks[:, None, None])
diag_i = diag_j = np.arange(3 * len(d))
i = np.hstack((A_i.ravel(), B_i.ravel(), diag_i))
j = np.hstack((A_j.ravel(), B_j.ravel(), diag_j))
values = np.hstack((A.ravel(), B.ravel(), np.repeat(d, 3)))
u = 5
l = 5
result = np.zeros((u + l + 1, 3 * len(d)))
result[u + i - j, j] = values
return result
class RotationSpline:
"""Interpolate rotations with continuous angular rate and acceleration.
The rotation vectors between each consecutive orientation are cubic
functions of time and it is guaranteed that angular rate and acceleration
are continuous. Such interpolation are analogous to cubic spline
interpolation.
Refer to [1]_ for math and implementation details.
Parameters
----------
times : array_like, shape (N,)
Times of the known rotations. At least 2 times must be specified.
rotations : `Rotation` instance
Rotations to perform the interpolation between. Must contain N
rotations.
Methods
-------
__call__
References
----------
.. [1] `Smooth Attitude Interpolation
<https://github.com/scipy/scipy/files/2932755/attitude_interpolation.pdf>`_
Examples
--------
>>> from scipy.spatial.transform import Rotation, RotationSpline
>>> import numpy as np
Define the sequence of times and rotations from the Euler angles:
>>> times = [0, 10, 20, 40]
>>> angles = [[-10, 20, 30], [0, 15, 40], [-30, 45, 30], [20, 45, 90]]
>>> rotations = Rotation.from_euler('XYZ', angles, degrees=True)
Create the interpolator object:
>>> spline = RotationSpline(times, rotations)
Interpolate the Euler angles, angular rate and acceleration:
>>> angular_rate = np.rad2deg(spline(times, 1))
>>> angular_acceleration = np.rad2deg(spline(times, 2))
>>> times_plot = np.linspace(times[0], times[-1], 100)
>>> angles_plot = spline(times_plot).as_euler('XYZ', degrees=True)
>>> angular_rate_plot = np.rad2deg(spline(times_plot, 1))
>>> angular_acceleration_plot = np.rad2deg(spline(times_plot, 2))
On this plot you see that Euler angles are continuous and smooth:
>>> import matplotlib.pyplot as plt
>>> plt.plot(times_plot, angles_plot)
>>> plt.plot(times, angles, 'x')
>>> plt.title("Euler angles")
>>> plt.show()
The angular rate is also smooth:
>>> plt.plot(times_plot, angular_rate_plot)
>>> plt.plot(times, angular_rate, 'x')
>>> plt.title("Angular rate")
>>> plt.show()
The angular acceleration is continuous, but not smooth. Also note that
the angular acceleration is not a piecewise-linear function, because
it is different from the second derivative of the rotation vector (which
is a piecewise-linear function as in the cubic spline).
>>> plt.plot(times_plot, angular_acceleration_plot)
>>> plt.plot(times, angular_acceleration, 'x')
>>> plt.title("Angular acceleration")
>>> plt.show()
"""
# Parameters for the solver for angular rate.
MAX_ITER = 10
TOL = 1e-9
def _solve_for_angular_rates(self, dt, angular_rates, rotvecs):
angular_rate_first = angular_rates[0].copy()
A = _angular_rate_to_rotvec_dot_matrix(rotvecs)
A_inv = _rotvec_dot_to_angular_rate_matrix(rotvecs)
M = _create_block_3_diagonal_matrix(
2 * A_inv[1:-1] / dt[1:-1, None, None],
2 * A[1:-1] / dt[1:-1, None, None],
4 * (1 / dt[:-1] + 1 / dt[1:]))
b0 = 6 * (rotvecs[:-1] * dt[:-1, None] ** -2 +
rotvecs[1:] * dt[1:, None] ** -2)
b0[0] -= 2 / dt[0] * A_inv[0].dot(angular_rate_first)
b0[-1] -= 2 / dt[-1] * A[-1].dot(angular_rates[-1])
for iteration in range(self.MAX_ITER):
rotvecs_dot = _matrix_vector_product_of_stacks(A, angular_rates)
delta_beta = _angular_acceleration_nonlinear_term(
rotvecs[:-1], rotvecs_dot[:-1])
b = b0 - delta_beta
angular_rates_new = solve_banded((5, 5), M, b.ravel())
angular_rates_new = angular_rates_new.reshape((-1, 3))
delta = np.abs(angular_rates_new - angular_rates[:-1])
angular_rates[:-1] = angular_rates_new
if np.all(delta < self.TOL * (1 + np.abs(angular_rates_new))):
break
rotvecs_dot = _matrix_vector_product_of_stacks(A, angular_rates)
angular_rates = np.vstack((angular_rate_first, angular_rates[:-1]))
return angular_rates, rotvecs_dot
def __init__(self, times, rotations):
from scipy.interpolate import PPoly
if rotations.single:
raise ValueError("`rotations` must be a sequence of rotations.")
if len(rotations) == 1:
raise ValueError("`rotations` must contain at least 2 rotations.")
times = np.asarray(times, dtype=float)
if times.ndim != 1:
raise ValueError("`times` must be 1-dimensional.")
if len(times) != len(rotations):
raise ValueError("Expected number of rotations to be equal to "
"number of timestamps given, got {} rotations "
"and {} timestamps."
.format(len(rotations), len(times)))
dt = np.diff(times)
if np.any(dt <= 0):
raise ValueError("Values in `times` must be in a strictly "
"increasing order.")
rotvecs = (rotations[:-1].inv() * rotations[1:]).as_rotvec()
angular_rates = rotvecs / dt[:, None]
if len(rotations) == 2:
rotvecs_dot = angular_rates
else:
angular_rates, rotvecs_dot = self._solve_for_angular_rates(
dt, angular_rates, rotvecs)
dt = dt[:, None]
coeff = np.empty((4, len(times) - 1, 3))
coeff[0] = (-2 * rotvecs + dt * angular_rates
+ dt * rotvecs_dot) / dt ** 3
coeff[1] = (3 * rotvecs - 2 * dt * angular_rates
- dt * rotvecs_dot) / dt ** 2
coeff[2] = angular_rates
coeff[3] = 0
self.times = times
self.rotations = rotations
self.interpolator = PPoly(coeff, times)
def __call__(self, times, order=0):
"""Compute interpolated values.
Parameters
----------
times : float or array_like
Times of interest.
order : {0, 1, 2}, optional
Order of differentiation:
* 0 (default) : return Rotation
* 1 : return the angular rate in rad/sec
* 2 : return the angular acceleration in rad/sec/sec
Returns
-------
Interpolated Rotation, angular rate or acceleration.
"""
if order not in [0, 1, 2]:
raise ValueError("`order` must be 0, 1 or 2.")
times = np.asarray(times, dtype=float)
if times.ndim > 1:
raise ValueError("`times` must be at most 1-dimensional.")
singe_time = times.ndim == 0
times = np.atleast_1d(times)
rotvecs = self.interpolator(times)
if order == 0:
index = np.searchsorted(self.times, times, side='right')
index -= 1
index[index < 0] = 0
n_segments = len(self.times) - 1
index[index > n_segments - 1] = n_segments - 1
result = self.rotations[index] * Rotation.from_rotvec(rotvecs)
elif order == 1:
rotvecs_dot = self.interpolator(times, 1)
result = _compute_angular_rate(rotvecs, rotvecs_dot)
elif order == 2:
rotvecs_dot = self.interpolator(times, 1)
rotvecs_dot_dot = self.interpolator(times, 2)
result = _compute_angular_acceleration(rotvecs, rotvecs_dot,
rotvecs_dot_dot)
else:
assert False
if singe_time:
result = result[0]
return result

View File

@ -0,0 +1,21 @@
# This file is not meant for public use and will be removed in SciPy v2.0.0.
# Use the `scipy.spatial` namespace for importing the functions
# included below.
from scipy._lib.deprecation import _sub_module_deprecation
__all__ = [ # noqa: F822
'Rotation',
'Slerp',
]
def __dir__():
return __all__
def __getattr__(name):
return _sub_module_deprecation(sub_package="spatial.transform", module="rotation",
private_modules=["_rotation"], all=__all__,
attribute=name)

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,169 @@
import pytest
import numpy as np
from numpy.testing import assert_array_almost_equal
from scipy.spatial.transform import Rotation
from scipy.optimize import linear_sum_assignment
from scipy.spatial.distance import cdist
from scipy.constants import golden as phi
from scipy.spatial import cKDTree
TOL = 1E-12
NS = range(1, 13)
NAMES = ["I", "O", "T"] + ["C%d" % n for n in NS] + ["D%d" % n for n in NS]
SIZES = [60, 24, 12] + list(NS) + [2 * n for n in NS]
def _calculate_rmsd(P, Q):
"""Calculates the root-mean-square distance between the points of P and Q.
The distance is taken as the minimum over all possible matchings. It is
zero if P and Q are identical and non-zero if not.
"""
distance_matrix = cdist(P, Q, metric='sqeuclidean')
matching = linear_sum_assignment(distance_matrix)
return np.sqrt(distance_matrix[matching].sum())
def _generate_pyramid(n, axis):
thetas = np.linspace(0, 2 * np.pi, n + 1)[:-1]
P = np.vstack([np.zeros(n), np.cos(thetas), np.sin(thetas)]).T
P = np.concatenate((P, [[1, 0, 0]]))
return np.roll(P, axis, axis=1)
def _generate_prism(n, axis):
thetas = np.linspace(0, 2 * np.pi, n + 1)[:-1]
bottom = np.vstack([-np.ones(n), np.cos(thetas), np.sin(thetas)]).T
top = np.vstack([+np.ones(n), np.cos(thetas), np.sin(thetas)]).T
P = np.concatenate((bottom, top))
return np.roll(P, axis, axis=1)
def _generate_icosahedron():
x = np.array([[0, -1, -phi],
[0, -1, +phi],
[0, +1, -phi],
[0, +1, +phi]])
return np.concatenate([np.roll(x, i, axis=1) for i in range(3)])
def _generate_octahedron():
return np.array([[-1, 0, 0], [+1, 0, 0], [0, -1, 0],
[0, +1, 0], [0, 0, -1], [0, 0, +1]])
def _generate_tetrahedron():
return np.array([[1, 1, 1], [1, -1, -1], [-1, 1, -1], [-1, -1, 1]])
@pytest.mark.parametrize("name", [-1, None, True, np.array(['C3'])])
def test_group_type(name):
with pytest.raises(ValueError,
match="must be a string"):
Rotation.create_group(name)
@pytest.mark.parametrize("name", ["Q", " ", "CA", "C ", "DA", "D ", "I2", ""])
def test_group_name(name):
with pytest.raises(ValueError,
match="must be one of 'I', 'O', 'T', 'Dn', 'Cn'"):
Rotation.create_group(name)
@pytest.mark.parametrize("name", ["C0", "D0"])
def test_group_order_positive(name):
with pytest.raises(ValueError,
match="Group order must be positive"):
Rotation.create_group(name)
@pytest.mark.parametrize("axis", ['A', 'b', 0, 1, 2, 4, False, None])
def test_axis_valid(axis):
with pytest.raises(ValueError,
match="`axis` must be one of"):
Rotation.create_group("C1", axis)
def test_icosahedral():
"""The icosahedral group fixes the rotations of an icosahedron. Here we
test that the icosahedron is invariant after application of the elements
of the rotation group."""
P = _generate_icosahedron()
for g in Rotation.create_group("I"):
g = Rotation.from_quat(g.as_quat())
assert _calculate_rmsd(P, g.apply(P)) < TOL
def test_octahedral():
"""Test that the octahedral group correctly fixes the rotations of an
octahedron."""
P = _generate_octahedron()
for g in Rotation.create_group("O"):
assert _calculate_rmsd(P, g.apply(P)) < TOL
def test_tetrahedral():
"""Test that the tetrahedral group correctly fixes the rotations of a
tetrahedron."""
P = _generate_tetrahedron()
for g in Rotation.create_group("T"):
assert _calculate_rmsd(P, g.apply(P)) < TOL
@pytest.mark.parametrize("n", NS)
@pytest.mark.parametrize("axis", 'XYZ')
def test_dicyclic(n, axis):
"""Test that the dicyclic group correctly fixes the rotations of a
prism."""
P = _generate_prism(n, axis='XYZ'.index(axis))
for g in Rotation.create_group("D%d" % n, axis=axis):
assert _calculate_rmsd(P, g.apply(P)) < TOL
@pytest.mark.parametrize("n", NS)
@pytest.mark.parametrize("axis", 'XYZ')
def test_cyclic(n, axis):
"""Test that the cyclic group correctly fixes the rotations of a
pyramid."""
P = _generate_pyramid(n, axis='XYZ'.index(axis))
for g in Rotation.create_group("C%d" % n, axis=axis):
assert _calculate_rmsd(P, g.apply(P)) < TOL
@pytest.mark.parametrize("name, size", zip(NAMES, SIZES))
def test_group_sizes(name, size):
assert len(Rotation.create_group(name)) == size
@pytest.mark.parametrize("name, size", zip(NAMES, SIZES))
def test_group_no_duplicates(name, size):
g = Rotation.create_group(name)
kdtree = cKDTree(g.as_quat())
assert len(kdtree.query_pairs(1E-3)) == 0
@pytest.mark.parametrize("name, size", zip(NAMES, SIZES))
def test_group_symmetry(name, size):
g = Rotation.create_group(name)
q = np.concatenate((-g.as_quat(), g.as_quat()))
distance = np.sort(cdist(q, q))
deltas = np.max(distance, axis=0) - np.min(distance, axis=0)
assert (deltas < TOL).all()
@pytest.mark.parametrize("name", NAMES)
def test_reduction(name):
"""Test that the elements of the rotation group are correctly
mapped onto the identity rotation."""
g = Rotation.create_group(name)
f = g.reduce(g)
assert_array_almost_equal(f.magnitude(), np.zeros(len(g)))
@pytest.mark.parametrize("name", NAMES)
def test_single_reduction(name):
g = Rotation.create_group(name)
f = g[-1].reduce(g)
assert_array_almost_equal(f.magnitude(), 0)
assert f.as_quat().shape == (4,)

View File

@ -0,0 +1,162 @@
from itertools import product
import numpy as np
from numpy.testing import assert_allclose
from pytest import raises
from scipy.spatial.transform import Rotation, RotationSpline
from scipy.spatial.transform._rotation_spline import (
_angular_rate_to_rotvec_dot_matrix,
_rotvec_dot_to_angular_rate_matrix,
_matrix_vector_product_of_stacks,
_angular_acceleration_nonlinear_term,
_create_block_3_diagonal_matrix)
def test_angular_rate_to_rotvec_conversions():
np.random.seed(0)
rv = np.random.randn(4, 3)
A = _angular_rate_to_rotvec_dot_matrix(rv)
A_inv = _rotvec_dot_to_angular_rate_matrix(rv)
# When the rotation vector is aligned with the angular rate, then
# the rotation vector rate and angular rate are the same.
assert_allclose(_matrix_vector_product_of_stacks(A, rv), rv)
assert_allclose(_matrix_vector_product_of_stacks(A_inv, rv), rv)
# A and A_inv must be reciprocal to each other.
I_stack = np.empty((4, 3, 3))
I_stack[:] = np.eye(3)
assert_allclose(np.matmul(A, A_inv), I_stack, atol=1e-15)
def test_angular_rate_nonlinear_term():
# The only simple test is to check that the term is zero when
# the rotation vector
np.random.seed(0)
rv = np.random.rand(4, 3)
assert_allclose(_angular_acceleration_nonlinear_term(rv, rv), 0,
atol=1e-19)
def test_create_block_3_diagonal_matrix():
np.random.seed(0)
A = np.empty((4, 3, 3))
A[:] = np.arange(1, 5)[:, None, None]
B = np.empty((4, 3, 3))
B[:] = -np.arange(1, 5)[:, None, None]
d = 10 * np.arange(10, 15)
banded = _create_block_3_diagonal_matrix(A, B, d)
# Convert the banded matrix to the full matrix.
k, l = list(zip(*product(np.arange(banded.shape[0]),
np.arange(banded.shape[1]))))
k = np.asarray(k)
l = np.asarray(l)
i = k - 5 + l
j = l
values = banded.ravel()
mask = (i >= 0) & (i < 15)
i = i[mask]
j = j[mask]
values = values[mask]
full = np.zeros((15, 15))
full[i, j] = values
zero = np.zeros((3, 3))
eye = np.eye(3)
# Create the reference full matrix in the most straightforward manner.
ref = np.block([
[d[0] * eye, B[0], zero, zero, zero],
[A[0], d[1] * eye, B[1], zero, zero],
[zero, A[1], d[2] * eye, B[2], zero],
[zero, zero, A[2], d[3] * eye, B[3]],
[zero, zero, zero, A[3], d[4] * eye],
])
assert_allclose(full, ref, atol=1e-19)
def test_spline_2_rotations():
times = [0, 10]
rotations = Rotation.from_euler('xyz', [[0, 0, 0], [10, -20, 30]],
degrees=True)
spline = RotationSpline(times, rotations)
rv = (rotations[0].inv() * rotations[1]).as_rotvec()
rate = rv / (times[1] - times[0])
times_check = np.array([-1, 5, 12])
dt = times_check - times[0]
rv_ref = rate * dt[:, None]
assert_allclose(spline(times_check).as_rotvec(), rv_ref)
assert_allclose(spline(times_check, 1), np.resize(rate, (3, 3)))
assert_allclose(spline(times_check, 2), 0, atol=1e-16)
def test_constant_attitude():
times = np.arange(10)
rotations = Rotation.from_rotvec(np.ones((10, 3)))
spline = RotationSpline(times, rotations)
times_check = np.linspace(-1, 11)
assert_allclose(spline(times_check).as_rotvec(), 1, rtol=1e-15)
assert_allclose(spline(times_check, 1), 0, atol=1e-17)
assert_allclose(spline(times_check, 2), 0, atol=1e-17)
assert_allclose(spline(5.5).as_rotvec(), 1, rtol=1e-15)
assert_allclose(spline(5.5, 1), 0, atol=1e-17)
assert_allclose(spline(5.5, 2), 0, atol=1e-17)
def test_spline_properties():
times = np.array([0, 5, 15, 27])
angles = [[-5, 10, 27], [3, 5, 38], [-12, 10, 25], [-15, 20, 11]]
rotations = Rotation.from_euler('xyz', angles, degrees=True)
spline = RotationSpline(times, rotations)
assert_allclose(spline(times).as_euler('xyz', degrees=True), angles)
assert_allclose(spline(0).as_euler('xyz', degrees=True), angles[0])
h = 1e-8
rv0 = spline(times).as_rotvec()
rvm = spline(times - h).as_rotvec()
rvp = spline(times + h).as_rotvec()
# rtol bumped from 1e-15 to 1.5e-15 in gh18414 for linux 32 bit
assert_allclose(rv0, 0.5 * (rvp + rvm), rtol=1.5e-15)
r0 = spline(times, 1)
rm = spline(times - h, 1)
rp = spline(times + h, 1)
assert_allclose(r0, 0.5 * (rm + rp), rtol=1e-14)
a0 = spline(times, 2)
am = spline(times - h, 2)
ap = spline(times + h, 2)
assert_allclose(a0, am, rtol=1e-7)
assert_allclose(a0, ap, rtol=1e-7)
def test_error_handling():
raises(ValueError, RotationSpline, [1.0], Rotation.random())
r = Rotation.random(10)
t = np.arange(10).reshape(5, 2)
raises(ValueError, RotationSpline, t, r)
t = np.arange(9)
raises(ValueError, RotationSpline, t, r)
t = np.arange(10)
t[5] = 0
raises(ValueError, RotationSpline, t, r)
t = np.arange(10)
s = RotationSpline(t, r)
raises(ValueError, s, 10, -1)
raises(ValueError, s, np.arange(10).reshape(5, 2))