MpdKalmanFilter.cxx 95 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754175517561757175817591760176117621763176417651766176717681769177017711772177317741775177617771778177917801781178217831784178517861787178817891790179117921793179417951796179717981799180018011802180318041805180618071808180918101811181218131814181518161817181818191820182118221823182418251826182718281829183018311832183318341835183618371838183918401841184218431844184518461847184818491850185118521853185418551856185718581859186018611862186318641865186618671868186918701871187218731874187518761877187818791880188118821883188418851886188718881889189018911892189318941895189618971898189919001901190219031904190519061907190819091910191119121913191419151916191719181919192019211922192319241925192619271928192919301931193219331934193519361937193819391940194119421943194419451946194719481949195019511952195319541955195619571958195919601961196219631964196519661967196819691970197119721973197419751976197719781979198019811982198319841985198619871988198919901991199219931994199519961997199819992000200120022003200420052006200720082009201020112012201320142015201620172018201920202021202220232024202520262027202820292030203120322033203420352036203720382039204020412042204320442045204620472048204920502051205220532054205520562057205820592060206120622063206420652066206720682069207020712072207320742075207620772078207920802081208220832084208520862087208820892090209120922093209420952096209720982099210021012102210321042105210621072108210921102111211221132114211521162117211821192120212121222123212421252126212721282129213021312132213321342135213621372138213921402141214221432144214521462147214821492150215121522153215421552156215721582159216021612162216321642165216621672168216921702171217221732174217521762177217821792180218121822183218421852186218721882189219021912192219321942195219621972198219922002201220222032204220522062207220822092210221122122213221422152216221722182219222022212222222322242225222622272228222922302231223222332234223522362237223822392240224122422243224422452246224722482249225022512252225322542255225622572258225922602261226222632264226522662267226822692270227122722273227422752276227722782279228022812282228322842285228622872288228922902291229222932294229522962297229822992300230123022303230423052306230723082309231023112312231323142315231623172318231923202321232223232324232523262327232823292330233123322333233423352336233723382339234023412342234323442345234623472348234923502351235223532354235523562357235823592360236123622363236423652366236723682369237023712372237323742375237623772378237923802381238223832384238523862387238823892390239123922393239423952396239723982399240024012402240324042405240624072408240924102411241224132414241524162417241824192420242124222423242424252426242724282429243024312432243324342435243624372438243924402441244224432444244524462447244824492450245124522453245424552456245724582459246024612462246324642465246624672468246924702471247224732474247524762477247824792480248124822483248424852486248724882489249024912492249324942495249624972498249925002501250225032504250525062507250825092510251125122513251425152516251725182519252025212522252325242525252625272528252925302531253225332534253525362537253825392540254125422543254425452546254725482549255025512552255325542555255625572558255925602561256225632564256525662567256825692570257125722573257425752576257725782579258025812582258325842585258625872588258925902591259225932594259525962597259825992600260126022603260426052606260726082609261026112612261326142615261626172618261926202621262226232624262526262627262826292630263126322633263426352636263726382639264026412642264326442645264626472648264926502651265226532654265526562657265826592660266126622663266426652666
  1. /// \class MpdKalmanFilter
  2. ///
  3. /// Kalman filter track reconstructor in the MPD detector
  4. /// \author Alexander Zinchenko (LHEP, JINR, Dubna)
  5. #include "MpdKalmanFilter.h"
  6. #include "MpdKalmanTrack.h"
  7. #include "MpdKalmanHit.h"
  8. #include "MpdKalmanGeoScheme.h"
  9. #include "MpdCodeTimer.h"
  10. #include "FairField.h"
  11. //#include "FairRootManager.h"
  12. #include "FairRunAna.h"
  13. #include "FairTask.h"
  14. #include "MpdConstField.h"
  15. #include "MpdMultiField.h"
  16. #include <TMath.h>
  17. #include <TGeoManager.h>
  18. #include <TClonesArray.h>
  19. #include <TLorentzVector.h>
  20. //#include "mkl.h"
  21. //#include "mkl_lapacke.h"
  22. #ifdef _OPENMP
  23. #include "omp.h"
  24. omp_lock_t geoManagerLock;
  25. #endif
  26. #include <iostream>
  27. using std::cout;
  28. using std::endl;
  29. const Int_t MpdKalmanFilter::fgkTriesMax = 1000;
  30. const Double_t MpdKalmanFilter::fgkEpsilon = 0.001;
  31. MpdKalmanFilter* MpdKalmanFilter::fgKF = 0x0;
  32. //__________________________________________________________________________
  33. MpdKalmanFilter::MpdKalmanFilter(const char *name,
  34. const char *title)
  35. : FairTask("Kalman Filter engine"),
  36. fGeoScheme(new MpdKalmanGeoScheme), fNumer(1)
  37. {
  38. /// Constructor
  39. fgKF = this;
  40. }
  41. //__________________________________________________________________________
  42. MpdKalmanFilter* MpdKalmanFilter::Instance()
  43. {
  44. /// Get pointer to the Kalman filter reconstructor singleton object
  45. if (!fgKF){
  46. fgKF = new MpdKalmanFilter;
  47. #ifdef _OPENMP
  48. omp_init_lock(&geoManagerLock);
  49. #endif
  50. // automatic destroy is supposed
  51. std::atexit(DestroyInstance);
  52. }
  53. return fgKF;
  54. }
  55. //__________________________________________________________________________
  56. MpdKalmanFilter* MpdKalmanFilter::Instance(const char *name, const char *title)
  57. {
  58. /// Get pointer to the Kalman filter reconstructor singleton object
  59. if (!fgKF){
  60. fgKF = new MpdKalmanFilter(name, title);
  61. #ifdef _OPENMP
  62. omp_init_lock(&geoManagerLock);
  63. #endif
  64. // automatic destroy is supposed
  65. std::atexit(DestroyInstance);
  66. }
  67. return fgKF;
  68. }
  69. //__________________________________________________________________________
  70. MpdKalmanFilter::~MpdKalmanFilter()
  71. {
  72. /// Destructor
  73. //FairRootManager *manager = FairRootManager::Instance();
  74. //manager->Write();
  75. delete fGeoScheme;
  76. fgKF = NULL;
  77. }
  78. //__________________________________________________________________________
  79. InitStatus MpdKalmanFilter::Init() {
  80. cout << "InitStatus MpdKalmanFilter::Init\n\n";
  81. // Check if mag. field was set - if not create the default
  82. fMagField = FairRunAna::Instance()->GetField();
  83. if (!fMagField || TMath::Abs(fMagField->GetBz(0,0,0)) < 0.01) {
  84. cout << " -I- Using the default constant magnetic field Bz = 5 kG " << endl;
  85. fMagField= new MpdMultiField();
  86. MpdConstField *field = new MpdConstField();
  87. field->SetField(0., 0., 5. ); // values are in kG
  88. field->SetFieldRegion(-230, 230, -230, 230, -375, 375); //cm
  89. ((MpdMultiField*)fMagField)->AddField(field);
  90. FairRunAna::Instance()->SetField(fMagField);
  91. fMagField->Init();
  92. cout << " -I- The magnetic field at (0,0,0) = (" << fMagField->GetBx(0,0,0) << ","
  93. << fMagField->GetBy(0,0,0) << "," << fMagField->GetBz(0,0,0) << ") kG" << endl;
  94. } else {
  95. cout << " -I- The magnetic field at (0,0,0) = (" << fMagField->GetBx(0,0,0) << ","
  96. << fMagField->GetBy(0,0,0) << "," << fMagField->GetBz(0,0,0) << ") kG" << endl;
  97. cout<<"OVERHEAD FIELD INFO: "<<((MpdMultiField*)fMagField)->GetFieldList()->UncheckedAt(0)<<
  98. " "<<((FairField*)((MpdMultiField*)fMagField)->GetFieldList()->UncheckedAt(0))->GetType()<<endl;
  99. }
  100. fJacob.ResizeTo(5,5);
  101. return kSUCCESS;
  102. }
  103. //__________________________________________________________________________
  104. InitStatus MpdKalmanFilter::ReInit()
  105. {
  106. return kSUCCESS;
  107. }
  108. //__________________________________________________________________________
  109. void MpdKalmanFilter::Reset()
  110. {
  111. ///
  112. //cout << " MpdKalmanFilter::Reset " << endl;
  113. }
  114. //__________________________________________________________________________
  115. void MpdKalmanFilter::Register()
  116. {
  117. ///
  118. }
  119. //__________________________________________________________________________
  120. void MpdKalmanFilter::Finish()
  121. {
  122. ///
  123. }
  124. //__________________________________________________________________________
  125. void MpdKalmanFilter::Exec(Option_t * option)
  126. {
  127. ///
  128. }
  129. //__________________________________________________________________________
  130. void MpdKalmanFilter::GoOutward(MpdKalmanTrack *track)
  131. {
  132. /// Propagate track in the outward direction
  133. TMatrixDSym weight = *track->GetWeight(); // save starting weight matrix
  134. //RunKalmanFilter(track);
  135. //track->SetLastLay();
  136. BackTrace(track, weight);
  137. }
  138. //__________________________________________________________________________
  139. void MpdKalmanFilter::BackTrace(MpdKalmanTrack *track, TMatrixDSym &weight0)
  140. {
  141. /// Propagate track using already found hits
  142. track->StartBack();
  143. Int_t nHits = track->GetNofHits();
  144. if (nHits == 1) track->SetWeight(weight0); // restore original weight
  145. TMatrixD param(5,1);
  146. TMatrixDSym weight(5), pointWeight(5);
  147. for (Int_t i = 1; i < nHits; ++i) {
  148. MpdKalmanHit *hit = (MpdKalmanHit*) track->GetHits()->UncheckedAt(i);
  149. PropagateToHit(track, hit);
  150. Double_t dChi2 = 0.;
  151. dChi2 = FilterHit(track,hit,pointWeight,param);
  152. track->SetChi2(track->GetChi2()+dChi2);
  153. weight = *track->GetWeight();
  154. weight += pointWeight;
  155. track->SetWeight(weight);
  156. track->SetParamNew(param);
  157. }
  158. }
  159. //__________________________________________________________________________
  160. Bool_t MpdKalmanFilter::PropagateToHit(MpdKalmanTrack *track, const MpdKalmanHit *hit,
  161. Bool_t calcLeng, Bool_t local, Double_t stepBack)
  162. {
  163. /// Propagate track to given hit
  164. // Start timing
  165. //if (FairRun::Instance()->GetTask("Code timer")) MpdCodeTimer::Instance()->Start(Class()->GetName(),__FUNCTION__);
  166. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Start(Class()->GetName(),__FUNCTION__);
  167. track->SetParam(*track->GetParamNew());
  168. track->SetPos(track->GetPosNew());
  169. track->SetNode(track->GetNodeNew());
  170. //TVector3 pos = fGeoScheme->GlobalPos(hit);
  171. Double_t sign = 1.;
  172. if (hit->GetType() == MpdKalmanHit::kFixedR) {
  173. //if (TMath::Abs(pos.Pt() - track->GetPos()) < fgkEpsilon) return kTRUE; // the "same" pos
  174. if (track->GetNode() == "" && TMath::Abs(hit->GetPos() - track->GetPos()) < fgkEpsilon) {
  175. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  176. return kTRUE; // the "same" pos
  177. }
  178. if (!PropagateParamR(track, hit, calcLeng)) {
  179. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  180. return kFALSE;
  181. }
  182. Double_t phi0 = track->GetParamNew(0) / track->GetPosNew();
  183. Double_t dphi = Proxim(phi0, track->GetParamNew(2)) - phi0;
  184. sign = TMath::Sign (1.,-dphi);
  185. } else if (hit->GetType() == MpdKalmanHit::kFixedZ) {
  186. //if (TMath::Abs(pos.Z() - track->GetPos()) < fgkEpsilon) return kTRUE; // the same pos
  187. if (TMath::Abs(hit->GetPos() - track->GetPos()) < fgkEpsilon) {
  188. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  189. return kTRUE; // the same pos
  190. }
  191. if (!PropagateParamZ(track, hit, calcLeng)) {
  192. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  193. return kFALSE;
  194. }
  195. } else {
  196. if (!PropagateParamP(track, hit, calcLeng, local, stepBack)) {
  197. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  198. return kFALSE;
  199. }
  200. if (track->GetNodeNew() != "") {
  201. Double_t v3[7];
  202. SetGeantParamB(track,v3,1);
  203. Double_t phi0 = TMath::ATan2 (v3[1], v3[0]);
  204. Double_t dphi = Proxim(phi0, track->GetParamNew(2)) - phi0;
  205. sign = TMath::Sign (1.,-dphi);
  206. }
  207. }
  208. //cout << " PropagateParam ok " << track->GetPos() << " " << track->GetPosNew() << endl;
  209. //if (!PropagateWeight(track, hit, sign, stepBack)) return kFALSE;
  210. if (!PropagateWeight(track, hit, sign, stepBack)) {
  211. // 15-may-12
  212. track->SetParamNew(*track->GetParam());
  213. track->SetPosNew(track->GetPos());
  214. track->SetNodeNew(track->GetNode());
  215. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  216. return kFALSE;
  217. }
  218. //cout << " PropagateWeight ok " << track->GetPos() << " " << track->GetPosNew() << endl;
  219. if (track->GetType() != MpdKalmanTrack::kEndcap && hit->GetType() == MpdKalmanHit::kFixedZ)
  220. track->SetType(MpdKalmanTrack::kEndcap);
  221. else if (track->GetType() == MpdKalmanTrack::kEndcap && hit->GetType() != MpdKalmanHit::kFixedZ)
  222. track->SetType(MpdKalmanTrack::kBarrel); // change of track type
  223. // Stop timing
  224. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  225. return kTRUE;
  226. }
  227. //__________________________________________________________________________
  228. Bool_t MpdKalmanFilter::PropagateParamToHit(MpdKalmanTrack *track, const MpdKalmanHit *hit,
  229. Bool_t calcLeng, Bool_t local, Double_t stepBack)
  230. {
  231. /// Propagate track parameters to given hit
  232. // Start timing
  233. //if (FairRun::Instance()->GetTask("Code timer")) MpdCodeTimer::Instance()->Start(Class()->GetName(),__FUNCTION__);
  234. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Start(Class()->GetName(),__FUNCTION__);
  235. track->SetParam(*track->GetParamNew());
  236. track->SetPos(track->GetPosNew());
  237. track->SetNode(track->GetNodeNew());
  238. //TVector3 pos = fGeoScheme->GlobalPos(hit);
  239. Double_t sign = 1.;
  240. if (hit->GetType() == MpdKalmanHit::kFixedR) {
  241. //if (TMath::Abs(pos.Pt() - track->GetPos()) < fgkEpsilon) return kTRUE; // the "same" pos
  242. if (TMath::Abs(hit->GetPos() - track->GetPos()) < fgkEpsilon) {
  243. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  244. return kTRUE; // the "same" pos
  245. }
  246. if (!PropagateParamR(track, hit, calcLeng)) {
  247. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  248. return kFALSE;
  249. }
  250. Double_t phi0 = track->GetParamNew(0) / track->GetPosNew();
  251. Double_t dphi = Proxim(phi0, track->GetParamNew(2)) - phi0;
  252. sign = TMath::Sign (1.,-dphi);
  253. } else if (hit->GetType() == MpdKalmanHit::kFixedZ) {
  254. //if (TMath::Abs(pos.Z() - track->GetPos()) < fgkEpsilon) return kTRUE; // the same pos
  255. if (TMath::Abs(hit->GetPos() - track->GetPos()) < fgkEpsilon) {
  256. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  257. return kTRUE; // the same pos
  258. }
  259. if (!PropagateParamZ(track, hit, calcLeng)) {
  260. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  261. return kFALSE;
  262. }
  263. } else {
  264. if (!PropagateParamP(track, hit, calcLeng, local, stepBack)) {
  265. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  266. return kFALSE;
  267. }
  268. if (track->GetNodeNew() != "") {
  269. Double_t v3[7];
  270. SetGeantParamB(track,v3,1);
  271. Double_t phi0 = TMath::ATan2 (v3[1], v3[0]);
  272. Double_t dphi = Proxim(phi0, track->GetParamNew(2)) - phi0;
  273. sign = TMath::Sign (1.,-dphi);
  274. }
  275. }
  276. /*
  277. //cout << " PropagateParam ok " << track->GetPos() << " " << track->GetPosNew() << endl;
  278. //if (!PropagateWeight(track, hit, sign, stepBack)) return kFALSE;
  279. if (!PropagateWeight(track, hit, sign, stepBack)) {
  280. // 15-may-12
  281. track->SetParamNew(*track->GetParam());
  282. track->SetPosNew(track->GetPos());
  283. track->SetNodeNew(track->GetNode());
  284. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  285. return kFALSE;
  286. }
  287. //cout << " PropagateWeight ok " << track->GetPos() << " " << track->GetPosNew() << endl;
  288. if (track->GetType() != MpdKalmanTrack::kEndcap && hit->GetType() == MpdKalmanHit::kFixedZ)
  289. track->SetType(MpdKalmanTrack::kEndcap);
  290. else if (track->GetType() == MpdKalmanTrack::kEndcap && hit->GetType() != MpdKalmanHit::kFixedZ)
  291. track->SetType(MpdKalmanTrack::kBarrel); // change of track type
  292. */
  293. // Stop timing
  294. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  295. return kTRUE;
  296. }
  297. //__________________________________________________________________________
  298. Bool_t MpdKalmanFilter::PropagateParamR(MpdKalmanTrack *track, const MpdKalmanHit *hit,
  299. Bool_t calcLeng, Double_t *vert)
  300. {
  301. /// Propagate parameter vector to the hit (from one R to another R)
  302. //TVector3 pos = fGeoScheme->GlobalPos(hit);
  303. //Double_t hitR = pos.Pt(), hitRphi = hitR * pos.Phi();
  304. Double_t hitR = hit->GetPos(), hitRphi = hit->GetMeas(0);
  305. track->SetNodeNew("");
  306. TString node = track->GetNode();
  307. Double_t v3[7], v3new[7], dR = 0, dir = 1, v000[3] = {0.};
  308. if (vert == 0x0) vert = v000;
  309. Int_t nTries = 0;
  310. track->SetParamNew(*track->GetParam());
  311. track->SetPosNew(track->GetPos());
  312. //track->GetParamNew()->Print();
  313. if (track->GetType() == MpdKalmanTrack::kBarrel) SetGeantParamB(track,v3,dir);
  314. else SetGeantParamE(track,v3,dir);
  315. // Propagation step estimate
  316. Double_t r0 = TMath::Sqrt (v3[0]*v3[0]+v3[1]*v3[1]);
  317. Double_t dR0 = hitR - r0;
  318. dir = TMath::Sign(1.0,dR0); // direction
  319. if (dir < 0.) {
  320. for (Int_t i = 3; i < 6; ++i) v3[i] = -v3[i];
  321. if (hitR < 1.e-7) {
  322. // Going to PCA - check if is not already passed
  323. TVector3 tr(v3[3],v3[4],0.0);
  324. TVector3 rad(v3[0]-vert[0],v3[1]-vert[1],0.0);
  325. Double_t sign = TMath::Sign(1.,tr * rad);
  326. if (sign > 0) return kFALSE;
  327. }
  328. }
  329. Double_t step = dR0 / TMath::Cos(track->GetParam(3));
  330. // sign of charge (sign of Pt if forward motion) must be changed if
  331. // backward extrapolation
  332. Double_t charge = -dir * TMath::Sign(1.0,track->GetParam(4));
  333. //cout << " Start:" << endl; for (Int_t i = 0; i < 7; ++i) cout << v3[i] << " "; cout << track->GetR() << " " << hit->GetR() << " " << step << endl;
  334. // Check if overstep or within precision
  335. Double_t step0 = 0.;
  336. do {
  337. step = TMath::Abs(step);
  338. step0 = step;
  339. // Propagate parameters
  340. //ExtrapOneStepRungekutta(charge,step,v3,v3new);
  341. ExtrapOneStepHelix(charge,step,v3,v3new);
  342. Double_t extrapRad = TMath::Sqrt (v3new[0]*v3new[0]+v3new[1]*v3new[1]);
  343. if (hitR > 1.e-7) {
  344. dR = hitR - extrapRad;
  345. step *= dR0 / (extrapRad - r0);
  346. } else {
  347. // Find PCA
  348. //TVector3 tr(v3new[0]-v3[0],v3new[1]-v3[1],v3new[2]-v3[2]); // in 3-D
  349. //TVector3 rad(-v3new[0],-v3new[1],-v3new[2]);
  350. TVector3 tr(v3new[3],v3new[4],0.0); // in 2-D
  351. TVector3 rad(v3new[0]-vert[0],v3new[1]-vert[1],0.0);
  352. Double_t sign = TMath::Sign(1.,tr * rad);
  353. //TVector3 tr(v3new[0]-v3[0],v3new[1]-v3[1]); // in 2-D
  354. //TVector3 rad(-v3new[0],-v3new[1]);
  355. //Double_t sign = -TMath::Sign(1.,tr * rad);
  356. dR = extrapRad * sign;
  357. //step *= dR0 / (-sign * extrapRad - r0);
  358. step *= dR0 / (-dR - r0);
  359. }
  360. nTries ++;
  361. } while (dR * dir < 0 && TMath::Abs(dR) > fgkEpsilon);
  362. //for (Int_t i = 0; i < 7; ++i) cout << v3new[i] << " "; cout << nTries << endl;
  363. GetFromGeantParamB(track,v3new,dir);
  364. if (calcLeng) track->UpdateLength(step0); // update track length
  365. track->SetNode("");
  366. //track->GetParamNew()->Print();
  367. // Position adjustment (until within tolerance)
  368. step = 1.;
  369. step0 = -1.;
  370. while (TMath::Abs(dR) > fgkEpsilon && step > fgkEpsilon) {
  371. dR0 = hitR - track->GetPosNew();
  372. Double_t dir1 = TMath::Sign(1.0,dR0);
  373. TMatrixD *parNew = track->GetParamNew();
  374. //Double_t dPhi = hitR > 1.e-7 ?
  375. //hitRphi / hitR - (*parNew)(2,0) : hitRphi - (*parNew)(2,0);
  376. Double_t dPhi = 0.; // 24-04-11
  377. if (hitR > 1.e-7 || step0 < 0) step = dR0 / TMath::Cos(dPhi) / TMath::Cos((*parNew)(3,0));
  378. else step = step0;
  379. step = TMath::Abs(step);
  380. SetGeantParamB(track,v3,dir1);
  381. do {
  382. // binary search
  383. //ExtrapOneStepRungekutta(charge,step,v3,v3new);
  384. ExtrapOneStepHelix(charge,step,v3,v3new);
  385. Double_t extrapRad = TMath::Sqrt (v3new[0]*v3new[0]+v3new[1]*v3new[1]);
  386. // !!! Check this if backpropagation is needed
  387. if (hitR > 1.e-7) {
  388. if (dir1*extrapRad < dir1*track->GetPosNew()) {
  389. if (track->GetType() == MpdKalmanTrack::kBarrel) return kFALSE; // curling track
  390. if (dir1*v3[2] < dir1*v3new[2]) return kFALSE; // curling track
  391. }
  392. dR = hitR - extrapRad;
  393. } else {
  394. // Find PCA
  395. //TVector3 tr(v3new[0]-v3[0],v3new[1]-v3[1],v3new[2]-v3[2]); // in 3-D
  396. //TVector3 rad(-v3new[0],-v3new[1],-v3new[2]);
  397. TVector3 tr(v3new[3],v3new[4],0.0); // in 2-D
  398. TVector3 rad(v3new[0]-vert[0],v3new[1]-vert[1],0.0);
  399. Double_t sign = TMath::Sign(1.,tr * rad);
  400. dR = sign;
  401. //TVector3 tr(v3new[0]-v3[0],v3new[1]-v3[1]); // in 2-D
  402. //TVector3 rad(-v3new[0],-v3new[1]);
  403. //Double_t sign = -TMath::Sign(1.,tr * rad);
  404. //dR = sign;
  405. }
  406. // !!!
  407. step0 = step;
  408. step /= 2.;
  409. nTries ++;
  410. if (nTries > fgkTriesMax) {
  411. cout << " Too many tries: " << nTries << endl;
  412. //exit(0);
  413. return kFALSE;
  414. }
  415. //exit(0);
  416. //if (nTries > 4) cout << " *** " << nTries << " " << dPhi << " " << step << " " << extrapRad << endl;
  417. } while (dR*dir1 < 0);
  418. GetFromGeantParamB(track,v3new,dir1);
  419. if (calcLeng) track->UpdateLength(step0); // update track length
  420. } // while (TMath::Abs(dR) > fgkEpsilon && step > fgkEpsilon
  421. /*
  422. if (hitR < 1.e-7) {
  423. cout << " Tries: " << nTries << " " << dR << " " << step << " " << track->GetPosNew() << endl;
  424. //for (Int_t i = 0; i < 7; ++i) cout << v3new[i] << " "; cout << endl;
  425. //if (track->GetParamNew(3) < 0) { cout << track->GetParamNew(3) << endl; exit(0); }
  426. track->GetParamNew()->Print();
  427. }
  428. */
  429. //if (hitR < 1.e-7) cout << "nTries: " << nTries << endl;
  430. track->SetNode(node);
  431. return kTRUE;
  432. }
  433. //__________________________________________________________________________
  434. void MpdKalmanFilter::SetGeantParamB(MpdKalmanTrack *track, Double_t *v3, Double_t dir)
  435. {
  436. /// Set vector of Geant3 parameters pointed to by "v3"
  437. /// from track parameters (for "barrel" tracks)
  438. TMatrixD *parNew = track->GetParamNew();
  439. Double_t rad = track->GetPosNew();
  440. if (track->GetNode() == "") {
  441. // Global coordinates
  442. //Double_t phi = 0;
  443. Double_t phi = (*parNew)(2,0);
  444. if (rad > 1.e-7) phi = (*parNew)(0,0) / rad;
  445. v3[0] = rad * TMath::Cos(phi); // X
  446. v3[1] = rad * TMath::Sin(phi); // Y
  447. v3[2] = (*parNew)(1,0); // Z
  448. } else {
  449. // Local coordinates
  450. //Double_t pos[3] = {(*parNew)(0,0), (*parNew)(1,0), rad};
  451. Double_t pos[3] = {-(*parNew)(0,0), (*parNew)(1,0), rad};
  452. if (track->GetUniqueID()) {
  453. // ITS
  454. pos[0] = -pos[0]; //
  455. pos[1] = rad;
  456. pos[2] = (*parNew)(1,0);
  457. }
  458. #ifdef _OPENMP
  459. omp_set_lock(&geoManagerLock);
  460. #endif
  461. gGeoManager->cd(track->GetNode());
  462. if (track->GetUniqueID() == 0) gGeoManager->CdUp(); // TPC
  463. gGeoManager->LocalToMaster(pos, v3);
  464. #ifdef _OPENMP
  465. omp_unset_lock(&geoManagerLock);
  466. #endif
  467. }
  468. Double_t cosTh = TMath::Cos((*parNew)(3,0)); // cos(Theta)
  469. v3[3] = dir * TMath::Cos((*parNew)(2,0)) * cosTh; // Px/Ptot
  470. v3[4] = dir * TMath::Sin((*parNew)(2,0)) * cosTh; // Py/Ptot
  471. v3[5] = dir * TMath::Sin((*parNew)(3,0)); // Pz/Ptot
  472. v3[6] = 1. / TMath::Abs((*parNew)(4,0)) / cosTh; // Ptot
  473. }
  474. //__________________________________________________________________________
  475. void MpdKalmanFilter::GetFromGeantParamB(MpdKalmanTrack *track,
  476. Double_t *v3, Double_t dir)
  477. {
  478. /// Get track parameters from vector of Geant3 parameters pointed
  479. /// to by "v3" (for "barrel" tracks)
  480. TMatrixD *parNew = track->GetParamNew();
  481. if (track->GetNodeNew() == "") {
  482. // Global coordinates
  483. Double_t rad = TMath::Sqrt (v3[0]*v3[0]+v3[1]*v3[1]);
  484. Double_t phi = TMath::ATan2 (v3[1],v3[0]);
  485. track->SetPosNew(rad);
  486. (*parNew)(0,0) = rad * phi; // RPhi
  487. (*parNew)(1,0) = v3[2]; // Z
  488. } else {
  489. // Local coordinates
  490. Double_t pos[3];
  491. #ifdef _OPENMP
  492. omp_set_lock(&geoManagerLock);
  493. #endif
  494. gGeoManager->cd(track->GetNodeNew());
  495. if (track->GetUniqueID() == 0) gGeoManager->CdUp(); // !!! for TPC
  496. gGeoManager->MasterToLocal(v3, pos);
  497. #ifdef _OPENMP
  498. omp_unset_lock(&geoManagerLock);
  499. #endif
  500. //(*parNew)(0,0) = pos[0];
  501. if (track->GetUniqueID()) {
  502. // ITS
  503. (*parNew)(0,0) = pos[0]; //
  504. (*parNew)(1,0) = pos[2];
  505. track->SetPosNew(pos[1]);
  506. } else {
  507. (*parNew)(0,0) = -pos[0]; //
  508. (*parNew)(1,0) = pos[1];
  509. track->SetPosNew(pos[2]);
  510. }
  511. }
  512. Double_t phi0 = (*parNew)(2,0);
  513. (*parNew)(2,0) = TMath::ATan2 (dir*v3[4],dir*v3[3]); // track Phi
  514. (*parNew)(2,0) = Proxim (phi0, (*parNew)(2,0)); // adjust angle for continuity
  515. (*parNew)(3,0) = TMath::ASin(dir*v3[5]); // Theta
  516. (*parNew)(4,0) = 1. / v3[6] / TMath::Cos((*parNew)(3,0)) *
  517. TMath::Sign(1.,(*parNew)(4,0)); // q/Pt
  518. }
  519. //__________________________________________________________________________
  520. Bool_t MpdKalmanFilter::PropagateParamZ(MpdKalmanTrack *track, const MpdKalmanHit *hit,
  521. Bool_t calcLeng)
  522. {
  523. /// Propagate parameter vector to the hit at fixed Z
  524. if (!fNumer && track->GetType() == MpdKalmanTrack::kEndcap && hit->GetType() == MpdKalmanHit::kFixedZ) {
  525. // Analytical extrapolation
  526. //TMatrixD tmp = *track->GetParamNew();
  527. AnalParamZ(track, hit, calcLeng);
  528. //track->SetParamNew(tmp);
  529. track->SetPosNew(hit->GetDist());
  530. return kTRUE;
  531. }
  532. Double_t hitZ = hit->GetPos();
  533. // Propagation step estimate
  534. Double_t dZ0 = hitZ - track->GetZ();
  535. Double_t step = dZ0 / TMath::Sin(track->GetParam(3));
  536. // Track parameter extrapolation to Z using Rungekutta algorithm.
  537. //dZ0 = TMath::Abs(hitZ) - TMath::Abs(track->GetZ());
  538. //Double_t dir = TMath::Sign(1.0,dZ0); // direction
  539. Double_t dir = TMath::Sign(1.0,TMath::Abs(hitZ)-TMath::Abs(track->GetZ())); // direction
  540. // sign of charge (sign of Pt if forward motion) must be changed if
  541. // backward extrapolation
  542. Double_t charge = -dir * TMath::Sign(1.0,track->GetParam(4));
  543. //if (track->GetDirection() == TpcLheKalmanTrack::kOutward) charge = -charge;
  544. Double_t v3[7], v3new[7], dZ = 0;
  545. Int_t nTries = 0;
  546. track->SetParamNew(*track->GetParam());
  547. track->SetPosNew(track->GetPos());
  548. //track->GetParamNew()->Print();
  549. if (track->GetType() == MpdKalmanTrack::kBarrel) SetGeantParamB(track,v3,dir);
  550. else SetGeantParamE(track,v3,dir);
  551. //cout << " Start:" << endl; for (Int_t i = 0; i < 7; ++i) cout << v3[i] << " "; cout << track->GetR() << " " << hit->GetR() << " " << step << endl;
  552. // Check if overstep or within precision
  553. Double_t step0 = 0.;
  554. do {
  555. step = TMath::Abs(step);
  556. step0 = step;
  557. // Propagate parameters
  558. //ExtrapOneStepRungekutta(charge,step,v3,v3new);
  559. ExtrapOneStepHelix(charge,step,v3,v3new);
  560. Double_t extrapZ = v3new[2];
  561. //for (Int_t i = 0; i < 7; ++i) cout << v3new[i] << " "; cout << extrapRad << endl;
  562. //dZ = hitZ - extrapZ;
  563. dZ = TMath::Abs(hitZ) - TMath::Abs(extrapZ);
  564. step *= dZ0 / (extrapZ - track->GetZ());
  565. nTries ++;
  566. } while (dZ * dir < 0 && TMath::Abs(dZ) > fgkEpsilon);
  567. GetFromGeantParamE(track,v3new,dir);
  568. if (calcLeng) track->UpdateLength(step0); // update track length
  569. //track->GetParamNew()->Print();
  570. // Position adjustment (until within tolerance)
  571. while (TMath::Abs(dZ) > fgkEpsilon) {
  572. dZ0 = hitZ - track->GetPosNew();
  573. //Double_t dir1 = TMath::Sign(1.0,dZ0);
  574. Double_t dir1 = TMath::Sign(1.0,TMath::Abs(hitZ)-TMath::Abs(track->GetPosNew()));
  575. TMatrixD *parNew = track->GetParamNew();
  576. step = dZ0 / TMath::Sin((*parNew)(3,0));
  577. step = TMath::Abs(step);
  578. SetGeantParamE(track,v3,dir1);
  579. do {
  580. // binary search
  581. //ExtrapOneStepRungekutta(charge,step,v3,v3new);
  582. ExtrapOneStepHelix(charge,step,v3,v3new);
  583. Double_t extrapZ = v3new[2];
  584. // !!! Check if backpropagation is needed
  585. //if (dir1*extrapRad < dir1*track->GetPosNew()) return kFALSE; // curling track
  586. // !!!
  587. //dZ = hitZ - extrapZ;
  588. dZ = TMath::Abs(hitZ) - TMath::Abs(extrapZ);
  589. step0 = step;
  590. step /= 2.;
  591. nTries ++;
  592. if (nTries > fgkTriesMax) {
  593. cout << " Too many tries: " << nTries << endl;
  594. //exit(0);
  595. return kFALSE;
  596. }
  597. //if (nTries > 4) cout << " *** " << nTries << " " << dPhi << " " << step << " " << extrapRad << endl;
  598. } while (dZ*dir1 < 0);
  599. GetFromGeantParamE(track,v3new,dir1);
  600. if (calcLeng) track->UpdateLength(step0); // update track length
  601. }
  602. //cout << " Tries: " << nTries << " " << dR << endl;
  603. //for (Int_t i = 0; i < 7; ++i) cout << v3new[i] << " "; cout << endl;
  604. //if (track->GetParamNew(3) < 0) { cout << track->GetParamNew(3) << endl; exit(0); }
  605. //track->GetParamNew()->Print();
  606. return kTRUE;
  607. }
  608. //__________________________________________________________________________
  609. Bool_t MpdKalmanFilter::AnalParamZ(MpdKalmanTrack *track, const MpdKalmanHit *hit,
  610. Bool_t calcLeng)
  611. {
  612. /// Propagate parameter vector to the hit at fixed Z analytically
  613. static Int_t first = 1;
  614. static Double_t coef = 0.;
  615. if (first) {
  616. first = 0;
  617. Double_t bZ = TMath::Abs (fMagField->GetBz(0,0,0));
  618. coef = 0.3 * 0.01 * bZ / 10;
  619. }
  620. Double_t dz = hit->GetDist() - track->GetPos();
  621. Double_t r = 1. / track->GetParam(4) / coef;
  622. Double_t ph0 = track->GetParam(2);
  623. Double_t ph = ph0 + dz / r / TMath::Tan(track->GetParam(3));
  624. Double_t x = track->GetParam(0) + r * (TMath::Sin(ph) - TMath::Sin(ph0));
  625. Double_t y = track->GetParam(1) - r * (TMath::Cos(ph) - TMath::Cos(ph0));
  626. TMatrixD &parNew = *track->GetParamNew();
  627. parNew(0,0) = x;
  628. parNew(1,0) = y;
  629. parNew(2,0) = ph;
  630. parNew(3,0) = track->GetParam(3);
  631. parNew(4,0) = track->GetParam(4);
  632. if (calcLeng) {
  633. Double_t step = r * (ph - ph0);
  634. track->UpdateLength(TMath::Abs(step/TMath::Cos(track->GetParam(3)))); // update track length
  635. }
  636. return kTRUE;
  637. }
  638. //__________________________________________________________________________
  639. Bool_t MpdKalmanFilter::AnalParamX(MpdKalmanTrack *track, const MpdKalmanHit *hit,
  640. Bool_t calcLeng, Bool_t local)
  641. {
  642. /// Propagate parameter vector to the hit at fixed X analytically
  643. static Int_t first = 1;
  644. static Double_t coef = 0.;
  645. if (first) {
  646. first = 0;
  647. Double_t bZ = TMath::Abs (fMagField->GetBz(0,0,0));
  648. coef = 0.3 * 0.01 * bZ / 10;
  649. }
  650. //cout << track->GetNode() << " " << fGeoScheme->Path(hit->GetDetectorID()) << endl;
  651. // Coordinate transformation
  652. gGeoManager->cd(fGeoScheme->Path(hit->GetDetectorID()));
  653. if (track->GetUniqueID() == 0) gGeoManager->CdUp(); // !!! for TPC
  654. Double_t v3[7], v31[3], dir = 1.0;
  655. fGeoScheme->GlobalPos(hit).GetXYZ(v3);
  656. gGeoManager->MasterToLocal(v3, v31);
  657. v31[0] = -v31[0]; //
  658. Double_t dx = v31[2], posNew = v31[2];
  659. if (track->GetUniqueID()) dx = posNew = v31[1]; // ITS
  660. Double_t phSec = MpdKalmanFilter::Proxim (track->GetParam(2),TMath::ATan2(v3[1],v3[0]));
  661. TString pathHit = gGeoManager->GetPath();
  662. TString pathTra = track->GetNode();
  663. if (track->GetUniqueID() == 0) {
  664. //if (track->GetUniqueID() >= 0) {
  665. // TPC
  666. Int_t ip = pathTra.Last('/');
  667. if (ip > 0) pathTra.Replace(ip,pathTra.Length()-ip+1,"");
  668. } //else cout << pathHit << " " << pathTra << endl;
  669. Double_t y0 = track->GetParam(0);
  670. Double_t z0 = track->GetParam(1);
  671. Int_t changeVol = 0;
  672. if (pathTra != pathHit) {
  673. if (track->GetDirection() == MpdKalmanTrack::kInward) dir = -1.0;
  674. track->SetParamNew(*track->GetParam());
  675. track->SetPosNew(track->GetPos());
  676. SetGeantParamB(track,v3,dir); // global coordinates
  677. gGeoManager->cd(pathHit);
  678. gGeoManager->MasterToLocal(v3, v31);
  679. if (track->GetUniqueID() == 0) {
  680. // TPC
  681. v31[0] = -v31[0]; //
  682. y0 = v31[0];
  683. z0 = v31[1];
  684. dx -= v31[2];
  685. } else {
  686. v31[0] = -v31[0]; //
  687. y0 = v31[0];
  688. z0 = v31[2];
  689. dx -= v31[1];
  690. }
  691. // Change of volume - set flag to use numerical weight propagation
  692. changeVol = 1;
  693. //} else dx -= track->GetPos();
  694. } else {
  695. dx -= track->GetPos();
  696. if (track->GetUniqueID()) y0 = -y0; // ITS
  697. }
  698. Double_t r = 1. / track->GetParam(4) / coef;
  699. //Double_t r = -1. / track->GetParam(4) / coef;
  700. Double_t ph0 = track->GetParam(2) - phSec;
  701. //Double_t ph0 = -track->GetParam(2) + phSec;
  702. Double_t arg = TMath::Sin(ph0) + dx / r;
  703. if (TMath::Abs(arg) > 0.999999) return kFALSE;
  704. Double_t ph = TMath::ASin (arg);
  705. Double_t y = y0 - r * (TMath::Cos(ph) - TMath::Cos(ph0));
  706. Double_t step = r * (ph - ph0);
  707. Double_t z = z0 + step * TMath::Tan(track->GetParam(3));
  708. TMatrixD &parNew = *track->GetParamNew();
  709. parNew(0,0) = y;
  710. if (track->GetUniqueID()) parNew(0,0) = -y; //
  711. parNew(1,0) = z;
  712. parNew(2,0) = ph + phSec;
  713. //parNew(2,0) = -ph + phSec;
  714. parNew(3,0) = track->GetParam(3);
  715. parNew(4,0) = track->GetParam(4);
  716. track->SetPosNew(posNew);
  717. if (calcLeng) track->UpdateLength(TMath::Abs(step/TMath::Cos(track->GetParam(3)))); // update track length
  718. track->SetChi2Vertex(dx); // just to pass value
  719. if (changeVol) track->SetChi2Vertex(999999);
  720. return kTRUE;
  721. }
  722. //__________________________________________________________________________
  723. Bool_t MpdKalmanFilter::PropagateParamP(MpdKalmanTrack *track, const MpdKalmanHit *hit,
  724. Bool_t calcLeng, Bool_t local, Double_t stepBack)
  725. {
  726. /// Propagate parameter vector to the hit at fixed plane
  727. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Start(Class()->GetName(),__FUNCTION__);
  728. if (!fNumer && track->GetType() == MpdKalmanTrack::kBarrel && hit->GetType() == MpdKalmanHit::kFixedP) {
  729. // Analytical extrapolation
  730. if (local) track->SetNodeNew(fGeoScheme->Path(hit->GetDetectorID())); // in local coordinates
  731. AnalParamX(track, hit, calcLeng, kTRUE);
  732. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  733. return kTRUE;
  734. }
  735. TVector3 pos = fGeoScheme->GlobalPos(hit);
  736. TVector3 norm = fGeoScheme->Normal(hit);
  737. Double_t plane[6] = {0};
  738. pos.GetXYZ(plane);
  739. norm.GetXYZ(&plane[3]);
  740. //cout << fGeoScheme->Path(hit->GetDetectorID()) << endl;
  741. //ITS if (local) track->SetNodeNew(fGeoScheme->Path(hit->GetDetectorID() % 1000000 | 1)); // in local coordinates
  742. if (local) track->SetNodeNew(fGeoScheme->Path(hit->GetDetectorID())); // in local coordinates
  743. if (!PropagateParamP(track, plane, calcLeng, local, stepBack)) {
  744. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  745. return kFALSE;
  746. }
  747. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  748. return kTRUE;
  749. }
  750. //__________________________________________________________________________
  751. Bool_t MpdKalmanFilter::PropagateParamP(MpdKalmanTrack *track, const Double_t *plane,
  752. Bool_t calcLeng, Bool_t local, Double_t stepBack)
  753. {
  754. /// Propagate parameter vector to the plane
  755. /// plane[0-2] - x, y, z of a point on the plane;
  756. /// plane[3-5] - direction cosines of the normal to the plane
  757. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Start(Class()->GetName(),__FUNCTION__);
  758. Double_t dir = 1., norm = 1., scalar = 0., normal[3] = {0}, normLeng = 0;
  759. if (track->Momentum() < 0.001) return kFALSE; //AZ - safety 19.04.2020
  760. for (Int_t i = 0; i < 3; ++i) {
  761. normal[i] = plane[i+3];
  762. normLeng += normal[i] * normal[i];
  763. scalar += plane[i] * normal[i];
  764. }
  765. normLeng = TMath::Sqrt (normLeng); // length of the normal vector
  766. //if (scalar < 0) norm = -1; // normal direction
  767. if (scalar < 0 && (TMath::Abs(plane[0]) > 3. || TMath::Abs(plane[1]) > 3.)) norm = -1; // normal direction
  768. normLeng *= norm;
  769. for (Int_t i = 0; i < 3; ++i) normal[i] /= normLeng; // "canonical" normal vector
  770. Double_t v3[7], v3new[7];
  771. Int_t nTries = 0;
  772. track->SetParamNew(*track->GetParam());
  773. track->SetPosNew(track->GetPos());
  774. //Double_t dZ0 = hit->GetZ() - track->GetZ();
  775. //Double_t dir = TMath::Sign(1.0,dZ0); // direction
  776. if (track->GetDirection() == MpdKalmanTrack::kInward) dir = -1.;
  777. Double_t charge = -dir * TMath::Sign(1.0,track->GetParam(4));
  778. if (track->GetType() == MpdKalmanTrack::kBarrel) SetGeantParamB(track,v3,dir);
  779. else {
  780. SetGeantParamE(track,v3,dir);
  781. //Fatal(" PropagateParamP "," !!! Local track parameterization is not implemented for Endcap tracks !!!");
  782. }
  783. Double_t d = 0.; // Ax+By+Cz+D=0, A=nx, B=ny, C=nz
  784. for (Int_t i = 0; i < 3; ++i) d -= plane[i] * normal[i];
  785. // Distance to the plane
  786. Double_t dist0 = d;
  787. for (Int_t i = 0; i < 3; ++i) dist0 += v3[i] * normal[i];
  788. if (TMath::Abs(dist0) < fgkEpsilon) {
  789. GetFromGeantParamB(track,v3,dir); // 11-may-12
  790. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  791. return kTRUE; // 02-11-11
  792. }
  793. if (dist0 * dir > 0) {
  794. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  795. return kFALSE; // track already passed the plane
  796. }
  797. //if (dist0 < 0) return kFALSE; // track last point behind plane (farther from interaction point)
  798. // Angle between momentum and plane normal
  799. Double_t cosA = 0;
  800. for (Int_t i = 0; i < 3; ++i) cosA += v3[i+3] * normal[i];
  801. if (TMath::Abs(cosA) < 0.5) cosA = 0.5; // to constrain step size
  802. // Propagation step estimate
  803. Double_t step = dist0 / cosA;
  804. //step = TMath::Max (TMath::Abs(step), 0.5*TMath::Abs(TMath::Sqrt(v3[0]*v3[0]+v3[1]*v3[1])-
  805. // TMath::Sqrt(plane[0]*plane[0]+plane[1]*plane[1])));
  806. if (stepBack > 0.0) step = 0.9 * stepBack;
  807. //cout << " " << norm << " " << normLeng << " " << step << " " << dist0 << endl;
  808. //cout << " Start:" << endl; for (Int_t i = 0; i < 7; ++i) cout << v3[i] << " "; cout << track->GetR() << " " << hit->GetR() << " " << step << endl;
  809. // Check if overstep or within precision
  810. Double_t step0 = 0., dist = 0.;
  811. do {
  812. step = TMath::Abs(step);
  813. step0 = step;
  814. // Propagate parameters
  815. //ExtrapOneStepRungekutta(charge,step,v3,v3new);
  816. ExtrapOneStepHelix(charge,step,v3,v3new);
  817. dist = d;
  818. for (Int_t i = 0; i < 3; ++i) dist += v3new[i] * normal[i];
  819. //for (Int_t i = 0; i < 7; ++i) cout << v3new[i] << " "; cout << endl;
  820. //step *= dist0 / (TMath::Abs(dist0) - TMath::Abs(dist));
  821. step *= dist0 / (dist0 - dist);
  822. nTries ++;
  823. //} while (dist * dir < 0 && TMath::Abs(dist) > fgkEpsilon);
  824. } while (dist * dir > 0 && TMath::Abs(dist) > fgkEpsilon);
  825. //} while (dist * dir * norm > 0 && TMath::Abs(dist) > fgkEpsilon);
  826. GetFromGeantParamE(track,v3new,dir);
  827. if (calcLeng) track->UpdateLength(step0); // update track length
  828. //track->GetParamNew()->Print();
  829. // Position adjustment (until within tolerance)
  830. Double_t dir1 = dir;
  831. //dist0 = TMath::Abs(dist);
  832. //AZ dist0 = dist;
  833. if (TMath::Abs(dist) < TMath::Abs(dist0)) dist0 = dist; //AZ
  834. while (TMath::Abs(dist) > fgkEpsilon) {
  835. dist = d;
  836. cosA = 0.;
  837. //dir = 1.;
  838. for (Int_t i = 0; i < 3; ++i) {
  839. dist += v3new[i] * normal[i];
  840. cosA += v3[i+3] * normal[i];
  841. }
  842. step = dist / cosA;
  843. step = TMath::Abs(step);
  844. dir1 = TMath::Sign(1.,-dist);
  845. //SetGeantParamE(track,v3,dir);
  846. SetGeantParamE(track,v3,dir1);
  847. do {
  848. // binary search
  849. //ExtrapOneStepRungekutta(charge,step,v3,v3new);
  850. ExtrapOneStepHelix(charge,step,v3,v3new);
  851. step0 = step;
  852. step /= 2.;
  853. nTries ++;
  854. if (nTries > fgkTriesMax) {
  855. cout << " Too many tries: " << nTries << endl;
  856. //exit(0);
  857. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  858. return kFALSE;
  859. }
  860. dist = d;
  861. for (Int_t i = 0; i < 3; ++i) dist += v3new[i] * normal[i];
  862. if (TMath::Abs(dist) < fgkEpsilon) break; // 02-11-11
  863. //if (TMath::Abs(dist) > dist0) return kFALSE; // track going in different direction
  864. if (dist > 0 && dist0 > 0 && dist > dist0 ||
  865. dist < 0 && dist0 < 0 && dist < dist0) return kFALSE; // track going in different direction
  866. //} while (dist * dir < 0);
  867. } while (dist * dir > 0);
  868. //} while (dist * dir1 < 0);
  869. //} while (dist*dir*norm > 0);
  870. //GetFromGeantParamE(track,v3new,dir);
  871. GetFromGeantParamE(track,v3new,dir1);
  872. if (calcLeng) track->UpdateLength(step0); // update track length
  873. }
  874. //if (track->GetType() == MpdKalmanTrack::kBarrel) GetFromGeantParamB(track,v3new,dir);
  875. //2-may-12 if (track->GetType() == MpdKalmanTrack::kBarrel) GetFromGeantParamB(track,v3new,dir1);
  876. GetFromGeantParamB(track,v3new,dir1);
  877. //cout << " Tries: " << nTries << endl;
  878. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  879. return kTRUE;
  880. }
  881. //__________________________________________________________________________
  882. void MpdKalmanFilter::SetGeantParamE(MpdKalmanTrack *track, Double_t *v3, Double_t dir)
  883. {
  884. /// Set vector of Geant3 parameters pointed to by "v3"
  885. /// from track parameters (for "end-cap" tracks)
  886. TMatrixD *parNew = track->GetParamNew();
  887. v3[0] = (*parNew)(0,0); // X
  888. v3[1] = (*parNew)(1,0); // Y
  889. v3[2] = track->GetPosNew(); // Z
  890. Double_t cosTh = TMath::Cos((*parNew)(3,0)); // cos(Theta)
  891. v3[3] = dir * TMath::Cos((*parNew)(2,0)) * cosTh; // Px/Ptot
  892. v3[4] = dir * TMath::Sin((*parNew)(2,0)) * cosTh; // Py/Ptot
  893. v3[5] = dir * TMath::Sin((*parNew)(3,0)); // Pz/Ptot
  894. v3[6] = 1. / TMath::Abs((*parNew)(4,0)) / cosTh; // Ptot
  895. }
  896. //__________________________________________________________________________
  897. void MpdKalmanFilter::GetFromGeantParamE(MpdKalmanTrack *track,
  898. Double_t *v3, Double_t dir)
  899. {
  900. /// Get track parameters from vector of Geant3 parameters pointed
  901. /// to by "v3" (for "end-cap" tracks)
  902. TMatrixD *parNew = track->GetParamNew();
  903. track->SetPosNew(v3[2]); // Z
  904. (*parNew)(0,0) = v3[0]; // X
  905. (*parNew)(1,0) = v3[1]; // Y
  906. Double_t phi0 = (*parNew)(2,0);
  907. (*parNew)(2,0) = TMath::ATan2 (dir*v3[4],dir*v3[3]); // track Phi
  908. (*parNew)(2,0) = Proxim (phi0, (*parNew)(2,0)); // adjust angle for continuity
  909. (*parNew)(3,0) = TMath::ASin(dir*v3[5]); // Theta
  910. (*parNew)(4,0) = 1. / v3[6] / TMath::Cos((*parNew)(3,0)) *
  911. TMath::Sign(1.,(*parNew)(4,0)); // q/Pt
  912. }
  913. //__________________________________________________________________________
  914. Bool_t MpdKalmanFilter::FindPca(MpdKalmanTrack *track, Double_t *vert)
  915. {
  916. /// Propagate parameter vector to PCA w.r.t. point vert
  917. //Double_t v3[7], v3new[7], dR = 0, dir0 = -1, dir = -1;
  918. Double_t v3[7], v3new[7], dR = 0, dir0 = 1, dir = 1;
  919. Int_t nTries = 0;
  920. track->SetParamNew(*track->GetParam());
  921. track->SetPosNew(track->GetPos());
  922. //track->GetParamNew()->Print();
  923. if (track->GetType() == MpdKalmanTrack::kBarrel) SetGeantParamB(track,v3,dir);
  924. else SetGeantParamE(track,v3,dir);
  925. Double_t step = 0.1, dist2d = 0;
  926. Double_t charge = -dir * TMath::Sign(1.0,track->GetParam(4));
  927. //for (Int_t i = 3; i < 6; ++i) v3[i] = -v3[i];
  928. //cout << " Start:" << endl; for (Int_t i = 0; i < 7; ++i) cout << v3[i] << " "; cout << track->GetR() << " " << hit->GetR() << " " << step << endl;
  929. // Go to the approximate position of PCA
  930. TVector3 rad(v3[0]-vert[0],v3[1]-vert[1],0.0);
  931. TVector3 tr0(v3[3],v3[4],v3[5]);
  932. dist2d = tr0 * rad / tr0.Pt();
  933. if (dist2d > 0) {
  934. // Change direction
  935. dir = -dir;
  936. for (Int_t i = 3; i < 6; ++i) v3[i] = -v3[i];
  937. }
  938. //dir = TMath::Sign(1.,-dist2d);
  939. step = TMath::Abs (dist2d / TMath::Sin(tr0.Theta()));
  940. TVector3 tr;
  941. for (Int_t i = 0; i < 1; ++i) {
  942. //ExtrapOneStepRungekutta(charge,step,v3,v3new);
  943. ExtrapOneStepHelix(dir*charge,step,v3,v3new);
  944. rad.SetXYZ(v3new[0]-vert[0],v3new[1]-vert[1],0);
  945. tr.SetXYZ(v3new[3],v3new[4],v3new[5]);
  946. dist2d = tr * rad / tr.Pt();
  947. //if (dist2d * dir > 0) {
  948. if (dist2d > 0) {
  949. // Change direction
  950. dir = -dir;
  951. for (Int_t j = 3; j < 6; ++j) v3new[j] = -v3new[j];
  952. }
  953. //dir = TMath::Sign(1.,-dist2d);
  954. step = TMath::Abs (dist2d / TMath::Sin(tr.Theta()));
  955. nTries ++;
  956. }
  957. // Position adjustment (until within tolerance)
  958. //step = 1.;
  959. while (step > fgkEpsilon) {
  960. for (Int_t i = 0; i < 7; ++i) v3[i] = v3new[i];
  961. // binary search
  962. //ExtrapOneStepRungekutta(charge,step,v3,v3new);
  963. ExtrapOneStepHelix(dir*charge,step,v3,v3new);
  964. //TVector3 tr(v3new[0]-v3[0],v3new[1]-v3[1]);
  965. tr.SetXYZ(v3new[3],v3new[4],v3new[5]);
  966. rad.SetXYZ(v3new[0]-vert[0],v3new[1]-vert[1],0.);
  967. dist2d = tr * rad;
  968. //if (dist2d * dir > 0) {
  969. if (dist2d > 0) {
  970. // Change direction
  971. dir = -dir;
  972. step /= 2.;
  973. for (Int_t i = 3; i < 6; ++i) v3new[i] = -v3new[i];
  974. }
  975. ++nTries;
  976. if (nTries > fgkTriesMax) {
  977. cout << " Too many tries: " << nTries << endl;
  978. //exit(0);
  979. return kFALSE;
  980. }
  981. //exit(0);
  982. //if (nTries > 4) cout << " *** " << nTries << " " << dPhi << " " << step << " " << extrapRad << endl;
  983. }
  984. //if (track->GetType() == MpdKalmanTrack::kFixedR) GetFromGeantParamR(track,v3new,dir0);
  985. //else GetFromGeantParamZ(track,v3new,dir0);
  986. tr.SetXYZ(v3new[3],v3new[4],v3new[5]);
  987. dir = 1;
  988. if (tr0*tr < 0) dir = -1;
  989. if (track->GetType() == MpdKalmanTrack::kBarrel) GetFromGeantParamB(track,v3new,dir);
  990. else GetFromGeantParamE(track,v3new,dir);
  991. /*cout << " Tries: " << nTries << " " << dist2d << " " << dir << " "
  992. << track->GetTrackID() << " "
  993. << TMath::Sqrt((v3new[0]-vert[0])*(v3new[0]-vert[0])+
  994. (v3new[1]-vert[1])*(v3new[1]-vert[1])) << endl;
  995. cout << tr0.X() << " " << tr0.Y() << " " << tr0.Z() << " "
  996. << tr.X() << " " << tr.Y() << " " << tr.Z() << endl;*/
  997. return kTRUE;
  998. }
  999. //__________________________________________________________________________
  1000. Double_t MpdKalmanFilter::Proxim(Double_t phi0, Double_t phi, Double_t scale)
  1001. {
  1002. /// Adjust angle phi to be "around" phi0 - to avoid discontinuity around +- Pi:
  1003. /// scale - scale factor due to stereo angle
  1004. Double_t dPhi = phi0 - phi;
  1005. //if (TMath::Abs(dPhi) > TMath::Pi()) phi += TMath::Pi() * 2 * TMath::Sign(1.,dPhi);
  1006. if (TMath::Abs(dPhi) > TMath::Pi()*scale) phi += TMath::Pi() * scale * TMath::Sign(2.,dPhi);
  1007. return phi;
  1008. }
  1009. //__________________________________________________________________________
  1010. Double_t MpdKalmanFilter::Proxim(MpdKalmanTrack *track, const MpdKalmanHit *hit, Double_t scale)
  1011. {
  1012. /// Adjust hit coord. R-Phi to be "around" track R-Phi - to avoid
  1013. /// discontinuity around +- Pi: scale - scale factor due to stereo angle
  1014. //TVector3 pos = fGeoScheme->GlobalPos(hit);
  1015. //Double_t hitPhi = pos.Phi();
  1016. Double_t hitPhi = hit->GetMeas(0) / hit->GetPos();
  1017. Double_t phi0 = track->GetParamNew(0) / track->GetPosNew();
  1018. return hit->GetPos() * Proxim(phi0, hitPhi, scale);
  1019. //return pos.Pt() * Proxim(phi0, hitPhi, scale);
  1020. }
  1021. //__________________________________________________________________________
  1022. Double_t MpdKalmanFilter::Proxim(const MpdKalmanHit *hit0, const MpdKalmanHit *hit, Double_t scale)
  1023. {
  1024. /// Adjust hit coord. R-Phi to be "around" hit0 R-Phi - to avoid
  1025. /// discontinuity around +- Pi: scale - scale factor due to stereo angle
  1026. //TVector3 pos0 = fGeoScheme->GlobalPos(hit0);
  1027. //TVector3 pos = fGeoScheme->GlobalPos(hit);
  1028. //Double_t phi0 = pos0.Phi();
  1029. //Double_t phi = pos.Phi();
  1030. Double_t phi0 = hit0->GetMeas(0) / hit0->GetPos();
  1031. Double_t phi = hit->GetMeas(0) / hit->GetPos();
  1032. return hit->GetPos() * Proxim(phi0, phi, scale);
  1033. //return pos.Pt() * Proxim(phi0, phi, scale);
  1034. }
  1035. //__________________________________________________________________________
  1036. Bool_t MpdKalmanFilter::PropagateWeight(MpdKalmanTrack *track, const MpdKalmanHit *hit, Double_t sign, Double_t stepBack)
  1037. {
  1038. /// Propagation of the weight matrix
  1039. /// W = DtWD, where D is Jacobian
  1040. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Start(Class()->GetName(),__FUNCTION__);
  1041. //TMatrixD jacob(5,5);
  1042. fJacob = 0.0;
  1043. TMatrixD &jacob = fJacob;
  1044. if (!fNumer && track->GetType() == MpdKalmanTrack::kEndcap && hit->GetType() == MpdKalmanHit::kFixedZ) {
  1045. // Analytical track propagation (Jacobian computation)
  1046. AnalyticJacob(track, hit, jacob);
  1047. //jacob.Print();
  1048. jacob.Invert();
  1049. //jacob.Print();
  1050. TMatrixD tmp(*track->GetWeight(),TMatrixD::kMult,jacob); // WD
  1051. TMatrixD weight1(jacob,TMatrixD::kTransposeMult,tmp); // DtWD
  1052. track->SetWeight (weight1);
  1053. //jacob = 0;
  1054. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  1055. return kTRUE;
  1056. }
  1057. if (!fNumer && track->GetType() == MpdKalmanTrack::kBarrel && hit->GetType() == MpdKalmanHit::kFixedP && track->GetChi2Vertex() < 99999) {
  1058. // Analytical track propagation (Jacobian computation) - no ITS at present !!!
  1059. AnalyticJacobX(track, hit, jacob);
  1060. //jacob.Print();
  1061. jacob.Invert();
  1062. //jacob.Print();
  1063. TMatrixD tmp(*track->GetWeight(),TMatrixD::kMult,jacob); // WD
  1064. TMatrixD weight1(jacob,TMatrixD::kTransposeMult,tmp); // DtWD
  1065. track->SetWeight (weight1);
  1066. //jacob = 0;
  1067. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  1068. return kTRUE;
  1069. }
  1070. MpdKalmanTrack tr = *track;
  1071. MpdKalmanHit hitTmp = *hit;
  1072. // Save initial and propagated parameters
  1073. TMatrixD param0 = *tr.GetParam();
  1074. TMatrixD paramNew0 = *tr.GetParamNew();
  1075. Double_t pos0 = tr.GetPosNew();
  1076. Bool_t invert = kTRUE;
  1077. //if (tr.GetDirection() == MpdKalmanTrack::kInward &&
  1078. // tr.GetNode() != "" && tr.GetNodeNew() != "") {
  1079. if (tr.GetDirection() == MpdKalmanTrack::kInward &&
  1080. tr.GetNode() != "" && tr.GetNodeNew() != "" &&
  1081. tr.GetNode() != tr.GetNodeNew()) {
  1082. // Change direction
  1083. param0 = *tr.GetParamNew();
  1084. paramNew0 = *tr.GetParam();
  1085. tr.SetPos(pos0);
  1086. invert = kFALSE;
  1087. TString detName = tr.GetNode();
  1088. if (track->GetUniqueID()) {
  1089. // ITS
  1090. detName = detName(16,detName.Length());
  1091. detName += "#0";
  1092. }
  1093. hitTmp.SetDetectorID(fGeoScheme->DetId(detName));
  1094. tr.SetNode(track->GetNodeNew());
  1095. tr.SetNodeNew(track->GetNode());
  1096. tr.SetDirection(MpdKalmanTrack::kOutward);
  1097. }
  1098. //param0.Print();
  1099. //paramNew0.Print();
  1100. TMatrixDSym *covar = tr.Weight2Cov(); // get covariance matrix
  1101. Double_t rNew = 0., phi0 = 0.;
  1102. if (hit->GetType() == MpdKalmanHit::kFixedR) {
  1103. //if (track->GetType() == MpdKalmanTrack::kBarrel) {
  1104. rNew = pos0; // barrel
  1105. phi0 = paramNew0(0,0) / rNew;
  1106. }
  1107. //covar->Print();
  1108. Bool_t local = kFALSE;
  1109. if (tr.GetNodeNew() != "") local = kTRUE;
  1110. Double_t dPar;
  1111. // Loop over parameters to find change of the propagated vs initial ones
  1112. Bool_t ok = kTRUE;
  1113. for (Int_t i = 0; i < 5; ++i) {
  1114. dPar = TMath::Sqrt((*covar)(i,i));
  1115. if (i < 4) dPar = TMath::Min (dPar, 0.1);
  1116. else dPar = TMath::Min (dPar, 0.1*TMath::Abs(param0(4,0)));
  1117. tr.SetParam(param0);
  1118. if (i == 4) dPar *= TMath::Sign(1.,-param0(4,0)); // 1/p
  1119. else if (i == 2) dPar *= sign;
  1120. else if (i == 3) dPar *= TMath::Sign(1.,-param0(3,0)); // dip-angle
  1121. tr.SetParam(i,param0(i,0)+dPar);
  1122. if (hit->GetType() == MpdKalmanHit::kFixedR) ok = PropagateParamR(&tr, &hitTmp, kFALSE);
  1123. else if (hit->GetType() == MpdKalmanHit::kFixedZ) ok = PropagateParamZ(&tr, &hitTmp, kFALSE);
  1124. //else ok = PropagateParamP(track, hit, kFALSE, kTRUE, stepBack);
  1125. //else ok = PropagateParamP(&tr, &hitTmp, kFALSE, kTRUE);
  1126. else ok = PropagateParamP(&tr, &hitTmp, kFALSE, local);
  1127. if (!ok) {
  1128. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  1129. return kFALSE;
  1130. }
  1131. for (Int_t j = 0; j < 5; ++j) {
  1132. if (j == 0 && hit->GetType() == MpdKalmanHit::kFixedR) {
  1133. Double_t dPhi = Proxim(phi0, tr.GetParamNew(j)/rNew) - phi0;
  1134. jacob(j,i) = dPhi*rNew / dPar;
  1135. /*} else if (j == 0 && hit->GetType() == MpdKalmanHit::kFixedP) {
  1136. Double_t phi = Proxim(phi0, track->GetParamNew(j)/track->GetPosNew());
  1137. jacob(j,i) = (phi * track->GetPosNew() - phi0 * rNew) / dPar;*/
  1138. } else jacob(j,i) = (tr.GetParamNew(j)-paramNew0(j,0)) / dPar;
  1139. //if (hitTmp.GetDetectorID()) cout << i << " " << j << " " << dPar << " " << tr.GetParamNew(j) << " " << paramNew0(j,0) << endl;
  1140. // << " " << track->GetPos() << " " << track->GetPosNew() << " " << jacob(j,i) << endl;
  1141. }
  1142. }
  1143. //jacob.Print();
  1144. if (invert) jacob.Invert();
  1145. TMatrixD tmp(*tr.GetWeight(),TMatrixD::kMult,jacob); // WD
  1146. TMatrixD weight1(jacob,TMatrixD::kTransposeMult,tmp); // DtWD
  1147. //track->SetWeight(TMatrixD(jacob,TMatrixD::kTransposeMult,weight1)); // DtWD
  1148. track->SetWeight (weight1);
  1149. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  1150. return kTRUE;
  1151. }
  1152. //__________________________________________________________________________
  1153. Bool_t MpdKalmanFilter::AnalyticJacob(MpdKalmanTrack *track, const MpdKalmanHit *hit, TMatrixD &jacob)
  1154. {
  1155. /// Analytical computation of Jacobian matrix (matrix of parameter derivatives)
  1156. static Int_t first = 1;
  1157. static Double_t coef = 0.;
  1158. if (first) {
  1159. first = 0;
  1160. Double_t bZ = TMath::Abs (fMagField->GetBz(0,0,0));
  1161. coef = 0.3 * 0.01 * bZ / 10;
  1162. }
  1163. Double_t cosPh1 = TMath::Cos (track->GetParamNew(2));
  1164. Double_t cosPh0 = TMath::Cos (track->GetParam(2));
  1165. Double_t dCosPh = cosPh1 - cosPh0;
  1166. Double_t sinPh1 = TMath::Sin (track->GetParamNew(2));
  1167. Double_t sinPh0 = TMath::Sin (track->GetParam(2));
  1168. Double_t dSinPh = sinPh1 - sinPh0;
  1169. Double_t r = 1. / track->GetParamNew(4) / coef;
  1170. Double_t dz = track->GetPosNew() - track->GetPos();
  1171. Double_t sinTh = TMath::Sin (track->GetParamNew(3));
  1172. Double_t sinTh2 = sinTh * sinTh;
  1173. Double_t tanTh = TMath::Tan (track->GetParamNew(3));
  1174. // dX1 / dP0
  1175. jacob(0,0) = 1.; // dX1/dX0
  1176. jacob(0,2) = r * dCosPh; // dX1/dPh0
  1177. jacob(0,3) = -cosPh1 * dz / sinTh2; // dX1/dTh
  1178. jacob(0,4) = r * (-r * dSinPh + cosPh1 * dz / tanTh); // dX1/d(1/R)
  1179. jacob(0,4) *= coef;
  1180. // dY1 / dP0
  1181. jacob(1,1) = 1.; // dY1/dY0
  1182. jacob(1,2) = r * dSinPh; // dY1/dPh0
  1183. jacob(1,3) = -sinPh1 * dz / sinTh2; // dY1/dTh0
  1184. jacob(1,4) = r * (r * dCosPh + sinPh1 * dz / tanTh); // dY1/d(1/R)
  1185. jacob(1,4) *= coef;
  1186. // dPh1 / dP0
  1187. jacob(2,2) = 1.; // dPh1/dPh0
  1188. jacob(2,3) = -dz / sinTh2 / r; // dPh1/dTh0
  1189. jacob(2,4) = dz / tanTh; // dPh1/d(1/R)
  1190. jacob(2,4) *= coef;
  1191. jacob(3,3) = jacob(4,4) = 1.;
  1192. return kTRUE;
  1193. }
  1194. //__________________________________________________________________________
  1195. Bool_t MpdKalmanFilter::AnalyticJacobX(MpdKalmanTrack *track, const MpdKalmanHit *hit, TMatrixD &jacob)
  1196. {
  1197. /// Analytical computation of Jacobian matrix (matrix of parameter derivatives)
  1198. static Int_t first = 1;
  1199. static Double_t coef = 0.;
  1200. if (first) {
  1201. first = 0;
  1202. Double_t bZ = TMath::Abs (fMagField->GetBz(0,0,0));
  1203. coef = 0.3 * 0.01 * bZ / 10;
  1204. }
  1205. // Sector angle
  1206. gGeoManager->cd(fGeoScheme->Path(hit->GetDetectorID()));
  1207. if (track->GetUniqueID() == 0) gGeoManager->CdUp(); // !!! for TPC
  1208. Double_t v3[7];
  1209. fGeoScheme->GlobalPos(hit).GetXYZ(v3);
  1210. //Double_t phSec = TMath::ATan2(v3[1],v3[0]);
  1211. Double_t phSec = MpdKalmanFilter::Proxim (track->GetParam(2),TMath::ATan2(v3[1],v3[0]));
  1212. Double_t ph1 = track->GetParamNew(2) - phSec;
  1213. Double_t ph0 = track->GetParam(2) - phSec;
  1214. Double_t cosPh1 = TMath::Cos (ph1);
  1215. Double_t cosPh0 = TMath::Cos (ph0);
  1216. Double_t dCosPh = cosPh1 - cosPh0;
  1217. Double_t tanPh1 = TMath::Tan (ph1);
  1218. Double_t r = 1. / track->GetParamNew(4) / coef;
  1219. //Double_t r = -1. / track->GetParamNew(4) / coef;
  1220. //Double_t dx = track->GetPosNew() - track->GetPos();
  1221. Double_t dx = track->GetChi2Vertex();
  1222. Double_t cosTh = TMath::Cos (track->GetParamNew(3));
  1223. Double_t cosTh2 = cosTh * cosTh;
  1224. Double_t tanTh = TMath::Tan (track->GetParamNew(3));
  1225. Double_t dPh = ph1 - ph0;
  1226. // dY1 / dP0
  1227. jacob(0,0) = 1.; // dY1/dY0
  1228. /*
  1229. if (track->GetNode() != "") {
  1230. TString pathTra = track->GetNode();
  1231. TString pathTraNew = track->GetNodeNew();
  1232. Int_t ip = pathTra.Last('/');
  1233. Int_t ipnew = pathTraNew.Last('/');
  1234. if (ip > 0) pathTra.Replace(ip,pathTra.Length()-ip+1,"");
  1235. if (ipnew > 0) pathTraNew.Replace(ipnew,pathTraNew.Length()-ipnew+1,"");
  1236. if (pathTra != pathTraNew)
  1237. { jacob(0,0) = TMath::Cos(TMath::DegToRad()*30.); cout << pathTra << " " << pathTraNew << endl; }
  1238. }
  1239. */
  1240. jacob(0,2) = r * sin(dPh) / cosPh1; // dY1/dPh0
  1241. jacob(0,3) = 0; // dY1/dTh
  1242. jacob(0,4) = r * (r * dCosPh + tanPh1 * dx); // dY1/d(1/R)
  1243. jacob(0,4) *= coef;
  1244. // dZ1 / dP0
  1245. jacob(1,1) = 1; // dZ1/dZ0
  1246. jacob(1,2) = -r * tanTh * dCosPh / cosPh1; // dZ1/dPh0
  1247. //AZ jacob(1,3) = -r * dPh / cosTh2; // dZ1/dTh0
  1248. jacob(1,3) = r * dPh / cosTh2; // dZ1/dTh0
  1249. jacob(1,4) = r * tanTh * (-r * dPh + dx/cosPh1); // dZ1/d(1/R)
  1250. jacob(1,4) *= coef;
  1251. // dPh1 / dP0
  1252. jacob(2,2) = cosPh0 / cosPh1; // dPh1/dPh0
  1253. jacob(2,3) = 0; // dPh1/dTh0
  1254. jacob(2,4) = dx / cosPh1; // dPh1/d(1/R)
  1255. jacob(2,4) *= coef;
  1256. jacob(3,3) = jacob(4,4) = 1.;
  1257. return kTRUE;
  1258. }
  1259. //__________________________________________________________________________
  1260. Double_t MpdKalmanFilter::FilterHit(MpdKalmanTrack *track, const MpdKalmanHit *hit,
  1261. TMatrixDSym &pointWeight, TMatrixD &paramTmp)
  1262. {
  1263. /// Hit filtering
  1264. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Start(Class()->GetName(),__FUNCTION__);
  1265. Double_t chi2 = 0.0;
  1266. //AZ if (hit->GetType() == MpdKalmanHit::kFixedR) return FilterHitR(track, hit, pointWeight, paramTmp);
  1267. if (hit->GetNofDim() > 1 && hit->GetType() != MpdKalmanHit::kFixedZ)
  1268. chi2 = FilterHitR(track, hit, pointWeight, paramTmp);
  1269. else if (hit->GetType() == MpdKalmanHit::kFixedZ) chi2 = FilterHitZ(track, hit, pointWeight, paramTmp);
  1270. else chi2 = FilterStrip(track, hit, pointWeight, paramTmp);
  1271. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  1272. return chi2;
  1273. }
  1274. //__________________________________________________________________________
  1275. Double_t MpdKalmanFilter::FilterHitR(MpdKalmanTrack *track, const MpdKalmanHit *hit,
  1276. TMatrixDSym &pointWeight, TMatrixD &paramTmp)
  1277. {
  1278. /// Compute Chi2 increment if the "barrel" hit is assigned to the track
  1279. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Start(Class()->GetName(),__FUNCTION__);
  1280. TMatrixD point(5,1);
  1281. if (hit->GetType() == MpdKalmanHit::kFixedR) point(0,0) = Proxim(track,hit);
  1282. else point(0,0) = hit->GetMeas(0);
  1283. point(1,0) = hit->GetMeas(1);
  1284. pointWeight = 0.0;
  1285. pointWeight(0,0) = 1. / hit->GetErr(0) / hit->GetErr(0);
  1286. pointWeight(1,1) = 1. / hit->GetErr(1) / hit->GetErr(1);
  1287. // Solving linear system (W+U)p' = U(m-p) + (W+U)p
  1288. //TMatrixD wu = *fWeight;
  1289. TMatrixDSym wu = *track->GetWeight();
  1290. wu += pointWeight; // W+U
  1291. paramTmp = point;
  1292. //paramTmp -= *fTrackParNew; // m-p
  1293. paramTmp -= *track->GetParamNew(); // m-p
  1294. TMatrixD right(pointWeight,TMatrixD::kMult,paramTmp); // U(m-p)
  1295. //TMatrixD right1(wu,TMatrixD::kMult,*fTrackParNew); // (W+U)p
  1296. TMatrixD right1(wu,TMatrixD::kMult,*track->GetParamNew()); // (W+U)p
  1297. right += right1; // U(m-p) + (W+U)p
  1298. //wu.Print();
  1299. //wu.Invert();
  1300. Int_t iok = 0;
  1301. fgKF->MnvertLocal(wu.GetMatrixArray(), 5, 5, 5, iok);
  1302. //wu.Print();
  1303. paramTmp = TMatrixD(wu,TMatrixD::kMult,right);
  1304. right1 = paramTmp;
  1305. right1 -= point; // p'-m
  1306. point = paramTmp;
  1307. point -= *track->GetParamNew(); // p'-p
  1308. right = TMatrixD(*track->GetWeight(),TMatrixD::kMult,point); // W(p'-p)
  1309. TMatrixD value(point,TMatrixD::kTransposeMult,right); // (p'-p)'W(p'-p)
  1310. Double_t dChi2 = value(0,0);
  1311. right = TMatrixD(pointWeight,TMatrixD::kMult,right1); // U(p'-m)
  1312. value = TMatrixD(right1,TMatrixD::kTransposeMult,right); // (p'-m)'U(p'-m)
  1313. dChi2 += value(0,0);
  1314. if (MpdCodeTimer::Active()) MpdCodeTimer::Instance()->Stop(Class()->GetName(),__FUNCTION__);
  1315. return dChi2;
  1316. }
  1317. //__________________________________________________________________________
  1318. Double_t MpdKalmanFilter::FilterHitZ(MpdKalmanTrack *track, const MpdKalmanHit *hit,
  1319. TMatrixDSym &pointWeight, TMatrixD &parFilt)
  1320. {
  1321. /// Compute Chi2 increment if the "end-cap" hit is assigned to the track
  1322. /// (assuming 1-d hit measurement)
  1323. TMatrixD meas(2,1); // measurements
  1324. TMatrixD h(2,5); // transformation matrix from hit meas. to track params
  1325. TMatrixD u(2,2); // weight matrix of measurements
  1326. //h(0,0) = -TMath::Sin(hit->GetPhi());
  1327. //h(0,1) = TMath::Cos(hit->GetPhi());
  1328. h(0,0) = -hit->GetCosSin(1);
  1329. h(0,1) = hit->GetCosSin(0);
  1330. //point(0,0) = Proxim(track,hit);
  1331. meas(0,0) = hit->GetMeas(0);
  1332. //pointWeight(0,0) = 1. / hit->GetRphiErr() / hit->GetRphiErr();
  1333. u(0,0) = 1. / hit->GetErr(0) / hit->GetErr(0);
  1334. if (hit->GetNofDim() > 1) {
  1335. meas(1,0) = hit->GetMeas(1);
  1336. u(1,1) = 1. / hit->GetErr(1) / hit->GetErr(1);
  1337. //h(1,0) = TMath::Sin(hit->GetPhi());
  1338. //h(1,1) = -TMath::Cos(hit->GetPhi());
  1339. h(1,0) = hit->GetCosSin(0);
  1340. h(1,1) = hit->GetCosSin(1);
  1341. }
  1342. // Solving linear system (W+H'UH)p' = H'U(m-Hp) + (W+H'UH)p
  1343. //TMatrixD uh(pointWeight,TMatrixD::kMult,h); // UH
  1344. TMatrixD uh(u,TMatrixD::kMult,h); // UH
  1345. TMatrixD huh(h,TMatrixD::kTransposeMult,uh); // H'UH
  1346. TMatrixDSym huhSym = pointWeight;
  1347. huhSym.SetMatrixArray(huh.GetMatrixArray());
  1348. TMatrixDSym wu = *track->GetWeight();
  1349. wu += huhSym; // W+H'UH
  1350. TMatrixD hp(h,TMatrixD::kMult,*track->GetParamNew()); // Hp
  1351. TMatrixD tmp = meas; // m
  1352. tmp -= hp; // m-Hp
  1353. //TMatrixD right0(pointWeight,TMatrixD::kMult,paramTmp); // U(m-Hp)
  1354. TMatrixD right0(u,TMatrixD::kMult,tmp); // U(m-Hp)
  1355. TMatrixD right(h,TMatrixD::kTransposeMult,right0); // H'U(m-Hp)
  1356. TMatrixD right1(wu,TMatrixD::kMult,*track->GetParamNew()); // (W+H'UH)p
  1357. right += right1; // H'U(m-Hp) + (W+H'UH)p
  1358. //wu.Print();
  1359. //wu.Invert();
  1360. Int_t iok = 0;
  1361. fgKF->MnvertLocal(wu.GetMatrixArray(), 5, 5, 5, iok);
  1362. //wu.Print();
  1363. parFilt = TMatrixD(wu,TMatrixD::kMult,right); // p'
  1364. //parFilt.Print();
  1365. //right1 = h;
  1366. //right1 *= parFilt; // Hp'
  1367. //right1 -= meas; // Hp'-m
  1368. tmp = TMatrixD(h,TMatrixD::kMult,parFilt);
  1369. tmp -= meas;
  1370. TMatrixD point = parFilt;
  1371. point -= *track->GetParamNew(); // p'-p
  1372. right = TMatrixD(*track->GetWeight(),TMatrixD::kMult,point); // W(p'-p)
  1373. TMatrixD value(point,TMatrixD::kTransposeMult,right); // (p'-p)'W(p'-p)
  1374. Double_t dChi2 = value(0,0);
  1375. //right = TMatrixD(pointWeight,TMatrixD::kMult,right1); // U(Hp'-m)
  1376. //right = TMatrixD(u,TMatrixD::kMult,right1); // U(Hp'-m)
  1377. //value = TMatrixD(right1,TMatrixD::kTransposeMult,right); // (Hp'-m)'U(Hp'-m)
  1378. meas = TMatrixD(u,TMatrixD::kMult,tmp); // U(Hp'-m)
  1379. value = TMatrixD(tmp,TMatrixD::kTransposeMult,meas); // (Hp'-m)'U(Hp'-m)
  1380. dChi2 += value(0,0);
  1381. pointWeight = huhSym;
  1382. //pointWeight.Print();
  1383. //cout<<"dChi2====="<<dChi2<<'\n';
  1384. //sleep(1);
  1385. return dChi2;
  1386. }
  1387. /*
  1388. //__________________________________________________________________________
  1389. Double_t MpdKalmanFilter::FilterHitZ(MpdKalmanTrack *track, const MpdKalmanHit *hit,
  1390. TMatrixDSym &pointWeight, TMatrixD &parFilt)
  1391. {
  1392. /// Compute Chi2 increment if the "end-cap" hit is assigned to the track
  1393. /// (assuming 1-d hit measurement)
  1394. //double meas[3][1], h[3][5], u[3][2];
  1395. double* u = (double *)calloc(6, sizeof(double));
  1396. double* h = (double *)calloc(15, sizeof(double));
  1397. double* meas = (double *)calloc(3, sizeof(double));
  1398. //TMatrixD meas(2,1); // measurements
  1399. //TMatrixD h(2,5); // transformation matrix from hit meas. to track params
  1400. //TMatrixD u(2,2); // weight matrix of measurements
  1401. MKL_INT mM = 2, nM = 1, mH = 2, nH = 5, mU = 2, nU = 2, mP = 5, nP = 1, info;
  1402. Double_t HitCosSin1 = hit->GetCosSin(1), HitCosSin0 = hit->GetCosSin(0), HitErr0 = hit->GetErr(0);
  1403. //h[0][0] = -HitCosSin1;
  1404. //h[0][1] = HitCosSin0;
  1405. h[0] = -HitCosSin1;
  1406. h[1] = HitCosSin0;
  1407. //meas[0][0] = hit->GetMeas(0);
  1408. meas[0] = hit->GetMeas(0);
  1409. //u[0][0] = 1. / HitErr0 / HitErr0;
  1410. u[0] = 1. / HitErr0 / HitErr0;
  1411. if (hit->GetNofDim() > 1) {
  1412. //meas[1][0] = hit->GetMeas(1);
  1413. meas[1] = hit->GetMeas(1);
  1414. Double_t HitErr1 = hit->GetErr(1);
  1415. //u[1][1] = 1. / HitErr1 / HitErr1;
  1416. u[3] = 1. / HitErr1 / HitErr1;
  1417. //h[1][0] = HitCosSin0;
  1418. //h[1][1] = HitCosSin1;
  1419. h[5] = HitCosSin0;
  1420. h[6] = HitCosSin1;
  1421. }
  1422. // Solving linear system (W+H'UH)p' = H'U(m-Hp) + (W+H'UH)p
  1423. //TMatrixD uh(u,TMatrixD::kMult,h); // UH
  1424. //double uh[3][5];
  1425. double uh[15], huhSym[30], tmp[3], param[6], tmp1[3], right[6];
  1426. cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, mU, nH, nU, 1, u, nU, h, nH, 0, uh, nH);
  1427. //cout<<"UH: "<<uh[0][0]<<';'<<uh[0][1]<<';'<<uh[0][2]<<';'<<uh[0][3]<<';'<<uh[0][4]<<';'<<uh[1][0]<<';'<<uh[1][1]<<';'<<uh[1][2]<<';'<<uh[1][3]<<';'<<uh[1][4]<<'\n';
  1428. //TMatrixD huh(h,TMatrixD::kTransposeMult,uh); // H'UH
  1429. //double huhSym[6][5];
  1430. cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans, nH, nH, mH, 1, h, nH, uh, nH, 0, pointWeight.GetMatrixArray(), nH);
  1431. //cout<<"HUH: "<<huhSym[0][0]<<';'<<huhSym[0][1]<<';'<<huhSym[0][2]<<';'<<huhSym[0][3]<<';'<<huhSym[0][4]<<';'<<huhSym[1][0]<<';'<<huhSym[1][1]<<';'<<huhSym[1][2]<<';'<<huhSym[1][3]<<';'<<huhSym[1][4]
  1432. // <<huhSym[2][0]<<';'<<huhSym[2][1]<<';'<<huhSym[2][2]<<';'<<huhSym[2][3]<<';'<<huhSym[2][4]<<';'<<huhSym[3][0]<<';'<<huhSym[3][1]<<';'<<huhSym[3][2]<<';'<<huhSym[3][3]<<';'<<huhSym[3][4]
  1433. // <<huhSym[4][0]<<';'<<huhSym[4][1]<<';'<<huhSym[4][2]<<';'<<huhSym[4][3]<<';'<<huhSym[4][4]<<'\n';
  1434. //pointWeight = huhSym;
  1435. //memcpy(pointWeight.GetMatrixArray(), huhSym, 25*sizeof(double));
  1436. //pointWeight.Print();
  1437. //TMatrixDSym huhSym = pointWeight;
  1438. //huhSym.SetMatrixArray(huh.GetMatrixArray());
  1439. //TMatrixDSym wu = *track->GetWeight();
  1440. //wu += huhSym; // W+H'UH
  1441. //double wu[6][5];
  1442. memcpy(huhSym, (*track->GetWeight()).GetMatrixArray(), 25*sizeof(double));
  1443. cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans, nH, nH, mH, 1, h, nH, uh, nH, 1, huhSym, nH);
  1444. //cout<<"WU: "<<wu[0][0]<<';'<<wu[0][1]<<';'<<wu[0][2]<<';'<<wu[0][3]<<';'<<wu[0][4]<<';'<<wu[1][0]<<';'<<wu[1][1]<<';'<<wu[1][2]<<';'<<wu[1][3]<<';'<<wu[1][4]
  1445. // <<wu[2][0]<<';'<<wu[2][1]<<';'<<wu[2][2]<<';'<<wu[2][3]<<';'<<wu[2][4]<<';'<<wu[3][0]<<';'<<wu[3][1]<<';'<<wu[3][2]<<';'<<wu[3][3]<<';'<<wu[3][4]
  1446. // <<wu[4][0]<<';'<<wu[4][1]<<';'<<wu[4][2]<<';'<<wu[4][3]<<';'<<wu[4][4]<<'\n';
  1447. //TMatrixD hp(h,TMatrixD::kMult,*track->GetParamNew()); // Hp
  1448. //TMatrixD tmp = meas; // m
  1449. //tmp -= hp; // m-Hp
  1450. //double tmp[3][1];
  1451. memcpy(tmp, meas, 2*sizeof(double));
  1452. //double param[6][1];
  1453. memcpy(param, (*track->GetParamNew()).GetMatrixArray(), 5*sizeof(double));
  1454. cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, mH, nP, nH, -1, h, nH, param, nP, 1, tmp, nP);
  1455. //cout<<"m-Hp: "<<tmp[0][0]<<';'<<tmp[1][0]<<'\n';
  1456. //TMatrixD right0(u,TMatrixD::kMult,tmp); // U(m-Hp)
  1457. //double tmp1[3][1];
  1458. cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, mU, nP, nU, 1, u, nU, tmp, nP, 0, tmp1, nP);
  1459. //cout<<"U(m-Hp): "<<tmp1[0][0]<<';'<<tmp1[1][0]<<'\n';
  1460. //TMatrixD right(h,TMatrixD::kTransposeMult,right0); // H'U(m-Hp)
  1461. //double right[6][1];
  1462. cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans, nH, nP, mH, 1, h, nH, tmp1, nP, 0, right, nP);
  1463. //cout<<"H'U(m-Hp): "<<right[0][0]<<';'<<right[1][0]<<';'<<right[2][0]<<';'<<right[3][0]<<';'<<right[4][0]<<'\n';
  1464. //TMatrixD right1(wu,TMatrixD::kMult,*track->GetParamNew()); // (W+H'UH)p
  1465. cblas_dsymm(CblasRowMajor, CblasLeft, CblasUpper, nH, nP, 1, huhSym, nH, param, nP, 1, right, nP);
  1466. //cout<<"H'U(m-Hp) + (W+H'UH)p: "<<right[0][0]<<';'<<right[1][0]<<';'<<right[2][0]<<';'<<right[3][0]<<';'<<right[4][0]<<'\n';
  1467. //Int_t iok = 0;
  1468. //fgKF->MnvertLocal(wu.GetMatrixArray(), 5, 5, 5, iok);
  1469. //parFilt = TMatrixD(wu,TMatrixD::kMult,right); // p'
  1470. int* ipiv = new int[nH];
  1471. //double LU[6][5];
  1472. //memcpy(LU, wu, 25*sizeof(double));;
  1473. info = LAPACKE_dgesv(LAPACK_COL_MAJOR, nH, 1, huhSym, nH, ipiv, right, nH);
  1474. //if (info > 0){
  1475. // cout<<"Error\n";
  1476. // return 0;
  1477. //}
  1478. //info = LAPACKE_dgetrf(LAPACK_ROW_MAJOR, 5, 5, (double*)LU, 5, &ipiv);
  1479. //info = LAPACKE_dgetrs(LAPACK_ROW_MAJOR, 'N', nH, right_hand, (double*)LU, nH, &ipiv, (double*)right, nH);
  1480. memcpy(parFilt.GetMatrixArray(), right, 5*sizeof(double));
  1481. //parFilt.Print();
  1482. //tmp = TMatrixD(h,TMatrixD::kMult,parFilt);
  1483. //tmp -= meas;
  1484. cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, mH, nP, nH, 1, h, nH, right, nP, -1, meas, nP);
  1485. //cout<<"meas: "<<meas[0][0]<<';'<<meas[1][0]<<'\n';
  1486. //TMatrixD point = parFilt;
  1487. //point -= *track->GetParamNew(); // p'-p
  1488. double value[2] = {1, 0};
  1489. cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, mP, nP, nP, 1, right, nP, value, nP, -1, param, nP);
  1490. //cout<<"p'-p: "<<param[0][0]<<';'<<param[1][0]<<';'<<param[2][0]<<';'<<param[3][0]<<';'<<param[4][0]<<'\n';
  1491. //right = TMatrixD(*track->GetWeight(),TMatrixD::kMult,point); // W(p'-p)
  1492. cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, nH, nP, nH, 1, (*track->GetWeight()).GetMatrixArray(), nH, param, nP, 0, right, nP);
  1493. //cout<<"W(p'-p): "<<right[0][0]<<';'<<right[1][0]<<';'<<right[2][0]<<';'<<right[3][0]<<';'<<right[4][0]<<'\n';
  1494. //TMatrixD value(point,TMatrixD::kTransposeMult,right); // (p'-p)'W(p'-p)
  1495. cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans, nP, nP, mP, 1, param, nP, right, nP, 0, value, nP);
  1496. //cout<<"(p'-p)'W(p'-p): "<<value[0][0]<<'\n';
  1497. //meas = TMatrixD(u,TMatrixD::kMult,tmp); // U(Hp'-m)
  1498. Double_t dChi2 = value[0];
  1499. cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, mU, nP, nU, 1, u, nU, meas, nP, 0, tmp, nP);
  1500. //cout<<"U(Hp'-m): "<<tmp[0][0]<<';'<<tmp[1][0]<<'\n';
  1501. //value = TMatrixD(tmp,TMatrixD::kTransposeMult,meas); // (Hp'-m)'U(Hp'-m)
  1502. cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans, nP, nP, mU, 1, meas, nP, tmp, nP, 0, value, nP);
  1503. //cout<<"(Hp'-m)'U(Hp'-m): "<<value[0][0]<<'\n';
  1504. dChi2 += value[0];
  1505. //cout<<"dChi2====="<<dChi2<<'\n';
  1506. ///sleep(5);
  1507. return dChi2;
  1508. }*/
  1509. //__________________________________________________________________________
  1510. Double_t MpdKalmanFilter::FilterStrip(MpdKalmanTrack *track, const MpdKalmanHit *hit,
  1511. TMatrixDSym &pointWeight, TMatrixD &parFilt)
  1512. {
  1513. /// Kalman filtering for "strip" hits
  1514. TMatrixD meas(2,1); // measurements
  1515. TMatrixD h(2,5); // transformation matrix from track params to measurements
  1516. TMatrixD u(2,2); // weight matrix of measurements
  1517. h(0,0) = hit->GetCosSin(0);
  1518. h(0,1) = hit->GetCosSin(1);
  1519. //h(0,0) = -TMath::Sin(hit->GetPhi());
  1520. //h(0,1) = TMath::Cos(hit->GetPhi());
  1521. meas(0,0) = hit->GetMeas(0);
  1522. u(0,0) = 1. / hit->GetErr(0) / hit->GetErr(0);
  1523. /*
  1524. if (hit->GetNofDim() > 1) {
  1525. meas(1,0) = hit->GetR();
  1526. u(1,1) = 1. / hit->GetRerr() / hit->GetRerr();
  1527. //h(1,0) = TMath::Sin(hit->GetPhi());
  1528. //h(1,1) = -TMath::Cos(hit->GetPhi());
  1529. h(1,0) = TMath::Cos(hit->GetPhi());
  1530. h(1,1) = TMath::Sin(hit->GetPhi());
  1531. }
  1532. */
  1533. // Solving linear system (W+H'UH)p' = H'U(m-Hp) + (W+H'UH)p
  1534. //TMatrixD uh(pointWeight,TMatrixD::kMult,h); // UH
  1535. TMatrixD uh(u,TMatrixD::kMult,h); // UH
  1536. TMatrixD huh(h,TMatrixD::kTransposeMult,uh); // H'UH
  1537. TMatrixDSym huhSym = pointWeight;
  1538. huhSym.SetMatrixArray(huh.GetMatrixArray());
  1539. TMatrixDSym wu = *track->GetWeight();
  1540. wu += huhSym; // W+H'UH
  1541. TMatrixD hp(h,TMatrixD::kMult,*track->GetParamNew()); // Hp
  1542. TMatrixD tmp = meas; // m
  1543. tmp -= hp; // m-Hp
  1544. //tmp.Print();
  1545. //TMatrixD right0(pointWeight,TMatrixD::kMult,paramTmp); // U(m-Hp)
  1546. TMatrixD right0(u,TMatrixD::kMult,tmp); // U(m-Hp)
  1547. TMatrixD right(h,TMatrixD::kTransposeMult,right0); // H'U(m-Hp)
  1548. TMatrixD right1(wu,TMatrixD::kMult,*track->GetParamNew()); // (W+H'UH)p
  1549. right += right1; // H'U(m-Hp) + (W+H'UH)p
  1550. //wu.Print();
  1551. //wu.Invert();
  1552. Int_t iok = 0;
  1553. fgKF->MnvertLocal(wu.GetMatrixArray(), 5, 5, 5, iok);
  1554. //wu.Print();
  1555. parFilt = TMatrixD(wu,TMatrixD::kMult,right); // p'
  1556. //right1 = h;
  1557. //right1 *= parFilt; // Hp'
  1558. //right1 -= meas; // Hp'-m
  1559. tmp = TMatrixD(h,TMatrixD::kMult,parFilt);
  1560. tmp -= meas;
  1561. TMatrixD point = parFilt;
  1562. point -= *track->GetParamNew(); // p'-p
  1563. right = TMatrixD(*track->GetWeight(),TMatrixD::kMult,point); // W(p'-p)
  1564. TMatrixD value(point,TMatrixD::kTransposeMult,right); // (p'-p)'W(p'-p)
  1565. Double_t dChi2 = value(0,0);
  1566. //right = TMatrixD(pointWeight,TMatrixD::kMult,right1); // U(Hp'-m)
  1567. //right = TMatrixD(u,TMatrixD::kMult,right1); // U(Hp'-m)
  1568. //value = TMatrixD(right1,TMatrixD::kTransposeMult,right); // (Hp'-m)'U(Hp'-m)
  1569. meas = TMatrixD(u,TMatrixD::kMult,tmp); // U(Hp'-m)
  1570. value = TMatrixD(tmp,TMatrixD::kTransposeMult,meas); // (Hp'-m)'U(Hp'-m)
  1571. dChi2 += value(0,0);
  1572. pointWeight = huhSym;
  1573. return dChi2;
  1574. }
  1575. //__________________________________________________________________________
  1576. Double_t MpdKalmanFilter::FilterStripLocal(MpdKalmanTrack *track, const MpdKalmanHit *hit,
  1577. TMatrixDSym &pointWeight, TMatrixD &parFilt, Double_t &posNew)
  1578. {
  1579. /// Kalman filtering for "strip" hits (in local detector coordinates)
  1580. TMatrixD meas(2,1); // measurements
  1581. TMatrixD h(2,5); // transformation matrix from track params to measurements
  1582. TMatrixD u(2,2); // weight matrix of measurements
  1583. if (hit->GetNofDim() == 1) {
  1584. h(0,0) = hit->GetCosSin(0);
  1585. h(0,1) = hit->GetCosSin(1);
  1586. meas(0,0) = hit->GetMeas(0);
  1587. u(0,0) = 1. / hit->GetErr(0) / hit->GetErr(0);
  1588. } else {
  1589. h(0,0) = 1;
  1590. h(1,1) = 1;
  1591. meas(0,0) = hit->GetMeas(0);
  1592. meas(1,0) = hit->GetMeas(1);
  1593. u(0,0) = hit->GetErr(0) * hit->GetErr(0);
  1594. u(0,1) = u(1,0) = -u(0,0) / TMath::Tan(hit->GetPhi());
  1595. u(1,1) = TMath::Abs(u(1,0) / TMath::Tan(hit->GetPhi())) + u(0,0) / TMath::Sin(hit->GetPhi()) / TMath::Sin(hit->GetPhi());
  1596. u.Invert();
  1597. }
  1598. /*
  1599. if (hit->GetNofDim() > 1) {
  1600. meas(1,0) = hit->GetR();
  1601. u(1,1) = 1. / hit->GetRerr() / hit->GetRerr();
  1602. //h(1,0) = TMath::Sin(hit->GetPhi());
  1603. //h(1,1) = -TMath::Cos(hit->GetPhi());
  1604. h(1,0) = TMath::Cos(hit->GetPhi());
  1605. h(1,1) = TMath::Sin(hit->GetPhi());
  1606. }
  1607. */
  1608. // Transform track params to local coordinates
  1609. /*
  1610. Double_t r = track->GetPosNew();
  1611. Double_t phi0 = track->GetParamNew(0) / r;
  1612. Double_t v3loc[3], xyz[3] = {r*TMath::Cos(phi0), r*TMath::Sin(phi0), track->GetParamNew(1)};
  1613. TGeoNode *node = gGeoManager->FindNode(xyz[0], xyz[1], xyz[2]);
  1614. TString path = gGeoManager->GetPath();
  1615. if (!path.Contains("sensor")) {
  1616. path += "/sts01sensor";
  1617. Int_t layer = hit->GetLayer() / 2 + 1;
  1618. path += layer;
  1619. path += "_0";
  1620. gGeoManager->cd(path);
  1621. }
  1622. cout << path << endl;
  1623. gGeoManager->MasterToLocal(xyz, v3loc);
  1624. */
  1625. // Solving linear system (W+H'UH)p' = H'U(m-Hp) + (W+H'UH)p
  1626. TMatrixD uh(u,TMatrixD::kMult,h); // UH
  1627. TMatrixD huh(h,TMatrixD::kTransposeMult,uh); // H'UH
  1628. TMatrixDSym huhSym = pointWeight;
  1629. huhSym.SetMatrixArray(huh.GetMatrixArray());
  1630. TMatrixDSym wu = *track->GetWeight();
  1631. wu += huhSym; // W+H'UH
  1632. /*
  1633. TMatrixD trackPar = *track->GetParamNew();
  1634. TMatrixD trackParLoc = *track->GetParamNew();
  1635. trackParLoc(0,0) = v3loc[0]; // Xlocal
  1636. trackParLoc(1,0) = v3loc[2]; // Zlocal
  1637. */
  1638. TMatrixD hp(h,TMatrixD::kMult,*track->GetParamNew()); // Hp
  1639. //TMatrixD hp(h,TMatrixD::kMult,trackParLoc); // Hp
  1640. TMatrixD tmp = meas; // m
  1641. tmp -= hp; // m-Hp
  1642. //tmp.Print();
  1643. TMatrixD right0(u,TMatrixD::kMult,tmp); // U(m-Hp)
  1644. TMatrixD right(h,TMatrixD::kTransposeMult,right0); // H'U(m-Hp)
  1645. TMatrixD right1(wu,TMatrixD::kMult,*track->GetParamNew()); // (W+H'UH)p
  1646. //TMatrixD right1(wu,TMatrixD::kMult,trackPar); // (W+H'UH)p
  1647. right += right1; // H'U(m-Hp) + (W+H'UH)p
  1648. //wu.Print();
  1649. //wu.Invert();
  1650. Int_t iok = 0;
  1651. MnvertLocal(wu.GetMatrixArray(), 5, 5, 5, iok);
  1652. //wu.Print();
  1653. parFilt = TMatrixD(wu,TMatrixD::kMult,right); // p'
  1654. /*
  1655. TMatrixD parFiltLoc = parFilt;
  1656. phi0 = parFilt(0,0) / r;
  1657. Double_t z = parFilt(1,0);
  1658. Double_t xyz1[3] = {r*TMath::Cos(phi0), r*TMath::Sin(phi0), z};
  1659. gGeoManager->MasterToLocal(xyz1, v3loc);
  1660. parFiltLoc(0,0) = v3loc[0]; // Xlocal
  1661. parFiltLoc(1,0) = v3loc[2]; // Zlocal
  1662. */
  1663. //right1 = h;
  1664. //right1 *= parFilt; // Hp'
  1665. //right1 -= meas; // Hp'-m
  1666. tmp = TMatrixD(h,TMatrixD::kMult,parFilt);
  1667. //tmp = TMatrixD(h,TMatrixD::kMult,parFiltLoc);
  1668. tmp -= meas;
  1669. TMatrixD point = parFilt;
  1670. point -= *track->GetParamNew(); // p'-p
  1671. //point -= trackPar; // p'-p
  1672. right = TMatrixD(*track->GetWeight(),TMatrixD::kMult,point); // W(p'-p)
  1673. TMatrixD value(point,TMatrixD::kTransposeMult,right); // (p'-p)'W(p'-p)
  1674. Double_t dChi2 = value(0,0);
  1675. //right = TMatrixD(pointWeight,TMatrixD::kMult,right1); // U(Hp'-m)
  1676. //right = TMatrixD(u,TMatrixD::kMult,right1); // U(Hp'-m)
  1677. //value = TMatrixD(right1,TMatrixD::kTransposeMult,right); // (Hp'-m)'U(Hp'-m)
  1678. meas = TMatrixD(u,TMatrixD::kMult,tmp); // U(Hp'-m)
  1679. value = TMatrixD(tmp,TMatrixD::kTransposeMult,meas); // (Hp'-m)'U(Hp'-m)
  1680. dChi2 += value(0,0);
  1681. pointWeight = huhSym;
  1682. // Transform track params to global system
  1683. /*
  1684. v3loc[0] = parFilt(0,0);
  1685. v3loc[2] = parFilt(1,0);
  1686. gGeoManager->LocalToMaster(v3loc, xyz);
  1687. parFilt(1,0) = xyz[2];
  1688. Double_t phi = TMath::ATan2 (xyz[1], xyz[0]);
  1689. r = xyz[0] * xyz[0] + xyz[1] * xyz[1];
  1690. r = TMath::Sqrt(r);
  1691. parFilt(0,0) = r * Proxim(phi0,phi);
  1692. */
  1693. //posNew = r;
  1694. return dChi2;
  1695. }
  1696. //__________________________________________________________________________
  1697. Bool_t MpdKalmanFilter::Refit(MpdKalmanTrack *track, Int_t iDir, Bool_t updLeng)
  1698. {
  1699. /// Refit track using track hits (from first to last if iDir == -1)
  1700. #pragma omp critical
  1701. {
  1702. track->GetHits()->Sort();
  1703. }
  1704. //cout << track->GetChi2() << endl;
  1705. track->SetChi2(0.);
  1706. //fTrackDir = kOutward;
  1707. //*fWeight = *fCovar;
  1708. //Int_t iok;
  1709. //MpdKalmanFilter::Instance()->MnvertLocal(fWeight->GetMatrixArray(), 5, 5, 5, iok);
  1710. //if (GetNofHits() == 1) return;
  1711. TMatrixDSym *w = track->GetWeight();
  1712. Int_t nHits = track->GetNofHits(), nHits2 = nHits * nHits;;
  1713. for (Int_t i = 0; i < 5; ++i) {
  1714. for (Int_t j = i; j < 5; ++j) {
  1715. //if (j == i) (*w)(i,j) /= 1000.;
  1716. //if (j == i) (*w)(i,j) /= nHits;
  1717. if (j == i) (*w)(i,j) /= nHits2;
  1718. //if (j == i) (*w)(i,j) /= 1;
  1719. else (*w)(i,j) = (*w)(j,i) = 0.;
  1720. }
  1721. }
  1722. TMatrixD param(5,1);
  1723. TMatrixDSym weight(5), pointWeight(5);
  1724. //Int_t ibeg = iDir > 0 ? 1 : nHits-2;
  1725. Int_t ibeg = iDir > 0 ? 0 : nHits-1;
  1726. Int_t iend = iDir > 0 ? nHits-1 : 0;
  1727. iend += iDir;
  1728. MpdKalmanHit *hit = 0x0;
  1729. //for (Int_t i = ibeg; i >= iend; i+=iDir) {
  1730. for (Int_t i = ibeg; i != iend; i+=iDir) {
  1731. hit = (MpdKalmanHit*) track->GetHits()->UncheckedAt(i);
  1732. //if (!MpdKalmanFilter::Instance()->PropagateToHit(track, hit, updLeng)) return kFALSE;
  1733. if (!MpdKalmanFilter::Instance()->PropagateToHit(track, hit, updLeng, kTRUE)) return kFALSE;
  1734. Double_t dChi2 = FilterHit(track, hit, pointWeight, param);
  1735. track->SetChi2(track->GetChi2()+dChi2);
  1736. weight = *track->GetWeight();
  1737. weight += pointWeight;
  1738. track->SetWeight(weight);
  1739. track->SetParamNew(param);
  1740. //cout << i << " " << dChi2 << " " << 1./track->GetParamNew(4) << " " << hit->GetLayer() << " " << hit->GetDist() << endl;
  1741. }
  1742. //if (hit) {
  1743. if (0) {
  1744. TVector3 pos = fGeoScheme->GlobalPos(hit);
  1745. cout << " Hit: " << pos.Pt()*pos.Phi() << " " << pos.Pt() << " " << pos.Z() << endl;
  1746. cout << " Track: " << nHits << " " << track->GetParamNew(0) << " " << track->GetPosNew() << " " << track->GetParamNew(1) << " " << track->GetChi2() << endl;
  1747. }
  1748. return kTRUE;
  1749. }
  1750. //__________________________________________________________________________
  1751. Double_t MpdKalmanFilter::Interp(Int_t nDim, const Double_t *mom, const Double_t *dedxm, Double_t p)
  1752. {
  1753. // Energy loss interpolation
  1754. //if (p < mom[0]) return dedxm[0];
  1755. if (p > mom[nDim-1]) return dedxm[nDim-1];
  1756. // Binary search
  1757. Int_t i1 = 0, i2 = nDim-1, i = i2;
  1758. if (p < mom[0]) {
  1759. // Extrapolation
  1760. i1 = 0;
  1761. i2 = 1;
  1762. } else {
  1763. do {
  1764. i = i1 + (i2-i1) / 2;
  1765. if (p > mom[i]) i1 = i;
  1766. else i2 = i;
  1767. } while (i2 - i1 > 1);
  1768. }
  1769. // Linear interpolation
  1770. return dedxm[i1] + (dedxm[i2]-dedxm[i1])/(mom[i2]-mom[i1]) * (p-mom[i1]);
  1771. }
  1772. //__________________________________________________________________________
  1773. void MpdKalmanFilter::Convert(const MpdKalmanTrack *track1, MpdKalmanTrack *track2)
  1774. {
  1775. // Convert track of diff. types (from "R" to "Z" at the moment)
  1776. if (track1->GetType() == track2->GetType()) return;
  1777. track2->SetPos(track1->GetParamNew(1)); // Z
  1778. track2->SetParam(1, track1->GetPosNew()); // R
  1779. track2->SetParam(3, TMath::PiOver2()-track1->GetParamNew(3)); // Theta
  1780. track2->SetParam(4, track1->GetParamNew(4)*TMath::Sin(track2->GetParam(3))); // q/P
  1781. }
  1782. //__________________________________________________________________________
  1783. void MpdKalmanFilter::ExtrapOneStepRungekutta(Double_t charge, Double_t step,
  1784. Double_t* vect, Double_t* vout)
  1785. {
  1786. /// <pre>
  1787. /// ******************************************************************
  1788. /// * *
  1789. /// * Runge-Kutta method for tracking a particle through a magnetic *
  1790. /// * field. Uses Nystroem algorithm (See Handbook Nat. Bur. of *
  1791. /// * Standards, procedure 25.5.20) *
  1792. /// * *
  1793. /// * Input parameters *
  1794. /// * CHARGE Particle charge *
  1795. /// * STEP Step size *
  1796. /// * VECT Initial co-ords,direction cosines,momentum *
  1797. /// * Output parameters *
  1798. /// * VOUT Output co-ords,direction cosines,momentum *
  1799. /// * User routine called *
  1800. /// * CALL GUFLD(X,F) *
  1801. /// * *
  1802. /// * ==>Called by : <USER>, GUSWIM *
  1803. /// * Authors R.Brun, M.Hansroul ********* *
  1804. /// * V.Perevoztchikov (CUT STEP implementation) *
  1805. /// * *
  1806. /// * *
  1807. /// ******************************************************************
  1808. /// </pre>
  1809. Double_t h2, h4, f[4];
  1810. Double_t xyzt[3], a, b, c, ph,ph2;
  1811. Double_t secxs[4],secys[4],seczs[4],hxp[3];
  1812. Double_t g1, g2, g3, g4, g5, g6, ang2, dxt, dyt, dzt;
  1813. Double_t est, at, bt, ct, cba;
  1814. Double_t f1, f2, f3, f4, rho, tet, hnorm, hp, rho1, sint, cost;
  1815. Double_t x;
  1816. Double_t y;
  1817. Double_t z;
  1818. Double_t xt;
  1819. Double_t yt;
  1820. Double_t zt;
  1821. Double_t maxit = 1992;
  1822. Double_t maxcut = 11;
  1823. const Double_t kdlt = 1e-4;
  1824. const Double_t kdlt32 = kdlt/32.;
  1825. const Double_t kthird = 1./3.;
  1826. const Double_t khalf = 0.5;
  1827. const Double_t kec = 2.9979251e-4;
  1828. const Double_t kpisqua = 9.86960440109;
  1829. const Int_t kix = 0;
  1830. const Int_t kiy = 1;
  1831. const Int_t kiz = 2;
  1832. const Int_t kipx = 3;
  1833. const Int_t kipy = 4;
  1834. const Int_t kipz = 5;
  1835. // *.
  1836. // *. ------------------------------------------------------------------
  1837. // *.
  1838. // * this constant is for units cm,gev/c and kgauss
  1839. // *
  1840. Int_t iter = 0;
  1841. Int_t ncut = 0;
  1842. for(Int_t j = 0; j < 7; j++)
  1843. vout[j] = vect[j];
  1844. Double_t pinv = kec * charge / vect[6];
  1845. Double_t tl = 0.;
  1846. Double_t h = step;
  1847. Double_t rest;
  1848. do {
  1849. rest = step - tl;
  1850. if (TMath::Abs(h) > TMath::Abs(rest)) h = rest;
  1851. //cmodif: call gufld(vout,f) changed into:
  1852. GetField(vout,f);
  1853. // *
  1854. // * start of integration
  1855. // *
  1856. x = vout[0];
  1857. y = vout[1];
  1858. z = vout[2];
  1859. a = vout[3];
  1860. b = vout[4];
  1861. c = vout[5];
  1862. h2 = khalf * h;
  1863. h4 = khalf * h2;
  1864. ph = pinv * h;
  1865. ph2 = khalf * ph;
  1866. secxs[0] = (b * f[2] - c * f[1]) * ph2;
  1867. secys[0] = (c * f[0] - a * f[2]) * ph2;
  1868. seczs[0] = (a * f[1] - b * f[0]) * ph2;
  1869. ang2 = (secxs[0]*secxs[0] + secys[0]*secys[0] + seczs[0]*seczs[0]);
  1870. if (ang2 > kpisqua) break;
  1871. dxt = h2 * a + h4 * secxs[0];
  1872. dyt = h2 * b + h4 * secys[0];
  1873. dzt = h2 * c + h4 * seczs[0];
  1874. xt = x + dxt;
  1875. yt = y + dyt;
  1876. zt = z + dzt;
  1877. // *
  1878. // * second intermediate point
  1879. // *
  1880. est = TMath::Abs(dxt) + TMath::Abs(dyt) + TMath::Abs(dzt);
  1881. if (est > h) {
  1882. if (ncut++ > maxcut) break;
  1883. h *= khalf;
  1884. continue;
  1885. }
  1886. xyzt[0] = xt;
  1887. xyzt[1] = yt;
  1888. xyzt[2] = zt;
  1889. //cmodif: call gufld(xyzt,f) changed into:
  1890. GetField(xyzt,f);
  1891. at = a + secxs[0];
  1892. bt = b + secys[0];
  1893. ct = c + seczs[0];
  1894. secxs[1] = (bt * f[2] - ct * f[1]) * ph2;
  1895. secys[1] = (ct * f[0] - at * f[2]) * ph2;
  1896. seczs[1] = (at * f[1] - bt * f[0]) * ph2;
  1897. at = a + secxs[1];
  1898. bt = b + secys[1];
  1899. ct = c + seczs[1];
  1900. secxs[2] = (bt * f[2] - ct * f[1]) * ph2;
  1901. secys[2] = (ct * f[0] - at * f[2]) * ph2;
  1902. seczs[2] = (at * f[1] - bt * f[0]) * ph2;
  1903. dxt = h * (a + secxs[2]);
  1904. dyt = h * (b + secys[2]);
  1905. dzt = h * (c + seczs[2]);
  1906. xt = x + dxt;
  1907. yt = y + dyt;
  1908. zt = z + dzt;
  1909. at = a + 2.*secxs[2];
  1910. bt = b + 2.*secys[2];
  1911. ct = c + 2.*seczs[2];
  1912. est = TMath::Abs(dxt)+TMath::Abs(dyt)+TMath::Abs(dzt);
  1913. if (est > 2.*TMath::Abs(h)) {
  1914. if (ncut++ > maxcut) break;
  1915. h *= khalf;
  1916. continue;
  1917. }
  1918. xyzt[0] = xt;
  1919. xyzt[1] = yt;
  1920. xyzt[2] = zt;
  1921. //cmodif: call gufld(xyzt,f) changed into:
  1922. GetField(xyzt,f);
  1923. z = z + (c + (seczs[0] + seczs[1] + seczs[2]) * kthird) * h;
  1924. y = y + (b + (secys[0] + secys[1] + secys[2]) * kthird) * h;
  1925. x = x + (a + (secxs[0] + secxs[1] + secxs[2]) * kthird) * h;
  1926. secxs[3] = (bt*f[2] - ct*f[1])* ph2;
  1927. secys[3] = (ct*f[0] - at*f[2])* ph2;
  1928. seczs[3] = (at*f[1] - bt*f[0])* ph2;
  1929. a = a+(secxs[0]+secxs[3]+2. * (secxs[1]+secxs[2])) * kthird;
  1930. b = b+(secys[0]+secys[3]+2. * (secys[1]+secys[2])) * kthird;
  1931. c = c+(seczs[0]+seczs[3]+2. * (seczs[1]+seczs[2])) * kthird;
  1932. est = TMath::Abs(secxs[0]+secxs[3] - (secxs[1]+secxs[2]))
  1933. + TMath::Abs(secys[0]+secys[3] - (secys[1]+secys[2]))
  1934. + TMath::Abs(seczs[0]+seczs[3] - (seczs[1]+seczs[2]));
  1935. if (est > kdlt && TMath::Abs(h) > 1.e-4) {
  1936. if (ncut++ > maxcut) break;
  1937. h *= khalf;
  1938. continue;
  1939. }
  1940. ncut = 0;
  1941. // * if too many iterations, go to helix
  1942. if (iter++ > maxit) break;
  1943. tl += h;
  1944. if (est < kdlt32)
  1945. h *= 2.;
  1946. cba = 1./ TMath::Sqrt(a*a + b*b + c*c);
  1947. vout[0] = x;
  1948. vout[1] = y;
  1949. vout[2] = z;
  1950. vout[3] = cba*a;
  1951. vout[4] = cba*b;
  1952. vout[5] = cba*c;
  1953. rest = step - tl;
  1954. if (step < 0.) rest = -rest;
  1955. if (rest < 1.e-5*TMath::Abs(step)) return;
  1956. } while(1);
  1957. // angle too big, use helix
  1958. f1 = f[0];
  1959. f2 = f[1];
  1960. f3 = f[2];
  1961. f4 = TMath::Sqrt(f1*f1+f2*f2+f3*f3);
  1962. rho = -f4*pinv;
  1963. tet = rho * step;
  1964. hnorm = 1./f4;
  1965. f1 = f1*hnorm;
  1966. f2 = f2*hnorm;
  1967. f3 = f3*hnorm;
  1968. hxp[0] = f2*vect[kipz] - f3*vect[kipy];
  1969. hxp[1] = f3*vect[kipx] - f1*vect[kipz];
  1970. hxp[2] = f1*vect[kipy] - f2*vect[kipx];
  1971. hp = f1*vect[kipx] + f2*vect[kipy] + f3*vect[kipz];
  1972. rho1 = 1./rho;
  1973. sint = TMath::Sin(tet);
  1974. cost = 2.*TMath::Sin(khalf*tet)*TMath::Sin(khalf*tet);
  1975. g1 = sint*rho1;
  1976. g2 = cost*rho1;
  1977. g3 = (tet-sint) * hp*rho1;
  1978. g4 = -cost;
  1979. g5 = sint;
  1980. g6 = cost * hp;
  1981. vout[kix] = vect[kix] + g1*vect[kipx] + g2*hxp[0] + g3*f1;
  1982. vout[kiy] = vect[kiy] + g1*vect[kipy] + g2*hxp[1] + g3*f2;
  1983. vout[kiz] = vect[kiz] + g1*vect[kipz] + g2*hxp[2] + g3*f3;
  1984. vout[kipx] = vect[kipx] + g4*vect[kipx] + g5*hxp[0] + g6*f1;
  1985. vout[kipy] = vect[kipy] + g4*vect[kipy] + g5*hxp[1] + g6*f2;
  1986. vout[kipz] = vect[kipz] + g4*vect[kipz] + g5*hxp[2] + g6*f3;
  1987. }
  1988. //__________________________________________________________________________
  1989. void MpdKalmanFilter::GetField(Double_t *position, Double_t *field)
  1990. {
  1991. /// Just simple interface
  1992. //if (fMagField) fMagField->GetFieldValue(position,field);
  1993. if (fMagField)
  1994. ((FairField*)((MpdMultiField*)fMagField)->GetFieldList()->UncheckedAt(0))->Field(position,field);
  1995. else {
  1996. cout<<"F-MpdKalmanFilter::GetField: fMagField = 0x0"<<endl;
  1997. exit(-1);
  1998. }
  1999. }
  2000. //__________________________________________________________________________
  2001. Double_t MpdKalmanFilter::Scattering(MpdKalmanTrack *track, Double_t dx, TString mass2, Int_t charge)
  2002. {
  2003. /// Compute multiple scattering angle. dx - material thickness (in X0 - for normal incidence)
  2004. //const Double_t piMass2 = 0.13957 * 0.13957; // pion hypothesis
  2005. Double_t piMass2 = mass2.Atof();
  2006. Double_t ph = track->GetParamNew(2);
  2007. Double_t cosTh = TMath::Cos (track->GetParamNew(3));
  2008. Double_t sinTh = TMath::Sin (track->GetParamNew(3));
  2009. Double_t ppp = charge / track->GetParamNew(4) / cosTh;
  2010. ppp = TMath::Abs (ppp);
  2011. ppp = TMath::Max (ppp,0.1);
  2012. ppp = TMath::Min (ppp,3.);
  2013. TVector3 p(0,0,ppp);
  2014. TLorentzVector pe(p,TMath::Sqrt(ppp*ppp+piMass2));
  2015. Double_t beta = pe.Beta(), phPos = 0, scaleL = 0;
  2016. if (track->GetType() == MpdKalmanTrack::kBarrel) {
  2017. // Path length correction - barrel
  2018. phPos = track->GetParamNew(0) / track->GetPosNew();
  2019. scaleL = TMath::Cos(phPos-ph) * cosTh;
  2020. } else {
  2021. phPos = TMath::ATan2 (track->GetParamNew(1), track->GetParamNew(0));
  2022. scaleL = sinTh; // path length correction - endcap
  2023. }
  2024. Double_t dxx0 = dx / TMath::Abs(scaleL);
  2025. Double_t angle = 0.0136 / beta / ppp * charge * (1. + 0.038 * TMath::Log(dxx0));
  2026. //cout << angle << " " << beta << " " << scaleL << " " << ppp << endl;
  2027. //exit(0);
  2028. return angle * angle * dxx0;
  2029. }
  2030. //__________________________________________________________________________
  2031. Double_t MpdKalmanFilter::Scattering(MpdKalmanTrack *track, Double_t x0, Double_t step,
  2032. TString mass2, Int_t charge)
  2033. {
  2034. /// Compute multiple scattering angle. x0 - rad. length, step - track path length
  2035. //const Double_t piMass2 = 0.13957 * 0.13957; // pion hypothesis
  2036. Double_t piMass2 = mass2.Atof();
  2037. //Double_t ph = track->GetParamNew(2);
  2038. Double_t cosTh = TMath::Cos (track->GetParamNew(3));
  2039. //Double_t sinTh = TMath::Sin (track->GetParamNew(3));
  2040. Double_t ppp = charge / track->GetParamNew(4) / cosTh;
  2041. ppp = TMath::Abs (ppp);
  2042. ppp = TMath::Max (ppp,0.1);
  2043. ppp = TMath::Min (ppp,3.);
  2044. TVector3 p(0,0,ppp);
  2045. TLorentzVector pe(p,TMath::Sqrt(ppp*ppp+piMass2));
  2046. Double_t beta = pe.Beta();
  2047. /*
  2048. Double_t beta = pe.Beta(), phPos = 0, scaleL = 0;
  2049. if (track->GetType() == MpdKalmanTrack::kFixedR) {
  2050. // Path length correction - fixed R
  2051. phPos = track->GetParamNew(0) / track->GetPosNew();
  2052. scaleL = TMath::Cos(phPos-ph) * cosTh;
  2053. } else {
  2054. phPos = TMath::ATan2 (track->GetParamNew(1), track->GetParamNew(0));
  2055. scaleL = sinTh; // path length correction - fixed Z
  2056. }
  2057. Double_t dxx0 = dx / TMath::Abs(scaleL);
  2058. */
  2059. Double_t dxx0 = step / x0;
  2060. Double_t angle = 0.0136 / beta / ppp * charge * (1. + 0.038 * TMath::Log(dxx0));
  2061. //cout << angle << " " << beta << " " << scaleL << " " << ppp << endl;
  2062. //exit(0);
  2063. return angle * angle * dxx0;
  2064. }
  2065. //__________________________________________________________________________
  2066. /*
  2067. TVector3 TpcLheKalmanFilter::GetTrackMom(const TpcLheHit *hit)
  2068. {
  2069. /// Returns track momentum for given hit
  2070. TpcPoint* point = (TpcPoint*) fTpcPoints->UncheckedAt(hit->GetUniqueID());
  2071. TVector3 p(point->GetPx(),point->GetPy(),point->GetPz());
  2072. return p;
  2073. }
  2074. */
  2075. //__________________________________________________________________________
  2076. void MpdKalmanFilter::ExtrapOneStepHelix(Double_t charge, Double_t step,
  2077. Double_t *vect, Double_t *vout)
  2078. {
  2079. // ******************************************************************
  2080. // * *
  2081. // * Performs the tracking of one step in a magnetic field *
  2082. // * The trajectory is assumed to be a helix in a constant field *
  2083. // * taken at the mid point of the step. *
  2084. // * Parameters: *
  2085. // * input *
  2086. // * STEP =arc length of the step asked *
  2087. // * VECT =input vector (position,direction cos and momentum) *
  2088. // * CHARGE= electric charge of the particle *
  2089. // * output *
  2090. // * VOUT = same as VECT after completion of the step *
  2091. // * *
  2092. // * ==>Called by : <USER>, GUSWIM *
  2093. // * Author m.hansroul ********* *
  2094. // * modified s.egli, s.v.levonian *
  2095. // * modified v.perevoztchikov
  2096. // * *
  2097. // ******************************************************************
  2098. //
  2099. // modif: everything in double precision
  2100. Double_t xyz[3], h[4], hxp[3];
  2101. Double_t h2xy, hp, rho, tet;
  2102. Double_t sint, sintt, tsint, cos1t;
  2103. Double_t f1, f2, f3, f4, f5, f6;
  2104. const Int_t kix = 0;
  2105. const Int_t kiy = 1;
  2106. const Int_t kiz = 2;
  2107. const Int_t kipx = 3;
  2108. const Int_t kipy = 4;
  2109. const Int_t kipz = 5;
  2110. const Int_t kipp = 6;
  2111. const Double_t kec = 2.9979251e-4;
  2112. //
  2113. // ------------------------------------------------------------------
  2114. //
  2115. // units are kgauss,centimeters,gev/c
  2116. //
  2117. vout[kipp] = vect[kipp];
  2118. if (TMath::Abs(charge) < 0.00001) {
  2119. for (Int_t i = 0; i < 3; i++) {
  2120. vout[i] = vect[i] + step * vect[i+3];
  2121. vout[i+3] = vect[i+3];
  2122. }
  2123. return;
  2124. }
  2125. xyz[0] = vect[kix] + 0.5 * step * vect[kipx];
  2126. xyz[1] = vect[kiy] + 0.5 * step * vect[kipy];
  2127. xyz[2] = vect[kiz] + 0.5 * step * vect[kipz];
  2128. //cmodif: call gufld (xyz, h) changed into:
  2129. GetField (xyz, h);
  2130. h2xy = h[0]*h[0] + h[1]*h[1];
  2131. h[3] = h[2]*h[2]+ h2xy;
  2132. if (h[3] < 1.e-12) {
  2133. for (Int_t i = 0; i < 3; i++) {
  2134. vout[i] = vect[i] + step * vect[i+3];
  2135. vout[i+3] = vect[i+3];
  2136. }
  2137. return;
  2138. }
  2139. if (h2xy < 1.e-12*h[3]) {
  2140. ExtrapOneStepHelix3(charge*h[2], step, vect, vout);
  2141. return;
  2142. }
  2143. h[3] = TMath::Sqrt(h[3]);
  2144. h[0] /= h[3];
  2145. h[1] /= h[3];
  2146. h[2] /= h[3];
  2147. h[3] *= kec;
  2148. hxp[0] = h[1]*vect[kipz] - h[2]*vect[kipy];
  2149. hxp[1] = h[2]*vect[kipx] - h[0]*vect[kipz];
  2150. hxp[2] = h[0]*vect[kipy] - h[1]*vect[kipx];
  2151. hp = h[0]*vect[kipx] + h[1]*vect[kipy] + h[2]*vect[kipz];
  2152. rho = -charge*h[3]/vect[kipp];
  2153. tet = rho * step;
  2154. if (TMath::Abs(tet) > 0.15) {
  2155. sint = TMath::Sin(tet);
  2156. sintt = (sint/tet);
  2157. tsint = (tet-sint)/tet;
  2158. cos1t = 2.*(TMath::Sin(0.5*tet))*(TMath::Sin(0.5*tet))/tet;
  2159. } else {
  2160. tsint = tet*tet/36.;
  2161. sintt = (1. - tsint);
  2162. sint = tet*sintt;
  2163. cos1t = 0.5*tet;
  2164. }
  2165. f1 = step * sintt;
  2166. f2 = step * cos1t;
  2167. f3 = step * tsint * hp;
  2168. f4 = -tet*cos1t;
  2169. f5 = sint;
  2170. f6 = tet * cos1t * hp;
  2171. vout[kix] = vect[kix] + f1*vect[kipx] + f2*hxp[0] + f3*h[0];
  2172. vout[kiy] = vect[kiy] + f1*vect[kipy] + f2*hxp[1] + f3*h[1];
  2173. vout[kiz] = vect[kiz] + f1*vect[kipz] + f2*hxp[2] + f3*h[2];
  2174. vout[kipx] = vect[kipx] + f4*vect[kipx] + f5*hxp[0] + f6*h[0];
  2175. vout[kipy] = vect[kipy] + f4*vect[kipy] + f5*hxp[1] + f6*h[1];
  2176. vout[kipz] = vect[kipz] + f4*vect[kipz] + f5*hxp[2] + f6*h[2];
  2177. return;
  2178. }
  2179. //__________________________________________________________________________
  2180. void MpdKalmanFilter::ExtrapOneStepHelix3(Double_t field, Double_t step,
  2181. Double_t *vect, Double_t *vout) const
  2182. {
  2183. //
  2184. // ******************************************************************
  2185. // * *
  2186. // * Tracking routine in a constant field oriented *
  2187. // * along axis 3 *
  2188. // * Tracking is performed with a conventional *
  2189. // * helix step method *
  2190. // * *
  2191. // * ==>Called by : <USER>, GUSWIM *
  2192. // * Authors R.Brun, M.Hansroul ********* *
  2193. // * Rewritten V.Perevoztchikov
  2194. // * *
  2195. // ******************************************************************
  2196. //
  2197. Double_t hxp[3];
  2198. Double_t h4, hp, rho, tet;
  2199. Double_t sint, sintt, tsint, cos1t;
  2200. Double_t f1, f2, f3, f4, f5, f6;
  2201. const Int_t kix = 0;
  2202. const Int_t kiy = 1;
  2203. const Int_t kiz = 2;
  2204. const Int_t kipx = 3;
  2205. const Int_t kipy = 4;
  2206. const Int_t kipz = 5;
  2207. const Int_t kipp = 6;
  2208. const Double_t kec = 2.9979251e-4;
  2209. //
  2210. // ------------------------------------------------------------------
  2211. //
  2212. // units are kgauss,centimeters,gev/c
  2213. //
  2214. vout[kipp] = vect[kipp];
  2215. h4 = field * kec;
  2216. hxp[0] = - vect[kipy];
  2217. hxp[1] = + vect[kipx];
  2218. hp = vect[kipz];
  2219. rho = -h4/vect[kipp];
  2220. tet = rho * step;
  2221. if (TMath::Abs(tet) > 0.15) {
  2222. sint = TMath::Sin(tet);
  2223. sintt = (sint/tet);
  2224. tsint = (tet-sint)/tet;
  2225. cos1t = 2.* TMath::Sin(0.5*tet) * TMath::Sin(0.5*tet)/tet;
  2226. } else {
  2227. tsint = tet*tet/36.;
  2228. sintt = (1. - tsint);
  2229. sint = tet*sintt;
  2230. cos1t = 0.5*tet;
  2231. }
  2232. f1 = step * sintt;
  2233. f2 = step * cos1t;
  2234. f3 = step * tsint * hp;
  2235. f4 = -tet*cos1t;
  2236. f5 = sint;
  2237. f6 = tet * cos1t * hp;
  2238. vout[kix] = vect[kix] + f1*vect[kipx] + f2*hxp[0];
  2239. vout[kiy] = vect[kiy] + f1*vect[kipy] + f2*hxp[1];
  2240. vout[kiz] = vect[kiz] + f1*vect[kipz] + f3;
  2241. vout[kipx] = vect[kipx] + f4*vect[kipx] + f5*hxp[0];
  2242. vout[kipy] = vect[kipy] + f4*vect[kipy] + f5*hxp[1];
  2243. vout[kipz] = vect[kipz] + f4*vect[kipz] + f6;
  2244. return;
  2245. }
  2246. //__________________________________________________________________________
  2247. void MpdKalmanFilter::MnvertLocal(Double_t *a, Int_t l, Int_t, Int_t n,
  2248. Int_t &ifail)
  2249. {
  2250. ///*-*-*-*-*-*-*-*-*-*-*-*Inverts a symmetric matrix*-*-*-*-*-*-*-*-*-*-*-*-*
  2251. ///*-* ==========================
  2252. ///*-* inverts a symmetric matrix. matrix is first scaled to
  2253. ///*-* have all ones on the diagonal (equivalent to change of units)
  2254. ///*-* but no pivoting is done since matrix is positive-definite.
  2255. ///*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*
  2256. // taken from TMinuit package of Root (l>=n)
  2257. // fVERTs, fVERTq and fVERTpp changed to localVERTs, localVERTq and localVERTpp
  2258. // Double_t localVERTs[n], localVERTq[n], localVERTpp[n];
  2259. Double_t * localVERTs = new Double_t[n];
  2260. Double_t * localVERTq = new Double_t[n];
  2261. Double_t * localVERTpp = new Double_t[n];
  2262. // fMaxint changed to localMaxint
  2263. Int_t localMaxint = n;
  2264. /* System generated locals */
  2265. Int_t aOffset;
  2266. /* Local variables */
  2267. Double_t si;
  2268. Int_t i, j, k, kp1, km1;
  2269. /* Parameter adjustments */
  2270. aOffset = l + 1;
  2271. a -= aOffset;
  2272. /* Function Body */
  2273. ifail = 0;
  2274. if (n < 1) goto L100;
  2275. if (n > localMaxint) goto L100;
  2276. //*-*- scale matrix by sqrt of diag elements
  2277. for (i = 1; i <= n; ++i) {
  2278. si = a[i + i*l];
  2279. if (si <= 0) goto L100;
  2280. localVERTs[i-1] = 1 / TMath::Sqrt(si);
  2281. }
  2282. for (i = 1; i <= n; ++i) {
  2283. for (j = 1; j <= n; ++j) {
  2284. a[i + j*l] = a[i + j*l]*localVERTs[i-1]*localVERTs[j-1];
  2285. }
  2286. }
  2287. //*-*- . . . start main loop . . . .
  2288. for (i = 1; i <= n; ++i) {
  2289. k = i;
  2290. //*-*- preparation for elimination step1
  2291. if (a[k + k*l] != 0) localVERTq[k-1] = 1 / a[k + k*l];
  2292. else goto L100;
  2293. localVERTpp[k-1] = 1;
  2294. a[k + k*l] = 0;
  2295. kp1 = k + 1;
  2296. km1 = k - 1;
  2297. if (km1 < 0) goto L100;
  2298. else if (km1 == 0) goto L50;
  2299. else goto L40;
  2300. L40:
  2301. for (j = 1; j <= km1; ++j) {
  2302. localVERTpp[j-1] = a[j + k*l];
  2303. localVERTq[j-1] = a[j + k*l]*localVERTq[k-1];
  2304. a[j + k*l] = 0;
  2305. }
  2306. L50:
  2307. if (k - n < 0) goto L51;
  2308. else if (k - n == 0) goto L60;
  2309. else goto L100;
  2310. L51:
  2311. for (j = kp1; j <= n; ++j) {
  2312. localVERTpp[j-1] = a[k + j*l];
  2313. localVERTq[j-1] = -a[k + j*l]*localVERTq[k-1];
  2314. a[k + j*l] = 0;
  2315. }
  2316. //*-*- elimination proper
  2317. L60:
  2318. for (j = 1; j <= n; ++j) {
  2319. for (k = j; k <= n; ++k) { a[j + k*l] += localVERTpp[j-1]*localVERTq[k-1]; }
  2320. }
  2321. }
  2322. //*-*- elements of left diagonal and unscaling
  2323. for (j = 1; j <= n; ++j) {
  2324. for (k = 1; k <= j; ++k) {
  2325. a[k + j*l] = a[k + j*l]*localVERTs[k-1]*localVERTs[j-1];
  2326. a[j + k*l] = a[k + j*l];
  2327. }
  2328. }
  2329. delete [] localVERTs;
  2330. delete [] localVERTq;
  2331. delete [] localVERTpp;
  2332. return;
  2333. //*-*- failure return
  2334. L100:
  2335. delete [] localVERTs;
  2336. delete [] localVERTq;
  2337. delete [] localVERTpp;
  2338. ifail = 1;
  2339. } /* mnvertLocal */
  2340. //__________________________________________________________________________
  2341. Double_t* MpdKalmanFilter::ExtrapOneStep(MpdKalmanTrack *track, Double_t step, Int_t flag)
  2342. {
  2343. // Extrapolate one step: if flag!=0 then use the cached parameters
  2344. Double_t vect[7], charge;
  2345. if (flag == 0) {
  2346. if (track->GetType() == MpdKalmanTrack::kBarrel) SetGeantParamB(track, vect, 1);
  2347. else SetGeantParamE(track, vect, 1);
  2348. charge = fVectorG[7] = -TMath::Sign(1.0,track->GetParam(4));
  2349. } else {
  2350. for (Int_t j = 0; j < 7; ++j) vect[j] = fVectorG[j];
  2351. charge = fVectorG[7];
  2352. }
  2353. ExtrapOneStepRungekutta(charge, step, vect, fVectorG);
  2354. return fVectorG;
  2355. }
  2356. //__________________________________________________________________________
  2357. ClassImp(MpdKalmanFilter)