LinkedStructure.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 <agxSDK/Assembly.h>
#include <agxSDK/LinkedSegmentRange.h>
#include <agxSDK/LinkedStructureComponent.h>
#include <agxSDK/LinkedStructureObjectData.h>
 
#include <agx/BitState.h>
 
namespace agx
{
  class InternalData;
}
 
namespace agxSDK
{
  AGX_DECLARE_POINTER_TYPES( LinkedStructure );
 
  class AGXPHYSICS_EXPORT LinkedStructure : public agxSDK::Assembly
  {
    public:
      static LinkedStructure* get(agxCollide::Geometry* geometry);
 
      static const LinkedStructure* get(const agxCollide::Geometry* geometry);
 
      static LinkedStructure* get(agx::RigidBody* body);
 
      static const LinkedStructure* get(const agx::RigidBody* body);
 
    public:
      virtual void setMaterial( agx::Material* material );
 
      virtual agx::Material* getMaterial() const;
 
      virtual agx::UInt32 getUniqueId() const;
 
      virtual agx::UInt32 getDisabledCollisionGroupId() const;
 
      virtual void setEnableCollisions( agx::RigidBody* rb, agx::Bool enable );
 
      virtual agx::Bool getEnableCollisions( const agx::RigidBody* rb ) const;
 
      virtual void setEnableCollisions( agxCollide::Geometry* geometry, agx::Bool enable );
 
      virtual agx::Bool getEnableCollisions( const agxCollide::Geometry* geometry ) const;
 
      virtual void addGroup( agx::UInt32 id );
 
      virtual void removeGroup( agx::UInt32 id );
 
      virtual agx::Bool hasGroup( agx::UInt32 id ) const;
 
      virtual void addGroup( const agx::Name& name );
 
      virtual void removeGroup( const agx::Name& name );
 
      virtual agx::Bool hasGroup( const agx::Name& name ) const;
 
      virtual agxCollide::GroupIdCollection findGroupIdCollection() const;
 
      virtual void setLinearVelocityDamping( const agx::Vec3& linearVelocityDamping );
 
      virtual agx::Vec3 getLinearVelocityDamping() const;
 
      virtual void setAngularVelocityDamping( const agx::Vec3& angularVelocityDamping );
 
      virtual agx::Vec3 getAngularVelocityDamping() const;
 
      virtual agx::Real getRestLength() const;
 
      virtual agx::Real getCurrentLength() const;
 
      virtual agx::Real getMass() const;
 
      virtual agx::Real getRestVolume() const;
 
      virtual agx::Vec3 findPoint( agx::Real restLengthFromStart ) const;
 
      virtual agx::Vec3 findPoint( const agx::Vec3& worldPoint ) const;
 
      virtual agx::Real findCurrentLength( agx::Real restLengthFromStart ) const;
 
      virtual agx::Real findRestLength( agx::Real currentLengthFromStart ) const;
 
      virtual agx::Real findRestLengthTo( const agx::Vec3& worldPoint ) const;
 
      virtual agx::Real findRestLengthTo( const agxSDK::LinkedSegment* segment ) const;
 
      virtual agx::Bool rebind();
 
      virtual agx::Bool addComponent( agxSDK::LinkedStructureComponent* component );
 
      virtual agx::Bool removeComponent( agxSDK::LinkedStructureComponent* component );
 
      template<typename T>
      T* getComponent() const;
 
      agxSDK::LinkedStructureComponent* getComponent( const agx::Name& name ) const;
 
 
      agxSDK::LinkedSegmentRange<agxSDK::LinkedSegment> segments();
 
      agx::UInt getSegmentIndex( const agx::RigidBody* rb ) const;
 
      const agxSDK::LinkedSegmentContainer& getSegmentsContainer() const;
 
    public:
      AGXSTREAM_DECLARE_SERIALIZABLE( agxSDK::LinkedStructure );
 
      void addNotification( agxSDK::Simulation* simulation ) final override;
 
      void removeNotification( agxSDK::Simulation* simulation ) final override;
 
      using Assembly::addNotification;
      using Assembly::removeNotification;
 
    public:
      struct IdData
      {
        IdData() : id( agx::InvalidIndex ), name( "" ) {}
 
