def _get_angle_axis(self):
lim = 1e-12
norm = np.linalg.norm(self.q)
if norm < lim:
angle = 0
axis = [0, 0, 0]
else:
rnorm = 1.0 / norm
angle = acos(max(-1, min(1, rnorm * self.q[3])));
sangle = sin(angle)
if sangle < lim:
axis = [0, 0, 0]
else:
axis = (rnorm / sangle) * np.array(self.q[0:3])
angle *= 2
return (angle, axis)
评论列表
文章目录