37 #define _USE_MATH_DEFINES
40 #define BOOST_TEST_MODULE FCL_BOX_BOX
41 #include <boost/test/included/unit_test.hpp>
43 #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps))
78 std::cerr <<
"Applied transformations on two boxes" << std::endl;
79 std::cerr <<
" T1 = " <<
tf1.getTranslation() << std::endl
80 <<
" R1 = " <<
tf1.getRotation() << std::endl
81 <<
" T2 = " <<
tf2.getTranslation() << std::endl
82 <<
" R2 = " <<
tf2.getRotation() << std::endl;
83 std::cerr <<
"Closest points: p1 = " << distanceResult.
nearest_points[0]
85 <<
", distance = " << distanceResult.
min_distance << std::endl;
86 double dx = 25 - 3 - 1;
87 double dy = 20 - 5 - 1;
88 double dz = 5 - 1 - 1;
93 sqrt(dx * dx + dy * dy + dz * dz), 1e-4);
95 BOOST_CHECK_CLOSE(
p1[0], 3, 1e-6);
96 BOOST_CHECK_CLOSE(
p1[1], 5, 1e-6);
97 BOOST_CHECK_CLOSE(
p1[2], 1, 1e-6);
98 BOOST_CHECK_CLOSE(p2[0], 24, 1e-6);
99 BOOST_CHECK_CLOSE(p2[1], 19, 1e-6);
100 BOOST_CHECK_CLOSE(p2[2], 4, 1e-6);
106 static double pi = M_PI;
110 sin(pi / 8) / sqrt(3), sin(pi / 8) / sqrt(3)),
122 std::cerr <<
"Applied transformations on two boxes" << std::endl;
123 std::cerr <<
" T1 = " <<
tf1.getTranslation() << std::endl
124 <<
" R1 = " <<
tf1.getRotation() << std::endl
125 <<
" T2 = " <<
tf2.getTranslation() << std::endl
126 <<
" R2 = " <<
tf2.getRotation() << std::endl;
127 std::cerr <<
"Closest points: p1 = " << distanceResult.
nearest_points[0]
129 <<
", distance = " << distanceResult.
min_distance << std::endl;
133 double distance = -1.62123444 + 10 - 1;
136 BOOST_CHECK_CLOSE(
p1[0], 0.60947571, 1e-4);
137 BOOST_CHECK_CLOSE(
p1[1], 0.01175873, 1e-4);
138 BOOST_CHECK_CLOSE(
p1[2], 1, 1e-6);
139 BOOST_CHECK_CLOSE(p2[0], 0.60947571, 1e-4);
140 BOOST_CHECK_CLOSE(p2[1], 0.01175873, 1e-4);
141 BOOST_CHECK_CLOSE(p2[2], -1.62123444 + 10, 1e-4);
147 static double pi = M_PI;
162 std::cerr <<
"Applied transformations on two boxes" << std::endl;
163 std::cerr <<
" T1 = " <<
tf1.getTranslation() << std::endl
164 <<
" R1 = " <<
tf1.getRotation() << std::endl
165 <<
" T2 = " <<
tf2.getTranslation() << std::endl
166 <<
" R2 = " <<
tf2.getRotation() << std::endl;
167 std::cerr <<
"Closest points: p1 = " << distanceResult.
nearest_points[0]
169 <<
", distance = " << distanceResult.
min_distance << std::endl;
176 const Vec3f p1Ref(sqrt(2) / 2 - 2, 1, .5);
177 const Vec3f p2Ref(2 - sqrt(2) / 2, 1, .5);
178 BOOST_CHECK_CLOSE(
p1[0], p1Ref[0], 1e-4);
179 BOOST_CHECK_CLOSE(
p1[1], p1Ref[1], 1e-4);
180 BOOST_CHECK_CLOSE(
p1[2], p1Ref[2], 1e-4);
181 BOOST_CHECK_CLOSE(p2[0], p2Ref[0], 1e-4);
182 BOOST_CHECK_CLOSE(p2[1], p2Ref[1], 1e-4);
183 BOOST_CHECK_CLOSE(p2[2], p2Ref[2], 1e-4);
187 0.310622451066, 0.444435113443),
194 distanceResult.
clear();
197 std::cerr <<
"Applied transformations on two boxes" << std::endl;
198 std::cerr <<
" T1 = " <<
tf1.getTranslation() << std::endl
199 <<
" R1 = " <<
tf1.getRotation() << std::endl
200 <<
" T2 = " <<
tf2.getTranslation() << std::endl
201 <<
" R2 = " <<
tf2.getRotation() << std::endl;
202 std::cerr <<
"Closest points: p1 = " << distanceResult.
nearest_points[0]
204 <<
", distance = " << distanceResult.
min_distance << std::endl;
210 BOOST_CHECK_CLOSE(
p1[0], p1Moved[0], 1e-4);
211 BOOST_CHECK_CLOSE(
p1[1], p1Moved[1], 1e-4);
212 BOOST_CHECK_CLOSE(
p1[2], p1Moved[2], 1e-4);
213 BOOST_CHECK_CLOSE(p2[0], p2Moved[0], 1e-4);
214 BOOST_CHECK_CLOSE(p2[1], p2Moved[1], 1e-4);
215 BOOST_CHECK_CLOSE(p2[2], p2Moved[2], 1e-4);
234 tf1.setTranslation(
Vec3f(1.01, 0, 0));
235 distanceResult.
clear();
241 tf1.setTranslation(
Vec3f(0.99, 0, 0));
242 distanceResult.
clear();
249 distanceResult.
clear();