point_cloud.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include "point_cloud.h"
31 #include <ros/assert.h>
32 #include <qglobal.h>
33 
34 #include <OgreSceneManager.h>
35 #include <OgreSceneNode.h>
37 #include <OgreQuaternion.h>
38 #include <OgreManualObject.h>
39 #include <OgreMaterialManager.h>
40 #include <OgreBillboard.h>
41 #include <OgreTexture.h>
42 #include <OgreTextureManager.h>
43 #include <OgreSharedPtr.h>
44 #include <OgreTechnique.h>
45 #include <OgreCamera.h>
46 
47 #include <sstream>
48 
52 
53 #define VERTEX_BUFFER_CAPACITY (36 * 1024 * 10)
54 
55 namespace rviz
56 {
57 static float g_point_vertices[3] = {0.0f, 0.0f, 0.0f};
58 
59 static float g_billboard_vertices[6 * 3] = {
60  -0.5f, 0.5f, 0.0f, -0.5f, -0.5f, 0.0f, 0.5f, 0.5f, 0.0f,
61  0.5f, 0.5f, 0.0f, -0.5f, -0.5f, 0.0f, 0.5f, -0.5f, 0.0f,
62 };
63 
64 static float g_billboard_sphere_vertices[3 * 3] = {
65  0.0f, 1.0f, 0.0f, -0.866025404f, -0.5f, 0.0f, 0.866025404f, -0.5f, 0.0f,
66 };
67 
68 static float g_box_vertices[6 * 6 * 3] = {
69  // clang-format off
70  // front
71  -0.5f, 0.5f, -0.5f, -0.5f, -0.5f, -0.5f, 0.5f, 0.5f, -0.5f, 0.5f, 0.5f, -0.5f, -0.5f, -0.5f, -0.5f,
72  0.5f, -0.5f, -0.5f,
73 
74  // back
75  -0.5f, 0.5f, 0.5f, 0.5f, 0.5f, 0.5f, -0.5f, -0.5f, 0.5f, 0.5f, 0.5f, 0.5f, 0.5f, -0.5f, 0.5f, -0.5f, -0.5f, 0.5f,
76 
77  // right
78  0.5, 0.5, 0.5, 0.5, 0.5, -0.5, 0.5, -0.5, 0.5, 0.5, 0.5, -0.5, 0.5, -0.5, -0.5, 0.5, -0.5, 0.5,
79 
80  // left
81  -0.5, 0.5, 0.5, -0.5, -0.5, 0.5, -0.5, 0.5, -0.5, -0.5, 0.5, -0.5, -0.5, -0.5, 0.5, -0.5, -0.5, -0.5,
82 
83  // top
84  -0.5, 0.5, -0.5, 0.5, 0.5, -0.5, -0.5, 0.5, 0.5, 0.5, 0.5, -0.5, 0.5, 0.5, 0.5, -0.5, 0.5, 0.5,
85 
86  // bottom
87  -0.5, -0.5, -0.5, -0.5, -0.5, 0.5, 0.5, -0.5, -0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5, 0.5, -0.5, 0.5,
88 }; // clang-format on
89 
90 Ogre::String PointCloud::sm_Type = "PointCloud";
91 
93  : bounding_radius_(0.0f)
94  , point_count_(0)
95  , common_direction_(Ogre::Vector3::NEGATIVE_UNIT_Z)
96  , common_up_vector_(Ogre::Vector3::UNIT_Y)
97  , color_by_index_(false)
98  , current_mode_supports_geometry_shader_(false)
99 {
100  std::stringstream ss;
101  static int count = 0;
102  ss << "PointCloudMaterial" << count++;
103  point_material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/PointCloudPoint");
104  square_material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/PointCloudSquare");
105  flat_square_material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/PointCloudFlatSquare");
106  sphere_material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/PointCloudSphere");
107  tile_material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/PointCloudTile");
108  box_material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/PointCloudBox");
109 
110  point_material_ = Ogre::MaterialPtr(point_material_)->clone(ss.str() + "Point");
111  square_material_ = Ogre::MaterialPtr(square_material_)->clone(ss.str() + "Square");
112  flat_square_material_ = Ogre::MaterialPtr(flat_square_material_)->clone(ss.str() + "FlatSquare");
113  sphere_material_ = Ogre::MaterialPtr(sphere_material_)->clone(ss.str() + "Sphere");
114  tile_material_ = Ogre::MaterialPtr(tile_material_)->clone(ss.str() + "Tiles");
115  box_material_ = Ogre::MaterialPtr(box_material_)->clone(ss.str() + "Box");
116 
117  point_material_->load();
118  square_material_->load();
119  flat_square_material_->load();
120  sphere_material_->load();
121  tile_material_->load();
122  box_material_->load();
123 
124  setAlpha(1.0f);
126  setDimensions(0.01f, 0.01f, 0.01f);
127 
128  clear();
129 }
130 
131 static void removeMaterial(Ogre::MaterialPtr& material)
132 {
133  Ogre::ResourcePtr resource(material);
134  Ogre::MaterialManager::getSingleton().remove(resource);
135 }
136 
138 {
139  clear();
140 
147 }
148 
149 const Ogre::AxisAlignedBox& PointCloud::getBoundingBox() const
150 {
151  return bounding_box_;
152 }
153 
155 {
156  return bounding_radius_;
157 }
158 
159 void PointCloud::getWorldTransforms(Ogre::Matrix4* xform) const
160 {
161  *xform = _getParentNodeFullTransform();
162 }
163 
165 {
166  point_count_ = 0;
167  bounding_box_.setNull();
168  bounding_radius_ = 0.0f;
169 
170  if (getParentSceneNode())
171  {
172  V_PointCloudRenderable::iterator it = renderables_.begin();
173  V_PointCloudRenderable::iterator end = renderables_.end();
174  for (; it != end; ++it)
175  {
176  getParentSceneNode()->detachObject(it->get());
177  }
178  getParentSceneNode()->needUpdate();
179  }
180 
181  renderables_.clear();
182 }
183 
185 {
186  if (point_count_ == 0)
187  {
188  return;
189  }
190 
191  V_Point points;
192  points.swap(points_);
193  uint32_t count = point_count_;
194 
195  clear();
196 
197  addPoints(&points.front(), count);
198 }
199 
201 {
203  regenerateAll();
204 }
205 
206 void PointCloud::setHighlightColor(float r, float g, float b)
207 {
208  Ogre::Vector4 highlight(r, g, b, 0.0f);
209 
210  V_PointCloudRenderable::iterator it = renderables_.begin();
211  V_PointCloudRenderable::iterator end = renderables_.end();
212  for (; it != end; ++it)
213  {
214  (*it)->setCustomParameter(HIGHLIGHT_PARAMETER, highlight);
215  }
216 }
217 
219 {
220  render_mode_ = mode;
221 
222  if (mode == RM_POINTS)
223  {
224  current_material_ = Ogre::MaterialPtr(point_material_);
225  }
226  else if (mode == RM_SQUARES)
227  {
228  current_material_ = Ogre::MaterialPtr(square_material_);
229  }
230  else if (mode == RM_FLAT_SQUARES)
231  {
232  current_material_ = Ogre::MaterialPtr(flat_square_material_);
233  }
234  else if (mode == RM_SPHERES)
235  {
236  current_material_ = Ogre::MaterialPtr(sphere_material_);
237  }
238  else if (mode == RM_TILES)
239  {
240  current_material_ = Ogre::MaterialPtr(tile_material_);
241  }
242  else if (mode == RM_BOXES)
243  {
244  current_material_ = Ogre::MaterialPtr(box_material_);
245  }
246 
247  current_material_->load();
248 
249  // ROS_INFO("Best technique [%s] [gp=%s]", current_material_->getBestTechnique()->getName().c_str(),
250  // current_material_->getBestTechnique()->getPass(0)->getGeometryProgramName().c_str());
251 
252  bool geom_support_changed = false;
253  Ogre::Technique* best = current_material_->getBestTechnique();
254  if (best)
255  {
256  if (current_material_->getBestTechnique()->getName() == "gp")
257  {
259  {
260  geom_support_changed = true;
261  }
262 
264 
265  // ROS_INFO("Using geometry shader");
266  }
267  else
268  {
270  {
271  geom_support_changed = true;
272  }
273 
275  }
276  }
277  else
278  {
279  geom_support_changed = true;
281 
282  ROS_ERROR("No techniques available for material [%s]", current_material_->getName().c_str());
283  }
284 
285  if (geom_support_changed)
286  {
287  renderables_.clear();
288  }
289 
290  V_PointCloudRenderable::iterator it = renderables_.begin();
291  V_PointCloudRenderable::iterator end = renderables_.end();
292  for (; it != end; ++it)
293  {
295  }
296 
297  regenerateAll();
298 }
299 
300 void PointCloud::setDimensions(float width, float height, float depth)
301 {
302  width_ = width;
303  height_ = height;
304  depth_ = depth;
305 
306  Ogre::Vector4 size(width_, height_, depth_, 0.0f);
307 
308  V_PointCloudRenderable::iterator it = renderables_.begin();
309  V_PointCloudRenderable::iterator end = renderables_.end();
310  for (; it != end; ++it)
311  {
312  (*it)->setCustomParameter(SIZE_PARAMETER, size);
313  }
314 }
315 
316 void PointCloud::setAutoSize(bool auto_size)
317 {
318  V_PointCloudRenderable::iterator it = renderables_.begin();
319  V_PointCloudRenderable::iterator end = renderables_.end();
320  for (; it != end; ++it)
321  {
322  (*it)->setCustomParameter(AUTO_SIZE_PARAMETER, Ogre::Vector4(auto_size));
323  }
324 }
325 
326 void PointCloud::setCommonDirection(const Ogre::Vector3& vec)
327 {
328  common_direction_ = vec;
329 
330  V_PointCloudRenderable::iterator it = renderables_.begin();
331  V_PointCloudRenderable::iterator end = renderables_.end();
332  for (; it != end; ++it)
333  {
334  (*it)->setCustomParameter(NORMAL_PARAMETER, Ogre::Vector4(vec));
335  }
336 }
337 
338 void PointCloud::setCommonUpVector(const Ogre::Vector3& vec)
339 {
340  common_up_vector_ = vec;
341 
342  V_PointCloudRenderable::iterator it = renderables_.begin();
343  V_PointCloudRenderable::iterator end = renderables_.end();
344  for (; it != end; ++it)
345  {
346  (*it)->setCustomParameter(UP_PARAMETER, Ogre::Vector4(vec));
347  }
348 }
349 
350 void setAlphaBlending(const Ogre::MaterialPtr& mat)
351 {
352  if (mat->getBestTechnique())
353  {
354  mat->getBestTechnique()->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
355  mat->getBestTechnique()->setDepthWriteEnabled(false);
356  }
357 }
358 
359 void setReplace(const Ogre::MaterialPtr& mat)
360 {
361  if (mat->getBestTechnique())
362  {
363  mat->getBestTechnique()->setSceneBlending(Ogre::SBT_REPLACE);
364  mat->getBestTechnique()->setDepthWriteEnabled(true);
365  }
366 }
367 
368 void PointCloud::setAlpha(float alpha, bool per_point_alpha)
369 {
370  alpha_ = alpha;
371 
372  if (alpha < 0.9998 || per_point_alpha)
373  {
380  }
381  else
382  {
389  }
390 
391  Ogre::Vector4 alpha4(alpha_, alpha_, alpha_, alpha_);
392  V_PointCloudRenderable::iterator it = renderables_.begin();
393  V_PointCloudRenderable::iterator end = renderables_.end();
394  for (; it != end; ++it)
395  {
396  (*it)->setCustomParameter(ALPHA_PARAMETER, alpha4);
397  }
398 }
399 
400 void PointCloud::addPoints(Point* points, uint32_t num_points)
401 {
402  if (num_points == 0)
403  {
404  return;
405  }
406  Ogre::Root* root = Ogre::Root::getSingletonPtr();
407 
408  if (points_.size() < point_count_ + num_points)
409  {
410  points_.resize(point_count_ + num_points);
411  }
412 
413  Point* begin = &points_.front() + point_count_;
414 #if defined(__GNUC__) && (__GNUC__ >= 8)
415 #pragma GCC diagnostic push
416 #pragma GCC diagnostic ignored "-Wclass-memaccess"
417 #endif
418  memcpy(begin, points, sizeof(Point) * num_points);
419 #if defined(__GNUC__) && (__GNUC__ >= 8)
420 #pragma GCC diagnostic pop
421 #endif
422 
423  uint32_t vpp = getVerticesPerPoint();
424  Ogre::RenderOperation::OperationType op_type;
426  {
427  op_type = Ogre::RenderOperation::OT_POINT_LIST;
428  }
429  else
430  {
431  if (render_mode_ == RM_POINTS)
432  {
433  op_type = Ogre::RenderOperation::OT_POINT_LIST;
434  }
435  else
436  {
437  op_type = Ogre::RenderOperation::OT_TRIANGLE_LIST;
438  }
439  }
440 
441  float* vertices = nullptr;
443  {
444  vertices = g_point_vertices;
445  }
446  else
447  {
448  switch (render_mode_)
449  {
450  case RM_POINTS:
451  vertices = g_point_vertices;
452  break;
453  case RM_SQUARES:
454  case RM_FLAT_SQUARES:
455  case RM_TILES:
456  vertices = g_billboard_vertices;
457  break;
458  case RM_SPHERES:
459  vertices = g_billboard_sphere_vertices;
460  break;
461  case RM_BOXES:
462  vertices = g_box_vertices;
463  break;
464  }
465  }
466 
468  Ogre::HardwareVertexBufferSharedPtr vbuf;
469  void* vdata = nullptr;
470  Ogre::RenderOperation* op = nullptr;
471  float* fptr = nullptr;
472 
473  Ogre::AxisAlignedBox aabb;
474  aabb.setNull();
475  uint32_t current_vertex_count = 0;
476  bounding_radius_ = 0.0f;
477  uint32_t vertex_size = 0;
478  uint32_t buffer_size = 0;
479  for (uint32_t current_point = 0; current_point < num_points; ++current_point)
480  {
481  // if we didn't create a renderable yet,
482  // or we've reached the vertex limit for the current renderable,
483  // create a new one.
484  while (!rend || current_vertex_count >= buffer_size)
485  {
486  if (rend)
487  {
488  ROS_ASSERT(current_vertex_count == buffer_size);
489 
490  op->vertexData->vertexCount = rend->getBuffer()->getNumVertices() - op->vertexData->vertexStart;
491  ROS_ASSERT(op->vertexData->vertexCount + op->vertexData->vertexStart <=
492  rend->getBuffer()->getNumVertices());
493  vbuf->unlock();
494  rend->setBoundingBox(aabb);
495  bounding_box_.merge(aabb);
496  }
497 
498  buffer_size = std::min<int>(VERTEX_BUFFER_CAPACITY, (num_points - current_point) * vpp);
499 
500  rend = createRenderable(buffer_size);
501  vbuf = rend->getBuffer();
502  vdata = vbuf->lock(Ogre::HardwareBuffer::HBL_NO_OVERWRITE);
503 
504  op = rend->getRenderOperation();
505  op->operationType = op_type;
506  current_vertex_count = 0;
507 
508  vertex_size = op->vertexData->vertexDeclaration->getVertexSize(0);
509  fptr = (float*)((uint8_t*)vdata);
510 
511  aabb.setNull();
512  }
513 
514  const Point& p = points[current_point];
515 
516  uint32_t color;
517 
518  if (color_by_index_)
519  {
520  // convert to ColourValue, so we can then convert to the rendersystem-specific color type
521  color = (current_point + point_count_ + 1);
522  Ogre::ColourValue c;
523  c.a = 1.0f;
524  c.r = ((color >> 16) & 0xff) / 255.0f;
525  c.g = ((color >> 8) & 0xff) / 255.0f;
526  c.b = (color & 0xff) / 255.0f;
527  root->convertColourValue(c, &color);
528  }
529  else
530  {
531  root->convertColourValue(p.color, &color);
532  }
533 
534  aabb.merge(p.position);
535  bounding_radius_ = std::max(bounding_radius_, p.position.squaredLength());
536 
537  float x = p.position.x;
538  float y = p.position.y;
539  float z = p.position.z;
540 
541  for (uint32_t j = 0; j < vpp; ++j, ++current_vertex_count)
542  {
543  *fptr++ = x;
544  *fptr++ = y;
545  *fptr++ = z;
546 
548  {
549  *fptr++ = vertices[(j * 3)];
550  *fptr++ = vertices[(j * 3) + 1];
551  *fptr++ = vertices[(j * 3) + 2];
552  }
553 
554  uint32_t* iptr = (uint32_t*)fptr;
555  *iptr = color;
556  ++fptr;
557 
558  ROS_ASSERT((uint8_t*)fptr <= (uint8_t*)vdata + rend->getBuffer()->getNumVertices() * vertex_size);
559  Q_UNUSED(vertex_size);
560  }
561  }
562 
563  op->vertexData->vertexCount = current_vertex_count - op->vertexData->vertexStart;
564  rend->setBoundingBox(aabb);
565  bounding_box_.merge(aabb);
566  ROS_ASSERT(op->vertexData->vertexCount + op->vertexData->vertexStart <=
567  rend->getBuffer()->getNumVertices());
568 
569  vbuf->unlock();
570 
571  point_count_ += num_points;
572 
574 
575  if (getParentSceneNode())
576  {
577  getParentSceneNode()->needUpdate();
578  }
579 }
580 
581 void PointCloud::popPoints(uint32_t num_points)
582 {
583  uint32_t vpp = getVerticesPerPoint();
584 
585  ROS_ASSERT(num_points <= point_count_);
586  points_.erase(points_.begin(), points_.begin() + num_points);
587 
588  point_count_ -= num_points;
589 
590  // Now clear out popped points
591  uint32_t popped_count = 0;
592  while (popped_count < num_points * vpp)
593  {
594  PointCloudRenderablePtr rend = renderables_.front();
595  Ogre::RenderOperation* op = rend->getRenderOperation();
596 
597  uint32_t popped = std::min((size_t)(num_points * vpp - popped_count), op->vertexData->vertexCount);
598  op->vertexData->vertexStart += popped;
599  op->vertexData->vertexCount -= popped;
600 
601  popped_count += popped;
602 
603  if (op->vertexData->vertexCount == 0)
604  {
605  renderables_.erase(renderables_.begin(), renderables_.begin() + 1);
606 
607  op->vertexData->vertexStart = 0;
608  renderables_.push_back(rend);
609  }
610  }
611  ROS_ASSERT(popped_count == num_points * vpp);
612 
613  // reset bounds
614  bounding_box_.setNull();
615  bounding_radius_ = 0.0f;
616  for (uint32_t i = 0; i < point_count_; ++i)
617  {
618  Point& p = points_[i];
619  bounding_box_.merge(p.position);
620  bounding_radius_ = std::max(bounding_radius_, p.position.squaredLength());
621  }
622 
624 
625  if (getParentSceneNode())
626  {
627  getParentSceneNode()->needUpdate();
628  }
629 }
630 
632 {
633  while (!renderables_.empty())
634  {
636  Ogre::RenderOperation* op = rend->getRenderOperation();
637  if (op->vertexData->vertexCount == 0)
638  {
639  renderables_.pop_back();
640  }
641  else
642  {
643  break;
644  }
645  }
646 }
647 
648 void PointCloud::_updateRenderQueue(Ogre::RenderQueue* queue)
649 {
650  V_PointCloudRenderable::iterator it = renderables_.begin();
651  V_PointCloudRenderable::iterator end = renderables_.end();
652  for (; it != end; ++it)
653  {
654  queue->addRenderable((*it).get());
655  }
656 }
657 
658 void PointCloud::_notifyAttached(Ogre::Node* parent, bool isTagPoint)
659 {
660  MovableObject::_notifyAttached(parent, isTagPoint);
661 }
662 
664 {
666  {
667  return 1;
668  }
669 
670  if (render_mode_ == RM_POINTS)
671  {
672  return 1;
673  }
674 
675  if (render_mode_ == RM_SQUARES)
676  {
677  return 6;
678  }
679 
681  {
682  return 6;
683  }
684 
685  if (render_mode_ == RM_TILES)
686  {
687  return 6;
688  }
689 
690  if (render_mode_ == RM_SPHERES)
691  {
692  return 3;
693  }
694 
695  if (render_mode_ == RM_BOXES)
696  {
697  return 36;
698  }
699 
700  return 1;
701 }
702 
703 void PointCloud::setPickColor(const Ogre::ColourValue& color)
704 {
705  pick_color_ = color;
706  Ogre::Vector4 pick_col(pick_color_.r, pick_color_.g, pick_color_.b, pick_color_.a);
707 
708  V_PointCloudRenderable::iterator it = renderables_.begin();
709  V_PointCloudRenderable::iterator end = renderables_.end();
710  for (; it != end; ++it)
711  {
712  (*it)->setCustomParameter(PICK_COLOR_PARAMETER, pick_col);
713  }
714  getUserObjectBindings().setUserAny("pick_handle", Ogre::Any(colorToHandle(color)));
715 }
716 
718 {
722  Ogre::Vector4 size(width_, height_, depth_, 0.0f);
723  Ogre::Vector4 alpha(alpha_, 0.0f, 0.0f, 0.0f);
724  Ogre::Vector4 highlight(0.0f, 0.0f, 0.0f, 0.0f);
725  Ogre::Vector4 pick_col(pick_color_.r, pick_color_.g, pick_color_.b, pick_color_.a);
726  rend->setCustomParameter(SIZE_PARAMETER, size);
727  rend->setCustomParameter(ALPHA_PARAMETER, alpha);
728  rend->setCustomParameter(HIGHLIGHT_PARAMETER, highlight);
729  rend->setCustomParameter(PICK_COLOR_PARAMETER, pick_col);
730  rend->setCustomParameter(NORMAL_PARAMETER, Ogre::Vector4(common_direction_));
731  rend->setCustomParameter(UP_PARAMETER, Ogre::Vector4(common_up_vector_));
732  if (getParentSceneNode())
733  {
734  getParentSceneNode()->attachObject(rend.get());
735  }
736  renderables_.push_back(rend);
737 
738  return rend;
739 }
740 
741 void PointCloud::visitRenderables(Ogre::Renderable::Visitor* /*visitor*/, bool /*debugRenderables*/)
742 {
743 }
744 
745 
749 
750 PointCloudRenderable::PointCloudRenderable(PointCloud* parent, int num_points, bool use_tex_coords)
751  : parent_(parent)
752 {
753  // Initialize render operation
754  mRenderOp.operationType = Ogre::RenderOperation::OT_POINT_LIST;
755  mRenderOp.useIndexes = false;
756  mRenderOp.vertexData = new Ogre::VertexData;
757  mRenderOp.vertexData->vertexStart = 0;
758  mRenderOp.vertexData->vertexCount = 0;
759 
760  Ogre::VertexDeclaration* decl = mRenderOp.vertexData->vertexDeclaration;
761  size_t offset = 0;
762 
763  decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_POSITION);
764  offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
765 
766  if (use_tex_coords)
767  {
768  decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_TEXTURE_COORDINATES, 0);
769  offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
770  }
771 
772  decl->addElement(0, offset, Ogre::VET_COLOUR, Ogre::VES_DIFFUSE);
773 
774  Ogre::HardwareVertexBufferSharedPtr vbuf =
775  Ogre::HardwareBufferManager::getSingleton().createVertexBuffer(
776  mRenderOp.vertexData->vertexDeclaration->getVertexSize(0), num_points,
777  Ogre::HardwareBuffer::HBU_DYNAMIC);
778 
779  // Bind buffer
780  mRenderOp.vertexData->vertexBufferBinding->setBinding(0, vbuf);
781 }
782 
784 {
785  delete mRenderOp.vertexData;
786  delete mRenderOp.indexData;
787 }
788 
789 Ogre::HardwareVertexBufferSharedPtr PointCloudRenderable::getBuffer()
790 {
791  return mRenderOp.vertexData->vertexBufferBinding->getBuffer(0);
792 }
793 
795 {
796  return Ogre::Math::Sqrt(std::max(mBox.getMaximum().squaredLength(), mBox.getMinimum().squaredLength()));
797 }
798 
799 Ogre::Real PointCloudRenderable::getSquaredViewDepth(const Ogre::Camera* cam) const
800 {
801  Ogre::Vector3 vMin, vMax, vMid, vDist;
802  vMin = mBox.getMinimum();
803  vMax = mBox.getMaximum();
804  vMid = ((vMax - vMin) * 0.5) + vMin;
805  vDist = cam->getDerivedPosition() - vMid;
806 
807  return vDist.squaredLength();
808 }
809 
810 void PointCloudRenderable::getWorldTransforms(Ogre::Matrix4* xform) const
811 {
812  parent_->getWorldTransforms(xform);
813 }
814 
815 const Ogre::LightList& PointCloudRenderable::getLights() const
816 {
817  return parent_->queryLights();
818 }
819 
820 } // namespace rviz
rviz::PointCloud
A visual representation of a set of points.
Definition: point_cloud.h:108
PICK_COLOR_PARAMETER
#define PICK_COLOR_PARAMETER
Definition: custom_parameter_indices.h:43
rviz::PointCloud::regenerateAll
void regenerateAll()
Definition: point_cloud.cpp:184
Ogre
Definition: axes_display.h:35
rviz::PointCloud::setAlpha
void setAlpha(float alpha, bool per_point_alpha=false)
Definition: point_cloud.cpp:368
compatibility.h
rviz::PointCloud::point_count_
uint32_t point_count_
The number of points currently in points_.
Definition: point_cloud.h:224
rviz::PointCloud::width_
float width_
width
Definition: point_cloud.h:227
boost::shared_ptr
rviz::g_billboard_sphere_vertices
static float g_billboard_sphere_vertices[3 *3]
Definition: point_cloud.cpp:64
forwards.h
rviz::PointCloud::addPoints
void addPoints(Point *points, uint32_t num_points)
Add points to this point cloud.
Definition: point_cloud.cpp:400
rviz::PointCloud::depth_
float depth_
depth
Definition: point_cloud.h:229
rviz::PointCloud::render_mode_
RenderMode render_mode_
Definition: point_cloud.h:226
rviz::g_point_vertices
static float g_point_vertices[3]
Definition: point_cloud.cpp:57
rviz::PointCloud::sphere_material_
Ogre::MaterialPtr sphere_material_
Definition: point_cloud.h:236
custom_parameter_indices.h
rviz::PointCloud::getWorldTransforms
virtual void getWorldTransforms(Ogre::Matrix4 *xform) const
Definition: point_cloud.cpp:159
AUTO_SIZE_PARAMETER
#define AUTO_SIZE_PARAMETER
Definition: custom_parameter_indices.h:47
point_cloud.h
rviz::PointCloud::visitRenderables
void visitRenderables(Ogre::Renderable::Visitor *visitor, bool debugRenderables) override
Definition: point_cloud.cpp:741
SIZE_PARAMETER
#define SIZE_PARAMETER
Definition: custom_parameter_indices.h:41
rviz::PointCloud::height_
float height_
height
Definition: point_cloud.h:228
rviz::PointCloud::pick_color_
Ogre::ColourValue pick_color_
Definition: point_cloud.h:247
rviz::setMaterial
void setMaterial(Ogre::SimpleRenderable &renderable, const std::string &material_name)
Definition: compatibility.h:69
rviz::PointCloud::bounding_box_
Ogre::AxisAlignedBox bounding_box_
The bounding box of this point cloud.
Definition: point_cloud.h:219
rviz::PointCloud::RM_POINTS
@ RM_POINTS
Definition: point_cloud.h:113
rviz::PointCloud::RM_SPHERES
@ RM_SPHERES
Definition: point_cloud.h:116
rviz::PointCloud::popPoints
void popPoints(uint32_t num_points)
Remove a number of points from this point cloud.
Definition: point_cloud.cpp:581
f
f
rviz::PointCloud::PointCloud
PointCloud()
Definition: point_cloud.cpp:92
rviz::PointCloudRenderable::PointCloudRenderable
PointCloudRenderable(PointCloud *parent, int num_points, bool use_tex_coords)
Definition: point_cloud.cpp:750
rviz::PointCloud::Point
Representation of a point, with x/y/z position and r/g/b color.
Definition: point_cloud.h:133
rviz::PointCloud::points_
V_Point points_
The list of points we're displaying. Allocates to a high-water-mark.
Definition: point_cloud.h:223
rviz::setAlphaBlending
void setAlphaBlending(const Ogre::MaterialPtr &mat)
Definition: point_cloud.cpp:350
rviz::PointCloudRenderable::parent_
PointCloud * parent_
Definition: point_cloud.h:90
rviz::PointCloud::setAutoSize
void setAutoSize(bool auto_size)
Definition: point_cloud.cpp:316
rviz::PointCloud::RM_SQUARES
@ RM_SQUARES
Definition: point_cloud.h:114
rviz::PointCloudRenderable::getWorldTransforms
void getWorldTransforms(Ogre::Matrix4 *xform) const override
Definition: point_cloud.cpp:810
rviz::PointCloud::_notifyAttached
void _notifyAttached(Ogre::Node *parent, bool isTagPoint=false) override
Definition: point_cloud.cpp:658
rviz::PointCloud::sm_Type
static Ogre::String sm_Type
The "renderable type" used by Ogre.
Definition: point_cloud.h:249
rviz::setReplace
void setReplace(const Ogre::MaterialPtr &mat)
Definition: point_cloud.cpp:359
rviz::PointCloud::Point::color
Ogre::ColourValue color
Definition: point_cloud.h:141
rviz
Definition: add_display_dialog.cpp:54
rviz::PointCloud::shrinkRenderables
void shrinkRenderables()
Definition: point_cloud.cpp:631
ALPHA_PARAMETER
#define ALPHA_PARAMETER
Definition: custom_parameter_indices.h:42
rviz::PointCloud::getBoundingRadius
float getBoundingRadius() const override
Definition: point_cloud.cpp:154
Vector3
rviz::PointCloud::current_material_
Ogre::MaterialPtr current_material_
Definition: point_cloud.h:239
ogre_vector.h
rviz::PointCloud::box_material_
Ogre::MaterialPtr box_material_
Definition: point_cloud.h:238
rviz::PointCloud::current_mode_supports_geometry_shader_
bool current_mode_supports_geometry_shader_
Definition: point_cloud.h:246
rviz::PointCloud::setPickColor
void setPickColor(const Ogre::ColourValue &color)
Definition: point_cloud.cpp:703
rviz::PointCloud::bounding_radius_
float bounding_radius_
The bounding radius of this point cloud.
Definition: point_cloud.h:220
rviz::PointCloud::~PointCloud
~PointCloud() override
Definition: point_cloud.cpp:137
rviz::PointCloud::tile_material_
Ogre::MaterialPtr tile_material_
Definition: point_cloud.h:237
rviz::PointCloud::setDimensions
void setDimensions(float width, float height, float depth)
Set the dimensions of the billboards used to render each point.
Definition: point_cloud.cpp:300
VERTEX_BUFFER_CAPACITY
#define VERTEX_BUFFER_CAPACITY
Definition: point_cloud.cpp:53
rviz::PointCloud::setRenderMode
void setRenderMode(RenderMode mode)
Set what type of rendering primitives should be used, currently points, billboards and boxes are supp...
Definition: point_cloud.cpp:218
rviz::PointCloudRenderable
Definition: point_cloud.h:64
rviz::PointCloudRenderable::getBoundingRadius
Ogre::Real getBoundingRadius() const override
Definition: point_cloud.cpp:794
set
ROSCPP_DECL void set(const std::string &key, bool b)
rviz::PointCloud::getVerticesPerPoint
uint32_t getVerticesPerPoint()
Definition: point_cloud.cpp:663
rviz::PointCloud::RM_TILES
@ RM_TILES
Definition: point_cloud.h:117
rviz::PointCloud::color_by_index_
bool color_by_index_
Definition: point_cloud.h:242
rviz::PointCloud::point_material_
Ogre::MaterialPtr point_material_
Definition: point_cloud.h:233
rviz::PointCloud::alpha_
float alpha_
Definition: point_cloud.h:240
rviz::PointCloud::Point::position
Ogre::Vector3 position
Definition: point_cloud.h:140
rviz::PointCloud::common_up_vector_
Ogre::Vector3 common_up_vector_
See Ogre::BillboardSet::setCommonUpVector.
Definition: point_cloud.h:231
rviz::PointCloudRenderable::getBuffer
Ogre::HardwareVertexBufferSharedPtr getBuffer()
Definition: point_cloud.cpp:789
rviz::PointCloud::clear
void clear()
Clear all the points.
Definition: point_cloud.cpp:164
rviz::PointCloud::setColorByIndex
void setColorByIndex(bool set)
Definition: point_cloud.cpp:200
rviz::PointCloud::flat_square_material_
Ogre::MaterialPtr flat_square_material_
Definition: point_cloud.h:235
ROS_ERROR
#define ROS_ERROR(...)
UP_PARAMETER
#define UP_PARAMETER
Definition: custom_parameter_indices.h:45
rviz::PointCloud::RM_BOXES
@ RM_BOXES
Definition: point_cloud.h:118
rviz::PointCloudRenderable::getLights
const Ogre::LightList & getLights() const override
Definition: point_cloud.cpp:815
NORMAL_PARAMETER
#define NORMAL_PARAMETER
Definition: custom_parameter_indices.h:44
rviz::PointCloudRenderable::~PointCloudRenderable
~PointCloudRenderable() override
Definition: point_cloud.cpp:783
rviz::PointCloud::setHighlightColor
void setHighlightColor(float r, float g, float b)
Definition: point_cloud.cpp:206
rviz::PointCloud::createRenderable
PointCloudRenderablePtr createRenderable(int num_points)
Definition: point_cloud.cpp:717
rviz::PointCloud::getBoundingBox
const Ogre::AxisAlignedBox & getBoundingBox() const override
Definition: point_cloud.cpp:149
rviz::PointCloud::common_direction_
Ogre::Vector3 common_direction_
See Ogre::BillboardSet::setCommonDirection.
Definition: point_cloud.h:230
rviz::g_box_vertices
static float g_box_vertices[6 *6 *3]
Definition: point_cloud.cpp:68
root
root
assert.h
HIGHLIGHT_PARAMETER
#define HIGHLIGHT_PARAMETER
Definition: custom_parameter_indices.h:46
rviz::PointCloud::square_material_
Ogre::MaterialPtr square_material_
Definition: point_cloud.h:234
ROS_ASSERT
#define ROS_ASSERT(cond)
rviz::PointCloud::renderables_
V_PointCloudRenderable renderables_
Definition: point_cloud.h:244
rviz::removeMaterial
static void removeMaterial(Ogre::MaterialPtr &material)
Definition: point_cloud.cpp:131
rviz::colorToHandle
uint32_t colorToHandle(Ogre::PixelFormat fmt, uint32_t col)
Definition: forwards.h:66
rviz::PointCloud::_updateRenderQueue
void _updateRenderQueue(Ogre::RenderQueue *queue) override
Definition: point_cloud.cpp:648
rviz::g_billboard_vertices
static float g_billboard_vertices[6 *3]
Definition: point_cloud.cpp:59
rviz::PointCloud::setCommonDirection
void setCommonDirection(const Ogre::Vector3 &vec)
See Ogre::BillboardSet::setCommonDirection.
Definition: point_cloud.cpp:326
rviz::PointCloud::setCommonUpVector
void setCommonUpVector(const Ogre::Vector3 &vec)
See Ogre::BillboardSet::setCommonUpVector.
Definition: point_cloud.cpp:338
rviz::PointCloud::RenderMode
RenderMode
Definition: point_cloud.h:111
rviz::PointCloud::V_Point
std::vector< Point > V_Point
Definition: point_cloud.h:222
rviz::PointCloud::RM_FLAT_SQUARES
@ RM_FLAT_SQUARES
Definition: point_cloud.h:115
rviz::PointCloudRenderable::getSquaredViewDepth
Ogre::Real getSquaredViewDepth(const Ogre::Camera *cam) const override
Definition: point_cloud.cpp:799


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust, William Woodall
autogenerated on Sat Jun 1 2024 02:31:53