--- DarthVader/OrbitalInfo/src/OrientationInfo.cpp	2008/10/01 15:26:35	1.1
+++ DarthVader/OrbitalInfo/src/OrientationInfo.cpp	2011/11/15 09:31:28	1.2
@@ -34,37 +34,20 @@
 }
 
 TMatrixD OrientationInfo::ECItoGreenwich(TMatrixD Aij, UInt_t t){
-    //t=1154304000+86400*365;
     TMatrixD Gij(3,3);
-    Double_t omg = (7.292115e-5)*a; // Earth rotation velosity (Around polar axis);
-    //Double_t t1 = 0;
-    //if(t<=1158883200) t1 = 1127347200+229.2732;		     //absTime at 22/09/2005 + diference between Solar midnight and Greenwich sidereal midnight
-    //if(t>1158883200&&t<=1190419200) t1 = 1158883200+172.3415;//absTime at 22/09/2006 + diference between Solar midnight and Greenwich sidereal midnight
-    //if(t>=1190419200&&t<1222041600) t1 = 1190419200+115.39;  //absTime at 22/09/2007 + diference between Solar midnight and Greenwich sidereal midnight
-    //if(t>=1222041600) t1 = 1222041600 + 294.9361;	     //absTime at 22/09/2008 + diference between Solar midnight and Greenwich sidereal midnight
-    //UInt_t Nd = (t-t1)/86400;
-    //Int_t DifSuSt = Nd*236.55;
+    Double_t omg = (7.292115e-5)*a; 			// Earth rotation velosity (Around polar axis);
     Double_t d = (t-10957*86400-43200); 		     //Number of day, passing from 01/01/2000 12:00:00 to t;
     d = d/86400;
-    //d = t-da*86400+DifSuSt
-    //cout<<"t = "<<t<<"\n";
-    //cout<<"t - 2000y = "<<t-10957*86400-43200<<"\n";
-    //cout<<"d = "<<d<<"\n";
-    //Int_t tl = t%86400;    //!!!!!!!!!!!!!!!!!!!!!!!!
     Double_t T = d/36525;				     //Number of Julian centuries;
     
-    //Double_t tl = t-t1-Nd*86400-DifSuSt;
     Double_t Se = 6*3600+41*60+236.555367908*d+0.093104*pow(T,2)-(6.2e-6)*pow(T,3);
-    //cout<<"Se = "<<Se<<"\n";
-    //cout<<t<<endl<<d<<endl<<tl<<endl<<Se+omg*tl*86400/360<<endl;
+
     Int_t tr = (t-10957*86400)%86400;
-    //cout<<"tr = "<<tr<<endl;
+
     Double_t Somg = (Se+49.077+omg*86400*tr/360)*360/86400;
-    //cout<<"t1 = "<<(t-10957*86400)%86400<<"\n";
-    //cout<<"tr = "<<tr<<"\n";
-    //cout<<"Somg = "<<Se+omg*86400*tr/360<<"\n";
-    //cout<<"Somg = "<<((Somg-360*6)*86400/360/3600-20)*60<<"\n";
-    //cout<<cos(Somg/a)<<endl;
+
+    //Somg = 25; //for test transition
+
     Gij(0,0) = cos(Somg/a);
     Gij(0,1) = -sin(Somg/a);
     Gij(0,2) = 0;
@@ -75,88 +58,78 @@
     Gij(2,1) = 0;
     Gij(2,2) = 1;
     Gij.Invert();
-    //SetDirAxisGreenwich(Aij);
-    //cout<<(Somg/a)<<endl<<Aij(0,0)<<" "<<Aij(1,0)<<" "<<Aij(2,0)<<endl;
     return Gij*Aij;
 }
 
 TMatrixD OrientationInfo::GreenwichtoGEO(Double_t lat, Double_t lon, TMatrixD Aij){
-    //Double_t a = 360/(2*TMath::Pi());
-    //Double_t Re = 6000000;
+
     TMatrixD Gij(3,3);
     TMatrixD Fij(3,3);
-    
-    TMatrixD Hij(3,3); //TEST
-    TMatrixD Iij(3,3); //TEST
-    
-//    if((lat<0.1)&&(lat>-0.1)){
-	//cout<<"lon = "<<lon<<" lat = "<<lat<<endl;
-	lon=(-lon)/a; lat=(-lat)/a;
-	//cout<<"lon = "<<lon<<" lat = "<<lat<<endl;
-//    
-//        cout<<"Quaternions Array"<<endl;
-	//cout<<Aij(0,0)<<" "<<Aij(0,1)<<" "<<Aij(0,2)<<endl;
-        //cout<<Aij(1,0)<<" "<<Aij(1,1)<<" "<<Aij(1,2)<<endl;
-	//cout<<Aij(2,0)<<" "<<Aij(2,1)<<" "<<Aij(2,2)<<endl<<endl;
-//    }
-    //Double_t x0 = (alt+Re)*sin(lat)*sin(lon);
-    //Double_t y0 = (alt+Re)*sin(lat)*cos(lon);
-    //Double_t Sa = lon-a*atan(y0/x0);
-    //if (y0>0&&x0<0) Sa=-Sa+90;
-    //if (y0<0&&x0<0) Sa=Sa-90;
-    //if (y0>0&&x0==0) Sa=90;
-    //if (y0<0&&x0==0) Sa=-90;
 
-    Gij(0,0) = cos(lon);
+    lon=(-lon)/a; lat=(-lat)/a;   // here has the same result as Gij.Invert() in ECItoGreenwich function
+
+    Gij(0,0) = cos(lon);	// rotation around z-axis:
     Gij(0,1) = -sin(lon);
-    Gij(0,2) = 0;
-    Gij(1,0) = sin(lon);
-    Gij(1,1) = cos(lon);
+    Gij(0,2) = 0;		//	| cos(lon) 	-sin(lon)	0|
+    Gij(1,0) = sin(lon);	//	| sin(lon) 	cos(lon)	0|
+    Gij(1,1) = cos(lon);	//	|   0			0	1|
     Gij(1,2) = 0;
     Gij(2,0) = 0;
     Gij(2,1) = 0;
     Gij(2,2) = 1;
-
-    //cout<<"First rotation"<<endl;
-    //cout<<Gij(0,0)<<" "<<Gij(0,1)<<" "<<Gij(0,2)<<endl;
-    //cout<<Gij(1,0)<<" "<<Gij(1,1)<<" "<<Gij(1,2)<<endl;
-    //cout<<Gij(2,0)<<" "<<Gij(2,1)<<" "<<Gij(2,2)<<endl<<endl;
-    
-    //Gij.Invert();
     
-    Fij(0,0) = cos(lat);
-    Fij(0,1) = 0;
-    Fij(0,2) = -sin(lat);
-    Fij(1,0) = 0;
-    Fij(1,1) = 1;
+    Fij(0,0) = cos(lat);	// rotation around y-axis at angle -lat, cause rotation around y from x to z axis is negative
+    Fij(0,1) = 0;		//
+    Fij(0,2) = -sin(lat);	//	|cos(-lat)	0	sin(-lat)|		|cos(lat)	0	-sin(lat)|
+    Fij(1,0) = 0;		//	|	0	1		0 |	==>	|	0	1		0 |
+    Fij(1,1) = 1;		//	|-sin(-lat)	0	cos(-lat)|		|sin(lat)	0	cos(lat) |
     Fij(1,2) = 0;
     Fij(2,0) = sin(lat);
     Fij(2,1) = 0;
     Fij(2,2) = cos(lat);
-    
-    //Fij.Invert();
-    
-    //if((lat<0.1)&&(lat>-0.1)){
-    /*    Hij=Gij*Aij;  //TEST
-    
-        cout<<"First rotation"<<endl;
-        cout<<Hij(0,0)<<" "<<Hij(0,1)<<" "<<Hij(0,2)<<endl;
-        cout<<Hij(1,0)<<" "<<Hij(1,1)<<" "<<Hij(1,2)<<endl;
-        cout<<Hij(2,0)<<" "<<Hij(2,1)<<" "<<Hij(2,2)<<endl<<endl;
-    
-        Iij = Fij*(Gij*Aij); //TEST
-    
-        cout<<"Second rotation"<<endl;
-	cout<<Iij(0,0)<<" "<<Iij(0,1)<<" "<<Iij(0,2)<<endl;
-        cout<<Iij(1,0)<<" "<<Iij(1,1)<<" "<<Iij(1,2)<<endl;
-	cout<<Iij(2,0)<<" "<<Iij(2,1)<<" "<<Iij(2,2)<<endl;
-//    
-        Int_t ret;
-	cin>>ret;*/
-//    }
+
     return Fij*(Gij*Aij);
 }
 
