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