MergeSplitUtils.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/MergedBodySolverData.h>
#include <agxSDK/MergeSplitHandler.h>
 
#define REPORT_SYSTEM_JOB_ENABLE 1
#if REPORT_SYSTEM_JOB_ENABLE
#include <agxUtil/TimerBlock.h>
#include <agx/Thread.h>
#include <agx/macros.h>
 
// Since winbase.h defines an IGNORE macro, but we like to define an ENUM with a IGNORE value
#ifdef WIN32
#ifdef IGNORE
#undef IGNORE
#endif
#endif
 
#define REPORT_SYSTEM_JOB_EX( str )\
  auto sjrtb = agxUtil::TimerBlock( []( agx::UInt64 start, agx::UInt64 stop )\
  {\
    agx::Thread::getCurrentThread()->reportSystemJob( start, stop, str );\
  } )
 
#define REPORT_SYSTEM_JOB\
  const char* AGX_CONCAT(agx_functionName,__LINE__) = __FUNCTION__; \
  auto sjrtb = agxUtil::TimerBlock( [AGX_CONCAT(agx_functionName,__LINE__)]( agx::UInt64 start, agx::UInt64 stop )\
  {\
    agx::Thread::getCurrentThread()->reportSystemJob( start, stop, AGX_CONCAT(agx_functionName,__LINE__) );\
  } )
#else
#define REPORT_SYSTEM_JOB_EX( str )
#define REPORT_SYSTEM_JOB
#endif
 
#include <agxWire/Node.h>
 
namespace agxWire
{
  class WireDistanceCompositeConstraint;
}
 
namespace agxSDK
{
  namespace MergeSplitUtils
  {
    AGX_FORCE_INLINE agx::Vec3 calculateRelativePointVelocity( const agx::Vec3& point,
                                                               const agx::Vec3& cmPos,
                                                               const agx::Vec3& linVel,
                                                               const agx::Vec3& angVel )
    {
      return ( linVel + ( angVel ^ ( point - cmPos ) ) );
    }
 
    AGX_FORCE_INLINE agx::Real calculateImpactSpeed( const agx::Vec3& point,
                                                     const agx::Vec3f& normal,
                                                     const agx::Vec3& cmPos,
                                                     const agx::Vec3& linVel,
                                                     const agx::Vec3& angVel )
    {
      return -( agx::Vec3( normal ) * calculateRelativePointVelocity( point, cmPos, linVel, angVel ) );
    }
 
    AGX_FORCE_INLINE agx::Vec3 calculateRelativePointVelocity( const agx::Vec3& point,
                                                               const agx::Vec3& relVel,
                                                               const agx::Vec3& cmPos1,
                                                               const agx::Vec3& angVel1,
                                                               const agx::Vec3& cmPos2,
                                                               const agx::Vec3& angVel2 )
    {
      return ( relVel + ( angVel1 ^ ( point - cmPos1 ) ) + ( ( point - cmPos2 ) ^ angVel2 ) );
    }
 
    AGX_FORCE_INLINE agx::Real calculateImpactSpeed( const agx::Vec3& point,
                                                     const agx::Vec3f& normal,
                                                     const agx::Vec3& relVel,
                                                     const agx::Vec3& cmPos1,
                                                     const agx::Vec3& angVel1,
                                                     const agx::Vec3& cmPos2,
                                                     const agx::Vec3& angVel2 )
    {
      return -( agx::Vec3( normal ) * calculateRelativePointVelocity( point, relVel, cmPos1, angVel1, cmPos2, angVel2 ) );
    }
 
    AGXPHYSICS_EXPORT agxSDK::MergedBodySolverData::ForceFrame calculateContactForceFrame( const agx::RigidBody* refRb,
                                                                                           const agx::MergedBody::GeometryContactEdgeInteraction* contactEdge,
                                                                                           const MergeSplitHandler* handler = nullptr );
 
    struct ExternalForceSplitResult
    {
      ExternalForceSplitResult()
        : shouldSplit( false ), rbTop( nullptr ), rbBottom( nullptr ) {}
      agx::Bool shouldSplit;
      const agx::RigidBody* rbTop;
      const agx::RigidBody* rbBottom;
    };
 
    AGXPHYSICS_EXPORT ExternalForceSplitResult shouldSplitGivenExternalForce( agxSDK::MergedBodySolverData::ForceFrame edgeStrength,
                                                                              const agx::RigidBody* rb,
                                                                              const agx::RigidBody* otherRb,
                                                                              const agx::Vec3& externalForces,
                                                                              const agx::Vec3& externalTorques,
                                                                              const agx::Vec3& gravity,
                                                                              const agxSDK::MergeSplitHandler& handler );
 
