void SelectionHandler::updateTrackedBoxes()
{
M_HandleToBox::iterator it = boxes_.begin();
M_HandleToBox::iterator end = boxes_.end();
for (; it != end; ++it)
{
V_AABB aabbs;
Picked p(it->first.first);
p.extra_handles.insert(it->first.second);
getAABBs(Picked(it->first.first), aabbs);
if (!aabbs.empty())
{
Ogre::AxisAlignedBox combined;
V_AABB::iterator aabb_it = aabbs.begin();
V_AABB::iterator aabb_end = aabbs.end();
for (; aabb_it != aabb_end; ++aabb_it)
{
combined.merge(*aabb_it);
}
createBox(std::make_pair(p.handle, it->first.second), combined, "RVIZ/Cyan");
}
}
}
void SelectionHandler::getAABBs(const Picked& obj, V_AABB& aabbs)
{
S_Movable::iterator it = tracked_objects_.begin();
S_Movable::iterator end = tracked_objects_.end();
for (; it != end; ++it)
{
aabbs.push_back((*it)->getWorldBoundingBox());
}
}
void SelectionHandler::onSelect(const Picked& obj)
{
ROS_DEBUG("Selected 0x%08x", obj.handle);
V_AABB aabbs;
getAABBs(obj, aabbs);
if (!aabbs.empty())
{
Ogre::AxisAlignedBox combined;
V_AABB::iterator it = aabbs.begin();
V_AABB::iterator end = aabbs.end();
for (; it != end; ++it)
{
combined.merge(*it);
}
createBox(std::make_pair(obj.handle, 0ULL), combined, "RVIZ/Cyan");
}
}
void PointCloudSelectionHandler::getAABBs(const Picked& obj, V_AABB& aabbs)
{
S_uint64::iterator it = obj.extra_handles.begin();
S_uint64::iterator end = obj.extra_handles.end();
for (; it != end; ++it)
{
M_HandleToBox::iterator find_it = boxes_.find(std::make_pair(obj.handle, *it - 1));
if (find_it != boxes_.end())
{
Ogre::WireBoundingBox* box = find_it->second.second;
aabbs.push_back(box->getWorldBoundingBox());
}
}
}