DebugDrawerTopic.cpp

Go to the documentation of this file.

#include "DebugDrawerTopic.h"
 
#include <SimoxUtility/color/hsv.h>
#include <VirtualRobot/Visualization/TriMeshModel.h>
#include <VirtualRobot/math/Helpers.h>
 
#include <ArmarXCore/core/ManagedIceObject.h>
 
#include "../Pose.h"
#include "GlasbeyLUT.h"
 
namespace armarx
{
 
    DebugDrawerTopic::VisuID::VisuID() : VisuID("", "")
    {
    }
 
    DebugDrawerTopic::VisuID::VisuID(const std::string& name) : VisuID("", name)
    {
    }
 
    DebugDrawerTopic::VisuID::VisuID(const std::string& layer, const std::string& name) :
        layer(layer), name(name)
    {
    }
 
    DebugDrawerTopic::VisuID
    DebugDrawerTopic::VisuID::withName(const std::string& newName) const
    {
        return VisuID(this->layer, newName);
    }
 
    std::ostream&
    operator<<(std::ostream& os, const DebugDrawerTopic::VisuID& rhs)
    {
        os << "Visu '" << rhs.name << "' on layer '" << rhs.layer << "'";
        return os;
    }
 
    const std::string DebugDrawerTopic::TOPIC_NAME = "DebugDrawerUpdates";
    const std::string DebugDrawerTopic::DEFAULT_LAYER = "debug";
    const DebugDrawerTopic::Defaults DebugDrawerTopic::DEFAULTS = {};
 
    DebugDrawerTopic::DebugDrawerTopic(const std::string& layer) : _layer(layer)
    {
    }
 
    DebugDrawerTopic::DebugDrawerTopic(const DebugDrawerInterfacePrx& topic,
                                       const std::string& layer) :
        topic(topic), _layer(layer)
    {
    }
 
    void
    DebugDrawerTopic::setTopic(const DebugDrawerInterfacePrx& topic)
    {
        this->topic = topic;
    }
 
    DebugDrawerInterfacePrx
    DebugDrawerTopic::getTopic() const
    {
        return topic;
    }
 
    void
    DebugDrawerTopic::setEnabled(bool enabled)
    {
        this->_enabled = enabled;
    }
 
    bool
    DebugDrawerTopic::enabled() const
    {
        return _enabled && topic;
    }
 
    void
    DebugDrawerTopic::offeringTopic(ManagedIceObject& component,
                                    const std::string& topicNameOverride) const
    {
        component.offeringTopic(topicNameOverride.empty() ? TOPIC_NAME : topicNameOverride);
    }
 
    void
    DebugDrawerTopic::getTopic(ManagedIceObject& component, const std::string& topicNameOverride)
    {
        setTopic(component.getTopic<DebugDrawerInterfacePrx>(
            topicNameOverride.empty() ? TOPIC_NAME : topicNameOverride));
    }
 
    const std::string&
    DebugDrawerTopic::getLayer() const
    {
        return _layer;
    }
 
    void
    DebugDrawerTopic::setLayer(const std::string& layer)
    {
        this->_layer = layer;
    }
 
    float
    DebugDrawerTopic::getLengthScale() const
    {
        return _lengthScale;
    }
 
    void
    DebugDrawerTopic::setLengthScale(float scale)
    {
        this->_lengthScale = scale;
    }
 
    void
    DebugDrawerTopic::setLengthScaleMetersToMillimeters()
    {
        setLengthScale(1000);
    }
 
    void
    DebugDrawerTopic::setLengthScaleMillimetersToMeters()
    {
        setLengthScale(0.001f);
    }
 
    float
    DebugDrawerTopic::getPoseScale() const
    {
        return _poseScale;
    }
 
    void
    DebugDrawerTopic::setPoseScale(float scale)
    {
        this->_poseScale = scale;
    }
 
    void
    DebugDrawerTopic::setPoseScaleMeters()
    {
        setPoseScale(0.001f);
    }
 
    void
    DebugDrawerTopic::setPoseScaleMillimeters()
    {
        setPoseScale(1);
    }
 
    void
    DebugDrawerTopic::shortSleep()
    {
        this->sleepFor(_shortSleepDuration);
    }
 
    void
    DebugDrawerTopic::clearAll(bool sleep)
    {
        if (enabled())
        {
            topic->clearAll();
            if (sleep)
            {
                shortSleep();
            }
        }
    }
 
