# Source code for pytransform3d.rotations._utils

"""Utility functions for rotations."""
import warnings
import math
import numpy as np
from ._constants import unitz, eps

[docs]def norm_vector(v):
"""Normalize vector.

Parameters
----------
v : array-like, shape (n,)
nd vector

Returns
-------
u : array, shape (n,)
nd unit vector with norm 1 or the zero vector
"""
norm = np.linalg.norm(v)
if norm == 0.0:
return v

return np.asarray(v) / norm

[docs]def norm_matrix(R):
"""Normalize rotation matrix.

Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix with small numerical errors

Returns
-------
R : array, shape (3, 3)
Normalized rotation matrix
"""
R = np.asarray(R)
c2 = R[:, 1]
c3 = norm_vector(R[:, 2])
c1 = norm_vector(np.cross(c2, c3))
c2 = norm_vector(np.cross(c3, c1))
return np.column_stack((c1, c2, c3))

[docs]def norm_angle(a):
"""Normalize angle to (-pi, pi].

Parameters
----------
a : float or array-like, shape (n,)

Returns
-------
a_norm : float or array-like, shape (n,)
"""
# Source of the solution: http://stackoverflow.com/a/32266181
return -((np.pi - np.asarray(a)) % (2.0 * np.pi) - np.pi)

[docs]def norm_axis_angle(a):
"""Normalize axis-angle representation.

Parameters
----------
a : array-like, shape (4,)
Axis of rotation and rotation angle: (x, y, z, angle)

Returns
-------
a : array-like, shape (4,)
Axis of rotation and rotation angle: (x, y, z, angle). The length
of the axis vector is 1 and the angle is in [0, pi). No rotation
is represented by [1, 0, 0, 0].
"""
angle = a[3]
norm = np.linalg.norm(a[:3])
if angle == 0.0 or norm == 0.0:
return np.array([1.0, 0.0, 0.0, 0.0])

res = np.empty(4)
res[:3] = a[:3] / norm

angle = norm_angle(angle)
if angle < 0.0:
angle *= -1.0
res[:3] *= -1.0

res[3] = angle

return res

[docs]def norm_compact_axis_angle(a):
"""Normalize compact axis-angle representation.

Parameters
----------
a : array-like, shape (3,)
Axis of rotation and rotation angle: angle * (x, y, z)

Returns
-------
a : array-like, shape (3,)
Axis of rotation and rotation angle: angle * (x, y, z).
The angle is in [0, pi). No rotation is represented by [0, 0, 0].
"""
angle = np.linalg.norm(a)
if angle == 0.0:
return np.zeros(3)
axis = a / angle
return axis * norm_angle(angle)

[docs]def perpendicular_to_vectors(a, b):
"""Compute perpendicular vector to two other vectors.

Parameters
----------
a : array-like, shape (3,)
3d vector

b : array-like, shape (3,)
3d vector

Returns
-------
c : array-like, shape (3,)
3d vector that is orthogonal to a and b
"""
return np.cross(a, b)

def perpendicular_to_vector(a):
"""Compute perpendicular vector to one other vector.

There is an infinite number of solutions to this problem. Thus, we
restrict the solutions to [1, 0, z] and return [0, 0, 1] if the
z component of a is 0.

Parameters
----------
a : array-like, shape (3,)
3d vector

Returns
-------
b : array-like, shape (3,)
A 3d vector that is orthogonal to a. It does not necessarily have
unit length.
"""
if abs(a[2]) < eps:
return np.copy(unitz)
# Now that we solved the problem for [x, y, 0], we can solve it for all
# other vectors by restricting solutions to [1, 0, z] and find z.
# The dot product of orthogonal vectors is 0, thus
# a[0] * 1 + a[1] * 0 + a[2] * z == 0 or -a[0] / a[2] = z
return np.array([1.0, 0.0, -a[0] / a[2]])

[docs]def angle_between_vectors(a, b, fast=False):
"""Compute angle between two vectors.

Parameters
----------
a : array-like, shape (n,)
nd vector

b : array-like, shape (n,)
nd vector

fast : bool, optional (default: False)
Use fast implementation instead of numerically stable solution

Returns
-------
angle : float
Angle between a and b
"""
if len(a) != 3 or fast:
return np.arccos(
np.clip(np.dot(a, b) / (np.linalg.norm(a) * np.linalg.norm(b)),
-1.0, 1.0))
else:
return np.arctan2(np.linalg.norm(np.cross(a, b)), np.dot(a, b))

[docs]def vector_projection(a, b):
"""Orthogonal projection of vector a on vector b.

Parameters
----------
a : array-like, shape (3,)
Vector a that will be projected on vector b

b : array-like, shape (3,)
Vector b on which vector a will be projected

Returns
-------
a_on_b : array, shape (3,)
Vector a
"""
b_norm_squared = np.dot(b, b)
if b_norm_squared == 0.0:
return np.zeros(3)
return np.dot(a, b) * b / b_norm_squared

