From b5cafc36e64d759b09d5477d1eb4ca103a74f40f Mon Sep 17 00:00:00 2001 From: Henrique-BO Date: Wed, 16 Aug 2023 16:29:48 -0300 Subject: [PATCH] Minor changes Signed-off-by: Henrique-BO --- include/gz/sim/rendering/WrenchVisualizer.hh | 28 +++++----- .../apply_force_torque/ApplyForceTorque.cc | 54 ++++++++++--------- src/rendering/WrenchVisualizer.cc | 19 ++++--- 3 files changed, 53 insertions(+), 48 deletions(-) diff --git a/include/gz/sim/rendering/WrenchVisualizer.hh b/include/gz/sim/rendering/WrenchVisualizer.hh index 1714f13418..c0a382467e 100644 --- a/include/gz/sim/rendering/WrenchVisualizer.hh +++ b/include/gz/sim/rendering/WrenchVisualizer.hh @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -30,11 +31,10 @@ namespace gz namespace sim { // Inline bracket to help doxygen filtering. -inline namespace GZ_SIM_VERSION_NAMESPACE { -namespace detail{ - // Forward declare private data class. - class WrenchVisualizerPrivate; - +inline namespace GZ_SIM_VERSION_NAMESPACE +{ +namespace detail +{ /// \brief Creates, deletes, and maintains force and torque visuals class GZ_SIM_RENDERING_VISIBLE WrenchVisualizer { @@ -42,10 +42,12 @@ namespace detail{ public: WrenchVisualizer(); /// \brief Destructor - public: virtual ~WrenchVisualizer(); + public: ~WrenchVisualizer(); /// \brief Initialize the Wrench visualizer - void Init(rendering::ScenePtr _scene); + /// \param[in] _scene The rendering scene where the visuals are created + /// \return True if the scene is valid + bool Init(rendering::ScenePtr _scene); /// \brief Create a new force visual /// \param[in] _material The material used for the visual @@ -60,21 +62,21 @@ namespace detail{ rendering::MaterialPtr _material); /// \brief Update the visual of a vector to match its direction and position - /// \param[in] _visual Pointer to the vector visual + /// \param[in] _visual Pointer to the vector visual to be updated /// \param[in] _direction Direction of the vector /// \param[in] _position Position of the arrow /// \param[in] _size Size of the arrow in meters /// \param[in] _tip True if _position specifies the tip of the vector, /// false if it specifies tha base of the vector public: void UpdateVectorVisual(rendering::VisualPtr _visual, - math::Vector3d _direction, - math::Vector3d _position, - double _size, - bool _tip = false); + const math::Vector3d &_direction, + const math::Vector3d &_position, + const double _size, + const bool _tip = false); /// \internal /// \brief Private data pointer - private: std::unique_ptr dataPtr; + GZ_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; } } diff --git a/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc b/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc index fd75a1b3f8..aec2846537 100644 --- a/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc +++ b/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc @@ -166,8 +166,9 @@ namespace sim /// \brief Holds the latest mouse event public: gz::common::MouseEvent mouseEvent; - /// \brief Vector currently being rotated by the rotation tool - public: RotationToolVector vector{RotationToolVector::NONE}; + /// \brief Vector (force or torque) currently being rotated + /// by the rotation tool + public: RotationToolVector activeVector{RotationToolVector::NONE}; /// \brief Vector value on start of rotation tool transformation, /// relative to link @@ -247,9 +248,9 @@ bool ApplyForceTorque::eventFilter(QObject *_obj, QEvent *_event) if (this->dataPtr->vectorDirty) { this->dataPtr->vectorDirty = false; - if (this->dataPtr->vector == RotationToolVector::FORCE) + if (this->dataPtr->activeVector == RotationToolVector::FORCE) emit this->ForceChanged(); - else if (this->dataPtr->vector == RotationToolVector::TORQUE) + else if (this->dataPtr->activeVector == RotationToolVector::TORQUE) emit this->TorqueChanged(); } } @@ -492,7 +493,7 @@ void ApplyForceTorque::SetForce(QVector3D _force) { this->dataPtr->force.Set(_force.x(), _force.y(), _force.z()); // Update rotation tool orientation when force is set by components - if (this->dataPtr->vector == RotationToolVector::FORCE + if (this->dataPtr->activeVector == RotationToolVector::FORCE && this->dataPtr->activeAxis == rendering::TransformAxis::TA_NONE) { this->dataPtr->vectorRot = math::Matrix4d::LookAt( @@ -514,7 +515,7 @@ void ApplyForceTorque::SetTorque(QVector3D _torque) { this->dataPtr->torque.Set(_torque.x(), _torque.y(), _torque.z()); // Update rotation tool orientation when torque is set by components - if (this->dataPtr->vector == RotationToolVector::TORQUE + if (this->dataPtr->activeVector == RotationToolVector::TORQUE && this->dataPtr->activeAxis == rendering::TransformAxis::TA_NONE) { this->dataPtr->vectorRot = math::Matrix4d::LookAt( @@ -600,7 +601,11 @@ void ApplyForceTorquePrivate::OnRender() mat->SetDepthWriteEnabled(false); // Create visuals - this->wrenchVis.Init(this->scene); + if (!this->wrenchVis.Init(this->scene)) + { + gzerr << "Invalid scene" << std::endl; + return; + } this->forceVisual = this->wrenchVis.CreateForceVisual(mat); this->torqueVisual = this->wrenchVis.CreateTorqueVisual(mat); this->gizmoVisual = this->scene->CreateGizmoVisual(); @@ -639,6 +644,7 @@ void ApplyForceTorquePrivate::UpdateVisuals() / 2.0; this->wrenchVis.UpdateVectorVisual( this->forceVisual, worldForce, applicationPoint, scale, true); + this->forceVisual->SetVisible(true); } else { @@ -657,6 +663,7 @@ void ApplyForceTorquePrivate::UpdateVisuals() / 2.0; this->wrenchVis.UpdateVectorVisual( this->torqueVisual, worldTorque, applicationPoint, scale, false); + this->torqueVisual->SetVisible(true); } else { @@ -664,27 +671,24 @@ void ApplyForceTorquePrivate::UpdateVisuals() } // Update gizmo visual - if (this->vector != RotationToolVector::NONE) + if (this->activeVector != RotationToolVector::NONE) { math::Vector3d pos; - math::Vector3d u; - if (this->vector == RotationToolVector::FORCE + if (this->activeVector == RotationToolVector::FORCE && this->force != math::Vector3d::Zero) { pos = this->linkWorldPose.Pos() + this->inertialPos + this->linkWorldPose.Rot().RotateVector(this->offset); - u = this->force; } - else if (this->vector == RotationToolVector::TORQUE + else if (this->activeVector == RotationToolVector::TORQUE && this->torque != math::Vector3d::Zero) { pos = this->linkWorldPose.Pos() + this->inertialPos; - u = this->torque; } else { - this->vector = RotationToolVector::NONE; + this->activeVector = RotationToolVector::NONE; this->gizmoVisual->SetTransformMode(rendering::TransformMode::TM_NONE); this->gizmoVisual->SetActiveAxis(math::Vector3d::Zero); return; @@ -694,7 +698,7 @@ void ApplyForceTorquePrivate::UpdateVisuals() / 2.0; // Update gizmo visual rotation so that they are always facing the - // eye position + // eye position and alligned with the active vector math::Quaterniond gizmoRot = this->linkWorldPose.Rot() * this->vectorRot; math::Vector3d eye = gizmoRot.RotateVectorReverse( (this->camera->WorldPosition() - pos).Normalized()); @@ -754,7 +758,7 @@ void ApplyForceTorquePrivate::HandleMouseEvents() { // Rotating vector around the clicked axis if (this->mouseEvent.Type() == common::MouseEvent::PRESS - && this->vector != RotationToolVector::NONE) + && this->activeVector != RotationToolVector::NONE) { this->mousePressPos = this->mouseEvent.Pos(); @@ -770,9 +774,9 @@ void ApplyForceTorquePrivate::HandleMouseEvents() if (axis == rendering::TransformAxis::TA_ROTATION_Y || axis == rendering::TransformAxis::TA_ROTATION_Z) { - if (this->vector == RotationToolVector::FORCE) + if (this->activeVector == RotationToolVector::FORCE) this->initialVector = this->force; - else if (this->vector == RotationToolVector::TORQUE) + else if (this->activeVector == RotationToolVector::TORQUE) this->initialVector = this->torque; else return; @@ -809,24 +813,24 @@ void ApplyForceTorquePrivate::HandleMouseEvents() auto id = visual->Id(); if ((this->forceVisual->Id() == id || this->forceVisual->ChildById(id)) - && this->vector != RotationToolVector::FORCE) + && this->activeVector != RotationToolVector::FORCE) { - this->vector = RotationToolVector::FORCE; + this->activeVector = RotationToolVector::FORCE; this->vectorRot = math::Matrix4d::LookAt( -this->force, math::Vector3d::Zero).Rotation(); } else if ((this->torqueVisual->Id() == id || this->torqueVisual->ChildById(id)) - && this->vector != RotationToolVector::TORQUE) + && this->activeVector != RotationToolVector::TORQUE) { - this->vector = RotationToolVector::TORQUE; + this->activeVector = RotationToolVector::TORQUE; this->vectorRot = math::Matrix4d::LookAt( -this->torque, math::Vector3d::Zero).Rotation(); } else if (this->gizmoVisual->AxisById(visual->Id()) == rendering::TransformAxis::TA_NONE) { - this->vector = RotationToolVector::NONE; + this->activeVector = RotationToolVector::NONE; } } } @@ -896,9 +900,9 @@ void ApplyForceTorquePrivate::HandleMouseEvents() math::Vector3d newVector = this->initialVector.Length() * this->vectorRot.XAxis(); - if (this->vector == RotationToolVector::FORCE) + if (this->activeVector == RotationToolVector::FORCE) this->force = newVector; - else if (this->vector == RotationToolVector::TORQUE) + else if (this->activeVector == RotationToolVector::TORQUE) this->torque = newVector; this->vectorDirty = true; } diff --git a/src/rendering/WrenchVisualizer.cc b/src/rendering/WrenchVisualizer.cc index f812d1ac3f..0adab74c16 100644 --- a/src/rendering/WrenchVisualizer.cc +++ b/src/rendering/WrenchVisualizer.cc @@ -28,7 +28,7 @@ using namespace sim; using namespace detail; /// Private data for the WrenchVisualizer class -class gz::sim::detail::WrenchVisualizerPrivate +class gz::sim::detail::WrenchVisualizer::Implementation { /// \brief Pointer to the rendering scene public: rendering::ScenePtr scene{nullptr}; @@ -36,7 +36,7 @@ class gz::sim::detail::WrenchVisualizerPrivate ///////////////////////////////////////////////// WrenchVisualizer::WrenchVisualizer() : - dataPtr(std::make_unique()) + dataPtr(utils::MakeUniqueImpl()) { } @@ -44,14 +44,14 @@ WrenchVisualizer::WrenchVisualizer() : WrenchVisualizer::~WrenchVisualizer() = default; ///////////////////////////////////////////////// -void WrenchVisualizer::Init(rendering::ScenePtr _scene) +bool WrenchVisualizer::Init(rendering::ScenePtr _scene) { if (!_scene) { - gzwarn << "Invalid scene" << std::endl; - return; + return false; } this->dataPtr->scene = _scene; + return true; } ///////////////////////////////////////////////// @@ -105,10 +105,10 @@ rendering::VisualPtr WrenchVisualizer::CreateTorqueVisual( ///////////////////////////////////////////////// void WrenchVisualizer::UpdateVectorVisual(rendering::VisualPtr _visual, - math::Vector3d _direction, - math::Vector3d _position, - double _size, - bool _tip) + const math::Vector3d &_direction, + const math::Vector3d &_position, + const double _size, + const bool _tip) { math::Quaterniond quat; quat.SetFrom2Axes(math::Vector3d::UnitZ, _direction); @@ -123,5 +123,4 @@ void WrenchVisualizer::UpdateVectorVisual(rendering::VisualPtr _visual, } _visual->SetLocalRotation(quat); _visual->SetLocalScale(_size); - _visual->SetVisible(true); }