src/distance.cpp
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 #include <hpp/fcl/distance.h>
42 
43 #include <iostream>
44 
45 namespace hpp {
46 namespace fcl {
47 
49  static DistanceFunctionMatrix table;
50  return table;
51 }
52 
54  const DistanceRequest& request, DistanceResult& result) {
55  return distance(o1->collisionGeometryPtr(), o1->getTransform(),
56  o2->collisionGeometryPtr(), o2->getTransform(), request,
57  result);
58 }
59 
61  const CollisionGeometry* o2, const Transform3f& tf2,
62  const DistanceRequest& request, DistanceResult& result) {
63  GJKSolver solver(request);
64 
66 
67  OBJECT_TYPE object_type1 = o1->getObjectType();
68  NODE_TYPE node_type1 = o1->getNodeType();
69  OBJECT_TYPE object_type2 = o2->getObjectType();
70  NODE_TYPE node_type2 = o2->getNodeType();
71 
72  FCL_REAL res = (std::numeric_limits<FCL_REAL>::max)();
73 
74  if (object_type1 == OT_GEOM &&
75  (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) {
76  if (!looktable.distance_matrix[node_type2][node_type1]) {
77  HPP_FCL_THROW_PRETTY("Distance function between node type "
78  << std::string(get_node_type_name(node_type1))
79  << " and node type "
80  << std::string(get_node_type_name(node_type2))
81  << " is not yet supported.",
82  std::invalid_argument);
83  } else {
84  res = looktable.distance_matrix[node_type2][node_type1](
85  o2, tf2, o1, tf1, &solver, request, result);
86  // If closest points are requested, switch object 1 and 2
87  if (request.enable_nearest_points) {
88  const CollisionGeometry* tmpo = result.o1;
89  result.o1 = result.o2;
90  result.o2 = tmpo;
91  Vec3f tmpn(result.nearest_points[0]);
92  result.nearest_points[0] = result.nearest_points[1];
93  result.nearest_points[1] = tmpn;
94  }
95  }
96  } else {
97  if (!looktable.distance_matrix[node_type1][node_type2]) {
98  HPP_FCL_THROW_PRETTY("Distance function between node type "
99  << std::string(get_node_type_name(node_type1))
100  << " and node type "
101  << std::string(get_node_type_name(node_type2))
102  << " is not yet supported.",
103  std::invalid_argument);
104  } else {
105  res = looktable.distance_matrix[node_type1][node_type2](
106  o1, tf1, o2, tf2, &solver, request, result);
107  }
108  }
110  solver.enable_cached_guess) {
111  result.cached_gjk_guess = solver.cached_guess;
113  }
114 
115  return res;
116 }
117 
119  const CollisionGeometry* o2)
120  : o1(o1), o2(o2) {
122 
123  OBJECT_TYPE object_type1 = o1->getObjectType();
124  NODE_TYPE node_type1 = o1->getNodeType();
125  OBJECT_TYPE object_type2 = o2->getObjectType();
126  NODE_TYPE node_type2 = o2->getNodeType();
127 
128  swap_geoms = object_type1 == OT_GEOM &&
129  (object_type2 == OT_BVH || object_type2 == OT_HFIELD);
130 
131  if ((swap_geoms && !looktable.distance_matrix[node_type2][node_type1]) ||
132  (!swap_geoms && !looktable.distance_matrix[node_type1][node_type2])) {
133  HPP_FCL_THROW_PRETTY("Distance function between node type "
134  << std::string(get_node_type_name(node_type1))
135  << " and node type "
136  << std::string(get_node_type_name(node_type2))
137  << " is not yet supported.",
138  std::invalid_argument);
139  }
140  if (swap_geoms)
141  func = looktable.distance_matrix[node_type2][node_type1];
142  else
143  func = looktable.distance_matrix[node_type1][node_type2];
144 }
145 
147  const DistanceRequest& request,
148  DistanceResult& result) const {
149  FCL_REAL res;
150 
151  if (swap_geoms) {
152  res = func(o2, tf2, o1, tf1, &solver, request, result);
153  if (request.enable_nearest_points) {
154  std::swap(result.o1, result.o2);
155  result.nearest_points[0].swap(result.nearest_points[1]);
156  }
157  } else {
158  res = func(o1, tf1, o2, tf2, &solver, request, result);
159  }
160 
161  return res;
162 }
163 
165  const Transform3f& tf2,
166  const DistanceRequest& request,
167  DistanceResult& result) const {
168  solver.set(request);
169 
170  FCL_REAL res;
171  if (request.enable_timings) {
172  Timer timer;
173  res = run(tf1, tf2, request, result);
174  result.timings = timer.elapsed();
175  } else
176  res = run(tf1, tf2, request, result);
177 
182  }
183  return res;
184 }
185 
186 } // namespace fcl
187 } // namespace hpp
distance_func_matrix.h
hpp::fcl::getDistanceFunctionLookTable
DistanceFunctionMatrix & getDistanceFunctionLookTable()
Definition: src/distance.cpp:48
hpp::fcl::CachedGuess
@ CachedGuess
Definition: data_types.h:80
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::Timer::elapsed
CPUTimes elapsed() const
Definition: timings.h:46
hpp::fcl::DistanceResult::nearest_points
Vec3f nearest_points[2]
nearest points
Definition: collision_data.h:427
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:27
hpp::fcl::OT_HFIELD
@ OT_HFIELD
Definition: collision_object.h:58
hpp::fcl::GJKSolver::gjk_initial_guess
GJKInitialGuess gjk_initial_guess
which warm start to use for GJK
Definition: narrowphase.h:502
hpp::fcl::CollisionObject::collisionGeometryPtr
const CollisionGeometry * collisionGeometryPtr() const
get raw pointer to collision geometry of the object instance
Definition: collision_object.h:312
hpp::fcl::ComputeDistance::ComputeDistance
ComputeDistance(const CollisionGeometry *o1, const CollisionGeometry *o2)
Definition: src/distance.cpp:118
hpp::fcl::DistanceResult::o1
const CollisionGeometry * o1
collision object 1
Definition: collision_data.h:433
hpp::fcl::distance
HPP_FCL_DLLAPI 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.
Definition: kIOS.cpp:181
hpp::fcl::CollisionGeometry::getObjectType
virtual OBJECT_TYPE getObjectType() const
get the type of the object
Definition: collision_object.h:128
hpp::fcl::ComputeDistance::o1
const CollisionGeometry * o1
Definition: distance.h:124
hpp::fcl::Timer
This class mimics the way "boost/timer/timer.hpp" operates while using the modern std::chrono library...
Definition: timings.h:32
hpp::fcl::CollisionGeometry
The geometry for the object for collision or distance computation.
Definition: collision_object.h:95
hpp::fcl::ComputeDistance::solver
GJKSolver solver
Definition: distance.h:134
hpp::fcl::NODE_TYPE
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_object.h:65
hpp::fcl::OT_BVH
@ OT_BVH
Definition: collision_object.h:55
narrowphase.h
res
res
hpp::fcl::OT_GEOM
@ OT_GEOM
Definition: collision_object.h:56
distance.h
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::QueryRequest::enable_timings
bool enable_timings
enable timings when performing collision/distance request
Definition: collision_data.h:150
hpp::fcl::OBJECT_TYPE
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:53
hpp::fcl::GJKSolver::set
void set(const DistanceRequest &request)
setter from a DistanceRequest
Definition: narrowphase.h:389
hpp::fcl::GJKSolver::enable_cached_guess
bool enable_cached_guess
Whether smart guess can be provided @Deprecated Use gjk_initial_guess instead.
Definition: narrowphase.h:496
hpp::fcl::GJKSolver
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: narrowphase.h:54
hpp::fcl::QueryResult::timings
CPUTimes timings
timings for the given request
Definition: collision_data.h:203
hpp::fcl::CollisionObject::getTransform
const Transform3f & getTransform() const
get object's transform
Definition: collision_object.h:283
hpp::fcl::get_node_type_name
const char * get_node_type_name(NODE_TYPE node_type)
Returns the name associated to a NODE_TYPE.
Definition: collision_utility.h:33
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:215
hpp::fcl::QueryResult::cached_gjk_guess
Vec3f cached_gjk_guess
stores the last GJK ray when relevant.
Definition: collision_data.h:197
hpp::fcl::ComputeDistance::swap_geoms
bool swap_geoms
Definition: distance.h:137
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:36
hpp::fcl::ComputeDistance::operator()
FCL_REAL operator()(const Transform3f &tf1, const Transform3f &tf2, const DistanceRequest &request, DistanceResult &result) const
Definition: src/distance.cpp:164
hpp::fcl::DistanceFunctionMatrix::distance_matrix
DistanceFunc distance_matrix[NODE_COUNT][NODE_COUNT]
each item in the distance matrix is a function to handle distance between objects of type1 and type2
Definition: distance_func_matrix.h:68
HPP_FCL_THROW_PRETTY
#define HPP_FCL_THROW_PRETTY(message, exception)
Definition: include/hpp/fcl/fwd.hh:57
hpp::fcl::ComputeDistance::o2
const CollisionGeometry * o2
Definition: distance.h:132
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
hpp::fcl::DistanceRequest
request to the distance computation
Definition: collision_data.h:392
hpp::fcl::ComputeDistance::run
virtual FCL_REAL run(const Transform3f &tf1, const Transform3f &tf2, const DistanceRequest &request, DistanceResult &result) const
Definition: src/distance.cpp:146
hpp::fcl::DistanceResult
distance result
Definition: collision_data.h:420
hpp::fcl::ComputeDistance::func
DistanceFunctionMatrix::DistanceFunc func
Definition: distance.h:136
hpp::fcl::DistanceFunctionMatrix
distance matrix stores the functions for distance between different types of objects and provides a u...
Definition: distance_func_matrix.h:50
hpp::fcl::CollisionGeometry::getNodeType
virtual NODE_TYPE getNodeType() const
get the node type
Definition: collision_object.h:131
hpp::fcl::DistanceResult::o2
const CollisionGeometry * o2
collision object 2
Definition: collision_data.h:436
hpp::fcl::DistanceRequest::enable_nearest_points
bool enable_nearest_points
whether to return the nearest points
Definition: collision_data.h:394
hpp::fcl::QueryResult::cached_support_func_guess
support_func_guess_t cached_support_func_guess
stores the last support function vertex index, when relevant.
Definition: collision_data.h:200
collision_utility.h
hpp::fcl::GJKSolver::support_func_cached_guess
support_func_guess_t support_func_cached_guess
smart guess for the support function
Definition: narrowphase.h:514
hpp::fcl::GJKSolver::cached_guess
Vec3f cached_guess
smart guess
Definition: narrowphase.h:499


hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:13