#include "hades.h"
#include "hmdccal2par.h"
#include "hmdcsizescells.h"
#include "hruntimedb.h"
#include "hkalfiltwire.h"
#include "hkalgeomtools.h"
ClassImp (HKalFiltWire)
HKalFiltWire::HKalFiltWire(Int_t n, Int_t measDim, Int_t stateDim,
HMdcTrackGField *fMap, Double_t fpol)
: HKalIFilt(n, measDim, stateDim, fMap, fpol) {
setHitType(Kalman::kWireHit);
}
Bool_t HKalFiltWire::calcProjector(Int_t iSite) const {
HKalTrackSite *site = getSite(iSite);
if(!site) {
return kFALSE;
}
Int_t mdim = getMeasDim();
Int_t sdim = getStateDim();
TMatrixD fProj(mdim, sdim);
fProj.Zero();
TRotation transSec = getRotMat()->Inverse();
HMdcSizesCells *fSizesCells = HMdcSizesCells::getExObject();
if(!fSizesCells) {
fSizesCells = HMdcSizesCells::getObject();
fSizesCells->initContainer();
}
Int_t s = site->getSector();
Int_t m = site->getModule();
Int_t l = site->getLayer();
Int_t c = site->getCell();
HMdcSizesCellsLayer &fSizesCellsLayer = (*fSizesCells)[s][m][l];
HMdcSizesCellsCell &fSizesCellsCell = (*fSizesCells)[s][m][l][c];
const HGeomRotation& transRotLaySysRSec =
fSizesCellsLayer.getRotLaySysRSec(c).getRotMatrix();
HGeomRotation transRotSecToRotLay = transRotLaySysRSec.inverse();
HGeomVector transVecRotLayToSec =
fSizesCellsLayer.getRotLaySysRSec(c).getTransVector();
Double_t R1 =
transRotSecToRotLay.getElement(1,0) * transSec(0,0) +
transRotSecToRotLay.getElement(1,1) * transSec(1,0) +
transRotSecToRotLay.getElement(1,2) * transSec(2,0);
Double_t R4 =
transRotSecToRotLay.getElement(1,0) * transSec(0,1) +
transRotSecToRotLay.getElement(1,1) * transSec(1,1) +
transRotSecToRotLay.getElement(1,2) * transSec(2,1);
Double_t R7 =
transRotSecToRotLay.getElement(1,0) * transSec(0,2) +
transRotSecToRotLay.getElement(1,1) * transSec(1,2) +
transRotSecToRotLay.getElement(1,2) * transSec(2,2);
const TVectorD &sv = site->getStateVec(Kalman::kPredicted);
TVector3 posAt;
if(!HKalTrackState::calcPosAtPlane(posAt, site->getHitMeasLayer(), sv)) {
if(getPrintErr()) {
Error("calcProjector()", "Could not extract a position from track state");
}
return kFALSE;
}
const TVector3 &n = site->getHitMeasLayer().getNormal();
if(getRotation() != Kalman::kNoRot) {
posAt.Transform(transSec);
}
Double_t vWire = fSizesCellsCell.getWirePos();
Double_t v =
transRotSecToRotLay.getElement(1,0) *
(posAt.X()-transVecRotLayToSec.X()) +
transRotSecToRotLay.getElement(1,1) *
(posAt.Y()-transVecRotLayToSec.Y()) +
transRotSecToRotLay.getElement(1,2) *
(posAt.Z()-transVecRotLayToSec.Z());
Double_t dv = v - vWire;
Double_t tv = R1 * sv(kIdxTanPhi) + R4 * sv(kIdxTanTheta) + R7;
Double_t cv = 1. / TMath::Sqrt (1. + TMath::Power(tv, 2.));
Double_t cv3 = 1. / TMath::Power(1. + TMath::Power(tv, 2.), 1.5);
fProj(0, kIdxX0) = (R1 - R7 * n.X()/n.z()) * cv;
fProj(0, kIdxY0) = (R4 - R7 * n.Y()/n.Z()) * cv;
fProj(0, kIdxTanPhi) = - dv * R1 * tv * cv3;
fProj(0, kIdxTanTheta) = - dv * R4 * tv * cv3;
fProj(0, kIdxQP) = 0.;
site->setStateProjMat(Kalman::kFiltered, fProj);
return kTRUE;
}
Bool_t HKalFiltWire::calcMeasVecFromState(TVectorD &projMeasVec, HKalTrackSite const* const site,
Kalman::kalFilterTypes stateType,
Kalman::coordSys sys) const {
const TVectorD &sv = site->getStateVec(stateType);
TVector3 posAt;
if(!HKalTrackState::calcPosAtPlane(posAt, site->getHitVirtPlane(), sv)) {
if(getPrintErr()) {
Error("calcMeasVecFromState()", "Could not extract position vector from track state.");
}
return kFALSE;
}
TVector3 dir;
HKalTrackState::calcDir(dir, sv);
Double_t mindist, alpha;
Int_t s = site->getSector();
Int_t m = site->getModule();
Int_t l = site->getLayer();
Int_t c = site->getCell(0);
Double_t driftTime = TMath::Abs(site->getHitDriftTime(0));
Bool_t bOk = getImpact(alpha, mindist, driftTime, posAt, dir, s, m, l, c);
projMeasVec(0) = mindist;
return bOk;
}
Bool_t HKalFiltWire::getImpact(Double_t& alpha, Double_t& mindist,
Double_t driftTime,
const TVector3 &pos, const TVector3 dir,
Int_t sec, Int_t mod, Int_t lay, Int_t cell) const {
TVector3 pos1 = pos - dir;
TVector3 pos2 = pos + dir;
if(getRotation() != Kalman::kNoRot) {
TRotation rotInv = getRotMat()->Inverse();
pos1.Transform(rotInv);
pos2.Transform(rotInv);
}
HMdcSizesCells *fSizesCells = HMdcSizesCells::getExObject();
if(!fSizesCells) {
fSizesCells = HMdcSizesCells::getObject();
fSizesCells->initContainer();
}
HMdcSizesCellsLayer &fSizesCellsLayer = (*fSizesCells)[sec][mod][lay];
HMdcSizesCellsCell &fSizesCellsCell = (*fSizesCells)[sec][mod][lay][cell];
HGeomVector &wire1 = fSizesCellsCell.getWirePnt1();
TVector3 wirePoint1(wire1.X(), wire1.Y(), wire1.Z());
HGeomVector &wire2 = fSizesCellsCell.getWirePnt2();
TVector3 wirePoint2(wire2.X(), wire2.Y(), wire2.Z());
Int_t iflag;
Double_t length;
TVector3 pcaFinal, pcaWire;
HKalGeomTools::Track2ToLine(pcaFinal, pcaWire, iflag,
mindist, length,
TVector3(pos1.x(),pos1.y(),pos1.z()),
TVector3(pos2.x(),pos2.y(),pos2.z()),
wirePoint1, wirePoint2);
Double_t yw, zw = 0.;
fSizesCellsLayer.getYZinWireSys(pcaFinal.x(), pcaFinal.y(), pcaFinal.z(),
cell, yw, zw);
if(yw < 0) mindist *= -1.;
if(iflag != 0) {
if(getPrintErr()) {
Error("getImpact()", "Error calculating point of closest approach.");
}
return kFALSE;
}
return kTRUE;
}