test_fcl_simple.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
36 #include <gtest/gtest.h>
37 
38 #include <sstream>
39 
40 #include "fcl/math/detail/project.h"
41 #include "fcl/narrowphase/collision.h"
42 #include "fcl/geometry/bvh/BVH_model.h"
43 #include "fcl_resources/config.h"
44 #include "fcl/math/sampler/sampler_r.h"
45 #include "fcl/math/sampler/sampler_se2.h"
46 #include "fcl/math/sampler/sampler_se2_disk.h"
47 #include "fcl/math/sampler/sampler_se3_euler.h"
48 #include "fcl/math/sampler/sampler_se3_euler_ball.h"
49 #include "fcl/math/sampler/sampler_se3_quat.h"
50 #include "fcl/math/sampler/sampler_se3_quat_ball.h"
51 #include "fcl/math/geometry.h"
52 
53 using namespace fcl;
54 
55 template <typename S>
56 S epsilon()
57 {
58  return 1e-6;
59 }
60 
61 template <>
62 float epsilon()
63 {
64  return 1e-4;
65 }
66 
67 template <typename S>
68 bool approx(S x, S y)
69 {
70  return std::abs(x - y) < epsilon<S>();
71 }
72 
73 template<typename S, std::size_t N>
74 S distance_Vecnf(const VectorN<S, N>& a, const VectorN<S, N>& b)
75 {
76  S d = 0;
77  for(std::size_t i = 0; i < N; ++i)
78  d += (a[i] - b[i]) * (a[i] - b[i]);
79 
80  return d;
81 }
82 
83 template <typename S>
84 void test_Vec_nf_test()
85 {
86  VectorN<S, 4> a;
87  VectorN<S, 4> b;
88  for(auto i = 0; i < a.size(); ++i)
89  a[i] = i;
90  for(auto i = 0; i < b.size(); ++i)
91  b[i] = 1;
92 
93  std::cout << a.transpose() << std::endl;
94  std::cout << b.transpose() << std::endl;
95  std::cout << (a + b).transpose() << std::endl;
96  std::cout << (a - b).transpose() << std::endl;
97  std::cout << (a -= b).transpose() << std::endl;
98  std::cout << (a += b).transpose() << std::endl;
99  std::cout << (a * 2).transpose() << std::endl;
100  std::cout << (a / 2).transpose() << std::endl;
101  std::cout << (a *= 2).transpose() << std::endl;
102  std::cout << (a /= 2).transpose() << std::endl;
103  std::cout << a.dot(b) << std::endl;
104 
105  VectorN<S, 8> c = combine(a, b);
106  std::cout << c.transpose() << std::endl;
107 
108  VectorN<S, 4> upper, lower;
109  for(int i = 0; i < 4; ++i)
110  upper[i] = 1;
111 
112  VectorN<S, 4> aa = VectorN<S, 4>(1, 2, 1, 2);
113  std::cout << aa.transpose() << std::endl;
114 
115  SamplerR<S, 4> sampler(lower, upper);
116  for(std::size_t i = 0; i < 10; ++i)
117  std::cout << sampler.sample().transpose() << std::endl;
118 
119  // Disabled broken test lines. Please see #25.
120  // SamplerSE2 sampler2(0, 1, -1, 1);
121  // for(std::size_t i = 0; i < 10; ++i)
122  // std::cout << sampler2.sample() << std::endl;
123 
124  SamplerSE3Euler<S> sampler3(Vector3<S>(0, 0, 0), Vector3<S>(1, 1, 1));
125  for(std::size_t i = 0; i < 10; ++i)
126  std::cout << sampler3.sample().transpose() << std::endl;
127 
128 }
129 
130 GTEST_TEST(FCL_SIMPLE, Vec_nf_test)
131 {
132 // test_Vec_nf_test<float>();
133  test_Vec_nf_test<double>();
134 }
135 
136 template <typename S>
137 void test_projection_test_line()
138 {
139  Vector3<S> v1(0, 0, 0);
140  Vector3<S> v2(2, 0, 0);
141 
142  Vector3<S> p(1, 0, 0);
143  auto res = detail::Project<S>::projectLine(v1, v2, p);
144  EXPECT_TRUE(res.encode == 3);
145  EXPECT_TRUE(approx(res.sqr_distance, (S)0));
146  EXPECT_TRUE(approx(res.parameterization[0], (S)0.5));
147  EXPECT_TRUE(approx(res.parameterization[1], (S)0.5));
148 
149  p = Vector3<S>(-1, 0, 0);
150  res = detail::Project<S>::projectLine(v1, v2, p);
151  EXPECT_TRUE(res.encode == 1);
152  EXPECT_TRUE(approx(res.sqr_distance, (S)1));
153  EXPECT_TRUE(approx(res.parameterization[0], (S)1));
154  EXPECT_TRUE(approx(res.parameterization[1], (S)0));
155 
156  p = Vector3<S>(3, 0, 0);
157  res = detail::Project<S>::projectLine(v1, v2, p);
158  EXPECT_TRUE(res.encode == 2);
159  EXPECT_TRUE(approx(res.sqr_distance, (S)1));
160  EXPECT_TRUE(approx(res.parameterization[0], (S)0));
161  EXPECT_TRUE(approx(res.parameterization[1], (S)1));
162 
163 }
164 
165 GTEST_TEST(FCL_SIMPLE, projection_test_line)
166 {
167 // test_projection_test_line<float>();
168  test_projection_test_line<double>();
169 }
170 
171 template <typename S>
172 void test_projection_test_triangle()
173 {
174  Vector3<S> v1(0, 0, 1);
175  Vector3<S> v2(0, 1, 0);
176  Vector3<S> v3(1, 0, 0);
177 
178  Vector3<S> p(1, 1, 1);
179  auto res = detail::Project<S>::projectTriangle(v1, v2, v3, p);
180  EXPECT_TRUE(res.encode == 7);
181  EXPECT_TRUE(approx(res.sqr_distance, (S)(4/3.0)));
182  EXPECT_TRUE(approx(res.parameterization[0], (S)(1/3.0)));
183  EXPECT_TRUE(approx(res.parameterization[1], (S)(1/3.0)));
184  EXPECT_TRUE(approx(res.parameterization[2], (S)(1/3.0)));
185 
186  p = Vector3<S>(0, 0, 1.5);
187  res = detail::Project<S>::projectTriangle(v1, v2, v3, p);
188  EXPECT_TRUE(res.encode == 1);
189  EXPECT_TRUE(approx(res.sqr_distance, (S)0.25));
190  EXPECT_TRUE(approx(res.parameterization[0], (S)1));
191  EXPECT_TRUE(approx(res.parameterization[1], (S)0));
192  EXPECT_TRUE(approx(res.parameterization[2], (S)0));
193 
194  p = Vector3<S>(1.5, 0, 0);
195  res = detail::Project<S>::projectTriangle(v1, v2, v3, p);
196  EXPECT_TRUE(res.encode == 4);
197  EXPECT_TRUE(approx(res.sqr_distance, (S)0.25));
198  EXPECT_TRUE(approx(res.parameterization[0], (S)0));
199  EXPECT_TRUE(approx(res.parameterization[1], (S)0));
200  EXPECT_TRUE(approx(res.parameterization[2], (S)1));
201 
202  p = Vector3<S>(0, 1.5, 0);
203  res = detail::Project<S>::projectTriangle(v1, v2, v3, p);
204  EXPECT_TRUE(res.encode == 2);
205  EXPECT_TRUE(approx(res.sqr_distance, (S)0.25));
206  EXPECT_TRUE(approx(res.parameterization[0], (S)0));
207  EXPECT_TRUE(approx(res.parameterization[1], (S)1));
208  EXPECT_TRUE(approx(res.parameterization[2], (S)0));
209 
210  p = Vector3<S>(1, 1, 0);
211  res = detail::Project<S>::projectTriangle(v1, v2, v3, p);
212  EXPECT_TRUE(res.encode == 6);
213  EXPECT_TRUE(approx(res.sqr_distance, (S)0.5));
214  EXPECT_TRUE(approx(res.parameterization[0], (S)0));
215  EXPECT_TRUE(approx(res.parameterization[1], (S)0.5));
216  EXPECT_TRUE(approx(res.parameterization[2], (S)0.5));
217 
218  p = Vector3<S>(1, 0, 1);
219  res = detail::Project<S>::projectTriangle(v1, v2, v3, p);
220  EXPECT_TRUE(res.encode == 5);
221  EXPECT_TRUE(approx(res.sqr_distance, (S)0.5));
222  EXPECT_TRUE(approx(res.parameterization[0], (S)0.5));
223  EXPECT_TRUE(approx(res.parameterization[1], (S)0));
224  EXPECT_TRUE(approx(res.parameterization[2], (S)0.5));
225 
226  p = Vector3<S>(0, 1, 1);
227  res = detail::Project<S>::projectTriangle(v1, v2, v3, p);
228  EXPECT_TRUE(res.encode == 3);
229  EXPECT_TRUE(approx(res.sqr_distance, (S)0.5));
230  EXPECT_TRUE(approx(res.parameterization[0], (S)0.5));
231  EXPECT_TRUE(approx(res.parameterization[1], (S)0.5));
232  EXPECT_TRUE(approx(res.parameterization[2], (S)0));
233 }
234 
235 GTEST_TEST(FCL_SIMPLE, projection_test_triangle)
236 {
237 // test_projection_test_triangle<float>();
238  test_projection_test_triangle<double>();
239 }
240 
241 template <typename S>
242 void test_projection_test_tetrahedron()
243 {
244  Vector3<S> v1(0, 0, 1);
245  Vector3<S> v2(0, 1, 0);
246  Vector3<S> v3(1, 0, 0);
247  Vector3<S> v4(1, 1, 1);
248 
249  Vector3<S> p(0.5, 0.5, 0.5);
250  auto res = detail::Project<S>::projectTetrahedra(v1, v2, v3, v4, p);
251  EXPECT_TRUE(res.encode == 15);
252  EXPECT_TRUE(approx(res.sqr_distance, (S)0));
253  EXPECT_TRUE(approx(res.parameterization[0], (S)0.25));
254  EXPECT_TRUE(approx(res.parameterization[1], (S)0.25));
255  EXPECT_TRUE(approx(res.parameterization[2], (S)0.25));
256  EXPECT_TRUE(approx(res.parameterization[3], (S)0.25));
257 
258  p = Vector3<S>(0, 0, 0);
259  res = detail::Project<S>::projectTetrahedra(v1, v2, v3, v4, p);
260  EXPECT_TRUE(res.encode == 7);
261  EXPECT_TRUE(approx(res.sqr_distance, (S)(1/3.0)));
262  EXPECT_TRUE(approx(res.parameterization[0], (S)(1/3.0)));
263  EXPECT_TRUE(approx(res.parameterization[1], (S)(1/3.0)));
264  EXPECT_TRUE(approx(res.parameterization[2], (S)(1/3.0)));
265  EXPECT_TRUE(approx(res.parameterization[3], (S)0));
266 
267  p = Vector3<S>(0, 1, 1);
268  res = detail::Project<S>::projectTetrahedra(v1, v2, v3, v4, p);
269  EXPECT_TRUE(res.encode == 11);
270  EXPECT_TRUE(approx(res.sqr_distance, (S)(1/3.0)));
271  EXPECT_TRUE(approx(res.parameterization[0], (S)(1/3.0)));
272  EXPECT_TRUE(approx(res.parameterization[1], (S)(1/3.0)));
273  EXPECT_TRUE(approx(res.parameterization[2], (S)0));
274  EXPECT_TRUE(approx(res.parameterization[3], (S)(1/3.0)));
275 
276  p = Vector3<S>(1, 1, 0);
277  res = detail::Project<S>::projectTetrahedra(v1, v2, v3, v4, p);
278  EXPECT_TRUE(res.encode == 14);
279  EXPECT_TRUE(approx(res.sqr_distance, (S)(1/3.0)));
280  EXPECT_TRUE(approx(res.parameterization[0], (S)0));
281  EXPECT_TRUE(approx(res.parameterization[1], (S)(1/3.0)));
282  EXPECT_TRUE(approx(res.parameterization[2], (S)(1/3.0)));
283  EXPECT_TRUE(approx(res.parameterization[3], (S)(1/3.0)));
284 
285  p = Vector3<S>(1, 0, 1);
286  res = detail::Project<S>::projectTetrahedra(v1, v2, v3, v4, p);
287  EXPECT_TRUE(res.encode == 13);
288  EXPECT_TRUE(approx(res.sqr_distance, (S)(1/3.0)));
289  EXPECT_TRUE(approx(res.parameterization[0], (S)(1/3.0)));
290  EXPECT_TRUE(approx(res.parameterization[1], (S)0));
291  EXPECT_TRUE(approx(res.parameterization[2], (S)(1/3.0)));
292  EXPECT_TRUE(approx(res.parameterization[3], (S)(1/3.0)));
293 
294  p = Vector3<S>(1.5, 1.5, 1.5);
295  res = detail::Project<S>::projectTetrahedra(v1, v2, v3, v4, p);
296  EXPECT_TRUE(res.encode == 8);
297  EXPECT_TRUE(approx(res.sqr_distance, (S)0.75));
298  EXPECT_TRUE(approx(res.parameterization[0], (S)0));
299  EXPECT_TRUE(approx(res.parameterization[1], (S)0));
300  EXPECT_TRUE(approx(res.parameterization[2], (S)0));
301  EXPECT_TRUE(approx(res.parameterization[3], (S)1));
302 
303  p = Vector3<S>(1.5, -0.5, -0.5);
304  res = detail::Project<S>::projectTetrahedra(v1, v2, v3, v4, p);
305  EXPECT_TRUE(res.encode == 4);
306  EXPECT_TRUE(approx(res.sqr_distance, (S)0.75));
307  EXPECT_TRUE(approx(res.parameterization[0], (S)0));
308  EXPECT_TRUE(approx(res.parameterization[1], (S)0));
309  EXPECT_TRUE(approx(res.parameterization[2], (S)1));
310  EXPECT_TRUE(approx(res.parameterization[3], (S)0));
311 
312  p = Vector3<S>(-0.5, -0.5, 1.5);
313  res = detail::Project<S>::projectTetrahedra(v1, v2, v3, v4, p);
314  EXPECT_TRUE(res.encode == 1);
315  EXPECT_TRUE(approx(res.sqr_distance, (S)0.75));
316  EXPECT_TRUE(approx(res.parameterization[0], (S)1));
317  EXPECT_TRUE(approx(res.parameterization[1], (S)0));
318  EXPECT_TRUE(approx(res.parameterization[2], (S)0));
319  EXPECT_TRUE(approx(res.parameterization[3], (S)0));
320 
321  p = Vector3<S>(-0.5, 1.5, -0.5);
322  res = detail::Project<S>::projectTetrahedra(v1, v2, v3, v4, p);
323  EXPECT_TRUE(res.encode == 2);
324  EXPECT_TRUE(approx(res.sqr_distance, (S)0.75));
325  EXPECT_TRUE(approx(res.parameterization[0], (S)0));
326  EXPECT_TRUE(approx(res.parameterization[1], (S)1));
327  EXPECT_TRUE(approx(res.parameterization[2], (S)0));
328  EXPECT_TRUE(approx(res.parameterization[3], (S)0));
329 
330  p = Vector3<S>(0.5, -0.5, 0.5);
331  res = detail::Project<S>::projectTetrahedra(v1, v2, v3, v4, p);
332  EXPECT_TRUE(res.encode == 5);
333  EXPECT_TRUE(approx(res.sqr_distance, (S)0.25));
334  EXPECT_TRUE(approx(res.parameterization[0], (S)0.5));
335  EXPECT_TRUE(approx(res.parameterization[1], (S)0));
336  EXPECT_TRUE(approx(res.parameterization[2], (S)0.5));
337  EXPECT_TRUE(approx(res.parameterization[3], (S)0));
338 
339  p = Vector3<S>(0.5, 1.5, 0.5);
340  res = detail::Project<S>::projectTetrahedra(v1, v2, v3, v4, p);
341  EXPECT_TRUE(res.encode == 10);
342  EXPECT_TRUE(approx(res.sqr_distance, (S)0.25));
343  EXPECT_TRUE(approx(res.parameterization[0], (S)0));
344  EXPECT_TRUE(approx(res.parameterization[1], (S)0.5));
345  EXPECT_TRUE(approx(res.parameterization[2], (S)0));
346  EXPECT_TRUE(approx(res.parameterization[3], (S)0.5));
347 
348  p = Vector3<S>(1.5, 0.5, 0.5);
349  res = detail::Project<S>::projectTetrahedra(v1, v2, v3, v4, p);
350  EXPECT_TRUE(res.encode == 12);
351  EXPECT_TRUE(approx(res.sqr_distance, (S)0.25));
352  EXPECT_TRUE(approx(res.parameterization[0], (S)0));
353  EXPECT_TRUE(approx(res.parameterization[1], (S)0));
354  EXPECT_TRUE(approx(res.parameterization[2], (S)0.5));
355  EXPECT_TRUE(approx(res.parameterization[3], (S)0.5));
356 
357  p = Vector3<S>(-0.5, 0.5, 0.5);
358  res = detail::Project<S>::projectTetrahedra(v1, v2, v3, v4, p);
359  EXPECT_TRUE(res.encode == 3);
360  EXPECT_TRUE(approx(res.sqr_distance, (S)0.25));
361  EXPECT_TRUE(approx(res.parameterization[0], (S)0.5));
362  EXPECT_TRUE(approx(res.parameterization[1], (S)0.5));
363  EXPECT_TRUE(approx(res.parameterization[2], (S)0));
364  EXPECT_TRUE(approx(res.parameterization[3], (S)0));
365 
366  p = Vector3<S>(0.5, 0.5, 1.5);
367  res = detail::Project<S>::projectTetrahedra(v1, v2, v3, v4, p);
368  EXPECT_TRUE(res.encode == 9);
369  EXPECT_TRUE(approx(res.sqr_distance, (S)0.25));
370  EXPECT_TRUE(approx(res.parameterization[0], (S)0.5));
371  EXPECT_TRUE(approx(res.parameterization[1], (S)0));
372  EXPECT_TRUE(approx(res.parameterization[2], (S)0));
373  EXPECT_TRUE(approx(res.parameterization[3], (S)0.5));
374 
375  p = Vector3<S>(0.5, 0.5, -0.5);
376  res = detail::Project<S>::projectTetrahedra(v1, v2, v3, v4, p);
377  EXPECT_TRUE(res.encode == 6);
378  EXPECT_TRUE(approx(res.sqr_distance, (S)0.25));
379  EXPECT_TRUE(approx(res.parameterization[0], (S)0));
380  EXPECT_TRUE(approx(res.parameterization[1], (S)0.5));
381  EXPECT_TRUE(approx(res.parameterization[2], (S)0.5));
382  EXPECT_TRUE(approx(res.parameterization[3], (S)0));
383 
384 }
385 
386 GTEST_TEST(FCL_SIMPLE, projection_test_tetrahedron)
387 {
388 // test_projection_test_tetrahedron<float>();
389  test_projection_test_tetrahedron<double>();
390 }
391 
392 //==============================================================================
393 int main(int argc, char* argv[])
394 {
395  ::testing::InitGoogleTest(&argc, argv);
396  return RUN_ALL_TESTS();
397 }
fcl::SamplerR::sample
VectorN< S, N > sample() const
Definition: sampler_r-inl.h:78
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::VectorN
Eigen::Matrix< S, N, 1 > VectorN
Definition: types.h:79
sampler_r.h
fcl::SamplerSE3Euler::sample
Vector6< S > sample() const
Definition: sampler_se3_euler-inl.h:67
fcl::detail::Project::projectTetrahedra
static ProjectResult projectTetrahedra(const Vector3< S > &a, const Vector3< S > &b, const Vector3< S > &c, const Vector3< S > &d, const Vector3< S > &p)
Project point p onto tetrahedra a-b-c-d.
Definition: project-inl.h:129
epsilon
S epsilon()
Definition: test_fcl_simple.cpp:56
test_Vec_nf_test
void test_Vec_nf_test()
Definition: test_fcl_simple.cpp:84
distance_Vecnf
S distance_Vecnf(const VectorN< S, N > &a, const VectorN< S, N > &b)
Definition: test_fcl_simple.cpp:74
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
sampler_se3_quat_ball.h
fcl::combine
FCL_EXPORT VectorN< S, M+N > combine(const VectorN< S, M > &v1, const VectorN< S, N > &v2)
Definition: geometry-inl.h:441
sampler_se3_quat.h
sampler_se2.h
fcl::detail::Project::projectTriangle
static ProjectResult projectTriangle(const Vector3< S > &a, const Vector3< S > &b, const Vector3< S > &c, const Vector3< S > &p)
Project point p onto triangle a-b-c.
Definition: project-inl.h:77
test_projection_test_tetrahedron
void test_projection_test_tetrahedron()
Definition: test_fcl_simple.cpp:242
sampler_se3_euler.h
sampler_se3_euler_ball.h
main
int main(int argc, char *argv[])
Definition: test_fcl_simple.cpp:393
BVH_model.h
test_projection_test_line
void test_projection_test_line()
Definition: test_fcl_simple.cpp:137
fcl::detail::Project::projectLine
static ProjectResult projectLine(const Vector3< S > &a, const Vector3< S > &b, const Vector3< S > &p)
Project point p onto line a-b.
Definition: project-inl.h:55
sampler_se2_disk.h
test_projection_test_triangle
void test_projection_test_triangle()
Definition: test_fcl_simple.cpp:172
EXPECT_TRUE
#define EXPECT_TRUE(args)
GTEST_TEST
GTEST_TEST(FCL_SIMPLE, Vec_nf_test)
Definition: test_fcl_simple.cpp:130
approx
bool approx(S x, S y)
Definition: test_fcl_simple.cpp:68

fcl_catkin
Author(s):
autogenerated on Thu Mar 23 2023 03:00:19