simple.cpp
Go to the documentation of this file.
1 #define BOOST_TEST_MODULE FCL_SIMPLE
2 #include <boost/test/included/unit_test.hpp>
3 
4 #include <hpp/fcl/internal/intersect.h>
5 #include <hpp/fcl/collision.h>
6 #include <hpp/fcl/BVH/BVH_model.h>
7 #include "fcl_resources/config.h"
8 #include <sstream>
9 
10 using namespace hpp::fcl;
11 
12 static FCL_REAL epsilon = 1e-6;
13 
14 static bool approx(FCL_REAL x, FCL_REAL y) { return std::abs(x - y) < epsilon; }
15 
16 BOOST_AUTO_TEST_CASE(projection_test_line) {
17  Vec3f v1(0, 0, 0);
18  Vec3f v2(2, 0, 0);
19 
20  Vec3f p(1, 0, 0);
21  Project::ProjectResult res = Project::projectLine(v1, v2, p);
22  BOOST_CHECK(res.encode == 3);
23  BOOST_CHECK(approx(res.sqr_distance, 0));
24  BOOST_CHECK(approx(res.parameterization[0], 0.5));
25  BOOST_CHECK(approx(res.parameterization[1], 0.5));
26 
27  p = Vec3f(-1, 0, 0);
28  res = Project::projectLine(v1, v2, p);
29  BOOST_CHECK(res.encode == 1);
30  BOOST_CHECK(approx(res.sqr_distance, 1));
31  BOOST_CHECK(approx(res.parameterization[0], 1));
32  BOOST_CHECK(approx(res.parameterization[1], 0));
33 
34  p = Vec3f(3, 0, 0);
35  res = Project::projectLine(v1, v2, p);
36  BOOST_CHECK(res.encode == 2);
37  BOOST_CHECK(approx(res.sqr_distance, 1));
38  BOOST_CHECK(approx(res.parameterization[0], 0));
39  BOOST_CHECK(approx(res.parameterization[1], 1));
40 }
41 
42 BOOST_AUTO_TEST_CASE(projection_test_triangle) {
43  Vec3f v1(0, 0, 1);
44  Vec3f v2(0, 1, 0);
45  Vec3f v3(1, 0, 0);
46 
47  Vec3f p(1, 1, 1);
48  Project::ProjectResult res = Project::projectTriangle(v1, v2, v3, p);
49  BOOST_CHECK(res.encode == 7);
50  BOOST_CHECK(approx(res.sqr_distance, 4 / 3.0));
51  BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0));
52  BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0));
53  BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0));
54 
55  p = Vec3f(0, 0, 1.5);
56  res = Project::projectTriangle(v1, v2, v3, p);
57  BOOST_CHECK(res.encode == 1);
58  BOOST_CHECK(approx(res.sqr_distance, 0.25));
59  BOOST_CHECK(approx(res.parameterization[0], 1));
60  BOOST_CHECK(approx(res.parameterization[1], 0));
61  BOOST_CHECK(approx(res.parameterization[2], 0));
62 
63  p = Vec3f(1.5, 0, 0);
64  res = Project::projectTriangle(v1, v2, v3, p);
65  BOOST_CHECK(res.encode == 4);
66  BOOST_CHECK(approx(res.sqr_distance, 0.25));
67  BOOST_CHECK(approx(res.parameterization[0], 0));
68  BOOST_CHECK(approx(res.parameterization[1], 0));
69  BOOST_CHECK(approx(res.parameterization[2], 1));
70 
71  p = Vec3f(0, 1.5, 0);
72  res = Project::projectTriangle(v1, v2, v3, p);
73  BOOST_CHECK(res.encode == 2);
74  BOOST_CHECK(approx(res.sqr_distance, 0.25));
75  BOOST_CHECK(approx(res.parameterization[0], 0));
76  BOOST_CHECK(approx(res.parameterization[1], 1));
77  BOOST_CHECK(approx(res.parameterization[2], 0));
78 
79  p = Vec3f(1, 1, 0);
80  res = Project::projectTriangle(v1, v2, v3, p);
81  BOOST_CHECK(res.encode == 6);
82  BOOST_CHECK(approx(res.sqr_distance, 0.5));
83  BOOST_CHECK(approx(res.parameterization[0], 0));
84  BOOST_CHECK(approx(res.parameterization[1], 0.5));
85  BOOST_CHECK(approx(res.parameterization[2], 0.5));
86 
87  p = Vec3f(1, 0, 1);
88  res = Project::projectTriangle(v1, v2, v3, p);
89  BOOST_CHECK(res.encode == 5);
90  BOOST_CHECK(approx(res.sqr_distance, 0.5));
91  BOOST_CHECK(approx(res.parameterization[0], 0.5));
92  BOOST_CHECK(approx(res.parameterization[1], 0));
93  BOOST_CHECK(approx(res.parameterization[2], 0.5));
94 
95  p = Vec3f(0, 1, 1);
96  res = Project::projectTriangle(v1, v2, v3, p);
97  BOOST_CHECK(res.encode == 3);
98  BOOST_CHECK(approx(res.sqr_distance, 0.5));
99  BOOST_CHECK(approx(res.parameterization[0], 0.5));
100  BOOST_CHECK(approx(res.parameterization[1], 0.5));
101  BOOST_CHECK(approx(res.parameterization[2], 0));
102 }
103 
104 BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) {
105  Vec3f v1(0, 0, 1);
106  Vec3f v2(0, 1, 0);
107  Vec3f v3(1, 0, 0);
108  Vec3f v4(1, 1, 1);
109 
110  Vec3f p(0.5, 0.5, 0.5);
111  Project::ProjectResult res = Project::projectTetrahedra(v1, v2, v3, v4, p);
112  BOOST_CHECK(res.encode == 15);
113  BOOST_CHECK(approx(res.sqr_distance, 0));
114  BOOST_CHECK(approx(res.parameterization[0], 0.25));
115  BOOST_CHECK(approx(res.parameterization[1], 0.25));
116  BOOST_CHECK(approx(res.parameterization[2], 0.25));
117  BOOST_CHECK(approx(res.parameterization[3], 0.25));
118 
119  p = Vec3f(0, 0, 0);
120  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
121  BOOST_CHECK(res.encode == 7);
122  BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0));
123  BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0));
124  BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0));
125  BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0));
126  BOOST_CHECK(approx(res.parameterization[3], 0));
127 
128  p = Vec3f(0, 1, 1);
129  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
130  BOOST_CHECK(res.encode == 11);
131  BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0));
132  BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0));
133  BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0));
134  BOOST_CHECK(approx(res.parameterization[2], 0));
135  BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0));
136 
137  p = Vec3f(1, 1, 0);
138  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
139  BOOST_CHECK(res.encode == 14);
140  BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0));
141  BOOST_CHECK(approx(res.parameterization[0], 0));
142  BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0));
143  BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0));
144  BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0));
145 
146  p = Vec3f(1, 0, 1);
147  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
148  BOOST_CHECK(res.encode == 13);
149  BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0));
150  BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0));
151  BOOST_CHECK(approx(res.parameterization[1], 0));
152  BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0));
153  BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0));
154 
155  p = Vec3f(1.5, 1.5, 1.5);
156  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
157  BOOST_CHECK(res.encode == 8);
158  BOOST_CHECK(approx(res.sqr_distance, 0.75));
159  BOOST_CHECK(approx(res.parameterization[0], 0));
160  BOOST_CHECK(approx(res.parameterization[1], 0));
161  BOOST_CHECK(approx(res.parameterization[2], 0));
162  BOOST_CHECK(approx(res.parameterization[3], 1));
163 
164  p = Vec3f(1.5, -0.5, -0.5);
165  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
166  BOOST_CHECK(res.encode == 4);
167  BOOST_CHECK(approx(res.sqr_distance, 0.75));
168  BOOST_CHECK(approx(res.parameterization[0], 0));
169  BOOST_CHECK(approx(res.parameterization[1], 0));
170  BOOST_CHECK(approx(res.parameterization[2], 1));
171  BOOST_CHECK(approx(res.parameterization[3], 0));
172 
173  p = Vec3f(-0.5, -0.5, 1.5);
174  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
175  BOOST_CHECK(res.encode == 1);
176  BOOST_CHECK(approx(res.sqr_distance, 0.75));
177  BOOST_CHECK(approx(res.parameterization[0], 1));
178  BOOST_CHECK(approx(res.parameterization[1], 0));
179  BOOST_CHECK(approx(res.parameterization[2], 0));
180  BOOST_CHECK(approx(res.parameterization[3], 0));
181 
182  p = Vec3f(-0.5, 1.5, -0.5);
183  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
184  BOOST_CHECK(res.encode == 2);
185  BOOST_CHECK(approx(res.sqr_distance, 0.75));
186  BOOST_CHECK(approx(res.parameterization[0], 0));
187  BOOST_CHECK(approx(res.parameterization[1], 1));
188  BOOST_CHECK(approx(res.parameterization[2], 0));
189  BOOST_CHECK(approx(res.parameterization[3], 0));
190 
191  p = Vec3f(0.5, -0.5, 0.5);
192  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
193  BOOST_CHECK(res.encode == 5);
194  BOOST_CHECK(approx(res.sqr_distance, 0.25));
195  BOOST_CHECK(approx(res.parameterization[0], 0.5));
196  BOOST_CHECK(approx(res.parameterization[1], 0));
197  BOOST_CHECK(approx(res.parameterization[2], 0.5));
198  BOOST_CHECK(approx(res.parameterization[3], 0));
199 
200  p = Vec3f(0.5, 1.5, 0.5);
201  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
202  BOOST_CHECK(res.encode == 10);
203  BOOST_CHECK(approx(res.sqr_distance, 0.25));
204  BOOST_CHECK(approx(res.parameterization[0], 0));
205  BOOST_CHECK(approx(res.parameterization[1], 0.5));
206  BOOST_CHECK(approx(res.parameterization[2], 0));
207  BOOST_CHECK(approx(res.parameterization[3], 0.5));
208 
209  p = Vec3f(1.5, 0.5, 0.5);
210  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
211  BOOST_CHECK(res.encode == 12);
212  BOOST_CHECK(approx(res.sqr_distance, 0.25));
213  BOOST_CHECK(approx(res.parameterization[0], 0));
214  BOOST_CHECK(approx(res.parameterization[1], 0));
215  BOOST_CHECK(approx(res.parameterization[2], 0.5));
216  BOOST_CHECK(approx(res.parameterization[3], 0.5));
217 
218  p = Vec3f(-0.5, 0.5, 0.5);
219  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
220  BOOST_CHECK(res.encode == 3);
221  BOOST_CHECK(approx(res.sqr_distance, 0.25));
222  BOOST_CHECK(approx(res.parameterization[0], 0.5));
223  BOOST_CHECK(approx(res.parameterization[1], 0.5));
224  BOOST_CHECK(approx(res.parameterization[2], 0));
225  BOOST_CHECK(approx(res.parameterization[3], 0));
226 
227  p = Vec3f(0.5, 0.5, 1.5);
228  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
229  BOOST_CHECK(res.encode == 9);
230  BOOST_CHECK(approx(res.sqr_distance, 0.25));
231  BOOST_CHECK(approx(res.parameterization[0], 0.5));
232  BOOST_CHECK(approx(res.parameterization[1], 0));
233  BOOST_CHECK(approx(res.parameterization[2], 0));
234  BOOST_CHECK(approx(res.parameterization[3], 0.5));
235 
236  p = Vec3f(0.5, 0.5, -0.5);
237  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
238  BOOST_CHECK(res.encode == 6);
239  BOOST_CHECK(approx(res.sqr_distance, 0.25));
240  BOOST_CHECK(approx(res.parameterization[0], 0));
241  BOOST_CHECK(approx(res.parameterization[1], 0.5));
242  BOOST_CHECK(approx(res.parameterization[2], 0.5));
243  BOOST_CHECK(approx(res.parameterization[3], 0));
244 }
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
y
y
res
res
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
epsilon
static FCL_REAL epsilon
Definition: simple.cpp:12
x
x
hpp::fcl
Definition: broadphase_bruteforce.h:45
BVH_model.h
approx
static bool approx(FCL_REAL x, FCL_REAL y)
Definition: simple.cpp:14
intersect.h
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(projection_test_line)
Definition: simple.cpp:16
collision.h

hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:15