Visualizer.cpp

Go to the documentation of this file.

#include "Visualizer.h"
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/GetTypeString.h>
 
#include "ExportVRML.h"
#include <Inventor/SoPath.h>
 
namespace armarx::viz
{
    namespace coin
    {
        void clearRobotCache();
        void clearObjectCache();
    } // namespace coin
 
    static const int ANY_TRANSFORM =
        data::InteractionEnableFlags::TRANSLATION_X | data::InteractionEnableFlags::TRANSLATION_Y |
        data::InteractionEnableFlags::TRANSLATION_Z | data::InteractionEnableFlags::ROTATION_X |
        data::InteractionEnableFlags::ROTATION_Y | data::InteractionEnableFlags::ROTATION_Z |
        data::InteractionEnableFlags::SCALING_X | data::InteractionEnableFlags::SCALING_Y |
        data::InteractionEnableFlags::SCALING_Z;
 
    struct CoinVisualizerWrapper : IceUtil::Shared
    {
        class CoinVisualizer* this_;
 
        void
        onUpdateSuccess(data::LayerUpdates const& updates)
        {
            this_->onUpdateSuccess(updates);
        }
 
        void
        onUpdateFailure(Ice::Exception const& ex)
        {
            this_->onUpdateFailure(ex);
        }
    };
 
    static void
    selectionCallback(void* data, SoPath* path)
    {
        CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
        this_->onSelectEvent(path, data::InteractionFeedbackType::SELECT);
    }
 
    static void
    deselectionCallback(void* data, SoPath* path)
    {
        CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
        this_->onSelectEvent(path, data::InteractionFeedbackType::DESELECT);
    }
 
    static void
    startManipulationCallback(void* data, SoDragger* dragger)
    {
        CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
        this_->onManipulation(dragger, data::InteractionFeedbackType::TRANSFORM_BEGIN_FLAG);
    }
 
    static void
    duringManipulationCallback(void* data, SoDragger* dragger)
    {
        CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
        this_->onManipulation(dragger, data::InteractionFeedbackType::TRANSFORM_DURING_FLAG);
    }
 
    static void
    finishManipulationCallback(void* data, SoDragger* dragger)
    {
        CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
        this_->onManipulation(dragger, data::InteractionFeedbackType::TRANSFORM_END_FLAG);
    }
 
    static const char*
    toString(CoinVisualizerState state)
    {
        switch (state)
        {
            case CoinVisualizerState::STOPPED:
                return "STOPPED";
            case CoinVisualizerState::STARTING:
                return "STARTING";
            case CoinVisualizerState::RUNNING:
                return "RUNNING";
            case CoinVisualizerState::STOPPING:
                return "STOPPING";
        }
        return "UNKNOWN";
    }
 
    struct TimedBlock
    {
        TimedBlock(const char* function) : start(IceUtil::Time::now()), function(function)
        {
        }
 
        ~TimedBlock()
        {
            IceUtil::Time diff = IceUtil::Time::now() - start;
            // Horrible: We need to plot this actually
            ARMARX_INFO << "Time '" << function << "': " << diff.toMilliSecondsDouble() << " ms";
        }
 
    private:
        IceUtil::Time start;
        const char* function;
    };
 
    CoinVisualizer::CoinVisualizer()
    {
        registerVisualizationTypes();
 
        callbackData = new CoinVisualizerWrapper;
        callbackData->this_ = this;
        callback = newCallback_StorageInterface_pullUpdatesSinceAndSendInteractions(
            callbackData,
            &CoinVisualizerWrapper::onUpdateSuccess,
            &CoinVisualizerWrapper::onUpdateFailure);
 
        manipulatorGroup = new SoSeparator;
 
        // The SoSelection node enable selection of nodes via mouse click / ray casting
        selection = new SoSelection;
        selection->addSelectionCallback(&selectionCallback, this);
        selection->addDeselectionCallback(&deselectionCallback, this);
        selection->setUserData(this);
 
        root = new SoSeparator;
        root->addChild(manipulatorGroup);
        root->addChild(selection);
 
        // Preallocate some space for layers
        layers.data.reserve(32);
    }
 