[docs]def plane_basis_from_normal(plane_normal):
"""Compute two basis vectors of a plane from the plane's normal vector.

Note that there are infinitely many solutions because any rotation of the
basis vectors about the normal is also a solution. This function
deterministically picks one of the solutions.

The two basis vectors of the plane together with the normal form an
orthonormal basis in 3D space and could be used as columns to form a
rotation matrix.

Parameters
----------
plane_normal : array-like, shape (3,)
Plane normal of unit length.

Returns
-------
x_axis : array, shape (3,)
x-axis of the plane.

y_axis : array, shape (3,)
y-axis of the plane.
"""
if abs(plane_normal[0]) >= abs(plane_normal[1]):
# x or z is the largest magnitude component, swap them
length = math.sqrt(
plane_normal[0] * plane_normal[0]
+ plane_normal[2] * plane_normal[2])
x_axis = np.array([-plane_normal[2] / length, 0.0,
plane_normal[0] / length])
y_axis = np.array([
plane_normal[1] * x_axis[2],
plane_normal[2] * x_axis[0] - plane_normal[0] * x_axis[2],
-plane_normal[1] * x_axis[0]])
else:
# y or z is the largest magnitude component, swap them
length = math.sqrt(plane_normal[1] * plane_normal[1]
+ plane_normal[2] * plane_normal[2])
x_axis = np.array([0.0, plane_normal[2] / length,
-plane_normal[1] / length])
y_axis = np.array([
plane_normal[1] * x_axis[2] - plane_normal[2] * x_axis[1],
-plane_normal[0] * x_axis[2], plane_normal[0] * x_axis[1]])
return x_axis, y_axis

[docs]def random_vector(random_state=np.random.RandomState(0), n=3):
r"""Generate an nd vector with normally distributed components.

Each component will be sampled from :math:\mathcal{N}(\mu=0, \sigma=1).

Parameters
----------
random_state : np.random.RandomState, optional (default: random seed 0)
Random number generator

n : int, optional (default: 3)
Number of vector components

Returns
-------
v : array, shape (n,)
Random vector
"""
return random_state.randn(n)

[docs]def random_axis_angle(random_state=np.random.RandomState(0)):
r"""Generate random axis-angle.

The angle will be sampled uniformly from the interval :math:[0, \pi)
and each component of the rotation axis will be sampled from
:math:\mathcal{N}(\mu=0, \sigma=1) and than the axis will be normalized
to length 1.

Parameters
----------
random_state : np.random.RandomState, optional (default: random seed 0)
Random number generator

Returns
-------
a : array, shape (4,)
Axis of rotation and rotation angle: (x, y, z, angle)
"""
angle = np.pi * random_state.rand()
a = np.array([0, 0, 0, angle])
a[:3] = norm_vector(random_state.randn(3))
return a

[docs]def random_compact_axis_angle(random_state=np.random.RandomState(0)):
r"""Generate random compact axis-angle.

The angle will be sampled uniformly from the interval :math:[0, \pi)
and each component of the rotation axis will be sampled from
:math:\mathcal{N}(\mu=0, \sigma=1) and than the axis will be normalized
to length 1.

Parameters
----------
random_state : np.random.RandomState, optional (default: random seed 0)
Random number generator

Returns
-------
a : array, shape (3,)
Axis of rotation and rotation angle: angle * (x, y, z)
"""
a = random_axis_angle(random_state)
return a[:3] * a[3]

[docs]def random_quaternion(random_state=np.random.RandomState(0)):
"""Generate random quaternion.

Parameters
----------
random_state : np.random.RandomState, optional (default: random seed 0)
Random number generator

Returns
-------
q : array, shape (4,)
Unit quaternion to represent rotation: (w, x, y, z)
"""
return norm_vector(random_state.randn(4))

[docs]def check_skew_symmetric_matrix(V, tolerance=1e-6, strict_check=True):
"""Input validation of a skew-symmetric matrix.

Check whether the transpose of the matrix is its negative:

.. math::

V^T = -V

Parameters
----------
V : array-like, shape (3, 3)
Cross-product matrix

tolerance : float, optional (default: 1e-6)
Tolerance threshold for checks.

strict_check : bool, optional (default: True)
Raise a ValueError if V.T is not numerically close enough to -V.
Otherwise we print a warning.

Returns
-------
V : array, shape (3, 3)
Validated cross-product matrix

Raises
------
ValueError
If input is invalid
"""
V = np.asarray(V, dtype=np.float64)
if V.ndim != 2 or V.shape[0] != 3 or V.shape[1] != 3:
raise ValueError("Expected skew-symmetric matrix with shape (3, 3), "
"got array-like object with shape %s" % (V.shape,))
if not np.allclose(V.T, -V, atol=tolerance):
error_msg = ("Expected skew-symmetric matrix, but it failed the test "
"V.T = %r\n-V = %r" % (V.T, -V))
if strict_check:
raise ValueError(error_msg)
else:
warnings.warn(error_msg)
return V

