10 #include <gtest/gtest.h>
20 TEST(SLERP_tests, correctShortestPath)
24 const TPose3D pose_a(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg);
25 const TPose3D pose_b(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg);
29 const TPose3D expected(0, 0, 0, 0, 0, 0);
33 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
34 <<
"\ninterp: " << pose_interp << endl;
39 const TPose3D expected(0, 0, 0, 0, 0, 0);
43 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
44 <<
"\ninterp: " << pose_interp << endl;
49 const TPose3D expected(0, 0, 0, 0, 0, 0);
53 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
54 <<
"\ninterp: " << pose_interp << endl;
59 const TPose3D pose_a(0, 0, 0, 179.0_deg, 0.0_deg, 0.0_deg);
60 const TPose3D pose_b(0, 0, 0, -179.0_deg, 0.0_deg, 0.0_deg);
63 const TPose3D expected(0, 0, 0, -180.0_deg, 0, 0);
67 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
68 <<
"\ninterp: " << pose_interp << endl;
71 const TPose3D pose_a(0, 0, 0, -179.0_deg, 0.0_deg, 0.0_deg);
72 const TPose3D pose_b(0, 0, 0, 179.0_deg, 0.0_deg, 0.0_deg);
75 const TPose3D expected(0, 0, 0, -180.0_deg, 0, 0);
79 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
80 <<
"\ninterp: " << pose_interp << endl;
83 const TPose3D pose_a(0, 0, 0, -179.0_deg, 0.0_deg, 0.0_deg);
84 const TPose3D pose_b(0, 0, 0, 179.0_deg, 0.0_deg, 0.0_deg);
87 const TPose3D expected(0, 0, 0, -180.0_deg, 0, 0);
93 <<
"\ninterp: " << pose_interp.
asString() << endl;
97 const TPose3D pose_a(0, 0, 0, 40.0_deg, 0.0_deg, 0.0_deg);
98 const TPose3D pose_b(0, 0, 0, -40.0_deg, 0.0_deg, 0.0_deg);
101 const TPose3D expected(0, 0, 0, 0.0_deg, 0, 0);
105 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
106 <<
"\ninterp: " << pose_interp << endl;
109 const TPose3D pose_a(0, 0, 0, -40.0_deg, 0.0_deg, 0.0_deg);
110 const TPose3D pose_b(0, 0, 0, 40.0_deg, 0.0_deg, 0.0_deg);
113 const TPose3D expected(0, 0, 0, 0.0_deg, 0, 0);
117 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
118 <<
"\ninterp: " << pose_interp << endl;
122 const TPose3D pose_a(0, 0, 0, 0.0_deg, 40.0_deg, 0.0_deg);
123 const TPose3D pose_b(0, 0, 0, 0.0_deg, -40.0_deg, 0.0_deg);
126 const TPose3D expected(0, 0, 0, 0.0_deg, 0, 0);
130 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
131 <<
"\ninterp: " << pose_interp << endl;
134 const TPose3D pose_a(0, 0, 0, 0.0_deg, -40.0_deg, 0.0_deg);
135 const TPose3D pose_b(0, 0, 0, 0.0_deg, 40.0_deg, 0.0_deg);
138 const TPose3D expected(0, 0, 0, 0.0_deg, 0, 0);
142 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
143 <<
"\ninterp: " << pose_interp << endl;
147 const TPose3D pose_a(0, 0, 0, 0.0_deg, 0.0_deg, -40.0_deg);
148 const TPose3D pose_b(0, 0, 0, 0.0_deg, 0.0_deg, 40.0_deg);
151 const TPose3D expected(0, 0, 0, 0.0_deg, 0, 0);
155 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
156 <<
"\ninterp: " << pose_interp << endl;
159 const TPose3D pose_a(0, 0, 0, 0.0_deg, 0.0_deg, 40.0_deg);
160 const TPose3D pose_b(0, 0, 0, 0.0_deg, 0.0_deg, -40.0_deg);
163 const TPose3D expected(0, 0, 0, 0.0_deg, 0, 0);
167 <<
"pose_a: " << pose_a <<
"\npose_b: " << pose_b
168 <<
"\ninterp: " << pose_interp << endl;