utility.cpp
Go to the documentation of this file.
1 #include "utility.h"
2 
3 #include "coal/BV/BV.h"
4 #include "coal/BVH/BVH_model.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 
20 #ifdef _WIN32
21  QueryPerformanceFrequency(&frequency);
22  startCount.QuadPart = 0;
23  endCount.QuadPart = 0;
24 #else
25  startCount.tv_sec = startCount.tv_usec = 0;
26  endCount.tv_sec = endCount.tv_usec = 0;
27 #endif
28 
29  stopped = 0;
32 }
33 
35 
37  stopped = 0; // reset stop flag
38 #ifdef _WIN32
39  QueryPerformanceCounter(&startCount);
40 #else
41  gettimeofday(&startCount, NULL);
42 #endif
43 }
44 
46  stopped = 1; // set timer stopped flag
47 
48 #ifdef _WIN32
49  QueryPerformanceCounter(&endCount);
50 #else
51  gettimeofday(&endCount, NULL);
52 #endif
53 }
54 
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  if (!stopped) gettimeofday(&endCount, NULL);
63 
65  ((double)startCount.tv_sec * 1000000.0) + (double)startCount.tv_usec;
67  ((double)endCount.tv_sec * 1000000.0) + (double)endCount.tv_usec;
68 #endif
69 
71 }
72 
74  return this->getElapsedTimeInMicroSec() * 0.001;
75 }
76 
78  return this->getElapsedTimeInMicroSec() * 0.000001;
79 }
80 
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 
93  CoalScalar t = rand() / ((CoalScalar)RAND_MAX + 1);
94  return (t * (rmax - rmin) + rmin);
95 }
96 
97 void loadOBJFile(const char* filename, std::vector<Vec3s>& points,
98  std::vector<Triangle>& triangles) {
99  FILE* file = fopen(filename, "rb");
100  if (!file) {
101  std::cerr << "file not exist" << std::endl;
102  return;
103  }
104 
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;
111 
112  switch (first_token[0]) {
113  case 'v': {
114  if (first_token[1] == 'n') {
115  strtok(NULL, "\t ");
116  strtok(NULL, "\t ");
117  strtok(NULL, "\t ");
118  has_normal = true;
119  } else if (first_token[1] == 't') {
120  strtok(NULL, "\t ");
121  strtok(NULL, "\t ");
122  has_texture = true;
123  } else {
124  CoalScalar x = (CoalScalar)atof(strtok(NULL, "\t "));
125  CoalScalar y = (CoalScalar)atof(strtok(NULL, "\t "));
126  CoalScalar z = (CoalScalar)atof(strtok(NULL, "\t "));
127  Vec3s p(x, y, z);
128  points.push_back(p);
129  }
130  } break;
131  case 'f': {
132  Triangle tri;
133  char* data[30];
134  int n = 0;
135  while ((data[n] = strtok(NULL, "\t \r\n")) != NULL) {
136  if (strlen(data[n])) n++;
137  }
138 
139  for (int t = 0; t < (n - 2); ++t) {
140  if ((!has_texture) && (!has_normal)) {
141  tri[0] = (Triangle::index_type)(atoi(data[0]) - 1);
142  tri[1] = (Triangle::index_type)(atoi(data[1]) - 1);
143  tri[2] = (Triangle::index_type)(atoi(data[2]) - 1);
144  } else {
145  const char* v1;
146  for (Triangle::index_type i = 0; i < 3; i++) {
147  // vertex ID
148  if (i == 0)
149  v1 = data[0];
150  else
151  v1 = data[(Triangle::index_type)t + i];
152 
153  tri[i] = (Triangle::index_type)(atoi(v1) - 1);
154  }
155  }
156  triangles.push_back(tri);
157  }
158  }
159  }
160  }
161 }
162 
163 void saveOBJFile(const char* filename, std::vector<Vec3s>& points,
164  std::vector<Triangle>& 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 OcTree loadOctreeFile(const std::string& filename,
186  const CoalScalar& resolution) {
187  octomap::OcTreePtr_t octree(new octomap::OcTree(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());
193  }
194  return coal::OcTree(octree);
195 }
196 #endif
197 
199  CoalScalar c1 = cos(a);
200  CoalScalar c2 = cos(b);
201  CoalScalar c3 = cos(c);
202  CoalScalar s1 = sin(a);
203  CoalScalar s2 = sin(b);
204  CoalScalar s3 = sin(c);
205 
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;
208 }
209 
214 
215  const CoalScalar pi = 3.1415926;
216  CoalScalar a = rand_interval(0, 2 * pi);
217  CoalScalar b = rand_interval(0, 2 * pi);
218  CoalScalar c = rand_interval(0, 2 * pi);
219 
220  Matrix3s R;
221  eulerToMatrix(a, b, c, R);
222  Vec3s T(x, y, z);
223  transform.setTransform(R, T);
224 }
225 
227  std::vector<Transform3s>& transforms,
228  std::size_t n) {
229  transforms.resize(n);
230  for (std::size_t i = 0; i < n; ++i) {
234 
235  const CoalScalar pi = 3.1415926;
236  CoalScalar a = rand_interval(0, 2 * pi);
237  CoalScalar b = rand_interval(0, 2 * pi);
238  CoalScalar c = rand_interval(0, 2 * pi);
239 
240  {
241  Matrix3s R;
242  eulerToMatrix(a, b, c, R);
243  Vec3s T(x, y, z);
244  transforms[i].setTransform(R, T);
245  }
246  }
247 }
248 
250  CoalScalar delta_rot,
251  std::vector<Transform3s>& transforms,
252  std::vector<Transform3s>& transforms2,
253  std::size_t n) {
254  transforms.resize(n);
255  transforms2.resize(n);
256  for (std::size_t i = 0; i < n; ++i) {
260 
261  const CoalScalar pi = 3.1415926;
262  CoalScalar a = rand_interval(0, 2 * pi);
263  CoalScalar b = rand_interval(0, 2 * pi);
264  CoalScalar c = rand_interval(0, 2 * pi);
265 
266  {
267  Matrix3s R;
268  eulerToMatrix(a, b, c, R);
269  Vec3s T(x, y, z);
270  transforms[i].setTransform(R, T);
271  }
272 
273  CoalScalar deltax = rand_interval(-delta_trans[0], delta_trans[0]);
274  CoalScalar deltay = rand_interval(-delta_trans[1], delta_trans[1]);
275  CoalScalar deltaz = rand_interval(-delta_trans[2], delta_trans[2]);
276 
277  CoalScalar deltaa = rand_interval(-delta_rot, delta_rot);
278  CoalScalar deltab = rand_interval(-delta_rot, delta_rot);
279  CoalScalar deltac = rand_interval(-delta_rot, delta_rot);
280 
281  {
282  Matrix3s R;
283  eulerToMatrix(a + deltaa, b + deltab, c + deltac, R);
284  Vec3s T(x + deltax, y + deltay, z + deltaz);
285  transforms2[i].setTransform(R, T);
286  }
287  }
288 }
289 
290 bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2,
291  void* cdata_) {
292  CollisionData* cdata = static_cast<CollisionData*>(cdata_);
293  const CollisionRequest& request = cdata->request;
294  CollisionResult& result = cdata->result;
295 
296  if (cdata->done) return true;
297 
298  collide(o1, o2, request, result);
299 
300  if ((result.isCollision()) &&
301  (result.numContacts() >= request.num_max_contacts))
302  cdata->done = true;
303 
304  return cdata->done;
305 }
306 
307 bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2,
308  void* cdata_, CoalScalar& dist) {
309  DistanceData* cdata = static_cast<DistanceData*>(cdata_);
310  const DistanceRequest& request = cdata->request;
311  DistanceResult& result = cdata->result;
312 
313  if (cdata->done) {
314  dist = result.min_distance;
315  return true;
316  }
317 
318  distance(o1, o2, request, result);
319 
320  dist = result.min_distance;
321 
322  if (dist <= 0) return true; // in collision or in touch
323 
324  return cdata->done;
325 }
326 
327 std::string getNodeTypeName(NODE_TYPE node_type) {
328  if (node_type == BV_UNKNOWN)
329  return std::string("BV_UNKNOWN");
330  else if (node_type == BV_AABB)
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");
336  else if (node_type == BV_kIOS)
337  return std::string("BV_kIOS");
338  else if (node_type == BV_OBBRSS)
339  return std::string("BV_OBBRSS");
340  else if (node_type == BV_KDOP16)
341  return std::string("BV_KDOP16");
342  else if (node_type == BV_KDOP18)
343  return std::string("BV_KDOP18");
344  else if (node_type == BV_KDOP24)
345  return std::string("BV_KDOP24");
346  else if (node_type == GEOM_BOX)
347  return std::string("GEOM_BOX");
348  else if (node_type == GEOM_SPHERE)
349  return std::string("GEOM_SPHERE");
350  else if (node_type == GEOM_CAPSULE)
351  return std::string("GEOM_CAPSULE");
352  else if (node_type == GEOM_CONE)
353  return std::string("GEOM_CONE");
354  else if (node_type == GEOM_CYLINDER)
355  return std::string("GEOM_CYLINDER");
356  else if (node_type == GEOM_CONVEX)
357  return std::string("GEOM_CONVEX");
358  else if (node_type == GEOM_PLANE)
359  return std::string("GEOM_PLANE");
360  else if (node_type == GEOM_HALFSPACE)
361  return std::string("GEOM_HALFSPACE");
362  else if (node_type == GEOM_TRIANGLE)
363  return std::string("GEOM_TRIANGLE");
364  else if (node_type == GEOM_OCTREE)
365  return std::string("GEOM_OCTREE");
366  else
367  return std::string("invalid");
368 }
369 
371  Quatf q;
372  q.w() = w;
373  q.x() = x;
374  q.y() = y;
375  q.z() = z;
376  return q;
377 }
378 
379 std::ostream& operator<<(std::ostream& os, const Transform3s& tf) {
380  return os << "[ " << tf.getTranslation().format(vfmt) << ", "
381  << tf.getQuatRotation().coeffs().format(vfmt) << " ]";
382 }
383 
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);
389  return defaultValue;
390 }
391 
392 void generateEnvironments(std::vector<CollisionObject*>& env,
393  CoalScalar env_scale, std::size_t n) {
394  CoalScalar extents[] = {-env_scale, env_scale, -env_scale,
395  env_scale, -env_scale, env_scale};
396  std::vector<Transform3s> transforms(n);
397 
398  generateRandomTransforms(extents, transforms, n);
399  for (std::size_t i = 0; i < n; ++i) {
400  Box* box = new Box(5, 10, 20);
401  env.push_back(
402  new CollisionObject(shared_ptr<CollisionGeometry>(box), transforms[i]));
403  env.back()->collisionGeometry()->computeLocalAABB();
404  }
405 
406  generateRandomTransforms(extents, transforms, n);
407  for (std::size_t i = 0; i < n; ++i) {
408  Sphere* sphere = new Sphere(30);
409  env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(sphere),
410  transforms[i]));
411  env.back()->collisionGeometry()->computeLocalAABB();
412  }
413 
414  generateRandomTransforms(extents, transforms, n);
415  for (std::size_t i = 0; i < n; ++i) {
416  Cylinder* cylinder = new Cylinder(10, 40);
417  env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(cylinder),
418  transforms[i]));
419  env.back()->collisionGeometry()->computeLocalAABB();
420  }
421 }
422 
423 void generateEnvironmentsMesh(std::vector<CollisionObject*>& env,
424  CoalScalar env_scale, std::size_t n) {
425  CoalScalar extents[] = {-env_scale, env_scale, -env_scale,
426  env_scale, -env_scale, env_scale};
427  std::vector<Transform3s> transforms;
428 
429  generateRandomTransforms(extents, transforms, n);
430  Box box(5, 10, 20);
431  for (std::size_t i = 0; i < n; ++i) {
432  BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
434  env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(model),
435  transforms[i]));
436  env.back()->collisionGeometry()->computeLocalAABB();
437  }
438 
439  generateRandomTransforms(extents, transforms, n);
440  Sphere sphere(30);
441  for (std::size_t i = 0; i < n; ++i) {
442  BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
443  generateBVHModel(*model, sphere, Transform3s::Identity(), 16, 16);
444  env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(model),
445  transforms[i]));
446  env.back()->collisionGeometry()->computeLocalAABB();
447  }
448 
449  generateRandomTransforms(extents, transforms, n);
450  Cylinder cylinder(10, 40);
451  for (std::size_t i = 0; i < n; ++i) {
452  BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
453  generateBVHModel(*model, cylinder, Transform3s::Identity(), 16, 16);
454  env.push_back(new CollisionObject(shared_ptr<CollisionGeometry>(model),
455  transforms[i]));
456  env.back()->collisionGeometry()->computeLocalAABB();
457  }
458 }
459 
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),
463  Vec3s(-l, w, d), Vec3s(-l, w, -d), Vec3s(-l, -w, d),
464  Vec3s(-l, -w, -d)}));
465 
466  std::shared_ptr<std::vector<Quadrilateral>> polygons(
467  new std::vector<Quadrilateral>(6));
468  (*polygons)[0].set(0, 2, 3, 1); // x+ side
469  (*polygons)[1].set(2, 6, 7, 3); // y- side
470  (*polygons)[2].set(4, 5, 7, 6); // x- side
471  (*polygons)[3].set(0, 1, 5, 4); // y+ side
472  (*polygons)[4].set(1, 3, 7, 5); // z- side
473  (*polygons)[5].set(0, 2, 6, 4); // z+ side
474 
475  return Convex<Quadrilateral>(pts, // points
476  8, // num points
477  polygons,
478  6 // number of polygons
479  );
480 }
481 
483 void toSphere(Vec3s& point) {
484  assert(point.norm() > 1e-8);
485  point /= point.norm();
486 }
487 
494 void toEllipsoid(Vec3s& point, const Ellipsoid& ellipsoid) {
495  toSphere(point);
496  point[0] *= ellipsoid.radii[0];
497  point[1] *= ellipsoid.radii[1];
498  point[2] *= ellipsoid.radii[2];
499 }
500 
502  CoalScalar PHI = (1 + std::sqrt(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  }));
521 
522  std::vector<Vec3s>& pts_ = *pts;
523  for (size_t i = 0; i < 12; ++i) {
524  toEllipsoid(pts_[i], ellipsoid);
525  }
526 
527  // faces
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);
534 
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);
540 
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);
546 
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);
552  return Convex<Triangle>(pts, // points
553  12, // num_points
554  tris, // triangles
555  20 // number of triangles
556  );
557 }
558 
559 Box makeRandomBox(CoalScalar min_size, CoalScalar max_size) {
560  return Box(Vec3s(rand_interval(min_size, max_size),
561  rand_interval(min_size, max_size),
562  rand_interval(min_size, max_size)));
563 }
564 
566  return Sphere(rand_interval(min_size, max_size));
567 }
568 
570  return Ellipsoid(Vec3s(rand_interval(min_size, max_size),
571  rand_interval(min_size, max_size),
572  rand_interval(min_size, max_size)));
573 }
574 
575 Capsule makeRandomCapsule(std::array<CoalScalar, 2> min_size,
576  std::array<CoalScalar, 2> max_size) {
577  return Capsule(rand_interval(min_size[0], max_size[0]),
578  rand_interval(min_size[1], max_size[1]));
579 }
580 
581 Cone makeRandomCone(std::array<CoalScalar, 2> min_size,
582  std::array<CoalScalar, 2> max_size) {
583  return Cone(rand_interval(min_size[0], max_size[0]),
584  rand_interval(min_size[1], max_size[1]));
585 }
586 
587 Cylinder makeRandomCylinder(std::array<CoalScalar, 2> min_size,
588  std::array<CoalScalar, 2> max_size) {
589  return Cylinder(rand_interval(min_size[0], max_size[0]),
590  rand_interval(min_size[1], max_size[1]));
591 }
592 
594  Ellipsoid ellipsoid = makeRandomEllipsoid(min_size, max_size);
595  return constructPolytopeFromEllipsoid(ellipsoid);
596 }
597 
599  return Plane(Vec3s::Random().normalized(), rand_interval(min_size, max_size));
600 }
601 
603  return Halfspace(Vec3s::Random().normalized(),
604  rand_interval(min_size, max_size));
605 }
606 
607 std::shared_ptr<ShapeBase> makeRandomGeometry(NODE_TYPE node_type) {
608  switch (node_type) {
609  case GEOM_TRIANGLE:
610  COAL_THROW_PRETTY(std::string(COAL_PRETTY_FUNCTION) + " for " +
611  std::string(get_node_type_name(node_type)) +
612  " is not yet implemented.",
613  std::invalid_argument);
614  break;
615  case GEOM_BOX:
616  return std::make_shared<Box>(makeRandomBox(0.1, 1.0));
617  break;
618  case GEOM_SPHERE:
619  return std::make_shared<Sphere>(makeRandomSphere(0.1, 1.0));
620  break;
621  case GEOM_ELLIPSOID:
622  return std::make_shared<Ellipsoid>(makeRandomEllipsoid(0.1, 1.0));
623  break;
624  case GEOM_CAPSULE:
625  return std::make_shared<Capsule>(
626  makeRandomCapsule({0.1, 0.2}, {0.8, 1.0}));
627  break;
628  case GEOM_CONE:
629  return std::make_shared<Cone>(makeRandomCone({0.1, 0.2}, {0.8, 1.0}));
630  break;
631  case GEOM_CYLINDER:
632  return std::make_shared<Cylinder>(
633  makeRandomCylinder({0.1, 0.2}, {0.8, 1.0}));
634  break;
635  case GEOM_CONVEX:
636  return std::make_shared<Convex<Triangle>>(makeRandomConvex(0.1, 1.0));
637  break;
638  case GEOM_PLANE:
639  return std::make_shared<Plane>(makeRandomPlane(0.1, 1.0));
640  break;
641  case GEOM_HALFSPACE:
642  return std::make_shared<Halfspace>(makeRandomHalfspace(0.1, 1.0));
643  break;
644  default:
645  COAL_THROW_PRETTY(std::string(get_node_type_name(node_type)) +
646  " is not a primitive shape.",
647  std::invalid_argument);
648  break;
649  }
650 }
651 
652 } // namespace coal
coal::BV_UNKNOWN
@ BV_UNKNOWN
Definition: coal/collision_object.h:65
coal::rand_interval
CoalScalar rand_interval(CoalScalar rmin, CoalScalar rmax)
Definition: utility.cpp:92
coal::makeRandomGeometry
std::shared_ptr< ShapeBase > makeRandomGeometry(NODE_TYPE node_type)
Definition: utility.cpp:607
collision.h
coal::vfmt
const Eigen::IOFormat vfmt
Definition: utility.cpp:83
coal::BV_AABB
@ BV_AABB
Definition: coal/collision_object.h:66
coal::BenchTimer::getElapsedTime
double getElapsedTime()
get elapsed time in milli-second
Definition: utility.cpp:81
coal::Vec3s
Eigen::Matrix< CoalScalar, 3, 1 > Vec3s
Definition: coal/data_types.h:77
coal::generateRandomTransform
void generateRandomTransform(CoalScalar extents[6], Transform3s &transform)
Generate one random transform whose translation is constrained by extents and rotation without constr...
Definition: utility.cpp:210
coal::BV_KDOP24
@ BV_KDOP24
Definition: coal/collision_object.h:73
collision_manager.box
box
Definition: collision_manager.py:11
coal::Transform3s::getQuatRotation
Quatf getQuatRotation() const
get quaternion
Definition: coal/math/transform.h:119
coal::BV_kIOS
@ BV_kIOS
Definition: coal/collision_object.h:69
y
y
coal::makeRandomCone
Cone makeRandomCone(std::array< CoalScalar, 2 > min_size, std::array< CoalScalar, 2 > max_size)
Definition: utility.cpp:581
coal::BV_OBBRSS
@ BV_OBBRSS
Definition: coal/collision_object.h:70
coal::BenchTimer::startTimeInMicroSec
double startTimeInMicroSec
starting time in micro-second
Definition: utility.h:101
coal::GEOM_CONE
@ GEOM_CONE
Definition: coal/collision_object.h:77
collision_manager.sphere
sphere
Definition: collision_manager.py:4
coal::BenchTimer::~BenchTimer
~BenchTimer()
Definition: utility.cpp:34
coal::BenchTimer::stop
void stop()
stop the timer
Definition: utility.cpp:45
coal::Halfspace
Half Space: this is equivalent to the Plane in ODE. A Half space has a priviledged direction: the dir...
Definition: coal/shape/geometric_shapes.h:892
coal::GEOM_CONVEX
@ GEOM_CONVEX
Definition: coal/collision_object.h:79
coal::makeRandomConvex
Convex< Triangle > makeRandomConvex(CoalScalar min_size, CoalScalar max_size)
Definition: utility.cpp:593
coal::generateEnvironments
void generateEnvironments(std::vector< CollisionObject * > &env, CoalScalar env_scale, std::size_t n)
Definition: utility.cpp:392
coal::generateEnvironmentsMesh
void generateEnvironmentsMesh(std::vector< CollisionObject * > &env, CoalScalar env_scale, std::size_t n)
Definition: utility.cpp:423
coal::makeRandomHalfspace
Halfspace makeRandomHalfspace(CoalScalar min_size, CoalScalar max_size)
Definition: utility.cpp:602
coal::NODE_TYPE
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: coal/collision_object.h:64
coal::Capsule
Capsule It is where is the distance between the point x and the capsule segment AB,...
Definition: coal/shape/geometric_shapes.h:383
data
data
coal::BV_RSS
@ BV_RSS
Definition: coal/collision_object.h:68
coal::getNodeTypeName
std::string getNodeTypeName(NODE_TYPE node_type)
Definition: utility.cpp:327
coal
Main namespace.
Definition: coal/broadphase/broadphase_bruteforce.h:44
coal::Quatf
Eigen::Quaternion< CoalScalar > Quatf
Definition: coal/math/transform.h:47
coal::makeRandomSphere
Sphere makeRandomSphere(CoalScalar min_size, CoalScalar max_size)
Definition: utility.cpp:565
coal::Triangle::index_type
std::size_t index_type
Definition: coal/data_types.h:113
utility.h
coal::GEOM_CAPSULE
@ GEOM_CAPSULE
Definition: coal/collision_object.h:76
coal::makeRandomBox
Box makeRandomBox(CoalScalar min_size, CoalScalar max_size)
Definition: utility.cpp:559
coal::buildBox
Convex< Quadrilateral > buildBox(CoalScalar l, CoalScalar w, CoalScalar d)
Constructs a box with halfsides (l, w, d), centered around 0. The box is 2*l wide on the x-axis,...
Definition: utility.cpp:460
coal::BenchTimer::endCount
timeval endCount
Definition: utility.h:110
R
R
octree
Definition: octree.py:1
coal::BV_KDOP18
@ BV_KDOP18
Definition: coal/collision_object.h:72
coal::Ellipsoid
Ellipsoid centered at point zero.
Definition: coal/shape/geometric_shapes.h:305
coal::distance
COAL_DLLAPI CoalScalar distance(const Matrix3s &R0, const Vec3s &T0, const kIOS &b1, const kIOS &b2, Vec3s *P=NULL, Vec3s *Q=NULL)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS.cpp:180
coal::GEOM_SPHERE
@ GEOM_SPHERE
Definition: coal/collision_object.h:75
BV.h
coal::BenchTimer::getElapsedTimeInMilliSec
double getElapsedTimeInMilliSec()
get elapsed time in milli-second
Definition: utility.cpp:73
coal::BenchTimer::endTimeInMicroSec
double endTimeInMicroSec
ending time in micro-second
Definition: utility.h:102
coal::Ellipsoid::radii
Vec3s radii
Radii of the Ellipsoid (such that on boundary: x^2/rx^2 + y^2/ry^2.
Definition: coal/shape/geometric_shapes.h:318
coal::Plane
Infinite plane. A plane can be viewed as two half spaces; it has no priviledged direction....
Definition: coal/shape/geometric_shapes.h:983
a
list a
coal::Box
Center at zero point, axis aligned box.
Definition: coal/shape/geometric_shapes.h:166
coal::constructPolytopeFromEllipsoid
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:501
coal::Sphere
Center at zero point sphere.
Definition: coal/shape/geometric_shapes.h:240
coal::Transform3s
Simple transform class used locally by InterpMotion.
Definition: coal/math/transform.h:55
coal::operator<<
static std::ostream & operator<<(std::ostream &o, const Quatf &q)
Definition: coal/math/transform.h:49
coal::BenchTimer::BenchTimer
BenchTimer()
Definition: utility.cpp:19
coal::UnitZ
const Vec3s UnitZ
Definition: utility.cpp:90
coal::eulerToMatrix
void eulerToMatrix(CoalScalar a, CoalScalar b, CoalScalar c, Matrix3s &R)
Definition: utility.cpp:198
BVH_model.h
coal::GEOM_PLANE
@ GEOM_PLANE
Definition: coal/collision_object.h:80
coal::defaultCollisionFunction
bool defaultCollisionFunction(CollisionObject *o1, CollisionObject *o2, void *data)
Provides a simple callback for the collision query in the BroadPhaseCollisionManager....
Definition: default_broadphase_callbacks.cpp:42
coal::GEOM_ELLIPSOID
@ GEOM_ELLIPSOID
Definition: coal/collision_object.h:84
d
d
c
c
coal::GEOM_HALFSPACE
@ GEOM_HALFSPACE
Definition: coal/collision_object.h:81
collision-bench.filename
filename
Definition: collision-bench.py:6
COAL_PRETTY_FUNCTION
#define COAL_PRETTY_FUNCTION
Definition: include/coal/fwd.hh:53
fwd.hh
coal::transform
COAL_DLLAPI Halfspace transform(const Halfspace &a, const Transform3s &tf)
Definition: geometric_shapes_utility.cpp:248
COAL_THROW_PRETTY
#define COAL_THROW_PRETTY(message, exception)
Definition: include/coal/fwd.hh:64
coal::BenchTimer::stopped
int stopped
stop flag
Definition: utility.h:103
coal::makeRandomPlane
Plane makeRandomPlane(CoalScalar min_size, CoalScalar max_size)
Definition: utility.cpp:598
coal::GEOM_TRIANGLE
@ GEOM_TRIANGLE
Definition: coal/collision_object.h:82
coal::Transform3s::Identity
static Transform3s Identity()
Definition: coal/math/transform.h:68
x
x
distance.h
coal::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: coal/collision_object.h:214
coal::Convex
Convex polytope.
Definition: coal/serialization/collision_object.h:51
coal::UnitX
const Vec3s UnitX
Definition: utility.cpp:88
coal::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: coal/shape/geometric_shapes.h:560
q
q
coal::generateRandomTransforms
void generateRandomTransforms(CoalScalar extents[6], std::vector< Transform3s > &transforms, std::size_t n)
Generate n random transforms whose translations are constrained by extents.
Definition: utility.cpp:226
extents
CoalScalar extents[6]
Definition: test/geometric_shapes.cpp:53
coal::toSphere
void toSphere(Vec3s &point)
Takes a point and projects it onto the surface of the unit sphere.
Definition: utility.cpp:483
coal::GEOM_CYLINDER
@ GEOM_CYLINDER
Definition: coal/collision_object.h:78
generate_distance_plot.b
float b
Definition: generate_distance_plot.py:7
coal::OcTreePtr_t
shared_ptr< OcTree > OcTreePtr_t
Definition: include/coal/fwd.hh:144
coal::get_node_type_name
const char * get_node_type_name(NODE_TYPE node_type)
Returns the name associated to a NODE_TYPE.
Definition: coal/collision_utility.h:32
coal::defaultDistanceFunction
bool defaultDistanceFunction(CollisionObject *o1, CollisionObject *o2, void *data, CoalScalar &dist)
Collision data for use with the DefaultContinuousCollisionFunction. It stores the collision request a...
Definition: default_broadphase_callbacks.cpp:66
coal::Cone
Cone The base of the cone is at and the top is at .
Definition: coal/shape/geometric_shapes.h:467
coal::GEOM_OCTREE
@ GEOM_OCTREE
Definition: coal/collision_object.h:83
coal::pyfmt
const Eigen::IOFormat pyfmt
Definition: utility.cpp:85
coal::makeQuat
Quatf makeQuat(CoalScalar w, CoalScalar x, CoalScalar y, CoalScalar z)
Definition: utility.cpp:370
coal::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: coal/BVH/BVH_model.h:314
coal::OcTree
Octree is one type of collision geometry which can encode uncertainty information in the sensor data.
Definition: coal/octree.h:53
coal::Matrix3s
Eigen::Matrix< CoalScalar, 3, 3 > Matrix3s
Definition: coal/data_types.h:81
coal::saveOBJFile
void saveOBJFile(const char *filename, std::vector< Vec3s > &points, std::vector< Triangle > &triangles)
Definition: utility.cpp:163
coal::BenchTimer::start
void start()
start timer
Definition: utility.cpp:36
coal::BenchTimer::getElapsedTimeInSec
double getElapsedTimeInSec()
Definition: utility.cpp:77
collision_utility.h
geometric_shape_to_BVH_model.h
coal::UnitY
const Vec3s UnitY
Definition: utility.cpp:89
coal::makeRandomEllipsoid
Ellipsoid makeRandomEllipsoid(CoalScalar min_size, CoalScalar max_size)
Definition: utility.cpp:569
coal::collide
COAL_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,...
Definition: src/collision.cpp:61
coal::BV_OBB
@ BV_OBB
Definition: coal/collision_object.h:67
coal::BenchTimer::startCount
timeval startCount
Definition: utility.h:109
pi
constexpr CoalScalar pi
Definition: collision_node_asserts.cpp:10
c2
c2
coal::getNbRun
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:384
coal::Triangle
Triangle with 3 indices for points.
Definition: coal/data_types.h:111
coal::GEOM_BOX
@ GEOM_BOX
Definition: coal/collision_object.h:74
coal::toEllipsoid
void toEllipsoid(Vec3s &point, const Ellipsoid &ellipsoid)
Definition: utility.cpp:494
t
dictionary t
coal::BenchTimer::getElapsedTimeInMicroSec
double getElapsedTimeInMicroSec()
get elapsed time in micro-second
Definition: utility.cpp:55
coal::makeRandomCylinder
Cylinder makeRandomCylinder(std::array< CoalScalar, 2 > min_size, std::array< CoalScalar, 2 > max_size)
Definition: utility.cpp:587
coal::CoalScalar
double CoalScalar
Definition: coal/data_types.h:76
coal::makeRandomCapsule
Capsule makeRandomCapsule(std::array< CoalScalar, 2 > min_size, std::array< CoalScalar, 2 > max_size)
Definition: utility.cpp:575
coal::generateBVHModel
void generateBVHModel(BVHModel< BV > &model, const Box &shape, const Transform3s &pose)
Generate BVH model from box.
Definition: coal/shape/geometric_shape_to_BVH_model.h:49
coal::loadOBJFile
void loadOBJFile(const char *filename, std::vector< Vec3s > &points, std::vector< Triangle > &triangles)
Load an obj mesh file.
Definition: utility.cpp:97
coal::BV_KDOP16
@ BV_KDOP16
Definition: coal/collision_object.h:71
coal::Transform3s::getTranslation
const Vec3s & getTranslation() const
get translation
Definition: coal/math/transform.h:101


hpp-fcl
Author(s):
autogenerated on Sat Nov 23 2024 03:44:59