def test_angle_between_vectors(self):
v = (1., 1., 1.)
theta = angle_between_vectors(v, v)
testing.assert_almost_equal(theta, 0.0)
unit_v = normalize_vector(v)
theta = angle_between_vectors(unit_v, unit_v, normalize=False)
testing.assert_almost_equal(theta, 0.0)
def need_mirror_for_nearest_axis(coords0, coords1, ax):
a0 = coords0.axis(ax)
a1 = coords1.axis(ax)
a1_mirror = -a1
dr1 = angle_between_vectors(a0, a1, normalize=False) \
* normalize_vector(cross_product(a0, a1))
dr1m = angle_between_vectors(a0, a1_mirror, normalize=False) \
* normalize_vector(cross_product(a0, a1_mirror))
return np.linalg.norm(dr1) < np.linalg.norm(dr1m)
def test_angle_between_vectors(self):
v = (1., 1., 1.)
theta = angle_between_vectors(v, v)
testing.assert_almost_equal(theta, 0.0)
unit_v = normalize_vector(v)
theta = angle_between_vectors(unit_v, unit_v, normalize=False)
testing.assert_almost_equal(theta, 0.0)
# directed
theta = angle_between_vectors([1, 0, 0], [-1, 0, 0], directed=False)
testing.assert_almost_equal(theta, 0.0)
theta = angle_between_vectors([1, 0, 0], [-1, 0, 0], directed=True)
testing.assert_almost_equal(theta, np.pi)
def rotate_points(points, a, b):
"""Rotate given points based on a starting and ending vector.
Axis vector k is calculated from the any two nonzero vectors a and b.
Rotated points are calculated from following Rodrigues rotation formula.
.. math::
`P_{rot} = P \\cos \\theta +
(k \\times P) \\sin \\theta + k (k \\cdot P) (1 - \\cos \\theta)`
Parameters
----------
points : numpy.ndarray
Input points. The shape should be (3, ) or (N, 3).
a : numpy.ndarray
nonzero vector.
b : numpy.ndarray
nonzero vector.
Returns
-------
points_rot : numpy.ndarray
rotated points.
"""
if points.ndim == 1:
points = points[None, :]
a = normalize_vector(a)
b = normalize_vector(b)
k = normalize_vector(np.cross(a, b))
theta = angle_between_vectors(a, b, normalize=False)
points_rot = points * np.cos(theta) \
+ np.cross(k, points) * np.sin(theta) \
+ k * np.dot(k, points.T).reshape(-1, 1) * (1 - np.cos(theta))
return points_rot
def difference_rotation(self, coords, rotation_axis=True):
"""Return differences in rotation of given coords.
Parameters
----------
coords : skrobot.coordinates.Coordinates
given coordinates
rotation_axis : str or bool or None (optional)
we can take 'x', 'y', 'z', 'xx', 'yy', 'zz', 'xm', 'ym', 'zm',
'xy', 'yx', 'yz', 'zy', 'zx', 'xz', True or False(None).
Returns
-------
dif_rot : numpy.ndarray
difference rotation of self coordinates and coords
considering rotation_axis.
Examples
--------
>>> from numpy import pi
>>> from skrobot.coordinates import Coordinates
>>> from skrobot.coordinates.math import rpy_matrix
>>> coord1 = Coordinates()
>>> coord2 = Coordinates(rot=rpy_matrix(pi / 2.0, pi / 3.0, pi / 5.0))
>>> coord1.difference_rotation(coord2)
array([-0.32855112, 1.17434985, 1.05738936])
>>> coord1.difference_rotation(coord2, rotation_axis=False)
array([0, 0, 0])
>>> coord1.difference_rotation(coord2, rotation_axis='x')
array([0. , 1.36034952, 0.78539816])
>>> coord1.difference_rotation(coord2, rotation_axis='y')
array([0.35398131, 0. , 0.97442695])
>>> coord1.difference_rotation(coord2, rotation_axis='z')
array([-0.88435715, 0.74192175, 0. ])
Using mirror option ['xm', 'ym', 'zm'], you can
allow differences of mirror direction.
>>> coord1 = Coordinates()
>>> coord2 = Coordinates().rotate(pi, 'x')
>>> coord1.difference_rotation(coord2, 'xm')
array([-2.99951957e-32, 0.00000000e+00, 0.00000000e+00])
>>> coord1 = Coordinates()
>>> coord2 = Coordinates().rotate(pi / 2.0, 'x')
>>> coord1.difference_rotation(coord2, 'xm')
array([-1.57079633, 0. , 0. ])
"""
def need_mirror_for_nearest_axis(coords0, coords1, ax):
a0 = coords0.axis(ax)
a1 = coords1.axis(ax)
a1_mirror = -a1
dr1 = angle_between_vectors(a0, a1, normalize=False) \
* normalize_vector(cross_product(a0, a1))
dr1m = angle_between_vectors(a0, a1_mirror, normalize=False) \
* normalize_vector(cross_product(a0, a1_mirror))
return np.linalg.norm(dr1) < np.linalg.norm(dr1m)
if rotation_axis in ['x', 'y', 'z']:
a0 = self.axis(rotation_axis)
a1 = coords.axis(rotation_axis)
if np.abs(np.linalg.norm(np.array(a0) - np.array(a1))) < 0.001:
dif_rot = np.array([0, 0, 0], 'f')
else:
dif_rot = np.matmul(
self.worldrot().T,
angle_between_vectors(a0, a1, normalize=False) *
normalize_vector(cross_product(a0, a1)))
elif rotation_axis in ['xx', 'yy', 'zz']:
ax = rotation_axis[0]
a0 = self.axis(ax)
a2 = coords.axis(ax)
if not need_mirror_for_nearest_axis(self, coords, ax):
a2 = -a2
dif_rot = np.matmul(
self.worldrot().T,
angle_between_vectors(a0, a2, normalize=False) *
normalize_vector(cross_product(a0, a2)))
elif rotation_axis in ['xy', 'yx', 'yz', 'zy', 'zx', 'xz']:
if rotation_axis in ['xy', 'yx']:
ax1 = 'z'
ax2 = 'x'
elif rotation_axis in ['yz', 'zy']:
ax1 = 'x'
ax2 = 'y'
else:
ax1 = 'y'
ax2 = 'z'
a0 = self.axis(ax1)
a1 = coords.axis(ax1)
dif_rot = np.matmul(
self.worldrot().T,
angle_between_vectors(a0, a1, normalize=False) *
normalize_vector(cross_product(a0, a1)))
norm = np.linalg.norm(dif_rot)
if np.isclose(norm, 0.0):
self_coords = self.copy_worldcoords()
else:
self_coords = self.copy_worldcoords().rotate(norm, dif_rot)
a0 = self_coords.axis(ax2)
a1 = coords.axis(ax2)
dif_rot = np.matmul(
self_coords.worldrot().T,
angle_between_vectors(a0, a1, normalize=False) *
normalize_vector(cross_product(a0, a1)))
elif rotation_axis in ['xm', 'ym', 'zm']:
rot = coords.worldrot()
ax = rotation_axis[0]
if not need_mirror_for_nearest_axis(self, coords, ax):
rot = rotate_matrix(rot, np.pi, ax)
dif_rot = matrix_log(np.matmul(self.worldrot().T, rot))
elif rotation_axis is False or rotation_axis is None:
dif_rot = np.array([0, 0, 0])
elif rotation_axis is True:
dif_rotmatrix = np.matmul(self.worldrot().T, coords.worldrot())
dif_rot = matrix_log(dif_rotmatrix)
else:
raise ValueError
return dif_rot