ElementOps.h

Go to the documentation of this file.

#pragma once
 
#include "Color.h"
 
#include <RobotAPI/interface/ArViz/Elements.h>
 
#include <SimoxUtility/color/GlasbeyLUT.h>
#include <SimoxUtility/math/convert/rpy_to_quat.h>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <string>
 
 
namespace armarx::viz
{
    using data::ColoredPoint;
 
    struct AxesFlags
    {
        bool x = false;
        bool y = false;
        bool z = false;
        bool local = false;
    };
 
    static const AxesFlags AXES_X   = {true, false, false, false};
    static const AxesFlags AXES_Y   = {false, true, false, false};
    static const AxesFlags AXES_Z   = {false, false, true, false};
    static const AxesFlags AXES_XY  = {true, true, false, false};
    static const AxesFlags AXES_YZ  = {false, true, true, false};
    static const AxesFlags AXES_XZ  = {true, false, true, false};
    static const AxesFlags AXES_XYZ = {true, true, true, false};
 
    struct InteractionDescription
    {
        using Self = InteractionDescription;
 
        Self& none()
        {
            data_.enableFlags = 0;
            return *this;
        }
 
        Self& selection()
        {
            data_.enableFlags |= data::InteractionEnableFlags::SELECT;
            return *this;
        }
 
        Self& contextMenu(std::vector<std::string> const& options)
        {
            data_.enableFlags |= data::InteractionEnableFlags::CONTEXT_MENU;
            data_.contextMenuOptions = options;
            // Context menu (right click) implies selection
            return selection();
        }
 
        Self& translation(AxesFlags const& axes = AXES_XYZ)
        {
            data_.enableFlags |= (axes.x ? data::InteractionEnableFlags::TRANSLATION_X : 0);
            data_.enableFlags |= (axes.y ? data::InteractionEnableFlags::TRANSLATION_Y : 0);
            data_.enableFlags |= (axes.z ? data::InteractionEnableFlags::TRANSLATION_Z : 0);
            // Translation implies selection
            return selection();
        }
 
        Self& rotation(AxesFlags const& axes = AXES_XYZ)
        {
            data_.enableFlags |= (axes.x ? data::InteractionEnableFlags::ROTATION_X : 0);
            data_.enableFlags |= (axes.y ? data::InteractionEnableFlags::ROTATION_Y : 0);
            data_.enableFlags |= (axes.z ? data::InteractionEnableFlags::ROTATION_Z : 0);
            // Rotation implies selection
            return selection();
        }
 
        Self& scaling(AxesFlags const& axes = AXES_XYZ)
        {
            data_.enableFlags |= (axes.x ? data::InteractionEnableFlags::SCALING_X : 0);
            data_.enableFlags |= (axes.y ? data::InteractionEnableFlags::SCALING_Y : 0);
            data_.enableFlags |= (axes.z ? data::InteractionEnableFlags::SCALING_Z : 0);
            // Rotation implies selection
            return selection();
        }
 
        Self& transform()
        {
            return translation().rotation();
        }
 
        Self& hideDuringTransform()
        {
            data_.enableFlags |= data::InteractionEnableFlags::TRANSFORM_HIDE;
            return *this;
        }
 
        data::InteractionDescription data_;
    };
 
    inline InteractionDescription interaction()
    {
        return InteractionDescription();
    }
 
    // Move the ice datatypes into their own namespace
    template <typename DerivedT, typename ElementT>
    class ElementOps
    {
    public:
        ElementOps(std::string const& id)
            : data_(new ElementT)
        {
            data_->id = id;
            data_->scale.e0 = 1.0f;
            data_->scale.e1 = 1.0f;
            data_->scale.e2 = 1.0f;
        }
 
        DerivedT& id(const std::string& id)
        {
            data_->id = id;
 
            return *static_cast<DerivedT*>(this);
        }
 
        DerivedT& position(float x, float y, float z)
        {
            auto& pose = data_->pose;
            pose.x = x;
            pose.y = y;
            pose.z = z;
            return *static_cast<DerivedT*>(this);
        }
        DerivedT& position(Eigen::Vector3f const& pos)
        {
            return position(pos.x(), pos.y(), pos.z());
        }
 
