hpp-fcl  1.7.0
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:
100 
101  std::size_t operator()(const Transform3f& tf1, const Transform3f& tf2,
102  const CollisionRequest& request, CollisionResult& result) const
103  {
104  bool cached = request.enable_cached_gjk_guess;
105  solver.enable_cached_guess = cached;
106  if (cached) {
107  solver.cached_guess = request.cached_gjk_guess;
108  solver.support_func_cached_guess = request.cached_support_func_guess;
109  }
110 
111  solver.distance_upper_bound = request.distance_upper_bound;
112 
113  std::size_t res;
114  {
115  Timer timer;
116  if (swap_geoms) {
117  res = func(o2, tf2, o1, tf1, &solver, request, result);
118  result.swapObjects();
119  } else {
120  res = func (o1, tf1, o2, tf2, &solver, request, result);
121  }
122  result.timings = timer.elapsed();
123  }
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  return res;
130  }
131 
132  inline std::size_t operator()(const Transform3f& tf1, const Transform3f& tf2,
133  CollisionRequest& request, CollisionResult& result) const
134  {
135  std::size_t res = operator()(tf1, tf2, (const CollisionRequest&) request, result);
136  request.updateGuess (result);
137  return res;
138  }
139 
140 private:
141  CollisionGeometry const *o1, *o2;
142  GJKSolver solver;
143 
145  bool swap_geoms;
146 };
147 
148 } // namespace fcl
149 } // namespace hpp
150 
151 #endif
support_func_guess_t cached_support_func_guess
the support function intial guess set by user
Definition: collision_data.h:147
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:173
CPUTimes elapsed() const
Definition: timings.h:60
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:226
Main namespace.
Definition: AABB.h:43
collision result
Definition: collision_data.h:264
bool enable_cached_gjk_guess
whether enable gjk intial guess
Definition: collision_data.h:141
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:203
void updateGuess(const QueryResult &result)
Definition: collision_data.h:184
Definition: collision.h:97
Vec3f cached_gjk_guess
the gjk intial guess set by user
Definition: collision_data.h:144
This class mimics the way "boost/timer/timer.hpp" operates while using moder boost::chrono library...
Definition: timings.h:52
CPUTimes timings
timings for the given request
Definition: collision_data.h:176
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:132
std::size_t operator()(const Transform3f &tf1, const Transform3f &tf2, const CollisionRequest &request, CollisionResult &result) const
Definition: collision.h:101
Vec3f cached_gjk_guess
stores the last GJK ray when relevant.
Definition: collision_data.h:170
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:198
The geometry for the object for collision or distance computation.
Definition: collision_object.h:65
void swapObjects()
reposition Contact objects when fcl inverts them during their construction.
#define HPP_FCL_DLLAPI
Definition: config.hh:64