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) {
98 FILE* file = fopen(filename,
"rb");
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) {
164 std::ofstream os(filename);
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 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);
294 if (cdata->
done)
return true;
296 collide(o1, o2, request, result);
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) {
400 new CollisionObject(shared_ptr<CollisionGeometry>(box), transforms[i]));
401 env.back()->collisionGeometry()->computeLocalAABB();
405 for (std::size_t i = 0; i < n; ++i) {
407 env.push_back(
new CollisionObject(shared_ptr<CollisionGeometry>(sphere),
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);
size_t num_max_contacts
The maximum number of contacts will return.
double getElapsedTimeInMilliSec()
get elapsed time in milli-second
request to the distance computation
Collision data stores the collision request and the result given by collision algorithm.
CollisionRequest request
Collision request.
Ellipsoid centered at point zero.
bool done
Whether the distance iteration can stop.
void generateEnvironmentsMesh(std::vector< CollisionObject *> &env, FCL_REAL env_scale, std::size_t n)
Cylinder along Z axis. The cylinder is defined at its centroid.
void loadOBJFile(const char *filename, std::vector< Vec3f > &points, std::vector< Triangle > &triangles)
Load an obj mesh file.
void generateEnvironments(std::vector< CollisionObject *> &env, FCL_REAL env_scale, std::size_t n)
void toSphere(Vec3f &point)
Takes a point and projects it onto the surface of the unit sphere.
DistanceResult result
Distance result.
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
void stop()
stop the timer
size_t numContacts() const
number of contacts found
FCL_REAL rand_interval(FCL_REAL rmin, FCL_REAL rmax)
void generateRandomTransforms(FCL_REAL extents[6], std::vector< Transform3f > &transforms, std::size_t n)
Generate n random transforms whose translations are constrained by extents.
Octree is one type of collision geometry which can encode uncertainty information in the sensor data...
std::size_t getNbRun(const int &argc, char const *const *argv, std::size_t defaultValue)
Get the argument –nb-run from argv.
Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z)
Convex< Triangle > constructPolytopeFromEllipsoid(const Ellipsoid &ellipsoid)
We give an ellipsoid as input. The output is a 20 faces polytope which vertices belong to the origina...
request to the collision algorithm
double startTimeInMicroSec
starting time in micro-second
void saveOBJFile(const char *filename, std::vector< Vec3f > &points, std::vector< Triangle > &triangles)
HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
shared_ptr< OcTree > OcTreePtr_t
FCL_REAL min_distance
minimum distance between two objects. if two objects are in collision, min_distance <= 0...
Center at zero point, axis aligned box.
void eulerToMatrix(FCL_REAL a, FCL_REAL b, FCL_REAL c, Matrix3f &R)
static std::ostream & operator<<(std::ostream &o, const Quaternion3f &q)
DistanceRequest request
Distance request.
std::string getNodeTypeName(NODE_TYPE node_type)
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
CollisionResult result
Collision result.
Eigen::Quaternion< FCL_REAL > Quaternion3f
bool defaultDistanceFunction(CollisionObject *o1, CollisionObject *o2, void *data, FCL_REAL &dist)
Collision data for use with the DefaultContinuousCollisionFunction. It stores the collision request a...
Center at zero point sphere.
void set(index_type p1, index_type p2, index_type p3)
Set the vertex indices of the triangle.
Triangle with 3 indices for points.
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone, cylinder, convex, plane, triangle), and octree
double getElapsedTime()
get elapsed time in milli-second
HPP_FCL_DLLAPI std::size_t collide(const CollisionObject *o1, const CollisionObject *o2, const CollisionRequest &request, CollisionResult &result)
Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function performs the collision between them. Return value is the number of contacts generated between the two objects.
double endTimeInMicroSec
ending time in micro-second
void toEllipsoid(Vec3f &point, const Ellipsoid &ellipsoid)
double getElapsedTimeInMicroSec()
get elapsed time in micro-second
bool isCollision() const
return binary collision result
double getElapsedTimeInSec()
const Eigen::IOFormat vfmt
bool done
Whether the collision iteration can stop.
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Vec3f radii
Radii of the Ellipsoid (such that on boundary: x^2/rx^2 + y^2/ry^2.
the object for collision or distance computation, contains the geometry and the transform information...
HPP_FCL_DLLAPI Halfspace transform(const Halfspace &a, const Transform3f &tf)
bool defaultCollisionFunction(CollisionObject *o1, CollisionObject *o2, void *data)
Provides a simple callback for the collision query in the BroadPhaseCollisionManager. It assumes the data parameter is non-null and points to an instance of CollisionData. It simply invokes the collide() method on the culled pair of geometries and stores the results in the data's CollisionResult instance.
Distance data stores the distance request and the result given by distance algorithm.
void generateBVHModel(BVHModel< BV > &model, const Box &shape, const Transform3f &pose)
Generate BVH model from box.
void generateRandomTransform(FCL_REAL extents[6], Transform3f &transform)
Generate one random transform whose translation is constrained by extents and rotation without constr...
const Eigen::IOFormat pyfmt