rviz_interaction_tools: rviz_interaction_tools: point

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 "rviz_interaction_tools/point_cloud.h"
00031 #include <ros/assert.h>
00032 
00033 #include <OGRE/OgreSceneManager.h>
00034 #include <OGRE/OgreSceneNode.h>
00035 #include <OGRE/OgreVector3.h>
00036 #include <OGRE/OgreQuaternion.h>
00037 #include <OGRE/OgreManualObject.h>
00038 #include <OGRE/OgreMaterialManager.h>
00039 #include <OGRE/OgreBillboardSet.h>
00040 #include <OGRE/OgreBillboard.h>
00041 #include <OGRE/OgreTexture.h>
00042 #include <OGRE/OgreTextureManager.h>
00043 
00044 #include <sstream>
00045 
00046 #define VERTEX_BUFFER_CAPACITY (36 * 1024 * 10)
00047 
00048 namespace rviz_interaction_tools
00049 {
00050 
00051 static float g_point_vertices[3] =
00052 {
00053   0.0f, 0.0f, 0.0f
00054 };
00055 
00056 Ogre::String PointCloud::sm_Type = "RvizInteractionToolsPointCloud";
00057 
00058 PointCloud::PointCloud()
00059 : bounding_radius_( 0.0f )
00060 , point_count_( 0 )
00061 , current_mode_supports_geometry_shader_(false)
00062 {
00063   setMaterial( "BaseWhiteNoLighting" );
00064   clear();
00065 }
00066 
00067 PointCloud::~PointCloud()
00068 {
00069 }
00070 
00071 const Ogre::AxisAlignedBox& PointCloud::getBoundingBox() const
00072 {
00073   return bounding_box_;
00074 }
00075 
00076 float PointCloud::getBoundingRadius() const
00077 {
00078   return bounding_radius_;
00079 }
00080 
00081 void PointCloud::getWorldTransforms(Ogre::Matrix4* xform) const
00082 {
00083   *xform = _getParentNodeFullTransform();
00084 }
00085 
00086 void PointCloud::clear()
00087 {
00088   point_count_ = 0;
00089   bounding_box_.setNull();
00090   bounding_radius_ = 0.0f;
00091 
00092   V_PointCloudRenderable::iterator it = renderables_.begin();
00093   V_PointCloudRenderable::iterator end = renderables_.end();
00094   for (; it != end; ++it)
00095   {
00096     (*it)->getRenderOperation()->vertexData->vertexStart = 0;
00097     (*it)->getRenderOperation()->vertexData->vertexCount = 0;
00098   }
00099 
00100   if (getParentSceneNode())
00101   {
00102     getParentSceneNode()->needUpdate();
00103   }
00104 }
00105 
00106 void PointCloud::regenerateAll()
00107 {
00108   if (point_count_ == 0)
00109   {
00110     return;
00111   }
00112 
00113   V_Point points;
00114   points.swap(points_);
00115   uint32_t count = point_count_;
00116 
00117   clear();
00118 
00119   addPoints(&points.front(), count);
00120 }
00121 
00122 void PointCloud::setMaterial( const Ogre::String& material_name )
00123 {
00124   current_material_ = Ogre::MaterialManager::getSingleton().getByName( material_name );
00125 
00126   bool geom_support_changed = false;
00127   Ogre::Technique* best = current_material_->getBestTechnique();
00128   if (best)
00129   {
00130     if (current_material_->getBestTechnique()->getName() == "gp")
00131     {
00132       if (!current_mode_supports_geometry_shader_)
00133       {
00134         geom_support_changed = true;
00135       }
00136 
00137       current_mode_supports_geometry_shader_ = true;
00138 
00139       //ROS_INFO("Using geometry shader");
00140     }
00141     else
00142     {
00143       if (current_mode_supports_geometry_shader_)
00144       {
00145         geom_support_changed = true;
00146       }
00147 
00148       current_mode_supports_geometry_shader_ = false;
00149     }
00150   }
00151   else
00152   {
00153     geom_support_changed = true;
00154     current_mode_supports_geometry_shader_ = false;
00155 
00156     ROS_ERROR("No techniques available for material [%s]", current_material_->getName().c_str());
00157   }
00158 
00159   if (geom_support_changed)
00160   {
00161     renderables_.clear();
00162   }
00163 
00164   V_PointCloudRenderable::iterator it = renderables_.begin();
00165   V_PointCloudRenderable::iterator end = renderables_.end();
00166   for (; it != end; ++it)
00167   {
00168     (*it)->setMaterial(current_material_->getName());
00169   }
00170 
00171   regenerateAll();
00172 }
00173 
00174 void PointCloud::setRenderQueueGroup( Ogre::uint8 queueID )
00175 {
00176   Ogre::MovableObject::setRenderQueueGroup( queueID );
00177   V_PointCloudRenderable::iterator it = renderables_.begin();
00178   V_PointCloudRenderable::iterator end = renderables_.end();
00179   for (; it != end; ++it)
00180   {
00181     (*it)->setRenderQueueGroup( queueID );
00182     ROS_INFO( "Setting render queue group %d", queueID );
00183   }
00184 }
00185 
00186 void PointCloud::addPoints(Point* points, uint32_t num_points)
00187 {
00188   if (num_points == 0)
00189   {
00190     return;
00191   }
00192 
00193   if ( points_.size() < point_count_ + num_points )
00194   {
00195     points_.resize( point_count_ + num_points );
00196   }
00197 
00198   Point* begin = &points_.front() + point_count_;
00199   memcpy( begin, points, sizeof( Point ) * num_points );
00200 
00201   uint32_t vpp = getVerticesPerPoint();
00202   Ogre::RenderOperation::OperationType op_type = Ogre::RenderOperation::OT_POINT_LIST;
00203 
00204   float* vertices = 0;
00205   if (current_mode_supports_geometry_shader_)
00206   {
00207     vertices = g_point_vertices;
00208   }
00209   else
00210   {
00211     vertices = g_point_vertices;
00212   }
00213 
00214   PointCloudRenderablePtr rend;
00215   Ogre::HardwareVertexBufferSharedPtr vbuf;
00216   void* vdata = 0;
00217   Ogre::RenderOperation* op = 0;
00218   float* fptr = 0;
00219 
00220   Ogre::AxisAlignedBox aabb;
00221   aabb.setNull();
00222   uint32_t current_vertex_count = 0;
00223   bounding_radius_ = 0.0f;
00224   uint32_t vertex_size = 0;
00225   for (uint32_t current_point = 0; current_point < num_points; ++current_point)
00226   {
00227     while (current_vertex_count >= VERTEX_BUFFER_CAPACITY || !rend)
00228     {
00229       if (rend)
00230       {
00231         ROS_ASSERT(current_vertex_count == VERTEX_BUFFER_CAPACITY);
00232 
00233         op->vertexData->vertexCount = VERTEX_BUFFER_CAPACITY - op->vertexData->vertexStart;
00234         ROS_ASSERT(op->vertexData->vertexCount + op->vertexData->vertexStart <= VERTEX_BUFFER_CAPACITY);
00235         vbuf->unlock();
00236         rend->setBoundingBox(aabb);
00237       }
00238 
00239       rend = getOrCreateRenderable();
00240       vbuf = rend->getBuffer();
00241       vdata = vbuf->lock(Ogre::HardwareBuffer::HBL_NO_OVERWRITE);
00242 
00243       op = rend->getRenderOperation();
00244       op->operationType = op_type;
00245       current_vertex_count = op->vertexData->vertexStart + op->vertexData->vertexCount;
00246 
00247       vertex_size = op->vertexData->vertexDeclaration->getVertexSize(0);
00248       uint32_t data_pos = current_vertex_count * vertex_size;
00249       fptr = (float*)((uint8_t*)vdata + data_pos);
00250 
00251       aabb.setNull();
00252     }
00253 
00254     const Point& p = points[current_point];
00255     float x = p.x;
00256     float y = p.y;
00257     float z = p.z;
00258     uint32_t color = p.color;
00259 
00260     Ogre::Vector3 pos(x, y, z);
00261     aabb.merge(pos);
00262     bounding_box_.merge( pos );
00263     bounding_radius_ = std::max( bounding_radius_, pos.squaredLength() );
00264 
00265     for (uint32_t j = 0; j < vpp; ++j, ++current_vertex_count)
00266     {
00267       *fptr++ = x;
00268       *fptr++ = y;
00269       *fptr++ = z;
00270 
00271       if (!current_mode_supports_geometry_shader_)
00272       {
00273         *fptr++ = vertices[(j*3)];
00274         *fptr++ = vertices[(j*3) + 1];
00275         *fptr++ = vertices[(j*3) + 2];
00276       }
00277 
00278       uint32_t* iptr = (uint32_t*)fptr;
00279       *iptr = color;
00280       ++fptr;
00281 
00282       ROS_ASSERT((uint8_t*)fptr <= (uint8_t*)vdata + VERTEX_BUFFER_CAPACITY * vertex_size);
00283     }
00284   }
00285 
00286   op->vertexData->vertexCount = current_vertex_count - op->vertexData->vertexStart;
00287   rend->setBoundingBox(aabb);
00288   ROS_ASSERT(op->vertexData->vertexCount + op->vertexData->vertexStart <= VERTEX_BUFFER_CAPACITY);
00289 
00290   vbuf->unlock();
00291 
00292   point_count_ += num_points;
00293 
00294   shrinkRenderables();
00295 
00296   if (getParentSceneNode())
00297   {
00298     getParentSceneNode()->needUpdate();
00299   }
00300 }
00301 
00302 void PointCloud::shrinkRenderables()
00303 {
00304   while (!renderables_.empty())
00305   {
00306     PointCloudRenderablePtr rend = renderables_.back();
00307     Ogre::RenderOperation* op = rend->getRenderOperation();
00308     if (op->vertexData->vertexCount == 0)
00309     {
00310       renderables_.pop_back();
00311     }
00312     else
00313     {
00314       break;
00315     }
00316   }
00317 }
00318 
00319 void PointCloud::_notifyCurrentCamera(Ogre::Camera* camera)
00320 {
00321   MovableObject::_notifyCurrentCamera( camera );
00322 }
00323 
00324 void PointCloud::_updateRenderQueue(Ogre::RenderQueue* queue)
00325 {
00326   V_PointCloudRenderable::iterator it = renderables_.begin();
00327   V_PointCloudRenderable::iterator end = renderables_.end();
00328   for (; it != end; ++it)
00329   {
00330     queue->addRenderable((*it).get());
00331   }
00332 }
00333 
00334 void PointCloud::_notifyAttached(Ogre::Node *parent, bool isTagPoint)
00335 {
00336   MovableObject::_notifyAttached(parent, isTagPoint);
00337 }
00338 
00339 uint32_t PointCloud::getVerticesPerPoint()
00340 {
00341   return 1;
00342 }
00343 
00344 PointCloudRenderablePtr PointCloud::getOrCreateRenderable()
00345 {
00346   V_PointCloudRenderable::iterator it = renderables_.begin();
00347   V_PointCloudRenderable::iterator end = renderables_.end();
00348   for (; it != end; ++it)
00349   {
00350     const PointCloudRenderablePtr& rend = *it;
00351     Ogre::RenderOperation* op = rend->getRenderOperation();
00352     if (op->vertexData->vertexCount + op->vertexData->vertexStart < VERTEX_BUFFER_CAPACITY)
00353     {
00354       return rend;
00355     }
00356   }
00357 
00358   PointCloudRenderablePtr rend(new PointCloudRenderable(this, !current_mode_supports_geometry_shader_));
00359   rend->setMaterial(current_material_->getName());
00360   rend->setRenderQueueGroup( getRenderQueueGroup() );
00361   ROS_INFO( "Setting render queue group %d", getRenderQueueGroup() );
00362 
00363   if (getParentSceneNode())
00364   {
00365     getParentSceneNode()->attachObject(rend.get());
00366   }
00367   renderables_.push_back(rend);
00368 
00369   return rend;
00370 }
00371 
00372 #if (OGRE_VERSION_MAJOR >= 1 && OGRE_VERSION_MINOR >= 6)
00373 void PointCloud::visitRenderables(Ogre::Renderable::Visitor* visitor, bool debugRenderables)
00374 {
00375   V_PointCloudRenderable::iterator it = renderables_.begin();
00376   V_PointCloudRenderable::iterator end = renderables_.end();
00377   for (; it != end; ++it)
00378   {
00379     visitor->visit( it->get(), 0, debugRenderables, 0 );
00380   }
00381 }
00382 #endif
00383 
00384 
00388 
00389 PointCloudRenderable::PointCloudRenderable(PointCloud* parent, bool use_tex_coords)
00390 : parent_(parent)
00391 {
00392   // Initialize render operation
00393   mRenderOp.operationType = Ogre::RenderOperation::OT_POINT_LIST;
00394   mRenderOp.useIndexes = false;
00395   mRenderOp.vertexData = new Ogre::VertexData;
00396   mRenderOp.vertexData->vertexStart = 0;
00397   mRenderOp.vertexData->vertexCount = 0;
00398 
00399   Ogre::VertexDeclaration *decl = mRenderOp.vertexData->vertexDeclaration;
00400   size_t offset = 0;
00401 
00402   decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_POSITION);
00403   offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
00404 
00405   if (use_tex_coords)
00406   {
00407     decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_TEXTURE_COORDINATES, 0);
00408     offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
00409   }
00410 
00411   decl->addElement(0, offset, Ogre::VET_COLOUR, Ogre::VES_DIFFUSE);
00412 
00413   Ogre::HardwareVertexBufferSharedPtr vbuf =
00414     Ogre::HardwareBufferManager::getSingleton().createVertexBuffer(
00415       mRenderOp.vertexData->vertexDeclaration->getVertexSize(0),
00416       VERTEX_BUFFER_CAPACITY,
00417       Ogre::HardwareBuffer::HBU_DYNAMIC);
00418 
00419   // Bind buffer
00420   mRenderOp.vertexData->vertexBufferBinding->setBinding(0, vbuf);
00421 }
00422 
00423 PointCloudRenderable::~PointCloudRenderable()
00424 {
00425   delete mRenderOp.vertexData;
00426   delete mRenderOp.indexData;
00427 }
00428 
00429 Ogre::HardwareVertexBufferSharedPtr PointCloudRenderable::getBuffer()
00430 {
00431   return mRenderOp.vertexData->vertexBufferBinding->getBuffer(0);
00432 }
00433 
00434 void PointCloudRenderable::_notifyCurrentCamera(Ogre::Camera* camera)
00435 {
00436   SimpleRenderable::_notifyCurrentCamera( camera );
00437 }
00438 
00439 Ogre::Real PointCloudRenderable::getBoundingRadius(void) const
00440 {
00441   return Ogre::Math::Sqrt(std::max(mBox.getMaximum().squaredLength(), mBox.getMinimum().squaredLength()));
00442 }
00443 
00444 Ogre::Real PointCloudRenderable::getSquaredViewDepth(const Ogre::Camera* cam) const
00445 {
00446   Ogre::Vector3 vMin, vMax, vMid, vDist;
00447    vMin = mBox.getMinimum();
00448    vMax = mBox.getMaximum();
00449    vMid = ((vMax - vMin) * 0.5) + vMin;
00450    vDist = cam->getDerivedPosition() - vMid;
00451 
00452    return vDist.squaredLength();
00453 }
00454 
00455 void PointCloudRenderable::getWorldTransforms(Ogre::Matrix4* xform) const
00456 {
00457    *xform = m_matWorldTransform * parent_->getParentNode()->_getFullTransform();
00458 }
00459 
00460 const Ogre::LightList& PointCloudRenderable::getLights() const
00461 {
00462   return parent_->queryLights();
00463 }
00464 
00465 void PointCloudRenderable::_updateRenderQueue(Ogre::RenderQueue* queue)
00466 {
00467     queue->addRenderable( this, mRenderQueueID, OGRE_RENDERABLE_DEFAULT_PRIORITY);
00468 }
00469 
00470 } // namespace ogre_tools
point_cloud.cpp
