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

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

Parent Directory Parent Directory | Revision Log Revision Log


Revision 1.5 - (hide 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 mocchiut 1.1 #include <iostream>
2     #include <stdio.h>
3     #include <TObject.h>
4     #include <TString.h>
5     #include <TMatrixD.h>
6 pam-mep 1.4 #include <TVector3.h>
7 mocchiut 1.1
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 malakhov 1.5 UInt_t t1=t-t%86400;
40     UInt_t t2=t1+86400;
41 pam-mep 1.2 Double_t omg = (7.292115e-5)*a; // Earth rotation velosity (Around polar axis);
42 malakhov 1.5 Double_t d = (t1-10957*86400-43200); //Number of day, passing from 01/01/2000 12:00:00 to t;
43 mocchiut 1.1 d = d/86400;
44     Double_t T = d/36525; //Number of Julian centuries;
45 malakhov 1.5 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 pam-mep 1.2
49 malakhov 1.5 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 pam-mep 1.2
61 mocchiut 1.1 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 pam-mep 1.2
76 mocchiut 1.1 TMatrixD Gij(3,3);
77     TMatrixD Fij(3,3);
78    
79 pam-mep 1.2 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 mocchiut 1.1 Gij(0,1) = -sin(lon);
83 pam-mep 1.2 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 mocchiut 1.1 Gij(1,2) = 0;
87     Gij(2,0) = 0;
88     Gij(2,1) = 0;
89     Gij(2,2) = 1;
90    
91 pam-mep 1.2 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 mocchiut 1.1 Fij(1,2) = 0;
97     Fij(2,0) = sin(lat);
98     Fij(2,1) = 0;
99     Fij(2,2) = cos(lat);
100 pam-mep 1.2
101 mocchiut 1.1 return Fij*(Gij*Aij);
102     }
103    
104 pam-mep 1.4 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 malakhov 1.5
145 pam-mep 1.4 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 malakhov 1.5 UInt_t t1=t-t%86400;
170     UInt_t t2=t1+86400;
171 pam-mep 1.4 Double_t omg = (7.292115e-5)*a; // Earth rotation velosity (Around polar axis);
172 malakhov 1.5 Double_t d = (t1-10957*86400-43200); //Number of day, passing from 01/01/2000 12:00:00 to t;
173 pam-mep 1.4 d = d/86400;
174     Double_t T = d/36525; //Number of Julian centuries;
175 malakhov 1.5 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 pam-mep 1.4
179 malakhov 1.5 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 pam-mep 1.4
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 malakhov 1.5 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 pam-mep 1.4
220 malakhov 1.5 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 pam-mep 1.4
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 pam-mep 1.2 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 mocchiut 1.1 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 pam-mep 1.4 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 pam-mep 1.2 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 mocchiut 1.3 Float_t mp = 938.272029;// Float_t amu = 931.494043e0;
352 pam-mep 1.2 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 pam-mep 1.4 larmor = 1e-3*Ek*cc/omega; //Ek here is p or for onecharged particle R; larmor in m
360 pam-mep 1.2 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 mocchiut 1.1 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