| 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 |
Quaternions::Quaternions()
|
| 27 |
: InclinationInfoItem()
|
| 28 |
{
|
| 29 |
}
|
| 30 |
|
| 31 |
|
| 32 |
Quaternions::~Quaternions()
|
| 33 |
{
|
| 34 |
}
|
| 35 |
|
| 36 |
InclinationInfo::InclinationInfo()
|
| 37 |
: TObject()
|
| 38 |
{
|
| 39 |
}
|
| 40 |
|
| 41 |
InclinationInfo::~InclinationInfo()
|
| 42 |
{
|
| 43 |
}
|
| 44 |
|
| 45 |
short int Sign_1(double_t a, Int_t b){
|
| 46 |
if(a>0){b=1;}
|
| 47 |
if(a<0){b=-1;}
|
| 48 |
else{b=0;}
|
| 49 |
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]);
|
| 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]);
|
| 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());
|
| 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;
|
| 70 |
if (a33<0){kj1=1;
|
| 71 |
} else {kj1=0;};
|
| 72 |
Int_t kj2;
|
| 73 |
if (ksksi>eksi){kj2=1;
|
| 74 |
} else {kj2=0;};
|
| 75 |
Int_t kj3;
|
| 76 |
if (ksksi<=eksi){kj3=1;
|
| 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.)));
|
| 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 |
|
| 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]));
|
| 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 |
|
| 101 |
|
| 102 |
A13=tetar*a;
|
| 103 |
A12=ksir*a;
|
| 104 |
A11=gamar*a;
|
| 105 |
|
| 106 |
return ;
|
| 107 |
}
|
| 108 |
|
| 109 |
/******************************************************************************************************************/
|
| 110 |
/******************************************************************************************************************/
|
| 111 |
//********************* ***************************************************************/
|
| 112 |
//********************* COORDINATE SYSTEMS ***************************************************************/
|
| 113 |
//********************* ***************************************************************/
|
| 114 |
//*****************************************************************************************************************/
|
| 115 |
//*****************************************************************************************************************/
|
| 116 |
//
|
| 117 |
// ZISK
|
| 118 |
// +
|
| 119 |
// / \ YOSK ZOSK (Directed by Radius)
|
| 120 |
// | _ _.
|
| 121 |
// | |\ /|
|
| 122 |
// | \ /
|
| 123 |
// | \ /
|
| 124 |
// |.__..__ \ /
|
| 125 |
// Orbit _._.***| **.\/_ XOSK (Directed by velocity)
|
| 126 |
// .* | (X0,Y0,Z0) **--.___\
|
| 127 |
// _** | / *. /
|
| 128 |
// .* | * *
|
| 129 |
// * ..****|***.. / R *
|
| 130 |
// .* | .*.
|
| 131 |
// .* | / *.
|
| 132 |
// * EARTH | / * YISK
|
| 133 |
// * | /_ _ _*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _\
|
| 134 |
// * / * /
|
| 135 |
// * / .*
|
| 136 |
// *. / .*
|
| 137 |
// **/*******
|
| 138 |
// /
|
| 139 |
// /
|
| 140 |
// /
|
| 141 |
// /
|
| 142 |
// /
|
| 143 |
// /
|
| 144 |
// |/
|
| 145 |
// *--
|
| 146 |
// XISK
|
| 147 |
//
|
| 148 |
//****************************************************************************************************/
|
| 149 |
//****************************************************************************************************/
|
| 150 |
|
| 151 |
//void OrbitalInfo::coefplane(Double_t x1,Double_t y1,Double_t z1,Double_t x2, Double_t y2, Double_t z2){
|
| 152 |
// k1 = ((z1/y1)*((x2*y1 - x1*y2)/(z2*y1 - z1*y2)) - x1/y1);
|
| 153 |
// k2 = (x1*y2 - x2*y1)/(z2*y1 - z1*y2);
|
| 154 |
// }
|
| 155 |
|
| 156 |
//Double_t OrbitalInfo::AngBetAxes(Double_t x1,Double_t y1,Double_t z1,Double_t x2, Double_t y2, Double_t z2){
|
| 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)));
|
| 158 |
// }
|
| 159 |
|
| 160 |
//Double_t OrbitalInfo::ValueT(Double_t x1, Double_t y1, Double_t z1, Double_t n1, Double_t n2){
|
| 161 |
// return -(x1+n1*y1+n2*z1)/(1+pow(n1,2)+pow(n2,2));
|
| 162 |
// }
|
| 163 |
|
| 164 |
//Double_t OrbitalInfo::AngBetPlan(Double_t x1, Double_t y1, Double_t z1, Double_t n1, Double_t n2){
|
| 165 |
// 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))));
|
| 166 |
// }
|
| 167 |
|
| 168 |
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){
|
| 169 |
|
| 170 |
double_t a = 360/(2*TMath::Pi());
|
| 171 |
|
| 172 |
// Points on three axes of Resurs' coordinate system (RCS)
|
| 173 |
Int_t XRCS[3]; Int_t YRCS[3]; Int_t ZRCS[3];
|
| 174 |
|
| 175 |
// Angles between our Axes(RCS) and planes of Orbital Coordinate System (OCS);
|
| 176 |
// Double_t AboAa0ZX[3];
|
| 177 |
// Double_t AboAa0XY[3];
|
| 178 |
// Double_t AboAa0YZ[3];
|
| 179 |
|
| 180 |
// Angles between our Axes(RCS) and Axes of OCS
|
| 181 |
// Double_t AboA0X[3];
|
| 182 |
// Double_t AboA0Y[3];
|
| 183 |
// Double_t AboA0Z[3];
|
| 184 |
|
| 185 |
//Angles between Proection of our axes on every plane of OCS and axes of it plane.
|
| 186 |
// Double_t AbPoAaAoP0ZX[3];
|
| 187 |
// Double_t AbPoAaAoP0XY[3];
|
| 188 |
// Double_t AbPoAaAoP0YZ[3];
|
| 189 |
|
| 190 |
XRCS[0] = 1; YRCS[0] = 0; ZRCS[0] = 0; // Points on X-axis RCS.
|
| 191 |
XRCS[1] = 0; YRCS[1] = 1; ZRCS[1] = 0; // Points on Y-axis RCS.
|
| 192 |
XRCS[2] = 0; YRCS[2] = 0; ZRCS[2] = 1; // Points on Z-axis
|
| 193 |
|
| 194 |
// Transition matrix RCS -> Inertial Coordinate System (ICS)
|
| 195 |
TMatrixD Bij(3,3);
|
| 196 |
Bij(0,0) = cos(tetar)*cos(ksir)-sin(tetar)*sin(gamar)*sin(ksir);
|
| 197 |
Bij(0,1) = -sin(tetar)*cos(gamar);
|
| 198 |
Bij(0,2) = cos(tetar)*sin(ksir)+sin(tetar)*sin(gamar)*cos(ksir);
|
| 199 |
Bij(1,0) = sin(tetar)*cos(ksir)+cos(tetar)*sin(gamar)*sin(ksir);
|
| 200 |
Bij(1,1) = cos(tetar)*cos(gamar);
|
| 201 |
Bij(1,2) = sin(tetar)*sin(ksir)-cos(tetar)*sin(gamar)*cos(ksir);
|
| 202 |
Bij(2,0) = -sin(ksir)*cos(gamar);
|
| 203 |
Bij(2,1) = sin(gamar);
|
| 204 |
Bij(2,2) = cos(ksir)*cos(gamar);
|
| 205 |
|
| 206 |
//********************************************************************************************/
|
| 207 |
//*********************** OTHER METHOD OF GETING COORDINATE IN OCS****************************/
|
| 208 |
//********************************************************************************************/
|
| 209 |
|
| 210 |
TMatrixD Pij(3,3);
|
| 211 |
Pij(0,0) = pow(q0,2)+pow(q1,2)-pow(q2,2)-pow(q3,2);
|
| 212 |
Pij(0,1) = 2*(q1*q2+q0*q3);
|
| 213 |
Pij(0,2) = 2*(q1*q3-q0*q2);
|
| 214 |
Pij(1,0) = 2*(q1*q2-q0*q3);
|
| 215 |
Pij(1,1) = pow(q0,2)-pow(q1,2)+pow(q2,2)-pow(q3,2);
|
| 216 |
Pij(1,2) = 2*(q2*q3+q0*q1);
|
| 217 |
Pij(2,0) = 2*(q1*q3+q0*q2);
|
| 218 |
Pij(2,1) = 2*(q2*q3-q0*q1);
|
| 219 |
Pij(2,2) = pow(q0,2)-pow(q1,2)-pow(q2,2)+pow(q3,2);
|
| 220 |
|
| 221 |
TMatrixD Aij(3,3);
|
| 222 |
// Double_t AbPoAaAoP0ZX_2m[3]; Double_t AboAa0ZX_2m[3];
|
| 223 |
// Double_t AbPoAaAoP0XY_2m[3]; Double_t AboAa0XY_2m[3];
|
| 224 |
// Double_t AbPoAaAoP0YZ_2m[3]; Double_t AboAa0YZ_2m[3];
|
| 225 |
|
| 226 |
Double_t C1 = y0*Vz0 - z0*Vy0;
|
| 227 |
Double_t C2 = z0*Vx0 - x0*Vz0;
|
| 228 |
Double_t C3 = x0*Vy0 - y0*Vx0;
|
| 229 |
Double_t C = sqrt(pow(C1,2) + pow(C2,2) + pow(C3,2));
|
| 230 |
Double_t V0 = sqrt(pow(Vx0,2)+pow(Vy0,2) + pow(Vz0,2));
|
| 231 |
//cout<<"C1= "<<(Vy0*C3-Vz0*C2)/(V0*C)<<", C2= "<<(Vz0*C1-Vx0*C3)/(V0*C)<<", C3="<<(Vx0*C2-Vy0*C1)/(V0*C)<<"\n";
|
| 232 |
Double_t R0 = sqrt(pow(x0,2)+pow(y0,2) + pow(z0,2));
|
| 233 |
Aij(0,0) = /*(C2*z0-C3*y0)/(C*R0);/*/Vx0/V0;
|
| 234 |
Aij(0,1) = C1/C;
|
| 235 |
Aij(0,2) = /*x0/R0;/*/(Vy0*C3-Vz0*C2)/(V0*C);
|
| 236 |
Aij(1,0) = /*(C3*x0-C1*z0)/(C*R0);/*/Vy0/V0;
|
| 237 |
Aij(1,1) = C2/C;
|
| 238 |
Aij(1,2) = /*y0/R0;/*/(Vz0*C1-Vx0*C3)/(V0*C);
|
| 239 |
Aij(2,0) = /*(C1*y0-C2*x0)/(C*R0);/*/Vz0/V0;
|
| 240 |
Aij(2,1) = C3/C;
|
| 241 |
Aij(2,2) = /*x0/R0;/*/(Vx0*C2-Vy0*C1)/(V0*C);
|
| 242 |
Aij.Invert();
|
| 243 |
// Double_t Xnew = Aij(0,0)*(X-x0)+Aij(0,1)*(Y-y0)+Aij(0,2)*(Z-z0);
|
| 244 |
// Double_t Ynew = Aij(1,0)*(X-x0)+Aij(1,1)*(Y-y0)+Aij(1,2)*(Z-z0);
|
| 245 |
// Double_t Znew = Aij(2,0)*(X-x0)+Aij(2,1)*(Y-y0)+Aij(2,2)*(Z-z0);
|
| 246 |
|
| 247 |
/*********************************************************************************************/
|
| 248 |
|
| 249 |
Double_t Azim = atan(R0*C3/(y0*C1-x0*C2));
|
| 250 |
Double_t Sa = sin(Azim); Double_t Ca = cos(Azim);
|
| 251 |
Double_t R1 = sqrt(pow(x0,2)+pow(y0,2));
|
| 252 |
Double_t Sb = z0/R0; Double_t Cb = R1/R0;
|
| 253 |
Double_t Sl = y0/R1; Double_t Cl = x0/R1;
|
| 254 |
|
| 255 |
TMatrixD Tij(3,3);
|
| 256 |
Tij(0,0) = -Cl*Sb*Ca-Sa*Sl;
|
| 257 |
Tij(0,1) = Sa*Cl-Ca*Sl*Sb;
|
| 258 |
Tij(0,2) = Ca*Cb;
|
| 259 |
Tij(1,0) = Ca*Sl-Sa*Sb*Cl;
|
| 260 |
Tij(1,1) = -Sa*Sl*Sb-Ca*Cl;
|
| 261 |
Tij(1,2) = Sa*Cb;
|
| 262 |
Tij(2,0) = Cb*Cl;
|
| 263 |
Tij(2,1) = Cb*Sl;
|
| 264 |
Tij(2,2) = Sb;
|
| 265 |
//cout<<"Tij\n";
|
| 266 |
//cout<<Tij(0,0)<<" "<<Tij(0,1)<<" "<<Tij(0,2)<<"\n";
|
| 267 |
//cout<<Tij(1,0)<<" "<<Tij(1,1)<<" "<<Tij(1,2)<<"\n";
|
| 268 |
//cout<<Tij(2,0)<<" "<<Tij(2,1)<<" "<<Tij(2,2)<<"\n";
|
| 269 |
//cout<<"Aij\n";
|
| 270 |
|
| 271 |
|
| 272 |
//TMatrixD Mij = new TMatrixD(Otestij,TMatrixD::kMult,Oij);
|
| 273 |
//Mij=Pij*Bij;
|
| 274 |
//Mij=Otestij*Oij;
|
| 275 |
//Mij*=Tij;
|
| 276 |
|
| 277 |
//cout<<Mij(0,0)<<" "<<Mij(0,1)<<" "<<Mij(0,2)<<"\n";
|
| 278 |
//cout<<Mij(1,0)<<" "<<Mij(1,1)<<" "<<Mij(1,2)<<"\n";
|
| 279 |
//cout<<Mij(2,0)<<" "<<Mij(2,1)<<" "<<Mij(2,2)<<"\n";
|
| 280 |
// Generaly idea is to Get orientation of Satellite as angles between RCS axes and all axes and planes of OCS
|
| 281 |
// We will get equations of RCS axes in ICS
|
| 282 |
|
| 283 |
// equation of line in space is (X-X0)/(X1-X0)=(Y-Y0)/(Y1-Y0)=(Z-Z0)/(Z1-Z0), where
|
| 284 |
// (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
|
| 285 |
// for us this equation is X/X1=Y/Y1=Z/Z1;
|
| 286 |
|
| 287 |
// We need in equation of line in spase for OCS also. For it take next points (Vx0,Vy0,Vz0) on X-axis
|
| 288 |
// and (x0,y0,z0) on Z-axis.
|
| 289 |
// Double_t XonX = Vx0; Double_t YonX = Vy0; Double_t ZonX = Vz0;
|
| 290 |
// Double_t XonZ = x0; Double_t YonZ = y0; Double_t ZonZ = z0;
|
| 291 |
//after this we have equations for Z- and X axis OCS it's
|
| 292 |
// X/XonX=Y/YonX=Z/ZonX for X-axis and X/XonZ=Y/YonZ=Z/ZonZ for Z-axis
|
| 293 |
|
| 294 |
// Next we need in equation of plane 0xz of OCS: Generaly equation is Ax+By+Cz+D=0;
|
| 295 |
// But all our plan pass through (0,0,0) and D=0 then we can write equation in naxt kind:
|
| 296 |
// x+(B/A)y+(C/A)z=0; => x+k1*y+k2*z=0;
|
| 297 |
// Double_t k1y;
|
| 298 |
// Double_t k2y;
|
| 299 |
//cout<<YonX<<" "<<ZonX*YonZ<<" "<<ZonZ*YonX<<"\n";
|
| 300 |
// if ((YonZ != 0) && (ZonX*YonZ != ZonZ*YonX)){
|
| 301 |
// coefplane(XonX,YonX,ZonX,XonZ,YonZ,ZonZ);
|
| 302 |
//coefplane(1,0.00001,0.00001,0,0,1);
|
| 303 |
// k1y = k1; k2y = k2;
|
| 304 |
// } else {k1y = 1; k2y = YonX/ZonX; cout<<"ELSE";}
|
| 305 |
//cout<<"P1= "<<(Vx0+k1y*Vy0+k2y*Vz0)<<"\n";
|
| 306 |
//cout<<"P2= "<<(x0+k1y*y0+k2y*z0)<<"\n";
|
| 307 |
//cout<<"P3= "<<(Vx0+k1y*Vy0+k2y*Vz0)<<"\n";
|
| 308 |
//cout<<"k1y= "<<k1y<<", k2y= "<<k2y<<"\n";
|
| 309 |
// int uchu;
|
| 310 |
// cin>>uchu;
|
| 311 |
|
| 312 |
// Next we must find equation of Y-axis of OCS. For it we must find equation of line passing through
|
| 313 |
// point (0,0,0) perpendicularly by 0ZX plane of OCS
|
| 314 |
// generaly equation is:
|
| 315 |
// x = x0 + At;
|
| 316 |
// y = y0 + Bt;
|
| 317 |
// z = z0 + Ct;
|
| 318 |
// But we have point (x0,y0,z0) is (0,0,0) and other plane equation. For us it's:
|
| 319 |
// x = t;
|
| 320 |
// y = (B/A)*t => y = (B/A)*x; or x/x1=y/y1=z/z1 where
|
| 321 |
// z = (C/A)*t z = (C/A)*x; y1=B/A,z1=C/A and x1 we must find
|
| 322 |
|
| 323 |
// if ((YonX<0 && ZonZ>0)||(YonX>0 && ZonZ<0)) XonY = 1;
|
| 324 |
// if ((YonX>0 && ZonZ>0)||(YonX>0 && ZonZ<0)) XonY = 1;
|
| 325 |
// Double_t XonY = 1; Double_t YonY = -k1; Double_t ZonY = k2;
|
| 326 |
|
| 327 |
// coefficients for equations of 0XY plane of OCS.
|
| 328 |
// coefplane(XonX,YonX,ZonX,XonY,YonY,ZonY);
|
| 329 |
// Double_t k1XY = k1; Double_t k2XY = k2;
|
| 330 |
//cout<<"P3= "<<(XonY+k1XY*YonY+k2XY*ZonY)<<"\n";
|
| 331 |
//cout<<"P3= "<<(XonX+k1XY*YonX+k2XY*ZonX)<<"\n";
|
| 332 |
//cout<<"k1XY= "<<k1XY<<", k2XY= "<<k2XY<<"\n";
|
| 333 |
// coefficients for equations of 0XY plane of OCS.
|
| 334 |
// coefplane(XonY,YonY,ZonY,XonZ,YonZ,ZonZ);
|
| 335 |
// Double_t k1YZ = k1; Double_t k2YZ = k2;
|
| 336 |
//cout<<"P4= "<<(XonY+k1YZ*YonY+k2YZ*ZonY)<<"\n";
|
| 337 |
//cout<<"P4= "<<(XonZ+k1YZ*YonZ+k2YZ*ZonZ)<<"\n";
|
| 338 |
//cout<<"k1YZ= "<<k1YZ<<", k2YZ= "<<k2YZ<<"\n";
|
| 339 |
|
| 340 |
// TMatrixD Gij(3,3);
|
| 341 |
Pij.Invert();
|
| 342 |
// Gij=Pij*Aij;
|
| 343 |
//Gij.Invert();
|
| 344 |
//XXRCS = Gij(0,0); XYRCS = Gij(0,1); XZRCS = Gij(2,0);
|
| 345 |
//YXRCS = Gij(1,0); YYRCS = Gij(1,1); YZRCS = Gij(2,1);
|
| 346 |
//ZXRCS = Gij(2,0); ZYRCS = Gij(1,2); ZZRCS = Gij(2,2);
|
| 347 |
|
| 348 |
//cout<<"XXRCS= "<<XXRCS<<", YXRCS= "<<YXRCS<<", ZXRCS= "<<ZXRCS<<"\n";
|
| 349 |
//cout<<"XYRCS= "<<XYRCS<<", YYRCS= "<<YYRCS<<", ZYRCS= "<<ZYRCS<<"\n";
|
| 350 |
//cout<<"XZRCS= "<<XZRCS<<", YZRCS= "<<YZRCS<<", ZZRCS= "<<ZZRCS<<"\n";
|
| 351 |
//int yuip;
|
| 352 |
//cin>>yuip;
|
| 353 |
|
| 354 |
for (Int_t i = 0; i<3; i++) {
|
| 355 |
// Values of points on axes of RCS in ICS
|
| 356 |
Double_t XICS = Pij(0,0)*XRCS[i] + Pij(0,1)*YRCS[i] + Pij(0,2)*ZRCS[i];// + x0;
|
| 357 |
Double_t YICS = Pij(1,0)*XRCS[i] + Pij(1,1)*YRCS[i] + Pij(1,2)*ZRCS[i];// + y0;
|
| 358 |
Double_t ZICS = Pij(2,0)*XRCS[i] + Pij(2,1)*YRCS[i] + Pij(2,2)*ZRCS[i];// + z0;
|
| 359 |
//cout<<"XICS= "<<XICS<<", YICS= "<<YICS<<", ZICS= "<<ZICS<<"\n";
|
| 360 |
//cout<<"XICS= "<<XICS<<", YICS= "<<YICS<<", ZICS= "<<ZICS<<"\n";
|
| 361 |
//int oiu;
|
| 362 |
//cin>>oiu;
|
| 363 |
|
| 364 |
// Angles between our Axis and Z,Y,X-axes of OCS
|
| 365 |
// AboA0Z[i] = AngBetAxes(XICS,YICS,ZICS,XonZ,YonZ,ZonZ);
|
| 366 |
// AboA0Y[i] = AngBetAxes(XICS,YICS,ZICS,XonY,YonY,ZonY);
|
| 367 |
// AboA0X[i] = AngBetAxes(XICS,YICS,ZICS,XonX,YonX,ZonX);
|
| 368 |
|
| 369 |
//Find coordinate of our point in OCS
|
| 370 |
// Double_t XOCS;
|
| 371 |
// Double_t YOCS;
|
| 372 |
// Double_t ZOCS;
|
| 373 |
// Double_t T = ValueT(XICS,YICS,ZICS,k1y,k2y);
|
| 374 |
// Double_t XonXZ = XICS + T;
|
| 375 |
// Double_t YonXZ = YICS + k1y*T;
|
| 376 |
// Double_t ZonXZ = ZICS + k2y*T;
|
| 377 |
// Double_t R = T*sqrt(1+pow(k1y,2)+pow(k2y,2));
|
| 378 |
// YOCS = R;
|
| 379 |
//cout<<"CHECK= "<<XonXZ+k1y*YonXZ+k2y*ZonXZ<<"\n";
|
| 380 |
// T = ValueT(XICS,YICS,ZICS,k1XY,k2XY);
|
| 381 |
// Double_t XonXY = XICS + T;
|
| 382 |
// Double_t YonXY = YICS + k1XY*T;
|
| 383 |
// Double_t ZonXY = ZICS + k2XY*T;
|
| 384 |
// R = T*sqrt(1+pow(k1XY,2)+pow(k2XY,2));
|
| 385 |
// ZOCS = R;
|
| 386 |
//cout<<"CHECK= "<<XonXY+k1XY*YonXY+k2XY*ZonXY<<"\n";
|
| 387 |
// T = ValueT(XICS,YICS,ZICS,k1YZ,k2YZ);
|
| 388 |
// Double_t XonYZ = XICS + T;
|
| 389 |
// Double_t YonYZ = YICS + k1YZ*T;
|
| 390 |
// Double_t ZonYZ = ZICS + k2YZ*T;
|
| 391 |
// R = T*sqrt(1+pow(k1YZ,2)+pow(k2YZ,2));
|
| 392 |
// XOCS = R;
|
| 393 |
//cout<<"CHECK= "<<XonYZ+k1YZ*YonYZ+k2YZ*ZonYZ<<"\n";
|
| 394 |
//cout<<"XOCS= "<<XOCS<<", YOCS= "<<YOCS<<", ZOCS="<<ZOCS<<"\n";
|
| 395 |
|
| 396 |
//Double_t AbPoAaAoP0ZX_2m[3]; Double_t AboAa0ZX_2m[3];
|
| 397 |
//Double_t AbPoAaAoP0XY_2m[3]; Double_t AboAa0XY_2m[3];
|
| 398 |
//Double_t AbPoAaAoP0YZ_2m[3]; Double_t AboAa0YZ_2m[3];
|
| 399 |
|
| 400 |
/* C1 = YICS*Vz0 - ZICS*Vy0;
|
| 401 |
C2 = ZICS*Vx0 - XICS*Vz0;
|
| 402 |
C3 = XICS*Vy0 - YICS*Vx0;
|
| 403 |
C = sqrt(pow(C1,2) + pow(C2,2) + pow(C3,2));
|
| 404 |
V0 = sqrt(pow(Vx0,2)+pow(Vy0,2) + pow(Vz0,2));
|
| 405 |
Aij(0,0) = Vx0/V0;
|
| 406 |
Aij(0,1) = C1/C;
|
| 407 |
Aij(0,2) = (Vy0*C3-Vz0*C2)/(V0*C);
|
| 408 |
Aij(1,0) = Vy0/V0;
|
| 409 |
Aij(1,1) = C2/C;
|
| 410 |
Aij(1,2) = (Vz0*C1-Vx0*C3)/(V0*C);
|
| 411 |
Aij(2,0) = Vz0/V0;
|
| 412 |
Aij(2,1) = C3/C;
|
| 413 |
Aij(2,2) = (Vx0*C2-Vy0*C1)/(V0*C);
|
| 414 |
Aij.Invert();
|
| 415 |
*/
|
| 416 |
//2th method of getting XOCS,YOCS,ZOCS
|
| 417 |
Double_t XOCS_2m = Aij(0,0)*(XICS)+Aij(0,1)*(YICS)+Aij(0,2)*(ZICS);
|
| 418 |
Double_t YOCS_2m = Aij(1,0)*(XICS)+Aij(1,1)*(YICS)+Aij(1,2)*(ZICS);
|
| 419 |
Double_t ZOCS_2m = Aij(2,0)*(XICS)+Aij(2,1)*(YICS)+Aij(2,2)*(ZICS);
|
| 420 |
|
| 421 |
if (i == 0) {XXRCS = XOCS_2m; YXRCS = YOCS_2m; ZXRCS = ZOCS_2m;}
|
| 422 |
if (i == 1) {XYRCS = XOCS_2m; YYRCS = YOCS_2m; ZYRCS = ZOCS_2m;}
|
| 423 |
if (i == 2) {XZRCS = XOCS_2m; YZRCS = YOCS_2m; ZZRCS = ZOCS_2m;}
|
| 424 |
|
| 425 |
//cout<<"XOCS_2m= "<<XOCS_2m<<", YOCS_2m= "<<YOCS_2m<<", ZOCS= "<<ZOCS_2m<<"\n";
|
| 426 |
//int alsdj;
|
| 427 |
//cin>>alsdj;
|
| 428 |
|
| 429 |
//Find Angles between RCS-axes and OCS-planes;
|
| 430 |
// AboAa0ZX[i] = AngBetPlan(XICS,YICS,ZICS,k1y,k2y);
|
| 431 |
// AboAa0XY[i] = AngBetPlan(XICS,YICS,ZICS,k1XY,k2XY);
|
| 432 |
// AboAa0YZ[i] = AngBetPlan(XICS,YICS,ZICS,k1YZ,k2YZ);
|
| 433 |
//AbPoAaAoP0ZX[i] = atan(ZOCS/XOCS); AbPoAaAoP0ZX_2m[i] = atan(ZOCS_2m/XOCS_2m);
|
| 434 |
//AbPoAaAoP0XY[i] = atan(XOCS/YOCS); AbPoAaAoP0XY_2m[i] = atan(YOCS_2m/XOCS_2m);
|
| 435 |
//AbPoAaAoP0YZ[i] = atan(ZOCS/YOCS); AbPoAaAoP0YZ_2m[i] = atan(ZOCS_2m/YOCS_2m);
|
| 436 |
|
| 437 |
// if (XOCS_2m>0) AbPoAaAoP0ZX_2m[i] = atan(ZOCS_2m/XOCS_2m);
|
| 438 |
// if ((XOCS_2m<0)&&(ZOCS_2m>=0)) AbPoAaAoP0ZX_2m[i] = 180/a + atan(ZOCS_2m/XOCS_2m);
|
| 439 |
// if ((XOCS_2m<0)&&(ZOCS_2m<0)) AbPoAaAoP0ZX_2m[i] = atan(ZOCS_2m/XOCS_2m) - 180/a;
|
| 440 |
// if ((XOCS_2m=0)&&(ZOCS_2m>0)) AbPoAaAoP0ZX_2m[i] = 90/a;
|
| 441 |
// if ((XOCS_2m=0)&&(ZOCS_2m<0)) AbPoAaAoP0ZX_2m[i] = -90/a;
|
| 442 |
// if ((XOCS_2m=0)&&(ZOCS_2m=0)) AbPoAaAoP0ZX_2m[i] = 0;
|
| 443 |
|
| 444 |
// if (XOCS_2m>0) AbPoAaAoP0XY_2m[i] = atan(YOCS_2m/XOCS_2m);
|
| 445 |
// if ((XOCS_2m<0)&&(YOCS_2m>=0)) AbPoAaAoP0XY_2m[i] = 180/a + atan(YOCS_2m/XOCS_2m);
|
| 446 |
// if ((XOCS_2m<0)&&(YOCS_2m<0)) AbPoAaAoP0XY_2m[i] = atan(YOCS_2m/XOCS_2m) - 180/a;
|
| 447 |
// if ((XOCS_2m=0)&&(YOCS_2m>0)) AbPoAaAoP0XY_2m[i] = 90/a;
|
| 448 |
// if ((XOCS_2m=0)&&(YOCS_2m<0)) AbPoAaAoP0XY_2m[i] = -90/a;
|
| 449 |
// if ((XOCS_2m=0)&&(YOCS_2m=0)) AbPoAaAoP0XY_2m[i] = 0;
|
| 450 |
|
| 451 |
// if (YOCS_2m>0) AbPoAaAoP0YZ_2m[i] = atan(ZOCS_2m/YOCS_2m);
|
| 452 |
// if ((YOCS_2m<0)&&(ZOCS_2m>=0)) AbPoAaAoP0YZ_2m[i] = 180/a + atan(ZOCS_2m/YOCS_2m);
|
| 453 |
// if ((YOCS_2m<0)&&(ZOCS_2m<0)) AbPoAaAoP0YZ_2m[i] = atan(ZOCS_2m/YOCS_2m) - 180/a;
|
| 454 |
// if ((YOCS_2m=0)&&(ZOCS_2m>0)) AbPoAaAoP0YZ_2m[i] = 90/a;
|
| 455 |
// if ((YOCS_2m=0)&&(ZOCS_2m<0)) AbPoAaAoP0YZ_2m[i] = -90/a;
|
| 456 |
// if ((YOCS_2m=0)&&(ZOCS_2m=0)) AbPoAaAoP0YZ_2m[i] = 0;
|
| 457 |
|
| 458 |
//if (i==0) cout<<"AbPoAaAoP0ZX_2m[i]"<<AbPoAaAoP0ZX_2m[i]<<"\n";
|
| 459 |
//cout<<"XOCS/ZOCS= "<<XOCS_2m/ZOCS_2m<<"\n";
|
| 460 |
//cout<<"Atan= "<<AbPoAaAoP0ZX_2m[i]<<"\n";
|
| 461 |
//cout<<"atan= "<<a*atan(0.2);
|
| 462 |
//int GJH;
|
| 463 |
//cin>>GJH;
|
| 464 |
|
| 465 |
}
|
| 466 |
Double_t u13 = XYRCS/*Gij(0,2)*/; Double_t u33 = ZYRCS/*Gij(2,2)*/;
|
| 467 |
Double_t u23 = YYRCS/*Gij(1,2)*/; Double_t u21 = YZRCS/*Gij(1,0)*/;
|
| 468 |
Double_t u22 = YXRCS/*Gij(1,1)*/;
|
| 469 |
Tangazh = a*atan(-u13/u33);
|
| 470 |
//cout<<"u13= "<<u13<<", u33= "<<u33<<"\n";
|
| 471 |
Kren = a*atan(-u23/sqrt(1 - pow(u23,2)));
|
| 472 |
//Ryskanie = a*atan(u21/u22);
|
| 473 |
|
| 474 |
if (u22>0) Ryskanie = a*atan(u21/u22);
|
| 475 |
if ((u22<0)&&(u21>=0)) Ryskanie = 180 + a*atan(u21/u22);
|
| 476 |
if ((u22<0)&&(u21<0)) Ryskanie = a*atan(u21/u22) - 180;
|
| 477 |
if ((u22=0)&&(u21>0)) Ryskanie = 90;
|
| 478 |
if ((u22=0)&&(u21<0)) Ryskanie = -90;
|
| 479 |
if ((u22=0)&&(u21=0)) Ryskanie = 0;
|
| 480 |
|
| 481 |
// AXrXo = AboA0X[0]*a; AXrZXo = AboAa0ZX[0]*a; ApXrZXoZ = AbPoAaAoP0ZX[0]*a;
|
| 482 |
// AXrYo = AboA0Y[0]*a; AXrXYo = AboAa0XY[0]*a; ApXrXYoZ = AbPoAaAoP0XY[0]*a;
|
| 483 |
// AXrZo = AboA0Z[0]*a; AXrYZo = AboAa0YZ[0]*a; ApXrYZoZ = AbPoAaAoP0YZ[0]*a;
|
| 484 |
// AYrXo = AboA0X[1]*a; AYrZXo = AboAa0ZX[1]*a; ApYrZXoZ = AbPoAaAoP0ZX[1]*a;
|
| 485 |
// AYrYo = AboA0Y[1]*a; AYrXYo = AboAa0XY[1]*a; ApYrXYoZ = AbPoAaAoP0XY[1]*a;
|
| 486 |
// AYrZo = AboA0Z[1]*a; AYrYZo = AboAa0YZ[1]*a; ApYrYZoZ = AbPoAaAoP0YZ[1]*a;
|
| 487 |
// AZrXo = AboA0X[2]*a; AZrZXo = AboAa0ZX[2]*a; ApZrZXoZ = AbPoAaAoP0ZX[2]*a;
|
| 488 |
// AZrYo = AboA0Y[2]*a; AZrXYo = AboAa0XY[2]*a; ApZrXYoZ = AbPoAaAoP0XY[2]*a;
|
| 489 |
// AZrZo = AboA0Z[2]*a; AZrYZo = AboAa0YZ[2]*a; ApZrYZoZ = AbPoAaAoP0YZ[2]*a;
|
| 490 |
|
| 491 |
// AXrZXo_2m = AboAa0ZX_2m[0]*a; ApXrZXoZ_2m = AbPoAaAoP0ZX_2m[0]*a;
|
| 492 |
// AXrXYo_2m = AboAa0XY_2m[0]*a; ApXrXYoZ_2m = AbPoAaAoP0XY_2m[0]*a;
|
| 493 |
// AXrYZo_2m = AboAa0YZ_2m[0]*a; ApXrYZoZ_2m = AbPoAaAoP0YZ_2m[0]*a;
|
| 494 |
// AYrZXo_2m = AboAa0ZX_2m[1]*a; ApYrZXoZ_2m = AbPoAaAoP0ZX_2m[1]*a;
|
| 495 |
// AYrXYo_2m = AboAa0XY_2m[1]*a; ApYrXYoZ_2m = AbPoAaAoP0XY_2m[1]*a;
|
| 496 |
// AYrYZo_2m = AboAa0YZ_2m[1]*a; ApYrYZoZ_2m = AbPoAaAoP0YZ_2m[1]*a;
|
| 497 |
// AZrZXo_2m = AboAa0ZX_2m[2]*a; ApZrZXoZ_2m = AbPoAaAoP0ZX_2m[2]*a;
|
| 498 |
// AZrXYo_2m = AboAa0XY_2m[2]*a; ApZrXYoZ_2m = AbPoAaAoP0XY_2m[2]*a;
|
| 499 |
// AZrYZo_2m = AboAa0YZ_2m[2]*a; ApZrYZoZ_2m = AbPoAaAoP0YZ_2m[2]*a;
|
| 500 |
|
| 501 |
/*
|
| 502 |
//Int_t Y=2;Int_t X=2;Int_t Z=4;Int_t Vx=5;Int_t Vz=9;Int_t Vy=5;
|
| 503 |
|
| 504 |
Double_t X[2]; Double_t Y[2]; Double_t Z[2]; Double_t Vx[2]; Double_t Vy[2]; Double_t Vz[2];
|
| 505 |
|
| 506 |
TMatrixD Aij(3,3);
|
| 507 |
TMatrixD Bij(3,3);
|
| 508 |
|
| 509 |
Bij(0,0) = cos(tetar)*cos(ksir)-sin(tetar)*sin(gamar)*sin(ksir);
|
| 510 |
Bij(0,1) = -sin(tetar)*cos(gamar);
|
| 511 |
Bij(0,2) = cos(tetar)*sin(ksir)+sin(tetar)*sin(gamar)*cos(ksir);
|
| 512 |
Bij(1,0) = sin(tetar)*cos(ksir)+cos(tetar)*sin(gamar)*sin(ksir);
|
| 513 |
Bij(1,1) = cos(tetar)*cos(gamar);
|
| 514 |
Bij(1,2) = sin(tetar)*sin(ksir)-cos(tetar)*sin(gamar)*cos(ksir);
|
| 515 |
Bij(2,0) = -sin(ksir)*cos(gamar);
|
| 516 |
Bij(2,1) = sin(gamar);
|
| 517 |
Bij(2,2) = cos(ksir)*cos(gamar);
|
| 518 |
|
| 519 |
Double_t C1 = Y[0]*Vz[0] - Z[0]*Vy[0];
|
| 520 |
Double_t C2 = Z[0]*Vx[0] - X[0]*Vz[0];
|
| 521 |
Double_t C3 = X[0]*Vy[0] - Y[0]*Vx[0];
|
| 522 |
Double_t C = sqrt(pow(C1,2) + pow(C2,2) + pow(C3,2));
|
| 523 |
Double_t V0 = sqrt(pow(Vx0,2)+pow(Vy0,2) + pow(Vz0,2));
|
| 524 |
Aij(0,0) = Vx0/V0;
|
| 525 |
Aij(0,1) = C1/C;
|
| 526 |
Aij(0,2) = (Vy0*C3-Vz0*C2)/(V0*C);
|
| 527 |
Aij(1,0) = Vy0/V0;
|
| 528 |
Aij(1,1) = C2/C;
|
| 529 |
Aij(1,2) = (Vz0*C1-Vx0*C3)/(V0*C);
|
| 530 |
Aij(2,0) = Vz0/V0;
|
| 531 |
Aij(2,1) = C3/C;
|
| 532 |
Aij(2,2) = (Vx0*C2-Vy0*C1)/(V0*C);
|
| 533 |
Aij.Invert();
|
| 534 |
Double_t Xnew = Aij(0,0)*(X-x0)+Aij(0,1)*(Y-y0)+Aij(0,2)*(Z-z0);
|
| 535 |
Double_t Ynew = Aij(1,0)*(X-x0)+Aij(1,1)*(Y-y0)+Aij(1,2)*(Z-z0);
|
| 536 |
Double_t Znew = Aij(2,0)*(X-x0)+Aij(2,1)*(Y-y0)+Aij(2,2)*(Z-z0);
|
| 537 |
*/
|
| 538 |
//A21 = NewTetar;
|
| 539 |
//A22 = NewGamar;
|
| 540 |
//A23 = NewKsir;
|
| 541 |
|
| 542 |
return ;
|
| 543 |
}
|
| 544 |
|
Parent Directory
| 
Revision Log