utility.cpp
Go to the documentation of this file.
1 #include "utility.h"
2 
3 #include <hpp/fcl/BV/BV.h>
6 
7 #include <hpp/fcl/collision.h>
8 #include <hpp/fcl/distance.h>
9 
10 #include <cstdio>
11 #include <cstddef>
12 #include <fstream>
13 #include <iostream>
14 
15 namespace hpp {
16 namespace fcl {
17 
19 #ifdef _WIN32
20  QueryPerformanceFrequency(&frequency);
21  startCount.QuadPart = 0;
22  endCount.QuadPart = 0;
23 #else
24  startCount.tv_sec = startCount.tv_usec = 0;
25  endCount.tv_sec = endCount.tv_usec = 0;
26 #endif
27 
28  stopped = 0;
31 }
32 
34 
36  stopped = 0; // reset stop flag
37 #ifdef _WIN32
38  QueryPerformanceCounter(&startCount);
39 #else
40  gettimeofday(&startCount, NULL);
41 #endif
42 }
43 
45  stopped = 1; // set timer stopped flag
46 
47 #ifdef _WIN32
48  QueryPerformanceCounter(&endCount);
49 #else
50  gettimeofday(&endCount, NULL);
51 #endif
52 }
53 
55 #ifdef _WIN32
56  if (!stopped) QueryPerformanceCounter(&endCount);
57 
58  startTimeInMicroSec = startCount.QuadPart * (1000000.0 / frequency.QuadPart);
59  endTimeInMicroSec = endCount.QuadPart * (1000000.0 / frequency.QuadPart);
60 #else
61  if (!stopped) gettimeofday(&endCount, NULL);
62 
64  ((double)startCount.tv_sec * 1000000.0) + (double)startCount.tv_usec;
66  ((double)endCount.tv_sec * 1000000.0) + (double)endCount.tv_usec;
67 #endif
68 
70 }
71 
73  return this->getElapsedTimeInMicroSec() * 0.001;
74 }
75 
77  return this->getElapsedTimeInMicroSec() * 0.000001;
78 }
79 
81 
82 const Eigen::IOFormat vfmt = Eigen::IOFormat(
83  Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "", "");
84 const Eigen::IOFormat pyfmt = Eigen::IOFormat(
85  Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]");
86 
87 const Vec3f UnitX = Vec3f(1, 0, 0);
88 const Vec3f UnitY = Vec3f(0, 1, 0);
89 const Vec3f UnitZ = Vec3f(0, 0, 1);
90 
92  FCL_REAL t = rand() / ((FCL_REAL)RAND_MAX + 1);
93  return (t * (rmax - rmin) + rmin);
94 }
95 
96 void loadOBJFile(const char* filename, std::vector<Vec3f>& points,
97  std::vector<Triangle>& triangles) {
98  FILE* file = fopen(filename, "rb");
99  if (!file) {
100  std::cerr << "file not exist" << std::endl;
101  return;
102  }
103 
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;
110 
111  switch (first_token[0]) {
112  case 'v': {
113  if (first_token[1] == 'n') {
114  strtok(NULL, "\t ");
115  strtok(NULL, "\t ");
116  strtok(NULL, "\t ");
117  has_normal = true;
118  } else if (first_token[1] == 't') {
119  strtok(NULL, "\t ");
120  strtok(NULL, "\t ");
121  has_texture = true;
122  } else {
123  FCL_REAL x = (FCL_REAL)atof(strtok(NULL, "\t "));
124  FCL_REAL y = (FCL_REAL)atof(strtok(NULL, "\t "));
125  FCL_REAL z = (FCL_REAL)atof(strtok(NULL, "\t "));
126  Vec3f p(x, y, z);
127  points.push_back(p);
128  }
129  } break;
130  case 'f': {
131  Triangle tri;
132  char* data[30];
133  int n = 0;
134  while ((data[n] = strtok(NULL, "\t \r\n")) != NULL) {
135  if (strlen(data[n])) n++;
136  }
137 
138  for (int t = 0; t < (n - 2); ++t) {
139  if ((!has_texture) && (!has_normal)) {
140  tri[0] = (Triangle::index_type)(atoi(data[0]) - 1);
141  tri[1] = (Triangle::index_type)(atoi(data[1]) - 1);
142  tri[2] = (Triangle::index_type)(atoi(data[2]) - 1);
143  } else {
144  const char* v1;
145  for (Triangle::index_type i = 0; i < 3; i++) {
146  // vertex ID
147  if (i == 0)
148  v1 = data[0];
149  else
150  v1 = data[(Triangle::index_type)t + i];
151 
152  tri[i] = (Triangle::index_type)(atoi(v1) - 1);
153  }
154  }
155  triangles.push_back(tri);
156  }
157  }
158  }
159  }
160 }
161 
162 void saveOBJFile(const char* filename, std::vector<Vec3f>& points,
163  std::vector<Triangle>& triangles) {
164  std::ofstream os(filename);
165  if (!os) {
166  std::cerr << "file not exist" << std::endl;
167  return;
168  }
169 
170  for (std::size_t i = 0; i < points.size(); ++i) {
171  os << "v " << points[i][0] << " " << points[i][1] << " " << points[i][2]
172  << std::endl;
173  }
174 
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;
178  }
179 
180  os.close();
181 }
182 
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());
191  }
192  return hpp::fcl::OcTree(octree);
193 }
194 #endif
195 
197  FCL_REAL c1 = cos(a);
198  FCL_REAL c2 = cos(b);
199  FCL_REAL c3 = cos(c);
200  FCL_REAL s1 = sin(a);
201  FCL_REAL s2 = sin(b);
202  FCL_REAL s3 = sin(c);
203 
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;
206 }
207 
209  FCL_REAL x = rand_interval(extents[0], extents[3]);
210  FCL_REAL y = rand_interval(extents[1], extents[4]);
211  FCL_REAL z = rand_interval(extents[2], extents[5]);
212 
213  const FCL_REAL pi = 3.1415926;
214  FCL_REAL a = rand_interval(0, 2 * pi);
215  FCL_REAL b = rand_interval(0, 2 * pi);
216  FCL_REAL c = rand_interval(0, 2 * pi);
217 
218  Matrix3f R;
219  eulerToMatrix(a, b, c, R);
220  Vec3f T(x, y, z);
221  transform.setTransform(R, T);
222 }
223 
225  std::vector<Transform3f>& transforms,
226  std::size_t n) {
227  transforms.resize(n);
228  for (std::size_t i = 0; i < n; ++i) {
229  FCL_REAL x = rand_interval(extents[0], extents[3]);
230  FCL_REAL y = rand_interval(extents[1], extents[4]);
231  FCL_REAL z = rand_interval(extents[2], extents[5]);
232 
233  const FCL_REAL pi = 3.1415926;
234  FCL_REAL a = rand_interval(0, 2 * pi);
235  FCL_REAL b = rand_interval(0, 2 * pi);
236  FCL_REAL c = rand_interval(0, 2 * pi);
237 
238  {
239  Matrix3f R;
240  eulerToMatrix(a, b, c, R);
241  Vec3f T(x, y, z);
242  transforms[i].setTransform(R, T);
243  }
244  }
245 }
246 
248  FCL_REAL delta_rot,
249  std::vector<Transform3f>& transforms,
250  std::vector<Transform3f>& transforms2,
251  std::size_t n) {
252  transforms.resize(n);
253  transforms2.resize(n);
254  for (std::size_t i = 0; i < n; ++i) {
255  FCL_REAL x = rand_interval(extents[0], extents[3]);
256  FCL_REAL y = rand_interval(extents[1], extents[4]);
257  FCL_REAL z = rand_interval(extents[2], extents[5]);
258 
259  const FCL_REAL pi = 3.1415926;
260  FCL_REAL a = rand_interval(0, 2 * pi);
261  FCL_REAL b = rand_interval(0, 2 * pi);
262  FCL_REAL c = rand_interval(0, 2 * pi);
263 
264  {
265  Matrix3f R;
266  eulerToMatrix(a, b, c, R);
267  Vec3f T(x, y, z);
268  transforms[i].setTransform(R, T);
269  }
270 
271  FCL_REAL deltax = rand_interval(-delta_trans[0], delta_trans[0]);
272  FCL_REAL deltay = rand_interval(-delta_trans[1], delta_trans[1]);
273  FCL_REAL deltaz = rand_interval(-delta_trans[2], delta_trans[2]);
274 
275  FCL_REAL deltaa = rand_interval(-delta_rot, delta_rot);
276  FCL_REAL deltab = rand_interval(-delta_rot, delta_rot);
277  FCL_REAL deltac = rand_interval(-delta_rot, delta_rot);
278 
279  {
280  Matrix3f R;
281  eulerToMatrix(a + deltaa, b + deltab, c + deltac, R);
282  Vec3f T(x + deltax, y + deltay, z + deltaz);
283  transforms2[i].setTransform(R, T);
284  }
285  }
286 }
287 
289  void* cdata_) {
290  CollisionData* cdata = static_cast<CollisionData*>(cdata_);
291  const CollisionRequest& request = cdata->request;
292  CollisionResult& result = cdata->result;
293 
294  if (cdata->done) return true;
295 
296  collide(o1, o2, request, result);
297 
298  if ((result.isCollision()) &&
299  (result.numContacts() >= request.num_max_contacts))
300  cdata->done = true;
301 
302  return cdata->done;
303 }
304 
306  void* cdata_, FCL_REAL& dist) {
307  DistanceData* cdata = static_cast<DistanceData*>(cdata_);
308  const DistanceRequest& request = cdata->request;
309  DistanceResult& result = cdata->result;
310 
311  if (cdata->done) {
312  dist = result.min_distance;
313  return true;
314  }
315 
316  distance(o1, o2, request, result);
317 
318  dist = result.min_distance;
319 
320  if (dist <= 0) return true; // in collision or in touch
321 
322  return cdata->done;
323 }
324 
325 std::string getNodeTypeName(NODE_TYPE node_type) {
326  if (node_type == BV_UNKNOWN)
327  return std::string("BV_UNKNOWN");
328  else if (node_type == BV_AABB)
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");
334  else if (node_type == BV_kIOS)
335  return std::string("BV_kIOS");
336  else if (node_type == BV_OBBRSS)
337  return std::string("BV_OBBRSS");
338  else if (node_type == BV_KDOP16)
339  return std::string("BV_KDOP16");
340  else if (node_type == BV_KDOP18)
341  return std::string("BV_KDOP18");
342  else if (node_type == BV_KDOP24)
343  return std::string("BV_KDOP24");
344  else if (node_type == GEOM_BOX)
345  return std::string("GEOM_BOX");
346  else if (node_type == GEOM_SPHERE)
347  return std::string("GEOM_SPHERE");
348  else if (node_type == GEOM_CAPSULE)
349  return std::string("GEOM_CAPSULE");
350  else if (node_type == GEOM_CONE)
351  return std::string("GEOM_CONE");
352  else if (node_type == GEOM_CYLINDER)
353  return std::string("GEOM_CYLINDER");
354  else if (node_type == GEOM_CONVEX)
355  return std::string("GEOM_CONVEX");
356  else if (node_type == GEOM_PLANE)
357  return std::string("GEOM_PLANE");
358  else if (node_type == GEOM_HALFSPACE)
359  return std::string("GEOM_HALFSPACE");
360  else if (node_type == GEOM_TRIANGLE)
361  return std::string("GEOM_TRIANGLE");
362  else if (node_type == GEOM_OCTREE)
363  return std::string("GEOM_OCTREE");
364  else
365  return std::string("invalid");
366 }
367 
369  Quaternion3f q;
370  q.w() = w;
371  q.x() = x;
372  q.y() = y;
373  q.z() = z;
374  return q;
375 }
376 
377 std::ostream& operator<<(std::ostream& os, const Transform3f& tf) {
378  return os << "[ " << tf.getTranslation().format(vfmt) << ", "
379  << tf.getQuatRotation().coeffs().format(vfmt) << " ]";
380 }
381 
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);
387  return defaultValue;
388 }
389 
390 void generateEnvironments(std::vector<CollisionObject*>& env,
391  FCL_REAL env_scale, std::size_t n) {
392  FCL_REAL extents[] = {-env_scale, env_scale, -env_scale,
393  env_scale, -env_scale, env_scale};
394  std::vector<Transform3f> transforms(n);
395 
396  generateRandomTransforms(extents, transforms, n);
397  for (std::size_t i = 0; i < n; ++i) {
398  Box* box = new Box(5, 10, 20);
399  env.push_back(
400  new CollisionObject(shared_ptr<CollisionGeometry>(box), transforms[i]));
401  env.back()->collisionGeometry()->computeLocalAABB();
402  }
403 
404  generateRandomTransforms(extents, transforms, n);
405  for (std::size_t i = 0; i < n; ++i) {
406  Sphere* sphere = new Sphere(30);
407  env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(sphere),
408  transforms[i]));
409  env.back()->collisionGeometry()->computeLocalAABB();
410  }
411 
412  generateRandomTransforms(extents, transforms, n);
413  for (std::size_t i = 0; i < n; ++i) {
414  Cylinder* cylinder = new Cylinder(10, 40);
415  env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(cylinder),
416  transforms[i]));
417  env.back()->collisionGeometry()->computeLocalAABB();
418  }
419 }
420 
421 void generateEnvironmentsMesh(std::vector<CollisionObject*>& env,
422  FCL_REAL env_scale, std::size_t n) {
423  FCL_REAL extents[] = {-env_scale, env_scale, -env_scale,
424  env_scale, -env_scale, env_scale};
425  std::vector<Transform3f> transforms;
426 
427  generateRandomTransforms(extents, transforms, n);
428  Box box(5, 10, 20);
429  for (std::size_t i = 0; i < n; ++i) {
430  BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
431  generateBVHModel(*model, box, Transform3f::Identity());
432  env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(model),
433  transforms[i]));
434  env.back()->collisionGeometry()->computeLocalAABB();
435  }
436 
437  generateRandomTransforms(extents, transforms, n);
438  Sphere sphere(30);
439  for (std::size_t i = 0; i < n; ++i) {
440  BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
441  generateBVHModel(*model, sphere, Transform3f::Identity(), 16, 16);
442  env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(model),
443  transforms[i]));
444  env.back()->collisionGeometry()->computeLocalAABB();
445  }
446 
447  generateRandomTransforms(extents, transforms, n);
448  Cylinder cylinder(10, 40);
449  for (std::size_t i = 0; i < n; ++i) {
450  BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
451  generateBVHModel(*model, cylinder, Transform3f::Identity(), 16, 16);
452  env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(model),
453  transforms[i]));
454  env.back()->collisionGeometry()->computeLocalAABB();
455  }
456 }
457 
459 void toSphere(Vec3f& point) {
460  assert(point.norm() > 1e-8);
461  point /= point.norm();
462 }
463 
470 void toEllipsoid(Vec3f& point, const Ellipsoid& ellipsoid) {
471  toSphere(point);
472  point[0] *= ellipsoid.radii[0];
473  point[1] *= ellipsoid.radii[1];
474  point[2] *= ellipsoid.radii[2];
475 }
476 
478  FCL_REAL PHI = (1 + std::sqrt(5)) / 2;
479 
480  // vertices
481  Vec3f* pts = new Vec3f[12];
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);
486 
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);
491 
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);
496 
497  for (int i = 0; i < 12; ++i) {
498  toEllipsoid(pts[i], ellipsoid);
499  }
500 
501  // faces
502  Triangle* tris = new Triangle[20];
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);
508 
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);
514 
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);
520 
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);
526  return Convex<Triangle>(true,
527  pts, // points
528  12, // num_points
529  tris, // triangles
530  20 // number of triangles
531  );
532 }
533 
534 } // namespace fcl
535 
536 } // namespace hpp
Quaternion3f getQuatRotation() const
get quaternion
Definition: transform.h:118
size_t num_max_contacts
The maximum number of contacts will return.
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
double getElapsedTimeInMilliSec()
get elapsed time in milli-second
Definition: utility.cpp:72
request to the distance computation
Collision data stores the collision request and the result given by collision algorithm.
CollisionRequest request
Collision request.
const Vec3f UnitY
Definition: utility.cpp:88
q
Ellipsoid centered at point zero.
y
bool done
Whether the distance iteration can stop.
void generateEnvironmentsMesh(std::vector< CollisionObject *> &env, FCL_REAL env_scale, std::size_t n)
Definition: utility.cpp:421
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.
Definition: utility.cpp:96
Main namespace.
void generateEnvironments(std::vector< CollisionObject *> &env, FCL_REAL env_scale, std::size_t n)
Definition: utility.cpp:390
t
void toSphere(Vec3f &point)
Takes a point and projects it onto the surface of the unit sphere.
Definition: utility.cpp:459
DistanceResult result
Distance result.
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
void stop()
stop the timer
Definition: utility.cpp:44
size_t numContacts() const
number of contacts found
FCL_REAL rand_interval(FCL_REAL rmin, FCL_REAL rmax)
Definition: utility.cpp:91
void setTransform(const Eigen::MatrixBase< Matrix3Like > &R_, const Eigen::MatrixBase< Vector3Like > &T_)
set transform from rotation and translation
Definition: transform.h:122
void generateRandomTransforms(FCL_REAL extents[6], std::vector< Transform3f > &transforms, std::size_t n)
Generate n random transforms whose translations are constrained by extents.
Definition: utility.cpp:224
timeval startCount
Definition: utility.h:104
c
data
Octree is one type of collision geometry which can encode uncertainty information in the sensor data...
Definition: octree.h:53
Definition: octree.py:1
std::size_t index_type
Definition: data_types.h:98
std::size_t getNbRun(const int &argc, char const *const *argv, std::size_t defaultValue)
Get the argument –nb-run from argv.
Definition: utility.cpp:382
Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z)
Definition: utility.cpp:368
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...
Definition: utility.cpp:477
request to the collision algorithm
double startTimeInMicroSec
starting time in micro-second
Definition: utility.h:96
void saveOBJFile(const char *filename, std::vector< Vec3f > &points, std::vector< Triangle > &triangles)
Definition: utility.cpp:162
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.
Definition: kIOS.cpp:181
double FCL_REAL
Definition: data_types.h:65
shared_ptr< OcTree > OcTreePtr_t
const Vec3f UnitX
Definition: utility.cpp:87
x
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)
Definition: utility.cpp:196
static std::ostream & operator<<(std::ostream &o, const Quaternion3f &q)
Definition: transform.h:48
FCL_REAL extents[6]
DistanceRequest request
Distance request.
std::string getNodeTypeName(NODE_TYPE node_type)
Definition: utility.cpp:325
const Vec3f & getTranslation() const
get translation
Definition: transform.h:100
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
CollisionResult result
Collision result.
R
Eigen::Quaternion< FCL_REAL > Quaternion3f
Definition: transform.h:46
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.
Definition: data_types.h:108
Triangle with 3 indices for points.
Definition: data_types.h:96
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
Definition: utility.cpp:80
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.
void start()
start timer
Definition: utility.cpp:35
double endTimeInMicroSec
ending time in micro-second
Definition: utility.h:97
void toEllipsoid(Vec3f &point, const Ellipsoid &ellipsoid)
Definition: utility.cpp:470
double getElapsedTimeInMicroSec()
get elapsed time in micro-second
Definition: utility.cpp:54
bool isCollision() const
return binary collision result
static Transform3f Identity()
Definition: transform.h:67
double getElapsedTimeInSec()
Definition: utility.cpp:76
c2
list a
const Vec3f UnitZ
Definition: utility.cpp:89
const Eigen::IOFormat vfmt
Definition: utility.cpp:82
bool done
Whether the collision iteration can stop.
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
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...
Convex polytope.
Definition: shape/convex.h:50
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&#39;s CollisionResult instance.
Distance data stores the distance request and the result given by distance algorithm.
int stopped
stop flag
Definition: utility.h:98
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...
Definition: utility.cpp:208
const Eigen::IOFormat pyfmt
Definition: utility.cpp:84


hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:02