    CoinVisualizer::~CoinVisualizer()
    {
    }
 
    void
    CoinVisualizer::clearCache()
    {
        coin::clearRobotCache();
        coin::clearObjectCache();
    }
 
    void
    CoinVisualizer::startAsync(StorageInterfacePrx const& storage)
    {
        std::unique_lock<std::mutex> lock(stateMutex);
        if (state != CoinVisualizerState::STOPPED)
        {
            ARMARX_WARNING << "Unexpected state of visualizer\n"
                           << "Expected: STOPPED\n"
                           << "But got: " << toString(state);
            return;
        }
        state = CoinVisualizerState::STARTING;
        stateStorage = storage;
    }
 
    void
    CoinVisualizer::stop()
    {
        if (state == CoinVisualizerState::STOPPED)
        {
            return;
        }
 
        state = CoinVisualizerState::STOPPED;
    }
 
    CoinVisualizer_ApplyTiming
    CoinVisualizer::apply(data::LayerUpdate const& update)
    {
        IceUtil::Time time_start = IceUtil::Time::now();
 
        CoinVisualizer_ApplyTiming timing;
        timing.layerName = update.name;
 
        CoinLayerID layerID(update.component, update.name);
        CoinLayer& layer = findOrAddLayer(layerID);
 
        IceUtil::Time time_addLayer = IceUtil::Time::now();
        timing.addLayer = time_addLayer - time_start;
 
        addOrUpdateElements(&layer, update);
 
        IceUtil::Time time_updates = IceUtil::Time::now();
        timing.updateElements = time_updates - time_addLayer;
 
        removeElementsIfNotUpdated(&layer);
 
        IceUtil::Time time_remove = IceUtil::Time::now();
        timing.removeElements = time_remove - time_updates;
 
        emitLayerUpdated(layerID, layer);
 
        IceUtil::Time time_end = IceUtil::Time::now();
        timing.total = time_end - time_start;
 
        return timing;
    }
 
    CoinLayer&
    CoinVisualizer::findOrAddLayer(CoinLayerID const& layerID)
    {
        auto layerIt = layers.lowerBound(layerID);
 
        if (layerIt == layers.data.end() || layerIt->id != layerID)
        {
            // Create a new layer
            SoSeparator* coinNode = new SoSeparator;
            coinNode->ref();
            selection->addChild(coinNode);
 
            layerIt = layers.data.insert(layerIt, CoinLayer(layerID, coinNode));
            layerIt->elements.reserve(64);
        }
 
        return *layerIt;
    }
 
