33 #include <OgreSceneManager.h> 34 #include <OgreSceneNode.h> 35 #include <OgreVector3.h> 36 #include <OgreQuaternion.h> 37 #include <OgreManualObject.h> 38 #include <OgreMaterialManager.h> 39 #include <OgreBillboardSet.h> 40 #include <OgreBillboard.h> 41 #include <OgreTexture.h> 42 #include <OgreTextureManager.h> 43 #include <OgreSharedPtr.h> 44 #include <OgreTechnique.h> 45 #include <OgreCamera.h> 52 #define VERTEX_BUFFER_CAPACITY (36 * 1024 * 10) 75 -0.866025404f, -0.5f, 0.0f,
76 0.866025404f, -0.5f, 0.0f,
133 : bounding_radius_( 0.0
f )
135 , common_direction_(
Ogre::
Vector3::NEGATIVE_UNIT_Z )
137 , color_by_index_(false)
138 , current_mode_supports_geometry_shader_(false)
140 std::stringstream ss;
141 static int count = 0;
142 ss <<
"PointCloudMaterial" << count++;
143 point_material_ = Ogre::MaterialManager::getSingleton().getByName(
"rviz/PointCloudPoint");
144 square_material_ = Ogre::MaterialManager::getSingleton().getByName(
"rviz/PointCloudSquare");
145 flat_square_material_ = Ogre::MaterialManager::getSingleton().getByName(
"rviz/PointCloudFlatSquare");
146 sphere_material_ = Ogre::MaterialManager::getSingleton().getByName(
"rviz/PointCloudSphere");
147 tile_material_ = Ogre::MaterialManager::getSingleton().getByName(
"rviz/PointCloudTile");
148 box_material_ = Ogre::MaterialManager::getSingleton().getByName(
"rviz/PointCloudBox");
173 Ogre::ResourcePtr resource(material);
174 Ogre::MaterialManager::getSingleton().remove(resource);
208 *xform = _getParentNodeFullTransform();
217 if (getParentSceneNode())
219 V_PointCloudRenderable::iterator it =
renderables_.begin();
220 V_PointCloudRenderable::iterator end =
renderables_.end();
221 for (; it != end; ++it)
223 getParentSceneNode()->detachObject(it->get());
225 getParentSceneNode()->needUpdate();
255 Ogre::Vector4 highlight( r, g, b, 0.0
f );
257 V_PointCloudRenderable::iterator it =
renderables_.begin();
258 V_PointCloudRenderable::iterator end =
renderables_.end();
259 for (; it != end; ++it)
298 bool geom_support_changed =
false;
306 geom_support_changed =
true;
317 geom_support_changed =
true;
325 geom_support_changed =
true;
331 if (geom_support_changed)
336 V_PointCloudRenderable::iterator it =
renderables_.begin();
337 V_PointCloudRenderable::iterator end =
renderables_.end();
338 for (; it != end; ++it)
354 V_PointCloudRenderable::iterator it =
renderables_.begin();
355 V_PointCloudRenderable::iterator end =
renderables_.end();
356 for (; it != end; ++it)
364 V_PointCloudRenderable::iterator it =
renderables_.begin();
365 V_PointCloudRenderable::iterator end =
renderables_.end();
366 for (; it != end; ++it)
376 V_PointCloudRenderable::iterator it =
renderables_.begin();
377 V_PointCloudRenderable::iterator end =
renderables_.end();
378 for (; it != end; ++it)
388 V_PointCloudRenderable::iterator it =
renderables_.begin();
389 V_PointCloudRenderable::iterator end =
renderables_.end();
390 for (; it != end; ++it)
392 (*it)->setCustomParameter(
UP_PARAMETER, Ogre::Vector4(vec));
398 if (mat->getBestTechnique())
400 mat->getBestTechnique()->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
401 mat->getBestTechnique()->setDepthWriteEnabled(
false );
407 if (mat->getBestTechnique())
409 mat->getBestTechnique()->setSceneBlending( Ogre::SBT_REPLACE );
410 mat->getBestTechnique()->setDepthWriteEnabled(
true );
418 if ( alpha < 0.9998 || per_point_alpha )
438 V_PointCloudRenderable::iterator it =
renderables_.begin();
439 V_PointCloudRenderable::iterator end =
renderables_.end();
440 for (; it != end; ++it)
452 Ogre::Root*
root = Ogre::Root::getSingletonPtr();
460 memcpy( begin, points,
sizeof(
Point ) * num_points );
463 Ogre::RenderOperation::OperationType op_type;
466 op_type = Ogre::RenderOperation::OT_POINT_LIST;
472 op_type = Ogre::RenderOperation::OT_POINT_LIST;
476 op_type = Ogre::RenderOperation::OT_TRIANGLE_LIST;
514 Ogre::HardwareVertexBufferSharedPtr vbuf;
516 Ogre::RenderOperation* op = 0;
519 Ogre::AxisAlignedBox aabb;
521 uint32_t current_vertex_count = 0;
523 uint32_t vertex_size = 0;
524 uint32_t buffer_size = 0;
525 for (uint32_t current_point = 0; current_point < num_points; ++current_point)
530 while (!rend || current_vertex_count >= buffer_size)
534 ROS_ASSERT(current_vertex_count == buffer_size);
536 op->vertexData->vertexCount = rend->getBuffer()->getNumVertices() - op->vertexData->vertexStart;
537 ROS_ASSERT(op->vertexData->vertexCount + op->vertexData->vertexStart <= rend->getBuffer()->getNumVertices());
539 rend->setBoundingBox(aabb);
546 vbuf = rend->getBuffer();
547 vdata = vbuf->lock(Ogre::HardwareBuffer::HBL_NO_OVERWRITE);
549 op = rend->getRenderOperation();
550 op->operationType = op_type;
551 current_vertex_count = 0;
553 vertex_size = op->vertexData->vertexDeclaration->getVertexSize(0);
554 fptr = (
float*)((uint8_t*)vdata);
559 const Point& p = points[current_point];
569 c.r = ((color >> 16) & 0xff) / 255.0f;
570 c.g = ((color >> 8) & 0xff) / 255.0f;
571 c.b = (color & 0xff) / 255.0
f;
572 root->convertColourValue(c, &color);
576 root->convertColourValue( p.
color, &color );
586 for (uint32_t j = 0; j < vpp; ++j, ++current_vertex_count)
594 *fptr++ = vertices[(j*3)];
595 *fptr++ = vertices[(j*3) + 1];
596 *fptr++ = vertices[(j*3) + 2];
599 uint32_t* iptr = (uint32_t*)fptr;
603 ROS_ASSERT((uint8_t*)fptr <= (uint8_t*)vdata + rend->getBuffer()->getNumVertices() * vertex_size);
607 op->vertexData->vertexCount = current_vertex_count - op->vertexData->vertexStart;
608 rend->setBoundingBox(aabb);
610 ROS_ASSERT(op->vertexData->vertexCount + op->vertexData->vertexStart <= rend->getBuffer()->getNumVertices());
618 if (getParentSceneNode())
620 getParentSceneNode()->needUpdate();
634 uint32_t popped_count = 0;
635 while (popped_count < num_points * vpp)
638 Ogre::RenderOperation* op = rend->getRenderOperation();
640 uint32_t popped = std::min((
size_t)(num_points * vpp - popped_count), op->vertexData->vertexCount);
641 op->vertexData->vertexStart += popped;
642 op->vertexData->vertexCount -= popped;
644 popped_count += popped;
646 if (op->vertexData->vertexCount == 0)
650 op->vertexData->vertexStart = 0;
668 if (getParentSceneNode())
670 getParentSceneNode()->needUpdate();
679 Ogre::RenderOperation* op = rend->getRenderOperation();
680 if (op->vertexData->vertexCount == 0)
693 MovableObject::_notifyCurrentCamera( camera );
698 V_PointCloudRenderable::iterator it =
renderables_.begin();
699 V_PointCloudRenderable::iterator end =
renderables_.end();
700 for (; it != end; ++it)
702 queue->addRenderable((*it).get());
708 MovableObject::_notifyAttached(parent, isTagPoint);
756 V_PointCloudRenderable::iterator it =
renderables_.begin();
757 V_PointCloudRenderable::iterator end =
renderables_.end();
758 for (; it != end; ++it)
762 getUserObjectBindings().setUserAny(
"pick_handle", Ogre::Any(
colorToHandle( color )));
770 Ogre::Vector4 alpha(
alpha_, 0.0
f, 0.0
f, 0.0
f);
771 Ogre::Vector4 highlight(0.0
f, 0.0
f, 0.0
f, 0.0
f);
779 if (getParentSceneNode())
781 getParentSceneNode()->attachObject(rend.get());
788 #if (OGRE_VERSION_MAJOR >= 1 && OGRE_VERSION_MINOR >= 6) 789 void PointCloud::visitRenderables(Ogre::Renderable::Visitor* visitor,
bool debugRenderables)
804 mRenderOp.operationType = Ogre::RenderOperation::OT_POINT_LIST;
805 mRenderOp.useIndexes =
false;
806 mRenderOp.vertexData =
new Ogre::VertexData;
807 mRenderOp.vertexData->vertexStart = 0;
808 mRenderOp.vertexData->vertexCount = 0;
810 Ogre::VertexDeclaration *decl = mRenderOp.vertexData->vertexDeclaration;
813 decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_POSITION);
814 offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
818 decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_TEXTURE_COORDINATES, 0);
819 offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
822 decl->addElement(0, offset, Ogre::VET_COLOUR, Ogre::VES_DIFFUSE);
824 Ogre::HardwareVertexBufferSharedPtr vbuf =
825 Ogre::HardwareBufferManager::getSingleton().createVertexBuffer(
826 mRenderOp.vertexData->vertexDeclaration->getVertexSize(0),
828 Ogre::HardwareBuffer::HBU_DYNAMIC);
831 mRenderOp.vertexData->vertexBufferBinding->setBinding(0, vbuf);
836 delete mRenderOp.vertexData;
837 delete mRenderOp.indexData;
842 return mRenderOp.vertexData->vertexBufferBinding->getBuffer(0);
847 SimpleRenderable::_notifyCurrentCamera( camera );
852 return Ogre::Math::Sqrt(std::max(mBox.getMaximum().squaredLength(), mBox.getMinimum().squaredLength()));
857 Ogre::Vector3 vMin, vMax, vMid, vDist;
858 vMin = mBox.getMinimum();
859 vMax = mBox.getMaximum();
860 vMid = ((vMax - vMin) * 0.5) + vMin;
861 vDist = cam->getDerivedPosition() - vMid;
863 return vDist.squaredLength();
void addPoints(Point *points, uint32_t num_points)
Add points to this point cloud.
virtual const Ogre::LightList & getLights() const
Ogre::MaterialPtr current_material_
Ogre::MaterialPtr sphere_material_
std::vector< Point > V_Point
void setAlphaBlending(const Ogre::MaterialPtr &mat)
void setRenderMode(RenderMode mode)
Set what type of rendering primitives should be used, currently points, billboards and boxes are supp...
#define AUTO_SIZE_PARAMETER
uint32_t point_count_
The number of points currently in points_.
void setPickColor(const Ogre::ColourValue &color)
virtual void _updateRenderQueue(Ogre::RenderQueue *queue)
Ogre::HardwareVertexBufferSharedPtr getBuffer()
static float g_box_vertices[6 *6 *3]
void setColorByIndex(bool set)
virtual void _notifyCurrentCamera(Ogre::Camera *camera)
Ogre::AxisAlignedBox bounding_box_
The bounding box of this point cloud.
Ogre::MaterialPtr flat_square_material_
void popPoints(uint32_t num_points)
Remove a number of points from this point cloud.
V_Point points_
The list of points we're displaying. Allocates to a high-water-mark.
TFSIMD_FORCE_INLINE const tfScalar & y() const
Ogre::MaterialPtr square_material_
Ogre::Vector3 common_direction_
See Ogre::BillboardSet::setCommonDirection.
virtual void _notifyCurrentCamera(Ogre::Camera *camera)
#define VERTEX_BUFFER_CAPACITY
void setCommonUpVector(const Ogre::Vector3 &vec)
See Ogre::BillboardSet::setCommonUpVector.
bool current_mode_supports_geometry_shader_
virtual void getWorldTransforms(Ogre::Matrix4 *xform) const
Ogre::MaterialPtr tile_material_
#define HIGHLIGHT_PARAMETER
TFSIMD_FORCE_INLINE Vector3()
Representation of a point, with x/y/z position and r/g/b color.
void setDimensions(float width, float height, float depth)
Set the dimensions of the billboards used to render each point.
virtual void _notifyAttached(Ogre::Node *parent, bool isTagPoint=false)
virtual float getBoundingRadius() const
virtual Ogre::Real getSquaredViewDepth(const Ogre::Camera *cam) const
TFSIMD_FORCE_INLINE const tfScalar & x() const
void setAlpha(float alpha, bool per_point_alpha=false)
void clear()
Clear all the points.
Ogre::MaterialPtr box_material_
virtual Ogre::Real getBoundingRadius(void) const
#define PICK_COLOR_PARAMETER
float bounding_radius_
The bounding radius of this point cloud.
TFSIMD_FORCE_INLINE const tfScalar & z() const
void setReplace(const Ogre::MaterialPtr &mat)
static void removeMaterial(Ogre::MaterialPtr &material)
uint32_t colorToHandle(Ogre::PixelFormat fmt, uint32_t col)
static float g_billboard_vertices[6 *3]
Ogre::ColourValue pick_color_
uint32_t getVerticesPerPoint()
void setHighlightColor(float r, float g, float b)
A visual representation of a set of points.
V_PointCloudRenderable renderables_
PointCloudRenderable(PointCloud *parent, int num_points, bool use_tex_coords)
Ogre::MaterialPtr point_material_
void setCommonDirection(const Ogre::Vector3 &vec)
See Ogre::BillboardSet::setCommonDirection.
Ogre::Vector3 common_up_vector_
See Ogre::BillboardSet::setCommonUpVector.
static float g_point_vertices[3]
static float g_billboard_sphere_vertices[3 *3]
static Ogre::String sm_Type
The "renderable type" used by Ogre.
void setAutoSize(bool auto_size)
virtual void getWorldTransforms(Ogre::Matrix4 *xform) const
PointCloudRenderablePtr createRenderable(int num_points)
virtual const Ogre::AxisAlignedBox & getBoundingBox() const