GCC Code Coverage Report

Directory: ./
File: test/utility.cpp
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 324 403 80.4%
Branches: 222 618 35.9%
Line Branch Exec Source
1 #include "utility.h"
2
3 #include "coal/BV/BV.h"
4 #include "coal/BVH/BVH_model.h"
5 #include "coal/shape/geometric_shape_to_BVH_model.h"
6 #include "coal/collision_utility.h"
7 #include "coal/fwd.hh"
8
9 #include "coal/collision.h"
10 #include "coal/distance.h"
11
12 #include <cstdio>
13 #include <cstddef>
14 #include <fstream>
15 #include <iostream>
16
17 namespace coal {
18
19 500 BenchTimer::BenchTimer() {
20 #ifdef _WIN32
21 QueryPerformanceFrequency(&frequency);
22 startCount.QuadPart = 0;
23 endCount.QuadPart = 0;
24 #else
25 500 startCount.tv_sec = startCount.tv_usec = 0;
26 500 endCount.tv_sec = endCount.tv_usec = 0;
27 #endif
28
29 500 stopped = 0;
30 500 startTimeInMicroSec = 0;
31 500 endTimeInMicroSec = 0;
32 500 }
33
34 500 BenchTimer::~BenchTimer() {}
35
36 36581 void BenchTimer::start() {
37 36581 stopped = 0; // reset stop flag
38 #ifdef _WIN32
39 QueryPerformanceCounter(&startCount);
40 #else
41 36581 gettimeofday(&startCount, NULL);
42 #endif
43 36581 }
44
45 36581 void BenchTimer::stop() {
46 36581 stopped = 1; // set timer stopped flag
47
48 #ifdef _WIN32
49 QueryPerformanceCounter(&endCount);
50 #else
51 36581 gettimeofday(&endCount, NULL);
52 #endif
53 36581 }
54
55 36581 double BenchTimer::getElapsedTimeInMicroSec() {
56 #ifdef _WIN32
57 if (!stopped) QueryPerformanceCounter(&endCount);
58
59 startTimeInMicroSec = startCount.QuadPart * (1000000.0 / frequency.QuadPart);
60 endTimeInMicroSec = endCount.QuadPart * (1000000.0 / frequency.QuadPart);
61 #else
62
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 36581 times.
 36581 if (!stopped) gettimeofday(&endCount, NULL);
63
64 36581 startTimeInMicroSec =
65 36581 ((double)startCount.tv_sec * 1000000.0) + (double)startCount.tv_usec;
66 36581 endTimeInMicroSec =
67 36581 ((double)endCount.tv_sec * 1000000.0) + (double)endCount.tv_usec;
68 #endif
69
70 36581 return endTimeInMicroSec - startTimeInMicroSec;
71 }
72
73 36540 double BenchTimer::getElapsedTimeInMilliSec() {
74 36540 return this->getElapsedTimeInMicroSec() * 0.001;
75 }
76
77 ✗ double BenchTimer::getElapsedTimeInSec() {
78 ✗ return this->getElapsedTimeInMicroSec() * 0.000001;
79 }
80
81 36540 double BenchTimer::getElapsedTime() { return this->getElapsedTimeInMilliSec(); }
82
83 const Eigen::IOFormat vfmt = Eigen::IOFormat(
84 Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "", "");
85 const Eigen::IOFormat pyfmt = Eigen::IOFormat(
86 Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]");
87
88 const Vec3s UnitX = Vec3s(1, 0, 0);
89 const Vec3s UnitY = Vec3s(0, 1, 0);
90 const Vec3s UnitZ = Vec3s(0, 0, 1);
91
92 364429 Scalar rand_interval(Scalar rmin, Scalar rmax) {
93 364429 Scalar t = Scalar(rand()) / ((Scalar)RAND_MAX + 1);
94 364429 return (t * (rmax - rmin) + rmin);
95 }
96
97 17 void loadOBJFile(const char* filename, std::vector<Vec3s>& points,
98 std::vector<Triangle>& triangles) {
99
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 FILE* file = fopen(filename, "rb");
100
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 17 times.
 17 if (!file) {
101 ✗ std::cerr << "file not exist" << std::endl;
102 ✗ return;
103 }
104
105 17 bool has_normal = false;
106 17 bool has_texture = false;
107 char line_buffer[2000];
108
3/4
✓ Branch 1 taken 93282 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 93265 times.
✓ Branch 4 taken 17 times.
 93282 while (fgets(line_buffer, 2000, file)) {
109 93265 char* first_token = strtok(line_buffer, "\r\n\t ");
110
3/6
✓ Branch 0 taken 93265 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 93265 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 93265 times.
 93265 if (!first_token || first_token[0] == '#' || first_token[0] == 0) continue;
111
112
3/3
✓ Branch 0 taken 69936 times.
✓ Branch 1 taken 23312 times.
✓ Branch 2 taken 17 times.
 93265 switch (first_token[0]) {
113 69936 case 'v': {
114
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 69936 times.
 69936 if (first_token[1] == 'n') {
115 ✗ strtok(NULL, "\t ");
116 ✗ strtok(NULL, "\t ");
117 ✗ strtok(NULL, "\t ");
118 ✗ has_normal = true;
119
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 69936 times.
 69936 } else if (first_token[1] == 't') {
120 ✗ strtok(NULL, "\t ");
121 ✗ strtok(NULL, "\t ");
122 ✗ has_texture = true;
123 } else {
124 69936 Scalar x = (Scalar)atof(strtok(NULL, "\t "));
125 69936 Scalar y = (Scalar)atof(strtok(NULL, "\t "));
126 69936 Scalar z = (Scalar)atof(strtok(NULL, "\t "));
127
1/2
✓ Branch 1 taken 69936 times.
✗ Branch 2 not taken.
 69936 Vec3s p(x, y, z);
128
1/2
✓ Branch 1 taken 69936 times.
✗ Branch 2 not taken.
 69936 points.push_back(p);
129 }
130 69936 } break;
131 23312 case 'f': {
132 23312 Triangle tri;
133 char* data[30];
134 23312 int n = 0;
135
2/2
✓ Branch 1 taken 69936 times.
✓ Branch 2 taken 23312 times.
 93248 while ((data[n] = strtok(NULL, "\t \r\n")) != NULL) {
136
1/2
✓ Branch 0 taken 69936 times.
✗ Branch 1 not taken.
 69936 if (strlen(data[n])) n++;
137 }
138
139
2/2
✓ Branch 0 taken 23312 times.
✓ Branch 1 taken 23312 times.
 46624 for (int t = 0; t < (n - 2); ++t) {
140
2/4
✓ Branch 0 taken 23312 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 23312 times.
✗ Branch 3 not taken.
 23312 if ((!has_texture) && (!has_normal)) {
141 23312 tri[0] = (Triangle::index_type)(atoi(data[0]) - 1);
142 23312 tri[1] = (Triangle::index_type)(atoi(data[1]) - 1);
143 23312 tri[2] = (Triangle::index_type)(atoi(data[2]) - 1);
144 } else {
145 const char* v1;
146 ✗ for (Triangle::index_type i = 0; i < 3; i++) {
147 // vertex ID
148 ✗ if (i == 0)
149 ✗ v1 = data[0];
150 else
151 ✗ v1 = data[(Triangle::index_type)t + i];
152
153 ✗ tri[i] = (Triangle::index_type)(atoi(v1) - 1);
154 }
155 }
156
1/2
✓ Branch 1 taken 23312 times.
✗ Branch 2 not taken.
 23312 triangles.push_back(tri);
157 }
158 }
159 }
160 }
161 }
162
163 ✗ void saveOBJFile(const char* filename, std::vector<Vec3s>& points,
164 std::vector<Triangle>& triangles) {
165 ✗ std::ofstream os(filename);
166 ✗ if (!os) {
167 ✗ std::cerr << "file not exist" << std::endl;
168 ✗ return;
169 }
170
171 ✗ for (std::size_t i = 0; i < points.size(); ++i) {
172 ✗ os << "v " << points[i][0] << " " << points[i][1] << " " << points[i][2]
173 ✗ << std::endl;
174 }
175
176 ✗ for (std::size_t i = 0; i < triangles.size(); ++i) {
177 ✗ os << "f " << triangles[i][0] + 1 << " " << triangles[i][1] + 1 << " "
178 ✗ << triangles[i][2] + 1 << std::endl;
179 }
180
181 ✗ os.close();
182 }
183
184 #ifdef COAL_HAS_OCTOMAP
185 2 OcTree loadOctreeFile(const std::string& filename, const Scalar& resolution) {
186
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
 2 octomap::OcTreePtr_t octree(new octomap::OcTree(filename));
187
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 2 times.
 2 if (octree->getResolution() != resolution) {
188 ✗ std::ostringstream oss;
189 ✗ oss << "Resolution of the OcTree is " << octree->getResolution()
190 ✗ << " and not " << resolution;
191 ✗ throw std::invalid_argument(oss.str());
192 }
193
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 return coal::OcTree(octree);
194 2 }
195 #endif
196
197 60677 void eulerToMatrix(Scalar a, Scalar b, Scalar c, Matrix3s& R) {
198 60677 Scalar c1 = cos(a);
199 60677 Scalar c2 = cos(b);
200 60677 Scalar c3 = cos(c);
201 60677 Scalar s1 = sin(a);
202 60677 Scalar s2 = sin(b);
203 60677 Scalar s3 = sin(c);
204
205
5/10
✓ Branch 1 taken 60677 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60677 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60677 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 60677 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 60677 times.
✗ Branch 14 not taken.
 60677 R << c1 * c2, -c2 * s1, s2, c3 * s1 + c1 * s2 * s3, c1 * c3 - s1 * s2 * s3,
206
4/8
✓ Branch 1 taken 60677 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60677 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60677 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 60677 times.
✗ Branch 11 not taken.
 60677 -c2 * s3, s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3, c2 * c3;
207 60677 }
208
209 1129 void generateRandomTransform(Scalar extents[6], Transform3s& transform) {
210 1129 Scalar x = rand_interval(extents[0], extents[3]);
211 1129 Scalar y = rand_interval(extents[1], extents[4]);
212 1129 Scalar z = rand_interval(extents[2], extents[5]);
213
214 1129 const Scalar pi = Scalar(3.1415926);
215 1129 Scalar a = rand_interval(0, 2 * pi);
216 1129 Scalar b = rand_interval(0, 2 * pi);
217 1129 Scalar c = rand_interval(0, 2 * pi);
218
219
1/2
✓ Branch 1 taken 1129 times.
✗ Branch 2 not taken.
 1129 Matrix3s R;
220
1/2
✓ Branch 1 taken 1129 times.
✗ Branch 2 not taken.
 1129 eulerToMatrix(a, b, c, R);
221
1/2
✓ Branch 1 taken 1129 times.
✗ Branch 2 not taken.
 1129 Vec3s T(x, y, z);
222
1/2
✓ Branch 1 taken 1129 times.
✗ Branch 2 not taken.
 1129 transform.setTransform(R, T);
223 1129 }
224
225 995 void generateRandomTransforms(Scalar extents[6],
226 std::vector<Transform3s>& transforms,
227 std::size_t n) {
228 995 transforms.resize(n);
229
2/2
✓ Branch 0 taken 59544 times.
✓ Branch 1 taken 995 times.
 60539 for (std::size_t i = 0; i < n; ++i) {
230 59544 Scalar x = rand_interval(extents[0], extents[3]);
231 59544 Scalar y = rand_interval(extents[1], extents[4]);
232 59544 Scalar z = rand_interval(extents[2], extents[5]);
233
234 59544 const Scalar pi = Scalar(3.1415926);
235 59544 Scalar a = rand_interval(0, 2 * pi);
236 59544 Scalar b = rand_interval(0, 2 * pi);
237 59544 Scalar c = rand_interval(0, 2 * pi);
238
239 {
240
1/2
✓ Branch 1 taken 59544 times.
✗ Branch 2 not taken.
 59544 Matrix3s R;
241
1/2
✓ Branch 1 taken 59544 times.
✗ Branch 2 not taken.
 59544 eulerToMatrix(a, b, c, R);
242
1/2
✓ Branch 1 taken 59544 times.
✗ Branch 2 not taken.
 59544 Vec3s T(x, y, z);
243
1/2
✓ Branch 2 taken 59544 times.
✗ Branch 3 not taken.
 59544 transforms[i].setTransform(R, T);
244 }
245 }
246 995 }
247
248 1 void generateRandomTransforms(Scalar extents[6], Scalar delta_trans[3],
249 Scalar delta_rot,
250 std::vector<Transform3s>& transforms,
251 std::vector<Transform3s>& transforms2,
252 std::size_t n) {
253 1 transforms.resize(n);
254 1 transforms2.resize(n);
255
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
 3 for (std::size_t i = 0; i < n; ++i) {
256 2 Scalar x = rand_interval(extents[0], extents[3]);
257 2 Scalar y = rand_interval(extents[1], extents[4]);
258 2 Scalar z = rand_interval(extents[2], extents[5]);
259
260 2 const Scalar pi = Scalar(3.1415926);
261 2 Scalar a = rand_interval(0, 2 * pi);
262 2 Scalar b = rand_interval(0, 2 * pi);
263 2 Scalar c = rand_interval(0, 2 * pi);
264
265 {
266
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Matrix3s R;
267
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 eulerToMatrix(a, b, c, R);
268
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Vec3s T(x, y, z);
269
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 transforms[i].setTransform(R, T);
270 }
271
272 2 Scalar deltax = rand_interval(-delta_trans[0], delta_trans[0]);
273 2 Scalar deltay = rand_interval(-delta_trans[1], delta_trans[1]);
274 2 Scalar deltaz = rand_interval(-delta_trans[2], delta_trans[2]);
275
276 2 Scalar deltaa = rand_interval(-delta_rot, delta_rot);
277 2 Scalar deltab = rand_interval(-delta_rot, delta_rot);
278 2 Scalar deltac = rand_interval(-delta_rot, delta_rot);
279
280 {
281
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Matrix3s R;
282
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 eulerToMatrix(a + deltaa, b + deltab, c + deltac, R);
283
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Vec3s T(x + deltax, y + deltay, z + deltaz);
284
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 transforms2[i].setTransform(R, T);
285 }
286 }
287 1 }
288
289 1222279 bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2,
290 void* cdata_) {
291 1222279 CollisionData* cdata = static_cast<CollisionData*>(cdata_);
292 1222279 const CollisionRequest& request = cdata->request;
293 1222279 CollisionResult& result = cdata->result;
294
295
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1222279 times.
 1222279 if (cdata->done) return true;
296
297 1222279 collide(o1, o2, request, result);
298
299
4/4
✓ Branch 1 taken 922 times.
✓ Branch 2 taken 1221357 times.
✓ Branch 3 taken 780 times.
✓ Branch 4 taken 1221499 times.
 1223201 if ((result.isCollision()) &&
300
2/2
✓ Branch 1 taken 780 times.
✓ Branch 2 taken 142 times.
 922 (result.numContacts() >= request.num_max_contacts))
301 780 cdata->done = true;
302
303 1222279 return cdata->done;
304 }
305
306 33012 bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2,
307 void* cdata_, Scalar& dist) {
308 33012 DistanceData* cdata = static_cast<DistanceData*>(cdata_);
309 33012 const DistanceRequest& request = cdata->request;
310 33012 DistanceResult& result = cdata->result;
311
312
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 33012 times.
 33012 if (cdata->done) {
313 ✗ dist = result.min_distance;
314 ✗ return true;
315 }
316
317 33012 distance(o1, o2, request, result);
318
319 33012 dist = result.min_distance;
320
321
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 33012 times.
 33012 if (dist <= 0) return true; // in collision or in touch
322
323 33012 return cdata->done;
324 }
325
326 4 std::string getNodeTypeName(NODE_TYPE node_type) {
327
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 4 if (node_type == BV_UNKNOWN)
328 ✗ return std::string("BV_UNKNOWN");
329
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 4 else if (node_type == BV_AABB)
330 ✗ return std::string("BV_AABB");
331
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 3 times.
 4 else if (node_type == BV_OBB)
332
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 return std::string("BV_OBB");
333
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 2 times.
 3 else if (node_type == BV_RSS)
334
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 return std::string("BV_RSS");
335
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
 2 else if (node_type == BV_kIOS)
336
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 return std::string("BV_kIOS");
337
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 else if (node_type == BV_OBBRSS)
338
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 return std::string("BV_OBBRSS");
339 ✗ else if (node_type == BV_KDOP16)
340 ✗ return std::string("BV_KDOP16");
341 ✗ else if (node_type == BV_KDOP18)
342 ✗ return std::string("BV_KDOP18");
343 ✗ else if (node_type == BV_KDOP24)
344 ✗ return std::string("BV_KDOP24");
345 ✗ else if (node_type == GEOM_BOX)
346 ✗ return std::string("GEOM_BOX");
347 ✗ else if (node_type == GEOM_SPHERE)
348 ✗ return std::string("GEOM_SPHERE");
349 ✗ else if (node_type == GEOM_CAPSULE)
350 ✗ return std::string("GEOM_CAPSULE");
351 ✗ else if (node_type == GEOM_CONE)
352 ✗ return std::string("GEOM_CONE");
353 ✗ else if (node_type == GEOM_CYLINDER)
354 ✗ return std::string("GEOM_CYLINDER");
355 ✗ else if (node_type == GEOM_CONVEX)
356 ✗ return std::string("GEOM_CONVEX");
357 ✗ else if (node_type == GEOM_PLANE)
358 ✗ return std::string("GEOM_PLANE");
359 ✗ else if (node_type == GEOM_HALFSPACE)
360 ✗ return std::string("GEOM_HALFSPACE");
361 ✗ else if (node_type == GEOM_TRIANGLE)
362 ✗ return std::string("GEOM_TRIANGLE");
363 ✗ else if (node_type == GEOM_OCTREE)
364 ✗ return std::string("GEOM_OCTREE");
365 else
366 ✗ return std::string("invalid");
367 }
368
369 7 Quats makeQuat(Scalar w, Scalar x, Scalar y, Scalar z) {
370 7 Quats q;
371 7 q.w() = w;
372 7 q.x() = x;
373 7 q.y() = y;
374 7 q.z() = z;
375 7 return q;
376 }
377
378 ✗ std::ostream& operator<<(std::ostream& os, const Transform3s& tf) {
379 ✗ return os << "[ " << tf.getTranslation().format(vfmt) << ", "
380 ✗ << tf.getQuatRotation().coeffs().format(vfmt) << " ]";
381 }
382
383 6 std::size_t getNbRun(const int& argc, char const* const* argv,
384 std::size_t defaultValue) {
385
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 6 times.
 12 for (int i = 0; i < argc; ++i)
386
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 6 times.
 6 if (strcmp(argv[i], "--nb-run") == 0)
387 ✗ if (i + 1 != argc) return (std::size_t)strtol(argv[i + 1], NULL, 10);
388 6 return defaultValue;
389 }
390
391 69 void generateEnvironments(std::vector<CollisionObject*>& env, Scalar env_scale,
392 std::size_t n) {
393 69 Scalar extents[] = {-env_scale, env_scale, -env_scale,
394 69 env_scale, -env_scale, env_scale};
395
1/2
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
 69 std::vector<Transform3s> transforms(n);
396
397
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 generateRandomTransforms(extents, transforms, n);
398
2/2
✓ Branch 0 taken 4454 times.
✓ Branch 1 taken 69 times.
 4523 for (std::size_t i = 0; i < n; ++i) {
399
2/4
✓ Branch 1 taken 4454 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4454 times.
✗ Branch 5 not taken.
 4454 Box* box = new Box(5, 10, 20);
400 4454 env.push_back(
401
4/8
✓ Branch 1 taken 4454 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4454 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4454 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4454 times.
✗ Branch 12 not taken.
 4454 new CollisionObject(shared_ptr<CollisionGeometry>(box), transforms[i]));
402
1/2
✓ Branch 4 taken 4454 times.
✗ Branch 5 not taken.
 4454 env.back()->collisionGeometry()->computeLocalAABB();
403 }
404
405
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 generateRandomTransforms(extents, transforms, n);
406
2/2
✓ Branch 0 taken 4454 times.
✓ Branch 1 taken 69 times.
 4523 for (std::size_t i = 0; i < n; ++i) {
407
2/4
✓ Branch 1 taken 4454 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4454 times.
✗ Branch 5 not taken.
 4454 Sphere* sphere = new Sphere(30);
408
2/4
✓ Branch 1 taken 4454 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4454 times.
✗ Branch 5 not taken.
 8908 env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(sphere),
409
2/4
✓ Branch 2 taken 4454 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4454 times.
✗ Branch 6 not taken.
 4454 transforms[i]));
