agxUtil.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/agx.h>
#include <agx/agxPhysics_export.h>
#include <agx/Singleton.h>
#include <agxCollide/agxcollide_vector_types.h>
#include <agx/agx_vector_types.h>
#include <agx/AffineMatrix4x4.h>
#include <agx/Constraint.h>
 
#include <agxSDK/StepEventListener.h>
#include <agxSDK/ContactEventListener.h>
#include <agxSDK/Simulation.h>
 
 
namespace agx
{
  typedef std::pair< Real, Real > RealPair;
  class RigidBody;
  class Material;
  class ConstraintImplementation;
 
}
 
namespace agxSDK {
  class Assembly;
}
 
namespace agxCollide {
  class Mesh;
}
 
namespace agxControl
{
  class EventSensor;
}
 
 
namespace agxUtil {
 
  AGXPHYSICS_EXPORT bool setEnableCollisions( agx::RigidBody *rb1, agx::RigidBody *rb2, bool enable );
 
  AGXPHYSICS_EXPORT bool setEnableCollisions( agx::RigidBody *body, const agxCollide::Geometry *geom, bool enable );
 
  AGXPHYSICS_EXPORT bool setEnableCollisions( const agxCollide::Geometry *geom, agx::RigidBody *body, bool enable );
 
  AGXPHYSICS_EXPORT bool setEnableCollisions( agxSDK::Assembly *a1, agxSDK::Assembly *a2, bool enable );
 
  AGXPHYSICS_EXPORT bool setEnableCollisions( agxSDK::Assembly *assembly, agx::RigidBody *body, bool enable );
 
  AGXPHYSICS_EXPORT bool setEnableCollisions( agx::RigidBody *body, agxSDK::Assembly *assembly, bool enable );
 
  AGXPHYSICS_EXPORT bool setEnableCollisions( agxSDK::Assembly *assembly, agxCollide::Geometry *geo, bool enable );
 
  AGXPHYSICS_EXPORT bool setEnableCollisions( agxCollide::Geometry *geo, agxSDK::Assembly *assembly, bool enable );
 
  AGXPHYSICS_EXPORT bool setEnableGeometries( agx::RigidBody *body, bool enable );
 
  AGXPHYSICS_EXPORT bool setEnableCollisions(agx::RigidBody* body, bool enable);
 
 
  AGXPHYSICS_EXPORT bool extractGeometries( agxCollide::GeometryPtrVector& geometries, agxSDK::Assembly *assembly );
 
  AGXPHYSICS_EXPORT bool extractGeometries( agxCollide::GeometryPtrVector& geometries, agx::RigidBody *body );
 
 
  AGXPHYSICS_EXPORT bool extractRigidBodies( agx::RigidBodyPtrVector& bodies, agxSDK::Assembly *assembly );
 
  AGXPHYSICS_EXPORT bool extractConstraints( agx::ConstraintPtrVector& vec, agx::DynamicsSystem *system );
 
  AGXPHYSICS_EXPORT bool extractGeometries( agxCollide::GeometryPtrVector& vec, agxCollide::Space *space );
 
  AGXPHYSICS_EXPORT bool setBodyMaterial( agx::RigidBody *body, agx::Material *material );
 
  AGXPHYSICS_EXPORT void setMotionControl( agx::RigidBodyPtrVector& bodies, agx::RigidBody::MotionControl motionControl );
 
  AGXPHYSICS_EXPORT void addParentVelocity( const agx::RigidBody *parentBody, agx::RigidBody *body );
 
 
  template <typename T>
  void addGroup(const agx::RigidBody* body, const T& id)
  {
    if (body == nullptr) {
      LOGGER_WARNING() << "Invalid null pointer for body " << std::endl << LOGGER_END();
      return;
    }
 
    for (const auto& geometry : body->getGeometries())
      geometry->addGroup(id);
  }
 
  template <typename T>
  void removeGroup(const agx::RigidBody* body, const T& id)
  {
    if (body == nullptr) {
      LOGGER_WARNING() << "Invalid null pointer for body " << std::endl << LOGGER_END();
      return;
    }
 
    for (const auto& geometry : body->getGeometries())
      geometry->removeGroup(id);
  }
 
