MergeSplitHandler.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/MergeSplitAlgorithm.h>
#include <agxSDK/MergeSplitProperties.h>
 
#include <agxCollide/Space.h>
 
namespace agxWire
{
  class Wire;
}
 
namespace agxSDK
{
  class AGXPHYSICS_EXPORT MergeSplitHandler : public agx::Referenced
  {
    public:
      typedef agx::Vector<agxSDK::MergeSplitAlgorithmRef> MergeSplitAlgorithmContainer;
      typedef agx::Vector<agx::MergedBodyRef> MergedBodyRefContainer;
 
    public:
      static agxSDK::MergeSplitProperties* getOrCreateProperties( agx::RigidBody* rb );
 
      static const agxSDK::MergeSplitProperties* getProperties( const agx::RigidBody* rb );
 
      static agxSDK::MergeSplitProperties* getProperties( agx::RigidBody* rb );
 
      static agxSDK::MergeSplitProperties* getOrCreateProperties( agxCollide::Geometry* geometry );
 
      static const agxSDK::MergeSplitProperties* getProperties( const agxCollide::Geometry* geometry );
 
      static agxSDK::MergeSplitProperties* getProperties( agxCollide::Geometry* geometry );
 
      static agxSDK::MergeSplitProperties* getOrCreateProperties( agx::Constraint* constraint );
 
      static agxSDK::MergeSplitProperties* getProperties( agx::Constraint* constraint );
 
      static const agxSDK::MergeSplitProperties* getProperties( const agx::Constraint* constraint );
 
      static agxSDK::MergeSplitProperties* getOrCreateProperties( agx::ConstraintImplementation* constraint );
 
      static agxSDK::MergeSplitProperties* getProperties( agx::ConstraintImplementation* constraint );
 
      static const agxSDK::MergeSplitProperties* getProperties( const agx::ConstraintImplementation* constraint );
 
      static agxSDK::MergeSplitProperties* getOrCreateProperties( agxWire::Wire* wire );
 
      static agxSDK::MergeSplitProperties* getProperties( agxWire::Wire* wire );
 
      static const agxSDK::MergeSplitProperties* getProperties( const agxWire::Wire* wire );
 
      static void transferProperties( const agxCollide::Geometry* source, agxCollide::Geometry* destination );
 
      static void transferProperties( const agx::RigidBody* source, agx::RigidBody* destination );
 
      static void transferProperties( const agxWire::Wire* wire, agx::RigidBody* rb );
 
      static void transferProperties( const agxWire::Wire* wire, agx::Constraint* constraint );
 
      static agx::Bool isMergedByHandler( const agx::RigidBody* rb );
 
      static agx::Bool split( agx::RigidBody* rb );
 
    public:
      void setEnable( agx::Bool enable );
 
      agx::Bool getEnable() const;
 
      void setEnableSerialization( agx::Bool enable );
 
      agx::Bool getEnableSerialization() const;
 
      void setEnableGlobalMergeSplit( agx::Bool enable );
 
      agx::Bool getEnableGlobalMergeSplit() const;
 
      void setEnableGlobalMerge( agx::Bool enable );
 
      agx::Bool getEnableGlobalMerge() const;
 
      void setEnableGlobalSplit( agx::Bool enable );
 
      agx::Bool getEnableGlobalSplit() const;
 
 
      void setEnableSplitOnSeparation(agx::Bool enable);
 
      agx::Bool getEnableSplitOnSeparation() const;
 
      void setEnableMergePair(const agx::Name& group1, const agx::Name& group2, agx::Bool enable);
 
      void setEnableMergePair(agx::UInt32 group1, agx::UInt32 group2, agx::Bool enable);
 
      void setEnableMergePair(agx::UInt32 group1, const agx::Name& group2, agx::Bool enable);
 
      void setEnableMergePair(const agx::Name& group1, agx::UInt32 group2, agx::Bool enable);
 
      agx::Bool getEnableMergePair(const agx::Name& group1, const agx::Name& group2);
 
      agx::Bool getEnableMergePair(agx::UInt32 group1, agx::UInt32 group2);
 
      agx::Bool getEnableMergePair(agx::UInt32 group1, const agx::Name& group2);
 
      agx::Bool getEnableMergePair(const agx::Name& group1, agx::UInt32 group2);
 
      agx::Bool add( agxSDK::MergeSplitAlgorithm* mergeSplitAlgorithm );
 
