base-visualizer.hpp

//
// Copyright (c) 2024-2025 INRIA
//
 
#ifndef __pinocchio_extra_base_visualizer_hpp__
#define __pinocchio_extra_base_visualizer_hpp__
 
#include "pinocchio/macros.hpp"
#include "pinocchio/visualizers/config.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/multibody/geometry.hpp"
 
#include <boost/optional.hpp>
#include <utility>
 
namespace pinocchio
{
  namespace visualizers
  {
    typedef PINOCCHIO_SCALAR_TYPE_DEFAULT Scalar;
    PINOCCHIO_COMMON_TYPEDEF(PINOCCHIO_SCALAR_TYPE_DEFAULT, PINOCCHIO_OPTIONS_DEFAULT)
 
    typedef Eigen::Ref<const VectorXs> ConstVectorRef;
    typedef Eigen::Ref<const MatrixXs> ConstMatrixRef;
 
    class PINOCCHIO_VISUALIZERS_DLLAPI BaseVisualizer
    {
    public:
      typedef SE3::Matrix4 Matrix4;
 
      BaseVisualizer(
        const Model & model,
        const GeometryModel & visual_model,
        const GeometryModel * collision_model,
        Data & data,
        GeometryData & visual_data,
        GeometryData * collision_data);
 
      BaseVisualizer(
        const Model & model,
        const GeometryModel & visual_model,
        const GeometryModel * collision_model = nullptr);
 
      virtual ~BaseVisualizer();
 
      virtual void initViewer()
      {
      }
 
      virtual void loadViewerModel() = 0;
 
      virtual void rebuildData();
 
      virtual void display(const boost::optional<ConstVectorRef> & q = boost::none);
 
      template<typename D>
      void display(const Eigen::MatrixBase<D> & q)
      {
        boost::optional<ConstVectorRef> q_(q);
        display(q_);
      }
 
      virtual void play(const std::vector<ConstVectorRef> & qs, Scalar dt);
 
      void play(const ConstMatrixRef & qs, Scalar dt);
 
      virtual bool forceRedraw()
      {
        return true;
      }
 
      virtual void setCameraTarget(const Eigen::Ref<const Vector3> & /*target*/)
      {
      }
 
      virtual void setCameraPosition(const Eigen::Ref<const Vector3> & /*position*/)
      {
      }
 
      virtual void setCameraPose(const Eigen::Ref<const Matrix4> & /*pose*/)
      {
      }
 
      inline void setCameraPose(const SE3 & pose)
      {
        this->setCameraPose(pose.toHomogeneousMatrix());
      }
 
      virtual void setCameraZoom(Scalar /*value*/)
      {
      }
 
      virtual void enableCameraControl(bool)
      {
      }
 
      virtual void clean()
      {
      }
 
      const Model & model() const
      {
        return m_model;
      }
 
      const GeometryModel & visualModel() const
      {
        return *m_visualModel;
      }
 
      const GeometryModel & collisionModel() const
      {
        PINOCCHIO_THROW(
          hasCollisionModel(), std::logic_error, "No collision model in the visualizer.");
        return *m_collisionModel;
      }
 
      bool hasCollisionModel() const
      {
        return m_collisionModel != nullptr;
      }
 
      bool hasExternalData() const
      {
        return !m_ownedData;
      }
 
      Data & data()
      {
        return *m_data;
      }
      const Data & data() const
      {
        return *m_data;
      }
 
      GeometryData & visualData()
      {
        return *m_visualData;
      }
      const GeometryData & visualData() const
      {
        return *m_visualData;
      }
 
      GeometryData & collisionData()
      {
        PINOCCHIO_THROW(
          hasCollisionModel(), std::logic_error, "No collision model in the visualizer.");
        return *m_collisionData;
      }
 
      const GeometryData & collisionData() const
      {
        PINOCCHIO_THROW(
          hasCollisionModel(), std::logic_error, "No collision model in the visualizer.");
        return *m_collisionData;
      }
 
    protected:
      std::reference_wrapper<Model const> m_model;
      GeometryModel const * m_visualModel;
      GeometryModel const * m_collisionModel;
 
      Data * m_data;
      GeometryData * m_visualData;
      GeometryData * m_collisionData;
      bool m_ownedData;
 
      virtual void displayPrecall()
      {
      }
      virtual void displayImpl() = 0;
 
      void destroyData();
    };
  } // namespace visualizers
} // namespace pinocchio
 
#endif // ifndef __pinocchio_extra_base_visualizer_hxx__