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;
161 return static_cast<bool>(children_[0]);
185 return !
static_cast<bool>(children_[0]);
192 return !(box[1] < bounds_[0] || box[3] < bounds_[2] || box[5] < bounds_[4] ||
193 box[0] > bounds_[1] || box[2] > bounds_[3] || box[4] > bounds_[5]);
200 return (bounds_[1] - bounds_[0]) * (bounds_[3] - bounds_[2]) * (bounds_[5] - bounds_[4]);
214 sorted_objects_ (nullptr)
230 build(std::vector<BoundedObject*>& objects)
234 if ( objects.empty () )
237 sorted_objects_ = &objects;
240 std::sort (objects.begin (), objects.end (), BoundedObject::compareCentroidsXCoordinates);
243 root_ =
new Node (objects, 0,
static_cast<int> (objects.size () - 1));
254 inline const std::vector<BoundedObject*>*
257 return (sorted_objects_);
263 intersect(
const float box[6], std::list<BoundedObject*>& intersected_objects)
const
268 bool got_intersection =
false;
271 std::list<Node*> working_list;
272 working_list.push_back (root_);
274 while ( !working_list.empty () )
276 Node* node = working_list.front ();
277 working_list.pop_front ();
290 intersected_objects.push_back (node->
getObject ());
291 got_intersection =
true;
296 return (got_intersection);