distance-inl.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 FCL_DISTANCE_INL_H
39 #define FCL_DISTANCE_INL_H
40 
42 
44 
45 namespace fcl
46 {
47 
48 //==============================================================================
49 extern template
50 double distance(
51  const CollisionObject<double>* o1,
52  const CollisionObject<double>* o2,
53  const DistanceRequest<double>& request,
54  DistanceResult<double>& result);
55 
56 //==============================================================================
57 extern template
58 double distance(
59  const CollisionGeometry<double>* o1, const Transform3<double>& tf1,
60  const CollisionGeometry<double>* o2, const Transform3<double>& tf2,
61  const DistanceRequest<double>& request, DistanceResult<double>& result);
62 
63 //==============================================================================
64 template <typename GJKSolver>
66 {
68  return table;
69 }
70 
71 //==============================================================================
72 template <typename NarrowPhaseSolver>
73 typename NarrowPhaseSolver::S distance(
76  const NarrowPhaseSolver* nsolver,
79 {
80  return distance<NarrowPhaseSolver>(
81  o1->collisionGeometry().get(),
82  o1->getTransform(),
83  o2->collisionGeometry().get(),
84  o2->getTransform(),
85  nsolver,
86  request,
87  result);
88 }
89 
90 //==============================================================================
91 template <typename NarrowPhaseSolver>
92 typename NarrowPhaseSolver::S distance(
97  const NarrowPhaseSolver* nsolver_,
100 {
101  using S = typename NarrowPhaseSolver::S;
102 
103  const NarrowPhaseSolver* nsolver = nsolver_;
104  if(!nsolver_)
105  nsolver = new NarrowPhaseSolver();
106 
107  const auto& looktable = getDistanceFunctionLookTable<NarrowPhaseSolver>();
108 
109  OBJECT_TYPE object_type1 = o1->getObjectType();
110  NODE_TYPE node_type1 = o1->getNodeType();
111  OBJECT_TYPE object_type2 = o2->getObjectType();
112  NODE_TYPE node_type2 = o2->getNodeType();
113 
114  S res = std::numeric_limits<S>::max();
115 
116 
117  if(object_type1 == OT_GEOM && object_type2 == OT_BVH)
118  {
119  if(!looktable.distance_matrix[node_type2][node_type1])
120  {
121  std::cerr << "Warning: distance function between node type " << node_type1 << " and node type " << node_type2 << " is not supported\n";
122  }
123  else
124  {
125  res = looktable.distance_matrix[node_type2][node_type1](o2, tf2, o1, tf1, nsolver, request, result);
126  }
127  }
128  else
129  {
130  if(!looktable.distance_matrix[node_type1][node_type2])
131  {
132  std::cerr << "Warning: distance function between node type " << node_type1 << " and node type " << node_type2 << " is not supported\n";
133  }
134  else
135  {
136  res = looktable.distance_matrix[node_type1][node_type2](o1, tf1, o2, tf2, nsolver, request, result);
137  }
138  }
139 
140  // TODO(JS): FCL supports negative distance calculation only for OT_GEOM shape
141  // types (i.e., primitive shapes like sphere, cylinder, box, and so on). As a
142  // workaround for the rest shape types like mesh and octree, following
143  // computes negative distance using additional penetration depth computation
144  // of collision checking routine. The downside of this workaround is that the
145  // pair of nearest points is not guaranteed to be on the surface of the
146  // objects.
147  if(res
148  && result.min_distance < static_cast<S>(0)
149  && request.enable_signed_distance)
150  {
151  if (std::is_same<NarrowPhaseSolver, detail::GJKSolver_libccd<S>>::value
152  && object_type1 == OT_GEOM && object_type2 == OT_GEOM)
153  {
154  return res;
155  }
156 
157  CollisionRequest<S> collision_request;
158  collision_request.enable_contact = true;
159 
160  CollisionResult<S> collision_result;
161 
162  collide(o1, tf1, o2, tf2, nsolver, collision_request, collision_result);
163  assert(collision_result.isCollision());
164 
165  std::size_t index = static_cast<std::size_t>(-1);
166  S max_pen_depth = std::numeric_limits<S>::min();
167  for (auto i = 0u; i < collision_result.numContacts(); ++i)
168  {
169  const auto& contact = collision_result.getContact(i);
170  if (max_pen_depth < contact.penetration_depth)
171  {
172  max_pen_depth = contact.penetration_depth;
173  index = i;
174  }
175  }
176  result.min_distance = -max_pen_depth;
177  assert(index != static_cast<std::size_t>(-1));
178 
179  if (request.enable_nearest_points)
180  {
181  const Vector3<S>& pos = collision_result.getContact(index).pos;
182  result.nearest_points[0] = pos;
183  result.nearest_points[1] = pos;
184  // Note: The pair of nearest points is not guaranteed to be on the
185  // surface of the objects.
186  }
187  }
188 
189  if(!nsolver_)
190  delete nsolver;
191 
192  return res;
193 }
194 
195 //==============================================================================
196 template <typename S>
198  const CollisionObject<S>* o1,
199  const CollisionObject<S>* o2,
200  const DistanceRequest<S>& request,
201  DistanceResult<S>& result)
202 {
203  switch(request.gjk_solver_type)
204  {
205  case GST_LIBCCD:
206  {
208  solver.distance_tolerance = request.distance_tolerance;
209  return distance(o1, o2, &solver, request, result);
210  }
211  case GST_INDEP:
212  {
214  solver.gjk_tolerance = request.distance_tolerance;
215  return distance(o1, o2, &solver, request, result);
216  }
217  default:
218  return -1; // error
219  }
220 }
221 
222 //==============================================================================
223 template <typename S>
225  const CollisionGeometry<S>* o1, const Transform3<S>& tf1,
226  const CollisionGeometry<S>* o2, const Transform3<S>& tf2,
227  const DistanceRequest<S>& request, DistanceResult<S>& result)
228 {
229  switch(request.gjk_solver_type)
230  {
231  case GST_LIBCCD:
232  {
234  solver.distance_tolerance = request.distance_tolerance;
235  return distance(o1, tf1, o2, tf2, &solver, request, result);
236  }
237  case GST_INDEP:
238  {
240  solver.gjk_tolerance = request.distance_tolerance;
241  return distance(o1, tf1, o2, tf2, &solver, request, result);
242  }
243  default:
244  return -1;
245  }
246 }
247 
248 } // namespace fcl
249 
250 #endif
fcl::DistanceRequest
request to the distance computation
Definition: distance_request.h:52
fcl::CollisionGeometry< double >
template class FCL_EXPORT CollisionGeometry< double >
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::detail::GJKSolver_libccd
collision and distance solver based on libccd library.
Definition: gjk_solver_libccd.h:54
fcl::distance
S distance(const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const kIOS< S > &b1, const kIOS< S > &b2, Vector3< S > *P, Vector3< S > *Q)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS-inl.h:266
fcl::DistanceResult::nearest_points
Vector3< S > nearest_points[2]
Nearest points in the world coordinates.
Definition: distance_result.h:69
fcl::CollisionResult::isCollision
bool isCollision() const
return binary collision result
Definition: collision_result-inl.h:76
fcl::getDistanceFunctionLookTable
detail::DistanceFunctionMatrix< GJKSolver > & getDistanceFunctionLookTable()
Definition: distance-inl.h:65
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::CollisionResult
collision result
Definition: collision_request.h:48
fcl::DistanceResult
distance result
Definition: distance_request.h:48
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
distance.h
fcl::CollisionGeometry
The geometry for the object for collision or distance computation.
Definition: collision_geometry.h:58
fcl::CollisionGeometry::getNodeType
virtual NODE_TYPE getNodeType() const
get the node type
Definition: collision_geometry-inl.h:79
fcl::DistanceRequest::gjk_solver_type
GJKSolverType gjk_solver_type
narrow phase solver type
Definition: distance_request.h:102
fcl::GST_LIBCCD
@ GST_LIBCCD
Definition: gjk_solver_type.h:45
fcl::detail::GJKSolver_indep::gjk_tolerance
S gjk_tolerance
the threshold used in GJK to stop iteration
Definition: gjk_solver_indep.h:174
fcl::detail::GJKSolver_libccd::distance_tolerance
S distance_tolerance
the threshold used in GJK algorithm to stop distance iteration
Definition: gjk_solver_libccd.h:171
fcl::detail::GJKSolver_indep
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: gjk_solver_indep.h:54
fcl::CollisionRequest
Parameters for performing collision request.
Definition: collision_request.h:52
fcl::CollisionResult::numContacts
size_t numContacts() const
number of contacts found
Definition: collision_result-inl.h:83
fcl::CollisionObject::collisionGeometry
const std::shared_ptr< const CollisionGeometry< S > > & collisionGeometry() const
get geometry from the object instance
Definition: collision_object-inl.h:243
fcl::DistanceRequest::enable_nearest_points
bool enable_nearest_points
whether to return the nearest points
Definition: distance_request.h:55
fcl::collide
template FCL_EXPORT std::size_t collide(const CollisionObject< double > *o1, const CollisionObject< double > *o2, const CollisionRequest< double > &request, CollisionResult< double > &result)
fcl::DistanceResult::min_distance
S min_distance
Minimum distance between two objects.
Definition: distance_result.h:64
fcl::OBJECT_TYPE
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_geometry.h:50
fcl::CollisionObject::getTransform
const Transform3< S > & getTransform() const
get object's transform
Definition: collision_object-inl.h:170
fcl::DistanceRequest::enable_signed_distance
bool enable_signed_distance
Whether to compute exact negative distance.
Definition: distance_request.h:92
fcl::CollisionObject< double >
template class FCL_EXPORT CollisionObject< double >
fcl::CollisionResult::getContact
const Contact< S > & getContact(size_t i) const
get the i-th contact calculated
Definition: collision_result-inl.h:97
fcl::DistanceRequest::distance_tolerance
S distance_tolerance
the threshold used in GJK algorithm to stop distance iteration
Definition: distance_request.h:99
fcl::OT_BVH
@ OT_BVH
Definition: collision_geometry.h:50
min
static T min(T x, T y)
Definition: svm.cpp:49
fcl::GST_INDEP
@ GST_INDEP
Definition: gjk_solver_type.h:45
fcl::CollisionGeometry::getObjectType
virtual OBJECT_TYPE getObjectType() const
get the type of the object
Definition: collision_geometry-inl.h:72
fcl::NODE_TYPE
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_geometry.h:53
fcl::CollisionRequest::enable_contact
bool enable_contact
If true, contact information (e.g., normal, penetration depth, and contact position) will be returned...
Definition: collision_request.h:63
fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:51
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::detail::DistanceFunctionMatrix
distance matrix stores the functions for distance between different types of objects and provides a u...
Definition: distance_func_matrix.h:54
fcl::OT_GEOM
@ OT_GEOM
Definition: collision_geometry.h:50
collision.h


fcl
Author(s):
autogenerated on Tue Jun 22 2021 07:27:50