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

#include "AliFMDSurveyToAlignObjs.h"
#include "AliLog.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"

//____________________________________________________________________
Double_t
AliFMDSurveyToAlignObjs::GetUnitFactor() const
{
  // Returns the conversion factor from the measured values to
  // centimeters. 
  if (!fSurveyObj) return 0;
  TString units(fSurveyObj->GetUnits());
  if      (units.CompareTo("mm", TString::kIgnoreCase) == 0) return .1;
  else if (units.CompareTo("cm", TString::kIgnoreCase) == 0) return 1.;
  else if (units.CompareTo("m",  TString::kIgnoreCase) == 0) return 100.;
  return 1;
}

//____________________________________________________________________
Bool_t
AliFMDSurveyToAlignObjs::GetPoint(const char* name, 
				  TVector3&   point, 
				  TVector3&   error) const
{
  // Get named point.   On return, point will contain the point
  // coordinates in centimeters, and error will contain the
  // meassurement errors in centimeters too.  If no point is found,
  // returns false, otherwise true. 
  if (!fSurveyPoints) return kFALSE;
  
  Double_t unit = GetUnitFactor();
  if (unit == 0) return kFALSE;
  
  TObject* obj  = fSurveyPoints->FindObject(name);
  if (!obj) return kFALSE;
  
  AliSurveyPoint* p = static_cast<AliSurveyPoint*>(obj);
  point.SetXYZ(unit * p->GetX(), 
	       unit * p->GetY(),
	       unit * p->GetZ());
  error.SetXYZ(unit * p->GetPrecisionX(),
	       unit * p->GetPrecisionY(),
	       unit * p->GetPrecisionZ());
  return kTRUE;
}

//____________________________________________________________________
Bool_t 
AliFMDSurveyToAlignObjs::CalculatePlane(const     TVector3& a, 
					const     TVector3& b,
					const     TVector3& c, 
					Double_t  depth,
					Double_t* trans,
					Double_t* rot) const
{
  // Vector a->b, b->c, and normal to plane defined by these two
  // vectors. 
  TVector3 ab(b-a), bc(c-b);
  
  // Normal vector to the plane of the fiducial marks obtained
  // as cross product of the two vectors on the plane d0^d1
  TVector3 nn(ab.Cross(bc));
  if (nn.Mag() < 1e-8) { 
    Info("CalculatePlane", "Normal vector is null vector");
    return kFALSE;
  }
  
  // We express the plane in Hessian normal form.
  //
  //   n x = -p,
  //
  // where n is the normalised normal vector given by 
  // 
  //   n_x = a / l,   n_y = b / l,   n_z = c / l,  p = d / l
  //
  // with l = sqrt(a^2+b^2+c^2) and a, b, c, and d are from the
  // normal plane equation 
  //
  //  ax + by + cz + d = 0
  // 
  // Normalize
  TVector3 n(nn.Unit());
  // Double_t p = - (n * a);
  
  // The center of the square with the fiducial marks as the
  // corners.  The mid-point of one diagonal - md.  Used to get the
  // center of the surveyd box. 
  TVector3 md(a + c);
  md *= 1/2.;

  // The center of the box. 
  TVector3 orig(md - depth * n);
  trans[0] = orig[0];
  trans[1] = orig[1];
  trans[2] = orig[2];
  
  // Normalize the spanning vectors 
  TVector3 uab(ab.Unit());
  TVector3 ubc(bc.Unit());
  
  for (size_t i = 0; i < 3; i++) { 
    rot[i * 3 + 0] = ubc[i];
    rot[i * 3 + 1] = uab[i];
    rot[i * 3 + 2] = n[i];
  }
  return kTRUE;
}

