| 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/10✓ 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.
✗ Branch 14 not taken.
✗ Branch 15 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 | 1222294 | bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, | |
| 290 | void* cdata_) { | ||
| 291 | 1222294 | CollisionData* cdata = static_cast<CollisionData*>(cdata_); | |
| 292 | 1222294 | const CollisionRequest& request = cdata->request; | |
| 293 | 1222294 | CollisionResult& result = cdata->result; | |
| 294 | |||
| 295 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1222294 times.
|
1222294 | if (cdata->done) return true; |
| 296 | |||
| 297 | 1222294 | collide(o1, o2, request, result); | |
| 298 | |||
| 299 |
4/4✓ Branch 1 taken 922 times.
✓ Branch 2 taken 1221372 times.
✓ Branch 3 taken 780 times.
✓ Branch 4 taken 1221514 times.
|
1223216 | 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 | 1222294 | return cdata->done; | |
| 304 | } | ||
| 305 | |||
| 306 | 33009 | bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, | |
| 307 | void* cdata_, Scalar& dist) { | ||
| 308 | 33009 | DistanceData* cdata = static_cast<DistanceData*>(cdata_); | |
| 309 | 33009 | const DistanceRequest& request = cdata->request; | |
| 310 | 33009 | DistanceResult& result = cdata->result; | |
| 311 | |||
| 312 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 33009 times.
|
33009 | if (cdata->done) { |
| 313 | ✗ | dist = result.min_distance; | |
| 314 | ✗ | return true; | |
| 315 | } | ||
| 316 | |||
| 317 | 33009 | distance(o1, o2, request, result); | |
| 318 | |||
| 319 | 33009 | dist = result.min_distance; | |
| 320 | |||
| 321 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 33009 times.
|
33009 | if (dist <= 0) return true; // in collision or in touch |
| 322 | |||
| 323 | 33009 | 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 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | 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 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | 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 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | 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 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | 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 1 taken 69 times.
✗ Branch 2 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/6✓ Branch 1 taken 4454 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4454 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
4454 | Box* box = new Box(5, 10, 20); |
| 400 | 4454 | env.push_back( | |
| 401 |
4/10✓ 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.
✗ Branch 15 not taken.
✗ Branch 16 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/6✓ Branch 1 taken 4454 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4454 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
4454 | Sphere* sphere = new Sphere(30); |
| 408 |
2/6✓ Branch 1 taken 4454 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4454 times.
✗ Branch 5 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
|
8908 | env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(sphere), |
| 409 |
2/4✓ Branch 1 taken 4454 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4454 times.
✗ Branch 6 not taken.
|
8908 | 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/6✓ Branch 1 taken 4454 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4454 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
4454 | Cylinder* cylinder = new Cylinder(10, 40); |
| 416 |
2/6✓ Branch 1 taken 4454 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4454 times.
✗ Branch 5 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
|
8908 | env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(cylinder), |
| 417 |
2/4✓ Branch 1 taken 4454 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4454 times.
✗ Branch 6 not taken.
|
8908 | 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/6✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 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/6✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
|
734 | env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(model), |
| 434 |
2/4✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 367 times.
✗ Branch 6 not taken.
|
734 | 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/6✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 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/6✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
|
734 | env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(model), |
| 444 |
2/4✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 367 times.
✗ Branch 6 not taken.
|
734 | 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/6✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 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/6✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
|
734 | env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(model), |
| 454 |
2/4✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 367 times.
✗ Branch 6 not taken.
|
734 | 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/24✓ 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 25 taken 4 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 4 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 4 times.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
|
8 | Vec3s(-l, -w, -d)})); |
| 464 | |||
| 465 | std::shared_ptr<std::vector<Quadrilateral32>> polygons( | ||
| 466 |
3/8✓ 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 9 not taken.
✗ Branch 10 not taken.
|
8 | 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 |
2/4✓ Branch 1 taken 840 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 840 times.
✗ Branch 5 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/32✓ 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 37 taken 70 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 70 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 70 times.
✗ Branch 44 not taken.
✗ Branch 46 not taken.
✗ Branch 47 not taken.
|
140 | })); |
| 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/8✓ 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 9 not taken.
✗ Branch 10 not taken.
|
140 | 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 |
3/6✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 14 times.
✗ Branch 9 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 |
2/4✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 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 |
2/4✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
|
18 | return std::make_shared<Box>(makeRandomBox(Scalar(0.1), Scalar(1))); |
| 618 | break; | ||
| 619 | 18 | case GEOM_SPHERE: | |
| 620 |
2/4✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 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 |
1/2✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
|
54 | makeRandomEllipsoid(Scalar(0.1), Scalar(1))); |
| 625 | break; | ||
| 626 | 18 | case GEOM_CAPSULE: | |
| 627 |
2/4✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 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 |
2/4✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 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 |
2/4✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 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 |
1/2✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
|
54 | makeRandomConvex(Scalar(0.1), Scalar(1))); |
| 641 | break; | ||
| 642 | 14 | case GEOM_PLANE: | |
| 643 |
2/4✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 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 |
1/2✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
|
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 |