20 QueryPerformanceFrequency(&frequency);
82 const Eigen::IOFormat
vfmt = Eigen::IOFormat(
83 Eigen::StreamPrecision, Eigen::DontAlignCols,
", ",
", ",
"",
"",
"",
"");
84 const Eigen::IOFormat
pyfmt = Eigen::IOFormat(
85 Eigen::StreamPrecision, Eigen::DontAlignCols,
", ",
", ",
"",
"",
"[",
"]");
93 return (
t * (rmax - rmin) + rmin);
97 std::vector<Triangle>& triangles) {
100 std::cerr <<
"file not exist" << std::endl;
104 bool has_normal =
false;
105 bool has_texture =
false;
106 char line_buffer[2000];
107 while (fgets(line_buffer, 2000, file)) {
108 char* first_token = strtok(line_buffer,
"\r\n\t ");
109 if (!first_token || first_token[0] ==
'#' || first_token[0] == 0)
continue;
111 switch (first_token[0]) {
113 if (first_token[1] ==
'n') {
118 }
else if (first_token[1] ==
't') {
134 while ((
data[n] = strtok(NULL,
"\t \r\n")) != NULL) {
135 if (strlen(
data[n])) n++;
138 for (
int t = 0;
t < (n - 2); ++
t) {
139 if ((!has_texture) && (!has_normal)) {
155 triangles.push_back(tri);
163 std::vector<Triangle>& triangles) {
166 std::cerr <<
"file not exist" << std::endl;
170 for (std::size_t i = 0; i < points.size(); ++i) {
171 os <<
"v " << points[i][0] <<
" " << points[i][1] <<
" " << points[i][2]
175 for (std::size_t i = 0; i < triangles.size(); ++i) {
176 os <<
"f " << triangles[i][0] + 1 <<
" " << triangles[i][1] + 1 <<
" "
177 << triangles[i][2] + 1 << std::endl;
183 #ifdef HPP_FCL_HAS_OCTOMAP
184 OcTree loadOctreeFile(
const std::string&
filename,
const FCL_REAL& resolution) {
186 if (
octree->getResolution() != resolution) {
187 std::ostringstream oss;
188 oss <<
"Resolution of the OcTree is " <<
octree->getResolution()
189 <<
" and not " << resolution;
190 throw std::invalid_argument(oss.str());
204 R << c1 *
c2, -
c2 * s1, s2, c3 * s1 + c1 * s2 * s3, c1 * c3 - s1 * s2 * s3,
205 -
c2 * s3, s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3,
c2 * c3;
225 std::vector<Transform3f>& transforms,
227 transforms.resize(n);
228 for (std::size_t i = 0; i < n; ++i) {
242 transforms[i].setTransform(
R, T);
249 std::vector<Transform3f>& transforms,
250 std::vector<Transform3f>& transforms2,
252 transforms.resize(n);
253 transforms2.resize(n);
254 for (std::size_t i = 0; i < n; ++i) {
268 transforms[i].setTransform(
R, T);
282 Vec3f T(
x + deltax,
y + deltay, z + deltaz);
283 transforms2[i].setTransform(
R, T);
290 CollisionData* cdata =
static_cast<CollisionData*
>(cdata_);
291 const CollisionRequest& request = cdata->request;
292 CollisionResult& result = cdata->result;
294 if (cdata->done)
return true;
296 collide(o1, o2, request, result);
298 if ((result.isCollision()) &&
299 (result.numContacts() >= request.num_max_contacts))
307 DistanceData* cdata =
static_cast<DistanceData*
>(cdata_);
308 const DistanceRequest& request = cdata->request;
309 DistanceResult& result = cdata->result;
312 dist = result.min_distance;
318 dist = result.min_distance;
320 if (dist <= 0)
return true;
327 return std::string(
"BV_UNKNOWN");
329 return std::string(
"BV_AABB");
330 else if (node_type ==
BV_OBB)
331 return std::string(
"BV_OBB");
332 else if (node_type ==
BV_RSS)
333 return std::string(
"BV_RSS");
335 return std::string(
"BV_kIOS");
337 return std::string(
"BV_OBBRSS");
339 return std::string(
"BV_KDOP16");
341 return std::string(
"BV_KDOP18");
343 return std::string(
"BV_KDOP24");
345 return std::string(
"GEOM_BOX");
347 return std::string(
"GEOM_SPHERE");
349 return std::string(
"GEOM_CAPSULE");
351 return std::string(
"GEOM_CONE");
353 return std::string(
"GEOM_CYLINDER");
355 return std::string(
"GEOM_CONVEX");
357 return std::string(
"GEOM_PLANE");
359 return std::string(
"GEOM_HALFSPACE");
361 return std::string(
"GEOM_TRIANGLE");
363 return std::string(
"GEOM_OCTREE");
365 return std::string(
"invalid");
382 std::size_t
getNbRun(
const int& argc,
char const*
const* argv,
383 std::size_t defaultValue) {
384 for (
int i = 0; i < argc; ++i)
385 if (strcmp(argv[i],
"--nb-run") == 0)
386 if (i + 1 != argc)
return (std::size_t)strtol(argv[i + 1], NULL, 10);
391 FCL_REAL env_scale, std::size_t n) {
393 env_scale, -env_scale, env_scale};
394 std::vector<Transform3f> transforms(n);
397 for (std::size_t i = 0; i < n; ++i) {
401 env.back()->collisionGeometry()->computeLocalAABB();
405 for (std::size_t i = 0; i < n; ++i) {
409 env.back()->collisionGeometry()->computeLocalAABB();
413 for (std::size_t i = 0; i < n; ++i) {
415 env.push_back(
new CollisionObject(shared_ptr<CollisionGeometry>(cylinder),
417 env.back()->collisionGeometry()->computeLocalAABB();
422 FCL_REAL env_scale, std::size_t n) {
424 env_scale, -env_scale, env_scale};
425 std::vector<Transform3f> transforms;
429 for (std::size_t i = 0; i < n; ++i) {
432 env.push_back(
new CollisionObject(shared_ptr<CollisionGeometry>(model),
434 env.back()->collisionGeometry()->computeLocalAABB();
439 for (std::size_t i = 0; i < n; ++i) {
442 env.push_back(
new CollisionObject(shared_ptr<CollisionGeometry>(model),
444 env.back()->collisionGeometry()->computeLocalAABB();
449 for (std::size_t i = 0; i < n; ++i) {
452 env.push_back(
new CollisionObject(shared_ptr<CollisionGeometry>(model),
454 env.back()->collisionGeometry()->computeLocalAABB();
460 assert(point.norm() > 1e-8);
461 point /= point.norm();
472 point[0] *= ellipsoid.
radii[0];
473 point[1] *= ellipsoid.
radii[1];
474 point[2] *= ellipsoid.
radii[2];
478 FCL_REAL PHI = (1 + std::sqrt(5)) / 2;
482 pts[0] =
Vec3f(-1, PHI, 0);
483 pts[1] =
Vec3f(1, PHI, 0);
484 pts[2] =
Vec3f(-1, -PHI, 0);
485 pts[3] =
Vec3f(1, -PHI, 0);
487 pts[4] =
Vec3f(0, -1, PHI);
488 pts[5] =
Vec3f(0, 1, PHI);
489 pts[6] =
Vec3f(0, -1, -PHI);
490 pts[7] =
Vec3f(0, 1, -PHI);
492 pts[8] =
Vec3f(PHI, 0, -1);
493 pts[9] =
Vec3f(PHI, 0, 1);
494 pts[10] =
Vec3f(-PHI, 0, -1);
495 pts[11] =
Vec3f(-PHI, 0, 1);
497 for (
int i = 0; i < 12; ++i) {
503 tris[0].
set(0, 11, 5);
504 tris[1].
set(0, 5, 1);
505 tris[2].
set(0, 1, 7);
506 tris[3].
set(0, 7, 10);
507 tris[4].
set(0, 10, 11);
509 tris[5].
set(1, 5, 9);
510 tris[6].
set(5, 11, 4);
511 tris[7].
set(11, 10, 2);
512 tris[8].
set(10, 7, 6);
513 tris[9].
set(7, 1, 8);
515 tris[10].
set(3, 9, 4);
516 tris[11].
set(3, 4, 2);
517 tris[12].
set(3, 2, 6);
518 tris[13].
set(3, 6, 8);
519 tris[14].
set(3, 8, 9);
521 tris[15].
set(4, 9, 5);
522 tris[16].
set(2, 4, 11);
523 tris[17].
set(6, 2, 10);
524 tris[18].
set(8, 6, 7);
525 tris[19].
set(9, 8, 1);