410
1/2
✓ Branch 4 taken 4454 times.
✗ Branch 5 not taken.
 4454 env.back()->collisionGeometry()->computeLocalAABB();
411 }
412
413
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 generateRandomTransforms(extents, transforms, n);
414
2/2
✓ Branch 0 taken 4454 times.
✓ Branch 1 taken 69 times.
 4523 for (std::size_t i = 0; i < n; ++i) {
415
2/4
✓ Branch 1 taken 4454 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4454 times.
✗ Branch 5 not taken.
 4454 Cylinder* cylinder = new Cylinder(10, 40);
416
2/4
✓ Branch 1 taken 4454 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4454 times.
✗ Branch 5 not taken.
 8908 env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(cylinder),
417
2/4
✓ Branch 2 taken 4454 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4454 times.
✗ Branch 6 not taken.
 4454 transforms[i]));
418
1/2
✓ Branch 4 taken 4454 times.
✗ Branch 5 not taken.
 4454 env.back()->collisionGeometry()->computeLocalAABB();
419 }
420 69 }
421
422 44 void generateEnvironmentsMesh(std::vector<CollisionObject*>& env,
423 Scalar env_scale, std::size_t n) {
424 44 Scalar extents[] = {-env_scale, env_scale, -env_scale,
425 44 env_scale, -env_scale, env_scale};
426 44 std::vector<Transform3s> transforms;
427
428
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 generateRandomTransforms(extents, transforms, n);
429
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 Box box(5, 10, 20);
430
2/2
✓ Branch 0 taken 367 times.
✓ Branch 1 taken 44 times.
 411 for (std::size_t i = 0; i < n; ++i) {
431
2/4
✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
 367 BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
432
2/4
✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
 367 generateBVHModel(*model, box, Transform3s::Identity());
433
2/4
✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
 734 env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(model),
434
2/4
✓ Branch 2 taken 367 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 367 times.
✗ Branch 6 not taken.
 367 transforms[i]));
