GCC Code Coverage Report

Directory: ./
File: test/utility.cpp
Date: 2025-05-09 13:08:51
Exec Total Coverage
Lines: 320 399 80.2%
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 CoalScalar rand_interval(CoalScalar rmin, CoalScalar rmax) {
93 364429 CoalScalar t = rand() / ((CoalScalar)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 CoalScalar x = (CoalScalar)atof(strtok(NULL, "\t "));
125 69936 CoalScalar y = (CoalScalar)atof(strtok(NULL, "\t "));
126 69936 CoalScalar z = (CoalScalar)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,
186 const CoalScalar& resolution) {
187
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));
188
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) {
189 ✗ std::ostringstream oss;
190 ✗ oss << "Resolution of the OcTree is " << octree->getResolution()
191 ✗ << " and not " << resolution;
192 ✗ throw std::invalid_argument(oss.str());
193 }
194
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 return coal::OcTree(octree);
195 2 }
196 #endif
197
198 60677 void eulerToMatrix(CoalScalar a, CoalScalar b, CoalScalar c, Matrix3s& R) {
199 60677 CoalScalar c1 = cos(a);
200 60677 CoalScalar c2 = cos(b);
201 60677 CoalScalar c3 = cos(c);
202 60677 CoalScalar s1 = sin(a);
203 60677 CoalScalar s2 = sin(b);
204 60677 CoalScalar s3 = sin(c);
205
206
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,
207
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;
208 60677 }
209
210 1129 void generateRandomTransform(CoalScalar extents[6], Transform3s& transform) {
211 1129 CoalScalar x = rand_interval(extents[0], extents[3]);
212 1129 CoalScalar y = rand_interval(extents[1], extents[4]);
213 1129 CoalScalar z = rand_interval(extents[2], extents[5]);
214
215 1129 const CoalScalar pi = 3.1415926;
216 1129 CoalScalar a = rand_interval(0, 2 * pi);
217 1129 CoalScalar b = rand_interval(0, 2 * pi);
218 1129 CoalScalar c = rand_interval(0, 2 * pi);
219
220
1/2
✓ Branch 1 taken 1129 times.
✗ Branch 2 not taken.
 1129 Matrix3s R;
221
1/2
✓ Branch 1 taken 1129 times.
✗ Branch 2 not taken.
 1129 eulerToMatrix(a, b, c, R);
222
1/2
✓ Branch 1 taken 1129 times.
✗ Branch 2 not taken.
 1129 Vec3s T(x, y, z);
223
1/2
✓ Branch 1 taken 1129 times.
✗ Branch 2 not taken.
 1129 transform.setTransform(R, T);
224 1129 }
225
226 995 void generateRandomTransforms(CoalScalar extents[6],
227 std::vector<Transform3s>& transforms,
228 std::size_t n) {
229 995 transforms.resize(n);
230
2/2
✓ Branch 0 taken 59544 times.
✓ Branch 1 taken 995 times.
 60539 for (std::size_t i = 0; i < n; ++i) {
231 59544 CoalScalar x = rand_interval(extents[0], extents[3]);
232 59544 CoalScalar y = rand_interval(extents[1], extents[4]);
233 59544 CoalScalar z = rand_interval(extents[2], extents[5]);
234
235 59544 const CoalScalar pi = 3.1415926;
236 59544 CoalScalar a = rand_interval(0, 2 * pi);
237 59544 CoalScalar b = rand_interval(0, 2 * pi);
238 59544 CoalScalar c = rand_interval(0, 2 * pi);
239
240 {
241
1/2
✓ Branch 1 taken 59544 times.
✗ Branch 2 not taken.
 59544 Matrix3s R;
242
1/2
✓ Branch 1 taken 59544 times.
✗ Branch 2 not taken.
 59544 eulerToMatrix(a, b, c, R);
243
1/2
✓ Branch 1 taken 59544 times.
✗ Branch 2 not taken.
 59544 Vec3s T(x, y, z);
244
1/2
✓ Branch 2 taken 59544 times.
✗ Branch 3 not taken.
 59544 transforms[i].setTransform(R, T);
245 }
246 }
247 995 }
248
249 1 void generateRandomTransforms(CoalScalar extents[6], CoalScalar delta_trans[3],
250 CoalScalar delta_rot,
251 std::vector<Transform3s>& transforms,
252 std::vector<Transform3s>& transforms2,
253 std::size_t n) {
254 1 transforms.resize(n);
255 1 transforms2.resize(n);
256
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
 3 for (std::size_t i = 0; i < n; ++i) {
257 2 CoalScalar x = rand_interval(extents[0], extents[3]);
258 2 CoalScalar y = rand_interval(extents[1], extents[4]);
259 2 CoalScalar z = rand_interval(extents[2], extents[5]);
260
261 2 const CoalScalar pi = 3.1415926;
262 2 CoalScalar a = rand_interval(0, 2 * pi);
263 2 CoalScalar b = rand_interval(0, 2 * pi);
264 2 CoalScalar c = rand_interval(0, 2 * pi);
265
266 {
267
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Matrix3s R;
268
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 eulerToMatrix(a, b, c, R);
269
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Vec3s T(x, y, z);
270
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 transforms[i].setTransform(R, T);
271 }
272
273 2 CoalScalar deltax = rand_interval(-delta_trans[0], delta_trans[0]);
274 2 CoalScalar deltay = rand_interval(-delta_trans[1], delta_trans[1]);
275 2 CoalScalar deltaz = rand_interval(-delta_trans[2], delta_trans[2]);
276
277 2 CoalScalar deltaa = rand_interval(-delta_rot, delta_rot);
278 2 CoalScalar deltab = rand_interval(-delta_rot, delta_rot);
279 2 CoalScalar deltac = rand_interval(-delta_rot, delta_rot);
280
281 {
282
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Matrix3s R;
283
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 eulerToMatrix(a + deltaa, b + deltab, c + deltac, R);
284
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Vec3s T(x + deltax, y + deltay, z + deltaz);
285
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 transforms2[i].setTransform(R, T);
286 }
287 }
288 1 }
289
290 1224052 bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2,
291 void* cdata_) {
292 1224052 CollisionData* cdata = static_cast<CollisionData*>(cdata_);
293 1224052 const CollisionRequest& request = cdata->request;
294 1224052 CollisionResult& result = cdata->result;
295
296
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1224052 times.
 1224052 if (cdata->done) return true;
297
298 1224052 collide(o1, o2, request, result);
299
300
4/4
✓ Branch 1 taken 1057 times.
✓ Branch 2 taken 1222995 times.
✓ Branch 3 taken 753 times.
✓ Branch 4 taken 1223299 times.
 1225109 if ((result.isCollision()) &&
301
2/2
✓ Branch 1 taken 753 times.
✓ Branch 2 taken 304 times.
 1057 (result.numContacts() >= request.num_max_contacts))
302 753 cdata->done = true;
303
304 1224052 return cdata->done;
305 }
306
307 33012 bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2,
308 void* cdata_, CoalScalar& dist) {
309 33012 DistanceData* cdata = static_cast<DistanceData*>(cdata_);
310 33012 const DistanceRequest& request = cdata->request;
311 33012 DistanceResult& result = cdata->result;
312
313
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 33012 times.
 33012 if (cdata->done) {
314 ✗ dist = result.min_distance;
315 ✗ return true;
316 }
317
318 33012 distance(o1, o2, request, result);
319
320 33012 dist = result.min_distance;
321
322
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 33012 times.
 33012 if (dist <= 0) return true; // in collision or in touch
323
324 33012 return cdata->done;
325 }
326
327 4 std::string getNodeTypeName(NODE_TYPE node_type) {
328
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 4 if (node_type == BV_UNKNOWN)
329 ✗ return std::string("BV_UNKNOWN");
330
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 4 else if (node_type == BV_AABB)
331 ✗ return std::string("BV_AABB");
332
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 3 times.
 4 else if (node_type == BV_OBB)
333
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 return std::string("BV_OBB");
334
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 2 times.
 3 else if (node_type == BV_RSS)
335
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 return std::string("BV_RSS");
336
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
 2 else if (node_type == BV_kIOS)
337
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 return std::string("BV_kIOS");
338
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 else if (node_type == BV_OBBRSS)
339
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 return std::string("BV_OBBRSS");
340 ✗ else if (node_type == BV_KDOP16)
341 ✗ return std::string("BV_KDOP16");
342 ✗ else if (node_type == BV_KDOP18)
343 ✗ return std::string("BV_KDOP18");
344 ✗ else if (node_type == BV_KDOP24)
345 ✗ return std::string("BV_KDOP24");
346 ✗ else if (node_type == GEOM_BOX)
347 ✗ return std::string("GEOM_BOX");
348 ✗ else if (node_type == GEOM_SPHERE)
349 ✗ return std::string("GEOM_SPHERE");
350 ✗ else if (node_type == GEOM_CAPSULE)
351 ✗ return std::string("GEOM_CAPSULE");
352 ✗ else if (node_type == GEOM_CONE)
353 ✗ return std::string("GEOM_CONE");
354 ✗ else if (node_type == GEOM_CYLINDER)
355 ✗ return std::string("GEOM_CYLINDER");
356 ✗ else if (node_type == GEOM_CONVEX)
357 ✗ return std::string("GEOM_CONVEX");
358 ✗ else if (node_type == GEOM_PLANE)
359 ✗ return std::string("GEOM_PLANE");
360 ✗ else if (node_type == GEOM_HALFSPACE)
361 ✗ return std::string("GEOM_HALFSPACE");
362 ✗ else if (node_type == GEOM_TRIANGLE)
363 ✗ return std::string("GEOM_TRIANGLE");
364 ✗ else if (node_type == GEOM_OCTREE)
365 ✗ return std::string("GEOM_OCTREE");
366 else
367 ✗ return std::string("invalid");
368 }
369
370 7 Quatf makeQuat(CoalScalar w, CoalScalar x, CoalScalar y, CoalScalar z) {
371 7 Quatf q;
372 7 q.w() = w;
373 7 q.x() = x;
374 7 q.y() = y;
375 7 q.z() = z;
376 7 return q;
377 }
378
379 ✗ std::ostream& operator<<(std::ostream& os, const Transform3s& tf) {
380 ✗ return os << "[ " << tf.getTranslation().format(vfmt) << ", "
381 ✗ << tf.getQuatRotation().coeffs().format(vfmt) << " ]";
382 }
383
384 6 std::size_t getNbRun(const int& argc, char const* const* argv,
385 std::size_t defaultValue) {
386
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 6 times.
 12 for (int i = 0; i < argc; ++i)
387
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 6 times.
 6 if (strcmp(argv[i], "--nb-run") == 0)
388 ✗ if (i + 1 != argc) return (std::size_t)strtol(argv[i + 1], NULL, 10);
389 6 return defaultValue;
390 }
391
392 69 void generateEnvironments(std::vector<CollisionObject*>& env,
393 CoalScalar env_scale, std::size_t n) {
394 69 CoalScalar extents[] = {-env_scale, env_scale, -env_scale,
395 69 env_scale, -env_scale, env_scale};
396
1/2
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
 69 std::vector<Transform3s> transforms(n);
397
398
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 generateRandomTransforms(extents, transforms, n);
399
2/2
✓ Branch 0 taken 4454 times.
✓ Branch 1 taken 69 times.
 4523 for (std::size_t i = 0; i < n; ++i) {
400
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);
401 4454 env.push_back(
402
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]));
403
1/2
✓ Branch 4 taken 4454 times.
✗ Branch 5 not taken.
 4454 env.back()->collisionGeometry()->computeLocalAABB();
