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

hpp-fcl
Author(s):
autogenerated on Fri Feb 14 2025 03:45:51