435
1/2
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
 367 env.back()->collisionGeometry()->computeLocalAABB();
436 }
437
438
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 generateRandomTransforms(extents, transforms, n);
439
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 Sphere sphere(30);
440
2/2
✓ Branch 0 taken 367 times.
✓ Branch 1 taken 44 times.
 411 for (std::size_t i = 0; i < n; ++i) {
441
2/4
✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
 367 BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
442
2/4
✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
 367 generateBVHModel(*model, sphere, Transform3s::Identity(), 16, 16);
443
2/4
✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
 734 env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(model),
444
2/4
✓ Branch 2 taken 367 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 367 times.
✗ Branch 6 not taken.
 367 transforms[i]));
445
1/2
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
 367 env.back()->collisionGeometry()->computeLocalAABB();
446 }
447
448
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 generateRandomTransforms(extents, transforms, n);
449
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 Cylinder cylinder(10, 40);
450
2/2
✓ Branch 0 taken 367 times.
✓ Branch 1 taken 44 times.
 411 for (std::size_t i = 0; i < n; ++i) {
451
2/4
✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
 367 BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
452
2/4
✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
 367 generateBVHModel(*model, cylinder, Transform3s::Identity(), 16, 16);
453
2/4
✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
 734 env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(model),
454
2/4
✓ Branch 2 taken 367 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 367 times.
✗ Branch 6 not taken.
 367 transforms[i]));
