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);
}
