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;
}
int testVectorUtil() {
std::cout << "\n************************************************************************\n "
<< " Utility Function Tests"
<< "\n************************************************************************\n";
std::cout << "Test Vector utility functions : ";
XYZVector v1(1.0, 2.0, 3.0);
Polar3DVector v2pol(v1.R(), v1.Theta()+TMath::PiOver2(), v1.Phi() + 1.0);
// mixedmethods not yet impl.
XYZVector v2; v2 = v2pol;
ok = 0;
ok += compare( VectorUtil::DeltaPhi(v1,v2), 1.0, "deltaPhi Vec");
RhoEtaPhiVector v2cyl(v1.Rho(), v1.Eta() + 1.0, v1.Phi() + 1.0);
v2 = v2cyl;
ok += compare( VectorUtil::DeltaR(v1,v2), sqrt(2.0), "DeltaR Vec");
XYZVector vperp = v1.Cross(v2);
ok += compare( VectorUtil::CosTheta(v1,vperp), 0.0, "costheta Vec");
ok += compare( VectorUtil::Angle(v1,vperp), TMath::PiOver2(), "angle Vec");
if (ok == 0) std::cout << "\t\t OK " << std::endl;
std::cout << "Test Point utility functions : ";
XYZPoint p1(1.0, 2.0, 3.0);
Polar3DPoint p2pol(p1.R(), p1.Theta()+TMath::PiOver2(), p1.Phi() + 1.0);
// mixedmethods not yet impl.
XYZPoint p2; p2 = p2pol;
ok = 0;
ok += compare( VectorUtil::DeltaPhi(p1,p2), 1.0, "deltaPhi Point");
RhoEtaPhiPoint p2cyl(p1.Rho(), p1.Eta() + 1.0, p1.Phi() + 1.0);
p2 = p2cyl;
ok += compare( VectorUtil::DeltaR(p1,p2), sqrt(2.0), "DeltaR Point");
XYZPoint pperp(vperp.X(), vperp.Y(), vperp.Z());
ok += compare( VectorUtil::CosTheta(p1,pperp), 0.0, "costheta Point");
ok += compare( VectorUtil::Angle(p1,pperp), TMath::PiOver2(), "angle Point");
if (ok == 0) std::cout << "\t\t OK " << std::endl;
std::cout << "LorentzVector utility funct.: ";
XYZTVector q1(1.0, 2.0, 3.0,4.0);
PtEtaPhiEVector q2cyl(q1.Pt(), q1.Eta()+1.0, q1.Phi() + 1.0, q1.E() );
// mixedmethods not yet impl.
XYZTVector q2; q2 = q2cyl;
ok = 0;
ok += compare( VectorUtil::DeltaPhi(q1,q2), 1.0, "deltaPhi LVec");
ok += compare( VectorUtil::DeltaR(q1,q2), sqrt(2.0), "DeltaR LVec");
XYZTVector qsum = q1+q2;
ok += compare( VectorUtil::InvariantMass(q1,q2), qsum.M(), "InvMass");
if (ok == 0) std::cout << "\t\t OK " << std::endl;
return ok;
}