    void
    CoinVisualizer::addOrUpdateElements(CoinLayer* layer, data::LayerUpdate const& update)
    {
        layer->elements.reserve(update.elements.size());
        for (viz::data::ElementPtr const& updatedElementPtr : update.elements)
        {
            if (!updatedElementPtr)
            {
                ARMARX_WARNING << deactivateSpam(10) << "Element is null in layer "
                               << update.component << "/" << update.name;
                continue;
            }
            data::Element const& updatedElement = *updatedElementPtr;
 
            std::type_index elementType = typeid(updatedElement);
            size_t visuIndex;
            size_t visuSize = elementVisualizersTypes.size();
            for (visuIndex = 0; visuIndex < visuSize; ++visuIndex)
            {
                if (elementVisualizersTypes[visuIndex] == elementType)
                {
                    break;
                }
            }
            if (visuIndex >= visuSize)
            {
                ARMARX_WARNING << deactivateSpam(1) << "No visualizer for element type found: "
                               << armarx::GetTypeString(elementType);
                continue;
            }
            coin::ElementVisualizer* visualizer = elementVisualizers[visuIndex].get();
 
            auto oldElementIter = layer->lowerBound(updatedElement.id);
            CoinLayerElement* oldElement = nullptr;
            if (oldElementIter != layer->elements.end() &&
                oldElementIter->data->id == updatedElement.id)
            {
                // Element already exists
                CoinLayerElement* oldElement = &*oldElementIter;
                coin::ElementVisualization& oldElementVisu = *oldElement->visu;
 
                oldElementVisu.wasUpdated = true;
 
                bool updated = visualizer->update(updatedElement, &oldElementVisu);
 
                if (updated)
                {
                    // Has an interaction been added?
                    viz::data::InteractionDescription& oldInteraction =
                        oldElement->data->interaction;
                    viz::data::InteractionDescription& newInteraction =
                        updatedElementPtr->interaction;
                    if (newInteraction.enableFlags != oldInteraction.enableFlags ||
                        oldInteraction.contextMenuOptions != newInteraction.contextMenuOptions)
                    {
                        addOrUpdateInteraction(
                            layer->id, updatedElement.id, newInteraction, oldElement->visu.get());
                    }
 
                    oldElement->data = updatedElementPtr;
                    continue;
                }
                else
                {
                    // Types are different, so we delete the old element and create a new one
                    layer->node->removeChild(oldElementVisu.separator);
                }
            }
 
            auto elementVisu = visualizer->create(updatedElement);
            if (elementVisu->separator)
            {
                // Has the new element interactions?
                viz::data::InteractionDescription& newInteraction = updatedElementPtr->interaction;
                if (newInteraction.enableFlags != 0)
                {
                    addOrUpdateInteraction(
                        layer->id, updatedElement.id, newInteraction, elementVisu.get());
                }
 
                layer->node->addChild(elementVisu->separator);
                if (oldElement)
                {
                    oldElement->data = updatedElementPtr;
                    oldElement->visu = std::move(elementVisu);
                }
                else
                {
                    // Need to add a new element
                    layer->elements.insert(
                        oldElementIter,
                        CoinLayerElement{updatedElementPtr, std::move(elementVisu)});
                }
            }
            else
            {
                std::string typeName = armarx::GetTypeString(elementType);
                ARMARX_WARNING << deactivateSpam(typeName, 1)
                               << "CoinElementVisualizer returned null for type: " << typeName
                               << "\n"
                               << "You need to register a visualizer for each type in "
                                  "ArViz/Coin/RegisterVisualizationTypes.cpp";
            }
        }
    }
 
    void
    CoinVisualizer::addOrUpdateInteraction(CoinLayerID const& layerID,
                                           std::string const& elementID,
                                           data::InteractionDescription const& interactionDesc,
                                           coin::ElementVisualization* visu)
    {
        // Lookup the interaction entry
        ElementInteractionData* foundInteraction = nullptr;
        for (auto& interaction : elementInteractions)
        {
            if (interaction->layer == layerID && interaction->element == elementID)
            {
                foundInteraction = interaction.get();
            }
        }
        if (foundInteraction == nullptr)
        {
            // Need to add a new entry
            foundInteraction = elementInteractions.emplace_back(new ElementInteractionData).get();
        }
        foundInteraction->layer = layerID;
        foundInteraction->element = elementID;
 
        foundInteraction->interaction = interactionDesc;
        foundInteraction->visu = visu;
 
        // Add user data to Coin node
        visu->separator->setUserData(foundInteraction);
        visu->separator->setName("InteractiveNode");
    }
 
    void
    CoinVisualizer::removeElementsIfNotUpdated(CoinLayer* layer)
    {
        for (auto iter = layer->elements.begin(); iter != layer->elements.end();)
        {
            coin::ElementVisualization& elementVisu = *iter->visu;
            if (elementVisu.wasUpdated)
            {
                elementVisu.wasUpdated = false;
                ++iter;
            }
            else
            {
                void* userData = elementVisu.separator->getUserData();
                if (userData)
                {
                    // Remove interaction entry if element has been removed
                    auto removedInteractionIter = std::find_if(
                        elementInteractions.begin(),
                        elementInteractions.end(),
                        [userData](std::unique_ptr<ElementInteractionData> const& entry)
                        { return entry.get() == userData; });
                    ElementInteractionData* removedInteraction = removedInteractionIter->get();
                    if (removedInteraction == selectedElement)
                    {
                        // The selected element is removed ==> Therefore deselect it
                        selection->deselectAll();
                        selectedElement = nullptr;
                    }
                    elementInteractions.erase(removedInteractionIter);
 
                    elementVisu.separator->setUserData(nullptr);
                    elementVisu.separator->setName("");
                }
                layer->node->removeChild(elementVisu.separator);
                iter = layer->elements.erase(iter);
            }
        }
    }
 
