42 #include <OgreSceneNode.h>
43 #include <OgreSceneManager.h>
44 #include <OgreManualObject.h>
45 #include <OgreMaterialManager.h>
53 #include <boost/lexical_cast.hpp>
54 #include <boost/math/constants/constants.hpp>
80 Eigen::Vector3d translation = p.translation();
81 Ogre::Vector3 position(translation.x(), translation.y(), translation.z());
83 Eigen::Quaterniond
q(p.linear());
84 Ogre::Quaternion orientation(
q.w(),
q.x(),
q.y(),
q.z());
98 const double* sz =
static_cast<const shapes::Box*
>(
s)->size;
99 ogre_shape->
setScale(Ogre::Vector3(sz[0], sz[1], sz[2]));
122 ogre_shape->
setScale(Ogre::Vector3(10, 10, 0.001));
125 Eigen::Vector3d normal(plane->a, plane->b, plane->c);
126 double norm = normal.norm();
129 Eigen::Quaterniond offset = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitZ(), normal);
130 orientation = orientation * Ogre::Quaternion(offset.w(), offset.x(), offset.y(), offset.z());
131 position += plane->d / norm * Ogre::Vector3(normal.x(), normal.y(), normal.z());
142 Ogre::Vector3 normal(0.0, 0.0, 0.0);
145 unsigned int i3 = i * 3;
153 for (
int k = 0; k < 3; ++k)
155 unsigned int vi = 3 * mesh->
triangles[i3 + k];
163 m->addVertex(v, normal);
175 OcTreeRenderPtr octree(
new OcTreeRender(
static_cast<const shapes::OcTree*
>(s)->octree, octree_voxel_rendering,
176 octree_color_mode, 0u, node));
177 octree->setPosition(position);
178 octree->setOrientation(orientation);
195 static Ogre::Quaternion fix(Ogre::Radian(boost::math::constants::pi<double>() / 2.0),
196 Ogre::Vector3(1.0, 0.0, 0.0));
197 orientation = orientation * fix;
208 for (
const std::unique_ptr<rviz::Shape>& shape :
scene_shapes_)
209 shape->setColor(r, g, b, a);