    void
    DebugDrawerTopic::clearLayer(bool sleep)
    {
        clearLayer(_layer, sleep);
    }
 
    void
    DebugDrawerTopic::clearLayer(const std::string& layer, bool sleep)
    {
        if (enabled())
        {
            topic->clearLayer(layer);
            if (sleep)
            {
                shortSleep();
            }
        }
    }
 
    void
    DebugDrawerTopic::drawText(const VisuID& id,
                               const Eigen::Vector3f& position,
                               const std::string& text,
                               int size,
                               const DrawColor color,
                               bool ignoreLengthScale)
    {
        if (enabled())
        {
            const float scale = lengthScale(ignoreLengthScale);
            topic->setTextVisu(layer(id), id.name, text, scaled(scale, position), color, size);
        }
    }
 
    void
    DebugDrawerTopic::drawBox(const VisuID& id,
                              const Eigen::Vector3f& position,
                              const Eigen::Quaternionf& orientation,
                              const Eigen::Vector3f& extents,
                              const DrawColor& color,
                              bool ignoreLengthScale)
    {
        if (enabled())
        {
            const float scale = lengthScale(ignoreLengthScale);
            topic->setBoxVisu(layer(id),
                              id.name,
                              new Pose(scaled(scale, position), new Quaternion(orientation)),
                              scaled(scale, extents),
                              color);
        }
    }
 
    void
    DebugDrawerTopic::drawBox(const VisuID& id,
                              const Eigen::Matrix4f& pose,
                              const Eigen::Vector3f& extents,
                              const DrawColor& color,
                              bool ignoreLengthScale)
    {
        drawBox(id,
                ::math::Helpers::Position(pose),
                Eigen::Quaternionf(::math::Helpers::Orientation(pose)),
                extents,
                color,
                ignoreLengthScale);
    }
 
    void
    DebugDrawerTopic::drawBox(const DebugDrawerTopic::VisuID& id,
                              const VirtualRobot::BoundingBox& boundingBox,
                              const DrawColor& color,
                              bool ignoreLengthScale)
    {
        drawBox(id, boundingBox, Eigen::Matrix4f::Identity(), color, ignoreLengthScale);
    }
 
    void
    DebugDrawerTopic::drawBox(const DebugDrawerTopic::VisuID& id,
                              const VirtualRobot::BoundingBox& boundingBox,
                              const Eigen::Matrix4f& pose,
                              const DrawColor& color,
                              bool ignoreLengthScale)
    {
        const Eigen::Vector3f center = .5 * (boundingBox.getMin() + boundingBox.getMax());
        drawBox(id,
                ::math::Helpers::TransformPosition(pose, center),
                Eigen::Quaternionf(::math::Helpers::Orientation(pose)),
                boundingBox.getMax() - boundingBox.getMin(),
                color,
                ignoreLengthScale);
    }
 
    void
    DebugDrawerTopic::removeBox(const DebugDrawerTopic::VisuID& id)
    {
        if (enabled())
        {
            topic->removeBoxVisu(layer(id), id.name);
        }
    }
 
    void
    DebugDrawerTopic::drawBoxEdges(const DebugDrawerTopic::VisuID& id,
                                   const Eigen::Vector3f& position,
                                   const Eigen::Quaternionf& orientation,
                                   const Eigen::Vector3f& extents,
                                   float width,
                                   const DrawColor& color,
                                   bool ignoreLengthScale)
    {
        drawBoxEdges(id,
                     ::math::Helpers::Pose(position, orientation),
                     extents,
                     width,
                     color,
                     ignoreLengthScale);
    }
 
