From 08ebe929f83f727b8921362423e8088ba5be3553 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 15 Jan 2025 18:32:08 +0000 Subject: [PATCH] Update function and variable naming, add demo Signed-off-by: Ian Chen --- examples/frustum_visual/CMakeLists.txt | 28 ++ examples/frustum_visual/GlutWindow.cc | 393 ++++++++++++++++++ examples/frustum_visual/GlutWindow.hh | 31 ++ examples/frustum_visual/Main.cc | 180 ++++++++ include/gz/rendering/FrustumVisual.hh | 38 +- .../gz/rendering/base/BaseFrustumVisual.hh | 60 +-- .../gz/rendering/ogre/OgreFrustumVisual.hh | 2 +- ogre/src/OgreFrustumVisual.cc | 3 + .../gz/rendering/ogre2/Ogre2FrustumVisual.hh | 2 +- ogre2/src/Ogre2FrustumVisual.cc | 3 +- 10 files changed, 675 insertions(+), 65 deletions(-) create mode 100644 examples/frustum_visual/CMakeLists.txt create mode 100644 examples/frustum_visual/GlutWindow.cc create mode 100644 examples/frustum_visual/GlutWindow.hh create mode 100644 examples/frustum_visual/Main.cc diff --git a/examples/frustum_visual/CMakeLists.txt b/examples/frustum_visual/CMakeLists.txt new file mode 100644 index 000000000..675e5081a --- /dev/null +++ b/examples/frustum_visual/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.22.1 FATAL_ERROR) +project(gz-rendering-frustum-visual) +find_package(gz-rendering10 REQUIRED) + +find_package(GLUT REQUIRED) +include_directories(SYSTEM ${GLUT_INCLUDE_DIRS}) +link_directories(${GLUT_LIBRARY_DIRS}) + +find_package(OpenGL REQUIRED) +include_directories(SYSTEM ${OpenGL_INCLUDE_DIRS}) +link_directories(${OpenGL_LIBRARY_DIRS}) + +if (NOT APPLE) + find_package(GLEW REQUIRED) + include_directories(SYSTEM ${GLEW_INCLUDE_DIRS}) + link_directories(${GLEW_LIBRARY_DIRS}) +endif() + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-declarations") + +add_executable(frustum_visual Main.cc GlutWindow.cc) + +target_link_libraries(frustum_visual + ${GLUT_LIBRARIES} + ${OPENGL_LIBRARIES} + ${GLEW_LIBRARIES} + ${GZ-RENDERING_LIBRARIES} +) diff --git a/examples/frustum_visual/GlutWindow.cc b/examples/frustum_visual/GlutWindow.cc new file mode 100644 index 000000000..4b1a93de3 --- /dev/null +++ b/examples/frustum_visual/GlutWindow.cc @@ -0,0 +1,393 @@ +/* + * Copyright (C) 2025 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. + * + */ + +#if __APPLE__ + #include + #include + #include +#else + #include + #include + #include +#endif + +#if !defined(__APPLE__) && !defined(_WIN32) + #include +#endif + +#include + +#include +#include +#include +#include +#include +#include + +#include "GlutWindow.hh" + +#define KEY_ESC 27 +#define KEY_TAB 9 + +////////////////////////////////////////////////// +unsigned int imgw = 0; +unsigned int imgh = 0; + +std::vector g_cameras; +ir::CameraPtr g_camera; +ir::CameraPtr g_currCamera; +unsigned int g_cameraIndex = 0; +ir::ImagePtr g_image; + + +std::vector g_nodes; + +bool g_initContext = false; + +#if __APPLE__ + CGLContextObj g_context; + CGLContextObj g_glutContext; +#elif _WIN32 +#else + GLXContext g_context; + Display *g_display; + GLXDrawable g_drawable; + GLXContext g_glutContext; + Display *g_glutDisplay; + GLXDrawable g_glutDrawable; +#endif + +// view control variables +ir::RayQueryPtr g_rayQuery; +ir::OrbitViewController g_viewControl; +ir::RayQueryResult g_target; +struct mouseButton +{ + int button = 0; + int state = GLUT_UP; + int x = 0; + int y = 0; + int motionX = 0; + int motionY = 0; + int dragX = 0; + int dragY = 0; + int scroll = 0; + bool buttonDirty = false; + bool motionDirty = false; +}; +struct mouseButton g_mouse; +std::mutex g_mouseMutex; + +////////////////////////////////////////////////// +void mouseCB(int _button, int _state, int _x, int _y) +{ + // ignore unknown mouse button numbers + if (_button >= 5) + return; + + std::lock_guard lock(g_mouseMutex); + g_mouse.button = _button; + g_mouse.state = _state; + g_mouse.x = _x; + g_mouse.y = _y; + g_mouse.motionX = _x; + g_mouse.motionY = _y; + g_mouse.buttonDirty = true; +} + +////////////////////////////////////////////////// +void motionCB(int _x, int _y) +{ + std::lock_guard lock(g_mouseMutex); + int deltaX = _x - g_mouse.motionX; + int deltaY = _y - g_mouse.motionY; + g_mouse.motionX = _x; + g_mouse.motionY = _y; + + if (g_mouse.motionDirty) + { + g_mouse.dragX += deltaX; + g_mouse.dragY += deltaY; + } + else + { + g_mouse.dragX = deltaX; + g_mouse.dragY = deltaY; + } + g_mouse.motionDirty = true; +} + +////////////////////////////////////////////////// +void handleMouse() +{ + std::lock_guard lock(g_mouseMutex); + // only ogre supports ray query for now so use + // ogre camera located at camera index = 0. + ir::CameraPtr rayCamera = g_cameras[0]; + if (!g_rayQuery) + { + g_rayQuery = rayCamera->Scene()->CreateRayQuery(); + if (!g_rayQuery) + { + gzerr << "Failed to create Ray Query" << std::endl; + return; + } + } + if (g_mouse.buttonDirty) + { + g_mouse.buttonDirty = false; + double nx = + 2.0 * g_mouse.x / static_cast(rayCamera->ImageWidth()) - 1.0; + double ny = 1.0 - + 2.0 * g_mouse.y / static_cast(rayCamera->ImageHeight()); + g_rayQuery->SetFromCamera(rayCamera, gz::math::Vector2d(nx, ny)); + g_target = g_rayQuery->ClosestPoint(); + if (!g_target) + { + // set point to be 10m away if no intersection found + g_target.point = g_rayQuery->Origin() + g_rayQuery->Direction() * 10; + return; + } + + // mouse wheel scroll zoom + if ((g_mouse.button == 3 || g_mouse.button == 4) && + g_mouse.state == GLUT_UP) + { + double scroll = (g_mouse.button == 3) ? -1.0 : 1.0; + double distance = rayCamera->WorldPosition().Distance( + g_target.point); + int factor = 1; + double amount = -(scroll * factor) * (distance / 5.0); + for (ir::CameraPtr camera : g_cameras) + { + g_viewControl.SetCamera(camera); + g_viewControl.SetTarget(g_target.point); + g_viewControl.Zoom(amount); + } + } + } + + if (g_mouse.motionDirty) + { + g_mouse.motionDirty = false; + auto drag = gz::math::Vector2d(g_mouse.dragX, g_mouse.dragY); + + // left mouse button pan + if (g_mouse.button == GLUT_LEFT_BUTTON && g_mouse.state == GLUT_DOWN) + { + for (ir::CameraPtr camera : g_cameras) + { + g_viewControl.SetCamera(camera); + g_viewControl.SetTarget(g_target.point); + g_viewControl.Pan(drag); + } + } + else if (g_mouse.button == GLUT_MIDDLE_BUTTON && g_mouse.state == GLUT_DOWN) + { + for (ir::CameraPtr camera : g_cameras) + { + g_viewControl.SetCamera(camera); + g_viewControl.SetTarget(g_target.point); + g_viewControl.Orbit(drag); + } + } + // right mouse button zoom + else if (g_mouse.button == GLUT_RIGHT_BUTTON && g_mouse.state == GLUT_DOWN) + { + double hfov = rayCamera->HFOV().Radian(); + double vfov = 2.0f * atan(tan(hfov / 2.0f) / + rayCamera->AspectRatio()); + double distance = rayCamera->WorldPosition().Distance( + g_target.point); + double amount = ((-g_mouse.dragY / + static_cast(rayCamera->ImageHeight())) + * distance * tan(vfov/2.0) * 6.0); + for (ir::CameraPtr camera : g_cameras) + { + g_viewControl.SetCamera(camera); + g_viewControl.SetTarget(g_target.point); + g_viewControl.Zoom(amount); + } + } + } +} + + +////////////////////////////////////////////////// +void displayCB() +{ +#if __APPLE__ + CGLSetCurrentContext(g_context); +#elif _WIN32 +#else + if (g_display) + { + glXMakeCurrent(g_display, g_drawable, g_context); + } +#endif + + g_cameras[g_cameraIndex]->Capture(*g_image); + handleMouse(); + +#if __APPLE__ + CGLSetCurrentContext(g_glutContext); +#elif _WIN32 +#else + glXMakeCurrent(g_glutDisplay, g_glutDrawable, g_glutContext); +#endif + + unsigned char *data = g_image->Data(); + + glClearColor(0.5, 0.5, 0.5, 1); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glPixelZoom(1, -1); + glRasterPos2f(-1, 1); + glDrawPixels(imgw, imgh, GL_RGB, GL_UNSIGNED_BYTE, data); + + glutSwapBuffers(); +} + +////////////////////////////////////////////////// +void idleCB() +{ + glutPostRedisplay(); +} + +////////////////////////////////////////////////// +void keyboardCB(unsigned char _key, int, int) +{ + if (_key == KEY_ESC || _key == 'q' || _key == 'Q') + { + exit(0); + } + else if (_key == KEY_TAB) + { + g_cameraIndex = (g_cameraIndex + 1) % g_cameras.size(); + } + + // main node movement control + double posIncr = 0.03; + double yawIncr = 0.03; + for (ir::NodePtr node : g_nodes) + { + if (!node) + { + std::cerr << "Main node not found! " << std::endl; + return; + } + if (_key == 'w' || _key == 'W') + { + node->SetWorldPosition(node->WorldPosition() + + node->WorldRotation() * gz::math::Vector3d(posIncr, 0, 0)); + } + else if (_key == 's' || _key == 'S') + { + node->SetWorldPosition(node->WorldPosition() + + node->WorldRotation() * gz::math::Vector3d(-posIncr, 0, 0)); + } + else if (_key == 'a' || _key == 'A') + { + node->SetWorldRotation(gz::math::Quaterniond(0, 0, + node->WorldRotation().Yaw() + yawIncr)); + } + else if (_key == 'd' || _key == 'D') + { + node->SetWorldRotation(gz::math::Quaterniond(0, 0, + node->WorldRotation().Yaw() - yawIncr)); + } + } +} + +////////////////////////////////////////////////// +void initCamera(ir::CameraPtr _camera) +{ + g_camera = _camera; + imgw = g_camera->ImageWidth(); + imgh = g_camera->ImageHeight(); + ir::Image image = g_camera->CreateImage(); + g_image = std::make_shared(image); + g_camera->Capture(*g_image); +} + +////////////////////////////////////////////////// +void initContext() +{ + glutInitDisplayMode(GLUT_DOUBLE); + glutInitWindowPosition(0, 0); + glutInitWindowSize(imgw, imgh); + glutCreateWindow("Frustum Visual"); + glutDisplayFunc(displayCB); + glutIdleFunc(idleCB); + glutKeyboardFunc(keyboardCB); + + glutMouseFunc(mouseCB); + glutMotionFunc(motionCB); +} + +////////////////////////////////////////////////// +void printUsage() +{ + std::cout << "===============================" << std::endl; + std::cout << " TAB - Switch render engines " << std::endl; + std::cout << " ESC - Exit " << std::endl; + std::cout << " " << std::endl; + std::cout << " W: Move box forward " << std::endl; + std::cout << " S: Move box backward " << std::endl; + std::cout << " A: Rotate box to the left " << std::endl; + std::cout << " D: Rotate box to the right " << std::endl; + std::cout << "===============================" << std::endl; +} + +////////////////////////////////////////////////// +void run(std::vector _cameras, + const std::vector &_nodes) +{ + if (_cameras.empty()) + { + gzerr << "No cameras found. Scene will not be rendered" << std::endl; + return; + } + +#if __APPLE__ + g_context = CGLGetCurrentContext(); +#elif _WIN32 +#else + g_context = glXGetCurrentContext(); + g_display = glXGetCurrentDisplay(); + g_drawable = glXGetCurrentDrawable(); +#endif + + g_cameras = _cameras; + initCamera(_cameras[0]); + + // main node to track + g_nodes = _nodes; + + initContext(); + printUsage(); + +#if __APPLE__ + g_glutContext = CGLGetCurrentContext(); +#elif _WIN32 +#else + g_glutDisplay = glXGetCurrentDisplay(); + g_glutDrawable = glXGetCurrentDrawable(); + g_glutContext = glXGetCurrentContext(); +#endif + + glutMainLoop(); +} diff --git a/examples/frustum_visual/GlutWindow.hh b/examples/frustum_visual/GlutWindow.hh new file mode 100644 index 000000000..8a4abcb52 --- /dev/null +++ b/examples/frustum_visual/GlutWindow.hh @@ -0,0 +1,31 @@ +/* + * Copyright (C) 2025 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 GZ_RENDERING_EXAMPLES_FRUSTUM_VISUAL_GLUTWINDOW_HH_ +#define GZ_RENDERING_EXAMPLES_FRUSTUM_VISUAL_GLUTWINDOW_HH_ + +#include +#include + +namespace ir = gz::rendering; + +/// \brief Run the demo and render the scene from the cameras +/// \param[in] _cameras Cameras in the scene +/// \param[in] _nodes Nodes being tracked / followed in the scene +void run(std::vector _cameras, + const std::vector &_nodes); + +#endif diff --git a/examples/frustum_visual/Main.cc b/examples/frustum_visual/Main.cc new file mode 100644 index 000000000..f05e8edef --- /dev/null +++ b/examples/frustum_visual/Main.cc @@ -0,0 +1,180 @@ +/* + * Copyright (C) 2025 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. + * + */ + +#if defined(__APPLE__) + #include + #include +#elif not defined(_WIN32) + #include + #include + #include +#endif + +#include +#include + +#include +#include +#include "GlutWindow.hh" + +using namespace gz; +using namespace rendering; + +////////////////////////////////////////////////// +void buildScene(ScenePtr _scene) +{ + // initialize _scene + _scene->SetAmbientLight(0.3, 0.3, 0.3); + VisualPtr root = _scene->RootVisual(); + + // create directional light + DirectionalLightPtr light0 = _scene->CreateDirectionalLight(); + light0->SetDirection(-0.5, 0.5, -1); + light0->SetDiffuseColor(0.5, 0.5, 0.5); + light0->SetSpecularColor(0.5, 0.5, 0.5); + root->AddChild(light0); + + // create grid visual + VisualPtr grid = _scene->CreateVisual(); + GridPtr gridGeom = _scene->CreateGrid(); + gridGeom->SetCellCount(20); + gridGeom->SetCellLength(1); + gridGeom->SetVerticalCellCount(0); + grid->AddGeometry(gridGeom); + grid->SetLocalPosition(3, 0, 0.0); + root->AddChild(grid); + + // create camera + CameraPtr camera = _scene->CreateCamera("camera"); + camera->SetLocalPosition(0.0, 0.0, 3.0); + camera->SetLocalRotation(0.0, 0.0, 0.0); + camera->SetImageWidth(800); + camera->SetImageHeight(600); + camera->SetAntiAliasing(2); + camera->SetAspectRatio(1.333); + camera->SetHFOV(GZ_PI / 2); + root->AddChild(camera); +} + + +////////////////////////////////////////////////// +NodePtr createMainNode(ScenePtr _scene) +{ + // create green material + MaterialPtr green = _scene->CreateMaterial(); + green->SetAmbient(0.0, 0.5, 0.0); + green->SetDiffuse(0.0, 0.7, 0.0); + green->SetSpecular(0.5, 0.5, 0.5); + green->SetShininess(50); + green->SetReflectivity(0); + + // create box visual + VisualPtr box = _scene->CreateVisual(); + box->AddGeometry(_scene->CreateBox()); + box->SetLocalPosition(3, 0, 0); + box->SetMaterial(green); + + VisualPtr root = _scene->RootVisual(); + root->AddChild(box); + + // create frustum visual and attach to main node + FrustumVisualPtr frustumVisual = _scene->CreateFrustumVisual(); + frustumVisual->SetNearClipPlane(1); + frustumVisual->SetFarClipPlane(5); + frustumVisual->SetHFOV(0.7); + frustumVisual->Update(); + box->AddChild(frustumVisual); + + return std::dynamic_pointer_cast(box); +} + +////////////////////////////////////////////////// +CameraPtr createCamera(const std::string &_engineName, + const std::map& _params) +{ + // create and populate scene + RenderEngine *engine = rendering::engine(_engineName, _params); + if (!engine) + { + std::cout << "Engine '" << _engineName + << "' is not supported" << std::endl; + return CameraPtr(); + } + ScenePtr scene = engine->CreateScene("scene"); + buildScene(scene); + + // return camera sensor + SensorPtr sensor = scene->SensorByName("camera"); + return std::dynamic_pointer_cast(sensor); +} + +////////////////////////////////////////////////// +int main(int _argc, char** _argv) +{ + glutInit(&_argc, _argv); + + // Expose engine name to command line because we can't instantiate both + // ogre and ogre2 at the same time + std::string ogreEngineName("ogre2"); + if (_argc > 1) + { + ogreEngineName = _argv[1]; + } + + GraphicsAPI graphicsApi = defaultGraphicsAPI(); + if (_argc > 2) + { + graphicsApi = GraphicsAPIUtils::Set(std::string(_argv[2])); + } + + common::Console::SetVerbosity(4); + std::vector engineNames; + std::vector cameras; + std::vector nodes; + + engineNames.push_back(ogreEngineName); + + for (auto engineName : engineNames) + { + try + { + std::map params; + if (engineName.compare("ogre2") == 0 + && graphicsApi == GraphicsAPI::METAL) + { + params["metal"] = "1"; + } + + CameraPtr camera = createCamera(engineName, params); + if (camera) + { + cameras.push_back(camera); + NodePtr node = createMainNode(camera->Scene()); + if (node) + nodes.push_back(node); + camera->SetTrackTarget(node, math::Vector3d(0.5, 0, 0)); + } + } + catch (...) + { + // std::cout << ex.what() << std::endl; + std::cerr << "Error starting up: " << engineName << std::endl; + } + } + run(cameras, nodes); + return 0; +} diff --git a/include/gz/rendering/FrustumVisual.hh b/include/gz/rendering/FrustumVisual.hh index 25519a11a..59552bbe5 100644 --- a/include/gz/rendering/FrustumVisual.hh +++ b/include/gz/rendering/FrustumVisual.hh @@ -64,7 +64,7 @@ namespace gz /// /// * near: 0.0 /// * far: 1.0 - /// * fov: 0.78539 radians (45 degrees) + /// * hfov: 0.78539 radians (45 degrees) /// * aspect ratio: 1.0 /// * pose: Pose3d::Zero protected: FrustumVisual(); @@ -78,40 +78,40 @@ namespace gz /// \brief Get the near distance. This is the distance from the /// frustum's vertex to the closest plane. /// \return Near distance. - /// \sa SetNear - public: virtual double Near() const = 0; + /// \sa SetNearClipPlane + public: virtual double NearClipPlane() const = 0; /// \brief Set the near distance. This is the distance from the /// frustum's vertex to the closest plane. /// \param[in] _near Near distance. - /// \sa Near - public: virtual void SetNear(double _near) = 0; + /// \sa NearClipPlane + public: virtual void SetNearClipPlane(double _near) = 0; /// \brief Get the far distance. This is the distance from the /// frustum's vertex to the farthest plane. /// \return Far distance. - /// \sa SetFar - public: virtual double Far() const = 0; + /// \sa SetFarClipPlane + public: virtual double FarClipPlane() const = 0; /// \brief Set the far distance. This is the distance from the /// frustum's vertex to the farthest plane. /// \param[in] _far Far distance. - /// \sa Far - public: virtual void SetFar(double _far) = 0; + /// \sa FarClipPlane + public: virtual void SetFarClipPlane(double _far) = 0; /// \brief Get the horizontal field of view. The field of view is the /// angle between the frustum's vertex and the edges of the near or far /// plane. This value represents the horizontal angle. /// \return The field of view. - /// \sa SetFOV - public: virtual gz::math::Angle FOV() const = 0; + /// \sa SetHFOV + public: virtual gz::math::Angle HFOV() const = 0; /// \brief Set the horizontal field of view. The field of view is the /// angle between the frustum's vertex and the edges of the near or far /// plane. This value represents the horizontal angle. - /// \param[in] _fov The field of view. - /// \sa FOV - public: virtual void SetFOV(const gz::math::Angle &_fov) = 0; + /// \param[in] _hfov The field of view. + /// \sa HFOV + public: virtual void SetHFOV(const gz::math::Angle &_hfov) = 0; /// \brief Get the aspect ratio, which is the width divided by height /// of the near or far planes. @@ -131,16 +131,6 @@ namespace gz public: virtual gz::math::Planed Plane( const FrustumVisualPlane _plane) const = 0; - /// \brief Get the pose of the frustum - /// \return Pose of the frustum - /// \sa SetPose - public: virtual gz::math::Pose3d Pose() const = 0; - - /// \brief Set the pose of the frustum - /// \param[in] _pose Pose of the frustum, top vertex. - /// \sa Pose - public: virtual void SetPose(const gz::math::Pose3d &_pose) = 0; - /// \brief Compute the planes of the frustum. This is called whenever /// a property of the frustum is changed. private: void ComputePlanes(); diff --git a/include/gz/rendering/base/BaseFrustumVisual.hh b/include/gz/rendering/base/BaseFrustumVisual.hh index 9fef37b72..51e31d5b9 100644 --- a/include/gz/rendering/base/BaseFrustumVisual.hh +++ b/include/gz/rendering/base/BaseFrustumVisual.hh @@ -17,7 +17,7 @@ #ifndef GZ_RENDERING_BASEFRUSTUMVISUAL_HH_ #define GZ_RENDERING_BASEFRUSTUMVISUAL_HH_ -#include +#include #include "gz/rendering/FrustumVisual.hh" #include "gz/rendering/base/BaseObject.hh" @@ -54,22 +54,22 @@ namespace gz public: virtual void Init() override; // Documentation inherited - public: virtual double Near() const override; + public: virtual double NearClipPlane() const override; // Documentation inherited - public: virtual void SetNear(double _near) override; + public: virtual void SetNearClipPlane(double _near) override; // Documentation inherited - public: virtual double Far() const override; + public: virtual double FarClipPlane() const override; // Documentation inherited - public: virtual void SetFar(double _far) override; + public: virtual void SetFarClipPlane(double _far) override; // Documentation inherited - public: virtual math::Angle FOV() const override; + public: virtual math::Angle HFOV() const override; // Documentation inherited - public: virtual void SetFOV(const math::Angle &_fov) override; + public: virtual void SetHFOV(const math::Angle &_hfov) override; // Documentation inherited public: virtual double AspectRatio() const override; @@ -81,11 +81,11 @@ namespace gz public: virtual gz::math::Planed Plane( const FrustumVisualPlane _plane) const override; - // Documentation inherited - public: virtual gz::math::Pose3d Pose() const override; - - // Documentation inherited - public: virtual void SetPose(const gz::math::Pose3d &_pose) override; +// // Documentation inherited +// public: virtual gz::math::Pose3d Pose() const override; +// +// // Documentation inherited +// public: virtual void SetPose(const gz::math::Pose3d &_pose) override; /// \brief Create predefined materials for lidar visual public: virtual void CreateMaterials(); @@ -97,7 +97,7 @@ namespace gz protected: double far = 1.0; /// \brief fov value - protected: gz::math::Angle fov = 0.78539; + protected: gz::math::Angle hfov = 0.78539; /// \brief aspect ratio value protected: double aspectRatio = 1.0; @@ -154,47 +154,45 @@ namespace gz ///////////////////////////////////////////////// template - void BaseFrustumVisual::SetNear( - double _near) + void BaseFrustumVisual::SetNearClipPlane(double _near) { this->near = _near; } ///////////////////////////////////////////////// template - double BaseFrustumVisual::Near() const + double BaseFrustumVisual::NearClipPlane() const { return this->near; } ///////////////////////////////////////////////// template - void BaseFrustumVisual::SetFar( - double _far) + void BaseFrustumVisual::SetFarClipPlane(double _far) { this->far = _far; } ///////////////////////////////////////////////// template - double BaseFrustumVisual::Far() const + double BaseFrustumVisual::FarClipPlane() const { return this->far; } ///////////////////////////////////////////////// template - void BaseFrustumVisual::SetFOV( - const gz::math::Angle &_fov) + void BaseFrustumVisual::SetHFOV( + const gz::math::Angle &_hfov) { - this->fov = _fov; + this->hfov = _hfov; } ///////////////////////////////////////////////// template - gz::math::Angle BaseFrustumVisual::FOV() const + gz::math::Angle BaseFrustumVisual::HFOV() const { - return this->fov; + return this->hfov; } ///////////////////////////////////////////////// @@ -220,19 +218,6 @@ namespace gz return this->planes[_plane]; } - ///////////////////////////////////////////////// - template - void BaseFrustumVisual::SetPose(const gz::math::Pose3d &_pose) - { - this->pose = _pose; - } - - ///////////////////////////////////////////////// - template - gz::math::Pose3d BaseFrustumVisual::Pose() const - { - return this->pose; - } ///////////////////////////////////////////////// template void BaseFrustumVisual::CreateMaterials() @@ -253,7 +238,6 @@ namespace gz mtl->SetMetalness(0.1f); mtl->SetReflectivity(0.2); } - return; } } } diff --git a/ogre/include/gz/rendering/ogre/OgreFrustumVisual.hh b/ogre/include/gz/rendering/ogre/OgreFrustumVisual.hh index 3b7ea484a..8985484ac 100644 --- a/ogre/include/gz/rendering/ogre/OgreFrustumVisual.hh +++ b/ogre/include/gz/rendering/ogre/OgreFrustumVisual.hh @@ -18,8 +18,8 @@ #ifndef GZ_RENDERING_OGRE_OGREFRUSTUMVISUAL_HH_ #define GZ_RENDERING_OGRE_OGREFRUSTUMVISUAL_HH_ -#include #include + #include "gz/rendering/base/BaseFrustumVisual.hh" #include "gz/rendering/ogre/OgreVisual.hh" #include "gz/rendering/ogre/OgreIncludes.hh" diff --git a/ogre/src/OgreFrustumVisual.cc b/ogre/src/OgreFrustumVisual.cc index ed76b1067..a2e8572a7 100644 --- a/ogre/src/OgreFrustumVisual.cc +++ b/ogre/src/OgreFrustumVisual.cc @@ -15,6 +15,9 @@ * */ +#include +#include + #include #include "gz/rendering/ogre/OgreDynamicLines.hh" #include "gz/rendering/ogre/OgreFrustumVisual.hh" diff --git a/ogre2/include/gz/rendering/ogre2/Ogre2FrustumVisual.hh b/ogre2/include/gz/rendering/ogre2/Ogre2FrustumVisual.hh index ad67cf85b..efe37afee 100644 --- a/ogre2/include/gz/rendering/ogre2/Ogre2FrustumVisual.hh +++ b/ogre2/include/gz/rendering/ogre2/Ogre2FrustumVisual.hh @@ -19,7 +19,7 @@ #define GZ_RENDERING_OGRE2_OGREFRUSTUMVISUAL_HH_ #include -#include + #include "gz/rendering/base/BaseFrustumVisual.hh" #include "gz/rendering/ogre2/Ogre2Visual.hh" #include "gz/rendering/ogre2/Ogre2Includes.hh" diff --git a/ogre2/src/Ogre2FrustumVisual.cc b/ogre2/src/Ogre2FrustumVisual.cc index 8b93d3a49..75a67a062 100644 --- a/ogre2/src/Ogre2FrustumVisual.cc +++ b/ogre2/src/Ogre2FrustumVisual.cc @@ -29,6 +29,7 @@ #include #include #include +#include #include @@ -157,7 +158,7 @@ void Ogre2FrustumVisual::Update() this->dataPtr->rayLines.push_back(renderable); // Tangent of half the field of view. - double tanFOV2 = std::tan(this->fov() * 0.5); + double tanFOV2 = std::tan(this->hfov() * 0.5); // Width of near plane double nearWidth = 2.0 * tanFOV2 * this->near;