4 #include <Eigen/Geometry>
10 #include <Inventor/SoPath.h>
20 static const int ANY_TRANSFORM =
21 data::InteractionEnableFlags::TRANSLATION_X | data::InteractionEnableFlags::TRANSLATION_Y |
22 data::InteractionEnableFlags::TRANSLATION_Z | data::InteractionEnableFlags::ROTATION_X |
23 data::InteractionEnableFlags::ROTATION_Y | data::InteractionEnableFlags::ROTATION_Z |
24 data::InteractionEnableFlags::SCALING_X | data::InteractionEnableFlags::SCALING_Y |
25 data::InteractionEnableFlags::SCALING_Z;
45 selectionCallback(
void*
data, SoPath* path)
47 CoinVisualizer* this_ =
static_cast<CoinVisualizer*
>(
data);
48 this_->onSelectEvent(path, data::InteractionFeedbackType::SELECT);
52 deselectionCallback(
void*
data, SoPath* path)
54 CoinVisualizer* this_ =
static_cast<CoinVisualizer*
>(
data);
55 this_->onSelectEvent(path, data::InteractionFeedbackType::DESELECT);
59 startManipulationCallback(
void*
data, SoDragger* dragger)
61 CoinVisualizer* this_ =
static_cast<CoinVisualizer*
>(
data);
62 this_->onManipulation(dragger, data::InteractionFeedbackType::TRANSFORM_BEGIN_FLAG);
66 duringManipulationCallback(
void*
data, SoDragger* dragger)
68 CoinVisualizer* this_ =
static_cast<CoinVisualizer*
>(
data);
69 this_->onManipulation(dragger, data::InteractionFeedbackType::TRANSFORM_DURING_FLAG);
73 finishManipulationCallback(
void*
data, SoDragger* dragger)
75 CoinVisualizer* this_ =
static_cast<CoinVisualizer*
>(
data);
76 this_->onManipulation(dragger, data::InteractionFeedbackType::TRANSFORM_END_FLAG);
106 ARMARX_INFO <<
"Time '" <<
function <<
"': " << diff.toMilliSecondsDouble() <<
" ms";
111 const char*
function;
120 callback = newCallback_StorageInterface_pullUpdatesSinceAndSendInteractions(
129 selection->addSelectionCallback(&selectionCallback,
this);
130 selection->addDeselectionCallback(&deselectionCallback,
this);
133 root =
new SoSeparator;
155 std::unique_lock<std::mutex> lock(
stateMutex);
159 <<
"Expected: STOPPED\n"
190 timing.
addLayer = time_addLayer - time_start;
205 timing.
total = time_end - time_start;
215 if (layerIt ==
layers.
data.end() || layerIt->id != layerID)
218 SoSeparator* coinNode =
new SoSeparator;
223 layerIt->elements.reserve(64);
233 for (viz::data::ElementPtr
const& updatedElementPtr :
update.elements)
235 if (!updatedElementPtr)
241 data::Element
const& updatedElement = *updatedElementPtr;
243 std::type_index elementType =
typeid(updatedElement);
246 for (visuIndex = 0; visuIndex < visuSize; ++visuIndex)
253 if (visuIndex >= visuSize)
261 auto oldElementIter = layer->
lowerBound(updatedElement.id);
263 if (oldElementIter != layer->
elements.end() &&
264 oldElementIter->data->id == updatedElement.id)
272 bool updated = visualizer->
update(updatedElement, &oldElementVisu);
277 viz::data::InteractionDescription& oldInteraction =
278 oldElement->
data->interaction;
279 viz::data::InteractionDescription& newInteraction =
280 updatedElementPtr->interaction;
281 if (newInteraction.enableFlags != oldInteraction.enableFlags ||
282 oldInteraction.contextMenuOptions != newInteraction.contextMenuOptions)
285 layer->
id, updatedElement.id, newInteraction, oldElement->
visu.get());
288 oldElement->
data = updatedElementPtr;
298 auto elementVisu = visualizer->
create(updatedElement);
299 if (elementVisu->separator)
302 viz::data::InteractionDescription& newInteraction = updatedElementPtr->interaction;
303 if (newInteraction.enableFlags != 0)
306 layer->
id, updatedElement.id, newInteraction, elementVisu.get());
309 layer->
node->addChild(elementVisu->separator);
312 oldElement->
data = updatedElementPtr;
313 oldElement->
visu = std::move(elementVisu);
327 <<
"CoinElementVisualizer returned null for type: " << typeName
329 <<
"You need to register a visualizer for each type in "
330 "ArViz/Coin/RegisterVisualizationTypes.cpp";
337 std::string
const& elementID,
338 data::InteractionDescription
const& interactionDesc,
350 if (foundInteraction ==
nullptr)
355 foundInteraction->
layer = layerID;
356 foundInteraction->
element = elementID;
359 foundInteraction->
visu = visu;
362 visu->
separator->setUserData(foundInteraction);
363 visu->
separator->setName(
"InteractiveNode");
379 void* userData = elementVisu.
separator->getUserData();
383 auto removedInteractionIter = std::find_if(
386 [userData](std::unique_ptr<ElementInteractionData>
const& entry)
387 { return entry.get() == userData; });
397 elementVisu.
separator->setUserData(
nullptr);
417 std::unique_lock<std::mutex> lock(
stateMutex);
458 storage->begin_pullUpdatesSinceAndSendInteractions(
467 timing.
pull = time_pull - time_start;
469 timing.
applies.reserve(currentUpdates.updates.size());
470 for (data::LayerUpdate
const&
update : currentUpdates.updates)
479 if (layerIDsAfter != layerIDsBefore)
487 timing.
total = time_end - time_start;
510 std::unique_lock<std::mutex> lock(
stateMutex);
528 ARMARX_WARNING <<
"Lost connection to ArVizStorage\n" << ex.what();
536 if (layer ==
nullptr)
573 std::vector<CoinLayerID>
576 std::vector<CoinLayerID> result;
580 result.push_back(layer.
id);
621 findInteractionDataOnPath(SoPath* path)
625 void* userData =
nullptr;
626 int pathLength = path->getLength();
627 for (
int i = 0; i < pathLength; ++i)
629 SoNode* node = path->getNode(i);
630 const char* name = node->getName().getString();
631 if (strcmp(name,
"InteractiveNode") == 0)
633 userData = node->getUserData();
634 if (userData !=
nullptr)
641 return static_cast<ElementInteractionData*
>(userData);
655 if (eventType == data::InteractionFeedbackType::SELECT)
664 int enableFlags =
id->interaction.enableFlags;
665 if ((enableFlags & data::InteractionEnableFlags::SELECT) == 0)
671 if (eventType == data::InteractionFeedbackType::SELECT)
676 if (enableFlags & ANY_TRANSFORM)
683 dragger->addStartCallback(&startManipulationCallback,
this);
684 dragger->addMotionCallback(&duringManipulationCallback,
this);
685 dragger->addFinishCallback(&finishManipulationCallback,
this);
690 SoSeparator* newSep =
new SoSeparator();
691 int childNum =
id->visu->separator->getNumChildren();
692 for (
int i = 0; i < childNum; ++i)
694 SoNode* child =
id->visu->separator->getChild(i);
695 if (SoSwitch* switch_ =
dynamic_cast<SoSwitch*
>(child))
697 child = switch_->copy();
699 newSep->addChild(child);
704 if (enableFlags & data::InteractionEnableFlags::TRANSFORM_HIDE)
706 id->visu->switch_->whichChild = SO_SWITCH_NONE;
716 if (enableFlags & data::InteractionEnableFlags::TRANSFORM_HIDE)
720 SbMatrix manipMatrix;
721 manipMatrix.setTransform(1000.0f *
manipulator->translation.getValue(),
726 id->visu->transform->multLeft(manipMatrix);
728 SbVec3f translation =
id->visu->transform->translation.getValue();
729 ARMARX_IMPORTANT <<
"Visu translation: " << translation[0] <<
", " << translation[1]
730 <<
", " << translation[2];
732 id->visu->switch_->whichChild = SO_SWITCH_ALL;
740 feedback.type = eventType;
741 feedback.component =
id->layer.first;
742 feedback.layer =
id->layer.second;
743 feedback.element =
id->element;
750 SbVec3f t_o_scaled(s_m[0] * t_o[0], s_m[1] * t_o[1], s_m[2] * t_o[2]);
751 SbVec3f t_added_rotation_and_scale;
752 r_m.multVec(t_o_scaled, t_added_rotation_and_scale);
753 t_added_rotation_and_scale -= t_o;
755 SbVec3f t_added_rotation;
756 r_m.multVec(t_o, t_added_rotation);
757 t_added_rotation -= t_o;
758 SbVec3f t_added_scale = t_added_rotation_and_scale - t_added_rotation;
759 return t_added_scale;
765 SbVec3f t_o_scaled(s_m[0] * t_o[0], s_m[1] * t_o[1], s_m[2] * t_o[2]);
766 SbVec3f t_added_rotation_and_scale;
767 r_m.multVec(t_o_scaled, t_added_rotation_and_scale);
768 t_added_rotation_and_scale -= t_o;
775 return t_added_rotation_and_scale;
782 for (
int y = 0; y < 4; ++y)
784 for (
int x = 0;
x < 4; ++
x)
786 result(
x, y) = mat[y][
x];
795 constrainRotation(SbRotation
input,
int enableFlags)
798 mat.setRotate(
input);
801 Eigen::Vector3f rpy = mat_rot.eulerAngles(0, 1, 2);
803 if ((enableFlags & data::InteractionEnableFlags::ROTATION_X) == 0)
807 if ((enableFlags & data::InteractionEnableFlags::ROTATION_Y) == 0)
811 if ((enableFlags & data::InteractionEnableFlags::ROTATION_Z) == 0)
817 mat_rot = Eigen::AngleAxisf(rpy(0), Eigen::Vector3f::UnitX()) *
818 Eigen::AngleAxisf(rpy(1), Eigen::Vector3f::UnitY()) *
819 Eigen::AngleAxisf(rpy(2), Eigen::Vector3f::UnitZ());
822 SbRotation result(
q.x(),
q.y(),
q.z(),
q.w());
827 constrainScaling(SbVec3f
input,
int enableFlags)
829 SbVec3f result =
input;
830 if ((enableFlags & data::InteractionEnableFlags::SCALING_X) == 0)
834 if ((enableFlags & data::InteractionEnableFlags::SCALING_Y) == 0)
838 if ((enableFlags & data::InteractionEnableFlags::SCALING_Z) == 0)
854 ARMARX_WARNING <<
"A manipulation event was fired but no element is selected";
860 viz::data::InteractionFeedback* newFeedback =
nullptr;
863 if ((feedback.type & 0x7) == data::InteractionFeedbackType::TRANSFORM &&
869 newFeedback = &feedback;
870 newFeedback->type |= eventType;
873 if (newFeedback ==
nullptr)
880 newFeedback->type = data::InteractionFeedbackType::TRANSFORM | eventType;
888 SbRotation r_m_old =
manipulator->rotation.getValue();
889 SbVec3f s_m_old =
manipulator->scaleFactor.getValue();
893 SbRotation r_m = constrainRotation(r_m_old, enableFlags);
894 SbVec3f s_m = constrainScaling(s_m_old, enableFlags);
907 SbVec3f t_diff = t_new - t_old;
917 SbVec3f t_diff = t_new - t_old;
929 SbVec3f t_o_scaled(s_m[0] * t_o[0], s_m[1] * t_o[1], s_m[2] * t_o[2]);
930 SbVec3f t_added_rotation_and_scale;
931 r_m.multVec(t_o_scaled, t_added_rotation_and_scale);
932 t_added_rotation_and_scale -= t_o;
933 SbVec3f delta_t = t_added_rotation_and_scale + t_m;
935 SbVec3f t_added_rotation;
936 r_m.multVec(t_o, t_added_rotation);
937 t_added_rotation -= t_o;
938 SbVec3f t_added_scale = t_added_rotation_and_scale - t_added_rotation;
941 if ((enableFlags & data::InteractionEnableFlags::TRANSLATION_X) == 0)
945 if ((enableFlags & data::InteractionEnableFlags::TRANSLATION_Y) == 0)
949 if ((enableFlags & data::InteractionEnableFlags::TRANSLATION_Z) == 0)
954 SbVec3f t_m_projected = delta_t - t_added_rotation_and_scale;
959 SbVec3f t_m_non_scaled = t_m_projected + t_added_scale;
961 data::GlobalPose& transformation = newFeedback->transformation;
962 transformation.x = 1000.0f * t_m_non_scaled[0];
963 transformation.y = 1000.0f * t_m_non_scaled[1];
964 transformation.z = 1000.0f * t_m_non_scaled[2];
965 transformation.qw = r_m[3];
966 transformation.qx = r_m[0];
967 transformation.qy = r_m[1];
968 transformation.qz = r_m[2];
970 armarx::Vector3f& scale = newFeedback->scale;