agxSensor/Environment.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 <agxSensor/SystemNode.h>
#include <agxSensor/EnvironmentInternalData.h>
#include <agxSensor/MagneticField.h>
#include <agxSensor/RaytraceHandles.h>
 
#include <agxSDK/ISensorEnvironment.h>
 
#include <agx/ReferencedHandler.h>
#include <agx/AtomicState.h>
#include <agx/Timer.h>
 
#include <thread>
 
namespace agxCollide
{
  class Shape;
  class Geometry;
}
 
namespace agx
{
  class RigidBody;
}
 
namespace agxSDK
{
  class Simulation;
  class LinkedStructure;
}
 
namespace agxWire
{
  class Wire;
}
 
namespace agxModel
{
  class DeformableObject;
}
 
namespace agxTerrain
{
  class Terrain;
  class TerrainPager;
}
 
namespace agxSensor
{
  AGX_DECLARE_POINTER_TYPES( Environment );
 
  class AGXSENSOR_EXPORT Environment : public agxSDK::ISensorEnvironment
  {
    public:
      static Environment* getOrCreate( agxSDK::Simulation* simulation );
 
      static Environment* get( agxSDK::Simulation* simulation );
 
      static const Environment* get( const agxSDK::Simulation* simulation );
 
    public:
      Environment();
 
      bool add( agxCollide::Shape* shape );
 
      bool remove( agxCollide::Shape* shape );
 
      bool add( agxCollide::Geometry* geometry );
 
      bool remove( agxCollide::Geometry* geometry );
 
      bool add( agx::RigidBody* rb );
 
      bool remove( agx::RigidBody* rb );
 
      bool add( agxTerrain::Terrain* terrain );
 
      bool remove( agxTerrain::Terrain* terrain);
 
      bool add( agxTerrain::TerrainPager* pager );
 
      bool remove( agxTerrain::TerrainPager* pager );
 
      bool add( agxSDK::LinkedStructure* linkedStructure );
 
      bool remove( agxSDK::LinkedStructure* linkedStructure );
 
      bool add( agxWire::Wire* wire );
 
      bool remove( agxWire::Wire* wire );
 
      bool add( agxModel::DeformableObject* deformable );
 
      bool remove( agxModel::DeformableObject* deformable );
 
      bool add( SystemNode* system );
 
      bool remove( SystemNode* system );
 
      RtSceneRef getScene() const;
 
      void setMagneticField( MagneticField* magneticField );
 
      MagneticField* getMagneticField();
 
      const MagneticField* getMagneticField() const;
 
    public:
      virtual ~Environment();
 
      DOXYGEN_START_INTERNAL_BLOCK()
 
      
      virtual void addNotification() override;
 
      virtual void removeNotification() override;
 
      virtual void step( agx::Real dt ) override;
 
      virtual void fetchStepResults() override;
 
      virtual void updateGraphics( const agxRender::RenderManager& renderManager ) override;
 
      template<typename T>
      agx::VectorPOD<T*> getObjects() const;
 
      template<typename T>
      T* findRootSystem() const;
 
      template<typename T>
      agx::VectorPOD<T*> findRootSystems() const;
 
      template<typename T>
      agx::VectorPOD<T*> findSystems() const;
 
      AGXSTREAM_DECLARE_SERIALIZABLE( agxSensor::Environment );
 
    private:
      struct RuntimeState
      {
        enum Flag : agx::UInt32
        {
          INITIALIZED = 1 << 0,
          DO_STEP = 1 << 1,
          SYNCHRONIZING_DATA = 1 << 2,
          SLEEP = 1 << 3,
          IS_SLEEPING = 1 << 4,
          JOIN_MAIN = 1 << 30,
        };
 
        using Value = agx::AtomicState<Flag>;
 
        Value state{ (agx::UInt32)0 };
        agx::Real dt = 1.0 / 60.0;
      };
 
    private:
      static size_t& instanceCount();
 
    private:
      void initialize();
      void cleanup();
 
      bool fireOnAdd( agx::Referenced& instance );
      bool fireOnRemove( agx::Referenced& instance );
 
      template<typename T>
      bool accept( bool handled, agx::Referenced& instance, bool isAdded );
 
      template<typename T, typename InOutHelper>
      void storeType( agxStream::OutputArchive& out,
                      InOutHelper& helper ) const;
 
