GCC Code Coverage Report

libs/base/src/base/
File: quat.cc
Date: 2025-03-19 20:55:25
Lines:
136/154
88.3%
Functions:
32/32
100.0%
Branches:
55/118
46.6%
Line Branch Exec Source
1 #include "base/quat.h"
2
3 #include <cmath>
4
5
6 namespace eu
7 {
8 89 v3 Q::get_vec_part() const
9 {
10 89 return {x, y, z};
11 }
12
13
14 [[nodiscard]] Q
15 49 Q::from(const AA& aa)
16 {
17
2/4
✓ Branch 0 (2 → 3) taken 49 times.
✗ Branch 1 (2 → 11) not taken.
✓ Branch 2 (3 → 4) taken 49 times.
✗ Branch 3 (3 → 11) not taken.
 49 const float sin_a = sin(aa.angle / 2);
18
2/4
✓ Branch 0 (4 → 5) taken 49 times.
✗ Branch 1 (4 → 12) not taken.
✓ Branch 2 (5 → 6) taken 49 times.
✗ Branch 3 (5 → 12) not taken.
 49 const float cos_a = cos(aa.angle / 2);
19
1/2
✓ Branch 0 (6 → 7) taken 49 times.
✗ Branch 1 (6 → 13) not taken.
 49 Q r(cos_a, aa.axis * sin_a);
20 49 r.normalize();
21 49 return r;
22 }
23
24
25 [[nodiscard]] Q
26 1 Q::from(const Ypr& ypr)
27 {
28 // Abbreviations for the various angular functions
29
2/4
✓ Branch 0 (2 → 3) taken 1 times.
✗ Branch 1 (2 → 19) not taken.
✓ Branch 2 (3 → 4) taken 1 times.
✗ Branch 3 (3 → 19) not taken.
 1 const auto cy = cos(ypr.yaw * 0.5);
30
2/4
✓ Branch 0 (4 → 5) taken 1 times.
✗ Branch 1 (4 → 20) not taken.
✓ Branch 2 (5 → 6) taken 1 times.
✗ Branch 3 (5 → 20) not taken.
 1 const auto sy = sin(ypr.yaw * 0.5);
31
2/4
✓ Branch 0 (6 → 7) taken 1 times.
✗ Branch 1 (6 → 21) not taken.
✓ Branch 2 (7 → 8) taken 1 times.
✗ Branch 3 (7 → 21) not taken.
 1 const auto cp = cos(ypr.pitch * 0.5);
32
2/4
✓ Branch 0 (8 → 9) taken 1 times.
✗ Branch 1 (8 → 22) not taken.
✓ Branch 2 (9 → 10) taken 1 times.
✗ Branch 3 (9 → 22) not taken.
 1 const auto sp = sin(ypr.pitch * 0.5);
33
2/4
✓ Branch 0 (10 → 11) taken 1 times.
✗ Branch 1 (10 → 23) not taken.
✓ Branch 2 (11 → 12) taken 1 times.
✗ Branch 3 (11 → 23) not taken.
 1 const auto cr = cos(ypr.roll * 0.5);
34
2/4
✓ Branch 0 (12 → 13) taken 1 times.
✗ Branch 1 (12 → 24) not taken.
✓ Branch 2 (13 → 14) taken 1 times.
✗ Branch 3 (13 → 24) not taken.
 1 const auto sr = sin(ypr.roll * 0.5);
35
36 return
37 {
38 1 cr * cp * cy + sr * sp * sy,
39 {
40 1 sr * cp * cy - cr * sp * sy,
41 1 cr * sp * cy + sr * cp * sy,
42 1 cr * cp * sy - sr * sp * cy
43 }
44 1 };
45 }
46
47
48 [[nodiscard]] Q
49 1 Q::from_to(const Q& from, const Q& to)
50 {
51 // https://stackoverflow.com/a/22167097
52
1/2
✓ Branch 0 (2 → 3) taken 1 times.
✗ Branch 1 (2 → 6) not taken.
 1 return to * from.get_inverse();
53 }
54
55
56 [[nodiscard]] std::optional<Q>
57 3 Q::look_at(const v3& from, const v3& to, const n3& up)
58 {
59
2/4
✓ Branch 0 (2 → 3) taken 3 times.
✗ Branch 1 (2 → 13) not taken.
✓ Branch 2 (3 → 4) taken 3 times.
✗ Branch 3 (3 → 13) not taken.
 3 const auto direction = v3::from_to(from, to).get_normalized();
60
1/2
✗ Branch 0 (5 → 6) not taken.
✓ Branch 1 (5 → 7) taken 3 times.
 3 if (direction.has_value() == false)
61 ✗ { return std::nullopt; }
62
1/2
✓ Branch 0 (8 → 9) taken 3 times.
✗ Branch 1 (8 → 14) not taken.
 3 return look_in_direction(*direction, up);
63 }
64
65
66 Q
67 4 Q::then_get_rotated(const Q& q) const
68 {
69 4 return q * *this;
70 }
71
72
73 Q
74 42 Q::get_conjugate() const
75 {
76
1/2
✓ Branch 0 (3 → 4) taken 42 times.
✗ Branch 1 (3 → 8) not taken.
 42 return {w, -get_vec_part()};
77 }
78
79
80 Q
81 2 Q::get_inverse() const
82 {
83
1/4
✗ Branch 0 (4 → 5) not taken.
✓ Branch 1 (4 → 11) taken 2 times.
✗ Branch 2 (8 → 9) not taken.
✗ Branch 3 (8 → 13) not taken.
 2 ASSERT(is_equal(get_length(), 1.0f));
84 2 return get_conjugate();
85 }
86
87
88 // the negated represents the same rotation
89 Q
90 1 Q::get_negated() const
91 {
92
1/2
✓ Branch 0 (3 → 4) taken 1 times.
✗ Branch 1 (3 → 8) not taken.
 1 return {-w, -get_vec_part()};
93 }
94
95 float
96 56 Q::get_length() const
97 {
98 56 const auto l2 = x * x + y * y + z * z + w * w;
99 56 return std::sqrt(l2);
100 }
101
102
103 void
104 54 Q::normalize()
105 {
106 54 const float l = get_length();
107
1/2
✗ Branch 0 (4 → 5) not taken.
✓ Branch 1 (4 → 6) taken 54 times.
 54 if(is_zero(l))
108 {
109 ✗ *this = q_identity;
110 }
111 else
112 {
113 54 x /= l;
114 54 y /= l;
115 54 z /= l;
116 54 w /= l;
117 }
118 54 }
119
120
121 Q
122 5 Q::get_normalized() const
123 {
124 5 Q r = *this;
125 5 r.normalize();
126 5 return r;
127 }
128
129
130
2/4
✓ Branch 0 (2 → 3) taken 9 times.
✗ Branch 1 (2 → 7) not taken.
✓ Branch 2 (3 → 4) taken 9 times.
✗ Branch 3 (3 → 7) not taken.
 9 n3 Q::get_local_in () const { return get_rotated(-kk::z_axis); }
131 4 n3 Q::get_local_out () const { return get_rotated( kk::z_axis); }
132 9 n3 Q::get_local_right() const { return get_rotated( kk::x_axis); }
133
2/4
✓ Branch 0 (2 → 3) taken 4 times.
✗ Branch 1 (2 → 7) not taken.
✓ Branch 2 (3 → 4) taken 4 times.
✗ Branch 3 (3 → 7) not taken.
 4 n3 Q::get_local_left () const { return get_rotated(-kk::x_axis); }
134 9 n3 Q::get_local_up () const { return get_rotated( kk::y_axis); }
135
2/4
✓ Branch 0 (2 → 3) taken 4 times.
✗ Branch 1 (2 → 7) not taken.
✓ Branch 2 (3 → 4) taken 4 times.
✗ Branch 3 (3 → 7) not taken.
 4 n3 Q::get_local_down () const { return get_rotated(-kk::y_axis); }
136
137
138 n3
139 39 Q::get_rotated(const n3& v) const
140 {
141 // http://gamedev.stackexchange.com/questions/28395/rotating-vector3-by-a-quaternion
142 39 const Q pure = {0, v};
143 39 const Q a = *this * pure;
144
1/2
✓ Branch 0 (4 → 5) taken 39 times.
✗ Branch 1 (4 → 18) not taken.
 39 const Q ret = a * get_conjugate();
145 // todo(Gustav): should we normalize here? Can we get a invalid vector?
146
1/2
✓ Branch 0 (7 → 8) taken 39 times.
✗ Branch 1 (7 → 19) not taken.
 39 const auto normalized = ret.get_vec_part().get_normalized();
147
1/2
✗ Branch 0 (9 → 10) not taken.
✓ Branch 1 (9 → 14) taken 39 times.
 39 if(normalized.has_value() == false)
148 {
149 ✗ DIE("invalid rotation vector");
150 ✗ return kk::up;
151 }
152
153 39 return *normalized;
154 }
155
156 10 Q add(const Q& lhs, const Q& rhs)
157 {
158 10 return { lhs.w + rhs.w, {lhs.x + rhs.x, lhs.y + rhs.y, lhs.z + rhs.z} };
159 }
160
161
162 Q
163 5 Q::nlerp(const Q& f, const float scale, const Q& t)
164 {
165 5 const auto lhs = f * (1 - scale);
166 5 const auto rhs = t * scale;
167 5 return add(lhs, rhs).get_normalized();
168 }
169
170
171 Q
172 5 Q::slerp_fast(const Q& qa, const float t, const Q& qb)
173 {
174 // from:
175 // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/
176 // Calculate angle between them.
177 5 const float cos_half_theta = qa.w * qb.w + qa.x * qb.x + qa.y * qb.y + qa.z * qb.z;
178 // if qa=qb or qa=-qb then theta = 0 and we can return qa
179
1/2
✗ Branch 0 (3 → 4) not taken.
✓ Branch 1 (3 → 5) taken 5 times.
 5 if(cabs(cos_half_theta) >= 1.0f)
180 {
181 ✗ return qa;
182 }
183 // Calculate temporary values.
184
1/2
✓ Branch 0 (5 → 6) taken 5 times.
✗ Branch 1 (5 → 26) not taken.
 5 const auto half_theta = eu::acos(cos_half_theta);
185 5 const auto sin_half_theta = std::sqrt(1.0f - cos_half_theta * cos_half_theta);
186
1/2
✗ Branch 0 (8 → 9) not taken.
✓ Branch 1 (8 → 14) taken 5 times.
 5 if(cabs(sin_half_theta) < 0.001f)
187 {
188 // if theta = 180 degrees then result is not fully defined
189 // we could rotate around any axis normal to qa or qb
190 ✗ const Q qt = add(qa, qb);
191 ✗ return Q
192 {
193 ✗ qt.w * 0.5f,
194 ✗ v3
195 {
196 ✗ qt.x * 0.5f,
197 ✗ qt.y * 0.5f,
198 ✗ qt.z * 0.5f
199 ✗ }
200 ✗ };
201 }
202
2/4
✓ Branch 0 (14 → 15) taken 5 times.
✗ Branch 1 (14 → 24) not taken.
✓ Branch 2 (15 → 16) taken 5 times.
✗ Branch 3 (15 → 24) not taken.
 5 const float ratio_a = eu::sin((1 - t) * half_theta) / sin_half_theta;
203
2/4
✓ Branch 0 (16 → 17) taken 5 times.
✗ Branch 1 (16 → 25) not taken.
✓ Branch 2 (17 → 18) taken 5 times.
✗ Branch 3 (17 → 25) not taken.
 5 const float ratio_b = eu::sin(t * half_theta) / sin_half_theta;
204 5 return add(qa * ratio_a, qb * ratio_b);
205 }
206
207
208 Q
209 5 Q::slerp(const Q& from, const float scale, const Q& to)
210 {
211
1/2
✗ Branch 0 (3 → 4) not taken.
✓ Branch 1 (3 → 8) taken 5 times.
 5 if(dot(from, to) < 0)
212 {
213 ✗ return slerp_fast(from.get_negated(), scale, to);
214 }
215 else
216 {
217 5 return slerp_fast(from, scale, to);
218 }
219 }
220
221
222 void
223 22 Q::operator*=(float rhs)
224 {
225 22 x *= rhs;
226 22 y *= rhs;
227 22 z *= rhs;
228 22 w *= rhs;
229 22 }
230
231
232 void
233 85 Q::operator*=(const Q& rhs)
234 {
235 #define VAR(a, b) const float a##1##b##2 = a * rhs.b
236 85 VAR(w, w);
237 85 VAR(w, x);
238 85 VAR(w, y);
239 85 VAR(w, z);
240
241 85 VAR(x, w);
242 85 VAR(x, x);
243 85 VAR(x, y);
244 85 VAR(x, z);
245
246 85 VAR(y, w);
247 85 VAR(y, x);
248 85 VAR(y, y);
249 85 VAR(y, z);
250
251 85 VAR(z, w);
252 85 VAR(z, x);
253 85 VAR(z, y);
254 85 VAR(z, z);
255 #undef VAR
256
257 85 w = w1w2 - x1x2 - y1y2 - z1z2;
258 85 x = w1x2 + x1w2 + y1z2 - z1y2;
259 85 y = w1y2 + y1w2 + z1x2 - x1z2;
260 85 z = w1z2 + z1w2 + x1y2 - y1x2;
261 85 }
262
263 6 Q Q::look_in_direction(const n3& dir, const n3& up)
264 {
265 6 const v3 in = kk::in;
266
1/2
✓ Branch 0 (2 → 3) taken 6 times.
✗ Branch 1 (2 → 32) not taken.
 6 const float dot_value = in.dot(dir);
267
268
1/2
✗ Branch 0 (4 → 5) not taken.
✓ Branch 1 (4 → 8) taken 6 times.
 6 if (cabs(dot_value - (-1.0f)) < 0.000001f)
269 {
270 // todo(Gustav): replace with a constant in general but this line specifically
271 ✗ return {3.1415926535897932f, up};
272 }
273
2/2
✓ Branch 0 (9 → 10) taken 1 times.
✓ Branch 1 (9 → 11) taken 5 times.
 6 if (cabs(dot_value - (1.0f)) < 0.000001f)
274 {
275 1 return q_identity;
276 }
277
278
1/2
✓ Branch 0 (11 → 12) taken 5 times.
✗ Branch 1 (11 → 32) not taken.
 5 const auto rot_angle = acos(dot_value);
279
2/4
✓ Branch 0 (12 → 13) taken 5 times.
✗ Branch 1 (12 → 27) not taken.
✓ Branch 2 (13 → 14) taken 5 times.
✗ Branch 3 (13 → 27) not taken.
 5 const auto rot_axis = in.cross(dir).get_normalized();
280
1/2
✗ Branch 0 (15 → 16) not taken.
✓ Branch 1 (15 → 20) taken 5 times.
 5 if(rot_axis.has_value() == false)
281 {
282 ✗ DIE("missing rot_axis");
283 ✗ return q_identity;
284 }
285
2/4
✓ Branch 0 (21 → 22) taken 5 times.
✗ Branch 1 (21 → 31) not taken.
✓ Branch 2 (22 → 23) taken 5 times.
✗ Branch 3 (22 → 31) not taken.
 5 return Q::from(rha(*rot_axis, rot_angle));
286 }
287
288
289 2 std::string string_from(const Q& v)
290 {
291 4 return fmt::format("({}, ({}, {}, {}))", v.w, v.x, v.y, v.z);
292 }
293
294
295 float
296 5 dot(const Q& lhs, const Q& rhs)
297 {
298 5 return lhs.x * rhs.x + lhs.y * rhs.y + lhs.z * rhs.z + lhs.w * rhs.w;
299 }
300
301
302 85 Q operator*(const Q& lhs, const Q& rhs)
303 {
304 85 Q r = lhs;
305 85 r *= rhs;
306 85 return r;
307 }
308
309
310 1 Q operator*(float scale, const Q& q)
311 {
312 1 Q r = q;
313 1 r *= scale;
314 1 return r;
315 }
316
317
318 21 Q operator*(const Q& q, float scale)
319 {
320 21 Q r = q;
321 21 r *= scale;
322 21 return r;
323 }
324
325
2/4
✓ Branch 0 (2 → 3) taken 1 times.
✗ Branch 1 (2 → 10) not taken.
✓ Branch 2 (3 → 4) taken 1 times.
✗ Branch 3 (3 → 8) not taken.
 1 ADD_CATCH_FORMATTER_IMPL(Q)
326 }
327
328