billboard_line.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "billboard_line.h"

#include <OgreSceneManager.h>
#include <OgreSceneNode.h>
#include <OgreVector3.h>
#include <OgreQuaternion.h>
#include <OgreBillboardChain.h>
#include <OgreMaterialManager.h>
#include <OgreTechnique.h>

#include <sstream>

#include <ros/assert.h>

#define MAX_ELEMENTS (65536 / 4)

namespace rviz
{
BillboardLine::BillboardLine(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node)
  : Object(scene_manager)
  , width_(0.1f)
  , current_line_(0)
  , total_elements_(0)
  , num_lines_(1)
  , max_points_per_line_(100)
  , lines_per_chain_(0)
  , current_chain_(0)
  , elements_in_current_chain_(0)
{
  if (!parent_node)
  {
    parent_node = scene_manager_->getRootSceneNode();
  }

  scene_node_ = parent_node->createChildSceneNode();

  static int count = 0;
  std::stringstream ss;
  ss << "BillboardLineMaterial" << count++;
  material_ = Ogre::MaterialManager::getSingleton().create(
      ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
  material_->setReceiveShadows(false);
  material_->getTechnique(0)->setLightingEnabled(false);

  setNumLines(num_lines_);
}

BillboardLine::~BillboardLine()
{
  V_Chain::iterator it = chains_.begin();
  V_Chain::iterator end = chains_.end();
  for (; it != end; ++it)
  {
    scene_manager_->destroyBillboardChain(*it);
  }

  scene_manager_->destroySceneNode(scene_node_->getName());

  Ogre::MaterialManager::getSingleton().remove(material_->getName());
}

Ogre::BillboardChain* BillboardLine::createChain()
{
  std::stringstream ss;
  static int count = 0;
  ss << "BillboardLine chain" << count++;
  Ogre::BillboardChain* chain = scene_manager_->createBillboardChain(ss.str());
  chain->setMaterialName(material_->getName());
  scene_node_->attachObject(chain);

  chains_.push_back(chain);

  return chain;
}

void BillboardLine::clear()
{
  V_Chain::iterator it = chains_.begin();
  V_Chain::iterator end = chains_.end();
  for (; it != end; ++it)
  {
    (*it)->clearAllChains();
  }

  current_line_ = 0;
  total_elements_ = 0;
  current_chain_ = 0;
  elements_in_current_chain_ = 0;

  for (V_uint32::iterator it = num_elements_.begin(); it != num_elements_.end(); ++it)
  {
    *it = 0;
  }
}

void BillboardLine::setupChains()
{
  uint32_t total_points = max_points_per_line_ * num_lines_;
  uint32_t num_chains = total_points / MAX_ELEMENTS;
  if (total_points % MAX_ELEMENTS != 0)
  {
    ++num_chains;
  }

  for (uint32_t i = chains_.size(); i < num_chains; ++i)
  {
    createChain();
  }

  lines_per_chain_ = max_points_per_line_ > 0 ? MAX_ELEMENTS / max_points_per_line_ : 1;

  V_Chain::iterator it = chains_.begin();
  V_Chain::iterator end = chains_.end();
  for (; it != end; ++it)
  {
    (*it)->setMaxChainElements(max_points_per_line_);

    // shorten the number of chains in the last bbchain, to avoid memory wasteage
    if (it + 1 == end)
    {
      uint32_t lines_left = num_lines_ % lines_per_chain_;

      // Handle the case where num_lines_ is a multiple of lines_per_chain
      if (lines_left == 0)
      {
        (*it)->setNumberOfChains(lines_per_chain_);
      }
      else
      {
        (*it)->setNumberOfChains(lines_left);
      }
    }
    else
    {
      (*it)->setNumberOfChains(lines_per_chain_);
    }
  }
}

void BillboardLine::setMaxPointsPerLine(uint32_t max)
{
  if (max <= MAX_ELEMENTS)
  {
    max_points_per_line_ = max;
    setupChains();
  }
  else // need to split points across several lines
  {
    ROS_ASSERT(num_lines_ == 1);
    max_points_per_line_ = MAX_ELEMENTS;
    num_lines_ = max / MAX_ELEMENTS;
    if (max % MAX_ELEMENTS != 0)
      ++num_lines_;
    setNumLines(num_lines_);
  }
}

void BillboardLine::setNumLines(uint32_t num)
{
  num_lines_ = num;

  setupChains();

  num_elements_.resize(num);

  for (V_uint32::iterator it = num_elements_.begin(); it != num_elements_.end(); ++it)
  {
    *it = 0;
  }
}

void BillboardLine::newLine()
{
  ++current_line_;

  ROS_ASSERT(current_line_ < num_lines_);
}

void BillboardLine::addPoint(const Ogre::Vector3& point)
{
  addPoint(point, color_);
}

void BillboardLine::addPoint(const Ogre::Vector3& point, const Ogre::ColourValue& color)
{
  ++elements_in_current_chain_;
  if (elements_in_current_chain_ > MAX_ELEMENTS)
  {
    ++current_chain_;
    elements_in_current_chain_ = 1;

    if (max_points_per_line_ == MAX_ELEMENTS)
      newLine();
  }

  ++num_elements_[current_line_];
  ++total_elements_;

  ROS_ASSERT(num_elements_[current_line_] <= max_points_per_line_);

  Ogre::BillboardChain::Element e;
  e.position = point;
  e.width = width_;
  e.colour = color;
  chains_[current_chain_]->addChainElement(current_line_ % lines_per_chain_, e);
}

void BillboardLine::setLineWidth(float width)
{
  width_ = width;

  for (uint32_t line = 0; line < num_lines_; ++line)
  {
    uint32_t element_count = num_elements_[line];

    for (uint32_t i = 0; i < element_count; ++i)
    {
      Ogre::BillboardChain* c = chains_[line / lines_per_chain_];
      Ogre::BillboardChain::Element e = c->getChainElement(line % lines_per_chain_, i);

      e.width = width_;
      c->updateChainElement(line % lines_per_chain_, i, e);
    }
  }
}

void BillboardLine::setPosition(const Ogre::Vector3& position)
{
  scene_node_->setPosition(position);
}

void BillboardLine::setOrientation(const Ogre::Quaternion& orientation)
{
  scene_node_->setOrientation(orientation);
}

void BillboardLine::setScale(const Ogre::Vector3& /*scale*/)
{
  // Setting scale doesn't really make sense here
}

void BillboardLine::setColor(float r, float g, float b, float a)
{
  if (a < 0.9998)
  {
    material_->getTechnique(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
    material_->getTechnique(0)->setDepthWriteEnabled(false);
  }
  else
  {
    material_->getTechnique(0)->setSceneBlending(Ogre::SBT_REPLACE);
    material_->getTechnique(0)->setDepthWriteEnabled(true);
  }

  color_ = Ogre::ColourValue(r, g, b, a);

  for (uint32_t line = 0; line < num_lines_; ++line)
  {
    uint32_t element_count = num_elements_[line];

    for (uint32_t i = 0; i < element_count; ++i)
    {
      Ogre::BillboardChain* c = chains_[line / lines_per_chain_];
      Ogre::BillboardChain::Element e = c->getChainElement(line % lines_per_chain_, i);

      e.colour = color_;
      c->updateChainElement(line % lines_per_chain_, i, e);
    }
  }
}

const Ogre::Vector3& BillboardLine::getPosition()
{
  return scene_node_->getPosition();
}

const Ogre::Quaternion& BillboardLine::getOrientation()
{
  return scene_node_->getOrientation();
}

} // namespace rviz