//  $Id: BrRotMatrix.cxx,v 1.8 2002/04/16 13:22:40 hagel Exp $
//
//
 /*************************************************************************
 * Copyright(c) 1995-1998, The ROOT System, All rights reserved.         *
 * Authors: Rene Brun, Nenad Buncic, Valery Fine, Fons Rademakers.       *
 *                                                                       *
 * Permission to use, copy, modify and distribute this software and its  *
 * documentation for non-commercial purposes is hereby granted without   *
 * fee, provided that the above copyright notice appears in all copies   *
 * and that both the copyright notice and this permission notice appear  *
 * in the supporting documentation. The authors make no claims about the *
 * suitability of this software for any purpose.                         *
 * It is provided "as is" without express or implied warranty.           *
 *************************************************************************/

#include "TMath.h"

#include "BrRotMatrix.h"
#include <iomanip.h>

ClassImp(BrRotMatrix)

//______________________________________________________________________________
//
// Manages a detector rotation matrix. See BrVector3D, BrLine3D,..
//
// This class is a simplified version of the ROOT class TRotMatrix. See
// The ROOT documentation for additional details.See also the copyright notice
// in the source documentation.
//
// July 2000;
//   Added code to define using Euler angles (thanks to T.Tveter)
//
//______________________________________________________________________________
 BrRotMatrix::BrRotMatrix()
{
//*-*-*-*-*-*-*-*-*-*-*RotMatrix default constructor*-*-*-*-*-*-*-*-*-*-*-*
//*-*                  ============================

   //do nothing
}

BrRotMatrix& BrRotMatrix::operator= (const BrRotMatrix& r){
	// assignement operator.
	//
fTheta= r.fTheta;
fPhi  = r.fPhi;
fPsi  = r.fPsi;
for(int i=0;i<9;i++) fMatrix[i]=r.fMatrix[i];
SetReflection();
return *this;
}

BrRotMatrix operator*(BrRotMatrix& r1, BrRotMatrix& r2){
  //Multiplication operator

  BrRotMatrix newMat(&r2,&r1);
  return newMat;
}

 BrRotMatrix::BrRotMatrix( Double_t *matrix){
  //
  //
  double *ma=matrix;
  double *ml=fMatrix;
  for(int i=0; i<9;i++)
    (*ml++)=(*ma++);
  }

//______________________________________________________________________________
 BrRotMatrix::BrRotMatrix(Double_t theta, Double_t phi, Double_t psi)
{
//
// Construction of Rotation matrix by the 3 Euler angles theta, phi and psi
//

   const Double_t degrad = 0.0174532925199432958;

   fTheta  = theta;
   fPhi    = phi;
   fPsi    = psi;
   fType   = 0;

/*Original by Trine (kept for moment to make sure formulas below are correct
   fMatrix[0] = TMath::Cos(phi*degrad)*TMath::Cos(theta*degrad)*TMath::Cos(psi*degrad) - TMath::Sin(phi*degrad)*TMath::Sin(psi*degrad);
   fMatrix[1] = TMath::Sin(phi*degrad)*TMath::Cos(theta*degrad)*TMath::Cos(psi*degrad) + TMath::Cos(phi*degrad)*TMath::Sin(psi*degrad);
   fMatrix[2] = -1.*TMath::Sin(theta*degrad)*TMath::Cos(psi*degrad);
   fMatrix[3] = -1.*TMath::Cos(phi*degrad)*TMath::Cos(theta*degrad)*TMath::Sin(psi*degrad) - TMath::Sin(phi*degrad)*TMath::Cos(psi*degrad);
   fMatrix[4] = TMath::Cos(phi*degrad)*TMath::Cos(psi*degrad) - TMath::Sin(phi*degrad)*TMath::Cos(theta*degrad)*TMath::Sin(psi*degrad);
   fMatrix[5] = TMath::Sin(theta*degrad)*TMath::Sin(psi*degrad);
   fMatrix[6] = TMath::Cos(phi*degrad)*TMath::Sin(theta*degrad);
   fMatrix[7] = TMath::Sin(phi*degrad)*TMath::Sin(theta*degrad);
   fMatrix[8] = TMath::Cos(theta*degrad);
*/

   Double_t cosTheta = TMath::Cos(fTheta*degrad);
   Double_t sinTheta = TMath::Sin(fTheta*degrad);
   Double_t cosPhi   = TMath::Cos(fPhi*degrad);
   Double_t sinPhi   = TMath::Sin(fPhi*degrad);
   Double_t cosPsi   = TMath::Cos(fPsi*degrad);
   Double_t sinPsi   = TMath::Sin(fPsi*degrad);

   fMatrix[0] =  cosPhi*cosTheta*cosPsi - sinPhi*sinPsi;
   fMatrix[1] =  sinPhi*cosTheta*cosPsi + cosPhi*sinPsi;
   fMatrix[2] = -sinTheta*cosPsi;
   fMatrix[3] = -cosPhi*cosTheta*sinPsi - sinPhi*cosPsi;
   fMatrix[4] =  cosPhi*cosPsi          - sinPhi*cosTheta*sinPsi;
   fMatrix[5] =  sinTheta*sinPsi;
   fMatrix[6] =  sinTheta*cosPhi;
   fMatrix[7] =  sinTheta*sinPhi;
   fMatrix[8] =  cosTheta;

   SetReflection();
 
}

