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))