  template<typename T>
  void addGroup(agxSDK::Assembly* assembly,T id)
  {
    agxCollide::GeometryPtrVector geometries;
    agxUtil::extractGeometries(geometries, assembly);
 
    for (auto& g: geometries)
      g->addGroup(id);
  }
 
  template<typename T>
  void removeGroup(agxSDK::Assembly* assembly, T id)
  {
    agxCollide::GeometryPtrVector geometries;
    agxUtil::extractGeometries(geometries, assembly);
 
    for (auto& g: geometries)
      g->removeGroup(id);
  }
 
  AGXPHYSICS_EXPORT agx::AffineMatrix4x4 calculateCylinderTransform( const agx::Vec3& startPoint, const agx::Vec3& endPoint );
 
  AGXPHYSICS_EXPORT agxControl::EventSensor* createSinkOnGeometry(agxCollide::Geometry* geometry,
                                                                  bool onlyIncludeEmittedBodies);
 
  AGXPHYSICS_EXPORT size_t getMeshData( const agxCollide::Mesh* mesh, agx::Vec3Vector& vertices, agx::UInt32Vector& indices );
 
  AGXPHYSICS_EXPORT bool isConvexMesh( const agxCollide::Mesh* mesh, agx::Real threshold=1E-5 );
 
  AGXPHYSICS_EXPORT size_t getContactMaterialVector(agxSDK::MaterialManager* mgr, agx::ContactMaterialPtrVector& contactMaterials);
 
  AGXPHYSICS_EXPORT size_t getMaterialVector(agxSDK::MaterialManager* mgr, agx::MaterialPtrVector& materials);
 
  AGXPHYSICS_EXPORT agx::Vec3 transformVectorFromTo( const agx::Vec3 vec, const agx::RigidBody* fromBody, const agx::RigidBody* toBody );
 
  AGXPHYSICS_EXPORT agx::Vec3 transformPointFromTo( const agx::Vec3 point, const agx::RigidBody* fromBody, const agx::RigidBody* toBody );
 
  AGXPHYSICS_EXPORT agx::RealVector linspace( agx::Real start, agx::Real end, agx::UInt num );
 
  struct JumpRequestWireOption
  {
    enum Option : agx::UInt32
    {
      DETACH_CONTACT_NODES = 1 << 0, 
      DETACH_EYE_NODES     = 1 << 1  
    };
  };
 
  AGXPHYSICS_EXPORT agxCollide::BoundingAABB computeRigidBodyBoundingVolume( const agx::RigidBody* body );
 
  AGXPHYSICS_EXPORT size_t jumpRequest( agxSDK::Assembly* collection,
                                        agx::RigidBody* parentBody,
                                        const agx::AffineMatrix4x4& parentBodyWorldTransform,
                                        agx::UInt32 wireOptions = agx::UInt32( 0 ) );
 
 
  template< typename T >
  struct LessThanPred
  {
    agx::Bool operator() ( const T& o1, const T& o2 ) { return o1 < o2; }
  };
 
  template< typename T >
  struct LargerThanPred
  {
    agx::Bool operator() ( const T& o1, const T& o2 ) { return o1 > o2; }
  };
 
  template< typename ContainerT, typename Pred >
  void insertionSort( ContainerT& container, Pred pred )
  {
    int size = (int)container.size();
    if ( size < 2 )
      return;
 
    for ( int i = 1; i < size; ++i ) {
      typename ContainerT::value_type tmp = container[ i ];
      int j = 0;
      for ( j = i; j > 0; --j )
        if ( pred( tmp, container[ j - 1 ] ) )
          container[ j ] = container[ j - 1 ];
        else break;
      container[ j ] = tmp;
    }
  }
 
  template< typename ContainerT >
  void insertionSort( ContainerT& container )
  {
    insertionSort( container, LessThanPred< typename ContainerT::value_type >() );
  }
 
  template< typename ContainerT >
  agx::Bool isSorted( const ContainerT& container )
  {
    if ( container.size() < 2 )
      return true;
 
    for ( size_t i = 0; i < container.size() - 1; ++i )
      if ( container[ i + 1 ] < container[ i ] )
        return false;
    return true;
  }
 
 
  template<typename ContainerT>
  void freeContainerMemory(ContainerT& container);
 
