GCC Code Coverage Report

Directory: ./
File: test/utility.cpp
Date: 2025-05-02 10:16:21
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<Triangle32>& 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 Triangle32 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] = (Triangle32::IndexType)(atoi(data[0]) - 1);
142 23312 tri[1] = (Triangle32::IndexType)(atoi(data[1]) - 1);
143 23312 tri[2] = (Triangle32::IndexType)(atoi(data[2]) - 1);
144 } else {
145 const char* v1;
146 ✗ for (Triangle32::IndexType i = 0; i < 3; i++) {
147 // vertex ID
148 ✗ if (i == 0)
149 ✗ v1 = data[0];
150 else
151 ✗ v1 = data[(Triangle32::IndexType)t + i];
152
153 ✗ tri[i] = (Triangle32::IndexType)(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<Triangle32>& 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 1222282 bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2,
290 void* cdata_) {
291 1222282 CollisionData* cdata = static_cast<CollisionData*>(cdata_);
292 1222282 const CollisionRequest& request = cdata->request;
293 1222282 CollisionResult& result = cdata->result;
294
295
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1222282 times.
 1222282 if (cdata->done) return true;
296
297 1222282 collide(o1, o2, request, result);
298
299
4/4
✓ Branch 1 taken 922 times.
✓ Branch 2 taken 1221360 times.
✓ Branch 3 taken 780 times.
✓ Branch 4 taken 1221502 times.
 1223204 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 1222282 return cdata->done;
304 }
305
306 32983 bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2,
307 void* cdata_, Scalar& dist) {
308 32983 DistanceData* cdata = static_cast<DistanceData*>(cdata_);
309 32983 const DistanceRequest& request = cdata->request;
310 32983 DistanceResult& result = cdata->result;
311
312
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 32983 times.
 32983 if (cdata->done) {
313 ✗ dist = result.min_distance;
314 ✗ return true;
315 }
316
317 32983 distance(o1, o2, request, result);
318
319 32983 dist = result.min_distance;
320
321
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 32983 times.
 32983 if (dist <= 0) return true; // in collision or in touch
322
323 32983 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 ConvexTpl<Quadrilateral32> 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<Quadrilateral32>> 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<Quadrilateral32>(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 ConvexTpl<Quadrilateral32>(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 ConvexTpl<Triangle32> constructPolytopeFromEllipsoid(
501 const Ellipsoid& ellipsoid) {
502 70 Scalar PHI = (1 + std::sqrt(Scalar(5))) / 2;
503
504 // vertices
505 std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>({
506 Vec3s(-1, PHI, 0),
507 Vec3s(1, PHI, 0),
508 ✗ Vec3s(-1, -PHI, 0),
509 ✗ Vec3s(1, -PHI, 0),
510
511 Vec3s(0, -1, PHI),
512 Vec3s(0, 1, PHI),
513 ✗ Vec3s(0, -1, -PHI),
514 ✗ Vec3s(0, 1, -PHI),
515
516 Vec3s(PHI, 0, -1),
517 Vec3s(PHI, 0, 1),
518 ✗ Vec3s(-PHI, 0, -1),
519 ✗ Vec3s(-PHI, 0, 1),
520
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 }));
521
522 70 std::vector<Vec3s>& pts_ = *pts;
523
2/2
✓ Branch 0 taken 840 times.
✓ Branch 1 taken 70 times.
 910 for (size_t i = 0; i < 12; ++i) {
524
1/2
✓ Branch 2 taken 840 times.
✗ Branch 3 not taken.
 840 toEllipsoid(pts_[i], ellipsoid);
525 }
526
527 // faces
528 std::shared_ptr<std::vector<Triangle32>> tris(
529
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 new std::vector<Triangle32>(20));
530 70 (*tris)[0].set(0, 11, 5);
531 70 (*tris)[1].set(0, 5, 1);
532 70 (*tris)[2].set(0, 1, 7);
533 70 (*tris)[3].set(0, 7, 10);
534 70 (*tris)[4].set(0, 10, 11);
535
536 70 (*tris)[5].set(1, 5, 9);
537 70 (*tris)[6].set(5, 11, 4);
538 70 (*tris)[7].set(11, 10, 2);
539 70 (*tris)[8].set(10, 7, 6);
540 70 (*tris)[9].set(7, 1, 8);
541
542 70 (*tris)[10].set(3, 9, 4);
543 70 (*tris)[11].set(3, 4, 2);
544 70 (*tris)[12].set(3, 2, 6);
545 70 (*tris)[13].set(3, 6, 8);
546 70 (*tris)[14].set(3, 8, 9);
547
548 70 (*tris)[15].set(4, 9, 5);
549 70 (*tris)[16].set(2, 4, 11);
550 70 (*tris)[17].set(6, 2, 10);
551 70 (*tris)[18].set(8, 6, 7);
552 70 (*tris)[19].set(9, 8, 1);
553 return ConvexTpl<Triangle32>(pts, // points
554 12, // num_points
555 tris, // triangles
556 20 // number of triangles
557
1/2
✓ Branch 3 taken 70 times.
✗ Branch 4 not taken.
 140 );
558 70 }
559
560 21 Box makeRandomBox(Scalar min_size, Scalar max_size) {
561
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 return Box(Vec3s(rand_interval(min_size, max_size),
562 21 rand_interval(min_size, max_size),
563
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 63 rand_interval(min_size, max_size)));
564 }
565
566 18 Sphere makeRandomSphere(Scalar min_size, Scalar max_size) {
567 18 return Sphere(rand_interval(min_size, max_size));
568 }
569
570 44 Ellipsoid makeRandomEllipsoid(Scalar min_size, Scalar max_size) {
571
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 return Ellipsoid(Vec3s(rand_interval(min_size, max_size),
572 44 rand_interval(min_size, max_size),
573
1/2
✓ Branch 3 taken 44 times.
✗ Branch 4 not taken.
 132 rand_interval(min_size, max_size)));
574 }
575
576 21 Capsule makeRandomCapsule(std::array<Scalar, 2> min_size,
577 std::array<Scalar, 2> max_size) {
578 21 return Capsule(rand_interval(min_size[0], max_size[0]),
579 42 rand_interval(min_size[1], max_size[1]));
580 }
581
582 22 Cone makeRandomCone(std::array<Scalar, 2> min_size,
583 std::array<Scalar, 2> max_size) {
584 22 return Cone(rand_interval(min_size[0], max_size[0]),
585 44 rand_interval(min_size[1], max_size[1]));
586 }
587
588 20 Cylinder makeRandomCylinder(std::array<Scalar, 2> min_size,
589 std::array<Scalar, 2> max_size) {
590 20 return Cylinder(rand_interval(min_size[0], max_size[0]),
591 40 rand_interval(min_size[1], max_size[1]));
592 }
593
594 21 ConvexTpl<Triangle32> makeRandomConvex(Scalar min_size, Scalar max_size) {
595
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 Ellipsoid ellipsoid = makeRandomEllipsoid(min_size, max_size);
596
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 42 return constructPolytopeFromEllipsoid(ellipsoid);
597 21 }
598
599 14 Plane makeRandomPlane(Scalar min_size, Scalar max_size) {
600
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));
601 }
602
603 14 Halfspace makeRandomHalfspace(Scalar min_size, Scalar max_size) {
604
1/2
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
 28 return Halfspace(Vec3s::Random().normalized(),
605
1/2
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
 42 rand_interval(min_size, max_size));
606 }
607
608 154 std::shared_ptr<ShapeBase> makeRandomGeometry(NODE_TYPE node_type) {
609
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) {
610 ✗ case GEOM_TRIANGLE:
611 ✗ COAL_THROW_PRETTY(std::string(COAL_PRETTY_FUNCTION) + " for " +
612 std::string(get_node_type_name(node_type)) +
613 " is not yet implemented.",
614 std::invalid_argument);
615 break;
616 18 case GEOM_BOX:
617
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 return std::make_shared<Box>(makeRandomBox(Scalar(0.1), Scalar(1)));
618 break;
619 18 case GEOM_SPHERE:
620
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 return std::make_shared<Sphere>(makeRandomSphere(Scalar(0.1), Scalar(1)));
621 break;
622 18 case GEOM_ELLIPSOID:
623
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 return std::make_shared<Ellipsoid>(
624 54 makeRandomEllipsoid(Scalar(0.1), Scalar(1)));
625 break;
626 18 case GEOM_CAPSULE:
627
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 36 return std::make_shared<Capsule>(makeRandomCapsule(
628 18 {Scalar(0.1), Scalar(0.2)}, {Scalar(0.8), Scalar(1)}));
629 break;
630 18 case GEOM_CONE:
631
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 36 return std::make_shared<Cone>(makeRandomCone({Scalar(0.1), Scalar(0.2)},
632 18 {Scalar(0.8), Scalar(1.0)}));
633 break;
634 18 case GEOM_CYLINDER:
635
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 36 return std::make_shared<Cylinder>(makeRandomCylinder(
636 18 {Scalar(0.1), Scalar(0.2)}, {Scalar(0.8), Scalar(1.0)}));
637 break;
638 18 case GEOM_CONVEX:
639
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 return std::make_shared<ConvexTpl<Triangle32>>(
640 54 makeRandomConvex(Scalar(0.1), Scalar(1)));
641 break;
642 14 case GEOM_PLANE:
643
1/2
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
 14 return std::make_shared<Plane>(makeRandomPlane(Scalar(0.1), Scalar(1)));
644 break;
645 14 case GEOM_HALFSPACE:
646
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 return std::make_shared<Halfspace>(
647 42 makeRandomHalfspace(Scalar(0.1), Scalar(1)));
648 break;
649 ✗ default:
650 ✗ COAL_THROW_PRETTY(std::string(get_node_type_name(node_type)) +
651 " is not a primitive shape.",
652 std::invalid_argument);
653 break;
654 }
655 }
656
657 } // namespace coal
658