37 |
time[i] = (((data->At(extIndex) << 24) & 0xFF000000) + |
time[i] = (((data->At(extIndex) << 24) & 0xFF000000) + |
38 |
((data->At(extIndex + 1) << 16) & 0x00FF0000) + ((data->At(extIndex + 2) << 8) & 0x0000FF00) + |
((data->At(extIndex + 1) << 16) & 0x00FF0000) + ((data->At(extIndex + 2) << 8) & 0x0000FF00) + |
39 |
(data->At(extIndex + 3) & 0x000000FF))/128.0; |
(data->At(extIndex + 3) & 0x000000FF))/128.0; |
|
|
|
40 |
for (int j = 0; j < 4; j++){ |
for (int j = 0; j < 4; j++){ |
41 |
innIndex = extIndex + 4*j; |
innIndex = extIndex + 4*j; |
42 |
tempData = ((data->At(innIndex + 4) << 24) & 0xFF000000) + ((data->At(innIndex + 5) << 16) & 0x00FF0000) + ((data->At(innIndex + 6) << 8) & 0x0000FF00) + (data->At(innIndex + 7) & 0x000000FF); |
tempData = ((data->At(innIndex + 4) << 24) & 0xFF000000) + ((data->At(innIndex + 5) << 16) & 0x00FF0000) + ((data->At(innIndex + 6) << 8) & 0x0000FF00) + (data->At(innIndex + 7) & 0x000000FF); |
49 |
} |
} |
50 |
} |
} |
51 |
|
|
52 |
// const char* InclinationInfoItem::toXML(char* tab = ""){ |
void InclinationInfoI::clear() { |
53 |
// stringstream oss; |
for(UInt_t i = 0; i < 6; i++){ |
54 |
// oss.str(""); |
time[i]=0; |
55 |
// for (int i = 0; i < 6; i++){ |
for(UInt_t j = 0; j < 4; j++) quat[i][j]=0; |
56 |
// oss << tab << "<QUATERNION>\n"; |
} |
57 |
// oss << tab << "\t <param name = 'time'>" << time[i] << "</param>\n"; |
return ; |
58 |
// oss << tab << "\t <param name = 'L0'>" << quat[i][0] << "</param>\n"; |
} |
|
// oss << tab << "\t <param name = 'L1'>" << quat[i][1] << "</param>\n"; |
|
|
// oss << tab << "\t <param name = 'L2'>" << quat[i][2] << "</param>\n"; |
|
|
// oss << tab << "\t <param name = 'L3'>" << quat[i][3] << "</param>\n"; |
|
|
// oss << tab << "</QUATERNION>\n"; |
|
|
// } |
|
|
// return oss.str().c_str(); |
|
|
// } |
|
59 |
|
|
60 |
|
|
61 |
Quaternions::Quaternions() |
Quaternions::Quaternions() |
77 |
{ |
{ |
78 |
} |
} |
79 |
|
|
80 |
short int Sign_1(Double_t a, Int_t b){ |
short int Sign_1(double_t a, Int_t b){ |
81 |
if(a>0){b=1;} |
if(a>0){b=1;} |
82 |
if(a<0){b=-1;} |
if(a<0){b=-1;} |
83 |
else{b=0;} |
else{b=0;} |
84 |
return b; |
return b; |
85 |
} |
} |
86 |
|
|
|
void InclinationInfo::Clear(){ |
|
|
}; |
|
|
|
|
|
void InclinationInfo::QuaternionstoAngle(Quaternions Qua){ |
|
|
|
|
|
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.); |
|
|
Double_t a12 = 2*(Qua.quat[0][1]*Qua.quat[0][2]+Qua.quat[0][0]*Qua.quat[0][3]); |
|
|
Double_t a13 = 2*(Qua.quat[0][1]*Qua.quat[0][3]-Qua.quat[0][0]*Qua.quat[0][2]); |
|
|
Double_t a21 = 2*(Qua.quat[0][1]*Qua.quat[0][2]-Qua.quat[0][0]*Qua.quat[0][3]); |
|
|
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.); |
|
|
Double_t a23 = 2*(Qua.quat[0][2]*Qua.quat[0][3]+Qua.quat[0][0]*Qua.quat[0][1]); |
|
|
Double_t a31 = 2*(Qua.quat[0][1]*Qua.quat[0][3]+Qua.quat[0][0]*Qua.quat[0][2]); |
|
|
Double_t a32 = 2*(Qua.quat[0][2]*Qua.quat[0][3]-Qua.quat[0][0]*Qua.quat[0][1]); |
|
|
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.); |
|
|
Double_t a = 360/(2*TMath::Pi()); |
|
|
Double_t eksi = 0.0000001; |
|
|
Double_t eteta = 0.0000001; |
|
|
Double_t ksteta = a22*a22/(a12*a12+a22*a22); |
|
|
Double_t ksksi = a33*a33/(a33*a33+a31*a31); |
|
|
|
|
|
Int_t kj1; |
|
|
if (a33<0){kj1=1; |
|
|
} else {kj1=0;}; |
|
|
Int_t kj2; |
|
|
if (ksksi>eksi){kj2=1; |
|
|
} else {kj2=0;}; |
|
|
Int_t kj3; |
|
|
if (ksksi<=eksi){kj3=1; |
|
|
} else {kj3=0;}; |
|
|
Int_t kj4; |
|
|
if (a22<0){kj4=1; |
|
|
} else {kj4=0;}; |
|
|
Int_t kj5; |
|
|
if (ksteta>eteta){kj5=1; |
|
|
} else {kj5=0;}; |
|
|
Int_t kj6; |
|
|
if (ksteta<=eteta){kj6=1; |
|
|
} else {kj6=0;}; |
|
|
if (abs((int)a32)>1){exit(1);}; |
|
|
Int_t fr; |
|
|
|
|
|
Double_t gamar = -atan(a32/sqrt(1-pow(a32,2.))); |
|
|
Double_t ksir = (-atan(a31/a33)-TMath::Pi()*kj1*Sign_1(a31, fr))*kj2-0.5*TMath::Pi()*kj3*Sign_1(a31, fr); |
|
|
Double_t tetar = -(-atan(a12/a22)-TMath::Pi()*kj4*Sign_1(a12, fr))*kj5+0.5*TMath::Pi()*kj6*Sign_1(a12, fr); |
|
|
// if (gamar<0){A11=gamar*a+360;}else{A11=gamar*a;}; |
|
|
// if (ksir<0){A11=ksir*a+360;}else{A11=ksir*a;}; |
|
|
// if (tetar<0){A13=tetar*a+360;}else{A13=tetar*a;}; |
|
|
|
|
|
// 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.)); |
|
|
// 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])); |
|
|
// 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])); |
|
|
|
|
|
|
|
|
A13=tetar*a; |
|
|
A12=ksir*a; |
|
|
A11=gamar*a; |
|
|
|
|
|
return ; |
|
|
} |
|
87 |
|
|
88 |
/******************************************************************************************************************/ |
/******************************************************************************************************************/ |
89 |
/******************************************************************************************************************/ |
/******************************************************************************************************************/ |
102 |
// | \ / |
// | \ / |
103 |
// |.__..__ \ / |
// |.__..__ \ / |
104 |
// Orbit _._.***| **.\/_ XOSK (Directed by velocity) |
// Orbit _._.***| **.\/_ XOSK (Directed by velocity) |
105 |
// .* | (X0,Y0,Z0) **--.___\ |
// .* | (X0,Y0,Z0) **--.___| |
106 |
// _** | / *. / |
// _** | / *. / |
107 |
// .* | * * |
// .* | * * |
108 |
// * ..****|***.. / R * |
// * ..****|***.. / R * |
109 |
// .* | .*. |
// .* | .*. |
110 |
// .* | / *. |
// .* | / *. |
111 |
// * EARTH | / * YISK |
// * EARTH | / * YISK |
112 |
// * | /_ _ _*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _\ |
// * | /_ _ _*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _| |
113 |
// * / * / |
// * / * / |
114 |
// * / .* |
// * / .* |
115 |
// *. / .* |
// *. / .* |
127 |
//****************************************************************************************************/ |
//****************************************************************************************************/ |
128 |
//****************************************************************************************************/ |
//****************************************************************************************************/ |
129 |
|
|
|
//void OrbitalInfo::coefplane(Double_t x1,Double_t y1,Double_t z1,Double_t x2, Double_t y2, Double_t z2){ |
|
|
// k1 = ((z1/y1)*((x2*y1 - x1*y2)/(z2*y1 - z1*y2)) - x1/y1); |
|
|
// k2 = (x1*y2 - x2*y1)/(z2*y1 - z1*y2); |
|
|
// } |
|
|
|
|
|
//Double_t OrbitalInfo::AngBetAxes(Double_t x1,Double_t y1,Double_t z1,Double_t x2, Double_t y2, Double_t z2){ |
|
|
// 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))); |
|
|
// } |
|
130 |
|
|
131 |
//Double_t OrbitalInfo::ValueT(Double_t x1, Double_t y1, Double_t z1, Double_t n1, Double_t n2){ |
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){ |
|
// return -(x1+n1*y1+n2*z1)/(1+pow(n1,2)+pow(n2,2)); |
|
|
// } |
|
132 |
|
|
133 |
//Double_t OrbitalInfo::AngBetPlan(Double_t x1, Double_t y1, Double_t z1, Double_t n1, Double_t n2){ |
double_t a = 360/(2*TMath::Pi()); |
|
// 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)))); |
|
|
// } |
|
134 |
|
|
135 |
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){ |
TMatrixD Xij(3,3); |
136 |
|
Xij(0,0) = 1; Xij(0,1) = 0; Xij(0,2) = 0; |
137 |
Double_t a = 360/(2*TMath::Pi()); |
Xij(1,0) = 0; Xij(1,1) = 0; Xij(1,2) = 1; |
138 |
|
Xij(2,0) = 0; Xij(2,1) = -1; Xij(2,2) = 0; |
139 |
|
|
140 |
|
TMatrixD Zij(3,3); |
141 |
|
Zij(0,0) = 0; Zij(0,1) = 0; Zij(0,2) = -1; |
142 |
|
Zij(1,0) = -1; Zij(1,1) = 0; Zij(1,2) = 0; |
143 |
|
Zij(2,0) = 0; Zij(2,1) = 1; Zij(2,2) = 0; |
144 |
|
|
|
// 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****************************/ |
|
|
//********************************************************************************************/ |
|
|
|
|
145 |
TMatrixD Pij(3,3); |
TMatrixD Pij(3,3); |
146 |
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); |
147 |
Pij(0,1) = 2*(q1*q2+q0*q3); |
Pij(0,1) = /*2*(q1*q2+q0*q3);/*/ 2*(q1*q2-q0*q3); |
148 |
Pij(0,2) = 2*(q1*q3-q0*q2); |
Pij(0,2) = /*2*(q1*q3-q0*q2);/*/ 2*(q1*q3+q0*q2); |
149 |
Pij(1,0) = 2*(q1*q2-q0*q3); |
Pij(1,0) = /*2*(q1*q2-q0*q3);/*/ 2*(q1*q2+q0*q3); |
150 |
Pij(1,1) = pow(q0,2)-pow(q1,2)+pow(q2,2)-pow(q3,2); |
Pij(1,1) = pow(q0,2)-pow(q1,2)+pow(q2,2)-pow(q3,2); |
151 |
Pij(1,2) = 2*(q2*q3+q0*q1); |
Pij(1,2) = /*2*(q2*q3+q0*q1);/*/ 2*(q2*q3-q0*q1); |
152 |
Pij(2,0) = 2*(q1*q3+q0*q2); |
Pij(2,0) = /*2*(q1*q3+q0*q2);/*/ 2*(q1*q3-q0*q2); |
153 |
Pij(2,1) = 2*(q2*q3-q0*q1); |
Pij(2,1) = /*2*(q2*q3-q0*q1);/*/ 2*(q2*q3+q0*q1); |
154 |
Pij(2,2) = pow(q0,2)-pow(q1,2)-pow(q2,2)+pow(q3,2); |
Pij(2,2) = pow(q0,2)-pow(q1,2)-pow(q2,2)+pow(q3,2); |
155 |
|
|
156 |
TMatrixD Aij(3,3); |
TMatrixD Aij(3,3); |
157 |
// 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]; |
|
|
|
|
158 |
Double_t C1 = y0*Vz0 - z0*Vy0; |
Double_t C1 = y0*Vz0 - z0*Vy0; |
159 |
Double_t C2 = z0*Vx0 - x0*Vz0; |
Double_t C2 = z0*Vx0 - x0*Vz0; |
160 |
Double_t C3 = x0*Vy0 - y0*Vx0; |
Double_t C3 = x0*Vy0 - y0*Vx0; |
161 |
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)); |
162 |
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)); |
163 |
//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)); |
|
Double_t R0 = sqrt(pow(x0,2)+pow(y0,2) + pow(z0,2)); |
|
164 |
Aij(0,0) = /*(C2*z0-C3*y0)/(C*R0);/*/Vx0/V0; |
Aij(0,0) = /*(C2*z0-C3*y0)/(C*R0);/*/Vx0/V0; |
165 |
Aij(0,1) = C1/C; |
Aij(0,1) = C1/C; |
166 |
Aij(0,2) = /*x0/R0;/*/(Vy0*C3-Vz0*C2)/(V0*C); |
Aij(0,2) = /*x0/R0;/*/(Vy0*C3-Vz0*C2)/(V0*C); |
171 |
Aij(2,1) = C3/C; |
Aij(2,1) = C3/C; |
172 |
Aij(2,2) = /*x0/R0;/*/(Vx0*C2-Vy0*C1)/(V0*C); |
Aij(2,2) = /*x0/R0;/*/(Vx0*C2-Vy0*C1)/(V0*C); |
173 |
Aij.Invert(); |
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"; |
|
174 |
|
|
175 |
// TMatrixD Gij(3,3); |
TMatrixD Full_(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); |
|
176 |
|
|
177 |
//cout<<"XXRCS= "<<XXRCS<<", YXRCS= "<<YXRCS<<", ZXRCS= "<<ZXRCS<<"\n"; |
Full_ = Aij*(Pij*Zij); |
178 |
//cout<<"XYRCS= "<<XYRCS<<", YYRCS= "<<YYRCS<<", ZYRCS= "<<ZYRCS<<"\n"; |
|
179 |
//cout<<"XZRCS= "<<XZRCS<<", YZRCS= "<<YZRCS<<", ZZRCS= "<<ZZRCS<<"\n"; |
//Double_t u13 = Full_(0,2); |
180 |
//int yuip; |
//Double_t u23 = Full_(1,2); |
181 |
//cin>>yuip; |
//Double_t u22 = Full_(1,1); |
182 |
|
//Double_t u33 = Full_(2,2); |
183 |
|
//Double_t u21 = Full_(1,0); |
184 |
|
|
185 |
|
Double_t u13 = Full_(0,0); |
186 |
|
Double_t u23 = -Full_(1,0); |
187 |
|
Double_t u22 = Full_(1,1); |
188 |
|
Double_t u33 = Full_(2,0); |
189 |
|
Double_t u21 = Full_(1,2); |
190 |
|
|
|
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)*/; |
|
191 |
Tangazh = a*atan(-u13/u33); |
Tangazh = a*atan(-u13/u33); |
|
//cout<<"u13= "<<u13<<", u33= "<<u33<<"\n"; |
|
192 |
Kren = a*atan(-u23/sqrt(1 - pow(u23,2))); |
Kren = a*atan(-u23/sqrt(1 - pow(u23,2))); |
193 |
//Ryskanie = a*atan(u21/u22); |
Ryskanie = a*atan(u21/u22); |
|
|
|
|
if (u22>0) Ryskanie = a*atan(u21/u22); |
|
|
//cout<<Ryskanie<<"\n"; |
|
|
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; |
|
194 |
|
|
195 |
return ; |
return ; |
196 |
} |
} |
197 |
|
|
198 |
|
|
199 |
|
void InclinationInfo::Clear(Option_t *t){ |
200 |
|
//Int_t gyh = 0; |
201 |
|
} |
202 |
|
|
203 |
|
|
204 |
//ClassImp(McmdItem) |
//ClassImp(McmdItem) |
205 |
ClassImp(InclinationInfoI) |
ClassImp(InclinationInfoI) |
206 |
ClassImp(Quaternions) |
ClassImp(Quaternions) |