DebugDrawerTopic.cpp

Go to the documentation of this file.

#include "DebugDrawerTopic.h"
 
#include <VirtualRobot/math/Helpers.h>
#include <VirtualRobot/Visualization/TriMeshModel.h>
 
#include <ArmarXCore/core/ManagedIceObject.h>
 
#include "../Pose.h"
 
#include "GlasbeyLUT.h"
 
#include <SimoxUtility/color/hsv.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);
    }
 
 
}