Merge branch 'master_patch'
[u/mrichter/AliRoot.git] / PHOS / AliPHOSModuleMisalignment.cxx
1 /**************************************************************************
2  * Copyright(c) 1998-1999, ALICE Experiment at CERN, All rights reserved. *
3  *                                                                        *
4  * Author: The ALICE Off-line Project.                                    *
5  * Contributors are mentioned in the code where appropriate.              *
6  *                                                                        *
7  * Permission to use, copy, modify and distribute this software and its   *
8  * documentation strictly for non-commercial purposes is hereby granted   *
9  * without fee, provided that the above copyright notice appears in all   *
10  * copies and that both the copyright notice and this permission notice   *
11  * appear in the supporting documentation. The authors make no claims     *
12  * about the suitability of this software for any purpose. It is          *
13  * provided "as is" without express or implied warranty.                  *
14  **************************************************************************/
15
16 /* $Id: $ */
17
18 #include <stdexcept>
19 #include <iostream>
20
21 #include <TGeoMatrix.h>
22 #include <TObjArray.h>
23 #include <TString.h>
24 #include <TError.h>
25 #include <TMath.h>
26
27 #include "AliSurveyPoint.h"
28
29 #include "AliPHOSModuleMisalignment.h"
30 #include "AliPHOSGeometry.h"
31
32 ClassImp(AliPHOSModuleMisalignment)
33
34 namespace {
35
36   /*
37     Tiny vector/matrix utility stuff. Operates on doubles directly.
38     Instead of points and vectors I use arrays of doubles with size 3.
39     To make this explicit - I use references to arrays.
40   */
41   
42   //___________________________________________________________________
43   void Vector(const Double_t (&p1)[3], const Double_t (&p2)[3], Double_t (&v)[3])
44   {
45     for(UInt_t i = 0; i < 3; ++i)
46       v[i] = p2[i] - p1[i];
47   }
48   
49 #if 0
50   //___________________________________________________________________
51   void MultVector(Double_t (&v)[3], Double_t m)
52   {
53     v[0] *= m;
54     v[1] *= m;
55     v[2] *= m;
56   }
57 #endif
58   
59   /*
60     Using points name0, name1, name2 find two orthogonal vectors.
61   */
62   //___________________________________________________________________
63   void FindVectors(const Double_t (&pts)[3][3], Double_t (&v)[3][3])
64   {
65     Vector(pts[0], pts[2], v[0]);
66     //v[1] will be cross-product (v[2] x v[0]).
67     Vector(pts[0], pts[1], v[2]);
68   }
69   
70   //___________________________________________________________________
71   Double_t Length(const Double_t (&v)[3])
72   {
73     return TMath::Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
74   }
75   
76 #if 0
77   //___________________________________________________________________
78   Double_t Distance(const Double_t (&p1)[3], const Double_t (&p2)[3])
79   {
80     return TMath::Sqrt((p2[0] - p1[0]) * (p2[0] - p1[0]) +
81                        (p2[1] - p1[1]) * (p2[1] - p1[1]) +
82                        (p2[2] - p1[2]) * (p2[2] - p1[2]));
83   }
84   
85   //______________________________________________________________________________
86 #endif
87   void CrossProduct(const Double_t (&v1)[3], const Double_t (&v2)[3], Double_t (&v3)[3])
88   {
89     v3[0] = v1[1] * v2[2] - v2[1] * v1[2];
90     v3[1] = v1[2] * v2[0] - v2[2] * v1[0];
91     v3[2] = v1[0] * v2[1] - v2[0] * v1[1];
92   }
93   
94   //___________________________________________________________________
95   void Normalize(Double_t (&v)[3])
96   {
97     const Double_t len = Length(v);
98     if(len < 1E-10)//Threshold?
99       throw std::runtime_error("Zero len vector");
100     v[0] /= len;
101     v[1] /= len;
102     v[2] /= len;
103   }
104   
105   //______________________________________________________________________________
106   void Normalize(Double_t (&v)[3][3])
107   {
108     for(UInt_t i = 0; i < 3; ++i)
109       Normalize(v[i]);
110   }
111   
112   
113   //___________________________________________________________________
114   void FindRotation(const Double_t (&u)[3][3], const Double_t (&v)[3][3], Double_t (&r)[9])
115   {
116     //I have orthogonal vectors and very nice rotation matrix.
117     //V = R * U, R = V * U ^ t
118     r[0] = v[0][0] * u[0][0] + v[1][0] * u[1][0] + v[2][0] * u[2][0];
119     r[1] = v[0][0] * u[0][1] + v[1][0] * u[1][1] + v[2][0] * u[2][1];
120     r[2] = v[0][0] * u[0][2] + v[1][0] * u[1][2] + v[2][0] * u[2][2];
121     
122     r[3] = v[0][1] * u[0][0] + v[1][1] * u[1][0] + v[2][1] * u[2][0];
123     r[4] = v[0][1] * u[0][1] + v[1][1] * u[1][1] + v[2][1] * u[2][1];
124     r[5] = v[0][1] * u[0][2] + v[1][1] * u[1][2] + v[2][1] * u[2][2];
125     
126     r[6] = v[0][2] * u[0][0] + v[1][2] * u[1][0] + v[2][2] * u[2][0];
127     r[7] = v[0][2] * u[0][1] + v[1][2] * u[1][1] + v[2][2] * u[2][1];
128     r[8] = v[0][2] * u[0][2] + v[1][2] * u[1][2] + v[2][2] * u[2][2];
129   }
130   
131   //___________________________________________________________________
132   void Rotate(const Double_t (&r)[9], const Double_t (&u)[3], Double_t (&v)[3])
133   {
134     v[0] = r[0] * u[0] + r[1] * u[1] + r[2] * u[2];
135     v[1] = r[3] * u[0] + r[4] * u[1] + r[5] * u[2];
136     v[2] = r[6] * u[0] + r[7] * u[1] + r[8] * u[2];
137   }
138   
139   //___________________________________________________________________
140   void Rotate(const Double_t (&r)[9], const Double_t (&u)[3][3], Double_t (&v)[3][3])
141   {
142     for(UInt_t i = 0; i < 3; ++i)
143       Rotate(r, u[i], v[i]);
144   }
145   
146   /*
147     PrintVector, PrintMatrix, Translate are used in "debug" mode only.
148   */
149   //___________________________________________________________________
150   void PrintVector(const Double_t (&v)[3])
151   {
152     std::cout<<v[0]<<' '<<v[1]<<' '<<v[2]<<std::endl;
153   }
154   
155   //___________________________________________________________________
156   void PrintMatrix(const Double_t (&u)[3][3])
157   {
158     for(UInt_t i = 0; i < 3; ++i)
159       PrintVector(u[i]);
160   }
161   
162   //___________________________________________________________________
163   void Translate(const Double_t (&t)[3], const Double_t (&u)[3], Double_t (&v)[3])
164   {
165     for(UInt_t i = 0; i < 3; ++i)
166       v[i] = u[i] + t[i];
167   }
168   
169   //___________________________________________________________________
170   void Translate(const Double_t (&t)[3], const Double_t (&u)[3][3], Double_t (&v)[3][3])
171   {
172     for(UInt_t i = 0; i < 3; ++i)
173       Translate(t, u[i], v[i]);
174   }
175   
176 }
177
178 //______________________________________________________________________________
179 AliPHOSModuleMisalignment::
180 AliPHOSModuleMisalignment(const AliPHOSGeometry & geom, Bool_t debug)
181                             : fDebug(debug),
182                               fAngles(),
183                               fCenters(),
184                               fModule(),
185                               fU(),
186                               fV()
187 {
188   //Ctor.
189   //Extract ideal module transformations from AliPHOSGeometry.
190
191   //Angles.
192   for (UInt_t module = 0; module < kModules; ++module)
193     for (UInt_t axis = 0; axis < 3; ++axis)
194       for (UInt_t angle = 0; angle < 2; ++angle)
195         fAngles[module][axis][angle] = geom.GetModuleAngle(module, axis, angle);
196   //Translations.
197   for (UInt_t module = 0; module < kModules; ++module)
198     for (UInt_t axis = 0; axis < 3; ++axis)
199       fCenters[module][axis] = geom.GetModuleCenter(module, axis);
200   //Points, will be rotated/translated using module angle/center.
201   fModule[0][0] = -geom.GetNPhi() / 2. * geom.GetCellStep() + geom.GetCellStep() / 2.;
202   fModule[0][1] = -geom.GetNZ()   / 2. * geom.GetCellStep() + geom.GetCellStep() / 2.;
203   fModule[0][2] = -22.61;//This number is hardcoded, AliPHOSGeometry does not have it,
204                          //only 460. but this is result of transformation applied already.
205   fModule[1][0] = fModule[0][0];
206   fModule[1][1] = -fModule[0][1] - geom.GetCellStep();
207   fModule[1][2] = -22.61;
208
209   fModule[2][0] = -fModule[0][0] - 7 * geom.GetCellStep();
210   fModule[2][1] = fModule[0][1];
211   fModule[2][2] = -22.61;
212 }
213
214 //______________________________________________________________________________
215 AliPHOSModuleMisalignment::~AliPHOSModuleMisalignment()
216 {
217 }
218
219 //______________________________________________________________________________
220 void AliPHOSModuleMisalignment::
221 DeltaTransformation(UInt_t module, const TObjArray * points, 
222                     const TString & name0, const TString & name1, 
223                     const TString & name2, TGeoHMatrix * delta)
224 {
225   //Find delta transformation to misalign module. Global transformation.
226   const AliSurveyPoint * pt0 = static_cast<AliSurveyPoint *>(points->FindObject(name0));
227   const AliSurveyPoint * pt1 = static_cast<AliSurveyPoint *>(points->FindObject(name1));
228   const AliSurveyPoint * pt2 = static_cast<AliSurveyPoint *>(points->FindObject(name2));
229
230   if (!pt0 || !pt1 || !pt2) {
231     Warning("AliPHOSModuleData::DeltaTransformation",
232             "One of points not found in TObjArray");
233     return;
234   }
235
236   //Transform fModule using angle and translation for module number "module".
237   //fU.
238   FindIdealModule(module);
239   //Extract coordinates from survey.
240   //fV.
241   FindRealModule(pt0, pt1, pt2);
242   //Find delta, using ideal module (fU) and survey data (fV).
243   FindDelta(delta);
244 }
245
246 //______________________________________________________________________________
247 void AliPHOSModuleMisalignment::FindIdealModule(UInt_t module)
248 {
249   //Ideal module described by fU.
250   TGeoHMatrix matrix;
251   //Rotation.
252   const TGeoRotation r("", 
253                        fAngles[module][0][0], fAngles[module][0][1], 
254                        fAngles[module][1][0], fAngles[module][1][1], 
255                        fAngles[module][2][0], fAngles[module][2][1]);
256   matrix.SetRotation(r.GetRotationMatrix());
257   //Translation.
258   matrix.SetDx(fCenters[module][0]);
259   matrix.SetDy(fCenters[module][1]);
260   matrix.SetDz(fCenters[module][2]);
261   //Find ideal module's points.
262   matrix.LocalToMaster(fModule[0], fU[0]);
263   matrix.LocalToMaster(fModule[1], fU[1]);
264   matrix.LocalToMaster(fModule[2], fU[2]);
265 }
266
267 //______________________________________________________________________________
268 void AliPHOSModuleMisalignment::FindRealModule(const AliSurveyPoint * pt0, 
269                                                const AliSurveyPoint * pt1,
270                                                const AliSurveyPoint * pt2)
271 {
272   //Real module - fV.
273   //Survey is in millimeters.
274   //AliPHOSGeometry is in centimeters.
275   const Double_t scale = 0.1;
276
277   fV[0][0] = pt0->GetX() * scale;
278   fV[0][1] = pt0->GetY() * scale;
279   fV[0][2] = pt0->GetZ() * scale;
280
281   fV[1][0] = pt1->GetX() * scale;
282   fV[1][1] = pt1->GetY() * scale;
283   fV[1][2] = pt1->GetZ() * scale;
284
285   fV[2][0] = pt2->GetX() * scale;
286   fV[2][1] = pt2->GetY() * scale;
287   fV[2][2] = pt2->GetZ() * scale;
288 }
289
290 //______________________________________________________________________________
291 void AliPHOSModuleMisalignment::FindDelta(TGeoHMatrix * delta)const
292 {
293   //Find rotation and translation wich can
294   //convert fU into fV (ideal module points into points from survey).
295   Double_t u[3][3] = {};
296   FindVectors(fU, u);                 
297   //Find cross product u2 x u0 and save it in u[2].
298   CrossProduct(u[2], u[0], u[1]);
299   /*
300   const Double_t lenXideal = Length(u[0]);
301   const Double_t lenZideal = Length(u[2]);
302   */
303   Double_t v[3][3] = {};
304   FindVectors(fV, v);                 
305   //Find cross product (v2 x v0) and save it in v[2].
306   CrossProduct(v[2], v[0], v[1]);
307   /*
308   const Double_t lenXreal = Length(v[0]);
309   const Double_t lenZreal = Length(v[2]);
310   */
311   //Now, find rotation matrix.
312   //1. Normalize vectors in u and v.
313   try {
314     Normalize(u);
315     Normalize(v);
316   } catch (const std::exception & e) {
317     //One of lengths is zero (in principle, impossible, just to be neat).
318     Error("AliPHOSModuleMisalignment::FindDelta", 
319           "\tone of vectors from ideal or real\n\tpoints have zero size\n"
320           "\tzero misalignment will be created");
321     return;
322   }
323
324   //2. Rotation matrix.
325   Double_t r[9] = {};
326   FindRotation(u, v, r);
327   delta->SetRotation(r);
328   
329 #if 1
330
331   //3. Now, rotate fU and look, what translation I have to add.
332   Double_t idealRotated[3] = {};
333   Rotate(r, fU[0], idealRotated);
334
335   delta->SetDx(fV[0][0] - idealRotated[0]);
336   delta->SetDy(fV[0][1] - idealRotated[1]);
337   delta->SetDz(fV[0][2] - idealRotated[2]);
338
339   if (fDebug) {
340     const Double_t shifts[3] = 
341     {fV[0][0] - idealRotated[0], 
342      fV[0][1] - idealRotated[1], 
343      fV[0][2] - idealRotated[2]};
344
345     Double_t test1[3][3] = {};
346     Rotate(r, fU, test1);
347     Double_t test2[3][3] = {};
348     Translate(shifts, test1, test2);
349     std::cout<<"ideal:\n";
350     PrintMatrix(fU);
351     std::cout<<"misaligned:\n";
352     PrintMatrix(test2);
353     std::cout<<"real:\n";
354     PrintMatrix(fV);
355   }
356
357 #endif
358
359 #if 0
360   //3. Now, rotate fU and look, what translation I have to add.
361   Double_t idealRotated[3][3] = {};
362   Rotate(r, fU, idealRotated);
363   //Because of measurement errors, distances 
364   //between points has errors. I can try to split
365   //this difference (and make "final errors" smaller).
366   Double_t zShift[3] = {};
367   Vector(fV[0], fV[1], zShift);
368   Normalize(zShift);
369   
370   Double_t xShift[3] = {};
371   Vector(fV[0], fV[2], xShift);
372   Normalize(xShift);
373
374   MultVector(zShift, 0.5 * (lenZreal - lenZideal));
375   MultVector(xShift, 0.5 * (lenXreal - lenXideal));
376
377   Double_t pt1[3] = {};
378   Translate(zShift, fV[0], pt1);
379   Double_t pt2[3] = {};
380   Translate(xShift, pt1, pt2);
381
382   Double_t shifts[] = {pt2[0] - idealRotated[0][0],
383                        pt2[1] - idealRotated[0][1],
384                        pt2[2] - idealRotated[0][2]};
385
386   delta->SetDx(shifts[0]);
387   delta->SetDy(shifts[1]);
388   delta->SetDz(shifts[2]);
389
390   if (fDebug) {
391     Double_t idealTr[3][3] = {};
392     Translate(shifts, idealRotated, idealTr);
393
394     std::cout<<"misaligned:\n";
395     PrintMatrix(idealTr);
396     std::cout<<"ideal1 "<<Distance(idealTr[0], idealTr[1])<<std::endl;
397     std::cout<<"ideal2 "<<Distance(idealTr[0], idealTr[2])<<std::endl;
398     std::cout<<"real:\n";
399     PrintMatrix(fV);
400     std::cout<<"real1 "<<Distance(fV[0], fV[1])<<std::endl;
401     std::cout<<"real2 "<<Distance(fV[0], fV[2])<<std::endl;
402   }
403 #endif
404 }