Skip to content

Commit

Permalink
Minor changes
Browse files Browse the repository at this point in the history 
Signed-off-by: Henrique-BO <[email protected]>
  • Loading branch information
@Henrique-BO
Henrique-BO committed Aug 16, 2023
1 parent c5f5cb6 commit b5cafc3
Show file tree
Hide file tree
Showing 3 changed files with 53 additions and 48 deletions.
28 changes: 15 additions & 13 deletions include/gz/sim/rendering/WrenchVisualizer.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@

#include <gz/sim/config.hh>
#include <gz/sim/rendering/Export.hh>
#include <gz/utils/ImplPtr.hh>

#include <gz/math/Vector3.hh>
#include <gz/rendering/RenderTypes.hh>
Expand All @@ -30,22 +31,23 @@ 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
{
/// \brief Constructor
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
Expand All @@ -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<WrenchVisualizerPrivate> dataPtr;
GZ_UTILS_UNIQUE_IMPL_PTR(dataPtr)
};
}
}
Expand Down
54 changes: 29 additions & 25 deletions src/gui/plugins/apply_force_torque/ApplyForceTorque.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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();
}
}
Expand Down Expand Up @@ -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(
Expand All @@ -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(
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -639,6 +644,7 @@ void ApplyForceTorquePrivate::UpdateVisuals()
/ 2.0;
this->wrenchVis.UpdateVectorVisual(
this->forceVisual, worldForce, applicationPoint, scale, true);
this->forceVisual->SetVisible(true);
}
else
{
Expand All @@ -657,34 +663,32 @@ void ApplyForceTorquePrivate::UpdateVisuals()
/ 2.0;
this->wrenchVis.UpdateVectorVisual(
this->torqueVisual, worldTorque, applicationPoint, scale, false);
this->torqueVisual->SetVisible(true);
}
else
{
this->torqueVisual->SetVisible(false);
}

// 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;
Expand All @@ -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());
Expand Down Expand Up @@ -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();

Expand All @@ -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;
Expand Down Expand Up @@ -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;
}
}
}
Expand Down Expand Up @@ -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;
}
Expand Down
19 changes: 9 additions & 10 deletions src/rendering/WrenchVisualizer.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,30 +28,30 @@ 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};
};

/////////////////////////////////////////////////
WrenchVisualizer::WrenchVisualizer() :
dataPtr(std::make_unique<WrenchVisualizerPrivate>())
dataPtr(utils::MakeUniqueImpl<Implementation>())
{
}

/////////////////////////////////////////////////
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;
}

/////////////////////////////////////////////////
Expand Down Expand Up @@ -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);
Expand All @@ -123,5 +123,4 @@ void WrenchVisualizer::UpdateVectorVisual(rendering::VisualPtr _visual,
}
_visual->SetLocalRotation(quat);
_visual->SetLocalScale(_size);
_visual->SetVisible(true);
}

0 comments on commit b5cafc3

Please sign in to comment.