      agx::Bool remove( agxSDK::MergeSplitAlgorithm* mergeSplitAlgorithm );
 
      MergeSplitAlgorithmContainer getAlgorithms() const;
 
      const MergedBodyRefContainer& getMergedBodies() const;
 
      agx::Bool merge( agx::MergedBody::EdgeInteraction* edgeInteraction );
 
      agx::MergedBodyRef createMergedBody() const;
 
      void registerMergedBody( agx::MergedBody* mergedBody );
 
      agxSDK::GeometryContactMergeSplitThresholds* getGlobalContactThresholds() const;
 
      agxSDK::ConstraintMergeSplitThresholds* getGlobalConstraintThresholds() const;
 
      agxSDK::WireMergeSplitThresholds* getGlobalWireThresholds() const;
 
      agx::Bool mayMerge( const agx::RigidBody* rb1, const agx::RigidBody* rb2 ) const;
 
      agx::Bool mayMerge( const agxCollide::Geometry* geometry1, const agxCollide::Geometry* geometry2 ) const;
 
      agx::Bool mayMerge( const agx::RigidBody* rb1,
                          const agxSDK::MergeSplitProperties* p1,
                          const agx::RigidBody* rb2,
                          const agxSDK::MergeSplitProperties* p2 ) const;
 
      agx::Bool maySplit( const agx::RigidBody* rb ) const;
 
      agx::Bool maySplit( const agx::RigidBody* rb, const agxSDK::MergeSplitProperties* properties ) const;
 
      agx::Bool mayMergeProperties( const agxSDK::MergeSplitProperties* properties ) const;
 
      agx::Bool maySplitProperties( const agxSDK::MergeSplitProperties* properties ) const;
 
      const agxSDK::Simulation* getSimulation() const;
 
      bool writeConnectivityGraph(const char* filename) const;
 
#ifndef SWIG
      void traverse( agx::MergedBody::EdgeInteractionVisitor visitor ) const;
 
      template<typename T>
      T* getAlgorithm() const;
#endif
 
    public:
      DOXYGEN_START_INTERNAL_BLOCK()
      static void storePersistentData( const agx::RigidBody* rb, agxStream::OutputArchive& out );
      static void storePersistentData( const agxCollide::Geometry* geometry, agxStream::OutputArchive& out );
      static void storePersistentData( const agx::HighLevelConstraintImplementation* constraint, agxStream::OutputArchive& out );
      static void storePersistentData( const agxWire::Wire* wire, agxStream::OutputArchive& out );
 
      static void restorePersistentData( agx::RigidBody* rb, agxStream::InputArchive& in );
      static void restorePersistentData( agxCollide::Geometry* geometry, agxStream::InputArchive& in );
      static void restorePersistentData( agx::HighLevelConstraintImplementation* constraint, agxStream::InputArchive& in );
      static void restorePersistentData( agxWire::Wire* wire, agxStream::InputArchive& in );
 
      template< typename T >
      static T* getOrCreateCustomData( agx::HighLevelConstraintImplementation* constraint );
 
      template< typename T >
      static T* getCustomData( const agx::HighLevelConstraintImplementation* constraint );
 
      static agx::Bool isMergedGranulate( const agx::RigidBody* rb );
      static void setIsMergedGranulate( agx::RigidBody* rb, agx::Bool flag );
      DOXYGEN_END_INTERNAL_BLOCK()
 
    public:
      DOXYGEN_START_INTERNAL_BLOCK()
      virtual void postIntegrate();
      DOXYGEN_END_INTERNAL_BLOCK()
 
    public:
      DOXYGEN_START_INTERNAL_BLOCK()
      void postSolve( const agxSDK::MergeSplitPostSolveData& postSolveData );
 
      template<typename T, typename ObjT>
      const T* getThresholds( ObjT obj, agxSDK::MergeSplitAlgorithm::Callback callback ) const;
 
      template<typename T>
      const T* getThresholdsFromProperties( const agxSDK::MergeSplitProperties* properties, agxSDK::MergeSplitAlgorithm::Callback callback ) const;
 
      agx::Bool isDefault( const agxSDK::MergeSplitThresholds* thresholds, agxSDK::MergeSplitAlgorithm::Callback callback ) const;
      DOXYGEN_END_INTERNAL_BLOCK()
 
    protected:
      virtual ~MergeSplitHandler();
 
