points_renderer.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010, 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 <rve_render_server/batching/points_renderer.h>
00031 #include <rve_render_server/init.h>
00032 #include <rve_render_server/renderer.h>
00033 #include <rve_render_server/batching/points_material_generator.h>
00034 #include <rve_render_server/convert.h>
00035 
00036 #include <rve_msgs/Points.h>
00037 
00038 #include <OGRE/OgreRoot.h>
00039 #include <OGRE/OgreSceneNode.h>
00040 #include <OGRE/OgreHardwareBufferManager.h>
00041 #include <OGRE/OgreHardwareVertexBuffer.h>
00042 
00043 #define POINTS_PER_VBO (1024 * 8)
00044 
00045 namespace rve_render_server
00046 {
00047 
00048 static float g_point_vertices[3] =
00049 {
00050   0.0f, 0.0f, 0.0f
00051 };
00052 
00053 static float g_billboard_vertices[6*3] =
00054 {
00055   -0.5f, 0.5f, 0.0f,
00056   -0.5f, -0.5f, 0.0f,
00057   0.5f, 0.5f, 0.0f,
00058   0.5f, 0.5f, 0.0f,
00059   -0.5f, -0.5f, 0.0f,
00060   0.5f, -0.5f, 0.0f,
00061 };
00062 
00063 #if 01
00064 static float* g_billboard_sphere_vertices = g_billboard_vertices;
00065 #else
00066 static float g_billboard_sphere_vertices[3*3] =
00067 {
00068   0.0f, 1.5f, 0.0f,
00069   -1.5f, -1.5f, 0.0f,
00070   1.5f, -1.5f, 0.0f,
00071 };
00072 #endif
00073 
00074 static float g_box_vertices[6*6*3] =
00075 {
00076   // front
00077   -0.5f, 0.5f, -0.5f,
00078   -0.5f, -0.5f, -0.5f,
00079   0.5f, 0.5f, -0.5f,
00080   0.5f, 0.5f, -0.5f,
00081   -0.5f, -0.5f, -0.5f,
00082   0.5f, -0.5f, -0.5f,
00083 
00084   // back
00085   -0.5f, 0.5f, 0.5f,
00086   0.5f, 0.5f, 0.5f,
00087   -0.5f, -0.5f, 0.5f,
00088   0.5f, 0.5f, 0.5f,
00089   0.5f, -0.5f, 0.5f,
00090   -0.5f, -0.5f, 0.5f,
00091 
00092   // right
00093   0.5, 0.5, 0.5,
00094   0.5, 0.5, -0.5,
00095   0.5, -0.5, 0.5,
00096   0.5, 0.5, -0.5,
00097   0.5, -0.5, -0.5,
00098   0.5, -0.5, 0.5,
00099 
00100   // left
00101   -0.5, 0.5, 0.5,
00102   -0.5, -0.5, 0.5,
00103   -0.5, 0.5, -0.5,
00104   -0.5, 0.5, -0.5,
00105   -0.5, -0.5, 0.5,
00106   -0.5, -0.5, -0.5,
00107 
00108   // top
00109   -0.5, 0.5, -0.5,
00110   0.5, 0.5, -0.5,
00111   -0.5, 0.5, 0.5,
00112   0.5, 0.5, -0.5,
00113   0.5, 0.5, 0.5,
00114   -0.5, 0.5, 0.5,
00115 
00116   // bottom
00117   -0.5, -0.5, -0.5,
00118   -0.5, -0.5, 0.5,
00119   0.5, -0.5, -0.5,
00120   0.5, -0.5, -0.5,
00121   -0.5, -0.5, 0.5,
00122   0.5, -0.5, 0.5,
00123 };
00124 
00125 static float g_box_normals[6*6*3] =
00126 {
00127   // front
00128   0.0, 0.0, 1.0,
00129   0.0, 0.0, 1.0,
00130   0.0, 0.0, 1.0,
00131   0.0, 0.0, 1.0,
00132   0.0, 0.0, 1.0,
00133   0.0, 0.0, 1.0,
00134 
00135   // back
00136   0.0, 0.0, -1.0,
00137   0.0, 0.0, -1.0,
00138   0.0, 0.0, -1.0,
00139   0.0, 0.0, -1.0,
00140   0.0, 0.0, -1.0,
00141   0.0, 0.0, -1.0,
00142 
00143   // right
00144   1.0, 0.0, 0.0,
00145   1.0, 0.0, 0.0,
00146   1.0, 0.0, 0.0,
00147   1.0, 0.0, 0.0,
00148   1.0, 0.0, 0.0,
00149   1.0, 0.0, 0.0,
00150 
00151   // left
00152   -1.0, 0.0, 0.0,
00153   -1.0, 0.0, 0.0,
00154   -1.0, 0.0, 0.0,
00155   -1.0, 0.0, 0.0,
00156   -1.0, 0.0, 0.0,
00157   -1.0, 0.0, 0.0,
00158 
00159   // top
00160   0.0, 1.0, 0.0,
00161   0.0, 1.0, 0.0,
00162   0.0, 1.0, 0.0,
00163   0.0, 1.0, 0.0,
00164   0.0, 1.0, 0.0,
00165   0.0, 1.0, 0.0,
00166 
00167   // bottom
00168   0.0, -1.0, 0.0,
00169   0.0, -1.0, 0.0,
00170   0.0, -1.0, 0.0,
00171   0.0, -1.0, 0.0,
00172   0.0, -1.0, 0.0,
00173   0.0, -1.0, 0.0,
00174 };
00175 
00176 PointsRenderable::PointsRenderable(const PointsRendererDesc& desc, bool alpha)
00177 : Parent(desc, alpha)
00178 {
00179   Ogre::Vector4 size(desc_.scale.x, desc_.scale.y, desc_.scale.z, 0.0f);
00180   setCustomParameter(PointsRendererDesc::CustomParam_Size, size);
00181   setCustomParameter(PointsRendererDesc::CustomParam_RenderableID, intToFloat4<Ogre::Vector4>(pick_id_));
00182 
00183   supports_geometry_programs_ = getRenderer()->useGeometryShaders();
00184   needs_offsets_ = !supports_geometry_programs_ && desc.type != rve_msgs::Points::TYPE_POINTS;
00185   needs_normals_ = !supports_geometry_programs_ && desc.type == rve_msgs::Points::TYPE_BOXES;
00186 
00187   if (supports_geometry_programs_)
00188   {
00189     mRenderOp.operationType = Ogre::RenderOperation::OT_POINT_LIST;
00190   }
00191   else
00192   {
00193     switch (desc.type)
00194     {
00195     case rve_msgs::Points::TYPE_POINTS:
00196       mRenderOp.operationType = Ogre::RenderOperation::OT_POINT_LIST;
00197       break;
00198     default:
00199       mRenderOp.operationType = Ogre::RenderOperation::OT_TRIANGLE_LIST;
00200     }
00201   }
00202   mRenderOp.useIndexes = false;
00203   mRenderOp.vertexData = new Ogre::VertexData;
00204   mRenderOp.vertexData->vertexStart = 0;
00205   mRenderOp.vertexData->vertexCount = 0;
00206 
00207   Ogre::VertexDeclaration *decl = mRenderOp.vertexData->vertexDeclaration;
00208   size_t offset = 0;
00209 
00210   decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_POSITION);
00211   offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
00212 
00213   if (needs_normals_)
00214   {
00215     decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_NORMAL, 0);
00216     offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
00217   }
00218 
00219   uint32_t tex_coord_num = 0;
00220   if (needs_offsets_)
00221   {
00222     decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_TEXTURE_COORDINATES, tex_coord_num++);
00223     offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
00224   }
00225 
00226   if (desc.has_normals)
00227   {
00228     // We use a texture coordinate set here because boxes need another per-vertex normal
00229     decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_TEXTURE_COORDINATES, tex_coord_num++);
00230     offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
00231   }
00232 
00233 #if 01
00234   if (desc.has_orientations)
00235   {
00236     decl->addElement(0, offset, Ogre::VET_FLOAT4, Ogre::VES_TEXTURE_COORDINATES, tex_coord_num++);
00237     offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT4);
00238   }
00239 #endif
00240 
00241   if (desc.has_scales)
00242   {
00243     decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_TEXTURE_COORDINATES, tex_coord_num++);
00244     offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
00245   }
00246 
00247   decl->addElement(0, offset, Ogre::VET_COLOUR, Ogre::VES_DIFFUSE);
00248   offset += Ogre::VertexElement::getTypeSize(Ogre::VET_COLOUR);
00249 
00250   // Extra picking color
00251   decl->addElement(0, offset, Ogre::VET_COLOUR, Ogre::VES_SPECULAR);
00252   offset += Ogre::VertexElement::getTypeSize(Ogre::VET_COLOUR);
00253 
00254   Ogre::HardwareVertexBufferSharedPtr vbuf =
00255     Ogre::HardwareBufferManager::getSingleton().createVertexBuffer(
00256       mRenderOp.vertexData->vertexDeclaration->getVertexSize(0),
00257           POINTS_PER_VBO * getVerticesPerPoint(),
00258       Ogre::HardwareBuffer::HBU_DYNAMIC_WRITE_ONLY);
00259 
00260   // Bind buffer
00261   mRenderOp.vertexData->vertexBufferBinding->setBinding(0, vbuf);
00262 }
00263 
00264 PointsRenderable::~PointsRenderable()
00265 {
00266   delete mRenderOp.vertexData;
00267   delete mRenderOp.indexData;
00268 }
00269 
00270 void PointsRenderable::add(const rve_common::UUID& id, const rve_msgs::Points& points, uint32_t start, uint32_t& out_start, uint32_t& out_count)
00271 {
00272   ROS_ASSERT(points.positions.size() == points.colors.size());
00273   ROS_ASSERT(!desc_.has_orientations || (points.positions.size() == points.orientations.size() || points.positions.size() == points.normals.size()));
00274   ROS_ASSERT(!desc_.has_normals || points.positions.size() == points.normals.size());
00275   ROS_ASSERT(!desc_.has_scales || points.positions.size() == points.scales.size());
00276 
00277   bool orientation_from_normal = desc_.has_orientations && points.orientations.empty() && points.positions.size() == points.normals.size();
00278 
00279   Ogre::Root* root = Ogre::Root::getSingletonPtr();
00280 
00281   uint32_t verts_per_point = getVerticesPerPoint();
00282   uint32_t point_stride = getPointStride();
00283   float* vertices = getVertices();
00284   float* normals = getNormals();
00285   bool alpha = alpha_;
00286 
00287   Ogre::HardwareVertexBufferSharedPtr vbuf = mRenderOp.vertexData->vertexBufferBinding->getBuffer(0);
00288   out_start = (mRenderOp.vertexData->vertexStart / verts_per_point) + (mRenderOp.vertexData->vertexCount / verts_per_point);
00289 
00290   uint32_t end = std::min((size_t)POINTS_PER_VBO, out_start + (points.positions.size() - start));
00291   out_count = 0;
00292 
00293   uint32_t lock_start = out_start * point_stride * verts_per_point;
00294   uint32_t lock_size = end * point_stride * verts_per_point;
00295   void* data = vbuf->lock(lock_start, lock_size, Ogre::HardwareVertexBuffer::HBL_NO_OVERWRITE);
00296   float* fptr = (float*)data;
00297 
00298   rve_msgs::Points::_positions_type::const_iterator pos_it = points.positions.begin() + start;
00299   rve_msgs::Points::_orientations_type::const_iterator orient_it = points.orientations.begin() + start;
00300   rve_msgs::Points::_colors_type::const_iterator col_it = points.colors.begin() + start;
00301   rve_msgs::Points::_normals_type::const_iterator norm_it = points.normals.begin() + start;
00302   rve_msgs::Points::_scales_type::const_iterator scales_it = points.scales.begin() + start;
00303   for (uint32_t i = out_start; i < end; ++i, ++pos_it, ++col_it)
00304   {
00305     float a = col_it->a;
00306     if ((a < 0.99) != alpha)
00307     {
00308       continue;
00309     }
00310 
00311     Ogre::Vector3 pos = fromRobot(Ogre::Vector3(pos_it->x, pos_it->y, pos_it->z));
00312     mBox.merge(pos);
00313 
00314     float r = col_it->r;
00315     float g = col_it->g;
00316     float b = col_it->b;
00317 
00318     uint32_t color = 0;
00319     root->convertColourValue(Ogre::ColourValue(r, g, b, a), &color);
00320 
00321     ++out_count;
00322 
00323     for (uint32_t j = 0; j < verts_per_point; ++j)
00324     {
00325       *fptr++ = pos.x;
00326       *fptr++ = pos.y;
00327       *fptr++ = pos.z;
00328 
00329       if (needs_normals_)
00330       {
00331         *fptr++ = normals[(j*3)];
00332         *fptr++ = normals[(j*3) + 1];
00333         *fptr++ = normals[(j*3) + 2];
00334       }
00335 
00336       if (needs_offsets_)
00337       {
00338         *fptr++ = vertices[(j*3)];
00339         *fptr++ = vertices[(j*3) + 1];
00340         *fptr++ = vertices[(j*3) + 2];
00341       }
00342 
00343       if (desc_.has_normals)
00344       {
00345         Ogre::Vector3 norm(normalFromRobot(Ogre::Vector3(norm_it->x, norm_it->y, norm_it->z)));
00346 
00347         *fptr++ = norm.x;
00348         *fptr++ = norm.y;
00349         *fptr++ = norm.z;
00350       }
00351 
00352 #if 01
00353       if (desc_.has_orientations)
00354       {
00355         Ogre::Quaternion quat;
00356         if (orientation_from_normal)
00357         {
00358           Ogre::Vector3 norm(normalFromRobot(Ogre::Vector3(norm_it->x, norm_it->y, norm_it->z)));
00359           quat = Ogre::Quaternion(Ogre::Vector3::UNIT_Z.getRotationTo(norm));
00360         }
00361         else
00362         {
00363           quat = fromRobotRelative(Ogre::Quaternion(orient_it->w, orient_it->x, orient_it->y, orient_it->z));
00364         }
00365 
00366         *fptr++ = quat.x;
00367         *fptr++ = quat.y;
00368         *fptr++ = quat.z;
00369         *fptr++ = quat.w;
00370       }
00371 #endif
00372 
00373       if (desc_.has_scales)
00374       {
00375         Ogre::Vector3 scale(scales_it->x, scales_it->y, scales_it->z);
00376         scale = scaleFromRobot(scale);
00377         *fptr++ = scale.x;
00378         *fptr++ = scale.y;
00379         *fptr++ = scale.z;
00380       }
00381 
00382       uint32_t* iptr = (uint32_t*)fptr;
00383       *iptr = color;
00384       ++fptr;
00385 
00386       iptr = (uint32_t*)fptr;
00387       *iptr = i;
00388       ++fptr;
00389     }
00390 
00391     if (desc_.has_normals || orientation_from_normal)
00392     {
00393       ++norm_it;
00394     }
00395 
00396     if (desc_.has_orientations && !orientation_from_normal)
00397     {
00398       ++orient_it;
00399     }
00400 
00401     if (desc_.has_scales)
00402     {
00403       ++scales_it;
00404     }
00405   }
00406 
00407   vbuf->unlock();
00408 
00409   mRenderOp.vertexData->vertexCount += out_count * verts_per_point;
00410   count_ += out_count;
00411 
00412   if (out_count > 0)
00413   {
00414     PointsInfo info;
00415     info.id = id;
00416     info.offset = out_start - start;
00417     info.begin = out_start;
00418     info.end = out_start + out_count;
00419     points_info_.push_back(info);
00420   }
00421 }
00422 
00423 void PointsRenderable::remove(uint32_t start, uint32_t count)
00424 {
00425   ROS_ASSERT_MSG(count <= count_, "count = %d, count_ = %d", count, count_);
00426   ROS_ASSERT_MSG(start + count <= POINTS_PER_VBO, "start = %d, count = %d, POINTS_PER_VBO = %d", start, count, POINTS_PER_VBO);
00427 
00428   uint32_t verts_per_point = getVerticesPerPoint();
00429   uint32_t point_stride = getPointStride();
00430 
00431   Ogre::HardwareVertexBufferSharedPtr vbuf = mRenderOp.vertexData->vertexBufferBinding->getBuffer(0);
00432   uint8_t* data = (uint8_t*)vbuf->lock(start * point_stride * verts_per_point, count * point_stride * verts_per_point, Ogre::HardwareVertexBuffer::HBL_NO_OVERWRITE);
00433 
00434   uint32_t total_vertices = verts_per_point * count;
00435   data = data + (point_stride * verts_per_point * start);
00436   for (uint32_t i = 0; i < total_vertices; ++i)
00437   {
00438     // Assign positions outside the viewable area
00439     float* fptr = (float*)data;
00440     *fptr++ = 99999999.0f;
00441     *fptr++ = 99999999.0f;
00442     *fptr++ = 99999999.0f;
00443 
00444     data += point_stride;
00445   }
00446 
00447   count_ -= count;
00448 
00449   vbuf->unlock();
00450 
00451   if (isEmpty())
00452   {
00453     mBox.setNull();
00454   }
00455 }
00456 
00457 uint32_t PointsRenderable::getElementsPerVBO()
00458 {
00459   return POINTS_PER_VBO;
00460 }
00461 
00462 uint32_t PointsRenderable::getPointStride()
00463 {
00464   return mRenderOp.vertexData->vertexDeclaration->getVertexSize(0);
00465 }
00466 
00467 float* PointsRenderable::getNormals()
00468 {
00469   if (supports_geometry_programs_)
00470   {
00471     return 0;
00472   }
00473   else
00474   {
00475     if (desc_.type == rve_msgs::Points::TYPE_POINTS)
00476     {
00477       return 0;
00478     }
00479     else if (desc_.type == rve_msgs::Points::TYPE_BILLBOARDS)
00480     {
00481       return 0;
00482     }
00483     else if (desc_.type == rve_msgs::Points::TYPE_BILLBOARD_SPHERES)
00484     {
00485       return 0;
00486     }
00487 #if 0
00488     else if (desc_.type == RM_BILLBOARDS_COMMON_FACING)
00489     {
00490       return 0;
00491     }
00492 #endif
00493     else if (desc_.type == rve_msgs::Points::TYPE_BOXES)
00494     {
00495       return g_box_normals;
00496     }
00497   }
00498 
00499   ROS_ASSERT_MSG(false, "Unknown points type %d", desc_.type);
00500 
00501   return 0;
00502 }
00503 
00504 float* PointsRenderable::getVertices()
00505 {
00506   if (supports_geometry_programs_)
00507   {
00508     return g_point_vertices;
00509   }
00510   else
00511   {
00512     if (desc_.type == rve_msgs::Points::TYPE_POINTS)
00513     {
00514       return g_point_vertices;
00515     }
00516     else if (desc_.type == rve_msgs::Points::TYPE_BILLBOARDS)
00517     {
00518       return g_billboard_vertices;
00519     }
00520     else if (desc_.type == rve_msgs::Points::TYPE_BILLBOARD_SPHERES)
00521     {
00522       return g_billboard_sphere_vertices;
00523     }
00524 #if 0
00525     else if (desc_.type == RM_BILLBOARDS_COMMON_FACING)
00526     {
00527       return g_billboard_vertices;
00528     }
00529 #endif
00530     else if (desc_.type == rve_msgs::Points::TYPE_BOXES)
00531     {
00532       return g_box_vertices;
00533     }
00534   }
00535 
00536   ROS_ASSERT_MSG(false, "Unknown points type %d", desc_.type);
00537 
00538   return 0;
00539 }
00540 
00541 uint32_t PointsRenderable::getVerticesPerPoint()
00542 {
00543   if (supports_geometry_programs_)
00544   {
00545     return 1;
00546   }
00547 
00548   if (desc_.type == rve_msgs::Points::TYPE_POINTS)
00549   {
00550     return 1;
00551   }
00552 
00553   if (desc_.type == rve_msgs::Points::TYPE_BILLBOARDS)
00554   {
00555     return 6;
00556   }
00557 
00558 #if 0
00559   if (desc_.type == RM_BILLBOARDS_COMMON_FACING)
00560     {
00561       return 6;
00562     }
00563 #endif
00564 
00565   if (desc_.type == rve_msgs::Points::TYPE_BILLBOARD_SPHERES)
00566   {
00567     return 6;
00568   }
00569 
00570   if (desc_.type == rve_msgs::Points::TYPE_BOXES)
00571   {
00572     return 36;
00573   }
00574 
00575   ROS_ASSERT_MSG(false, "Unknown point type %d", desc_.type);
00576 
00577   return 1;
00578 }
00579 
00580 Picked PointsRenderable::translatePick(PickRenderValues id)
00581 {
00582   V_PointsInfo::const_iterator it = points_info_.begin();
00583   V_PointsInfo::const_iterator end = points_info_.end();
00584   for (; it != end; ++it)
00585   {
00586     const PointsInfo& info = *it;
00587     if (id.extra >= info.begin && id.extra < info.end)
00588     {
00589       return Picked(info.id, id.extra - info.offset);
00590     }
00591   }
00592 
00593   return Picked();
00594 }
00595 
00599 
00600 PointsRenderer::PointsRenderer(Ogre::SceneManager* scene_manager, const PointsRendererDesc& desc)
00601 : Parent(scene_manager, desc)
00602 {
00603 }
00604 
00605 std::pair<Ogre::MaterialPtr, Ogre::MaterialPtr> PointsRenderer::createMaterials(const PointsRendererDesc& desc)
00606 {
00607   return generateMaterialsForPoints(desc);
00608 }
00609 
00610 size_t PointsRenderer::getInputCount(const rve_msgs::Points& points)
00611 {
00612   return points.positions.size();
00613 }
00614 
00615 } // namespace rve_render_server


rve_render_server
Author(s): Josh Faust
autogenerated on Wed Dec 11 2013 14:31:15