    template<typename T>
    using CollectConnectingEdgesPtrContainer = agx::Vector<T*>;
 
    template<typename T>
    void collectConnectingEdges( const T& referenceEdge,
                                 agx::MergedBody::EdgeInteraction::InteractionTag edgeTag,
                                 agx::Bool bounceAtNonSplittableBodies,
                                 const agx::MergedBody& mergedBody,
                                 const MergeSplitHandler& handler,
                                 CollectConnectingEdgesPtrContainer<T>& result )
    {
      using VisitedContainer          = agx::HashSet<const agx::RigidBody*>;
      using SymmetricRigidBodyPtrPair = agx::SymmetricPair<const agx::RigidBody*>;
      using VisitedPairContainer      = agx::HashSet<SymmetricRigidBodyPtrPair>;
      using BfsQueueContainer         = std::queue<const agx::RigidBody*>;
 
      VisitedContainer visited;
      VisitedPairContainer visitedPairs;
      BfsQueueContainer bfsQueue;
 
      const auto visit = [&visited, &bfsQueue]( const agx::RigidBody* rb )
      {
        agxAssert( !visited.contains( rb ) );
        visited.insert( rb );
        bfsQueue.push( rb );
      };
 
      const auto visitedPair = [&visitedPairs]( const agx::RigidBody* rb1, const agx::RigidBody* rb2 ) -> agx::Bool
      {
        SymmetricRigidBodyPtrPair rb1Rb2Pair( rb1, rb2 );
        if ( visitedPairs.contains( rb1Rb2Pair ) )
          return true;
 
        visitedPairs.insert( rb1Rb2Pair );
 
        return false;
      };
 
      const auto pop = [&bfsQueue]() -> const agx::RigidBody*
      {
        agxAssert( !bfsQueue.empty() );
        auto front = bfsQueue.front();
        bfsQueue.pop();
        return front;
      };
 
      visit( referenceEdge.getRigidBody1() );
      while ( !bfsQueue.empty() ) {
        const auto rb        = pop();
        const auto neighbors = mergedBody.getNeighbors( rb );
 
        agxAssert( neighbors != nullptr );
        for ( const auto neighbor : *neighbors ) {
          if ( visitedPair( neighbor, rb ) )
            continue;
 
          const auto edges = mergedBody.getEdges( neighbor, rb );
          agxAssert( edges != nullptr );
          auto oneHasEdgeTag = false;
          for ( const auto& edge : *edges ) {
            const auto edgeHasTag = edge->isTagged( edgeTag );
            oneHasEdgeTag         = oneHasEdgeTag || edgeHasTag;
            if ( edge != &referenceEdge && edgeHasTag ) {
              agxAssert( !result.contains( edge ) );
              result.push_back( edge->template as<T>() );
            }
          }
 
          // Continue search if rb<->neighbor has the given tag and hasn't already
          // been visited. Stop if:
          //   bounceAtNonSplittableBodies && !handler.maySplit( neighbor )
          const auto visitThisNeighbor = neighbor != nullptr &&
                                         oneHasEdgeTag &&
                                         ( !bounceAtNonSplittableBodies || handler.maySplit( neighbor ) ) &&
                                         !visited.contains( neighbor );
          if ( visitThisNeighbor )
            visit( neighbor );
        }
      }
    }
 
    class WireSegmentMergedState;
    using WireSegmentMergedStateContainer = agx::VectorPOD<WireSegmentMergedState>;
 
    class AGXPHYSICS_EXPORT WireSegmentMergedState
    {
      public:
        static WireSegmentMergedStateContainer create( const agxWire::Wire* wire );
 
      public:
        enum EState : agx::UInt32
        {
          FREE   = 1 << 0, 
          MERGED = 1 << 1, 
          IGNORE = 1 << 2  
        };
        using State = agx::BitState<EState, agx::UInt32>;
 
      public:
        WireSegmentMergedState( const agxWire::WireDistanceCompositeConstraint* wire );
 
        agxWire::NodeConstIterator construct( agxWire::NodeConstIterator it );
 
        State getState() const;
 
        agx::UInt size() const;
 
        agxWire::NodeConstIterator begin() const;
 
        agxWire::NodeConstIterator end() const;
 
        agxWire::BodyFixedNode* front() const;
 
        agxWire::Node* back() const;
 
        agx::MergedBody* getMergedBody() const;
 
      private:
        const agxWire::WireDistanceCompositeConstraint* m_wire;
        agx::MergedBody* m_mergedBody;
        State m_state;
        agxWire::NodeConstIterator m_begin;
        agxWire::NodeConstIterator m_end;
        agx::UInt m_size;
    };
  }
}