        IdData( agx::UInt32 id ) : id( id ), name( "" ) {}
 
        IdData( const agx::Name& name ) : id( agx::InvalidIndex ), name( name ) {}
 
        agx::Bool operator== ( const IdData& other ) const { return id == other.id && name == other.name; }
 
        agx::Bool operator!= ( const IdData& other ) const { return !(*this == other); }
 
        agx::UInt32 id;
        agx::Name name;
      };
 
    protected:
      virtual void onAddNotification( agxSDK::Simulation* simulation );
 
      virtual void onRemoveNotification( agxSDK::Simulation* simulation );
 
      virtual void onMaterialNew();
 
      virtual void onGroupAdd( const IdData& group );
 
      virtual void onGroupRemove( const IdData& group );
 
      virtual void onSetEnableCollisions( agxCollide::Geometry* geometry, agx::Bool enable );
 
      virtual void onPreCollideStep();
 
      virtual void onPreStep();
 
      virtual void onPostStep();
 
      virtual void onLastStep();
 
      virtual void onPostIntegrate();
 
    protected:
      template<typename T>
      static T* getT( agx::RigidBody* rb );
 
      template<typename T>
      static const T* getT( const agx::RigidBody* rb );
 
    protected:
      LinkedStructure();
 
      virtual ~LinkedStructure();
 
      void add( LinkedSegment* segment );
 
      agx::UInt getNumSegments() const;
 
      template<typename T>
      LinkedSegmentRange<T> getSegments() const;
 
      template<typename T>
      LinkedSegmentRange<T> getSegments( agx::UInt beginIndex, agx::UInt endIndex ) const;
 
      LinkedSegment* getSegment( agx::UInt index ) const;
 
      template<typename T>
      T* getSegment( agx::Real restLengthFromStart ) const;
 
      template<typename T>
      T* findSegment( const agx::Vec3& worldPoint ) const;
 
      template<typename ObjT>
      LinkedSegment* getSegment( const ObjT* rb ) const;
 
      template<typename T>
      LinkedSegmentIterator<T> getSegmentIterator( agx::UInt index, agx::Bool boundaryless = false ) const;
 
      template<typename T, typename ObjT>
      LinkedSegmentIterator<T> getSegmentIterator( const ObjT* rb, agx::Bool boundaryless = false ) const;
 
      LinkedStructureObjectData* getData( const agx::RigidBody* rb ) const;
 
      std::tuple<agx::Vec3, LinkedSegment*> findPointAndSegment( const agx::Vec3& worldPoint ) const;
 
    private:
      // Events from the simulation.
      friend class Simulation;
 
      enum SimulationEvent
      {
        PRE_COLLIDE_STEP,
        PRE_STEP,
        POST_STEP,
        LAST_STEP,
        POST_INTEGRATE,
      };
 
      using IdDataContainer = agx::Vector<IdData>;
 
    private:
      void trigger( SimulationEvent simulationEvent );
      void addGroup( const IdData& group );
      void removeGroup( const IdData& group );
      agx::Bool hasGroup( const IdData& group ) const;
 
      DOXYGEN_START_INTERNAL_BLOCK()
      friend class agx::InternalData;
      agx::Referenced* getInternalData() const;
 
      void setInternalData( agx::Referenced* data );
      DOXYGEN_END_INTERNAL_BLOCK()
 
    private:
      agx::Uuid m_uniqueId;
      agx::Uuid m_disabledCollisionsId;
      LinkedSegmentContainer m_segments;
      agx::MaterialRef m_material;
      IdDataContainer m_externallyAddedGroups;
      LinkedStructureComponentContainer m_components;
      agx::Vec3 m_linearVelocityDamping;
      agx::Vec3 m_angularVelocityDamping;
      agx::ref_ptr<agx::Referenced> m_internalData;
  };
 
  template<typename T>
  T* LinkedStructure::getT( agx::RigidBody* rb )
  {
    auto data = LinkedStructureObjectData::get<LinkedStructureObjectData>( rb );
    return data != nullptr && data->linkedStructure != nullptr ? data->linkedStructure->template asSafe<T>() : nullptr;
  }
 
