def rbda_eq_4_13(q):
p0, p1, p2, p3 = q
return matrix.rec((
-p1, -p2, -p3,
p0, -p3, p2,
p3, p0, -p1,
-p2, p1, p0), n=(4,3)) * 0.5
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, 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): 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 rbda_eq_4_13(q):
p0, p1, p2, p3 = q
return matrix.rec(
(-p1, -p2, -p3, p0, -p3, p2, p3, p0, -p1, -p2, p1, p0), n=(4, 3)) * 0.5
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))