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
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
|
#include <u.h>
#include <libc.h>
#define DEG 0.01745329251994330
typedef struct Quaternion Quaternion;
struct Quaternion {
double r, i, j, k;
};
typedef struct Point3 Point3;
struct Point3 {
double x, y, z, w;
};
Point3
Vec3(double x, double y, double z)
{
return (Point3){x, y, z, 0};
}
Point3
addpt3(Point3 a, Point3 b)
{
return (Point3){a.x+b.x, a.y+b.y, a.z+b.z, a.w+b.w};
}
Point3
mulpt3(Point3 p, double s)
{
return (Point3){p.x*s, p.y*s, p.z*s, p.w*s};
}
double
dotvec3(Point3 a, Point3 b)
{
return a.x*b.x + a.y*b.y + a.z*b.z;
}
Point3
crossvec3(Point3 a, Point3 b)
{
return (Point3){
a.y*b.z - a.z*b.y,
a.z*b.x - a.x*b.z,
a.x*b.y - a.y*b.x,
0
};
}
Quaternion
mulq(Quaternion q, Quaternion r)
{
Point3 qv, rv, tmp;
qv = Vec3(q.i, q.j, q.k);
rv = Vec3(r.i, r.j, r.k);
tmp = addpt3(addpt3(mulpt3(rv, q.r), mulpt3(qv, r.r)), crossvec3(qv, rv));
return (Quaternion){q.r*r.r - dotvec3(qv, rv), tmp.x, tmp.y, tmp.z};
}
Quaternion
invq(Quaternion q)
{
double len²;
len² = q.r*q.r + q.i*q.i + q.j*q.j + q.k*q.k;
if(len² == 0)
return (Quaternion){0, 0, 0, 0};
return (Quaternion){q.r/len², -q.i/len², -q.j/len², -q.k/len²};
}
#pragma varargck type "q" Quaternion
int
qfmt(Fmt *f)
{
Quaternion q;
q = va_arg(f->args, Quaternion);
return fmtprint(f, "[%g %g %g %g]", q.r, q.i, q.j, q.k);
}
void
main()
{
Quaternion q;
double c, s;
fmtinstall('q', qfmt);
c = cos(45*DEG);
s = sin(45*DEG);
q = (Quaternion){c, s, s, s};
print("q %q\nq⁻¹ %q\nqq⁻¹ %q\n", q, invq(q), mulq(q, invq(q)));
exits(nil);
}
|
