CbmBbaAlignmentBody.cxx

Go to the documentation of this file.

/* Copyright (C) 2025 GSI Helmholtzzentrum fuer Schwerionenforschung, Darmstadt
   SPDX-License-Identifier: GPL-3.0-only
   Authors: Sergey Gorbunov [committer] */

 
#include "CbmBbaAlignmentBody.h"
 
#include <Logger.h>
 
#include <TGeoManager.h>
#include <TGeoMatrix.h>
#include <TGeoNode.h>
#include <TGeoPhysicalNode.h>
#include <TGeoTube.h>
#include <TGeoVolume.h>
#include <TObjArray.h>
 
#include <iomanip>
 
namespace
{
  constexpr bool kEulerAngles = false;  // use euler angles for rotations instead of extrinsic rotations
}
 
namespace cbm::bba
{
  AlignmentBody::AlignmentBody(double shiftX, double shiftY, double shiftZ, double rotX, double rotY, double rotZ)
  {
 
    fGlobal.Clear();
    fGlobalInv.Clear();
    fAlignment.Clear();
    fHitTransform.Clear();
 
    if (kEulerAngles) {
      TGeoRotation rot("", rotX, rotY, rotZ);
      fGlobal.SetRotation(rot.GetRotationMatrix());
    }
    else {
      // in order to use extrinisc rotations we have to do it step by step and before the translations
      fGlobal.RotateX(rotX);
      fGlobal.RotateY(rotY);
      fGlobal.RotateZ(rotZ);
    }
    // the order of SetDx, SetDy, SetDz and SetRotation does not matter, but for extrinsic rotations via RotateX/Y/Z is important to do it before the translations
    fGlobal.SetDx(shiftX);
    fGlobal.SetDy(shiftY);
    fGlobal.SetDz(shiftZ);
    fGlobalNominal = fGlobal;  // since there will never be pre-alignment
    fGlobalInv     = fGlobal.Inverse();
    fHitTransform  = fGlobalNominal * (fAlignment * fGlobalInv);
    fIsValid       = true;
  }
 
  AlignmentBody::AlignmentBody(std::string nodePath) { CreateFromGeoNode(nodePath); }
 