+TMatrixD OrientationInfo::GEOtoGeomag(TMatrixD Aij,Double_t Bnorth, Double_t Beast, Double_t Bup){	//Geomagnetic geodetic reference frame
+	Double_t alpha = 0;
+	if(Beast==0. && Bnorth>0) alpha = 0; else
+	if(Beast==0. && Bnorth<0) alpha = 180.; else{
+		if(Beast > 0) alpha = TMath::ATan(Bnorth/Beast)*TMath::RadToDeg() - 90.;
+		if(Beast < 0) alpha = TMath::ATan(Bnorth/Beast)*TMath::RadToDeg() + 90.;
+	}
+	alpha = alpha*TMath::DegToRad();
+	Double_t beta = TMath::ATan(Bup/sqrt(pow(Bnorth,2)+pow(Beast,2)));
+	//if(Bup<0.0) beta = TMath::ATan(TMath::Abs(Bup/sqrt(pow(Bnorth,2)+pow(Beast,2))));
+	//if(Bup>0.0) beta = TMath::ATan(TMath::Abs(sqrt(pow(Bnorth,2)+pow(Beast,2))/Bup));
+	//cout<<"GEOtomag:alpha = "<<alpha*TMath::RadToDeg()<<"\tbeta = "<<beta*TMath::RadToDeg()<<endl;
+	TMatrixD Gij(3,3);
+    	TMatrixD Fij(3,3);
+	Gij(0,0) = 1;			//rotation around x-axis at angle alpha
+	Gij(0,1) = 0;
+	Gij(0,2) = 0;			//	|1		0		0	|
+	Gij(1,0) = 0;			//	|0	cos(alpha)	-sin(alpha)	|
+	Gij(1,1) = cos(alpha);	//	|0	sin(alpha)	cos(alpha)	|
+	Gij(1,2) = -sin(alpha);
+	Gij(2,0) = 0;
+	Gij(2,1) = sin(alpha);
+	Gij(2,2) = cos(alpha);
+	Gij.Invert();
+	Fij(0,0) = cos(beta);	//rotation around y-axis at angle beta
+	Fij(0,1) = 0;
+    	Fij(0,2) = sin(beta);	//	|cos(beta)	0	sin(beta)|
+    	Fij(1,0) = 0;			//	|	0	1		0 |
+    	Fij(1,1) = 1;			//	|-sin(beta)	0	cos(beta)|
+    	Fij(1,2) = 0;
+    	Fij(2,0) = -sin(beta);
+    	Fij(2,1) = 0;
+    	Fij(2,2) = cos(beta);
+	Fij.Invert();
+	//Int_t tri;
+	//cin >> tri;
+	return Fij*(Gij*Aij);
+}
+
 TMatrixD OrientationInfo::PamelatoGEO(TMatrixD Aij, Double_t B1, Double_t B2, Double_t B3){
     //TMatrixD Gij(3,3);
     TMatrixD Hij(3,1);
@@ -164,23 +137,8 @@
     Bij(0,0) = B1;
     Bij(1,0) = B2;
     Bij(2,0) = B3;
-    //Double_t alfa = TMath::ASin(sqrt(1/((Aij(1,2))/Aij(0,2)+1))) * TMath::RadToDeg();
-    //Gij(0,0) = cos(alfa/a);   		
-    //Gij(0,1) = -sin(alfa/a);  		
-    //Gij(0,2) = 0;	  			
-    //Gij(1,0) = 0;	  			
-    //Gij(1,1) = 1;	  			
-    //Gij(1,2) = 0;	  			
-    //Gij(2,0) = sin(alfa/a);  			
-    //Gij(2,1) = cos(alfa/a);  			
-    //Gij(2,2) = 0;				
-    
     Hij=Aij*Bij;
     return Hij;
-    //cout<<0.25-Aij(2,2)/(Aij(2,1)*Aij(2,0))<<endl;
-    //cout<<Hij(0,0)<<endl;//" "<<Hij(0,1)<<" "<<Hij(0,2)<<endl;
-    //cout<<Hij(1,0)<<endl;//" "<<Hij(1,1)<<" "<<Hij(1,2)<<endl;
-    //cout<<Hij(2,0)<<endl;//" "<<Hij(2,1)<<" "<<Hij(2,2)<<endl;
 }
 
 TMatrixD OrientationInfo::ColPermutation(TMatrixD Aij){
@@ -191,6 +149,32 @@
     return Aij*Gij;
 }
 
+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 mp = 938.272029;	Float_t amu = 931.494043e0;
+    Float_t cc = 299792458.;
+    Float_t ee = 1.60217653e-19;
+    Float_t kg = 1.7826619e-30;
+    Float_t gam = (Ek+mp)/mp;
+    Float_t mm = mp*kg;
+    Float_t omega = iZ*ee*Bm*1e-9/(gam*mm);
+    Float_t larmor = 1e-3*sqrt(1e0-1e0/pow(gam,2))*cc/omega;
+    larmor = 1e-3*Ek*cc/omega; //Ek here is p or for onecharged particle R;
+    return larmor;
+}
+
+TMatrixD OrientationInfo::GetDirectiontoGirocenter(Float_t R, Float_t Px, Float_t Py){
+    TMatrixD GirDir(3,1);
+    if(R>0){
+	GirDir(0,0) = Py;
+	GirDir(1,0) = -Px;
+    }else{
+	GirDir(0,0) = -Py;
+	GirDir(1,0) = Px;
+    }
+    GirDir(2,0) = 0.;
+    return GirDir;
+}
+
 Double_t OrientationInfo::GetPitchAngle(Double_t x1, Double_t y1, Double_t z1, Double_t x2, Double_t y2, Double_t z2){
     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();
 }