Rotation Matrix


(theeth) #1

Ok, here’s the deal. I’m making a script to constrians tracking rotation around a single axis.

All the math calculations are done, but the problem is that it spits back a rotation matrix for the object and we can’t write those directly in the api.

The rotation matrix is only rotational matrix, no size, no location offset.

So my question is, does anyone here has some basic code to revere engineer a rotation matrix into Euler rotations. Only the normal back engineering code would be needed, not the special handling for the exceptions.

thanks
Martin


(eeshlo) #2

# only extract rotation component
def mat2euler_rot(mat):
	mtx = [list(mat[0][:3]), list(mat[1][:3]), list(mat[2][:3])]
	angle_y = -asin(max(min(mtx[0][2], 1.0), -1.0))
	C = cos(angle_y)
	if C!=0.0: C = 1.0/C
	angle_x = atan2(mtx[1][2] * C, mtx[2][2] * C)
	angle_z = atan2(mtx[0][1] * C, mtx[0][0] * C)
	return (angle_x, angle_y, angle_z)


(theeth) #3

thanks, it works great.

Ok, here’s what I have so far:


ObjectMain = "Empty"
ObjectTrack = "Track"

from vecf import *
from Blender import Object

ObMain = Object.Get(ObjectMain)
ObTrack = Object.Get(ObjectTrack)

# Normal of the plane on which the tracking rotation occures
N = vnorm(ObMain.matrix[2][:3]) # in this case, the Z axis

# Vector from the Tracking object to the Target
AB = vecsub(ObTrack.matrix[3][:3], ObMain.matrix[3][:3])

# projection of AB on the plane of the tracking rotation
T1 = vnorm(vecsub(AB,proj(AB, N))) # in this case, the Y axis

T2 = vnorm(crossp(T1, N)) # in this case, the X axis

Rot = mat2euler_rot([T2, T1, N])

ObMain.RotX = Rot[0]
ObMain.RotY = Rot[1]
ObMain.RotZ = Rot[2]

using the vecf module


"""
	VECTOR FUNCTIONS
"""
from math import *

# Return a new vector initialised to zero
def ZeroVector():
	return [0.0, 0.0, 0.0]

#resize a until it is the same lenght as b, while retaining the orientation
def resize(a,b):
	return vecmul( vnorm(a), veclen(b) )

def vecadd(a,b):
	return [a[0]+b[0], a[1]+b[1], a[2]+b[2]]

def vecsub(a,b):
	return [a[0]-b[0], a[1]-b[1], a[2]-b[2]]

def vecmul(a, s):
	return [a[0]*s, a[1]*s, a[2]*s]

def vecdiv(a, s):
	if s!=0.0: s = 1.0/s
	return vecmul(a, s)

# transform a Cartesian vector into a Geographic oriented vector ANGLES ARE IN DEGREES
def makeGeo(v):
	tv = [0.0, 0.0, 0.0]
	tv[0] = veclen(v)
	tv[1] = atan2(v[0], v[1]) * 180 / pi
	tv[2] = asin(vecnorm(v)[2]) * 180 / pi
	return tv

# transfrom a geographic vector into a Cartesian vector ANGLES ARE IN DEGREES
def makeCart(v):
	v[1] = v[1] / 180 * pi
	v[2] = v[2] / 180 * pi
	tv = [0.0, 0.0, 0.0]
	tv[0] = v[0] * cos(v[2]) * cos(v[1])
	tv[1] = v[0] * cos(v[2]) * sin(v[1])
	tv[2] = v[0] * sin(v[2])
	return tv

# return a copy of a vector, this is to prevent cross referencing
# like vector1 = vector2 which both point to the same object
def vcopy(v):
	return [v[0], v[1], v[2]]

# normalize, but don't modify
def vecnorm(v):
	# normalize vector
	d = vdot(v, v)
	if d>0.0: d = 1./sqrt(d)
	return vecmul(v, d)

# normalize
def vnorm(v):
	# normalize vector
	d = vdot(v, v)
	if d>0.0:
		d = 1./sqrt(d); v[0]*=d; v[1]*=d; v[2]*=d
	return v

# normalize and return length
def vnormlen(v):
	# normalize vector
	d = vdot(v, v)
	if d>0.0:
		d = sqrt(d)
		dv = 1./d; v[0]*=dv; v[1]*=dv; v[2]*=dv
	return d

# normalize and return length squared
def vnormlensq(v):
	# normalize vector
	d = vdot(v, v)
	if d>0.0:
		dv = 1./sqrt(d); v[0]*=dv; v[1]*=dv; v[2]*=dv
	return d

# dotproduct
def vdot(v1, v2):
	# dot product
	return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]

