From 5e2a4c73f9253cc89dc91f68a144b29d3fa949af Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Sat, 25 Feb 2023 16:15:45 +0100
Subject: [PATCH] Fixed merging visualization nodes by removing SoUnit Fixed
RobotFactory::createReducedModel by applying displacement only if
createUnitedVisualization is needed
---
VirtualRobot/RobotFactory.cpp | 45 ++++++++++++-------
.../CoinVisualizationFactory.cpp | 3 --
2 files changed, 30 insertions(+), 18 deletions(-)
diff --git a/VirtualRobot/RobotFactory.cpp b/VirtualRobot/RobotFactory.cpp
index 22869b7ff..025904d0d 100644
--- a/VirtualRobot/RobotFactory.cpp
+++ b/VirtualRobot/RobotFactory.cpp
@@ -1184,32 +1184,47 @@ namespace VirtualRobot
const Eigen::Matrix4f globalPoseInv = simox::math::inverted_pose(n.node_cloned->getGlobalPose());
if (!visus.empty())
{
- if (const auto visu = n.node->getVisualization())
+ if (visus.size() == 1)
{
- visus.insert(visus.begin(), visu->clone());
+ n.node_cloned->setVisualization(visus.front()->clone());
}
- auto v = vf->createUnitedVisualization(visus);
- if (n.parentNode_cloned)
+ else
{
- vf->applyDisplacement(v, globalPoseInv);
+ if (const auto visu = n.node->getVisualization())
+ {
+ visus.insert(visus.begin(), visu->clone());
+ }
+ auto v = vf->createUnitedVisualization(visus);
+ if (n.parentNode_cloned)
+ {
+ v->setLocalPose(globalPoseInv);
+ }
+ n.node_cloned->setVisualization(v);
}
- n.node_cloned->setVisualization(v);
}
if (!colVisus.empty())
{
- if (const auto colModel = n.node->getCollisionModel())
+ if (colVisus.size() == 1)
{
- if (const auto vis = colModel->getVisualization())
- {
- colVisus.insert(colVisus.begin(), vis->clone());
- }
+ n.node_cloned->setCollisionModel(std::make_shared<CollisionModel>(colVisus.front(), n.node->getName()));
}
- auto colVisu = vf->createUnitedVisualization(colVisus);
- if (n.parentNode_cloned)
+ else
{
- vf->applyDisplacement(colVisu, globalPoseInv);
+ if (const auto colModel = n.node->getCollisionModel())
+ {
+ if (const auto vis = colModel->getVisualization())
+ {
+ colVisus.insert(colVisus.begin(), vis->clone());
+ }
+ }
+ auto colVisu = vf->createUnitedVisualization(colVisus);
+ if (n.parentNode_cloned)
+ {
+ colVisu->setLocalPose(globalPoseInv);
+ }
+ n.node_cloned->setCollisionModel(std::make_shared<CollisionModel>(colVisu, n.node->getName()));
}
- n.node_cloned->setCollisionModel(std::make_shared<CollisionModel>(colVisu, n.node->getName()));
+
}
for (const auto& childNode : n.childNodes)
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
index 7a4e74191..b41899e0a 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
@@ -4076,8 +4076,6 @@ namespace VirtualRobot
{
SoSeparator* ss = new SoSeparator();
- SoUnits* u = new SoUnits();
- u->units = SoUnits::MILLIMETERS;
const Eigen::Isometry3f localPose(visualizations[i]->getLocalPose());
const Eigen::Vector3f& translation = localPose.translation();
@@ -4087,7 +4085,6 @@ namespace VirtualRobot
const Eigen::Quaternionf q = simox::math::mat3f_to_quat(localPose.linear());
t->rotation.setValue(q.x(), q.y(), q.z(), q.w());
- ss->addChild(u);
ss->addChild(t);
ss->addChild(n->copy(FALSE));
--
GitLab