[docs]def check_matrix(R, tolerance=1e-6, strict_check=True):
"""Input validation of a rotation matrix.

We check whether R multiplied by its inverse is approximately the identity
matrix and the determinant is approximately 1.

Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix

tolerance : float, optional (default: 1e-6)
Tolerance threshold for checks. Default tolerance is the same as in
assert_rotation_matrix(R).

strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.

Returns
-------
R : array, shape (3, 3)
Validated rotation matrix

Raises
------
ValueError
If input is invalid
"""
R = np.asarray(R, dtype=np.float64)
if R.ndim != 2 or R.shape[0] != 3 or R.shape[1] != 3:
raise ValueError("Expected rotation matrix with shape (3, 3), got "
"array-like object with shape %s" % (R.shape,))
RRT = np.dot(R, R.T)
if not np.allclose(RRT, np.eye(3), atol=tolerance):
error_msg = ("Expected rotation matrix, but it failed the test "
"for inversion by transposition. np.dot(R, R.T) "
"gives %r" % RRT)
if strict_check:
raise ValueError(error_msg)
else:
warnings.warn(error_msg)
R_det = np.linalg.det(R)
if abs(R_det - 1) > tolerance:
error_msg = ("Expected rotation matrix, but it failed the test "
"for the determinant, which should be 1 but is %g; "
"that is, it probably represents a rotoreflection"
% R_det)
if strict_check:
raise ValueError(error_msg)
else:
warnings.warn(error_msg)
return R

[docs]def check_axis_angle(a):
"""Input validation of axis-angle representation.

Parameters
----------
a : array-like, shape (4,)
Axis of rotation and rotation angle: (x, y, z, angle)

Returns
-------
a : array, shape (4,)
Validated axis of rotation and rotation angle: (x, y, z, angle)

Raises
------
ValueError
If input is invalid
"""
a = np.asarray(a, dtype=np.float64)
if a.ndim != 1 or a.shape[0] != 4:
raise ValueError("Expected axis and angle in array with shape (4,), "
"got array-like object with shape %s" % (a.shape,))
return norm_axis_angle(a)

[docs]def check_compact_axis_angle(a):
"""Input validation of compact axis-angle representation.

Parameters
----------
a : array-like, shape (3,)
Axis of rotation and rotation angle: angle * (x, y, z)

Returns
-------
a : array, shape (3,)
Validated axis of rotation and rotation angle: angle * (x, y, z)

Raises
------
ValueError
If input is invalid
"""
a = np.asarray(a, dtype=np.float64)
if a.ndim != 1 or a.shape[0] != 3:
raise ValueError("Expected axis and angle in array with shape (3,), "
"got array-like object with shape %s" % (a.shape,))
return norm_compact_axis_angle(a)

[docs]def check_quaternion(q, unit=True):
"""Input validation of quaternion representation.

Parameters
----------
q : array-like, shape (4,)
Quaternion to represent rotation: (w, x, y, z)

unit : bool, optional (default: True)
Normalize the quaternion so that it is a unit quaternion

Returns
-------
q : array-like, shape (4,)
Validated quaternion to represent rotation: (w, x, y, z)

Raises
------
ValueError
If input is invalid
"""
q = np.asarray(q, dtype=np.float64)
if q.ndim != 1 or q.shape[0] != 4:
raise ValueError("Expected quaternion with shape (4,), got "
"array-like object with shape %s" % (q.shape,))
if unit:
return norm_vector(q)
else:
return q

[docs]def check_quaternions(Q, unit=True):
"""Input validation of quaternion representation.

Parameters
----------
Q : array-like, shape (n_steps, 4)
Quaternions to represent rotations: (w, x, y, z)

unit : bool, optional (default: True)
Normalize the quaternions so that they are unit quaternions

Returns
-------
Q : array-like, shape (n_steps, 4)
Validated quaternions to represent rotations: (w, x, y, z)

Raises
------
ValueError
If input is invalid
"""
Q_checked = np.asarray(Q, dtype=np.float64)
if Q_checked.ndim != 2 or Q_checked.shape[1] != 4:
raise ValueError(
"Expected quaternion array with shape (n_steps, 4), got "
"array-like object with shape %s" % (Q_checked.shape,))
if unit:
for i in range(len(Q)):
Q_checked[i] = norm_vector(Q_checked[i])
return Q_checked

[docs]def check_rotor(rotor):
"""Input validation of rotor.

Parameters
----------
rotor : array-like, shape (4,)
Rotor: (a, b_yz, b_zx, b_xy)

Returns
-------
rotor : array, shape (4,)
Validated rotor (with unit norm): (a, b_yz, b_zx, b_xy)

Raises
------
ValueError
If input is invalid
"""
rotor = np.asarray(rotor, dtype=np.float64)
if rotor.ndim != 1 or rotor.shape[0] != 4:
raise ValueError("Expected rotor with shape (4,), got "
"array-like object with shape %s" % (rotor.shape,))
return norm_vector(rotor)