本文整理汇总了C++中Rotation::GetRotAngle方法的典型用法代码示例。如果您正苦于以下问题:C++ Rotation::GetRotAngle方法的具体用法?C++ Rotation::GetRotAngle怎么用?C++ Rotation::GetRotAngle使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Rotation
的用法示例。
在下文中一共展示了Rotation::GetRotAngle方法的1个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: w
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);
}