GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/broadphase/default_broadphase_callbacks.cpp Lines: 15 41 36.6 %
Date: 2024-02-09 12:57:42 Branches: 5 22 22.7 %

Line Branch Exec Source
1
/*
2
 * Software License Agreement (BSD License)
3
 *
4
 *  Copyright (c) 2020, Toyota Research Institute
5
 *  All rights reserved.
6
 *
7
 *  Redistribution and use in source and binary forms, with or without
8
 *  modification, are permitted provided that the following conditions
9
 *  are met:
10
 *
11
 *   * Redistributions of source code must retain the above copyright
12
 *     notice, this list of conditions and the following disclaimer.
13
 *   * Redistributions in binary form must reproduce the above
14
 *     copyright notice, this list of conditions and the following
15
 *     disclaimer in the documentation and/or other materials provided
16
 *     with the distribution.
17
 *   * Neither the name of the copyright holder nor the names of its
18
 *     contributors may be used to endorse or promote products derived
19
 *     from this software without specific prior written permission.
20
 *
21
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32
 *  POSSIBILITY OF SUCH DAMAGE.
33
 */
34
35
/** @author Sean Curtis (sean@tri.global) */
36
37
#include "hpp/fcl/broadphase/default_broadphase_callbacks.h"
38
#include <algorithm>
39
40
namespace hpp {
41
namespace fcl {
42
43
1
bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2,
44
                              void* data) {
45
1
  assert(data != nullptr);
46
1
  auto* collision_data = static_cast<CollisionData*>(data);
47
1
  const CollisionRequest& request = collision_data->request;
48
1
  CollisionResult& result = collision_data->result;
49
50
1
  if (collision_data->done) return true;
51
52
1
  collide(o1, o2, request, result);
53
54

2
  if (result.isCollision() &&
55
1
      result.numContacts() >= request.num_max_contacts) {
56
1
    collision_data->done = true;
57
  }
58
59
1
  return collision_data->done;
60
}
61
62
1224141
bool CollisionCallBackDefault::collide(CollisionObject* o1,
63
                                       CollisionObject* o2) {
64
1224141
  return defaultCollisionFunction(o1, o2, &data);
65
}
66
67
bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2,
68
                             void* data, FCL_REAL& dist) {
69
  assert(data != nullptr);
70
  auto* cdata = static_cast<DistanceData*>(data);
71
  const DistanceRequest& request = cdata->request;
72
  DistanceResult& result = cdata->result;
73
74
  if (cdata->done) {
75
    dist = result.min_distance;
76
    return true;
77
  }
78
79
  distance(o1, o2, request, result);
80
81
  dist = result.min_distance;
82
83
  if (dist <= 0) return true;  // in collision or in touch
84
85
  return cdata->done;
86
}
87
88
17230
bool DistanceCallBackDefault::distance(CollisionObject* o1, CollisionObject* o2,
89
                                       FCL_REAL& dist) {
90
17230
  return defaultDistanceFunction(o1, o2, &data, dist);
91
}
92
93
CollisionCallBackCollect::CollisionCallBackCollect(const size_t max_size)
94
    : max_size(max_size) {
95
  collision_pairs.resize(max_size);
96
}
97
98
bool CollisionCallBackCollect::collide(CollisionObject* o1,
99
                                       CollisionObject* o2) {
100
  collision_pairs.push_back(std::make_pair(o1, o2));
101
  return false;
102
}
103
104
size_t CollisionCallBackCollect::numCollisionPairs() const {
105
  return collision_pairs.size();
106
}
107
108
const std::vector<CollisionCallBackCollect::CollisionPair>&
109
CollisionCallBackCollect::getCollisionPairs() const {
110
  return collision_pairs;
111
}
112
113
void CollisionCallBackCollect::init() { collision_pairs.clear(); }
114
115
bool CollisionCallBackCollect::exist(const CollisionPair& pair) const {
116
  return std::find(collision_pairs.begin(), collision_pairs.end(), pair) !=
117
         collision_pairs.end();
118
}
119
120
}  // namespace fcl
121
}  // namespace hpp