    void
    CoinVisualizer::update()
    {
        {
            std::lock_guard lock(timingMutex);
            lastTiming.updateToggle = (lastTiming.updateToggle + 1) % 10;
        }
        switch (state)
        {
            case CoinVisualizerState::STARTING:
            {
                std::unique_lock<std::mutex> lock(stateMutex);
                storage = stateStorage;
                selection->deselectAll();
                selection->removeAllChildren();
                manipulatorGroup->removeAllChildren();
                interactionFeedbackBuffer.clear();
                elementInteractions.clear();
                selectedElement = nullptr;
                layers.data.clear();
                pulledUpdates.revision = 0;
                pulledUpdates.updates.clear();
                updateResult = CoinVisualizerUpdateResult::SUCCESS;
                state = CoinVisualizerState::RUNNING;
            }
            break;
 
            case CoinVisualizerState::RUNNING:
                break;
 
            case CoinVisualizerState::STOPPING:
            {
                // After we have reached the STOPPED state, we know that updates are no longer running
                state = CoinVisualizerState::STOPPED;
                return;
            }
            break;
 
            case CoinVisualizerState::STOPPED:
                return;
        }
 
        switch (updateResult)
        {
            case CoinVisualizerUpdateResult::SUCCESS:
            {
                IceUtil::Time time_start = IceUtil::Time::now();
                CoinVisualizer_UpdateTiming timing;
 
                data::LayerUpdates currentUpdates = pulledUpdates;
                updateResult = CoinVisualizerUpdateResult::WAITING;
                timing.waitStart = time_start;
                storage->begin_pullUpdatesSinceAndSendInteractions(
                    currentUpdates.revision, interactionFeedbackBuffer, callback);
 
                // Clear interaction feedback buffer after it has been sent
                interactionFeedbackBuffer.clear();
 
                auto layerIDsBefore = getLayerIDs();
 
                IceUtil::Time time_pull = IceUtil::Time::now();
                timing.pull = time_pull - time_start;
 
                timing.applies.reserve(currentUpdates.updates.size());
                for (data::LayerUpdate const& update : currentUpdates.updates)
                {
                    auto& applyTiming = timing.applies.emplace_back(apply(update));
                    timing.applyTotal.add(applyTiming);
                }
                IceUtil::Time time_apply = IceUtil::Time::now();
                timing.applyTotal.total = time_apply - time_pull;
 
                auto layerIDsAfter = getLayerIDs();
                if (layerIDsAfter != layerIDsBefore)
                {
                    emitLayersChanged(layerIDsAfter);
                }
                IceUtil::Time time_layersChanged = IceUtil::Time::now();
                timing.layersChanged = time_layersChanged - time_apply;
 
                IceUtil::Time time_end = IceUtil::Time::now();
                timing.total = time_end - time_start;
 
                {
                    // Copy the timing result
                    std::lock_guard lock(timingMutex);
                    timing.counter = lastTiming.counter + 1;
                    timing.updateToggle = lastTiming.updateToggle;
                    lastTiming = std::move(timing);
                }
            }
            break;
            case CoinVisualizerUpdateResult::WAITING:
            {
                // Still waiting for result
                {
                    // Copy the timing result
                    std::lock_guard lock(timingMutex);
                    lastTiming.waitDuration = IceUtil::Time::now() - lastTiming.waitStart;
                }
            }
            break;
            case CoinVisualizerUpdateResult::FAILURE:
            {
                std::unique_lock<std::mutex> lock(stateMutex);
                storage = nullptr;
                state = CoinVisualizerState::STOPPED;
            }
            break;
        }
    }
 
