]>
Commit | Line | Data |
---|---|---|
1 | #include "AliFMDSurveyToAlignObjs.h" | |
2 | #include "AliSurveyPoint.h" | |
3 | #include <TGraph2DErrors.h> | |
4 | #include <TF2.h> | |
5 | #include <TVector3.h> | |
6 | #include <iostream> | |
7 | #include <iomanip> | |
8 | #include <TMath.h> | |
9 | #include <TRotation.h> | |
10 | #include <TGeoMatrix.h> | |
11 | #include <TGeoManager.h> | |
12 | #include <TGeoPhysicalNode.h> | |
13 | #include "AliFMDGeometry.h" | |
14 | ||
15 | //____________________________________________________________________ | |
16 | Bool_t | |
17 | AliFMDSurveyToAlignObjs::GetFMD2Plane(Double_t* rot, | |
18 | Double_t* trans) | |
19 | { | |
20 | ||
21 | // The possile survey points | |
22 | const char* names[] = { "FMD2_ITOP", "FMD2_OTOP", | |
23 | "FMD2_IBOTM", "FMD2_OBOTM", | |
24 | "FMD2_IBOT", "FMD2_OBOT", | |
25 | 0 }; | |
26 | const char** name = names; | |
27 | Int_t i = 0; | |
28 | TGraph2DErrors g; | |
29 | ||
30 | Double_t unit = 1.; | |
31 | TString units(fSurveyObj->GetUnits()); | |
32 | if (units.CompareTo("mm", TString::kIgnoreCase) == 0) unit = .1; | |
33 | else if (units.CompareTo("cm", TString::kIgnoreCase) == 0) unit = 1.; | |
34 | else if (units.CompareTo("m", TString::kIgnoreCase) == 0) unit = 100.; | |
35 | ||
36 | // Loop and fill graph | |
37 | while (*name) { | |
38 | TObject* obj = fSurveyPoints->FindObject(*name); | |
39 | name++; | |
40 | if (!obj) continue; | |
41 | ||
42 | AliSurveyPoint* p = static_cast<AliSurveyPoint*>(obj); | |
43 | Double_t x = unit * p->GetX(); | |
44 | Double_t y = unit * p->GetY(); | |
45 | Double_t z = unit * p->GetZ(); | |
46 | Double_t ex = unit * p->GetPrecisionX(); | |
47 | Double_t ey = unit * p->GetPrecisionY(); | |
48 | Double_t ez = unit * p->GetPrecisionZ(); | |
49 | ||
50 | g.SetPoint(i, x, y, z); | |
51 | g.SetPointError(i, ex, ey, ez); | |
52 | i++; | |
53 | } | |
54 | ||
55 | // Check that we have enough points | |
56 | if (g.GetN() < 4) { | |
57 | Error("GetFMD2Plane", "Only got %d survey points - no good for FMD2 plane", | |
58 | g.GetN()); | |
59 | return kFALSE; | |
60 | } | |
61 | ||
62 | // Next, declare fitting function and fit to graph. | |
63 | // Fit to the plane equation: | |
64 | // | |
65 | // ax + by + cz + d = 0 | |
66 | // | |
67 | // or | |
68 | // | |
69 | // z = - ax/c - by/c - d/c | |
70 | // | |
71 | TF2 f("fmd2Plane", "-[0]*x-[1]*y-[2]", | |
72 | g.GetXmin(), g.GetXmax(), g.GetYmin(), g.GetYmax()); | |
73 | g.Fit(&f, "Q"); | |
74 | ||
75 | // Now, extract the normal and offset | |
76 | TVector3 nv(f.GetParameter(0), f.GetParameter(1), 1); | |
77 | TVector3 n(nv.Unit()); | |
78 | Double_t p = -f.GetParameter(2); | |
79 | ||
80 | // Create two vectors spanning the plane | |
81 | TVector3 a(1, 0, f.Eval(1, 0)-p); | |
82 | TVector3 b(0, -1, f.Eval(0, -1)-p); | |
83 | TVector3 ua(a.Unit()); | |
84 | TVector3 ub(b.Unit()); | |
85 | Double_t angAb = ua.Angle(ub); | |
86 | PrintVector("ua: ", ua); | |
87 | PrintVector("ub: ", ub); | |
88 | std::cout << "Angle: " << angAb * 180 / TMath::Pi() << std::endl; | |
89 | ||
90 | for (size_t i = 0; i < 3; i++) { | |
91 | rot[i * 3 + 0] = ua[i]; | |
92 | rot[i * 3 + 1] = ub[i]; | |
93 | rot[i * 3 + 2] = n[i]; | |
94 | } | |
95 | ||
96 | // The intersection of the plane is given by (0, 0, -d/c) | |
97 | trans[0] = 0; | |
98 | trans[1] = 0; | |
99 | trans[2] = p; | |
100 | ||
101 | return kTRUE; | |
102 | } | |
103 | ||
104 | #define M(I,J) rot[(J-1) * 3 + (I-1)] | |
105 | //____________________________________________________________________ | |
106 | Bool_t | |
107 | AliFMDSurveyToAlignObjs::DoFMD2() | |
108 | { | |
109 | Double_t rot[9], trans[3]; | |
110 | if (!GetFMD2Plane(rot, trans)) return kFALSE; | |
111 | ||
112 | PrintRotation("Rotation: ", rot); | |
113 | PrintVector("Translation: ", trans); | |
114 | ||
115 | #if 0 | |
116 | Double_t theta = TMath::ATan2(M(3,1), M(3,2)); | |
117 | Double_t phi = TMath::ACos(M(3,3)); | |
118 | Double_t psi = -TMath::ATan2(M(1,3), M(2,3)); | |
119 | ||
120 | std::cout << " Theta=" << theta * 180 / TMath::Pi() | |
121 | << " Phi=" << phi * 180 / TMath::Pi() | |
122 | << " Psi=" << psi * 180 / TMath::Pi() | |
123 | << std::endl; | |
124 | ||
125 | TRotation r; | |
126 | r.SetXEulerAngles(theta, phi, psi); | |
127 | r.Print(); | |
128 | ||
129 | TGeoRotation* geoR = new TGeoRotation("FMD2_survey_rotation", | |
130 | r.GetYTheta(), | |
131 | r.GetYPhi(), | |
132 | r.GetYPsi()); | |
133 | TGeoCombiTrans* geoM = new TGeoCombiTrans(trans[0], trans[1], trans[2], geoR); | |
134 | #else | |
135 | TGeoHMatrix* geoM = new TGeoHMatrix; | |
136 | geoM->SetRotation(rot); | |
137 | geoM->SetTranslation(trans); | |
138 | #endif | |
139 | geoM->Print(); | |
140 | ||
141 | const char* path = "/ALIC_1/F2MT_2/FMD2_support_0/FMD2_back_cover_2"; | |
142 | gGeoManager->cd(path); | |
143 | TGeoMatrix* globalBack = gGeoManager->GetCurrentMatrix(); | |
144 | globalBack->Print(); | |
145 | PrintRotation("Back rotation:", globalBack->GetRotationMatrix()); | |
146 | PrintVector("Back translation:", globalBack->GetTranslation()); | |
147 | ||
148 | // TObjArray* pns = gGeoManager->GetListOfPhysicalNodes(); | |
149 | // TObject* oT = pns->FindObject("/ALIC_1/F2MT_2"); | |
150 | // TObject* oB = pns->FindObject("/ALIC_1/F2MB_2"); | |
151 | // if (!oT) { | |
152 | // Warning("DoFMD2", Form("Physical node /ALIC_1/F2MT_2 not found")); | |
153 | // return kFALSE; | |
154 | // } | |
155 | // if (!oB) { | |
156 | // Warning("DoFMD2", Form("Physical node /ALIC_1/F2MB_2 not found")); | |
157 | // return kFALSE; | |
158 | // } | |
159 | // TGeoPhysicalNode* top = static_cast<TGeoPhysicalNode*>(oT); | |
160 | // TGeoPhysicalNode* bot = static_cast<TGeoPhysicalNode*>(oB); | |
161 | TGeoHMatrix tDelta(globalBack->Inverse()); | |
162 | TGeoHMatrix bDelta(globalBack->Inverse()); | |
163 | PrintRotation("Back^-1 rotation:", tDelta.GetRotationMatrix()); | |
164 | PrintVector("Back^-1 translation:", tDelta.GetTranslation()); | |
165 | ||
166 | std::cout << "tDelta = 1? " << tDelta.IsIdentity() << std::endl; | |
167 | ||
168 | tDelta.MultiplyLeft(geoM); | |
169 | bDelta.MultiplyLeft(geoM); | |
170 | ||
171 | PrintRotation("tDelta rotation:", tDelta.GetRotationMatrix()); | |
172 | PrintVector("tDelta translation:", tDelta.GetTranslation()); | |
173 | PrintRotation("bDelta rotation:", bDelta.GetRotationMatrix()); | |
174 | PrintVector("bDelta translation:", bDelta.GetTranslation()); | |
175 | ||
176 | return kTRUE; | |
177 | } | |
178 | ||
179 | //____________________________________________________________________ | |
180 | void | |
181 | AliFMDSurveyToAlignObjs::Run() | |
182 | { | |
183 | AliFMDGeometry* geom = AliFMDGeometry::Instance(); | |
184 | geom->Init(); | |
185 | geom->InitTransformations(); | |
186 | ||
187 | DoFMD2(); | |
188 | } | |
189 | ||
190 | //____________________________________________________________________ | |
191 | void | |
192 | AliFMDSurveyToAlignObjs::PrintVector(const char* text, const TVector3& v) | |
193 | { | |
194 | Double_t va[] = { v.X(), v.Y(), v.Z() }; | |
195 | PrintVector(text, va); | |
196 | } | |
197 | //____________________________________________________________________ | |
198 | void | |
199 | AliFMDSurveyToAlignObjs::PrintVector(const char* text, const Double_t* v) | |
200 | { | |
201 | std::cout << text | |
202 | << std::setw(15) << v[0] | |
203 | << std::setw(15) << v[1] | |
204 | << std::setw(15) << v[2] | |
205 | << std::endl; | |
206 | } | |
207 | ||
208 | ||
209 | //____________________________________________________________________ | |
210 | void | |
211 | AliFMDSurveyToAlignObjs::PrintRotation(const char* text, const Double_t* rot) | |
212 | { | |
213 | std::cout << text << std::endl; | |
214 | for (size_t i = 0; i < 3; i++) { | |
215 | for (size_t j = 0; j < 3; j++) | |
216 | std::cout << std::setw(15) << rot[i * 3 + j]; | |
217 | std::cout << std::endl; | |
218 | } | |
219 | } | |
220 | ||
221 | //____________________________________________________________________ | |
222 | // | |
223 | // EOF | |
224 | // |