test_fcl_utility.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #ifndef TEST_FCL_UTILITY_H
39 #define TEST_FCL_UTILITY_H
40 
41 #include <array>
42 #include <fstream>
43 #include <iostream>
44 
45 #include "fcl/common/unused.h"
46 
47 #include "fcl/math/constants.h"
48 #include "fcl/math/triangle.h"
49 
50 #include "fcl/geometry/shape/box.h"
55 
63 
64 #ifdef _WIN32
65 #define NOMINMAX // required to avoid compilation errors with Visual Studio 2010
66 #include <windows.h>
67 #else
68 #include <sys/time.h>
69 #endif
70 
71 namespace fcl
72 {
73 
74 namespace test
75 {
76 
77 class Timer
78 {
79 public:
80  Timer();
81  ~Timer();
82 
83  void start();
84  void stop();
85  double getElapsedTime();
86  double getElapsedTimeInSec();
87  double getElapsedTimeInMilliSec();
88  double getElapsedTimeInMicroSec();
89 
90 private:
93  int stopped;
94 #ifdef _WIN32
95  LARGE_INTEGER frequency;
96  LARGE_INTEGER startCount;
97  LARGE_INTEGER endCount;
98 #else
99  timeval startCount;
100  timeval endCount;
101 #endif
102 };
103 
104 struct TStruct
105 {
106  std::vector<double> records;
107  double overall_time;
108 
110 
111  void push_back(double t)
112  {
113  records.push_back(t);
114  overall_time += t;
115  }
116 };
117 
119 template <typename S>
120 void loadOBJFile(const char* filename, std::vector<Vector3<S>>& points, std::vector<Triangle>& triangles);
121 
122 template <typename S>
123 void saveOBJFile(const char* filename, std::vector<Vector3<S>>& points, std::vector<Triangle>& triangles);
124 
125 template <typename S>
126 S rand_interval(S rmin, S rmax);
127 
128 template <typename S>
129 void eulerToMatrix(S a, S b, S c, Matrix3<S>& R);
130 
133 template <typename S>
135 
137 template <typename S>
138 void generateRandomTransforms(S extents[6], aligned_vector<Transform3<S>>& transforms, std::size_t n);
139 
141 template <typename S>
142 void generateRandomTransforms(S extents[6], S delta_trans[3], S delta_rot, aligned_vector<Transform3<S>>& transforms, aligned_vector<Transform3<S>>& transforms2, std::size_t n);
143 
145 template <typename S>
146 void generateRandomTransforms_ccd(S extents[6], aligned_vector<Transform3<S>>& transforms, aligned_vector<Transform3<S>>& transforms2, S delta_trans[3], S delta_rot, std::size_t n,
147  const std::vector<Vector3<S>>& vertices1, const std::vector<Triangle>& triangles1,
148  const std::vector<Vector3<S>>& vertices2, const std::vector<Triangle>& triangles2);
149 
151 template <typename S>
152 void generateEnvironments(std::vector<CollisionObject<S>*>& env, S env_scale, std::size_t n);
153 
155 template <typename S>
156 void generateEnvironmentsMesh(std::vector<CollisionObject<S>*>& env, S env_scale, std::size_t n);
157 
159 template <typename S>
161 {
165 };
166 
168 
170 
171 #if FCL_HAVE_OCTOMAP
172 
174 template <typename S>
175 void generateBoxesFromOctomap(std::vector<CollisionObject<S>*>& env, OcTree<S>& tree);
176 
178 template <typename S>
179 void generateBoxesFromOctomapMesh(std::vector<CollisionObject<S>*>& env, OcTree<S>& tree);
180 
182 octomap::OcTree* generateOcTree(double resolution = 0.1);
183 
184 #endif
185 
186 //============================================================================//
187 // //
188 // Implementations //
189 // //
190 //============================================================================//
191 
192 //==============================================================================
193 template <typename S>
194 void loadOBJFile(const char* filename, std::vector<Vector3<S>>& points, std::vector<Triangle>& triangles)
195 {
196 
197  FILE* file = fopen(filename, "rb");
198  if(!file)
199  {
200  std::cerr << "file not exist" << std::endl;
201  return;
202  }
203 
204  bool has_normal = false;
205  bool has_texture = false;
206  char line_buffer[2000];
207  while(fgets(line_buffer, 2000, file))
208  {
209  char* first_token = strtok(line_buffer, "\r\n\t ");
210  if(!first_token || first_token[0] == '#' || first_token[0] == 0)
211  continue;
212 
213  switch(first_token[0])
214  {
215  case 'v':
216  {
217  if(first_token[1] == 'n')
218  {
219  strtok(nullptr, "\t ");
220  strtok(nullptr, "\t ");
221  strtok(nullptr, "\t ");
222  has_normal = true;
223  }
224  else if(first_token[1] == 't')
225  {
226  strtok(nullptr, "\t ");
227  strtok(nullptr, "\t ");
228  has_texture = true;
229  }
230  else
231  {
232  S x = (S)atof(strtok(nullptr, "\t "));
233  S y = (S)atof(strtok(nullptr, "\t "));
234  S z = (S)atof(strtok(nullptr, "\t "));
235  points.emplace_back(x, y, z);
236  }
237  }
238  break;
239  case 'f':
240  {
241  Triangle tri;
242  char* data[30];
243  int n = 0;
244  while((data[n] = strtok(nullptr, "\t \r\n")) != nullptr)
245  {
246  if(strlen(data[n]))
247  n++;
248  }
249 
250  for(int t = 0; t < (n - 2); ++t)
251  {
252  if((!has_texture) && (!has_normal))
253  {
254  tri[0] = atoi(data[0]) - 1;
255  tri[1] = atoi(data[1]) - 1;
256  tri[2] = atoi(data[2]) - 1;
257  }
258  else
259  {
260  const char *v1;
261  for(int i = 0; i < 3; i++)
262  {
263  // vertex ID
264  if(i == 0)
265  v1 = data[0];
266  else
267  v1 = data[t + i];
268 
269  tri[i] = atoi(v1) - 1;
270  }
271  }
272  triangles.push_back(tri);
273  }
274  }
275  }
276  }
277 }
278 
279 //==============================================================================
280 template <typename S>
281 void saveOBJFile(const char* filename, std::vector<Vector3<S>>& points, std::vector<Triangle>& triangles)
282 {
283  std::ofstream os(filename);
284  if(!os)
285  {
286  std::cerr << "file not exist" << std::endl;
287  return;
288  }
289 
290  for(std::size_t i = 0; i < points.size(); ++i)
291  {
292  os << "v " << points[i][0] << " " << points[i][1] << " " << points[i][2] << std::endl;
293  }
294 
295  for(std::size_t i = 0; i < triangles.size(); ++i)
296  {
297  os << "f " << triangles[i][0] + 1 << " " << triangles[i][1] + 1 << " " << triangles[i][2] + 1 << std::endl;
298  }
299 
300  os.close();
301 }
302 
303 //==============================================================================
304 template <typename S>
305 S rand_interval(S rmin, S rmax)
306 {
307  S t = rand() / ((S)RAND_MAX + 1);
308  return (t * (rmax - rmin) + rmin);
309 }
310 
311 //==============================================================================
312 template <typename S>
313 void eulerToMatrix(S a, S b, S c, Matrix3<S>& R)
314 {
315  auto c1 = std::cos(a);
316  auto c2 = std::cos(b);
317  auto c3 = std::cos(c);
318  auto s1 = std::sin(a);
319  auto s2 = std::sin(b);
320  auto s3 = std::sin(c);
321 
322  R << c1 * c2, - c2 * s1, s2,
323  c3 * s1 + c1 * s2 * s3, c1 * c3 - s1 * s2 * s3, - c2 * s3,
324  s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3, c2 * c3;
325 }
326 
327 //==============================================================================
328 template <typename S>
330 {
331  auto x = rand_interval(extents[0], extents[3]);
332  auto y = rand_interval(extents[1], extents[4]);
333  auto z = rand_interval(extents[2], extents[5]);
334 
335  const auto pi = constants<S>::pi();
336  auto a = rand_interval((S)0, 2 * pi);
337  auto b = rand_interval((S)0, 2 * pi);
338  auto c = rand_interval((S)0, 2 * pi);
339 
340  Matrix3<S> R;
341  eulerToMatrix(a, b, c, R);
342  Vector3<S> T(x, y, z);
343  transform.linear() = R;
344  transform.translation() = T;
345 }
346 
347 //==============================================================================
348 template <typename S>
349 void generateRandomTransforms(S extents[6], aligned_vector<Transform3<S>>& transforms, std::size_t n)
350 {
351  transforms.resize(n);
352  for(std::size_t i = 0; i < n; ++i)
353  {
354  auto x = rand_interval(extents[0], extents[3]);
355  auto y = rand_interval(extents[1], extents[4]);
356  auto z = rand_interval(extents[2], extents[5]);
357 
358  const auto pi = constants<S>::pi();
359  auto a = rand_interval((S)0, 2 * pi);
360  auto b = rand_interval((S)0, 2 * pi);
361  auto c = rand_interval((S)0, 2 * pi);
362 
363  {
364  Matrix3<S> R;
365  eulerToMatrix(a, b, c, R);
366  Vector3<S> T(x, y, z);
367  transforms[i].setIdentity();
368  transforms[i].linear() = R;
369  transforms[i].translation() = T;
370  }
371  }
372 }
373 
374 //==============================================================================
375 template <typename S>
376 void generateRandomTransforms(S extents[6], S delta_trans[3], S delta_rot, aligned_vector<Transform3<S>>& transforms, aligned_vector<Transform3<S>>& transforms2, std::size_t n)
377 {
378  transforms.resize(n);
379  transforms2.resize(n);
380  for(std::size_t i = 0; i < n; ++i)
381  {
382  auto x = rand_interval(extents[0], extents[3]);
383  auto y = rand_interval(extents[1], extents[4]);
384  auto z = rand_interval(extents[2], extents[5]);
385 
386  const auto pi = constants<S>::pi();
387  auto a = rand_interval((S)0, 2 * pi);
388  auto b = rand_interval((S)0, 2 * pi);
389  auto c = rand_interval((S)0, 2 * pi);
390 
391  {
392  Matrix3<S> R;
393  eulerToMatrix(a, b, c, R);
394  Vector3<S> T(x, y, z);
395  transforms[i].setIdentity();
396  transforms[i].linear() = R;
397  transforms[i].translation() = T;
398  }
399 
400  auto deltax = rand_interval(-delta_trans[0], delta_trans[0]);
401  auto deltay = rand_interval(-delta_trans[1], delta_trans[1]);
402  auto deltaz = rand_interval(-delta_trans[2], delta_trans[2]);
403 
404  auto deltaa = rand_interval(-delta_rot, delta_rot);
405  auto deltab = rand_interval(-delta_rot, delta_rot);
406  auto deltac = rand_interval(-delta_rot, delta_rot);
407 
408  {
409  Matrix3<S> R;
410  eulerToMatrix(a + deltaa, b + deltab, c + deltac, R);
411  Vector3<S> T(x + deltax, y + deltay, z + deltaz);
412  transforms2[i].setIdentity();
413  transforms2[i].linear() = R;
414  transforms2[i].translation() = T;
415  }
416  }
417 }
418 
419 //==============================================================================
420 template <typename S>
421 void generateRandomTransforms_ccd(S extents[6], aligned_vector<Transform3<S>>& transforms, aligned_vector<Transform3<S>>& transforms2, S delta_trans[3], S delta_rot, std::size_t n,
422  const std::vector<Vector3<S>>& vertices1, const std::vector<Triangle>& triangles1,
423  const std::vector<Vector3<S>>& vertices2, const std::vector<Triangle>& triangles2)
424 {
425  FCL_UNUSED(vertices1);
426  FCL_UNUSED(triangles1);
427  FCL_UNUSED(vertices2);
428  FCL_UNUSED(triangles2);
429 
430  transforms.resize(n);
431  transforms2.resize(n);
432 
433  for(std::size_t i = 0; i < n;)
434  {
435  auto x = rand_interval(extents[0], extents[3]);
436  auto y = rand_interval(extents[1], extents[4]);
437  auto z = rand_interval(extents[2], extents[5]);
438 
439  const auto pi = constants<S>::pi();
440  auto a = rand_interval(0, 2 * pi);
441  auto b = rand_interval(0, 2 * pi);
442  auto c = rand_interval(0, 2 * pi);
443 
444 
445  Matrix3<S> R;
446  eulerToMatrix(a, b, c, R);
447  Vector3<S> T(x, y, z);
449  tf.linear() = R;
450  tf.translation() = T;
451 
452  std::vector<std::pair<int, int> > results;
453  {
454  transforms[i] = tf;
455 
456  auto deltax = rand_interval(-delta_trans[0], delta_trans[0]);
457  auto deltay = rand_interval(-delta_trans[1], delta_trans[1]);
458  auto deltaz = rand_interval(-delta_trans[2], delta_trans[2]);
459 
460  auto deltaa = rand_interval(-delta_rot, delta_rot);
461  auto deltab = rand_interval(-delta_rot, delta_rot);
462  auto deltac = rand_interval(-delta_rot, delta_rot);
463 
464  Matrix3<S> R2;
465  eulerToMatrix(a + deltaa, b + deltab, c + deltac, R2);
466  Vector3<S> T2(x + deltax, y + deltay, z + deltaz);
467  transforms2[i].linear() = R2;
468  transforms2[i].translation() = T2;
469  ++i;
470  }
471  }
472 }
473 
474 //==============================================================================
475 template <typename S>
476 void generateEnvironments(std::vector<CollisionObject<S>*>& env, S env_scale, std::size_t n)
477 {
478  S extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale};
479  aligned_vector<Transform3<S>> transforms;
480 
481  generateRandomTransforms(extents, transforms, n);
482  for(std::size_t i = 0; i < n; ++i)
483  {
484  Box<S>* box = new Box<S>(5, 10, 20);
485  env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(box), transforms[i]));
486  }
487 
488  generateRandomTransforms(extents, transforms, n);
489  for(std::size_t i = 0; i < n; ++i)
490  {
491  Sphere<S>* sphere = new Sphere<S>(30);
492  env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(sphere), transforms[i]));
493  }
494 
495  generateRandomTransforms(extents, transforms, n);
496  for(std::size_t i = 0; i < n; ++i)
497  {
498  Cylinder<S>* cylinder = new Cylinder<S>(10, 40);
499  env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(cylinder), transforms[i]));
500  }
501 }
502 
503 //==============================================================================
504 template <typename S>
505 void generateEnvironmentsMesh(std::vector<CollisionObject<S>*>& env, S env_scale, std::size_t n)
506 {
507  S extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale};
508  aligned_vector<Transform3<S>> transforms;
509 
510  generateRandomTransforms(extents, transforms, n);
511  Box<S> box(5, 10, 20);
512  for(std::size_t i = 0; i < n; ++i)
513  {
514  BVHModel<OBBRSS<S>>* model = new BVHModel<OBBRSS<S>>();
516  env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(model), transforms[i]));
517  }
518 
519  generateRandomTransforms(extents, transforms, n);
520  Sphere<S> sphere(30);
521  for(std::size_t i = 0; i < n; ++i)
522  {
523  BVHModel<OBBRSS<S>>* model = new BVHModel<OBBRSS<S>>();
524  generateBVHModel(*model, sphere, Transform3<S>::Identity(), 16, 16);
525  env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(model), transforms[i]));
526  }
527 
528  generateRandomTransforms(extents, transforms, n);
529  Cylinder<S> cylinder(10, 40);
530  for(std::size_t i = 0; i < n; ++i)
531  {
532  BVHModel<OBBRSS<S>>* model = new BVHModel<OBBRSS<S>>();
533  generateBVHModel(*model, cylinder, Transform3<S>::Identity(), 16, 16);
534  env.push_back(new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(model), transforms[i]));
535  }
536 }
537 
538 #if FCL_HAVE_OCTOMAP
539 
540 //==============================================================================
541 template <typename S>
542 void generateBoxesFromOctomap(std::vector<CollisionObject<S>*>& boxes, OcTree<S>& tree)
543 {
544  std::vector<std::array<S, 6> > boxes_ = tree.toBoxes();
545 
546  for(std::size_t i = 0; i < boxes_.size(); ++i)
547  {
548  S x = boxes_[i][0];
549  S y = boxes_[i][1];
550  S z = boxes_[i][2];
551  S size = boxes_[i][3];
552  S cost = boxes_[i][4];
553  S threshold = boxes_[i][5];
554 
555  Box<S>* box = new Box<S>(size, size, size);
556  box->cost_density = cost;
557  box->threshold_occupied = threshold;
559  boxes.push_back(obj);
560  }
561 
562  std::cout << "boxes size: " << boxes.size() << std::endl;
563 
564 }
565 
566 //==============================================================================
567 template <typename S>
568 void generateBoxesFromOctomapMesh(std::vector<CollisionObject<S>*>& boxes, OcTree<S>& tree)
569 {
570  std::vector<std::array<S, 6> > boxes_ = tree.toBoxes();
571 
572  for(std::size_t i = 0; i < boxes_.size(); ++i)
573  {
574  S x = boxes_[i][0];
575  S y = boxes_[i][1];
576  S z = boxes_[i][2];
577  S size = boxes_[i][3];
578  S cost = boxes_[i][4];
579  S threshold = boxes_[i][5];
580 
581  Box<S> box(size, size, size);
582  BVHModel<OBBRSS<S>>* model = new BVHModel<OBBRSS<S>>();
583  generateBVHModel(*model, box, Transform3<S>::Identity());
584  model->cost_density = cost;
585  model->threshold_occupied = threshold;
586  CollisionObject<S>* obj = new CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>(model), Transform3<S>(Translation3<S>(Vector3<S>(x, y, z))));
587  boxes.push_back(obj);
588  }
589 
590  std::cout << "boxes size: " << boxes.size() << std::endl;
591 }
592 
593 //==============================================================================
594 inline octomap::OcTree* generateOcTree(double resolution)
595 {
596  octomap::OcTree* tree = new octomap::OcTree(resolution);
597 
598  // insert some measurements of occupied cells
599  for(int x = -20; x < 20; x++)
600  {
601  for(int y = -20; y < 20; y++)
602  {
603  for(int z = -20; z < 20; z++)
604  {
605  tree->updateNode(octomap::point3d(x * 0.05, y * 0.05, z * 0.05), true);
606  }
607  }
608  }
609 
610  // insert some measurements of free cells
611  for(int x = -30; x < 30; x++)
612  {
613  for(int y = -30; y < 30; y++)
614  {
615  for(int z = -30; z < 30; z++)
616  {
617  tree->updateNode(octomap::point3d(x*0.02 -1.0, y*0.02-1.0, z*0.02-1.0), false);
618  }
619  }
620  }
621 
622  return tree;
623 }
624 
625 #endif // FCL_HAVE_OCTOMAP
626 
627 } // namespace test
628 } // namespace fcl
629 
630 #endif
fcl::test::Timer::getElapsedTimeInMilliSec
double getElapsedTimeInMilliSec()
get elapsed time in milli-second
Definition: test_fcl_utility.cpp:117
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::test::TStruct::push_back
void push_back(double t)
Definition: test_fcl_utility.h:111
fcl::test::Timer
Definition: test_fcl_utility.h:77
fcl::GJKSolverType
GJKSolverType
Type of narrow phase GJK solver.
Definition: gjk_solver_type.h:45
fcl::test::Timer::endTimeInMicroSec
double endTimeInMicroSec
ending time in micro-second
Definition: test_fcl_utility.h:92
fcl::test::TStruct::overall_time
double overall_time
Definition: test_fcl_utility.h:107
collision_object.h
fcl::test::loadOBJFile
void loadOBJFile(const char *filename, std::vector< Vector3< S >> &points, std::vector< Triangle > &triangles)
Load an obj mesh file.
Definition: test_fcl_utility.h:194
fcl::test::Timer::getElapsedTime
double getElapsedTime()
get elapsed time in milli-second
Definition: test_fcl_utility.cpp:129
fcl::test::TStruct::records
std::vector< double > records
Definition: test_fcl_utility.h:106
sphere.h
fcl::test::generateRandomTransforms_ccd
void generateRandomTransforms_ccd(S extents[6], aligned_vector< Transform3< S >> &transforms, aligned_vector< Transform3< S >> &transforms2, S delta_trans[3], S delta_rot, std::size_t n, const std::vector< Vector3< S >> &vertices1, const std::vector< Triangle > &triangles1, const std::vector< Vector3< S >> &vertices2, const std::vector< Triangle > &triangles2)
Generate n random tranforms and transform2 with addtional random translation/rotation....
Definition: test_fcl_utility.h:421
fcl::test::rand_interval
S rand_interval(S rmin, S rmax)
Definition: test_fcl_utility.h:305
fcl::test::Timer::~Timer
~Timer()
Definition: test_fcl_utility.cpp:69
triangle.h
fcl::CollisionGeometry< S_ >::cost_density
S_ cost_density
collision cost for unit volume
Definition: collision_geometry.h:102
fcl::test::Timer::start
void start()
start timer
Definition: test_fcl_utility.cpp:75
fcl::test::DistanceRes::p2
Vector3< S > p2
Definition: test_fcl_utility.h:164
fcl::test::Timer::getElapsedTimeInMicroSec
double getElapsedTimeInMicroSec()
get elapsed time in micro-second
Definition: test_fcl_utility.cpp:97
fcl::test::eulerToMatrix
void eulerToMatrix(S a, S b, S c, Matrix3< S > &R)
Definition: test_fcl_utility.h:313
unused.h
fcl::transform
template Halfspace< double > transform(const Halfspace< double > &a, const Transform3< double > &tf)
BVH_model.h
obj
CollisionObject< S > * obj
object
Definition: broadphase_SaP.h:164
fcl::test::Timer::getElapsedTimeInSec
double getElapsedTimeInSec()
get elapsed time in second (same as getElapsedTime)
Definition: test_fcl_utility.cpp:123
fcl::test::TStruct::TStruct
TStruct()
Definition: test_fcl_utility.h:109
fcl::Triangle
Triangle with 3 indices for points.
Definition: triangle.h:48
octomath::Vector3
testing::internal::string
::std::string string
Definition: gtest-port.h:875
octomap::OcTree
fcl::test::Timer::startTimeInMicroSec
double startTimeInMicroSec
starting time in micro-second
Definition: test_fcl_utility.h:91
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::test::TStruct
Definition: test_fcl_utility.h:104
continuous_collision_result.h
box.h
distance.h
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:85
fcl::CollisionGeometry
The geometry for the object for collision or distance computation.
Definition: collision_geometry.h:58
continuous_collision_object.h
fcl::test::DistanceRes
Structure for minimum distance between two meshes and the corresponding nearest point pair.
Definition: test_fcl_utility.h:160
fcl::test::getNodeTypeName
std::string getNodeTypeName(NODE_TYPE node_type)
Definition: test_fcl_utility.cpp:135
fcl::test::Timer::stopped
int stopped
stop flag
Definition: test_fcl_utility.h:93
fcl::test::Timer::startCount
timeval startCount
Definition: test_fcl_utility.h:99
fcl::Translation3
Eigen::Translation< S, 3 > Translation3
Definition: types.h:94
continuous_collision_request.h
fcl::test::saveOBJFile
void saveOBJFile(const char *filename, std::vector< Vector3< S >> &points, std::vector< Triangle > &triangles)
Definition: test_fcl_utility.h:281
fcl::test::Timer::Timer
Timer()
Definition: test_fcl_utility.cpp:52
fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:57
fcl::Cylinder
Center at zero cylinder.
Definition: cylinder.h:50
fcl::test::generateRandomTransforms
void generateRandomTransforms(S extents[6], aligned_vector< Transform3< S >> &transforms, std::size_t n)
Generate n random transforms whose translations are constrained by extents.
Definition: test_fcl_utility.h:349
fcl::test::Timer::endCount
timeval endCount
Definition: test_fcl_utility.h:100
collision_result.h
fcl::test::Timer::stop
void stop()
stop the timer
Definition: test_fcl_utility.cpp:86
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:50
FCL_UNUSED
#define FCL_UNUSED(x)
Definition: unused.h:39
constants.h
fcl::Sphere
Center at zero point sphere.
Definition: sphere.h:50
fcl::constants::pi
static constexpr S pi()
The mathematical constant pi.
Definition: constants.h:134
fcl::test::generateEnvironments
void generateEnvironments(std::vector< CollisionObject< S > * > &env, S env_scale, std::size_t n)
Generate environment with 3 * n objects: n boxes, n spheres and n cylinders.
Definition: test_fcl_utility.h:476
fcl::test::generateRandomTransform
void generateRandomTransform(S extents[6], Transform3< S > &transform)
Generate one random transform whose translation is constrained by extents and rotation without constr...
fcl::generateBVHModel
int generateBVHModel(BVHModel< BV > &model, const Box< typename BV::S > &shape, const Transform3< typename BV::S > &pose, FinalizeModel finalize_model)
Generate BVH model from box.
Definition: geometric_shape_to_BVH_model-inl.h:69
fcl::test::generateEnvironmentsMesh
void generateEnvironmentsMesh(std::vector< CollisionObject< S > * > &env, S env_scale, std::size_t n)
Generate environment with 3 * n objects, but all in meshes.
Definition: test_fcl_utility.h:505
fcl::test::DistanceRes::p1
Vector3< S > p1
Definition: test_fcl_utility.h:163
fcl::CollisionGeometry< S_ >::threshold_occupied
S_ threshold_occupied
threshold for occupied ( >= is occupied)
Definition: collision_geometry.h:105
fcl::test::DistanceRes::distance
S distance
Definition: test_fcl_utility.h:162
extents
std::array< S, 6 > & extents()
Definition: test_fcl_geometric_shapes.cpp:63
octree.h
fcl::NODE_TYPE
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_geometry.h:53
OccupancyOcTreeBase< OcTreeNode >::updateNode
virtual OcTreeNode * updateNode(const OcTreeKey &key, bool occupied, bool lazy_eval=false)
fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:51
fcl::test::getGJKSolverName
std::string getGJKSolverName(GJKSolverType solver_type)
Definition: test_fcl_utility.cpp:182
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
cylinder.h
fcl::aligned_vector
std::vector< _Tp, Eigen::aligned_allocator< _Tp > > aligned_vector
Definition: types.h:122
collision.h


fcl
Author(s):
autogenerated on Sat Sep 11 2021 02:37:42