    void
    CoinVisualizer::onUpdateSuccess(const data::LayerUpdates& updates)
    {
        pulledUpdates = updates;
        updateResult = CoinVisualizerUpdateResult::SUCCESS;
    }
 
    void
    CoinVisualizer::onUpdateFailure(const Ice::Exception& ex)
    {
        ARMARX_WARNING << "Lost connection to ArVizStorage\n" << ex.what();
        updateResult = CoinVisualizerUpdateResult::FAILURE;
    }
 
    void
    CoinVisualizer::showLayer(CoinLayerID const& id, bool visible)
    {
        CoinLayer* layer = layers.findLayer(id);
        if (layer == nullptr)
        {
            return;
        }
 
        int childIndex = selection->findChild(layer->node);
        if (childIndex < 0)
        {
            // Layer is currently not visible
            if (visible)
            {
                selection->addChild(layer->node);
            }
        }
        else
        {
            // Layer is currently visible
            if (!visible)
            {
                // If we hide a layer wit a selected element, Coin will crash during rendering
                // So we check if the selected element is in the layer to be hidden
                if (selectedElement && selectedElement->layer == id)
                {
                    selection->deselectAll();
                }
                selection->removeChild(childIndex);
            }
        }
    }
 
    CoinVisualizer_UpdateTiming
    CoinVisualizer::getTiming()
    {
        std::lock_guard lock(timingMutex);
        return lastTiming;
    }
 
    std::vector<CoinLayerID>
    CoinVisualizer::getLayerIDs()
    {
        std::vector<CoinLayerID> result;
        result.reserve(layers.data.size());
        for (CoinLayer& layer : layers.data)
        {
            result.push_back(layer.id);
        }
        return result;
    }
 
    void
    CoinVisualizer::emitLayersChanged(std::vector<CoinLayerID> const& layerIDs)
    {
        if (layersChangedCallback)
        {
            layersChangedCallback(layerIDs);
        }
    }
 
    void
    CoinVisualizer::emitLayerUpdated(const CoinLayerID& layerID, const CoinLayer& layer)
    {
        for (auto& callback : layerUpdatedCallbacks)
        {
            if (callback)
            {
                callback(layerID, layer);
            }
        }
    }
 
    void
    CoinVisualizer::selectElement(int index)
    {
        if (index >= (int)elementInteractions.size())
        {
            return;
        }
 
        ElementInteractionData* id = elementInteractions[index].get();
 
        selection->deselectAll();
        selection->select(id->visu->separator);
    }
 
    static ElementInteractionData*
    findInteractionDataOnPath(SoPath* path)
    {
        // Search for user data in the path
        // We stored ElementInteractionData into the user data of the parent SoSeparator
        void* userData = nullptr;
        int pathLength = path->getLength();
        for (int i = 0; i < pathLength; ++i)
        {
            SoNode* node = path->getNode(i);
            const char* name = node->getName().getString();
            if (strcmp(name, "InteractiveNode") == 0)
            {
                userData = node->getUserData();
                if (userData != nullptr)
                {
                    break;
                }
            }
        }
 
        return static_cast<ElementInteractionData*>(userData);
    }
 