    void
    DebugDrawerTopic::drawBoxEdges(const DebugDrawerTopic::VisuID& id,
                                   const Eigen::Matrix4f& pose,
                                   const Eigen::Vector3f& extents,
                                   float width,
                                   const DrawColor& color,
                                   bool ignoreLengthScale)
    {
        if (!enabled())
        {
            return;
        }
 
        std::vector<Eigen::Vector3f> points;
 
        Eigen::Matrix<float, 3, 2> bb;
        bb.col(0) = -extents / 2;
        bb.col(1) = extents / 2;
 
        auto addLine = [&](int x1, int y1, int z1, int x2, int y2, int z2)
        {
            Eigen::Vector3f start = {bb.col(x1).x(), bb.col(y1).y(), bb.col(z1).z()};
            Eigen::Vector3f end = {bb.col(x2).x(), bb.col(y2).y(), bb.col(z2).z()};
 
            start = ::math::Helpers::TransformPosition(pose, start);
            end = ::math::Helpers::TransformPosition(pose, end);
 
            points.push_back(start);
            points.push_back(end);
        };
 
        /*   001 +-----+ 011
         *      /| 111/|
         * 101 +-----+ |
         *     | +---|-+ 010
         *     |/000 |/
         * 100 +-----+ 110
         */
 
        // 000 -> 100, 010, 001
        addLine(0, 0, 0, 1, 0, 0);
        addLine(0, 0, 0, 0, 1, 0);
        addLine(0, 0, 0, 0, 0, 1);
 
        // 111 -> 011, 101, 110
        addLine(1, 1, 1, 0, 1, 1);
        addLine(1, 1, 1, 1, 0, 1);
        addLine(1, 1, 1, 1, 1, 0);
 
        // 100 -> 101, 110
        addLine(1, 0, 0, 1, 0, 1);
        addLine(1, 0, 0, 1, 1, 0);
 
        // 010 -> 110, 011
        addLine(0, 1, 0, 1, 1, 0);
        addLine(0, 1, 0, 0, 1, 1);
 
        // 001 -> 101, 011
        addLine(0, 0, 1, 1, 0, 1);
        addLine(0, 0, 1, 0, 1, 1);
 
        drawLineSet(id, points, width, color, ignoreLengthScale);
    }
 
    void
    DebugDrawerTopic::drawBoxEdges(const DebugDrawerTopic::VisuID& id,
                                   const VirtualRobot::BoundingBox& boundingBox,
                                   float width,
                                   const DrawColor& color,
                                   bool ignoreLengthScale)
    {
        drawBoxEdges(id, boundingBox, Eigen::Matrix4f::Identity(), width, color, ignoreLengthScale);
    }
 
    void
    DebugDrawerTopic::drawBoxEdges(const DebugDrawerTopic::VisuID& id,
                                   const Eigen::Matrix32f& aabb,
                                   float width,
                                   const DrawColor& color,
                                   bool ignoreLengthScale)
    {
        drawBoxEdges(id, aabb, Eigen::Matrix4f::Identity(), width, color, ignoreLengthScale);
    }
 
    void
    DebugDrawerTopic::drawBoxEdges(const DebugDrawerTopic::VisuID& id,
                                   const VirtualRobot::BoundingBox& boundingBox,
                                   const Eigen::Matrix4f& pose,
                                   float width,
                                   const DrawColor& color,
                                   bool ignoreLengthScale)
    {
        const Eigen::Vector3f center = .5 * (boundingBox.getMin() + boundingBox.getMax());
 
        drawBoxEdges(id,
                     ::math::Helpers::Pose(::math::Helpers::TransformPosition(pose, center),
                                           ::math::Helpers::Orientation(pose)),
                     boundingBox.getMax() - boundingBox.getMin(),
                     width,
                     color,
                     ignoreLengthScale);
    }
 
    void
    DebugDrawerTopic::drawBoxEdges(const DebugDrawerTopic::VisuID& id,
                                   const Eigen::Matrix32f& aabb,
                                   const Eigen::Matrix4f& pose,
                                   float width,
                                   const DrawColor& color,
                                   bool ignoreLengthScale)
    {
        const Eigen::Vector3f center = 0.5 * (aabb.col(0) + aabb.col(1));
 
        drawBoxEdges(id,
                     ::math::Helpers::Pose(::math::Helpers::TransformPosition(pose, center),
                                           ::math::Helpers::Orientation(pose)),
                     aabb.col(1) - aabb.col(0),
                     width,
                     color,
                     ignoreLengthScale);
    }
 
    void
    DebugDrawerTopic::removeboxEdges(const DebugDrawerTopic::VisuID& id)
    {
        removeLineSet(id);
    }
 
    void
    DebugDrawerTopic::drawCylinder(const DebugDrawerTopic::VisuID& id,
                                   const Eigen::Vector3f& center,
                                   const Eigen::Vector3f& direction,
                                   float length,
                                   float radius,
                                   const DrawColor& color,
                                   bool ignoreLengthScale)
    {
        if (enabled())
        {
            const float scale = lengthScale(ignoreLengthScale);
            topic->setCylinderVisu(layer(id),
                                   id.name,
                                   scaled(scale, center),
                                   new Vector3(direction),
                                   scaled(scale, length),
                                   scaled(scale, radius),
                                   color);
        }
    }
 