//__________________________________________________________________________
 BrRotMatrix::BrRotMatrix(const BrRotMatrix* mat1, const BrRotMatrix* mat2 )
{
  //New rotation matrix from transformation of two rotation matrices
  //This multiplies mat2 * mat1

  //Order is important
  //Do not change this order without changing corresponding order in 
  //multiplication operator which uses this
  //KH 17-Jan-2002

  fType = 0;

  for (Int_t i=0; i<3; i++){
    for (Int_t j=0; j<3; j++){
      Int_t index = i*3 + j;
      fMatrix[index] = 0.0;
      for (Int_t k=0; k<3; k++){
	fMatrix[index] += mat1->GetMatrix()[k*3+j]*mat2->GetMatrix()[i*3+k];
      }
    }
  }
  SetReflection();

  //Now calculate angles
  UpdateAngles();
}

//_____________________________________________________________________________
 BrRotMatrix::BrRotMatrix( Double_t theta1, Double_t phi1
                        , Double_t theta2, Double_t phi2
                        , Double_t theta3, Double_t phi3)
{
//*-*-*-*-*-*RotMatrix normal constructor defined a la GEANT*-*-*-*-*-*-*
//*-*        ===============================================

   const Double_t degrad = 0.0174532925199432958;

   fTheta  = theta1;
   fPhi    = phi1;
   fPsi    = theta2;
   fType   = 0;

   fMatrix[0] = TMath::Sin(theta1*degrad)*TMath::Cos(phi1*degrad);
   fMatrix[1] = TMath::Sin(theta1*degrad)*TMath::Sin(phi1*degrad);
   fMatrix[2] = TMath::Cos(theta1*degrad);
   fMatrix[3] = TMath::Sin(theta2*degrad)*TMath::Cos(phi2*degrad);
   fMatrix[4] = TMath::Sin(theta2*degrad)*TMath::Sin(phi2*degrad);
   fMatrix[5] = TMath::Cos(theta2*degrad);
   fMatrix[6] = TMath::Sin(theta3*degrad)*TMath::Cos(phi3*degrad);
   fMatrix[7] = TMath::Sin(theta3*degrad)*TMath::Sin(phi3*degrad);
   fMatrix[8] = TMath::Cos(theta3*degrad);

   SetReflection();

}


//______________________________________________________________________________
 BrRotMatrix::~BrRotMatrix()
{
//*-*-*-*-*-*-*-*-*-*-*RotMatrix default destructor*-*-*-*-*-*-*-*-*-*-*-*-*
//*-*                  ============================

   //do nothing
}

