NASA Logo
Ocean Color Science Software

ocssw V2022
ocorient.c
Go to the documentation of this file.
1 #include "libnav.h"
2 #include "nav.h"
3 #include "ocorient.h"
4 #include "math_utils.h"
5 void matmpy(const float xm1[3][3], const float xm2[3][3], float xm3[3][3]) {
6  int i, j, m, n;
7  for (i = 0; i < 3; i++) {
8  for (j = 0; j < 3; j++) {
9  xm3[i][j] = 0;
10  for (m = 0; m < 3; m++)
11  xm3[i][j] += xm1[i][m] * xm2[m][j];
12  }
13  }
14 }
15 
16 void matmpy_(const float xm1[3][3], const float xm2[3][3], float xm3[3][3]) {
17  float xsave3[3][3];
18  float xsave1[3][3];
19  float xsave2[3][3];
20  transpose3d(xm1, xsave1);
21  transpose3d(xm2, xsave2);
22  matmpy(xsave1, xsave2, xsave3);
23  transpose3d(xsave3, xm3);
24 }
25 void oceuler(float a[3], float xm[3][3]) {
26  float xm1[3][3], xm2[3][3], xm3[3][3], xmm[3][3];
27 
28  // Initialize all matrix elements to zero.
29  for (int i = 0; i < 3; i++) {
30  for (int j = 0; j < 3; j++) {
31  xm1[i][j] = xm2[i][j] = xm3[i][j] = 0.0f;
32  }
33  }
34  // c Compute sines and cosines
35  float c1 = cos(a[0] / radeg);
36  float s1 = sin(a[0] / radeg);
37  float c2 = cos(a[1] / radeg);
38  float s2 = sin(a[1] / radeg);
39  float c3 = cos(a[2] / radeg);
40  float s3 = sin(a[2] / radeg);
41  // c Convert individual rotations to matrices xm1(1, 1) = 1.d0;
42  xm1[1 - 1][1 - 1] = 1.0f;
43  xm1[2 - 1][2 - 1] = c1;
44  xm1[3 - 1][3 - 1] = c1;
45  xm1[2 - 1][3 - 1] = s1;
46  xm1[3 - 1][2 - 1] = -s1;
47  xm2[2 - 1][2 - 1] = 1.0f;
48  xm2[1 - 1][1 - 1] = c2;
49  xm2[3 - 1][3 - 1] = c2;
50  xm2[3 - 1][1 - 1] = s2;
51  xm2[1 - 1][3 - 1] = -s2;
52  xm3[3 - 1][3 - 1] = 1.0f;
53  xm3[2 - 1][2 - 1] = c3;
54  xm3[1 - 1][1 - 1] = c3;
55  xm3[1 - 1][2 - 1] = s3;
56  xm3[2 - 1][1 - 1] = -s3;
57  matmpy(xm2, xm3, xmm);
58  matmpy(xm1, xmm, xm);
59 }
60 
103 void ocorient_(float pos[3], float vel[3], float att[3], float rm[3][3], float coef[10]) {
104  float vc[3], xpri[3], ypri[3], zpri[3], yrp[3][3], rn[3][3];
112  vc[1 - 1] = vel[1 - 1] - omegae * pos[2 - 1];
113  vc[2 - 1] = vel[2 - 1] + omegae * pos[1 - 1];
114  vc[3 - 1] = vel[3 - 1];
121  double pm = sqrt((double)(pos[0] * pos[0] + pos[1] * pos[1] + pos[2] * pos[2]));
122  double omf2p = (omf2 * rem + pm - rem) / pm;
123  double pxy = pos[0] * pos[0] + pos[1] * pos[1];
124  double temp = sqrt((double)(pos[2] * pos[2] + omf2p * omf2p * pxy));
125  zpri[0] = -omf2p * pos[0] / temp;
126  zpri[1] = -omf2p * pos[1] / temp;
127  zpri[2] = -pos[2] / temp;
128  // c Compute Y axis along negative orbit normal
129  crossp_(vc, zpri, ypri);
130  double yprim = sqrt((double)(ypri[0] * ypri[0] + ypri[1] * ypri[1] + ypri[2] * ypri[2]));
131  // normalization to length 1
132  for (int i = 0; i < 3; i++)
133  ypri[i] /= -yprim;
134  // c Compute X axis to complete orthonormal triad
135  crossp_(ypri, zpri, xpri);
136  // c Store in matrix
137  for (int i = 0; i < 3; i++) {
138  rn[0][i] = xpri[i];
139  rn[1][i] = ypri[i];
140  rn[2][i] = zpri[i];
141  }
142  // c Convert attitude (Euler) angles to YRP matrix
143  oceuler(att, yrp);
144  // c Compute sensor orientation matrix
145  float rm_temp[3][3];
146  matmpy(yrp, rn, rm);
147  memcpy(rm_temp,rm, sizeof rm_temp);
148 // transpose3d(rm_temp,rm);
149  // c Compute coefficients of intersection ellipse in scan plane
150  double rd = 1.e0 / omf2;
151  *(coef + 1 - 1) = 1.e0 + (rd - 1.e0) * rm[1 - 1][3 - 1] * rm[1 - 1][3 - 1];
152  *(coef + 2 - 1) = 1.e0 + (rd - 1.e0) * rm[2 - 1][3 - 1] * rm[2 - 1][3 - 1];
153  *(coef + 3 - 1) = 1.e0 + (rd - 1.e0) * rm[3 - 1][3 - 1] * rm[3 - 1][3 - 1];
154  *(coef + 4 - 1) = (rd - 1.e0) * rm[1 - 1][3 - 1] * rm[2 - 1][3 - 1] * 2.e0;
155  *(coef + 5 - 1) = (rd - 1.e0) * rm[1 - 1][3 - 1] * rm[3 - 1][3 - 1] * 2.e0;
156  *(coef + 6 - 1) = (rd - 1.e0) * rm[2 - 1][3 - 1] * rm[3 - 1][3 - 1] * 2.e0;
157  *(coef + 7 - 1) =
158  (rm[1 - 1][1 - 1] * pos[1 - 1] + rm[1 - 1][2 - 1] * pos[2 - 1] + rm[1 - 1][3 - 1] * pos[3 - 1] * rd) *
159  2.e0;
160  *(coef + 8 - 1) =
161  (rm[2 - 1][1 - 1] * pos[1 - 1] + rm[2 - 1][2 - 1] * pos[2 - 1] + rm[2 - 1][3 - 1] * pos[3 - 1] * rd) *
162  2.e0;
163  *(coef + 9 - 1) =
164  (rm[3 - 1][1 - 1] * pos[1 - 1] + rm[3 - 1][2 - 1] * pos[2 - 1] + rm[3 - 1][3 - 1] * pos[3 - 1] * rd) *
165  2.e0;
166  *(coef + 10 - 1) = pos[1 - 1] * pos[1 - 1] + pos[2 - 1] * pos[2 - 1] + pos[3 - 1] * pos[3 - 1] * rd -
167  re_const * re_const;
168  transpose3d(rm_temp,rm);
169 }
int crossp_(float *v1, float *v2, float *v3)
computes cross product v3 = v1 x v2
Definition: crossp.c:13
int j
Definition: decode_rs.h:73
void ocorient_(float pos[3], float vel[3], float att[3], float rm[3][3], float coef[10])
c This subroutine performs a simple calculation of the sensor c orientation from the orbit position v...
Definition: ocorient.c:103
void transpose3d(const float inp[3][3], float out[3][3])
Matrix transpose.
Definition: math_utils.c:3
float32 * pos
Definition: l1_czcs_hdf.c:35
void matmpy(const float xm1[3][3], const float xm2[3][3], float xm3[3][3])
Definition: ocorient.c:5
float rd(float x, float y, float z)
void oceuler(float a[3], float xm[3][3])
c Computes coordinate transformation matrix corresponding to Euler c sequence. The order of angles in...
Definition: ocorient.c:25
float pm[MODELMAX]
void matmpy_(const float xm1[3][3], const float xm2[3][3], float xm3[3][3])
Definition: ocorient.c:16
#define omf2
Definition: l1_czcs.c:697
int i
Definition: decode_rs.h:71
PGE01 indicating that PGE02 PGE01 V6 for and PGE01 V2 for MOD03 were used to produce the granule By convention adopted in all MODIS Terra PGE02 code versions are The fourth digit of the PGE02 version denotes the LUT version used to produce the granule The source of the metadata environment variable ProcessingCenter was changed from a QA LUT value to the Process Configuration A sign used in error in the second order term was changed to a
Definition: HISTORY.txt:424