hpp-fcl  1.7.8
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  if(request.enable_timings)
115  {
116  Timer timer;
117  res = run(tf1, tf2, request, result);
118  result.timings = timer.elapsed();
119  }
120  else
121  res = run(tf1, tf2, request, result);
122 
123  if (cached) {
124  result.cached_gjk_guess = solver.cached_guess;
125  result.cached_support_func_guess = solver.support_func_cached_guess;
126  }
127 
128  return res;
129  }
130 
131  inline std::size_t operator()(const Transform3f& tf1, const Transform3f& tf2,
132  CollisionRequest& request, CollisionResult& result) const
133  {
134  std::size_t res = operator()(tf1, tf2, (const CollisionRequest&) request, result);
135  request.updateGuess (result);
136  return res;
137  }
138 
139  virtual ~ComputeCollision() {};
140 
141 protected:
142  CollisionGeometry const *o1, *o2;
144 
147 
148  virtual std::size_t run(const Transform3f& tf1, const Transform3f& tf2,
149  const CollisionRequest& request, CollisionResult& result) const;
150 public:
151 
152  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
153 };
154 
155 
156 } // namespace fcl
157 } // namespace hpp
158 
159 #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:178
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:231
Main namespace.
Definition: AABB.h:43
GJKSolver solver
Definition: collision.h:143
virtual ~ComputeCollision()
Definition: collision.h:139
collision result
Definition: collision_data.h:269
bool enable_cached_gjk_guess
whether enable gjk intial guess
Definition: collision_data.h:141
bool swap_geoms
Definition: collision.h:146
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:208
void updateGuess(const QueryResult &result)
Definition: collision_data.h:189
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 the modern std::chrono library...
Definition: timings.h:39
CPUTimes timings
timings for the given request
Definition: collision_data.h:181
bool enable_timings
enable timings when performing collision/distance request
Definition: collision_data.h:150
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:131
std::size_t operator()(const Transform3f &tf1, const Transform3f &tf2, const CollisionRequest &request, CollisionResult &result) const
Definition: collision.h:101
CollisionFunctionMatrix::CollisionFunc func
Definition: collision.h:145
Vec3f cached_gjk_guess
stores the last GJK ray when relevant.
Definition: collision_data.h:175
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
#define HPP_FCL_DLLAPI
Definition: config.hh:64