/[PAMELA software]/quicklook/SatelliteInclination/src/InclinationInfo.cpp
ViewVC logotype

Diff of /quicklook/SatelliteInclination/src/InclinationInfo.cpp

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

revision 1.1 by cafagna, Thu Feb 8 00:49:35 2007 UTC revision 1.2 by pam-rm2, Tue Apr 17 07:46:48 2007 UTC
# Line 1  Line 1 
1  /***************************************************************************  /***************************************************************************
2   *   Copyright (C) 2006 by pamelaprod                                      *   *   Copyright (C) 2006 by pamelaprod                                      *
3   *   pamelaprod@P1.pamela                                                  *   *   pamelaprod@P1.pamela                                                  *
4   *                                                                         *   *                                                                         *
5   *   This program is free software; you can redistribute it and/or modify  *   *   This program is free software; you can redistribute it and/or modify  *
6   *   it under the terms of the GNU General Public License as published by  *   *   it under the terms of the GNU General Public License as published by  *
7   *   the Free Software Foundation; either version 2 of the License, or     *   *   the Free Software Foundation; either version 2 of the License, or     *
8   *   (at your option) any later version.                                   *   *   (at your option) any later version.                                   *
9   *                                                                         *   *                                                                         *
10   *   This program is distributed in the hope that it will be useful,       *   *   This program is distributed in the hope that it will be useful,       *
11   *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *   *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
12   *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *   *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
13   *   GNU General Public License for more details.                          *   *   GNU General Public License for more details.                          *
14   *                                                                         *   *                                                                         *
15   *   You should have received a copy of the GNU General Public License     *   *   You should have received a copy of the GNU General Public License     *
16   *   along with this program; if not, write to the                         *   *   along with this program; if not, write to the                         *
17   *   Free Software Foundation, Inc.,                                       *   *   Free Software Foundation, Inc.,                                       *
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>
21  #include <TMath.h>  #include <TMath.h>
22  #include "TMatrixD.h"  #include "TMatrixD.h"
23    
24  using namespace std;  using namespace std;
25    
26  Quaternions::Quaternions()  Quaternions::Quaternions()
27    : InclinationInfoItem()    : InclinationInfoItem()
28  {  {
29  }  }
30    
31    
32  Quaternions::~Quaternions()  Quaternions::~Quaternions()
33  {  {
34  }  }
35    
36  InclinationInfo::InclinationInfo()  InclinationInfo::InclinationInfo()
37    : TObject()    : TObject()
38  {  {
39  }  }
40    
41  InclinationInfo::~InclinationInfo()  InclinationInfo::~InclinationInfo()
42  {  {
43  }  }
44    
45  short int Sign_1(double_t a, Int_t b){  short int Sign_1(double_t a, Int_t b){
46  if(a>0){b=1;}  if(a>0){b=1;}
47  if(a<0){b=-1;}  if(a<0){b=-1;}
48  else{b=0;}  else{b=0;}
49  return b;  return b;
50  }  }
51    
52  void InclinationInfo::QuaternionstoAngle(Quaternions Qua){  
53      /******************************************************************************************************************/
54    double_t a11     = pow(Qua.quat[0][0],2.)+pow(Qua.quat[0][1],2.)-pow(Qua.quat[0][2],2.)-pow(Qua.quat[0][3],2.);  /******************************************************************************************************************/
55    double_t a12     = 2*(Qua.quat[0][1]*Qua.quat[0][2]+Qua.quat[0][0]*Qua.quat[0][3]);  //*********************                             ***************************************************************/
56    double_t a13     = 2*(Qua.quat[0][1]*Qua.quat[0][3]-Qua.quat[0][0]*Qua.quat[0][2]);  //*********************     COORDINATE SYSTEMS      ***************************************************************/
57    double_t a21     = 2*(Qua.quat[0][1]*Qua.quat[0][2]-Qua.quat[0][0]*Qua.quat[0][3]);  //*********************                             ***************************************************************/
58    double_t a22     = pow(Qua.quat[0][0],2.)+pow(Qua.quat[0][2],2.)-pow(Qua.quat[0][1],2.)-pow(Qua.quat[0][3],2.);  //*****************************************************************************************************************/
59    double_t a23     = 2*(Qua.quat[0][2]*Qua.quat[0][3]+Qua.quat[0][0]*Qua.quat[0][1]);  //*****************************************************************************************************************/
60    double_t a31     = 2*(Qua.quat[0][1]*Qua.quat[0][3]+Qua.quat[0][0]*Qua.quat[0][2]);  //
61    double_t a32     = 2*(Qua.quat[0][2]*Qua.quat[0][3]-Qua.quat[0][0]*Qua.quat[0][1]);  //                                  ZISK
62    double_t a33     = pow(Qua.quat[0][0],2.)+pow(Qua.quat[0][3],2.)-pow(Qua.quat[0][1],2.)-pow(Qua.quat[0][2],2.);  //                                 +
63    double_t a       = 360/(2*TMath::Pi());  //                                / \       YOSK      ZOSK (Directed by Radius)
64    double_t eksi    = 0.0000001;  //                                 |       _        _.
65    double_t eteta   = 0.0000001;  //                                 |      |\        /|
66    double_t ksteta  = a22*a22/(a12*a12+a22*a22);  //                                 |        \      /
67    double_t ksksi   = a33*a33/(a33*a33+a31*a31);  //                                 |         \    /
68      //                                 |.__..__   \  /
69    Int_t kj1;  //                Orbit     _._.***|        **.\/_        XOSK (Directed by velocity)
70    if (a33<0){kj1=1;  //                        .*       | (X0,Y0,Z0) **--.___\
71    } else {kj1=0;};  //                     _**         |        /     *.    /
72    Int_t kj2;  //                   .*            |       *        *
73    if (ksksi>eksi){kj2=1;  //                  *        ..****|***.. /  R       *
74    } else {kj2=0;};  //                         .*      |    .*.
75    Int_t kj3;  //                        .*       |   /  *.
76    if (ksksi<=eksi){kj3=1;  //                        * EARTH  |  /    *                                    YISK
77    } else {kj3=0;};  //                        *        | /_ _  _*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _\
78    Int_t kj4;  //                        *        /       *                                   /
79    if (a22<0){kj4=1;  //                         *      /      .*
80    } else {kj4=0;};  //                          *.   /      .*
81    Int_t kj5;  //                            **/*******
82    if (ksteta>eteta){kj5=1;  //                             /
83    } else {kj5=0;};  //                            /
84    Int_t kj6;  //                           /
85    if (ksteta<=eteta){kj6=1;  //                          /
86    } else {kj6=0;};  //                         /
87    if (abs((int)a32)>1){exit(1);};  //                        /
88    Int_t fr;  //                      |/
89    //                      *--
90    Double_t gamar = -atan(a32/sqrt(1-pow(a32,2.)));  //                  XISK
91    Double_t ksir  = (-atan(a31/a33)-TMath::Pi()*kj1*Sign_1(a31, fr))*kj2-0.5*TMath::Pi()*kj3*Sign_1(a31, fr);  //
92    Double_t tetar = -(-atan(a12/a22)-TMath::Pi()*kj4*Sign_1(a12, fr))*kj5+0.5*TMath::Pi()*kj6*Sign_1(a12, fr);  //****************************************************************************************************/
93  //  if (gamar<0){A11=gamar*a+360;}else{A11=gamar*a;};  //****************************************************************************************************/
94  //  if (ksir<0){A11=ksir*a+360;}else{A11=ksir*a;};  
95  //  if (tetar<0){A13=tetar*a+360;}else{A13=tetar*a;};      
96    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){
97  //  gamar = acos(pow(Qua.quat[0][0],2.)+pow(Qua.quat[0][3],2.)-pow(Qua.quat[0][1],2.)-pow(Qua.quat[0][2],2.));      
98  //  tetar = atan((Qua.quat[0][2]*Qua.quat[0][3]+Qua.quat[0][0]*Qua.quat[0][2])/(Qua.quat[0][2]*Qua.quat[0][3]+Qua.quat[0][1]*Qua.quat[0][0]));      double_t a = 360/(2*TMath::Pi());
99  //  ksir = atan((Qua.quat[0][1]*Qua.quat[0][3]-Qua.quat[0][0]*Qua.quat[0][2])/(Qua.quat[0][2]*Qua.quat[0][3]-Qua.quat[0][1]*Qua.quat[0][0]));      
100          TMatrixD Xij(3,3);
101          Xij(0,0) = 1; Xij(0,1) = 0; Xij(0,2) = 0;
102    A13=tetar*a;      Xij(1,0) = 0; Xij(1,1) = 0; Xij(1,2) = 1;
103    A12=ksir*a;      Xij(2,0) = 0; Xij(2,1) = -1; Xij(2,2) = 0;
104    A11=gamar*a;      
105        TMatrixD Zij(3,3);
106  return ;      Zij(0,0) = 0; Zij(0,1) = 0; Zij(0,2) = -1;
107  }      Zij(1,0) = -1; Zij(1,1) = 0; Zij(1,2) = 0;
108        Zij(2,0) = 0; Zij(2,1) = 1; Zij(2,2) = 0;
109  /******************************************************************************************************************/  
110  /******************************************************************************************************************/      TMatrixD Pij(3,3);
111  //*********************                             ***************************************************************/      Pij(0,0) = pow(q0,2)+pow(q1,2)-pow(q2,2)-pow(q3,2);
112  //*********************     COORDINATE SYSTEMS      ***************************************************************/      Pij(0,1) = /*2*(q1*q2+q0*q3);/*/ 2*(q1*q2-q0*q3);
113  //*********************                             ***************************************************************/      Pij(0,2) = /*2*(q1*q3-q0*q2);/*/ 2*(q1*q3+q0*q2);
114  //*****************************************************************************************************************/      Pij(1,0) = /*2*(q1*q2-q0*q3);/*/ 2*(q1*q2+q0*q3);
115  //*****************************************************************************************************************/      Pij(1,1) = pow(q0,2)-pow(q1,2)+pow(q2,2)-pow(q3,2);
116  //      Pij(1,2) = /*2*(q2*q3+q0*q1);/*/ 2*(q2*q3-q0*q1);
117  //                                  ZISK      Pij(2,0) = /*2*(q1*q3+q0*q2);/*/ 2*(q1*q3-q0*q2);
118  //                                 +      Pij(2,1) = /*2*(q2*q3-q0*q1);/*/ 2*(q2*q3+q0*q1);
119  //                                / \       YOSK      ZOSK (Directed by Radius)      Pij(2,2) = pow(q0,2)-pow(q1,2)-pow(q2,2)+pow(q3,2);
120  //                                 |       _        _.  
121  //                                 |      |\        /|      TMatrixD Aij(3,3);
122  //                                 |        \      /  
123  //                                 |         \    /      Double_t C1 = y0*Vz0 - z0*Vy0;
124  //                                 |.__..__   \  /      Double_t C2 = z0*Vx0 - x0*Vz0;
125  //                Orbit     _._.***|        **.\/_        XOSK (Directed by velocity)      Double_t C3 = x0*Vy0 - y0*Vx0;
126  //                        .*       | (X0,Y0,Z0) **--.___\      Double_t C  = sqrt(pow(C1,2) + pow(C2,2) + pow(C3,2));
127  //                     _**         |        /     *.    /      Double_t V0 = sqrt(pow(Vx0,2)+pow(Vy0,2) + pow(Vz0,2));
128  //                   .*            |       *        *      Double_t R0 = sqrt(pow(x0,2)+pow(y0,2) + pow(z0,2));
129  //                  *        ..****|***.. /  R       *      Aij(0,0) = /*(C2*z0-C3*y0)/(C*R0);/*/Vx0/V0;
130  //                         .*      |    .*.      Aij(0,1) = C1/C;
131  //                        .*       |   /  *.      Aij(0,2) = /*x0/R0;/*/(Vy0*C3-Vz0*C2)/(V0*C);
132  //                        * EARTH  |  /    *                                    YISK      Aij(1,0) = /*(C3*x0-C1*z0)/(C*R0);/*/Vy0/V0;
133  //                        *        | /_ _  _*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _\      Aij(1,1) = C2/C;
134  //                        *        /       *                                   /      Aij(1,2) = /*y0/R0;/*/(Vz0*C1-Vx0*C3)/(V0*C);
135  //                         *      /      .*      Aij(2,0) = /*(C1*y0-C2*x0)/(C*R0);/*/Vz0/V0;
136  //                          *.   /      .*      Aij(2,1) = C3/C;
137  //                            **/*******      Aij(2,2) = /*x0/R0;/*/(Vx0*C2-Vy0*C1)/(V0*C);
138  //                             /      Aij.Invert();
139  //                            /      
140  //                           /      TMatrixD Full_(3,3);
141  //                          /      
142  //                         /      Full_ = Aij*(Pij*Zij);
143  //                        /          
144  //                      |/      //Double_t u13 = Full_(0,2);
145  //                      *--      //Double_t u23 = Full_(1,2);
146  //                  XISK      //Double_t u22 = Full_(1,1);
147  //      //Double_t u33 = Full_(2,2);
148  //****************************************************************************************************/      //Double_t u21 = Full_(1,0);
149  //****************************************************************************************************/      
150        Double_t u13 = Full_(0,0);
151  //void OrbitalInfo::coefplane(Double_t x1,Double_t y1,Double_t z1,Double_t x2, Double_t y2, Double_t z2){      Double_t u23 = -Full_(1,0);
152  //    k1 = ((z1/y1)*((x2*y1 - x1*y2)/(z2*y1 - z1*y2)) - x1/y1);      Double_t u22 = Full_(1,1);
153  //    k2 = (x1*y2 - x2*y1)/(z2*y1 - z1*y2);      Double_t u33 = Full_(2,0);
154  //    }      Double_t u21 = Full_(1,2);
155            
156  //Double_t OrbitalInfo::AngBetAxes(Double_t x1,Double_t y1,Double_t z1,Double_t x2, Double_t y2, Double_t z2){      Tangazh = a*atan(-u13/u33);
157  //    return 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)));      Kren = a*atan(-u23/sqrt(1 - pow(u23,2)));
158  //    }      Ryskanie = a*atan(u21/u22);
159        
160  //Double_t OrbitalInfo::ValueT(Double_t x1, Double_t y1, Double_t z1, Double_t n1, Double_t n2){  return ;    
161  //    return -(x1+n1*y1+n2*z1)/(1+pow(n1,2)+pow(n2,2));  }
162  //    }  
       
 //Double_t OrbitalInfo::AngBetPlan(Double_t x1, Double_t y1, Double_t z1, Double_t n1, Double_t n2){  
 //    return asin((x1+n1*y1+n2*z1)/(sqrt(pow(x1,2)+pow(y1,2)+pow(z1,2))*sqrt(1+pow(n1,2)+pow(n2,2))));  
 //    }  
       
 void InclinationInfo::TransAngle(Double_t x0, Double_t y0, Double_t z0, Double_t Vx0, Double_t Vy0, Double_t Vz0, Double_t gamar, Double_t ksir, Double_t tetar, Double_t q0, Double_t q1, Double_t q2, Double_t q3){  
       
     double_t a = 360/(2*TMath::Pi());  
   
 //  Points on three axes of Resurs' coordinate system (RCS)  
     Int_t XRCS[3]; Int_t YRCS[3]; Int_t ZRCS[3];  
       
 // Angles between our Axes(RCS) and planes of Orbital Coordinate System (OCS);  
 //    Double_t AboAa0ZX[3];  
 //    Double_t AboAa0XY[3];  
 //    Double_t AboAa0YZ[3];  
       
 // Angles between our Axes(RCS) and Axes of OCS    
 //    Double_t AboA0X[3];  
 //    Double_t AboA0Y[3];  
 //    Double_t AboA0Z[3];  
   
 //Angles between Proection of our axes on every plane of OCS and axes of it plane.  
 //    Double_t AbPoAaAoP0ZX[3];  
 //    Double_t AbPoAaAoP0XY[3];  
 //    Double_t AbPoAaAoP0YZ[3];  
       
     XRCS[0] = 1;  YRCS[0] = 0;  ZRCS[0] = 0;   // Points on X-axis RCS.  
     XRCS[1] = 0;  YRCS[1] = 1;  ZRCS[1] = 0;   // Points on Y-axis RCS.  
     XRCS[2] = 0;  YRCS[2] = 0;  ZRCS[2] = 1;   // Points on Z-axis  
       
 // Transition matrix RCS -> Inertial Coordinate System (ICS)    
     TMatrixD Bij(3,3);  
     Bij(0,0) = cos(tetar)*cos(ksir)-sin(tetar)*sin(gamar)*sin(ksir);  
     Bij(0,1) = -sin(tetar)*cos(gamar);  
     Bij(0,2) = cos(tetar)*sin(ksir)+sin(tetar)*sin(gamar)*cos(ksir);  
     Bij(1,0) = sin(tetar)*cos(ksir)+cos(tetar)*sin(gamar)*sin(ksir);  
     Bij(1,1) = cos(tetar)*cos(gamar);  
     Bij(1,2) = sin(tetar)*sin(ksir)-cos(tetar)*sin(gamar)*cos(ksir);  
     Bij(2,0) = -sin(ksir)*cos(gamar);  
     Bij(2,1) = sin(gamar);  
     Bij(2,2) = cos(ksir)*cos(gamar);  
       
 //********************************************************************************************/  
 //*********************** OTHER METHOD OF GETING COORDINATE IN OCS****************************/  
 //********************************************************************************************/  
       
     TMatrixD Pij(3,3);  
     Pij(0,0) = pow(q0,2)+pow(q1,2)-pow(q2,2)-pow(q3,2);  
     Pij(0,1) = 2*(q1*q2+q0*q3);  
     Pij(0,2) = 2*(q1*q3-q0*q2);  
     Pij(1,0) = 2*(q1*q2-q0*q3);  
     Pij(1,1) = pow(q0,2)-pow(q1,2)+pow(q2,2)-pow(q3,2);  
     Pij(1,2) = 2*(q2*q3+q0*q1);  
     Pij(2,0) = 2*(q1*q3+q0*q2);  
     Pij(2,1) = 2*(q2*q3-q0*q1);  
     Pij(2,2) = pow(q0,2)-pow(q1,2)-pow(q2,2)+pow(q3,2);  
   
     TMatrixD Aij(3,3);  
 //    Double_t AbPoAaAoP0ZX_2m[3];  Double_t AboAa0ZX_2m[3];  
 //    Double_t AbPoAaAoP0XY_2m[3];  Double_t AboAa0XY_2m[3];  
 //    Double_t AbPoAaAoP0YZ_2m[3];  Double_t AboAa0YZ_2m[3];  
       
     Double_t C1 = y0*Vz0 - z0*Vy0;  
     Double_t C2 = z0*Vx0 - x0*Vz0;  
     Double_t C3 = x0*Vy0 - y0*Vx0;  
     Double_t C  = sqrt(pow(C1,2) + pow(C2,2) + pow(C3,2));  
     Double_t V0 = sqrt(pow(Vx0,2)+pow(Vy0,2) + pow(Vz0,2));  
     //cout<<"C1= "<<(Vy0*C3-Vz0*C2)/(V0*C)<<", C2= "<<(Vz0*C1-Vx0*C3)/(V0*C)<<", C3="<<(Vx0*C2-Vy0*C1)/(V0*C)<<"\n";  
     Double_t R0 = sqrt(pow(x0,2)+pow(y0,2) + pow(z0,2));  
     Aij(0,0) = /*(C2*z0-C3*y0)/(C*R0);/*/Vx0/V0;  
     Aij(0,1) = C1/C;  
     Aij(0,2) = /*x0/R0;/*/(Vy0*C3-Vz0*C2)/(V0*C);  
     Aij(1,0) = /*(C3*x0-C1*z0)/(C*R0);/*/Vy0/V0;  
     Aij(1,1) = C2/C;  
     Aij(1,2) = /*y0/R0;/*/(Vz0*C1-Vx0*C3)/(V0*C);  
     Aij(2,0) = /*(C1*y0-C2*x0)/(C*R0);/*/Vz0/V0;  
     Aij(2,1) = C3/C;  
     Aij(2,2) = /*x0/R0;/*/(Vx0*C2-Vy0*C1)/(V0*C);  
     Aij.Invert();  
 //    Double_t Xnew = Aij(0,0)*(X-x0)+Aij(0,1)*(Y-y0)+Aij(0,2)*(Z-z0);  
 //    Double_t Ynew = Aij(1,0)*(X-x0)+Aij(1,1)*(Y-y0)+Aij(1,2)*(Z-z0);  
 //    Double_t Znew = Aij(2,0)*(X-x0)+Aij(2,1)*(Y-y0)+Aij(2,2)*(Z-z0);  
   
 /*********************************************************************************************/      
   
     Double_t Azim = atan(R0*C3/(y0*C1-x0*C2));  
     Double_t Sa = sin(Azim); Double_t Ca = cos(Azim);  
     Double_t R1 = sqrt(pow(x0,2)+pow(y0,2));  
     Double_t Sb = z0/R0; Double_t Cb = R1/R0;  
     Double_t Sl = y0/R1; Double_t Cl = x0/R1;  
       
     TMatrixD Tij(3,3);  
     Tij(0,0) = -Cl*Sb*Ca-Sa*Sl;  
     Tij(0,1) = Sa*Cl-Ca*Sl*Sb;  
     Tij(0,2) = Ca*Cb;  
     Tij(1,0) = Ca*Sl-Sa*Sb*Cl;  
     Tij(1,1) = -Sa*Sl*Sb-Ca*Cl;  
     Tij(1,2) = Sa*Cb;  
     Tij(2,0) = Cb*Cl;  
     Tij(2,1) = Cb*Sl;  
     Tij(2,2) = Sb;  
     //cout<<"Tij\n";  
     //cout<<Tij(0,0)<<" "<<Tij(0,1)<<" "<<Tij(0,2)<<"\n";  
     //cout<<Tij(1,0)<<" "<<Tij(1,1)<<" "<<Tij(1,2)<<"\n";  
     //cout<<Tij(2,0)<<" "<<Tij(2,1)<<" "<<Tij(2,2)<<"\n";  
     //cout<<"Aij\n";  
       
       
     //TMatrixD Mij = new TMatrixD(Otestij,TMatrixD::kMult,Oij);  
     //Mij=Pij*Bij;  
     //Mij=Otestij*Oij;  
     //Mij*=Tij;  
       
     //cout<<Mij(0,0)<<" "<<Mij(0,1)<<" "<<Mij(0,2)<<"\n";  
     //cout<<Mij(1,0)<<" "<<Mij(1,1)<<" "<<Mij(1,2)<<"\n";  
     //cout<<Mij(2,0)<<" "<<Mij(2,1)<<" "<<Mij(2,2)<<"\n";  
 // Generaly idea is to Get orientation of Satellite as angles between RCS axes and all axes and planes of OCS  
 // We will get equations of RCS axes in ICS  
   
 // equation of line in space is (X-X0)/(X1-X0)=(Y-Y0)/(Y1-Y0)=(Z-Z0)/(Z1-Z0), where  
 // (X0,Y0,Z0)=(0,0,0) and (X1,Y1,Z1)=(XRCS[i],YRCS[i],ZRCS[i]) here i is may be x, y or z  
 // for us this equation is X/X1=Y/Y1=Z/Z1;  
   
 // We need in equation of line in spase for OCS also. For it take next points (Vx0,Vy0,Vz0) on X-axis  
 // and (x0,y0,z0) on Z-axis.      
 //    Double_t XonX = Vx0; Double_t YonX = Vy0; Double_t ZonX = Vz0;  
 //    Double_t XonZ = x0;  Double_t YonZ = y0;  Double_t ZonZ = z0;  
 //after this we have equations for Z- and X axis OCS it's  
 // X/XonX=Y/YonX=Z/ZonX for X-axis and X/XonZ=Y/YonZ=Z/ZonZ for Z-axis      
       
 // Next we need in equation of plane 0xz of OCS: Generaly equation is Ax+By+Cz+D=0;  
 // But all our plan pass through (0,0,0) and D=0 then we can write equation in naxt kind:  
 //  x+(B/A)y+(C/A)z=0; => x+k1*y+k2*z=0;  
 //    Double_t k1y;  
 //    Double_t k2y;  
     //cout<<YonX<<"  "<<ZonX*YonZ<<" "<<ZonZ*YonX<<"\n";  
 //    if ((YonZ != 0) && (ZonX*YonZ != ZonZ*YonX)){  
 //      coefplane(XonX,YonX,ZonX,XonZ,YonZ,ZonZ);  
         //coefplane(1,0.00001,0.00001,0,0,1);  
 //      k1y = k1; k2y = k2;  
 //    } else {k1y = 1; k2y = YonX/ZonX; cout<<"ELSE";}  
     //cout<<"P1= "<<(Vx0+k1y*Vy0+k2y*Vz0)<<"\n";  
     //cout<<"P2= "<<(x0+k1y*y0+k2y*z0)<<"\n";  
     //cout<<"P3= "<<(Vx0+k1y*Vy0+k2y*Vz0)<<"\n";  
     //cout<<"k1y= "<<k1y<<", k2y= "<<k2y<<"\n";  
 //    int uchu;  
 //    cin>>uchu;  
   
 // Next we must find equation of Y-axis of OCS. For it we must find equation  of line passing through  
 // point (0,0,0) perpendicularly by 0ZX plane of OCS  
 // generaly equation is:  
 //                       x = x0 + At;  
 //                       y = y0 + Bt;  
 //                       z = z0 + Ct;  
 // But we have point (x0,y0,z0) is (0,0,0) and other plane equation. For us it's:  
 //                       x = t;  
 //                       y = (B/A)*t   =>  y = (B/A)*x; or x/x1=y/y1=z/z1 where  
 //                       z = (C/A)*t       z = (C/A)*x;    y1=B/A,z1=C/A and x1 we must find  
   
 //    if ((YonX<0 && ZonZ>0)||(YonX>0 && ZonZ<0)) XonY = 1;  
 //    if ((YonX>0 && ZonZ>0)||(YonX>0 && ZonZ<0)) XonY = 1;  
 //    Double_t XonY = 1; Double_t YonY = -k1; Double_t ZonY = k2;  
       
 // coefficients for equations of 0XY plane of OCS.      
 //    coefplane(XonX,YonX,ZonX,XonY,YonY,ZonY);  
 //    Double_t k1XY = k1; Double_t k2XY = k2;  
     //cout<<"P3= "<<(XonY+k1XY*YonY+k2XY*ZonY)<<"\n";  
     //cout<<"P3= "<<(XonX+k1XY*YonX+k2XY*ZonX)<<"\n";  
     //cout<<"k1XY= "<<k1XY<<", k2XY= "<<k2XY<<"\n";  
 // coefficients for equations of 0XY plane of OCS.  
 //    coefplane(XonY,YonY,ZonY,XonZ,YonZ,ZonZ);  
 //    Double_t k1YZ = k1; Double_t k2YZ = k2;  
     //cout<<"P4= "<<(XonY+k1YZ*YonY+k2YZ*ZonY)<<"\n";  
     //cout<<"P4= "<<(XonZ+k1YZ*YonZ+k2YZ*ZonZ)<<"\n";  
     //cout<<"k1YZ= "<<k1YZ<<", k2YZ= "<<k2YZ<<"\n";  
       
 //    TMatrixD Gij(3,3);  
     Pij.Invert();  
 //    Gij=Pij*Aij;  
     //Gij.Invert();  
     //XXRCS = Gij(0,0); XYRCS = Gij(0,1); XZRCS = Gij(2,0);  
     //YXRCS = Gij(1,0); YYRCS = Gij(1,1); YZRCS = Gij(2,1);  
     //ZXRCS = Gij(2,0); ZYRCS = Gij(1,2); ZZRCS = Gij(2,2);  
       
     //cout<<"XXRCS= "<<XXRCS<<", YXRCS= "<<YXRCS<<", ZXRCS= "<<ZXRCS<<"\n";  
     //cout<<"XYRCS= "<<XYRCS<<", YYRCS= "<<YYRCS<<", ZYRCS= "<<ZYRCS<<"\n";  
     //cout<<"XZRCS= "<<XZRCS<<", YZRCS= "<<YZRCS<<", ZZRCS= "<<ZZRCS<<"\n";  
     //int yuip;  
     //cin>>yuip;  
       
     for (Int_t i = 0; i<3; i++) {  
 // Values of points on axes of RCS in ICS  
         Double_t XICS = Pij(0,0)*XRCS[i] + Pij(0,1)*YRCS[i] + Pij(0,2)*ZRCS[i];// + x0;  
         Double_t YICS = Pij(1,0)*XRCS[i] + Pij(1,1)*YRCS[i] + Pij(1,2)*ZRCS[i];// + y0;  
         Double_t ZICS = Pij(2,0)*XRCS[i] + Pij(2,1)*YRCS[i] + Pij(2,2)*ZRCS[i];// + z0;  
         //cout<<"XICS= "<<XICS<<", YICS= "<<YICS<<", ZICS= "<<ZICS<<"\n";  
         //cout<<"XICS= "<<XICS<<", YICS= "<<YICS<<", ZICS= "<<ZICS<<"\n";  
         //int oiu;  
         //cin>>oiu;  
   
 // Angles between our Axis and Z,Y,X-axes of OCS  
 //      AboA0Z[i]  = AngBetAxes(XICS,YICS,ZICS,XonZ,YonZ,ZonZ);  
 //      AboA0Y[i]  = AngBetAxes(XICS,YICS,ZICS,XonY,YonY,ZonY);  
 //      AboA0X[i]  = AngBetAxes(XICS,YICS,ZICS,XonX,YonX,ZonX);  
   
 //Find coordinate of our point in OCS    
 //      Double_t XOCS;  
 //      Double_t YOCS;  
 //      Double_t ZOCS;  
 //      Double_t T     = ValueT(XICS,YICS,ZICS,k1y,k2y);  
 //      Double_t XonXZ = XICS + T;  
 //      Double_t YonXZ = YICS + k1y*T;  
 //      Double_t ZonXZ = ZICS + k2y*T;  
 //      Double_t R     = T*sqrt(1+pow(k1y,2)+pow(k2y,2));  
 //      YOCS = R;  
         //cout<<"CHECK= "<<XonXZ+k1y*YonXZ+k2y*ZonXZ<<"\n";  
 //      T = ValueT(XICS,YICS,ZICS,k1XY,k2XY);  
 //      Double_t XonXY = XICS + T;  
 //      Double_t YonXY = YICS + k1XY*T;  
 //      Double_t ZonXY = ZICS + k2XY*T;  
 //      R = T*sqrt(1+pow(k1XY,2)+pow(k2XY,2));  
 //      ZOCS = R;  
         //cout<<"CHECK= "<<XonXY+k1XY*YonXY+k2XY*ZonXY<<"\n";  
 //      T = ValueT(XICS,YICS,ZICS,k1YZ,k2YZ);  
 //      Double_t XonYZ = XICS + T;  
 //      Double_t YonYZ = YICS + k1YZ*T;  
 //      Double_t ZonYZ = ZICS + k2YZ*T;  
 //      R = T*sqrt(1+pow(k1YZ,2)+pow(k2YZ,2));  
 //      XOCS = R;  
         //cout<<"CHECK= "<<XonYZ+k1YZ*YonYZ+k2YZ*ZonYZ<<"\n";  
         //cout<<"XOCS= "<<XOCS<<", YOCS= "<<YOCS<<", ZOCS="<<ZOCS<<"\n";  
           
         //Double_t AbPoAaAoP0ZX_2m[3];  Double_t AboAa0ZX_2m[3];  
         //Double_t AbPoAaAoP0XY_2m[3];  Double_t AboAa0XY_2m[3];  
         //Double_t AbPoAaAoP0YZ_2m[3];  Double_t AboAa0YZ_2m[3];  
       
 /*      C1 = YICS*Vz0 - ZICS*Vy0;  
         C2 = ZICS*Vx0 - XICS*Vz0;  
         C3 = XICS*Vy0 - YICS*Vx0;  
         C  = sqrt(pow(C1,2) + pow(C2,2) + pow(C3,2));  
         V0 = sqrt(pow(Vx0,2)+pow(Vy0,2) + pow(Vz0,2));  
         Aij(0,0) = Vx0/V0;  
         Aij(0,1) = C1/C;  
         Aij(0,2) = (Vy0*C3-Vz0*C2)/(V0*C);  
         Aij(1,0) = Vy0/V0;  
         Aij(1,1) = C2/C;  
         Aij(1,2) = (Vz0*C1-Vx0*C3)/(V0*C);  
         Aij(2,0) = Vz0/V0;  
         Aij(2,1) = C3/C;  
         Aij(2,2) = (Vx0*C2-Vy0*C1)/(V0*C);  
         Aij.Invert();  
 */  
 //2th method of getting XOCS,YOCS,ZOCS    
         Double_t XOCS_2m = Aij(0,0)*(XICS)+Aij(0,1)*(YICS)+Aij(0,2)*(ZICS);  
         Double_t YOCS_2m = Aij(1,0)*(XICS)+Aij(1,1)*(YICS)+Aij(1,2)*(ZICS);  
         Double_t ZOCS_2m = Aij(2,0)*(XICS)+Aij(2,1)*(YICS)+Aij(2,2)*(ZICS);  
           
         if (i == 0) {XXRCS = XOCS_2m; YXRCS = YOCS_2m; ZXRCS = ZOCS_2m;}  
         if (i == 1) {XYRCS = XOCS_2m; YYRCS = YOCS_2m; ZYRCS = ZOCS_2m;}  
         if (i == 2) {XZRCS = XOCS_2m; YZRCS = YOCS_2m; ZZRCS = ZOCS_2m;}  
   
         //cout<<"XOCS_2m= "<<XOCS_2m<<", YOCS_2m= "<<YOCS_2m<<", ZOCS= "<<ZOCS_2m<<"\n";  
         //int alsdj;  
         //cin>>alsdj;  
           
 //Find  Angles between RCS-axes and OCS-planes;  
 //      AboAa0ZX[i]     = AngBetPlan(XICS,YICS,ZICS,k1y,k2y);  
 //      AboAa0XY[i]     = AngBetPlan(XICS,YICS,ZICS,k1XY,k2XY);  
 //      AboAa0YZ[i]     = AngBetPlan(XICS,YICS,ZICS,k1YZ,k2YZ);  
         //AbPoAaAoP0ZX[i] = atan(ZOCS/XOCS);    AbPoAaAoP0ZX_2m[i] = atan(ZOCS_2m/XOCS_2m);  
         //AbPoAaAoP0XY[i] = atan(XOCS/YOCS);    AbPoAaAoP0XY_2m[i] = atan(YOCS_2m/XOCS_2m);  
         //AbPoAaAoP0YZ[i] = atan(ZOCS/YOCS);    AbPoAaAoP0YZ_2m[i] = atan(ZOCS_2m/YOCS_2m);  
           
 //      if (XOCS_2m>0) AbPoAaAoP0ZX_2m[i] = atan(ZOCS_2m/XOCS_2m);  
 //      if ((XOCS_2m<0)&&(ZOCS_2m>=0)) AbPoAaAoP0ZX_2m[i] = 180/a + atan(ZOCS_2m/XOCS_2m);  
 //      if ((XOCS_2m<0)&&(ZOCS_2m<0)) AbPoAaAoP0ZX_2m[i] = atan(ZOCS_2m/XOCS_2m) - 180/a;  
 //      if ((XOCS_2m=0)&&(ZOCS_2m>0)) AbPoAaAoP0ZX_2m[i] = 90/a;  
 //      if ((XOCS_2m=0)&&(ZOCS_2m<0)) AbPoAaAoP0ZX_2m[i] = -90/a;  
 //      if ((XOCS_2m=0)&&(ZOCS_2m=0)) AbPoAaAoP0ZX_2m[i] = 0;  
           
 //      if (XOCS_2m>0) AbPoAaAoP0XY_2m[i] = atan(YOCS_2m/XOCS_2m);  
 //      if ((XOCS_2m<0)&&(YOCS_2m>=0)) AbPoAaAoP0XY_2m[i] = 180/a + atan(YOCS_2m/XOCS_2m);  
 //      if ((XOCS_2m<0)&&(YOCS_2m<0)) AbPoAaAoP0XY_2m[i] = atan(YOCS_2m/XOCS_2m) - 180/a;  
 //      if ((XOCS_2m=0)&&(YOCS_2m>0)) AbPoAaAoP0XY_2m[i] = 90/a;  
 //      if ((XOCS_2m=0)&&(YOCS_2m<0)) AbPoAaAoP0XY_2m[i] = -90/a;  
 //      if ((XOCS_2m=0)&&(YOCS_2m=0)) AbPoAaAoP0XY_2m[i] = 0;  
           
 //      if (YOCS_2m>0) AbPoAaAoP0YZ_2m[i] = atan(ZOCS_2m/YOCS_2m);  
 //      if ((YOCS_2m<0)&&(ZOCS_2m>=0)) AbPoAaAoP0YZ_2m[i] = 180/a + atan(ZOCS_2m/YOCS_2m);  
 //      if ((YOCS_2m<0)&&(ZOCS_2m<0)) AbPoAaAoP0YZ_2m[i] = atan(ZOCS_2m/YOCS_2m) - 180/a;  
 //      if ((YOCS_2m=0)&&(ZOCS_2m>0)) AbPoAaAoP0YZ_2m[i] = 90/a;  
 //      if ((YOCS_2m=0)&&(ZOCS_2m<0)) AbPoAaAoP0YZ_2m[i] = -90/a;  
 //      if ((YOCS_2m=0)&&(ZOCS_2m=0)) AbPoAaAoP0YZ_2m[i] = 0;  
   
         //if (i==0) cout<<"AbPoAaAoP0ZX_2m[i]"<<AbPoAaAoP0ZX_2m[i]<<"\n";  
         //cout<<"XOCS/ZOCS= "<<XOCS_2m/ZOCS_2m<<"\n";  
         //cout<<"Atan= "<<AbPoAaAoP0ZX_2m[i]<<"\n";  
         //cout<<"atan= "<<a*atan(0.2);  
         //int GJH;  
         //cin>>GJH;  
           
     }  
     Double_t u13 = XYRCS/*Gij(0,2)*/; Double_t u33 = ZYRCS/*Gij(2,2)*/;  
     Double_t u23 = YYRCS/*Gij(1,2)*/; Double_t u21 = YZRCS/*Gij(1,0)*/;  
     Double_t u22 = YXRCS/*Gij(1,1)*/;  
     Tangazh = a*atan(-u13/u33);  
     //cout<<"u13= "<<u13<<", u33= "<<u33<<"\n";  
     Kren = a*atan(-u23/sqrt(1 - pow(u23,2)));  
     //Ryskanie = a*atan(u21/u22);  
       
     if (u22>0) Ryskanie = a*atan(u21/u22);  
     if ((u22<0)&&(u21>=0)) Ryskanie = 180 + a*atan(u21/u22);  
     if ((u22<0)&&(u21<0)) Ryskanie = a*atan(u21/u22) - 180;  
     if ((u22=0)&&(u21>0)) Ryskanie = 90;  
     if ((u22=0)&&(u21<0)) Ryskanie = -90;  
     if ((u22=0)&&(u21=0)) Ryskanie = 0;  
       
 //    AXrXo = AboA0X[0]*a;    AXrZXo = AboAa0ZX[0]*a;   ApXrZXoZ = AbPoAaAoP0ZX[0]*a;  
 //    AXrYo = AboA0Y[0]*a;    AXrXYo = AboAa0XY[0]*a;   ApXrXYoZ = AbPoAaAoP0XY[0]*a;  
 //    AXrZo = AboA0Z[0]*a;    AXrYZo = AboAa0YZ[0]*a;   ApXrYZoZ = AbPoAaAoP0YZ[0]*a;    
 //    AYrXo = AboA0X[1]*a;    AYrZXo = AboAa0ZX[1]*a;   ApYrZXoZ = AbPoAaAoP0ZX[1]*a;  
 //    AYrYo = AboA0Y[1]*a;    AYrXYo = AboAa0XY[1]*a;   ApYrXYoZ = AbPoAaAoP0XY[1]*a;  
 //    AYrZo = AboA0Z[1]*a;    AYrYZo = AboAa0YZ[1]*a;   ApYrYZoZ = AbPoAaAoP0YZ[1]*a;  
 //    AZrXo = AboA0X[2]*a;    AZrZXo = AboAa0ZX[2]*a;   ApZrZXoZ = AbPoAaAoP0ZX[2]*a;  
 //    AZrYo = AboA0Y[2]*a;    AZrXYo = AboAa0XY[2]*a;   ApZrXYoZ = AbPoAaAoP0XY[2]*a;  
 //    AZrZo = AboA0Z[2]*a;    AZrYZo = AboAa0YZ[2]*a;   ApZrYZoZ = AbPoAaAoP0YZ[2]*a;  
   
 //    AXrZXo_2m = AboAa0ZX_2m[0]*a;     ApXrZXoZ_2m = AbPoAaAoP0ZX_2m[0]*a;  
 //    AXrXYo_2m = AboAa0XY_2m[0]*a;     ApXrXYoZ_2m = AbPoAaAoP0XY_2m[0]*a;  
 //    AXrYZo_2m = AboAa0YZ_2m[0]*a;     ApXrYZoZ_2m = AbPoAaAoP0YZ_2m[0]*a;      
 //    AYrZXo_2m = AboAa0ZX_2m[1]*a;     ApYrZXoZ_2m = AbPoAaAoP0ZX_2m[1]*a;  
 //    AYrXYo_2m = AboAa0XY_2m[1]*a;     ApYrXYoZ_2m = AbPoAaAoP0XY_2m[1]*a;  
 //    AYrYZo_2m = AboAa0YZ_2m[1]*a;     ApYrYZoZ_2m = AbPoAaAoP0YZ_2m[1]*a;  
 //    AZrZXo_2m = AboAa0ZX_2m[2]*a;     ApZrZXoZ_2m = AbPoAaAoP0ZX_2m[2]*a;  
 //    AZrXYo_2m = AboAa0XY_2m[2]*a;     ApZrXYoZ_2m = AbPoAaAoP0XY_2m[2]*a;  
 //    AZrYZo_2m = AboAa0YZ_2m[2]*a;     ApZrYZoZ_2m = AbPoAaAoP0YZ_2m[2]*a;  
       
 /*          
     //Int_t Y=2;Int_t X=2;Int_t Z=4;Int_t Vx=5;Int_t Vz=9;Int_t Vy=5;  
       
     Double_t X[2]; Double_t Y[2]; Double_t Z[2]; Double_t Vx[2]; Double_t Vy[2]; Double_t Vz[2];  
       
     TMatrixD Aij(3,3);  
     TMatrixD Bij(3,3);  
       
     Bij(0,0) = cos(tetar)*cos(ksir)-sin(tetar)*sin(gamar)*sin(ksir);  
     Bij(0,1) = -sin(tetar)*cos(gamar);  
     Bij(0,2) = cos(tetar)*sin(ksir)+sin(tetar)*sin(gamar)*cos(ksir);  
     Bij(1,0) = sin(tetar)*cos(ksir)+cos(tetar)*sin(gamar)*sin(ksir);  
     Bij(1,1) = cos(tetar)*cos(gamar);  
     Bij(1,2) = sin(tetar)*sin(ksir)-cos(tetar)*sin(gamar)*cos(ksir);  
     Bij(2,0) = -sin(ksir)*cos(gamar);  
     Bij(2,1) = sin(gamar);  
     Bij(2,2) = cos(ksir)*cos(gamar);  
       
     Double_t C1 = Y[0]*Vz[0] - Z[0]*Vy[0];  
     Double_t C2 = Z[0]*Vx[0] - X[0]*Vz[0];  
     Double_t C3 = X[0]*Vy[0] - Y[0]*Vx[0];  
     Double_t C  = sqrt(pow(C1,2) + pow(C2,2) + pow(C3,2));  
     Double_t V0 = sqrt(pow(Vx0,2)+pow(Vy0,2) + pow(Vz0,2));  
     Aij(0,0) = Vx0/V0;  
     Aij(0,1) = C1/C;  
     Aij(0,2) = (Vy0*C3-Vz0*C2)/(V0*C);  
     Aij(1,0) = Vy0/V0;  
     Aij(1,1) = C2/C;  
     Aij(1,2) = (Vz0*C1-Vx0*C3)/(V0*C);  
     Aij(2,0) = Vz0/V0;  
     Aij(2,1) = C3/C;  
     Aij(2,2) = (Vx0*C2-Vy0*C1)/(V0*C);  
     Aij.Invert();  
     Double_t Xnew = Aij(0,0)*(X-x0)+Aij(0,1)*(Y-y0)+Aij(0,2)*(Z-z0);  
     Double_t Ynew = Aij(1,0)*(X-x0)+Aij(1,1)*(Y-y0)+Aij(1,2)*(Z-z0);  
     Double_t Znew = Aij(2,0)*(X-x0)+Aij(2,1)*(Y-y0)+Aij(2,2)*(Z-z0);  
     */  
     //A21 = NewTetar;  
     //A22 = NewGamar;  
     //A23 = NewKsir;  
   
 return ;      
 }  
   
Legend:
Removed from v.1.1  
changed lines
  Added in v.1.2
  ViewVC Help
Powered by ViewVC 1.1.23