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

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

Parent Directory Parent Directory | Revision Log Revision Log | View Patch Patch

revision 1.3 by mocchiut, Tue Apr 10 14:28:19 2012 UTC revision 1.4 by pam-mep, Fri Mar 28 20:47:15 2014 UTC
# Line 3  Line 3 
3  #include <TObject.h>  #include <TObject.h>
4  #include <TString.h>  #include <TString.h>
5  #include <TMatrixD.h>  #include <TMatrixD.h>
6    #include <TVector3.h>
7    
8  #include <OrientationInfo.h>  #include <OrientationInfo.h>
9    
# Line 91  TMatrixD OrientationInfo::GreenwichtoGEO Line 92  TMatrixD OrientationInfo::GreenwichtoGEO
92      return Fij*(Gij*Aij);      return Fij*(Gij*Aij);
93  }  }
94    
95    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){
96    //cerr.precision(12);
97    //cerr<<"Position:\t"<<x0<<"\t"<<y0<<"\t"<<z0<<"\tVelocity:\t"<<Vx0<<"\t"<<Vy0<<"\t"<<Vz0<<endl;
98        //Sangur to Resurs transition
99        TMatrixD Zij(3,3);
100        Zij(0,0) = 0.0; Zij(0,1) = 0.0; Zij(0,2) = -1.0;
101        Zij(1,0) = -1.0; Zij(1,1) = 0.0; Zij(1,2) = 0.0;
102        Zij(2,0) = 0.0; Zij(2,1) = 1.0; Zij(2,2) = 0.0;
103    
104        //Spacecraft velosity referenca frame in Eci
105        TMatrixD Aij(3,3);
106        Double_t C1 = y0*Vz0 - z0*Vy0;
107        Double_t C2 = z0*Vx0 - x0*Vz0;
108        Double_t C3 = x0*Vy0 - y0*Vx0;
109        Double_t C  = sqrt(C1*C1 + C2*C2 + C3*C3);
110        Double_t V0 = sqrt(Vx0*Vx0+Vy0*Vy0 + Vz0*Vz0);
111        Aij(0,0) = Vx0/V0;  Aij(0,1) = C1/C;        Aij(0,2) = (Vy0*C3-Vz0*C2)/(V0*C);
112        Aij(1,0) = Vy0/V0;  Aij(1,1) = C2/C;        Aij(1,2) = (Vz0*C1-Vx0*C3)/(V0*C);
113        Aij(2,0) = Vz0/V0;  Aij(2,1) = C3/C;        Aij(2,2) = (Vx0*C2-Vy0*C1)/(V0*C);
114    
115        //Elements of matrix elements described orientation of spacecraft on velocity reference frame
116         Double_t u10 = tan(Bank*TMath::DegToRad())/sqrt(tan(Bank*TMath::DegToRad())*tan(Bank*TMath::DegToRad())+1);
117         Double_t u11 = -sqrt((1-u10*u10))/(1+tan(Yaw*TMath::DegToRad())*tan(Yaw*TMath::DegToRad()));
118         Double_t u12 = u11*tan(Yaw*TMath::DegToRad());
119         Double_t u20 = -sqrt((1-u10*u10)/(1+tan(SPitch*TMath::DegToRad())*tan(SPitch*TMath::DegToRad())));
120         Double_t u00 = -u20*tan(SPitch*TMath::DegToRad());
121    
122         Double_t ab = 1+(u20*u20/(u00*u00));
123         Double_t by = 2*u10*u11*u20/(u00*u00);
124         Double_t cy = (1+u10*u10/(u00*u00))*u11*u11-1;
125         Double_t bz = 2*u10*u12*u20/(u00*u00);
126         Double_t cz = (1+u10*u10/(u00*u00))*u12*u12-1;
127    
128        Int_t uj = TMath::Sign(1.,Yaw)*TMath::Sign(1.,SPitch);
129         //long double by_l = by;
130         Double_t Ds = by*by-4*ab*cy;
131         if(Ds<0) Ds = 0.;
132         Double_t u21 = (-by+uj*sqrt(Ds))/(2*ab);
133         Double_t u21s = -TMath::Sign(1.,Bank)*TMath::Abs(u21);
134         Double_t u01 = TMath::Sign(1.,Yaw)*TMath::Abs((u10*u11+u20*u21)/u00);
135    //    cerr<<"by = " << by<<"\tuj"<<uj<<"\tab: "<<ab<<"\t"<<by*by-4*ab*cy<<endl;
136    //    cerr<<"u21: "<<u21<<"\tu01: "<<u01<<"\t"<<TMath::Abs((u10*u11+u20*u21)/u00)<<"\t"<<TMath::Sign(1.,Yaw)<<"\t"<<(u10*u11+u20*u21)<<endl;
137        Int_t fj=1;
138        if(TMath::Sign(1.,SPitch)>0 && TMath::Sign(1.,Yaw)>0) fj=-1;
139    //    cout<<"bla-bla-bla"<<endl;
140    
141         Double_t u22 = (-bz+fj*sqrt(bz*bz-4*ab*cz))/(2*ab);
142         Double_t u22s = -TMath::Sign(1.,SPitch)*TMath::Abs(u22);
143         Double_t u02 = -TMath::Abs((u10*u12+u20*u22)/u00);
144    
145    //    cout<<fj<<"\t"<<ab<<"\t"<<by<<"\t"<<cy<<"\t"<<bz<<"\t"<<cz<<endl;
146    //    cout<<"INSIDE EULERTOECI"<<endl;
147    //    cout<<u00<<"\t"<<u01<<"\t"<<u02<<endl;
148    //    cout<<u10<<"\t"<<u11<<"\t"<<u12<<endl;
149    //    cout<<u20<<"\t"<<u21s<<"\t"<<u22s<<endl;
150    
151        TMatrixD Dij(3,3);
152        Dij(0,0) = u00;  Dij(0,1) = u01;  Dij(0,2) = u02;
153        Dij(1,0) = u10;  Dij(1,1) = u11;  Dij(1,2) = u12;
154        Dij(2,0) = u20;  Dij(2,1) = u21s;  Dij(2,2) = u22s;
155    
156        TMatrixD Shij(3,3);
157        TMatrixD Usij(3,3);
158        Usij = (Aij*Dij);
159        Usij.Invert();
160        Shij = Zij*Usij;
161        Shij.Invert();
162    
163        return Shij;
164    }
165    
166    TMatrixD OrientationInfo::ECItoGEO(TMatrixD Aij, UInt_t t, Double_t lat, Double_t lon){
167        TMatrixD Gij(3,3);
168        Double_t omg = (7.292115e-5)*a;                     // Earth rotation velosity (Around polar axis);
169        Double_t d = (t-10957*86400-43200);                      //Number of day, passing from 01/01/2000 12:00:00 to t;
170        d = d/86400;
171        Double_t T = d/36525;                                    //Number of Julian centuries;
172        
173        Double_t Se = 6*3600+41*60+236.555367908*d+0.093104*pow(T,2)-(6.2e-6)*pow(T,3);
174    
175        Int_t tr = (t-10957*86400)%86400;
176    
177        Double_t Somg = (Se+49.077+omg*86400*tr/360)*360/86400;
178    
179        lon=(-lon)/a; lat=(-lat)/a;
180    
181        Gij(0,0)=cos(lat)*cos(lon)*cos(Somg/a)+cos(lat)*sin(lon)*sin(Somg/a);
182        Gij(0,1)=cos(lat)*cos(lon)*sin(Somg/a)-cos(lat)*sin(lon)*cos(Somg/a);
183        Gij(0,2)=-sin(lat);
184        Gij(1,0)=sin(lon)*cos(Somg/a)-cos(lon)*sin(Somg/a);
185        Gij(1,1)=sin(lon)*sin(Somg/a)+cos(lon)*cos(Somg/a);
186        Gij(1,2)=0;
187        Gij(2,0)=sin(lat)*cos(lon)*cos(Somg/a)+sin(lat)*sin(lon)*sin(Somg/a);
188        Gij(2,1)=sin(lat)*cos(lon)*sin(Somg/a)-sin(lat)*sin(lon)*cos(Somg/a);
189        Gij(2,2)=cos(lat);
190    
191        TMatrixD Tij=Gij*Aij;
192    
193        return Tij;
194    }
195    
196    TMatrixD OrientationInfo::GEOtoECI(TMatrixD Aij, UInt_t t, Double_t lat, Double_t lon){
197       TMatrixD Gij(3,3);
198       Double_t omg = (7.292115e-5)*a;                              // Earth rotation velosity (Around polar axis);
199       Double_t d = (t-10957*86400-43200);                          //Number of day, passing from 01/01/2000 12:00:00 to t;
200       d = d/86400;
201       Double_t T = d/36525;                                        //Number of Julian centuries;
202        
203       Double_t Se = 6*3600+41*60+236.555367908*d+0.093104*pow(T,2)-(6.2e-6)*pow(T,3);
204    
205       Int_t tr = (t-10957*86400)%86400;
206    
207       Double_t Somg = (Se+49.077+omg*86400*tr/360)*360/86400;
208    
209       lon=(-lon)/a; lat=(-lat)/a;
210    
211       Gij(0,0)=cos(lat)*cos(lon)*cos(Somg/a)+cos(lat)*sin(lon)*sin(Somg/a);
212       Gij(1,0)=cos(lat)*cos(lon)*sin(Somg/a)-cos(lat)*sin(lon)*cos(Somg/a);
213       Gij(2,0)=-sin(lat);
214       Gij(0,1)=sin(lon)*cos(Somg/a)-cos(lon)*sin(Somg/a);
215       Gij(1,1)=sin(lon)*sin(Somg/a)+cos(lon)*cos(Somg/a);
216       Gij(2,1)=0;
217       Gij(0,2)=sin(lat)*cos(lon)*cos(Somg/a)+sin(lat)*sin(lon)*sin(Somg/a);
218       Gij(1,2)=sin(lat)*cos(lon)*sin(Somg/a)-sin(lat)*sin(lon)*cos(Somg/a);
219       Gij(2,2)=cos(lat);
220    
221       return Gij*Aij;
222    }
223    
224    
225  TMatrixD OrientationInfo::GEOtoGeomag(TMatrixD Aij,Double_t Bnorth, Double_t Beast, Double_t Bup){      //Geomagnetic geodetic reference frame  TMatrixD OrientationInfo::GEOtoGeomag(TMatrixD Aij,Double_t Bnorth, Double_t Beast, Double_t Bup){      //Geomagnetic geodetic reference frame
226          Double_t alpha = 0;          Double_t alpha = 0;
227          if(Beast==0. && Bnorth>0) alpha = 0; else          if(Beast==0. && Bnorth>0) alpha = 0; else
# Line 149  TMatrixD OrientationInfo::ColPermutation Line 280  TMatrixD OrientationInfo::ColPermutation
280      return Aij*Gij;      return Aij*Gij;
281  }  }
282    
283    TVector3 OrientationInfo::GetSunPosition(UInt_t atime){
284      TVector3 sunout;
285      Float_t JD=atime/86400.+2440587.5;
286    //SAV
287    //  cout << "JD = " << JD <<endl;
288    //SAV
289     //test June 1997 JD=2451545.0-877.047;
290      Float_t Tm = (JD - 2451545.0)/36525.;
291      Float_t Mo = (357.52910+35999.05030*Tm-0.0001559*Tm*Tm-0.00000048*Tm*Tm*Tm);
292    //SAV
293    //  cout<<"Tm = " << Tm << "Mo = " << Mo <<endl;
294    //SAV
295      Mo=Mo*TMath::DegToRad();
296    
297      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));
298      Co=Co* TMath::DegToRad();
299      
300      Float_t Lo = (280.46645 + 36000.76983*Tm +0.0003032*Tm*Tm);
301      Lo=Lo*TMath::DegToRad();
302      
303      Float_t theta = (Lo + Co); // * TMath::DegToRad();
304      
305      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();
306    
307    //SAV
308    //  cout << "Co = " << Co*TMath::RadToDeg() << "\tLo = " << Lo*TMath::RadToDeg() << "\ttheta = " << theta << "\teps = " << eps << endl;
309    //SAV
310    
311      Float_t YY=cos(eps)*sin(theta);
312      Float_t XX=cos(theta);
313    //SAV
314    //  cout << "XX = " << XX << "\tYY" << YY << endl;
315    //SAV
316      Float_t RASun=atan(YY/XX);
317      if(XX<0. ) RASun=RASun+TMath::Pi();
318      if(XX >0. && YY <0.) RASun=RASun+2*TMath::Pi();
319      Float_t DESun = asin(sin(eps)*sin(theta));
320    //SAV
321    //  cout << "DE = " << DESun << "\t" << RASun << endl;
322    //SAV
323      sunout.SetMagThetaPhi(1.0,TMath::Pi()/2.-DESun,RASun);
324      return sunout;
325    }
326    
327  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  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
328    Float_t mp = 938.272029;//    Float_t amu = 931.494043e0;    Float_t mp = 938.272029;//    Float_t amu = 931.494043e0;
329      Float_t cc = 299792458.;      Float_t cc = 299792458.;
# Line 158  Float_t OrientationInfo::Larmor(Float_t Line 333  Float_t OrientationInfo::Larmor(Float_t
333      Float_t mm = mp*kg;      Float_t mm = mp*kg;
334      Float_t omega = iZ*ee*Bm*1e-9/(gam*mm);      Float_t omega = iZ*ee*Bm*1e-9/(gam*mm);
335      Float_t larmor = 1e-3*sqrt(1e0-1e0/pow(gam,2))*cc/omega;      Float_t larmor = 1e-3*sqrt(1e0-1e0/pow(gam,2))*cc/omega;
336      larmor = 1e-3*Ek*cc/omega; //Ek here is p or for onecharged particle R;      larmor = 1e-3*Ek*cc/omega; //Ek here is p or for onecharged particle R; larmor in m
337      return larmor;      return larmor;
338  }  }
339    
Legend:
Removed from v.1.3  
changed lines
  Added in v.1.4
  ViewVC Help
Powered by ViewVC 1.1.23