//______________________________________________________________________________
 Double_t  BrRotMatrix::Determinant()
{
//*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*
//*-*
//*-*  Determinant() returns the value of the determiant of this matrix
//*-*
//*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*
 return
        fMatrix[0] * (fMatrix[4]*fMatrix[8] - fMatrix[7]*fMatrix[5])
      - fMatrix[3] * (fMatrix[1]*fMatrix[8] - fMatrix[7]*fMatrix[2])
      + fMatrix[6] * (fMatrix[1]*fMatrix[5] - fMatrix[4]*fMatrix[2]);
}

 void BrRotMatrix::SetMatrix(const Double_t *matrix){
  //
  //
  double *ml=fMatrix;
  for(int i=0; i<9;i++)
    *(ml++)= matrix[i];
}

//______________________________________________________________________________
 void BrRotMatrix::UpdateAngles(){
  //Routine to calculate angles if constructed from other rotation
  //matrices.
  //Need to check carefully that all of the ranges are handled correctly
  //But this should get us started for typical BRAHMS angles.
  //ie theta FS -2.3 -> -30 and theta MRS+30 -> +90
  //KH 31-Oct-2001

  const Double_t degrad = 0.0174532925199432958;
  const Double_t small = 1.e-12;
  const Double_t almostOne = .99999999999;
  const Double_t one = 1.0;
  const Double_t pi = TMath::Pi();

  Double_t theta1,theta2,phi1,phi2,psi1,psi2;
  Double_t sinTheta,cosTheta,sinPhi,cosPhi,sinPsi,cosPsi;

//New way of calculating angles.  Can be done much more cleanly than
//"original way" (see below) using TMath::Atan2 KH 7-Feb-2002 14:00

/*On the other hand, it doesn't seem to work! KH 7-Feb-2002 15:30
//Calculate phi
  phi1 = TMath::ATan2(fMatrix[7],fMatrix[6]);
  cosPhi = TMath::Cos(phi1);

//Calculate psi
  psi1 = -TMath::ATan2(fMatrix[5],fMatrix[2]);
  printf("Got phi = %f, [7] = %f, [6] = %f, psi = %f, [5] = %f, [2] = %fn",phi1,fMatrix[7],fMatrix[6],psi1,fMatrix[5],fMatrix[2]);

  Bool_t thetaIsNegative = kFALSE;
  if((fMatrix[6] < 0 && cosPhi > 0)||(fMatrix[6] > 0 && cosPhi < 0)) 
    thetaIsNegative = kTRUE;

//Calculate theta, here we need to be careful about the sign.
  sinTheta = TMath::Sqrt(fMatrix[6]*fMatrix[6]+fMatrix[7]*fMatrix[7]);
  //Need to get sign of sinTheta.  As written above, it is always positive
  if(thetaIsNegative) sinTheta = -sinTheta;
  theta1 = TMath::ATan2(sinTheta,cosTheta);

//This algorithm lifted from class TThreeRotation
//http://zeus.phys.uconn.edu/refs/root/src/TThreeRotation.cxx.html
  Double_t m11 = cosPhi*cosTheta*cosPsi - sinPhi*sinPsi;
  if(TMath::Abs(m11-fMatrix[0]) > small) {
     Double_t sumPhiPsi = TMath::ATan2(-fMatrix[3],fMatrix[0]);
     psi1 = sumPhiPsi - phi1;
     psi1 += (psi1 > -pi) ? 0: 2*pi;
     psi1 -= (psi1 <= pi) ? 0: 2*pi;
     }

  fTheta = theta1 / degrad;
  fPhi = phi1 / degrad;
  fPsi = psi1 / degrad;
*/

/*Original way of calculating angles*/
  theta1 = TMath::ACos(fMatrix[8]);                    //range 0 -> pi
  sinTheta = TMath::Sin(theta1);
  if(TMath::Abs(sinTheta) < small) sinTheta = 0.0;
  if(TMath::Abs(sinTheta) > one) sinTheta = TMath::Sign(one,sinTheta);

  if(fMatrix[6] != 0) {
     phi2 = TMath::ATan(fMatrix[7] / fMatrix[6]);      //range -pi/2 -> pi/2
     cosPhi = TMath::Cos(phi2);
     }
  else if(sinTheta != 0.0) {
     //If sinTheta != 0, cosPhi will be = 0
     phi2 = 90.*degrad;
     cosPhi = 0.0;
     }

  if(cosPhi != 0.0) {
     theta2 = TMath::ASin(fMatrix[6]/cosPhi);   //range -pi/2 -> pi/2
     }
  else {
     theta2 = TMath::ASin(fMatrix[7]);          //range -pi/2 -> pi/2
     }

  //Determine proper theta.
  if((theta1 >= 0.0 && theta2 >= 0)|| (theta1 < 0 && theta2 > 0)) {
     //0 -> pi
     fTheta = theta1 / degrad;
     }
  else if(theta1 >= 0.0 && theta2 < 0.0) {
     //-pi/2 -> 0
     fTheta = theta2 / degrad;
     }
  else if(theta1 < 0.0 && theta2 < 0.0) {
     //-pi -> -pi/2
     fTheta = -theta1 / degrad;
     }
  else {
     Warning("UpdateAngles","Did not find proper theta combination %f, %f",theta1,theta2);
     }

  sinTheta = TMath::Sin(fTheta * degrad);  //Get with proper sign.
  if(sinTheta != 0) {
     cosPhi = fMatrix[6] / sinTheta;
     if(TMath::Abs(cosPhi) < small) cosPhi = 0.0;
     if(TMath::Abs(cosPhi) > one) cosPhi = TMath::Sign(one,cosPhi);
     phi1 = TMath::ACos(cosPhi);                //range 0 -> pi
     }
  else {
     cosPhi = 1.0;
     phi1 = 0.0;
     }

  if(sinTheta != 0) {
     cosPsi = -fMatrix[2]/ sinTheta;
     sinPsi = fMatrix[5] / sinTheta;
     if(TMath::Abs(cosPsi) < small) cosPsi = 0.0;
     if(TMath::Abs(cosPsi) > one) cosPsi = TMath::Sign(one,cosPsi);
     if(TMath::Abs(sinPsi) < small) sinPsi = 0.0;
     if(TMath::Abs(sinPsi) > one) sinPsi = TMath::Sign(one,sinPsi);
     psi1 = TMath::ACos(cosPsi); //range 0 -> pi
     psi2 = TMath::ASin(sinPsi); //range -pi/2 -> pi/2
     }
  else {
     psi1 = 0.0;
     psi2 = 0.0;
     }

  //Determine proper phi
  if((phi1 >= 0.0 && phi2 >= 0)|| (phi1 < 0 && phi2 > 0)) {
     //0 -> pi
     fPhi = phi1 / degrad;
     }
  else if(phi1 >= 0.0 && phi2 < 0.0) {
     //-pi/2 -> 0
     fPhi = phi2 / degrad;
     }
  else if(phi1 < 0.0 && phi2 < 0.0) {
     //-pi -> -pi/2
     fPhi = -phi1 / degrad;
     }
  else {
     Warning("UpdateAngles","Did not find proper phi combination %f, %f %f %f",phi1,phi2,sinTheta,cosPhi);
     }

  //Determine proper psi
  if((psi1 >= 0.0 && psi2 >= 0)|| (psi1 < 0 && psi2 > 0)) {
     //0 -> pi
     fPsi = psi1 / degrad;
     }
  else if(psi1 >= 0.0 && psi2 < 0.0) {
     //-pi/2 -> 0
     fPsi = psi2 / degrad;
     }
  else if(psi1 < 0.0 && psi2 < 0.0) {
     //-pi -> -pi/2
     fPsi = -psi1 / degrad;
     }
  else {
     Warning("UpdateAngles","Did not find proper psi combination %f, %f",psi1,psi2);
     }



  //Get rid of small angle roundoff error

  if(TMath::Abs(fTheta) < small) fTheta = 0.0;
  if(TMath::Abs(fPhi) < small) fPhi = 0.0;
  if(TMath::Abs(fPsi) < small) fPsi = 0.0;

//Do a test to see if things are consistent
   Double_t testMat[9];
   cosTheta = TMath::Cos(fTheta*degrad);
   sinTheta = TMath::Sin(fTheta*degrad);
   cosPhi   = TMath::Cos(fPhi*degrad);
   sinPhi   = TMath::Sin(fPhi*degrad);
   cosPsi   = TMath::Cos(fPsi*degrad);
   sinPsi   = TMath::Sin(fPsi*degrad);

   testMat[0] =  cosPhi*cosTheta*cosPsi - sinPhi*sinPsi;
   testMat[1] =  sinPhi*cosTheta*cosPsi + cosPhi*sinPsi;
   testMat[2] = -sinTheta*cosPsi;
   testMat[3] = -cosPhi*cosTheta*sinPsi - sinPhi*cosPsi;
   testMat[4] =  cosPhi*cosPsi          - sinPhi*cosTheta*sinPsi;
   testMat[5] =  sinTheta*sinPsi;
   testMat[6] =  cosPhi*sinTheta;
   testMat[7] =  sinPhi*sinTheta;
   testMat[8] =  cosTheta;

   //printf("Checking updated anglesn");
/*
   for(Int_t ii=0;ii<9;ii++) {
     if(TMath::Abs(testMat[ii]-fMatrix[ii])>small) 
       printf("Matrices not equal; testMat[%d]=%7.3e, fMatrix[%d]=%7.3en",ii,testMat[ii],ii,fMatrix[ii]);
   }
*/

}

