docs/html/mat33_8h_source.html
| | Jetson Inference
DNN Vision Library |
mat33.h
Go to the documentation of this file.
1 /*
2 * Copyright (c) 2019, NVIDIA CORPORATION. All rights reserved.
3 *
4 * Permission is hereby granted, free of charge, to any person obtaining a
5 * copy of this software and associated documentation files (the "Software"),
6 * to deal in the Software without restriction, including without limitation
7 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
8 * and/or sell copies of the Software, and to permit persons to whom the
9 * Software is furnished to do so, subject to the following conditions:
10 *
11 * The above copyright notice and this permission notice shall be included in
12 * all copies or substantial portions of the Software.
13 *
14 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
17 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
19 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
20 * DEALINGS IN THE SOFTWARE.
21 */
22
23 #ifndef __MATRIX_33_H_
24 #define __MATRIX_33_H_
25
26 #include <stdio.h>
27 #include <unistd.h>
28 #include <math.h>
29
30 #include "logging.h"
31
32
37 template<typename T1, typename T2>
38 inline void mat33_cast( T1 dst[3][3], const T2 src[3][3] )
39 {
40for( uint32_t i=0; i < 3; i++ )
41for( uint32_t j=0; j < 3; j++ )
42 dst[i][j] = (T1)src[i][j];
43 }
44
45
50 template<typename T>
51 inline void mat33_copy( T dst[3][3], const T src[3][3] )
52 {
53 memcpy(dst, src, sizeof(T) * 9);
54 }
55
56
61 template <typename T>
62 inline T mat33_det( const T src[3][3] )
63 {
64return src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1])
65 - src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0])
66 + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
67 }
68
69
74 template<typename T>
75 inline void mat33_identity( T dst[3][3] )
76 {
77 dst[0][0] = 1; dst[0][1] = 0; dst[0][2] = 0;
78 dst[1][0] = 0; dst[1][1] = 1; dst[1][2] = 0;
79 dst[2][0] = 0; dst[2][1] = 0; dst[2][2] = 1;
80 }
81
82
88 template<typename T>
89 inline void mat33_inverse( T dst[3][3], const T src[3][3] )
90 {
91 T inv[3][3];
92
93// invert
94const T det = mat33_det(src);
95
96 inv[0][0] = + (src[1][1] * src[2][2] - src[1][2] * src[2][1]);
97 inv[0][1] = - (src[0][1] * src[2][2] - src[0][2] * src[2][1]);
98 inv[0][2] = + (src[0][1] * src[1][2] - src[0][2] * src[1][1]);
99 inv[1][0] = - (src[1][0] * src[2][2] - src[1][2] * src[2][0]);
100 inv[1][1] = + (src[0][0] * src[2][2] - src[0][2] * src[2][0]);
101 inv[1][2] = - (src[0][0] * src[1][2] - src[0][2] * src[1][0]);
102 inv[2][0] = + (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
103 inv[2][1] = - (src[0][0] * src[2][1] - src[0][1] * src[2][0]);
104 inv[2][2] = + (src[0][0] * src[1][1] - src[0][1] * src[1][0]);
105
106// scale by determinant
107for( uint32_t i=0; i < 3; i++ )
108for( uint32_t k=0; k < 3; k++ )
109 dst[i][k] = inv[i][k] / det;
110 }
111
112
117 template<typename T>
118 inline void mat33_multiply( T dst[3][3], const T a[3][3], const T b[3][3] )
119 {
120 T tmp[3][3];
121
122for( int i=0; i < 3; i++ )
123 {
124for( int j=0; j < 3; j++ )
125 {
126 tmp[i][j] = 0;
127
128for( int k=0; k < 3; k++ )
129 tmp[i][j] = tmp[i][j] + a[i][k] * b[k][j];
130 }
131 }
132
133mat33_copy(dst, tmp);
134 }
135
136
141 template<typename T>
142 inline void mat33_print( const T src[3][3], const char* name=NULL )
143 {
144if( name != NULL )
145LogInfo("%s = \n", name);
146
147 printf(" [ ");
148
149for( uint32_t i=0; i < 3; i++ )
150 {
151for( uint32_t j=0; j < 3; j++ )
152LogInfo("%f ", src[i][j]);
153
154if( i < 2 )
155 {
156LogInfo("\n ");
157 }
158else
159 {
160LogInfo("]\n");
161 }
162 }
163 }
164
165
170 template<typename T>
171 inline int mat33_rank( const T src[3][3] )
172 {
173 T mat[3][3];
174
175// reducing to row-echelon form alters the matrix
176mat33_copy(mat, src);
177
178// iteratively compute the rank
179int rank = 3; // (= #columns)
180
181for( int row=0; row < rank; row++ )
182 {
183if( mat[row][row] != 0 )
184 {
185for( int col=0; col < 3; col++ )
186 {
187if (col != row)
188 {
189const T mult = mat[col][row] / mat[row][row];
190
191for( int i = 0; i < rank; i++ )
192 mat[col][i] -= mult * mat[row][i];
193 }
194 }
195 }
196else
197 {
198bool reduce = true;
199
200for( int i=row+1; i < 3; i++ )
201 {
202if( mat[i][row] != 0 )
203 {
204for( int k=0; k < rank; k++ )
205 {
206const T tmp = mat[row][k];
207 mat[row][k] = mat[i][k];
208 mat[i][k] = tmp;
209 }
210
211 reduce = false;
212 break ;
213 }
214 }
215
216if( reduce )
217 {
218 rank--;
219
220for( int i=0; i < 3; i++ )
221 mat[i][row] = mat[i][rank];
222 }
223
224 row--;
225 }
226 }
227
228return rank;
229 }
230
231
236 template<typename T>
237 inline void mat33_translate( T dst[3][3], T x, T y )
238 {
239mat33_identity(dst);
240
241 dst[0][2] = x;
242 dst[1][2] = y;
243 }
244
245
250 template<typename T>
251 inline void mat33_translate( T dst[3][3], T src[3][3], T x, T y )
252 {
253 T m[3][3];
254
255mat33_translate(m, x, y);
256mat33_multiply(dst, src, m);
257 }
258
259
264 template<typename T>
265 inline void mat33_rotation( T dst[3][3], T degrees )
266 {
267mat33_identity(dst);
268
269const T rad = 0.01745329251 * degrees;
270
271const T c = cos(rad);
272const T s = sin(rad);
273
274 dst[0][0] = c;
275 dst[0][1] = -s;
276 dst[1][0] = s;
277 dst[1][1] = c;
278 }
279
280
285 template<typename T>
286 inline void mat33_rotation( T dst[3][3], T src[3][3], T degrees )
287 {
288 T m[3][3];
289
290mat33_rotation(m, degrees);
291mat33_multiply(dst, src, m);
292 }
293
294
299 template<typename T>
300 inline void mat33_rotation( T dst[3][3], T degrees, float origin_x, float origin_y )
301 {
302mat33_translate(dst, origin_x, origin_y);
303mat33_rotation(dst, dst, degrees);
304mat33_translate(dst, dst, -origin_x, -origin_y);
305 }
306
307
312 template<typename T>
313 inline void mat33_rotation( T dst[3][3], T src[3][3], T degrees, float origin_x, float origin_y )
314 {
315mat33_translate(dst, src, origin_x, origin_y);
316mat33_rotation(dst, dst, degrees);
317mat33_translate(dst, dst, -origin_x, -origin_y);
318 }
319
320
325 template<typename T>
326 inline void mat33_scale( T dst[3][3], T sx, T sy )
327 {
328mat33_identity(dst);
329
330 dst[0][0] = sx;
331 dst[1][1] = sy;
332 }
333
334
339 template<typename T>
340 inline void mat33_scale( T dst[3][3], T src[3][3], T sx, T sy )
341 {
342 T m[3][3];
343
344mat33_scale(m, sx, sy);
345mat33_multiply(dst, src, m);
346 }
347
348
353 template<typename T>
354 inline void mat33_shear( T dst[3][3], T sx, T sy )
355 {
356mat33_identity(dst);
357
358 dst[0][1] = sx;
359 dst[1][0] = sy;
360 }
361
362
367 template<typename T>
368 inline void mat33_shear( T dst[3][3], T src[3][3], T sx, T sy )
369 {
370 T m[3][3];
371
372mat33_shear(m, sx, sy);
373mat33_multiply(dst, src, m);
374 }
375
376
381 template<typename T>
382 inline void mat33_swap( T a[3][3], T b[3][3] )
383 {
384 T c[3][3];
385
386mat33_copy(c, a);
387mat33_copy(a, b);
388mat33_copy(b, c);
389 }
390
391
396 template<typename T>
397 inline T mat33_trace( const T src[3][3] )
398 {
399return src[0][0] + src[1][1] + src[2][2];
400 }
401
402
407 template<typename T>
408 inline void mat33_transform( T& x_out, T& y_out, const T x_in, const T y_in, const T mat[3][3] )
409 {
410const T x = mat[0][0] * x_in + mat[0][1] * y_in + mat[0][2];
411const T y = mat[1][0] * x_in + mat[1][1] * y_in + mat[1][2];
412const T z = mat[2][0] * x_in + mat[2][1] * y_in + mat[2][2];
413
414 x_out = x / z;
415 y_out = y / z;
416 }
417
418
423 template<typename T>
424 inline void mat33_transform( T dst[2], const T src[2], const T mat[3][3] )
425 {
426mat33_transform(dst[0], dst[1], src[0], src[1], mat);
427 }
428
429
434 template<typename T>
435 inline void mat33_transform( T* dst, const T* src, const int N, const T mat[3][3] )
436 {
437for( uint32_t n=0; n < N; n++ )
438mat33_transform(dst[n*2], dst[n*2+1], src[n*2], src[n*2+1], mat);
439 }
440
445 template<typename T>
446 inline void mat33_transpose( T dst[3][3], const T src[3][3] )
447 {
448 T tmp[3][3];
449
450for( uint32_t i=0; i < 3; i++ )
451for( uint32_t j=0; j < 3; j++ )
452 tmp[i][j] = src[j][i];
453
454mat33_copy(dst, tmp);
455 }
456
457
462 template<typename T>
463 inline void mat33_zero( T dst[3][3] )
464 {
465 memset(dst, 0, sizeof(T) * 9);
466 }
467
468
469 #endif
470
void mat33_inverse(T dst[3][3], const T src[3][3])
Compute the inverse of a 3x3 matrix, dst=src^-1 It is safe to have dst and src be the same memory.
Definition: mat33.h:89
#define LogInfo(format, args...)
Log a printf-style info message (Log::INFO)
Definition: logging.h:168
void mat33_cast(T1 dst[3][3], const T2 src[3][3])
Cast a 3x3 matrix from one type to another.
Definition: mat33.h:38
void mat33_transform(T &x_out, T &y_out, const T x_in, const T y_in, const T mat[3][3])
Transform a 2D vector by a 3x3 matrix.
Definition: mat33.h:408
void mat33_scale(T dst[3][3], T sx, T sy)
Initialize a 3x3 scaling matrix.
Definition: mat33.h:326
void mat33_print(const T src[3][3], const char *name=NULL)
Print out a 3x3 matrix to stdout.
Definition: mat33.h:142
int mat33_rank(const T src[3][3])
Determine the rank of a 3x3 matrix.
Definition: mat33.h:171
void mat33_shear(T dst[3][3], T sx, T sy)
Initialize a 3x3 shear matrix.
Definition: mat33.h:354
T mat33_trace(const T src[3][3])
Compute the trace of a 3x3 matrix, returns tr(src)
Definition: mat33.h:397
void mat33_zero(T dst[3][3])
Set a 3x3 matrix to all zero's.
Definition: mat33.h:463
T mat33_det(const T src[3][3])
Compute the determinant of a 3x3 matrix, returns |src|
Definition: mat33.h:62
void mat33_swap(T a[3][3], T b[3][3])
Swap two 3x3 matrices inline, a=b and b=a
Definition: mat33.h:382
void mat33_multiply(T dst[3][3], const T a[3][3], const T b[3][3])
Multiply two 3x3 matrices, dst=a*b
Definition: mat33.h:118
void mat33_rotation(T dst[3][3], T degrees)
Initialize a 3x3 rotation matrix.
Definition: mat33.h:265
void mat33_identity(T dst[3][3])
Initialize a 3x3 identity matrix.
Definition: mat33.h:75
void mat33_copy(T dst[3][3], const T src[3][3])
Copy src input matrix to dst output.
Definition: mat33.h:51
void mat33_translate(T dst[3][3], T x, T y)
Initialize a 3x3 translation matrix.
Definition: mat33.h:237
void mat33_transpose(T dst[3][3], const T src[3][3])
Transpose a 3x3 matrix, dst=src^T
Definition: mat33.h:446