[u/mrichter/AliRoot.git] / FMD / AliFMDSurveyToAlignObjs.cxx

#include "AliFMDSurveyToAlignObjs.h"
#include "AliSurveyPoint.h"
#include <TGraph2DErrors.h>
#include <TF2.h>
#include <TVector3.h>
#include <iostream>
#include <iomanip>
#include <TMath.h>
#include <TRotation.h>
#include <TGeoMatrix.h>
#include <TGeoManager.h>
#include <TGeoPhysicalNode.h>
#include "AliFMDGeometry.h"

//____________________________________________________________________
Bool_t
AliFMDSurveyToAlignObjs::GetFMD2Plane(Double_t* rot, 
				      Double_t* trans)
{

  // The possile survey points 
  const char*  names[] = { "FMD2_ITOP",  "FMD2_OTOP", 
			   "FMD2_IBOTM", "FMD2_OBOTM", 
			   "FMD2_IBOT",  "FMD2_OBOT", 
			   0 };
  const char** name    = names;
  Int_t    i = 0;
  TGraph2DErrors g;
  
  Double_t unit = 1.;
  TString units(fSurveyObj->GetUnits());
  if      (units.CompareTo("mm", TString::kIgnoreCase) == 0) unit = .1;
  else if (units.CompareTo("cm", TString::kIgnoreCase) == 0) unit = 1.;
  else if (units.CompareTo("m",  TString::kIgnoreCase) == 0) unit = 100.;
  
  // Loop and fill graph 
  while (*name) {
    TObject* obj = fSurveyPoints->FindObject(*name);
    name++;
    if (!obj) continue;
    
    AliSurveyPoint* p = static_cast<AliSurveyPoint*>(obj);
    Double_t x  = unit * p->GetX();
    Double_t y  = unit * p->GetY();
    Double_t z  = unit * p->GetZ();
    Double_t ex = unit * p->GetPrecisionX();
    Double_t ey = unit * p->GetPrecisionY();
    Double_t ez = unit * p->GetPrecisionZ();
    
    g.SetPoint(i, x, y, z);
    g.SetPointError(i, ex, ey, ez);
    i++;
  }
  
  // Check that we have enough points
  if (g.GetN() < 4) { 
    Error("GetFMD2Plane", "Only got %d survey points - no good for FMD2 plane",
	  g.GetN());
    return kFALSE;
  }

  // Next, declare fitting function and fit to graph. 
  // Fit to the plane equation: 
  // 
  //   ax + by + cz + d = 0
  //
  // or 
  // 
  //   z = - ax/c - by/c - d/c
  //
  TF2 f("fmd2Plane", "-[0]*x-[1]*y-[2]", 
	g.GetXmin(), g.GetXmax(), g.GetYmin(), g.GetYmax());
  g.Fit(&f, "Q");

  // Now, extract the normal and offset
  TVector3 nv(f.GetParameter(0), f.GetParameter(1), 1);
  TVector3 n(nv.Unit());
  Double_t p = -f.GetParameter(2);

  // Create two vectors spanning the plane 
  TVector3 a(1, 0, f.Eval(1, 0)-p);
  TVector3 b(0, -1, f.Eval(0, -1)-p);
  TVector3 ua(a.Unit());
  TVector3 ub(b.Unit());
  Double_t angAb = ua.Angle(ub);
  PrintVector("ua: ", ua);
  PrintVector("ub: ", ub);
  std::cout << "Angle: " << angAb * 180 / TMath::Pi() << std::endl;
  
  for (size_t i = 0; i < 3; i++) { 
    rot[i * 3 + 0] = ua[i];
    rot[i * 3 + 1] = ub[i];
    rot[i * 3 + 2] = n[i];
  }
  
  // The intersection of the plane is given by (0, 0, -d/c)
  trans[0] = 0;
  trans[1] = 0;
  trans[2] = p;

  return kTRUE;
}

