def multiply_into_group(group, mx):
if mx in group:
return
group.add(mx)
mx = matrix.sqr(mx)
for mg in list(group):
multiply_into_group(group, (matrix.sqr(mg) * mx).elems)
def mci(m, c, i):
"""RBDA Eq. 2.63, p. 33:
Spatial rigid-body inertia from mass, CoM and rotational inertia.
Calculates the spatial inertia matrix of a rigid body from its
mass, centre of mass (3D vector) and rotational inertia (3x3 matrix)
about its centre of mass.
"""
cx = matrix.cross_product_matrix(c)
return matrix.sqr((i + m * cx * cx.transpose(), m * cx, m * cx.transpose(),
m * matrix.identity(3))).resolve_partitions()
def crm(v):
"""RBDA Eq. 2.31, p. 25:
Spatial cross-product operator (motion).
Calculates the 6x6 matrix such that the expression crm(v)*m is the
cross product of the spatial motion vectors v and m.
"""
v1, v2, v3, v4, v5, v6 = v
return matrix.sqr(
(0, -v3, v2, 0, 0, 0, v3, 0, -v1, 0, 0, 0, -v2, v1, 0, 0, 0, 0, 0, -v6,
v5, 0, -v3, v2, v6, 0, -v4, v3, 0, -v1, -v5, v4, 0, -v2, v1, 0))
def xrot(e):
"""RBDA Tab. 2.2, p. 23:
Spatial coordinate transform (rotation around origin).
Calculates the coordinate transform matrix from A to B coordinates
for spatial motion vectors, in which frame B is rotated relative to
frame A.
"""
a, b, c, d, e, f, g, h, i = e
return matrix.sqr((a, b, c, 0, 0, 0, d, e, f, 0, 0, 0, g, h, i, 0, 0, 0, 0,
0, 0, a, b, c, 0, 0, 0, d, e, f, 0, 0, 0, g, h, i))
def __init__(O, qe): O.qe = qe O.q_size = len(qe) # c, s = math.cos(qe[0]), math.sin(qe[0]) e = matrix.sqr((c, s, 0, -s, c, 0, 0, 0, 1)) # RBDA Tab. 2.2 # O.cb_ps = matrix.rt((e, (0,0,0))) O.cb_sp = matrix.rt((e.transpose(), (0,0,0))) O.motion_subspace = matrix.col((0,0,1,0,0,0))
def __init__(O, qe):
O.qe = qe
O.q_size = len(qe)
#
c, s = math.cos(qe[0]), math.sin(qe[0])
e = matrix.sqr((c, s, 0, -s, c, 0, 0, 0, 1)) # RBDA Tab. 2.2
#
O.cb_ps = matrix.rt((e, (0, 0, 0)))
O.cb_sp = matrix.rt((e.transpose(), (0, 0, 0)))
O.motion_subspace = matrix.col((0, 0, 1, 0, 0, 0))
def xtrans(r):
"""RBDA Tab. 2.2, p. 23:
Spatial coordinate transform (translation of origin).
Calculates the coordinate transform matrix from A to B coordinates
for spatial motion vectors, in which frame B is translated by an
amount r (3D vector) relative to frame A.
"""
r1, r2, r3 = r
return matrix.sqr(
(1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, r3, -r2, 1,
0, 0, -r3, 0, r1, 0, 1, 0, r2, -r1, 0, 0, 0, 1))
def mci(m, c, i):
"""RBDA Eq. 2.63, p. 33:
Spatial rigid-body inertia from mass, CoM and rotational inertia.
Calculates the spatial inertia matrix of a rigid body from its
mass, centre of mass (3D vector) and rotational inertia (3x3 matrix)
about its centre of mass.
"""
cx = matrix.cross_product_matrix(c)
return matrix.sqr((
i + m*cx*cx.transpose(), m*cx,
m*cx.transpose(), m*matrix.identity(3))).resolve_partitions()
def crm(v):
"""RBDA Eq. 2.31, p. 25:
Spatial cross-product operator (motion).
Calculates the 6x6 matrix such that the expression crm(v)*m is the
cross product of the spatial motion vectors v and m.
"""
v1,v2,v3,v4,v5,v6 = v
return matrix.sqr((
0, -v3, v2, 0, 0, 0,
v3, 0, -v1, 0, 0, 0,
-v2, v1, 0, 0, 0, 0,
0, -v6, v5, 0, -v3, v2,
v6, 0, -v4, v3, 0, -v1,
-v5, v4, 0, -v2, v1, 0))
def xtrans(r):
"""RBDA Tab. 2.2, p. 23:
Spatial coordinate transform (translation of origin).
Calculates the coordinate transform matrix from A to B coordinates
for spatial motion vectors, in which frame B is translated by an
amount r (3D vector) relative to frame A.
"""
r1,r2,r3 = r
return matrix.sqr((
1, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0,
0, 0, 1, 0, 0, 0,
0, r3, -r2, 1, 0, 0,
-r3, 0, r1, 0, 1, 0,
r2, -r1, 0, 0, 0, 1))
def xrot(e):
"""RBDA Tab. 2.2, p. 23:
Spatial coordinate transform (rotation around origin).
Calculates the coordinate transform matrix from A to B coordinates
for spatial motion vectors, in which frame B is rotated relative to
frame A.
"""
a,b,c,d,e,f,g,h,i = e
return matrix.sqr((
a, b, c, 0, 0, 0,
d, e, f, 0, 0, 0,
g, h, i, 0, 0, 0,
0, 0, 0, a, b, c,
0, 0, 0, d, e, f,
0, 0, 0, g, h, i))
def d_unit_quaternion_d_qe_matrix(q):
"""
Coefficent matrix for converting gradients w.r.t. normalized Euler
parameters to gradients w.r.t. non-normalized parameters, as produced
e.g. by a minimizer in the line search.
Mathematica code:
nsq = p0^2+p1^2+p2^2+p3^2
p0p = p0 / Sqrt[nsq]
p1p = p1 / Sqrt[nsq]
p2p = p2 / Sqrt[nsq]
p3p = p3 / Sqrt[nsq]
n3 = (p0^2+p1^2+p2^2+p3^2)^(3/2)
FortranForm[FullSimplify[D[p0p,p0]*n3]]
FortranForm[FullSimplify[D[p1p,p0]*n3]]
FortranForm[FullSimplify[D[p2p,p0]*n3]]
FortranForm[FullSimplify[D[p3p,p0]*n3]]
FortranForm[FullSimplify[D[p0p,p1]*n3]]
FortranForm[FullSimplify[D[p1p,p1]*n3]]
FortranForm[FullSimplify[D[p2p,p1]*n3]]
FortranForm[FullSimplify[D[p3p,p1]*n3]]
FortranForm[FullSimplify[D[p0p,p2]*n3]]
FortranForm[FullSimplify[D[p1p,p2]*n3]]
FortranForm[FullSimplify[D[p2p,p2]*n3]]
FortranForm[FullSimplify[D[p3p,p2]*n3]]
FortranForm[FullSimplify[D[p0p,p3]*n3]]
FortranForm[FullSimplify[D[p1p,p3]*n3]]
FortranForm[FullSimplify[D[p2p,p3]*n3]]
FortranForm[FullSimplify[D[p3p,p3]*n3]]
"""
p0,p1,p2,p3 = q
p0s,p1s,p2s,p3s = p0**2, p1**2, p2**2, p3**2
n3 = (p0s+p1s+p2s+p3s)**(3/2.)
c00 = p1s+p2s+p3s
c11 = p0s+p2s+p3s
c22 = p0s+p1s+p3s
c33 = p0s+p1s+p2s
c01 = -p0*p1
c02 = -p0*p2
c03 = -p0*p3
c12 = -p1*p2
c13 = -p1*p3
c23 = -p2*p3
return matrix.sqr((
c00, c01, c02, c03,
c01, c11, c12, c13,
c02, c12, c22, c23,
c03, c13, c23, c33)) / n3
def d_unit_quaternion_d_qe_matrix(q):
"""
Coefficent matrix for converting gradients w.r.t. normalized Euler
parameters to gradients w.r.t. non-normalized parameters, as produced
e.g. by a minimizer in the line search.
Mathematica code:
nsq = p0^2+p1^2+p2^2+p3^2
p0p = p0 / Sqrt[nsq]
p1p = p1 / Sqrt[nsq]
p2p = p2 / Sqrt[nsq]
p3p = p3 / Sqrt[nsq]
n3 = (p0^2+p1^2+p2^2+p3^2)^(3/2)
FortranForm[FullSimplify[D[p0p,p0]*n3]]
FortranForm[FullSimplify[D[p1p,p0]*n3]]
FortranForm[FullSimplify[D[p2p,p0]*n3]]
FortranForm[FullSimplify[D[p3p,p0]*n3]]
FortranForm[FullSimplify[D[p0p,p1]*n3]]
FortranForm[FullSimplify[D[p1p,p1]*n3]]
FortranForm[FullSimplify[D[p2p,p1]*n3]]
FortranForm[FullSimplify[D[p3p,p1]*n3]]
FortranForm[FullSimplify[D[p0p,p2]*n3]]
FortranForm[FullSimplify[D[p1p,p2]*n3]]
FortranForm[FullSimplify[D[p2p,p2]*n3]]
FortranForm[FullSimplify[D[p3p,p2]*n3]]
FortranForm[FullSimplify[D[p0p,p3]*n3]]
FortranForm[FullSimplify[D[p1p,p3]*n3]]
FortranForm[FullSimplify[D[p2p,p3]*n3]]
FortranForm[FullSimplify[D[p3p,p3]*n3]]
"""
p0, p1, p2, p3 = q
p0s, p1s, p2s, p3s = p0**2, p1**2, p2**2, p3**2
n3 = (p0s + p1s + p2s + p3s)**(3 / 2.)
c00 = p1s + p2s + p3s
c11 = p0s + p2s + p3s
c22 = p0s + p1s + p3s
c33 = p0s + p1s + p2s
c01 = -p0 * p1
c02 = -p0 * p2
c03 = -p0 * p3
c12 = -p1 * p2
c13 = -p1 * p3
c23 = -p2 * p3
return matrix.sqr((c00, c01, c02, c03, c01, c11, c12, c13, c02, c12, c22,
c23, c03, c13, c23, c33)) / n3
def run(args):
assert len(args) == 3, 'radius isituation cutoff'
radius = int(args[0])
isituation = int(args[1])
cutoff = int(args[2])
group_symmetry_masks = []
r90 = (0, -1, 1, 0)
mx = (-1, 0, 0, 1)
group = set()
multiply_into_group(group, r90)
multiply_into_group(group, mx)
ij_lookup_table = build_ij_lookup_table(radius)
num_bits = len(ij_lookup_table)
for mx in reversed(sorted(group)):
mx = matrix.sqr(mx)
symmetry_masks = [None] * num_bits
a = 0
for i in xrange(-radius, radius + 1):
for j in xrange(-radius, radius + 1):
p = matrix.col((i, j))
q = mx * p
b = get_ipacked(radius, *q.elems)
if b is not None:
assert ij_lookup_table[a] == p.elems
assert ij_lookup_table[b] == q.elems
symmetry_masks[b] = (0x1 << a)
a += 1
assert None not in symmetry_masks
for a in xrange(num_bits):
p = mx * matrix.col(ij_lookup_table[a])
b = get_ipacked(radius, *p.elems)
assert apply_symmetry_masks(symmetry_masks, (0x1 << a)) == (0x1 << b)
group_symmetry_masks.append(tuple(symmetry_masks))
group_symmetry_masks = tuple(group_symmetry_masks)
assert len(group_symmetry_masks) == 8
for ipacked in xrange(num_bits):
assert get_ipacked_from_bit(0x1 << ipacked) == ipacked
moves = build_moves(radius)
center_bit = 0x1 << get_ipacked(radius, 0, 0)
situations = (
center_bit,
(0x1 << get_ipacked(radius, 0, -2) |
0x1 << get_ipacked(radius, -2, -1) |
0x1 << get_ipacked(radius, -1, -1)),
~center_bit & ((0x1 << num_bits) - 1),
(0x1 << get_ipacked(radius, -3, -1) |
0x1 << get_ipacked(radius, 1, -3) |
0x1 << get_ipacked(radius, 3, 1) |
0x1 << get_ipacked(radius, -1, 3)),
)
print 'Constants for dfs_core.cc:'
print num_bits
print situations
print get_ipacked(radius, 0, 0);
for symmetry_mask in group_symmetry_masks:
print symmetry_mask
for move in moves:
print move.fingerprint
print
situation = situations[isituation]
show_game(radius, situation)
print
show_equiv_games(radius, situation, None, group_symmetry_masks)
imoves_canonical = (
7, 13, 0, 4, 20, 11, 22, 29, 32, 0, 30, 32, 40, 3, 45, 40,
66, 47, 50, 5, 35, 58, 8, 65, 57, 74, 41, 72, 74, 63, 68)
imoves = None
if cutoff in [100, 101]:
imoves = imoves_canonical
if cutoff == 101:
imoves = imoves[:-1] + (55,)
elif cutoff == 200:
imove_by_wiki = {}
wne = build_wikipedia_notation_english(radius)
for imove, move in enumerate(moves):
pair = []
for i in [0, 2]:
ipacked = get_ipacked_from_bit(move.fingerprint[i])
pair.append(wne[ipacked])
imove_by_wiki[''.join(pair)] = imove
wiki_solution = (
'ex,lj,ck,Pf,DP,GI,JH,mG,GI,ik,gi,LJ,JH,Hl,lj,jh,'
'CK,pF,AC,CK,Mg,gi,ac,ck,kI,dp,pF,FD,DP,Pp,ox')
imoves = tuple([imove_by_wiki[pair] for pair in wiki_solution.split(',')])
if imoves is not None:
print
tracked_situations = [situation]
for imove in imoves:
move = moves[imove]
next_situation = move.apply(situation)
if next_situation is None:
raise RuntimeError('Invalid move.')
show_equiv_games(radius, next_situation, situation, group_symmetry_masks)
print
situation = next_situation
tracked_situations.append(situation)
if cutoff == 200:
tracked_iter = iter(tracked_situations)
situation = tracked_iter.next()
for imove in imoves_canonical:
move = moves[imove]
situation = move.apply(situation)
tracked_situation = tracked_iter.next()
print 'Canonical'
show_game(radius, situation)
print 'Wikipedia'
show_game(radius, tracked_situation)
for symmetry_masks in group_symmetry_masks:
equiv_situation = apply_symmetry_masks(symmetry_masks, situation)
if equiv_situation == tracked_situation:
print 'MATCH OK'
break
else:
print 'MATCH OFF'
sys.stdout.flush()
return
all_lexmins = {}
pruning_counts = []
for len_path in xrange(num_bits - 1):
all_lexmins[len_path] = set()
pruning_counts.append(0)
path = []
continue_play(radius, cutoff, group_symmetry_masks, moves, all_lexmins,
situation, path, pruning_counts)
for len_path in xrange(num_bits - 1):
print len_path, len(all_lexmins[len_path]), pruning_counts[len_path]
print 'Done.'
def rbda_eq_4_12(q):
p0, p1, p2, p3 = q
return matrix.sqr((
p0**2+p1**2-0.5, p1*p2+p0*p3, p1*p3-p0*p2,
p1*p2-p0*p3, p0**2+p2**2-0.5, p2*p3+p0*p1,
p1*p3+p0*p2, p2*p3-p0*p1, p0**2+p3**2-0.5)) * 2
def rbda_eq_4_12(q):
p0, p1, p2, p3 = q
return matrix.sqr(
(p0**2 + p1**2 - 0.5, p1 * p2 + p0 * p3, p1 * p3 - p0 * p2,
p1 * p2 - p0 * p3, p0**2 + p2**2 - 0.5, p2 * p3 + p0 * p1,
p1 * p3 + p0 * p2, p2 * p3 - p0 * p1, p0**2 + p3**2 - 0.5)) * 2
