diff --git a/source/arviz_godot/lib/avi/scene/Converter.cpp b/source/arviz_godot/lib/avi/scene/Converter.cpp
index a919eb43746e7f78ab9c2473016fe1c3ca1fbe1c..1ffc6f7aef4a7acd2f68fe94778026f48916f585 100644
--- a/source/arviz_godot/lib/avi/scene/Converter.cpp
+++ b/source/arviz_godot/lib/avi/scene/Converter.cpp
@@ -156,52 +156,53 @@ avi::scene::Converter::createElement(avi::scene::LayerNode* layer,
{
case connection::SentElementType::BOX:
{
- godot::CubeMesh* mesh = godot::CubeMesh::_new();
- avi::scene::CubeElementNode* boxNode = avi::scene::CubeElementNode::newCubeElementNode(
- mesh, avi::connection::SentElementType::BOX, elementInfo->id, layer);
+ avi::scene::CubeElementNode* boxNode =
+ avi::scene::CubeElementNode::newCubeElementNode(elementInfo->id, layer);
boxNode->update(elementInfo);
+ boxNode->setShapeFromMesh();
+
created = boxNode;
- boxNode->setShape(mesh);
+
break;
}
case connection::SentElementType::SPHERE:
{
- godot::SphereMesh* mesh = godot::SphereMesh::_new();
avi::scene::MeshElementNode* sphereNode =
- avi::scene::SphereElementNode::newSphereElementNode(
- mesh, avi::connection::SentElementType::SPHERE, elementInfo->id, layer);
+ avi::scene::SphereElementNode::newSphereElementNode(elementInfo->id, layer);
sphereNode->update(elementInfo);
+ sphereNode->setShapeFromMesh();
+
created = sphereNode;
- sphereNode->setShape(mesh);
+
break;
}
case connection::SentElementType::CYLINDER:
{
- godot::CylinderMesh* mesh = godot::CylinderMesh::_new();
avi::scene::MeshElementNode* cylinderNode =
- avi::scene::CylinderElementNode::newCylinderElementNode(
- mesh, avi::connection::SentElementType::CYLINDER, elementInfo->id, layer);
+ avi::scene::CylinderElementNode::newCylinderElementNode(elementInfo->id, layer);
cylinderNode->update(elementInfo);
+ cylinderNode->setShapeFromMesh();
+
created = cylinderNode;
- cylinderNode->setShape(mesh);
+
break;
}
case connection::SentElementType::ELLIPSOID:
{
- godot::SphereMesh* mesh = godot::SphereMesh::_new();
avi::scene::MeshElementNode* ellipsoidNode =
- avi::scene::EllipsoidElementNode::newEllipsoidElementNode(
- mesh, avi::connection::SentElementType::ELLIPSOID, elementInfo->id, layer);
+ avi::scene::EllipsoidElementNode::newEllipsoidElementNode(elementInfo->id, layer);
ellipsoidNode->update(elementInfo);
+ ellipsoidNode->setShapeFromMesh();
+
created = ellipsoidNode;
- ellipsoidNode->setShape(mesh);
+
break;
}
@@ -209,14 +210,14 @@ avi::scene::Converter::createElement(avi::scene::LayerNode* layer,
{
auto cylindroid =
IceInternal::Handle<armarx::viz::data::ElementCylindroid>::dynamicCast(elementInfo);
- godot::CylinderMesh* mesh = godot::CylinderMesh::_new();
avi::scene::CylindroidElementNode* cylindroidNode =
- avi::scene::CylindroidElementNode::newCylindroidElementNode(
- mesh, avi::connection::SentElementType::CYLINDROID, cylindroid->id, layer);
+ avi::scene::CylindroidElementNode::newCylindroidElementNode(cylindroid->id, layer);
cylindroidNode->update(elementInfo);
+ cylindroidNode->setShapeFromMesh();
+
created = cylindroidNode;
- cylindroidNode->setShape(mesh);
+
break;
}
diff --git a/source/arviz_godot/lib/avi/scene/CubeElementNode.cpp b/source/arviz_godot/lib/avi/scene/CubeElementNode.cpp
index 13f57f03246f9caff761c512a216ff68d6d4ab4f..f702d417abc843572d1fe59f6161e8417fe2923e 100644
--- a/source/arviz_godot/lib/avi/scene/CubeElementNode.cpp
+++ b/source/arviz_godot/lib/avi/scene/CubeElementNode.cpp
@@ -1,5 +1,8 @@
#include "CubeElementNode.h"
+#include <utility>
+
+#include <arviz_godot/lib/avi/core/Utilities.h>
#include <arviz_godot/lib/avi/scene/Converter.h>
void
@@ -22,8 +25,14 @@ void
avi::scene::CubeElementNode::update(IceInternal::Handle<armarx::viz::data::Element> elementInfo)
{
auto box = IceInternal::Handle<armarx::viz::data::ElementBox>::dynamicCast(elementInfo);
- auto* mesh = godot::Object::cast_to<godot::CubeMesh>(getMesh());
- mesh->set_size(godot::Vector3(box->size.e0, box->size.e1, box->size.e2));
+
+ godot::Vector3 newSize = {box->size.e0, box->size.e1, box->size.e2};
+ if (!avi::core::utilities::is_equal_approx(size, newSize))
+ {
+ size = newSize;
+ getMeshInstance()->set_scale(size / 2);
+ }
+
updateAttributes(box->interaction.enableFlags,
box->interaction.contextMenuOptions,
avi::scene::Converter::getPosition(box->pose),
@@ -33,13 +42,12 @@ avi::scene::CubeElementNode::update(IceInternal::Handle<armarx::viz::data::Eleme
}
avi::scene::CubeElementNode*
-avi::scene::CubeElementNode::newCubeElementNode(godot::Mesh* mesh,
- avi::connection::SentElementType type,
- std::string name,
- avi::core::Layer* layer)
+avi::scene::CubeElementNode::newCubeElementNode(std::string name, avi::core::Layer* layer)
{
CubeElementNode* node = CubeElementNode::_new();
- node->setStartAttributes(mesh, type, name, layer);
+ godot::Mesh* mesh = godot::CubeMesh::_new();
+
+ node->setStartAttributes(mesh, avi::connection::SentElementType::BOX, std::move(name), layer);
return node;
}
diff --git a/source/arviz_godot/lib/avi/scene/CubeElementNode.h b/source/arviz_godot/lib/avi/scene/CubeElementNode.h
index cdd828da2972ce31272a2da74b742c006d8be43b..c893d560d2585d2461cd90919177fc7805666e8b 100644
--- a/source/arviz_godot/lib/avi/scene/CubeElementNode.h
+++ b/source/arviz_godot/lib/avi/scene/CubeElementNode.h
@@ -18,11 +18,12 @@ namespace avi::scene
void _init();
- static CubeElementNode* newCubeElementNode(godot::Mesh* mesh,
- avi::connection::SentElementType type,
- std::string name,
- avi::core::Layer* layer);
+ static avi::scene::CubeElementNode* newCubeElementNode(std::string name,
+ avi::core::Layer* layer);
void update(IceInternal::Handle<armarx::viz::data::Element> elementInfo) override;
+
+ private:
+ godot::Vector3 size;
};
} // namespace avi::scene
diff --git a/source/arviz_godot/lib/avi/scene/CylinderElementNode.cpp b/source/arviz_godot/lib/avi/scene/CylinderElementNode.cpp
index 2b36c490c4596fe458bd354e1452f955839665f6..4389c68c3a89852a284a8c773a722909f052e94b 100644
--- a/source/arviz_godot/lib/avi/scene/CylinderElementNode.cpp
+++ b/source/arviz_godot/lib/avi/scene/CylinderElementNode.cpp
@@ -1,5 +1,7 @@
#include "CylinderElementNode.h"
+#include <utility>
+
#include "Converter.h"
void
@@ -23,10 +25,16 @@ avi::scene::CylinderElementNode::update(IceInternal::Handle<armarx::viz::data::E
{
auto cylinder =
IceInternal::Handle<armarx::viz::data::ElementCylinder>::dynamicCast(elementInfo);
- auto* mesh = godot::Object::cast_to<godot::CylinderMesh>(getMesh());
- mesh->set_height(cylinder->height);
- mesh->set_top_radius(cylinder->radius);
- mesh->set_bottom_radius(cylinder->radius);
+
+ if (!godot::Math::is_equal_approx(height, cylinder->height) ||
+ !godot::Math::is_equal_approx(radius, cylinder->radius))
+ {
+ radius = cylinder->radius;
+ height = cylinder->height;
+
+ getMeshInstance()->set_scale({radius, height / 2, radius});
+ }
+
updateAttributes(cylinder->interaction.enableFlags,
cylinder->interaction.contextMenuOptions,
avi::scene::Converter::getPosition(cylinder->pose),
@@ -36,12 +44,12 @@ avi::scene::CylinderElementNode::update(IceInternal::Handle<armarx::viz::data::E
}
avi::scene::CylinderElementNode*
-avi::scene::CylinderElementNode::newCylinderElementNode(godot::Mesh* mesh,
- avi::connection::SentElementType type,
- std::string name,
- avi::core::Layer* layer)
+avi::scene::CylinderElementNode::newCylinderElementNode(std::string name, avi::core::Layer* layer)
{
CylinderElementNode* node = CylinderElementNode::_new();
- node->setStartAttributes(mesh, type, name, layer);
+ godot::Mesh* mesh = godot::CylinderMesh::_new();
+
+ node->setStartAttributes(
+ mesh, avi::connection::SentElementType::CYLINDER, std::move(name), layer);
return node;
}
diff --git a/source/arviz_godot/lib/avi/scene/CylinderElementNode.h b/source/arviz_godot/lib/avi/scene/CylinderElementNode.h
index 5bc09f8001997731d7bbe8e91d8048ded4b16218..e5fa3630c923017d1dbacd31808880a423de4daa 100644
--- a/source/arviz_godot/lib/avi/scene/CylinderElementNode.h
+++ b/source/arviz_godot/lib/avi/scene/CylinderElementNode.h
@@ -18,11 +18,13 @@ namespace avi::scene
void _init();
- static CylinderElementNode* newCylinderElementNode(godot::Mesh* mesh,
- avi::connection::SentElementType type,
- std::string name,
+ static CylinderElementNode* newCylinderElementNode(std::string name,
avi::core::Layer* layer);
void update(IceInternal::Handle<armarx::viz::data::Element> elementInfo) override;
+
+ private:
+ float height = 0.0f;
+ float radius = 0.0f;
};
} // namespace avi::scene
diff --git a/source/arviz_godot/lib/avi/scene/CylindroidElementNode.cpp b/source/arviz_godot/lib/avi/scene/CylindroidElementNode.cpp
index 32c5a898ad96df61335d23f391392af09000febd..f8dfde1073d315ca9db601c793cd79e5ce2ad0de 100644
--- a/source/arviz_godot/lib/avi/scene/CylindroidElementNode.cpp
+++ b/source/arviz_godot/lib/avi/scene/CylindroidElementNode.cpp
@@ -24,10 +24,14 @@ avi::scene::CylindroidElementNode::update(
{
auto cylindroid =
IceInternal::Handle<armarx::viz::data::ElementCylindroid>::dynamicCast(elementInfo);
- auto* mesh = godot::Object::cast_to<godot::CylinderMesh>(getMesh());
- mesh->set_height(cylindroid->height);
- mesh->set_top_radius(1);
- mesh->set_bottom_radius(1);
+
+ if (!godot::Math::is_equal_approx(height, cylindroid->height))
+ {
+ height = cylindroid->height;
+
+ getMeshInstance()->set_scale({1, height / 2, 1});
+ }
+
updateAttributes(cylindroid->interaction.enableFlags,
cylindroid->interaction.contextMenuOptions,
avi::scene::Converter::getPosition(cylindroid->pose),
@@ -37,12 +41,12 @@ avi::scene::CylindroidElementNode::update(
}
avi::scene::CylindroidElementNode*
-avi::scene::CylindroidElementNode::newCylindroidElementNode(godot::Mesh* mesh,
- avi::connection::SentElementType type,
- std::string name,
+avi::scene::CylindroidElementNode::newCylindroidElementNode(std::string name,
avi::core::Layer* layer)
{
CylindroidElementNode* node = CylindroidElementNode::_new();
- node->setStartAttributes(mesh, type, name, layer);
+ godot::Mesh* mesh = godot::CylinderMesh::_new();
+
+ node->setStartAttributes(mesh, avi::connection::SentElementType::CYLINDROID, name, layer);
return node;
}
diff --git a/source/arviz_godot/lib/avi/scene/CylindroidElementNode.h b/source/arviz_godot/lib/avi/scene/CylindroidElementNode.h
index f098f7a95dca314d8b904041ecdfa925b6e9dad6..773f29b9bea6a51383d104a388e6a041970d63e6 100644
--- a/source/arviz_godot/lib/avi/scene/CylindroidElementNode.h
+++ b/source/arviz_godot/lib/avi/scene/CylindroidElementNode.h
@@ -18,12 +18,12 @@ namespace avi::scene
void _init();
- static CylindroidElementNode*
- newCylindroidElementNode(godot::Mesh* mesh,
- avi::connection::SentElementType type,
- std::string name,
- avi::core::Layer* layer);
+ static CylindroidElementNode* newCylindroidElementNode(std::string name,
+ avi::core::Layer* layer);
void update(IceInternal::Handle<armarx::viz::data::Element> elementInfo) override;
+
+ private:
+ float height = 0.0f;
};
} // namespace avi::scene
diff --git a/source/arviz_godot/lib/avi/scene/EllipsoidElementNode.cpp b/source/arviz_godot/lib/avi/scene/EllipsoidElementNode.cpp
index 9bb8eb96b6923d36e72aa50705e29de2b6880cd1..8f2aa6407e15e070fa876dfd0c0a96810679c649 100644
--- a/source/arviz_godot/lib/avi/scene/EllipsoidElementNode.cpp
+++ b/source/arviz_godot/lib/avi/scene/EllipsoidElementNode.cpp
@@ -1,5 +1,6 @@
#include "EllipsoidElementNode.h"
+#include <arviz_godot/lib/avi/core/Utilities.h>
#include <arviz_godot/lib/avi/scene/Converter.h>
void
@@ -24,29 +25,34 @@ avi::scene::EllipsoidElementNode::update(
{
auto ellipsoid =
IceInternal::Handle<armarx::viz::data::ElementEllipsoid>::dynamicCast(elementInfo);
- auto* mesh = godot::Object::cast_to<godot::SphereMesh>(getMesh());
- mesh->set_height(2);
- mesh->set_radius(1);
- armarx::Vector3f scale = ellipsoid->scale;
- godot::Vector3 meshScale = {
+
+ godot::Vector3 newScale = {
ellipsoid->axisLengths.e0, ellipsoid->axisLengths.e1, ellipsoid->axisLengths.e2};
- godot::MeshInstance* meshInstance = getMeshInstance();
- meshInstance->set_scale(meshScale);
+
+ if (!avi::core::utilities::is_equal_approx(scale, newScale))
+ {
+ scale = newScale;
+ getMeshInstance()->set_scale(scale);
+ }
+
updateAttributes(ellipsoid->interaction.enableFlags,
ellipsoid->interaction.contextMenuOptions,
avi::scene::Converter::getPosition(ellipsoid->pose),
- avi::scene::Converter::getScale(scale),
+ avi::scene::Converter::getScale(ellipsoid->scale),
avi::scene::Converter::getRotation(ellipsoid->pose),
avi::scene::Converter::getColor(ellipsoid->color));
}
avi::scene::EllipsoidElementNode*
-avi::scene::EllipsoidElementNode::newEllipsoidElementNode(godot::Mesh* mesh,
- avi::connection::SentElementType type,
- std::string name,
- avi::core::Layer* layer)
+avi::scene::EllipsoidElementNode::newEllipsoidElementNode(std::string name, avi::core::Layer* layer)
{
EllipsoidElementNode* node = EllipsoidElementNode::_new();
- node->setStartAttributes(mesh, type, name, layer);
+ godot::SphereMesh* mesh = godot::SphereMesh::_new();
+
+ mesh->set_height(2);
+ mesh->set_radius(1);
+
+ node->setStartAttributes(mesh, avi::connection::SentElementType::ELLIPSOID, name, layer);
+
return node;
}
diff --git a/source/arviz_godot/lib/avi/scene/EllipsoidElementNode.h b/source/arviz_godot/lib/avi/scene/EllipsoidElementNode.h
index 23488d238f445ca54aa155c562bd7cfc9d154d6f..9524d002518de2abf83053ae7e107457a899e358 100644
--- a/source/arviz_godot/lib/avi/scene/EllipsoidElementNode.h
+++ b/source/arviz_godot/lib/avi/scene/EllipsoidElementNode.h
@@ -18,11 +18,12 @@ namespace avi::scene
void _init();
- static EllipsoidElementNode* newEllipsoidElementNode(godot::Mesh* mesh,
- avi::connection::SentElementType type,
- std::string name,
+ static EllipsoidElementNode* newEllipsoidElementNode(std::string name,
avi::core::Layer* layer);
void update(IceInternal::Handle<armarx::viz::data::Element> elementInfo) override;
+
+ private:
+ godot::Vector3 scale;
};
} // namespace avi::scene
diff --git a/source/arviz_godot/lib/avi/scene/MeshElementNode.cpp b/source/arviz_godot/lib/avi/scene/MeshElementNode.cpp
index f7ab3a6f600f553027a643cc68d91680ce87dd5b..b1b834f70edfe5941aa7cd8a07826139ed859613 100644
--- a/source/arviz_godot/lib/avi/scene/MeshElementNode.cpp
+++ b/source/arviz_godot/lib/avi/scene/MeshElementNode.cpp
@@ -18,7 +18,7 @@ avi::scene::MeshElementNode::getVolume()
{
godot::AABB correctedAABB = meshInstance->get_aabb();
correctedAABB.position = staticBody->get_transform().origin;
- correctedAABB.size = correctedAABB.size * staticBody->get_scale() * 0.5;
+ correctedAABB.size *= staticBody->get_scale() * meshInstance->get_scale() * 0.5;
return correctedAABB;
}
@@ -150,6 +150,12 @@ avi::scene::MeshElementNode::setShape(godot::Mesh* mesh)
collisionShape->set_scale(meshInstance->get_scale());
}
+void
+avi::scene::MeshElementNode::setShapeFromMesh()
+{
+ setShape(getMesh());
+}
+
godot::Transform
avi::scene::MeshElementNode::getTransform()
{
@@ -172,3 +178,4 @@ avi::scene::MeshElementNode::getMeshInstance()
{
return meshInstance;
}
+
diff --git a/source/arviz_godot/lib/avi/scene/MeshElementNode.h b/source/arviz_godot/lib/avi/scene/MeshElementNode.h
index ada8c4ddaed5b4c14df5e12b26a6214347b5695b..8c9b555df8610898139bd2af83a4114aaeaac353 100644
--- a/source/arviz_godot/lib/avi/scene/MeshElementNode.h
+++ b/source/arviz_godot/lib/avi/scene/MeshElementNode.h
@@ -37,6 +37,7 @@ namespace avi::scene
void setMesh(godot::Mesh* mesh);
void setShape(godot::Mesh* mesh);
+ void setShapeFromMesh();
godot::Vector3 getPosition();
diff --git a/source/arviz_godot/lib/avi/scene/SphereElementNode.cpp b/source/arviz_godot/lib/avi/scene/SphereElementNode.cpp
index 478e4a9e6e4c32807e1819437059d0fcab5176ac..a2d074d2c7eb098be62d855398627e5bf1a05da3 100644
--- a/source/arviz_godot/lib/avi/scene/SphereElementNode.cpp
+++ b/source/arviz_godot/lib/avi/scene/SphereElementNode.cpp
@@ -23,9 +23,16 @@ void
avi::scene::SphereElementNode::update(IceInternal::Handle<armarx::viz::data::Element> elementInfo)
{
auto sphere = IceInternal::Handle<armarx::viz::data::ElementSphere>::dynamicCast(elementInfo);
- auto* mesh = godot::Object::cast_to<godot::SphereMesh>(getMesh());
- mesh->set_radius(sphere->radius);
- mesh->set_height(2 * sphere->radius);
+
+ float newRadius = sphere->radius;
+
+ if (!godot::Math::is_equal_approx(radius, newRadius))
+ {
+ radius = newRadius;
+
+ getMeshInstance()->set_scale({radius, radius, radius});
+ }
+
updateAttributes(sphere->interaction.enableFlags,
sphere->interaction.contextMenuOptions,
avi::scene::Converter::getPosition(sphere->pose),
@@ -35,13 +42,12 @@ avi::scene::SphereElementNode::update(IceInternal::Handle<armarx::viz::data::Ele
}
avi::scene::SphereElementNode*
-avi::scene::SphereElementNode::newSphereElementNode(godot::Mesh* mesh,
- avi::connection::SentElementType type,
- std::string name,
- avi::core::Layer* layer)
+avi::scene::SphereElementNode::newSphereElementNode(std::string name, avi::core::Layer* layer)
{
SphereElementNode* node = SphereElementNode::_new();
- node->setStartAttributes(mesh, type, name, layer);
+ godot::Mesh* mesh = godot::SphereMesh::_new();
+
+ node->setStartAttributes(mesh, avi::connection::SentElementType::SPHERE, name, layer);
return node;
}
diff --git a/source/arviz_godot/lib/avi/scene/SphereElementNode.h b/source/arviz_godot/lib/avi/scene/SphereElementNode.h
index c11ea6bdfc6e73697cb09b9174ab5234630104fc..2fe61243f5dab4cc71c8485813260402f3e700b0 100644
--- a/source/arviz_godot/lib/avi/scene/SphereElementNode.h
+++ b/source/arviz_godot/lib/avi/scene/SphereElementNode.h
@@ -17,11 +17,11 @@ namespace avi::scene
void _init();
- static SphereElementNode* newSphereElementNode(godot::Mesh* mesh,
- avi::connection::SentElementType type,
- std::string name,
- avi::core::Layer* layer);
+ static SphereElementNode* newSphereElementNode(std::string name, avi::core::Layer* layer);
void update(IceInternal::Handle<armarx::viz::data::Element> elementInfo) override;
+
+ private:
+ float radius = 0.0f;
};
} // namespace avi::scene