GCC Code Coverage Report

Directory: ./
File: include/pinocchio/collision/broadphase.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 13 17 76.5%
Branches: 5 12 41.7%
Line Branch Exec Source
1 //
2 // Copyright (c) 2022 INRIA
3 //
4
5 #ifndef __pinocchio_collision_broadphase_hpp__
6 #define __pinocchio_collision_broadphase_hpp__
7
8 #include <hpp/fcl/broadphase/broadphase_collision_manager.h>
9
10 #include "pinocchio/multibody/fcl.hpp"
11
12 #include "pinocchio/algorithm/geometry.hpp"
13
14 #include "pinocchio/collision/broadphase-manager.hpp"
15 #include "pinocchio/collision/broadphase-callbacks.hpp"
16
17 namespace pinocchio
18 {
19
20 ///
21 /// \brief Calls computeCollision for every active pairs of GeometryData.
22 /// This function assumes that \ref updateGeometryPlacements and broadphase_manager.update() have
23 /// been called first.
24 ///
25 /// \param[in] broadphase_manager broadphase instance for collision detection.
26 /// \param[in] callback callback pointer used for collision detection.
27 /// \param[in] stopAtFirstCollision if true, stop the loop over the collision pairs when the first
28 /// collision is detected.
29 ///
30 /// \warning if stopAtFirstcollision = true, then the collisions vector will
31 /// not be entirely fulfilled (of course).
32 ///
33 template<typename BroadPhaseManagerDerived>
34 2008 bool computeCollisions(
35 BroadPhaseManagerBase<BroadPhaseManagerDerived> & broadphase_manager,
36 CollisionCallBackBase * callback)
37 {
38
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 2008 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 2008 PINOCCHIO_CHECK_INPUT_ARGUMENT(broadphase_manager.check(callback));
39 2008 broadphase_manager.collide(callback);
40 2008 callback->done();
41 2008 return callback->collision;
42 }
43
44 ///
45 /// \brief Calls computeCollision for every active pairs of GeometryData.
46 /// This function assumes that \ref updateGeometryPlacements and broadphase_manager.update() have
47 /// been called first.
48 ///
49 /// \param[in] broadphase_manager broadphase instance for collision detection.
50 /// \param[in] stopAtFirstCollision if true, stop the loop over the collision pairs when the first
51 /// collision is detected.
52 ///
53 /// \warning if stopAtFirstcollision = true, then the collisions vector will
54 /// not be entirely fulfilled (of course).
55 ///
56 template<typename BroadPhaseManagerDerived>
57 4 bool computeCollisions(
58 BroadPhaseManagerBase<BroadPhaseManagerDerived> & broadphase_manager,
59 const bool stopAtFirstCollision = false)
60 {
61 4 CollisionCallBackDefault callback(
62 broadphase_manager.getGeometryModel(), broadphase_manager.getGeometryData(),
63 stopAtFirstCollision);
64
65
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 return computeCollisions(broadphase_manager, &callback);
66 }
67
68 ///
69 /// Compute the forward kinematics, update the geometry placements and run the collision detection
70 /// using the broadphase manager.
71 ///
72 /// \tparam JointCollection Collection of Joint types.
73 /// \tparam ConfigVectorType Type of the joint configuration vector.
74 ///
75 /// \param[in] model robot model (const)
76 /// \param[out] data corresponding data (nonconst) where the forward kinematics results are stored
77 /// \param[in] broadphase_manager broadphase manager for collision detection.
78 /// \param[in] callback callback pointer used for collision detection.///
79 /// \param[in] q robot configuration.
80 /// \param[in] stopAtFirstCollision if true, stop the loop over the collision pairs when the first
81 /// collision is detected.
82 ///
83 /// \warning if stopAtFirstcollision = true, then the collisions vector will
84 /// not be entirely fulfilled (of course).
85 /// \note A similar function is available without model, data and q, not recomputing the forward
86 /// kinematics.
87 ///
88 template<
89 typename Scalar,
90 int Options,
91 template<typename, int> class JointCollectionTpl,
92 typename BroadPhaseManagerDerived,
93 typename ConfigVectorType>
94 ✗ inline bool computeCollisions(
95 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
96 DataTpl<Scalar, Options, JointCollectionTpl> & data,
97 BroadPhaseManagerBase<BroadPhaseManagerDerived> & broadphase_manager,
98 CollisionCallBackBase * callback,
99 const Eigen::MatrixBase<ConfigVectorType> & q)
100 {
101 ✗ updateGeometryPlacements(
102 model, data, broadphase_manager.getGeometryModel(), broadphase_manager.getGeometryData(), q);
103
104 ✗ broadphase_manager.update(false);
105 ✗ return computeCollisions(broadphase_manager, &callback);
106 }
107
108 ///
109 /// Compute the forward kinematics, update the geometry placements and run the collision detection
110 /// using the broadphase manager.
111 ///
112 /// \tparam JointCollection Collection of Joint types.
113 /// \tparam ConfigVectorType Type of the joint configuration vector.
114 ///
115 /// \param[in] model robot model (const)
116 /// \param[out] data corresponding data (nonconst) where the forward kinematics results are stored
117 /// \param[in] broadphase_manager broadphase manager for collision detection.
118 /// \param[in] q robot configuration.
119 /// \param[in] stopAtFirstCollision if true, stop the loop over the collision pairs when the first
120 /// collision is detected.
121 ///
122 /// \warning if stopAtFirstcollision = true, then the collisions vector will
123 /// not be entirely fulfilled (of course).
124 /// \note A similar function is available without model, data and q, not recomputing the forward
125 /// kinematics.
126 ///
127 template<
128 typename Scalar,
129 int Options,
130 template<typename, int> class JointCollectionTpl,
131 typename BroadPhaseManagerDerived,
132 typename ConfigVectorType>
133 2004 inline bool computeCollisions(
134 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
135 DataTpl<Scalar, Options, JointCollectionTpl> & data,
136 BroadPhaseManagerBase<BroadPhaseManagerDerived> & broadphase_manager,
137 const Eigen::MatrixBase<ConfigVectorType> & q,
138 const bool stopAtFirstCollision = false)
139 {
140
1/2
✓ Branch 3 taken 2004 times.
✗ Branch 4 not taken.
 2004 updateGeometryPlacements(
141 model, data, broadphase_manager.getGeometryModel(), broadphase_manager.getGeometryData(), q);
142
143
1/2
✓ Branch 1 taken 2004 times.
✗ Branch 2 not taken.
 2004 broadphase_manager.update(false);
144
145 2004 CollisionCallBackDefault callback(
146 broadphase_manager.getGeometryModel(), broadphase_manager.getGeometryData(),
147 stopAtFirstCollision);
148
1/2
✓ Branch 1 taken 2004 times.
✗ Branch 2 not taken.
 4008 return computeCollisions(broadphase_manager, &callback);
149 }
150
151 } // namespace pinocchio
152
153 /* --- Details -------------------------------------------------------------------- */
154 #include "pinocchio/collision/broadphase.hxx"
155
156 #endif // ifndef __pinocchio_collision_broadphase_hpp__
157