GCC Code Coverage Report

Directory: ./
File: include/pinocchio/collision/tree-broadphase-manager.hxx
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 72 0.0%
Branches: 0 38 0.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2022 INRIA
3 //
4
5 #ifndef __pinocchio_collision_tree_broadphase_manager_hxx__
6 #define __pinocchio_collision_tree_broadphase_manager_hxx__
7
8 namespace pinocchio
9 {
10
11 template<typename Manager>
12 ✗ void TreeBroadPhaseManagerTpl<Manager>::update(bool compute_local_aabb)
13 {
14 ✗ for (auto && manager : managers)
15 {
16 ✗ manager.update(compute_local_aabb);
17 }
18 }
19
20 template<typename Manager>
21 ✗ void TreeBroadPhaseManagerTpl<Manager>::update(GeometryData * geom_data_ptr_new)
22 {
23 ✗ for (auto && manager : managers)
24 {
25 ✗ manager.update(geom_data_ptr_new);
26 }
27 }
28
29 template<typename Manager>
30 ✗ bool TreeBroadPhaseManagerTpl<Manager>::check() const
31 {
32 ✗ for (auto && manager : managers)
33 {
34 ✗ if (!manager.check())
35 ✗ return false;
36 }
37
38 ✗ return true;
39 }
40
41 template<typename Manager>
42 ✗ bool TreeBroadPhaseManagerTpl<Manager>::check(CollisionCallBackBase * callback) const
43 {
44 ✗ for (auto && manager : managers)
45 {
46 ✗ if (!manager.check(callback))
47 ✗ return false;
48 }
49
50 ✗ return true;
51 }
52
53 template<typename Manager>
54 ✗ void TreeBroadPhaseManagerTpl<Manager>::init(const size_t njoints)
55 {
56 ✗ managers.reserve(njoints);
57 ✗ for (size_t joint_id = 0; joint_id < njoints; ++joint_id)
58 {
59 ✗ GeometryObjectFilterSelectByJoint filter(joint_id);
60 ✗ managers.push_back(
61 ✗ BroadPhaseManager(&getModel(), &getGeometryModel(), &getGeometryData(), filter));
62 }
63 }
64
65 template<typename Manager>
66 ✗ bool TreeBroadPhaseManagerTpl<Manager>::collide(CollisionCallBackBase * callback) const
67 {
68 ✗ const size_t num_joints = managers.size();
69
70 ✗ callback->init();
71 ✗ const bool accumulate_save_value = callback->accumulate;
72 ✗ callback->accumulate = true;
73
74 ✗ for (size_t i = 0; i < num_joints; ++i)
75 {
76 ✗ const BroadPhaseManager & manager_outer = managers[i];
77 ✗ bool should_stop = false;
78 ✗ for (size_t j = i + 1; j < num_joints; ++j)
79 {
80 ✗ BroadPhaseManager & manager_inner = const_cast<BroadPhaseManager &>(managers[j]);
81 ✗ manager_outer.collide(manager_inner, callback);
82 ✗ should_stop = callback->stop();
83
84 ✗ if (should_stop)
85 ✗ break;
86 }
87
88 ✗ if (should_stop)
89 ✗ break;
90 }
91
92 ✗ callback->accumulate = accumulate_save_value; // restore initial value
93
94 ✗ callback->done();
95 ✗ return callback->collision;
96 }
97
98 template<typename Manager>
99 ✗ bool TreeBroadPhaseManagerTpl<Manager>::collide(
100 CollisionObject & collision_object, CollisionCallBackBase * callback) const
101 {
102 ✗ const size_t num_joints = managers.size();
103
104 ✗ callback->init();
105 ✗ const bool accumulate_save_value = callback->accumulate;
106 ✗ callback->accumulate = true;
107
108 ✗ for (size_t i = 0; i < num_joints; ++i)
109 {
110 ✗ const BroadPhaseManager & manager = managers[i];
111 ✗ manager.collide(collision_object, callback);
112 ✗ if (callback->stop())
113 ✗ break;
114 }
115
116 ✗ callback->accumulate = accumulate_save_value; // restore initial value
117
118 ✗ callback->done();
119 ✗ return callback->collision;
120 }
121
122 template<typename Manager>
123 ✗ bool TreeBroadPhaseManagerTpl<Manager>::collide(
124 TreeBroadPhaseManagerTpl & other_manager, CollisionCallBackBase * callback) const
125 {
126 ✗ const size_t num_joints = managers.size();
127
128 ✗ callback->init();
129 ✗ const bool accumulate_save_value = callback->accumulate;
130 ✗ callback->accumulate = true;
131
132 ✗ for (size_t i = 0; i < num_joints; ++i)
133 {
134 ✗ const BroadPhaseManager & manager_outer = managers[i];
135 ✗ bool should_stop = false;
136 ✗ for (auto && manager_inner : other_manager.managers)
137 {
138 ✗ manager_outer.collide(manager_inner, callback);
139 ✗ should_stop = callback->stop();
140
141 ✗ if (should_stop)
142 ✗ break;
143 }
144 ✗ if (should_stop)
145 ✗ break;
146 }
147
148 ✗ callback->accumulate = accumulate_save_value; // restore initial value
149
150 ✗ callback->done();
151 ✗ return callback->collision;
152 }
153
154 } // namespace pinocchio
155
156 #endif // ifndef __pinocchio_collision_tree_broadphase_manager_hxx__
157