-
Notifications
You must be signed in to change notification settings - Fork 4
/
Copy pathinertial_math.h
134 lines (109 loc) · 2.62 KB
/
inertial_math.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
//
// Created by tdhuang on 2022/4/12.
//
#ifndef GRIZZ_INS_INERTIAL_MATH_H
#define GRIZZ_INS_INERTIAL_MATH_H
#include <stddef.h>
#include <malloc.h>
enum{ERR_MAT_DIM=-1,ERR_NORM_ZERO=-2};
#define MID(C, i, j) ((i)*(C)+(j))
#define SQR(d) ((d)*(d))
/// zero-judge
/// \param d
/// \return
extern int is_zero(double d);
/// matrix zero-judge
/// \param d
/// \param n
/// \return
extern int all_zero(double *d, size_t n);
/// create matrix rXc
/// \param r
/// \param c
/// \return
extern double *matrix(size_t r, size_t c);
/// create zero-matrix rXc
/// \param r
/// \param c
/// \return
extern double *zeros(size_t r, size_t c);
/// create identity matrix rXc
/// \param r
/// \return
extern double *identity(size_t r);
/// create vector rX1
/// \param r
/// \return
extern double *vector(size_t r);
/// multiply of quaternion, self-assign unsafe
/// \param p
/// \param q
/// \param m
extern void quaternion_multiply(const double *p, const double *q, double *m);
/// vector dot multiply
/// \param a
/// \param b
/// \param r
/// \return
extern double dot(const double *a, const double *b, size_t r);
/// module of vector
/// \param a
/// \param r
/// \return
extern double norm(double *a, size_t r);
/// normalization of vector
/// \param a
/// \param n
/// \param m
extern void normalize(double *a, size_t n, double *m);
/// cross multiply of vector3, self-assign unsafe
/// \param a
/// \param b
/// \param m
extern void cross3(const double *a, const double *b, double *m);
/// cross multiply matrix of vector3, self-assign unsafe
/// \param a
/// \param m
extern void cross3_matrix(const double *a, double *m);
/// copy of matrix
/// \param a
/// \param r
/// \param c
/// \param m
extern void copy(const double *a, size_t r, size_t c, double *m);
/// matrix plus
/// \param a
/// \param b
/// \param r
/// \param c
/// \param m
extern void plus(const double *a, const double *b, size_t r, size_t c, double *m);
/// matrix subtract
/// \param a
/// \param b
/// \param r
/// \param c
/// \param m
extern void subtract(const double *a, const double *b, size_t r, size_t c, double *m);
/// n*I-A
/// \param a
/// \param n
/// \param r
/// \param m
extern void identity_subtract(const double *a, double n, size_t r, double *m);
/// matrix multiply, self-assign unsafe
/// \param a
/// \param b
/// \param r
/// \param n
/// \param c
/// \param m
extern void multiply(const double *a, const double *b, size_t r, size_t n, size_t c, double *m);
/// scalar multiply
/// \param a
/// \param n
/// \param r
/// \param c
/// \param m
extern void multiply_number(const double *a, double n, size_t r, size_t c, double *m);
#endif //GRIZZ_INS_INERTIAL_MATH_H