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

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

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

revision 1.5 by mocchiut, Mon Nov 26 08:01:13 2007 UTC revision 1.9 by pam-mep, Fri Mar 28 20:47:15 2014 UTC
# Line 18  Line 18 
18   *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *   *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
19   ***************************************************************************/   ***************************************************************************/
20  #include <InclinationInfo.h>  #include <InclinationInfo.h>
 #include <TMath.h>  
 #include <TMatrixD.h>  
21    
22  using namespace std;  using namespace std;
23    
# Line 77  InclinationInfo::~InclinationInfo() Line 75  InclinationInfo::~InclinationInfo()
75  {  {
76  }  }
77    
78    /*Sine::Sine()
79      : TObject()
80    {
81    }
82    
83    Sine::~Sine()
84    {
85    }*/
86    
87  short int Sign_1(double_t a, Int_t b){  short int Sign_1(double_t a, Int_t b){
88    if(a>0){b=1;}    if(a>0){b=1;}
89    if(a<0){b=-1;}    if(a<0){b=-1;}
# Line 130  short int Sign_1(double_t a, Int_t b){ Line 137  short int Sign_1(double_t a, Int_t b){
137            
138  void InclinationInfo::TransAngle(Double_t x0, Double_t y0, Double_t z0, Double_t Vx0, Double_t Vy0, Double_t Vz0, Double_t q0, Double_t q1, Double_t q2, Double_t q3){  void InclinationInfo::TransAngle(Double_t x0, Double_t y0, Double_t z0, Double_t Vx0, Double_t Vy0, Double_t Vz0, Double_t q0, Double_t q1, Double_t q2, Double_t q3){
139            
140        cout.precision(12);
141    
142      double_t a = 360/(2*TMath::Pi());      double_t a = 360/(2*TMath::Pi());
143            
144      TMatrixD Xij(3,3);      TMatrixD Xij(3,3);
# Line 138  void InclinationInfo::TransAngle(Double_ Line 147  void InclinationInfo::TransAngle(Double_
147      Xij(2,0) = 0; Xij(2,1) = -1; Xij(2,2) = 0;      Xij(2,0) = 0; Xij(2,1) = -1; Xij(2,2) = 0;
148            
149      TMatrixD Zij(3,3);      TMatrixD Zij(3,3);
150      Zij(0,0) = 0; Zij(0,1) = 0; Zij(0,2) = -1;      Zij(0,0) = 0.0; Zij(0,1) = 0.0; Zij(0,2) = -1.0;
151      Zij(1,0) = -1; Zij(1,1) = 0; Zij(1,2) = 0;      Zij(1,0) = -1.0; Zij(1,1) = 0.0; Zij(1,2) = 0.0;
152      Zij(2,0) = 0; Zij(2,1) = 1; Zij(2,2) = 0;      Zij(2,0) = 0.0; Zij(2,1) = 1.0; Zij(2,2) = 0.0;
153    
154      TMatrixD Pij(3,3);      TMatrixD Pij(3,3);
155      Pij(0,0) = pow(q0,2)+pow(q1,2)-pow(q2,2)-pow(q3,2);      Pij(0,0) = pow(q0,2)+pow(q1,2)-pow(q2,2)-pow(q3,2);
# Line 161  void InclinationInfo::TransAngle(Double_ Line 170  void InclinationInfo::TransAngle(Double_
170      Double_t C  = sqrt(pow(C1,2) + pow(C2,2) + pow(C3,2));      Double_t C  = sqrt(pow(C1,2) + pow(C2,2) + pow(C3,2));
171      Double_t V0 = sqrt(pow(Vx0,2)+pow(Vy0,2) + pow(Vz0,2));      Double_t V0 = sqrt(pow(Vx0,2)+pow(Vy0,2) + pow(Vz0,2));
172      //    Double_t R0 = sqrt(pow(x0,2)+pow(y0,2) + pow(z0,2));      //    Double_t R0 = sqrt(pow(x0,2)+pow(y0,2) + pow(z0,2));
173      Aij(0,0) = /*(C2*z0-C3*y0)/(C*R0);/*/Vx0/V0;      Aij(0,0) = Vx0/V0;
174      Aij(0,1) = C1/C;      Aij(0,1) = C1/C;
175      Aij(0,2) = /*x0/R0;/*/(Vy0*C3-Vz0*C2)/(V0*C);      Aij(0,2) = (Vy0*C3-Vz0*C2)/(V0*C);
176      Aij(1,0) = /*(C3*x0-C1*z0)/(C*R0);/*/Vy0/V0;      Aij(1,0) = Vy0/V0;
177      Aij(1,1) = C2/C;      Aij(1,1) = C2/C;
178      Aij(1,2) = /*y0/R0;/*/(Vz0*C1-Vx0*C3)/(V0*C);      Aij(1,2) = (Vz0*C1-Vx0*C3)/(V0*C);
179      Aij(2,0) = /*(C1*y0-C2*x0)/(C*R0);/*/Vz0/V0;      Aij(2,0) = Vz0/V0;
180      Aij(2,1) = C3/C;      Aij(2,1) = C3/C;
181      Aij(2,2) = /*x0/R0;/*/(Vx0*C2-Vy0*C1)/(V0*C);      Aij(2,2) = (Vx0*C2-Vy0*C1)/(V0*C);
182      Aij.Invert();  
183        TMatrixD Bij(3,3);
184        Bij(0,0) = Vx0/V0;
185        Bij(1,0) = C1/C;
186        Bij(2,0) = (Vy0*C3-Vz0*C2)/(V0*C);
187        Bij(0,1) = Vy0/V0;
188        Bij(1,1) = C2/C;
189        Bij(2,1) = (Vz0*C1-Vx0*C3)/(V0*C);
190        Bij(0,2) = Vz0/V0;
191        Bij(1,2) = C3/C;
192        Bij(2,2) = (Vx0*C2-Vy0*C1)/(V0*C);
193    /*
194        cout<<"Coordinates:"<<endl;
195        cout<<x0<<"\t"<<y0<<"\t"<<z0<<"\t"<<Vx0/V0<<"\t"<<Vy0/V0<<"\t"<<Vz0/V0<<endl;
196    
197        cout<<"Pij"<<endl;
198        cout<<Pij(0,0)<<"\t"<<Pij(0,1)<<"\t"<<Pij(0,2)<<endl;
199        cout<<Pij(1,0)<<"\t"<<Pij(1,1)<<"\t"<<Pij(1,2)<<endl;
200        cout<<Pij(2,0)<<"\t"<<Pij(2,1)<<"\t"<<Pij(2,2)<<endl;
201    */
202        //Aij.Invert();
203            
204      TMatrixD Full_(3,3);      TMatrixD Full_(3,3);
205            
206      Full_ = Aij*(Pij*Zij);      Full_ = Bij*(Pij*Zij);
207            /*/temprary
208      //Double_t u13 = Full_(0,2);      TMatrixD Tmp(3,3);
209      //Double_t u23 = Full_(1,2);      Tmp=Pij*Zij;
210        
211        cout<<"Quaternion matrix (Tmp)"<<endl;
212        cout<<Tmp(0,0)<<"\t"<<Tmp(0,1)<<"\t"<<Tmp(0,2)<<endl;
213        cout<<Tmp(1,0)<<"\t"<<Tmp(1,1)<<"\t"<<Tmp(1,2)<<endl;
214        cout<<Tmp(2,0)<<"\t"<<Tmp(2,1)<<"\t"<<Tmp(2,2)<<endl;
215    
216        cout<<"Orientation based on Velocity"<<endl;
217        cout<<Aij(0,0)<<"\t"<<Aij(0,1)<<"\t"<<Aij(0,2)<<endl;
218        cout<<Aij(1,0)<<"\t"<<Aij(1,1)<<"\t"<<Aij(1,2)<<endl;
219        cout<<Aij(2,0)<<"\t"<<Aij(2,1)<<"\t"<<Aij(2,2)<<endl;
220    
221        cout<<"Satellite Axis in Velocity Reference frame (Full_):"<<endl;
222        cout<<Full_(0,0)<<"\t"<<Full_(0,1)<<"\t"<<Full_(0,2)<<endl;
223        cout<<Full_(1,0)<<"\t"<<Full_(1,1)<<"\t"<<Full_(1,2)<<endl;
224        cout<<Full_(2,0)<<"\t"<<Full_(2,1)<<"\t"<<Full_(2,2)<<endl;
225    */
226        Double_t u00 = Full_(0,0);
227        Double_t u10 = Full_(1,0);
228        Double_t u11 = Full_(1,1);
229        Double_t u20 = Full_(2,0);
230        Double_t u12 = Full_(1,2);
231        
232        //Double_t u13 = Full_(0,0);
233        //Double_t u23 = -Full_(1,0);
234      //Double_t u22 = Full_(1,1);      //Double_t u22 = Full_(1,1);
235      //Double_t u33 = Full_(2,2);      //Double_t u33 = Full_(2,0);
236      //Double_t u21 = Full_(1,0);      //Double_t u21 = Full_(1,2);
237            
238      Double_t u13 = Full_(0,0);      Tangazh = a*atan(-u00/u20);
239      Double_t u23 = -Full_(1,0);      Kren = a*atan(u10/sqrt(1 - pow(u10,2)));
240      Double_t u22 = Full_(1,1);      Ryskanie = a*atan(u12/u11);
241      Double_t u33 = Full_(2,0);  
242      Double_t u21 = Full_(1,2);      //Tangazh = a*atan(-u13/u33);
243            //Kren = a*atan(-u23/sqrt(1 - pow(u23,2)));
244      Tangazh = a*atan(-u13/u33);      //Ryskanie = a*atan(u21/u22);
245      Kren = a*atan(-u23/sqrt(1 - pow(u23,2)));  // end temprary
246      Ryskanie = a*atan(u21/u22);  
247    //10RED CHECK
248    /*
249    u10 = tan(Kren*TMath::DegToRad())/sqrt(pow(tan(Kren*TMath::DegToRad()),2)+1);
250    u11 = -sqrt((1-pow(u10,2))/(1+pow(tan(Ryskanie*TMath::DegToRad()),2)));
251    u12 = u11*tan(Ryskanie*TMath::DegToRad());
252    u20 = -sqrt((1-pow(u10,2))/(1+pow(tan(Tangazh*TMath::DegToRad()),2)));
253    u00 = -u20*tan(Tangazh*TMath::DegToRad());
254    
255    Double_t aa = 1+pow((u20/u00),2);
256    Double_t by = 2*u10*u11*u20/pow(u00,2);
257    Double_t cy = (1+pow(u10/u00,2))*pow(u11,2)-1;
258    Double_t bz = 2*u10*u12*u20/pow(u00,2);
259    Double_t cz = (1+pow(u10/u00,2))*pow(u12,2)-1;
260    
261    Int_t uj = TMath::Sign(1.,Ryskanie)*TMath::Sign(1.,Tangazh);
262    Double_t u21 = (-by+uj*sqrt(pow(by,2)-4*aa*cy))/(2*aa);
263    Double_t u21s = -TMath::Sign(1.,Kren)*TMath::Abs(u21);
264    Double_t u01 = TMath::Sign(1.,Ryskanie)*TMath::Abs((u10*u11+u20*u21)/u00);
265    
266    Int_t fj=1;
267    if(TMath::Sign(1.,Tangazh)>0 && TMath::Sign(1.,Ryskanie)>0) fj=-1;
268    
269    Double_t u22 = (-bz+fj*sqrt(pow(bz,2)-4*aa*cz))/(2*aa);
270    Double_t u22s = -TMath::Sign(1.,Tangazh)*TMath::Abs(u22);
271    Double_t u02 = -TMath::Abs((u10*u12+u20*u22)/u00);
272    
273    TMatrixD Dij(3,3);
274    Dij(0,0) = u00;  Dij(0,1) = u01;  Dij(0,2) = u02;
275    Dij(1,0) = u10;  Dij(1,1) = u11;  Dij(1,2) = u12;
276    Dij(2,0) = u20;  Dij(2,1) = u21s;  Dij(2,2) = u22s;
277    
278    //cout<<"Dij"<<endl;
279    //cout<<Dij(0,0)<<"\t"<<Dij(0,1)<<"\t"<<Dij(0,2)<<endl;
280    //cout<<Dij(1,0)<<"\t"<<Dij(1,1)<<"\t"<<Dij(1,2)<<endl;
281    //cout<<Dij(2,0)<<"\t"<<Dij(2,1)<<"\t"<<Dij(2,2)<<endl;
282    
283    //Aij.Invert();
284    //Zij.Invert();
285    TMatrixD Shij(3,3);
286    TMatrixD Usij(3,3);
287    Usij = (Aij*Dij);
288    Usij.Invert();
289    Shij = Zij*Usij;
290    Shij.Invert();
291    
292    cout<<"Full_ matrix having got from Euler angles"<<endl;
293    cout<<Shij(0,0)<<"\t"<<Shij(0,1)<<"\t"<<Shij(0,2)<<endl;
294    cout<<Shij(1,0)<<"\t"<<Shij(1,1)<<"\t"<<Shij(1,2)<<endl;
295    cout<<Shij(2,0)<<"\t"<<Shij(2,1)<<"\t"<<Shij(2,2)<<endl;
296    
297        cout<<"Bank = "<<Kren<<"\tSPitch = "<<Tangazh<<"\tYaw = "<<Ryskanie<<endl;
298        if(TMath::Abs(Kren)>10.0){
299    //    if(Vz0/V0>0.99){
300          Int_t Fer;
301          cin>>Fer;
302        }
303    
304        Full_.Delete();
305        Aij.Delete();
306        Pij.Delete();
307        Zij.Delete();
308        Xij.Delete();
309        Dij.Delete();
310        Shij.Delete();
311        Usij.Delete();
312    
313    // END 10RED CHECK
314    */
315  return ;      return ;    
316  }  }
317    
# Line 205  void InclinationInfo::Clear(Option_t *t) Line 325  void InclinationInfo::Clear(Option_t *t)
325  ClassImp(InclinationInfoI)  ClassImp(InclinationInfoI)
326  ClassImp(Quaternions)  ClassImp(Quaternions)
327  ClassImp(InclinationInfo)  ClassImp(InclinationInfo)
328    //ClassImp(Sine)
Legend:
Removed from v.1.5  
changed lines
  Added in v.1.9
  ViewVC Help
Powered by ViewVC 1.1.23