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 getangax2(self):
#Axis:
#Get the eigenvector for the eigenvalue == 1:
nullspace, matrref, minv, ones, zeros = multidim.nullspace(self.matrix - multidim.identity(3), tolerance=0.00000000001)
axis = matrix2vector3D(nullspace)
axis.normalizethis()
if (axis - vector3d.xbasev()).norm() > 0.1:
v = vector3d.xbasev()
elif (axis - vector3d.ybasev()).norm() > 0.1:
v = vector3d.ybasev()
elif (axis - vector3d.zbasev()).norm() > 0.1:
v = vector3d.zbasev()
else:
raise RuntimeError('Rotation3D.getangax: Bad internal matrix!!')
v = vector3d.orthonormal(axis,v) # Make v orthonormal against axis
vrot = self.rotvec(v)
angle = vector3d.anglebetween(v, vrot)
axis1 = vector3d.crossproduct(v, vrot)
if vector3d.dotproduct(axis, axis1) < 0:
axis = -axis
return angle, axis
def getangax(self):
#Problems: at no/whole revolutions or half revolutions
axis = quaternion2vector3D(self.quaternion)
norm = axis.norm()
if norm > 0:
axis /= norm
angle = (2 * math2.asin(norm))
else:
#No rotation:
axis = vector3d.xbasev()
angle = 0
return angle, axis