      virtual void preCollide();
 
      virtual void separations(const agxCollide::SeparationPairVector& separations);
 
      virtual void preStep();
 
      virtual void postStep();
 
      virtual void reset( agx::Bool resetState = true );
 
      virtual void mergeSplitBegin();
 
      virtual void mergeSplitEnd();
 
      virtual void store( agxStream::OutputArchive& out ) const;
 
      virtual void restore( agxStream::InputArchive& in );
 
    private:
      enum EState
      {
        ENABLED               = 1 << 0, 
        GLOBAL_MERGE_ENABLED  = 1 << 1, 
        GLOBAL_SPLIT_ENABLED  = 1 << 2, 
        SERIALIZATION_ENABLED = 1 << 3, 
        SPLIT_ON_SEPARATION   = 1 << 4, 
        IN_POST_SOLVE         = 1 << 5  
      };
      typedef agx::BitState< EState, agx::Int32 > State;
 
      struct HandlerData : public agx::ICloneable
      {
        agxSDK::MergeSplitPropertiesRef properties;
      };
 
      struct MergeableHandlerData : public HandlerData
      {
        enum EState
        {
          MERGED_BY_HANDLER = 1 << 0,
          MERGED_GRANULATE  = 1 << 1
        };
        typedef agx::BitState< EState, agx::Int32 > State;
 
        State state;
        MergeSplitHandler* handler;
 
        protected:
          MergeableHandlerData() : state(), handler( nullptr ) {}
          virtual ~MergeableHandlerData() {}
 
        private:
          virtual agx::ICloneableRef clone( agx::ICloneable* /*child*/ ) override
          {
            // It's not supported to clone a merged object?
            // Implement this if the object is cloneable.
            return agx::ICloneableRef( nullptr );
          }
      };
      typedef agx::ref_ptr<MergeableHandlerData> HandlerDataRef;
 
      struct BodyHandlerData : public MergeableHandlerData
      {
        protected:
          virtual ~BodyHandlerData() {}
      };
 
      struct GeometryHandlerData : public MergeableHandlerData
      {
        protected:
          virtual ~GeometryHandlerData() {}
      };
 
      struct ConstraintHandlerData : public MergeableHandlerData
      {
        agx::ref_ptr<agx::Referenced> customData;
 
        protected:
          virtual ~ConstraintHandlerData() {}
      };
      typedef agx::ref_ptr< ConstraintHandlerData > ConstraintHandlerDataRef;
 
      struct WireHandlerData : public HandlerData
      {
        virtual agx::ICloneableRef clone( agx::ICloneable* child ) override
        {
          if ( child == nullptr )
            child = new WireHandlerData();
 
          child->as<WireHandlerData>()->properties = properties->clone()->as<MergeSplitProperties>();
 
          return agx::ICloneableRef( child );
        }
      };
 
    private:
      using MergeSplitThresholdsContainer = agx::Vector<MergeSplitThresholdsRef>;
 
    private:
      template<typename DataT, typename ObjT>
      static DataT* getHandlerData( const ObjT& obj );
 
      template<typename DataT, typename ObjT>
      static DataT* getOrCreateHandlerData( ObjT obj );
 
      template<typename DataT, typename SourceT, typename DestT>
      static void _transferProperties( const SourceT* source, DestT* destination );
 
      static void setDefaultState( State& state );
 
    private:
      friend class Simulation;
      MergeSplitHandler( agxSDK::Simulation* simulation );
 
      void onRemove( agx::Constraint* constraint );
 
      friend class MergeSplitMergedBodyListener;
      void onAddToMergedBody( agx::RigidBody* rb, const agx::MergedBody* mergedBody );
 
      void onRemovedFromMergedBody( agx::RigidBody* rb, const agx::MergedBody* mergedBody );
 
      void onAddToMergedBody( agx::MergedBody::EdgeInteraction* edge, const agx::MergedBody* mergedBody );
 
      void onRemovedFromMergedBody( const agx::MergedBody::EdgeInteractionRefContainer& edges, const agx::MergedBody* mergedBody );
 
      void onMovedFromToMergedBody( const agx::MergedBody::EdgeInteractionRefContainer& edges, const agx::MergedBody* fromMergedBody, const agx::MergedBody* toMergedBody );
 
    private:
      void clearAndAddDefaultAlgorithms();
 
      void handlePossibleGranulateBodies(bool updateSpace);
 
