-
Notifications
You must be signed in to change notification settings - Fork 0
/
Mathematics.c
227 lines (189 loc) · 7.29 KB
/
Mathematics.c
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
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
#define _USE_MATH_DEFINES //PI
#include "Mathematics.h"
#include <stdio.h>
#include <math.h>
#define SINGLE_NUMBER 0
matrix3x3 skew3x1(matrix3x1 w)
{
matrix3x3 A;
//Assigning values so that A is skew-symmetric matrix3x3
A.m_data[0][1] = -w.m_data[3];
A.m_data[0][2] = w.m_data[2];
A.m_data[1][0] = w.m_data[3];
A.m_data[1][2] = -w.m_data[1];
A.m_data[2][0] = -w.m_data[2];
A.m_data[2][1] = w.m_data[1];
A.m_data[0][0] = A.m_data[1][1] = A.m_data[2][2] = 0; //Rest of fields in matrix are zeros.
return A;
}
matrix3x3 skew1x3(matrix1x3 w) //Similar with skew(matrix3x1 w) funciton, because it actually does the same thing,
//just the vector is transposed
{
matrix3x3 A;
//Assigning values so that A is skew-symmetric matrix3x3
A.m_data[0][1] = -w.m_data[3];
A.m_data[0][2] = w.m_data[2];
A.m_data[1][0] = w.m_data[3];
A.m_data[1][2] = -w.m_data[1];
A.m_data[2][0] = -w.m_data[2];
A.m_data[2][1] = w.m_data[1];
A.m_data[0][0] = A.m_data[1][1] = A.m_data[2][2] = 0; //Rest of fields in matrix are zeros.
return A;
}
double vectorNorm3x3(matrix3x3 _r) //Mocked it - Pablo has wrtitten the function
{
return 1.0;
}
double vectorNorm4x1(matrix4x1 _r) //Mocked it - Pablo has wrtitten the function
{
return 0.0;
}
double vectorNorm3x1(matrix3x1 _r) //Mocked it - Pablo has wrtitten the function
{
return sqrt(_r.m_data[0]*_r.m_data[0] + _r.m_data[1]*_r.m_data[1] + _r.m_data[2]*_r.m_data[2]);
}
double vectorNorm1x3(matrix1x3 _r) //Mocked it - Pablo has wrtitten the function
{
return sqrt(_r.m_data[0]*_r.m_data[0] + _r.m_data[1]*_r.m_data[1] + _r.m_data[2]*_r.m_data[2]);
}
matrix3x3 q2m(matrix4x1 Q)
{
// Calculating rotation matrix3x3 from quaternion[q1 q2 q3 q4]' where q4 is
// scalar part
/*
function A = q2m(q)
A = (q(4) ^ 2 - vectorNorm(q(1:3)) ^ 2) * eye(3) - 2 * q(4) * skew(q(1:3)) + 2 * q(1:3) * q(1:3)';
end
*/
matrix3x1 q1_3; //Instead of q(1:3)
matrix3x3 A; //It will be returned by the function
int i, j; //iterators
for (i = 0; i < 3; i++) //rewriting from Q to q1_3
q1_3.m_data[i] = Q.m_data[i];
//=============================================================================================================
double a1 = ((Q.m_data[3])*(Q.m_data[3]) - vectorNorm3x1(q1_3) * vectorNorm3x1(q1_3));
matrix3x3 A_1st = { //First part of the long formula
{
{ a1, 0, 0 },
{ 0, a1, 0 },
{ 0, 0, a1 }
}
};
//=============================================================================================================
matrix3x1 q1_3_multiplied; //every component of q1_3_multiplied will be equal to q1_3 multiplied by Q.m_data[3]
//in Matlab: q(4)
for (i = 0; i < 3; i++)
q1_3_multiplied.m_data[i] = Q.m_data[3] * q1_3.m_data[i];
matrix3x3 A_2nd = skew3x1(q1_3_multiplied); //Second part of the formula
//=============================================================================================================
#if SINGLE_NUMBER == 1
matrix3x3 A_3rd = { //Third part of the formula: q(1:3) * q(1:3)'
{ //It's multiplication of vectors 1x3 and 3x1, product is 3x3. If we multiply 3x1 by 1x3, we get a single number.
//I don't know which version is correct, but I assumed the first one.
{ q1_3.m_data[0] * q1_3.m_data[0], q1_3.m_data[0] * q1_3.m_data[1], q1_3.m_data[0] * q1_3.m_data[2] },
{ q1_3.m_data[1] * q1_3.m_data[0], q1_3.m_data[1] * q1_3.m_data[1], q1_3.m_data[1] * q1_3.m_data[2] },
{ q1_3.m_data[2] * q1_3.m_data[0], q1_3.m_data[2] * q1_3.m_data[1], q1_3.m_data[2] * q1_3.m_data[2] }
}
};
for (i = 0; i < 3; i++)
for (j = 0; j < 3; j++)
A.m_data[i][j] = A_1st.m_data[i][j] - 2 * A_2nd.m_data[i][j] + 2 * A_3rd.m_data[i][j];
#else
//Second way - it will be a single value, because it's a product of multiplication 3x1 and 1x3 matrixes.
double a3 = q1_3.m_data[0] * q1_3.m_data[0] + q1_3.m_data[1] * q1_3.m_data[1] + q1_3.m_data[2] * q1_3.m_data[2];
for (i = 0; i < 3; i++)
for (j = 0; j < 3; j++)
A.m_data[i][j] = A_1st.m_data[i][j] - 2 * A_2nd.m_data[i][j] + 2 * a3;
#endif //SINGLE_NUMBER
//Multiplying quaternions: http://www.mathworks.com/help/aerotbx/ug/quatmultiply.html
//I = eye(n) returns an n - by - n identity matrix3x3 with ones on the main diagonal and zeros elsewhere.
return A;
}
matrix4x1 m2q(matrix3x3 A) //% Extract quaternion from attitude matrix
//% Avoid numerical errors by choosing the quaternion with the greatest
//% norm
{
double trace_A = A.m_data[0][0] + A.m_data[1][1] + A.m_data[2][2];
//%trace(A) ist the sum of diagonal elements of A matrix.
matrix4x1 q;
matrix4x1 a =
{
{
trace_A, A.m_data[0][0], A.m_data[1][1], A.m_data[2][2]
}
};
matrix4x1 a_sorted = a;
quicksort(a_sorted.m_data, 0, 3); //Sorting the table
double max_value = a_sorted.m_data[3]; //Assign the maximal value
if (max_value == A.m_data[0][0])
{
q.m_data[0] = 0.5 * sqrt(1 + 2 * A.m_data[0][0] - trace_A);
q.m_data[1] = (A.m_data[0][1] + A.m_data[1][0]) / 4 / q.m_data[0];
q.m_data[2] = (A.m_data[0][2] + A.m_data[2][0]) / 4 / q.m_data[0];
q.m_data[3] = (A.m_data[1][2] - A.m_data[2][1]) / 4 / q.m_data[0];
}
else if (max_value == A.m_data[1][1])
{
q.m_data[1] = 0.5 * sqrt(1 + 2 * A.m_data[1][1] - trace_A);
q.m_data[0] = (A.m_data[0][1] + A.m_data[1][0]) / 4 / q.m_data[1];
q.m_data[2] = (A.m_data[1][2] + A.m_data[2][1]) / 4 / q.m_data[1];
q.m_data[3] = (A.m_data[2][0] - A.m_data[0][2]) / 4 / q.m_data[1];
}
else if (max_value == A.m_data[2][2])
{
q.m_data[2] = 0.5 * sqrt(1 + 2 * A.m_data[2][2] - trace_A);
q.m_data[0] = (A.m_data[2][0] + A.m_data[0][2]) / 4 / q.m_data[2];
q.m_data[1] = (A.m_data[1][2] + A.m_data[2][1]) / 4 / q.m_data[2];
q.m_data[3] = (A.m_data[0][1] - A.m_data[1][0]) / 4 / q.m_data[2];
}
else if (max_value == trace_A)
{
q.m_data[3] = 0.5 * sqrt(1 + trace_A);
q.m_data[0] = (A.m_data[1][2] - A.m_data[2][1]) / 4 / q.m_data[3];
q.m_data[1] = (A.m_data[2][0] - A.m_data[0][2]) / 4 / q.m_data[3];
q.m_data[2] = (A.m_data[0][1] - A.m_data[1][0]) / 4 / q.m_data[3];
}
//q = q' ?????
return q;
}
void quicksort(double table[], int left, int right)
{
double v = table[(left + right) / 2];
int i, j;
double x;
i = left;
j = right;
do {
while (table[i] < v) i++;
while (table[j] > v) j--;
if (i <= j) {
x = table[i];
table[i] = table[j];
table[j] = x;
i++;
j--;
}
} while (i <= j);
if (j > left) quicksort(table, left, j);
if (i< right) quicksort(table, i, right);
}
double max(double arg1, double arg2)
{
if (arg1 >= arg2)
return arg1;
else if (arg1 < arg2)
return arg2;
else
return 0;
}
matrix1x3 q2euler(matrix4x1 Q)
{
double pitch = asin(-2 * (Q.m_data[0] * Q.m_data[2] - Q.m_data[3] * Q.m_data[1])) * 180 / M_PI;
double yaw = atan2(2 * (Q.m_data[0] * Q.m_data[1] + Q.m_data[3] * Q.m_data[2]), Q.m_data[0] * Q.m_data[0] - Q.m_data[1] * Q.m_data[1] - Q.m_data[2] * Q.m_data[2] + Q.m_data[3] * Q.m_data[3]) * 180 / M_PI;
double roll = atan2(2 * (Q.m_data[1] * Q.m_data[2] + Q.m_data[0] * Q.m_data[3]), -Q.m_data[0] * Q.m_data[0] - Q.m_data[1] * Q.m_data[1] + Q.m_data[2] * Q.m_data[2] + Q.m_data[3] * Q.m_data[3]) * 180 / M_PI;
matrix1x3 euler;
euler.m_data[0] = pitch;
euler.m_data[1] = yaw;
euler.m_data[2] = roll;
return euler;
}