LidarOutputRenderer.h

Go to the documentation of this file.

/*
Copyright 2007-2025. Algoryx Simulation AB.
 
All AGX source code, intellectual property, documentation, sample code,
tutorials, scene files and technical white papers, are copyrighted, proprietary
and confidential material of Algoryx Simulation AB. You may not download, read,
store, distribute, publish, copy or otherwise disseminate, use or expose this
material unless having a written signed agreement with Algoryx Simulation AB, or having been
advised so by Algoryx Simulation AB for a time limited evaluation, or having purchased a
valid commercial license from Algoryx Simulation AB.
 
Algoryx Simulation AB disclaims all responsibilities for loss or damage caused
from using this software, unless otherwise stated in written agreements with
Algoryx Simulation AB.
*/
 
#pragma once
 
#include <agx/config/AGX_USE_AGXSENSOR.h>
 
#if AGX_USE_AGXSENSOR()
  #include <agxOSG/export.h>
 
  #include <agx/Quat.h>
  #include <agx/Vec4.h>
 
  #include <osg/Geode>
  #include <osg/Geometry>
  #include <osg/MatrixTransform>
  #include <osg/PrimitiveSet>
 
  #include <functional>
 
#endif  // AGX_USE_AGXSENSOR()
 
#if AGX_USE_AGXSENSOR()
 
namespace agxSensor
{
  class Lidar;
}
 
namespace agxOSG
{
  struct AGXOSG_EXPORT LidarOutputRendererLegendPoint
  {
      float intensity;
      osg::Vec4 color;
 
      LidarOutputRendererLegendPoint();
      LidarOutputRendererLegendPoint(float intensity, osg::Vec4 color);
  };
 
  class AGXOSG_EXPORT LidarOutputRenderer : public osg::MatrixTransform
  {
    public:
      using GeodeRef = osg::ref_ptr<osg::Geode>;
      using GeometryRef = osg::ref_ptr<osg::Geometry>;
      using FloatArrayRef = osg::ref_ptr<osg::FloatArray>;
      using Vec3ArrayRef = osg::ref_ptr<osg::Vec3Array>;
      using Vec4ArrayRef = osg::ref_ptr<osg::Vec4Array>;
      using ColorArrayRef = osg::ref_ptr<osg::Vec4Array>;
      using ProgramRef = osg::ref_ptr<osg::Program>;
      using UniformRef = osg::ref_ptr<osg::Uniform>;
 
      struct IntensityState
      {
          using ColorFunc = std::function<osg::Vec4(float intensity)>;
 
          enum Mode
          {
            COLOR_RG, 
            COLOR_A, 
          };
 
          float scale = 120.0f;
          ColorFunc colorFunc = {};
          struct
          {
              std::vector<LidarOutputRendererLegendPoint> colorLegend;
              float exponent;
          } interpolationFunctionValues;
      };
 
      class LidarObserverHolder;
 
    public:
      LidarOutputRenderer(const agxSensor::Lidar* lidar, bool renderRays = false);
 
      void setHitPointScale(float scale);
 
      float getHitPointScale() const;
 
      void setIntensityScale(float intensityScale);
 
      float getIntensityScale() const;
 
      void setIntensityColorFunctionFromMode(IntensityState::Mode mode);
 
      void setIntensityColorFunctionFromInterpolation(osg::Vec4 low, osg::Vec4 high, float exponent);
 
      void setIntensityColorFunctionFromLegend(
        const std::vector<LidarOutputRendererLegendPoint>& legend, float exponent);
 
      void setIntensityColorFunction(IntensityState::ColorFunc colorFunc);
 
  #ifndef SWIG
      IntensityState::ColorFunc getIntensityColorFunction() const;
 
  #endif
 
      void setHitPointLifetime(float hitPointLifetime);
 
      float getHitPointLifetime() const;
 
      void setHitPointsAlwaysOnTop(bool alwaysOnTop);
 
      bool isAlwaysRenderedOnTop() const;
 
      DOXYGEN_START_INTERNAL_BLOCK()
 
    public:
      LidarOutputRenderer(const LidarOutputRenderer& object);
      virtual ~LidarOutputRenderer() noexcept;
 
      LidarOutputRenderer& operator=(const LidarOutputRenderer& object);
 
      DOXYGEN_END_INTERNAL_BLOCK()
 
    private:
      struct HitOutput : agx::Vec4f
      {
          using agx::Vec4f::Vec4f;
 
          agx::Vec3f position() const { return {x(), y(), z()}; }
 
          osg::Vec3 osgPosition() const { return {x(), y(), z()}; }
 
          osg::Vec4 osgZToNormal(agx::Quat32 toWorld) const
          {
            agx::Quat32 q = agx::Quat32 {agx::Vec3f::Z_AXIS(), normal} * toWorld.conj();
            return {q.x(), q.y(), q.z(), q.w()};
          }
 
          float intensity() const { return w(); }
 
          agx::Vec3f normal;
      };
 
      struct FrameCallback : osg::NodeCallback
      {
          virtual void operator()(osg::Node* node, osg::NodeVisitor* /*visitor*/) override
          {
            dynamic_cast<LidarOutputRenderer*>(node)->onPreRender();
          }
      };
 
      struct CloudRenderData
      {
          CloudRenderData(size_t maxNumPoints);
          GeometryRef createGeometry(osg::PrimitiveSet::Mode renderMode);
          osg::StateSet* getCommonStateSet();
 
          void age(float dt);
          void push(
            size_t newCount, const HitOutput* newPoints, float lifetime, const agx::Quat32& rotationToWorldF,
            const IntensityState& intensityState);
          void updatePrimitiveCount(size_t primitiveCount);
          void invalidate();
 
          GeodeRef geode;
          size_t count;
          FloatArrayRef lifetimes;
          Vec3ArrayRef vertices;
          ColorArrayRef colors;
          Vec4ArrayRef zToNormals;
      };
 
      struct GeometryShaderData
      {
          GeometryRef rayGeometry;
          ProgramRef rayProgram;
          UniformRef rayColor;
 
          GeometryRef pointsGeometry;
          ProgramRef pointsProgram;
          UniformRef hitPointScale;
      };
 
    private:
      static size_t determinePointBufferCount(const agxSensor::Lidar* lidar, float hitPointLifetime);
 
    private:
      friend FrameCallback;
 
      void synchronizeTransform();
      ProgramRef createShader(const agx::String& filename);
 
      void initializeOutputCloudGeometryShader(bool renderRays);
      void onPreRender();
 
    private:
      LidarObserverHolder* m_lidarObserver;  // This is fully managed by the LidarOutputRenderer
      CloudRenderData m_cloudRenderData;
      GeometryShaderData m_geometryShaderData;
      IntensityState m_intensityState;
      agx::Real m_lastRenderTime;
      float m_hitPointLifetime;
  };
 
  using LidarOutputRendererRef = osg::ref_ptr<LidarOutputRenderer>;
}
 
#endif  // AGX_USE_AGXSENSOR()