  bool AlignmentBody::CreateFromGeoNode(const std::string nodePath)
  {
    fNodePath = nodePath;
 
    // set the initial alignment to zero
    fGlobal.Clear();
    fGlobalInv.Clear();
    fAlignment.Clear();
    fHitTransform.Clear();
 
    if (!gGeoManager) {
      LOG(fatal) << "TGeoManager is not initialized!";
      return false;
    }
 
    if (fNodePath.empty()) {
      LOG(fatal) << "Alignment node path is empty!";
      return false;
    }
 
    if (fNodePath[0] != '/') {
      LOG(fatal) << "Alignment node path is not global. It must start with \"/\" "
                 << ": " << fNodePath;
      return false;
    }
 
    if (!gGeoManager->cd(fNodePath.c_str())) {
      LOG(fatal) << "Failed to cd to path " << fNodePath;
      return false;
    }
    LOG(debug) << "Found alignment body: " << fNodePath;
 
    // Get the current node and its global transformation
    const TGeoNode* node = gGeoManager->GetCurrentNode();
    if (!node) {
      LOG(fatal) << "Failed to find TGeoNode at path " << fNodePath;
      return false;
    }
 
    fGlobal = *(
      gGeoManager
        ->GetCurrentMatrix());  // global transformation matrix of the current node (TODO: assume that it will contain potential prealignment, to be tested)
    fGlobalInv = fGlobal.Inverse();
 
    // get the mother node to be able to search for the physical node
    gGeoManager->CdUp();
    // get the global transformation of the mother node (TODO: assume that it will contain potential prealignment, to be tested)
    TGeoHMatrix GlobalMother =
      *(gGeoManager
          ->GetCurrentMatrix());  // global transformation matrix of the mother node including potential prealignment
 
 
    // find the exsisting alignment matrix for the node
    // when the alignment was applied, the node is presented in the list of physical nodes
    {
      TObjArray* pNodes = gGeoManager->GetListOfPhysicalNodes();
      for (int i = 0; i < pNodes->GetEntriesFast(); i++) {
        const TGeoPhysicalNode* pnode = dynamic_cast<const TGeoPhysicalNode*>(pNodes->At(i));
        if (!pnode) {
          LOG(fatal) << "Failed to cast TGeoPhysicalNode at index " << i;
          return false;
        }
        if (pnode->GetNode() == node) {
          // there is a physical node for this geo node -> the node already has some alignment
          LOG(warn) << "Found pre-existing alignment for node " << fNodePath
                    << ". Pre-existing alignment will be ignored and replaced by new alignment.";
          TGeoHMatrix localNoniminal = *(pnode->GetOriginalMatrix());  // unaligned local matrix
          //TGeoHMatrix globalPreAligned = *(pnode->GetMatrix());          // aligned global matrix (TODO: hopefully the same as in fGlobalTransform? -> check!)
          fGlobalNominal = GlobalMother * localNoniminal;  // unaligned global matrix
          // Calculate the pre-existing alignment matrix A0
          TGeoHMatrix A0 = localNoniminal.Inverse() * (GlobalMother.Inverse() * fGlobal);  // pre-existing alignment
 
          // TODO: extraction of current alignment parameters from A0 into fPar0
          //fPar          = fPar0;
          //fA            = A0; // initial alignment matrix for extraction of start parameters
          fHitTransform = fGlobalNominal * (fAlignment * fGlobalInv);
 
          break;
        }
      }
    }
 
    fIsValid = true;
    return fIsValid;
  }
 
 
  void AlignmentBody::ActivateParameters(const std::array<bool, NofParameters>& isParActive)
  {
    fIsParActive = isParActive;
  }
 
 
  void AlignmentBody::SetParameters(double shiftX, double shiftY, double shiftZ, double rotX, double rotY, double rotZ)
  {
    fPar[0] = shiftX;
    fPar[1] = shiftY;
    fPar[2] = shiftZ;
    fPar[3] = rotX;
    fPar[4] = rotY;
    fPar[5] = rotZ;
    if (kEulerAngles) {
      TGeoRotation rot("", fPar[3], fPar[4], fPar[5]);
      fAlignment.SetRotation(rot.GetRotationMatrix());
    }
    else {
      // TODO!!! THIS ONLY WORKS WITHOUT PRE-EXISTING ALIGNMENT!! BECAUSE RotateX/Y/Z ALSO ROTATES PREEXISTING TRANSLATTION, OR IS IT OKAY LIKE THIS --> CHECK!!
      fAlignment.Clear();
      fAlignment.RotateX(rotX);
      fAlignment.RotateY(rotY);
      fAlignment.RotateZ(rotZ);
    }
    fAlignment.SetTranslation(fPar.data());
    fHitTransform = fGlobalNominal * (fAlignment * fGlobalInv);
  }
 
  std::array<double, 3> AlignmentBody::ApplyAlignmentToHit(const std::array<double, 3> hit) const
  {
    std::array<double, 3> alignedHit;
    // we use TGeoHMatrix to perform the transformation
    // LocalToMaster calculates: master = Matrix * local
    fHitTransform.LocalToMaster(hit.data(), alignedHit.data());
    return alignedHit;
  }
 
  std::array<double, 3> AlignmentBody::ApplyInverseAlignmentToHit(const std::array<double, 3> hit) const
  {
    std::array<double, 3> alignedHit;
    // we use TGeoHMatrix to perform the transformation
    // MasterToLocal calculates: local = Matrix^{-1} * global
    fHitTransform.MasterToLocal(hit.data(), alignedHit.data());
    return alignedHit;
  }
 
  // TODO: this could be a memcpy
  void AlignmentBody::GetParameters(double* par) const
  {
    for (int i = 0; i < NofParameters; i++) {
      par[i] = fPar[i];
    }
  }
 
}  // namespace cbm::bba