/[PAMELA software]/DarthVader/OrbitalInfo/src/OrientationInfo.cpp
ViewVC logotype

Contents of /DarthVader/OrbitalInfo/src/OrientationInfo.cpp

Parent Directory Parent Directory | Revision Log Revision Log


Revision 1.5 - (show annotations) (download)
Wed Sep 10 06:34:12 2014 UTC (10 years, 3 months ago) by malakhov
Branch: MAIN
CVS Tags: v10REDr01, v10RED, HEAD
Changes since 1.4: +55 -32 lines
shifting from start to end of day Dij matrix of orientation bug is fixed

1 #include <iostream>
2 #include <stdio.h>
3 #include <TObject.h>
4 #include <TString.h>
5 #include <TMatrixD.h>
6 #include <TVector3.h>
7
8 #include <OrientationInfo.h>
9
10 ClassImp(OrientationInfo)
11
12
13 using namespace std;
14
15 OrientationInfo::OrientationInfo() : TObject(){
16 a = 360/(2*TMath::Pi());
17 Re = 6000000;
18 }
19
20 OrientationInfo::~OrientationInfo(){
21 }
22
23 TMatrixD OrientationInfo::QuatoECI(Float_t q0, Float_t q1, Float_t q2, Float_t q3){
24 TMatrixD Pij(3,3);
25 Pij(0,0) = pow(q0,2)+pow(q1,2)-pow(q2,2)-pow(q3,2);
26 Pij(0,1) = /*2*(q1*q2+q0*q3);/*/ 2*(q1*q2-q0*q3);
27 Pij(0,2) = /*2*(q1*q3-q0*q2);/*/ 2*(q1*q3+q0*q2);
28 Pij(1,0) = /*2*(q1*q2-q0*q3);/*/ 2*(q1*q2+q0*q3);
29 Pij(1,1) = pow(q0,2)-pow(q1,2)+pow(q2,2)-pow(q3,2);
30 Pij(1,2) = /*2*(q2*q3+q0*q1);/*/ 2*(q2*q3-q0*q1);
31 Pij(2,0) = /*2*(q1*q3+q0*q2);/*/ 2*(q1*q3-q0*q2);
32 Pij(2,1) = /*2*(q2*q3-q0*q1);/*/ 2*(q2*q3+q0*q1);
33 Pij(2,2) = pow(q0,2)-pow(q1,2)-pow(q2,2)+pow(q3,2);
34 return Pij;
35 }
36
37 TMatrixD OrientationInfo::ECItoGreenwich(TMatrixD Aij, UInt_t t){
38 TMatrixD Gij(3,3);
39 UInt_t t1=t-t%86400;
40 UInt_t t2=t1+86400;
41 Double_t omg = (7.292115e-5)*a; // Earth rotation velosity (Around polar axis);
42 Double_t d = (t1-10957*86400-43200); //Number of day, passing from 01/01/2000 12:00:00 to t;
43 d = d/86400;
44 Double_t T = d/36525; //Number of Julian centuries;
45 Double_t Se = 6*3600+41*60+236.555367908*d+0.093104*T*T-(6.2e-6)*T*T*T; //18 <-> 6
46 Double_t tr = (t1-10957*86400)%86400;
47 Double_t Somg1 = (Se+49.077+omg*86400*tr/360.)*360/86400.;
48
49 d = (t2-10957*86400-43200); //Number of day, passing from 01/01/2000 12:00:00 to t;
50 d = d/86400;
51 T = d/36525; //Number of Julian centuries;
52 Se = 6*3600+41*60+236.555367908*d+0.093104*T*T-(6.2e-6)*T*T*T; //18 <-> 6
53 tr = (t2-10957*86400)%86400;
54 Double_t Somg2 = (Se+49.077+omg*86400*tr/360.)*360/86400.;
55 Somg2+=360.0;
56
57 Double_t kk=(Somg2-Somg1)/(t2-t1);
58 Double_t bb= Somg1-kk*t1;
59 Double_t Somg=kk*t+bb;
60
61 Gij(0,0) = cos(Somg/a);
62 Gij(0,1) = -sin(Somg/a);
63 Gij(0,2) = 0;
64 Gij(1,0) = sin(Somg/a);
65 Gij(1,1) = cos(Somg/a);
66 Gij(1,2) = 0;
67 Gij(2,0) = 0;
68 Gij(2,1) = 0;
69 Gij(2,2) = 1;
70 Gij.Invert();
71 return Gij*Aij;
72 }
73
74 TMatrixD OrientationInfo::GreenwichtoGEO(Double_t lat, Double_t lon, TMatrixD Aij){
75
76 TMatrixD Gij(3,3);
77 TMatrixD Fij(3,3);
78
79 lon=(-lon)/a; lat=(-lat)/a; // here has the same result as Gij.Invert() in ECItoGreenwich function
80
81 Gij(0,0) = cos(lon); // rotation around z-axis:
82 Gij(0,1) = -sin(lon);
83 Gij(0,2) = 0; // | cos(lon) -sin(lon) 0|
84 Gij(1,0) = sin(lon); // | sin(lon) cos(lon) 0|
85 Gij(1,1) = cos(lon); // | 0 0 1|
86 Gij(1,2) = 0;
87 Gij(2,0) = 0;
88 Gij(2,1) = 0;
89 Gij(2,2) = 1;
90
91 Fij(0,0) = cos(lat); // rotation around y-axis at angle -lat, cause rotation around y from x to z axis is negative
92 Fij(0,1) = 0; //
93 Fij(0,2) = -sin(lat); // |cos(-lat) 0 sin(-lat)| |cos(lat) 0 -sin(lat)|
94 Fij(1,0) = 0; // | 0 1 0 | ==> | 0 1 0 |
95 Fij(1,1) = 1; // |-sin(-lat) 0 cos(-lat)| |sin(lat) 0 cos(lat) |
96 Fij(1,2) = 0;
97 Fij(2,0) = sin(lat);
98 Fij(2,1) = 0;
99 Fij(2,2) = cos(lat);
100
101 return Fij*(Gij*Aij);
102 }
103
104 TMatrixD OrientationInfo::EulertoEci(Double_t x0, Double_t y0, Double_t z0, Double_t Vx0, Double_t Vy0, Double_t Vz0, Double_t Bank, Double_t Yaw, Double_t SPitch){
105 //cerr.precision(12);
106 //cerr<<"Position:\t"<<x0<<"\t"<<y0<<"\t"<<z0<<"\tVelocity:\t"<<Vx0<<"\t"<<Vy0<<"\t"<<Vz0<<endl;
107 //Sangur to Resurs transition
108 TMatrixD Zij(3,3);
109 Zij(0,0) = 0.0; Zij(0,1) = 0.0; Zij(0,2) = -1.0;
110 Zij(1,0) = -1.0; Zij(1,1) = 0.0; Zij(1,2) = 0.0;
111 Zij(2,0) = 0.0; Zij(2,1) = 1.0; Zij(2,2) = 0.0;
112
113 //Spacecraft velosity referenca frame in Eci
114 TMatrixD Aij(3,3);
115 Double_t C1 = y0*Vz0 - z0*Vy0;
116 Double_t C2 = z0*Vx0 - x0*Vz0;
117 Double_t C3 = x0*Vy0 - y0*Vx0;
118 Double_t C = sqrt(C1*C1 + C2*C2 + C3*C3);
119 Double_t V0 = sqrt(Vx0*Vx0+Vy0*Vy0 + Vz0*Vz0);
120 Aij(0,0) = Vx0/V0; Aij(0,1) = C1/C; Aij(0,2) = (Vy0*C3-Vz0*C2)/(V0*C);
121 Aij(1,0) = Vy0/V0; Aij(1,1) = C2/C; Aij(1,2) = (Vz0*C1-Vx0*C3)/(V0*C);
122 Aij(2,0) = Vz0/V0; Aij(2,1) = C3/C; Aij(2,2) = (Vx0*C2-Vy0*C1)/(V0*C);
123
124 //Elements of matrix elements described orientation of spacecraft on velocity reference frame
125 Double_t u10 = tan(Bank*TMath::DegToRad())/sqrt(tan(Bank*TMath::DegToRad())*tan(Bank*TMath::DegToRad())+1);
126 Double_t u11 = -sqrt((1-u10*u10))/(1+tan(Yaw*TMath::DegToRad())*tan(Yaw*TMath::DegToRad()));
127 Double_t u12 = u11*tan(Yaw*TMath::DegToRad());
128 Double_t u20 = -sqrt((1-u10*u10)/(1+tan(SPitch*TMath::DegToRad())*tan(SPitch*TMath::DegToRad())));
129 Double_t u00 = -u20*tan(SPitch*TMath::DegToRad());
130
131 Double_t ab = 1+(u20*u20/(u00*u00));
132 Double_t by = 2*u10*u11*u20/(u00*u00);
133 Double_t cy = (1+u10*u10/(u00*u00))*u11*u11-1;
134 Double_t bz = 2*u10*u12*u20/(u00*u00);
135 Double_t cz = (1+u10*u10/(u00*u00))*u12*u12-1;
136
137 Int_t uj = TMath::Sign(1.,Yaw)*TMath::Sign(1.,SPitch);
138 //long double by_l = by;
139 Double_t Ds = by*by-4*ab*cy;
140 if(Ds<0) Ds = 0.;
141 Double_t u21 = (-by+uj*sqrt(Ds))/(2*ab);
142 Double_t u21s = -TMath::Sign(1.,Bank)*TMath::Abs(u21);
143 Double_t u01 = TMath::Sign(1.,Yaw)*TMath::Abs((u10*u11+u20*u21)/u00);
144
145 Int_t fj=1;
146 if(TMath::Sign(1.,SPitch)>0 && TMath::Sign(1.,Yaw)>0) fj=-1;
147
148 Double_t u22 = (-bz+fj*sqrt(bz*bz-4*ab*cz))/(2*ab);
149 Double_t u22s = -TMath::Sign(1.,SPitch)*TMath::Abs(u22);
150 Double_t u02 = -TMath::Abs((u10*u12+u20*u22)/u00);
151
152 TMatrixD Dij(3,3);
153 Dij(0,0) = u00; Dij(0,1) = u01; Dij(0,2) = u02;
154 Dij(1,0) = u10; Dij(1,1) = u11; Dij(1,2) = u12;
155 Dij(2,0) = u20; Dij(2,1) = u21s; Dij(2,2) = u22s;
156
157 TMatrixD Shij(3,3);
158 TMatrixD Usij(3,3);
159 Usij = (Aij*Dij);
160 Usij.Invert();
161 Shij = Zij*Usij;
162 Shij.Invert();
163
164 return Shij;
165 }
166
167 TMatrixD OrientationInfo::ECItoGEO(TMatrixD Aij, UInt_t t, Double_t lat, Double_t lon){
168 TMatrixD Gij(3,3);
169 UInt_t t1=t-t%86400;
170 UInt_t t2=t1+86400;
171 Double_t omg = (7.292115e-5)*a; // Earth rotation velosity (Around polar axis);
172 Double_t d = (t1-10957*86400-43200); //Number of day, passing from 01/01/2000 12:00:00 to t;
173 d = d/86400;
174 Double_t T = d/36525; //Number of Julian centuries;
175 Double_t Se = 6*3600+41*60+236.555367908*d+0.093104*T*T-(6.2e-6)*T*T*T; //18 <-> 6
176 Double_t tr = (t1-10957*86400)%86400;
177 Double_t Somg1 = (Se+49.077+omg*86400*tr/360.)*360/86400.;
178
179 d = (t2-10957*86400-43200); //Number of day, passing from 01/01/2000 12:00:00 to t;
180 d = d/86400;
181 T = d/36525; //Number of Julian centuries;
182 Se = 6*3600+41*60+236.555367908*d+0.093104*T*T-(6.2e-6)*T*T*T; //18 <-> 6
183 tr = (t2-10957*86400)%86400;
184 Double_t Somg2 = (Se+49.077+omg*86400*tr/360.)*360/86400.;
185 Somg2+=360.0;
186
187 Double_t kk=(Somg2-Somg1)/(t2-t1);
188 Double_t bb= Somg1-kk*t1;
189 Double_t Somg=kk*t+bb;
190
191 lon=(-lon)/a; lat=(-lat)/a;
192
193 Gij(0,0)=cos(lat)*cos(lon)*cos(Somg/a)+cos(lat)*sin(lon)*sin(Somg/a);
194 Gij(0,1)=cos(lat)*cos(lon)*sin(Somg/a)-cos(lat)*sin(lon)*cos(Somg/a);
195 Gij(0,2)=-sin(lat);
196 Gij(1,0)=sin(lon)*cos(Somg/a)-cos(lon)*sin(Somg/a);
197 Gij(1,1)=sin(lon)*sin(Somg/a)+cos(lon)*cos(Somg/a);
198 Gij(1,2)=0;
199 Gij(2,0)=sin(lat)*cos(lon)*cos(Somg/a)+sin(lat)*sin(lon)*sin(Somg/a);
200 Gij(2,1)=sin(lat)*cos(lon)*sin(Somg/a)-sin(lat)*sin(lon)*cos(Somg/a);
201 Gij(2,2)=cos(lat);
202
203 TMatrixD Tij=Gij*Aij;
204
205 return Tij;
206 }
207
208 TMatrixD OrientationInfo::GEOtoECI(TMatrixD Aij, UInt_t t, Double_t lat, Double_t lon){
209 TMatrixD Gij(3,3);
210 UInt_t t1=t-t%86400;
211 UInt_t t2=t1+86400;
212 Double_t omg = (7.292115e-5)*a; // Earth rotation velosity (Around polar axis);
213 Double_t d = (t1-10957*86400-43200); //Number of day, passing from 01/01/2000 12:00:00 to t;
214 d = d/86400;
215 Double_t T = d/36525; //Number of Julian centuries;
216 Double_t Se = 6*3600+41*60+236.555367908*d+0.093104*T*T-(6.2e-6)*T*T*T; //18 <-> 6
217 Double_t tr = (t1-10957*86400)%86400;
218 Double_t Somg1 = (Se+49.077+omg*86400*tr/360.)*360/86400.;
219
220 d = (t2-10957*86400-43200); //Number of day, passing from 01/01/2000 12:00:00 to t;
221 d = d/86400;
222 T = d/36525; //Number of Julian centuries;
223 Se = 6*3600+41*60+236.555367908*d+0.093104*T*T-(6.2e-6)*T*T*T; //18 <-> 6
224 tr = (t2-10957*86400)%86400;
225 Double_t Somg2 = (Se+49.077+omg*86400*tr/360.)*360/86400.;
226 Somg2+=360.0;
227
228 Double_t kk=(Somg2-Somg1)/(t2-t1);
229 Double_t bb= Somg1-kk*t1;
230 Double_t Somg=kk*t+bb;
231
232 lon=(-lon)/a; lat=(-lat)/a;
233
234 Gij(0,0)=cos(lat)*cos(lon)*cos(Somg/a)+cos(lat)*sin(lon)*sin(Somg/a);
235 Gij(1,0)=cos(lat)*cos(lon)*sin(Somg/a)-cos(lat)*sin(lon)*cos(Somg/a);
236 Gij(2,0)=-sin(lat);
237 Gij(0,1)=sin(lon)*cos(Somg/a)-cos(lon)*sin(Somg/a);
238 Gij(1,1)=sin(lon)*sin(Somg/a)+cos(lon)*cos(Somg/a);
239 Gij(2,1)=0;
240 Gij(0,2)=sin(lat)*cos(lon)*cos(Somg/a)+sin(lat)*sin(lon)*sin(Somg/a);
241 Gij(1,2)=sin(lat)*cos(lon)*sin(Somg/a)-sin(lat)*sin(lon)*cos(Somg/a);
242 Gij(2,2)=cos(lat);
243
244 return Gij*Aij;
245 }
246
247
248 TMatrixD OrientationInfo::GEOtoGeomag(TMatrixD Aij,Double_t Bnorth, Double_t Beast, Double_t Bup){ //Geomagnetic geodetic reference frame
249 Double_t alpha = 0;
250 if(Beast==0. && Bnorth>0) alpha = 0; else
251 if(Beast==0. && Bnorth<0) alpha = 180.; else{
252 if(Beast > 0) alpha = TMath::ATan(Bnorth/Beast)*TMath::RadToDeg() - 90.;
253 if(Beast < 0) alpha = TMath::ATan(Bnorth/Beast)*TMath::RadToDeg() + 90.;
254 }
255 alpha = alpha*TMath::DegToRad();
256 Double_t beta = TMath::ATan(Bup/sqrt(pow(Bnorth,2)+pow(Beast,2)));
257 //if(Bup<0.0) beta = TMath::ATan(TMath::Abs(Bup/sqrt(pow(Bnorth,2)+pow(Beast,2))));
258 //if(Bup>0.0) beta = TMath::ATan(TMath::Abs(sqrt(pow(Bnorth,2)+pow(Beast,2))/Bup));
259 //cout<<"GEOtomag:alpha = "<<alpha*TMath::RadToDeg()<<"\tbeta = "<<beta*TMath::RadToDeg()<<endl;
260 TMatrixD Gij(3,3);
261 TMatrixD Fij(3,3);
262 Gij(0,0) = 1; //rotation around x-axis at angle alpha
263 Gij(0,1) = 0;
264 Gij(0,2) = 0; // |1 0 0 |
265 Gij(1,0) = 0; // |0 cos(alpha) -sin(alpha) |
266 Gij(1,1) = cos(alpha); // |0 sin(alpha) cos(alpha) |
267 Gij(1,2) = -sin(alpha);
268 Gij(2,0) = 0;
269 Gij(2,1) = sin(alpha);
270 Gij(2,2) = cos(alpha);
271 Gij.Invert();
272 Fij(0,0) = cos(beta); //rotation around y-axis at angle beta
273 Fij(0,1) = 0;
274 Fij(0,2) = sin(beta); // |cos(beta) 0 sin(beta)|
275 Fij(1,0) = 0; // | 0 1 0 |
276 Fij(1,1) = 1; // |-sin(beta) 0 cos(beta)|
277 Fij(1,2) = 0;
278 Fij(2,0) = -sin(beta);
279 Fij(2,1) = 0;
280 Fij(2,2) = cos(beta);
281 Fij.Invert();
282 //Int_t tri;
283 //cin >> tri;
284 return Fij*(Gij*Aij);
285 }
286
287 TMatrixD OrientationInfo::PamelatoGEO(TMatrixD Aij, Double_t B1, Double_t B2, Double_t B3){
288 //TMatrixD Gij(3,3);
289 TMatrixD Hij(3,1);
290 TMatrixD Bij(3,1);
291 Bij(0,0) = B1;
292 Bij(1,0) = B2;
293 Bij(2,0) = B3;
294 Hij=Aij*Bij;
295 return Hij;
296 }
297
298 TMatrixD OrientationInfo::ColPermutation(TMatrixD Aij){
299 TMatrixD Gij(3,3);
300 Gij(0,0) = 1; Gij(0,1) = 0; Gij(0,2) = 0;
301 Gij(1,0) = 0; Gij(1,1) = 0; Gij(1,2) = 1;
302 Gij(2,0) = 0; Gij(2,1) = -1; Gij(2,2) = 0;
303 return Aij*Gij;
304 }
305
306 TVector3 OrientationInfo::GetSunPosition(UInt_t atime){
307 TVector3 sunout;
308 Float_t JD=atime/86400.+2440587.5;
309 //SAV
310 // cout << "JD = " << JD <<endl;
311 //SAV
312 //test June 1997 JD=2451545.0-877.047;
313 Float_t Tm = (JD - 2451545.0)/36525.;
314 Float_t Mo = (357.52910+35999.05030*Tm-0.0001559*Tm*Tm-0.00000048*Tm*Tm*Tm);
315 //SAV
316 // cout<<"Tm = " << Tm << "Mo = " << Mo <<endl;
317 //SAV
318 Mo=Mo*TMath::DegToRad();
319
320 Float_t Co = ((1.914600 - 0.004817*Tm - 0.00014*Tm*Tm)*sin(Mo) + (0.019993 - 0.000101*Tm)* sin(2.*Mo) + 0.000290* sin(3.*Mo));
321 Co=Co* TMath::DegToRad();
322
323 Float_t Lo = (280.46645 + 36000.76983*Tm +0.0003032*Tm*Tm);
324 Lo=Lo*TMath::DegToRad();
325
326 Float_t theta = (Lo + Co); // * TMath::DegToRad();
327
328 Float_t eps = (23.+26./60.+21.448/3600. - 46.8150/3600.*Tm - 0.00059/3600.*Tm*Tm + 0.001813*Tm*Tm*Tm)*TMath::DegToRad();
329
330 //SAV
331 // cout << "Co = " << Co*TMath::RadToDeg() << "\tLo = " << Lo*TMath::RadToDeg() << "\ttheta = " << theta << "\teps = " << eps << endl;
332 //SAV
333
334 Float_t YY=cos(eps)*sin(theta);
335 Float_t XX=cos(theta);
336 //SAV
337 // cout << "XX = " << XX << "\tYY" << YY << endl;
338 //SAV
339 Float_t RASun=atan(YY/XX);
340 if(XX<0. ) RASun=RASun+TMath::Pi();
341 if(XX >0. && YY <0.) RASun=RASun+2*TMath::Pi();
342 Float_t DESun = asin(sin(eps)*sin(theta));
343 //SAV
344 // cout << "DE = " << DESun << "\t" << RASun << endl;
345 //SAV
346 sunout.SetMagThetaPhi(1.0,TMath::Pi()/2.-DESun,RASun);
347 return sunout;
348 }
349
350 Float_t OrientationInfo::Larmor(Float_t Ek,Float_t Bm,Int_t iZ,Float_t xA){ //Ek in MeV, Bm in nT, Pitch-angle, rad
351 Float_t mp = 938.272029;// Float_t amu = 931.494043e0;
352 Float_t cc = 299792458.;
353 Float_t ee = 1.60217653e-19;
354 Float_t kg = 1.7826619e-30;
355 Float_t gam = (Ek+mp)/mp;
356 Float_t mm = mp*kg;
357 Float_t omega = iZ*ee*Bm*1e-9/(gam*mm);
358 Float_t larmor = 1e-3*sqrt(1e0-1e0/pow(gam,2))*cc/omega;
359 larmor = 1e-3*Ek*cc/omega; //Ek here is p or for onecharged particle R; larmor in m
360 return larmor;
361 }
362
363 TMatrixD OrientationInfo::GetDirectiontoGirocenter(Float_t R, Float_t Px, Float_t Py){
364 TMatrixD GirDir(3,1);
365 if(R>0){
366 GirDir(0,0) = Py;
367 GirDir(1,0) = -Px;
368 }else{
369 GirDir(0,0) = -Py;
370 GirDir(1,0) = Px;
371 }
372 GirDir(2,0) = 0.;
373 return GirDir;
374 }
375
376 Double_t OrientationInfo::GetPitchAngle(Double_t x1, Double_t y1, Double_t z1, Double_t x2, Double_t y2, Double_t z2){
377 return TMath::ACos((x1*x2 + y1*y2 + z1*z2)/(sqrt(pow(x1,2)+pow(y1,2)+pow(z1,2))*sqrt(pow(x2,2)+pow(y2,2)+pow(z2,2)))) * TMath::RadToDeg();
378 }

  ViewVC Help
Powered by ViewVC 1.1.23