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