GCC Code Coverage Report

Directory: ./
File: src/util/numerical/spatialRotations.c
Date: 2025-09-01 06:19:01
Exec Total Coverage
Lines: 3 33 9.1%
Functions: 3 53 5.7%
Branches: 1 6 16.7%
Line Branch Exec Source
1 #include "na64util/numerical/spatialRotations.h"
2
3 #include <math.h>
4 #include <string.h>
5 #include <assert.h>
6
7 #ifdef STANDALONE_BUILD
8 # include <stdio.h>
9 #endif
10
11 /* Reference: https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix */
12
13 static const struct {
14 char ext[3], intr[3];
15 } _gStrOrder[] = {
16 { "XZX", "xzx" }, { "XYX", "xyx" }, { "YXY", "yxy" },
17 { "YZY", "yzy" }, { "ZYZ", "zyz" }, { "ZXZ", "zxz" },
18 { "XZY", "yzx" }, { "XYZ", "zyx" }, { "YXZ", "zxy" },
19 { "YZX", "xzy" }, { "ZYX", "xyz" }, { "ZXY", "yxz" },
20 };
21
22 int
23 ✗ na64sw_rotation_order_from_str( const char * str
24 , enum na64sw_RotationOrder * dest ) {
25 ✗ for(int i = 0; i < sizeof(_gStrOrder)/sizeof(*_gStrOrder); ++i) {
26 ✗ if( strncmp(str, _gStrOrder[i].ext, 3)
27 ✗ && strncmp(str, _gStrOrder[i].intr, 3) ) continue;
28 ✗ *dest = (enum na64sw_RotationOrder) i;
29 ✗ return 0;
30 }
31 ✗ return 1;
32 }
33
34 /*
35 * Euler rotation matrices
36 */
37 #define _M_Eul_X1_Z2_X3 \
38 c2, -c3*s2, s2*s3, \
39 c1*s2, c1*c2*c3 - s1*s3, -c3*s1 - c1*c2*s3, \
40 s1*s2, c1*s3 + c2*c3*s1, c1*c3 - c2*s1*s3
41 #define _M_Rev_X1_Z2_X3 \
42 a[0] = atan2(R[2*3+0], R[1*3+0]); \
43 a[1] = acos(R[0*3+0]); \
44 a[2] = atan2(R[0*3+2], -R[0*3+1]);
45
46 #define _M_Eul_X1_Y2_X3 \
47 c2, s2*s3, c3*s2, \
48 s1*s2, c1*c3-c2*s1*s3, -c1*s3-c2*c3*s1, \
49 -c1*s2, c3*s1+c1*c2*s3, c1*c2*c3-s1*s3
50 #define _M_Rev_X1_Y2_X3 \
51 a[0] = atan2(R[1*3+0], -R[2*3+0]); \
52 a[1] = acos(R[0*3+0]); \
53 a[2] = atan2(R[0*3+1], R[0*3+2]);
54
55 #define _M_Eul_Y1_X2_Y3 \
56 c1*c3 - c2*s1*s3, s1*s2, c1*s3 + c2*c3*s1, \
57 s2*s3, c2, -c3*s2, \
58 -c3*s1 - c1*c2*s3, c1*s2, c1*c2*c3 - s1*s3
59 #define _M_Rev_Y1_X2_Y3 \
60 a[0] = atan2(R[0*3+1], R[2*3+1]); \
61 a[1] = acos(R[1*3+1]); \
62 a[2] = atan2(R[1*3+0], -R[1*3+2]);
63
64 #define _M_Eul_Y1_Z2_Y3 \
65 c1*c2*c3 - s1*s3, -c1*s2, c3*s1 + c1*c2*s3, \
66 c3*s2, c2, s2*s3, \
67 -c1*s3 - c2*c3*s1, s1*s2, c1*c3 - c2*s1*s3
68 #define _M_Rev_Y1_Z2_Y3 \
69 a[0] = atan2(R[2*3+1], -R[0*3+1]); \
70 a[1] = acos(R[1*3+1]); \
71 a[2] = atan2(R[1*3+2], R[1*3+0]);
72
73 #define _M_Eul_Z1_Y2_Z3 \
74 c1*c2*c3 - s1*s3, -c3*s1 - c1*c2*s3, c1*s2, \
75 c1*s3 + c2*c3*s1, c1*c3 - c2*s1*s3, s1*s2, \
76 -c3*s2, s2*s3, c2
77 #define _M_Rev_Z1_Y2_Z3 \
78 a[0] = atan2(R[1*3+2], R[0*3+2]); \
79 a[1] = atan2(sqrt(1 - R[2*3+2]*R[2*3+2]), R[2*3+2]); \
80 a[2] = atan2(R[2*3+1], -R[2*3+0]);
81
82 #define _M_Eul_Z1_X2_Z3 \
83 c1*c3 - c2*s1*s3, -c1*s3 - c2*c3*s1, s1*s2, \
84 c3*s1 + c1*c2*s3, c1*c2*c3 - s1*s3, -c1*s2, \
85 s2*s3, c3*s2, c2
86 #define _M_Rev_Z1_X2_Z3 \
87 a[0] = atan2(R[0*3+2], -R[1*3+2]); \
88 a[1] = acos(R[2*3+2]); \
89 a[2] = atan2(R[2*3+0], R[2*3+1]);
90
91 /*
92 * Tait-Bryan rotation matrices
93 */
94 #define _M_TBr_X1_Z2_Y3 \
95 c2*c3, -s2, c2*s3, \
96 s1*s3 + c1*c3*s2, c1*c2, c1*s2*s3-c3*s1, \
97 c3*s1*s2 - c1*s3, c2*s1, c1*c3+s1*s2*s3
98 #define _M_Rev_X1_Z2_Y3 \
99 a[0] = atan2(R[2*3+1], R[1*3+1]); \
100 a[1] = asin(-R[0*3+1]); \
101 a[2] = atan2(R[0*3+2], R[0*3+0]);
102
103 #define _M_TBr_X1_Y2_Z3 \
104 c2*c3, -c2*s3, s2, \
105 c1*s3 + c3*s1*s2, c1*c3 - s1*s2*s3, -c2*s1, \
106 s1*s3 - c1*c3*s2, c3*s1 + c1*s2*s3, c1*c2
107 #define _M_Rev_X1_Y2_Z3 \
108 a[0] = atan2(-R[1*3+2], R[2*3+2]); \
109 a[1] = asin(R[0*3+2]); \
110 a[2] = atan2(-R[0*3+1], R[0*3+0]);
111
112 #define _M_TBr_Y1_X2_Z3 \
113 c1*c3 + s1*s2*s3, c3*s1*s2 - c1*s3, c2*s1, \
114 c2*s3, c2*c3, -s2, \
115 c1*s2*s3 - c3*s1, c1*c3*s2 + s1*s3, c1*c2
116 #define _M_Rev_Y1_X2_Z3 \
117 a[0] = atan2(R[0*3+2], R[2*3+2]); \
118 a[1] = asin(-R[1*3+2]); \
119 a[2] = atan2(R[1*3+0], R[1*3+1]);
120
121 #define _M_TBr_Y1_Z2_X3 \
122 c1*c2, s1*s3 - c1*c3*s2, c3*s1 + c1*s2*s3, \
123 s2, c2*c3, -c2*s3, \
124 -c2*s1, c1*s3 + c3*s1*s2, c1*c3 - s1*s2*s3
125 #define _M_Rev_Y1_Z2_X3 \
126 a[0] = atan2(-R[2*3+0], R[0*3+0]); \
127 a[1] = asin(R[1*3+0]); \
128 a[2] = atan2(-R[1*3+2], R[1*3+1]);
129
130 #define _M_TBr_Z1_Y2_X3 \
131 c1*c2, c1*s2*s3 - c3*s1, s1*s3 + c1*c3*s2, \
132 c2*s1, c1*c3 + s1*s2*s3, c3*s1*s2 - c1*s3, \
133 -s2, c2*s3, c2*c3
134 #define _M_Rev_Z1_Y2_X3 \
135 a[0] = atan2(R[1*3+0], R[0*3+0]); \
136 a[1] = asin(-R[2*3+0]); \
137 a[2] = atan2(R[2*3+1], R[2*3+2]);
138
139 #define _M_TBr_Z1_X2_Y3 \
140 c1*c3 - s1*s2*s3, -c2*s1, c1*s3 + c3*s1*s2, \
141 c3*s1 + c1*s2*s3, c1*c2, s1*s3 - c1*c3*s2, \
142 -c2*s3, s2, c2*c3
143 #define _M_Rev_Z1_X2_Y3 \
144 a[0] = atan2(-R[0*3+1], R[1*3+1]); \
145 a[1] = asin(R[2*3+1]); \
146 a[2] = atan2(-R[2*3+0], R[2*3+2]);
147
148 /*
149 * Implementation
150 */
151
152 #define _M_implem( NAME_SUFFIX, FLOAT, BODY1, BODY2 ) \
153 static void _mx_ ## NAME_SUFFIX (FLOAT * dest, const FLOAT * a) { \
154 const FLOAT s1 = sin(a[0]), c1 = cos(a[0]) \
155 , s2 = sin(a[1]), c2 = cos(a[1]) \
156 , s3 = sin(a[2]), c3 = cos(a[2]) \
157 ; \
158 const FLOAT m[] = { BODY1 }; \
159 memcpy(dest, m, sizeof(m)); \
160 } \
161 static void _angles_ ## NAME_SUFFIX (FLOAT * a, const FLOAT * R) { \
162 BODY2 ; \
163 }
164
165 /* Euler */
166 ✗ _M_implem(xzx_f, float, _M_Eul_X1_Z2_X3, _M_Rev_X1_Z2_X3);
167 ✗ _M_implem(xyx_f, float, _M_Eul_X1_Y2_X3, _M_Rev_X1_Y2_X3);
168 ✗ _M_implem(yxy_f, float, _M_Eul_Y1_X2_Y3, _M_Rev_Y1_X2_Y3);
169 ✗ _M_implem(yzy_f, float, _M_Eul_Y1_Z2_Y3, _M_Rev_Y1_Z2_Y3);
170 ✗ _M_implem(zyz_f, float, _M_Eul_Z1_Y2_Z3, _M_Rev_Z1_Y2_Z3);
171 ✗ _M_implem(zxz_f, float, _M_Eul_Z1_X2_Z3, _M_Rev_Z1_X2_Z3);
172 /* Tait-Bryan */
173 ✗ _M_implem(xzy_f, float, _M_TBr_X1_Z2_Y3, _M_Rev_X1_Z2_Y3);
174 2 _M_implem(xyz_f, float, _M_TBr_X1_Y2_Z3, _M_Rev_X1_Y2_Z3);
175 ✗ _M_implem(yxz_f, float, _M_TBr_Y1_X2_Z3, _M_Rev_Y1_X2_Z3);
176 ✗ _M_implem(yzx_f, float, _M_TBr_Y1_Z2_X3, _M_Rev_Y1_Z2_X3);
177 10 _M_implem(zyx_f, float, _M_TBr_Z1_Y2_X3, _M_Rev_Z1_Y2_X3);
178 ✗ _M_implem(zxy_f, float, _M_TBr_Z1_X2_Y3, _M_Rev_Z1_X2_Y3);
179
180 #define x (0x1)
181 #define y (0x2)
182 #define z (0x3)
183 #define _M_encode_seq( first, second, third ) \
184 (first) | (second << 2) | (third << 4)
185
186 static struct {
187 void (*implem)(float *, const float *);
188 void (*rev)(float *, const float *);
189 } _float_matrices[] = {
190 #define _M_entry(first, second, third) \
191 { _mx_ ## first ## second ## third ## _f \
192 , _angles_ ## first ## second ## third ## _f },
193 _M_entry( x, z, x ) _M_entry( x, y, x )
194 _M_entry( y, x, y ) _M_entry( y, z, y )
195 _M_entry( z, y, z ) _M_entry( z, x, z )
196
197 _M_entry( x, z, y ) _M_entry( x, y, z )
198 _M_entry( y, x, z ) _M_entry( y, z, x )
199 _M_entry( z, y, x ) _M_entry( z, x, y )
200 #undef _M_entry
201 };
202
203 /* Euler */
204 ✗ _M_implem(xzx_d, double, _M_Eul_X1_Z2_X3, _M_Rev_X1_Z2_X3);
205 ✗ _M_implem(xyx_d, double, _M_Eul_X1_Y2_X3, _M_Rev_X1_Y2_X3);
206 ✗ _M_implem(yxy_d, double, _M_Eul_Y1_X2_Y3, _M_Rev_Y1_X2_Y3);
207 ✗ _M_implem(yzy_d, double, _M_Eul_Y1_Z2_Y3, _M_Rev_Y1_Z2_Y3);
208 ✗ _M_implem(zyz_d, double, _M_Eul_Z1_Y2_Z3, _M_Rev_Z1_Y2_Z3);
209 ✗ _M_implem(zxz_d, double, _M_Eul_Z1_X2_Z3, _M_Rev_Z1_X2_Z3);
210 /* Tait-Bryan */
211 ✗ _M_implem(xzy_d, double, _M_TBr_X1_Z2_Y3, _M_Rev_X1_Z2_Y3);
212 ✗ _M_implem(xyz_d, double, _M_TBr_X1_Y2_Z3, _M_Rev_X1_Y2_Z3);
213 ✗ _M_implem(yxz_d, double, _M_TBr_Y1_X2_Z3, _M_Rev_Y1_X2_Z3);
214 ✗ _M_implem(yzx_d, double, _M_TBr_Y1_Z2_X3, _M_Rev_Y1_Z2_X3);
215 ✗ _M_implem(zyx_d, double, _M_TBr_Z1_Y2_X3, _M_Rev_Z1_Y2_X3);
216 ✗ _M_implem(zxy_d, double, _M_TBr_Z1_X2_Y3, _M_Rev_Z1_X2_Y3);
217
218 static struct {
219 void (*implem)(double *, const double *);
220 void (*rev)(double *, const double *);
221 } _double_matrices[] = {
222 #define _M_entry(first, second, third) \
223 { _mx_ ## first ## second ## third ## _d \
224 , _angles_ ## first ## second ## third ## _d },
225 _M_entry( x, z, x ) _M_entry( x, y, x )
226 _M_entry( y, x, y ) _M_entry( y, z, y )
227 _M_entry( z, y, z ) _M_entry( z, x, z )
228
229 _M_entry( x, z, y ) _M_entry( x, y, z )
230 _M_entry( y, x, z ) _M_entry( y, z, x )
231 _M_entry( z, y, x ) _M_entry( z, x, y )
232 #undef _M_entry
233 };
234
235 #define _M_implement_unifunc(FLOAT, SUFFIX) \
236 int \
237 na64sw_rotation_matrix_ ## SUFFIX ( enum na64sw_RotationOrder n \
238 , const FLOAT * angles \
239 , FLOAT * dest \
240 ) { \
241 assert(n < sizeof(_ ## FLOAT ## _matrices) / sizeof(* _ ## FLOAT ## _matrices)); \
242 _ ## FLOAT ## _matrices[n].implem(dest, angles); \
243 return 0; \
244 } \
245 int \
246 na64sw_find_rotation_for_ ## SUFFIX ( enum na64sw_RotationOrder n \
247 , const FLOAT * mx \
248 , FLOAT * dest \
249 ) { \
250 assert(n < sizeof(_ ## FLOAT ## _matrices) / sizeof(* _ ## FLOAT ## _matrices)); \
251 _ ## FLOAT ## _matrices[n].rev(dest, mx); \
252 return 0; \
253 } \
254
255
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 6 times.
 12 _M_implement_unifunc( float, f );
256 ✗ _M_implement_unifunc( double, d );
257
258 #ifdef STANDALONE_BUILD
259 int
260 main(int argc, char * argv[]) {
261 float m[9]
262 , angles[3] = {2*M_PI/6, 3*M_PI/7, 4*M_PI/9}
263 , revAngles[3];
264
265 for(int i = 0; i <= (int) na64sw_Rotation_zxy; ++i ) {
266 const enum na64sw_RotationOrder order
267 = (enum na64sw_RotationOrder) i;
268 na64sw_rotation_matrix_f( order, angles, m );
269 /*for(int p = 0; p < 3; ++p) {
270 for(int k = 0; k < 3; ++k) {
271 printf( "\t%e", m[p*3 + k] );
272 }
273 printf("\n");
274 }*/
275 na64sw_find_rotation_for_f( order, m, revAngles );
276 printf( " #%i: {%e, %e, %e} - {%e %e %e}\n"
277 , i
278 , angles[0], angles[1], angles[2]
279 , revAngles[0], revAngles[1], revAngles[2]
280 );
281 }
282
283 return 0;
284 }
285 #endif /*STANDALONE_BUILD*/
286
287