GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/distance.cpp Lines: 21 70 30.0 %
Date: 2024-02-09 12:57:42 Branches: 18 208 8.7 %

Line Branch Exec Source
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
36
/** \author Jia Pan */
37
38
#include <hpp/fcl/distance.h>
39
#include <hpp/fcl/collision_utility.h>
40
#include <hpp/fcl/distance_func_matrix.h>
41
#include <hpp/fcl/narrowphase/narrowphase.h>
42
43
#include <iostream>
44
45
namespace hpp {
46
namespace fcl {
47
48
21454
DistanceFunctionMatrix& getDistanceFunctionLookTable() {
49


21454
  static DistanceFunctionMatrix table;
50
21454
  return table;
51
}
52
53
19444
FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2,
54
                  const DistanceRequest& request, DistanceResult& result) {
55
19444
  return distance(o1->collisionGeometryPtr(), o1->getTransform(),
56
                  o2->collisionGeometryPtr(), o2->getTransform(), request,
57
19444
                  result);
58
}
59
60
21454
FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
61
                  const CollisionGeometry* o2, const Transform3f& tf2,
62
                  const DistanceRequest& request, DistanceResult& result) {
63
21454
  GJKSolver solver(request);
64
65
21454
  const DistanceFunctionMatrix& looktable = getDistanceFunctionLookTable();
66
67
21454
  OBJECT_TYPE object_type1 = o1->getObjectType();
68
21454
  NODE_TYPE node_type1 = o1->getNodeType();
69
21454
  OBJECT_TYPE object_type2 = o2->getObjectType();
70
21454
  NODE_TYPE node_type2 = o2->getNodeType();
71
72
21454
  FCL_REAL res = (std::numeric_limits<FCL_REAL>::max)();
73
74

21454
  if (object_type1 == OT_GEOM &&
75
14923
      (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
21454
    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
21454
      res = looktable.distance_matrix[node_type1][node_type2](
106
          o1, tf1, o2, tf2, &solver, request, result);
107
    }
108
  }
109
21454
  if (solver.gjk_initial_guess == GJKInitialGuess::CachedGuess ||
110
21454
      solver.enable_cached_guess) {
111
    result.cached_gjk_guess = solver.cached_guess;
112
    result.cached_support_func_guess = solver.support_func_cached_guess;
113
  }
114
115
21454
  return res;
116
}
117
118
ComputeDistance::ComputeDistance(const CollisionGeometry* o1,
119
                                 const CollisionGeometry* o2)
120
    : o1(o1), o2(o2) {
121
  const DistanceFunctionMatrix& looktable = getDistanceFunctionLookTable();
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
146
FCL_REAL ComputeDistance::run(const Transform3f& tf1, const Transform3f& tf2,
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
164
FCL_REAL ComputeDistance::operator()(const Transform3f& tf1,
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
178
  if (solver.gjk_initial_guess == GJKInitialGuess::CachedGuess ||
179
      solver.enable_cached_guess) {
180
    result.cached_gjk_guess = solver.cached_guess;
181
    result.cached_support_func_guess = solver.support_func_cached_guess;
182
  }
183
  return res;
184
}
185
186
}  // namespace fcl
187
}  // namespace hpp