#define M(I,J) rot[(J-1) * 3 + (I-1)]
//____________________________________________________________________
Bool_t
AliFMDSurveyToAlignObjs::DoFMD2()
{
  Double_t rot[9], trans[3];
  if (!GetFMD2Plane(rot, trans)) return kFALSE;
  
  PrintRotation("Rotation: ", rot);
  PrintVector("Translation: ", trans);

#if 0
  Double_t theta = TMath::ATan2(M(3,1), M(3,2));
  Double_t phi   = TMath::ACos(M(3,3));
  Double_t psi   = -TMath::ATan2(M(1,3), M(2,3));
  
  std::cout << " Theta=" << theta * 180 / TMath::Pi() 
	    << " Phi="   << phi   * 180 / TMath::Pi() 
	    << " Psi="   << psi   * 180 / TMath::Pi() 
	    << std::endl;

  TRotation r;
  r.SetXEulerAngles(theta, phi, psi);
  r.Print();
  
  TGeoRotation*   geoR = new TGeoRotation("FMD2_survey_rotation", 
					  r.GetYTheta(),
					  r.GetYPhi(),
					  r.GetYPsi());
  TGeoCombiTrans* geoM = new TGeoCombiTrans(trans[0], trans[1], trans[2], geoR);
#else
  TGeoHMatrix* geoM = new TGeoHMatrix;
  geoM->SetRotation(rot);
  geoM->SetTranslation(trans);
#endif
  geoM->Print();

  const char* path = "/ALIC_1/F2MT_2/FMD2_support_0/FMD2_back_cover_2";
  gGeoManager->cd(path);
  TGeoMatrix* globalBack = gGeoManager->GetCurrentMatrix();
  globalBack->Print();
  PrintRotation("Back rotation:",    globalBack->GetRotationMatrix());
  PrintVector("Back translation:",   globalBack->GetTranslation());
  
  // TObjArray* pns = gGeoManager->GetListOfPhysicalNodes();
  // TObject*   oT  = pns->FindObject("/ALIC_1/F2MT_2");
  // TObject*   oB  = pns->FindObject("/ALIC_1/F2MB_2");
  // if (!oT) { 
  //   Warning("DoFMD2", Form("Physical node /ALIC_1/F2MT_2 not found"));
  //   return kFALSE;
  // }
  // if (!oB) { 
  //   Warning("DoFMD2", Form("Physical node /ALIC_1/F2MB_2 not found"));
  //   return kFALSE;
  // }
  // TGeoPhysicalNode* top = static_cast<TGeoPhysicalNode*>(oT);
  // TGeoPhysicalNode* bot = static_cast<TGeoPhysicalNode*>(oB);
  TGeoHMatrix tDelta(globalBack->Inverse());
  TGeoHMatrix bDelta(globalBack->Inverse());
  PrintRotation("Back^-1 rotation:",    tDelta.GetRotationMatrix());
  PrintVector("Back^-1 translation:",   tDelta.GetTranslation());

  std::cout << "tDelta = 1? " << tDelta.IsIdentity() << std::endl;
  
  tDelta.MultiplyLeft(geoM);
  bDelta.MultiplyLeft(geoM);
  
  PrintRotation("tDelta rotation:",  tDelta.GetRotationMatrix());
  PrintVector("tDelta translation:", tDelta.GetTranslation());
  PrintRotation("bDelta rotation:",  bDelta.GetRotationMatrix());
  PrintVector("bDelta translation:", bDelta.GetTranslation());
  
  return kTRUE;
}

//____________________________________________________________________
void
AliFMDSurveyToAlignObjs::Run()
{
  AliFMDGeometry* geom = AliFMDGeometry::Instance();
  geom->Init();
  geom->InitTransformations();
  
  DoFMD2();
}

//____________________________________________________________________
void 
AliFMDSurveyToAlignObjs::PrintVector(const char* text, const TVector3& v)
{
  Double_t va[] = { v.X(), v.Y(), v.Z() };
  PrintVector(text, va);
}
//____________________________________________________________________
void 
AliFMDSurveyToAlignObjs::PrintVector(const char* text, const Double_t* v)
{
  std::cout << text 
	    << std::setw(15) << v[0] 
	    << std::setw(15) << v[1]
	    << std::setw(15) << v[2]
	    << std::endl;
}


//____________________________________________________________________
void 
AliFMDSurveyToAlignObjs::PrintRotation(const char* text, const Double_t* rot)
{
  std::cout << text << std::endl;
  for (size_t i = 0; i < 3; i++) { 
    for (size_t j = 0; j < 3; j++) 
      std::cout << std::setw(15) << rot[i * 3 + j];
    std::cout << std::endl;
  }
}

//____________________________________________________________________
//
// EOF
//
Commit	Line	Data
b2e6f0b0	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	//