# vector length
def veclen(v):
	return sqrt(vdot(v, v))

# crossproduct
def crossp(v1, v2):
	r = ZeroVector()
	r[0] = v1[1]*v2[2] - v1[2]*v2[1]
	r[1] = v1[2]*v2[0] - v1[0]*v2[2]
	r[2] = v1[0]*v2[1] - v1[1]*v2[0]
	return r

# projection of v2 on v1
def proj(v2, v1):
	# basic equation is (v1 v2 / v1 v1) v1
	return vecmul(v1, vdot(v1, v2) / vdot(v1, v1))

#######################

# calculate reflected direction
def reflect(dirc, norm):
	ndir = ZeroVector()	# new direction
	t = -2.0 * vdot(dirc, norm)
	ndir[0] = t * norm[0] + dirc[0]
	ndir[1] = t * norm[1] + dirc[1]
	ndir[2] = t * norm[2] + dirc[2]
	return ndir

#######################

# Curve calculation (cubic hermite interpolation)
# From p1 to p2, p0 & p3 are the points before and after these points to calculate the gradient
# t is the time value
def HCurve(p0, p1, p2, p3, t):
	t2 = t * t
	t3 = t2 * t
	kp0 = (2.0 * t2 - t3 - t) * 0.5
	kp1 = 1.5 * t3 - 2.5 * t2 + 1.0
	kp2 = (4.0 * t2 - 3.0 * t3 + t) * 0.5
	kp3 = (t3 - t2) * 0.5
	r = ZeroVector()
	r[0] = p0[0] * kp0 + p1[0] * kp1 + p2[0] * kp2 + p3[0] * kp3
	r[1] = p0[1] * kp0 + p1[1] * kp1 + p2[1] * kp2 + p3[1] * kp3
	r[2] = p0[2] * kp0 + p1[2] * kp1 + p2[2] * kp2 + p3[2] * kp3
	return r

# alternate method, cubic interpolation, same arguments as HCurve
def CCurve(p0, p1, p2, p3, t):
	t2 = t * t
	t3 = t2 * t
	d01 = vecsub(p0, p1)
	a = vecsub(vecsub(p3, p2), d01)
	b = vecsub(d01, a)
	c = vecsub(p2, p0)
	r = ZeroVector()
	r[0] = a[0]*t3 + b[0]*t2 + c[0]*t + p1[0]
	r[1] = a[1]*t3 + b[1]*t2 + c[1]*t + p1[1]
	r[2] = a[2]*t3 + b[2]*t2 + c[2]*t + p1[2]
	return r

#########################
# MATRIX FUNCTIONS
#########################

#returns a list of list of float from a Matrix object (unpickleable)
def convertFL(mtx):
	fmtx = []
	for row in mtx:
		fmtx.append([x for x in row])
	return fmtx

# returns 3x3 identity matrix
def MtxIdentity3x3():
	return [[ 1.0, 0.0, 0.0],
			[ 0.0, 1.0, 0.0],
			[ 0.0, 0.0, 1.0]]

# returns 4x4 identity matrix
def MtxIdentity4x4():
	return [[ 1.0, 0.0, 0.0, 0.0],
			[ 0.0, 1.0, 0.0, 0.0],
			[ 0.0, 0.0, 1.0, 0.0],
			[ 0.0, 0.0, 0.0, 1.0]]

# multiplies two matrices, d determines 3x3(d=3) or 4x4(d=4)
def mulmat(m1, m2, d):
	r = MtxIdentity4x4()	#just for initialisation
	for i in range(d):
		for j in range(d):
			rij = 0.0
			for k in range(d): rij += m1[i][k] * m2[k][j]
			r[i][j] = rij
	return r

# multiply 3X3 matrix (rotation, scaling) with vector
def mulmatvec3x3(m, v):
	r = ZeroVector()
	r[0] = v[0]*m[0][0] + v[1]*m[1][0] + v[2]*m[2][0]
	r[1] = v[0]*m[0][1] + v[1]*m[1][1] + v[2]*m[2][1]
	r[2] = v[0]*m[0][2] + v[1]*m[1][2] + v[2]*m[2][2]
	return r

# multiply 4X3 matrix (rotation, scaling & translation) with vector
def mulmatvec4x3(m, v):
	r = ZeroVector()
	r[0] = v[0]*m[0][0] + v[1]*m[1][0] + v[2]*m[2][0] + m[3][0]
	r[1] = v[0]*m[0][1] + v[1]*m[1][1] + v[2]*m[2][1] + m[3][1]
	r[2] = v[0]*m[0][2] + v[1]*m[1][2] + v[2]*m[2][2] + m[3][2]
	return r

