00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
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
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
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
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
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
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
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
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
00528
00529
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
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
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
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
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
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 }