      agx::Bool getMergedByHandler( const agx::RigidBody* rb ) const;
 
      void updateMergedBodiesSimulationState( agx::Bool commitGeneratedInteractions );
 
      void performIslandSplitting();
 
      agx::Bool apply( const agxSDK::MergeSplitAction& action );
 
      agx::Bool merge( const agxSDK::MergeSplitAction& action );
 
      agx::Bool split( const agxSDK::MergeSplitAction& action );
 
    private:
      agxSDK::Simulation*           m_simulation;
      State                         m_state;
      MergeSplitAlgorithmContainer  m_mergeSplitAlgorithms;
      MergedBodyRefContainer        m_mergedBodies;
      MergeSplitActionContainer     m_currentActions;
      agx::MergedBody::ListenerRef  m_mergedBodyListener;
      MergeSplitThresholdsContainer m_globalThresholds;
      MergeIgnoreFilter             m_mergeIgnoreFilter;
 
      agx::RigidBodyRefVector       m_possibleGranulateBodies;
  };
 
  AGX_FORCE_INLINE const MergeSplitProperties* MergeSplitHandler::getProperties( const agx::RigidBody* rb )
  {
    const HandlerData* data = getHandlerData<HandlerData>( rb );
    return data != nullptr ? data->properties : nullptr;
  }
 
  AGX_FORCE_INLINE MergeSplitProperties* MergeSplitHandler::getProperties( agx::RigidBody* rb )
  {
    HandlerData* data = getHandlerData<HandlerData>(rb);
    return data != nullptr ? data->properties : nullptr;
  }
 
  AGX_FORCE_INLINE const MergeSplitProperties* MergeSplitHandler::getProperties( const agxCollide::Geometry* geometry )
  {
    const HandlerData* data = getHandlerData<HandlerData>( geometry );
    if ( data != nullptr && data->properties != nullptr )
      return data->properties;
    return geometry != nullptr ? MergeSplitHandler::getProperties( geometry->getRigidBody() ) : nullptr;
  }
 
  AGX_FORCE_INLINE MergeSplitProperties* MergeSplitHandler::getProperties( agxCollide::Geometry* geometry )
  {
    const HandlerData* data = getHandlerData<HandlerData>( geometry );
    if ( data != nullptr && data->properties != nullptr )
      return data->properties;
 
    // Special case so that the user doesn't have to call getOrCreateProperties for the geometry
    // when the properties are inherited. That's why we have to const_cast this case.
    return geometry != nullptr ? const_cast<MergeSplitProperties*>( MergeSplitHandler::getProperties( geometry->getRigidBody() ) ) : nullptr;
  }
 
  inline MergeSplitProperties* MergeSplitHandler::getProperties( agx::Constraint* constraint )
  {
    return constraint != nullptr ? MergeSplitHandler::getProperties( constraint->getRep() ) : nullptr;
  }
 
  inline const MergeSplitProperties* MergeSplitHandler::getProperties( const agx::Constraint* constraint )
  {
    return constraint != nullptr ? MergeSplitHandler::getProperties( constraint->getRep() ) : nullptr;
  }
 
  AGX_FORCE_INLINE MergeSplitProperties* MergeSplitHandler::getProperties( agx::ConstraintImplementation* constraint )
  {
    const HandlerData* data = getHandlerData<HandlerData>( constraint );
    return data != nullptr ? data->properties : nullptr;
  }
 
  AGX_FORCE_INLINE const MergeSplitProperties* MergeSplitHandler::getProperties( const agx::ConstraintImplementation* constraint )
  {
    const HandlerData* data = getHandlerData<HandlerData>( constraint );
    return data != nullptr ? data->properties : nullptr;
  }
 
  inline agx::Bool MergeSplitHandler::getMergedByHandler( const agx::RigidBody* rb ) const
  {
    const MergeableHandlerData* handlerData = getHandlerData<MergeableHandlerData>( rb );
    return handlerData != nullptr && handlerData->state.Is( MergeableHandlerData::MERGED_BY_HANDLER ) && handlerData->handler == this;
  }
 
  template<typename DataT, typename ObjT>
  inline DataT* MergeSplitHandler::getHandlerData( const ObjT& obj )
  {
    return agx::InternalData::get<DataT>( obj, agx::InternalData::MERGE_SPLIT );
  }
 
