GCC Code Coverage Report

Directory: ./
File: include/pinocchio/collision/broadphase-callbacks.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 62 69 89.9%
Branches: 28 132 21.2%
Line Branch Exec Source
1 //
2 // Copyright (c) 2022 INRIA
3 //
4
5 #ifndef __pinocchio_collision_broadphase_callback_hpp__
6 #define __pinocchio_collision_broadphase_callback_hpp__
7
8 #include <hpp/fcl/broadphase/broadphase_callbacks.h>
9
10 #include "pinocchio/multibody/fcl.hpp"
11 #include "pinocchio/multibody/geometry.hpp"
12
13 #include "pinocchio/collision/collision.hpp"
14
15 namespace pinocchio
16 {
17
18 /// @brief Interface for Pinocchio collision callback functors
19 struct CollisionCallBackBase : hpp::fcl::CollisionCallBackBase
20 {
21 2008 CollisionCallBackBase(const GeometryModel & geometry_model, GeometryData & geometry_data)
22 4016 : geometry_model_ptr(&geometry_model)
23 2008 , geometry_data_ptr(&geometry_data)
24 2008 , collision(false)
25 2008 , accumulate(false)
26 {
27 2008 }
28
29 34136 const GeometryModel & getGeometryModel() const
30 {
31 34136 return *geometry_model_ptr;
32 }
33 const GeometryData & getGeometryData() const
34 {
35 return *geometry_data_ptr;
36 }
37 34136 GeometryData & getGeometryData()
38 {
39 34136 return *geometry_data_ptr;
40 }
41
42 /// \brief If true, the stopping criteria related to the collision callback has been met and one
43 /// can stop.
44 virtual bool stop() const = 0;
45
46 /// \brief Callback method called after the termination of a collisition detection algorithm.
47 /// The default implementation does nothing.
48 ✗ virtual void done() {};
49
50 protected:
51 /// @brief Geometry model associated to the callback
52 const GeometryModel * geometry_model_ptr;
53
54 /// @brief Geometry data associated to the callback
55 GeometryData * geometry_data_ptr;
56
57 public:
58 /// @brief Whether there is a collision or not
59 bool collision;
60
61 /// @brief Whether the callback is used in an accumulate mode where several collide methods are
62 /// called successively.
63 bool accumulate;
64 };
65
66 struct CollisionCallBackDefault : CollisionCallBackBase
67 {
68 2008 CollisionCallBackDefault(
69 const GeometryModel & geometry_model,
70 GeometryData & geometry_data,
71 bool stopAtFirstCollision = false)
72 2008 : CollisionCallBackBase(geometry_model, geometry_data)
73 2008 , stopAtFirstCollision(stopAtFirstCollision)
74 2008 , count(0)
75 // , visited(Eigen::MatrixXd::Zero(geometry_model.ngeoms,geometry_model.ngeoms))
76 {
77 2008 }
78
79 532120 void init()
80 {
81
2/2
✓ Branch 0 taken 530112 times.
✓ Branch 1 taken 2008 times.
 532120 if (accumulate) // skip reseting of the parameters
82 530112 return;
83
84 2008 count = 0;
85 2008 collision = false;
86 2008 collisionPairIndex = std::numeric_limits<PairIndex>::max();
87 // visited.setZero();
88 }
89
90 67826 bool collide(hpp::fcl::CollisionObject * o1, hpp::fcl::CollisionObject * o2)
91 {
92
93
1/2
✓ Branch 1 taken 67826 times.
✗ Branch 2 not taken.
 67826 assert(!stop() && "must never happened");
94
95 67826 CollisionObject & co1 = reinterpret_cast<CollisionObject &>(*o1);
96 67826 CollisionObject & co2 = reinterpret_cast<CollisionObject &>(*o2);
97
98 67826 const Eigen::DenseIndex go1_index = (Eigen::DenseIndex)co1.geometryObjectIndex;
99 67826 const Eigen::DenseIndex go2_index = (Eigen::DenseIndex)co2.geometryObjectIndex;
100
101 67826 const GeometryModel & geometry_model = *geometry_model_ptr;
102
103
2/6
✓ Branch 0 taken 67826 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 67826 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 67826 PINOCCHIO_CHECK_INPUT_ARGUMENT(
104 go1_index < (Eigen::DenseIndex)geometry_model.ngeoms && go1_index >= 0);
105
2/6
✓ Branch 0 taken 67826 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 67826 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 67826 PINOCCHIO_CHECK_INPUT_ARGUMENT(
106 go2_index < (Eigen::DenseIndex)geometry_model.ngeoms && go2_index >= 0);
107
108
1/2
✓ Branch 1 taken 67826 times.
✗ Branch 2 not taken.
 67826 const int pair_index = geometry_model.collisionPairMapping(go1_index, go2_index);
109
2/2
✓ Branch 0 taken 57516 times.
✓ Branch 1 taken 10310 times.
 67826 if (pair_index == -1)
110 57516 return false;
111
112 10310 const GeometryData & geometry_data = *geometry_data_ptr;
113 10310 const CollisionPair & cp = geometry_model.collisionPairs[(PairIndex)pair_index];
114 const bool do_collision_check =
115
1/2
✓ Branch 1 taken 10310 times.
✗ Branch 2 not taken.
 10310 geometry_data.activeCollisionPairs[(PairIndex)pair_index]
116
1/2
✓ Branch 0 taken 10310 times.
✗ Branch 1 not taken.
 20620 && !(
117
1/2
✓ Branch 1 taken 10310 times.
✗ Branch 2 not taken.
 10310 geometry_model.geometryObjects[cp.first].disableCollision
118
1/2
✓ Branch 1 taken 10310 times.
✗ Branch 2 not taken.
 10310 || geometry_model.geometryObjects[cp.second].disableCollision);
119
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 10310 times.
 10310 if (!do_collision_check)
120 ✗ return false;
121
122 10310 count++;
123
124 fcl::CollisionRequest collision_request(
125
1/2
✓ Branch 2 taken 10310 times.
✗ Branch 3 not taken.
 10310 geometry_data_ptr->collisionRequests[size_t(pair_index)]);
126 10310 collision_request.gjk_variant = fcl::GJKVariant::NesterovAcceleration;
127 // collision_request.gjk_initial_guess = fcl::GJKInitialGuess::BoundingVolumeGuess;
128
129 10310 if (
130 10310 co1.collisionGeometry().get()
131 10310 != geometry_model.geometryObjects[size_t(go1_index)].geometry.get()
132
3/6
✓ Branch 0 taken 10310 times.
✗ Branch 1 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 10310 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 10310 times.
 20620 || co2.collisionGeometry().get()
133 10310 != geometry_model.geometryObjects[size_t(go2_index)].geometry.get())
134 ✗ PINOCCHIO_THROW_PRETTY(
135 std::logic_error, "go1: " << go1_index << " or go2: " << go2_index
136 << " have not been updated and have missmatching pointers.");
137 // if(!(co1.collisionGeometry()->aabb_local.volume() < 0 ||
138 // co2.collisionGeometry()->aabb_local.volume() <0)) { // TODO(jcarpent): check potential
139 // bug
140 // collision_request.gjk_initial_guess = fcl::GJKInitialGuess::BoundingVolumeGuess;
141 // }
142
143 bool res;
144 try
145 {
146 20620 res = computeCollision(
147
1/2
✓ Branch 1 taken 10310 times.
✗ Branch 2 not taken.
 10310 *geometry_model_ptr, *geometry_data_ptr, (PairIndex)pair_index, collision_request);
148 }
149 ✗ catch (std::logic_error & e)
150 {
151 ✗ PINOCCHIO_THROW_PRETTY(
152 std::logic_error, "Geometries with index go1: "
153 << go1_index << " or go2: " << go2_index
154 << " have produced an internal error within HPP-FCL.\n what:\n"
155 << e.what());
156 }
157
158
4/4
✓ Branch 0 taken 1004 times.
✓ Branch 1 taken 9306 times.
✓ Branch 2 taken 330 times.
✓ Branch 3 taken 674 times.
 10310 if (res && !collision)
159 {
160 330 collision = true;
161 330 collisionPairIndex = (PairIndex)pair_index;
162 }
163
164
1/2
✓ Branch 0 taken 10310 times.
✗ Branch 1 not taken.
 10310 if (!stopAtFirstCollision)
165 10310 return false;
166 else
167 ✗ return res;
168 }
169
170 597938 bool stop() const final
171 {
172
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 597938 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 597938 if (stopAtFirstCollision && collision)
173 ✗ return true;
174
175 597938 return false;
176 }
177
178 3012 void done() final
179 {
180
2/2
✓ Branch 0 taken 495 times.
✓ Branch 1 taken 2517 times.
 3012 if (collision)
181 495 geometry_data_ptr->collisionPairIndex = collisionPairIndex;
182 3012 }
183
184 /// @brief Whether to stop or not when localizing a first collision
185 bool stopAtFirstCollision;
186
187 /// @brief The collision index of the first pair in collision
188 PairIndex collisionPairIndex;
189
190 /// @brief Number of visits of the collide method
191 size_t count;
192
193 // Eigen::MatrixXd visited;
194 };
195
196 } // namespace pinocchio
197
198 /* --- Details -------------------------------------------------------------------- */
199 #include "pinocchio/collision/broadphase-callbacks.hxx"
200
201 #endif // ifndef __pinocchio_collision_broadphase_callback_hpp__
202