point_cloud.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "point_cloud.h"
00031 #include <ros/assert.h>
00032 
00033 #include <OgreSceneManager.h>
00034 #include <OgreSceneNode.h>
00035 #include <OgreVector3.h>
00036 #include <OgreQuaternion.h>
00037 #include <OgreManualObject.h>
00038 #include <OgreMaterialManager.h>
00039 #include <OgreBillboardSet.h>
00040 #include <OgreBillboard.h>
00041 #include <OgreTexture.h>
00042 #include <OgreTextureManager.h>
00043 #include <OgreSharedPtr.h>
00044 #include <OgreTechnique.h>
00045 #include <OgreCamera.h>
00046 
00047 #include <sstream>
00048 
00049 #include "rviz/ogre_helpers/custom_parameter_indices.h"
00050 #include "rviz/selection/forwards.h"
00051 
00052 #define VERTEX_BUFFER_CAPACITY (36 * 1024 * 10)
00053 
00054 namespace rviz
00055 {
00056 
00057 static float g_point_vertices[3] =
00058 {
00059   0.0f, 0.0f, 0.0f
00060 };
00061 
00062 static float g_billboard_vertices[6*3] =
00063 {
00064   -0.5f, 0.5f, 0.0f,
00065   -0.5f, -0.5f, 0.0f,
00066   0.5f, 0.5f, 0.0f,
00067   0.5f, 0.5f, 0.0f,
00068   -0.5f, -0.5f, 0.0f,
00069   0.5f, -0.5f, 0.0f,
00070 };
00071 
00072 static float g_billboard_sphere_vertices[3*3] =
00073 {
00074   0.0f, 1.0f, 0.0f,
00075   -0.866025404f, -0.5f, 0.0f,
00076   0.866025404f, -0.5f, 0.0f,
00077 };
00078 
00079 static float g_box_vertices[6*6*3] =
00080 {
00081   // front
00082   -0.5f, 0.5f, -0.5f,
00083   -0.5f, -0.5f, -0.5f,
00084   0.5f, 0.5f, -0.5f,
00085   0.5f, 0.5f, -0.5f,
00086   -0.5f, -0.5f, -0.5f,
00087   0.5f, -0.5f, -0.5f,
00088 
00089   // back
00090   -0.5f, 0.5f, 0.5f,
00091   0.5f, 0.5f, 0.5f,
00092   -0.5f, -0.5f, 0.5f,
00093   0.5f, 0.5f, 0.5f,
00094   0.5f, -0.5f, 0.5f,
00095   -0.5f, -0.5f, 0.5f,
00096 
00097   // right
00098   0.5, 0.5, 0.5,
00099   0.5, 0.5, -0.5,
00100   0.5, -0.5, 0.5,
00101   0.5, 0.5, -0.5,
00102   0.5, -0.5, -0.5,
00103   0.5, -0.5, 0.5,
00104 
00105   // left
00106   -0.5, 0.5, 0.5,
00107   -0.5, -0.5, 0.5,
00108   -0.5, 0.5, -0.5,
00109   -0.5, 0.5, -0.5,
00110   -0.5, -0.5, 0.5,
00111   -0.5, -0.5, -0.5,
00112 
00113   // top
00114   -0.5, 0.5, -0.5,
00115   0.5, 0.5, -0.5,
00116   -0.5, 0.5, 0.5,
00117   0.5, 0.5, -0.5,
00118   0.5, 0.5, 0.5,
00119   -0.5, 0.5, 0.5,
00120 
00121   // bottom
00122   -0.5, -0.5, -0.5,
00123   -0.5, -0.5, 0.5,
00124   0.5, -0.5, -0.5,
00125   0.5, -0.5, -0.5,
00126   -0.5, -0.5, 0.5,
00127   0.5, -0.5, 0.5,
00128 };
00129 
00130 Ogre::String PointCloud::sm_Type = "PointCloud";
00131 
00132 PointCloud::PointCloud()
00133 : bounding_radius_( 0.0f )
00134 , point_count_( 0 )
00135 , common_direction_( Ogre::Vector3::NEGATIVE_UNIT_Z )
00136 , common_up_vector_( Ogre::Vector3::UNIT_Y )
00137 , color_by_index_(false)
00138 , current_mode_supports_geometry_shader_(false)
00139 {
00140   std::stringstream ss;
00141   static int count = 0;
00142   ss << "PointCloudMaterial" << count++;
00143   point_material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/PointCloudPoint");
00144   square_material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/PointCloudSquare");
00145   flat_square_material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/PointCloudFlatSquare");
00146   sphere_material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/PointCloudSphere");
00147   tile_material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/PointCloudTile");
00148   box_material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/PointCloudBox");
00149 
00150   point_material_ = Ogre::MaterialPtr(point_material_)->clone(ss.str() + "Point");
00151   square_material_ = Ogre::MaterialPtr(square_material_)->clone(ss.str() + "Square");
00152   flat_square_material_ = Ogre::MaterialPtr(flat_square_material_)->clone(ss.str() + "FlatSquare");
00153   sphere_material_ = Ogre::MaterialPtr(sphere_material_)->clone(ss.str() + "Sphere");
00154   tile_material_ = Ogre::MaterialPtr(tile_material_)->clone(ss.str() + "Tiles");
00155   box_material_ = Ogre::MaterialPtr(box_material_)->clone(ss.str() + "Box");
00156 
00157   point_material_->load();
00158   square_material_->load();
00159   flat_square_material_->load();
00160   sphere_material_->load();
00161   tile_material_->load();
00162   box_material_->load();
00163 
00164   setAlpha( 1.0f );
00165   setRenderMode(RM_SPHERES);
00166   setDimensions(0.01f, 0.01f, 0.01f);
00167 
00168   clear();
00169 }
00170 
00171 static void removeMaterial(Ogre::MaterialPtr& material)
00172 {
00173   Ogre::ResourcePtr resource(material);
00174   Ogre::MaterialManager::getSingleton().remove(resource);
00175 }
00176 
00177 PointCloud::~PointCloud()
00178 {
00179   clear();
00180 
00181   point_material_->unload();
00182   square_material_->unload();
00183   flat_square_material_->unload();
00184   sphere_material_->unload();
00185   tile_material_->unload();
00186   box_material_->unload();
00187 
00188   removeMaterial(point_material_);
00189   removeMaterial(square_material_);
00190   removeMaterial(flat_square_material_);
00191   removeMaterial(sphere_material_);
00192   removeMaterial(tile_material_);
00193   removeMaterial(box_material_);
00194 }
00195 
00196 const Ogre::AxisAlignedBox& PointCloud::getBoundingBox() const
00197 {
00198   return bounding_box_;
00199 }
00200 
00201 float PointCloud::getBoundingRadius() const
00202 {
00203   return bounding_radius_;
00204 }
00205 
00206 void PointCloud::getWorldTransforms(Ogre::Matrix4* xform) const
00207 {
00208   *xform = _getParentNodeFullTransform();
00209 }
00210 
00211 void PointCloud::clear()
00212 {
00213   point_count_ = 0;
00214   bounding_box_.setNull();
00215   bounding_radius_ = 0.0f;
00216 
00217   if (getParentSceneNode())
00218   {
00219     V_PointCloudRenderable::iterator it = renderables_.begin();
00220     V_PointCloudRenderable::iterator end = renderables_.end();
00221     for (; it != end; ++it)
00222     {
00223       getParentSceneNode()->detachObject(it->get());
00224     }
00225     getParentSceneNode()->needUpdate();
00226   }
00227 
00228   renderables_.clear();
00229 }
00230 
00231 void PointCloud::regenerateAll()
00232 {
00233   if (point_count_ == 0)
00234   {
00235     return;
00236   }
00237 
00238   V_Point points;
00239   points.swap(points_);
00240   uint32_t count = point_count_;
00241 
00242   clear();
00243 
00244   addPoints(&points.front(), count);
00245 }
00246 
00247 void PointCloud::setColorByIndex(bool set)
00248 {
00249   color_by_index_ = set;
00250   regenerateAll();
00251 }
00252 
00253 void PointCloud::setHighlightColor( float r, float g, float b )
00254 {
00255   Ogre::Vector4 highlight( r, g, b, 0.0f );
00256 
00257   V_PointCloudRenderable::iterator it = renderables_.begin();
00258   V_PointCloudRenderable::iterator end = renderables_.end();
00259   for (; it != end; ++it)
00260   {
00261     (*it)->setCustomParameter(HIGHLIGHT_PARAMETER, highlight);
00262   }
00263 }
00264 
00265 void PointCloud::setRenderMode(RenderMode mode)
00266 {
00267   render_mode_ = mode;
00268 
00269   if (mode == RM_POINTS)
00270   {
00271     current_material_ = Ogre::MaterialPtr(point_material_);
00272   }
00273   else if (mode == RM_SQUARES)
00274   {
00275     current_material_ = Ogre::MaterialPtr(square_material_);
00276   }
00277   else if (mode == RM_FLAT_SQUARES)
00278   {
00279     current_material_ = Ogre::MaterialPtr(flat_square_material_);
00280   }
00281   else if (mode == RM_SPHERES)
00282   {
00283     current_material_ = Ogre::MaterialPtr(sphere_material_);
00284   }
00285   else if (mode == RM_TILES)
00286   {
00287     current_material_ = Ogre::MaterialPtr(tile_material_);
00288   }
00289   else if (mode == RM_BOXES)
00290   {
00291     current_material_ = Ogre::MaterialPtr(box_material_);
00292   }
00293 
00294   current_material_->load();
00295 
00296   //ROS_INFO("Best technique [%s] [gp=%s]", current_material_->getBestTechnique()->getName().c_str(), current_material_->getBestTechnique()->getPass(0)->getGeometryProgramName().c_str());
00297 
00298   bool geom_support_changed = false;
00299   Ogre::Technique* best = current_material_->getBestTechnique();
00300   if (best)
00301   {
00302     if (current_material_->getBestTechnique()->getName() == "gp")
00303     {
00304       if (!current_mode_supports_geometry_shader_)
00305       {
00306         geom_support_changed = true;
00307       }
00308 
00309       current_mode_supports_geometry_shader_ = true;
00310 
00311       //ROS_INFO("Using geometry shader");
00312     }
00313     else
00314     {
00315       if (current_mode_supports_geometry_shader_)
00316       {
00317         geom_support_changed = true;
00318       }
00319 
00320       current_mode_supports_geometry_shader_ = false;
00321     }
00322   }
00323   else
00324   {
00325     geom_support_changed = true;
00326     current_mode_supports_geometry_shader_ = false;
00327 
00328     ROS_ERROR("No techniques available for material [%s]", current_material_->getName().c_str());
00329   }
00330 
00331   if (geom_support_changed)
00332   {
00333     renderables_.clear();
00334   }
00335 
00336   V_PointCloudRenderable::iterator it = renderables_.begin();
00337   V_PointCloudRenderable::iterator end = renderables_.end();
00338   for (; it != end; ++it)
00339   {
00340     (*it)->setMaterial(current_material_->getName());
00341   }
00342 
00343   regenerateAll();
00344 }
00345 
00346 void PointCloud::setDimensions(float width, float height, float depth)
00347 {
00348   width_ = width;
00349   height_ = height;
00350   depth_ = depth;
00351 
00352   Ogre::Vector4 size(width_, height_, depth_, 0.0f);
00353 
00354   V_PointCloudRenderable::iterator it = renderables_.begin();
00355   V_PointCloudRenderable::iterator end = renderables_.end();
00356   for (; it != end; ++it)
00357   {
00358     (*it)->setCustomParameter(SIZE_PARAMETER, size);
00359   }
00360 }
00361 
00362 void PointCloud::setAutoSize(bool auto_size)
00363 {
00364   V_PointCloudRenderable::iterator it = renderables_.begin();
00365   V_PointCloudRenderable::iterator end = renderables_.end();
00366   for (; it != end; ++it)
00367   {
00368     (*it)->setCustomParameter(AUTO_SIZE_PARAMETER, Ogre::Vector4(auto_size));
00369   }
00370 }
00371 
00372 void PointCloud::setCommonDirection(const Ogre::Vector3& vec)
00373 {
00374   common_direction_ = vec;
00375 
00376   V_PointCloudRenderable::iterator it = renderables_.begin();
00377   V_PointCloudRenderable::iterator end = renderables_.end();
00378   for (; it != end; ++it)
00379   {
00380     (*it)->setCustomParameter(NORMAL_PARAMETER, Ogre::Vector4(vec));
00381   }
00382 }
00383 
00384 void PointCloud::setCommonUpVector(const Ogre::Vector3& vec)
00385 {
00386   common_up_vector_ = vec;
00387 
00388   V_PointCloudRenderable::iterator it = renderables_.begin();
00389   V_PointCloudRenderable::iterator end = renderables_.end();
00390   for (; it != end; ++it)
00391   {
00392     (*it)->setCustomParameter(UP_PARAMETER, Ogre::Vector4(vec));
00393   }
00394 }
00395 
00396 void setAlphaBlending(const Ogre::MaterialPtr& mat)
00397 {
00398   if (mat->getBestTechnique())
00399   {
00400     mat->getBestTechnique()->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
00401     mat->getBestTechnique()->setDepthWriteEnabled( false );
00402   }
00403 }
00404 
00405 void setReplace(const Ogre::MaterialPtr& mat)
00406 {
00407   if (mat->getBestTechnique())
00408   {
00409     mat->getBestTechnique()->setSceneBlending( Ogre::SBT_REPLACE );
00410     mat->getBestTechnique()->setDepthWriteEnabled( true );
00411   }
00412 }
00413 
00414 void PointCloud::setAlpha(float alpha, bool per_point_alpha)
00415 {
00416   alpha_ = alpha;
00417 
00418   if ( alpha < 0.9998 || per_point_alpha )
00419   {
00420     setAlphaBlending(point_material_);
00421     setAlphaBlending(square_material_);
00422     setAlphaBlending(flat_square_material_);
00423     setAlphaBlending(sphere_material_);
00424     setAlphaBlending(tile_material_);
00425     setAlphaBlending(box_material_);
00426   }
00427   else
00428   {
00429     setReplace(point_material_);
00430     setReplace(square_material_);
00431     setReplace(flat_square_material_);
00432     setReplace(sphere_material_);
00433     setReplace(tile_material_);
00434     setReplace(box_material_);
00435   }
00436 
00437   Ogre::Vector4 alpha4(alpha_, alpha_, alpha_, alpha_);
00438   V_PointCloudRenderable::iterator it = renderables_.begin();
00439   V_PointCloudRenderable::iterator end = renderables_.end();
00440   for (; it != end; ++it)
00441   {
00442     (*it)->setCustomParameter(ALPHA_PARAMETER, alpha4);
00443   }
00444 }
00445 
00446 void PointCloud::addPoints(Point* points, uint32_t num_points)
00447 {
00448   if (num_points == 0)
00449   {
00450     return;
00451   }
00452   Ogre::Root* root = Ogre::Root::getSingletonPtr();
00453 
00454   if ( points_.size() < point_count_ + num_points )
00455   {
00456     points_.resize( point_count_ + num_points );
00457   }
00458 
00459   Point* begin = &points_.front() + point_count_;
00460   memcpy( begin, points, sizeof( Point ) * num_points );
00461 
00462   uint32_t vpp = getVerticesPerPoint();
00463   Ogre::RenderOperation::OperationType op_type;
00464   if (current_mode_supports_geometry_shader_)
00465   {
00466     op_type = Ogre::RenderOperation::OT_POINT_LIST;
00467   }
00468   else
00469   {
00470     if (render_mode_ == RM_POINTS)
00471     {
00472       op_type = Ogre::RenderOperation::OT_POINT_LIST;
00473     }
00474     else
00475     {
00476       op_type = Ogre::RenderOperation::OT_TRIANGLE_LIST;
00477     }
00478   }
00479 
00480   float* vertices = 0;
00481   if (current_mode_supports_geometry_shader_)
00482   {
00483     vertices = g_point_vertices;
00484   }
00485   else
00486   {
00487     if (render_mode_ == RM_POINTS)
00488     {
00489       vertices = g_point_vertices;
00490     }
00491     else if (render_mode_ == RM_SQUARES)
00492     {
00493       vertices = g_billboard_vertices;
00494     }
00495     else if (render_mode_ == RM_FLAT_SQUARES)
00496     {
00497       vertices = g_billboard_vertices;
00498     }
00499     else if (render_mode_ == RM_SPHERES)
00500     {
00501       vertices = g_billboard_sphere_vertices;
00502     }
00503     else if (render_mode_ == RM_TILES)
00504     {
00505       vertices = g_billboard_vertices;
00506     }
00507     else if (render_mode_ == RM_BOXES)
00508     {
00509       vertices = g_box_vertices;
00510     }
00511   }
00512 
00513   PointCloudRenderablePtr rend;
00514   Ogre::HardwareVertexBufferSharedPtr vbuf;
00515   void* vdata = 0;
00516   Ogre::RenderOperation* op = 0;
00517   float* fptr = 0;
00518 
00519   Ogre::AxisAlignedBox aabb;
00520   aabb.setNull();
00521   uint32_t current_vertex_count = 0;
00522   bounding_radius_ = 0.0f;
00523   uint32_t vertex_size = 0;
00524   uint32_t buffer_size = 0;
00525   for (uint32_t current_point = 0; current_point < num_points; ++current_point)
00526   {
00527     // if we didn't create a renderable yet,
00528     // or we've reached the vertex limit for the current renderable,
00529     // create a new one.
00530     while (!rend || current_vertex_count >= buffer_size)
00531     {
00532       if (rend)
00533       {
00534         ROS_ASSERT(current_vertex_count == buffer_size);
00535 
00536         op->vertexData->vertexCount = rend->getBuffer()->getNumVertices() - op->vertexData->vertexStart;
00537         ROS_ASSERT(op->vertexData->vertexCount + op->vertexData->vertexStart <= rend->getBuffer()->getNumVertices());
00538         vbuf->unlock();
00539         rend->setBoundingBox(aabb);
00540         bounding_box_.merge(aabb);
00541       }
00542 
00543       buffer_size = std::min<int>( VERTEX_BUFFER_CAPACITY, (num_points - current_point)*vpp );
00544 
00545       rend = createRenderable( buffer_size );
00546       vbuf = rend->getBuffer();
00547       vdata = vbuf->lock(Ogre::HardwareBuffer::HBL_NO_OVERWRITE);
00548 
00549       op = rend->getRenderOperation();
00550       op->operationType = op_type;
00551       current_vertex_count = 0;
00552 
00553       vertex_size = op->vertexData->vertexDeclaration->getVertexSize(0);
00554       fptr = (float*)((uint8_t*)vdata);
00555 
00556       aabb.setNull();
00557     }
00558 
00559     const Point& p = points[current_point];
00560 
00561     uint32_t color;
00562 
00563     if (color_by_index_)
00564     {
00565       // convert to ColourValue, so we can then convert to the rendersystem-specific color type
00566       color = (current_point + point_count_ + 1);
00567       Ogre::ColourValue c;
00568       c.a = 1.0f;
00569       c.r = ((color >> 16) & 0xff) / 255.0f;
00570       c.g = ((color >> 8) & 0xff) / 255.0f;
00571       c.b = (color & 0xff) / 255.0f;
00572       root->convertColourValue(c, &color);
00573     }
00574     else
00575     {
00576       root->convertColourValue( p.color, &color );
00577     }
00578 
00579     aabb.merge(p.position);
00580     bounding_radius_ = std::max( bounding_radius_, p.position.squaredLength() );
00581 
00582     float x = p.position.x;
00583     float y = p.position.y;
00584     float z = p.position.z;
00585 
00586     for (uint32_t j = 0; j < vpp; ++j, ++current_vertex_count)
00587     {
00588       *fptr++ = x;
00589       *fptr++ = y;
00590       *fptr++ = z;
00591 
00592       if (!current_mode_supports_geometry_shader_)
00593       {
00594         *fptr++ = vertices[(j*3)];
00595         *fptr++ = vertices[(j*3) + 1];
00596         *fptr++ = vertices[(j*3) + 2];
00597       }
00598 
00599       uint32_t* iptr = (uint32_t*)fptr;
00600       *iptr = color;
00601       ++fptr;
00602 
00603       ROS_ASSERT((uint8_t*)fptr <= (uint8_t*)vdata + rend->getBuffer()->getNumVertices() * vertex_size);
00604     }
00605   }
00606 
00607   op->vertexData->vertexCount = current_vertex_count - op->vertexData->vertexStart;
00608   rend->setBoundingBox(aabb);
00609   bounding_box_.merge(aabb);
00610   ROS_ASSERT(op->vertexData->vertexCount + op->vertexData->vertexStart <= rend->getBuffer()->getNumVertices());
00611 
00612   vbuf->unlock();
00613 
00614   point_count_ += num_points;
00615 
00616   shrinkRenderables();
00617 
00618   if (getParentSceneNode())
00619   {
00620     getParentSceneNode()->needUpdate();
00621   }
00622 }
00623 
00624 void PointCloud::popPoints(uint32_t num_points)
00625 {
00626   uint32_t vpp = getVerticesPerPoint();
00627 
00628   ROS_ASSERT(num_points <= point_count_);
00629   points_.erase(points_.begin(), points_.begin() + num_points);
00630 
00631   point_count_ -= num_points;
00632 
00633   // Now clear out popped points
00634   uint32_t popped_count = 0;
00635   while (popped_count < num_points * vpp)
00636   {
00637     PointCloudRenderablePtr rend = renderables_.front();
00638     Ogre::RenderOperation* op = rend->getRenderOperation();
00639 
00640     uint32_t popped = std::min((size_t)(num_points * vpp - popped_count), op->vertexData->vertexCount);
00641     op->vertexData->vertexStart += popped;
00642     op->vertexData->vertexCount -= popped;
00643 
00644     popped_count += popped;
00645 
00646     if (op->vertexData->vertexCount == 0)
00647     {
00648       renderables_.erase(renderables_.begin(), renderables_.begin() + 1);
00649 
00650       op->vertexData->vertexStart = 0;
00651       renderables_.push_back(rend);
00652     }
00653   }
00654   ROS_ASSERT(popped_count == num_points * vpp);
00655 
00656   // reset bounds
00657   bounding_box_.setNull();
00658   bounding_radius_ = 0.0f;
00659   for (uint32_t i = 0; i < point_count_; ++i)
00660   {
00661     Point& p = points_[i];
00662     bounding_box_.merge(p.position);
00663     bounding_radius_ = std::max(bounding_radius_, p.position.squaredLength());
00664   }
00665 
00666   shrinkRenderables();
00667 
00668   if (getParentSceneNode())
00669   {
00670     getParentSceneNode()->needUpdate();
00671   }
00672 }
00673 
00674 void PointCloud::shrinkRenderables()
00675 {
00676   while (!renderables_.empty())
00677   {
00678     PointCloudRenderablePtr rend = renderables_.back();
00679     Ogre::RenderOperation* op = rend->getRenderOperation();
00680     if (op->vertexData->vertexCount == 0)
00681     {
00682       renderables_.pop_back();
00683     }
00684     else
00685     {
00686       break;
00687     }
00688   }
00689 }
00690 
00691 void PointCloud::_notifyCurrentCamera(Ogre::Camera* camera)
00692 {
00693   MovableObject::_notifyCurrentCamera( camera );
00694 }
00695 
00696 void PointCloud::_updateRenderQueue(Ogre::RenderQueue* queue)
00697 {
00698   V_PointCloudRenderable::iterator it = renderables_.begin();
00699   V_PointCloudRenderable::iterator end = renderables_.end();
00700   for (; it != end; ++it)
00701   {
00702     queue->addRenderable((*it).get());
00703   }
00704 }
00705 
00706 void PointCloud::_notifyAttached(Ogre::Node *parent, bool isTagPoint)
00707 {
00708   MovableObject::_notifyAttached(parent, isTagPoint);
00709 }
00710 
00711 uint32_t PointCloud::getVerticesPerPoint()
00712 {
00713   if (current_mode_supports_geometry_shader_)
00714   {
00715     return 1;
00716   }
00717 
00718   if (render_mode_ == RM_POINTS)
00719   {
00720     return 1;
00721   }
00722 
00723   if (render_mode_ == RM_SQUARES)
00724   {
00725     return 6;
00726   }
00727 
00728   if (render_mode_ == RM_FLAT_SQUARES)
00729   {
00730     return 6;
00731   }
00732 
00733   if (render_mode_ == RM_TILES)
00734     {
00735       return 6;
00736     }
00737 
00738   if (render_mode_ == RM_SPHERES)
00739   {
00740     return 3;
00741   }
00742 
00743   if (render_mode_ == RM_BOXES)
00744   {
00745     return 36;
00746   }
00747 
00748   return 1;
00749 }
00750 
00751 void PointCloud::setPickColor(const Ogre::ColourValue& color)
00752 {
00753   pick_color_ = color;
00754   Ogre::Vector4 pick_col(pick_color_.r, pick_color_.g, pick_color_.b, pick_color_.a);
00755 
00756   V_PointCloudRenderable::iterator it = renderables_.begin();
00757   V_PointCloudRenderable::iterator end = renderables_.end();
00758   for (; it != end; ++it)
00759   {
00760     (*it)->setCustomParameter(PICK_COLOR_PARAMETER, pick_col);
00761   }
00762   getUserObjectBindings().setUserAny( "pick_handle", Ogre::Any( colorToHandle( color )));
00763 }
00764 
00765 PointCloudRenderablePtr PointCloud::createRenderable( int num_points )
00766 {
00767   PointCloudRenderablePtr rend(new PointCloudRenderable(this, num_points, !current_mode_supports_geometry_shader_));
00768   rend->setMaterial(current_material_->getName());
00769   Ogre::Vector4 size(width_, height_, depth_, 0.0f);
00770   Ogre::Vector4 alpha(alpha_, 0.0f, 0.0f, 0.0f);
00771   Ogre::Vector4 highlight(0.0f, 0.0f, 0.0f, 0.0f);
00772   Ogre::Vector4 pick_col(pick_color_.r, pick_color_.g, pick_color_.b, pick_color_.a);
00773   rend->setCustomParameter(SIZE_PARAMETER, size);
00774   rend->setCustomParameter(ALPHA_PARAMETER, alpha);
00775   rend->setCustomParameter(HIGHLIGHT_PARAMETER, highlight);
00776   rend->setCustomParameter(PICK_COLOR_PARAMETER, pick_col);
00777   rend->setCustomParameter(NORMAL_PARAMETER, Ogre::Vector4(common_direction_));
00778   rend->setCustomParameter(UP_PARAMETER, Ogre::Vector4(common_up_vector_));
00779   if (getParentSceneNode())
00780   {
00781     getParentSceneNode()->attachObject(rend.get());
00782   }
00783   renderables_.push_back(rend);
00784 
00785   return rend;
00786 }
00787 
00788 #if (OGRE_VERSION_MAJOR >= 1 && OGRE_VERSION_MINOR >= 6)
00789 void PointCloud::visitRenderables(Ogre::Renderable::Visitor* visitor, bool debugRenderables)
00790 {
00791 
00792 }
00793 #endif
00794 
00795 
00799 
00800 PointCloudRenderable::PointCloudRenderable(PointCloud* parent, int num_points, bool use_tex_coords)
00801 : parent_(parent)
00802 {
00803   // Initialize render operation
00804   mRenderOp.operationType = Ogre::RenderOperation::OT_POINT_LIST;
00805   mRenderOp.useIndexes = false;
00806   mRenderOp.vertexData = new Ogre::VertexData;
00807   mRenderOp.vertexData->vertexStart = 0;
00808   mRenderOp.vertexData->vertexCount = 0;
00809 
00810   Ogre::VertexDeclaration *decl = mRenderOp.vertexData->vertexDeclaration;
00811   size_t offset = 0;
00812 
00813   decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_POSITION);
00814   offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
00815 
00816   if (use_tex_coords)
00817   {
00818     decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_TEXTURE_COORDINATES, 0);
00819     offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
00820   }
00821 
00822   decl->addElement(0, offset, Ogre::VET_COLOUR, Ogre::VES_DIFFUSE);
00823 
00824   Ogre::HardwareVertexBufferSharedPtr vbuf =
00825     Ogre::HardwareBufferManager::getSingleton().createVertexBuffer(
00826       mRenderOp.vertexData->vertexDeclaration->getVertexSize(0),
00827       num_points,
00828       Ogre::HardwareBuffer::HBU_DYNAMIC);
00829 
00830   // Bind buffer
00831   mRenderOp.vertexData->vertexBufferBinding->setBinding(0, vbuf);
00832 }
00833 
00834 PointCloudRenderable::~PointCloudRenderable()
00835 {
00836   delete mRenderOp.vertexData;
00837   delete mRenderOp.indexData;
00838 }
00839 
00840 Ogre::HardwareVertexBufferSharedPtr PointCloudRenderable::getBuffer()
00841 {
00842   return mRenderOp.vertexData->vertexBufferBinding->getBuffer(0);
00843 }
00844 
00845 void PointCloudRenderable::_notifyCurrentCamera(Ogre::Camera* camera)
00846 {
00847   SimpleRenderable::_notifyCurrentCamera( camera );
00848 }
00849 
00850 Ogre::Real PointCloudRenderable::getBoundingRadius(void) const
00851 {
00852   return Ogre::Math::Sqrt(std::max(mBox.getMaximum().squaredLength(), mBox.getMinimum().squaredLength()));
00853 }
00854 
00855 Ogre::Real PointCloudRenderable::getSquaredViewDepth(const Ogre::Camera* cam) const
00856 {
00857   Ogre::Vector3 vMin, vMax, vMid, vDist;
00858    vMin = mBox.getMinimum();
00859    vMax = mBox.getMaximum();
00860    vMid = ((vMax - vMin) * 0.5) + vMin;
00861    vDist = cam->getDerivedPosition() - vMid;
00862 
00863    return vDist.squaredLength();
00864 }
00865 
00866 void PointCloudRenderable::getWorldTransforms(Ogre::Matrix4* xform) const
00867 {
00868    parent_->getWorldTransforms(xform);
00869 }
00870 
00871 const Ogre::LightList& PointCloudRenderable::getLights() const
00872 {
00873   return parent_->queryLights();
00874 }
00875 
00876 } // namespace rviz


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Jun 6 2019 18:02:15