import Blender
from Blender import *
from Blender import Mathutils
from Blender.Mathutils import *
from Blender import Armature
import math 


#Get Object
ob = Blender.Object.Get("Armature")
arm = Blender.Armature.Get("Armature")

#Get pose
poses = ob.getPose().bones
bones = arm.bones

def EulerAngleFromMatrix(R, axis):
	t = math.sqrt(R[0][0]*R[0][0] + R[1][0]*R[1][0]);

	if t > 0.00001:
		if axis == 0:
			return -math.atan2(R[2][1], R[2][2])
		if axis == 1:
			return math.atan2(-R[2][0], t)
		return -math.atan2(R[1][0], R[0][0])
	else:
		if axis == 0:
			return -math.atan2(-R[1][2], R[1][1])
		if axis == 1:
			return math.atan2(-R[2][0], t)
		return 0.0

def ComputeTwist(R):
	## qy and qw are the y and w components of the quaternion from R
	qy = R[2][0] - R[0][2]
	qw = R[0][0] + R[1][1] + R[2][2] + 1
	return 2*math.atan2(qy, qw)

# product of 2 column major matrices
def prod(A,B):
	C1 = [A[0][0]*B[0][0]+A[1][0]*B[0][1]+A[2][0]*B[0][2], A[0][1]*B[0][0]+A[1][1]*B[0][1]+A[2][1]*B[0][2], A[0][2]*B[0][0]+A[1][2]*B[0][1]+A[2][2]*B[0][2]]
	C2 = [A[0][0]*B[1][0]+A[1][0]*B[1][1]+A[2][0]*B[1][2], A[0][1]*B[1][0]+A[1][1]*B[1][1]+A[2][1]*B[1][2], A[0][2]*B[1][0]+A[1][2]*B[1][1]+A[2][2]*B[1][2]]
	C3 = [A[0][0]*B[2][0]+A[1][0]*B[2][1]+A[2][0]*B[2][2], A[0][1]*B[2][0]+A[1][1]*B[2][1]+A[2][1]*B[2][2], A[0][2]*B[2][0]+A[1][2]*B[2][1]+A[2][2]*B[2][2]]
	return [C1,C2,C3]

# returns rotation that goes from A to B = A^-1*B
def diff(A,B):
	C1 = [A[0][0]*B[0][0]+A[0][1]*B[0][1]+A[0][2]*B[0][2], A[1][0]*B[0][0]+A[1][1]*B[0][1]+A[1][2]*B[0][2], A[2][0]*B[0][0]+A[2][1]*B[0][1]+A[2][2]*B[0][2]]
	C2 = [A[0][0]*B[1][0]+A[0][1]*B[1][1]+A[0][2]*B[1][2], A[1][0]*B[1][0]+A[1][1]*B[1][1]+A[1][2]*B[1][2], A[2][0]*B[1][0]+A[2][1]*B[1][1]+A[2][2]*B[1][2]]
	C3 = [A[0][0]*B[2][0]+A[0][1]*B[2][1]+A[0][2]*B[2][2], A[1][0]*B[2][0]+A[1][1]*B[2][1]+A[1][2]*B[2][2], A[2][0]*B[2][0]+A[2][1]*B[2][1]+A[2][2]*B[2][2]]
	return [C1,C2,C3]

BA = bones['shoulder'].matrix['ARMATURESPACE'].rotationPart()
BB = bones['ulift'].matrix['BONESPACE'].rotationPart()
BC = bones['urot'].matrix['BONESPACE'].rotationPart()
BD = bones['llift'].matrix['BONESPACE'].rotationPart()
BE = bones['lturn'].matrix['BONESPACE'].rotationPart()
A = poses['shoulder'].poseMatrix.rotationPart() # Shoulder
B = poses['ulift'].poseMatrix.rotationPart() # ulift
C = poses['urot'].poseMatrix.rotationPart() # urot
D = poses['llift'].poseMatrix.rotationPart() # llift
E = poses['lturn'].poseMatrix.rotationPart() # ltrun

print ""
print "New Pose"
print ""

# we could also use the local matrix
R = diff(BA,A)
Shoulder = ComputeTwist(R)
print "Angle SHOULDER: ", Shoulder 

# Deriving ULIFT ANGLE
# compute rest pose of B based on actual pose of A 
A = prod(A,BB)
# difference with acutal pose of B give the joint angle
R = diff(A,B)
Ulift = EulerAngleFromMatrix(R,0)
print "Angle: ULIFT: ", Ulift

# Deriving UROT ANGLE
B = prod(B,BC)
R = diff(B,C)
Urot = ComputeTwist(R)
print "Angle: UROT: ", Urot

# Deriving LLIFT ANGLE
C = prod(C,BD)
R = diff(C,D)
Llift = EulerAngleFromMatrix(R,0)
print "Angle: LLIFT: ", Llift

# Deriving LTRUN ANGLE
D = prod(D,BE)
R = diff(D,E)
Lturn = ComputeTwist(R)
print "Angle: LTURN: ", Lturn

print "EOF"