lib/linear/src/quaternion.zig
daab053ee43316e1809a84551d573ddd1e5bf3d2
1 //! Rotation quaternions over f32.
2 //!
3 //! A quaternion is stored w first, `(w, x, y, z)`, with the layout of
4 //! `[4]f32`. Rotations assume a unit quaternion. Callers that accumulate
5 //! products renormalize with `normalized`.
6 const std = @import("std");
7 const Vec3 = @import("vector.zig").Vec3;
8
9 const assert = std.debug.assert;
10
11 pub const Quat = extern struct {
12 w: f32 = 1,
13 x: f32 = 0,
14 y: f32 = 0,
15 z: f32 = 0,
16
17 pub const identity: Quat = .{};
18
19 /// Above this cosine between endpoints, `slerp` interpolates linearly and
20 /// renormalizes, because the arc is too short for a stable sine ratio.
21 pub const slerp_linear_cosine: f32 = 0.9995;
22
23 pub fn init(w: f32, x: f32, y: f32, z: f32) Quat {
24 return .{ .w = w, .x = x, .y = y, .z = z };
25 }
26
27 /// Lanes in storage order: w, x, y, z.
28 pub fn fromArray(lanes: [4]f32) Quat {
29 return .{ .w = lanes[0], .x = lanes[1], .y = lanes[2], .z = lanes[3] };
30 }
31
32 /// Lanes in storage order: w, x, y, z.
33 pub fn toArray(q: Quat) [4]f32 {
34 return .{ q.w, q.x, q.y, q.z };
35 }
36
37 /// The rotation by `angle` radians about the unit vector `axis`.
38 pub fn fromAxisAngle(axis: Vec3, angle: f32) Quat {
39 const half = angle * 0.5;
40 const sin_half = @sin(half);
41 return .{
42 .w = @cos(half),
43 .x = axis.x * sin_half,
44 .y = axis.y * sin_half,
45 .z = axis.z * sin_half,
46 };
47 }
48
49 /// The imaginary part `(x, y, z)`.
50 pub fn vector(q: Quat) Vec3 {
51 return .{ .x = q.x, .y = q.y, .z = q.z };
52 }
53
54 pub fn add(a: Quat, b: Quat) Quat {
55 return .{ .w = a.w + b.w, .x = a.x + b.x, .y = a.y + b.y, .z = a.z + b.z };
56 }
57
58 pub fn sub(a: Quat, b: Quat) Quat {
59 return .{ .w = a.w - b.w, .x = a.x - b.x, .y = a.y - b.y, .z = a.z - b.z };
60 }
61
62 pub fn scale(q: Quat, s: f32) Quat {
63 return .{ .w = q.w * s, .x = q.x * s, .y = q.y * s, .z = q.z * s };
64 }
65
66 /// The same rotation with every lane negated.
67 pub fn negate(q: Quat) Quat {
68 return .{ .w = -q.w, .x = -q.x, .y = -q.y, .z = -q.z };
69 }
70
71 /// The inverse rotation of a unit quaternion.
72 pub fn conjugate(q: Quat) Quat {
73 return .{ .w = q.w, .x = -q.x, .y = -q.y, .z = -q.z };
74 }
75
76 /// The four-lane dot product, summed from w.
77 pub fn dot(a: Quat, b: Quat) f32 {
78 return a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z;
79 }
80
81 pub fn length(q: Quat) f32 {
82 return @sqrt(q.dot(q));
83 }
84
85 /// The unit quaternion along `q`, or null when its length is at most
86 /// `min_length` or is NaN.
87 pub fn normalized(q: Quat, min_length: f32) ?Quat {
88 assert(min_length >= 0);
89 const len = q.length();
90 if (!(len > min_length)) return null;
91 return q.scale(1 / len);
92 }
93
94 /// The Hamilton product: rotating by `b`, then by `a`.
95 pub fn mul(a: Quat, b: Quat) Quat {
96 return .{
97 .w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
98 .x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
99 .y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
100 .z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,
101 };
102 }
103
104 /// `v` rotated by the unit quaternion `q`, as
105 /// `v + (2w (u × v) + 2 u × (u × v))` with `u` the imaginary part.
106 pub fn rotate(q: Quat, v: Vec3) Vec3 {
107 const u = q.vector();
108 const uv = u.cross(v);
109 const uuv = u.cross(uv);
110 return v.add(uv.scale(2 * q.w).add(uuv.scale(2)));
111 }
112
113 /// Spherical interpolation between unit quaternions along the shorter
114 /// arc: `a` at `t = 0`, and `b` or `-b` at `t = 1`.
115 pub fn slerp(a: Quat, b: Quat, t: f32) Quat {
116 var end = b;
117 var cosine = a.dot(b);
118 if (cosine < 0) {
119 end = b.negate();
120 cosine = -cosine;
121 }
122 if (cosine > slerp_linear_cosine) {
123 return a.add(end.sub(a).scale(t)).normalized(0).?;
124 }
125 const theta = std.math.acos(@min(cosine, 1));
126 const sin_theta = @sin(theta);
127 const weight_a = @sin((1 - t) * theta) / sin_theta;
128 const weight_b = @sin(t * theta) / sin_theta;
129 return a.scale(weight_a).add(end.scale(weight_b));
130 }
131 };
132
133 comptime {
134 assert(@sizeOf(Quat) == @sizeOf([4]f32));
135 assert(@alignOf(Quat) == @alignOf(f32));
136 }
137
138 const testing = std.testing;
139 const tolerance: f32 = 1e-6;
140
141 fn expectApproxVec3(expected: Vec3, actual: Vec3) !void {
142 try testing.expectApproxEqAbs(expected.x, actual.x, tolerance);
143 try testing.expectApproxEqAbs(expected.y, actual.y, tolerance);
144 try testing.expectApproxEqAbs(expected.z, actual.z, tolerance);
145 }
146
147 fn expectApproxQuat(expected: Quat, actual: Quat) !void {
148 try testing.expectApproxEqAbs(expected.w, actual.w, tolerance);
149 try testing.expectApproxEqAbs(expected.x, actual.x, tolerance);
150 try testing.expectApproxEqAbs(expected.y, actual.y, tolerance);
151 try testing.expectApproxEqAbs(expected.z, actual.z, tolerance);
152 }
153
154 test "identity leaves vectors unchanged" {
155 const v = Vec3.init(0.3, -1.5, 2.25);
156 try testing.expectEqual(v, Quat.identity.rotate(v));
157 try testing.expectEqual(Quat.init(1, 0, 0, 0), Quat{});
158 }
159
160 test "axis-angle rotation turns x toward y about z" {
161 const q = Quat.fromAxisAngle(Vec3.init(0, 0, 1), std.math.pi / 2.0);
162 try expectApproxVec3(Vec3.init(0, 1, 0), q.rotate(Vec3.init(1, 0, 0)));
163 try expectApproxVec3(Vec3.init(-1, 0, 0), q.rotate(Vec3.init(0, 1, 0)));
164 }
165
166 test "product applies the right operand first" {
167 const about_z = Quat.fromAxisAngle(Vec3.init(0, 0, 1), std.math.pi / 2.0);
168 const about_x = Quat.fromAxisAngle(Vec3.init(1, 0, 0), std.math.pi / 2.0);
169 const v = Vec3.init(1, 0, 0);
170 try expectApproxVec3(about_x.rotate(about_z.rotate(v)), about_x.mul(about_z).rotate(v));
171 try expectApproxVec3(Vec3.init(0, 0, 1), about_x.mul(about_z).rotate(v));
172 }
173
174 test "conjugate undoes a unit rotation" {
175 const q = Quat.fromAxisAngle(Vec3.init(0, 0.6, 0.8), 1.1);
176 try expectApproxQuat(Quat.identity, q.mul(q.conjugate()));
177 const v = Vec3.init(0.2, -0.7, 1.9);
178 try expectApproxVec3(v, q.conjugate().rotate(q.rotate(v)));
179 }
180
181 test "normalized sums the norm from w and returns null for zero" {
182 try testing.expectEqual(@as(?Quat, null), Quat.init(0, 0, 0, 0).normalized(0));
183 try testing.expectEqual(@as(?Quat, null), Quat.init(1e-9, 0, 0, 0).normalized(1e-8));
184 try testing.expectEqual(Quat.init(1, 0, 0, 0), Quat.init(2, 0, 0, 0).normalized(0).?);
185 try testing.expectEqual([4]f32{ 1, 2, 3, 4 }, Quat.fromArray(.{ 1, 2, 3, 4 }).toArray());
186 }
187
188 test "slerp reaches its endpoints and bisects the arc" {
189 const axis = Vec3.init(0, 1, 0);
190 const a = Quat.fromAxisAngle(axis, 0);
191 const b = Quat.fromAxisAngle(axis, std.math.pi / 2.0);
192 try expectApproxQuat(a, a.slerp(b, 0));
193 try expectApproxQuat(b, a.slerp(b, 1));
194 try expectApproxQuat(Quat.fromAxisAngle(axis, std.math.pi / 4.0), a.slerp(b, 0.5));
195 }
196
197 test "slerp takes the shorter arc" {
198 const axis = Vec3.init(0, 0, 1);
199 const a = Quat.fromAxisAngle(axis, 0.1);
200 const b = Quat.fromAxisAngle(axis, 0.3).negate();
201 try expectApproxQuat(Quat.fromAxisAngle(axis, 0.2), a.slerp(b, 0.5));
202 }
203
204 test "slerp interpolates nearly equal rotations linearly" {
205 const axis = Vec3.init(1, 0, 0);
206 const a = Quat.fromAxisAngle(axis, 0.5);
207 const b = Quat.fromAxisAngle(axis, 0.5001);
208 const mid = a.slerp(b, 0.5);
209 try testing.expectApproxEqAbs(@as(f32, 1), mid.length(), tolerance);
210 try expectApproxQuat(Quat.fromAxisAngle(axis, 0.50005), mid);
211 }