Ogre2FrustumVisual: draw wireframe at the actual frustum extents (backport #1276)

ogre2/src/Ogre2FrustumVisual.cc

@@ -0,0 +1,324 @@
+/*
+ * 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 <OpenGL/gl.h>
+  #include <OpenGL/glext.h>
+#else
+#ifndef _WIN32
+  #include <GL/gl.h>
+#endif
+#endif
+
+#include <array>
+#include <cmath>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <gz/common/Console.hh>
+#include <gz/common/Profiler.hh>
+
+#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 <OgreItem.h>
+#include <OgreMaterialManager.h>
+#include <OgreRoot.h>
+#include <OgreSceneNode.h>
+#include <OgreTechnique.h>
+#ifdef _MSC_VER
+  #pragma warning(pop)
+#endif
+
+class gz::rendering::Ogre2FrustumVisualPrivate
+{
+  /// \brief Frustum Ray DynamicLines Object to display
+  public: std::vector<std::shared_ptr<Ogre2DynamicRenderable>> rayLines;
+
+  /// \brief The visibility of the visual
+  public: bool visible = true;
+
+  /// \brief Each corner of the frustum.
+  public: std::array<gz::math::Vector3d, 8> points;
+
+  /// \brief each edge of the frustum.
+  public: std::array<std::pair<gz::math::Vector3d,
+    gz::math::Vector3d>, 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()
+{
+  GZ_PROFILE("Ogre2FrustumVisual::Update");
+  std::shared_ptr<Ogre2DynamicRenderable> renderable;
+
+  // check if the renderable exists
+  if (this->dataPtr->rayLines.empty())
+  {
+    renderable = std::shared_ptr<Ogre2DynamicRenderable>(
+                              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);
+  }
+  else
+  {
+    // clear the existing renderable
+    renderable = this->dataPtr->rayLines.front();
+    renderable->Clear();
+  }
+
+  // Tangent of half the field of view.
+  double tanFOV2 = std::tan(this->hfov() * 0.5);
+
+  // Half-width of near plane (distance from optical axis to plane edge).
+  double nearWidth = tanFOV2 * this->nearClip;
+
+  // Half-height of near plane.
+  double nearHeight = nearWidth / this->aspectRatio;
+
+  // Half-width of far plane.
+  double farWidth = tanFOV2 * this->farClip;
+
+  // Half-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;
+
+  // Half-extent vectors used to offset each plane center to a corner.
+  gz::math::Vector3d upNearHeight2 = up * nearHeight;
+  gz::math::Vector3d rightNearWidth2 = right * nearWidth;
+  gz::math::Vector3d upFarHeight2 = up * farHeight;
+  gz::math::Vector3d rightFarWidth2 = right * farWidth;
+
+  // 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);
+}