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

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

Parent Directory Parent Directory | Revision Log Revision Log

Revision 1.2 - (hide annotations) (download)
Thu Mar 15 12:46:04 2007 UTC (17 years, 8 months ago) by mocchiut
Branch: MAIN
Changes since 1.1: +3 -0 lines 
Inclination bug fixed
1 pamelaprod 1.1 /***************************************************************************
2     * Copyright (C) 2006 by pamelaprod *
3     * pamelaprod@P1.pamela *
4     * *
5     * 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 *
7     * the Free Software Foundation; either version 2 of the License, or *
8     * (at your option) any later version. *
9     * *
10     * This program is distributed in the hope that it will be useful, *
11     * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12     * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13     * GNU General Public License for more details. *
14     * *
15     * You should have received a copy of the GNU General Public License *
16     * along with this program; if not, write to the *
17     * Free Software Foundation, Inc., *
18     * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
19     ***************************************************************************/
20     #include <InclinationInfo.h>
21     #include <TMath.h>
22     #include <TMatrixD.h>
23    
24     using namespace std;
25    
26     // InclinationInfoI()::InclinationInfoI() {
27     // // memset(time,0,6*sizeof(double));
28     // // memset(quad,0,6*4*sizeof(double));
29     // };
30    
31     void InclinationInfoI::fill(TArrayC* data){
32     short extIndex = 0;
33     short innIndex = 0;
34     long tempData = 0;
35     for (int i = 0; i < 6; i++){
36     extIndex = 20*i;
37     time[i] = (((data->At(extIndex) << 24) & 0xFF000000) +
38     ((data->At(extIndex + 1) << 16) & 0x00FF0000) + ((data->At(extIndex + 2) << 8) & 0x0000FF00) +
39     (data->At(extIndex + 3) & 0x000000FF))/128.0;
40    
41     for (int j = 0; j < 4; j++){
42     innIndex = extIndex + 4*j;
43     tempData = ((data->At(innIndex + 4) << 24) & 0xFF000000) + ((data->At(innIndex + 5) << 16) & 0x00FF0000) + ((data->At(innIndex + 6) << 8) & 0x0000FF00) + (data->At(innIndex + 7) & 0x000000FF);
44     if (data->At(innIndex + 4) >> 8) {
45     quat[i][j] = (~tempData * -1.0)/1073741824.0;
46     } else {
47     quat[i][j] = tempData / 1073741824.0;
48     }
49     }
50     }
51     }
52    
53     // const char* InclinationInfoItem::toXML(char* tab = ""){
54     // stringstream oss;
55     // oss.str("");
56     // for (int i = 0; i < 6; i++){
57     // oss << tab << "<QUATERNION>\n";
58     // oss << tab << "\t <param name = 'time'>" << time[i] << "</param>\n";
59     // oss << tab << "\t <param name = 'L0'>" << quat[i][0] << "</param>\n";
60     // oss << tab << "\t <param name = 'L1'>" << quat[i][1] << "</param>\n";
61     // oss << tab << "\t <param name = 'L2'>" << quat[i][2] << "</param>\n";
62     // oss << tab << "\t <param name = 'L3'>" << quat[i][3] << "</param>\n";
63     // oss << tab << "</QUATERNION>\n";
64     // }
65     // return oss.str().c_str();
66     // }
67    
68    
69     Quaternions::Quaternions()
70     : InclinationInfoI()
71     {
72     }
73    
74    
75     Quaternions::~Quaternions()
76     {
77     }
78    
79     InclinationInfo::InclinationInfo()
80     : TObject()
81     {
82     }
83    
84     InclinationInfo::~InclinationInfo()
85     {
86     }
87    
88     short int Sign_1(double_t a, Int_t b){
89     if(a>0){b=1;}
90     if(a<0){b=-1;}
91     else{b=0;}
92     return b;
93     }
94    
95 mocchiut 1.2 void InclinationInfo::Clear(){
96     };
97    
98 pamelaprod 1.1 void InclinationInfo::QuaternionstoAngle(Quaternions Qua){
99    
100     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.);
101     double_t a12 = 2*(Qua.quat[0][1]*Qua.quat[0][2]+Qua.quat[0][0]*Qua.quat[0][3]);
102     double_t a13 = 2*(Qua.quat[0][1]*Qua.quat[0][3]-Qua.quat[0][0]*Qua.quat[0][2]);
103     double_t a21 = 2*(Qua.quat[0][1]*Qua.quat[0][2]-Qua.quat[0][0]*Qua.quat[0][3]);
104     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.);
105     double_t a23 = 2*(Qua.quat[0][2]*Qua.quat[0][3]+Qua.quat[0][0]*Qua.quat[0][1]);
106     double_t a31 = 2*(Qua.quat[0][1]*Qua.quat[0][3]+Qua.quat[0][0]*Qua.quat[0][2]);
107     double_t a32 = 2*(Qua.quat[0][2]*Qua.quat[0][3]-Qua.quat[0][0]*Qua.quat[0][1]);
108     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.);
109     double_t a = 360/(2*TMath::Pi());
110     double_t eksi = 0.0000001;
111     double_t eteta = 0.0000001;
112     double_t ksteta = a22*a22/(a12*a12+a22*a22);
113     double_t ksksi = a33*a33/(a33*a33+a31*a31);
114    
115     Int_t kj1;
116     if (a33<0){kj1=1;
117     } else {kj1=0;};
118     Int_t kj2;
119     if (ksksi>eksi){kj2=1;
120     } else {kj2=0;};
121     Int_t kj3;
122     if (ksksi<=eksi){kj3=1;
123     } else {kj3=0;};
124     Int_t kj4;
125     if (a22<0){kj4=1;
126     } else {kj4=0;};
127     Int_t kj5;
128     if (ksteta>eteta){kj5=1;
129     } else {kj5=0;};
130     Int_t kj6;
131     if (ksteta<=eteta){kj6=1;
132     } else {kj6=0;};
133     if (abs((int)a32)>1){exit(1);};
134     Int_t fr;
135    
136     Double_t gamar = -atan(a32/sqrt(1-pow(a32,2.)));
137     Double_t ksir = (-atan(a31/a33)-TMath::Pi()*kj1*Sign_1(a31, fr))*kj2-0.5*TMath::Pi()*kj3*Sign_1(a31, fr);
138     Double_t tetar = -(-atan(a12/a22)-TMath::Pi()*kj4*Sign_1(a12, fr))*kj5+0.5*TMath::Pi()*kj6*Sign_1(a12, fr);
139     // if (gamar<0){A11=gamar*a+360;}else{A11=gamar*a;};
140     // if (ksir<0){A11=ksir*a+360;}else{A11=ksir*a;};
141     // if (tetar<0){A13=tetar*a+360;}else{A13=tetar*a;};
142    
143     // 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.));
144     // 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]));
145     // 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]));
146    
147    
148     A13=tetar*a;
149     A12=ksir*a;
150     A11=gamar*a;
151    
152     return ;
153     }
154    
155     /******************************************************************************************************************/
156     /******************************************************************************************************************/
157     //********************* ***************************************************************/
158     //********************* COORDINATE SYSTEMS ***************************************************************/
159     //********************* ***************************************************************/
160     //*****************************************************************************************************************/
161     //*****************************************************************************************************************/
162     //
163     // ZISK
164     // +
165     // / \ YOSK ZOSK (Directed by Radius)
166     // | _ _.
167     // | |\ /|
168     // | \ /
169     // | \ /
170     // |.__..__ \ /
171     // Orbit _._.***| **.\/_ XOSK (Directed by velocity)
172     // .* | (X0,Y0,Z0) **--.___\
173     // _** | / *. /
174     // .* | * *
175     // * ..****|***.. / R *
176     // .* | .*.
177     // .* | / *.
178     // * EARTH | / * YISK
179     // * | /_ _ _*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _\
180     // * / * /
181     // * / .*
182     // *. / .*
183     // **/*******
184     // /
185     // /
186     // /
187     // /
188     // /
189     // /
190     // |/
191     // *--
192     // XISK
193     //
194     //****************************************************************************************************/
195     //****************************************************************************************************/
196    
197     //void OrbitalInfo::coefplane(Double_t x1,Double_t y1,Double_t z1,Double_t x2, Double_t y2, Double_t z2){
198     // k1 = ((z1/y1)*((x2*y1 - x1*y2)/(z2*y1 - z1*y2)) - x1/y1);
199     // k2 = (x1*y2 - x2*y1)/(z2*y1 - z1*y2);
200     // }
201    
202     //Double_t OrbitalInfo::AngBetAxes(Double_t x1,Double_t y1,Double_t z1,Double_t x2, Double_t y2, Double_t z2){
203     // 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)));
204     // }
205    
206     //Double_t OrbitalInfo::ValueT(Double_t x1, Double_t y1, Double_t z1, Double_t n1, Double_t n2){
207     // return -(x1+n1*y1+n2*z1)/(1+pow(n1,2)+pow(n2,2));
208     // }
209    
210     //Double_t OrbitalInfo::AngBetPlan(Double_t x1, Double_t y1, Double_t z1, Double_t n1, Double_t n2){
211     // 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))));
212     // }
213    
214     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){
215    
216     double_t a = 360/(2*TMath::Pi());
217    
218     // Points on three axes of Resurs' coordinate system (RCS)
219     Int_t XRCS[3]; Int_t YRCS[3]; Int_t ZRCS[3];
220    
221     // Angles between our Axes(RCS) and planes of Orbital Coordinate System (OCS);
222     // Double_t AboAa0ZX[3];
223     // Double_t AboAa0XY[3];
224     // Double_t AboAa0YZ[3];
225    
226     // Angles between our Axes(RCS) and Axes of OCS
227     // Double_t AboA0X[3];
228     // Double_t AboA0Y[3];
229     // Double_t AboA0Z[3];
230    
231     //Angles between Proection of our axes on every plane of OCS and axes of it plane.
232     // Double_t AbPoAaAoP0ZX[3];
233     // Double_t AbPoAaAoP0XY[3];
234     // Double_t AbPoAaAoP0YZ[3];
235    
236     XRCS[0] = 1; YRCS[0] = 0; ZRCS[0] = 0; // Points on X-axis RCS.
237     XRCS[1] = 0; YRCS[1] = 1; ZRCS[1] = 0; // Points on Y-axis RCS.
238     XRCS[2] = 0; YRCS[2] = 0; ZRCS[2] = 1; // Points on Z-axis
239    
240     // Transition matrix RCS -> Inertial Coordinate System (ICS)
241     TMatrixD Bij(3,3);
242     Bij(0,0) = cos(tetar)*cos(ksir)-sin(tetar)*sin(gamar)*sin(ksir);
243     Bij(0,1) = -sin(tetar)*cos(gamar);
244     Bij(0,2) = cos(tetar)*sin(ksir)+sin(tetar)*sin(gamar)*cos(ksir);
245     Bij(1,0) = sin(tetar)*cos(ksir)+cos(tetar)*sin(gamar)*sin(ksir);
246     Bij(1,1) = cos(tetar)*cos(gamar);
247     Bij(1,2) = sin(tetar)*sin(ksir)-cos(tetar)*sin(gamar)*cos(ksir);
248     Bij(2,0) = -sin(ksir)*cos(gamar);
249     Bij(2,1) = sin(gamar);
250     Bij(2,2) = cos(ksir)*cos(gamar);
251    
252     //********************************************************************************************/
253     //*********************** OTHER METHOD OF GETING COORDINATE IN OCS****************************/
254     //********************************************************************************************/
255    
256     TMatrixD Pij(3,3);
257     Pij(0,0) = pow(q0,2)+pow(q1,2)-pow(q2,2)-pow(q3,2);
258     Pij(0,1) = 2*(q1*q2+q0*q3);
259     Pij(0,2) = 2*(q1*q3-q0*q2);
260     Pij(1,0) = 2*(q1*q2-q0*q3);
261     Pij(1,1) = pow(q0,2)-pow(q1,2)+pow(q2,2)-pow(q3,2);
262     Pij(1,2) = 2*(q2*q3+q0*q1);
263     Pij(2,0) = 2*(q1*q3+q0*q2);
264     Pij(2,1) = 2*(q2*q3-q0*q1);
265     Pij(2,2) = pow(q0,2)-pow(q1,2)-pow(q2,2)+pow(q3,2);
266    
267     TMatrixD Aij(3,3);
268     // Double_t AbPoAaAoP0ZX_2m[3]; Double_t AboAa0ZX_2m[3];
269     // Double_t AbPoAaAoP0XY_2m[3]; Double_t AboAa0XY_2m[3];
270     // Double_t AbPoAaAoP0YZ_2m[3]; Double_t AboAa0YZ_2m[3];
271    
272     Double_t C1 = y0*Vz0 - z0*Vy0;
273     Double_t C2 = z0*Vx0 - x0*Vz0;
274     Double_t C3 = x0*Vy0 - y0*Vx0;
275     Double_t C = sqrt(pow(C1,2) + pow(C2,2) + pow(C3,2));
276     Double_t V0 = sqrt(pow(Vx0,2)+pow(Vy0,2) + pow(Vz0,2));
277     //cout<<"C1= "<<(Vy0*C3-Vz0*C2)/(V0*C)<<", C2= "<<(Vz0*C1-Vx0*C3)/(V0*C)<<", C3="<<(Vx0*C2-Vy0*C1)/(V0*C)<<"\n";
278     Double_t R0 = sqrt(pow(x0,2)+pow(y0,2) + pow(z0,2));
279     Aij(0,0) = /*(C2*z0-C3*y0)/(C*R0);/*/Vx0/V0;
280     Aij(0,1) = C1/C;
281     Aij(0,2) = /*x0/R0;/*/(Vy0*C3-Vz0*C2)/(V0*C);
282     Aij(1,0) = /*(C3*x0-C1*z0)/(C*R0);/*/Vy0/V0;
283     Aij(1,1) = C2/C;
284     Aij(1,2) = /*y0/R0;/*/(Vz0*C1-Vx0*C3)/(V0*C);
285     Aij(2,0) = /*(C1*y0-C2*x0)/(C*R0);/*/Vz0/V0;
286     Aij(2,1) = C3/C;
287     Aij(2,2) = /*x0/R0;/*/(Vx0*C2-Vy0*C1)/(V0*C);
288     Aij.Invert();
289     // Double_t Xnew = Aij(0,0)*(X-x0)+Aij(0,1)*(Y-y0)+Aij(0,2)*(Z-z0);
290     // Double_t Ynew = Aij(1,0)*(X-x0)+Aij(1,1)*(Y-y0)+Aij(1,2)*(Z-z0);
291     // Double_t Znew = Aij(2,0)*(X-x0)+Aij(2,1)*(Y-y0)+Aij(2,2)*(Z-z0);
292    
293     /*********************************************************************************************/
294    
295     Double_t Azim = atan(R0*C3/(y0*C1-x0*C2));
296     Double_t Sa = sin(Azim); Double_t Ca = cos(Azim);
297     Double_t R1 = sqrt(pow(x0,2)+pow(y0,2));
298     Double_t Sb = z0/R0; Double_t Cb = R1/R0;
299     Double_t Sl = y0/R1; Double_t Cl = x0/R1;
300    
301     TMatrixD Tij(3,3);
302     Tij(0,0) = -Cl*Sb*Ca-Sa*Sl;
303     Tij(0,1) = Sa*Cl-Ca*Sl*Sb;
304     Tij(0,2) = Ca*Cb;
305     Tij(1,0) = Ca*Sl-Sa*Sb*Cl;
306     Tij(1,1) = -Sa*Sl*Sb-Ca*Cl;
307     Tij(1,2) = Sa*Cb;
308     Tij(2,0) = Cb*Cl;
309     Tij(2,1) = Cb*Sl;
310     Tij(2,2) = Sb;
311     //cout<<"Tij\n";
312     //cout<<Tij(0,0)<<" "<<Tij(0,1)<<" "<<Tij(0,2)<<"\n";
313     //cout<<Tij(1,0)<<" "<<Tij(1,1)<<" "<<Tij(1,2)<<"\n";
314     //cout<<Tij(2,0)<<" "<<Tij(2,1)<<" "<<Tij(2,2)<<"\n";
315     //cout<<"Aij\n";
316    
317    
318     //TMatrixD Mij = new TMatrixD(Otestij,TMatrixD::kMult,Oij);
319     //Mij=Pij*Bij;
320     //Mij=Otestij*Oij;
321     //Mij*=Tij;
322    
323     //cout<<Mij(0,0)<<" "<<Mij(0,1)<<" "<<Mij(0,2)<<"\n";
324     //cout<<Mij(1,0)<<" "<<Mij(1,1)<<" "<<Mij(1,2)<<"\n";
325     //cout<<Mij(2,0)<<" "<<Mij(2,1)<<" "<<Mij(2,2)<<"\n";
326     // Generaly idea is to Get orientation of Satellite as angles between RCS axes and all axes and planes of OCS
327     // We will get equations of RCS axes in ICS
328    
329     // equation of line in space is (X-X0)/(X1-X0)=(Y-Y0)/(Y1-Y0)=(Z-Z0)/(Z1-Z0), where
330     // (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
331     // for us this equation is X/X1=Y/Y1=Z/Z1;
332    
333     // We need in equation of line in spase for OCS also. For it take next points (Vx0,Vy0,Vz0) on X-axis
334     // and (x0,y0,z0) on Z-axis.
335     // Double_t XonX = Vx0; Double_t YonX = Vy0; Double_t ZonX = Vz0;
336     // Double_t XonZ = x0; Double_t YonZ = y0; Double_t ZonZ = z0;
337     //after this we have equations for Z- and X axis OCS it's
338     // X/XonX=Y/YonX=Z/ZonX for X-axis and X/XonZ=Y/YonZ=Z/ZonZ for Z-axis
339    
340     // Next we need in equation of plane 0xz of OCS: Generaly equation is Ax+By+Cz+D=0;
341     // But all our plan pass through (0,0,0) and D=0 then we can write equation in naxt kind:
342     // x+(B/A)y+(C/A)z=0; => x+k1*y+k2*z=0;
343     // Double_t k1y;
344     // Double_t k2y;
345     //cout<<YonX<<" "<<ZonX*YonZ<<" "<<ZonZ*YonX<<"\n";
346     // if ((YonZ != 0) && (ZonX*YonZ != ZonZ*YonX)){
347     // coefplane(XonX,YonX,ZonX,XonZ,YonZ,ZonZ);
348     //coefplane(1,0.00001,0.00001,0,0,1);
349     // k1y = k1; k2y = k2;
350     // } else {k1y = 1; k2y = YonX/ZonX; cout<<"ELSE";}
351     //cout<<"P1= "<<(Vx0+k1y*Vy0+k2y*Vz0)<<"\n";
352     //cout<<"P2= "<<(x0+k1y*y0+k2y*z0)<<"\n";
353     //cout<<"P3= "<<(Vx0+k1y*Vy0+k2y*Vz0)<<"\n";
354     //cout<<"k1y= "<<k1y<<", k2y= "<<k2y<<"\n";
355     // int uchu;
356     // cin>>uchu;
357    
358     // Next we must find equation of Y-axis of OCS. For it we must find equation of line passing through
359     // point (0,0,0) perpendicularly by 0ZX plane of OCS
360     // generaly equation is:
361     // x = x0 + At;
362     // y = y0 + Bt;
363     // z = z0 + Ct;
364     // But we have point (x0,y0,z0) is (0,0,0) and other plane equation. For us it's:
365     // x = t;
366     // y = (B/A)*t => y = (B/A)*x; or x/x1=y/y1=z/z1 where
367     // z = (C/A)*t z = (C/A)*x; y1=B/A,z1=C/A and x1 we must find
368    
369     // if ((YonX<0 && ZonZ>0)||(YonX>0 && ZonZ<0)) XonY = 1;
370     // if ((YonX>0 && ZonZ>0)||(YonX>0 && ZonZ<0)) XonY = 1;
371     // Double_t XonY = 1; Double_t YonY = -k1; Double_t ZonY = k2;
372    
373     // coefficients for equations of 0XY plane of OCS.
374     // coefplane(XonX,YonX,ZonX,XonY,YonY,ZonY);
375     // Double_t k1XY = k1; Double_t k2XY = k2;
376     //cout<<"P3= "<<(XonY+k1XY*YonY+k2XY*ZonY)<<"\n";
377     //cout<<"P3= "<<(XonX+k1XY*YonX+k2XY*ZonX)<<"\n";
378     //cout<<"k1XY= "<<k1XY<<", k2XY= "<<k2XY<<"\n";
379     // coefficients for equations of 0XY plane of OCS.
380     // coefplane(XonY,YonY,ZonY,XonZ,YonZ,ZonZ);
381     // Double_t k1YZ = k1; Double_t k2YZ = k2;
382     //cout<<"P4= "<<(XonY+k1YZ*YonY+k2YZ*ZonY)<<"\n";
383     //cout<<"P4= "<<(XonZ+k1YZ*YonZ+k2YZ*ZonZ)<<"\n";
384     //cout<<"k1YZ= "<<k1YZ<<", k2YZ= "<<k2YZ<<"\n";
385    
386     // TMatrixD Gij(3,3);
387     Pij.Invert();
388     // Gij=Pij*Aij;
389     //Gij.Invert();
390     //XXRCS = Gij(0,0); XYRCS = Gij(0,1); XZRCS = Gij(2,0);
391     //YXRCS = Gij(1,0); YYRCS = Gij(1,1); YZRCS = Gij(2,1);
392     //ZXRCS = Gij(2,0); ZYRCS = Gij(1,2); ZZRCS = Gij(2,2);
393    
394     //cout<<"XXRCS= "<<XXRCS<<", YXRCS= "<<YXRCS<<", ZXRCS= "<<ZXRCS<<"\n";
395     //cout<<"XYRCS= "<<XYRCS<<", YYRCS= "<<YYRCS<<", ZYRCS= "<<ZYRCS<<"\n";
396     //cout<<"XZRCS= "<<XZRCS<<", YZRCS= "<<YZRCS<<", ZZRCS= "<<ZZRCS<<"\n";
397     //int yuip;
398     //cin>>yuip;
399    
400     for (Int_t i = 0; i<3; i++) {
401     // Values of points on axes of RCS in ICS
402     Double_t XICS = Pij(0,0)*XRCS[i] + Pij(0,1)*YRCS[i] + Pij(0,2)*ZRCS[i];// + x0;
403     Double_t YICS = Pij(1,0)*XRCS[i] + Pij(1,1)*YRCS[i] + Pij(1,2)*ZRCS[i];// + y0;
404     Double_t ZICS = Pij(2,0)*XRCS[i] + Pij(2,1)*YRCS[i] + Pij(2,2)*ZRCS[i];// + z0;
405     //cout<<"XICS= "<<XICS<<", YICS= "<<YICS<<", ZICS= "<<ZICS<<"\n";
406     //cout<<"XICS= "<<XICS<<", YICS= "<<YICS<<", ZICS= "<<ZICS<<"\n";
407     //int oiu;
408     //cin>>oiu;
409    
410     // Angles between our Axis and Z,Y,X-axes of OCS
411     // AboA0Z[i] = AngBetAxes(XICS,YICS,ZICS,XonZ,YonZ,ZonZ);
412     // AboA0Y[i] = AngBetAxes(XICS,YICS,ZICS,XonY,YonY,ZonY);
413     // AboA0X[i] = AngBetAxes(XICS,YICS,ZICS,XonX,YonX,ZonX);
414    
415     //Find coordinate of our point in OCS
416     // Double_t XOCS;
417     // Double_t YOCS;
418     // Double_t ZOCS;
419     // Double_t T = ValueT(XICS,YICS,ZICS,k1y,k2y);
420     // Double_t XonXZ = XICS + T;
421     // Double_t YonXZ = YICS + k1y*T;
422     // Double_t ZonXZ = ZICS + k2y*T;
423     // Double_t R = T*sqrt(1+pow(k1y,2)+pow(k2y,2));
424     // YOCS = R;
425     //cout<<"CHECK= "<<XonXZ+k1y*YonXZ+k2y*ZonXZ<<"\n";
426     // T = ValueT(XICS,YICS,ZICS,k1XY,k2XY);
427     // Double_t XonXY = XICS + T;
428     // Double_t YonXY = YICS + k1XY*T;
429     // Double_t ZonXY = ZICS + k2XY*T;
430     // R = T*sqrt(1+pow(k1XY,2)+pow(k2XY,2));
431     // ZOCS = R;
432     //cout<<"CHECK= "<<XonXY+k1XY*YonXY+k2XY*ZonXY<<"\n";
433     // T = ValueT(XICS,YICS,ZICS,k1YZ,k2YZ);
434     // Double_t XonYZ = XICS + T;
435     // Double_t YonYZ = YICS + k1YZ*T;
436     // Double_t ZonYZ = ZICS + k2YZ*T;
437     // R = T*sqrt(1+pow(k1YZ,2)+pow(k2YZ,2));
438     // XOCS = R;
439     //cout<<"CHECK= "<<XonYZ+k1YZ*YonYZ+k2YZ*ZonYZ<<"\n";
440     //cout<<"XOCS= "<<XOCS<<", YOCS= "<<YOCS<<", ZOCS="<<ZOCS<<"\n";
441    
442     //Double_t AbPoAaAoP0ZX_2m[3]; Double_t AboAa0ZX_2m[3];
443     //Double_t AbPoAaAoP0XY_2m[3]; Double_t AboAa0XY_2m[3];
444     //Double_t AbPoAaAoP0YZ_2m[3]; Double_t AboAa0YZ_2m[3];
445    
446     /* C1 = YICS*Vz0 - ZICS*Vy0;
447     C2 = ZICS*Vx0 - XICS*Vz0;
448     C3 = XICS*Vy0 - YICS*Vx0;
449     C = sqrt(pow(C1,2) + pow(C2,2) + pow(C3,2));
450     V0 = sqrt(pow(Vx0,2)+pow(Vy0,2) + pow(Vz0,2));
451     Aij(0,0) = Vx0/V0;
452     Aij(0,1) = C1/C;
453     Aij(0,2) = (Vy0*C3-Vz0*C2)/(V0*C);
454     Aij(1,0) = Vy0/V0;
455     Aij(1,1) = C2/C;
456     Aij(1,2) = (Vz0*C1-Vx0*C3)/(V0*C);
457     Aij(2,0) = Vz0/V0;
458     Aij(2,1) = C3/C;
459     Aij(2,2) = (Vx0*C2-Vy0*C1)/(V0*C);
460     Aij.Invert();
461     */
462     //2th method of getting XOCS,YOCS,ZOCS
463     Double_t XOCS_2m = Aij(0,0)*(XICS)+Aij(0,1)*(YICS)+Aij(0,2)*(ZICS);
464     Double_t YOCS_2m = Aij(1,0)*(XICS)+Aij(1,1)*(YICS)+Aij(1,2)*(ZICS);
465     Double_t ZOCS_2m = Aij(2,0)*(XICS)+Aij(2,1)*(YICS)+Aij(2,2)*(ZICS);
466    
467     if (i == 0) {XXRCS = XOCS_2m; YXRCS = YOCS_2m; ZXRCS = ZOCS_2m;}
468     if (i == 1) {XYRCS = XOCS_2m; YYRCS = YOCS_2m; ZYRCS = ZOCS_2m;}
469     if (i == 2) {XZRCS = XOCS_2m; YZRCS = YOCS_2m; ZZRCS = ZOCS_2m;}
470    
471     //cout<<"XOCS_2m= "<<XOCS_2m<<", YOCS_2m= "<<YOCS_2m<<", ZOCS= "<<ZOCS_2m<<"\n";
472     //int alsdj;
473     //cin>>alsdj;
474    
475     //Find Angles between RCS-axes and OCS-planes;
476     // AboAa0ZX[i] = AngBetPlan(XICS,YICS,ZICS,k1y,k2y);
477     // AboAa0XY[i] = AngBetPlan(XICS,YICS,ZICS,k1XY,k2XY);
478     // AboAa0YZ[i] = AngBetPlan(XICS,YICS,ZICS,k1YZ,k2YZ);
479     //AbPoAaAoP0ZX[i] = atan(ZOCS/XOCS); AbPoAaAoP0ZX_2m[i] = atan(ZOCS_2m/XOCS_2m);
480     //AbPoAaAoP0XY[i] = atan(XOCS/YOCS); AbPoAaAoP0XY_2m[i] = atan(YOCS_2m/XOCS_2m);
481     //AbPoAaAoP0YZ[i] = atan(ZOCS/YOCS); AbPoAaAoP0YZ_2m[i] = atan(ZOCS_2m/YOCS_2m);
482    
483     // if (XOCS_2m>0) AbPoAaAoP0ZX_2m[i] = atan(ZOCS_2m/XOCS_2m);
484     // if ((XOCS_2m<0)&&(ZOCS_2m>=0)) AbPoAaAoP0ZX_2m[i] = 180/a + atan(ZOCS_2m/XOCS_2m);
485     // if ((XOCS_2m<0)&&(ZOCS_2m<0)) AbPoAaAoP0ZX_2m[i] = atan(ZOCS_2m/XOCS_2m) - 180/a;
486     // if ((XOCS_2m=0)&&(ZOCS_2m>0)) AbPoAaAoP0ZX_2m[i] = 90/a;
487     // if ((XOCS_2m=0)&&(ZOCS_2m<0)) AbPoAaAoP0ZX_2m[i] = -90/a;
488     // if ((XOCS_2m=0)&&(ZOCS_2m=0)) AbPoAaAoP0ZX_2m[i] = 0;
489    
490     // if (XOCS_2m>0) AbPoAaAoP0XY_2m[i] = atan(YOCS_2m/XOCS_2m);
491     // if ((XOCS_2m<0)&&(YOCS_2m>=0)) AbPoAaAoP0XY_2m[i] = 180/a + atan(YOCS_2m/XOCS_2m);
492     // if ((XOCS_2m<0)&&(YOCS_2m<0)) AbPoAaAoP0XY_2m[i] = atan(YOCS_2m/XOCS_2m) - 180/a;
493     // if ((XOCS_2m=0)&&(YOCS_2m>0)) AbPoAaAoP0XY_2m[i] = 90/a;
494     // if ((XOCS_2m=0)&&(YOCS_2m<0)) AbPoAaAoP0XY_2m[i] = -90/a;
495     // if ((XOCS_2m=0)&&(YOCS_2m=0)) AbPoAaAoP0XY_2m[i] = 0;
496    
497     // if (YOCS_2m>0) AbPoAaAoP0YZ_2m[i] = atan(ZOCS_2m/YOCS_2m);
498     // if ((YOCS_2m<0)&&(ZOCS_2m>=0)) AbPoAaAoP0YZ_2m[i] = 180/a + atan(ZOCS_2m/YOCS_2m);
499     // if ((YOCS_2m<0)&&(ZOCS_2m<0)) AbPoAaAoP0YZ_2m[i] = atan(ZOCS_2m/YOCS_2m) - 180/a;
500     // if ((YOCS_2m=0)&&(ZOCS_2m>0)) AbPoAaAoP0YZ_2m[i] = 90/a;
501     // if ((YOCS_2m=0)&&(ZOCS_2m<0)) AbPoAaAoP0YZ_2m[i] = -90/a;
502     // if ((YOCS_2m=0)&&(ZOCS_2m=0)) AbPoAaAoP0YZ_2m[i] = 0;
503    
504     //if (i==0) cout<<"AbPoAaAoP0ZX_2m[i]"<<AbPoAaAoP0ZX_2m[i]<<"\n";
505     //cout<<"XOCS/ZOCS= "<<XOCS_2m/ZOCS_2m<<"\n";
506     //cout<<"Atan= "<<AbPoAaAoP0ZX_2m[i]<<"\n";
507     //cout<<"atan= "<<a*atan(0.2);
508     //int GJH;
509     //cin>>GJH;
510    
511     }
512     Double_t u13 = XYRCS/*Gij(0,2)*/; Double_t u33 = ZYRCS/*Gij(2,2)*/;
513     Double_t u23 = YYRCS/*Gij(1,2)*/; Double_t u21 = YZRCS/*Gij(1,0)*/;
514     Double_t u22 = YXRCS/*Gij(1,1)*/;
515     Tangazh = a*atan(-u13/u33);
516     //cout<<"u13= "<<u13<<", u33= "<<u33<<"\n";
517     Kren = a*atan(-u23/sqrt(1 - pow(u23,2)));
518     //Ryskanie = a*atan(u21/u22);
519    
520     if (u22>0) Ryskanie = a*atan(u21/u22);
521     //cout<<Ryskanie<<"\n";
522     if ((u22<0)&&(u21>=0)) Ryskanie = 180 + a*atan(u21/u22);
523     if ((u22<0)&&(u21<0)) Ryskanie = a*atan(u21/u22) - 180;
524     if ((u22=0)&&(u21>0)) Ryskanie = 90;
525     if ((u22=0)&&(u21<0)) Ryskanie = -90;
526     if ((u22=0)&&(u21=0)) Ryskanie = 0;
527    
528     // AXrXo = AboA0X[0]*a; AXrZXo = AboAa0ZX[0]*a; ApXrZXoZ = AbPoAaAoP0ZX[0]*a;
529     // AXrYo = AboA0Y[0]*a; AXrXYo = AboAa0XY[0]*a; ApXrXYoZ = AbPoAaAoP0XY[0]*a;
530     // AXrZo = AboA0Z[0]*a; AXrYZo = AboAa0YZ[0]*a; ApXrYZoZ = AbPoAaAoP0YZ[0]*a;
531     // AYrXo = AboA0X[1]*a; AYrZXo = AboAa0ZX[1]*a; ApYrZXoZ = AbPoAaAoP0ZX[1]*a;
532     // AYrYo = AboA0Y[1]*a; AYrXYo = AboAa0XY[1]*a; ApYrXYoZ = AbPoAaAoP0XY[1]*a;
533     // AYrZo = AboA0Z[1]*a; AYrYZo = AboAa0YZ[1]*a; ApYrYZoZ = AbPoAaAoP0YZ[1]*a;
534     // AZrXo = AboA0X[2]*a; AZrZXo = AboAa0ZX[2]*a; ApZrZXoZ = AbPoAaAoP0ZX[2]*a;
535     // AZrYo = AboA0Y[2]*a; AZrXYo = AboAa0XY[2]*a; ApZrXYoZ = AbPoAaAoP0XY[2]*a;
536     // AZrZo = AboA0Z[2]*a; AZrYZo = AboAa0YZ[2]*a; ApZrYZoZ = AbPoAaAoP0YZ[2]*a;
537    
538     // AXrZXo_2m = AboAa0ZX_2m[0]*a; ApXrZXoZ_2m = AbPoAaAoP0ZX_2m[0]*a;
539     // AXrXYo_2m = AboAa0XY_2m[0]*a; ApXrXYoZ_2m = AbPoAaAoP0XY_2m[0]*a;
540     // AXrYZo_2m = AboAa0YZ_2m[0]*a; ApXrYZoZ_2m = AbPoAaAoP0YZ_2m[0]*a;
541     // AYrZXo_2m = AboAa0ZX_2m[1]*a; ApYrZXoZ_2m = AbPoAaAoP0ZX_2m[1]*a;
542     // AYrXYo_2m = AboAa0XY_2m[1]*a; ApYrXYoZ_2m = AbPoAaAoP0XY_2m[1]*a;
543     // AYrYZo_2m = AboAa0YZ_2m[1]*a; ApYrYZoZ_2m = AbPoAaAoP0YZ_2m[1]*a;
544     // AZrZXo_2m = AboAa0ZX_2m[2]*a; ApZrZXoZ_2m = AbPoAaAoP0ZX_2m[2]*a;
545     // AZrXYo_2m = AboAa0XY_2m[2]*a; ApZrXYoZ_2m = AbPoAaAoP0XY_2m[2]*a;
546     // AZrYZo_2m = AboAa0YZ_2m[2]*a; ApZrYZoZ_2m = AbPoAaAoP0YZ_2m[2]*a;
547    
548     /*
549     //Int_t Y=2;Int_t X=2;Int_t Z=4;Int_t Vx=5;Int_t Vz=9;Int_t Vy=5;
550    
551     Double_t X[2]; Double_t Y[2]; Double_t Z[2]; Double_t Vx[2]; Double_t Vy[2]; Double_t Vz[2];
552    
553     TMatrixD Aij(3,3);
554     TMatrixD Bij(3,3);
555    
556     Bij(0,0) = cos(tetar)*cos(ksir)-sin(tetar)*sin(gamar)*sin(ksir);
557     Bij(0,1) = -sin(tetar)*cos(gamar);
558     Bij(0,2) = cos(tetar)*sin(ksir)+sin(tetar)*sin(gamar)*cos(ksir);
559     Bij(1,0) = sin(tetar)*cos(ksir)+cos(tetar)*sin(gamar)*sin(ksir);
560     Bij(1,1) = cos(tetar)*cos(gamar);
561     Bij(1,2) = sin(tetar)*sin(ksir)-cos(tetar)*sin(gamar)*cos(ksir);
562     Bij(2,0) = -sin(ksir)*cos(gamar);
563     Bij(2,1) = sin(gamar);
564     Bij(2,2) = cos(ksir)*cos(gamar);
565    
566     Double_t C1 = Y[0]*Vz[0] - Z[0]*Vy[0];
567     Double_t C2 = Z[0]*Vx[0] - X[0]*Vz[0];
568     Double_t C3 = X[0]*Vy[0] - Y[0]*Vx[0];
569     Double_t C = sqrt(pow(C1,2) + pow(C2,2) + pow(C3,2));
570     Double_t V0 = sqrt(pow(Vx0,2)+pow(Vy0,2) + pow(Vz0,2));
571     Aij(0,0) = Vx0/V0;
572     Aij(0,1) = C1/C;
573     Aij(0,2) = (Vy0*C3-Vz0*C2)/(V0*C);
574     Aij(1,0) = Vy0/V0;
575     Aij(1,1) = C2/C;
576     Aij(1,2) = (Vz0*C1-Vx0*C3)/(V0*C);
577     Aij(2,0) = Vz0/V0;
578     Aij(2,1) = C3/C;
579     Aij(2,2) = (Vx0*C2-Vy0*C1)/(V0*C);
580     Aij.Invert();
581     Double_t Xnew = Aij(0,0)*(X-x0)+Aij(0,1)*(Y-y0)+Aij(0,2)*(Z-z0);
582     Double_t Ynew = Aij(1,0)*(X-x0)+Aij(1,1)*(Y-y0)+Aij(1,2)*(Z-z0);
583     Double_t Znew = Aij(2,0)*(X-x0)+Aij(2,1)*(Y-y0)+Aij(2,2)*(Z-z0);
584     */
585     //A21 = NewTetar;
586     //A22 = NewGamar;
587     //A23 = NewKsir;
588    
589     return ;
590     }
591    
592    
593     //ClassImp(McmdItem)
594     ClassImp(InclinationInfoI)
595     ClassImp(Quaternions)
596     ClassImp(InclinationInfo)
  ViewVC Help
Powered by ViewVC 1.1.23