ArViz: Add Transformable<T> abstraction

source/RobotAPI/components/ArViz/Example/ArVizInteractExample.cpp

@@ -31,6 +31,7 @@
 
 namespace armarx
 {
+
     struct SingleSlider
     {
         SingleSlider(std::string const& name, viz::Color color)
@@ -202,6 +203,216 @@ namespace armarx
     };
 
 
+    // ---------------
+
+    // What abstractions are needed?
+    // MovableElement
+    // SpawnerElement
+
+    //template <typename ElementT>
+    template <typename ElementT>
+    struct Transformable
+    {
+        Transformable(std::string const& name)
+            : element(name)
+        {
+        }
+
+        Transformable& pose(Eigen::Matrix4f const& pose)
+        {
+            element.pose(pose);
+            initialPose = pose;
+            return *this;
+        }
+
+        Transformable& position(Eigen::Vector3f const& position)
+        {
+            element.position(position);
+            initialPose.block<3, 1>(0, 3) = position;
+            return *this;
+        }
+
+        Transformable& orientation(Eigen::Matrix3f const& rotationMatrix)
+        {
+            element.orientation(rotationMatrix);
+            initialPose.block<3, 3>(0, 0) = rotationMatrix;
+            return *this;
+        }
+
+        Transformable& orientation(Eigen::Quaternionf const& quaternion)
+        {
+            element.orientation(quaternion);
+            initialPose.block<3, 3>(0, 0) = quaternion.toRotationMatrix();
+            return *this;
+        }
+
+        Transformable& enable(viz::InteractionDescription const& interaction)
+        {
+            element.enable(interaction);
+            // A movable element is always hidden during the interaction
+            element.data_->interaction.enableFlags |= viz::data::InteractionEnableFlags::TRANSFORM_HIDE;
+            return *this;
+        }
+
+        // The pose after the current transformation has been applied
+        Eigen::Matrix4f getCurrentPose() const
+        {
+            return transformation * initialPose;
+        }
+
+        // Returns true, if the element has been changed and the layer needs to be comitted again
+        bool handle(viz::InteractionFeedback const& interaction)
+        {
+            if (interaction.element() == element.data_->id)
+            {
+                switch (interaction.type())
+                {
+                case viz::InteractionFeedbackType::Transform:
+                {
+                    // Keep track of the transformation
+                    transformation = interaction.transformation();
+                } break;
+                case viz::InteractionFeedbackType::Deselect:
+                {
+                    // If an object is deselected, we apply the transformation
+                    initialPose = transformation * initialPose;
+                    transformation = Eigen::Matrix4f::Identity();
+                    element.pose(initialPose);
+                    return true;
+                } break;
+                default:
+                {
+                    // Ignore the other events
+                }
+                }
+            }
+            return false;
+        }
+
+
+        Eigen::Matrix4f initialPose = Eigen::Matrix4f::Identity();
+        Eigen::Matrix4f transformation = Eigen::Matrix4f::Identity();
+
+        ElementT element;
+    };
+
+
+    struct SlidersState2
+    {
+        SlidersState2(Eigen::Vector3f origin)
+            : origin(origin)
+            , x("BoxX")
+            , y("BoxY")
+            , z("BoxZ")
+            , sphere("Sphere")
+        {
+            float boxSize = 50.0f;
+
+            // We use the Transformable<T>::position to set the position
+            // This keeps track of the internal state
+            x.position(origin + Eigen::Vector3f(0.5f * ARROW_LENGTH, 0.0f, 0.0f));
+            // A movable object is always hidden during the transformation
+            // The hideDuringTransform() flag is automatically set here
+            x.enable(viz::interaction().translation(viz::AXES_X));
+
+            // Other attributes of the element can be set directly on the member
+            x.element.color(viz::Color::red())
+                    .size(boxSize);
+
+            y.position(origin + Eigen::Vector3f(0.0f, 0.5f * ARROW_LENGTH, 0.0f));
+            y.enable(viz::interaction().translation(viz::AXES_Y));
+
+            y.element.color(viz::Color::green())
+                    .size(boxSize);
+
+            z.position(origin + Eigen::Vector3f(0.0f, 0.0f, 0.5f * ARROW_LENGTH));
+            z.enable(viz::interaction().translation(viz::AXES_Z));
+            z.element.color(viz::Color::blue())
+                    .size(boxSize);
+
+            sphere.position(origin + 0.5f * ARROW_LENGTH * Eigen::Vector3f(1.0f, 1.0f, 1.0f))
+                  .color(viz::Color::orange())
+                  .radius(30.0f);
+        }
+
+        static constexpr const float ARROW_LENGTH = 1000.0f;
+
+        void visualize(viz::Client& arviz)
+        {
+            layerInteract = arviz.layer("Sliders2");
+
+            float arrowWidth = 10.0f;
+
+            viz::Arrow arrowX = viz::Arrow("ArrowX")
+                                .color(viz::Color::red())
+                                .fromTo(origin, origin + Eigen::Vector3f(ARROW_LENGTH, 0.0f, 0.0f))
+                                .width(arrowWidth);
+            layerInteract.add(arrowX);
+
+            viz::Arrow arrowY = viz::Arrow("ArrowY")
+                                .color(viz::Color::green())
+                                .fromTo(origin, origin + Eigen::Vector3f(0.0f, ARROW_LENGTH, 0.0f))
+                                .width(arrowWidth);
+            layerInteract.add(arrowY);
+
+            viz::Arrow arrowZ = viz::Arrow("ArrowZ")
+                                .color(viz::Color::blue())
+                                .fromTo(origin, origin + Eigen::Vector3f(0.0f, 0.0f, ARROW_LENGTH))
+                                .width(arrowWidth);
+            layerInteract.add(arrowZ);
+
+
+            layerInteract.add(x.element);
+            layerInteract.add(y.element);
+            layerInteract.add(z.element);
+
+            layerResult = arviz.layer("SlidersResult2");
+            layerResult.add(sphere);
+        }
+
+        void handle(viz::InteractionFeedback const& interaction,
+                    viz::StagedCommit* stage)
+        {
+            // Let the Transformable<T> handle all internal events
+            bool needsLayerUpdate = false;
+            needsLayerUpdate |= x.handle(interaction);
+            needsLayerUpdate |= y.handle(interaction);
+            needsLayerUpdate |= z.handle(interaction);
+            if (needsLayerUpdate)
+            {
+                // The handle() functions indicated that the layer needs to be updated
+                stage->add(layerInteract);
+            }
+
+            // We handle the transform event ourselves to add custom behavior
+            // Here, we move the sphere based on the position of the sliders
+            if (interaction.type() == viz::InteractionFeedbackType::Transform)
+            {
+                // We can query getCurrentPose() to determine the poses of the sliders
+                // with the transformation applied to them
+                Eigen::Vector3f spherePosition(
+                            x.getCurrentPose().col(3).x(),
+                            y.getCurrentPose().col(3).y(),
+                            z.getCurrentPose().col(3).z());
+                sphere.position(spherePosition);
+
+                stage->add(layerResult);
+            }
+        }
+
+        Eigen::Vector3f origin;
+        Transformable<viz::Box> x;
+        Transformable<viz::Box> y;
+        Transformable<viz::Box> z;
+
+        viz::Sphere sphere;
+
+        viz::Layer layerInteract;
+        viz::Layer layerResult;
+    };
+    // ---------------
+
+
     enum class SpawnerType
     {
         Box,
@@ -587,12 +798,17 @@ namespace armarx
 
 
             SlidersState sliders(origin1 + Eigen::Vector3f(500.0f, 500.0f, 0.0f));
+            SlidersState2 sliders2(origin3 + Eigen::Vector3f(500.0f, 500.0f, 0.0f));
             SpawnersState spawners(origin2);
 
             sliders.visualize(arviz);
             stage.add(sliders.layerInteract);
             stage.add(sliders.layerResult);
 
+            sliders2.visualize(arviz);
+            stage.add(sliders2.layerInteract);
+            stage.add(sliders2.layerResult);
+
             spawners.visualize(arviz);
             stage.add(spawners.layerSpawners);
             stage.add(spawners.layerObjects);
@@ -609,6 +825,7 @@ namespace armarx
                 stage.reset();
 
                 stage.requestInteraction(sliders.layerInteract);
+                stage.requestInteraction(sliders2.layerInteract);
                 stage.requestInteraction(spawners.layerSpawners);
                 stage.requestInteraction(spawners.layerObjects);
 
@@ -619,6 +836,10 @@ namespace armarx
                     {
                         sliders.handle(interaction, &stage);
                     }
+                    if (interaction.layer() == "Sliders2")
+                    {
+                        sliders2.handle(interaction, &stage);
+                    }
                     if (interaction.layer() == "Spawners" || interaction.layer() == "SpawnedObjects")
                     {
                         spawners.handle(interaction, &stage);