hpp-fcl  1.7.0
HPP fork of FCL -- The Flexible Collision Library
distance.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  * 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 HPP_FCL_DISTANCE_H
39 #define HPP_FCL_DISTANCE_H
40 
42 #include <hpp/fcl/collision_data.h>
44 #include <hpp/fcl/timings.h>
45 
46 namespace hpp
47 {
48 namespace fcl
49 {
50 
53 HPP_FCL_DLLAPI FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2,
54  const DistanceRequest& request, DistanceResult& result);
55 
57 HPP_FCL_DLLAPI FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
58  const CollisionGeometry* o2, const Transform3f& tf2,
59  const DistanceRequest& request, DistanceResult& result);
60 
64 inline FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2,
65  DistanceRequest& request, DistanceResult& result)
66 {
67  FCL_REAL res = distance(o1, o2, (const DistanceRequest&) request, result);
68  request.updateGuess (result);
69  return res;
70 }
71 
75 inline FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
76  const CollisionGeometry* o2, const Transform3f& tf2,
77  DistanceRequest& request, DistanceResult& result)
78 {
79  FCL_REAL res = distance(o1, tf1, o2, tf2,
80  (const DistanceRequest&) request, result);
81  request.updateGuess (result);
82  return res;
83 }
84 
93 public:
95 
96  FCL_REAL operator()(const Transform3f& tf1, const Transform3f& tf2,
97  const DistanceRequest& request, DistanceResult& result) const
98  {
99  bool cached = request.enable_cached_gjk_guess;
100  solver.enable_cached_guess = cached;
101  if (cached) {
102  solver.cached_guess = request.cached_gjk_guess;
103  solver.support_func_cached_guess = request.cached_support_func_guess;
104  }
105 
106  FCL_REAL res;
107  {
108  Timer timer;
109  if (swap_geoms) {
110  res = func(o2, tf2, o1, tf1, &solver, request, result);
111  if (request.enable_nearest_points) {
112  std::swap(result.o1, result.o2);
113  result.nearest_points[0].swap(result.nearest_points[1]);
114  }
115  } else {
116  res = func (o1, tf1, o2, tf2, &solver, request, result);
117  }
118  result.timings = timer.elapsed();
119  }
120 
121  if (cached) {
122  result.cached_gjk_guess = solver.cached_guess;
123  result.cached_support_func_guess = solver.support_func_cached_guess;
124  }
125  return res;
126  }
127 
128  inline FCL_REAL operator()(const Transform3f& tf1, const Transform3f& tf2,
129  DistanceRequest& request, DistanceResult& result) const
130  {
131  FCL_REAL res = operator()(tf1, tf2, (const DistanceRequest&) request, result);
132  request.updateGuess (result);
133  return res;
134  }
135 
136 private:
137  CollisionGeometry const *o1, *o2;
138  GJKSolver solver;
139 
141  bool swap_geoms;
142 };
143 
144 } // namespace fcl
145 } // namespace hpp
146 
147 #endif
support_func_guess_t cached_support_func_guess
the support function intial guess set by user
Definition: collision_data.h:147
FCL_REAL(* DistanceFunc)(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
the uniform call interface for distance: for distance, we need know
Definition: distance_func_matrix.h:57
Simple transform class used locally by InterpMotion.
Definition: transform.h:58
request to the distance computation
Definition: collision_data.h:356
support_func_guess_t cached_support_func_guess
stores the last support function vertex index, when relevant.
Definition: collision_data.h:173
const CollisionGeometry * o1
collision object 1
Definition: collision_data.h:405
CPUTimes elapsed() const
Definition: timings.h:60
Main namespace.
Definition: AABB.h:43
Vec3f nearest_points[2]
nearest points
Definition: collision_data.h:399
bool enable_cached_gjk_guess
whether enable gjk intial guess
Definition: collision_data.h:141
Definition: distance.h:92
FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
double FCL_REAL
Definition: data_types.h:66
void updateGuess(const QueryResult &result)
Definition: collision_data.h:184
Vec3f cached_gjk_guess
the gjk intial guess set by user
Definition: collision_data.h:144
FCL_REAL operator()(const Transform3f &tf1, const Transform3f &tf2, const DistanceRequest &request, DistanceResult &result) const
Definition: distance.h:96
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
bool enable_nearest_points
whether to return the nearest points
Definition: collision_data.h:359
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
FCL_REAL operator()(const Transform3f &tf1, const Transform3f &tf2, DistanceRequest &request, DistanceResult &result) const
Definition: distance.h:128
const CollisionGeometry * o2
collision object 2
Definition: collision_data.h:408
#define HPP_FCL_DLLAPI
Definition: config.hh:64
distance result
Definition: collision_data.h:390