21 QueryPerformanceFrequency(&frequency);
83 const Eigen::IOFormat
vfmt = Eigen::IOFormat(
84 Eigen::StreamPrecision, Eigen::DontAlignCols,
", ",
", ",
"",
"",
"",
"");
85 const Eigen::IOFormat
pyfmt = Eigen::IOFormat(
86 Eigen::StreamPrecision, Eigen::DontAlignCols,
", ",
", ",
"",
"",
"[",
"]");
94 return (
t * (rmax - rmin) + rmin);
98 std::vector<Triangle>& triangles) {
101 std::cerr <<
"file not exist" << std::endl;
105 bool has_normal =
false;
106 bool has_texture =
false;
107 char line_buffer[2000];
108 while (fgets(line_buffer, 2000, file)) {
109 char* first_token = strtok(line_buffer,
"\r\n\t ");
110 if (!first_token || first_token[0] ==
'#' || first_token[0] == 0)
continue;
112 switch (first_token[0]) {
114 if (first_token[1] ==
'n') {
119 }
else if (first_token[1] ==
't') {
135 while ((
data[n] = strtok(NULL,
"\t \r\n")) != NULL) {
136 if (strlen(
data[n])) n++;
139 for (
int t = 0;
t < (n - 2); ++
t) {
140 if ((!has_texture) && (!has_normal)) {
156 triangles.push_back(tri);
164 std::vector<Triangle>& triangles) {
167 std::cerr <<
"file not exist" << std::endl;
171 for (std::size_t i = 0; i < points.size(); ++i) {
172 os <<
"v " << points[i][0] <<
" " << points[i][1] <<
" " << points[i][2]
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;
184 #ifdef COAL_HAS_OCTOMAP
185 OcTree loadOctreeFile(
const std::string&
filename,
188 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());
206 R << c1 *
c2, -
c2 * s1, s2, c3 * s1 + c1 * s2 * s3, c1 * c3 - s1 * s2 * s3,
207 -
c2 * s3, s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3,
c2 * c3;
227 std::vector<Transform3s>& transforms,
229 transforms.resize(n);
230 for (std::size_t i = 0; i < n; ++i) {
244 transforms[i].setTransform(
R, T);
251 std::vector<Transform3s>& transforms,
252 std::vector<Transform3s>& transforms2,
254 transforms.resize(n);
255 transforms2.resize(n);
256 for (std::size_t i = 0; i < n; ++i) {
270 transforms[i].setTransform(
R, T);
284 Vec3s T(
x + deltax,
y + deltay, z + deltaz);
285 transforms2[i].setTransform(
R, T);
292 CollisionData* cdata =
static_cast<CollisionData*
>(cdata_);
293 const CollisionRequest& request = cdata->request;
294 CollisionResult& result = cdata->result;
296 if (cdata->done)
return true;
298 collide(o1, o2, request, result);
300 if ((result.isCollision()) &&
301 (result.numContacts() >= request.num_max_contacts))
309 DistanceData* cdata =
static_cast<DistanceData*
>(cdata_);
310 const DistanceRequest& request = cdata->request;
311 DistanceResult& result = cdata->result;
314 dist = result.min_distance;
320 dist = result.min_distance;
322 if (dist <= 0)
return true;
329 return std::string(
"BV_UNKNOWN");
331 return std::string(
"BV_AABB");
332 else if (node_type ==
BV_OBB)
333 return std::string(
"BV_OBB");
334 else if (node_type ==
BV_RSS)
335 return std::string(
"BV_RSS");
337 return std::string(
"BV_kIOS");
339 return std::string(
"BV_OBBRSS");
341 return std::string(
"BV_KDOP16");
343 return std::string(
"BV_KDOP18");
345 return std::string(
"BV_KDOP24");
347 return std::string(
"GEOM_BOX");
349 return std::string(
"GEOM_SPHERE");
351 return std::string(
"GEOM_CAPSULE");
353 return std::string(
"GEOM_CONE");
355 return std::string(
"GEOM_CYLINDER");
357 return std::string(
"GEOM_CONVEX");
359 return std::string(
"GEOM_PLANE");
361 return std::string(
"GEOM_HALFSPACE");
363 return std::string(
"GEOM_TRIANGLE");
365 return std::string(
"GEOM_OCTREE");
367 return std::string(
"invalid");
384 std::size_t
getNbRun(
const int& argc,
char const*
const* argv,
385 std::size_t defaultValue) {
386 for (
int i = 0; i < argc; ++i)
387 if (strcmp(argv[i],
"--nb-run") == 0)
388 if (i + 1 != argc)
return (std::size_t)strtol(argv[i + 1], NULL, 10);
395 env_scale, -env_scale, env_scale};
396 std::vector<Transform3s> transforms(n);
399 for (std::size_t i = 0; i < n; ++i) {
403 env.back()->collisionGeometry()->computeLocalAABB();
407 for (std::size_t i = 0; i < n; ++i) {
411 env.back()->collisionGeometry()->computeLocalAABB();
415 for (std::size_t i = 0; i < n; ++i) {
417 env.push_back(
new CollisionObject(shared_ptr<CollisionGeometry>(cylinder),
419 env.back()->collisionGeometry()->computeLocalAABB();
426 env_scale, -env_scale, env_scale};
427 std::vector<Transform3s> transforms;
431 for (std::size_t i = 0; i < n; ++i) {
434 env.push_back(
new CollisionObject(shared_ptr<CollisionGeometry>(model),
436 env.back()->collisionGeometry()->computeLocalAABB();
441 for (std::size_t i = 0; i < n; ++i) {
444 env.push_back(
new CollisionObject(shared_ptr<CollisionGeometry>(model),
446 env.back()->collisionGeometry()->computeLocalAABB();
451 for (std::size_t i = 0; i < n; ++i) {
454 env.push_back(
new CollisionObject(shared_ptr<CollisionGeometry>(model),
456 env.back()->collisionGeometry()->computeLocalAABB();
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),
466 std::shared_ptr<std::vector<Quadrilateral>> polygons(
467 new std::vector<Quadrilateral>(6));
468 (*polygons)[0].set(0, 2, 3, 1);
469 (*polygons)[1].set(2, 6, 7, 3);
470 (*polygons)[2].set(4, 5, 7, 6);
471 (*polygons)[3].set(0, 1, 5, 4);
472 (*polygons)[4].set(1, 3, 7, 5);
473 (*polygons)[5].set(0, 2, 6, 4);
484 assert(point.norm() > 1e-8);
485 point /= point.norm();
496 point[0] *= ellipsoid.
radii[0];
497 point[1] *= ellipsoid.
radii[1];
498 point[2] *= ellipsoid.
radii[2];
505 std::shared_ptr<std::vector<Vec3s>> pts(
new std::vector<Vec3s>({
522 std::vector<Vec3s>& pts_ = *pts;
523 for (
size_t i = 0; i < 12; ++i) {
528 std::shared_ptr<std::vector<Triangle>> tris(
new std::vector<Triangle>(20));
529 (*tris)[0].set(0, 11, 5);
530 (*tris)[1].set(0, 5, 1);
531 (*tris)[2].set(0, 1, 7);
532 (*tris)[3].set(0, 7, 10);
533 (*tris)[4].set(0, 10, 11);
535 (*tris)[5].set(1, 5, 9);
536 (*tris)[6].set(5, 11, 4);
537 (*tris)[7].set(11, 10, 2);
538 (*tris)[8].set(10, 7, 6);
539 (*tris)[9].set(7, 1, 8);
541 (*tris)[10].set(3, 9, 4);
542 (*tris)[11].set(3, 4, 2);
543 (*tris)[12].set(3, 2, 6);
544 (*tris)[13].set(3, 6, 8);
545 (*tris)[14].set(3, 8, 9);
547 (*tris)[15].set(4, 9, 5);
548 (*tris)[16].set(2, 4, 11);
549 (*tris)[17].set(6, 2, 10);
550 (*tris)[18].set(8, 6, 7);
551 (*tris)[19].set(9, 8, 1);
576 std::array<CoalScalar, 2> max_size) {
582 std::array<CoalScalar, 2> max_size) {
588 std::array<CoalScalar, 2> max_size) {
603 return Halfspace(Vec3s::Random().normalized(),
612 " is not yet implemented.",
613 std::invalid_argument);
625 return std::make_shared<Capsule>(
629 return std::make_shared<Cone>(
makeRandomCone({0.1, 0.2}, {0.8, 1.0}));
632 return std::make_shared<Cylinder>(
646 " is not a primitive shape.",
647 std::invalid_argument);