    void
    DebugDrawerTopic::drawCylinder(const DebugDrawerTopic::VisuID& id,
                                   const Eigen::Vector3f& center,
                                   const Eigen::Quaternionf& orientation,
                                   float length,
                                   float radius,
                                   const DrawColor& color,
                                   bool ignoreLengthScale)
    {
        drawCylinder(id,
                     center,
                     orientation * Eigen::Vector3f::UnitY(),
                     radius,
                     length,
                     color,
                     ignoreLengthScale);
    }
 
    void
    DebugDrawerTopic::drawCylinderFromTo(const DebugDrawerTopic::VisuID& id,
                                         const Eigen::Vector3f& from,
                                         const Eigen::Vector3f& to,
                                         float radius,
                                         const DrawColor& color,
                                         bool ignoreLengthScale)
    {
        if (enabled())
        {
            const Eigen::Vector3f dir = (to - from); // no need for scaling at this point
            const float length = dir.norm();
            drawCylinder(
                id, .5 * (from + to), dir / length, radius, length, color, ignoreLengthScale);
        }
    }
 
    void
    DebugDrawerTopic::removeCylinder(const DebugDrawerTopic::VisuID& id)
    {
        if (enabled())
        {
            topic->removeCylinderVisu(layer(id), id.name);
        }
    }
 
    void
    DebugDrawerTopic::drawSphere(const DebugDrawerTopic::VisuID& id,
                                 const Eigen::Vector3f& center,
                                 float radius,
                                 const DrawColor& color,
                                 bool ignoreLengthScale)
    {
        if (enabled())
        {
            const float scale = lengthScale(ignoreLengthScale);
            topic->setSphereVisu(
                layer(id), id.name, scaled(scale, center), color, scaled(scale, radius));
        }
    }
 
    void
    DebugDrawerTopic::removeSphere(const DebugDrawerTopic::VisuID& id)
    {
        if (enabled())
        {
            topic->removeSphereVisu(layer(id), id.name);
        }
    }
 
    void
    DebugDrawerTopic::drawArrow(const VisuID& id,
                                const Eigen::Vector3f& position,
                                const Eigen::Vector3f& direction,
                                float length,
                                float width,
                                const DrawColor& color,
                                bool ignoreLengthScale)
    {
        if (enabled())
        {
            const float scale = lengthScale(ignoreLengthScale);
            topic->setArrowVisu(layer(id),
                                id.name,
                                scaled(scale, position),
                                new Vector3(direction),
                                color,
                                scaled(scale, length),
                                scaled(scale, width));
        }
    }
 
    void
    DebugDrawerTopic::drawArrowFromTo(const VisuID& id,
                                      const Eigen::Vector3f& from,
                                      const Eigen::Vector3f& to,
                                      float width,
                                      const DrawColor& color,
                                      bool ignoreLengthScale)
    {
        if (enabled())
        {
            const Eigen::Vector3f dir = (to - from); // no need for scaling at this point
            const float length = dir.norm();
            drawArrow(id, from, dir / length, length, width, color, ignoreLengthScale);
        }
    }
 
    void
    DebugDrawerTopic::removeArrow(const DebugDrawerTopic::VisuID& id)
    {
        if (enabled())
        {
            topic->removeArrowVisu(layer(id), id.name);
        }
    }
 
    void
    DebugDrawerTopic::drawPolygon(const VisuID& id,
                                  const std::vector<Eigen::Vector3f>& points,
                                  const DrawColor& colorFace,
                                  float lineWidth,
                                  const DrawColor& colorEdge,
                                  bool ignoreLengthScale)
    {
        if (enabled())
        {
            const float scale = lengthScale(ignoreLengthScale);
 
            PolygonPointList polyPoints;
            polyPoints.reserve(points.size());
 
            for (const auto& point : points)
            {
                polyPoints.push_back(scaled(scale, point));
            }
 
            topic->setPolygonVisu(layer(id), id.name, polyPoints, colorFace, colorEdge, lineWidth);
        }
    }
 
    void
    DebugDrawerTopic::removePolygon(const DebugDrawerTopic::VisuID& id)
    {
        if (enabled())
        {
            topic->removePolygonVisu(layer(id), id.name);
        }
    }
 
