def test_axis_and_angle_from_matrix():
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)
assert allclose(axis1, axis2)
assert allclose([angle1], [angle2])
def test_axis_and_angle():
axis1 = normalize_vector([-0.043, -0.254, 0.617])
angle1 = 0.1
R = Rotation.from_axis_and_angle(axis1, angle1)
axis2, angle2 = R.axis_and_angle
assert allclose(axis1, axis2)
assert allclose([angle1], [angle2])
def intersection_line_box_xy(line, box, tol=1e-6):
points = []
for segment in pairwise(box + box[:1]):
x = intersection_line_segment_xy(line, segment, tol=tol)
if x:
points.append(x)
if len(points) < 3:
return points
if len(points) == 3:
a, b, c = points
if allclose(a, b, tol=tol):
return [a, c]
if allclose(b, c, tol=tol):
return [a, b]
return [a, b]
return [a, c]
def test_from_euler_angles():
ea1 = 1.4, 0.5, 2.3
args = False, 'xyz'
R1 = Rotation.from_euler_angles(ea1, *args)
ea2 = R1.euler_angles(*args)
assert allclose(ea1, ea2)
alpha, beta, gamma = ea1
xaxis, yaxis, zaxis = [1, 0, 0], [0, 1, 0], [0, 0, 1]
Rx = Rotation.from_axis_and_angle(xaxis, alpha)
Ry = Rotation.from_axis_and_angle(yaxis, beta)
Rz = Rotation.from_axis_and_angle(zaxis, gamma)
R2 = Rx * Ry * Rz
assert np.allclose(R1, R2)
def quaternion_is_unit(q, tol=ATOL):
"""Checks if a quaternion is unit-length.
Parameters
----------
q : list
Quaternion as a list of four real values ``[w, x, y, z]``.
tol : float, optional
Requested decimal precision.
Returns
-------
bool
``True`` if the quaternion is unit-length, and ``False`` if otherwise.
"""
n = quaternion_norm(q)
return allclose([n], [1.0], tol)
def quaternion_unitize(q):
"""Makes a quaternion unit-length.
Parameters
----------
q : list
Quaternion as a list of four real values ``[w, x, y, z]``.
Returns
-------
list
Quaternion of length 1 as a list of four real values ``[nw, nx, ny, nz]``.
"""
n = quaternion_norm(q)
if allclose([n], [0.0], ATOL):
raise ValueError("The given quaternion has zero length.")
else:
return [x / n for x in q]
def basis_vectors_from_matrix(R):
"""Returns the basis vectors from the rotation matrix R.
Raises
------
ValueError
If rotation matrix is invalid.
Examples
--------
>>> from compas.geometry import Frame
>>> f = Frame([0, 0, 0], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])
>>> R = matrix_from_frame(f)
>>> xaxis, yaxis = basis_vectors_from_matrix(R)
"""
xaxis = [R[0][0], R[1][0], R[2][0]]
yaxis = [R[0][1], R[1][1], R[2][1]]
zaxis = [R[0][2], R[1][2], R[2][2]]
if not allclose(zaxis, cross_vectors(xaxis, yaxis)):
raise ValueError("Matrix is invalid rotation matrix.")
else:
return [xaxis, yaxis]
def test_quaternion_from_axis_angle():
axis = [1.0, 0.0, 0.0]
angle = math.pi/2
q = quaternion_from_axis_angle(axis, angle)
assert allclose(q, [math.sqrt(2)/2, math.sqrt(2)/2, 0, 0])
def test_quaternion_from_matrix():
q1 = [0.945, -0.021, -0.125, 0.303]
R = matrix_from_quaternion(q1)
q2 = quaternion_from_matrix(R)
assert allclose(q1, q2, tol=1e-03)
def test_axis_angle_from_quaternion():
q = [1., 1., 0., 0.]
axis, angle = axis_angle_from_quaternion(q)
assert allclose(axis, [1., 0., 0.])
assert allclose([angle], [math.pi/2], 1e-6)
def test_axis_angle_vector_from_matrix():
aav1 = [-0.043, -0.254, 0.617]
R = matrix_from_axis_angle_vector(aav1)
aav2 = axis_angle_vector_from_matrix(R)
assert allclose(aav1, aav2)
def test_euler_angles():
ea1 = 1.4, 0.5, 2.3
args = False, 'xyz'
R1 = Rotation.from_euler_angles(ea1, *args)
ea2 = R1.euler_angles(*args)
assert allclose(ea1, ea2)
def test_axis_angle_vector():
aav1 = [-0.043, -0.254, 0.617]
R = Rotation.from_axis_angle_vector(aav1)
aav2 = R.axis_angle_vector
assert allclose(aav1, aav2)
def test_quaternion():
q1 = [0.945, -0.021, -0.125, 0.303]
R = Rotation.from_quaternion(q1)
q2 = R.quaternion
assert allclose(q1, q2, tol=1e-3)
R = Rotation.from_frame(f1)
f2 = Frame.from_rotation(R, point=f1.point)
print(f1 == f2)
print(f2)
f1 = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])
T = Transformation.from_frame(f1)
f2 = Frame.from_transformation(T)
print(f1 == f2)
ea1 = [0.5, 0.4, 0.8]
M = matrix_from_euler_angles(ea1)
f = Frame.from_matrix(M)
ea2 = f.euler_angles()
print(allclose(ea1, ea2))
q1 = [0.945, -0.021, -0.125, 0.303]
f = Frame.from_quaternion(q1, point=[1., 1., 1.])
q2 = f.quaternion
print(allclose(q1, q2, tol=1e-03))
aav1 = [-0.043, -0.254, 0.617]
f = Frame.from_axis_angle_vector(aav1, point=[0, 0, 0])
aav2 = f.axis_angle_vector
print(allclose(aav1, aav2))
ea1 = 1.4, 0.5, 2.3
f = Frame.from_euler_angles(ea1, static=True, axes='xyz')
ea2 = f.euler_angles(static=True, axes='xyz')
print(allclose(ea1, ea2))
