def __init__(O, qr):
O.qr = qr
O.q_size = 3
O.cb_ps = matrix.rt(((1, 0, 0, 0, 1, 0, 0, 0, 1), -qr))
O.cb_sp = matrix.rt(((1, 0, 0, 0, 1, 0, 0, 0, 1), qr))
O.motion_subspace = matrix.rec(
(0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1), n=(6, 3))
def __init__(O, qe):
O.qe = qe
O.q_size = 4
O.unit_quaternion = qe.normalize() # RBDA, bottom of p. 86
e = rbda_eq_4_12(q=O.unit_quaternion)
O.cb_ps = matrix.rt((e, (0, 0, 0)))
O.cb_sp = matrix.rt((e.transpose(), (0, 0, 0)))
O.motion_subspace = matrix.rec(
(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0), n=(6, 3))
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, qr):
O.qe = qe
O.qr = qr
O.q_size = 7
O.unit_quaternion = qe.normalize() # RBDA, bottom of p. 86
O.e = rbda_eq_4_12(q=O.unit_quaternion)
O.r = qr
O.cb_ps = matrix.rt((O.e, -O.e * O.r)) # RBDA Eq. 2.28
O.cb_sp = matrix.rt((O.e.transpose(), O.r))
O.motion_subspace = None
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, qr): O.qe = qe O.qr = qr O.q_size = 7 O.unit_quaternion = qe.normalize() # RBDA, bottom of p. 86 O.e = rbda_eq_4_12(q=O.unit_quaternion) O.r = qr O.cb_ps = matrix.rt((O.e, -O.e * O.r)) # RBDA Eq. 2.28 O.cb_sp = matrix.rt((O.e.transpose(), O.r)) O.motion_subspace = None
def __init__(O, qr):
O.qr = qr
O.q_size = 3
O.cb_ps = matrix.rt(((1,0,0,0,1,0,0,0,1), -qr))
O.cb_sp = matrix.rt(((1,0,0,0,1,0,0,0,1), qr))
O.motion_subspace = matrix.rec((
0,0,0,
0,0,0,
0,0,0,
1,0,0,
0,1,0,
0,0,1), n=(6,3))
def __init__(O, qe):
O.qe = qe
O.q_size = 4
O.unit_quaternion = qe.normalize() # RBDA, bottom of p. 86
e = rbda_eq_4_12(q=O.unit_quaternion)
O.cb_ps = matrix.rt((e, (0,0,0)))
O.cb_sp = matrix.rt((e.transpose(), (0,0,0)))
O.motion_subspace = matrix.rec((
1,0,0,
0,1,0,
0,0,1,
0,0,0,
0,0,0,
0,0,0), n=(6,3))
def __init__(O, center_of_mass): O.cb_0b = matrix.rt(((1,0,0,0,1,0,0,0,1), -center_of_mass)) O.cb_b0 = matrix.rt(((1,0,0,0,1,0,0,0,1), center_of_mass))
def __init__(O): O.q_size = 0 O.cb_ps = matrix.rt(((1,0,0,0,1,0,0,0,1), (0,0,0))) O.cb_sp = O.cb_ps O.motion_subspace = matrix.rec(elems=(), n=(6,0))
def __init__(O, pivot, normal): r = normal.vector_to_001_rotation() O.cb_0b = matrix.rt((r, -r * pivot)) O.cb_b0 = matrix.rt((r.transpose(), pivot))
def __init__(O):
O.cb_0b = matrix.rt(((1, 0, 0, 0, 1, 0, 0, 0, 1), (0, 0, 0)))
O.cb_b0 = O.cb_0b
def __init__(O, center_of_mass):
O.cb_0b = matrix.rt(((1, 0, 0, 0, 1, 0, 0, 0, 1), -center_of_mass))
O.cb_b0 = matrix.rt(((1, 0, 0, 0, 1, 0, 0, 0, 1), center_of_mass))
def __init__(O):
O.q_size = 0
O.cb_ps = matrix.rt(((1, 0, 0, 0, 1, 0, 0, 0, 1), (0, 0, 0)))
O.cb_sp = O.cb_ps
O.motion_subspace = matrix.rec(elems=(), n=(6, 0))
def __init__(O, pivot, normal):
r = normal.vector_to_001_rotation()
O.cb_0b = matrix.rt((r, -r * pivot))
O.cb_b0 = matrix.rt((r.transpose(), pivot))
def __init__(O, pivot): O.cb_0b = matrix.rt(((1,0,0,0,1,0,0,0,1), -pivot)) O.cb_b0 = matrix.rt(((1,0,0,0,1,0,0,0,1), pivot))
def __init__(O): O.cb_0b = matrix.rt(((1,0,0,0,1,0,0,0,1), (0,0,0))) O.cb_b0 = O.cb_0b
def __init__(O, pivot):
O.cb_0b = matrix.rt(((1, 0, 0, 0, 1, 0, 0, 0, 1), -pivot))
O.cb_b0 = matrix.rt(((1, 0, 0, 0, 1, 0, 0, 0, 1), pivot))