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