diff --git a/utils/space_ops.py b/utils/space_ops.py index 07d8ea49..821e86ac 100644 --- a/utils/space_ops.py +++ b/utils/space_ops.py @@ -7,13 +7,46 @@ from constants import TAU from functools import reduce from utils.iterables import adjacent_pairs -# Matrix operations - def get_norm(vect): return sum([x**2 for x in vect])**0.5 +# Quaternions +# TODO, implement quaternion type + + +def quaternion_mult(q1, q2): + w1, x1, y1, z1 = q1 + w2, x2, y2, z2 = q2 + return np.array([ + w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2, + w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2, + w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2, + w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2, + ]) + + +def quaternion_from_angle_axis(angle, axis): + return np.append( + np.cos(angle / 2), + np.sin(angle / 2) * normalize(axis) + ) + + +def quaternion_conjugate(quaternion): + result = np.array(quaternion) + result[1:] *= -1 + return result + + +def rotate_vector(vector, angle, axis=OUT): + quat = quaternion_from_angle_axis(angle, axis) + quat_inv = quaternion_conjugate(quat) + product = reduce(quaternion_mult, [quat, vector, quat_inv]) + return product[1:] + + def thick_diagonal(dim, thickness=2): row_indices = np.arange(dim).repeat(dim).reshape((dim, dim)) col_indices = np.transpose(row_indices) @@ -64,10 +97,6 @@ def z_to_vector(vector): return np.dot(rotation_about_z(theta), phi_down) -def rotate_vector(vector, angle, axis=OUT): - return np.dot(rotation_matrix(angle, axis), vector) - - def angle_between(v1, v2): return np.arccos(np.dot( v1 / get_norm(v1),