      template<typename T, typename InOutHelper>
      void restoreType( agxStream::InputArchive& in,
                        InOutHelper& helper,
                        bool callAddOnRestored );
 
      bool isDone() const;
      void realize();
 
      DOXYGEN_END_INTERNAL_BLOCK()
 
    private:
      SystemNodeRefVector m_systems;
      std::thread m_thread;
      RuntimeState m_runtimeData;
      agx::ReferencedHandler m_refHandler;
      RtSceneRef m_scene;
      MagneticFieldRef m_magneticField;
  };
 
  template<typename T>
  agx::VectorPOD<T*> Environment::getObjects() const
  {
    agx::VectorPOD<T*> objects;
    m_refHandler.template visit<T>( [&objects]( const T& instance )
    {
      objects.push_back( const_cast<T*>( &instance ) );
    } );
    return objects;
  }
 
  template<typename T>
  T* Environment::findRootSystem() const
  {
    for ( const auto& system : m_systems )
      if ( system->template is<T>() )
        return system->template as<T>();
    return nullptr;
  }
 
  template<typename T>
  agx::VectorPOD<T*> Environment::findRootSystems() const
  {
    agx::VectorPOD<T*> result;
    for ( const auto& system : m_systems )
      if ( system->template is<T>() )
        result.push_back( system->template as<T>() );
    return result;
  }
 
  template<typename T>
  agx::VectorPOD<T*> Environment::findSystems() const
  {
    agx::VectorPOD<T*> result;
    for ( const auto& system : m_systems ) {
      if ( system->template is<T>() )
        result.push_back( system->template as<T>() );
 
      system->template visitChildrenOfType<T>( [&result]( T& child )
      {
        result.push_back( &child );
      } );
    }
 
    return result;
  }
 
  template<typename T>
  bool Environment::accept( bool handled, agx::Referenced& instance, bool isAdded )
  {
    static_assert( std::is_base_of<agxStream::Serializable, T>::value,
                   "Unexpected that type doesn't suport serialization." );
 
    agxAssert( !handled || instance.template is<T>() );
    if ( handled && isAdded ) {
      agxSensor::getOrCreateEnvData( instance.template asSafe<T>() )->environment = this;
      m_refHandler.template add<T>( instance );
    }
    else if ( handled ) {
      agxSensor::getOrCreateEnvData( instance.template asSafe<T>() )->environment = nullptr;
      m_refHandler.template remove<T>( instance );
    }
 
    return handled;
  }
 
  template<typename T, typename InOutHelper>
  void Environment::storeType( agxStream::OutputArchive& out, InOutHelper& helper ) const
  {
    out << agxStream::out( "numInstances", m_refHandler.template size<T>() );
    m_refHandler.template visit<T>( [&out, &helper]( const T& instance )
    {
      out << agxStream::out( "instance", &instance );
 
      // By design that instances added to the ref-handler has
      // environment data.
      const EnvInternalData* envData = getEnvData( &instance );
      out << agxStream::out( "hasRtData", envData->rtData != nullptr );
      if ( envData->rtData != nullptr ) {
        envData->rtData->state.flags.store( out );
        out << agxStream::out( "rtEntityIndex", indexOf( envData->rtData->getEntityId() ) );
 
        helper.store( out, envData->rtData->getMaterial() );
      }
    } );
  }
 
  template<typename T, typename InOutHelper>
  void Environment::restoreType( agxStream::InputArchive& in,
                                 InOutHelper& helper,
                                 bool callAddOnRestored )
  {
    size_t numInstances = 0u;
    in >> agxStream::in( "numInstances", numInstances );
    for ( size_t i = 0u; i < numInstances; ++i ) {
      T* instance = nullptr;
      in >> agxStream::in( "instance", instance );
      if ( instance != nullptr ) {
        bool hasRtData = false;
        in >> agxStream::in( "hasRtData", hasRtData );
        if ( hasRtData ) {
          RtInternalData* rtData = getOrCreateRtData( instance );
          rtData->state.flags.restore( in );
          RtEntityIdIndex rtEntityIndex = 0;
          in >> agxStream::in( "rtEntityIndex", rtEntityIndex );
          rtData->setEntityId( (RtEntityId)rtEntityIndex );
 
          RtMaterialInstance material{};
          helper.restore( in, material );
          rtData->setMaterial( material );
        }
 
        if ( callAddOnRestored )
          this->add( instance );
        this->template accept<T>( true, *instance, true );
        instance->setFinished();
      }
    }
  }
}