Collision Checker
fcl_distance_queries.cc
Go to the documentation of this file.
1 #include "collision/solvers/fcl/fcl_distance_queries.h"
2 #include "collision/solvers/fcl/fcl_distance_requests.h"
3 //#include "collision/solverFCL/fcl_collision_object.h"
4 #include <fcl/broadphase/broadphase_collision_manager.h>
5 #include <fcl/narrowphase/distance.h>
6 namespace collision {
7 namespace solvers {
8 namespace solverFCL {
9 namespace fcl_primitive_queries {
10 int FCL_CalculateDistance(const FCLCollisionObject &obj1,
11  const FCLCollisionObject &obj2,
12  FCL_PRECISION &distance, double distance_tolerance) {
13  fcl::DistanceRequest<FCL_PRECISION> request;
14  request.gjk_solver_type = fcl::GST_LIBCCD;
15  request.distance_tolerance = distance_tolerance;
16  fcl::DistanceResult<FCL_PRECISION> result;
17  fcl::CollisionObject<FCL_PRECISION> *fcl_obj1 =
18  obj1.getCollisionObject_fcl().get();
19  fcl::CollisionObject<FCL_PRECISION> *fcl_obj2 =
20  obj2.getCollisionObject_fcl().get();
21  if (!fcl_obj1 || !fcl_obj2) {
22  return -1;
23  }
24  FCL_PRECISION retval = 1;
25 
26  if (fcl_obj1->collisionGeometry() == fcl_obj2->collisionGeometry()) {
27  retval = -2;
28  distance = get_max_distance();
29  } else {
30  retval = fcl::distance(fcl_obj1, fcl_obj2, request, result);
31 
32  distance = result.min_distance;
33  }
34 
35  return int(retval);
36 }
37 int FCL_DistanceTolerance(const FCLCollisionObject &obj1,
38  const FCLCollisionObject &obj2,
39  FCL_PRECISION &distance,
40  FCL_TOLERANCE_CHECK_TYPE check_type,
41  double distance_tolerance) {
42  fcl::DistanceRequest<FCL_PRECISION> request;
43  request.distance_tolerance = distance_tolerance;
44  request.enable_signed_distance = true;
45  request.enable_nearest_points = false;
46  request.gjk_solver_type = fcl::GST_LIBCCD;
47  fcl::DistanceResult<FCL_PRECISION> dist_result;
48  fcl::CollisionObject<FCL_PRECISION> *fcl_obj1 =
49  obj1.getCollisionObject_fcl().get();
50  fcl::CollisionObject<FCL_PRECISION> *fcl_obj2 =
51  obj2.getCollisionObject_fcl().get();
52  if (!fcl_obj1 || !fcl_obj2) {
53  return -1;
54  }
55  if (fcl_obj1->collisionGeometry() == fcl_obj2->collisionGeometry()) {
56  return -2;
57  distance = get_max_distance();
58  } else {
59  if (check_type == FCL_TOLERANCE_CHECK_TYPE::TOLERANCE_CHECK_NARROWPHASE) {
60  FCL_PRECISION retval =
61  fcl::distance(fcl_obj1, fcl_obj2, request, dist_result);
62 
63  distance = dist_result.min_distance;
64  } else {
65  distance =
66  -1 * penetrationDepth(fcl_obj1->getAABB(), fcl_obj2->getAABB());
67  }
68  }
69 
70  if (distance == get_max_distance()) {
71  return 1;
72  } else
73  return 0;
74 }
75 int FCL_CalculateDistance(const FCLCollisionObjectGroup &group1,
76  const FCLCollisionObject &obj2,
77  FCL_PRECISION &distance, double distance_tolerance) {
78  DistanceData distData;
79  fcl::BroadPhaseCollisionManager<double> *mngr;
80  distData.request.gjk_solver_type = fcl::GST_LIBCCD;
81  group1.getManager_fcl(mngr);
82  if (!mngr) return -1;
83  fcl::CollisionObject<FCL_PRECISION> *obj2_ptr =
84  obj2.getCollisionObject_fcl().get();
85  if (!obj2_ptr) {
86  return -1;
87  }
88  mngr->distance(obj2_ptr, &distData, defaultDistanceFunction);
89  if (distData.result_code == -1) {
90  return -2; // Group contained object with the same Geometry as obj2
91  }
92  if (distData.minDist == get_max_distance()) {
93  return 1;
94  } else {
95  distance = distData.minDist;
96  return 0;
97  }
98 }
99 int FCL_CalculateDistance(const FCLCollisionObjectGroup &group1,
100  const FCLCollisionObjectGroup &group2,
101  FCL_PRECISION &distance, double distance_tolerance) {
102  DistanceData distData;
103  fcl::BroadPhaseCollisionManager<double> *mngr1;
104  fcl::BroadPhaseCollisionManager<double> *mngr2;
105  distData.request.gjk_solver_type = fcl::GST_LIBCCD;
106  group1.getManager_fcl(mngr1);
107  if (!mngr1) return -1;
108  group2.getManager_fcl(mngr2);
109  if (!mngr2) return -1;
110  mngr1->distance(mngr2, &distData, defaultDistanceFunction);
111  if (distData.result_code == -1) {
112  return -2; // Group contained object with the same Geometry as obj2
113  }
114  if (distData.minDist == get_max_distance()) {
115  return 1;
116  } else {
117  distance = distData.minDist;
118  return 0;
119  }
120 }
121 
122 int FCL_DistanceTolerance(const FCLCollisionObjectGroup &group1,
123  const FCLCollisionObject &obj2,
124  FCL_PRECISION &distance,
125  FCL_TOLERANCE_CHECK_TYPE check_type,
126  double distance_tolerance) {
127  ToleranceDistanceData distData;
128  distData.tolerance = distance_tolerance;
129  distData.request.distance_tolerance = distance_tolerance;
130  distData.request.enable_signed_distance = true;
131  distData.request.enable_nearest_points = false;
132  distData.request.gjk_solver_type = fcl::GJKSolverType::GST_LIBCCD;
133  fcl::BroadPhaseCollisionManager<double> *mngr;
134  group1.getManager_fcl(mngr);
135  if (!mngr) return -1;
136  fcl::CollisionObject<FCL_PRECISION> *obj2_ptr =
137  obj2.getCollisionObject_fcl().get();
138  if (!obj2_ptr) {
139  return -1;
140  }
141  if (check_type == FCL_TOLERANCE_CHECK_TYPE::TOLERANCE_CHECK_NARROWPHASE) {
142  mngr->distance(obj2_ptr, &distData, toleranceDistanceFunction);
143  } else {
144  mngr->distance(obj2_ptr, &distData, toleranceBBDistanceFunction);
145  }
146 
147  if (distData.result_code == -1) {
148  return -2; // Group contained object with the same Geometry as obj2
149  }
150  if (distData.minDist == get_max_distance()) {
151  return 1;
152  } else {
153  distance = distData.minDist;
154  return 0;
155  }
156 }
157 int FCL_DistanceTolerance(const FCLCollisionObjectGroup &group1,
158  const FCLCollisionObjectGroup &group2,
159  FCL_PRECISION &distance,
160  FCL_TOLERANCE_CHECK_TYPE check_type,
161  double distance_tolerance) {
162  ToleranceDistanceData distData;
163  distData.tolerance = distance_tolerance;
164  distData.request.distance_tolerance = distance_tolerance;
165  distData.request.enable_signed_distance = true;
166  distData.request.enable_nearest_points = false;
167  distData.request.gjk_solver_type = fcl::GJKSolverType::GST_LIBCCD;
168  fcl::BroadPhaseCollisionManager<double> *mngr1;
169  fcl::BroadPhaseCollisionManager<double> *mngr2;
170  group1.getManager_fcl(mngr1);
171  if (!mngr1) return -1;
172  group2.getManager_fcl(mngr2);
173  if (!mngr2) return -1;
174  if (check_type == FCL_TOLERANCE_CHECK_TYPE::TOLERANCE_CHECK_NARROWPHASE) {
175  mngr1->distance(mngr2, &distData, toleranceDistanceFunction);
176  } else {
177  mngr1->distance(mngr2, &distData, toleranceBBDistanceFunction);
178  }
179 
180  if (distData.result_code == -1) {
181  return -2; // Group contained object with the same Geometry as obj2
182  }
183  if (distData.minDist == get_max_distance()) {
184  return 1;
185  } else {
186  distance = distData.minDist;
187  return 0;
188  }
189 }
190 } // namespace fcl_primitive_queries
191 } // namespace solverFCL
192 } // namespace solvers
193 
194 } // namespace collision
collision::solvers::solverFCL::fcl_primitive_queries::FCL_CalculateDistance
int FCL_CalculateDistance(const FCLCollisionObject &obj1, const FCLCollisionObject &obj2, FCL_PRECISION &distance, double distance_tolerance=1e-6)
Definition: fcl_distance_queries.cc:10
fcl_distance_requests.h
collision::solvers::solverFCL::fcl_primitive_queries::FCL_DistanceTolerance
int FCL_DistanceTolerance(const FCLCollisionObjectGroup &group1, const FCLCollisionObject &obj2, FCL_PRECISION &distance, FCL_TOLERANCE_CHECK_TYPE check_type=TOLERANCE_CHECK_NARROWPHASE, double distance_tolerance=1e-6)
Definition: fcl_distance_queries.cc:122
collision::solvers::solverFCL::penetrationDepth
double penetrationDepth(fcl::AABBd boxA, fcl::AABBd boxB)
Definition: fcl_distance_requests.cc:31
collision::solvers::solverFCL::DistanceDataBase::request
fcl::DistanceRequest< FCL_PRECISION > request
Definition: fcl_distance_requests.h:24
collision::solvers::solverFCL::toleranceDistanceFunction
bool toleranceDistanceFunction(fcl::CollisionObject< FCL_PRECISION > *o1, fcl::CollisionObject< FCL_PRECISION > *o2, void *cdata_, FCL_PRECISION &dist)
Definition: fcl_distance_requests.cc:80
collision::distance
std::size_t distance(const CollisionObject &obj1, const CollisionObject &obj2, DistanceResult &res, const DistanceRequest &req)
Definition: distance_queries.cc:95
collision::solvers::solverFCL::FCLCollisionObject::getCollisionObject_fcl
std::shared_ptr< fcl::CollisionObject< FCL_PRECISION > > getCollisionObject_fcl(void) const
Definition: fcl_collision_object.cc:124
fcl_distance_queries.h
collision::solvers::solverFCL::toleranceBBDistanceFunction
bool toleranceBBDistanceFunction(fcl::CollisionObject< FCL_PRECISION > *o1, fcl::CollisionObject< FCL_PRECISION > *o2, void *cdata_, FCL_PRECISION &dist)
Definition: fcl_distance_requests.cc:53
collision::solvers::solverFCL::FCLCollisionObjectGroup::getManager_fcl
int getManager_fcl(fcl::BroadPhaseCollisionManager< FCL_PRECISION > *&) const
Definition: fcl_collision_object_group.cc:114
collision::solvers::solverFCL::defaultDistanceFunction
bool defaultDistanceFunction(fcl::CollisionObject< FCL_PRECISION > *o1, fcl::CollisionObject< FCL_PRECISION > *o2, void *cdata_, FCL_PRECISION &dist)
Definition: fcl_distance_requests.cc:7