void FramesTest::TestRotation2(const Vector& v,double a,double b,double c) { Vector v2; // Wrench Wrench w(Vector(7,-1,3),Vector(2,-3,3)); Twist t(Vector(6,3,5),Vector(4,-2,7)); // Rotation Rotation R; Rotation R2; double ra; double rb; double rc; R = Rotation::RPY(a,b,c); CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitX(),R.UnitX()),1.0,epsilon); CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitY(),R.UnitY()),1.0,epsilon); CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitZ(),R.UnitZ()),1.0,epsilon); CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitX(),R.UnitY()),0.0,epsilon); CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitX(),R.UnitZ()),0.0,epsilon); CPPUNIT_ASSERT_DOUBLES_EQUAL(dot(R.UnitY(),R.UnitZ()),0.0,epsilon); R2=R; CPPUNIT_ASSERT_EQUAL(R,R2); CPPUNIT_ASSERT_DOUBLES_EQUAL((R*v).Norm(),v.Norm(),epsilon); CPPUNIT_ASSERT_EQUAL(R.Inverse(R*v),v); CPPUNIT_ASSERT_EQUAL(R.Inverse(R*t),t); CPPUNIT_ASSERT_EQUAL(R.Inverse(R*w),w); CPPUNIT_ASSERT_EQUAL(R*R.Inverse(v),v); CPPUNIT_ASSERT_EQUAL(R*Rotation::Identity(),R); CPPUNIT_ASSERT_EQUAL(Rotation::Identity()*R,R); CPPUNIT_ASSERT_EQUAL(R*(R*(R*v)),(R*R*R)*v); CPPUNIT_ASSERT_EQUAL(R*(R*(R*t)),(R*R*R)*t); CPPUNIT_ASSERT_EQUAL(R*(R*(R*w)),(R*R*R)*w); CPPUNIT_ASSERT_EQUAL(R*R.Inverse(),Rotation::Identity()); CPPUNIT_ASSERT_EQUAL(R.Inverse()*R,Rotation::Identity()); CPPUNIT_ASSERT_EQUAL(R.Inverse()*v,R.Inverse(v)); R.GetRPY(ra,rb,rc); CPPUNIT_ASSERT_DOUBLES_EQUAL(ra,a,epsilon); CPPUNIT_ASSERT_DOUBLES_EQUAL(rb,b,epsilon); CPPUNIT_ASSERT_DOUBLES_EQUAL(rc,c,epsilon); R = Rotation::EulerZYX(a,b,c); R.GetEulerZYX(ra,rb,rc); CPPUNIT_ASSERT_DOUBLES_EQUAL(ra,a,epsilon); CPPUNIT_ASSERT_DOUBLES_EQUAL(rb,b,epsilon); CPPUNIT_ASSERT_DOUBLES_EQUAL(rc,c,epsilon); R = Rotation::EulerZYZ(a,b,c); R.GetEulerZYZ(ra,rb,rc); CPPUNIT_ASSERT_DOUBLES_EQUAL(ra,a,epsilon); CPPUNIT_ASSERT_DOUBLES_EQUAL(rb,b,epsilon); CPPUNIT_ASSERT_DOUBLES_EQUAL(rc,c,epsilon); double angle= R.GetRotAngle(v2); R2=Rotation::Rot2(v2,angle); CPPUNIT_ASSERT_EQUAL(R2,R); R2=Rotation::Rot(v2*1E20,angle); CPPUNIT_ASSERT_EQUAL(R,R2); v2=Vector(6,2,4); CPPUNIT_ASSERT_DOUBLES_EQUAL((v2).Norm(),::sqrt(dot(v2,v2)),epsilon); }