    void
    DebugDrawerTopic::drawLine(const VisuID& id,
                               const Eigen::Vector3f& from,
                               const Eigen::Vector3f& to,
                               float width,
                               const DrawColor& color,
                               bool ignoreLengthScale)
    {
        if (enabled())
        {
            const float scale = lengthScale(ignoreLengthScale);
 
            topic->setLineVisu(
                layer(id), id.name, scaled(scale, from), scaled(scale, to), width, color);
        }
    }
 
    void
    DebugDrawerTopic::removeLine(const DebugDrawerTopic::VisuID& id)
    {
        if (enabled())
        {
            topic->removeLineVisu(layer(id), id.name);
        }
    }
 
    void
    DebugDrawerTopic::drawLineSet(const VisuID& id,
                                  const DebugDrawerLineSet& lineSet,
                                  bool ignoreLengthScale)
    {
        if (enabled())
        {
            const float scale = lengthScale(ignoreLengthScale);
 
            if (1.f <= scale && scale <= 1.f)
            {
                // Can use lineSet directly.
                topic->setLineSetVisu(layer(id), id.name, lineSet);
            }
            else
            {
                // Need to scale line set, hence, reconstruct it.
                DebugDrawerLineSet scaledLineSet = lineSet;
                for (auto& point : scaledLineSet.points)
                {
                    scaleXYZ(scale, point);
                }
                topic->setLineSetVisu(layer(id), id.name, scaledLineSet);
            }
        }
    }
 
    void
    DebugDrawerTopic::drawLineSet(const VisuID& id,
                                  const std::vector<Eigen::Vector3f>& points,
                                  float width,
                                  const DrawColor& color,
                                  bool ignoreLengthScale)
    {
        if (enabled())
        {
            const float scale = lengthScale(ignoreLengthScale);
 
            DebugDrawerLineSet lineSet;
            for (const auto& point : points)
            {
                lineSet.points.push_back(scaledT<DebugDrawerPointCloudElement>(scale, point));
            }
 
            lineSet.lineWidth = width;
            lineSet.colorNoIntensity = lineSet.colorFullIntensity = color;
            lineSet.intensities.assign(points.size() / 2, 0.);
 
            topic->setLineSetVisu(layer(id), id.name, lineSet);
        }
    }
 
    void
    DebugDrawerTopic::drawLineSet(const VisuID& id,
                                  const std::vector<Eigen::Vector3f>& points,
                                  float width,
                                  const DrawColor& colorA,
                                  const DrawColor& colorB,
                                  const std::vector<float>& intensitiesB,
                                  bool ignoreLengthScale)
    {
        if (enabled())
        {
            const float scale = lengthScale(ignoreLengthScale);
 
            DebugDrawerLineSet lineSet;
            for (const auto& point : points)
            {
                lineSet.points.push_back(scaledT<DebugDrawerPointCloudElement>(scale, point));
            }
 
            lineSet.lineWidth = width;
            lineSet.colorNoIntensity = colorA;
            lineSet.colorFullIntensity = colorB;
            lineSet.intensities = intensitiesB;
 
            topic->setLineSetVisu(layer(id), id.name, lineSet);
        }
    }
 
    void
    DebugDrawerTopic::removeLineSet(const DebugDrawerTopic::VisuID& id)
    {
        if (enabled())
        {
            topic->removeLineSetVisu(layer(id), id.name);
        }
    }
 
    void
    DebugDrawerTopic::drawPose(const VisuID& id,
                               const Eigen::Matrix4f& pose,
                               bool ignoreLengthScale)
    {
        drawPose(id, pose, _poseScale, ignoreLengthScale);
    }
 
    void
    DebugDrawerTopic::drawPose(const VisuID& id,
                               const Eigen::Vector3f& pos,
                               const Eigen::Quaternionf& ori,
                               bool ignoreLengthScale)
    {
        drawPose(id, ::math::Helpers::Pose(pos, ori), _poseScale, ignoreLengthScale);
    }
 
    void
    DebugDrawerTopic::drawPose(const VisuID& id,
                               const Eigen::Matrix4f& pose,
                               float scale,
                               bool ignoreLengthScale)
    {
        if (enabled())
        {
            const float lenghtScale = lengthScale(ignoreLengthScale);
 
            if (scale >= 1 && scale <= 1) // squelch compiler warning that == is unsafe
            {
                topic->setPoseVisu(layer(id), id.name, scaled(lenghtScale, pose));
            }
            else
            {
                topic->setScaledPoseVisu(layer(id), id.name, scaled(lenghtScale, pose), scale);
            }
        }
    }
 
