GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/collision.cpp Lines: 61 78 78.2 %
Date: 2024-02-09 12:57:42 Branches: 40 240 16.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/collision.h>
39
#include <hpp/fcl/collision_utility.h>
40
#include <hpp/fcl/collision_func_matrix.h>
41
#include <hpp/fcl/narrowphase/narrowphase.h>
42
43
#include <iostream>
44
45
namespace hpp {
46
namespace fcl {
47
48
1235473
CollisionFunctionMatrix& getCollisionFunctionLookTable() {
49


1235473
  static CollisionFunctionMatrix table;
50
1235473
  return table;
51
}
52
53
// reorder collision results in the order the call has been made.
54
20
void CollisionResult::swapObjects() {
55
20
  for (std::vector<Contact>::iterator it = contacts.begin();
56
20
       it != contacts.end(); ++it) {
57
    std::swap(it->o1, it->o2);
58
    std::swap(it->b1, it->b2);
59
    it->normal *= -1;
60
  }
61
20
}
62
63
1232447
std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
64
                    const CollisionRequest& request, CollisionResult& result) {
65
1232447
  return collide(o1->collisionGeometryPtr(), o1->getTransform(),
66
                 o2->collisionGeometryPtr(), o2->getTransform(), request,
67
1232447
                 result);
68
}
69
70
1235472
std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
71
                    const CollisionGeometry* o2, const Transform3f& tf2,
72
                    const CollisionRequest& request, CollisionResult& result) {
73
  // If securit margin is set to -infinity, return that there is no collision
74
1235472
  if (request.security_margin == -std::numeric_limits<FCL_REAL>::infinity()) {
75
    result.clear();
76
    return false;
77
  }
78
79
1235472
  GJKSolver solver(request);
80
81
1235472
  const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable();
82
  std::size_t res;
83
1235472
  if (request.num_max_contacts == 0) {
84
    HPP_FCL_THROW_PRETTY("Invalid number of max contacts (current value is 0).",
85
                         std::invalid_argument);
86
    res = 0;
87
  } else {
88
1235472
    OBJECT_TYPE object_type1 = o1->getObjectType();
89
1235472
    OBJECT_TYPE object_type2 = o2->getObjectType();
90
1235472
    NODE_TYPE node_type1 = o1->getNodeType();
91
1235472
    NODE_TYPE node_type2 = o2->getNodeType();
92
93

1235472
    if (object_type1 == OT_GEOM &&
94
1225030
        (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) {
95
20
      if (!looktable.collision_matrix[node_type2][node_type1]) {
96
        HPP_FCL_THROW_PRETTY("Collision function between node type "
97
                                 << std::string(get_node_type_name(node_type1))
98
                                 << " and node type "
99
                                 << std::string(get_node_type_name(node_type2))
100
                                 << " is not yet supported.",
101
                             std::invalid_argument);
102
        res = 0;
103
      } else {
104
20
        res = looktable.collision_matrix[node_type2][node_type1](
105
            o2, tf2, o1, tf1, &solver, request, result);
106
20
        result.swapObjects();
107
20
      }
108
    } else {
109
1235452
      if (!looktable.collision_matrix[node_type1][node_type2]) {
110
        HPP_FCL_THROW_PRETTY("Collision function between node type "
111
                                 << std::string(get_node_type_name(node_type1))
112
                                 << " and node type "
113
                                 << std::string(get_node_type_name(node_type2))
114
                                 << " is not yet supported.",
115
                             std::invalid_argument);
116
        res = 0;
117
      } else
118
1235452
        res = looktable.collision_matrix[node_type1][node_type2](
119
            o1, tf1, o2, tf2, &solver, request, result);
120
    }
121
  }
122
1235472
  if (solver.gjk_initial_guess == GJKInitialGuess::CachedGuess ||
123
1235472
      solver.enable_cached_guess) {
124
2
    result.cached_gjk_guess = solver.cached_guess;
125
2
    result.cached_support_func_guess = solver.support_func_cached_guess;
126
  }
127
128
1235472
  return res;
129
}
130
131
1
ComputeCollision::ComputeCollision(const CollisionGeometry* o1,
132
1
                                   const CollisionGeometry* o2)
133
1
    : o1(o1), o2(o2) {
134
1
  const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable();
135
136
1
  OBJECT_TYPE object_type1 = o1->getObjectType();
137
1
  NODE_TYPE node_type1 = o1->getNodeType();
138
1
  OBJECT_TYPE object_type2 = o2->getObjectType();
139
1
  NODE_TYPE node_type2 = o2->getNodeType();
140
141

2
  swap_geoms = object_type1 == OT_GEOM &&
142
1
               (object_type2 == OT_BVH || object_type2 == OT_HFIELD);
143
144

1
  if ((swap_geoms && !looktable.collision_matrix[node_type2][node_type1]) ||
145

1
      (!swap_geoms && !looktable.collision_matrix[node_type1][node_type2])) {
146
    HPP_FCL_THROW_PRETTY("Collision function between node type "
147
                             << std::string(get_node_type_name(node_type1))
148
                             << " and node type "
149
                             << std::string(get_node_type_name(node_type2))
150
                             << " is not yet supported.",
151
                         std::invalid_argument);
152
  }
153
1
  if (swap_geoms)
154
    func = looktable.collision_matrix[node_type2][node_type1];
155
  else
156
1
    func = looktable.collision_matrix[node_type1][node_type2];
157
1
}
158
159
2
std::size_t ComputeCollision::run(const Transform3f& tf1,
160
                                  const Transform3f& tf2,
161
                                  const CollisionRequest& request,
162
                                  CollisionResult& result) const {
163
  // If security margin is set to -infinity, return that there is no collision
164
2
  if (request.security_margin == -std::numeric_limits<FCL_REAL>::infinity()) {
165
    result.clear();
166
    return false;
167
  }
168
  std::size_t res;
169
2
  if (swap_geoms) {
170
    res = func(o2, tf2, o1, tf1, &solver, request, result);
171
    result.swapObjects();
172
  } else {
173
2
    res = func(o1, tf1, o2, tf2, &solver, request, result);
174
  }
175
2
  return res;
176
}
177
178
2
std::size_t ComputeCollision::operator()(const Transform3f& tf1,
179
                                         const Transform3f& tf2,
180
                                         const CollisionRequest& request,
181
                                         CollisionResult& result) const
182
183
{
184
2
  solver.set(request);
185
186
  std::size_t res;
187
2
  if (request.enable_timings) {
188
    Timer timer;
189
    res = run(tf1, tf2, request, result);
190
    result.timings = timer.elapsed();
191
  } else
192
2
    res = run(tf1, tf2, request, result);
193
194
2
  if (solver.gjk_initial_guess == GJKInitialGuess::CachedGuess ||
195
2
      solver.enable_cached_guess) {
196
2
    result.cached_gjk_guess = solver.cached_guess;
197
2
    result.cached_support_func_guess = solver.support_func_cached_guess;
198
  }
199
200
2
  return res;
201
}
202
203
}  // namespace fcl
204
}  // namespace hpp