diff --git a/src/rviz/ogre_helpers/point_cloud.cpp b/src/rviz/ogre_helpers/point_cloud.cpp index 5861cdc170..2025703cf2 100644 --- a/src/rviz/ogre_helpers/point_cloud.cpp +++ b/src/rviz/ogre_helpers/point_cloud.cpp @@ -481,7 +481,6 @@ void PointCloud::addPoints(Point* points, uint32_t num_points) Ogre::AxisAlignedBox aabb; aabb.setNull(); uint32_t current_vertex_count = 0; - bounding_radius_ = 0.0f; uint32_t vertex_size = 0; uint32_t buffer_size = 0; for (uint32_t current_point = 0; current_point < num_points; ++current_point) @@ -540,7 +539,6 @@ void PointCloud::addPoints(Point* points, uint32_t num_points) } aabb.merge(p.position); - bounding_radius_ = std::max(bounding_radius_, p.position.squaredLength()); float x = p.position.x; float y = p.position.y; @@ -571,6 +569,7 @@ void PointCloud::addPoints(Point* points, uint32_t num_points) op->vertexData->vertexCount = current_vertex_count - op->vertexData->vertexStart; rend->setBoundingBox(aabb); bounding_box_.merge(aabb); + bounding_radius_ = bounding_box_.getHalfSize().length(); ROS_ASSERT(op->vertexData->vertexCount + op->vertexData->vertexStart <= rend->getBuffer()->getNumVertices()); @@ -620,13 +619,12 @@ void PointCloud::popPoints(uint32_t num_points) // reset bounds bounding_box_.setNull(); - bounding_radius_ = 0.0f; for (uint32_t i = 0; i < point_count_; ++i) { Point& p = points_[i]; bounding_box_.merge(p.position); - bounding_radius_ = std::max(bounding_radius_, p.position.squaredLength()); } + bounding_radius_ = bounding_box_.getHalfSize().length(); shrinkRenderables(); @@ -813,17 +811,14 @@ void PointCloudRenderable::_notifyCurrentCamera(Ogre::Camera* camera) Ogre::Real PointCloudRenderable::getBoundingRadius() const { - return Ogre::Math::Sqrt(std::max(mBox.getMaximum().squaredLength(), mBox.getMinimum().squaredLength())); + return mBox.getHalfSize().length(); } Ogre::Real PointCloudRenderable::getSquaredViewDepth(const Ogre::Camera* cam) const { - Ogre::Vector3 vMin, vMax, vMid, vDist; - vMin = mBox.getMinimum(); - vMax = mBox.getMaximum(); - vMid = ((vMax - vMin) * 0.5) + vMin; - vDist = cam->getDerivedPosition() - vMid; - + const Ogre::Vector3 vWorldCenter = + _getParentNodeFullTransform().transformAffine(getBoundingBox().getCenter()); + const Ogre::Vector3 vDist = cam->getDerivedPosition() - vWorldCenter; return vDist.squaredLength(); } diff --git a/src/rviz/ogre_helpers/point_cloud.h b/src/rviz/ogre_helpers/point_cloud.h index 6ddefbd759..a93fe76d6d 100644 --- a/src/rviz/ogre_helpers/point_cloud.h +++ b/src/rviz/ogre_helpers/point_cloud.h @@ -96,8 +96,7 @@ typedef std::vector V_PointCloudRenderable; * \brief A visual representation of a set of points. * * Displays a set of points using any number of Ogre BillboardSets. PointCloud is optimized for sets of - * points that change - * rapidly, rather than for large clouds that never change. + * points that change rapidly, rather than for large clouds that never change. * * Most of the functions in PointCloud are not safe to call from any thread but the render thread. * Exceptions are clear() and addPoints(), which