    void
    DebugDrawerTopic::drawPose(const VisuID& id,
                               const Eigen::Vector3f& pos,
                               const Eigen::Quaternionf& ori,
                               float scale,
                               bool ignoreLengthScale)
    {
        drawPose(id, ::math::Helpers::Pose(pos, ori), scale, ignoreLengthScale);
    }
 
    void
    DebugDrawerTopic::removePose(const DebugDrawerTopic::VisuID& id)
    {
        if (enabled())
        {
            topic->removePoseVisu(layer(id), id.name);
        }
    }
 
    void
    DebugDrawerTopic::drawRobot(const DebugDrawerTopic::VisuID& id,
                                const std::string& robotFile,
                                const std::string& armarxProject,
                                DrawStyle drawStyle)
    {
        if (enabled())
        {
            topic->setRobotVisu(layer(id), id.name, robotFile, armarxProject, drawStyle);
        }
    }
 
    void
    DebugDrawerTopic::updateRobotPose(const DebugDrawerTopic::VisuID& id,
                                      const Eigen::Matrix4f& pose,
                                      bool ignoreScale)
    {
        if (enabled())
        {
            topic->updateRobotPose(layer(id), id.name, scaled(lengthScale(ignoreScale), pose));
        }
    }
 
    void
    DebugDrawerTopic::updateRobotPose(const DebugDrawerTopic::VisuID& id,
                                      const Eigen::Vector3f& pos,
                                      const Eigen::Quaternionf& ori,
                                      bool ignoreScale)
    {
        updateRobotPose(id, ::math::Helpers::Pose(pos, ori), ignoreScale);
    }
 
    void
    DebugDrawerTopic::updateRobotConfig(const DebugDrawerTopic::VisuID& id,
                                        const std::map<std::string, float>& config)
    {
        if (enabled())
        {
            topic->updateRobotConfig(layer(id), id.name, config);
        }
    }
 
    void
    DebugDrawerTopic::updateRobotColor(const DebugDrawerTopic::VisuID& id, const DrawColor& color)
    {
        if (enabled())
        {
            topic->updateRobotColor(layer(id), id.name, color);
        }
    }
 
    void
    DebugDrawerTopic::updateRobotNodeColor(const DebugDrawerTopic::VisuID& id,
                                           const std::string& nodeName,
                                           const DrawColor& color)
    {
        if (enabled())
        {
            topic->updateRobotNodeColor(layer(id), id.name, nodeName, color);
        }
    }
 
    void
    DebugDrawerTopic::removeRobot(const DebugDrawerTopic::VisuID& id)
    {
        if (enabled())
        {
            topic->removeRobotVisu(layer(id), id.name);
        }
    }
 
    void
    DebugDrawerTopic::drawTriMesh(const VisuID& id,
                                  const VirtualRobot::TriMeshModel& triMesh,
                                  const DrawColor& color,
                                  bool ignoreLengthScale)
    {
        if (!enabled())
        {
            return;
        }
 
        const float scale = lengthScale(ignoreLengthScale);
 
        DebugDrawerTriMesh dd;
        dd.colors.push_back(color);
 
        for (const auto& vertex : triMesh.vertices)
        {
            auto scaled = vertex * scale;
            dd.vertices.push_back({scaled.x(), scaled.y(), scaled.z()});
        }
 
        const std::size_t normalBase = dd.vertices.size();
        for (const auto& normal : triMesh.normals)
        {
            dd.vertices.push_back({normal.x(), normal.y(), normal.z()});
        }
 
        for (const auto& face : triMesh.faces)
        {
            DebugDrawerFace ddf;
            ddf.vertex1.vertexID = static_cast<Ice::Int>(face.id1);
            ddf.vertex2.vertexID = static_cast<Ice::Int>(face.id2);
            ddf.vertex3.vertexID = static_cast<Ice::Int>(face.id3);
 
            ddf.vertex1.colorID = ddf.vertex2.colorID = ddf.vertex3.colorID = 0;
            ddf.vertex1.normalID = ddf.vertex2.normalID = ddf.vertex3.normalID = -1;
 
            bool validNormalIDs = true;
            for (const auto& id : {face.idNormal1, face.idNormal2, face.idNormal3})
            {
                validNormalIDs &= id < triMesh.normals.size();
            }
 
            if (validNormalIDs)
            {
                ddf.vertex1.normalID = static_cast<Ice::Int>(normalBase + face.idNormal1);
                ddf.vertex2.normalID = static_cast<Ice::Int>(normalBase + face.idNormal2);
                ddf.vertex3.normalID = static_cast<Ice::Int>(normalBase + face.idNormal3);
            }
            else
            {
                const Eigen::Vector3f& normal = face.normal;
                ddf.normal = {normal.x(), normal.y(), normal.z()};
            }
 
            dd.faces.push_back(ddf);
        }
 
        topic->setTriMeshVisu(layer(id), id.name, dd);
    }
 