//______________________________________________________________________________
 void BrRotMatrix::SetReflection()
{
//*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*
//*-*  SetReflection() -  checks whether the determinant of this
//*-*                     matrix defines the reflection transformation
//*-*                     and set the "reflection" flag if any
//*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*
   
   ResetBit(kBrReflection);
   Float_t det = Determinant();
   if(TMath::Abs(TMath::Abs(det)-1.0)>1.0e-12)
	   Warning("BrRotMatrix","Non conforming Rotation Matrix");
   if (det < 0) SetBit(kBrReflection);
}

 BrRotMatrix BrRotMatrix::GetInverse() {
  //Calculate and return inverse matrix
  //More or less copied from inverse matrix calculation of BrCoordinateSystem

  Double_t invMatrix[9];
  invMatrix[0] =      fMatrix[4]*fMatrix[8] - fMatrix[5]*fMatrix[7];
  invMatrix[3] = -1.*(fMatrix[3]*fMatrix[8] - fMatrix[5]*fMatrix[6]);
  invMatrix[6] =      fMatrix[3]*fMatrix[7] - fMatrix[6]*fMatrix[4];
  invMatrix[1] = -1.*(fMatrix[1]*fMatrix[8] - fMatrix[2]*fMatrix[7]);
  invMatrix[4] =      fMatrix[0]*fMatrix[8] - fMatrix[2]*fMatrix[6];
  invMatrix[7] = -1.*(fMatrix[0]*fMatrix[7] - fMatrix[1]*fMatrix[6]);
  invMatrix[2] =      fMatrix[1]*fMatrix[5] - fMatrix[4]*fMatrix[2];
  invMatrix[5] = -1.*(fMatrix[0]*fMatrix[5] - fMatrix[3]*fMatrix[2]);
  invMatrix[8] =      fMatrix[0]*fMatrix[4] - fMatrix[3]*fMatrix[1];

  BrRotMatrix inv(invMatrix);
  return inv;
}

