hpp-fcl  1.8.1
HPP fork of FCL -- The Flexible Collision Library
collision.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-2015, Open Source Robotics Foundation
6  * Copyright (c) 2021, INRIA
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Open Source Robotics Foundation nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  */
36 
40 #ifndef HPP_FCL_COLLISION_H
41 #define HPP_FCL_COLLISION_H
42 
43 #include <hpp/fcl/data_types.h>
45 #include <hpp/fcl/collision_data.h>
47 #include <hpp/fcl/timings.h>
48 
49 namespace hpp
50 {
51 namespace fcl
52 {
53 
58 HPP_FCL_DLLAPI std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
59  const CollisionRequest& request, CollisionResult& result);
60 
62 HPP_FCL_DLLAPI std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
63  const CollisionGeometry* o2, const Transform3f& tf2,
64  const CollisionRequest& request, CollisionResult& result);
65 
69 inline std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
70  CollisionRequest& request, CollisionResult& result)
71 {
72  std::size_t res = collide(o1, o2, (const CollisionRequest&) request, result);
73  request.updateGuess (result);
74  return res;
75 }
76 
80 inline std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
81  const CollisionGeometry* o2, const Transform3f& tf2,
82  CollisionRequest& request, CollisionResult& result)
83 {
84  std::size_t res = collide(o1, tf1, o2, tf2,
85  (const CollisionRequest&) request, result);
86  request.updateGuess (result);
87  return res;
88 }
89 
98 public:
99 
102 
103  std::size_t operator()(const Transform3f& tf1, const Transform3f& tf2,
104  const CollisionRequest& request, CollisionResult& result) const
105  {
106  bool cached = request.enable_cached_gjk_guess;
107  solver.enable_cached_guess = cached;
108  if (cached) {
109  solver.cached_guess = request.cached_gjk_guess;
110  solver.support_func_cached_guess = request.cached_support_func_guess;
111  }
112 
113  solver.distance_upper_bound = request.distance_upper_bound;
114 
115  std::size_t res;
116  if(request.enable_timings)
117  {
118  Timer timer;
119  res = run(tf1, tf2, request, result);
120  result.timings = timer.elapsed();
121  }
122  else
123  res = run(tf1, tf2, request, result);
124 
125  if (cached) {
126  result.cached_gjk_guess = solver.cached_guess;
127  result.cached_support_func_guess = solver.support_func_cached_guess;
128  }
129 
130  return res;
131  }
132 
133  inline std::size_t operator()(const Transform3f& tf1, const Transform3f& tf2,
134  CollisionRequest& request, CollisionResult& result) const
135  {
136  std::size_t res = operator()(tf1, tf2, (const CollisionRequest&) request, result);
137  request.updateGuess (result);
138  return res;
139  }
140 
141  virtual ~ComputeCollision() {};
142 
143 protected:
144  CollisionGeometry const *o1, *o2;
146 
149 
150  virtual std::size_t run(const Transform3f& tf1, const Transform3f& tf2,
151  const CollisionRequest& request, CollisionResult& result) const;
152 public:
153 
154  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
155 };
156 
157 
158 } // namespace fcl
159 } // namespace hpp
160 
161 #endif
The geometry for the object for collision or distance computation.
Definition: collision_object.h:67
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:200
This class reduces the cost of identifying the geometry pair. This is mostly useful for repeated shap...
Definition: collision.h:97
CollisionFunctionMatrix::CollisionFunc func
Definition: collision.h:147
virtual std::size_t run(const Transform3f &tf1, const Transform3f &tf2, const CollisionRequest &request, CollisionResult &result) const
std::size_t operator()(const Transform3f &tf1, const Transform3f &tf2, CollisionRequest &request, CollisionResult &result) const
Definition: collision.h:133
virtual ~ComputeCollision()
Definition: collision.h:141
GJKSolver solver
Definition: collision.h:145
ComputeCollision(const CollisionGeometry *o1, const CollisionGeometry *o2)
Default constructor from two Collision Geometries.
bool swap_geoms
Definition: collision.h:148
std::size_t operator()(const Transform3f &tf1, const Transform3f &tf2, const CollisionRequest &request, CollisionResult &result) const
Definition: collision.h:103
CollisionGeometry const * o1
Definition: collision.h:141
Simple transform class used locally by InterpMotion.
Definition: transform.h:59
#define HPP_FCL_DLLAPI
Definition: config.hh:64
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,...
Main namespace.
Definition: AABB.h:44
std::size_t(* CollisionFunc)(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *nsolver, const CollisionRequest &request, CollisionResult &result)
the uniform call interface for collision: for collision, we need know
Definition: collision_func_matrix.h:61
request to the collision algorithm
Definition: collision_data.h:210
FCL_REAL distance_upper_bound
Distance above which GJK solver makes an early stopping. GJK stops searching for the closest points w...
Definition: collision_data.h:232
collision result
Definition: collision_data.h:277
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: narrowphase.h:54
Vec3f cached_gjk_guess
the gjk initial guess set by user
Definition: collision_data.h:145
support_func_guess_t cached_support_func_guess
the support function initial guess set by user
Definition: collision_data.h:148
void updateGuess(const QueryResult &result)
Definition: collision_data.h:190
bool enable_timings
enable timings when performing collision/distance request
Definition: collision_data.h:151
bool enable_cached_gjk_guess
whether enable gjk initial guess
Definition: collision_data.h:142
support_func_guess_t cached_support_func_guess
stores the last support function vertex index, when relevant.
Definition: collision_data.h:179
Vec3f cached_gjk_guess
stores the last GJK ray when relevant.
Definition: collision_data.h:176
CPUTimes timings
timings for the given request
Definition: collision_data.h:182
This class mimics the way "boost/timer/timer.hpp" operates while using the modern std::chrono library...
Definition: timings.h:40
CPUTimes elapsed() const
Definition: timings.h:48