Skip to content

Commit

Permalink
Use moveToHelper from ign-rendering (#825)
Browse files Browse the repository at this point in the history 
Signed-off-by: ahcorde <[email protected]>
  • Loading branch information
@ahcorde
ahcorde authored May 26, 2021
1 parent 3d4caa8 commit 0cacbd5
Showing 1 changed file with 3 additions and 218 deletions.
221 changes: 3 additions & 218 deletions src/gui/plugins/scene3d/Scene3D.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@

#include <ignition/rendering/Image.hh>
#include <ignition/rendering/OrbitViewController.hh>
#include <ignition/rendering/MoveToHelper.hh>
#include <ignition/rendering/RayQuery.hh>
#include <ignition/rendering/RenderEngine.hh>
#include <ignition/rendering/RenderingIface.hh>
Expand Down Expand Up @@ -94,70 +95,6 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
bool sendEvent{false};
};

//
/// \brief Helper class for animating a user camera to move to a target entity
/// todo(anyone) Move this functionality to rendering::Camera class in
/// ign-rendering3
class MoveToHelper
{
/// \brief Move the camera to look at the specified target
/// param[in] _camera Camera to be moved
/// param[in] _target Target to look at
/// param[in] _duration Duration of the move to animation, in seconds.
/// param[in] _onAnimationComplete Callback function when animation is
/// complete
public: void MoveTo(const rendering::CameraPtr &_camera,
const rendering::NodePtr &_target, double _duration,
std::function<void()> _onAnimationComplete);

/// \brief Move the camera to the specified pose.
/// param[in] _camera Camera to be moved
/// param[in] _target Pose to move to
/// param[in] _duration Duration of the move to animation, in seconds.
/// param[in] _onAnimationComplete Callback function when animation is
/// complete
public: void MoveTo(const rendering::CameraPtr &_camera,
const math::Pose3d &_target, double _duration,
std::function<void()> _onAnimationComplete);

/// \brief Move the camera to look at the specified target
/// param[in] _camera Camera to be moved
/// param[in] _direction The pose to assume relative to the entit(y/ies),
/// (0, 0, 0) indicates to return the camera back to the home pose
/// originally loaded in from the sdf.
/// param[in] _duration Duration of the move to animation, in seconds.
/// param[in] _onAnimationComplete Callback function when animation is
/// complete
public: void LookDirection(const rendering::CameraPtr &_camera,
const math::Vector3d &_direction, const math::Vector3d &_lookAt,
double _duration, std::function<void()> _onAnimationComplete);

/// \brief Add time to the animation.
/// \param[in] _time Time to add in seconds
public: void AddTime(double _time);

/// \brief Get whether the move to helper is idle, i.e. no animation
/// is being executed.
/// \return True if idle, false otherwise
public: bool Idle() const;

/// \brief Set the initial camera pose
/// param[in] _pose The init pose of the camera
public: void SetInitCameraPose(const math::Pose3d &_pose);

/// \brief Pose animation object
public: std::unique_ptr<common::PoseAnimation> poseAnim;

/// \brief Pointer to the camera being moved
public: rendering::CameraPtr camera;

/// \brief Callback function when animation is complete.
public: std::function<void()> onAnimationComplete;

/// \brief Initial pose of the camera used for view angles
public: math::Pose3d initCameraPose;
};