//____________________________________________________________________
ostream& operator<< (ostream & os, const BrRotMatrix& rotm) 
{
  int w=10;
  os <<"(" << rotm.GetTheta()
     << "," << rotm.GetPhi() 
     << "," << rotm.GetPsi() << ")n"; 
  os << setprecision(6) 
     <<"(" << setw(w) << rotm.GetMatrix()[0]
     <<"," << setw(w) << rotm.GetMatrix()[1]
     <<"," << setw(w) << rotm.GetMatrix()[2] << ")n"
     <<"(" << setw(w) << rotm.GetMatrix()[3]
     <<"," << setw(w) << rotm.GetMatrix()[4]
     <<"," << setw(w) << rotm.GetMatrix()[5] <<")n"
     <<"(" << setw(w) << rotm.GetMatrix()[6]
     <<"," << setw(w) << rotm.GetMatrix()[7]
     <<"," << setw(w) << rotm.GetMatrix()[8] <<")";
  return os;
}

//____________________________________________________________________
ostream& operator<< (ostream & os, const BrRotMatrix* rotm) 
{
  os << *rotm;
  return os;
}

//////////////////////////////////////////////////////////////////////////
//
// CVS log info
//
//  $Log: BrRotMatrix.cxx,v $
//  Revision 1.8  2002/04/16 13:22:40  hagel
//  Made ostream output 'prettier'
//
//  Revision 1.7  2002/02/26 08:59:03  cholm
//  Added const'ness to many methods - needed by ISO C++
//
//  Revision 1.6  2002/02/11 20:42:54  hagel
//  Remove matrix difference printout
//
//  Revision 1.5  2002/02/08 22:43:41  hagel
//  Added more funcationality for debugging processes as well as GetInverse()
//
//  Revision 1.4  2002/01/16 15:05:01  hagel
//  Added method to calculate Inverse matrix
//
//  Revision 1.3  2001/11/05 23:42:24  hagel
//  Added ostream stuff as well as UpdateAngles method to calculate angles from matrix
//
//  Revision 1.2  2001/10/09 16:23:34  videbaek
//  Modified output widths in the writting so it comes out aligned.
//  Added the #pragma for operator function so bratroot can execute
//  stuff like cout << rotm .. as well as vector multiplictaions etc.
//  This was in brat 1 , but did not propagate.
//
//  Revision 1.1.1.1  2001/06/21 14:55:20  hagel
//  Initial revision of brat2
//
//  Revision 1.7  2000/10/10 18:10:29  videbaek
//  Changed the method name GetRotMatrix to GetMatrix to be symmetric
//  with SetMatrix and to distinguish from BrCoorDianteSystem->GetRotMatrix
//  that return a BrRotMatrix.
//
//  Revision 1.6  2000/09/18 21:26:04  videbaek
//  Modified internal structure of BrDetectorvolume
//  Associated changes.
//
//  Revision 1.5  2000/07/16 19:46:17  videbaek
//  Addition for declaring a new rotmatrix by means of two exsisting.
//  Fixing of errors in inverse matrix fro non-simple rotations, and constructor for
//  Rotmatrix using Euler angles. All thanks to Trine Tveter.
//
//  Revision 1.4  1999/12/23 15:55:46  videbaek
//  Added the SetRelativeSystem
//  Fully debugged.
//
//  Revision 1.3  1998/12/21 20:32:05  videbaek
//  coordinate system changes mainly
//
//  Revision 1.2  1998/12/08 20:43:56  hagel
//  Change kReflection to kBrReflection to avoid conflicts with TRotMatrix in TNode
//
//  Revision 1.1  1998/12/01 20:50:11  videbaek
//  Added several classes to Geometry. Updated source and makefile(s)
//  - BrLine3D
//  - BrCoordinateSystem
//  - BrRotMatrix
//

Copyright ; 2002 BRAHMS Collaboration <brahmlib@rcf.rhic.bnl.gov>
Last Update on by

Validate HTML
Validate CSS