Visualizer.cpp
Go to the documentation of this file.
1 #include "Visualizer.h"
2 
3 #include "ExportVRML.h"
4 
7 
8 #include <Inventor/SoPath.h>
9 
10 #include <Eigen/Core>
11 #include <Eigen/Geometry>
12 
13 namespace armarx::viz
14 {
15 namespace coin
16 {
17  void clearRobotCache();
18  void clearObjectCache();
19 }
20  static const int ANY_TRANSFORM = data::InteractionEnableFlags::TRANSLATION_X |
21  data::InteractionEnableFlags::TRANSLATION_Y |
22  data::InteractionEnableFlags::TRANSLATION_Z |
23  data::InteractionEnableFlags::ROTATION_X |
24  data::InteractionEnableFlags::ROTATION_Y |
25  data::InteractionEnableFlags::ROTATION_Z |
26  data::InteractionEnableFlags::SCALING_X |
27  data::InteractionEnableFlags::SCALING_Y |
28  data::InteractionEnableFlags::SCALING_Z;
29 
30  struct CoinVisualizerWrapper : IceUtil::Shared
31  {
33 
34  void onUpdateSuccess(data::LayerUpdates const& updates)
35  {
36  this_->onUpdateSuccess(updates);
37  }
38 
39  void onUpdateFailure(Ice::Exception const& ex)
40  {
42  }
43  };
44 
45  static void selectionCallback(void* data, SoPath* path)
46  {
47  CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
48  this_->onSelectEvent(path, data::InteractionFeedbackType::SELECT);
49  }
50 
51  static void deselectionCallback(void* data, SoPath* path)
52  {
53  CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
54  this_->onSelectEvent(path, data::InteractionFeedbackType::DESELECT);
55  }
56 
57  static void startManipulationCallback(void* data, SoDragger* dragger)
58  {
59  CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
60  this_->onManipulation(dragger, data::InteractionFeedbackType::TRANSFORM_BEGIN_FLAG);
61  }
62 
63  static void duringManipulationCallback(void* data, SoDragger* dragger)
64  {
65  CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
66  this_->onManipulation(dragger, data::InteractionFeedbackType::TRANSFORM_DURING_FLAG);
67  }
68 
69  static void finishManipulationCallback(void* data, SoDragger* dragger)
70  {
71  CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
72  this_->onManipulation(dragger, data::InteractionFeedbackType::TRANSFORM_END_FLAG);
73  }
74 
75  static const char* toString(CoinVisualizerState state)
76  {
77  switch (state)
78  {
80  return "STOPPED";
82  return "STARTING";
84  return "RUNNING";
86  return "STOPPING";
87  }
88  return "UNKNOWN";
89  }
90 
91  struct TimedBlock
92  {
93  TimedBlock(const char* function)
94  : start(IceUtil::Time::now())
95  , function(function)
96  {
97  }
98 
100  {
101  IceUtil::Time diff = IceUtil::Time::now() - start;
102  // Horrible: We need to plot this actually
103  ARMARX_INFO << "Time '" << function << "': "
104  << diff.toMilliSecondsDouble() << " ms";
105  }
106 
107  private:
108  IceUtil::Time start;
109  const char* function;
110  };
111 
113  {
115 
117  callbackData->this_ = this;
118  callback = newCallback_StorageInterface_pullUpdatesSinceAndSendInteractions(callbackData,
121 
122  manipulatorGroup = new SoSeparator;
123 
124  // The SoSelection node enable selection of nodes via mouse click / ray casting
125  selection = new SoSelection;
126  selection->addSelectionCallback(&selectionCallback, this);
127  selection->addDeselectionCallback(&deselectionCallback, this);
128  selection->setUserData(this);
129 
130  root = new SoSeparator;
131  root->addChild(manipulatorGroup);
132  root->addChild(selection);
133 
134  // Preallocate some space for layers
135  layers.data.reserve(32);
136  }
137 
139  {
140  }
141 
143  {
146  }
147 
148 
149  void CoinVisualizer::startAsync(StorageInterfacePrx const& storage)
150  {
151  std::unique_lock<std::mutex> lock(stateMutex);
153  {
154  ARMARX_WARNING << "Unexpected state of visualizer\n"
155  << "Expected: STOPPED\n"
156  << "But got: " << toString(state);
157  return;
158  }
161  }
162 
164  {
166  {
167  return;
168  }
169 
171  }
172 
174  {
175  IceUtil::Time time_start = IceUtil::Time::now();
176 
178  timing.layerName = update.name;
179 
180  CoinLayerID layerID(update.component, update.name);
181  CoinLayer& layer = findOrAddLayer(layerID);
182 
183  IceUtil::Time time_addLayer = IceUtil::Time::now();
184  timing.addLayer = time_addLayer - time_start;
185 
186  addOrUpdateElements(&layer, update);
187 
188  IceUtil::Time time_updates = IceUtil::Time::now();
189  timing.updateElements = time_updates - time_addLayer;
190 
192 
193  IceUtil::Time time_remove = IceUtil::Time::now();
194  timing.removeElements = time_remove - time_updates;
195 
196  emitLayerUpdated(layerID, layer);
197 
198  IceUtil::Time time_end = IceUtil::Time::now();
199  timing.total = time_end - time_start;
200 
201  return timing;
202  }
203 
205  {
206  auto layerIt = layers.lowerBound(layerID);
207 
208  if (layerIt == layers.data.end() || layerIt->id != layerID)
209  {
210  // Create a new layer
211  SoSeparator* coinNode = new SoSeparator;
212  coinNode->ref();
213  selection->addChild(coinNode);
214 
215  layerIt = layers.data.insert(layerIt, CoinLayer(layerID, coinNode));
216  layerIt->elements.reserve(64);
217  }
218 
219  return *layerIt;
220  }
221 
222  void CoinVisualizer::addOrUpdateElements(CoinLayer* layer, data::LayerUpdate const& update)
223  {
224  layer->elements.reserve(update.elements.size());
225  for (viz::data::ElementPtr const& updatedElementPtr : update.elements)
226  {
227  if (!updatedElementPtr)
228  {
229  ARMARX_WARNING << deactivateSpam(10) << "Element is null in layer "
230  << update.component << "/" << update.name;
231  continue;
232  }
233  data::Element const& updatedElement = *updatedElementPtr;
234 
235  std::type_index elementType = typeid(updatedElement);
236  size_t visuIndex;
237  size_t visuSize = elementVisualizersTypes.size();
238  for (visuIndex = 0; visuIndex < visuSize; ++visuIndex)
239  {
240  if (elementVisualizersTypes[visuIndex] == elementType)
241  {
242  break;
243  }
244  }
245  if (visuIndex >= visuSize)
246  {
248  << "No visualizer for element type found: "
249  << armarx::GetTypeString(elementType);
250  continue;
251  }
252  coin::ElementVisualizer* visualizer = elementVisualizers[visuIndex].get();
253 
254  auto oldElementIter = layer->lowerBound(updatedElement.id);
255  CoinLayerElement* oldElement = nullptr;
256  if (oldElementIter != layer->elements.end() && oldElementIter->data->id == updatedElement.id)
257  {
258  // Element already exists
259  CoinLayerElement* oldElement = &*oldElementIter;
260  coin::ElementVisualization& oldElementVisu = *oldElement->visu;
261 
262  oldElementVisu.wasUpdated = true;
263 
264  bool updated = visualizer->update(updatedElement, &oldElementVisu);
265 
266  if (updated)
267  {
268  // Has an interaction been added?
269  viz::data::InteractionDescription& oldInteraction = oldElement->data->interaction;
270  viz::data::InteractionDescription& newInteraction = updatedElementPtr->interaction;
271  if (newInteraction.enableFlags != oldInteraction.enableFlags
272  || oldInteraction.contextMenuOptions != newInteraction.contextMenuOptions)
273  {
274  addOrUpdateInteraction(layer->id, updatedElement.id, newInteraction, oldElement->visu.get());
275  }
276 
277  oldElement->data = updatedElementPtr;
278  continue;
279  }
280  else
281  {
282  // Types are different, so we delete the old element and create a new one
283  layer->node->removeChild(oldElementVisu.separator);
284  }
285  }
286 
287  auto elementVisu = visualizer->create(updatedElement);
288  if (elementVisu->separator)
289  {
290  // Has the new element interactions?
291  viz::data::InteractionDescription& newInteraction = updatedElementPtr->interaction;
292  if (newInteraction.enableFlags != 0)
293  {
294  addOrUpdateInteraction(layer->id, updatedElement.id, newInteraction, elementVisu.get());
295  }
296 
297  layer->node->addChild(elementVisu->separator);
298  if (oldElement)
299  {
300  oldElement->data = updatedElementPtr;
301  oldElement->visu = std::move(elementVisu);
302  }
303  else
304  {
305  // Need to add a new element
306  layer->elements.insert(oldElementIter, CoinLayerElement{updatedElementPtr, std::move(elementVisu)});
307  }
308  }
309  else
310  {
311  std::string typeName = armarx::GetTypeString(elementType);
312  ARMARX_WARNING << deactivateSpam(typeName, 1)
313  << "CoinElementVisualizer returned null for type: " << typeName << "\n"
314  << "You need to register a visualizer for each type in ArViz/Coin/RegisterVisualizationTypes.cpp";
315  }
316  }
317  }
318 
319  void CoinVisualizer::addOrUpdateInteraction(CoinLayerID const& layerID, std::string const& elementID,
320  data::InteractionDescription const& interactionDesc,
322  {
323  // Lookup the interaction entry
324  ElementInteractionData* foundInteraction = nullptr;
325  for (auto& interaction : elementInteractions)
326  {
327  if (interaction->layer == layerID
328  && interaction->element == elementID)
329  {
330  foundInteraction = interaction.get();
331  }
332  }
333  if (foundInteraction == nullptr)
334  {
335  // Need to add a new entry
336  foundInteraction = elementInteractions.emplace_back(
337  new ElementInteractionData).get();
338  }
339  foundInteraction->layer = layerID;
340  foundInteraction->element = elementID;
341 
342  foundInteraction->interaction = interactionDesc;
343  foundInteraction->visu = visu;
344 
345  // Add user data to Coin node
346  visu->separator->setUserData(foundInteraction);
347  visu->separator->setName("InteractiveNode");
348  }
349 
351  {
352  for (auto iter = layer->elements.begin(); iter != layer->elements.end();)
353  {
354  coin::ElementVisualization& elementVisu = *iter->visu;
355  if (elementVisu.wasUpdated)
356  {
357  elementVisu.wasUpdated = false;
358  ++iter;
359  }
360  else
361  {
362  void* userData = elementVisu.separator->getUserData();
363  if (userData)
364  {
365  // Remove interaction entry if element has been removed
366  auto removedInteractionIter = std::find_if(elementInteractions.begin(), elementInteractions.end(),
367  [userData](std::unique_ptr<ElementInteractionData> const& entry)
368  {
369  return entry.get() == userData;
370  });
371  ElementInteractionData* removedInteraction = removedInteractionIter->get();
372  if (removedInteraction == selectedElement)
373  {
374  // The selected element is removed ==> Therefore deselect it
375  selection->deselectAll();
376  selectedElement = nullptr;
377  }
378  elementInteractions.erase(removedInteractionIter);
379 
380  elementVisu.separator->setUserData(nullptr);
381  elementVisu.separator->setName("");
382  }
383  layer->node->removeChild(elementVisu.separator);
384  iter = layer->elements.erase(iter);
385  }
386  }
387  }
388 
390  {
391  {
392  std::lock_guard lock(timingMutex);
394  }
395  switch (state)
396  {
398  {
399  std::unique_lock<std::mutex> lock(stateMutex);
401  selection->deselectAll();
402  selection->removeAllChildren();
403  manipulatorGroup->removeAllChildren();
405  elementInteractions.clear();
406  selectedElement = nullptr;
407  layers.data.clear();
408  pulledUpdates.revision = 0;
409  pulledUpdates.updates.clear();
412  }
413  break;
414 
416  break;
417 
419  {
420  // After we have reached the STOPPED state, we know that updates are no longer running
422  return;
423  }
424  break;
425 
427  return;
428  }
429 
430  switch (updateResult)
431  {
433  {
434  IceUtil::Time time_start = IceUtil::Time::now();
436 
437  data::LayerUpdates currentUpdates = pulledUpdates;
439  timing.waitStart = time_start;
440  storage->begin_pullUpdatesSinceAndSendInteractions(
441  currentUpdates.revision, interactionFeedbackBuffer, callback);
442 
443  // Clear interaction feedback buffer after it has been sent
445 
446  auto layerIDsBefore = getLayerIDs();
447 
448  IceUtil::Time time_pull = IceUtil::Time::now();
449  timing.pull = time_pull - time_start;
450 
451  timing.applies.reserve(currentUpdates.updates.size());
452  for (data::LayerUpdate const& update : currentUpdates.updates)
453  {
454  auto& applyTiming = timing.applies.emplace_back(apply(update));
455  timing.applyTotal.add(applyTiming);
456  }
457  IceUtil::Time time_apply = IceUtil::Time::now();
458  timing.applyTotal.total = time_apply - time_pull;
459 
460  auto layerIDsAfter = getLayerIDs();
461  if (layerIDsAfter != layerIDsBefore)
462  {
463  emitLayersChanged(layerIDsAfter);
464  }
465  IceUtil::Time time_layersChanged = IceUtil::Time::now();
466  timing.layersChanged = time_layersChanged - time_apply;
467 
468  IceUtil::Time time_end = IceUtil::Time::now();
469  timing.total = time_end - time_start;
470 
471  {
472  // Copy the timing result
473  std::lock_guard lock(timingMutex);
474  timing.counter = lastTiming.counter + 1;
476  lastTiming = std::move(timing);
477  }
478  }
479  break;
481  {
482  // Still waiting for result
483  {
484  // Copy the timing result
485  std::lock_guard lock(timingMutex);
486  lastTiming.waitDuration = IceUtil::Time::now() - lastTiming.waitStart;
487  }
488  }
489  break;
491  {
492  std::unique_lock<std::mutex> lock(stateMutex);
493  storage = nullptr;
495  }
496  break;
497  }
498 
499  }
500 
501  void CoinVisualizer::onUpdateSuccess(const data::LayerUpdates& updates)
502  {
503  pulledUpdates = updates;
505  }
506 
507  void CoinVisualizer::onUpdateFailure(const Ice::Exception& ex)
508  {
509  ARMARX_WARNING << "Lost connection to ArVizStorage\n"
510  << ex.what();
512  }
513 
514  void CoinVisualizer::showLayer(CoinLayerID const& id, bool visible)
515  {
516  CoinLayer* layer = layers.findLayer(id);
517  if (layer == nullptr)
518  {
519  return;
520  }
521 
522  int childIndex = selection->findChild(layer->node);
523  if (childIndex < 0)
524  {
525  // Layer is currently not visible
526  if (visible)
527  {
528  selection->addChild(layer->node);
529  }
530  }
531  else
532  {
533  // Layer is currently visible
534  if (!visible)
535  {
536  // If we hide a layer wit a selected element, Coin will crash during rendering
537  // So we check if the selected element is in the layer to be hidden
538  if (selectedElement && selectedElement->layer == id)
539  {
540  selection->deselectAll();
541  }
542  selection->removeChild(childIndex);
543  }
544  }
545  }
546 
548  {
549  std::lock_guard lock(timingMutex);
550  return lastTiming;
551  }
552 
553  std::vector<CoinLayerID> CoinVisualizer::getLayerIDs()
554  {
555  std::vector<CoinLayerID> result;
556  result.reserve(layers.data.size());
557  for (CoinLayer& layer : layers.data)
558  {
559  result.push_back(layer.id);
560  }
561  return result;
562  }
563 
564  void CoinVisualizer::emitLayersChanged(std::vector<CoinLayerID> const& layerIDs)
565  {
567  {
568  layersChangedCallback(layerIDs);
569  }
570  }
571 
572  void CoinVisualizer::emitLayerUpdated(const CoinLayerID& layerID, const CoinLayer& layer)
573  {
574  for (auto& callback : layerUpdatedCallbacks)
575  {
576  if (callback)
577  {
578  callback(layerID, layer);
579  }
580  }
581  }
582 
584  {
585  if (index >= (int)elementInteractions.size())
586  {
587  return;
588  }
589 
591 
592  selection->deselectAll();
593  selection->select(id->visu->separator);
594  }
595 
596  static ElementInteractionData* findInteractionDataOnPath(SoPath* path)
597  {
598  // Search for user data in the path
599  // We stored ElementInteractionData into the user data of the parent SoSeparator
600  void* userData = nullptr;
601  int pathLength = path->getLength();
602  for (int i = 0; i < pathLength; ++i)
603  {
604  SoNode* node = path->getNode(i);
605  const char* name = node->getName().getString();
606  if (strcmp(name, "InteractiveNode") == 0)
607  {
608  userData = node->getUserData();
609  if (userData != nullptr)
610  {
611  break;
612  }
613  }
614  }
615 
616  return static_cast<ElementInteractionData*>(userData);
617  }
618 
619  void CoinVisualizer::onSelectEvent(SoPath* path, int eventType)
620  {
622  {
623  return;
624  }
625 
626  ElementInteractionData* id = findInteractionDataOnPath(path);
627  if (id == nullptr)
628  {
629  if (eventType == data::InteractionFeedbackType::SELECT)
630  {
631  // An object was selected that does not have any interactions enabled
632  // We deselect all other elements in this case to avoid highlighting
633  // non-interactable elements
634  selection->deselectAll();
635  }
636  return;
637  }
638  int enableFlags = id->interaction.enableFlags;
639  if ((enableFlags & data::InteractionEnableFlags::SELECT) == 0)
640  {
641  // Element was not marked for selection
642  return;
643  }
644 
645  if (eventType == data::InteractionFeedbackType::SELECT)
646  {
647  selectedElement = id;
648 
649  // Does the element support transform interactions?
650  if (enableFlags & ANY_TRANSFORM)
651  {
652  manipulatorGroup->removeAllChildren();
653  // We need to create the manipulator everytime to achieve the desired effect
654  // If we reuse an instance, the manipulator gets stuck to old objects...
655  manipulator = new SoTransformerManip;
656  SoDragger* dragger = manipulator->getDragger();
657  dragger->addStartCallback(&startManipulationCallback, this);
658  dragger->addMotionCallback(&duringManipulationCallback, this);
659  dragger->addFinishCallback(&finishManipulationCallback, this);
660 
661  manipulatorGroup->addChild(manipulator);
662 
663  // We add the same visualization node again
664  SoSeparator* newSep = new SoSeparator();
665  int childNum = id->visu->separator->getNumChildren();
666  for (int i = 0; i < childNum; ++i)
667  {
668  SoNode* child = id->visu->separator->getChild(i);
669  if (SoSwitch* switch_ = dynamic_cast<SoSwitch*>(child))
670  {
671  child = switch_->copy();
672  }
673  newSep->addChild(child);
674  }
675  manipulatorGroup->addChild(newSep);
676  manipulator->unsquishKnobs();
677 
678  if (enableFlags & data::InteractionEnableFlags::TRANSFORM_HIDE)
679  {
680  id->visu->switch_->whichChild = SO_SWITCH_NONE;
681  }
682 
683  }
684  else
685  {
686  selection->touch();
687  }
688  }
689  else
690  {
691  if (enableFlags & data::InteractionEnableFlags::TRANSFORM_HIDE)
692  {
693  // Now, we should apply the transformation to the original object (at least temporary)
694  // This avoids flickering, after the object is deselected
695  SbMatrix manipMatrix;
696  manipMatrix.setTransform(1000.0f * manipulator->translation.getValue(), manipulator->rotation.getValue(),
697  manipulator->scaleFactor.getValue(), manipulator->scaleOrientation.getValue(),
698  manipulator->center.getValue());
699  id->visu->transform->multLeft(manipMatrix);
700 
701  SbVec3f translation = id->visu->transform->translation.getValue();
702  ARMARX_IMPORTANT << "Visu translation: " << translation[0] << ", " << translation[1] << ", " << translation[2];
703 
704  id->visu->switch_->whichChild = SO_SWITCH_ALL;
705  }
706 
707  selectedElement = nullptr;
708  manipulatorGroup->removeAllChildren();
709  }
710 
711  viz::data::InteractionFeedback& feedback = interactionFeedbackBuffer.emplace_back();
712  feedback.type = eventType;
713  feedback.component = id->layer.first;
714  feedback.layer = id->layer.second;
715  feedback.element = id->element;
716  feedback.revision = pulledUpdates.revision;
717  }
718 
719  SbVec3f translationDueToScaling(SbRotation r_m, SbVec3f t_m, SbVec3f s_m, SbVec3f t_o)
720  {
721  SbVec3f t_o_scaled(s_m[0] * t_o[0], s_m[1] * t_o[1], s_m[2] * t_o[2]);
722  SbVec3f t_added_rotation_and_scale;
723  r_m.multVec(t_o_scaled, t_added_rotation_and_scale);
724  t_added_rotation_and_scale -= t_o;
725 
726  SbVec3f t_added_rotation;
727  r_m.multVec(t_o, t_added_rotation);
728  t_added_rotation -= t_o;
729  SbVec3f t_added_scale = t_added_rotation_and_scale - t_added_rotation;
730  return t_added_scale;
731  }
732 
733  SbVec3f translationDueToRotation(SbRotation r_m, SbVec3f t_m, SbVec3f s_m, SbVec3f t_o)
734  {
735  SbVec3f t_o_scaled(s_m[0] * t_o[0], s_m[1] * t_o[1], s_m[2] * t_o[2]);
736  SbVec3f t_added_rotation_and_scale;
737  r_m.multVec(t_o_scaled, t_added_rotation_and_scale);
738  t_added_rotation_and_scale -= t_o;
739 
740  // Do we need to exclude
741 // SbVec3f t_added_rotation;
742 // r_m.multVec(t_o, t_added_rotation);
743 // t_added_rotation -= t_o;
744 // SbVec3f t_added_scale = t_added_rotation_and_scale - t_added_rotation;
745  return t_added_rotation_and_scale;
746  }
747 
748  Eigen::Matrix4f toEigen(SbMat const& mat)
749  {
750  Eigen::Matrix4f result;
751  for (int y = 0; y < 4; ++y)
752  {
753  for (int x = 0; x < 4; ++x)
754  {
755  result(x, y) = mat[y][x];
756  }
757  }
758 
759  return result;
760  }
761 
762  // This should constrain the rotation according to the enabled axes
763  static SbRotation constrainRotation(SbRotation input, int enableFlags)
764  {
765  SbMatrix mat;
766  mat.setRotate(input);
767  Eigen::Matrix4f mat_eigen = toEigen(mat);
768  Eigen::Matrix3f mat_rot = mat_eigen.block<3, 3>(0, 0);
769  Eigen::Vector3f rpy = mat_rot.eulerAngles(0, 1, 2);
770  // ARMARX_INFO << "rpy before: " << rpy.transpose();
771  if ((enableFlags & data::InteractionEnableFlags::ROTATION_X) == 0)
772  {
773  rpy(0) = 0.0f;
774  }
775  if ((enableFlags & data::InteractionEnableFlags::ROTATION_Y) == 0)
776  {
777  rpy(1) = 0.0f;
778  }
779  if ((enableFlags & data::InteractionEnableFlags::ROTATION_Z) == 0)
780  {
781  rpy(2) = 0.0f;
782  }
783  // ARMARX_INFO << "rpy after: " << rpy.transpose();
784 
785  mat_rot = Eigen::AngleAxisf(rpy(0), Eigen::Vector3f::UnitX())
786  * Eigen::AngleAxisf(rpy(1), Eigen::Vector3f::UnitY())
787  * Eigen::AngleAxisf(rpy(2), Eigen::Vector3f::UnitZ());
788  Eigen::Quaternionf q(mat_rot);
789 
790  SbRotation result(q.x(), q.y(), q.z(), q.w());
791  return result;
792  }
793 
794  static SbVec3f constrainScaling(SbVec3f input, int enableFlags)
795  {
796  SbVec3f result = input;
797  if ((enableFlags & data::InteractionEnableFlags::SCALING_X) == 0)
798  {
799  result[0] = 1.0f;
800  }
801  if ((enableFlags & data::InteractionEnableFlags::SCALING_Y) == 0)
802  {
803  result[1] = 1.0f;
804  }
805  if ((enableFlags & data::InteractionEnableFlags::SCALING_Z) == 0)
806  {
807  result[2] = 1.0f;
808  }
809  return result;
810  }
811 
812  void CoinVisualizer::onManipulation(SoDragger* dragger, int eventType)
813  {
815  {
816  return;
817  }
818  if (selectedElement == nullptr)
819  {
820  ARMARX_WARNING << "A manipulation event was fired but no element is selected";
821  return;
822  }
823 
824  // If there is an entry in the feedback buffer already, update it
825  // This way, we prevent multiple events being sent to the client
826  viz::data::InteractionFeedback* newFeedback = nullptr;
827  for (viz::data::InteractionFeedback& feedback : interactionFeedbackBuffer)
828  {
829  if ((feedback.type & 0x7) == data::InteractionFeedbackType::TRANSFORM
830  && feedback.component == selectedElement->layer.first
831  && feedback.layer == selectedElement->layer.second
832  && feedback.element == selectedElement->element)
833  {
834  // This is a transform interaction concerning the same element
835  newFeedback = &feedback;
836  newFeedback->type |= eventType;
837  }
838  }
839  if (newFeedback == nullptr)
840  {
841  // Create a new interaction
842  newFeedback = &interactionFeedbackBuffer.emplace_back();
843  newFeedback->component = selectedElement->layer.first;
844  newFeedback->layer = selectedElement->layer.second;
845  newFeedback->element = selectedElement->element;
846  newFeedback->type = data::InteractionFeedbackType::TRANSFORM | eventType;
847  }
848 
849  newFeedback->revision = pulledUpdates.revision;
850 
851 
852  // Transformation applied by the manipulator
853  SbVec3f t_m = manipulator->translation.getValue();
854  SbRotation r_m_old = manipulator->rotation.getValue();
855  SbVec3f s_m_old = manipulator->scaleFactor.getValue();
856 
857  int enableFlags = selectedElement->interaction.enableFlags;
858 
859  SbRotation r_m = constrainRotation(r_m_old, enableFlags);
860  SbVec3f s_m = constrainScaling(s_m_old, enableFlags);
861 
862  // Transformation applied to the object
863  // Translation is in mm, but the manipulator works in m!
864  SbVec3f t_o = 0.001f * selectedElement->visu->transform->translation.getValue();
865 
866  if (s_m != s_m_old)
867  {
868  manipulator->scaleFactor.setValue(s_m);
869 
870  // Remove motion induced by scaling
871  SbVec3f t_old = translationDueToScaling(r_m_old, t_m, s_m_old, t_o);
872  SbVec3f t_new = translationDueToScaling(r_m, t_m, s_m, t_o);
873  SbVec3f t_diff = t_new - t_old;
874  t_m -= t_diff;
875  }
876  if (r_m != r_m_old)
877  {
878  manipulator->rotation.setValue(r_m);
879 
880  // Remove motion induced by rotation
881  SbVec3f t_old = translationDueToRotation(r_m_old, t_m, s_m, t_o);
882  SbVec3f t_new = translationDueToRotation(r_m, t_m, s_m, t_o);
883  SbVec3f t_diff = t_new - t_old;
884  t_m -= t_diff;
885  }
886 
887  // TODO: Should we use the rotation of the object?
888  // SbRotation r_o = selectedElement->visu->transform->rotation.getValue();
889  // TODO: Do we need to consider scale of the object as well?
890  // SbVec3f s_o = selectedElement->visu->transform->scaleFactor.getValue();
891 
892  // This value stays constant during rotation and scaling!
893  // It is zero when the manipulation first starts
894  // So we can reproduce the perceived motion here (hopefully)
895  SbVec3f t_o_scaled(s_m[0] * t_o[0], s_m[1] * t_o[1], s_m[2] * t_o[2]);
896  SbVec3f t_added_rotation_and_scale;
897  r_m.multVec(t_o_scaled, t_added_rotation_and_scale);
898  t_added_rotation_and_scale -= t_o;
899  SbVec3f delta_t = t_added_rotation_and_scale + t_m;
900 
901  SbVec3f t_added_rotation;
902  r_m.multVec(t_o, t_added_rotation);
903  t_added_rotation -= t_o;
904  SbVec3f t_added_scale = t_added_rotation_and_scale - t_added_rotation;
905 
906  // Prevent translation along disabled axes
907  if ((enableFlags & data::InteractionEnableFlags::TRANSLATION_X) == 0)
908  {
909  delta_t[0] = 0.0f;
910  }
911  if ((enableFlags & data::InteractionEnableFlags::TRANSLATION_Y) == 0)
912  {
913  delta_t[1] = 0.0f;
914  }
915  if ((enableFlags & data::InteractionEnableFlags::TRANSLATION_Z) == 0)
916  {
917  delta_t[2] = 0.0f;
918  }
919 
920  SbVec3f t_m_projected = delta_t - t_added_rotation_and_scale;
921  manipulator->translation.setValue(t_m_projected);
922 
923  // t_m_projected is the correct value for the manipulator, but it still contains translation due to scaling!
924  // We should subtract the translation induced by scaling!
925  SbVec3f t_m_non_scaled = t_m_projected + t_added_scale;
926 
927  data::GlobalPose& transformation = newFeedback->transformation;
928  transformation.x = 1000.0f * t_m_non_scaled[0];
929  transformation.y = 1000.0f * t_m_non_scaled[1];
930  transformation.z = 1000.0f * t_m_non_scaled[2];
931  transformation.qw = r_m[3];
932  transformation.qx = r_m[0];
933  transformation.qy = r_m[1];
934  transformation.qz = r_m[2];
935 
936  armarx::Vector3f& scale = newFeedback->scale;
937  scale.e0 = s_m[0];
938  scale.e1 = s_m[1];
939  scale.e2 = s_m[2];
940  }
941 
942  void CoinVisualizer::exportToVRML(const std::string& exportFilePath)
943  {
944  coin::exportToVRML(selection, exportFilePath);
945  }
946 }
armarx::viz::CoinVisualizer::selectedElement
ElementInteractionData * selectedElement
Definition: Visualizer.h:253
armarx::viz::CoinLayer
Definition: Visualizer.h:36
armarx::viz::CoinVisualizer_ApplyTiming::removeElements
IceUtil::Time removeElements
Definition: Visualizer.h:158
armarx::viz::CoinVisualizer::findOrAddLayer
CoinLayer & findOrAddLayer(CoinLayerID const &layerID)
Definition: Visualizer.cpp:204
armarx::viz::coin::exportToVRML
void exportToVRML(SoNode *node, std::string const &exportFilePath)
Definition: ExportVRML.cpp:68
armarx::viz::CoinVisualizer_UpdateTiming::waitDuration
IceUtil::Time waitDuration
Definition: Visualizer.h:181
armarx::viz::CoinLayerMap::findLayer
CoinLayer * findLayer(CoinLayerID const &id)
Definition: Visualizer.h:118
armarx::viz::CoinLayer::elements
std::vector< CoinLayerElement > elements
Definition: Visualizer.h:101
armarx::viz::CoinVisualizer::pulledUpdates
data::LayerUpdates pulledUpdates
Definition: Visualizer.h:274
armarx::viz::CoinVisualizer::state
std::atomic< CoinVisualizerState > state
Definition: Visualizer.h:277
armarx::viz::CoinVisualizer::selectElement
void selectElement(int index)
Definition: Visualizer.cpp:583
armarx::viz::coin::ElementVisualization
Definition: ElementVisualizer.h:21
armarx::viz::CoinVisualizer::elementVisualizers
std::vector< std::unique_ptr< coin::ElementVisualizer > > elementVisualizers
Definition: Visualizer.h:266
armarx::viz::CoinVisualizer_UpdateTiming::total
IceUtil::Time total
Definition: Visualizer.h:178
armarx::viz::CoinVisualizer::showLayer
void showLayer(CoinLayerID const &id, bool visible)
Definition: Visualizer.cpp:514
armarx::viz::CoinVisualizer::onSelectEvent
void onSelectEvent(SoPath *path, int eventType)
Definition: Visualizer.cpp:619
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
armarx::viz::toEigen
Eigen::Matrix4f toEigen(data::GlobalPose const &pose)
Definition: Interaction.h:46
armarx::viz::ElementInteractionData::visu
coin::ElementVisualization * visu
Definition: Visualizer.h:195
armarx::viz::interaction
InteractionDescription interaction()
Definition: ElementOps.h:101
GetTypeString.h
index
uint8_t index
Definition: EtherCATFrame.h:59
armarx::viz::CoinVisualizer::clearCache
void clearCache()
Definition: Visualizer.cpp:142
armarx::viz::CoinVisualizer::startAsync
void startAsync(StorageInterfacePrx const &storage)
Definition: Visualizer.cpp:149
armarx::viz::CoinVisualizer::stateStorage
viz::StorageInterfacePrx stateStorage
Definition: Visualizer.h:278
armarx::viz::CoinVisualizer::manipulator
SoTransformerManip * manipulator
Definition: Visualizer.h:271
armarx::viz::CoinVisualizer::selection
SoSelection * selection
Definition: Visualizer.h:268
armarx::viz::coin::clearObjectCache
void clearObjectCache()
Definition: VisualizationObject.cpp:245
armarx::viz::ElementInteractionData::layer
CoinLayerID layer
Definition: Visualizer.h:192
armarx::viz::CoinVisualizer::layerUpdatedCallbacks
std::vector< std::function< void(CoinLayerID const &layerID, CoinLayer const &layer)> > layerUpdatedCallbacks
A layer's data has changed.
Definition: Visualizer.h:283
armarx::viz::CoinVisualizer::getLayerIDs
std::vector< CoinLayerID > getLayerIDs()
Definition: Visualizer.cpp:553
armarx::viz::CoinVisualizer::onUpdateFailure
void onUpdateFailure(Ice::Exception const &ex)
Definition: Visualizer.cpp:507
armarx::viz::CoinVisualizerState::STOPPING
@ STOPPING
armarx::viz::CoinVisualizer::emitLayerUpdated
void emitLayerUpdated(CoinLayerID const &layerID, CoinLayer const &layer)
Definition: Visualizer.cpp:572
armarx::viz::CoinVisualizer::removeElementsIfNotUpdated
void removeElementsIfNotUpdated(CoinLayer *layer)
Definition: Visualizer.cpp:350
armarx::viz::coin::clearRobotCache
void clearRobotCache()
Definition: VisualizationRobot.cpp:352
armarx::viz::CoinVisualizer::stateMutex
std::mutex stateMutex
Definition: Visualizer.h:276
armarx::viz::CoinVisualizer::onManipulation
void onManipulation(SoDragger *dragger, int eventType)
Definition: Visualizer.cpp:812
armarx::viz::CoinVisualizer_UpdateTiming::pull
IceUtil::Time pull
Definition: Visualizer.h:176
armarx::viz::coin::ElementVisualizer::update
bool update(data::Element const &element, ElementVisualization *visu)
Definition: ElementVisualizer.cpp:59
armarx::viz::CoinVisualizer::callback
armarx::viz::Callback_StorageInterface_pullUpdatesSinceAndSendInteractionsPtr callback
Definition: Visualizer.h:258
armarx::viz::translationDueToScaling
SbVec3f translationDueToScaling(SbRotation r_m, SbVec3f t_m, SbVec3f s_m, SbVec3f t_o)
Definition: Visualizer.cpp:719
armarx::viz::CoinVisualizer_ApplyTiming::updateElements
IceUtil::Time updateElements
Definition: Visualizer.h:157
armarx::viz::CoinLayer::node
SoSeparator * node
Definition: Visualizer.h:100
armarx::viz::CoinVisualizerState::RUNNING
@ RUNNING
IceUtil
Definition: Instance.h:21
armarx::viz::CoinVisualizer_ApplyTiming::add
void add(CoinVisualizer_ApplyTiming const &other)
Definition: Visualizer.h:161
armarx::viz::TimedBlock::TimedBlock
TimedBlock(const char *function)
Definition: Visualizer.cpp:93
armarx::viz::coin::ElementVisualization::transform
SoTransform * transform
Definition: ElementVisualizer.h:31
ExportVRML.h
armarx::viz::CoinVisualizerState::STARTING
@ STARTING
armarx::viz::CoinVisualizer_UpdateTiming::updateToggle
int updateToggle
Definition: Visualizer.h:183
armarx::viz::CoinVisualizer_UpdateTiming::counter
int counter
Definition: Visualizer.h:185
armarx::viz::CoinVisualizerWrapper::onUpdateFailure
void onUpdateFailure(Ice::Exception const &ex)
Definition: Visualizer.cpp:39
armarx::viz::CoinVisualizer_UpdateTiming
Definition: Visualizer.h:170
armarx::viz::CoinLayer::lowerBound
auto lowerBound(std::string const &id)
Definition: Visualizer.h:78
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:72
armarx::viz::CoinVisualizerUpdateResult::FAILURE
@ FAILURE
armarx::viz::ElementInteractionData::interaction
viz::data::InteractionDescription interaction
Definition: Visualizer.h:194
armarx::viz::ElementInteractionData
Definition: Visualizer.h:190
armarx::viz::CoinLayer::id
CoinLayerID id
Definition: Visualizer.h:99
armarx::viz::CoinVisualizer::callbackData
IceUtil::Handle< CoinVisualizerWrapper > callbackData
Definition: Visualizer.h:257
armarx::viz::CoinVisualizer::CoinVisualizer
CoinVisualizer()
Definition: Visualizer.cpp:112
armarx::viz::CoinVisualizerUpdateResult::WAITING
@ WAITING
armarx::viz::CoinLayerMap::data
std::vector< CoinLayer > data
Definition: Visualizer.h:133
armarx::viz::CoinVisualizer_UpdateTiming::waitStart
IceUtil::Time waitStart
Definition: Visualizer.h:180
armarx::viz::translationDueToRotation
SbVec3f translationDueToRotation(SbRotation r_m, SbVec3f t_m, SbVec3f s_m, SbVec3f t_o)
Definition: Visualizer.cpp:733
armarx::viz::CoinVisualizer_UpdateTiming::layersChanged
IceUtil::Time layersChanged
Definition: Visualizer.h:177
armarx::viz::CoinVisualizer::registerVisualizationTypes
void registerVisualizationTypes()
Definition: RegisterVisualizationTypes.cpp:21
armarx::viz::CoinVisualizerWrapper::onUpdateSuccess
void onUpdateSuccess(data::LayerUpdates const &updates)
Definition: Visualizer.cpp:34
armarx::viz::CoinVisualizer::addOrUpdateInteraction
void addOrUpdateInteraction(CoinLayerID const &layerID, std::string const &elementID, data::InteractionDescription const &interactionDesc, coin::ElementVisualization *visu)
Definition: Visualizer.cpp:319
armarx::viz::ElementInteractionData::element
std::string element
Definition: Visualizer.h:193
armarx::viz::CoinVisualizer::~CoinVisualizer
~CoinVisualizer()
Definition: Visualizer.cpp:138
armarx::viz::CoinVisualizer::emitLayersChanged
void emitLayersChanged(std::vector< CoinLayerID > const &layerIDs)
Definition: Visualizer.cpp:564
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
armarx::aron::input
ReaderT::InputType & input
Definition: rw.h:19
armarx::viz::CoinVisualizer_ApplyTiming::total
IceUtil::Time total
Definition: Visualizer.h:159
armarx::viz::CoinVisualizer::layers
CoinLayerMap layers
Definition: Visualizer.h:263
armarx::viz::CoinLayerElement::data
data::ElementPtr data
Definition: Visualizer.h:26
armarx::viz::CoinLayerElement
Definition: Visualizer.h:24
armarx::viz::CoinVisualizer::stop
void stop()
Definition: Visualizer.cpp:163
armarx::viz::TimedBlock
Definition: Visualizer.cpp:91
armarx::viz::CoinVisualizer::exportToVRML
void exportToVRML(std::string const &exportFilePath)
Definition: Visualizer.cpp:942
armarx::viz::CoinVisualizer::addOrUpdateElements
void addOrUpdateElements(CoinLayer *layer, data::LayerUpdate const &update)
Definition: Visualizer.cpp:222
armarx::viz::CoinVisualizer::update
void update()
Definition: Visualizer.cpp:389
armarx::viz::CoinVisualizerState
CoinVisualizerState
Definition: Visualizer.h:137
armarx::viz::CoinVisualizer_ApplyTiming::layerName
std::string layerName
Definition: Visualizer.h:154
armarx::viz::CoinVisualizer_UpdateTiming::applyTotal
CoinVisualizer_ApplyTiming applyTotal
Definition: Visualizer.h:174
armarx::viz::CoinLayerElement::visu
std::unique_ptr< coin::ElementVisualization > visu
Definition: Visualizer.h:27
armarx::viz::TimedBlock::~TimedBlock
~TimedBlock()
Definition: Visualizer.cpp:99
armarx::viz::CoinVisualizerWrapper::this_
class CoinVisualizer * this_
Definition: Visualizer.cpp:32
armarx::viz::CoinVisualizer::root
SoSeparator * root
Definition: Visualizer.h:269
Visualizer.h
q
#define q
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::viz::CoinVisualizer::lastTiming
CoinVisualizer_UpdateTiming lastTiming
Definition: Visualizer.h:286
armarx::armem::server::ltm::util::mongodb::detail::update
bool update(mongocxx::collection &coll, const nlohmann::json &query, const nlohmann::json &update)
Definition: mongodb.cpp:67
armarx::GetTypeString
std::string GetTypeString(const std::type_info &tinf, bool withoutNamespaceSpecifier=false)
Definition: GetTypeString.h:36
armarx::viz::CoinVisualizer
Definition: Visualizer.h:198
armarx::viz::CoinVisualizer::getTiming
CoinVisualizer_UpdateTiming getTiming()
Definition: Visualizer.cpp:547
armarx::viz::CoinVisualizerState::STOPPED
@ STOPPED
armarx::viz::CoinVisualizer::storage
viz::StorageInterfacePrx storage
Definition: Visualizer.h:261
armarx::viz::CoinVisualizer::apply
CoinVisualizer_ApplyTiming apply(data::LayerUpdate const &update)
Definition: Visualizer.cpp:173
armarx::viz::CoinVisualizer::onUpdateSuccess
void onUpdateSuccess(data::LayerUpdates const &updates)
Definition: Visualizer.cpp:501
armarx::viz::CoinVisualizer_UpdateTiming::applies
std::vector< CoinVisualizer_ApplyTiming > applies
Definition: Visualizer.h:172
armarx::viz::CoinVisualizer::timingMutex
std::mutex timingMutex
Definition: Visualizer.h:285
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::viz::CoinVisualizer_ApplyTiming
Definition: Visualizer.h:152
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::viz::CoinVisualizerWrapper
Definition: Visualizer.cpp:30
armarx::viz::CoinVisualizer::elementInteractions
std::vector< std::unique_ptr< ElementInteractionData > > elementInteractions
Definition: Visualizer.h:250
armarx::viz::coin::ElementVisualizer
Definition: ElementVisualizer.h:39
armarx::Quaternion< float, 0 >
armarx::viz::CoinVisualizer::updateResult
std::atomic< CoinVisualizerUpdateResult > updateResult
Definition: Visualizer.h:273
armarx::viz::toString
const char * toString(InteractionFeedbackType type)
Definition: Interaction.h:27
armarx::viz::coin::ElementVisualization::wasUpdated
bool wasUpdated
Definition: ElementVisualizer.h:35
armarx::viz::coin::ElementVisualizer::create
std::unique_ptr< ElementVisualization > create(data::Element const &element)
Definition: ElementVisualizer.cpp:51
armarx::viz::coin::ElementVisualization::separator
SoSeparator * separator
Definition: ElementVisualizer.h:29
armarx::viz::CoinVisualizer::layersChangedCallback
std::function< void(std::vector< CoinLayerID > const &)> layersChangedCallback
Definition: Visualizer.h:280
Logging.h
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::viz::CoinVisualizerUpdateResult::SUCCESS
@ SUCCESS
armarx::viz::CoinVisualizer::elementVisualizersTypes
std::vector< std::type_index > elementVisualizersTypes
Definition: Visualizer.h:265
armarx::viz::CoinLayerMap::lowerBound
auto lowerBound(CoinLayerID const &id)
Definition: Visualizer.h:112
armarx::viz
This file is part of ArmarX.
Definition: ArVizStorage.cpp:416
armarx::viz::CoinLayerID
std::pair< std::string, std::string > CoinLayerID
Definition: Visualizer.h:21
armarx::viz::CoinVisualizer_ApplyTiming::addLayer
IceUtil::Time addLayer
Definition: Visualizer.h:156
armarx::viz::CoinVisualizer::interactionFeedbackBuffer
std::vector< viz::data::InteractionFeedback > interactionFeedbackBuffer
Definition: Visualizer.h:251
armarx::viz::CoinVisualizer::manipulatorGroup
SoSeparator * manipulatorGroup
Definition: Visualizer.h:270