コード例 #1
0
ファイル: mathcoreGenVector.C プロジェクト: My-Source/root
int testRotation() { 

 
  std::cout << "\n************************************************************************\n " 
	    << " Rotation and Transformation Tests" 
	    << "\n************************************************************************\n";

  std::cout << "Test Vector Rotations :         ";
  ok = 0; 

  XYZPoint v(1.,2,3.); 

  double pi = TMath::Pi();
  // initiate rotation with some non -trivial angles to test all matrix
  EulerAngles r1( pi/2.,pi/4., pi/3 );
  Rotation3D  r2(r1);
  // only operator= is in CINT for the other rotations
  Quaternion  r3; r3 = r2;
  AxisAngle   r4; r4 = r3;
  RotationZYX r5; r5 = r2;

  XYZPoint v1 = r1 * v;
  XYZPoint v2 = r2 * v;
  XYZPoint v3 = r3 * v;
  XYZPoint v4 = r4 * v;
  XYZPoint v5 = r5 * v;
  
  ok+= compare(v1.X(), v2.X(), "x",2); 
  ok+= compare(v1.Y(), v2.Y(), "y",2); 
  ok+= compare(v1.Z(), v2.Z(), "z",2); 

  ok+= compare(v1.X(), v3.X(), "x",2); 
  ok+= compare(v1.Y(), v3.Y(), "y",2); 
  ok+= compare(v1.Z(), v3.Z(), "z",2); 

  ok+= compare(v1.X(), v4.X(), "x",5); 
  ok+= compare(v1.Y(), v4.Y(), "y",5); 
  ok+= compare(v1.Z(), v4.Z(), "z",5); 

  ok+= compare(v1.X(), v5.X(), "x",2); 
  ok+= compare(v1.Y(), v5.Y(), "y",2); 
  ok+= compare(v1.Z(), v5.Z(), "z",2); 

  // test with matrix
  double rdata[9]; 
  r2.GetComponents(rdata, rdata+9);
  TMatrixD m(3,3,rdata);
  double vdata[3];
  v.GetCoordinates(vdata);
  TVectorD q(3,vdata);
  TVectorD q2 = m*q; 
  
  XYZPoint v6; 
  v6.SetCoordinates( q2.GetMatrixArray() );

  ok+= compare(v1.X(), v6.X(), "x"); 
  ok+= compare(v1.Y(), v6.Y(), "y"); 
  ok+= compare(v1.Z(), v6.Z(), "z"); 


  if (ok == 0) std::cout << "\t OK " << std::endl;
  else  std::cout << std::endl;

  std::cout << "Test Axial Rotations :          ";
  ok = 0; 

  RotationX rx( pi/3);
  RotationY ry( pi/4);
  RotationZ rz( 4*pi/5);

  Rotation3D r3x(rx);
  Rotation3D r3y(ry);
  Rotation3D r3z(rz);

  Quaternion qx; qx = rx;
  Quaternion qy; qy = ry;
  Quaternion qz; qz = rz;

  RotationZYX rzyx( rz.Angle(), ry.Angle(), rx.Angle() );

  XYZPoint vrot1 = rx * ry * rz * v;
  XYZPoint vrot2 = r3x * r3y * r3z * v;

  ok+= compare(vrot1.X(), vrot2.X(), "x"); 
  ok+= compare(vrot1.Y(), vrot2.Y(), "y"); 
  ok+= compare(vrot1.Z(), vrot2.Z(), "z"); 

  vrot2 = qx * qy * qz * v;

  ok+= compare(vrot1.X(), vrot2.X(), "x",2); 
  ok+= compare(vrot1.Y(), vrot2.Y(), "y",2); 
  ok+= compare(vrot1.Z(), vrot2.Z(), "z",2); 

  vrot2 = rzyx * v;

  ok+= compare(vrot1.X(), vrot2.X(), "x"); 
  ok+= compare(vrot1.Y(), vrot2.Y(), "y"); 
  ok+= compare(vrot1.Z(), vrot2.Z(), "z"); 

  // now inverse (first x then y then z)
  vrot1 = rz * ry * rx * v;
  vrot2 = r3z * r3y * r3x * v;

  ok+= compare(vrot1.X(), vrot2.X(), "x"); 
  ok+= compare(vrot1.Y(), vrot2.Y(), "y"); 
  ok+= compare(vrot1.Z(), vrot2.Z(), "z"); 

  
  XYZPoint vinv1 = rx.Inverse()*ry.Inverse()*rz.Inverse()*vrot1;

  ok+= compare(vinv1.X(), v.X(), "x",2); 
  ok+= compare(vinv1.Y(), v.Y(), "y"); 
  ok+= compare(vinv1.Z(), v.Z(), "z"); 

  if (ok == 0) std::cout << "\t OK " << std::endl;
  else  std::cout << std::endl;


  std::cout << "Test Rotations by a PI angle :  ";
  ok = 0;

  double b[4] = { 6,8,10,3.14159265358979323 };
  AxisAngle  arPi(b,b+4 );
  Rotation3D rPi(arPi);
  AxisAngle  a1; a1 = rPi;
  ok+= compare(arPi.Axis().X(), a1.Axis().X(),"x"); 
  ok+= compare(arPi.Axis().Y(), a1.Axis().Y(),"y"); 
  ok+= compare(arPi.Axis().Z(), a1.Axis().Z(),"z");   
  ok+= compare(arPi.Angle(), a1.Angle(),"angle");   

  EulerAngles ePi; ePi=rPi;
  EulerAngles e1; e1=Rotation3D(a1);
  ok+= compare(ePi.Phi(), e1.Phi(),"phi");   
  ok+= compare(ePi.Theta(), e1.Theta(),"theta");   
  ok+= compare(ePi.Psi(), e1.Psi(),"ps1");   

  if (ok == 0) std::cout << "\t\t OK " << std::endl;
  else  std::cout << std::endl;

  std::cout << "Test Inversions :               "; 
  ok = 0; 


  EulerAngles s1 = r1.Inverse();
  Rotation3D  s2 = r2.Inverse();
  Quaternion  s3 = r3.Inverse();
  AxisAngle   s4 = r4.Inverse();
  RotationZYX s5 = r5.Inverse();

  
  // euler angles not yet impl.
  XYZPoint p = s2 * r2 * v; 
  
  ok+= compare(p.X(), v.X(), "x",10); 
  ok+= compare(p.Y(), v.Y(), "y",10); 
  ok+= compare(p.Z(), v.Z(), "z",10); 


  p = s3 * r3 * v; 
  
  ok+= compare(p.X(), v.X(), "x",10); 
  ok+= compare(p.Y(), v.Y(), "y",10); 
  ok+= compare(p.Z(), v.Z(), "z",10); 

  p = s4 * r4 * v; 
  // axis angle inversion not very precise
  ok+= compare(p.X(), v.X(), "x",1E9); 
  ok+= compare(p.Y(), v.Y(), "y",1E9); 
  ok+= compare(p.Z(), v.Z(), "z",1E9); 

  p = s5 * r5 * v; 
  
  ok+= compare(p.X(), v.X(), "x",10); 
  ok+= compare(p.Y(), v.Y(), "y",10); 
  ok+= compare(p.Z(), v.Z(), "z",10); 


  Rotation3D r6(r5);
  Rotation3D s6 = r6.Inverse();

  p = s6 * r6 * v; 
  
  ok+= compare(p.X(), v.X(), "x",10); 
  ok+= compare(p.Y(), v.Y(), "y",10); 
  ok+= compare(p.Z(), v.Z(), "z",10); 
  
  if (ok == 0) std::cout << "\t OK " << std::endl;
  else  std::cout << std::endl;

  // test Rectify 

  std::cout << "Test rectify :                  "; 
  ok = 0; 

  XYZVector u1(0.999498,-0.00118212,-0.0316611); 
  XYZVector u2(0,0.999304,-0.0373108); 
  XYZVector u3(0.0316832,0.0372921,0.998802); 
  Rotation3D rr(u1,u2,u3); 
  // check orto-normality
  XYZPoint vrr = rr* v; 
  ok+= compare(v.R(), vrr.R(), "R",1.E9); 

  if (ok == 0) std::cout << "\t\t OK " << std::endl;
  else  std::cout << std::endl;
  
  std::cout << "Test Transform3D :              "; 
  ok = 0; 

  XYZVector d(1.,-2.,3.);
  Transform3D t(r2,d);
  
  XYZPoint pd = t * v;
  // apply directly rotation
  XYZPoint vd = r2 * v + d; 

  ok+= compare(pd.X(), vd.X(), "x"); 
  ok+= compare(pd.Y(), vd.Y(), "y"); 
  ok+= compare(pd.Z(), vd.Z(), "z"); 

  // test with matrix 
  double tdata[12]; 
  t.GetComponents(tdata);
  TMatrixD mt(3,4,tdata);
  double vData[4]; // needs a vector of dim 4 
  v.GetCoordinates(vData);
  vData[3] = 1;
  TVectorD q0(4,vData);
  
  TVectorD qt = mt*q0; 

  ok+= compare(pd.X(), qt(0), "x"); 
  ok+= compare(pd.Y(), qt(1), "y"); 
  ok+= compare(pd.Z(), qt(2), "z"); 


  // test inverse 

  Transform3D tinv = t.Inverse();
  
  p = tinv * t * v; 

  ok+= compare(p.X(), v.X(), "x",10); 
  ok+= compare(p.Y(), v.Y(), "y",10); 
  ok+= compare(p.Z(), v.Z(), "z",10); 

  // test costruct inverse from translation first

  //Transform3D tinv2( -d, r2.Inverse() );
  //Transform3D tinv2 =  r2.Inverse() * Translation3D(-d) ;
  Transform3D tinv2 ( r2.Inverse(), r2.Inverse() *( -d) ) ;
  p = tinv2 * t * v; 

  ok+= compare(p.X(), v.X(), "x",10); 
  ok+= compare(p.Y(), v.Y(), "y",10); 
  ok+= compare(p.Z(), v.Z(), "z",10); 

  // test from only rotation and only translation 
  Transform3D ta( EulerAngles(1.,2.,3.) );
  Transform3D tb( XYZVector(1,2,3) );
  Transform3D tc(  Rotation3D(EulerAngles(1.,2.,3.)) ,  XYZVector(1,2,3) );
  Transform3D td(  ta.Rotation(), ta.Rotation()  * XYZVector(1,2,3) ) ;
  
  ok+= compare( tc == tb*ta, static_cast<double>(true), "== Rot*Tra");
  ok+= compare( td == ta*tb, static_cast<double>(true), "== Rot*Tra");


  if (ok == 0) std::cout << "\t OK " << std::endl;
  else  std::cout << std::endl;

  std::cout << "Test Plane3D :                  "; 
  ok = 0; 

  // test transfrom a 3D plane

  
  XYZPoint p1(1,2,3);
  XYZPoint p2(-2,-1,4);
  XYZPoint p3(-1,3,2);
  Plane3D plane(p1,p2,p3);

  XYZVector n = plane.Normal();
  // normal is perpendicular to vectors on the planes obtained from subracting the points
  ok+= compare(n.Dot(p2-p1), 0.0, "n.v12",10); 
  ok+= compare(n.Dot(p3-p1), 0.0, "n.v13",10); 
  ok+= compare(n.Dot(p3-p2), 0.0, "n.v23",10); 

  Plane3D plane1 = t(plane);
  
  // transform the points
  XYZPoint pt1 = t(p1);
  XYZPoint pt2 = t(p2);
  XYZPoint pt3 = t(p3);
  Plane3D plane2(pt1,pt2,pt3);

  XYZVector n1 = plane1.Normal();
  XYZVector n2 = plane2.Normal();


  ok+= compare(n1.X(), n2.X(), "a",10); 
  ok+= compare(n1.Y(), n2.Y(), "b",10); 
  ok+= compare(n1.Z(), n2.Z(), "c",10); 
  ok+= compare(plane1.HesseDistance(), plane2.HesseDistance(), "d",10); 

  // check distances  
  ok += compare(plane1.Distance(pt1), 0.0, "distance",10);

  if (ok == 0) std::cout << "\t OK " << std::endl;
  else  std::cout << std::endl;

  std::cout << "Test LorentzRotation :          "; 
  ok = 0; 

  XYZTVector lv(1.,2.,3.,4.);
  
  // test from rotx (using boosts and 3D rotations not yet impl.)
  // rx,ry and rz already defined
  Rotation3D r3d = rx*ry*rz; 

  LorentzRotation rlx(rx);
  LorentzRotation rly(ry);
  LorentzRotation rlz(rz);

  LorentzRotation rl0 = rlx*rly*rlz;
  LorentzRotation rl1( r3d);
  
//   cout << rl << endl;
//   cout << rl0 << endl;
//   int eq = rl0 == rl;
//   cout << eq << endl;
//   double d1[16];
//   double d2[16];
//   rl.GetComponents(d1,d1+16);
//   rl0.GetComponents(d2,d2+16);
//   for (int i = 0; i < 16; ++i) 
//     ok+= compare(d1[i], d2[i], "i",1); 

  //ok+= compare( rl == rl2, static_cast<double>(true), " LorenzRot");


  //  cout << Rotation3D(rx) << endl;

  XYZTVector lv0 = rl0 * lv; 

  XYZTVector lv1 = rl1 * lv; 

  XYZTVector lv2 = r3d * lv; 


  ok+= compare(lv1== lv2,true,"V0==V2"); 
  ok+= compare(lv1== lv2,true,"V1==V2"); 

  double rlData[16];
  rl0.GetComponents(rlData);
  TMatrixD ml(4,4,rlData); 
  //  ml.Print();
  double lvData[4];
  lv.GetCoordinates(lvData);
  TVectorD ql(4,lvData); 

  TVectorD qlr = ml*ql; 

  ok+= compare(lv1.X(), qlr(0), "x"); 
  ok+= compare(lv1.Y(), qlr(1), "y"); 
  ok+= compare(lv1.Z(), qlr(2), "z"); 
  ok+= compare(lv1.E(), qlr(3), "t"); 

  // test inverse 

  lv0 = rl0 * rl0.Inverse() * lv; 

  ok+= compare(lv0.X(), lv.X(), "x"); 
  ok+= compare(lv0.Y(), lv.Y(), "y"); 
  ok+= compare(lv0.Z(), lv.Z(), "z"); 
  ok+= compare(lv0.E(), lv.E(), "t"); 

  if (ok == 0) std::cout << "\t OK " << std::endl;
  else  std::cout << std::endl;

  // test Boosts

  std::cout << "Test Boost :                    "; 
  ok = 0; 


  Boost bst( 0.3,0.4,0.5);   //  boost (must be <= 1)


  XYZTVector lvb = bst ( lv );

  LorentzRotation rl2 (bst);

  XYZTVector lvb2 = rl2 (lv);


  // test with lorentz rotation
  ok+= compare(lvb.X(), lvb2.X(), "x"); 
  ok+= compare(lvb.Y(), lvb2.Y(), "y"); 
  ok+= compare(lvb.Z(), lvb2.Z(), "z"); 
  ok+= compare(lvb.E(), lvb2.E(), "t"); 
  ok+= compare(lvb.M(), lv.M(), "m",50); // m must stay constant 


  // test inverse
  lv0 = bst.Inverse() * lvb;

  ok+= compare(lv0.X(), lv.X(), "x",5); 
  ok+= compare(lv0.Y(), lv.Y(), "y",5); 
  ok+= compare(lv0.Z(), lv.Z(), "z",3); 
  ok+= compare(lv0.E(), lv.E(), "t",3); 

  XYZVector brest = lv.BoostToCM();
  bst.SetComponents( brest.X(), brest.Y(), brest.Z() );

  XYZTVector lvr = bst * lv; 

  ok+= compare(lvr.X(), 0.0, "x",10); 
  ok+= compare(lvr.Y(), 0.0, "y",10); 
  ok+= compare(lvr.Z(), 0.0, "z",10); 
  ok+= compare(lvr.M(), lv.M(), "m",10); 


  if (ok == 0) std::cout << "\t OK " << std::endl;
  else  std::cout << std::endl;

  return ok;
}