def time_step_position(O, qd, delta_t): w_F1, v_F1 = matrix.col_list([qd.elems[:3], qd.elems[3:]]) qEd = RBDA_Eq_4_13(O.qE.elems) * w_F1 qrd = v_F1 - w_F1.cross(O.qr) # RBDA Eq. 2.38 p. 27 new_qE = (O.qE + qEd * delta_t).normalize() # RBDA, bottom of p. 86 new_qr = O.qr + qrd * delta_t return six_dof_joint_euler_params(new_qE, new_qr)
def time_step_position(O, qd, delta_t):
w_F1, v_F1 = matrix.col_list([qd.elems[:3], qd.elems[3:]])
qEd = RBDA_Eq_4_13(O.qE.elems) * w_F1
qrd = v_F1 - w_F1.cross(O.qr) # RBDA Eq. 2.38 p. 27
new_qE = (O.qE + qEd * delta_t).normalize() # RBDA, bottom of p. 86
new_qr = O.qr + qrd * delta_t
return six_dof_joint_euler_params(new_qE, new_qr)
def fix_near_singular_hinges(O, sites, angular_tolerance_deg):
assert O.loop_edge_bendings is not None
assert O.fixed_hinges is None
O.fixed_hinges = []
if (O.all_in_one_rigid_body()): return
if (sites is None): return
if (hasattr(sites, "accessor")):
from scitbx import matrix
sites = matrix.col_list(sites)
abs_cos_limit = abs(math.cos(math.radians(angular_tolerance_deg)))
for jc in xrange(len(O.clusters)-1,-1,-1):
hi,hj = O.hinge_edges[jc]
if (hi == -1):
continue
pivot = sites[hi]
axis = sites[hj] - pivot
for i in O.clusters[jc]:
abs_cos = abs(axis.cos_angle(sites[i] - pivot, value_if_undefined=1))
if (abs_cos < abs_cos_limit):
break
else:
O.fixed_hinges.append(O.hinge_edges[jc])
del O.hinge_edges[jc]
ic = O.cluster_indices[hj]
O.clusters[ic].extend(O.clusters[jc])
O.clusters[ic].sort()
ci = O.cluster_indices
for i in O.clusters[jc]:
ci[i] = ic
del O.clusters[jc]
for ic in xrange(jc,len(O.clusters)):
for i in O.clusters[ic]:
ci[i] = ic
O.fixed_hinges.sort()
def simulation_zigzag(NB=5):
mersenne_twister = flex.mersenne_twister(seed=0)
body = six_dof_body(labels=["00", "01", "02"],
sites=matrix.col_list([(0.3, -0.5, 0), (0.4, 0.5, 0),
(0, 0, 0)]),
bonds=[(0, 2), (1, 2)],
mersenne_twister=mersenne_twister)
body.parent = -1
bodies = [body]
vu = matrix.col((0, 1, 0)).rotate_around_origin(axis=matrix.col((1, 0, 0)),
angle=75,
deg=True)
vr = matrix.col((0, 1, 0))
v = vu
pivot = matrix.col((0, 0, 0))
for ib in xrange(1, NB):
body = revolute_body(labels=[str(ib)],
sites=[pivot + v * 0.5],
bonds=[(-1, 0)],
pivot=pivot,
normal=matrix.col((1, 0, 0)),
mersenne_twister=mersenne_twister)
body.parent = ib - 1
bodies.append(body)
pivot += v
if (v is vu): v = vr
else: v = vu
return simulation(bodies=bodies)
def build_sites(eps):
return matrix.col_list([
(x,-y,0),
(x,y,0),
(0,0,0),
(1,0,0),
(2,0,eps)])
def simulation_zigzag(NB=5):
mersenne_twister = flex.mersenne_twister(seed=0)
body = six_dof_body(
labels=["00", "01", "02"],
sites=matrix.col_list([
(0.3,-0.5,0),
(0.4,0.5,0),
(0,0,0)]),
bonds=[(0,2),(1,2)],
mersenne_twister=mersenne_twister)
body.parent = -1
bodies = [body]
vu = matrix.col((0,1,0)).rotate_around_origin(
axis=matrix.col((1,0,0)), angle=75, deg=True)
vr = matrix.col((0,1,0))
v = vu
pivot = matrix.col((0,0,0))
for ib in xrange(1,NB):
body = revolute_body(
labels=[str(ib)],
sites=[pivot + v*0.5],
bonds=[(-1,0)],
pivot=pivot,
normal=matrix.col((1,0,0)),
mersenne_twister=mersenne_twister)
body.parent = ib-1
bodies.append(body)
pivot += v
if (v is vu): v = vr
else: v = vu
return simulation(bodies=bodies)
def fix_near_singular_hinges(O, sites, angular_tolerance_deg):
assert O.loop_edge_bendings is not None
assert O.fixed_hinges is None
O.fixed_hinges = []
if (O.all_in_one_rigid_body()): return
if (sites is None): return
if (hasattr(sites, "accessor")):
from scitbx import matrix
sites = matrix.col_list(sites)
abs_cos_limit = abs(math.cos(math.radians(angular_tolerance_deg)))
for jc in range(len(O.clusters) - 1, -1, -1):
hi, hj = O.hinge_edges[jc]
if (hi == -1):
continue
pivot = sites[hi]
axis = sites[hj] - pivot
for i in O.clusters[jc]:
abs_cos = abs(
axis.cos_angle(sites[i] - pivot, value_if_undefined=1))
if (abs_cos < abs_cos_limit):
break
else:
O.fixed_hinges.append(O.hinge_edges[jc])
del O.hinge_edges[jc]
ic = O.cluster_indices[hj]
O.clusters[ic].extend(O.clusters[jc])
O.clusters[ic].sort()
ci = O.cluster_indices
for i in O.clusters[jc]:
ci[i] = ic
del O.clusters[jc]
for ic in range(jc, len(O.clusters)):
for i in O.clusters[ic]:
ci[i] = ic
O.fixed_hinges.sort()
def tau_as_d_pot_d_q(O, tau):
if (O.type == "euler_params"):
d = d_unit_quaternion_d_qE_matrix(q=O.qE)
c = d * 4 * RBDA_Eq_4_13(q=O.unit_quaternion)
else:
c = RBDA_Eq_4_8(q=O.qE).transpose()
n, f = matrix.col_list([tau.elems[:3], tau.elems[3:]])
if (O.r_is_qr): result = (c * n, O.E.transpose() * f)
else: result = (c * (n + O.qr.cross(f)), f)
return matrix.col(result).resolve_partitions()
def tau_as_d_pot_d_q(O, tau):
if (O.type == "euler_params"):
d = d_unit_quaternion_d_qE_matrix(q=O.qE)
c = d * 4 * RBDA_Eq_4_13(q=O.unit_quaternion)
else:
c = RBDA_Eq_4_8(q=O.qE).transpose()
n, f = matrix.col_list([tau.elems[:3], tau.elems[3:]])
if (O.r_is_qr): result = (c * n, O.E.transpose() * f)
else: result = (c * (n + O.qr.cross(f)), f)
return matrix.col(result).resolve_partitions()
def create_triangle_with_center_of_mass_at_origin():
sites_cart = matrix.col_list([(0, 0, 0), (1, 0, 0),
(0.5, 0.5 * 3**0.5, 0)])
com = matrix.col((0, 0, 0))
for xyz in sites_cart:
com += xyz
com /= len(sites_cart)
sites_cart = [xyz - com for xyz in sites_cart]
E = RBDA_Eq_4_12(matrix.col((0.81, -0.32, -0.42, -0.26)).normalize())
sites_cart = [E * xyz for xyz in sites_cart]
return sites_cart
def check(n_vertices, edge_list, clusters, nosiet, somiet):
sites = matrix.col_list([(i,i%2,0) for i in xrange(n_vertices)])
labels = [str(i) for i in xrange(n_vertices)]
masses = [13, 7, 23, 19, 29, 11, 17][:n_vertices]
assert len(masses) == n_vertices
tt = tardy_tree.construct(sites=sites, edge_list=edge_list)
assert len(tt.cluster_manager.fixed_hinges) == 0
assert tt.cluster_manager.clusters == clusters
tm = construct_tardy_model(
labels=labels, sites=sites, masses=masses, tardy_tree=tt)
assert tm.root_indices() == list(reversed([i for i,n in nosiet]))
assert tm.number_of_sites_in_each_tree() == nosiet
assert tm.sum_of_masses_in_each_tree() == somiet
def create_triangle_with_center_of_mass_at_origin():
sites_cart = matrix.col_list([
(0,0,0),
(1,0,0),
(0.5,0.5*3**0.5,0)])
com = matrix.col((0,0,0))
for xyz in sites_cart:
com += xyz
com /= len(sites_cart)
sites_cart = [xyz-com for xyz in sites_cart]
E = RBDA_Eq_4_12(matrix.col((0.81, -0.32, -0.42, -0.26)).normalize())
sites_cart = [E*xyz for xyz in sites_cart]
return sites_cart
def exercise_fixed_hinges():
# disulfide bond: CA - CB - SG - SG - CB - CA
edge_list = [(0, 1), (1, 2), (2, 5), (3, 4), (4, 5)]
sites = matrix.col_list([(2.031, 74.980, 4.910), (0.672, 75.625, 4.635),
(-0.061, 75.171, 3.047), (-2.009, 74.382, 0.323),
(-0.709, 75.082, 0.718), (-0.355, 75.059, 2.491)])
tt = construct(sites=sites,
edge_list=edge_list,
near_singular_hinges_angular_tolerance_deg=0)
assert tt.cluster_manager.cluster_indices == [0, 0, 0, 3, 2, 1]
tt = construct(sites=sites,
edge_list=edge_list,
near_singular_hinges_angular_tolerance_deg=5)
assert tt.cluster_manager.cluster_indices == [0, 0, 0, 2, 1, 0]
def time_step_position(O, qd, delta_t):
w_body_frame, v_body_frame = matrix.col_list([qd.elems[:3], qd.elems[3:]])
if (O.type == "euler_params"):
qEd = RBDA_Eq_4_13(q=O.unit_quaternion) * w_body_frame
new_qE = (O.qE + qEd * delta_t).normalize()
else:
qEd = RBDA_Eq_4_8_inv(q=O.qE) * w_body_frame
new_qE = O.qE + qEd * delta_t
if (O.r_is_qr):
qrd = O.E.transpose() * v_body_frame
else:
qrd = v_body_frame - w_body_frame.cross(O.qr) # RBDA Eq. 2.38 p. 27
new_qr = O.qr + qrd * delta_t
return six_dof(type=O.type, qE=new_qE, qr=new_qr, r_is_qr=O.r_is_qr)
def check(n_vertices, edge_list, clusters, nosiet, somiet):
sites = matrix.col_list([(i, i % 2, 0) for i in range(n_vertices)])
labels = [str(i) for i in range(n_vertices)]
masses = [13, 7, 23, 19, 29, 11, 17][:n_vertices]
assert len(masses) == n_vertices
tt = tardy_tree.construct(sites=sites, edge_list=edge_list)
assert len(tt.cluster_manager.fixed_hinges) == 0
assert tt.cluster_manager.clusters == clusters
tm = construct_tardy_model(labels=labels,
sites=sites,
masses=masses,
tardy_tree=tt)
assert tm.root_indices() == list(reversed([i for i, n in nosiet]))
assert tm.number_of_sites_in_each_tree() == nosiet
assert tm.sum_of_masses_in_each_tree() == somiet
def time_step_position(O, qd, delta_t):
w_body_frame, v_body_frame = matrix.col_list(
[qd.elems[:3], qd.elems[3:]])
if (O.type == "euler_params"):
qEd = RBDA_Eq_4_13(q=O.unit_quaternion) * w_body_frame
new_qE = (O.qE + qEd * delta_t).normalize()
else:
qEd = RBDA_Eq_4_8_inv(q=O.qE) * w_body_frame
new_qE = O.qE + qEd * delta_t
if (O.r_is_qr):
qrd = O.E.transpose() * v_body_frame
else:
qrd = v_body_frame - w_body_frame.cross(
O.qr) # RBDA Eq. 2.38 p. 27
new_qr = O.qr + qrd * delta_t
return six_dof(type=O.type, qE=new_qE, qr=new_qr, r_is_qr=O.r_is_qr)
def exercise_all_in_one_rigid_body():
tt = construct(
sites=matrix.col_list([(0,0,0), (0,0,1), (0,0,2)]),
edge_list="all_in_one_rigid_body")
sio = StringIO()
tt.show_summary(vertex_labels=None, out=sio)
assert not show_diff(sio.getvalue(), """\
number of vertices: 3
number of edges: None
find cluster loops: 0 repeats
number of fixed vertex lists: 0
number of fixed vertices: 0
number of clusters: 1
merge clusters with multiple connections: 0 passes
number of hinge edges: 1
number of loop edges: 0
number of loop edge bendings: 0
number of fixed hinges: 0
""")
def exercise_fixed_hinges():
# disulfide bond: CA - CB - SG - SG - CB - CA
edge_list = [(0,1), (1,2), (2,5), (3,4), (4,5)]
sites = matrix.col_list([
( 2.031, 74.980, 4.910),
( 0.672, 75.625, 4.635),
(-0.061, 75.171, 3.047),
(-2.009, 74.382, 0.323),
(-0.709, 75.082, 0.718),
(-0.355, 75.059, 2.491)])
tt = construct(
sites=sites,
edge_list=edge_list,
near_singular_hinges_angular_tolerance_deg=0)
assert tt.cluster_manager.cluster_indices == [0,0,0,3,2,1]
tt = construct(
sites=sites,
edge_list=edge_list,
near_singular_hinges_angular_tolerance_deg=5)
assert tt.cluster_manager.cluster_indices == [0,0,0,2,1,0]
def new_q(O, q): i = len(O.qE.elems) new_qE, new_qr = matrix.col_list((q[:i], q[i:])) return six_dof(type=O.type, qE=new_qE, qr=new_qr, r_is_qr=O.r_is_qr)
def new_q(O, q):
i = len(O.qE.elems)
new_qE, new_qr = matrix.col_list((q[:i], q[i:]))
return six_dof(type=O.type, qE=new_qE, qr=new_qr, r_is_qr=O.r_is_qr)
def exercise_fixed_vertices_special_cases():
tardy_models = []
"""
2
/
0---1
"""
x = 0.5*3**0.5
y = 0.5
sites = matrix.col_list([
(0,0,0),
(1,0,0),
(1+x,y,0)])
edge_list = [(0,1),(1,2)]
for i,j in edge_list:
assert approx_equal(abs(sites[i]-sites[j]), 1)
labels = [str(i) for i in xrange(len(sites))]
masses = [1] * len(sites)
#
tt = tardy_tree.construct(
sites=sites,
edge_list=edge_list,
fixed_vertex_lists=[])
assert tt.cluster_manager.clusters == [[0,1,2]]
tm = construct_tardy_model(
labels=labels, sites=sites, masses=masses, tardy_tree=tt)
tardy_models.append(tm)
assert len(tm.bodies) == 1
assert tm.bodies[0].joint.degrees_of_freedom == 6
exercise_linear_velocity_manipulations(tardy_model=tm)
#
expected_e_kin_1 = [
1.00009768395,
1.00002522865,
1.00000107257,
1.0,
1.0,
1.0,
0.0]
rnd = random.Random(0)
for i_fv,fixed_vertices in enumerate([[0], [1], [2],
[0,1], [0,2], [1,2],
[0,1,2]]):
tt = tardy_tree.construct(
sites=sites,
edge_list=edge_list,
fixed_vertex_lists=[fixed_vertices])
assert tt.cluster_manager.clusters == [[0,1,2]]
tm = construct_tardy_model(
labels=labels, sites=sites, masses=masses, tardy_tree=tt)
tardy_models.append(tm)
assert len(tm.bodies) == 1
assert tm.bodies[0].joint.degrees_of_freedom \
== [3,1,0][len(fixed_vertices)-1]
tm.assign_random_velocities(e_kin_target=1, random_gauss=rnd.gauss)
if (len(fixed_vertices) != 3):
assert approx_equal(tm.e_kin(), 1, eps=1e-10)
else:
assert approx_equal(tm.e_kin(), 0, eps=1e-10)
tm.dynamics_step(delta_t=0.01)
assert approx_equal(tm.e_kin(), expected_e_kin_1[i_fv], eps=1e-10)
exercise_linear_velocity_manipulations(tardy_model=tm)
#
sites[2] = matrix.col([2,0,0])
assert approx_equal((sites[0]-sites[1]).cos_angle(sites[2]-sites[1]), -1)
for fixed_vertices in [[0,1], [0,2], [1,2]]:
tt = tardy_tree.construct(
sites=sites,
edge_list=edge_list,
fixed_vertex_lists=[fixed_vertices])
assert tt.cluster_manager.clusters == [[0,1,2]]
tm = construct_tardy_model(
labels=labels, sites=sites, masses=masses, tardy_tree=tt)
tardy_models.append(tm)
assert len(tm.bodies) == 1
assert tm.bodies[0].joint.degrees_of_freedom == 0
exercise_linear_velocity_manipulations(tardy_model=tm)
return tardy_models
def build_sites(eps):
return matrix.col_list([(x, -y, 0), (x, y, 0), (0, 0, 0), (1, 0, 0),
(2, 0, eps)])
def exercise_fixed_vertices_special_cases():
tardy_models = []
"""
2
/
0---1
"""
x = 0.5 * 3**0.5
y = 0.5
sites = matrix.col_list([(0, 0, 0), (1, 0, 0), (1 + x, y, 0)])
edge_list = [(0, 1), (1, 2)]
for i, j in edge_list:
assert approx_equal(abs(sites[i] - sites[j]), 1)
labels = [str(i) for i in range(len(sites))]
masses = [1] * len(sites)
#
tt = tardy_tree.construct(sites=sites,
edge_list=edge_list,
fixed_vertex_lists=[])
assert tt.cluster_manager.clusters == [[0, 1, 2]]
tm = construct_tardy_model(labels=labels,
sites=sites,
masses=masses,
tardy_tree=tt)
tardy_models.append(tm)
assert len(tm.bodies) == 1
assert tm.bodies[0].joint.degrees_of_freedom == 6
exercise_linear_velocity_manipulations(tardy_model=tm)
#
expected_e_kin_1 = [
1.00009768395, 1.00002522865, 1.00000107257, 1.0, 1.0, 1.0, 0.0
]
rnd = random.Random(0)
for i_fv, fixed_vertices in enumerate([[0], [1], [2], [0, 1], [0, 2],
[1, 2], [0, 1, 2]]):
tt = tardy_tree.construct(sites=sites,
edge_list=edge_list,
fixed_vertex_lists=[fixed_vertices])
assert tt.cluster_manager.clusters == [[0, 1, 2]]
tm = construct_tardy_model(labels=labels,
sites=sites,
masses=masses,
tardy_tree=tt)
tardy_models.append(tm)
assert len(tm.bodies) == 1
assert tm.bodies[0].joint.degrees_of_freedom \
== [3,1,0][len(fixed_vertices)-1]
tm.assign_random_velocities(e_kin_target=1, random_gauss=rnd.gauss)
if (len(fixed_vertices) != 3):
assert approx_equal(tm.e_kin(), 1, eps=1e-10)
else:
assert approx_equal(tm.e_kin(), 0, eps=1e-10)
tm.dynamics_step(delta_t=0.01)
assert approx_equal(tm.e_kin(), expected_e_kin_1[i_fv], eps=1e-10)
exercise_linear_velocity_manipulations(tardy_model=tm)
#
sites[2] = matrix.col([2, 0, 0])
assert approx_equal((sites[0] - sites[1]).cos_angle(sites[2] - sites[1]),
-1)
for fixed_vertices in [[0, 1], [0, 2], [1, 2]]:
tt = tardy_tree.construct(sites=sites,
edge_list=edge_list,
fixed_vertex_lists=[fixed_vertices])
assert tt.cluster_manager.clusters == [[0, 1, 2]]
tm = construct_tardy_model(labels=labels,
sites=sites,
masses=masses,
tardy_tree=tt)
tardy_models.append(tm)
assert len(tm.bodies) == 1
assert tm.bodies[0].joint.degrees_of_freedom == 0
exercise_linear_velocity_manipulations(tardy_model=tm)
return tardy_models