    void
    CoinVisualizer::onSelectEvent(SoPath* path, int eventType)
    {
        if (state != CoinVisualizerState::RUNNING)
        {
            return;
        }
 
        ElementInteractionData* id = findInteractionDataOnPath(path);
        if (id == nullptr)
        {
            if (eventType == data::InteractionFeedbackType::SELECT)
            {
                // An object was selected that does not have any interactions enabled
                // We deselect all other elements in this case to avoid highlighting
                // non-interactable elements
                selection->deselectAll();
            }
            return;
        }
        int enableFlags = id->interaction.enableFlags;
        if ((enableFlags & data::InteractionEnableFlags::SELECT) == 0)
        {
            // Element was not marked for selection
            return;
        }
 
        if (eventType == data::InteractionFeedbackType::SELECT)
        {
            selectedElement = id;
 
            // Does the element support transform interactions?
            if (enableFlags & ANY_TRANSFORM)
            {
                manipulatorGroup->removeAllChildren();
                // We need to create the manipulator everytime to achieve the desired effect
                // If we reuse an instance, the manipulator gets stuck to old objects...
                manipulator = new SoTransformerManip;
                SoDragger* dragger = manipulator->getDragger();
                dragger->addStartCallback(&startManipulationCallback, this);
                dragger->addMotionCallback(&duringManipulationCallback, this);
                dragger->addFinishCallback(&finishManipulationCallback, this);
 
                manipulatorGroup->addChild(manipulator);
 
                // We add the same visualization node again
                SoSeparator* newSep = new SoSeparator();
                int childNum = id->visu->separator->getNumChildren();
                for (int i = 0; i < childNum; ++i)
                {
                    SoNode* child = id->visu->separator->getChild(i);
                    if (SoSwitch* switch_ = dynamic_cast<SoSwitch*>(child))
                    {
                        child = switch_->copy();
                    }
                    newSep->addChild(child);
                }
                manipulatorGroup->addChild(newSep);
                manipulator->unsquishKnobs();
 
                if (enableFlags & data::InteractionEnableFlags::TRANSFORM_HIDE)
                {
                    id->visu->switch_->whichChild = SO_SWITCH_NONE;
                }
            }
            else
            {
                selection->touch();
            }
        }
        else
        {
            if (enableFlags & data::InteractionEnableFlags::TRANSFORM_HIDE)
            {
                // Now, we should apply the transformation to the original object (at least temporary)
                // This avoids flickering, after the object is deselected
                SbMatrix manipMatrix;
                manipMatrix.setTransform(1000.0f * manipulator->translation.getValue(),
                                         manipulator->rotation.getValue(),
                                         manipulator->scaleFactor.getValue(),
                                         manipulator->scaleOrientation.getValue(),
                                         manipulator->center.getValue());
                id->visu->transform->multLeft(manipMatrix);
 
                SbVec3f translation = id->visu->transform->translation.getValue();
                ARMARX_IMPORTANT << "Visu translation: " << translation[0] << ", " << translation[1]
                                 << ", " << translation[2];
 
                id->visu->switch_->whichChild = SO_SWITCH_ALL;
            }
 
            selectedElement = nullptr;
            manipulatorGroup->removeAllChildren();
        }
 
        viz::data::InteractionFeedback& feedback = interactionFeedbackBuffer.emplace_back();
        feedback.type = eventType;
        feedback.component = id->layer.first;
        feedback.layer = id->layer.second;
        feedback.element = id->element;
        feedback.revision = pulledUpdates.revision;
    }
 
    SbVec3f
    translationDueToScaling(SbRotation r_m, SbVec3f t_m, SbVec3f s_m, SbVec3f t_o)
    {
        SbVec3f t_o_scaled(s_m[0] * t_o[0], s_m[1] * t_o[1], s_m[2] * t_o[2]);
        SbVec3f t_added_rotation_and_scale;
        r_m.multVec(t_o_scaled, t_added_rotation_and_scale);
        t_added_rotation_and_scale -= t_o;
 
        SbVec3f t_added_rotation;
        r_m.multVec(t_o, t_added_rotation);
        t_added_rotation -= t_o;
        SbVec3f t_added_scale = t_added_rotation_and_scale - t_added_rotation;
        return t_added_scale;
    }
 
    SbVec3f
    translationDueToRotation(SbRotation r_m, SbVec3f t_m, SbVec3f s_m, SbVec3f t_o)
    {
        SbVec3f t_o_scaled(s_m[0] * t_o[0], s_m[1] * t_o[1], s_m[2] * t_o[2]);
        SbVec3f t_added_rotation_and_scale;
        r_m.multVec(t_o_scaled, t_added_rotation_and_scale);
        t_added_rotation_and_scale -= t_o;
 
        // Do we need to exclude
        //        SbVec3f t_added_rotation;
        //        r_m.multVec(t_o, t_added_rotation);
        //        t_added_rotation -= t_o;
        //        SbVec3f t_added_scale = t_added_rotation_and_scale - t_added_rotation;
        return t_added_rotation_and_scale;
    }
 