        DerivedT& orientation(Eigen::Quaternionf const& ori)
        {
            auto& pose = data_->pose;
            pose.qw = ori.w();
            pose.qx = ori.x();
            pose.qy = ori.y();
            pose.qz = ori.z();
 
            return *static_cast<DerivedT*>(this);
        }
        DerivedT& orientation(Eigen::Matrix3f const& ori)
        {
            return orientation(Eigen::Quaternionf(ori));
        }
        DerivedT& orientation(float r, float p, float y)
        {
            return orientation(simox::math::rpy_to_quat(r, p, y));
        }
 
        DerivedT& pose(Eigen::Matrix4f const& pose)
        {
            return position(pose.block<3, 1>(0, 3)).orientation(pose.block<3, 3>(0, 0));
        }
 
        DerivedT& pose(Eigen::Vector3f const& position, Eigen::Quaternionf const& orientation)
        {
            return this->position(position).orientation(orientation);
        }
 
        DerivedT& pose(Eigen::Vector3f const& position, Eigen::Matrix3f const& orientation)
        {
            return this->position(position).orientation(orientation);
        }
 
        DerivedT& pose(const Eigen::Affine3f& pose)
        {
            return this->position(pose.translation()).orientation(pose.linear());
        }
 
        Eigen::Matrix4f pose() const
        {
            auto& p = data_->pose;
            Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
            m(0, 3) = p.x;
            m(1, 3) = p.y;
            m(2, 3) = p.z;
            m.topLeftCorner<3, 3>() = Eigen::Quaternionf{p.qw, p.qx, p.qy, p.qz}.toRotationMatrix();
            return m;
        }
 
        DerivedT& transformPose(Eigen::Matrix4f const& p)
        {
            return pose(p * pose());
        }
 
        DerivedT& color(Color color)
        {
            data_->color = color;
 
            return *static_cast<DerivedT*>(this);
        }
 
        template<class...Ts>
        DerivedT& color(Ts&& ...ts)
        {
            return color({std::forward<Ts>(ts)...});
        }
 
        DerivedT& colorGlasbeyLUT(std::size_t id, int alpha = 255)
        {
            return color(Color::fromRGBA(simox::color::GlasbeyLUT::at(id, alpha)));
        }
 
        DerivedT& overrideMaterial(bool value)
        {
            if (value)
            {
                data_->flags |= data::ElementFlags::OVERRIDE_MATERIAL;
            }
            else
            {
                data_->flags &= ~data::ElementFlags::OVERRIDE_MATERIAL;
            }
 
            return *static_cast<DerivedT*>(this);
        }
 
        DerivedT& scale(Eigen::Vector3f scale)
        {
            data_->scale.e0 = scale.x();
            data_->scale.e1 = scale.y();
            data_->scale.e2 = scale.z();
 
            return *static_cast<DerivedT*>(this);
        }
        DerivedT& scale(float x, float y, float z)
        {
            data_->scale.e0 = x;
            data_->scale.e1 = y;
            data_->scale.e2 = z;
 
            return *static_cast<DerivedT*>(this);
        }
        DerivedT& scale(float s)
        {
            return scale(s, s, s);
        }
 
        DerivedT& hide()
        {
            data_->flags |= data::ElementFlags::HIDDEN;
 
            return *static_cast<DerivedT*>(this);
        }
 
        DerivedT& show()
        {
            data_->flags &= ~data::ElementFlags::HIDDEN;
 
            return *static_cast<DerivedT*>(this);
        }
 
        DerivedT& visible(bool visible)
        {
            if (visible)
            {
                return show();
            }
            else
            {
                return hide();
            }
        }
 
        DerivedT& enable(InteractionDescription const& interactionDescription)
        {
            data_->interaction = interactionDescription.data_;
            return *static_cast<DerivedT*>(this);
        }
 
 
        IceInternal::Handle<ElementT> data_;
    };
 
}