13 #include <gtest/gtest.h>
20 TEST(SLERP_tests, correctShortestPath)
29 const TPose3D expected(0, 0, 0, 0, 0, 0);
32 EXPECT_NEAR(0, (HM - HMe).array().abs().
sum(), 1e-4)
33 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
34 <<
"\ninterp: " << pose_interp << endl;
39 const TPose3D expected(0, 0, 0, 0, 0, 0);
42 EXPECT_NEAR(0, (HM - HMe).array().abs().
sum(), 1e-4)
43 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
44 <<
"\ninterp: " << pose_interp << endl;
49 const TPose3D expected(0, 0, 0, 0, 0, 0);
52 EXPECT_NEAR(0, (HM - HMe).array().abs().
sum(), 1e-4)
53 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
54 <<
"\ninterp: " << pose_interp << endl;
66 EXPECT_NEAR(0, (HM - HMe).array().abs().
sum(), 1e-4)
67 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
68 <<
"\ninterp: " << pose_interp << endl;
78 EXPECT_NEAR(0, (HM - HMe).array().abs().
sum(), 1e-4)
79 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
80 <<
"\ninterp: " << pose_interp << endl;
90 EXPECT_NEAR(0, (HM - HMe).array().abs().
sum(), 1e-4)
93 <<
"\ninterp: " << pose_interp.
asString() << endl;
104 EXPECT_NEAR(0, (HM - HMe).array().abs().
sum(), 1e-4)
105 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
106 <<
"\ninterp: " << pose_interp << endl;
116 EXPECT_NEAR(0, (HM - HMe).array().abs().
sum(), 1e-4)
117 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
118 <<
"\ninterp: " << pose_interp << endl;
129 EXPECT_NEAR(0, (HM - HMe).array().abs().
sum(), 1e-4)
130 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
131 <<
"\ninterp: " << pose_interp << endl;
141 EXPECT_NEAR(0, (HM - HMe).array().abs().
sum(), 1e-4)
142 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
143 <<
"\ninterp: " << pose_interp << endl;
154 EXPECT_NEAR(0, (HM - HMe).array().abs().
sum(), 1e-4)
155 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
156 <<
"\ninterp: " << pose_interp << endl;
166 EXPECT_NEAR(0, (HM - HMe).array().abs().
sum(), 1e-4)
167 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
168 <<
"\ninterp: " << pose_interp << endl;