    Eigen::Matrix4f
    toEigen(SbMat const& mat)
    {
        Eigen::Matrix4f result;
        for (int y = 0; y < 4; ++y)
        {
            for (int x = 0; x < 4; ++x)
            {
                result(x, y) = mat[y][x];
            }
        }
 
        return result;
    }
 
    // This should constrain the rotation according to the enabled axes
    static SbRotation
    constrainRotation(SbRotation input, int enableFlags)
    {
        SbMatrix mat;
        mat.setRotate(input);
        Eigen::Matrix4f mat_eigen = toEigen(mat);
        Eigen::Matrix3f mat_rot = mat_eigen.block<3, 3>(0, 0);
        Eigen::Vector3f rpy = mat_rot.eulerAngles(0, 1, 2);
        // ARMARX_INFO << "rpy before: " << rpy.transpose();
        if ((enableFlags & data::InteractionEnableFlags::ROTATION_X) == 0)
        {
            rpy(0) = 0.0f;
        }
        if ((enableFlags & data::InteractionEnableFlags::ROTATION_Y) == 0)
        {
            rpy(1) = 0.0f;
        }
        if ((enableFlags & data::InteractionEnableFlags::ROTATION_Z) == 0)
        {
            rpy(2) = 0.0f;
        }
        // ARMARX_INFO << "rpy after: " << rpy.transpose();
 
        mat_rot = Eigen::AngleAxisf(rpy(0), Eigen::Vector3f::UnitX()) *
                  Eigen::AngleAxisf(rpy(1), Eigen::Vector3f::UnitY()) *
                  Eigen::AngleAxisf(rpy(2), Eigen::Vector3f::UnitZ());
        Eigen::Quaternionf q(mat_rot);
 
        SbRotation result(q.x(), q.y(), q.z(), q.w());
        return result;
    }
 
    static SbVec3f
    constrainScaling(SbVec3f input, int enableFlags)
    {
        SbVec3f result = input;
        if ((enableFlags & data::InteractionEnableFlags::SCALING_X) == 0)
        {
            result[0] = 1.0f;
        }
        if ((enableFlags & data::InteractionEnableFlags::SCALING_Y) == 0)
        {
            result[1] = 1.0f;
        }
        if ((enableFlags & data::InteractionEnableFlags::SCALING_Z) == 0)
        {
            result[2] = 1.0f;
        }
        return result;
    }
 
