matrixEuler.py

import math

# Given a matrix, return euler angles of rotation as (X,Y,Z) in radians.
# Matrix is assumed to be either a lx.object.Matrix or a list[3][3] of floats.
def matrixToEuler (matrix, rotOrder):
	if type(matrix) != list:
		m = matrix.Get3 ()
	else:
		m = matrix

	X = 0.0
	Y = 0.0
	Z = 0.0

	if rotOrder == 'XYZ':
		if m[0][2] < 1:
			if m[0][2] > -1:
				Y = math.asin(m[0][2])
				X = math.atan2(-m[1][2],m[2][2])
				Z = math.atan2(-m[0][1],m[0][0])
			else: # m[0][2] = -1
				# Not a unique solution: Z - X = math.atan2(m[1][0],m[1][1])
				Y = -math.pi/2
				X = -math.atan2(m[1][0],m[1][1])
				Z = 0.0
		else: # m[0][2] = 1
			# Not a unique solution: Z + X = math.atan2(m[1][0],m[1][1])
			Y = math.pi/2
			X = math.atan2(m[1][0],m[1][1])
			Z = 0.0

	elif rotOrder == 'XZY':
		if m[0][1] < 1:
			if m[0][1] > -1:
				Z = math.asin(-m[0][1])
				X = math.atan2(m[2][1],m[1][1])
				Y = math.atan2(m[0][2],m[0][0])
			else: # m[0][1] = -1
				# Not a unique solution: Y - X = math.atan2(-m[2][0],m[2][2])
				Z = math.pi/2
				X = math.atan2(-m[2][0],m[2][2])
				Y = 0.0
		else: # m[0][1] = 1
			# Not a unique solution: Y + X = math.atan2(-m[2][0],m[2][2])
			Z = -math.pi/2
			X = math.atan2(-m[2][0],m[2][2])
			Y = 0.0

	elif rotOrder == 'YXZ':
		if m[1][2] < 1:
			if m[1][2] > -1:
				X = math.asin(-m[1][2])
				Y = math.atan2(m[0][2],m[2][2])
				Z = math.atan2(m[1][0],m[1][1])
			else: # m[1][2] = -1
			# Not a unique solution: Z - Y = math.atan2(-m[0][1],m[0][0])
				X = math.pi/2
				Y = -math.atan2(-m[0][1],m[0][0])
				Z = 0.0
		else: # m[1][2] = 1
			# Not a unique solution: Z + Y = math.atan2(-m[0][1],m[0][0])
			X = -math.pi/2
			Y = math.atan2(-m[0][1],m[0][0])
			Z = 0.0

	elif rotOrder == 'YZX':
		if m[1][0] < 1:
			if m[1][0] > -1:
				Z = math.asin(m[1][0])
				Y = math.atan2(-m[2][0],m[0][0])
				X = math.atan2(-m[1][2],m[1][1])
			else: # m[1][0] = -1
				# Not a unique solution: X - Y = math.atan2(m[2][1],m[2][2])
				Z = -math.pi/2
				Y = -math.atan2(m[2][1],m[2][2])
				X = 0.0
		else:
			# Not a unique solution: X + Y = math.atan2(m[2][1],m[2][2])
			Z = math.pi/2
			Y = math.atan2(m[2][1],m[2][2])
			X = 0.0

	elif rotOrder == 'ZXY':
		if m[2][1] < 1:
			if m[2][1] > -1:
				X = math.asin(m[2][1])
				Z = math.atan2(-m[0][1],m[1][1])
				Y = math.atan2(-m[2][0],m[2][2])
			else: # m[2][1] = -1
				# Not a unique solution: Y - Z = math.atan2(m[0][2],m[0][0])
				X = -math.pi/2
				Z = -math.atan2(m[0][2],m[0][0])
				Y = 0.0
		else: # m[2][1] = 1
			# Not a unique solution: Y + Z = math.atan2(m[0][2],m[0][0])
			X = math.pi/2
			Z = math.atan2(m[0][2],m[0][0])
			Y = 0.0

	elif rotOrder == 'ZYX':
		if m[2][0] < 1:
			if m[2][0] > -1:
				Y = math.asin(-m[2][0])
				Z = math.atan2(m[1][0],m[0][0])
				X = math.atan2(m[2][1],m[2][2])
			else: # m[2][0] = -1
				# Not a unique solution: X - Z = math.atan2(-m[1][2],m[1][1])
				Y = math.pi/2
				Z = -math.atan2(-m[1][2],m[1][1])
				X = 0.0
		else: # m[2][0] = 1
			# Not a unique solution: X + Z = math.atan2(-m[1][2],m[1][1])
			Y = -math.pi/2
			Z = math.atan2(-m[1][2],m[1][1])
			X = 0.0

	return (X, Y, Z)

