-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
collisions_exist_callback.cc
54 lines (43 loc) · 1.84 KB
/
collisions_exist_callback.cc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
#include "drake/geometry/proximity/collisions_exist_callback.h"
#include "drake/geometry/proximity/proximity_utilities.h"
namespace drake {
namespace geometry {
namespace internal {
namespace has_collisions {
CallbackData::CallbackData(const CollisionFilter* collision_filter_in)
: collision_filter(*collision_filter_in) {
DRAKE_DEMAND(collision_filter_in != nullptr);
request.num_max_contacts = 1;
request.enable_contact = false;
// NOTE: As of 5/1/2018 the GJK implementation of Libccd appears to be
// superior to FCL's "independent" implementation. Furthermore, libccd
// appears to behave badly if its gjk tolerance is much tighter than
// 2e-12. Until this changes, we explicitly specify these parameters rather
// than relying on FCL's defaults.
request.gjk_tolerance = 2e-12;
request.gjk_solver_type = fcl::GJKSolverType::GST_LIBCCD;
}
bool Callback(fcl::CollisionObjectd* object_A_ptr,
fcl::CollisionObjectd* object_B_ptr,
void* callback_data) {
auto& data = *static_cast<CallbackData*>(callback_data);
const EncodedData encoding_a(*object_A_ptr);
const EncodedData encoding_b(*object_B_ptr);
const bool can_collide = data.collision_filter.CanCollideWith(
encoding_a.id(), encoding_b.id());
if (!can_collide) return false;
// Unpack the callback data.
const fcl::CollisionRequestd& request = data.request;
// This callback only works for a single contact, this confirms a request
// hasn't been made for more contacts.
DRAKE_ASSERT(request.num_max_contacts == 1);
fcl::CollisionResultd result;
// Perform nearphase collision detection.
fcl::collide(object_A_ptr, object_B_ptr, request, result);
data.collisions_exist = result.isCollision();
return data.collisions_exist;
}
} // namespace has_collisions
} // namespace internal
} // namespace geometry
} // namespace drake