    void
    CoinVisualizer::onManipulation(SoDragger* dragger, int eventType)
    {
        if (state != CoinVisualizerState::RUNNING)
        {
            return;
        }
        if (selectedElement == nullptr)
        {
            ARMARX_WARNING << "A manipulation event was fired but no element is selected";
            return;
        }
 
        // If there is an entry in the feedback buffer already, update it
        // This way, we prevent multiple events being sent to the client
        viz::data::InteractionFeedback* newFeedback = nullptr;
        for (viz::data::InteractionFeedback& feedback : interactionFeedbackBuffer)
        {
            if ((feedback.type & 0x7) == data::InteractionFeedbackType::TRANSFORM &&
                feedback.component == selectedElement->layer.first &&
                feedback.layer == selectedElement->layer.second &&
                feedback.element == selectedElement->element)
            {
                // This is a transform interaction concerning the same element
                newFeedback = &feedback;
                newFeedback->type |= eventType;
            }
        }
        if (newFeedback == nullptr)
        {
            // Create a new interaction
            newFeedback = &interactionFeedbackBuffer.emplace_back();
            newFeedback->component = selectedElement->layer.first;
            newFeedback->layer = selectedElement->layer.second;
            newFeedback->element = selectedElement->element;
            newFeedback->type = data::InteractionFeedbackType::TRANSFORM | eventType;
        }
 
        newFeedback->revision = pulledUpdates.revision;
 
 
        // Transformation applied by the manipulator
        SbVec3f t_m = manipulator->translation.getValue();
        SbRotation r_m_old = manipulator->rotation.getValue();
        SbVec3f s_m_old = manipulator->scaleFactor.getValue();
 
        int enableFlags = selectedElement->interaction.enableFlags;
 
        SbRotation r_m = constrainRotation(r_m_old, enableFlags);
        SbVec3f s_m = constrainScaling(s_m_old, enableFlags);
 
        // Transformation applied to the object
        // Translation is in mm, but the manipulator works in m!
        SbVec3f t_o = 0.001f * selectedElement->visu->transform->translation.getValue();
 
        if (s_m != s_m_old)
        {
            manipulator->scaleFactor.setValue(s_m);
 
            // Remove motion induced by scaling
            SbVec3f t_old = translationDueToScaling(r_m_old, t_m, s_m_old, t_o);
            SbVec3f t_new = translationDueToScaling(r_m, t_m, s_m, t_o);
            SbVec3f t_diff = t_new - t_old;
            t_m -= t_diff;
        }
        if (r_m != r_m_old)
        {
            manipulator->rotation.setValue(r_m);
 
            // Remove motion induced by rotation
            SbVec3f t_old = translationDueToRotation(r_m_old, t_m, s_m, t_o);
            SbVec3f t_new = translationDueToRotation(r_m, t_m, s_m, t_o);
            SbVec3f t_diff = t_new - t_old;
            t_m -= t_diff;
        }
 
        // TODO: Should we use the rotation of the object?
        // SbRotation r_o = selectedElement->visu->transform->rotation.getValue();
        // TODO: Do we need to consider scale of the object as well?
        // SbVec3f s_o = selectedElement->visu->transform->scaleFactor.getValue();
 
        // This value stays constant during rotation and scaling!
        // It is zero when the manipulation first starts
        // So we can reproduce the perceived motion here (hopefully)
        SbVec3f t_o_scaled(s_m[0] * t_o[0], s_m[1] * t_o[1], s_m[2] * t_o[2]);
        SbVec3f t_added_rotation_and_scale;
        r_m.multVec(t_o_scaled, t_added_rotation_and_scale);
        t_added_rotation_and_scale -= t_o;
        SbVec3f delta_t = t_added_rotation_and_scale + t_m;
 
        SbVec3f t_added_rotation;
        r_m.multVec(t_o, t_added_rotation);
        t_added_rotation -= t_o;
        SbVec3f t_added_scale = t_added_rotation_and_scale - t_added_rotation;
 
        // Prevent translation along disabled axes
        if ((enableFlags & data::InteractionEnableFlags::TRANSLATION_X) == 0)
        {
            delta_t[0] = 0.0f;
        }
        if ((enableFlags & data::InteractionEnableFlags::TRANSLATION_Y) == 0)
        {
            delta_t[1] = 0.0f;
        }
        if ((enableFlags & data::InteractionEnableFlags::TRANSLATION_Z) == 0)
        {
            delta_t[2] = 0.0f;
        }
 
        SbVec3f t_m_projected = delta_t - t_added_rotation_and_scale;
        manipulator->translation.setValue(t_m_projected);
 
        // t_m_projected is the correct value for the manipulator, but it still contains translation due to scaling!
        // We should subtract the translation induced by scaling!
        SbVec3f t_m_non_scaled = t_m_projected + t_added_scale;
 
        data::GlobalPose& transformation = newFeedback->transformation;
        transformation.x = 1000.0f * t_m_non_scaled[0];
        transformation.y = 1000.0f * t_m_non_scaled[1];
        transformation.z = 1000.0f * t_m_non_scaled[2];
        transformation.qw = r_m[3];
        transformation.qx = r_m[0];
        transformation.qy = r_m[1];
        transformation.qz = r_m[2];
 
        armarx::Vector3f& scale = newFeedback->scale;
        scale.e0 = s_m[0];
        scale.e1 = s_m[1];
        scale.e2 = s_m[2];
    }
 
    void
    CoinVisualizer::exportToVRML(const std::string& exportFilePath)
    {
        coin::exportToVRML(selection, exportFilePath);
    }
} // namespace armarx::viz