  template<typename T>
  const T* LinkedStructure::getT( const agx::RigidBody* rb )
  {
    return LinkedStructure::getT<T>( const_cast<agx::RigidBody*>( rb ) );
  }
 
  template<typename T>
  T* LinkedStructure::getComponent() const
  {
    for ( auto component : m_components ) {
      auto typed = component->template asSafe<T>();
      if ( typed != nullptr )
        return typed;
    }
 
    return nullptr;
  }
 
  inline const LinkedSegmentContainer& LinkedStructure::getSegmentsContainer() const
  {
    return m_segments;
  }
 
  inline agx::UInt LinkedStructure::getNumSegments() const
  {
    return m_segments.size();
  }
 
  template<typename T>
  LinkedSegmentRange<T> LinkedStructure::getSegments() const
  {
    return LinkedSegmentRange<T>( LinkedSegmentIterator<T>::begin( m_segments ),
                                  LinkedSegmentIterator<T>::end( m_segments ) );
  }
 
  template<typename T>
  LinkedSegmentRange<T> LinkedStructure::getSegments( agx::UInt beginIndex, agx::UInt endIndex ) const
  {
    return LinkedSegmentRange<T>( getSegmentIterator<T>( beginIndex ),
                                  getSegmentIterator<T>( endIndex ) );
  }
 
  inline LinkedSegment* LinkedStructure::getSegment( agx::UInt index ) const
  {
    agxAssert( index < m_segments.size() );
    return m_segments[ index ];
  }
 
  template<typename T>
  T* LinkedStructure::getSegment( agx::Real restLengthFromStart ) const
  {
    if ( m_segments.empty() )
      return nullptr;
 
    if ( restLengthFromStart <= 0.0 )
      return m_segments.front()->template as<T>();
 
    agx::Real restLengthAlongStructure = 0.0;
    for ( auto segment : this->template getSegments<T>() ) {
      if ( restLengthAlongStructure + segment->getLength() < restLengthFromStart )
        restLengthAlongStructure += segment->getLength();
      else
        return segment->template as<T>();
    }
 
    return m_segments.back()->template as<T>();
  }
 
  template<typename T>
  T* LinkedStructure::findSegment( const agx::Vec3 &worldPoint ) const
  {
    auto [point, segment] = findPointAndSegment( worldPoint );
    return segment->template as<T>();
  }
 
  template<>
  inline LinkedSegment* LinkedStructure::getSegment( const agx::RigidBody* rb ) const
  {
    auto index = getSegmentIndex( rb );
    return index != agx::InvalidIndex ?
             m_segments[ index ] :
             nullptr;
  }
 
  inline agx::UInt LinkedStructure::getSegmentIndex( const agx::RigidBody* rb ) const
  {
    auto data = getData( rb );
    return data != nullptr && data->segment != nullptr && data->linkedStructure == this ?
             data->segment->getIndex() :
             agx::InvalidIndex;
  }
 
  template<typename T>
  LinkedSegmentIterator<T> LinkedStructure::getSegmentIterator( agx::UInt index, agx::Bool boundaryless /* = false */ ) const
  {
    return LinkedSegmentIterator<T>( index, boundaryless, m_segments );
  }
 
  template<typename T, typename ObjT>
  LinkedSegmentIterator<T> LinkedStructure::getSegmentIterator( const ObjT* rb, agx::Bool boundaryless /* = false */ ) const
  {
    auto segment = this->getSegment( rb );
    if ( segment == nullptr )
      return LinkedSegmentIterator<T>::end( m_segments );
    return getSegmentIterator<T>( segment->getIndex(), boundaryless );
  }
 
  inline LinkedStructureObjectData* LinkedStructure::getData( const agx::RigidBody* rb ) const
  {
    auto data = LinkedStructureObjectData::get<LinkedStructureObjectData>( rb );
    // Accepting data when linkedStructure == null since it can be considered implicitly
    // related to this linked structure (data->linkedStructure = this to be made).
    return data != nullptr && ( data->linkedStructure == this || data->linkedStructure == nullptr ) ?
             data :
             nullptr;
  }
 
  inline agx::Referenced* LinkedStructure::getInternalData() const
  {
    return m_internalData;
  }
}