void NeutrinoEllipseCalculator::labSystemTransform()
{
//rotate Htilde to H
TMatrixD R(3,3);
R.Zero();
TMatrixD Rz=rotationMatrix(2,-lepton_.Phi());
TMatrixD Ry=rotationMatrix(1,0.5*M_PI-lepton_.Theta());
double bJetP[3]={bJet_.Px(),bJet_.Py(), bJet_.Pz()};
TMatrixD bJet_xyz(3,1,bJetP);
TMatrixD rM(Ry,TMatrixD::kMult,TMatrixD(Rz,TMatrixD::kMult,bJet_xyz));
double* rA=rM.GetMatrixArray();
double phi=-TMath::ATan2(rA[2],rA[1]);
TMatrixD Rx=rotationMatrix(0,phi);
R=TMatrixD(Rz,TMatrixD::kTransposeMult,TMatrixD(Ry,TMatrixD::kTransposeMult,Rx.T()));
H_=TMatrixD(R,TMatrixD::kMult,Htilde_);
//calculate Hperp
double Hvalues[9]={H_[0][0],H_[0][1],H_[0][2],H_[1][0],H_[1][1],H_[1][2],0,0,1};
TArrayD Harray(9,Hvalues);
Hperp_.SetMatrixArray(Harray.GetArray());
//calculate Nperp
TMatrixD HperpInv(Hperp_);
HperpInv.Invert();
TMatrixD U(3,3);
U.Zero();
U[0][0]=1;
U[1][1]=1;
U[2][2]=-1;
Nperp_=TMatrixD(HperpInv,TMatrixD::kTransposeMult,TMatrixD(U,TMatrixD::kMult,HperpInv));
}
void Crotation::print(const bool rd)
{
//update things
if ( rStatus & OLD_EULER ) updateEuler();
if ( rStatus & OLD_MATRIX ) updateMatrix();
if ( rStatus & OLD_QUATERNION ) updateQuaternion();
//print them !
std::cout << "euler(hpr): [" << ea[0].getAngle() << "," << ea[1].getAngle() << "," << ea[2].getAngle() << "]" << std::endl;
std::cout << "matrix: [" << rM(0,0) << "," << rM(0,1) << "," << rM(0,2)
<< " | " << rM(1,0) << "," << rM(1,1) << "," << rM(1,2)
<< " | " << rM(2,0) << "," << rM(2,1) << "," << rM(2,2) << "]" << std::endl;
std::cout << "quaternion: [" << qt.qq(0) << "," << qt.qq(1) << "," << qt.qq(2) << "," << qt.qq(3) << "]" << std::endl;
}
Transform Transform::RotateZ(float angle) {
float radianAngle = Radians(angle);
float cosAngle = cosf(radianAngle);
float sinAngle = sinf(radianAngle);
Matrix4x4 rM(cosAngle, -sinAngle, 0, 0,
sinAngle, cosAngle, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1);
return Transform(rM, Matrix4x4::Transpose(rM));
}
void Crotation::updateEuler()
{
dlib::matrix<double,3,1> lny, lnz, ys;
double aux;
double hh, pp, rr;
//first check if matrix is updated
if ( rStatus & OLD_MATRIX ) updateMatrix();
//check for gimbal lock situation (forward axis is pointing downwards or upwards)
if ( fabs(this->rM(2,0)-1) < EPSILON ) //forward axis pointing upwards
{
hh = this->ea[0].getAngle(); //keeps the old heading value
pp = -M_PI/2;//pitch
rr = atan2(-this->rM(0,1),this->rM(1,1))-hh;//left axis at xy plane marks roll
}
else if ( fabs(this->rM(2,0)+1) < EPSILON ) //forward axis pointing downwards
{
hh = this->ea[0].getAngle(); //keeps the old heading value
pp = M_PI/2;//pitch
rr = -atan2(-this->rM(0,1),this->rM(1,1))-hh;//left axis at xy plane marks roll
}
else
{
hh = atan2( rM(1,0) , rM(0,0) );//heading
aux = rM(0,0)*rM(0,0)+rM(1,0)*rM(1,0);
pp = -atan2( rM(2,0) , sqrt(aux) );//pitch
rr = atan2( rM(2,1)*aux-rM(2,0)*(rM(0,0)*rM(0,1)+rM(1,0)*rM(1,1)) , rM(0,0)*rM(1,1)-rM(1,0)*rM(0,1) ); //roll
}
//sets angles assuring range correctness
setEulerWithoutStatusUpdate(hh,pp,rr);
//Updates status by resetting OLD_EULER bit
rStatus &= ( 0xFFFF & ~(OLD_EULER) );
}
void Crotation::turnPitch(const double alpha, bool rd)
{
if ( rStatus & OLD_MATRIX ) updateMatrix();
rotateUaxis(alpha, rM(0,1), rM(1,1), rM(2,1), rd);
}
void Crotation::turnHeading(const double alpha, bool rd)
{
if ( rStatus & OLD_MATRIX ) updateMatrix();
rotateUaxis(alpha, rM(0,2), rM(1,2), rM(2,2), rd);
}
