def make_kappa_goniometer(alpha, omega, kappa, phi, direction, scan_axis):
import math
omega_axis = (1, 0, 0)
phi_axis = (1, 0, 0)
c = math.cos(alpha * math.pi / 180);
s = math.sin(alpha * math.pi / 180);
if direction == "+y":
kappa_axis = (c, s, 0.0)
elif direction == "+z":
kappa_axis = (c, 0.0, s)
elif direction == "-y":
kappa_axis = (c, -s, 0.0)
elif direction == "-z":
kappa_axis = (c, 0.0, -s)
else:
raise RuntimeError("Invalid direction")
if scan_axis == "phi":
scan_axis = 0
else:
scan_axis = 2
from scitbx.array_family import flex
axes = flex.vec3_double((phi_axis, kappa_axis, omega_axis))
angles = flex.double((phi, kappa, omega))
names = flex.std_string(("PHI", "KAPPA", "OMEGA"))
return goniometer_factory.make_multi_axis_goniometer(
axes, angles, names, scan_axis)
def Xrotz(theta):
"""
% Xrotz spatial coordinate transform (Z-axis rotation).
% Xrotz(theta) calculates the coordinate transform matrix from A to B
% coordinates for spatial motion vectors, where coordinate frame B is
% rotated by an angle theta (radians) relative to frame A about their
% common Z axis.
"""
c = math.cos(theta)
s = math.sin(theta)
return matrix.sqr((c, s, 0, 0, 0, 0, -s, c, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
0, 0, 0, c, s, 0, 0, 0, 0, -s, c, 0, 0, 0, 0, 0, 0, 1))
def __init__(O, qE):
if (qE is None): qE = matrix.col((0,0,0))
O.qE = qE
th = abs(qE)
if (th == 0):
p = matrix.col((1,0,0,0))
else:
p0 = math.cos(th)
p1, p2, p3 = math.sin(th) / th * qE
p = matrix.col((p0,p1,p2,p3))
assert abs(abs(p)-1) < 1.e-12
O.E = joint_lib.RBDA_Eq_4_12(q=p)
O.setT()
def __init__(O, qE):
if (qE is None): qE = matrix.col((0, 0, 0))
O.qE = qE
th = abs(qE)
if (th == 0):
p = matrix.col((1, 0, 0, 0))
else:
p0 = math.cos(th)
p1, p2, p3 = math.sin(th) / th * qE
p = matrix.col((p0, p1, p2, p3))
assert abs(abs(p) - 1) < 1.e-12
O.E = joint_lib.RBDA_Eq_4_12(q=p)
O.setT()
def Xrotz(theta):
"""
% Xrotz spatial coordinate transform (Z-axis rotation).
% Xrotz(theta) calculates the coordinate transform matrix from A to B
% coordinates for spatial motion vectors, where coordinate frame B is
% rotated by an angle theta (radians) relative to frame A about their
% common Z axis.
"""
c = math.cos(theta)
s = math.sin(theta)
return matrix.sqr((
c, s, 0, 0, 0, 0,
-s, c, 0, 0, 0, 0,
0, 0, 1, 0, 0, 0,
0, 0, 0, c, s, 0,
0, 0, 0, -s, c, 0,
0, 0, 0, 0, 0, 1))
def draw_minimum_covering_sphere(self):
if (self.minimum_covering_sphere_display_list is None):
self.minimum_covering_sphere_display_list=gltbx.gl_managed.display_list()
self.minimum_covering_sphere_display_list.compile()
s = self.minimum_covering_sphere
c = s.center()
r = s.radius()
gray = 0.3
glColor3f(gray,gray,gray)
glBegin(GL_POLYGON)
for i in xrange(360):
a = i * math.pi / 180
rs = r * math.sin(a)
rc = r * math.cos(a)
glVertex3f(c[0],c[1]+rs,c[2]+rc)
glEnd()
self.draw_cross_at(c, color=(1,0,0))
self.minimum_covering_sphere_display_list.end()
self.minimum_covering_sphere_display_list.call()
def draw_minimum_covering_sphere(self):
if (self.minimum_covering_sphere_display_list is None):
self.minimum_covering_sphere_display_list = gltbx.gl_managed.display_list(
)
self.minimum_covering_sphere_display_list.compile()
s = self.minimum_covering_sphere
c = s.center()
r = s.radius()
gray = 0.3
glColor3f(gray, gray, gray)
glBegin(GL_POLYGON)
for i in xrange(360):
a = i * math.pi / 180
rs = r * math.sin(a)
rc = r * math.cos(a)
glVertex3f(c[0], c[1] + rs, c[2] + rc)
glEnd()
self.draw_cross_at(c, color=(1, 0, 0))
self.minimum_covering_sphere_display_list.end()
self.minimum_covering_sphere_display_list.call()
def j2(x): result = (3.0/(x*x) -1.0)*(math.sin(x)/x) - 3.0*math.cos(x)/(x*x) return result
def j1(x): result = math.sin(x)/(x*x) - math.cos(x)/x return result
def cs(a): return math.cos(a), math.sin(a) c1,s1 = cs(q1)
def cs(a): return math.cos(a), math.sin(a) c2,s2 = cs(q2)
def cs(a):
return math.cos(a), math.sin(a)
def exercise_axis_and_angle(axis_range=2,
angle_max_division=12,
angle_min_power=-30):
from_matrix = scitbx.math.r3_rotation_axis_and_angle_from_matrix(
r=[1, 0, 0, 0, 1, 0, 0, 0, 1])
assert approx_equal(from_matrix.axis, [1 / 3**0.5] * 3)
assert approx_equal(from_matrix.angle(deg=True), 0)
assert approx_equal(from_matrix.angle(deg=False), 0)
from_matrix = scitbx.math.r3_rotation_axis_and_angle_from_matrix(r=[0] * 9)
assert approx_equal(from_matrix.axis, [0, 0, 1])
assert approx_equal(from_matrix.angle(deg=True), 90)
assert approx_equal(from_matrix.angle(deg=False), math.pi / 2)
#
angles = []
for d in range(1, angle_max_division + 1):
angles.append(360 / d)
angles.append(-360 / d)
for p in range(-angle_min_power + 1):
angles.append(10**(-p))
angles.append(-10**(-p))
hex_orth = matrix.sqr([
8.7903631196301042, -4.3951815598150503, 0, 0, 7.6126777700894994, 0,
0, 0, 14.943617303371177
])
for u in range(-axis_range, axis_range + 1):
for v in range(-axis_range, axis_range + 1):
for w in range(axis_range + 1):
for axis in [(u, v, w), (hex_orth * matrix.col(
(u, v, w))).elems]:
for angle in angles:
try:
r = scitbx.math.r3_rotation_axis_and_angle_as_matrix(
axis=axis, angle=angle, deg=True)
except RuntimeError:
assert axis == (0, 0, 0)
try:
scitbx.math.r3_rotation_axis_and_angle_as_unit_quaternion(
axis=axis, angle=angle, deg=True)
except RuntimeError:
pass
else:
raise Exception_expected
else:
q = scitbx.math.r3_rotation_axis_and_angle_as_unit_quaternion(
axis=axis, angle=angle, deg=True)
assert approx_equal(abs(matrix.col(q)), 1)
rq = scitbx.math.r3_rotation_unit_quaternion_as_matrix(
q=q)
assert approx_equal(rq, r)
from_matrix = scitbx.math.r3_rotation_axis_and_angle_from_matrix(
r=r)
rr = from_matrix.as_matrix()
assert approx_equal(rr, r)
qq = from_matrix.as_unit_quaternion()
assert approx_equal(abs(matrix.col(qq)), 1)
rq = scitbx.math.r3_rotation_unit_quaternion_as_matrix(
q=qq)
assert approx_equal(rq, r)
qq = scitbx.math.r3_rotation_matrix_as_unit_quaternion(
r=r)
assert approx_equal(abs(matrix.col(qq)), 1)
rq = scitbx.math.r3_rotation_unit_quaternion_as_matrix(
q=qq)
assert approx_equal(rq, r)
rm = matrix.sqr(r)
assert rm.is_r3_rotation_matrix()
qm = rm.r3_rotation_matrix_as_unit_quaternion()
assert approx_equal(abs(qm), 1)
assert approx_equal(qm, qq)
rqmm = qm.unit_quaternion_as_r3_rotation_matrix()
assert approx_equal(rqmm, r)
for deg in [False, True]:
rr = scitbx.math.r3_rotation_axis_and_angle_as_matrix(
axis=from_matrix.axis,
angle=from_matrix.angle(deg=deg),
deg=deg,
min_axis_length=1 - 1.e-5)
assert approx_equal(rr, r)
qq = scitbx.math.r3_rotation_axis_and_angle_as_unit_quaternion(
axis=from_matrix.axis,
angle=from_matrix.angle(deg=deg),
deg=deg,
min_axis_length=1 - 1.e-5)
qq = from_matrix.as_unit_quaternion()
assert approx_equal(abs(matrix.col(qq)), 1)
rq = scitbx.math.r3_rotation_unit_quaternion_as_matrix(
q=qq)
assert approx_equal(rq, r)
for conv in [
scitbx.math.
r3_rotation_axis_and_angle_as_matrix,
scitbx.math.
r3_rotation_axis_and_angle_as_unit_quaternion
]:
try:
conv(axis=from_matrix.axis,
angle=from_matrix.angle(deg=deg),
deg=deg,
min_axis_length=1 + 1.e-5)
except RuntimeError:
pass
else:
raise Exception_expected
#
for i_trial in range(100):
r = flex.random_double_r3_rotation_matrix()
from_matrix = scitbx.math.r3_rotation_axis_and_angle_from_matrix(r=r)
rr = from_matrix.as_matrix()
assert approx_equal(rr, r)
assert approx_equal(math.cos(from_matrix.angle()),
(r[0] + r[4] + r[8] - 1) / 2)
q = matrix.col(from_matrix.as_unit_quaternion())
assert approx_equal(abs(q), 1)
rq = scitbx.math.r3_rotation_unit_quaternion_as_matrix(q=q)
assert approx_equal(rq, r)
rq = matrix.col(q).unit_quaternion_as_r3_rotation_matrix()
assert approx_equal(rq, r)
def cs(a): return math.cos(a), math.sin(a) c1,s1 = cs(q1)
def cs(a): return math.cos(a), math.sin(a) c2,s2 = cs(q2)
def exercise_axis_and_angle(
axis_range=2,
angle_max_division=12,
angle_min_power=-30):
from_matrix = scitbx.math.r3_rotation_axis_and_angle_from_matrix(
r=[1,0,0,0,1,0,0,0,1])
assert approx_equal(from_matrix.axis, [1/3**0.5]*3)
assert approx_equal(from_matrix.angle(deg=True), 0)
assert approx_equal(from_matrix.angle(deg=False), 0)
from_matrix = scitbx.math.r3_rotation_axis_and_angle_from_matrix(r=[0]*9)
assert approx_equal(from_matrix.axis, [0,0,1])
assert approx_equal(from_matrix.angle(deg=True), 90)
assert approx_equal(from_matrix.angle(deg=False), math.pi/2)
#
angles = []
for d in xrange(1,angle_max_division+1):
angles.append(360/d)
angles.append(-360/d)
for p in xrange(-angle_min_power+1):
angles.append(10**(-p))
angles.append(-10**(-p))
hex_orth = matrix.sqr([
8.7903631196301042, -4.3951815598150503, 0,
0, 7.6126777700894994, 0,
0, 0, 14.943617303371177])
for u in xrange(-axis_range, axis_range+1):
for v in xrange(-axis_range, axis_range+1):
for w in xrange(axis_range+1):
for axis in [(u,v,w), (hex_orth*matrix.col((u,v,w))).elems]:
for angle in angles:
try:
r = scitbx.math.r3_rotation_axis_and_angle_as_matrix(
axis=axis, angle=angle, deg=True)
except RuntimeError:
assert axis == (0,0,0)
try:
scitbx.math.r3_rotation_axis_and_angle_as_unit_quaternion(
axis=axis, angle=angle, deg=True)
except RuntimeError: pass
else: raise Exception_expected
else:
q = scitbx.math.r3_rotation_axis_and_angle_as_unit_quaternion(
axis=axis, angle=angle, deg=True)
assert approx_equal(abs(matrix.col(q)), 1)
rq = scitbx.math.r3_rotation_unit_quaternion_as_matrix(q=q)
assert approx_equal(rq, r)
from_matrix = scitbx.math.r3_rotation_axis_and_angle_from_matrix(
r=r)
rr = from_matrix.as_matrix()
assert approx_equal(rr, r)
qq = from_matrix.as_unit_quaternion()
assert approx_equal(abs(matrix.col(qq)), 1)
rq = scitbx.math.r3_rotation_unit_quaternion_as_matrix(q=qq)
assert approx_equal(rq, r)
qq = scitbx.math.r3_rotation_matrix_as_unit_quaternion(r=r)
assert approx_equal(abs(matrix.col(qq)), 1)
rq = scitbx.math.r3_rotation_unit_quaternion_as_matrix(q=qq)
assert approx_equal(rq, r)
rm = matrix.sqr(r)
assert rm.is_r3_rotation_matrix()
qm = rm.r3_rotation_matrix_as_unit_quaternion()
assert approx_equal(abs(qm), 1)
assert approx_equal(qm, qq)
rqmm = qm.unit_quaternion_as_r3_rotation_matrix()
assert approx_equal(rqmm, r)
for deg in [False, True]:
rr = scitbx.math.r3_rotation_axis_and_angle_as_matrix(
axis=from_matrix.axis,
angle=from_matrix.angle(deg=deg),
deg=deg,
min_axis_length=1-1.e-5)
assert approx_equal(rr, r)
qq = scitbx.math.r3_rotation_axis_and_angle_as_unit_quaternion(
axis=from_matrix.axis,
angle=from_matrix.angle(deg=deg),
deg=deg,
min_axis_length=1-1.e-5)
qq = from_matrix.as_unit_quaternion()
assert approx_equal(abs(matrix.col(qq)), 1)
rq = scitbx.math.r3_rotation_unit_quaternion_as_matrix(q=qq)
assert approx_equal(rq, r)
for conv in [
scitbx.math.r3_rotation_axis_and_angle_as_matrix,
scitbx.math.r3_rotation_axis_and_angle_as_unit_quaternion]:
try:
conv(
axis=from_matrix.axis,
angle=from_matrix.angle(deg=deg),
deg=deg,
min_axis_length=1+1.e-5)
except RuntimeError: pass
else: raise Exception_expected
#
for i_trial in xrange(100):
r = flex.random_double_r3_rotation_matrix()
from_matrix = scitbx.math.r3_rotation_axis_and_angle_from_matrix(r=r)
rr = from_matrix.as_matrix()
assert approx_equal(rr, r)
assert approx_equal(math.cos(from_matrix.angle()), (r[0]+r[4]+r[8]-1)/2)
q = matrix.col(from_matrix.as_unit_quaternion())
assert approx_equal(abs(q), 1)
rq = scitbx.math.r3_rotation_unit_quaternion_as_matrix(q=q)
assert approx_equal(rq, r)
rq = matrix.col(q).unit_quaternion_as_r3_rotation_matrix()
assert approx_equal(rq, r)
