ROOT logo
#include "hades.h"
#include "hmdccal2par.h"
#include "hmdcsizescells.h"
#include "hruntimedb.h"

#include "hkalfiltwire.h"
#include "hkalgeomtools.h"


//_HADES_CLASS_DESCRIPTION
///////////////////////////////////////////////////////////////////////////////
//
// Kalman filter for wire hits. See doc for HKalIFilter
//-----------------------------------------------------------------------
///////////////////////////////////////////////////////////////////////////////

ClassImp (HKalFiltWire)

//  -----------------------------------
//  Ctors and Dtor
//  -----------------------------------

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

//  -----------------------------------
//  Implementation of protected methods
//  -----------------------------------

Bool_t HKalFiltWire::calcProjector(Int_t iSite) const {
    // The projection function h(a) extracts a measurement vector from a track state a.
    // The projector matrix is the Jacobian of this function, i.e. @h(a)/@a.
    // The projector matrix is stored in the site's filtered track state.

    HKalTrackSite *site = getSite(iSite);
    if(!site) {
        return kFALSE;
    }

    Int_t mdim = getMeasDim();
    Int_t sdim = getStateDim();

    TMatrixD fProj(mdim, sdim);

    fProj.Zero();

    // Matrix that transforms from track coordinate system (sector coordinates
    // rotated in initial track direction) to sector coordinate system.
    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];

    // Track state has to be transformed from track to sector and then from
    // sector to rotated layer coordinates.
    // Calculate the composition R of both rotations.
    // For the projector matrix, only the matrix elements for the calculation
    // of the v component matter.

    // Rotation matrix from rotated layer system to sector coordinates.
    // Needs to be inverted.
    const HGeomRotation& transRotLaySysRSec =
	fSizesCellsLayer.getRotLaySysRSec(c).getRotMatrix();
    HGeomRotation transRotSecToRotLay = transRotLaySysRSec.inverse();
    HGeomVector transVecRotLayToSec =
	fSizesCellsLayer.getRotLaySysRSec(c).getTransVector();

    // Compose track to sector rotation and sector to rotated layer rotation.
    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);

    // Wire coordinate system:
    // Axis U points along wire.
    // Axis V is on layer and perpendicular to wire.
    // Axis W is perpendicular to U and V.

    //     W
    //
    //     ^
    //     |  |------------*----|
    //     |  | cell        *  -|---ThetaV
    //     |  |            / *  |
    //     |  |           /90 * |
    //     |  | driftDist/     *|
    //     |  |         /       *
    //   --|--|--------*^-|-----|*---------> V
    //     |  |      Wire |     | *
    //     |  |           |     |  *
    //        |         ThetaV  |   *
    //        |                 |    *
    //        |-----------------|     * track

    // Projection function:
    // h(a) = driftDist = v * cos(ThetaV) = v / sqrt(1 + tan(ThetaV)^2)

    // In order to calculate the drift radius from a track state with position
    // vector (x,y,z) on the measurement layer and track direction tangents tx
    // and ty, transform the position and direction vectors to the rotated
    // layer coordinate system.

    const TVectorD &sv = site->getStateVec(Kalman::kPredicted);

    // Transform track position to wire system. v is component on the layer
    // that is perpendicular to the wire.
    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();

    // Rotate from track to sector coordinates.
    if(getRotation() != Kalman::kNoRot) {
	posAt.Transform(transSec);
    }

    // Transform to rotated layer coordinates.
    Double_t vWire = fSizesCellsCell.getWirePos();
    // Transform position from state vector to rotated layer coordinates.
    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;

    // Transform track direction vector (tx,ty,1).
    // tv = Tan(ThetaV) = Tan(pv / pw).
    Double_t tv = R1 * sv(kIdxTanPhi) + R4 * sv(kIdxTanTheta) + R7;

    // cosine of angle ThetaV.
    // cos(alpha)  = 1. / sqrt(1 + tan(alpha)^2)
    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;
}

//  --------------------------------
//  Implementation of public methods
//  --------------------------------

