GCC Code Coverage Report

Directory: ./
File: include/coal/broadphase/default_broadphase_callbacks.h
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 14 15 93.3%
Branches: 0 0 -%
Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2020, Toyota Research Institute
5 * Copyright (c) 2022-2023, INRIA
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 the copyright holder 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 Sean Curtis (sean@tri.global) */
37 /** @author Justin Carpentier (justin.carpentier@inria.fr) */
38
39 #ifndef COAL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H
40 #define COAL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H
41
42 #include "coal/broadphase/broadphase_callbacks.h"
43 #include "coal/collision.h"
44 #include "coal/distance.h"
45 // #include "coal/narrowphase/continuous_collision.h"
46 // #include "coal/narrowphase/continuous_collision_request.h"
47 // #include "coal/narrowphase/continuous_collision_result.h"
48 // #include "coal/narrowphase/distance_request.h"
49 // #include "coal/narrowphase/distance_result.h"
50
51 namespace coal {
52
53 /// @brief Collision data stores the collision request and the result given by
54 /// collision algorithm.
55 struct CollisionData {
56 35655 CollisionData() { done = false; }
57
58 /// @brief Collision request
59 CollisionRequest request;
60
61 /// @brief Collision result
62 CollisionResult result;
63
64 /// @brief Whether the collision iteration can stop
65 bool done;
66
67 /// @brief Clears the CollisionData
68 31689 void clear() {
69 31689 result.clear();
70 31689 done = false;
71 31689 }
72 };
73
74 /// @brief Distance data stores the distance request and the result given by
75 /// distance algorithm.
76 struct DistanceData {
77 774 DistanceData() { done = false; }
78
79 /// @brief Distance request
80 DistanceRequest request;
81
82 /// @brief Distance result
83 DistanceResult result;
84
85 /// @brief Whether the distance iteration can stop
86 bool done;
87
88 /// @brief Clears the DistanceData
89 688 void clear() {
90 688 result.clear();
91 688 done = false;
92 688 }
93 };
94
95 /// @brief Provides a simple callback for the collision query in the
96 /// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and
97 /// points to an instance of CollisionData. It simply invokes the
98 /// `collide()` method on the culled pair of geometries and stores the results
99 /// in the data's CollisionResult instance.
100 ///
101 /// This callback will cause the broadphase evaluation to stop if:
102 /// - the collision requests _disables_ cost _and_
103 /// - the collide() reports a collision for the culled pair, _and_
104 /// - we've reported the number of contacts requested in the CollisionRequest.
105 ///
106 /// For a given instance of CollisionData, if broadphase evaluation has
107 /// already terminated (i.e., defaultCollisionFunction() returned `true`),
108 /// subsequent invocations with the same instance of CollisionData will
109 /// return immediately, requesting termination of broadphase evaluation (i.e.,
110 /// return `true`).
111 ///
112 /// @param o1 The first object in the culled pair.
113 /// @param o2 The second object in the culled pair.
114 /// @param data A non-null pointer to a CollisionData instance.
115 /// @return `true` if the broadphase evaluation should stop.
116 /// @tparam S The scalar type with which the computation will be performed.
117 bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2,
118 void* data);
119
120 /// @brief Collision data for use with the DefaultContinuousCollisionFunction.
121 /// It stores the collision request and the result given by the collision
122 /// algorithm (and stores the conclusion of whether further evaluation of the
123 /// broadphase collision manager has been deemed unnecessary).
124 // struct DefaultContinuousCollisionData {
125 // ContinuousCollisionRequest request;
126 // ContinuousCollisionResult result;
127 //
128 // /// If `true`, requests that the broadphase evaluation stop.
129 // bool done{false};
130 // };
131
132 /// @brief Provides a simple callback for the continuous collision query in the
133 /// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and
134 /// points to an instance of DefaultContinuousCollisionData. It simply invokes
135 /// the `collide()` method on the culled pair of geometries and stores the
136 /// results in the data's ContinuousCollisionResult instance.
137 ///
138 /// This callback will never cause the broadphase evaluation to terminate early.
139 /// However, if the `done` member of the DefaultContinuousCollisionData is set
140 /// to true, this method will simply return without doing any computation.
141 ///
142 /// For a given instance of DefaultContinuousCollisionData, if broadphase
143 /// evaluation has already terminated (i.e.,
144 /// DefaultContinuousCollisionFunction() returned `true`), subsequent
145 /// invocations with the same instance of CollisionData will return
146 /// immediately, requesting termination of broadphase evaluation (i.e., return
147 /// `true`).
148 ///
149 /// @param o1 The first object in the culled pair.
150 /// @param o2 The second object in the culled pair.
151 /// @param data A non-null pointer to a CollisionData instance.
152 /// @return True if the broadphase evaluation should stop.
153 /// @tparam S The scalar type with which the computation will be performed.
154 // bool DefaultContinuousCollisionFunction(ContinuousCollisionObject* o1,
155 // ContinuousCollisionObject* o2,
156 // void* data) {
157 // assert(data != nullptr);
158 // auto* cdata = static_cast<DefaultContinuousCollisionData*>(data);
159 //
160 // if (cdata->done) return true;
161 //
162 // const ContinuousCollisionRequest& request = cdata->request;
163 // ContinuousCollisionResult& result = cdata->result;
164 // collide(o1, o2, request, result);
165 //
166 // return cdata->done;
167 // }
168
169 /// @brief Provides a simple callback for the distance query in the
170 /// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and
171 /// points to an instance of DistanceData. It simply invokes the
172 /// `distance()` method on the culled pair of geometries and stores the results
173 /// in the data's DistanceResult instance.
174 ///
175 /// This callback will cause the broadphase evaluation to stop if:
176 /// - The distance is less than or equal to zero (i.e., the pair is in
177 /// contact).
178 ///
179 /// For a given instance of DistanceData, if broadphase evaluation has
180 /// already terminated (i.e., defaultDistanceFunction() returned `true`),
181 /// subsequent invocations with the same instance of DistanceData will
182 /// simply report the previously reported minimum distance and request
183 /// termination of broadphase evaluation (i.e., return `true`).
184 ///
185 /// @param o1 The first object in the culled pair.
186 /// @param o2 The second object in the culled pair.
187 /// @param data A non-null pointer to a DistanceData instance.
188 /// @param dist The distance computed by distance().
189 /// @return `true` if the broadphase evaluation should stop.
190 /// @tparam S The scalar type with which the computation will be performed.
191 bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2,
192 void* data, Scalar& dist);
193
194 /// @brief Default collision callback to check collision between collision
195 /// objects.
196 struct COAL_DLLAPI CollisionCallBackDefault : CollisionCallBackBase {
197 /// @brief Initialize the callback.
198 /// Clears the collision result and sets the done boolean to false.
199 31689 void init() { data.clear(); }
200
201 bool collide(CollisionObject* o1, CollisionObject* o2);
202
203 CollisionData data;
204
205 71094 virtual ~CollisionCallBackDefault() {};
206 };
207
208 /// @brief Default distance callback to check collision between collision
209 /// objects.
210 struct COAL_DLLAPI DistanceCallBackDefault : DistanceCallBackBase {
211 /// @brief Initialize the callback.
212 /// Clears the distance result and sets the done boolean to false.
213 688 void init() { data.clear(); }
214
215 bool distance(CollisionObject* o1, CollisionObject* o2, Scalar& dist);
216
217 DistanceData data;
218
219 1548 virtual ~DistanceCallBackDefault() {};
220 };
221
222 /// @brief Collision callback to collect collision pairs potentially in contacts
223 struct COAL_DLLAPI CollisionCallBackCollect : CollisionCallBackBase {
224 typedef std::pair<CollisionObject*, CollisionObject*> CollisionPair;
225
226 /// @brief Default constructor.
227 CollisionCallBackCollect(const size_t max_size);
228
229 bool collide(CollisionObject* o1, CollisionObject* o2);
230
231 /// @brief Returns the number of registered collision pairs
232 size_t numCollisionPairs() const;
233
234 /// @brief Returns a const reference to the active collision_pairs to check
235 const std::vector<CollisionPair>& getCollisionPairs() const;
236
237 /// @brief Reset the callback
238 void init();
239
240 /// @brief Check whether a collision pair exists
241 bool exist(const CollisionPair& pair) const;
242
243 /// @brief Check whether a collision pair exists
244 bool exist(CollisionObject* o1, CollisionObject* o2) const;
245
246 ✗ virtual ~CollisionCallBackCollect() {};
247
248 protected:
249 std::vector<CollisionPair> collision_pairs;
250 size_t max_size;
251 };
252
253 } // namespace coal
254
255 #endif // COAL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H
256