def getangax(self):
#http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm
tolerance = 1e-12
trace = 0
for i in xrange(3):
trace += self.matrix.getelement(i,i)
arg = (trace - 1)/2
angle = math2.acos(arg)
d1 = self.matrix.getelement(2,1) - self.matrix.getelement(1,2)
d2 = self.matrix.getelement(0,2) - self.matrix.getelement(2,0)
d3 = self.matrix.getelement(1,0) - self.matrix.getelement(0,1)
s = d1*d1 + d2*d2 + d3*d3
if s > tolerance:
isqrt = 1/math.sqrt(s)
x = d1*isqrt
y = d2*isqrt
z = d3*isqrt
axis = vector3d.Vector3D(x, y, z)
elif arg + 1 < tolerance:
#Half revolution
xx = (self.matrix.getelement(0,0)+1)/2
yy = (self.matrix.getelement(1,1)+1)/2
zz = (self.matrix.getelement(2,2)+1)/2
xy = (self.matrix.getelement(0,1)+self.matrix.getelement(1,0))/4
xz = (self.matrix.getelement(0,2)+self.matrix.getelement(2,0))/4
yz = (self.matrix.getelement(1,2)+self.matrix.getelement(2,1))/4
if xx > yy and xx > zz:
if xx < tolerance:
x = 0
y = SQRTHALF
z = SQRTHALF
else:
x = math.sqrt(xx)
y = xy/x
z = xz/x
elif yy > zz:
if yy < tolerance:
x = SQRTHALF
y = 0
z = SQRTHALF
else:
y = math.sqrt(yy)
x = xy/y
z = yz/y
else:
if zz < tolerance:
x = SQRTHALF
y = SQRTHALF
z = 0
else:
z = math.sqrt(zz)
x = xz/z
y = yz/z
axis = vector3d.Vector3D(x, y, z)
else:
#No revolution
axis = vector3d.xbasev() #Dummy vector
return angle, axis
def anglebetween(v1, v2):
#Should be valid for all vectors v1, v2.
#v1 or v2 = (0, 0, 0) will return 0
normprod = v1.norm() * v2.norm();
if normprod == 0:
# Correct way to handle zero vectors
return 0
dp = dotproduct(v1, v2)
return math2.acos(dp / normprod)