  template<typename ContainerT>
  ContainerT copyContainerMemory(const ContainerT& container);
 
  class AGXPHYSICS_EXPORT ConstraintHolder : public agx::Constraint
  {
    public:
      ConstraintHolder();
 
      bool add( agx::ConstraintImplementation* constraint );
 
      bool remove( agx::ConstraintImplementation* constraint );
 
      void clear();
 
    protected:
      virtual ~ConstraintHolder();
 
      virtual int getNumDOF() const override
      {
        return 0;
      }
      virtual void render( class agxRender::RenderManager* , float /*scale = 1.0f */ ) const override
      {}
 
      AGXSTREAM_DECLARE_SERIALIZABLE( agxUtil::ConstraintHolder );
 
    private:
      class ConstraintHolderImplementation* m_implementation;
  };
 
  typedef agx::ref_ptr< ConstraintHolder > ConstraintHolderRef;
 
  template< typename T >
  class GeneralStepListener : public agxSDK::StepEventListener
  {
    public:
      GeneralStepListener( T* obj, int mask = agxSDK::StepEventListener::DEFAULT )
        : agxSDK::StepEventListener( mask ), m_obj( obj ) {}
 
    protected:
      virtual ~GeneralStepListener() {}
 
      DOXYGEN_START_INTERNAL_BLOCK()
#define callback_implementation( callback_name )            \
      virtual void callback_name( const agx::TimeStamp& t ) \
      {                                                     \
        if ( !m_obj.isValid() ) {                           \
          getSimulation()->remove( this );                  \
          return;                                           \
        }                                                   \
        m_obj->callback_name( t );                          \
      }
 
      callback_implementation( preCollide )
      callback_implementation( pre )
      callback_implementation( post )
#undef callback_implementation
      DOXYGEN_END_INTERNAL_BLOCK()
 
    protected:
      agx::observer_ptr< T > m_obj;
  };
 
  template< typename T >
  class GeneralContactEventListener : public agxSDK::ContactEventListener
  {
    public:
      GeneralContactEventListener( T* obj, int mask = agxSDK::ContactEventListener::DEFAULT )
        : agxSDK::ContactEventListener( mask ), m_obj( obj ) {}
 
    protected:
      virtual ~GeneralContactEventListener() {}
 
#define callback_implementation( callback_name, ArgT )                                                           \
      virtual agxSDK::ContactEventListener::KeepContactPolicy callback_name( const agx::TimeStamp& t, ArgT obj ) \
      {                                                                                                          \
        if ( !m_obj.isValid() ) {                                                                                \
          getSimulation()->remove( this );                                                                       \
          return agxSDK::ContactEventListener::KEEP_CONTACT;                                                     \
        }                                                                                                        \
        return m_obj->callback_name( t, obj );                                                                   \
      }
 
      callback_implementation( impact,  agxCollide::GeometryContact* )
      callback_implementation( contact, agxCollide::GeometryContact* )
#undef callback_implementation
 
      virtual void separation( const agx::TimeStamp& t, agxCollide::GeometryPair& gp )
      {
        if ( !m_obj.isValid() ) {
          getSimulation()->remove( this );
          return;
        }
 
        m_obj->separation( t, gp );
      }
 
    protected:
      agx::observer_ptr< T > m_obj;
  };
 
 
 
  class AGXPHYSICS_EXPORT SceneRoot : public agx::Singleton
  {
    public:
      static SceneRoot* object();
 
      virtual bool createVisual( agx::RigidBody* /*rb*/, float /*detailRation*/ = 0.30f ) { return false; };
      virtual bool createVisual( agxCollide::Geometry* /*geometry*/, float /*detailRation*/ = 0.30f ) { return false; }
 
      static void setInstance( SceneRoot* baseInstance );
 
      SINGLETON_CLASSNAME_METHOD();
 
    protected:
      virtual ~SceneRoot() {}
      void shutdown() override;
 
    protected:
      static SceneRoot* s_baseInstance;
  };
 
 
 
}