455
1/2
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
 367 env.back()->collisionGeometry()->computeLocalAABB();
456 }
457 44 }
458
459 4 Convex<Quadrilateral> buildBox(Scalar l, Scalar w, Scalar d) {
460 std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>(
461 ✗ {Vec3s(l, w, d), Vec3s(l, w, -d), Vec3s(l, -w, d), Vec3s(l, -w, -d),
462 ✗ Vec3s(-l, w, d), Vec3s(-l, w, -d), Vec3s(-l, -w, d),
463
11/22
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 4 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 4 times.
✗ Branch 23 not taken.
✓ Branch 26 taken 4 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 4 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 4 times.
✗ Branch 33 not taken.
 4 Vec3s(-l, -w, -d)}));
464
465 std::shared_ptr<std::vector<Quadrilateral>> polygons(
466
3/6
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
 4 new std::vector<Quadrilateral>(6));
467 4 (*polygons)[0].set(0, 2, 3, 1); // x+ side
468 4 (*polygons)[1].set(2, 6, 7, 3); // y- side
469 4 (*polygons)[2].set(4, 5, 7, 6); // x- side
470 4 (*polygons)[3].set(0, 1, 5, 4); // y+ side
471 4 (*polygons)[4].set(1, 3, 7, 5); // z- side
472 4 (*polygons)[5].set(0, 2, 6, 4); // z+ side
473
474 return Convex<Quadrilateral>(pts, // points
475 8, // num points
476 polygons,
477 6 // number of polygons
478
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 8 );
479 4 }
480
481 /// Takes a point and projects it onto the surface of the unit sphere
482 840 void toSphere(Vec3s& point) {
483
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 840 times.
 840 assert(point.norm() > 1e-8);
484
1/2
✓ Branch 2 taken 840 times.
✗ Branch 3 not taken.
 840 point /= point.norm();
485 840 }
486
487 /// Takes a point, projects it on the unit sphere and applies an ellipsoid
488 /// transformation to it. A point x belongs to the surface of an ellipsoid if
489 /// x^T * A * x = 1, where A = diag(1/r**2) with r being the radii of the
490 /// ellipsoid. Thus, the point y = A^(1/2) * x belongs to the unit sphere if y *
491 /// y = 1. Therefore, the tranformation which brings y to x is A^(-1/2) =
492 /// diag(r).
493 840 void toEllipsoid(Vec3s& point, const Ellipsoid& ellipsoid) {
494 840 toSphere(point);
495 840 point[0] *= ellipsoid.radii[0];
496 840 point[1] *= ellipsoid.radii[1];
497 840 point[2] *= ellipsoid.radii[2];
498 840 }
499
500 70 Convex<Triangle> constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid) {
501 70 Scalar PHI = (1 + std::sqrt(Scalar(5))) / 2;
502
503 // vertices
504 std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>({
505 Vec3s(-1, PHI, 0),
506 Vec3s(1, PHI, 0),
507 ✗ Vec3s(-1, -PHI, 0),
508 ✗ Vec3s(1, -PHI, 0),
509
510 Vec3s(0, -1, PHI),
511 Vec3s(0, 1, PHI),
512 ✗ Vec3s(0, -1, -PHI),
513 ✗ Vec3s(0, 1, -PHI),
514
515 Vec3s(PHI, 0, -1),
516 Vec3s(PHI, 0, 1),
517 ✗ Vec3s(-PHI, 0, -1),
518 ✗ Vec3s(-PHI, 0, 1),
519
15/30
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 70 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 70 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 70 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 70 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 70 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 70 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 70 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 70 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 70 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 70 times.
✗ Branch 35 not taken.
✓ Branch 38 taken 70 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 70 times.
✗ Branch 42 not taken.
✓ Branch 44 taken 70 times.
✗ Branch 45 not taken.
 70 }));
