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