int main() {
quatTest();
cout << "-----------------\n";
cout << "X,Y,Z=" << XAxis << "," <<YAxis << "," <<ZAxis << endl;
printf("%d %d %d\n", (int)XAxis, (int)YAxis, (int)ZAxis);
f(XAxis); f(YAxis); f(ZAxis);
printf("index: %d %d %d\n", a[XAxis], a[YAxis], a[ZAxis]);
try {
UnitVec3 u;
cout << "should be NaN: u()=" << u << endl;
cout << "typename(u)=" << typeid(u).name() << " ~u=" << typeid(~u).name() << endl;
u = UnitVec3(1,2,3);
cout << "u=UnitVec(1,2,3): " << u << " transposed: " << ~u << endl;
cout << "u[2]=" << u[2] << " u(2)=" << u(2) << endl;
cout << "(~u)[2]=" << (~u)[2] << " (~u)(2)=" << (~u)(2) << endl;
cout << "u.perp()=" << u.perp() << " ~u*u.perp()=" << ~u*u.perp() << endl;
cout << "(~u).perp()=" << (~u).perp() << " (~u).perp()*u=" << (~u).perp()*u << endl;
Vec3 vv(u);
Vec3 w = u;
cout << "Vec3 vv(u)=" << vv << " Vec3 w=u, w=" << w << endl;
// these lines shouldn't compile
//u = Vec3(1,2,3);
//u += UnitVec3(1,2,3);
//u[2] = 5.;
//u(2) = 5.;
//u=0.;
// end of lines which shouldn't compile
Rotation R;
cout << "should be identity: R()=" << R;
cout << "typename(R)=" << typeid(R).name() << " ~R=" << typeid(~R).name() << endl;
Transform X;
cout << "should be identity: X()=" << X;
cout << "typename(X)=" << typeid(X).name() << " ~X=" << typeid(~X).name() << endl;
R = Rotation( u, ZAxis );
cout << "Rotation with u as z axis (norm=" << R.norm() << "): " << R;
cout << "~R: " << ~R;
cout << "typename(R[1])=" << typeid(R[1]).name() << " R(1)=" << typeid(R(1)).name() << endl;
X = Transform(R, Vec3(-1, 2, 20));
cout << "Transform X=" << X;
cout << " X.R=" << X.R() << " X.p=" << X.p() << " X.pInv=" << X.pInv() << endl;
cout << " X.x=" << X.x() << " y=" << X.y() << " z=" << X.z() << endl;
cout << "Transform ~X=" << ~X;
cout << " (~X).R=" << (~X).R() << " (~X).p=" << (~X).p() << " (~X).pInv=" << (~X).pInv() << endl;
cout << " (~X).x=" << (~X).x() << " y=" << (~X).y() << " z=" << (~X).z() << endl;
cout << "Transform X asMat34():" << X.asMat34();
cout << "Transform X toMat34():" << X.toMat34();
cout << "Transform X toMat44():" << X.toMat44();
cout << "Transform ~X toMat34():" << (~X).toMat34();
cout << "Transform ~X toMat44():" << (~X).toMat44();
cout << "should be identity: X*~X=" << X*~X;
cout << "should be identity: ~X*X=" << ~X*X;
Vec3 v(1,2,3);
cout << "v=" << v << endl;
cout << "X.R()*v=" << X.R()*v << endl;
cout << "X*v=" << X*v << endl;
cout << "X*[v,0]=" << X*v.append1(0) << endl;
cout << "X*[v,1]=" << X*v.append1(1) << endl;
cout << "~X*(X*v)=" << ~X*(X*v) << endl;
cout << "~X*(X*[v,0])=" << ~X*(X*v.append1(0)) << endl;
cout << "~X*(X*[v,1])=" << ~X*(X*v.append1(1)) << endl;
Rotation rx( 0.1, XAxis );
Rotation ry( 0.17, YAxis );
Rotation rz( 0.31, ZAxis );
Rotation rxoldy( SpaceRotationSequence, 0.1, XAxis, 0.17, YAxis );
Rotation rxyz = rz*ry*rx; // space fixed
// reverse order to use body fixed like space fixed
Rotation r123( BodyRotationSequence, 0.31, ZAxis, 0.17, YAxis, 0.1, XAxis );
cout << "Rotation r123:" << r123;
cout << "Rotation r123.asMat33():" << r123.asMat33();
cout << "Rotation r123.toMat33():" << r123.toMat33();
cout << "InvRotation ~r123:" << (~r123);
cout << "InvRotation ~r123.asMat33():" << (~r123).asMat33();
cout << "InvRotation ~r123.toMat33():" << (~r123).toMat33();
cout << "ry(.17)*rx(.1)=" << ry*rx;
cout << "rxoldy(.1,.17)=" << rxoldy;
cout << "norm(rxoldy-ry*rx)=" << (rxoldy-ry*rx).norm() << endl;
cout << "rxyz=" << rxyz;
cout << "r123=" << r123;
// Test inverse assignments, copy construct
Rotation invr123(~r123);
Rotation invr123eq;
invr123eq = ~r123;
cout << "Check inverse assignment/copy." << endl;
cout << "norm invr123*r123-identity=" << (invr123*r123-Mat33(1)).norm() << endl;
cout << "norm invr123eq*r123-identity=" << (invr123eq*r123-Mat33(1)).norm() << endl;
Rotation bodyXY( BodyRotationSequence, 0.03, XAxis, 0.11, YAxis, 0.0, ZAxis );
Rotation aboutYthenX( SpaceRotationSequence, 0.11, YAxis, 0.03, XAxis );
cout << "bodyXY(.03,.11)=" << bodyXY;
cout << "aboutYthenoldX(.11,.03)=" << aboutYthenX;
Rotation bodyZX( BodyRotationSequence, 0.07, ZAxis, 0.0, YAxis, -0.22, XAxis );
Rotation aboutZThenNewX( BodyRotationSequence, 0.07, ZAxis, -0.22, XAxis );
Rotation aboutZ( 0.07, ZAxis );
Rotation aboutXaboutZ = Rotation(-0.22, aboutZ*Vec3(1,0,0)) * aboutZ;
cout << "bodyZX(.07,-.22)=" << bodyZX;
cout << "aboutZThenNewX(.07,-.22)=" << aboutZThenNewX;
cout << "aboutXaboutZ(.07,-.22)=" << aboutXaboutZ;
cout << " diff norm=" << (aboutXaboutZ-aboutZThenNewX).norm() << endl;
// Check all space-fixed sequences vs composed single rotations.
cout << "SXY err=" << ( Rotation( SpaceRotationSequence, 0.13, XAxis, -0.29, YAxis ) - Rotation( -0.29, YAxis )*Rotation( 0.13, XAxis) ).norm() << endl;
cout << "SXZ err=" << ( Rotation( SpaceRotationSequence, 0.13, XAxis, -0.29, ZAxis ) - Rotation( -0.29, ZAxis )*Rotation( 0.13, XAxis) ).norm() << endl;
cout << "SZX err=" << ( Rotation( SpaceRotationSequence, 0.13, ZAxis, -0.29, XAxis ) - Rotation( -0.29, XAxis )*Rotation( 0.13, ZAxis) ).norm() << endl;
cout << "SYZ err=" << ( Rotation( SpaceRotationSequence, 0.13, YAxis, -0.29, ZAxis ) - Rotation( -0.29, ZAxis )*Rotation( 0.13, YAxis) ).norm() << endl;
cout << "SZY err=" << ( Rotation( SpaceRotationSequence, 0.13, ZAxis, -0.29, YAxis ) - Rotation( -0.29, YAxis )*Rotation( 0.13, ZAxis) ).norm() << endl;
// Check all body-fixed sequences vs. transformed & composed single rotations.
const Real a1=-.23, a2=1.09;
const Rotation ax( a1, XAxis ); const Vec3 x(1,0,0);
const Rotation ay( a1, YAxis ); const Vec3 y(0,1,0);
const Rotation az( a1, ZAxis ); const Vec3 z(0,0,1);
cout << "BXY err=" << ( Rotation( BodyRotationSequence, a1, XAxis, a2, YAxis ) - Rotation(a2,ax*y)*ax ).norm() << endl;
cout << "BYX err=" << ( Rotation( BodyRotationSequence, a1, YAxis, a2, XAxis ) - Rotation(a2,ay*x)*ay ).norm() << endl;
cout << "BXZ err=" << ( Rotation( BodyRotationSequence, a1, XAxis, a2, ZAxis ) - Rotation(a2,ax*z)*ax ).norm() << endl;
cout << "BZX err=" << ( Rotation( BodyRotationSequence, a1, ZAxis, a2, XAxis ) - Rotation(a2,az*x)*az ).norm() << endl;
cout << "BYZ err=" << ( Rotation( BodyRotationSequence, a1, YAxis, a2, ZAxis ) - Rotation(a2,ay*z)*ay ).norm() << endl;
cout << "BZY err=" << ( Rotation( BodyRotationSequence, a1, ZAxis, a2, YAxis ) - Rotation(a2,az*y)*az ).norm() << endl;
cout << "rotate1(0,.03)=" << ( rotate1(0,.03) - Rotation(.03,XAxis) ).norm() << endl;
cout << "rotate1(1,.03)=" << ( rotate1(1,.03) - Rotation(.03,YAxis) ).norm() << endl;
cout << "rotate1(2,.03)=" << ( rotate1(2,.03) - Rotation(.03,ZAxis) ).norm() << endl;
cout << "-1 % 3=" << -1 % 3 << endl;
const Transform X_01( Rotation( .03, Vec3(1,.1,.7) ), Vec3(1,2,3) );
const Real masses[] = {1,2,3,4,5};
const Real mtot = Vector(5,masses).sum();
const Vec3 stations000[] = {Vec3(.1,.2,.3), Vec3(-2,-9,1), Vec3(.01,.02,.05),
Vec3(1,-1,1), Vec3(0,0,0)};
const Vector_<Vec3> s000(5, stations000);
const Vec3 com000 = (~Vector(5,masses) * s000)/mtot;
Vector_<Vec3> s123(s000);
for (int i=0;i<5;++i) s123[i] = ~X_01*s123[i];
const Vec3 com123 = (~Vector(5,masses) * s123)/mtot;
Inertia I000(0), I123(0);
for (int i=0; i<5; ++i) I000 += Inertia(s000[i],masses[i]);
for (int i=0; i<5; ++i) I123 += Inertia(s123[i],masses[i]);
cout << "mtot=" << mtot << endl;
cout << "com000=" << com000 << endl;
cout << "com123=" << com123 << endl;
cout << "I000=" << I000;
cout << "I123=" << I123;
MassProperties mp(mtot, com000, I000);
cout << mp;
Real scale = std::sqrt(I123.toMat33().diag().normSqr()/3); cout << "inertia scale=" << scale << endl;
cout << "norm(I000->123-I123)/rms(I123)="
<< (mp.calcTransformedInertia(X_01).toMat33()-I123.toMat33()).norm()/scale
<< endl;
Rotation R_AB( BodyRotationSequence, 0.31, ZAxis, 0.17, YAxis, 0.1, XAxis );
Rotation R_BC( BodyRotationSequence, -123.3, ZAxis, 41.1, YAxis, 14, XAxis );
Rotation Rtmp;
cout << "R_AB*R_BC=" << R_AB*R_BC;
Rtmp = R_AB;
cout << "R_AB*=R_BC=" << (Rtmp*=R_BC);
cout << "R_AB*~R_BC=" << R_AB*~R_BC;
cout << "R_AB/R_BC=" << R_AB/R_BC;
Rtmp=R_AB;
cout << "R_AB/=R_BC=" << (Rtmp/=R_BC);
cout << "COMPARE ROTATIONS" << endl;
Rotation R_GB, R_GX; Real backOut;
R_GB.setRotationFromAngleAboutNonUnitVector( 0.17, Vec3(1,2,3) );
backOut = R_GB.convertRotationToAngleAxis()[0];
cout << " in=0.17 radians, out=" << backOut << " err=" << std::abs(backOut-0.17) << endl;
R_GB.setRotationFromAngleAboutNonUnitVector( 0.17+1e-13, Vec3(1,2,3) );
R_GX.setRotationFromAngleAboutNonUnitVector( 0.17, Vec3(1,2,3) );
cout << " 0.17+1e-13:0.17 isSameToPrecision? " << R_GB.isSameRotationToWithinAngleOfMachinePrecision(R_GX)
<< " isSameToAngle(1e-12)? " << R_GB.isSameRotationToWithinAngle(R_GX, 1e-12) << endl;
R_GB.setRotationFromAngleAboutNonUnitVector( 0.17+1e-15, Vec3(1,2,3) );
R_GX.setRotationFromAngleAboutNonUnitVector( 0.17, Vec3(1,2,3) );
cout << " 0.17+1e-15:0.17 isSameToPrecision? " << R_GB.isSameRotationToWithinAngleOfMachinePrecision(R_GX)
<< " isSameToAngle(1e-18)? " << R_GB.isSameRotationToWithinAngle(R_GX, 1e-18) << endl;
const Real pi2 = Pi/2;
const Real pi2x = -pi2 + 1e-8;
cout << "pi2x=pi2-" << pi2-pi2x << " sin(pi2x)-1=" << std::sin(pi2x)-1 << endl;
const Vec3 vin(-3, pi2x, 0.1);
Rotation b123( BodyRotationSequence, vin[0], XAxis, vin[1], YAxis, vin[2], ZAxis );
Mat33 m123=b123; m123[0][0] += 1e-14; m123[1][2] += 1e-14;
//b123 = Rotation::trustMe(m123);
//cout << "bad b123*~b123 angle=" << (b123*~b123).convertToAngleAxis()[0] << endl;
//b123 = Rotation(m123);
//cout << "good b123*~b123.norm()=" << (b123*~b123).convertToAngleAxis()[0] << endl;
b123 *= R_GX;
b123 *= ~R_GX;
cout << "b123=" << b123;
Rotation cleanb123(b123.asMat33());
// b123=cleanb123;
cout << "cos(pi2x)=" << cos(pi2x) << "cos(pi2)=" << cos(pi2) << endl;
cout << "atan2(02/00)-pi2=" << atan2(b123[0][2],b123[0][0])-pi2 << endl;
Vec3 v123 = b123.convertThreeAxesRotationToThreeAngles( BodyRotationSequence, XAxis, YAxis, ZAxis );
Vec4 aax = b123.convertRotationToAngleAxis();
cout << "vin=" << vin << "\nvout=" << v123 << endl;
Rotation b123x( BodyRotationSequence, v123[0], XAxis, v123[1], YAxis, v123[2], ZAxis );
Vec4 aax2 = (~b123*b123x).convertRotationToAngleAxis();
cout << " aax2=" << aax2 << endl;
Rotation chrisRot( UnitVec3( Vec3(1,1,0) ), XAxis, Vec3(0,1,0), YAxis );
cout << "chrisRot+y=" << chrisRot;
chrisRot = Rotation( UnitVec3( Vec3(1,1,0) ), XAxis, Vec3(0,-1,0), YAxis );
cout << "chrisRot-y=" << chrisRot;
chrisRot = Rotation( UnitVec3( Vec3(1,1,0) ), XAxis, Vec3(0,0,1), YAxis );
cout << "chrisRot+z=" << chrisRot;
return 0;
}
catch(const Exception::Base& e) {
std::cout << e.getMessage() << std::endl;
}
}