//____________________________________________________________________
Bool_t 
AliFMDSurveyToAlignObjs::FitPlane(const TObjArray& points, 
				  const TObjArray& errors,
				  Double_t         /* depth */,
				  Double_t*        trans,
				  Double_t*        rot) const
{
  Int_t nPoints = points.GetEntries();
  if (nPoints < 4) { 
    AliError(Form("Cannot fit a plane equation to less than 4 survey points, "
		  "got only %d", nPoints));
    return kFALSE;
  }
  
  TGraph2DErrors g;
  // Loop and fill graph 
  for (int i = 0; i < nPoints; i++) {
    TVector3* p = static_cast<TVector3*>(points.At(i));
    TVector3* e = static_cast<TVector3*>(errors.At(i));
  
    if (!p || !e) continue;
    
    g.SetPoint(i, p->X(), p->Y(), p->Z());
    g.SetPointError(i, e->X(), e->Y(), e->Z());
  }

  // Check that we have enough points
  if (g.GetN() < 4) { 
    AliError(Form("Only got %d survey points - no good for plane fit",
		  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("plane", "-[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;
}


//____________________________________________________________________
Bool_t
AliFMDSurveyToAlignObjs::MakeDelta(const char*  path, 
				   Double_t*    rot, 
				   Double_t*    trans, 
				   TGeoHMatrix& delta) const
{
  // Make delta transformation
  if (!gGeoManager)           return kFALSE;
  if (!gGeoManager->cd(path)) return kFALSE;
  
  
  TGeoMatrix*  global = gGeoManager->GetCurrentMatrix();
#if 0
  PrintRotation(Form("%s rot:", global->GetName()),global->GetRotationMatrix());
  PrintVector(Form("%s trans:", global->GetName()),global->GetTranslation());
#endif

  return MakeDelta(global, rot, trans, delta);
}

//____________________________________________________________________
Bool_t
AliFMDSurveyToAlignObjs::MakeDelta(TGeoMatrix*  global,
				   Double_t*    rot, 
				   Double_t*    trans, 
				   TGeoHMatrix& delta) const
{
  // Make delta transformation
  TGeoHMatrix* geoM = new TGeoHMatrix;
  geoM->SetTranslation(trans);
  geoM->SetRotation(rot);

  delta = global->Inverse();
  delta.MultiplyLeft(geoM);
  
  return true;
}

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

  // The possile survey points 
  TVector3  icb, ict, ocb, oct, dummy;
  Int_t     missing = 0;
  if (!GetPoint("V0L_ICB", icb, dummy)) missing++;
  if (!GetPoint("V0L_ICT", ict, dummy)) missing++;
  if (!GetPoint("V0L_OCB", ocb, dummy)) missing++;
  if (!GetPoint("V0L_OCT", oct, dummy)) missing++;

  // Check that we have enough points
  if (missing > 1) { 
    AliWarning(Form("Only got %d survey points - no good for FMD1 plane",
		    4-missing));
    return kFALSE;
  }

#if 0
  const char* lidN = "FMD1_lid_mat0";
  TGeoMatrix* lidM = static_cast<TGeoMatrix*>(gGeoManager->GetListOfMatrices()
					      ->FindObject(lidN));
  if (!lidM) { 
    AliError(Form("Couldn't find FMD1 lid transformation %s", lidN));
    return kFALSE;
  }

  const Double_t* lidT = lidM->GetTranslation();
  Double_t        lidZ = lidT[2];
  Double_t        off  = lidZ-3.3;
#else
  Double_t        off  = 0;
#endif

  if (!CalculatePlane(ocb, icb, ict, off, trans, rot)) return kFALSE;
  // PrintRotation("FMD1 rotation:",  rot);
  // PrintVector("FMD1 translation:", trans);

  return kTRUE;
}

//____________________________________________________________________
Bool_t
AliFMDSurveyToAlignObjs::DoFMD1()
{
  // Do the FMD1 stuff
  Double_t rot[9], trans[3];
  if (!GetFMD1Plane(rot, trans)) return kFALSE;
  // const char* path = "/ALIC_1/F1MT_1/FMD1_lid_0";
  
  // TGeoHMatrix delta;
  TGeoTranslation global(0,0,324.670);
  if (!MakeDelta(&global, rot, trans, fFMD1Delta)) 
    return kFALSE;
  
  // PrintRotation("FMD1 delta rotation:",  fFMD1Delta.GetRotationMatrix());
  // PrintVector("FMD1 delta translation:", fFMD1Delta.GetTranslation());

  return kTRUE;
}

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

  // The possible survey points 
  const char*    names[] = { "FMD2_ITOP",  "FMD2_OTOP", 
			     "FMD2_IBOTM", "FMD2_OBOTM", 
			     "FMD2_IBOT",  "FMD2_OBOT", 
			     0 };
  const char**   name    = names;

  TObjArray points;
  TObjArray errors;
  
  // Loop and fill graph 
  while (*name) {
    TVector3 p, e;
    if (!GetPoint(*name++, p, e)) continue;
    
    points.Add(new TVector3(p));
    errors.Add(new TVector3(e));
  }
  if (points.GetEntries() < 4) { 
    AliWarning(Form("Only got %d survey points - no good for FMD2 plane",
		    points.GetEntries()));
    return kFALSE;
  }

  return FitPlane(points, errors, 0, trans, rot);
}

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

  for (int i = 0; i < 3; i++) { 
    for (int j = 0; j < 3; j++) { 
      rot[i*3+j] = (i == j ? 1 : 0);
    }
  }
  trans[0] = trans[1] = 0;
  trans[2] += 0.015;
  // PrintRotation("FMD2 rotation:",  rot);
  // PrintVector("FMD2 translation:", trans);
  
  // TGeoHMatrix delta;
  if (!MakeDelta("/ALIC_1/F2MT_2/FMD2_support_0/FMD2_back_cover_2", 
		 rot, trans, fFMD2Delta)) return kFALSE;
  
  // PrintRotation("FMD2 delta rotation:",  fFMD2Delta.GetRotationMatrix());
  // PrintVector("FMD2 delta translation:", fFMD2Delta.GetTranslation());

  return kTRUE;
}

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

//____________________________________________________________________
Bool_t 
AliFMDSurveyToAlignObjs::CreateAlignObjs()
{
  TClonesArray& array = *fAlignObjArray;
  Int_t         n     = array.GetEntriesFast();

  if (!fFMD1Delta.IsIdentity()) { 
    new (array[n++]) AliAlignObjParams("FMD1/FMD1_T", 0, fFMD1Delta, kTRUE);
    new (array[n++]) AliAlignObjParams("FMD1/FMD1_B", 0, fFMD1Delta, kTRUE);
  }
  if (!fFMD2Delta.IsIdentity()) { 
    new (array[n++]) AliAlignObjParams("FMD2/FMD2_T", 0, fFMD2Delta, kTRUE);
    new (array[n++]) AliAlignObjParams("FMD2/FMD2_B", 0, fFMD2Delta, kTRUE);
  }
  // array.Print();
  
  return kTRUE;
}

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