    void
    DebugDrawerTopic::drawTriMeshAsPolygons(const VisuID& id,
                                            const VirtualRobot::TriMeshModel& trimesh,
                                            const DrawColor& colorFace,
                                            float lineWidth,
                                            const DrawColor& colorEdge,
                                            bool ignoreLengthScale)
    {
        drawTriMeshAsPolygons(id,
                              trimesh,
                              Eigen::Matrix4f::Identity(),
                              colorFace,
                              lineWidth,
                              colorEdge,
                              ignoreLengthScale);
    }
 
    void
    DebugDrawerTopic::drawTriMeshAsPolygons(const VisuID& id,
                                            const VirtualRobot::TriMeshModel& trimesh,
                                            const Eigen::Matrix4f& pose,
                                            const DrawColor& colorFace,
                                            float lineWidth,
                                            const DrawColor& colorEdge,
                                            bool ignoreLengthScale)
    {
        if (!enabled())
        {
            return;
        }
 
        const float scale = lengthScale(ignoreLengthScale);
        bool isIdentity = pose.isIdentity();
 
        auto toVector3 = [&scale, &isIdentity, &pose](const Eigen::Vector3f& v)
        { return scaled(scale, isIdentity ? v : ::math::Helpers::TransformPosition(pose, v)); };
 
        ARMARX_INFO << "Drawing trimesh as polygons";
 
        int counter = 0;
        for (std::size_t i = 0; i < trimesh.faces.size(); ++i)
        {
            const auto& face = trimesh.faces[i];
            PolygonPointList points{toVector3(trimesh.vertices.at(face.id1)),
                                    toVector3(trimesh.vertices.at(face.id2)),
                                    toVector3(trimesh.vertices.at(face.id3))};
 
            topic->setPolygonVisu(layer(id),
                                  id.name + "_" + std::to_string(counter),
                                  points,
                                  colorFace,
                                  colorEdge,
                                  lineWidth);
            ++counter;
        }
    }
 
    void
    DebugDrawerTopic::drawTriMeshAsPolygons(const VisuID& id,
                                            const VirtualRobot::TriMeshModel& trimesh,
                                            const std::vector<DrawColor>& faceColorsInner,
                                            float lineWidth,
                                            const DrawColor& colorEdge,
                                            bool ignoreLengthScale)
    {
        if (!enabled())
        {
            return;
        }
 
        //ARMARX_INFO << "Drawing trimesh as polygons colored by area";
        const float scale = lengthScale(ignoreLengthScale);
 
        for (std::size_t i = 0; i < trimesh.faces.size(); ++i)
        {
            const auto& face = trimesh.faces[i];
            PolygonPointList points{scaled(scale, trimesh.vertices[face.id1]),
                                    scaled(scale, trimesh.vertices[face.id2]),
                                    scaled(scale, trimesh.vertices[face.id3])};
 
            topic->setPolygonVisu(layer(id),
                                  id.name + "_" + std::to_string(i),
                                  points,
                                  faceColorsInner.at(i),
                                  colorEdge,
                                  lineWidth);
        }
    }
 
    void
    DebugDrawerTopic::drawPointCloud(const DebugDrawerTopic::VisuID& id,
                                     const DebugDrawerPointCloud& pointCloud)
    {
        if (enabled())
        {
            topic->setPointCloudVisu(id.layer, id.name, pointCloud);
        }
    }
 
    void
    DebugDrawerTopic::drawPointCloud(const DebugDrawerTopic::VisuID& id,
                                     const DebugDrawerColoredPointCloud& pointCloud)
    {
        if (enabled())
        {
            topic->setColoredPointCloudVisu(id.layer, id.name, pointCloud);
        }
    }
 
