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
support_func_guess_t cached_support_func_guess
the support function initial guess set by user
Definition: collision_data.h:148
Simple transform class used locally by InterpMotion.
Definition: transform.h:58
support_func_guess_t cached_support_func_guess
stores the last support function vertex index, when relevant.
Definition: collision_data.h:179
CPUTimes elapsed() const
Definition: timings.h:48
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
Main namespace.
Definition: AABB.h:43
GJKSolver solver
Definition: collision.h:145
virtual ~ComputeCollision()
Definition: collision.h:141
collision result
Definition: collision_data.h:276
bool enable_cached_gjk_guess
whether enable gjk initial guess
Definition: collision_data.h:142
bool swap_geoms
Definition: collision.h:148
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:209
void updateGuess(const QueryResult &result)
Definition: collision_data.h:190
This class reduces the cost of identifying the geometry pair. This is mostly useful for repeated shap...
Definition: collision.h:97
Vec3f cached_gjk_guess
the gjk initial guess set by user
Definition: collision_data.h:145
This class mimics the way "boost/timer/timer.hpp" operates while using the modern std::chrono library...
Definition: timings.h:39
CPUTimes timings
timings for the given request
Definition: collision_data.h:182
bool enable_timings
enable timings when performing collision/distance request
Definition: collision_data.h:151
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, including num of max contacts, whether perform exhaustive collision (i.e., returning returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function performs the collision between them. Return value is the number of contacts generated between the two objects.
std::size_t operator()(const Transform3f &tf1, const Transform3f &tf2, CollisionRequest &request, CollisionResult &result) const
Definition: collision.h:133
std::size_t operator()(const Transform3f &tf1, const Transform3f &tf2, const CollisionRequest &request, CollisionResult &result) const
Definition: collision.h:103
CollisionFunctionMatrix::CollisionFunc func
Definition: collision.h:147
Vec3f cached_gjk_guess
stores the last GJK ray when relevant.
Definition: collision_data.h:176
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: narrowphase.h:53
the object for collision or distance computation, contains the geometry and the transform information...
Definition: collision_object.h:199
The geometry for the object for collision or distance computation.
Definition: collision_object.h:66
#define HPP_FCL_DLLAPI
Definition: config.hh:64