GCC Code Coverage Report

./
Coverage:
low: ≥ 0%
medium: ≥ 75.0%
high: ≥ 90.0%
Lines:
157 / 194
80.9%
Branches:
67 / 163
41.1%
Functions:
35 / 35
100.0%
libs/base/src/eu/base/quat.cc
Line Branch Exec Source
1 #include "eu/base/quat.h"
2
3 #include <algorithm>
4 #include <cmath>
5
6 namespace
7 {
8 template <typename T, size_t array_size>
9 6 size_t argmax(const std::array<T, array_size>& array)
10 {
11
1/6
✗ Branch 6 → 7 not taken.
✓ Branch 6 → 13 taken 6 times.
✗ Branch 8 → 9 not taken.
✗ Branch 8 → 13 not taken.
✗ Branch 10 → 11 not taken.
✗ Branch 10 → 25 not taken.
 6 ASSERT(array.empty() == false);
12 12 return std::distance(array.begin(), std::max_element(array.begin(), array.end()));
13 }
14 }
15
16 namespace eu
17 {
18 1113 v3 Q::get_vec_part() const
19 {
20 1113 return {x, y, z};
21 }
22
23
24 [[nodiscard]] Q
25 1581 Q::from(const AA& aa)
26 {
27
2/4
✓ Branch 2 → 3 taken 1581 times.
✗ Branch 2 → 11 not taken.
✓ Branch 3 → 4 taken 1581 times.
✗ Branch 3 → 11 not taken.
 1581 const float sin_a = sin(aa.angle / 2);
28
2/4
✓ Branch 4 → 5 taken 1581 times.
✗ Branch 4 → 12 not taken.
✓ Branch 5 → 6 taken 1581 times.
✗ Branch 5 → 12 not taken.
 1581 const float cos_a = cos(aa.angle / 2);
29
1/2
✓ Branch 6 → 7 taken 1581 times.
✗ Branch 6 → 13 not taken.
 1581 Q r(cos_a, aa.axis * sin_a);
30 1581 r.normalize();
31 1581 return r;
32 }
33
34
35 [[nodiscard]] Q
36 513 Q::from(const Ypr& ypr)
37 {
38
3/6
✓ Branch 2 → 3 taken 513 times.
✗ Branch 2 → 16 not taken.
✓ Branch 3 → 4 taken 513 times.
✗ Branch 3 → 16 not taken.
✓ Branch 4 → 5 taken 513 times.
✗ Branch 4 → 16 not taken.
 513 const auto yaw = Q::from(AA{kk::y_axis, -ypr.yaw});
39
3/6
✓ Branch 5 → 6 taken 513 times.
✗ Branch 5 → 18 not taken.
✓ Branch 6 → 7 taken 513 times.
✗ Branch 6 → 18 not taken.
✓ Branch 7 → 8 taken 513 times.
✗ Branch 7 → 18 not taken.
 513 const auto pitch = Q::from(AA{ kk::x_axis, -ypr.pitch});
40
1/2
✓ Branch 8 → 9 taken 513 times.
✗ Branch 8 → 22 not taken.
 513 const auto yp = pitch.then_get_rotated(yaw);
41
3/6
✓ Branch 9 → 10 taken 513 times.
✗ Branch 9 → 20 not taken.
✓ Branch 10 → 11 taken 513 times.
✗ Branch 10 → 20 not taken.
✓ Branch 11 → 12 taken 513 times.
✗ Branch 11 → 20 not taken.
 513 const auto roll = Q::from(AA{ yp.get_local_out(), ypr.roll });
42
1/2
✓ Branch 12 → 13 taken 513 times.
✗ Branch 12 → 22 not taken.
 1026 return yp.then_get_rotated(roll);
43 }
44
45 [[nodiscard]] Q
46 512 Q::from_fast(const Ypr& ypr)
47 {
48 // Abbreviations for the various angular functions
49
2/4
✓ Branch 2 → 3 taken 512 times.
✗ Branch 2 → 19 not taken.
✓ Branch 3 → 4 taken 512 times.
✗ Branch 3 → 19 not taken.
 512 const auto cy = cos(ypr.yaw * 0.5);
50
2/4
✓ Branch 4 → 5 taken 512 times.
✗ Branch 4 → 20 not taken.
✓ Branch 5 → 6 taken 512 times.
✗ Branch 5 → 20 not taken.
 512 const auto sy = sin(ypr.yaw * 0.5);
51
2/4
✓ Branch 6 → 7 taken 512 times.
✗ Branch 6 → 21 not taken.
✓ Branch 7 → 8 taken 512 times.
✗ Branch 7 → 21 not taken.
 512 const auto cp = cos(ypr.pitch * 0.5);
52
2/4
✓ Branch 8 → 9 taken 512 times.
✗ Branch 8 → 22 not taken.
✓ Branch 9 → 10 taken 512 times.
✗ Branch 9 → 22 not taken.
 512 const auto sp = sin(ypr.pitch * 0.5);
53
2/4
✓ Branch 10 → 11 taken 512 times.
✗ Branch 10 → 23 not taken.
✓ Branch 11 → 12 taken 512 times.
✗ Branch 11 → 23 not taken.
 512 const auto cr = cos(ypr.roll * 0.5);
54
2/4
✓ Branch 12 → 13 taken 512 times.
✗ Branch 12 → 24 not taken.
✓ Branch 13 → 14 taken 512 times.
✗ Branch 13 → 24 not taken.
 512 const auto sr = sin(ypr.roll * 0.5);
55
56 return
57 {
58 512 cy * cp * cr + sy * sp * sr,
59 {
60 512 -(cy * sp * cr + sy * cp * sr),
61 512 cy * sp * sr - sy * cp * cr,
62 512 cy * cp * sr - sy * sp * cr
63 }
64 512 };
65 }
66
67
68 [[nodiscard]] Q
69 1 Q::from_to(const Q& from, const Q& to)
70 {
71 // https://stackoverflow.com/a/22167097
72
1/2
✓ Branch 2 → 3 taken 1 time.
✗ Branch 2 → 6 not taken.
 1 return to * from.get_inverse();
73 }
74
75
76 [[nodiscard]] std::optional<Q>
77 3 Q::look_at(const v3& from, const v3& to, const n3& up)
78 {
79
2/4
✓ Branch 2 → 3 taken 3 times.
✗ Branch 2 → 13 not taken.
✓ Branch 3 → 4 taken 3 times.
✗ Branch 3 → 13 not taken.
 3 const auto direction = v3::from_to(from, to).get_normalized();
80
1/2
✗ Branch 5 → 6 not taken.
✓ Branch 5 → 7 taken 3 times.
 3 if (direction.has_value() == false)
81 ✗ { return std::nullopt; }
82
1/2
✓ Branch 8 → 9 taken 3 times.
✗ Branch 8 → 14 not taken.
 3 return look_in_direction(*direction, up);
83 }
84
85
86 Q
87 1030 Q::then_get_rotated(const Q& q) const
88 {
89 1030 return q * *this;
90 }
91
92
93 Q
94 555 Q::get_conjugate() const
95 {
96
1/2
✓ Branch 3 → 4 taken 555 times.
✗ Branch 3 → 8 not taken.
 555 return {w, -get_vec_part()};
97 }
98
99
100 Q
101 2 Q::get_inverse() const
102 {
103
1/6
✗ Branch 4 → 5 not taken.
✓ Branch 4 → 11 taken 2 times.
✗ Branch 6 → 7 not taken.
✗ Branch 6 → 11 not taken.
✗ Branch 8 → 9 not taken.
✗ Branch 8 → 13 not taken.
 2 ASSERT(is_equal(get_length(), 1.0f));
104 2 return get_conjugate();
105 }
106
107
108 // the negated represents the same rotation
109 Q
110 1 Q::get_negated() const
111 {
112
1/2
✓ Branch 3 → 4 taken 1 time.
✗ Branch 3 → 8 not taken.
 1 return {-w, -get_vec_part()};
113 }
114
115 float
116 1594 Q::get_length() const
117 {
118 1594 const auto l2 = x * x + y * y + z * z + w * w;
119 1594 return std::sqrt(l2);
120 }
121
122
123 void
124 1592 Q::normalize()
125 {
126 1592 const float l = get_length();
127
1/2
✗ Branch 4 → 5 not taken.
✓ Branch 4 → 6 taken 1592 times.
 1592 if(is_zero(l))
128 {
129 ✗ *this = q_identity;
130 }
131 else
132 {
133 1592 x /= l;
134 1592 y /= l;
135 1592 z /= l;
136 1592 w /= l;
137 }
138 1592 }
139
140
141 Q
142 11 Q::get_normalized() const
143 {
144 11 Q r = *this;
145 11 r.normalize();
146 11 return r;
147 }
148
149
150
2/4
✓ Branch 2 → 3 taken 9 times.
✗ Branch 2 → 7 not taken.
✓ Branch 3 → 4 taken 9 times.
✗ Branch 3 → 7 not taken.
 9 n3 Q::get_local_in () const { return get_rotated(-kk::z_axis); }
151 517 n3 Q::get_local_out () const { return get_rotated( kk::z_axis); }
152 9 n3 Q::get_local_right() const { return get_rotated( kk::x_axis); }
153
2/4
✓ Branch 2 → 3 taken 4 times.
✗ Branch 2 → 7 not taken.
✓ Branch 3 → 4 taken 4 times.
✗ Branch 3 → 7 not taken.
 4 n3 Q::get_local_left () const { return get_rotated(-kk::x_axis); }
154 9 n3 Q::get_local_up () const { return get_rotated( kk::y_axis); }
155
2/4
✓ Branch 2 → 3 taken 4 times.
✗ Branch 2 → 7 not taken.
✓ Branch 3 → 4 taken 4 times.
✗ Branch 3 → 7 not taken.
 4 n3 Q::get_local_down () const { return get_rotated(-kk::y_axis); }
156
157
158 n3
159 552 Q::get_rotated(const n3& v) const
160 {
161 // http://gamedev.stackexchange.com/questions/28395/rotating-vector3-by-a-quaternion
162 552 const Q pure = {0, v};
163 552 const Q a = *this * pure;
164
1/2
✓ Branch 4 → 5 taken 552 times.
✗ Branch 4 → 18 not taken.
 552 const Q ret = a * get_conjugate();
165 // todo(Gustav): should we normalize here? Can we get a invalid vector?
166
1/2
✓ Branch 7 → 8 taken 552 times.
✗ Branch 7 → 19 not taken.
 552 const auto normalized = ret.get_vec_part().get_normalized();
167
1/2
✗ Branch 9 → 10 not taken.
✓ Branch 9 → 14 taken 552 times.
 552 if(normalized.has_value() == false)
168 {
169
0/2
✗ Branch 11 → 12 not taken.
✗ Branch 11 → 20 not taken.
 ✗ DIE("invalid rotation vector");
170 ✗ return kk::up;
171 }
172
173 552 return *normalized;
174 }
175
176 10 Q add(const Q& lhs, const Q& rhs)
177 {
178 10 return { lhs.w + rhs.w, {lhs.x + rhs.x, lhs.y + rhs.y, lhs.z + rhs.z} };
179 }
180
181
182 Q
183 5 Q::nlerp(const Q& f, const float scale, const Q& t)
184 {
185 5 const auto lhs = f * (1 - scale);
186 5 const auto rhs = t * scale;
187 5 return add(lhs, rhs).get_normalized();
188 }
189
190
191 Q
192 5 Q::slerp_fast(const Q& qa, const float t, const Q& qb)
193 {
194 // from:
195 // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/
196 // Calculate angle between them.
197 5 const float cos_half_theta = qa.w * qb.w + qa.x * qb.x + qa.y * qb.y + qa.z * qb.z;
198 // if qa=qb or qa=-qb then theta = 0 and we can return qa
199
1/2
✗ Branch 3 → 4 not taken.
✓ Branch 3 → 5 taken 5 times.
 5 if(cabs(cos_half_theta) >= 1.0f)
200 {
201 ✗ return qa;
202 }
203 // Calculate temporary values.
204
1/2
✓ Branch 5 → 6 taken 5 times.
✗ Branch 5 → 26 not taken.
 5 const auto half_theta = eu::acos(cos_half_theta);
205 5 const auto sin_half_theta = std::sqrt(1.0f - cos_half_theta * cos_half_theta);
206
1/2
✗ Branch 8 → 9 not taken.
✓ Branch 8 → 14 taken 5 times.
 5 if(cabs(sin_half_theta) < 0.001f)
207 {
208 // if theta = 180 degrees then result is not fully defined
209 // we could rotate around any axis normal to qa or qb
210 ✗ const Q qt = add(qa, qb);
211 ✗ return Q
212 {
213 ✗ qt.w * 0.5f,
214 ✗ v3
215 {
216 ✗ qt.x * 0.5f,
217 ✗ qt.y * 0.5f,
218 ✗ qt.z * 0.5f
219 ✗ }
220 ✗ };
221 }
222
2/4
✓ Branch 14 → 15 taken 5 times.
✗ Branch 14 → 24 not taken.
✓ Branch 15 → 16 taken 5 times.
✗ Branch 15 → 24 not taken.
 5 const float ratio_a = eu::sin((1 - t) * half_theta) / sin_half_theta;
223
2/4
✓ Branch 16 → 17 taken 5 times.
✗ Branch 16 → 25 not taken.
✓ Branch 17 → 18 taken 5 times.
✗ Branch 17 → 25 not taken.
 5 const float ratio_b = eu::sin(t * half_theta) / sin_half_theta;
224 5 return add(qa * ratio_a, qb * ratio_b);
225 }
226
227
228 Q
229 5 Q::slerp(const Q& from, const float scale, const Q& to)
230 {
231
1/2
✗ Branch 3 → 4 not taken.
✓ Branch 3 → 8 taken 5 times.
 5 if(dot(from, to) < 0)
232 {
233
0/4
✗ Branch 4 → 5 not taken.
✗ Branch 4 → 10 not taken.
✗ Branch 5 → 6 not taken.
✗ Branch 5 → 10 not taken.
 ✗ return slerp_fast(from.get_negated(), scale, to);
234 }
235 else
236 {
237 5 return slerp_fast(from, scale, to);
238 }
239 }
240
241
242 void
243 22 Q::operator*=(float rhs)
244 {
245 22 x *= rhs;
246 22 y *= rhs;
247 22 z *= rhs;
248 22 w *= rhs;
249 22 }
250
251
252 void
253 2137 Q::operator*=(const Q& rhs)
254 {
255 #define VAR(a, b) const float a##1##b##2 = a * rhs.b
256 2137 VAR(w, w);
257 2137 VAR(w, x);
258 2137 VAR(w, y);
259 2137 VAR(w, z);
260
261 2137 VAR(x, w);
262 2137 VAR(x, x);
263 2137 VAR(x, y);
264 2137 VAR(x, z);
265
266 2137 VAR(y, w);
267 2137 VAR(y, x);
268 2137 VAR(y, y);
269 2137 VAR(y, z);
270
271 2137 VAR(z, w);
272 2137 VAR(z, x);
273 2137 VAR(z, y);
274 2137 VAR(z, z);
275 #undef VAR
276
277 2137 w = w1w2 - x1x2 - y1y2 - z1z2;
278 2137 x = w1x2 + x1w2 + y1z2 - z1y2;
279 2137 y = w1y2 + y1w2 + z1x2 - x1z2;
280 2137 z = w1z2 + z1w2 + x1y2 - y1x2;
281 2137 }
282
283 6 Q Q::look_in_direction(const n3& forward, const n3& upwards)
284 {
285 6 const auto& z = forward;
286
1/2
✓ Branch 2 → 3 taken 6 times.
✗ Branch 2 → 31 not taken.
 6 const auto x_new = z.cross_norm(upwards);
287
288
1/2
✗ Branch 4 → 5 not taken.
✓ Branch 4 → 10 taken 6 times.
 6 if (x_new.has_value() == false)
289 {
290
0/4
✗ Branch 5 → 6 not taken.
✗ Branch 5 → 7 not taken.
✗ Branch 8 → 9 not taken.
✗ Branch 8 → 31 not taken.
 ✗ return look_in_direction(forward, forward.y > 0 ? kk::out : kk::in);
291 }
292 6 const auto& x = *x_new;
293
294
1/2
✓ Branch 11 → 12 taken 6 times.
✗ Branch 11 → 31 not taken.
 6 const auto y_new = x.cross_norm(z);
295
1/8
✗ Branch 13 → 14 not taken.
✓ Branch 13 → 20 taken 6 times.
✗ Branch 14 → 15 not taken.
✗ Branch 14 → 31 not taken.
✗ Branch 15 → 16 not taken.
✗ Branch 15 → 20 not taken.
✗ Branch 17 → 18 not taken.
✗ Branch 17 → 27 not taken.
 6 ASSERT(y_new.has_value());
296 6 const auto& y = *y_new;
297
298
2/4
✓ Branch 21 → 22 taken 6 times.
✗ Branch 21 → 30 not taken.
✓ Branch 22 → 23 taken 6 times.
✗ Branch 22 → 30 not taken.
 6 const auto m = m4::from_basis(x, y, -z);
299
1/2
✓ Branch 23 → 24 taken 6 times.
✗ Branch 23 → 31 not taken.
 6 return Q::from_rotation_matrix(m);
300 }
301
302
303 2 std::string string_from(const Q& v)
304 {
305
1/2
✓ Branch 2 → 3 taken 2 times.
✗ Branch 2 → 5 not taken.
 2 return fmt::format("({}, ({}, {}, {}))", v.w, v.x, v.y, v.z);
306 }
307
308
309 float
310 5 dot(const Q& lhs, const Q& rhs)
311 {
312 5 return lhs.x * rhs.x + lhs.y * rhs.y + lhs.z * rhs.z + lhs.w * rhs.w;
313 }
314
315
316 2137 Q operator*(const Q& lhs, const Q& rhs)
317 {
318 2137 Q r = lhs;
319 2137 r *= rhs;
320 2137 return r;
321 }
322
323
324 1 Q operator*(float scale, const Q& q)
325 {
326 1 Q r = q;
327 1 r *= scale;
328 1 return r;
329 }
330
331
332 21 Q operator*(const Q& q, float scale)
333 {
334 21 Q r = q;
335 21 r *= scale;
336 21 return r;
337 }
338
339 Q
340 6 Q::from_rotation_matrix(const m4& m)
341 {
342 /// Implements the "Shepperd's method" as described in
343 /// "3-D Computer Graphics A Mathematical Introduction with OpenGL" by Samuel R. Buss (2022)
344 /// in section XII 3.6 Quaternion and rotation matrix conversions on page 465
345 6 const auto m11 = m.get(0, 0);
346 6 const auto m22 = m.get(1, 1);
347 6 const auto m33 = m.get(2, 2);
348
349 // aka trace
350 6 const auto m00 = m11 + m22 + m33;
351
352
2/7
✓ Branch 5 → 6 taken 6 times.
✗ Branch 5 → 56 not taken.
✓ Branch 6 → 7 taken 6 times.
✗ Branch 6 → 18 not taken.
✗ Branch 6 → 29 not taken.
✗ Branch 6 → 40 not taken.
✗ Branch 6 → 51 not taken.
 6 switch (argmax(std::array{ m00, m11, m22, m33 }))
353 {
354 6 case 0:
355 {
356 6 const auto d = 0.5f * std::sqrt(m00 + 1.0f);
357
358 6 const auto a = (m.get1(3, 2) - m.get1(2, 3)) / (4.0f * d);
359 6 const auto b = (m.get1(1, 3) - m.get1(3, 1)) / (4.0f * d);
360 6 const auto c = (m.get1(2, 1) - m.get1(1, 2)) / (4.0f * d);
361
362 6 return Q{ d, {a, b, c} }.get_normalized();
363 }
364 ✗ case 1:
365 {
366 ✗ const auto a = 0.5f * std::sqrt(2.0f * m11 - m00 + 1.0f);
367
368 ✗ const auto d = (m.get1(3, 2) - m.get1(2, 3)) / (4.0f * a);
369 ✗ const auto b = (m.get1(2, 1) + m.get1(1, 2)) / (4.0f * a);
370 ✗ const auto c = (m.get1(1, 3) + m.get1(3, 1)) / (4.0f * a);
371
372 ✗ return Q{ d, {a, b, c} }.get_normalized();
373 }
374 ✗ case 2:
375 {
376 ✗ const auto b = 0.5f * std::sqrt(2.0f * m22 - m00 + 1.0f);
377
378 ✗ const auto d = (m.get1(1, 3) - m.get1(3, 1)) / (4.0f * b);
379 ✗ const auto a = (m.get1(2, 1) + m.get1(1, 2)) / (4.0f * b);
380 ✗ const auto c = (m.get1(3, 2) + m.get1(2, 3)) / (4.0f * b);
381
382 ✗ return Q{ d, {a, b, c} }.get_normalized();
383 }
384 ✗ case 3:
385 {
386 ✗ const auto c = 0.5f * std::sqrt(2.0f * m33 - m00 + 1.0f);
387
388 ✗ const auto d = (m.get1(2, 1) - m.get1(1, 2)) / (4.0f * c);
389 ✗ const auto a = (m.get1(1, 3) + m.get1(3, 1)) / (4.0f * c);
390 ✗ const auto b = (m.get1(3, 2) + m.get1(2, 3)) / (4.0f * c);
391
392 ✗ return Q{ d, {a, b, c} }.get_normalized();
393 }
394 ✗ default:
395
0/2
✗ Branch 52 → 53 not taken.
✗ Branch 52 → 57 not taken.
 ✗ DIE("shouldn't happen!!!");
396 ✗ return q_identity;
397 }
398 }
399
400
2/4
✓ Branch 2 → 3 taken 1 time.
✗ Branch 2 → 10 not taken.
✓ Branch 3 → 4 taken 1 time.
✗ Branch 3 → 8 not taken.
 1 ADD_CATCH_FORMATTER_IMPL(Q)
401 }
402
403