    void
    DebugDrawerTopic::drawPointCloud(const DebugDrawerTopic::VisuID& id,
                                     const DebugDrawer24BitColoredPointCloud& pointCloud)
    {
        if (enabled())
        {
            topic->set24BitColoredPointCloudVisu(id.layer, id.name, pointCloud);
        }
    }
 
    void
    DebugDrawerTopic::clearColoredPointCloud(const DebugDrawerTopic::VisuID& id)
    {
        // Draw an empty point cloud.
        drawPointCloud(id, DebugDrawerColoredPointCloud{});
    }
 
    void
    DebugDrawerTopic::drawFloor(const VisuID& id,
                                const Eigen::Vector3f& at,
                                const Eigen::Vector3f& up,
                                float size,
                                const DrawColor& color,
                                bool ignoreLengthScale)
    {
        if (!enabled())
        {
            return;
        }
 
        Eigen::Vector3f seed = seed.UnitX();
        if (std::abs(up.dot(seed)) <= 1e-6f)
        {
            seed = seed.UnitY();
        }
 
        /* ^ b
         * | 3---0
         * | | + |
         * | 2---1
         * +--------> a
         */
 
        const float halfSize = size / 2;
 
        const Eigen::Vector3f a = halfSize * up.cross(seed).normalized();
        const Eigen::Vector3f b = halfSize * up.cross(a).normalized();
 
        std::vector<Eigen::Vector3f> points;
        points.push_back(at + a + b);
        points.push_back(at + a - b);
        points.push_back(at - a - b);
        points.push_back(at - a + b);
 
        drawPolygon(id, points, color, ignoreLengthScale);
    }
 
    const std::string&
    DebugDrawerTopic::layer(const std::string& passedLayer) const
    {
        return passedLayer.empty() ? _layer : passedLayer;
    }
 
    const std::string&
    DebugDrawerTopic::layer(const VisuID& id) const
    {
        return layer(id.layer);
    }
 
    float
    DebugDrawerTopic::lengthScale(bool ignore) const
    {
        return ignore ? 1 : _lengthScale;
    }
 
    float
    DebugDrawerTopic::scaled(float scale, float value)
    {
        return scale * value;
    }
 
    Vector3BasePtr
    DebugDrawerTopic::scaled(float scale, const Eigen::Vector3f& vector)
    {
        if (scale >= 1 && scale <= 1)
        {
            return new Vector3(vector);
        }
        else
        {
            return new Vector3((vector * scale).eval());
        }
    }
 
    PoseBasePtr
    DebugDrawerTopic::scaled(float scale, const Eigen::Matrix4f& pose)
    {
        if (scale >= 1 && scale <= 1)
        {
            return new Pose(pose);
        }
        else
        {
            Eigen::Matrix4f out = pose;
            ::math::Helpers::Position(out) *= scale;
            return new Pose(out);
        }
    }
 
    DebugDrawerTopic::operator bool() const
    {
        return enabled();
    }
 
    armarx::DebugDrawerTopic::operator DebugDrawerInterfacePrx&()
    {
        return topic;
    }
 
    armarx::DebugDrawerTopic::operator const DebugDrawerInterfacePrx&() const
    {
        return topic;
    }
 
    DebugDrawerInterfacePrx&
    DebugDrawerTopic::operator->()
    {
        return topic;
    }
 
    const DebugDrawerInterfacePrx&
    DebugDrawerTopic::operator->() const
    {
        return topic;
    }
 
    Eigen::Vector3f
    DebugDrawerTopic::rgb2hsv(const Eigen::Vector3f& rgb)
    {
        return simox::color::rgb_to_hsv(rgb);
    }
 
    Eigen::Vector3f
    DebugDrawerTopic::hsv2rgb(const Eigen::Vector3f& hsv)
    {
        return simox::color::hsv_to_rgb(hsv);
    }
 
    DrawColor
    DebugDrawerTopic::getGlasbeyLUTColor(int id, float alpha)
    {
        return GlasbeyLUT::at(id, alpha);
    }
 
    DrawColor
    DebugDrawerTopic::getGlasbeyLUTColor(uint32_t id, float alpha)
    {
        return GlasbeyLUT::at(id, alpha);
    }
 
    DrawColor
    DebugDrawerTopic::getGlasbeyLUTColor(std::size_t id, float alpha)
    {
        return GlasbeyLUT::at(id, alpha);
    }
 
 
} // namespace armarx