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 new file mode 100644 index 000000000..59552bbe5 --- /dev/null +++ b/include/gz/rendering/FrustumVisual.hh @@ -0,0 +1,141 @@ +/* + * 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_FRUSTUMVISUAL_HH_ +#define GZ_RENDERING_FRUSTUMVISUAL_HH_ + +#include +#include +#include +#include +#include +#include "gz/rendering/config.hh" +#include "gz/rendering/Visual.hh" +#include "gz/rendering/Object.hh" +#include "gz/rendering/RenderTypes.hh" +#include "gz/rendering/Marker.hh" + +namespace gz +{ + namespace rendering + { + inline namespace GZ_RENDERING_VERSION_NAMESPACE { + // + /// \brief Planes that define the boundaries of the frustum. + enum GZ_RENDERING_VISIBLE FrustumVisualPlane + { + /// \brief Near plane + FRUSTUM_PLANE_NEAR = 0, + + /// \brief Far plane + FRUSTUM_PLANE_FAR = 1, + + /// \brief Left plane + FRUSTUM_PLANE_LEFT = 2, + + /// \brief Right plane + FRUSTUM_PLANE_RIGHT = 3, + + /// \brief Top plane + FRUSTUM_PLANE_TOP = 4, + + /// \brief Bottom plane + FRUSTUM_PLANE_BOTTOM = 5 + }; + + /// \brief Mathematical representation of a frustum and related functions. + /// This is also known as a view frustum. + class GZ_RENDERING_VISIBLE FrustumVisual : public virtual Visual + { + /// \brief Default constructor. With the following default values: + /// + /// * near: 0.0 + /// * far: 1.0 + /// * hfov: 0.78539 radians (45 degrees) + /// * aspect ratio: 1.0 + /// * pose: Pose3d::Zero + protected: FrustumVisual(); + + /// \brief Destructor + public: virtual ~FrustumVisual(); + + /// \brief Update the Visual + public: virtual void Update() = 0; + + /// \brief Get the near distance. This is the distance from the + /// frustum's vertex to the closest plane. + /// \return Near distance. + /// \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 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 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 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 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] _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. + /// \return The frustum's aspect ratio. + /// \sa SetAspectRatio + public: virtual double AspectRatio() const = 0; + + /// \brief Set the aspect ratio, which is the width divided by height + /// of the near or far planes. + /// \param[in] _aspectRatio The frustum's aspect ratio. + /// \sa AspectRatio + public: virtual void SetAspectRatio(double _aspectRatio) = 0; + + /// \brief Get a plane of the frustum. + /// \param[in] _plane The plane to return. + /// \return Plane of the frustum. + public: virtual gz::math::Planed Plane( + const FrustumVisualPlane _plane) const = 0; + + /// \brief Compute the planes of the frustum. This is called whenever + /// a property of the frustum is changed. + private: void ComputePlanes(); + }; + } + } +} +#endif diff --git a/include/gz/rendering/RenderTypes.hh b/include/gz/rendering/RenderTypes.hh index 2a7c5e0e8..addac9636 100644 --- a/include/gz/rendering/RenderTypes.hh +++ b/include/gz/rendering/RenderTypes.hh @@ -72,6 +72,7 @@ namespace gz class LightVisual; class JointVisual; class LidarVisual; + class FrustumVisual; class Light; class Marker; class Material; @@ -206,6 +207,10 @@ namespace gz /// \brief Shared pointer to LidarVisual typedef shared_ptr LidarVisualPtr; + /// \typedef FrustumVisualPtr + /// \brief Shared pointer to FrustumVisual + typedef shared_ptr FrustumVisualPtr; + /// \typedef MaterialPtr /// \brief Shared pointer to Material typedef shared_ptr MaterialPtr; @@ -384,6 +389,10 @@ namespace gz /// \brief Shared pointer to const LidarVisual typedef shared_ptr ConstLidarVisualPtr; + /// \typedef const FrustumVisualPtr + /// \brief Shared pointer to const FrustumVisual + typedef shared_ptr ConstFrustumVisualPtr; + /// \typedef const MaterialPtr /// \brief Shared pointer to const Material typedef shared_ptr ConstMaterialPtr; diff --git a/include/gz/rendering/Scene.hh b/include/gz/rendering/Scene.hh index fdaa6436d..71f2467da 100644 --- a/include/gz/rendering/Scene.hh +++ b/include/gz/rendering/Scene.hh @@ -1108,6 +1108,35 @@ namespace gz public: virtual LidarVisualPtr CreateLidarVisual( unsigned int _id, const std::string &_name) = 0; + /// \brief Create new frusum visual. A unique ID and name will + /// automatically be assigned to the frustum visual. + /// \return The created frustum visual + public: virtual FrustumVisualPtr CreateFrustumVisual() = 0; + + /// \brief Create new frustum visual with the given ID. A unique name + /// will automatically be assigned to the frustum visual. If the given + /// ID is already in use, NULL will be returned. + /// \param[in] _id ID of the new frustum visual + /// \return The created frustum visual + public: virtual FrustumVisualPtr CreateFrustumVisual( + unsigned int _id) = 0; + + /// \brief Create new frustum visual with the given name. A unique ID + /// will automatically be assigned to the frustum visual. If the given + /// name is already in use, NULL will be returned. + /// \param[in] _name Name of the new frustum visual + /// \return The created frustum visual + public: virtual FrustumVisualPtr CreateFrustumVisual( + const std::string &_name) = 0; + + /// \brief Create new frustum visual with the given name. If either + /// the given ID or name is already in use, NULL will be returned. + /// \param[in] _id ID of the frustum visual. + /// \param[in] _name Name of the new frustum visual. + /// \return The created frustum visual + public: virtual FrustumVisualPtr CreateFrustumVisual( + unsigned int _id, const std::string &_name) = 0; + /// \brief Create new heightmap geomerty. The rendering::Heightmap will be /// created from the given HeightmapDescriptor. /// \param[in] _desc Data about the heightmap diff --git a/include/gz/rendering/base/BaseFrustumVisual.hh b/include/gz/rendering/base/BaseFrustumVisual.hh new file mode 100644 index 000000000..a26fb7f6e --- /dev/null +++ b/include/gz/rendering/base/BaseFrustumVisual.hh @@ -0,0 +1,239 @@ +/* + * 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_BASEFRUSTUMVISUAL_HH_ +#define GZ_RENDERING_BASEFRUSTUMVISUAL_HH_ + +#include + +#include "gz/rendering/FrustumVisual.hh" +#include "gz/rendering/base/BaseObject.hh" +#include "gz/rendering/base/BaseRenderTypes.hh" +#include "gz/rendering/Scene.hh" + +namespace gz +{ + namespace rendering + { + inline namespace GZ_RENDERING_VERSION_NAMESPACE { + /// \brief Base implementation of a Frustum Visual + template + class BaseFrustumVisual : + public virtual FrustumVisual, + public virtual T + { + // Documentation inherited + protected: BaseFrustumVisual(); + + // Documentation inherited + public: virtual ~BaseFrustumVisual(); + + // Documentation inherited + public: virtual void PreRender() override; + + // Documentation inherited + public: virtual void Destroy() override; + + // Documentation inherited + public: virtual void Update() override; + + // Documentation inherited + public: virtual void Init() override; + + // Documentation inherited + public: virtual double NearClipPlane() const override; + + // Documentation inherited + public: virtual void SetNearClipPlane(double _near) override; + + // Documentation inherited + public: virtual double FarClipPlane() const override; + + // Documentation inherited + public: virtual void SetFarClipPlane(double _far) override; + + // Documentation inherited + public: virtual math::Angle HFOV() const override; + + // Documentation inherited + public: virtual void SetHFOV(const math::Angle &_hfov) override; + + // Documentation inherited + public: virtual double AspectRatio() const override; + + // Documentation inherited + public: virtual void SetAspectRatio(double _aspectRatio) override; + + // Documentation inherited + public: virtual gz::math::Planed Plane( + const FrustumVisualPlane _plane) const override; + + /// \brief Create predefined materials for lidar visual + public: virtual void CreateMaterials(); + + /// \brief near value + protected: double nearClip = 0.0; + + /// \brief far value + protected: double farClip = 1.0; + + /// \brief fov value + protected: gz::math::Angle hfov = gz::math::Angle(0.78539); + + /// \brief aspect ratio value + protected: double aspectRatio = 1.0; + + /// \brief array of plane + protected: std::array planes; + + /// \brief pose of visual + protected: gz::math::Pose3d pose = gz::math::Pose3d::Zero; + }; + + ///////////////////////////////////////////////// + // BaseFrustumVisual + ///////////////////////////////////////////////// + template + BaseFrustumVisual::BaseFrustumVisual() + { + } + + ///////////////////////////////////////////////// + template + BaseFrustumVisual::~BaseFrustumVisual() + { + } + + ///////////////////////////////////////////////// + template + void BaseFrustumVisual::PreRender() + { + T::PreRender(); + } + + ///////////////////////////////////////////////// + template + void BaseFrustumVisual::Destroy() + { + T::Destroy(); + } + + ///////////////////////////////////////////////// + template + void BaseFrustumVisual::Update() + { + // no op + } + + ///////////////////////////////////////////////// + template + void BaseFrustumVisual::Init() + { + T::Init(); + this->CreateMaterials(); + } + + ///////////////////////////////////////////////// + template + void BaseFrustumVisual::SetNearClipPlane(double _near) + { + this->nearClip = _near; + } + + ///////////////////////////////////////////////// + template + double BaseFrustumVisual::NearClipPlane() const + { + return this->nearClip; + } + + ///////////////////////////////////////////////// + template + void BaseFrustumVisual::SetFarClipPlane(double _far) + { + this->farClip = _far; + } + + ///////////////////////////////////////////////// + template + double BaseFrustumVisual::FarClipPlane() const + { + return this->farClip; + } + + ///////////////////////////////////////////////// + template + void BaseFrustumVisual::SetHFOV( + const gz::math::Angle &_hfov) + { + this->hfov = _hfov; + } + + ///////////////////////////////////////////////// + template + gz::math::Angle BaseFrustumVisual::HFOV() const + { + return this->hfov; + } + + ///////////////////////////////////////////////// + template + void BaseFrustumVisual::SetAspectRatio( + double _aspectRatio) + { + this->aspectRatio = _aspectRatio; + } + + ///////////////////////////////////////////////// + template + double BaseFrustumVisual::AspectRatio() const + { + return this->aspectRatio; + } + + ///////////////////////////////////////////////// + template + gz::math::Planed BaseFrustumVisual::Plane( + const FrustumVisualPlane _plane) const + { + return this->planes[_plane]; + } + + ///////////////////////////////////////////////// + template + void BaseFrustumVisual::CreateMaterials() + { + MaterialPtr mtl; + + if (!this->Scene()->MaterialRegistered("Frustum/BlueRay")) + { + mtl = this->Scene()->CreateMaterial("Frustum/BlueRay"); + mtl->SetAmbient(0.0, 0.0, 1.0); + mtl->SetDiffuse(0.0, 0.0, 1.0); + mtl->SetEmissive(0.0, 0.0, 1.0); + mtl->SetSpecular(0.0, 0.0, 1.0); + mtl->SetTransparency(0.0); + mtl->SetCastShadows(false); + mtl->SetReceiveShadows(false); + mtl->SetLightingEnabled(false); + mtl->SetMetalness(0.1f); + mtl->SetReflectivity(0.2); + } + } + } + } +} +#endif diff --git a/include/gz/rendering/base/BaseScene.hh b/include/gz/rendering/base/BaseScene.hh index 200246a2b..7ec3b8688 100644 --- a/include/gz/rendering/base/BaseScene.hh +++ b/include/gz/rendering/base/BaseScene.hh @@ -537,6 +537,21 @@ namespace gz public: virtual LidarVisualPtr CreateLidarVisual(unsigned int _id, const std::string &_name) override; + // Documentation inherited. + public: virtual FrustumVisualPtr CreateFrustumVisual() override; + + // Documentation inherited. + public: virtual FrustumVisualPtr CreateFrustumVisual( + unsigned int _id) override; + + // Documentation inherited. + public: virtual FrustumVisualPtr CreateFrustumVisual( + const std::string &_name) override; + + // Documentation inherited. + public: virtual FrustumVisualPtr CreateFrustumVisual(unsigned int _id, + const std::string &_name) override; + // Documentation inherited. public: virtual HeightmapPtr CreateHeightmap( const HeightmapDescriptor &_desc) override; @@ -828,6 +843,13 @@ namespace gz protected: virtual LidarVisualPtr CreateLidarVisualImpl(unsigned int _id, const std::string &_name) = 0; + /// \brief Implementation for creating a frustum visual + /// \param[in] _id unique object id. + /// \param[in] _name unique object name. + /// \return Pointer to a frustum visual + protected: virtual FrustumVisualPtr CreateFrustumVisualImpl( + unsigned int _id, const std::string &_name) = 0; + /// \brief Implementation for creating a heightmap geometry /// \param[in] _id Unique object id. /// \param[in] _name Unique object name. diff --git a/ogre/include/gz/rendering/ogre/OgreFrustumVisual.hh b/ogre/include/gz/rendering/ogre/OgreFrustumVisual.hh new file mode 100644 index 000000000..8985484ac --- /dev/null +++ b/ogre/include/gz/rendering/ogre/OgreFrustumVisual.hh @@ -0,0 +1,77 @@ +/* + * 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_OGRE_OGREFRUSTUMVISUAL_HH_ +#define GZ_RENDERING_OGRE_OGREFRUSTUMVISUAL_HH_ + +#include + +#include "gz/rendering/base/BaseFrustumVisual.hh" +#include "gz/rendering/ogre/OgreVisual.hh" +#include "gz/rendering/ogre/OgreIncludes.hh" +#include "gz/rendering/ogre/OgreScene.hh" + +namespace gz +{ + namespace rendering + { + inline namespace GZ_RENDERING_VERSION_NAMESPACE { + // + // Forward declaration + class OgreFrustumVisualPrivate; + + /// \brief Ogre implementation of a Frustum Visual. + class GZ_RENDERING_OGRE_VISIBLE OgreFrustumVisual + : public BaseFrustumVisual + { + /// \brief Constructor + protected: OgreFrustumVisual(); + + /// \brief Destructor + public: virtual ~OgreFrustumVisual(); + + // Documentation inherited. + public: virtual void Init() override; + + // Documentation inherited. + public: virtual void PreRender() override; + + // Documentation inherited. + public: virtual void Destroy() override; + + // Documentation inherited + public: virtual void Update() override; + + /// \brief Create the Frustum Visual in ogre + private: void Create(); + + /// \brief Clear data stored by dynamiclines + private: void ClearVisualData(); + + // Documentation inherited + public: virtual void SetVisible(bool _visible) override; + + /// \brief Frustum Visual should only be created by scene. + private: friend class OgreScene; + + /// \brief Private data class + private: std::unique_ptr dataPtr; + }; + } + } +} +#endif diff --git a/ogre/include/gz/rendering/ogre/OgreRenderTypes.hh b/ogre/include/gz/rendering/ogre/OgreRenderTypes.hh index 06fabd6f8..031caa34d 100644 --- a/ogre/include/gz/rendering/ogre/OgreRenderTypes.hh +++ b/ogre/include/gz/rendering/ogre/OgreRenderTypes.hh @@ -33,6 +33,7 @@ namespace gz class OgreCOMVisual; class OgreDepthCamera; class OgreDirectionalLight; + class OgreFrustumVisual; class OgreGeometry; class OgreGizmoVisual; class OgreGpuRays; @@ -86,6 +87,7 @@ namespace gz typedef shared_ptr OgreCOMVisualPtr; typedef shared_ptr OgreDepthCameraPtr; typedef shared_ptr OgreDirectionalLightPtr; + typedef shared_ptr OgreFrustumVisualPtr; typedef shared_ptr OgreGeometryPtr; typedef shared_ptr OgreGeometryStorePtr; typedef shared_ptr OgreGizmoVisualPtr; diff --git a/ogre/include/gz/rendering/ogre/OgreScene.hh b/ogre/include/gz/rendering/ogre/OgreScene.hh index a55c8de66..bebc39dd9 100644 --- a/ogre/include/gz/rendering/ogre/OgreScene.hh +++ b/ogre/include/gz/rendering/ogre/OgreScene.hh @@ -180,6 +180,10 @@ namespace gz protected: virtual LidarVisualPtr CreateLidarVisualImpl(unsigned int _id, const std::string &_name) override; + // Documentation inherited + protected: virtual FrustumVisualPtr CreateFrustumVisualImpl( + unsigned int _id, const std::string &_name) override; + // Documentation inherited protected: virtual WireBoxPtr CreateWireBoxImpl(unsigned int _id, const std::string &_name) override; diff --git a/ogre/src/OgreFrustumVisual.cc b/ogre/src/OgreFrustumVisual.cc new file mode 100644 index 000000000..0e3594842 --- /dev/null +++ b/ogre/src/OgreFrustumVisual.cc @@ -0,0 +1,102 @@ +/* + * 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. + * + */ + +#include +#include + +#include +#include "gz/rendering/ogre/OgreDynamicLines.hh" +#include "gz/rendering/ogre/OgreFrustumVisual.hh" +#include "gz/rendering/ogre/OgreScene.hh" +#include "gz/rendering/ogre/OgreMarker.hh" +#include "gz/rendering/ogre/OgreGeometry.hh" + + +class gz::rendering::OgreFrustumVisualPrivate +{ + /// \brief Frustum Ray DynamicLines Object to display + public: std::vector> rayLines; + + /// \brief The visibility of the visual + public: bool visible = true; + + /// \brief Each corner of the frustum. + public: std::array points; + + /// \brief each edge of the frustum. + public: std::array, 12> edges; +}; + +using namespace gz; +using namespace rendering; + +////////////////////////////////////////////////// +OgreFrustumVisual::OgreFrustumVisual() + : dataPtr(new OgreFrustumVisualPrivate) +{ +} + +////////////////////////////////////////////////// +OgreFrustumVisual::~OgreFrustumVisual() +{ + // no ops +} + +////////////////////////////////////////////////// +void OgreFrustumVisual::PreRender() +{ + // no ops +} + +////////////////////////////////////////////////// +void OgreFrustumVisual::Destroy() +{ + BaseFrustumVisual::Destroy(); +} + +////////////////////////////////////////////////// +void OgreFrustumVisual::Init() +{ + BaseFrustumVisual::Init(); + this->Create(); +} + +////////////////////////////////////////////////// +void OgreFrustumVisual::Create() +{ + // no ops +} + +////////////////////////////////////////////////// +void OgreFrustumVisual::ClearVisualData() +{ + this->dataPtr->rayLines.clear(); +} + +////////////////////////////////////////////////// +void OgreFrustumVisual::Update() +{ + // no ops +} + +////////////////////////////////////////////////// +void OgreFrustumVisual::SetVisible(bool _visible) +{ + this->dataPtr->visible = _visible; + this->ogreNode->setVisible(this->dataPtr->visible); +} diff --git a/ogre/src/OgreScene.cc b/ogre/src/OgreScene.cc index 36531eb21..981c64fe4 100644 --- a/ogre/src/OgreScene.cc +++ b/ogre/src/OgreScene.cc @@ -35,6 +35,7 @@ #include "gz/rendering/ogre/OgreInertiaVisual.hh" #include "gz/rendering/ogre/OgreJointVisual.hh" #include "gz/rendering/ogre/OgreLidarVisual.hh" +#include "gz/rendering/ogre/OgreFrustumVisual.hh" #include "gz/rendering/ogre/OgreLightVisual.hh" #include "gz/rendering/ogre/OgreMarker.hh" #include "gz/rendering/ogre/OgreMaterial.hh" @@ -622,6 +623,15 @@ LidarVisualPtr OgreScene::CreateLidarVisualImpl(unsigned int _id, return (result) ? lidar: nullptr; } +////////////////////////////////////////////////// +FrustumVisualPtr OgreScene::CreateFrustumVisualImpl(unsigned int _id, + const std::string &_name) +{ + OgreFrustumVisualPtr frustum(new OgreFrustumVisual); + bool result = this->InitObject(frustum, _id, _name); + return (result) ? frustum: nullptr; +} + ////////////////////////////////////////////////// TextPtr OgreScene::CreateTextImpl(unsigned int _id, const std::string &_name) { diff --git a/ogre2/include/gz/rendering/ogre2/Ogre2FrustumVisual.hh b/ogre2/include/gz/rendering/ogre2/Ogre2FrustumVisual.hh new file mode 100644 index 000000000..efe37afee --- /dev/null +++ b/ogre2/include/gz/rendering/ogre2/Ogre2FrustumVisual.hh @@ -0,0 +1,77 @@ +/* + * 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_OGRE2_OGREFRUSTUMVISUAL_HH_ +#define GZ_RENDERING_OGRE2_OGREFRUSTUMVISUAL_HH_ + +#include + +#include "gz/rendering/base/BaseFrustumVisual.hh" +#include "gz/rendering/ogre2/Ogre2Visual.hh" +#include "gz/rendering/ogre2/Ogre2Includes.hh" +#include "gz/rendering/ogre2/Ogre2Scene.hh" + +namespace gz +{ + namespace rendering + { + inline namespace GZ_RENDERING_VERSION_NAMESPACE { + // + // Forward declaration + class Ogre2FrustumVisualPrivate; + + /// \brief Ogre 2.x implementation of a Frustum Visual. + class GZ_RENDERING_OGRE2_VISIBLE Ogre2FrustumVisual + : public BaseFrustumVisual + { + /// \brief Constructor + protected: Ogre2FrustumVisual(); + + /// \brief Destructor + public: virtual ~Ogre2FrustumVisual(); + + // Documentation inherited. + public: virtual void Init() override; + + // Documentation inherited. + public: virtual void PreRender() override; + + // Documentation inherited. + public: virtual void Destroy() override; + + // Documentation inherited + public: virtual void Update() override; + + /// \brief Create the Frustum Visual in ogre + private: void Create(); + + /// \brief Clear data stored by dynamiclines + private: void ClearVisualData(); + + // Documentation inherited + public: virtual void SetVisible(bool _visible) override; + + /// \brief Frustum Visual should only be created by scene. + private: friend class Ogre2Scene; + + /// \brief Private data class + private: std::unique_ptr dataPtr; + }; + } + } +} +#endif diff --git a/ogre2/include/gz/rendering/ogre2/Ogre2RenderTypes.hh b/ogre2/include/gz/rendering/ogre2/Ogre2RenderTypes.hh index b2c0fd6dd..d8c5ef4eb 100644 --- a/ogre2/include/gz/rendering/ogre2/Ogre2RenderTypes.hh +++ b/ogre2/include/gz/rendering/ogre2/Ogre2RenderTypes.hh @@ -36,6 +36,7 @@ namespace gz class Ogre2COMVisual; class Ogre2DepthCamera; class Ogre2DirectionalLight; + class Ogre2FrustumVisual; class Ogre2Geometry; class Ogre2GizmoVisual; class Ogre2GlobalIlluminationCiVct; @@ -92,6 +93,7 @@ namespace gz typedef shared_ptr Ogre2COMVisualPtr; typedef shared_ptr Ogre2DepthCameraPtr; typedef shared_ptr Ogre2DirectionalLightPtr; + typedef shared_ptr Ogre2FrustumVisualPtr; typedef shared_ptr Ogre2GeometryPtr; typedef shared_ptr Ogre2GizmoVisualPtr; typedef shared_ptr Ogre2GpuRaysPtr; diff --git a/ogre2/include/gz/rendering/ogre2/Ogre2Scene.hh b/ogre2/include/gz/rendering/ogre2/Ogre2Scene.hh index 323c2b30b..5be78d9c1 100644 --- a/ogre2/include/gz/rendering/ogre2/Ogre2Scene.hh +++ b/ogre2/include/gz/rendering/ogre2/Ogre2Scene.hh @@ -343,6 +343,10 @@ namespace gz protected: virtual LidarVisualPtr CreateLidarVisualImpl(unsigned int _id, const std::string &_name) override; + // Documentation inherited + protected: virtual FrustumVisualPtr CreateFrustumVisualImpl( + unsigned int _id, const std::string &_name) override; + // Documentation inherited protected: virtual WireBoxPtr CreateWireBoxImpl(unsigned int _id, const std::string &_name) override; diff --git a/ogre2/src/Ogre2FrustumVisual.cc b/ogre2/src/Ogre2FrustumVisual.cc new file mode 100644 index 000000000..361f8d29f --- /dev/null +++ b/ogre2/src/Ogre2FrustumVisual.cc @@ -0,0 +1,315 @@ +/* + * 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. + * + */ + +#ifdef __APPLE__ + #define GL_SILENCE_DEPRECATION + #include + #include +#else +#ifndef _WIN32 + #include +#endif +#endif + +#include +#include +#include +#include +#include + +#include + +#include "gz/rendering/ogre2/Ogre2Conversions.hh" +#include "gz/rendering/ogre2/Ogre2DynamicRenderable.hh" +#include "gz/rendering/ogre2/Ogre2FrustumVisual.hh" +#include "gz/rendering/ogre2/Ogre2RenderEngine.hh" +#include "gz/rendering/ogre2/Ogre2Scene.hh" +#include "gz/rendering/ogre2/Ogre2Marker.hh" +#include "gz/rendering/ogre2/Ogre2Geometry.hh" + +#ifdef _MSC_VER + #pragma warning(push, 0) +#endif +#include +#include +#include +#include +#include +#ifdef _MSC_VER + #pragma warning(pop) +#endif + +class gz::rendering::Ogre2FrustumVisualPrivate +{ + /// \brief Frustum Ray DynamicLines Object to display + public: std::vector> rayLines; + + /// \brief Frustum visual type + // public: FrustumVisualPlane frustumVisPlane = + // FrustumVisualPlane::FRUSTUM_PLANE_TOP; + + /// \brief The visibility of the visual + public: bool visible = true; + + /// \brief Each corner of the frustum. + public: std::array points; + + /// \brief each edge of the frustum. + public: std::array, 12> edges; +}; + +using namespace gz; +using namespace rendering; + +////////////////////////////////////////////////// +Ogre2FrustumVisual::Ogre2FrustumVisual() + : dataPtr(new Ogre2FrustumVisualPrivate) +{ +} + +////////////////////////////////////////////////// +Ogre2FrustumVisual::~Ogre2FrustumVisual() +{ + // no ops +} + +////////////////////////////////////////////////// +void Ogre2FrustumVisual::PreRender() +{ + // no ops +} + +////////////////////////////////////////////////// +void Ogre2FrustumVisual::Destroy() +{ + BaseFrustumVisual::Destroy(); +} + +////////////////////////////////////////////////// +void Ogre2FrustumVisual::Init() +{ + BaseFrustumVisual::Init(); + this->Create(); +} + +////////////////////////////////////////////////// +void Ogre2FrustumVisual::Create() +{ + // enable GL_PROGRAM_POINT_SIZE so we can set gl_PointSize in vertex shader + auto engine = Ogre2RenderEngine::Instance(); + std::string renderSystemName = + engine->OgreRoot()->getRenderSystem()->getFriendlyName(); + if (renderSystemName.find("OpenGL") != std::string::npos) + { +#ifdef __APPLE__ + glEnable(GL_VERTEX_PROGRAM_POINT_SIZE); +#else +#ifndef _WIN32 + glEnable(GL_PROGRAM_POINT_SIZE); +#endif +#endif + } +} + +////////////////////////////////////////////////// +void Ogre2FrustumVisual::ClearVisualData() +{ + this->dataPtr->rayLines.clear(); +} + +////////////////////////////////////////////////// +void Ogre2FrustumVisual::Update() +{ + std::shared_ptr renderable = + std::shared_ptr( + new Ogre2DynamicRenderable(this->Scene())); + this->ogreNode->attachObject(renderable->OgreObject()); + + #if (!(OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7))) + // the Materials are assigned here to avoid repetitive search for materials + Ogre::MaterialPtr rayLineMat = + Ogre::MaterialManager::getSingleton().getByName( + "Frustum/BlueRay"); + #endif + + #if (OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7)) + MaterialPtr mat = this->Scene()->Material("Frustum/BlueRay"); + #else + MaterialPtr mat = this->Scene()->Material("Frustum/BlueRay"); + #endif + + renderable->SetMaterial(mat, false); + renderable->SetOperationType(MT_LINE_LIST); + this->dataPtr->rayLines.push_back(renderable); + + // Tangent of half the field of view. + double tanFOV2 = std::tan(this->hfov() * 0.5); + + // Width of near plane + double nearWidth = 2.0 * tanFOV2 * this->nearClip; + + // Height of near plane + double nearHeight = nearWidth / this->aspectRatio; + + // Width of far plane + double farWidth = 2.0 * tanFOV2 * this->farClip; + + // Height of far plane + double farHeight = farWidth / this->aspectRatio; + + // Up, right, and forward unit vectors. + gz::math::Vector3d forward = + this->pose.Rot().RotateVector(gz::math::Vector3d::UnitX); + gz::math::Vector3d up = + this->pose.Rot().RotateVector(gz::math::Vector3d::UnitZ); + gz::math::Vector3d right = + this->pose.Rot().RotateVector(-gz::math::Vector3d::UnitY); + + // Near plane center + gz::math::Vector3d nearCenter = this->pose.Pos() + forward * this->nearClip; + + // Far plane center + gz::math::Vector3d farCenter = this->pose.Pos() + forward * this->farClip; + + // These four variables are here for convenience. + gz::math::Vector3d upNearHeight2 = up * (nearHeight * 0.5); + gz::math::Vector3d rightNearWidth2 = right * (nearWidth * 0.5); + gz::math::Vector3d upFarHeight2 = up * (farHeight * 0.5); + gz::math::Vector3d rightFarWidth2 = right * (farWidth * 0.5); + + // Compute the vertices of the near plane + gz::math::Vector3d nearTopLeft = + nearCenter + upNearHeight2 - rightNearWidth2; + gz::math::Vector3d nearTopRight = + nearCenter + upNearHeight2 + rightNearWidth2; + gz::math::Vector3d nearBottomLeft = + nearCenter - upNearHeight2 - rightNearWidth2; + gz::math::Vector3d nearBottomRight = + nearCenter - upNearHeight2 + rightNearWidth2; + + // Compute the vertices of the far plane + gz::math::Vector3d farTopLeft = farCenter + upFarHeight2 - rightFarWidth2; + gz::math::Vector3d farTopRight = farCenter + upFarHeight2 + rightFarWidth2; + gz::math::Vector3d farBottomLeft = farCenter - upFarHeight2 - rightFarWidth2; + gz::math::Vector3d farBottomRight = farCenter - upFarHeight2 + rightFarWidth2; + + // Save these vertices + this->dataPtr->points[0] = nearTopLeft; + this->dataPtr->points[1] = nearTopRight; + this->dataPtr->points[2] = nearBottomLeft; + this->dataPtr->points[3] = nearBottomRight; + this->dataPtr->points[4] = farTopLeft; + this->dataPtr->points[5] = farTopRight; + this->dataPtr->points[6] = farBottomLeft; + this->dataPtr->points[7] = farBottomRight; + + // Save the edges + this->dataPtr->edges[0] = {nearTopLeft, nearTopRight}; + this->dataPtr->edges[1] = {nearTopLeft, nearBottomLeft}; + this->dataPtr->edges[2] = {nearTopLeft, farTopLeft}; + this->dataPtr->edges[3] = {nearTopRight, nearBottomRight}; + this->dataPtr->edges[4] = {nearTopRight, farTopRight}; + this->dataPtr->edges[5] = {nearBottomLeft, nearBottomRight}; + this->dataPtr->edges[6] = {nearBottomLeft, farBottomLeft}; + this->dataPtr->edges[7] = {farTopLeft, farTopRight}; + this->dataPtr->edges[8] = {farTopLeft, farBottomLeft}; + this->dataPtr->edges[9] = {farTopRight, farBottomRight}; + this->dataPtr->edges[10] = {farBottomLeft, farBottomRight}; + this->dataPtr->edges[11] = {farBottomRight, nearBottomRight}; + + gz::math::Vector3d leftCenter = + (farTopLeft + nearTopLeft + farBottomLeft + nearBottomLeft) / 4.0; + + gz::math::Vector3d rightCenter = + (farTopRight + nearTopRight + farBottomRight + nearBottomRight) / 4.0; + + gz::math::Vector3d topCenter = + (farTopRight + nearTopRight + farTopLeft + nearTopLeft) / 4.0; + + gz::math::Vector3d bottomCenter = + (farBottomRight + nearBottomRight + farBottomLeft + nearBottomLeft) / 4.0; + + // For creating the frustum visuals + renderable->AddPoint(math::Vector3d(this->nearClip, nearWidth, nearHeight)); + renderable->AddPoint(math::Vector3d(this->nearClip, nearWidth, -nearHeight)); + renderable->AddPoint(math::Vector3d(this->nearClip, nearWidth, -nearHeight)); + renderable->AddPoint(math::Vector3d(this->nearClip, -nearWidth, -nearHeight)); + renderable->AddPoint(math::Vector3d(this->nearClip, -nearWidth, -nearHeight)); + renderable->AddPoint(math::Vector3d(this->nearClip, -nearWidth, nearHeight)); + renderable->AddPoint(math::Vector3d(this->nearClip, -nearWidth, nearHeight)); + renderable->AddPoint(math::Vector3d(this->nearClip, nearWidth, nearHeight)); + renderable->AddPoint(math::Vector3d(this->farClip, farWidth, farHeight)); + renderable->AddPoint(math::Vector3d(this->farClip, farWidth, -farHeight)); + renderable->AddPoint(math::Vector3d(this->farClip, farWidth, -farHeight)); + renderable->AddPoint(math::Vector3d(this->farClip, -farWidth, -farHeight)); + renderable->AddPoint(math::Vector3d(this->farClip, -farWidth, -farHeight)); + renderable->AddPoint(math::Vector3d(this->farClip, -farWidth, farHeight)); + renderable->AddPoint(math::Vector3d(this->farClip, -farWidth, farHeight)); + renderable->AddPoint(math::Vector3d(this->farClip, farWidth, farHeight)); + renderable->AddPoint(math::Vector3d(this->nearClip, nearWidth, nearHeight)); + renderable->AddPoint(math::Vector3d(this->farClip, farWidth, farHeight)); + renderable->AddPoint(math::Vector3d(this->nearClip, -nearWidth, nearHeight)); + renderable->AddPoint(math::Vector3d(this->farClip, -farWidth, farHeight)); + renderable->AddPoint(math::Vector3d(this->nearClip, -nearWidth, -nearHeight)); + renderable->AddPoint(math::Vector3d(this->farClip, -farWidth, -farHeight)); + renderable->AddPoint(math::Vector3d(this->nearClip, nearWidth, -nearHeight)); + renderable->AddPoint(math::Vector3d(this->farClip, farWidth, -farHeight)); + + // Compute plane offsets + // Set the planes, where the first value is the plane normal and the + // second the plane offset + gz::math::Vector3d norm = gz::math::Vector3d::Normal( + nearTopLeft, nearTopRight, nearBottomLeft); + this->planes[FrustumVisualPlane::FRUSTUM_PLANE_NEAR].Set( + norm, nearCenter.Dot(norm)); + + norm = gz::math::Vector3d::Normal( + farTopRight, farTopLeft, farBottomLeft); + this->planes[FrustumVisualPlane::FRUSTUM_PLANE_FAR].Set( + norm, farCenter.Dot(norm)); + + norm = gz::math::Vector3d::Normal( + farTopLeft, nearTopLeft, nearBottomLeft); + this->planes[FrustumVisualPlane::FRUSTUM_PLANE_LEFT].Set( + norm, leftCenter.Dot(norm)); + + norm = gz::math::Vector3d::Normal( + nearTopRight, farTopRight, farBottomRight); + this->planes[FrustumVisualPlane::FRUSTUM_PLANE_RIGHT].Set( + norm, rightCenter.Dot(norm)); + + norm = gz::math::Vector3d::Normal( + nearTopLeft, farTopLeft, nearTopRight); + this->planes[FrustumVisualPlane::FRUSTUM_PLANE_TOP].Set( + norm, topCenter.Dot(norm)); + + norm = gz::math::Vector3d::Normal( + nearBottomLeft, nearBottomRight, farBottomRight); + this->planes[FrustumVisualPlane::FRUSTUM_PLANE_BOTTOM].Set( + norm, bottomCenter.Dot(norm)); + + renderable->Update(); + this->SetVisible(this->dataPtr->visible); +} + +////////////////////////////////////////////////// +void Ogre2FrustumVisual::SetVisible(bool _visible) +{ + this->dataPtr->visible = _visible; + this->ogreNode->setVisible(this->dataPtr->visible); +} diff --git a/ogre2/src/Ogre2Scene.cc b/ogre2/src/Ogre2Scene.cc index 2d164e235..22d172593 100644 --- a/ogre2/src/Ogre2Scene.cc +++ b/ogre2/src/Ogre2Scene.cc @@ -53,6 +53,7 @@ #include "gz/rendering/ogre2/Ogre2Light.hh" #include "gz/rendering/ogre2/Ogre2LightVisual.hh" #include "gz/rendering/ogre2/Ogre2LidarVisual.hh" +#include "gz/rendering/ogre2/Ogre2FrustumVisual.hh" #include "gz/rendering/ogre2/Ogre2Marker.hh" #include "gz/rendering/ogre2/Ogre2Material.hh" #include "gz/rendering/ogre2/Ogre2MeshFactory.hh" @@ -1344,6 +1345,15 @@ LidarVisualPtr Ogre2Scene::CreateLidarVisualImpl(unsigned int _id, return (result) ? lidar: nullptr; } +////////////////////////////////////////////////// +FrustumVisualPtr Ogre2Scene::CreateFrustumVisualImpl(unsigned int _id, + const std::string &_name) +{ + Ogre2FrustumVisualPtr frustum(new Ogre2FrustumVisual); + bool result = this->InitObject(frustum, _id, _name); + return (result) ? frustum: nullptr; +} + ////////////////////////////////////////////////// TextPtr Ogre2Scene::CreateTextImpl(unsigned int /*_id*/, const std::string &/*_name*/) diff --git a/src/FrustumVisual.cc b/src/FrustumVisual.cc new file mode 100644 index 000000000..9c057dec2 --- /dev/null +++ b/src/FrustumVisual.cc @@ -0,0 +1,28 @@ +/* + * 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. + * + */ + + +#include "gz/rendering/FrustumVisual.hh" + +using namespace gz; +using namespace rendering; + +////////////////////////////////////////////////// +FrustumVisual::FrustumVisual() = default; + +////////////////////////////////////////////////// +FrustumVisual::~FrustumVisual() = default; diff --git a/src/base/BaseScene.cc b/src/base/BaseScene.cc index b65112fb3..2a03743c7 100644 --- a/src/base/BaseScene.cc +++ b/src/base/BaseScene.cc @@ -30,6 +30,7 @@ #include "gz/rendering/InstallationDirectories.hh" #include "gz/rendering/JointVisual.hh" #include "gz/rendering/LidarVisual.hh" +#include "gz/rendering/FrustumVisual.hh" #include "gz/rendering/LightVisual.hh" #include "gz/rendering/Camera.hh" #include "gz/rendering/Capsule.hh" @@ -1262,6 +1263,36 @@ LidarVisualPtr BaseScene::CreateLidarVisual(unsigned int _id, return (result) ? lidar : nullptr; } +////////////////////////////////////////////////// +FrustumVisualPtr BaseScene::CreateFrustumVisual() +{ + unsigned int objId = this->CreateObjectId(); + return this->CreateFrustumVisual(objId); +} + +////////////////////////////////////////////////// +FrustumVisualPtr BaseScene::CreateFrustumVisual(unsigned int _id) +{ + const std::string objName = this->CreateObjectName(_id, "FrustumVisual"); + return this->CreateFrustumVisual(_id, objName); +} + +////////////////////////////////////////////////// +FrustumVisualPtr BaseScene::CreateFrustumVisual(const std::string &_name) +{ + unsigned int objId = this->CreateObjectId(); + return this->CreateFrustumVisual(objId, _name); +} + +////////////////////////////////////////////////// +FrustumVisualPtr BaseScene::CreateFrustumVisual(unsigned int _id, + const std::string &_name) +{ + FrustumVisualPtr frustum = this->CreateFrustumVisualImpl(_id, _name); + bool result = this->RegisterVisual(frustum); + return (result) ? frustum : nullptr; +} + ////////////////////////////////////////////////// WireBoxPtr BaseScene::CreateWireBox() {