hpp-fcl 1.8.1
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
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
44#include <hpp/fcl/timings.h>
45
46namespace hpp
47{
48namespace fcl
49{
50
54 const DistanceRequest& request, DistanceResult& result);
55
58 const CollisionGeometry* o2, const Transform3f& tf2,
59 const DistanceRequest& request, DistanceResult& result);
60
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
75inline 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
93public:
95
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 if(request.enable_timings)
108 {
109 Timer timer;
110 res = run(tf1, tf2, request, result);
111 result.timings = timer.elapsed();
112 }
113 else
114 res = run(tf1, tf2, request, result);
115
116 if (cached) {
117 result.cached_gjk_guess = solver.cached_guess;
118 result.cached_support_func_guess = solver.support_func_cached_guess;
119 }
120 return res;
121 }
122
123 inline FCL_REAL operator()(const Transform3f& tf1, const Transform3f& tf2,
124 DistanceRequest& request, DistanceResult& result) const
125 {
126 FCL_REAL res = operator()(tf1, tf2, (const DistanceRequest&) request, result);
127 request.updateGuess (result);
128 return res;
129 }
130
131 virtual ~ComputeDistance() {};
132
133protected:
136
137 DistanceFunctionMatrix::DistanceFunc func;
139
140 virtual FCL_REAL run(const Transform3f& tf1, const Transform3f& tf2,
141 const DistanceRequest& request, DistanceResult& result) const;
142public:
143
144 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
145};
146
147
148
149} // namespace fcl
150} // namespace hpp
151
152#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
Definition: distance.h:92
FCL_REAL operator()(const Transform3f &tf1, const Transform3f &tf2, const DistanceRequest &request, DistanceResult &result) const
Definition: distance.h:96
ComputeDistance(const CollisionGeometry *o1, const CollisionGeometry *o2)
GJKSolver solver
Definition: distance.h:135
FCL_REAL operator()(const Transform3f &tf1, const Transform3f &tf2, DistanceRequest &request, DistanceResult &result) const
Definition: distance.h:123
DistanceFunctionMatrix::DistanceFunc func
Definition: distance.h:137
CollisionGeometry const * o1
Definition: distance.h:134
bool swap_geoms
Definition: distance.h:138
virtual ~ComputeDistance()
Definition: distance.h:131
virtual FCL_REAL run(const Transform3f &tf1, const Transform3f &tf2, const DistanceRequest &request, DistanceResult &result) const
Simple transform class used locally by InterpMotion.
Definition: transform.h:59
#define HPP_FCL_DLLAPI
Definition: config.hh:64
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
Main namespace.
Definition: AABB.h:44
request to the distance computation
Definition: collision_data.h:369
distance result
Definition: collision_data.h:403
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