# transpose 3X3 matrix, the inverse rotation matrix
def transp3x3(m):
	r = MtxIdentity3x3()
	for i in range(3):
		for j in range(3): r[i][j] = m[j][i]
	return r

# creates rotation matrix around X, Y or Z
# axis must be 'X', 'Y' or anything else=='Z'
# angle in radians, all made equivalent to blenders matrices from RotXYZ
def makeRotMtx(axis, angle):
	if axis=='X':	
		rs, rc = sin(angle), cos(angle)
		mtx = [	[1.0, 0.0, 0.0],
				[0.0,  rc, rs],
				[0.0, -rs, rc] ]
	elif axis=='Y':
		rs, rc = sin(angle), cos(angle)
		mtx = [	[ rc, 0.0, -rs],
				[0.0, 1.0, 0.0],
				[ rs, 0.0,  rc] ]
	else:
		rs, rc = sin(angle), cos(angle)
		mtx = [	[ rc,  rs, 0.0],
				[-rs,  rc, 0.0],
				[0.0, 0.0, 1.0] ]
	return mtx

# creates full 3D rotation matrix
# rx, ry, rz angles in radians
def makeRotMtx3D(rx, ry, rz):
	# old slow indirect method
	#mtx_x = makeRotMtx('X', rx)
	#mtx_y = makeRotMtx('Y', ry)
	#mtx_z = makeRotMtx('Z', rz)
	#mtx = mulmat(mulmat(mtx_x, mtx_y, 3), mtx_z, 3)
	# faster equivalent
	# mtx_x =	[1, 0, 0]
	#			[0, B, A]
	#			[0,-A, B]
	# mtx_y =	[D, 0, -C]
	#			[0, 1,  0]
	#			[C, 0,  D]
	# 1.D +  0.0 +  0.C, 1.0 +  0.1 +  0.0, 1.-C +  0.0 +  0.D =  D,  0, -C
	# 0.D +  B.0 +  A.C, 0.0 +  B.1 +  A.0, 0.-C +  B.0 +  A.D = AC,  B, AD
	# 0.D + -A.0 +  B.C, 0.0 + -A.1 +  B.0, 0.-C + -A.0 +  B.D = BC, -A, BD
	# mtx_xy =	[ D, 0,  -C]
	#			[AC, B,  AD]
	#			[BC, -A, BD]
	# mtx_z =	[ F, E, 0]
	#			[-E, F, 0]
	#			[ 0, 0, 1]
	# D.F + 0.-E + -C.0 , D.E + 0.F + -C.0 , D.0 + 0.0 + -C.1 = DF, DE, -C
	# AC.F + B.-E + AD.0 , AC.E + B.F + AD.0 , AC.0 + B.0 + AD.1 = ACF-BE, ACE+BF, AD
	# BC.F + -A.-E + BD.0 , BC.E + -A.F + BD.0 , BC.0 + -A.0 + BD.1 = BCF+AE, BCE-AF, BD
	A, B = sin(rx), cos(rx)
	C, D = sin(ry), cos(ry)
	E, F = sin(rz), cos(rz)
	AC, BC = A*C, B*C
	return [[D*F, 	   D*E, 	 -C],
			[AC*F-B*E, AC*E+B*F, A*D],
			[BC*F+A*E, BC*E-A*F, B*D]]

# creates rotation matrix from a normalized direction vector
# z-rotation is ignored!
def makeRotMtx_fromVec(tv):
	v = vcopy(tv)
	v[1], v[2] = v[2], v[1]
	r = v[0]*v[0] + v[1]*v[1]
	if r>0.0: r = (acos(v[2]) / sqrt(r)) / pi
	u, v = v[0]*r, v[1]*r
	theta, phi = atan2(v, u), pi*sqrt(u*u + v*v)
	mtx = makeRotMtx3D(phi+0.5*pi, 0.5*pi-theta, 0.0)
	return mtx

# only extract rotation component 
def mat2euler_rot(mat): 
	mtx = [list(mat[0][:3]), list(mat[1][:3]), list(mat[2][:3])] 
	angle_y = -asin(max(min(mtx[0][2], 1.0), -1.0)) 
	C = cos(angle_y) 
	if C!=0.0: C = 1.0/C 
	angle_x = atan2(mtx[1][2] * C, mtx[2][2] * C) 
	angle_z = atan2(mtx[0][1] * C, mtx[0][0] * C) 
	return [angle_x, angle_y, angle_z]

Now, I’m going to write an IPO exporter for that.

Martin


(Jamesk) #4

Heh… that’s pretty funny. I was sitting here thinking about that exact thing. Thanx eeshlo… And theeth for asking before I got around to it =)