From da9d1458fb8912fe51c73e62ccd2beb9c74c8804 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 29 Apr 2021 22:09:24 +0200 Subject: [PATCH] Include MoveTo Helper class to ign-rendering Signed-off-by: ahcorde --- include/ignition/rendering/MoveToHelper.hh | 97 +++++++++++ src/MoveToHelper.cc | 178 +++++++++++++++++++++ 2 files changed, 275 insertions(+) create mode 100644 include/ignition/rendering/MoveToHelper.hh create mode 100644 src/MoveToHelper.cc diff --git a/include/ignition/rendering/MoveToHelper.hh b/include/ignition/rendering/MoveToHelper.hh new file mode 100644 index 000000000..34a55235c --- /dev/null +++ b/include/ignition/rendering/MoveToHelper.hh @@ -0,0 +1,97 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_GUI_MOVETOHELPER_HH_ +#define IGNITION_GUI_MOVETOHELPER_HH_ + +#include + +#include +#include + +#include +#include + +#include "ignition/rendering/Camera.hh" + +namespace ignition +{ + namespace rendering + { + /// \brief Helper class for animating a user camera to move to a target + /// entity + 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 _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 _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 _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 poseAnim; + + /// \brief Pointer to the camera being moved + public: rendering::CameraPtr camera; + + /// \brief Callback function when animation is complete. + public: std::function onAnimationComplete; + + /// \brief Initial pose of the camera used for view angles + public: math::Pose3d initCameraPose; + }; + } +} +#endif diff --git a/src/MoveToHelper.cc b/src/MoveToHelper.cc new file mode 100644 index 000000000..d20728f7d --- /dev/null +++ b/src/MoveToHelper.cc @@ -0,0 +1,178 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "ignition/rendering/MoveToHelper.hh" + +#include + +namespace ignition +{ +namespace rendering +{ +//////////////////////////////////////////////// +void MoveToHelper::MoveTo(const rendering::CameraPtr &_camera, + const ignition::math::Pose3d &_target, + double _duration, std::function _onAnimationComplete) +{ + this->camera = _camera; + this->poseAnim = std::make_unique( + "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 _onAnimationComplete) +{ + this->camera = _camera; + this->poseAnim = std::make_unique( + "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 _onAnimationComplete) +{ + this->camera = _camera; + this->poseAnim = std::make_unique( + "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; +} +} +}