48 #include <pcl/pcl_exports.h>
64 template<
class UserData>
127 Node (std::vector<BoundedObject*>& sorted_objects,
int first_id,
int last_id)
130 memcpy (bounds_, sorted_objects[first_id]->getBounds (), 6*
sizeof (
float));
133 for (
int i = first_id + 1 ; i <= last_id ; ++i )
137 if ( first_id != last_id )
140 int mid_id = (first_id + last_id) >> 1;
141 children_[0] =
new Node(sorted_objects, first_id, mid_id);
142 children_[1] =
new Node(sorted_objects, mid_id + 1, last_id);
147 object_ = sorted_objects[first_id];
148 children_[0] = children_[1] =
nullptr;
164 return static_cast<bool>(children_[0]);
188 return !static_cast<bool>(children_[0]);
195 return !(box[1] < bounds_[0] || box[3] < bounds_[2] || box[5] < bounds_[4] ||
196 box[0] > bounds_[1] || box[2] > bounds_[3] || box[4] > bounds_[5]);
203 return (bounds_[1] - bounds_[0]) * (bounds_[3] - bounds_[2]) * (bounds_[5] - bounds_[4]);
217 sorted_objects_ (nullptr)
233 build(std::vector<BoundedObject*>& objects)
237 if ( objects.empty () )
240 sorted_objects_ = &objects;
243 std::sort (objects.begin (), objects.end (), BoundedObject::compareCentroidsXCoordinates);
246 root_ =
new Node (objects, 0, static_cast<int> (objects.size () - 1));
260 inline const std::vector<BoundedObject*>*
263 return (sorted_objects_);
269 intersect(
const float box[6], std::list<BoundedObject*>& intersected_objects)
const
274 bool got_intersection =
false;
277 std::list<Node*> working_list;
278 working_list.push_back (root_);
280 while ( !working_list.empty () )
282 Node* node = working_list.front ();
283 working_list.pop_front ();
296 intersected_objects.push_back (node->
getObject ());
297 got_intersection =
true;
302 return (got_intersection);