def rotate_points_2d(points, axis, angle, origin=None):
"""Rotates points around an arbitrary axis in 2D.
Parameters:
points (sequence of sequence of float): XY coordinates of the points.
axis (sequence of float): The rotation axis.
angle (float): the angle of rotation in radians.
origin (sequence of float): Optional. The origin of the rotation axis.
Default is ``[0.0, 0.0, 0.0]``.
Returns:
list: the rotated points
References:
https://en.wikipedia.org/wiki/Rotation_matrix
"""
if not origin:
origin = [0.0, 0.0]
# rotation matrix
x, y = normalize_vector_2d(axis)
cosa = cos(angle)
sina = sin(angle)
R = [[cosa, -sina], [sina, cosa]]
# translate points
points = translate_points_2d(points, scale_vector_2d(origin, -1.0))
# rotate points
points = [multiply_matrix_vector(R, point) for point in points]
# translate points back
points = translate_points_2d(points, origin)
return points
def rotate_points_xy(points, angle, origin=None):
"""Rotates points in the XY plane around the Z axis at a specific origin.
Parameters
----------
points : sequence[[float, float, float] | :class:`compas.geometry.Point`]
A list of points.
angle : float
The angle of rotation in radians.
origin : [float, float, float] | :class:`compas.geometry.Point`, optional
The origin of the rotation axis.
Default is ``[0.0, 0.0, 0.0]``.
Returns
-------
list[[float, float, 0.0]]
The rotated points in the XY plane (Z=0).
Examples
--------
>>>
"""
if not origin:
origin = [0.0, 0.0, 0.0]
cosa = math.cos(angle)
sina = math.sin(angle)
R = [[cosa, -sina, 0.0], [sina, cosa, 0.0], [0.0, 0.0, 1.0]]
# translate points
points = translate_points_xy(points, scale_vector_xy(origin, -1.0))
# rotate points
points = [multiply_matrix_vector(R, point) for point in points]
# translate points back
points = translate_points_xy(points, origin)
return points
def decompose_matrix(M):
"""Calculates the components of rotation, translation, scale, shear, and
perspective of a given transformation matrix M. [1]_
Parameters
----------
M : list[list[float]]
The square matrix of any dimension.
Raises
------
ValueError
If matrix is singular or degenerative.
Returns
-------
scale : [float, float, float]
The 3 scale factors in x-, y-, and z-direction.
shear : [float, float, float]
The 3 shear factors for x-y, x-z, and y-z axes.
angles : [float, float, float]
The rotation specified through the 3 Euler angles about static x, y, z axes.
translation : [float, float, float]
The 3 values of translation.
perspective : [float, float, float, float]
The 4 perspective entries of the matrix.
Examples
--------
>>> trans1 = [1, 2, 3]
>>> angle1 = [-2.142, 1.141, -0.142]
>>> scale1 = [0.123, 2, 0.5]
>>> T = matrix_from_translation(trans1)
>>> R = matrix_from_euler_angles(angle1)
>>> S = matrix_from_scale_factors(scale1)
>>> M = multiply_matrices(multiply_matrices(T, R), S)
>>> # M = compose_matrix(scale1, None, angle1, trans1, None)
>>> scale2, shear2, angle2, trans2, persp2 = decompose_matrix(M)
>>> allclose(scale1, scale2)
True
>>> allclose(angle1, angle2)
True
>>> allclose(trans1, trans2)
True
References
----------
.. [1] Slabaugh, 1999. *Computing Euler angles from a rotation matrix*.
Available at: http://www.gregslabaugh.net/publications/euler.pdf
"""
fabs = math.fabs
cos = math.cos
atan2 = math.atan2
asin = math.asin
pi = math.pi
detM = matrix_determinant(M) # raises ValueError if matrix is not squared
if detM == 0:
ValueError("The matrix is singular.")
Mt = transpose_matrix(M)
if abs(Mt[3][3]) < _EPS:
raise ValueError('The element [3,3] of the matrix is zero.')
for i in range(4):
for j in range(4):
Mt[i][j] /= Mt[3][3]
translation = [M[0][3], M[1][3], M[2][3]]
# scale, shear, angles
scale = [0.0, 0.0, 0.0]
shear = [0.0, 0.0, 0.0]
angles = [0.0, 0.0, 0.0]
# copy Mt[:3, :3] into row
row = [[0, 0, 0] for i in range(3)]
for i in range(3):
for j in range(3):
row[i][j] = Mt[i][j]
scale[0] = norm_vector(row[0])
for i in range(3):
row[0][i] /= scale[0]
shear[0] = dot_vectors(row[0], row[1])
for i in range(3):
row[1][i] -= row[0][i] * shear[0]
scale[1] = norm_vector(row[1])
for i in range(3):
row[1][i] /= scale[1]
shear[0] /= scale[1]
shear[1] = dot_vectors(row[0], row[2])
for i in range(3):
row[2][i] -= row[0][i] * shear[1]
shear[2] = dot_vectors(row[1], row[2])
for i in range(3):
row[2][i] -= row[0][i] * shear[2]
scale[2] = norm_vector(row[2])
for i in range(3):
row[2][i] /= scale[2]
shear[1] /= scale[2]
shear[2] /= scale[2]
if dot_vectors(row[0], cross_vectors(row[1], row[2])) < 0:
scale = [-x for x in scale]
row = [[-x for x in y] for y in row]
# angles
if row[0][2] != -1. and row[0][2] != 1.:
beta1 = asin(-row[0][2])
# beta2 = pi - beta1
alpha1 = atan2(row[1][2] / cos(beta1), row[2][2] / cos(beta1))
# alpha2 = atan2(row[1][2] / cos(beta2), row[2][2] / cos(beta2))
gamma1 = atan2(row[0][1] / cos(beta1), row[0][0] / cos(beta1))
# gamma2 = atan2(row[0][1] / cos(beta2), row[0][0] / cos(beta2))
angles = [alpha1, beta1, gamma1]
else:
gamma = 0.
if row[0][2] == -1.:
beta = pi / 2.
alpha = gamma + atan2(row[1][0], row[2][0])
else: # row[0][2] == 1
beta = -pi / 2.
alpha = -gamma + atan2(-row[1][0], -row[2][0])
angles = [alpha, beta, gamma]
# perspective
if fabs(Mt[0][3]) > _EPS and fabs(Mt[1][3]) > _EPS and fabs(
Mt[2][3]) > _EPS:
P = deepcopy(Mt)
P[0][3], P[1][3], P[2][3], P[3][3] = 0.0, 0.0, 0.0, 1.0
Ptinv = matrix_inverse(transpose_matrix(P))
perspective = multiply_matrix_vector(
Ptinv, [Mt[0][3], Mt[1][3], Mt[2][3], Mt[3][3]])
else:
perspective = [0.0, 0.0, 0.0, 1.0]
return scale, shear, angles, translation, perspective
def matrix_from_axis_and_angle(axis, angle, point=None):
"""Calculates a rotation matrix from an rotation axis, an angle and an optional
point of rotation.
Parameters
----------
axis : [float, float, float]
Three numbers that represent the axis of rotation.
angle : float
The rotation angle in radians.
point : [float, float, float] | :class:`compas.geometry.Point`, optional
A point to perform a rotation around an origin other than [0, 0, 0].
Returns
-------
list[list[float]]
A 4x4 transformation matrix representing a rotation.
Notes
-----
The rotation is based on the right hand rule, i.e. anti-clockwise if the
axis of rotation points towards the observer.
Examples
--------
>>> axis1 = normalize_vector([-0.043, -0.254, 0.617])
>>> angle1 = 0.1
>>> R = matrix_from_axis_and_angle(axis1, angle1)
>>> axis2, angle2 = axis_and_angle_from_matrix(R)
>>> allclose(axis1, axis2)
True
>>> allclose([angle1], [angle2])
True
"""
if not point:
point = [0.0, 0.0, 0.0]
axis = list(axis)
if length_vector(axis):
axis = normalize_vector(axis)
sina = math.sin(angle)
cosa = math.cos(angle)
R = [[cosa, 0.0, 0.0], [0.0, cosa, 0.0], [0.0, 0.0, cosa]]
outer_product = [[axis[i] * axis[j] * (1.0 - cosa) for i in range(3)]
for j in range(3)]
R = [[R[i][j] + outer_product[i][j] for i in range(3)] for j in range(3)]
axis = scale_vector(axis, sina)
m = [[0.0, -axis[2], axis[1]], [axis[2], 0.0, -axis[0]],
[-axis[1], axis[0], 0.0]]
M = identity_matrix(4)
for i in range(3):
for j in range(3):
R[i][j] += m[i][j]
M[i][j] = R[i][j]
# rotation about axis, angle AND point includes also translation
t = subtract_vectors(point, multiply_matrix_vector(R, point))
M[0][3] = t[0]
M[1][3] = t[1]
M[2][3] = t[2]
return M
def matrix_from_axis_and_angle(axis, angle, point=None, rtype='list'):
"""Calculates a rotation matrix from an rotation axis, an angle and an optional
point of rotation.
Parameters
----------
axis : list of float
Three numbers that represent the axis of rotation.
angle : float
The rotation angle in radians.
point : list of float, optional
A point to perform a rotation around an origin other than [0, 0, 0].
Returns
-------
list of list of float
The matrix.
Notes
-----
The rotation is based on the right hand rule, i.e. anti-clockwise if the
axis of rotation points towards the observer.
Examples
--------
>>> axis1 = normalize_vector([-0.043, -0.254, 0.617])
>>> angle1 = 0.1
>>> R = matrix_from_axis_and_angle(axis1, angle1)
>>> axis2, angle2 = axis_and_angle_from_matrix(R)
>>> allclose(axis1, axis2)
True
>>> allclose([angle1], [angle2])
True
"""
if not point:
point = [0.0, 0.0, 0.0]
axis = list(axis)
if length_vector(axis):
axis = normalize_vector(axis)
sina = math.sin(angle)
cosa = math.cos(angle)
R = [[cosa, 0.0, 0.0], [0.0, cosa, 0.0], [0.0, 0.0, cosa]]
outer_product = [[axis[i] * axis[j] * (1.0 - cosa) for i in range(3)]
for j in range(3)]
R = [[R[i][j] + outer_product[i][j] for i in range(3)] for j in range(3)]
axis = scale_vector(axis, sina)
m = [[0.0, -axis[2], axis[1]], [axis[2], 0.0, -axis[0]],
[-axis[1], axis[0], 0.0]]
M = [[1. if x == y else 0. for x in range(4)] for y in range(4)]
for i in range(3):
for j in range(3):
R[i][j] += m[i][j]
M[i][j] = R[i][j]
# rotation about axis, angle AND point includes also translation
t = subtract_vectors(point, multiply_matrix_vector(R, point))
M[0][3] = t[0]
M[1][3] = t[1]
M[2][3] = t[2]
if rtype == 'list':
return M
if rtype == 'array':
from numpy import asarray
return asarray(M)
raise NotImplementedError