404 }
405
406
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 generateRandomTransforms(extents, transforms, n);
407
2/2
✓ Branch 0 taken 4454 times.
✓ Branch 1 taken 69 times.
 4523 for (std::size_t i = 0; i < n; ++i) {
408
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);
409
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),
410
2/4
✓ Branch 2 taken 4454 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4454 times.
✗ Branch 6 not taken.
 4454 transforms[i]));
411
1/2
✓ Branch 4 taken 4454 times.
✗ Branch 5 not taken.
 4454 env.back()->collisionGeometry()->computeLocalAABB();
412 }
413
414
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 generateRandomTransforms(extents, transforms, n);
415
2/2
✓ Branch 0 taken 4454 times.
✓ Branch 1 taken 69 times.
 4523 for (std::size_t i = 0; i < n; ++i) {
416
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);
417
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),
418
2/4
✓ Branch 2 taken 4454 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4454 times.
✗ Branch 6 not taken.
 4454 transforms[i]));
419
1/2
✓ Branch 4 taken 4454 times.
✗ Branch 5 not taken.
 4454 env.back()->collisionGeometry()->computeLocalAABB();
420 }
421 69 }
422
423 44 void generateEnvironmentsMesh(std::vector<CollisionObject*>& env,
424 CoalScalar env_scale, std::size_t n) {
425 44 CoalScalar extents[] = {-env_scale, env_scale, -env_scale,
426 44 env_scale, -env_scale, env_scale};
427 44 std::vector<Transform3s> transforms;
428
429
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 generateRandomTransforms(extents, transforms, n);
430
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 Box box(5, 10, 20);
431
2/2
✓ Branch 0 taken 367 times.
✓ Branch 1 taken 44 times.
 411 for (std::size_t i = 0; i < n; ++i) {
432
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>();
433
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());
434
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),
435
2/4
✓ Branch 2 taken 367 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 367 times.
✗ Branch 6 not taken.
 367 transforms[i]));