Bool_t HKalFiltWire::calcMeasVecFromState(TVectorD &projMeasVec, HKalTrackSite const* const site,
					  Kalman::kalFilterTypes stateType,
					  Kalman::coordSys sys) const {
    // Extracts measurement vector from state vector for segment hits.
    //
    // Output:
    // projMeasVec: The calculated measurement vector.
    // Input:
    // site:      Pointer to current site of the track.
    // stateType: Which state (predicted, filtered, smoothed) to project on a
    //            measurement vector.
    // sys:       Coordinate system to use (sector or virtual layer coordinates)
    // iHit:      Index of the competitor in the measurement hit.

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

    // State vector is in sector coordinates.
    // The projection of the track state onto a measurement vector is
    // the insection of a straight line defined by the track position
    // and direction with the drift chamber cell.

    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 {
    // Calculates impact angle and drift distance of a straight line defined
    // by position and direction with an MDC cell. Returns true if the line
    // intersects the drift cell, otherwise false.
    //
    // Output:
    // alpha:   Impact angle, 0° <= alpha <= 90°.
    // mindist: Drift distance.
    //
    // Input:
    // driftTime: drift time
    // pos: track position.
    // dir: track direction.
    // sec/mod/lay/cell: Sector, module, layer, drift cell number.

    TVector3 pos1 = pos - dir;
    TVector3 pos2 = pos + dir;

    // getImpact() from fSizesCellsLayer expects sector coordinates.
    if(getRotation() != Kalman::kNoRot) {
        TRotation rotInv = getRotMat()->Inverse();
        pos1.Transform(rotInv);
        pos2.Transform(rotInv);
    }

    HMdcSizesCells *fSizesCells = HMdcSizesCells::getExObject();  // check if is already there
    if(!fSizesCells) {
        fSizesCells = HMdcSizesCells::getObject();
        fSizesCells->initContainer();
    }

    HMdcSizesCellsLayer  &fSizesCellsLayer  = (*fSizesCells)[sec][mod][lay];

    // Calculate the impact angle alpha of the track with the drift cell.

    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;
}
 hkalfiltwire.cc:1
 hkalfiltwire.cc:2
 hkalfiltwire.cc:3
 hkalfiltwire.cc:4
 hkalfiltwire.cc:5
 hkalfiltwire.cc:6
 hkalfiltwire.cc:7
 hkalfiltwire.cc:8
 hkalfiltwire.cc:9
 hkalfiltwire.cc:10
 hkalfiltwire.cc:11
 hkalfiltwire.cc:12
 hkalfiltwire.cc:13
 hkalfiltwire.cc:14
 hkalfiltwire.cc:15
 hkalfiltwire.cc:16
 hkalfiltwire.cc:17
 hkalfiltwire.cc:18
 hkalfiltwire.cc:19
 hkalfiltwire.cc:20
 hkalfiltwire.cc:21
 hkalfiltwire.cc:22
 hkalfiltwire.cc:23
 hkalfiltwire.cc:24
 hkalfiltwire.cc:25
 hkalfiltwire.cc:26
 hkalfiltwire.cc:27
 hkalfiltwire.cc:28
 hkalfiltwire.cc:29
 hkalfiltwire.cc:30
 hkalfiltwire.cc:31
 hkalfiltwire.cc:32
 hkalfiltwire.cc:33
 hkalfiltwire.cc:34
 hkalfiltwire.cc:35
 hkalfiltwire.cc:36
 hkalfiltwire.cc:37
 hkalfiltwire.cc:38
 hkalfiltwire.cc:39
 hkalfiltwire.cc:40
 hkalfiltwire.cc:41
 hkalfiltwire.cc:42
 hkalfiltwire.cc:43
 hkalfiltwire.cc:44
 hkalfiltwire.cc:45
 hkalfiltwire.cc:46
 hkalfiltwire.cc:47
 hkalfiltwire.cc:48
 hkalfiltwire.cc:49
 hkalfiltwire.cc:50
 hkalfiltwire.cc:51
 hkalfiltwire.cc:52
 hkalfiltwire.cc:53
 hkalfiltwire.cc:54
 hkalfiltwire.cc:55
 hkalfiltwire.cc:56
 hkalfiltwire.cc:57
 hkalfiltwire.cc:58
 hkalfiltwire.cc:59
 hkalfiltwire.cc:60
 hkalfiltwire.cc:61
 hkalfiltwire.cc:62
 hkalfiltwire.cc:63
 hkalfiltwire.cc:64
 hkalfiltwire.cc:65
 hkalfiltwire.cc:66
 hkalfiltwire.cc:67
 hkalfiltwire.cc:68
 hkalfiltwire.cc:69
 hkalfiltwire.cc:70
 hkalfiltwire.cc:71
 hkalfiltwire.cc:72
 hkalfiltwire.cc:73
 hkalfiltwire.cc:74
 hkalfiltwire.cc:75
 hkalfiltwire.cc:76
 hkalfiltwire.cc:77
 hkalfiltwire.cc:78
 hkalfiltwire.cc:79
 hkalfiltwire.cc:80
 hkalfiltwire.cc:81
 hkalfiltwire.cc:82
 hkalfiltwire.cc:83
 hkalfiltwire.cc:84
 hkalfiltwire.cc:85
 hkalfiltwire.cc:86
 hkalfiltwire.cc:87
 hkalfiltwire.cc:88
 hkalfiltwire.cc:89
 hkalfiltwire.cc:90
 hkalfiltwire.cc:91
 hkalfiltwire.cc:92
 hkalfiltwire.cc:93
 hkalfiltwire.cc:94
 hkalfiltwire.cc:95
 hkalfiltwire.cc:96
 hkalfiltwire.cc:97
 hkalfiltwire.cc:98
 hkalfiltwire.cc:99
 hkalfiltwire.cc:100
 hkalfiltwire.cc:101
 hkalfiltwire.cc:102
 hkalfiltwire.cc:103
 hkalfiltwire.cc:104
 hkalfiltwire.cc:105
 hkalfiltwire.cc:106
 hkalfiltwire.cc:107
 hkalfiltwire.cc:108
 hkalfiltwire.cc:109
 hkalfiltwire.cc:110
 hkalfiltwire.cc:111
 hkalfiltwire.cc:112
 hkalfiltwire.cc:113
 hkalfiltwire.cc:114
 hkalfiltwire.cc:115
 hkalfiltwire.cc:116
 hkalfiltwire.cc:117
 hkalfiltwire.cc:118
 hkalfiltwire.cc:119
 hkalfiltwire.cc:120
 hkalfiltwire.cc:121
 hkalfiltwire.cc:122
 hkalfiltwire.cc:123
 hkalfiltwire.cc:124
 hkalfiltwire.cc:125
 hkalfiltwire.cc:126
 hkalfiltwire.cc:127
 hkalfiltwire.cc:128
 hkalfiltwire.cc:129
 hkalfiltwire.cc:130
 hkalfiltwire.cc:131
 hkalfiltwire.cc:132
 hkalfiltwire.cc:133
 hkalfiltwire.cc:134
 hkalfiltwire.cc:135
 hkalfiltwire.cc:136
 hkalfiltwire.cc:137
 hkalfiltwire.cc:138
 hkalfiltwire.cc:139
 hkalfiltwire.cc:140
 hkalfiltwire.cc:141
 hkalfiltwire.cc:142
 hkalfiltwire.cc:143
 hkalfiltwire.cc:144
 hkalfiltwire.cc:145
 hkalfiltwire.cc:146
 hkalfiltwire.cc:147
 hkalfiltwire.cc:148
 hkalfiltwire.cc:149
 hkalfiltwire.cc:150
 hkalfiltwire.cc:151
 hkalfiltwire.cc:152
 hkalfiltwire.cc:153
 hkalfiltwire.cc:154
 hkalfiltwire.cc:155
 hkalfiltwire.cc:156
 hkalfiltwire.cc:157
 hkalfiltwire.cc:158
 hkalfiltwire.cc:159
 hkalfiltwire.cc:160
 hkalfiltwire.cc:161
 hkalfiltwire.cc:162
 hkalfiltwire.cc:163
 hkalfiltwire.cc:164
 hkalfiltwire.cc:165
 hkalfiltwire.cc:166
 hkalfiltwire.cc:167
 hkalfiltwire.cc:168
 hkalfiltwire.cc:169
 hkalfiltwire.cc:170
 hkalfiltwire.cc:171
 hkalfiltwire.cc:172
 hkalfiltwire.cc:173
 hkalfiltwire.cc:174
 hkalfiltwire.cc:175
 hkalfiltwire.cc:176
 hkalfiltwire.cc:177
 hkalfiltwire.cc:178
 hkalfiltwire.cc:179
 hkalfiltwire.cc:180
 hkalfiltwire.cc:181
 hkalfiltwire.cc:182
 hkalfiltwire.cc:183
 hkalfiltwire.cc:184
 hkalfiltwire.cc:185
 hkalfiltwire.cc:186
 hkalfiltwire.cc:187
 hkalfiltwire.cc:188
 hkalfiltwire.cc:189
 hkalfiltwire.cc:190
 hkalfiltwire.cc:191
 hkalfiltwire.cc:192
 hkalfiltwire.cc:193
 hkalfiltwire.cc:194
 hkalfiltwire.cc:195
 hkalfiltwire.cc:196
 hkalfiltwire.cc:197
 hkalfiltwire.cc:198
 hkalfiltwire.cc:199
 hkalfiltwire.cc:200
 hkalfiltwire.cc:201
 hkalfiltwire.cc:202
 hkalfiltwire.cc:203
 hkalfiltwire.cc:204
 hkalfiltwire.cc:205
 hkalfiltwire.cc:206
 hkalfiltwire.cc:207
 hkalfiltwire.cc:208
 hkalfiltwire.cc:209
 hkalfiltwire.cc:210
 hkalfiltwire.cc:211
 hkalfiltwire.cc:212
 hkalfiltwire.cc:213
 hkalfiltwire.cc:214
 hkalfiltwire.cc:215
 hkalfiltwire.cc:216
 hkalfiltwire.cc:217
 hkalfiltwire.cc:218
 hkalfiltwire.cc:219
 hkalfiltwire.cc:220
 hkalfiltwire.cc:221
 hkalfiltwire.cc:222
 hkalfiltwire.cc:223
 hkalfiltwire.cc:224
 hkalfiltwire.cc:225
 hkalfiltwire.cc:226
 hkalfiltwire.cc:227
 hkalfiltwire.cc:228
 hkalfiltwire.cc:229
 hkalfiltwire.cc:230
 hkalfiltwire.cc:231
 hkalfiltwire.cc:232
 hkalfiltwire.cc:233
 hkalfiltwire.cc:234
 hkalfiltwire.cc:235
 hkalfiltwire.cc:236
 hkalfiltwire.cc:237
 hkalfiltwire.cc:238
 hkalfiltwire.cc:239
 hkalfiltwire.cc:240
 hkalfiltwire.cc:241
 hkalfiltwire.cc:242
 hkalfiltwire.cc:243
 hkalfiltwire.cc:244
 hkalfiltwire.cc:245
 hkalfiltwire.cc:246
 hkalfiltwire.cc:247
 hkalfiltwire.cc:248
 hkalfiltwire.cc:249
 hkalfiltwire.cc:250
 hkalfiltwire.cc:251
 hkalfiltwire.cc:252
 hkalfiltwire.cc:253
 hkalfiltwire.cc:254
 hkalfiltwire.cc:255
 hkalfiltwire.cc:256
 hkalfiltwire.cc:257
 hkalfiltwire.cc:258
 hkalfiltwire.cc:259
 hkalfiltwire.cc:260
 hkalfiltwire.cc:261
 hkalfiltwire.cc:262
 hkalfiltwire.cc:263
 hkalfiltwire.cc:264
 hkalfiltwire.cc:265
 hkalfiltwire.cc:266
 hkalfiltwire.cc:267
 hkalfiltwire.cc:268
 hkalfiltwire.cc:269
 hkalfiltwire.cc:270
 hkalfiltwire.cc:271
 hkalfiltwire.cc:272
 hkalfiltwire.cc:273
 hkalfiltwire.cc:274
 hkalfiltwire.cc:275
 hkalfiltwire.cc:276
 hkalfiltwire.cc:277
 hkalfiltwire.cc:278
 hkalfiltwire.cc:279
 hkalfiltwire.cc:280
 hkalfiltwire.cc:281
 hkalfiltwire.cc:282
 hkalfiltwire.cc:283
 hkalfiltwire.cc:284
 hkalfiltwire.cc:285
 hkalfiltwire.cc:286
 hkalfiltwire.cc:287
 hkalfiltwire.cc:288
 hkalfiltwire.cc:289
 hkalfiltwire.cc:290
 hkalfiltwire.cc:291
 hkalfiltwire.cc:292