xyC = x * yC
yzC = y * zC
zxC = z * xC
return Matrix([[x * xC + c, xyC - zs, zxC + ys],
[xyC + zs, y * yC + c, yzC - xs],
[zxC - ys, yzC + xs, z * zC + c]])
# Formulae for axis_angle to matrix
theta, v0, v1, v2 = symbols('theta, v0, v1, v2')
vec = (v0, v1, v2)
# These give the same formula
M1 = angle_axis2mat(theta, vec)
M2 = orig_aa2mat(theta, vec)
assert matrices_equal(M1, M2)
# This does not, but leads to the same numerical result (see tests)
M3 = quat2mat(quat_around_axis(theta, vec))
assert not matrices_equal(M1, M3)
# Applying a rotation about a point
R = Matrix(3, 3, lambda i, j: Symbol('R%d%d' % (i, j)))
aR = eye(4)
aR[:3, :3] = R
T = eye(4)
point = Matrix(3, 1, symbols('P0, P1, P2'))
T[:3, 3] = point
# Move to new origin (inverse point), rotate, move back to original origin
T_R_iT = T * aR * T.inv()
def z_rotation(theta):
''' Rotation angle `theta` around z-axis
http://en.wikipedia.org/wiki/Rotation_matrix#Dimension_three
'''
return Matrix([[cos(theta), -sin(theta), 0],
[sin(theta), cos(theta), 0],
[0, 0, 1]])
# Formula for rotation matrix given Euler angles and z, y, x ordering
M_zyx = (x_rotation(Symbol('x')) *
y_rotation(Symbol('y')) *
z_rotation(Symbol('z')))
# Formula for quaternion given Euler angles, z, y, x ordering
q_zrot = quat_around_axis(Symbol('z'), [0, 0, 1])
q_yrot = quat_around_axis(Symbol('y'), [0, 1, 0])
q_xrot = quat_around_axis(Symbol('x'), [1, 0, 0])
# quaternion from composition of x on y on z rotations
q_zyx = qmult(q_xrot, qmult(q_yrot, q_zrot))
# Formula for gimbal lock example
alpha, beta, gamma = symbols('\\alpha, \\beta, \\gamma')
M_xyz = (z_rotation(gamma) *
y_rotation(beta) *
x_rotation(alpha))
# Substitute for cos(beta) = 0, sin(beta) = +-1
pm1 = Symbol('\\pm{1}')
subs = {cos(beta): 0,
xyC = x*yC; yzC = y*zC; zxC = z*xC
return Matrix([
[ x*xC+c, xyC-zs, zxC+ys ],
[ xyC+zs, y*yC+c, yzC-xs ],
[ zxC-ys, yzC+xs, z*zC+c ]])
# Formulae for axis_angle to matrix
theta, v0, v1, v2 = symbols('theta, v0, v1, v2')
vec = (v0, v1, v2)
# These give the same formula
M1 = angle_axis2mat(theta, vec)
M2 = orig_aa2mat(theta, vec)
assert matrices_equal(M1, M2)
# This does not, but leads to the same numerical result (see tests)
M3 = quat2mat(quat_around_axis(theta, vec))
assert not matrices_equal(M1, M3)
# Applying a rotation about a point
R = Matrix(3, 3, lambda i, j : Symbol('R%d%d' % (i, j)))
aR = eye(4)
aR[:3,:3] = R
T = eye(4)
point = Matrix(3, 1, symbols('P0, P1, P2'))
T[:3,3] = point
# Move to new origin (inverse point), rotate, move back to original origin
T_R_iT = T * aR * T.inv()
