Adding calibration object for the sharing efficiency
[u/mrichter/AliRoot.git] / FMD / AliFMDSurveyToAlignObjs.cxx
1 #include "AliFMDSurveyToAlignObjs.h"
2 #include "AliLog.h"
3 #include "AliSurveyPoint.h"
4 #include <TGraph2DErrors.h>
5 #include <TF2.h>
6 #include <TVector3.h>
7 #include <iostream>
8 #include <iomanip>
9 #include <TMath.h>
10 #include <TRotation.h>
11 #include <TGeoMatrix.h>
12 #include <TGeoManager.h>
13 #include <TGeoPhysicalNode.h>
14 #include "AliFMDGeometry.h"
15
16 //____________________________________________________________________
17 Double_t
18 AliFMDSurveyToAlignObjs::GetUnitFactor() const
19 {
20   // Returns the conversion factor from the measured values to
21   // centimeters. 
22   if (!fSurveyObj) return 0;
23   TString units(fSurveyObj->GetUnits());
24   if      (units.CompareTo("mm", TString::kIgnoreCase) == 0) return .1;
25   else if (units.CompareTo("cm", TString::kIgnoreCase) == 0) return 1.;
26   else if (units.CompareTo("m",  TString::kIgnoreCase) == 0) return 100.;
27   return 1;
28 }
29
30 //____________________________________________________________________
31 Bool_t
32 AliFMDSurveyToAlignObjs::GetPoint(const char* name, 
33                                   TVector3&   point, 
34                                   TVector3&   error) const
35 {
36   // Get named point.   On return, point will contain the point
37   // coordinates in centimeters, and error will contain the
38   // meassurement errors in centimeters too.  If no point is found,
39   // returns false, otherwise true. 
40   if (!fSurveyPoints) return kFALSE;
41   
42   Double_t unit = GetUnitFactor();
43   if (unit == 0) return kFALSE;
44   
45   TObject* obj  = fSurveyPoints->FindObject(name);
46   if (!obj) return kFALSE;
47   
48   AliSurveyPoint* p = static_cast<AliSurveyPoint*>(obj);
49   point.SetXYZ(unit * p->GetX(), 
50                unit * p->GetY(),
51                unit * p->GetZ());
52   error.SetXYZ(unit * p->GetPrecisionX(),
53                unit * p->GetPrecisionY(),
54                unit * p->GetPrecisionZ());
55   return kTRUE;
56 }
57
58 //____________________________________________________________________
59 Bool_t 
60 AliFMDSurveyToAlignObjs::CalculatePlane(const     TVector3& a, 
61                                         const     TVector3& b,
62                                         const     TVector3& c, 
63                                         Double_t  depth,
64                                         Double_t* trans,
65                                         Double_t* rot) const
66 {
67   // Vector a->b, b->c, and normal to plane defined by these two
68   // vectors. 
69   TVector3 ab(b-a), bc(c-b);
70   
71   // Normal vector to the plane of the fiducial marks obtained
72   // as cross product of the two vectors on the plane d0^d1
73   TVector3 nn(ab.Cross(bc));
74   if (nn.Mag() < 1e-8) { 
75     Info("CalculatePlane", "Normal vector is null vector");
76     return kFALSE;
77   }
78   
79   // We express the plane in Hessian normal form.
80   //
81   //   n x = -p,
82   //
83   // where n is the normalised normal vector given by 
84   // 
85   //   n_x = a / l,   n_y = b / l,   n_z = c / l,  p = d / l
86   //
87   // with l = sqrt(a^2+b^2+c^2) and a, b, c, and d are from the
88   // normal plane equation 
89   //
90   //  ax + by + cz + d = 0
91   // 
92   // Normalize
93   TVector3 n(nn.Unit());
94   // Double_t p = - (n * a);
95   
96   // The center of the square with the fiducial marks as the
97   // corners.  The mid-point of one diagonal - md.  Used to get the
98   // center of the surveyd box. 
99   TVector3 md(a + c);
100   md *= 1/2.;
101
102   // The center of the box. 
103   TVector3 orig(md - depth * n);
104   trans[0] = orig[0];
105   trans[1] = orig[1];
106   trans[2] = orig[2];
107   
108   // Normalize the spanning vectors 
109   TVector3 uab(ab.Unit());
110   TVector3 ubc(bc.Unit());
111   
112   for (size_t i = 0; i < 3; i++) { 
113     rot[i * 3 + 0] = ubc[i];
114     rot[i * 3 + 1] = uab[i];
115     rot[i * 3 + 2] = n[i];
116   }
117   return kTRUE;
118 }
119
120 //____________________________________________________________________
121 Bool_t 
122 AliFMDSurveyToAlignObjs::FitPlane(const TObjArray& points, 
123                                   const TObjArray& errors,
124                                   Double_t         /* depth */,
125                                   Double_t*        trans,
126                                   Double_t*        rot) const
127 {
128   Int_t nPoints = points.GetEntries();
129   if (nPoints < 4) { 
130     AliError(Form("Cannot fit a plane equation to less than 4 survey points, "
131                   "got only %d", nPoints));
132     return kFALSE;
133   }
134   
135   TGraph2DErrors g;
136   // Loop and fill graph 
137   for (int i = 0; i < nPoints; i++) {
138     TVector3* p = static_cast<TVector3*>(points.At(i));
139     TVector3* e = static_cast<TVector3*>(errors.At(i));
140   
141     if (!p || !e) continue;
142     
143     g.SetPoint(i, p->X(), p->Y(), p->Z());
144     g.SetPointError(i, e->X(), e->Y(), e->Z());
145   }
146
147   // Check that we have enough points
148   if (g.GetN() < 4) { 
149     AliError(Form("Only got %d survey points - no good for plane fit",
150                   g.GetN()));
151     return kFALSE;
152   }
153
154   // Next, declare fitting function and fit to graph. 
155   // Fit to the plane equation: 
156   // 
157   //   ax + by + cz + d = 0
158   //
159   // or 
160   // 
161   //   z = - ax/c - by/c - d/c
162   //
163   TF2 f("plane", "-[0]*x-[1]*y-[2]", 
164         g.GetXmin(), g.GetXmax(), g.GetYmin(), g.GetYmax());
165   g.Fit(&f, "Q");
166   
167   // Now, extract the normal and offset
168   TVector3 nv(f.GetParameter(0), f.GetParameter(1), 1);
169   TVector3 n(nv.Unit());
170   Double_t p = -f.GetParameter(2);
171   
172   // Create two vectors spanning the plane 
173   TVector3 a(1, 0, f.Eval(1, 0)-p);
174   TVector3 b(0, -1, f.Eval(0, -1)-p);
175   TVector3 ua(a.Unit());
176   TVector3 ub(b.Unit());
177   // Double_t angAb = ua.Angle(ub);
178   // PrintVector("ua: ", ua);
179   // PrintVector("ub: ", ub);
180   // std::cout << "Angle: " << angAb * 180 / TMath::Pi() << std::endl;
181     
182   for (size_t i = 0; i < 3; i++) { 
183     rot[i * 3 + 0] = ua[i];
184     rot[i * 3 + 1] = ub[i];
185     rot[i * 3 + 2] = n[i];
186   }
187     
188   // The intersection of the plane is given by (0, 0, -d/c)
189   trans[0] = 0;
190   trans[1] = 0;
191   trans[2] = p;
192   
193   return kTRUE;
194 }
195
196
197 //____________________________________________________________________
198 Bool_t
199 AliFMDSurveyToAlignObjs::MakeDelta(const char*  path, 
200                                    Double_t*    rot, 
201                                    Double_t*    trans, 
202                                    TGeoHMatrix& delta) const
203 {
204   // Make delta transformation
205   if (!gGeoManager)           return kFALSE;
206   if (!gGeoManager->cd(path)) return kFALSE;
207   
208   
209   TGeoMatrix*  global = gGeoManager->GetCurrentMatrix();
210 #if 0
211   PrintRotation(Form("%s rot:", global->GetName()),global->GetRotationMatrix());
212   PrintVector(Form("%s trans:", global->GetName()),global->GetTranslation());
213 #endif
214
215   return MakeDelta(global, rot, trans, delta);
216 }
217
218 //____________________________________________________________________
219 Bool_t
220 AliFMDSurveyToAlignObjs::MakeDelta(TGeoMatrix*  global,
221                                    Double_t*    rot, 
222                                    Double_t*    trans, 
223                                    TGeoHMatrix& delta) const
224 {
225   // Make delta transformation
226   TGeoHMatrix* geoM = new TGeoHMatrix;
227   geoM->SetTranslation(trans);
228   geoM->SetRotation(rot);
229
230   delta = global->Inverse();
231   delta.MultiplyLeft(geoM);
232   
233   return true;
234 }
235
236 //____________________________________________________________________
237 Bool_t
238 AliFMDSurveyToAlignObjs::GetFMD1Plane(Double_t* rot, Double_t* trans) const
239 {
240
241   // The possile survey points 
242   TVector3  icb, ict, ocb, oct, dummy;
243   Int_t     missing = 0;
244   if (!GetPoint("V0L_ICB", icb, dummy)) missing++;
245   if (!GetPoint("V0L_ICT", ict, dummy)) missing++;
246   if (!GetPoint("V0L_OCB", ocb, dummy)) missing++;
247   if (!GetPoint("V0L_OCT", oct, dummy)) missing++;
248
249   // Check that we have enough points
250   if (missing > 1) { 
251     AliWarning(Form("Only got %d survey points - no good for FMD1 plane",
252                     4-missing));
253     return kFALSE;
254   }
255
256 #if 0
257   const char* lidN = "FMD1_lid_mat0";
258   TGeoMatrix* lidM = static_cast<TGeoMatrix*>(gGeoManager->GetListOfMatrices()
259                                               ->FindObject(lidN));
260   if (!lidM) { 
261     AliError(Form("Couldn't find FMD1 lid transformation %s", lidN));
262     return kFALSE;
263   }
264
265   const Double_t* lidT = lidM->GetTranslation();
266   Double_t        lidZ = lidT[2];
267   Double_t        off  = lidZ-3.3;
268 #else
269   Double_t        off  = 0;
270 #endif
271
272   if (!CalculatePlane(ocb, icb, ict, off, trans, rot)) return kFALSE;
273   // PrintRotation("FMD1 rotation:",  rot);
274   // PrintVector("FMD1 translation:", trans);
275
276   return kTRUE;
277 }
278
279 //____________________________________________________________________
280 Bool_t
281 AliFMDSurveyToAlignObjs::DoFMD1()
282 {
283   // Do the FMD1 stuff
284   Double_t rot[9], trans[3];
285   if (!GetFMD1Plane(rot, trans)) return kFALSE;
286   // const char* path = "/ALIC_1/F1MT_1/FMD1_lid_0";
287   
288   // TGeoHMatrix delta;
289   TGeoTranslation global(0,0,324.670);
290   if (!MakeDelta(&global, rot, trans, fFMD1Delta)) 
291     return kFALSE;
292   
293   // PrintRotation("FMD1 delta rotation:",  fFMD1Delta.GetRotationMatrix());
294   // PrintVector("FMD1 delta translation:", fFMD1Delta.GetTranslation());
295
296   return kTRUE;
297 }
298
299 //____________________________________________________________________
300 Bool_t
301 AliFMDSurveyToAlignObjs::GetFMD2Plane(Double_t* rot, Double_t* trans) const
302 {
303
304   // The possible survey points 
305   const char*    names[] = { "FMD2_ITOP",  "FMD2_OTOP", 
306                              "FMD2_IBOTM", "FMD2_OBOTM", 
307                              "FMD2_IBOT",  "FMD2_OBOT", 
308                              0 };
309   const char**   name    = names;
310
311   TObjArray points;
312   TObjArray errors;
313   
314   // Loop and fill graph 
315   while (*name) {
316     TVector3 p, e;
317     if (!GetPoint(*name++, p, e)) continue;
318     
319     points.Add(new TVector3(p));
320     errors.Add(new TVector3(e));
321   }
322   if (points.GetEntries() < 4) { 
323     AliWarning(Form("Only got %d survey points - no good for FMD2 plane",
324                     points.GetEntries()));
325     return kFALSE;
326   }
327
328   return FitPlane(points, errors, 0, trans, rot);
329 }
330
331 #define M(I,J) rot[(J-1) * 3 + (I-1)]
332 //____________________________________________________________________
333 Bool_t
334 AliFMDSurveyToAlignObjs::DoFMD2()
335 {
336   // Do the FMD2 stuff
337   Double_t rot[9], trans[3];
338   if (!GetFMD2Plane(rot, trans)) return kFALSE;
339   // PrintRotation("FMD2 rotation:",  rot);
340   // PrintVector("FMD2 translation:", trans);
341
342   for (int i = 0; i < 3; i++) { 
343     for (int j = 0; j < 3; j++) { 
344       rot[i*3+j] = (i == j ? 1 : 0);
345     }
346   }
347   trans[0] = trans[1] = 0;
348   trans[2] += 0.015;
349   // PrintRotation("FMD2 rotation:",  rot);
350   // PrintVector("FMD2 translation:", trans);
351   
352   // TGeoHMatrix delta;
353   if (!MakeDelta("/ALIC_1/F2MT_2/FMD2_support_0/FMD2_back_cover_2", 
354                  rot, trans, fFMD2Delta)) return kFALSE;
355   
356   // PrintRotation("FMD2 delta rotation:",  fFMD2Delta.GetRotationMatrix());
357   // PrintVector("FMD2 delta translation:", fFMD2Delta.GetTranslation());
358
359   return kTRUE;
360 }
361
362 //____________________________________________________________________
363 void
364 AliFMDSurveyToAlignObjs::Run()
365 {
366   AliFMDGeometry* geom = AliFMDGeometry::Instance();
367   geom->Init();
368   geom->InitTransformations();
369   
370   DoFMD1();
371   DoFMD2();
372 }
373
374 //____________________________________________________________________
375 Bool_t 
376 AliFMDSurveyToAlignObjs::CreateAlignObjs()
377 {
378   TClonesArray& array = *fAlignObjArray;
379   Int_t         n     = array.GetEntriesFast();
380
381   if (!fFMD1Delta.IsIdentity()) { 
382     new (array[n++]) AliAlignObjParams("FMD1/FMD1_T", 0, fFMD1Delta, kTRUE);
383     new (array[n++]) AliAlignObjParams("FMD1/FMD1_B", 0, fFMD1Delta, kTRUE);
384   }
385   if (!fFMD2Delta.IsIdentity()) { 
386     new (array[n++]) AliAlignObjParams("FMD2/FMD2_T", 0, fFMD2Delta, kTRUE);
387     new (array[n++]) AliAlignObjParams("FMD2/FMD2_B", 0, fFMD2Delta, kTRUE);
388   }
389   // array.Print();
390   
391   return kTRUE;
392 }
393
394 //____________________________________________________________________
395 void 
396 AliFMDSurveyToAlignObjs::PrintVector(const char* text, const TVector3& v)
397 {
398   // Print a vector
399   Double_t va[] = { v.X(), v.Y(), v.Z() };
400   PrintVector(text, va);
401 }
402 //____________________________________________________________________
403 void 
404 AliFMDSurveyToAlignObjs::PrintVector(const char* text, const Double_t* v)
405 {
406   // Print a vector
407   std::cout << text 
408             << std::setw(15) << v[0] 
409             << std::setw(15) << v[1]
410             << std::setw(15) << v[2]
411             << std::endl;
412 }
413
414
415 //____________________________________________________________________
416 void 
417 AliFMDSurveyToAlignObjs::PrintRotation(const char* text, const Double_t* rot)
418 {
419   // Print a rotation matrix
420   std::cout << text << std::endl;
421   for (size_t i = 0; i < 3; i++) { 
422     for (size_t j = 0; j < 3; j++) 
423       std::cout << std::setw(15) << rot[i * 3 + j];
424     std::cout << std::endl;
425   }
426 }
427
428 //____________________________________________________________________
429 //
430 // EOF
431 //