# Given Euler agnles in radians, return a rotation matrix.
def eulerToMatrix (x=0.0, y=0.0, z=0.0, rotOrder='XYZ'):	
	cx = math.cos (x)
	sx = -math.sin (x)
	cy = math.cos (y)
	sy = -math.sin (y)
	cz = math.cos (z)
	sz = -math.sin (z)

	if rotOrder == 'XYZ':
		return [[cy*cz, cy*sz, -sy],
				[sx*sy*cz - cx*sz, sx*sy*sz + cx*cz, sx*cy],
				[cx*sy*cz + sx*sz, cx*sy*sz - sx*cz, cx*cy]]

	elif rotOrder == 'XZY':
		return [[cy*cz, sz, -cz*sy],
				[sx*sy - cx*cy*sz, cz*cx, cx*sy*sz + cy*sx],
				[cy*sx*sz + cx*sy, -cz*sx, cx*cy - sx*sy*sz]]
	
	elif rotOrder == 'YXZ':
		return [[cy*cz - sx*sy*sz, cy*sz + cz*sx*sy, -cx*sy],
				[-cx*sz, cx*cz, sx],
				[cy*sx*sz + cz*sy, sy*sz - cy*cz*sx, cx*cy]]
	
	elif rotOrder == 'YZX':
		return [[cy*cz, cx*cy*sz + sx*sy, cy*sx*sz - cx*sy],
				[-sz, cx*cz, cz*sx],
				[cz*sy, cx*sy*sz - cy*sx, sx*sy*sz + cx*cy]]

	elif rotOrder == 'ZXY':
		return [[sx*sy*sz + cy*cz, cx*sz, cy*sx*sz - cz*sy],
				[cz*sx*sy - cy*sz, cx*cz, sy*sz + cy*cz*sx],
				[cx*sy, -sx, cx*cy]]
	
	elif rotOrder == 'ZYX':
		return [[cy*cz, cx*sz + cz*sx*sy, sx*sz - cx*cz*sy],
				[-cy*sz, cx*cz - sx*sy*sz, cx*sy*sz + cz*sx],
				[sy, -cy*sx, cx*cy]]




# item is the item you're reading the matrix from.
# chan_read is the ChannelRead object you're using.
# current_scene is the current Scene.

# Get the rotation order.
xfrm_graph = lx.object.ItemGraph (current_scene.GraphLookup (lx.symbol.sGRAPH_XFRMCORE))
rotOrders = ['XYZ', 'XZY', 'YXZ', 'YZX', 'ZXY', 'ZYX']
rotOrder = rotOrders[2] # Seems to be a decent default?
transform_item_count = xfrm_graph.RevCount (item)
for ri in xrange(transform_item_count):
	transform_item = xfrm_graph.RevByIndex (item, ri)
	if transform_item.Type () == type_rotation:
		chanval = chan_read.Integer (transform_item, transform_item.ChannelLookup (lx.symbol.sICHAN_ROTATION_ORDER))
		rotOrder = rotOrders[chanval][::-1] # Reverse it for matrix-euler.
		break

# Get the transform matrix.
matrix = lx.object.Matrix (chan_read.ValueObj (item, item.ChannelLookup (lx.symbol.sICHAN_XFRMCORE_WORLDMATRIX)))

pos = matrix.GetOffset ()					# Position.
rot = matrixToEuler (matrix, rotOrder)		# Get Euler rotation values from rotation matrix.