bug fix
[u/mrichter/AliRoot.git] / FMD / AliFMDSurveyToAlignObjs.cxx
CommitLineData
b2e6f0b0 1#include "AliFMDSurveyToAlignObjs.h"
f567c3ce 2#include "AliLog.h"
b2e6f0b0 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//____________________________________________________________________
faf80567 17Double_t
18AliFMDSurveyToAlignObjs::GetUnitFactor() const
19{
20 // Returns the conversion factor from the measured values to
21 // centimeters.
f567c3ce 22 if (!fSurveyObj) return 0;
faf80567 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//____________________________________________________________________
b2e6f0b0 31Bool_t
faf80567 32AliFMDSurveyToAlignObjs::GetPoint(const char* name,
33 TVector3& point,
34 TVector3& error) const
b2e6f0b0 35{
faf80567 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.
f567c3ce 40 if (!fSurveyPoints) return kFALSE;
41
faf80567 42 Double_t unit = GetUnitFactor();
f567c3ce 43 if (unit == 0) return kFALSE;
44
faf80567 45 TObject* obj = fSurveyPoints->FindObject(name);
46 if (!obj) return kFALSE;
47
f567c3ce 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;
faf80567 56}
b2e6f0b0 57
faf80567 58//____________________________________________________________________
59Bool_t
60AliFMDSurveyToAlignObjs::CalculatePlane(const TVector3& a,
61 const TVector3& b,
62 const TVector3& c,
f567c3ce 63 Double_t depth,
faf80567 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);
b2e6f0b0 70
faf80567 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) {
f567c3ce 75 Info("CalculatePlane", "Normal vector is null vector");
faf80567 76 return kFALSE;
77 }
b2e6f0b0 78
faf80567 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());
f567c3ce 94 // Double_t p = - (n * a);
faf80567 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.;
f567c3ce 101
faf80567 102 // The center of the box.
f567c3ce 103 TVector3 orig(md - depth * n);
faf80567 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++) {
f567c3ce 113 rot[i * 3 + 0] = ubc[i];
114 rot[i * 3 + 1] = uab[i];
faf80567 115 rot[i * 3 + 2] = n[i];
116 }
117 return kTRUE;
118}
119
faf80567 120//____________________________________________________________________
f567c3ce 121Bool_t
122AliFMDSurveyToAlignObjs::FitPlane(const TObjArray& points,
123 const TObjArray& errors,
124 Double_t /* depth */,
125 Double_t* trans,
126 Double_t* rot) const
faf80567 127{
f567c3ce 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));
faf80567 132 return kFALSE;
133 }
f567c3ce 134
faf80567 135 TGraph2DErrors g;
b2e6f0b0 136 // Loop and fill graph
f567c3ce 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));
b2e6f0b0 140
f567c3ce 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
b2e6f0b0 147 // Check that we have enough points
148 if (g.GetN() < 4) {
f567c3ce 149 AliError(Form("Only got %d survey points - no good for plane fit",
150 g.GetN()));
b2e6f0b0 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 //
f567c3ce 163 TF2 f("plane", "-[0]*x-[1]*y-[2]",
164 g.GetXmin(), g.GetXmax(), g.GetYmin(), g.GetYmax());
b2e6f0b0 165 g.Fit(&f, "Q");
f567c3ce 166
b2e6f0b0 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);
f567c3ce 171
b2e6f0b0 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());
f567c3ce 177 // Double_t angAb = ua.Angle(ub);
faf80567 178 // PrintVector("ua: ", ua);
179 // PrintVector("ub: ", ub);
180 // std::cout << "Angle: " << angAb * 180 / TMath::Pi() << std::endl;
f567c3ce 181
b2e6f0b0 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 }
f567c3ce 187
b2e6f0b0 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;
f567c3ce 192
b2e6f0b0 193 return kTRUE;
194}
195
f567c3ce 196
b2e6f0b0 197//____________________________________________________________________
198Bool_t
f567c3ce 199AliFMDSurveyToAlignObjs::MakeDelta(const char* path,
200 Double_t* rot,
201 Double_t* trans,
202 TGeoHMatrix& delta) const
b2e6f0b0 203{
f567c3ce 204 // Make delta transformation
205 if (!gGeoManager) return kFALSE;
206 if (!gGeoManager->cd(path)) return kFALSE;
b2e6f0b0 207
b2e6f0b0 208
f567c3ce 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
b2e6f0b0 214
f567c3ce 215 return MakeDelta(global, rot, trans, delta);
216}
217
218//____________________________________________________________________
219Bool_t
220AliFMDSurveyToAlignObjs::MakeDelta(TGeoMatrix* global,
221 Double_t* rot,
222 Double_t* trans,
223 TGeoHMatrix& delta) const
224{
225 // Make delta transformation
b2e6f0b0 226 TGeoHMatrix* geoM = new TGeoHMatrix;
b2e6f0b0 227 geoM->SetTranslation(trans);
f567c3ce 228 geoM->SetRotation(rot);
229
230 delta = global->Inverse();
231 delta.MultiplyLeft(geoM);
232
233 return true;
234}
235
236//____________________________________________________________________
237Bool_t
238AliFMDSurveyToAlignObjs::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;
b2e6f0b0 270#endif
f567c3ce 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//____________________________________________________________________
280Bool_t
281AliFMDSurveyToAlignObjs::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;
b2e6f0b0 292
f567c3ce 293 // PrintRotation("FMD1 delta rotation:", fFMD1Delta.GetRotationMatrix());
294 // PrintVector("FMD1 delta translation:", fFMD1Delta.GetTranslation());
295
296 return kTRUE;
297}
298
299//____________________________________________________________________
300Bool_t
301AliFMDSurveyToAlignObjs::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;
b2e6f0b0 313
f567c3ce 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//____________________________________________________________________
333Bool_t
334AliFMDSurveyToAlignObjs::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);
b2e6f0b0 351
f567c3ce 352 // TGeoHMatrix delta;
353 if (!MakeDelta("/ALIC_1/F2MT_2/FMD2_support_0/FMD2_back_cover_2",
354 rot, trans, fFMD2Delta)) return kFALSE;
b2e6f0b0 355
f567c3ce 356 // PrintRotation("FMD2 delta rotation:", fFMD2Delta.GetRotationMatrix());
357 // PrintVector("FMD2 delta translation:", fFMD2Delta.GetTranslation());
358
b2e6f0b0 359 return kTRUE;
360}
361
362//____________________________________________________________________
363void
364AliFMDSurveyToAlignObjs::Run()
365{
366 AliFMDGeometry* geom = AliFMDGeometry::Instance();
367 geom->Init();
368 geom->InitTransformations();
369
f567c3ce 370 DoFMD1();
b2e6f0b0 371 DoFMD2();
372}
373
374//____________________________________________________________________
f567c3ce 375Bool_t
376AliFMDSurveyToAlignObjs::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//____________________________________________________________________
b2e6f0b0 395void
396AliFMDSurveyToAlignObjs::PrintVector(const char* text, const TVector3& v)
397{
f567c3ce 398 // Print a vector
b2e6f0b0 399 Double_t va[] = { v.X(), v.Y(), v.Z() };
400 PrintVector(text, va);
401}
402//____________________________________________________________________
403void
404AliFMDSurveyToAlignObjs::PrintVector(const char* text, const Double_t* v)
405{
f567c3ce 406 // Print a vector
b2e6f0b0 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//____________________________________________________________________
416void
417AliFMDSurveyToAlignObjs::PrintRotation(const char* text, const Double_t* rot)
418{
f567c3ce 419 // Print a rotation matrix
b2e6f0b0 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//