520
521 70 std::vector<Vec3s>& pts_ = *pts;
522
2/2
✓ Branch 0 taken 840 times.
✓ Branch 1 taken 70 times.
 910 for (size_t i = 0; i < 12; ++i) {
523
1/2
✓ Branch 2 taken 840 times.
✗ Branch 3 not taken.
 840 toEllipsoid(pts_[i], ellipsoid);
524 }
525
526 // faces
527
3/6
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 70 times.
✗ Branch 9 not taken.
 70 std::shared_ptr<std::vector<Triangle>> tris(new std::vector<Triangle>(20));
528 70 (*tris)[0].set(0, 11, 5);
529 70 (*tris)[1].set(0, 5, 1);
530 70 (*tris)[2].set(0, 1, 7);
531 70 (*tris)[3].set(0, 7, 10);
532 70 (*tris)[4].set(0, 10, 11);
533
534 70 (*tris)[5].set(1, 5, 9);
535 70 (*tris)[6].set(5, 11, 4);
536 70 (*tris)[7].set(11, 10, 2);
537 70 (*tris)[8].set(10, 7, 6);
538 70 (*tris)[9].set(7, 1, 8);
539
540 70 (*tris)[10].set(3, 9, 4);
541 70 (*tris)[11].set(3, 4, 2);
542 70 (*tris)[12].set(3, 2, 6);
543 70 (*tris)[13].set(3, 6, 8);
544 70 (*tris)[14].set(3, 8, 9);
545
546 70 (*tris)[15].set(4, 9, 5);
547 70 (*tris)[16].set(2, 4, 11);
548 70 (*tris)[17].set(6, 2, 10);
549 70 (*tris)[18].set(8, 6, 7);
550 70 (*tris)[19].set(9, 8, 1);
551 return Convex<Triangle>(pts, // points
552 12, // num_points
553 tris, // triangles
554 20 // number of triangles
555
1/2
✓ Branch 3 taken 70 times.
✗ Branch 4 not taken.
 140 );
