#include <TGeoManager.h>
#include <TGeoMatrix.h>
#include <TGeoPhysicalNode.h>
#include <TGeoOverlap.h>
#include <TMath.h>
#include "AliAlignObj.h"
#include "AliTrackPointArray.h"
#include "AliLog.h"
ClassImp(AliAlignObj)
AliAlignObj::AliAlignObj():
TObject(),
fVolPath(),
fVolUID(0)
{
for(Int_t i=0; i<6; i++) fDiag[i]=-999.;
for(Int_t i=0; i<15; i++) fODia[i]=-999.;
}
AliAlignObj::AliAlignObj(const char* symname, UShort_t voluid) :
TObject(),
fVolPath(symname),
fVolUID(voluid)
{
for(Int_t i=0; i<6; i++) fDiag[i]=-999.;
for(Int_t i=0; i<15; i++) fODia[i]=-999.;
}
AliAlignObj::AliAlignObj(const char* symname, UShort_t voluid, Double_t* cmat) :
TObject(),
fVolPath(symname),
fVolUID(voluid)
{
SetCorrMatrix(cmat);
}
AliAlignObj::AliAlignObj(const AliAlignObj& theAlignObj) :
TObject(theAlignObj),
fVolPath(theAlignObj.GetSymName()),
fVolUID(theAlignObj.GetVolUID())
{
for(Int_t i=0; i<6; i++) fDiag[i]=theAlignObj.fDiag[i];
for(Int_t i=0; i<15; i++) fODia[i]=theAlignObj.fODia[i];
}
AliAlignObj &AliAlignObj::operator =(const AliAlignObj& theAlignObj)
{
if(this==&theAlignObj) return *this;
fVolPath = theAlignObj.GetSymName();
fVolUID = theAlignObj.GetVolUID();
for(Int_t i=0; i<6; i++) fDiag[i]=theAlignObj.fDiag[i];
for(Int_t i=0; i<15; i++) fODia[i]=theAlignObj.fODia[i];
return *this;
}
AliAlignObj &AliAlignObj::operator*=(const AliAlignObj& theAlignObj)
{
TGeoHMatrix m1;
GetMatrix(m1);
TGeoHMatrix m2;
theAlignObj.GetMatrix(m2);
m1.MultiplyLeft(&m2);
SetMatrix(m1);
for(Int_t i=0; i<6; i++) fDiag[i] = theAlignObj.fDiag[i];
for(Int_t i=0; i<15; i++) fODia[i] = theAlignObj.fODia[i];
return *this;
}
AliAlignObj::~AliAlignObj()
{
}
void AliAlignObj::SetVolUID(AliGeomManager::ELayerID detId, Int_t modId)
{
fVolUID = AliGeomManager::LayerToVolUID(detId,modId);
}
void AliAlignObj::GetVolUID(AliGeomManager::ELayerID &layerId, Int_t &modId) const
{
layerId = AliGeomManager::VolUIDToLayer(fVolUID,modId);
}
Bool_t AliAlignObj::GetPars(Double_t tr[], Double_t angles[]) const
{
GetTranslation(tr);
return GetAngles(angles);
}
Int_t AliAlignObj::GetLevel() const
{
if(!gGeoManager){
AliWarning("gGeoManager doesn't exist or it is still open: unable to return meaningful level value.");
return (-1);
}
const char* symname = GetSymName();
const char* path;
TGeoPNEntry* pne = gGeoManager->GetAlignableEntry(symname);
if(pne){
path = pne->GetTitle();
}else{
path = symname;
}
TString pathStr = path;
if(pathStr[0]!='/') pathStr.Prepend('/');
return pathStr.CountChar('/');
}
Int_t AliAlignObj::Compare(const TObject *obj) const
{
Int_t level = GetLevel();
Int_t level2 = ((AliAlignObj *)obj)->GetLevel();
if (level == level2)
return 0;
else
return ((level > level2) ? 1 : -1);
}
void AliAlignObj::GetCovMatrix(Double_t *cmat) const
{
for(Int_t i=0; i<6; ++i) {
for(Int_t j=0; j<i; ++j) {
cmat[i*(i+1)/2+j] = (fDiag[j] >= 0. && fDiag[i] >= 0.) ? fODia[(i-1)*i/2+j]*fDiag[j]*fDiag[i]: -999.;
}
cmat[i*(i+1)/2+i] = (fDiag[i] >= 0.) ? fDiag[i]*fDiag[i] : -999.;
}
return;
}
void AliAlignObj::GetCovMatrix(TMatrixDSym& mcov) const
{
for(Int_t i=0; i<6; ++i) {
for(Int_t j=0; j<i; ++j) {
mcov(j,i) = mcov(i,j) = (fDiag[j] >= 0. && fDiag[i] >= 0.) ? fODia[(i-1)*i/2+j]*fDiag[j]*fDiag[i]: -999.;
}
mcov(i,i) = (fDiag[i] >= 0.) ? fDiag[i]*fDiag[i] : -999.;
}
}
Bool_t AliAlignObj::GetLocalCovMatrix(TMatrixDSym& lCov) const
{
TMatrixD mJ(6,6);
if(!GetJacobian(mJ)) return kFALSE;
TMatrixDSym gCov(6);
GetCovMatrix(gCov);
TMatrixD gcovJ(gCov,TMatrixD::kMult,mJ);
TMatrixD lCovM(mJ,TMatrixD::kTransposeMult,gcovJ);
for(Int_t i=0; i<6; i++)
{
lCov(i,i) = lCovM(i,i);
for(Int_t j=i+1; j<6; j++)
{
lCov(i,j)=lCovM(i,j);
lCov(j,i)=lCovM(i,j);
}
}
return kTRUE;
}
Bool_t AliAlignObj::GetLocalCovMatrix(Double_t *lCov) const
{
TMatrixDSym lCovMatrix(6);
GetLocalCovMatrix(lCovMatrix);
Int_t k=0;
for(Int_t i=0; i<6; i++)
for(Int_t j=i; j<6; j++)
{
lCov[k++] = lCovMatrix(i,j);
}
return kTRUE;
}
Bool_t AliAlignObj::GetJacobian(TMatrixD& mJ) const
{
if (!gGeoManager || !gGeoManager->IsClosed()) {
AliError("Can't compute the global covariance matrix from the local one without an open geometry!");
return kFALSE;
}
const char* symname = GetSymName();
TGeoPhysicalNode* node;
TGeoPNEntry* pne = gGeoManager->GetAlignableEntry(symname);
if(pne){
if(!pne->GetPhysicalNode()){
node = gGeoManager->MakeAlignablePN(pne);
}else{
node = pne->GetPhysicalNode();
}
}else{
AliWarning(Form("The symbolic volume name %s does not correspond to a physical entry. Using it as volume path!",symname));
node = (TGeoPhysicalNode*) gGeoManager->MakePhysicalNode(symname);
}
if (!node) {
AliError(Form("Volume name or path %s not valid!",symname));
return kFALSE;
}
TGeoHMatrix gm;
gm = *node->GetMatrix();
Double_t *tr = gm.GetTranslation();
Double_t *rot = gm.GetRotationMatrix();
TGeoHMatrix m;
GetMatrix(m);
for(Int_t i=0; i<3; i++)
{
for(Int_t j=0; j<3; j++)
{
mJ(i,j) = rot[i+3*j];
}
}
for(Int_t i=0; i<3; i++)
{
for(Int_t j=0; j<3; j++)
{
mJ(i+3,j) = 0.;
}
}
for(Int_t i=0; i<3; i++)
{
for(Int_t j=0; j<3; j++)
{
Double_t mEl = 0.;
Int_t b = (j+1)%3;
Int_t d = (j+2)%3;
for(Int_t k=0; k<3; k++)
{
mEl += (rot[3*i+b]*rot[3*k+d])*tr[k]-(rot[3*i+d]*rot[3*k+b])*tr[k];
}
mJ(i,j+3) = mEl;
}
}
for(Int_t i=0; i<3; i++)
for(Int_t j=0; j<3; j++)
{
Int_t a = (i+1)%3;
Int_t b = (j+1)%3;
Int_t c = (i+2)%3;
Int_t d = (j+2)%3;
mJ(i+3,j+3) = rot[3*a+b]*rot[3*c+d]-rot[3*a+d]*rot[3*c+b];
}
return kTRUE;
}
Bool_t AliAlignObj::SetFromLocalCov(TMatrixDSym& lCov)
{
TMatrixD mJ(6,6);
if(!GetJacobian(mJ)) return kFALSE;
TMatrixD trJ(TMatrixD::kTransposed, mJ);
TMatrixD lcovTrJ(lCov,TMatrixD::kMult,trJ);
TMatrixD gCovM(mJ,TMatrixD::kMult,lcovTrJ);
TMatrixDSym gCov(6);
for(Int_t i=0; i<6; i++)
{
gCov(i,i) = gCovM(i,i);
for(Int_t j=i+1; j<6; j++)
{
gCov(i,j)=gCovM(i,j);
gCov(j,i)=gCovM(i,j);
}
}
SetCorrMatrix(gCov);
return kTRUE;
}
Bool_t AliAlignObj::SetFromLocalCov(Double_t *lCov)
{
TMatrixDSym lCovMatrix(6);
Int_t k=0;
for(Int_t i=0; i<6; i++)
for(Int_t j=i; j<6; j++)
{
lCovMatrix(i,j) = lCov[k++];
if(j!=i) lCovMatrix(j,i) = lCovMatrix(i,j);
}
return SetFromLocalCov(lCovMatrix);
}
void AliAlignObj::SetCorrMatrix(Double_t *cmat)
{
if(cmat) {
for(Int_t i=0; i<6; ++i) {
fDiag[i] = (cmat[i*(i+1)/2+i] >= 0.) ? TMath::Sqrt(cmat[i*(i+1)/2+i]) : -999.;
}
for(Int_t i=0; i<6; ++i)
for(Int_t j=0; j<i; ++j) {
fODia[(i-1)*i/2+j] = (fDiag[i] > 0. && fDiag[j] > 0.) ? cmat[i*(i+1)/2+j]/(fDiag[j]*fDiag[i]) : 0.;
if (fODia[(i-1)*i/2+j]>1.) fODia[(i-1)*i/2+j] = 1.;
if (fODia[(i-1)*i/2+j]<-1.) fODia[(i-1)*i/2+j] = -1.;
}
} else {
for(Int_t i=0; i< 6; ++i) fDiag[i]=-999.;
for(Int_t i=0; i< 6*(6-1)/2; ++i) fODia[i]=0.;
}
return;
}
void AliAlignObj::SetCorrMatrix(TMatrixDSym& mcov)
{
if(mcov.IsValid()) {
for(Int_t i=0; i<6; ++i) {
fDiag[i] = (mcov(i,i) >= 0.) ? TMath::Sqrt(mcov(i,i)) : -999.;
}
for(Int_t i=0; i<6; ++i)
for(Int_t j=0; j<i; ++j) {
fODia[(i-1)*i/2+j] = (fDiag[i] > 0. && fDiag[j] > 0.) ? mcov(i,j)/(fDiag[j]*fDiag[i]) : 0.;
if (fODia[(i-1)*i/2+j]>1.) fODia[(i-1)*i/2+j] = 1.;
if (fODia[(i-1)*i/2+j]<-1.) fODia[(i-1)*i/2+j] = -1.;
}
} else {
for(Int_t i=0; i< 6; ++i) fDiag[i]=-999.;
for(Int_t i=0; i< 6*(6-1)/2; ++i) fODia[i]=0.;
}
return;
}
void AliAlignObj::AnglesToMatrix(const Double_t *angles, Double_t *rot) const
{
Double_t degrad = TMath::DegToRad();
Double_t sinpsi = TMath::Sin(degrad*angles[0]);
Double_t cospsi = TMath::Cos(degrad*angles[0]);
Double_t sinthe = TMath::Sin(degrad*angles[1]);
Double_t costhe = TMath::Cos(degrad*angles[1]);
Double_t sinphi = TMath::Sin(degrad*angles[2]);
Double_t cosphi = TMath::Cos(degrad*angles[2]);
rot[0] = costhe*cosphi;
rot[1] = -costhe*sinphi;
rot[2] = sinthe;
rot[3] = sinpsi*sinthe*cosphi + cospsi*sinphi;
rot[4] = -sinpsi*sinthe*sinphi + cospsi*cosphi;
rot[5] = -costhe*sinpsi;
rot[6] = -cospsi*sinthe*cosphi + sinpsi*sinphi;
rot[7] = cospsi*sinthe*sinphi + sinpsi*cosphi;
rot[8] = costhe*cospsi;
}
Bool_t AliAlignObj::MatrixToAngles(const Double_t *rot, Double_t *angles) const
{
if(TMath::Abs(rot[0])<1e-7 || TMath::Abs(rot[8])<1e-7) {
AliError("Failed to extract roll-pitch-yall angles!");
return kFALSE;
}
Double_t raddeg = TMath::RadToDeg();
angles[0]=raddeg*TMath::ATan2(-rot[5],rot[8]);
angles[1]=raddeg*TMath::ASin(rot[2]);
angles[2]=raddeg*TMath::ATan2(-rot[1],rot[0]);
return kTRUE;
}
void AliAlignObj::Transform(AliTrackPoint &p, Bool_t copycov) const
{
if (fVolUID != p.GetVolumeID())
AliWarning(Form("Alignment object ID is not equal to the space-point ID (%d != %d)",fVolUID,p.GetVolumeID()));
TGeoHMatrix m;
GetMatrix(m);
Double_t *rot = m.GetRotationMatrix();
Double_t *tr = m.GetTranslation();
Float_t xyzin[3],xyzout[3];
p.GetXYZ(xyzin);
for (Int_t i = 0; i < 3; i++)
xyzout[i] = tr[i]+
xyzin[0]*rot[3*i]+
xyzin[1]*rot[3*i+1]+
xyzin[2]*rot[3*i+2];
p.SetXYZ(xyzout);
if(copycov){
TMatrixDSym covmat(6);
GetCovMatrix(covmat);
p.SetAlignCovMatrix(covmat);
}
}
void AliAlignObj::Transform(AliTrackPointArray &array) const
{
AliTrackPoint p;
for (Int_t i = 0; i < array.GetNPoints(); i++) {
array.GetPoint(p,i);
Transform(p);
array.AddPoint(i,&p);
}
}
void AliAlignObj::Print(Option_t *) const
{
Double_t tr[3];
GetTranslation(tr);
Double_t angles[3];
GetAngles(angles);
TGeoHMatrix m;
GetMatrix(m);
const Double_t *rot = m.GetRotationMatrix();
printf("Volume=%s\n",GetSymName());
if (GetVolUID() != 0) {
AliGeomManager::ELayerID layerId;
Int_t modId;
GetVolUID(layerId,modId);
printf("VolumeID=%d LayerID=%d ( %s ) ModuleID=%d\n", GetVolUID(),layerId,AliGeomManager::LayerName(layerId),modId);
}
printf("%12.8f%12.8f%12.8f Tx = %12.8f Psi = %12.8f\n", rot[0], rot[1], rot[2], tr[0], angles[0]);
printf("%12.8f%12.8f%12.8f Ty = %12.8f Theta = %12.8f\n", rot[3], rot[4], rot[5], tr[1], angles[1]);
printf("%12.8f%12.8f%12.8f Tz = %12.8f Phi = %12.8f\n", rot[6], rot[7], rot[8], tr[2], angles[2]);
}
void AliAlignObj::SetPars(Double_t x, Double_t y, Double_t z,
Double_t psi, Double_t theta, Double_t phi)
{
SetTranslation(x,y,z);
SetRotation(psi,theta,phi);
}
Bool_t AliAlignObj::SetLocalPars(Double_t x, Double_t y, Double_t z,
Double_t psi, Double_t theta, Double_t phi)
{
TGeoHMatrix m;
Double_t tr[3] = {x, y, z};
m.SetTranslation(tr);
Double_t angles[3] = {psi, theta, phi};
Double_t rot[9];
AnglesToMatrix(angles,rot);
m.SetRotation(rot);
return SetLocalMatrix(m);
}
Bool_t AliAlignObj::SetLocalTranslation(Double_t x, Double_t y, Double_t z)
{
TGeoHMatrix m;
Double_t tr[3] = {x, y, z};
m.SetTranslation(tr);
return SetLocalMatrix(m);
}
Bool_t AliAlignObj::SetLocalTranslation(const TGeoMatrix& m)
{
const Double_t* tr = m.GetTranslation();
TGeoHMatrix mtr;
mtr.SetTranslation(tr);
return SetLocalMatrix(mtr);
}
Bool_t AliAlignObj::SetLocalRotation(Double_t psi, Double_t theta, Double_t phi)
{
TGeoHMatrix m;
Double_t angles[3] = {psi, theta, phi};
Double_t rot[9];
AnglesToMatrix(angles,rot);
m.SetRotation(rot);
return SetLocalMatrix(m);
}
Bool_t AliAlignObj::SetLocalRotation(const TGeoMatrix& m)
{
TGeoHMatrix rotm;
const Double_t* rot = m.GetRotationMatrix();
rotm.SetRotation(rot);
return SetLocalMatrix(rotm);
}
Bool_t AliAlignObj::SetLocalMatrix(const TGeoMatrix& m)
{
if (!gGeoManager || !gGeoManager->IsClosed()) {
AliError("Can't set the local alignment object parameters! gGeoManager doesn't exist or it is still open!");
return kFALSE;
}
const char* symname = GetSymName();
TGeoHMatrix gprime,gprimeinv;
TGeoPhysicalNode* pn = 0;
TGeoPNEntry* pne = gGeoManager->GetAlignableEntry(symname);
if(pne)
{
pn = pne->GetPhysicalNode();
if(pn){
if (pn->IsAligned())
AliWarning(Form("Volume %s has been misaligned already!",symname));
gprime = *pn->GetMatrix();
}else{
gprime = pne->GetGlobalOrig();
}
}else{
AliWarning(Form("The symbolic volume name %s does not correspond to a physical entry. Using it as volume path!",symname));
if(!gGeoManager->cd(symname)) {
AliError(Form("Volume name or path %s not valid!",symname));
return kFALSE;
}
gprime = *gGeoManager->GetCurrentMatrix();
}
TGeoHMatrix m1;
const Double_t *tr = m.GetTranslation();
m1.SetTranslation(tr);
const Double_t* rot = m.GetRotationMatrix();
m1.SetRotation(rot);
gprimeinv = gprime.Inverse();
m1.Multiply(&gprimeinv);
m1.MultiplyLeft(&gprime);
return SetMatrix(m1);
}
Bool_t AliAlignObj::SetMatrix(const TGeoMatrix& m)
{
SetTranslation(m);
return SetRotation(m);
}
Bool_t AliAlignObj::GetLocalPars(Double_t transl[], Double_t angles[]) const
{
if(!GetLocalTranslation(transl)) return kFALSE;
return GetLocalAngles(angles);
}
Bool_t AliAlignObj::GetLocalTranslation(Double_t* tr) const
{
TGeoHMatrix ml;
if(!GetLocalMatrix(ml)) return kFALSE;
const Double_t* transl;
transl = ml.GetTranslation();
tr[0]=transl[0];
tr[1]=transl[1];
tr[2]=transl[2];
return kTRUE;
}
Bool_t AliAlignObj::GetLocalAngles(Double_t* angles) const
{
TGeoHMatrix ml;
if(!GetLocalMatrix(ml)) return kFALSE;
const Double_t *rot = ml.GetRotationMatrix();
return MatrixToAngles(rot,angles);
}
Bool_t AliAlignObj::GetLocalMatrix(TGeoHMatrix& m) const
{
if (!gGeoManager || !gGeoManager->IsClosed()) {
AliError("Can't get the local alignment object parameters! gGeoManager doesn't exist or it is still open!");
return kFALSE;
}
const char* symname = GetSymName();
TGeoPhysicalNode* node;
TGeoPNEntry* pne = gGeoManager->GetAlignableEntry(symname);
if(pne){
if(!pne->GetPhysicalNode()){
node = gGeoManager->MakeAlignablePN(pne);
}else{
node = pne->GetPhysicalNode();
}
}else{
AliWarning(Form("The symbolic volume name %s does not correspond to a physical entry. Using it as volume path!",symname));
node = (TGeoPhysicalNode*) gGeoManager->MakePhysicalNode(symname);
}
if (!node) {
AliError(Form("Volume name or path %s not valid!",symname));
return kFALSE;
}
GetMatrix(m);
TGeoHMatrix gprime,gprimeinv;
gprime = *node->GetMatrix();
gprimeinv = gprime.Inverse();
m.Multiply(&gprime);
m.MultiplyLeft(&gprimeinv);
return kTRUE;
}
Bool_t AliAlignObj::ApplyToGeometry(Bool_t ovlpcheck)
{
if (!gGeoManager || !gGeoManager->IsClosed()) {
AliError("Can't apply the alignment object! gGeoManager doesn't exist or it is still open!");
return kFALSE;
}
if (gGeoManager->IsLocked()){
AliError("Can't apply the alignment object! Geometry is locked!");
return kFALSE;
}
const char* symname = GetSymName();
const char* path;
TGeoPhysicalNode* node;
TGeoPNEntry* pne = gGeoManager->GetAlignableEntry(symname);
if(pne){
path = pne->GetTitle();
node = gGeoManager->MakeAlignablePN(pne);
}else{
AliDebug(1,Form("The symbolic volume name %s does not correspond to a physical entry. Using it as a volume path!",symname));
path=symname;
if (!gGeoManager->CheckPath(path)) {
AliDebug(1,Form("Volume path %s not valid!",path));
return kFALSE;
}
if (gGeoManager->GetListOfPhysicalNodes()->FindObject(path)) {
AliError(Form("Volume %s has been misaligned already!",path));
return kFALSE;
}
node = (TGeoPhysicalNode*) gGeoManager->MakePhysicalNode(path);
}
if (!node) {
AliError(Form("Volume path %s not valid!",path));
return kFALSE;
}
TGeoHMatrix align,gprime;
gprime = *node->GetMatrix();
GetMatrix(align);
gprime.MultiplyLeft(&align);
TGeoHMatrix *ginv = new TGeoHMatrix;
TGeoHMatrix *g = node->GetMatrix(node->GetLevel()-1);
*ginv = g->Inverse();
*ginv *= gprime;
AliGeomManager::ELayerID layerId;
Int_t modId;
GetVolUID(layerId, modId);
AliDebug(2,Form("Aligning volume %s of detector layer %d with local ID %d",symname,layerId,modId));
if(ovlpcheck){
node->Align(ginv,0,kTRUE);
}else{
node->Align(ginv,0,kFALSE);
}
if(ovlpcheck)
{
TObjArray* ovlpArray = gGeoManager->GetListOfOverlaps();
Int_t nOvlp = ovlpArray->GetEntriesFast();
if(nOvlp)
{
AliInfo(Form("Misalignment of node %s generated the following overlaps/extrusions:",node->GetName()));
for(Int_t i=0; i<nOvlp; i++)
((TGeoOverlap*)ovlpArray->UncheckedAt(i))->PrintInfo();
}
}
return kTRUE;
}