/// \brief Private data class for IgnRenderer
class IgnRendererPrivate
{
Expand Down Expand Up @@ -229,7 +166,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
public: std::string moveToTarget;

/// \brief Helper object to move user camera
public: MoveToHelper moveToHelper;
public: ignition::rendering::MoveToHelper moveToHelper;

/// \brief Target to view collisions
public: std::string viewCollisionsTarget;
Expand Down Expand Up @@ -449,7 +386,7 @@ QList<QThread *> RenderWindowItemPrivate::threads;
IgnRenderer::IgnRenderer()
: dataPtr(new IgnRendererPrivate)
{
this->dataPtr->moveToHelper.initCameraPose = this->cameraPose;
this->dataPtr->moveToHelper.SetInitCameraPose(this->cameraPose);

// recorder stats topic
std::string recorderStatsTopic = "/gui/record_video/stats";
Expand Down Expand Up @@ -3273,158 +3210,6 @@ void RenderWindowItem::HandleKeyRelease(QKeyEvent *_e)
// }
//

////////////////////////////////////////////////
void MoveToHelper::MoveTo(const rendering::CameraPtr &_camera,
const ignition::math::Pose3d &_target,
double _duration, std::function<void()> _onAnimationComplete)
{
this->camera = _camera;
this->poseAnim = std::make_unique<common::PoseAnimation>(
"move_to", _duration, false);
this->onAnimationComplete = std::move(_onAnimationComplete);

math::Pose3d start = _camera->WorldPose();

common::PoseKeyFrame *key = this->poseAnim->CreateKeyFrame(0);
key->Translation(start.Pos());
key->Rotation(start.Rot());

key = this->poseAnim->CreateKeyFrame(_duration);
if (_target.Pos().IsFinite())
key->Translation(_target.Pos());
else
key->Translation(start.Pos());

if (_target.Rot().IsFinite())
key->Rotation(_target.Rot());
else
key->Rotation(start.Rot());
}

////////////////////////////////////////////////
void MoveToHelper::MoveTo(const rendering::CameraPtr &_camera,
const rendering::NodePtr &_target,
double _duration, std::function<void()> _onAnimationComplete)
{
this->camera = _camera;
this->poseAnim = std::make_unique<common::PoseAnimation>(
"move_to", _duration, false);
this->onAnimationComplete = std::move(_onAnimationComplete);

math::Pose3d start = _camera->WorldPose();

// todo(anyone) implement bounding box function in rendering to get
// target size and center.
// Assume fixed size and target world position is its center
math::Box targetBBox(1.0, 1.0, 1.0);
math::Vector3d targetCenter = _target->WorldPosition();
math::Vector3d dir = targetCenter - start.Pos();
dir.Correct();
dir.Normalize();

// distance to move
double maxSize = targetBBox.Size().Max();
double dist = start.Pos().Distance(targetCenter) - maxSize;

// Scale to fit in view
double hfov = this->camera->HFOV().Radian();
double offset = maxSize*0.5 / std::tan(hfov/2.0);

// End position and rotation
math::Vector3d endPos = start.Pos() + dir*(dist - offset);
math::Quaterniond endRot =
math::Matrix4d::LookAt(endPos, targetCenter).Rotation();
math::Pose3d end(endPos, endRot);

common::PoseKeyFrame *key = this->poseAnim->CreateKeyFrame(0);
key->Translation(start.Pos());
key->Rotation(start.Rot());

key = this->poseAnim->CreateKeyFrame(_duration);
key->Translation(end.Pos());
key->Rotation(end.Rot());
}

////////////////////////////////////////////////
void MoveToHelper::LookDirection(const rendering::CameraPtr &_camera,
const math::Vector3d &_direction, const math::Vector3d &_lookAt,
double _duration, std::function<void()> _onAnimationComplete)
{
this->camera = _camera;
this->poseAnim = std::make_unique<common::PoseAnimation>(
"view_angle", _duration, false);
this->onAnimationComplete = std::move(_onAnimationComplete);

math::Pose3d start = _camera->WorldPose();

// Look at world origin unless there are visuals selected
// Keep current distance to look at target
math::Vector3d camPos = _camera->WorldPose().Pos();
double distance = std::fabs((camPos - _lookAt).Length());

// Calculate camera position
math::Vector3d endPos = _lookAt - _direction * distance;

// Calculate camera orientation
math::Quaterniond endRot =
ignition::math::Matrix4d::LookAt(endPos, _lookAt).Rotation();

// Move camera to that pose
common::PoseKeyFrame *key = this->poseAnim->CreateKeyFrame(0);
key->Translation(start.Pos());
key->Rotation(start.Rot());

// Move camera back to initial pose
if (_direction == math::Vector3d::Zero)
{
endPos = this->initCameraPose.Pos();
endRot = this->initCameraPose.Rot();
}

key = this->poseAnim->CreateKeyFrame(_duration);
key->Translation(endPos);
key->Rotation(endRot);
}

////////////////////////////////////////////////
void MoveToHelper::AddTime(double _time)
{
if (!this->camera || !this->poseAnim)
return;

common::PoseKeyFrame kf(0);

this->poseAnim->AddTime(_time);
this->poseAnim->InterpolatedKeyFrame(kf);

math::Pose3d offset(kf.Translation(), kf.Rotation());

this->camera->SetWorldPose(offset);

if (this->poseAnim->Length() <= this->poseAnim->Time())
{
if (this->onAnimationComplete)
{
this->onAnimationComplete();
}
this->camera.reset();
this->poseAnim.reset();
this->onAnimationComplete = nullptr;
}
}

////////////////////////////////////////////////
bool MoveToHelper::Idle() const
{
return this->poseAnim == nullptr;
}

////////////////////////////////////////////////
void MoveToHelper::SetInitCameraPose(const math::Pose3d &_pose)
{
this->initCameraPose = _pose;
}

// Register this plugin
IGNITION_ADD_PLUGIN(ignition::gazebo::Scene3D,
ignition::gui::Plugin)

0 comments on commit 0cacbd5

Please sign in to comment.