$search
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 <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 #define SIZE_PARAMETER 0 00048 #define ALPHA_PARAMETER 1 00049 #define PICK_COLOR_PARAMETER 2 00050 #define NORMAL_PARAMETER 3 00051 #define UP_PARAMETER 4 00052 #define HIGHLIGHT_PARAMETER 5 00053 00054 namespace ogre_tools 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.5f, 0.0f, 00075 -1.5f, -1.5f, 0.0f, 00076 1.5f, -1.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("ogre_tools/PointCloudPoint"); 00144 point_material_ = point_material_->clone(ss.str() + "Point"); 00145 billboard_material_ = Ogre::MaterialManager::getSingleton().getByName("ogre_tools/PointCloudBillboard"); 00146 billboard_material_ = billboard_material_->clone(ss.str() + "Billboard"); 00147 billboard_sphere_material_ = Ogre::MaterialManager::getSingleton().getByName("ogre_tools/PointCloudBillboardSphere"); 00148 billboard_sphere_material_ = billboard_sphere_material_->clone(ss.str() + "BillboardSphere"); 00149 billboard_common_facing_material_ = Ogre::MaterialManager::getSingleton().getByName("ogre_tools/PointCloudBillboardCommonFacing"); 00150 billboard_common_facing_material_ = billboard_common_facing_material_->clone(ss.str() + "BillboardCommonFacing"); 00151 box_material_ = Ogre::MaterialManager::getSingleton().getByName("ogre_tools/PointCloudBox"); 00152 box_material_ = box_material_->clone(ss.str() + "Box"); 00153 00154 point_material_->load(); 00155 billboard_material_->load(); 00156 billboard_sphere_material_->load(); 00157 billboard_common_facing_material_->load(); 00158 box_material_->load(); 00159 00160 setAlpha( 1.0f ); 00161 setRenderMode(RM_BILLBOARD_SPHERES); 00162 setDimensions(0.01f, 0.01f, 0.01f); 00163 00164 clear(); 00165 } 00166 00167 PointCloud::~PointCloud() 00168 { 00169 point_material_->unload(); 00170 billboard_material_->unload(); 00171 billboard_sphere_material_->unload(); 00172 billboard_common_facing_material_->unload(); 00173 box_material_->unload(); 00174 } 00175 00176 const Ogre::AxisAlignedBox& PointCloud::getBoundingBox() const 00177 { 00178 return bounding_box_; 00179 } 00180 00181 float PointCloud::getBoundingRadius() const 00182 { 00183 return bounding_radius_; 00184 } 00185 00186 void PointCloud::getWorldTransforms(Ogre::Matrix4* xform) const 00187 { 00188 *xform = _getParentNodeFullTransform(); 00189 } 00190 00191 void PointCloud::clear() 00192 { 00193 point_count_ = 0; 00194 bounding_box_.setNull(); 00195 bounding_radius_ = 0.0f; 00196 00197 V_PointCloudRenderable::iterator it = renderables_.begin(); 00198 V_PointCloudRenderable::iterator end = renderables_.end(); 00199 for (; it != end; ++it) 00200 { 00201 (*it)->getRenderOperation()->vertexData->vertexStart = 0; 00202 (*it)->getRenderOperation()->vertexData->vertexCount = 0; 00203 } 00204 00205 if (getParentSceneNode()) 00206 { 00207 getParentSceneNode()->needUpdate(); 00208 } 00209 } 00210 00211 void PointCloud::regenerateAll() 00212 { 00213 if (point_count_ == 0) 00214 { 00215 return; 00216 } 00217 00218 V_Point points; 00219 points.swap(points_); 00220 uint32_t count = point_count_; 00221 00222 clear(); 00223 00224 addPoints(&points.front(), count); 00225 } 00226 00227 void PointCloud::setColorByIndex(bool set) 00228 { 00229 color_by_index_ = set; 00230 00231 regenerateAll(); 00232 } 00233 00234 void PointCloud::setHighlightColor( float r, float g, float b ) 00235 { 00236 Ogre::Vector4 highlight( r, g, b, 0.0f ); 00237 00238 V_PointCloudRenderable::iterator it = renderables_.begin(); 00239 V_PointCloudRenderable::iterator end = renderables_.end(); 00240 for (; it != end; ++it) 00241 { 00242 (*it)->setCustomParameter(HIGHLIGHT_PARAMETER, highlight); 00243 } 00244 } 00245 00246 void PointCloud::setRenderMode(RenderMode mode) 00247 { 00248 render_mode_ = mode; 00249 00250 if (mode == RM_POINTS) 00251 { 00252 current_material_ = point_material_; 00253 } 00254 else if (mode == RM_BILLBOARDS) 00255 { 00256 current_material_ = billboard_material_; 00257 } 00258 else if (mode == RM_BILLBOARD_SPHERES) 00259 { 00260 current_material_ = billboard_sphere_material_; 00261 } 00262 else if (mode == RM_BILLBOARDS_COMMON_FACING) 00263 { 00264 current_material_ = billboard_common_facing_material_; 00265 } 00266 else if (mode == RM_BOXES) 00267 { 00268 current_material_ = box_material_; 00269 } 00270 00271 current_material_->load(); 00272 00273 //ROS_INFO("Best technique [%s] [gp=%s]", current_material_->getBestTechnique()->getName().c_str(), current_material_->getBestTechnique()->getPass(0)->getGeometryProgramName().c_str()); 00274 00275 bool geom_support_changed = false; 00276 Ogre::Technique* best = current_material_->getBestTechnique(); 00277 if (best) 00278 { 00279 if (current_material_->getBestTechnique()->getName() == "gp") 00280 { 00281 if (!current_mode_supports_geometry_shader_) 00282 { 00283 geom_support_changed = true; 00284 } 00285 00286 current_mode_supports_geometry_shader_ = true; 00287 00288 //ROS_INFO("Using geometry shader"); 00289 } 00290 else 00291 { 00292 if (current_mode_supports_geometry_shader_) 00293 { 00294 geom_support_changed = true; 00295 } 00296 00297 current_mode_supports_geometry_shader_ = false; 00298 } 00299 } 00300 else 00301 { 00302 geom_support_changed = true; 00303 current_mode_supports_geometry_shader_ = false; 00304 00305 ROS_ERROR("No techniques available for material [%s]", current_material_->getName().c_str()); 00306 } 00307 00308 if (geom_support_changed) 00309 { 00310 renderables_.clear(); 00311 } 00312 00313 V_PointCloudRenderable::iterator it = renderables_.begin(); 00314 V_PointCloudRenderable::iterator end = renderables_.end(); 00315 for (; it != end; ++it) 00316 { 00317 (*it)->setMaterial(current_material_->getName()); 00318 } 00319 00320 regenerateAll(); 00321 } 00322 00323 void PointCloud::setDimensions(float width, float height, float depth) 00324 { 00325 width_ = width; 00326 height_ = height; 00327 depth_ = depth; 00328 00329 Ogre::Vector4 size(width_, height_, depth_, 0.0f); 00330 00331 V_PointCloudRenderable::iterator it = renderables_.begin(); 00332 V_PointCloudRenderable::iterator end = renderables_.end(); 00333 for (; it != end; ++it) 00334 { 00335 (*it)->setCustomParameter(SIZE_PARAMETER, size); 00336 } 00337 } 00338 00339 void PointCloud::setCommonDirection(const Ogre::Vector3& vec) 00340 { 00341 common_direction_ = vec; 00342 00343 V_PointCloudRenderable::iterator it = renderables_.begin(); 00344 V_PointCloudRenderable::iterator end = renderables_.end(); 00345 for (; it != end; ++it) 00346 { 00347 (*it)->setCustomParameter(NORMAL_PARAMETER, Ogre::Vector4(vec)); 00348 } 00349 } 00350 00351 void PointCloud::setCommonUpVector(const Ogre::Vector3& vec) 00352 { 00353 common_up_vector_ = vec; 00354 00355 V_PointCloudRenderable::iterator it = renderables_.begin(); 00356 V_PointCloudRenderable::iterator end = renderables_.end(); 00357 for (; it != end; ++it) 00358 { 00359 (*it)->setCustomParameter(UP_PARAMETER, Ogre::Vector4(vec)); 00360 } 00361 } 00362 00363 void setAlphaBlending(const Ogre::MaterialPtr& mat) 00364 { 00365 if (mat->getBestTechnique()) 00366 { 00367 mat->getBestTechnique()->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA ); 00368 mat->getBestTechnique()->setDepthWriteEnabled( false ); 00369 } 00370 } 00371 00372 void setReplace(const Ogre::MaterialPtr& mat) 00373 { 00374 if (mat->getBestTechnique()) 00375 { 00376 mat->getBestTechnique()->setSceneBlending( Ogre::SBT_REPLACE ); 00377 mat->getBestTechnique()->setDepthWriteEnabled( true ); 00378 } 00379 } 00380 00381 void PointCloud::setAlpha(float alpha) 00382 { 00383 alpha_ = alpha; 00384 00385 if ( alpha < 0.9998 ) 00386 { 00387 setAlphaBlending(point_material_); 00388 setAlphaBlending(billboard_material_); 00389 setAlphaBlending(billboard_sphere_material_); 00390 setAlphaBlending(billboard_common_facing_material_); 00391 setAlphaBlending(box_material_); 00392 } 00393 else 00394 { 00395 setReplace(point_material_); 00396 setReplace(billboard_material_); 00397 setReplace(billboard_sphere_material_); 00398 setReplace(billboard_common_facing_material_); 00399 setReplace(box_material_); 00400 } 00401 00402 Ogre::Vector4 alpha4(alpha_, alpha_, alpha_, alpha_); 00403 V_PointCloudRenderable::iterator it = renderables_.begin(); 00404 V_PointCloudRenderable::iterator end = renderables_.end(); 00405 for (; it != end; ++it) 00406 { 00407 (*it)->setCustomParameter(ALPHA_PARAMETER, alpha4); 00408 } 00409 } 00410 00411 void PointCloud::addPoints(Point* points, uint32_t num_points) 00412 { 00413 if (num_points == 0) 00414 { 00415 return; 00416 } 00417 00418 Ogre::Root* root = Ogre::Root::getSingletonPtr(); 00419 00420 if ( points_.size() < point_count_ + num_points ) 00421 { 00422 points_.resize( point_count_ + num_points ); 00423 } 00424 00425 Point* begin = &points_.front() + point_count_; 00426 memcpy( begin, points, sizeof( Point ) * num_points ); 00427 00428 uint32_t vpp = getVerticesPerPoint(); 00429 Ogre::RenderOperation::OperationType op_type; 00430 if (current_mode_supports_geometry_shader_) 00431 { 00432 op_type = Ogre::RenderOperation::OT_POINT_LIST; 00433 } 00434 else 00435 { 00436 if (render_mode_ == RM_POINTS) 00437 { 00438 op_type = Ogre::RenderOperation::OT_POINT_LIST; 00439 } 00440 else 00441 { 00442 op_type = Ogre::RenderOperation::OT_TRIANGLE_LIST; 00443 } 00444 } 00445 00446 float* vertices = 0; 00447 if (current_mode_supports_geometry_shader_) 00448 { 00449 vertices = g_point_vertices; 00450 } 00451 else 00452 { 00453 if (render_mode_ == RM_POINTS) 00454 { 00455 vertices = g_point_vertices; 00456 } 00457 else if (render_mode_ == RM_BILLBOARDS) 00458 { 00459 vertices = g_billboard_vertices; 00460 } 00461 else if (render_mode_ == RM_BILLBOARD_SPHERES) 00462 { 00463 vertices = g_billboard_sphere_vertices; 00464 } 00465 else if (render_mode_ == RM_BILLBOARDS_COMMON_FACING) 00466 { 00467 vertices = g_billboard_vertices; 00468 } 00469 else if (render_mode_ == RM_BOXES) 00470 { 00471 vertices = g_box_vertices; 00472 } 00473 } 00474 00475 PointCloudRenderablePtr rend; 00476 Ogre::HardwareVertexBufferSharedPtr vbuf; 00477 void* vdata = 0; 00478 Ogre::RenderOperation* op = 0; 00479 float* fptr = 0; 00480 00481 Ogre::AxisAlignedBox aabb; 00482 aabb.setNull(); 00483 uint32_t current_vertex_count = 0; 00484 bounding_radius_ = 0.0f; 00485 uint32_t vertex_size = 0; 00486 for (uint32_t current_point = 0; current_point < num_points; ++current_point) 00487 { 00488 while (current_vertex_count >= VERTEX_BUFFER_CAPACITY || !rend) 00489 { 00490 if (rend) 00491 { 00492 ROS_ASSERT(current_vertex_count == VERTEX_BUFFER_CAPACITY); 00493 00494 op->vertexData->vertexCount = VERTEX_BUFFER_CAPACITY - op->vertexData->vertexStart; 00495 ROS_ASSERT(op->vertexData->vertexCount + op->vertexData->vertexStart <= VERTEX_BUFFER_CAPACITY); 00496 vbuf->unlock(); 00497 rend->setBoundingBox(aabb); 00498 } 00499 00500 rend = getOrCreateRenderable(); 00501 vbuf = rend->getBuffer(); 00502 vdata = vbuf->lock(Ogre::HardwareBuffer::HBL_NO_OVERWRITE); 00503 00504 op = rend->getRenderOperation(); 00505 op->operationType = op_type; 00506 current_vertex_count = op->vertexData->vertexStart + op->vertexData->vertexCount; 00507 00508 vertex_size = op->vertexData->vertexDeclaration->getVertexSize(0); 00509 uint32_t data_pos = current_vertex_count * vertex_size; 00510 fptr = (float*)((uint8_t*)vdata + data_pos); 00511 00512 aabb.setNull(); 00513 } 00514 00515 const Point& p = points[current_point]; 00516 float x = p.x; 00517 float y = p.y; 00518 float z = p.z; 00519 uint32_t color = p.color; 00520 00521 if (color_by_index_) 00522 { 00523 // convert to ColourValue, so we can then convert to the rendersystem-specific color type 00524 color = (current_point + point_count_ + 1); 00525 Ogre::ColourValue c; 00526 c.a = 1.0f; 00527 c.r = ((color >> 16) & 0xff) / 255.0f; 00528 c.g = ((color >> 8) & 0xff) / 255.0f; 00529 c.b = (color & 0xff) / 255.0f; 00530 root->convertColourValue(c, &color); 00531 } 00532 00533 Ogre::Vector3 pos(x, y, z); 00534 aabb.merge(pos); 00535 bounding_box_.merge( pos ); 00536 bounding_radius_ = std::max( bounding_radius_, pos.squaredLength() ); 00537 00538 for (uint32_t j = 0; j < vpp; ++j, ++current_vertex_count) 00539 { 00540 *fptr++ = x; 00541 *fptr++ = y; 00542 *fptr++ = z; 00543 00544 if (!current_mode_supports_geometry_shader_) 00545 { 00546 *fptr++ = vertices[(j*3)]; 00547 *fptr++ = vertices[(j*3) + 1]; 00548 *fptr++ = vertices[(j*3) + 2]; 00549 } 00550 00551 uint32_t* iptr = (uint32_t*)fptr; 00552 *iptr = color; 00553 ++fptr; 00554 00555 ROS_ASSERT((uint8_t*)fptr <= (uint8_t*)vdata + VERTEX_BUFFER_CAPACITY * vertex_size); 00556 } 00557 } 00558 00559 op->vertexData->vertexCount = current_vertex_count - op->vertexData->vertexStart; 00560 rend->setBoundingBox(aabb); 00561 ROS_ASSERT(op->vertexData->vertexCount + op->vertexData->vertexStart <= VERTEX_BUFFER_CAPACITY); 00562 00563 vbuf->unlock(); 00564 00565 point_count_ += num_points; 00566 00567 shrinkRenderables(); 00568 00569 if (getParentSceneNode()) 00570 { 00571 getParentSceneNode()->needUpdate(); 00572 } 00573 } 00574 00575 void PointCloud::popPoints(uint32_t num_points) 00576 { 00577 uint32_t vpp = getVerticesPerPoint(); 00578 00579 ROS_ASSERT(num_points <= point_count_); 00580 points_.erase(points_.begin(), points_.begin() + num_points); 00581 00582 point_count_ -= num_points; 00583 00584 // Now clear out popped points 00585 uint32_t popped_count = 0; 00586 while (popped_count < num_points * vpp) 00587 { 00588 PointCloudRenderablePtr rend = renderables_.front(); 00589 Ogre::RenderOperation* op = rend->getRenderOperation(); 00590 00591 uint32_t popped = std::min((size_t)(num_points * vpp - popped_count), op->vertexData->vertexCount); 00592 op->vertexData->vertexStart += popped; 00593 op->vertexData->vertexCount -= popped; 00594 00595 popped_count += popped; 00596 00597 if (op->vertexData->vertexCount == 0) 00598 { 00599 renderables_.erase(renderables_.begin(), renderables_.begin() + 1); 00600 00601 op->vertexData->vertexStart = 0; 00602 renderables_.push_back(rend); 00603 } 00604 } 00605 ROS_ASSERT(popped_count == num_points * vpp); 00606 00607 // reset bounds 00608 bounding_box_.setNull(); 00609 bounding_radius_ = 0.0f; 00610 for (uint32_t i = 0; i < point_count_; ++i) 00611 { 00612 Point& p = points_[i]; 00613 Ogre::Vector3 pos(p.x, p.y, p.z); 00614 bounding_box_.merge(pos); 00615 bounding_radius_ = std::max(bounding_radius_, pos.squaredLength()); 00616 } 00617 00618 shrinkRenderables(); 00619 00620 if (getParentSceneNode()) 00621 { 00622 getParentSceneNode()->needUpdate(); 00623 } 00624 } 00625 00626 void PointCloud::shrinkRenderables() 00627 { 00628 while (!renderables_.empty()) 00629 { 00630 PointCloudRenderablePtr rend = renderables_.back(); 00631 Ogre::RenderOperation* op = rend->getRenderOperation(); 00632 if (op->vertexData->vertexCount == 0) 00633 { 00634 renderables_.pop_back(); 00635 } 00636 else 00637 { 00638 break; 00639 } 00640 } 00641 } 00642 00643 void PointCloud::_notifyCurrentCamera(Ogre::Camera* camera) 00644 { 00645 MovableObject::_notifyCurrentCamera( camera ); 00646 } 00647 00648 void PointCloud::_updateRenderQueue(Ogre::RenderQueue* queue) 00649 { 00650 V_PointCloudRenderable::iterator it = renderables_.begin(); 00651 V_PointCloudRenderable::iterator end = renderables_.end(); 00652 for (; it != end; ++it) 00653 { 00654 queue->addRenderable((*it).get()); 00655 } 00656 } 00657 00658 void PointCloud::_notifyAttached(Ogre::Node *parent, bool isTagPoint) 00659 { 00660 MovableObject::_notifyAttached(parent, isTagPoint); 00661 } 00662 00663 uint32_t PointCloud::getVerticesPerPoint() 00664 { 00665 if (current_mode_supports_geometry_shader_) 00666 { 00667 return 1; 00668 } 00669 00670 if (render_mode_ == RM_POINTS) 00671 { 00672 return 1; 00673 } 00674 00675 if (render_mode_ == RM_BILLBOARDS) 00676 { 00677 return 6; 00678 } 00679 00680 if (render_mode_ == RM_BILLBOARDS_COMMON_FACING) 00681 { 00682 return 6; 00683 } 00684 00685 if (render_mode_ == RM_BILLBOARD_SPHERES) 00686 { 00687 return 3; 00688 } 00689 00690 if (render_mode_ == RM_BOXES) 00691 { 00692 return 36; 00693 } 00694 00695 return 1; 00696 } 00697 00698 void PointCloud::setPickColor(const Ogre::ColourValue& color) 00699 { 00700 pick_color_ = color; 00701 Ogre::Vector4 pick_col(pick_color_.r, pick_color_.g, pick_color_.b, pick_color_.a); 00702 00703 V_PointCloudRenderable::iterator it = renderables_.begin(); 00704 V_PointCloudRenderable::iterator end = renderables_.end(); 00705 for (; it != end; ++it) 00706 { 00707 (*it)->setCustomParameter(PICK_COLOR_PARAMETER, pick_col); 00708 } 00709 } 00710 00711 PointCloudRenderablePtr PointCloud::getOrCreateRenderable() 00712 { 00713 V_PointCloudRenderable::iterator it = renderables_.begin(); 00714 V_PointCloudRenderable::iterator end = renderables_.end(); 00715 for (; it != end; ++it) 00716 { 00717 const PointCloudRenderablePtr& rend = *it; 00718 Ogre::RenderOperation* op = rend->getRenderOperation(); 00719 if (op->vertexData->vertexCount + op->vertexData->vertexStart < VERTEX_BUFFER_CAPACITY) 00720 { 00721 return rend; 00722 } 00723 } 00724 00725 PointCloudRenderablePtr rend(new PointCloudRenderable(this, !current_mode_supports_geometry_shader_)); 00726 rend->setMaterial(current_material_->getName()); 00727 Ogre::Vector4 size(width_, height_, depth_, 0.0f); 00728 Ogre::Vector4 alpha(alpha_, 0.0f, 0.0f, 0.0f); 00729 Ogre::Vector4 highlight(0.0f, 0.0f, 0.0f, 0.0f); 00730 Ogre::Vector4 pick_col(pick_color_.r, pick_color_.g, pick_color_.b, pick_color_.a); 00731 rend->setCustomParameter(SIZE_PARAMETER, size); 00732 rend->setCustomParameter(ALPHA_PARAMETER, alpha); 00733 rend->setCustomParameter(HIGHLIGHT_PARAMETER, highlight); 00734 rend->setCustomParameter(PICK_COLOR_PARAMETER, pick_col); 00735 rend->setCustomParameter(NORMAL_PARAMETER, Ogre::Vector4(common_direction_)); 00736 rend->setCustomParameter(UP_PARAMETER, Ogre::Vector4(common_up_vector_)); 00737 if (getParentSceneNode()) 00738 { 00739 getParentSceneNode()->attachObject(rend.get()); 00740 } 00741 renderables_.push_back(rend); 00742 00743 return rend; 00744 } 00745 00746 #if (OGRE_VERSION_MAJOR >= 1 && OGRE_VERSION_MINOR >= 6) 00747 void PointCloud::visitRenderables(Ogre::Renderable::Visitor* visitor, bool debugRenderables) 00748 { 00749 00750 } 00751 #endif 00752 00753 00757 00758 PointCloudRenderable::PointCloudRenderable(PointCloud* parent, bool use_tex_coords) 00759 : parent_(parent) 00760 { 00761 // Initialize render operation 00762 mRenderOp.operationType = Ogre::RenderOperation::OT_POINT_LIST; 00763 mRenderOp.useIndexes = false; 00764 mRenderOp.vertexData = new Ogre::VertexData; 00765 mRenderOp.vertexData->vertexStart = 0; 00766 mRenderOp.vertexData->vertexCount = 0; 00767 00768 Ogre::VertexDeclaration *decl = mRenderOp.vertexData->vertexDeclaration; 00769 size_t offset = 0; 00770 00771 decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_POSITION); 00772 offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3); 00773 00774 if (use_tex_coords) 00775 { 00776 decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_TEXTURE_COORDINATES, 0); 00777 offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3); 00778 } 00779 00780 decl->addElement(0, offset, Ogre::VET_COLOUR, Ogre::VES_DIFFUSE); 00781 00782 Ogre::HardwareVertexBufferSharedPtr vbuf = 00783 Ogre::HardwareBufferManager::getSingleton().createVertexBuffer( 00784 mRenderOp.vertexData->vertexDeclaration->getVertexSize(0), 00785 VERTEX_BUFFER_CAPACITY, 00786 Ogre::HardwareBuffer::HBU_DYNAMIC); 00787 00788 // Bind buffer 00789 mRenderOp.vertexData->vertexBufferBinding->setBinding(0, vbuf); 00790 } 00791 00792 PointCloudRenderable::~PointCloudRenderable() 00793 { 00794 delete mRenderOp.vertexData; 00795 delete mRenderOp.indexData; 00796 } 00797 00798 Ogre::HardwareVertexBufferSharedPtr PointCloudRenderable::getBuffer() 00799 { 00800 return mRenderOp.vertexData->vertexBufferBinding->getBuffer(0); 00801 } 00802 00803 void PointCloudRenderable::_notifyCurrentCamera(Ogre::Camera* camera) 00804 { 00805 SimpleRenderable::_notifyCurrentCamera( camera ); 00806 } 00807 00808 Ogre::Real PointCloudRenderable::getBoundingRadius(void) const 00809 { 00810 return Ogre::Math::Sqrt(std::max(mBox.getMaximum().squaredLength(), mBox.getMinimum().squaredLength())); 00811 } 00812 00813 Ogre::Real PointCloudRenderable::getSquaredViewDepth(const Ogre::Camera* cam) const 00814 { 00815 Ogre::Vector3 vMin, vMax, vMid, vDist; 00816 vMin = mBox.getMinimum(); 00817 vMax = mBox.getMaximum(); 00818 vMid = ((vMax - vMin) * 0.5) + vMin; 00819 vDist = cam->getDerivedPosition() - vMid; 00820 00821 return vDist.squaredLength(); 00822 } 00823 00824 void PointCloudRenderable::getWorldTransforms(Ogre::Matrix4* xform) const 00825 { 00826 *xform = m_matWorldTransform * parent_->getParentNode()->_getFullTransform(); 00827 } 00828 00829 const Ogre::LightList& PointCloudRenderable::getLights() const 00830 { 00831 return parent_->queryLights(); 00832 } 00833 00834 } // namespace ogre_tools