summaryrefslogtreecommitdiff
path: root/ucoo/math/quaternion.py
blob: fe48cc6b60eebfb03b9e315eccea6cf79928ca4e (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
import sympy as sp

sp.init_printing()

def qmul(q1, q2):
    a1, b1, c1, d1 = q1
    a2, b2, c2, d2 = q2
    return (
            a1 * a2 - b1 * b2 - c1 * c2 - d1 * d2,
            a1 * b2 + b1 * a2 + c1 * d2 - d1 * c2,
            a1 * c2 - b1 * d2 + c1 * a2 + d1 * b2,
            a1 * d2 + b1 * c2 - c1 * b2 + d1 * a2
            )

def rotate(q, v):
    qv = (0, v[0], v[1], v[2])
    # q is unitary.
    qinv = (q[0], -q[1], -q[2], -q[3])
    return qmul(q, qmul(qv, qinv))[1:4]

if 0:
    yaw, pitch, roll = sp.symbols('yaw pitch roll')
    cyaw = sp.cos(yaw/2)
    syaw = sp.sin(yaw/2)
    cpitch = sp.cos(pitch/2)
    spitch = sp.sin(pitch/2)
    croll = sp.cos(roll/2)
    sroll = sp.sin(roll/2)
else:
    cyaw, syaw, cpitch, spitch, croll, sroll = \
            sp.symbols('cyaw syaw cpitch spitch croll sroll')

print "yaw, pitch, roll to quaternion"
q1 = (cyaw, 0, 0, syaw)
q2 = (cpitch, 0, spitch, 0)
q3 = (croll, sroll, 0, 0)
qt = qmul(q1, qmul(q2, q3))
for n, l in zip(('w', 'x', 'y', 'z'), qt):
    print '%s = %s;' % (n, l)

print "rotation"
v = sp.symbols('v.x v.y v.z')
q = sp.symbols('w x y z')
rv = rotate(q, v)
for n, l in zip(('x', 'y', 'z'), rv):
    print 'n%s = %s;' % (n, sp.expand(l))