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); }