556 70 }
557
558 21 Box makeRandomBox(Scalar min_size, Scalar max_size) {
559
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 return Box(Vec3s(rand_interval(min_size, max_size),
560 21 rand_interval(min_size, max_size),
561
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 63 rand_interval(min_size, max_size)));
562 }
563
564 18 Sphere makeRandomSphere(Scalar min_size, Scalar max_size) {
565 18 return Sphere(rand_interval(min_size, max_size));
566 }
567
568 44 Ellipsoid makeRandomEllipsoid(Scalar min_size, Scalar max_size) {
569
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 return Ellipsoid(Vec3s(rand_interval(min_size, max_size),
570 44 rand_interval(min_size, max_size),
571
1/2
✓ Branch 3 taken 44 times.
✗ Branch 4 not taken.
 132 rand_interval(min_size, max_size)));
572 }
573
574 21 Capsule makeRandomCapsule(std::array<Scalar, 2> min_size,
575 std::array<Scalar, 2> max_size) {
576 21 return Capsule(rand_interval(min_size[0], max_size[0]),
577 42 rand_interval(min_size[1], max_size[1]));
578 }
579
580 22 Cone makeRandomCone(std::array<Scalar, 2> min_size,
581 std::array<Scalar, 2> max_size) {
582 22 return Cone(rand_interval(min_size[0], max_size[0]),
583 44 rand_interval(min_size[1], max_size[1]));
584 }
585
586 20 Cylinder makeRandomCylinder(std::array<Scalar, 2> min_size,
587 std::array<Scalar, 2> max_size) {
588 20 return Cylinder(rand_interval(min_size[0], max_size[0]),
589 40 rand_interval(min_size[1], max_size[1]));
590 }
591
592 21 Convex<Triangle> makeRandomConvex(Scalar min_size, Scalar max_size) {
593
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 Ellipsoid ellipsoid = makeRandomEllipsoid(min_size, max_size);
594
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 42 return constructPolytopeFromEllipsoid(ellipsoid);
595 21 }
596
597 14 Plane makeRandomPlane(Scalar min_size, Scalar max_size) {
598
2/4
✓ Branch 3 taken 14 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 14 times.
✗ Branch 7 not taken.
 28 return Plane(Vec3s::Random().normalized(), rand_interval(min_size, max_size));
599 }
600
601 14 Halfspace makeRandomHalfspace(Scalar min_size, Scalar max_size) {
602
1/2
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
 28 return Halfspace(Vec3s::Random().normalized(),
603
1/2
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
 42 rand_interval(min_size, max_size));
604 }
605
606 154 std::shared_ptr<ShapeBase> makeRandomGeometry(NODE_TYPE node_type) {
607
9/11
✗ Branch 0 not taken.
✓ Branch 1 taken 18 times.
✓ Branch 2 taken 18 times.
✓ Branch 3 taken 18 times.
✓ Branch 4 taken 18 times.
✓ Branch 5 taken 18 times.
✓ Branch 6 taken 18 times.
✓ Branch 7 taken 18 times.
✓ Branch 8 taken 14 times.
✓ Branch 9 taken 14 times.
✗ Branch 10 not taken.
 154 switch (node_type) {
608 ✗ case GEOM_TRIANGLE:
609 ✗ COAL_THROW_PRETTY(std::string(COAL_PRETTY_FUNCTION) + " for " +
610 std::string(get_node_type_name(node_type)) +
611 " is not yet implemented.",
612 std::invalid_argument);
613 break;
614 18 case GEOM_BOX:
615
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 return std::make_shared<Box>(makeRandomBox(Scalar(0.1), Scalar(1)));
616 break;
617 18 case GEOM_SPHERE:
618
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 return std::make_shared<Sphere>(makeRandomSphere(Scalar(0.1), Scalar(1)));
619 break;
620 18 case GEOM_ELLIPSOID:
621
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 return std::make_shared<Ellipsoid>(
622 54 makeRandomEllipsoid(Scalar(0.1), Scalar(1)));
623 break;
624 18 case GEOM_CAPSULE:
625
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 36 return std::make_shared<Capsule>(makeRandomCapsule(
626 18 {Scalar(0.1), Scalar(0.2)}, {Scalar(0.8), Scalar(1)}));
627 break;
628 18 case GEOM_CONE:
629
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 36 return std::make_shared<Cone>(makeRandomCone({Scalar(0.1), Scalar(0.2)},
630 18 {Scalar(0.8), Scalar(1.0)}));
631 break;
632 18 case GEOM_CYLINDER:
633
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 36 return std::make_shared<Cylinder>(makeRandomCylinder(
634 18 {Scalar(0.1), Scalar(0.2)}, {Scalar(0.8), Scalar(1.0)}));
635 break;
636 18 case GEOM_CONVEX:
637
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 return std::make_shared<Convex<Triangle>>(
638 54 makeRandomConvex(Scalar(0.1), Scalar(1)));
639 break;
640 14 case GEOM_PLANE:
641
1/2
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
 14 return std::make_shared<Plane>(makeRandomPlane(Scalar(0.1), Scalar(1)));
642 break;
643 14 case GEOM_HALFSPACE:
644
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 return std::make_shared<Halfspace>(
645 42 makeRandomHalfspace(Scalar(0.1), Scalar(1)));
646 break;
647 ✗ default:
648 ✗ COAL_THROW_PRETTY(std::string(get_node_type_name(node_type)) +
649 " is not a primitive shape.",
650 std::invalid_argument);
651 break;
652 }
653 }
654
655 } // namespace coal
656