  template<typename DataT, typename ObjT>
  inline DataT* MergeSplitHandler::getOrCreateHandlerData( ObjT obj )
  {
    agxAssert( obj != nullptr );
    return agx::InternalData::getOrCreate<DataT>( obj, agx::InternalData::MERGE_SPLIT );
  }
 
  template<typename DataT, typename SourceT, typename DestT>
  void MergeSplitHandler::_transferProperties( const SourceT* source, DestT* destination )
  {
    if ( static_cast<const void*>( source ) == static_cast<void*>( destination ) ||
         source == nullptr ||
         destination == nullptr )
      return;
 
    const auto sourceData = MergeSplitHandler::getHandlerData<HandlerData>( source );
    if ( sourceData == nullptr )
      return;
 
    auto destinationData = MergeSplitHandler::getOrCreateHandlerData<DataT>( destination );
    destinationData->properties = sourceData->properties;
  }
 
  template< typename T >
  T* MergeSplitHandler::getOrCreateCustomData( agx::HighLevelConstraintImplementation* constraint )
  {
    ConstraintHandlerData* data = MergeSplitHandler::getOrCreateHandlerData<ConstraintHandlerData>( constraint );
    if ( data->customData == nullptr )
      data->customData = new T();
 
    return data->customData->as<T>();
  }
 
  template< typename T >
  T* MergeSplitHandler::getCustomData( const agx::HighLevelConstraintImplementation* constraint )
  {
    const ConstraintHandlerData* data = MergeSplitHandler::getHandlerData<ConstraintHandlerData>( constraint );
    return data != nullptr && data->customData != nullptr ? data->customData->as<T>() : nullptr;
  }
 
  inline agx::Bool MergeSplitHandler::isMergedGranulate( const agx::RigidBody* rb )
  {
    const BodyHandlerData* data = getHandlerData<BodyHandlerData>( rb );
    return data != nullptr && data->state.Is( MergeableHandlerData::MERGED_GRANULATE );
  }
 
  inline void MergeSplitHandler::setIsMergedGranulate( agx::RigidBody* rb, agx::Bool flag )
  {
    BodyHandlerData* data = getOrCreateHandlerData<BodyHandlerData>( rb );
    data->state.update( MergeableHandlerData::MERGED_GRANULATE, flag );
  }
 
  template<typename T>
  T* MergeSplitHandler::getAlgorithm() const
  {
    for ( const auto& algorithm : m_mergeSplitAlgorithms )
      if ( algorithm->template is<T>() )
        return algorithm->template as<T>();
    return nullptr;
  }
 
  template<typename T, typename ObjT>
  const T* MergeSplitHandler::getThresholds( const ObjT obj, MergeSplitAlgorithm::Callback callback ) const
  {
    return getThresholdsFromProperties<T>( getProperties( obj ), callback );
  }
 
  template<typename T>
  const T* MergeSplitHandler::getThresholdsFromProperties( const MergeSplitProperties* properties,
                                                           MergeSplitAlgorithm::Callback callback ) const
  {
    const T* thresholds       = nullptr;
    const auto callbackIndex  = MergeSplitProperties::findCallbackIndex( callback );
    if ( properties == nullptr || (thresholds = properties->getMergeSplitThresholds<T>( callbackIndex )) == nullptr )
      thresholds = (T*)m_globalThresholds[ callbackIndex ].get();
 
    return thresholds;
  }
 
  inline agx::Bool MergeSplitHandler::isDefault( const MergeSplitThresholds* thresholds, MergeSplitAlgorithm::Callback callback ) const
  {
    return thresholds != nullptr && thresholds == m_globalThresholds[ MergeSplitProperties::findCallbackIndex( callback ) ];
  }
 
  inline agx::Bool MergeSplitHandler::mayMergeProperties( const MergeSplitProperties* properties ) const
  {
    return ( properties != nullptr && properties->getEnableMerge() ) ||     // Merge enabled in properties?
           ( properties == nullptr && m_state.Is( GLOBAL_MERGE_ENABLED ) ); // Only check global flag if this object hasn't got properties!
 
  }
 
  inline agx::Bool MergeSplitHandler::maySplitProperties( const MergeSplitProperties* properties ) const
  {
    return ( properties != nullptr && properties->getEnableSplit() ) ||     // Enabled in properties?
           ( properties == nullptr && m_state.Is( GLOBAL_SPLIT_ENABLED ) ); // Only check global flag if this object hasn't got properties!
 
  }
}