436
1/2
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
 367 env.back()->collisionGeometry()->computeLocalAABB();
437 }
438
439
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 generateRandomTransforms(extents, transforms, n);
440
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 Sphere sphere(30);
441
2/2
✓ Branch 0 taken 367 times.
✓ Branch 1 taken 44 times.
 411 for (std::size_t i = 0; i < n; ++i) {
442
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>();
443
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);
444
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),
445
2/4
✓ Branch 2 taken 367 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 367 times.
✗ Branch 6 not taken.
 367 transforms[i]));
446
1/2
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
 367 env.back()->collisionGeometry()->computeLocalAABB();
447 }
448
449
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 generateRandomTransforms(extents, transforms, n);
450
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 Cylinder cylinder(10, 40);
451
2/2
✓ Branch 0 taken 367 times.
✓ Branch 1 taken 44 times.
 411 for (std::size_t i = 0; i < n; ++i) {
452
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>();
453
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);
454
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),
455
2/4
✓ Branch 2 taken 367 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 367 times.
✗ Branch 6 not taken.
 367 transforms[i]));
456
1/2
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
 367 env.back()->collisionGeometry()->computeLocalAABB();
457 }
458 44 }
459
460 4 Convex<Quadrilateral> buildBox(CoalScalar l, CoalScalar w, CoalScalar d) {
461 std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>(
462 ✗ {Vec3s(l, w, d), Vec3s(l, w, -d), Vec3s(l, -w, d), Vec3s(l, -w, -d),
463 ✗ Vec3s(-l, w, d), Vec3s(-l, w, -d), Vec3s(-l, -w, d),
464
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)}));
465
466 std::shared_ptr<std::vector<Quadrilateral>> polygons(
467
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));
468 4 (*polygons)[0].set(0, 2, 3, 1); // x+ side
469 4 (*polygons)[1].set(2, 6, 7, 3); // y- side
470 4 (*polygons)[2].set(4, 5, 7, 6); // x- side
471 4 (*polygons)[3].set(0, 1, 5, 4); // y+ side
472 4 (*polygons)[4].set(1, 3, 7, 5); // z- side
473 4 (*polygons)[5].set(0, 2, 6, 4); // z+ side
474
475 return Convex<Quadrilateral>(pts, // points
476 8, // num points
477 polygons,
478 6 // number of polygons
479
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 8 );
480 4 }
481
482 /// Takes a point and projects it onto the surface of the unit sphere
483 840 void toSphere(Vec3s& point) {
484
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 840 times.
 840 assert(point.norm() > 1e-8);
485
1/2
✓ Branch 2 taken 840 times.
✗ Branch 3 not taken.
 840 point /= point.norm();
486 840 }
487
488 /// Takes a point, projects it on the unit sphere and applies an ellipsoid
489 /// transformation to it. A point x belongs to the surface of an ellipsoid if
490 /// x^T * A * x = 1, where A = diag(1/r**2) with r being the radii of the
491 /// ellipsoid. Thus, the point y = A^(1/2) * x belongs to the unit sphere if y *
492 /// y = 1. Therefore, the tranformation which brings y to x is A^(-1/2) =
493 /// diag(r).
494 840 void toEllipsoid(Vec3s& point, const Ellipsoid& ellipsoid) {
495 840 toSphere(point);
496 840 point[0] *= ellipsoid.radii[0];
497 840 point[1] *= ellipsoid.radii[1];
498 840 point[2] *= ellipsoid.radii[2];
499 840 }
500
501 70 Convex<Triangle> constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid) {
502 70 CoalScalar PHI = (1 + std::sqrt(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
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));
529 70 (*tris)[0].set(0, 11, 5);
530 70 (*tris)[1].set(0, 5, 1);
531 70 (*tris)[2].set(0, 1, 7);
532 70 (*tris)[3].set(0, 7, 10);
533 70 (*tris)[4].set(0, 10, 11);
534
535 70 (*tris)[5].set(1, 5, 9);
536 70 (*tris)[6].set(5, 11, 4);
537 70 (*tris)[7].set(11, 10, 2);
538 70 (*tris)[8].set(10, 7, 6);
539 70 (*tris)[9].set(7, 1, 8);
540
541 70 (*tris)[10].set(3, 9, 4);
542 70 (*tris)[11].set(3, 4, 2);
543 70 (*tris)[12].set(3, 2, 6);
544 70 (*tris)[13].set(3, 6, 8);
545 70 (*tris)[14].set(3, 8, 9);
546
547 70 (*tris)[15].set(4, 9, 5);
548 70 (*tris)[16].set(2, 4, 11);
549 70 (*tris)[17].set(6, 2, 10);
550 70 (*tris)[18].set(8, 6, 7);
551 70 (*tris)[19].set(9, 8, 1);
552 return Convex<Triangle>(pts, // points
553 12, // num_points
554 tris, // triangles
555 20 // number of triangles
556
1/2
✓ Branch 3 taken 70 times.
✗ Branch 4 not taken.
 140 );
557 70 }
558
559 21 Box makeRandomBox(CoalScalar min_size, CoalScalar max_size) {
560
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 return Box(Vec3s(rand_interval(min_size, max_size),
561 21 rand_interval(min_size, max_size),
562
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 63 rand_interval(min_size, max_size)));
563 }
564
565 18 Sphere makeRandomSphere(CoalScalar min_size, CoalScalar max_size) {
566 18 return Sphere(rand_interval(min_size, max_size));
567 }
568
569 44 Ellipsoid makeRandomEllipsoid(CoalScalar min_size, CoalScalar max_size) {
570
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 return Ellipsoid(Vec3s(rand_interval(min_size, max_size),
571 44 rand_interval(min_size, max_size),
572
1/2
✓ Branch 3 taken 44 times.
✗ Branch 4 not taken.
 132 rand_interval(min_size, max_size)));
573 }
574
575 21 Capsule makeRandomCapsule(std::array<CoalScalar, 2> min_size,
576 std::array<CoalScalar, 2> max_size) {
577 21 return Capsule(rand_interval(min_size[0], max_size[0]),
578 42 rand_interval(min_size[1], max_size[1]));
579 }
580
581 22 Cone makeRandomCone(std::array<CoalScalar, 2> min_size,
582 std::array<CoalScalar, 2> max_size) {
583 22 return Cone(rand_interval(min_size[0], max_size[0]),
584 44 rand_interval(min_size[1], max_size[1]));
585 }
586
587 20 Cylinder makeRandomCylinder(std::array<CoalScalar, 2> min_size,
588 std::array<CoalScalar, 2> max_size) {
589 20 return Cylinder(rand_interval(min_size[0], max_size[0]),
590 40 rand_interval(min_size[1], max_size[1]));
591 }
592
593 21 Convex<Triangle> makeRandomConvex(CoalScalar min_size, CoalScalar max_size) {
594
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 Ellipsoid ellipsoid = makeRandomEllipsoid(min_size, max_size);
595
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 42 return constructPolytopeFromEllipsoid(ellipsoid);
596 21 }
597
598 14 Plane makeRandomPlane(CoalScalar min_size, CoalScalar max_size) {
599
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));
600 }
601
602 14 Halfspace makeRandomHalfspace(CoalScalar min_size, CoalScalar max_size) {
603
1/2
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
 28 return Halfspace(Vec3s::Random().normalized(),
604
1/2
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
 42 rand_interval(min_size, max_size));
605 }
606
607 154 std::shared_ptr<ShapeBase> makeRandomGeometry(NODE_TYPE node_type) {
608
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) {
609 ✗ case GEOM_TRIANGLE:
610 ✗ COAL_THROW_PRETTY(std::string(COAL_PRETTY_FUNCTION) + " for " +
611 std::string(get_node_type_name(node_type)) +
612 " is not yet implemented.",
613 std::invalid_argument);
614 break;
615 18 case GEOM_BOX:
616
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 return std::make_shared<Box>(makeRandomBox(0.1, 1.0));
617 break;
618 18 case GEOM_SPHERE:
619
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 return std::make_shared<Sphere>(makeRandomSphere(0.1, 1.0));
620 break;
621 18 case GEOM_ELLIPSOID:
622
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 return std::make_shared<Ellipsoid>(makeRandomEllipsoid(0.1, 1.0));
623 break;
624 18 case GEOM_CAPSULE:
625
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 return std::make_shared<Capsule>(
626 54 makeRandomCapsule({0.1, 0.2}, {0.8, 1.0}));
627 break;
628 18 case GEOM_CONE:
629
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 return std::make_shared<Cone>(makeRandomCone({0.1, 0.2}, {0.8, 1.0}));
630 break;
631 18 case GEOM_CYLINDER:
632
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 return std::make_shared<Cylinder>(
633 54 makeRandomCylinder({0.1, 0.2}, {0.8, 1.0}));
634 break;
635 18 case GEOM_CONVEX:
636
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 return std::make_shared<Convex<Triangle>>(makeRandomConvex(0.1, 1.0));
637 break;
638 14 case GEOM_PLANE:
639
1/2
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
 14 return std::make_shared<Plane>(makeRandomPlane(0.1, 1.0));
640 break;
641 14 case GEOM_HALFSPACE:
642
1/2
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
 14 return std::make_shared<Halfspace>(makeRandomHalfspace(0.1, 1.0));
643 break;
644 ✗ default:
645 ✗ COAL_THROW_PRETTY(std::string(get_node_type_name(node_type)) +
646 " is not a primitive shape.",
647 std::invalid_argument);
648 break;
649 }
650 }
651
652 } // namespace coal
653