-
Notifications
You must be signed in to change notification settings - Fork 1
/
math.c
305 lines (246 loc) · 9.07 KB
/
math.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
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
//---------------------------------------------------------------------------
//
// This file contains various vector, matrix, and math routines used
// throughout the E2ES
//
//****************************************************************************/
#include "gnssr.h"
double cot(double z){ return 1.0 / tan(z); }
double sec(double z){ return 1.0 / cos(z); }
double csc(double z){ return 1.0 / sin(z); }
// Rotation matrices for coordinate transformation
void RotationMatrix_X (double theta, double xfrmMatrix[9]){
xfrmMatrix[0]=1; xfrmMatrix[1]=0; xfrmMatrix[2]=0;
xfrmMatrix[3]=0; xfrmMatrix[4]=cos(theta); xfrmMatrix[5]=sin(theta);
xfrmMatrix[6]=0; xfrmMatrix[7]=-sin(theta); xfrmMatrix[8]=cos(theta);
}
void RotationMatrix_Y (double theta, double xfrmMatrix[9]){
xfrmMatrix[0]=cos(theta); xfrmMatrix[1]=0; xfrmMatrix[2]=-sin(theta);
xfrmMatrix[3]=0; xfrmMatrix[4]=1; xfrmMatrix[5]=0;
xfrmMatrix[6]=sin(theta); xfrmMatrix[7]=0; xfrmMatrix[8]=cos(theta);
}
void RotationMatrix_Z (double theta, double xfrmMatrix[9]){
xfrmMatrix[0]=cos(theta); xfrmMatrix[1]=sin(theta); xfrmMatrix[2]=0;
xfrmMatrix[3]=-sin(theta); xfrmMatrix[4]=cos(theta); xfrmMatrix[5]=0;
xfrmMatrix[6]=0; xfrmMatrix[7]=0; xfrmMatrix[8]=1;
}
//-----------------------------------------------------------------------------------
void bubble(int *a,int n)
// bubble ordering
// array a and length n
{
int i,j,temp;
for(i=0;i<n-1;i++){
for(j=i+1;j<n;j++){
if(a[i]>a[j]) {
temp=a[i];
a[i]=a[j];
a[j]=temp;
}
}
}
}
void bilinear_interp(double *x_vec, double *y_vec, int size_x, int size_y, double x, double y, int *bi_index, double *bi_weight, double resolution){
// Find indices from bilinear interpolation
// resolution = lat/lon resolution
// x=lon, y=lat, index= lat * NLON + lon;
int ix1,ix2,iy1,iy2;
double x1,x2,y1,y2;
// find ix1 and ix2, the two index that are nearest to x
for (int i=0; i<size_x; i++){
if (fabs(x_vec[i]-x)<resolution){
ix1=i; ix2=i+1;
x1=x_vec[ix1]; x2=x_vec[ix2];
break;
}
}
for (int i=0; i<size_y; i++){
if (fabs(y_vec[i]-y)<resolution){
iy1=i; iy2=i+1;
y1=y_vec[iy1]; y2=y_vec[iy2];
break;
}
}
//printf("x1 x2 y1 y2= %f %f %f %f\n",x1,x2,y1,y2);
bi_index[0] = iy1*size_x+ix1; //index of Q11
bi_index[1] = iy1*size_x+ix2; //index of Q21
bi_index[2] = iy2*size_x+ix1; //index of Q12
bi_index[3] = iy2*size_x+ix2;//index of Q22
bi_weight[0] = (x2-x)*(y2-y)/((x2-x1)*(y2-y1)); //weight of Q11
bi_weight[1] = (x-x1)*(y2-y)/((x2-x1)*(y2-y1)); //weight of Q21
bi_weight[2] = (x2-x)*(y-y1)/((x2-x1)*(y2-y1)); //weight of Q12
bi_weight[3] = (x-x1)*(y-y1)/((x2-x1)*(y2-y1)); //weight of Q22
}
int find_nearest(double *vec, int size, double value){ //by Feixiong
// find the nearest value in vec[] and return the index
double diff, temp_diff;
int index;
index = 0;
diff = fabs(vec[0]-value);
for (int i = 0;i < size; i++){
temp_diff = fabs(vec[i] - value);
if (temp_diff < diff){
diff = temp_diff;
index = i;
}
}
return index;
}
double linear_interp( double a, double b, int direction, double time_01 ){
// Linear interpolation
if( direction == 0 )
return (1-time_01) * a + time_01 * b;
else
return (1-time_01) * b + time_01 * a;
}
void cubic_interpolation( double f0, double f1, double df0, double df1,
double t, double *ft, double *dft, double timeInterval_s){
// t must be between 0 and 1
if( (t < 0) || (t > 1) ){
printf("Time error in cubic interp!\n");
exit(0);
}
// special cases when no interpolation needed
if(t == 0){ *ft = f0; *dft = df0; return; }
if(t == 1){ *ft = f1; *dft = df1; return; }
df0 = df0 * timeInterval_s;
df1 = df1 * timeInterval_s;
// solve for coef of thrid deg polynom
const double a = 2*f0 - 2*f1 + df0 + df1;
const double b = -3*f0 + 3*f1 - 2*df0 - df1;
const double c = df0;
const double d = f0;
// values of polynom and derivative at t
*ft = a*pow(t,3) + b*pow(t,2) + c*t + d;
*dft = 3*a*pow(t,2) + 2*b*t + c;
*dft = *dft * ( 1.0 / timeInterval_s );
}
void cubic_interpolation_3vector( double f0[3], double f1[3], double df0[3], double df1[3],
double t, double ft[3], double dft[3], double timeInterval_s){
// cubic interpolate a 3d vector
cubic_interpolation(f0[0], f1[0], df0[0], df1[0], t, &(ft[0]), &(dft[0]), timeInterval_s);
cubic_interpolation(f0[1], f1[1], df0[1], df1[1], t, &(ft[1]), &(dft[1]), timeInterval_s);
cubic_interpolation(f0[2], f1[2], df0[2], df1[2], t, &(ft[2]), &(dft[2]), timeInterval_s);
}
//-----------------------------------------------------------------------------------
void vector_orthoNorm( double a[3], double b[3] ){
// take vector b, remove component parallel to a
// and then normalize length to 1
double s = vector_dot_product(a,b);
b[0] = b[0] - s*a[0];
b[1] = b[1] - s*a[1];
b[2] = b[2] - s*a[2];
vector_unit(b,b);
}
void vector_scale( double a[3], double b[3], double s ){
// b = a * s
b[0] = a[0]*s;
b[1] = a[1]*s;
b[2] = a[2]*s;
}
double vector_dot_product (double *a, double *b) {
return (a[0]*b[0] + a[1]*b[1] + a[2]*b[2]);
}
void vector_cross_product(double a[3], double b[3],double c[3]){
// c = a x b
c[0] = a[1]*b[2]-a[2]*b[1];
c[1] = a[2]*b[0]-a[0]*b[2];
c[2] = a[0]*b[1]-a[1]*b[0];
}
double vector_norm(double v[3]) {
return(sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
}
void vector_unit(double *x, double *x_unit) {
// takes x and produces a unit vector in the
// same direction (x and x_unit can be the same
// when calling this function)
double X = vector_norm(x);
x_unit[0] = x[0] / X;
x_unit[1] = x[1] / X;
x_unit[2] = x[2] / X;
}
void vector_add(double a[3], double b[3],double c[3]) {
c[0] = a[0] + b[0];
c[1] = a[1] + b[1];
c[2] = a[2] + b[2];
}
void vector_subtract(double a[3], double b[3],double c[3]) {
c[0] = a[0] - b[0];
c[1] = a[1] - b[1];
c[2] = a[2] - b[2];
}
void vector_constrainToPlane( double a[3], double b[3], double c[3] ){
double tempa[3], tempb[3];
vector_unit(a, tempa);
vector_scale(b, tempb, 1);
vector_orthoNorm(tempa, tempb);
double c1 = vector_dot_product(tempa, c);
double c2 = vector_dot_product(tempb, c);
c[0] = tempa[0] * c1 + tempb[0] * c2;
c[1] = tempa[1] * c1 + tempb[1] * c2;
c[2] = tempa[2] * c1 + tempb[2] * c2;
}
//-----------------------------------------------------------------------------------
void matrix_multiply(unsigned rowsA,unsigned columnsA,unsigned columnsB,double *A,double *B,double *C) {
unsigned i, j, k;
for(i = 0; i < rowsA; i++) {
for(j = 0; j < columnsB; j++) {
C[i*columnsB +j] = 0.0;
for(k = 0; k < columnsA; k++) {
C[i*columnsB +j] += A[i*columnsA+k]*B[k*columnsB+j];
}
}
}
}
void matrix_transpose(unsigned rows,unsigned columns,double *A,double *B) {
unsigned i, j;
for(i = 0; i < columns; i++) {
for(j = 0; j < rows; j++) {
B[i*rows+j] = A[j*columns+i];
}
}
}
void matrixVectorMult3x3( double M[9], double x[3], double y[3] ){
// y = M*x
y[0] = M[0] * x[0] + M[1] * x[1] + M[2] * x[2];
y[1] = M[3] * x[0] + M[4] * x[1] + M[5] * x[2];
y[2] = M[6] * x[0] + M[7] * x[1] + M[8] * x[2];
}
void matrix_form3x3( double row1[3], double row2[3], double row3[3], double *M ){
// form a matrix M by combining three row vectors
for(int i=0;i<3;i++){
M[i] = row1[i];
M[i+3] = row2[i];
M[i+6] = row3[i];
}
}
double matrix_det_3x3(double m[3][3]){
return m[0][0] * (m[1][1]*m[2][2] - m[1][2] * m[2][1])
- m[0][1] * (m[1][0]*m[2][2] - m[1][2] * m[2][0])
+ m[0][2] * (m[1][0]*m[2][1] - m[1][1] * m[2][0]);
}
void matrix_scaleAdjoint_3x3(double a[3][3],double s,double m[3][3]){
a[0][0] = (s) * (m[1][1] * m[2][2] - m[1][2] * m[2][1]);
a[1][0] = (s) * (m[1][2] * m[2][0] - m[1][0] * m[2][2]);
a[2][0] = (s) * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
a[0][1] = (s) * (m[0][2] * m[2][1] - m[0][1] * m[2][2]);
a[1][1] = (s) * (m[0][0] * m[2][2] - m[0][2] * m[2][0]);
a[2][1] = (s) * (m[0][1] * m[2][0] - m[0][0] * m[2][1]);
a[0][2] = (s) * (m[0][1] * m[1][2] - m[0][2] * m[1][1]);
a[1][2] = (s) * (m[0][2] * m[1][0] - m[0][0] * m[1][2]);
a[2][2] = (s) * (m[0][0] * m[1][1] - m[0][1] * m[1][0]);
}
void matrix_invert_3x3(double A[9], double invA[9]){
double invM[3][3], M[3][3];
for(int row=0; row<3; row++){
for(int col=0; col<3; col++){
M[row][col] = A[col + row*3];
}
}
matrix_scaleAdjoint_3x3(invM, (1.0 / matrix_det_3x3(M)), M);
for(int row=0; row<3; row++){
for(int col=0; col<3; col++){
invA[col + row*3] = invM[row][col];
}
}
}