ConstraintImplementation.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.
*/
 
#ifndef AGX_CONSTRAINT_IMPLEMENTATION_H
#define AGX_CONSTRAINT_IMPLEMENTATION_H
 
#include <agxSDK/SimulationProxy.h>
 
#include <agx/ElementaryConstraint.h>
#include <agx/Constraint.h>
 
#include <agx/agx_hash_types.h>
#include <agx/agx_valarray_types.h>
 
#include <agx/Physics/ConstraintForcesEntity.h>
 
#include <agxStream/Serializable.h>
 
#ifdef _MSC_VER
# pragma warning(push)
# pragma warning( disable : 4251 ) // class X needs to have dll-interface to be used by clients of class Y
#endif
 
#define DEFAULT_COMPLIANCE 1.0E-8
 
#define CONSTRAINT_SUCCESS( success ) \
  if ( !(success) ) { \
    LOGGER_WARNING() << "Unable to create constraint. Constraint cleared." << std::endl << LOGGER_END(); \
    m_bodies.clear(); \
    m_localState.remove( VALID ); \
    return; \
  }
 
#define CONSTRAINT_SUCCESS_RETURN_BOOL( success ) \
  if ( !(success) ) { \
    LOGGER_WARNING() << "Unable to create constraint. Constraint cleared." << std::endl << LOGGER_END(); \
    m_bodies.clear(); \
    m_localState.remove( VALID ); \
    return false; \
  }
 
namespace agx
{
  // Forward declaration
  class ElementaryConstraint;
  class Constraint;
  class RegularizationParameters;
  class SparseRangeReal;
  class LSquareComplianceMatrix;
  struct NlmcpCallbackSolverData;
 
  typedef Vector< RigidBody* > ConstraintRigidBodyContainer;
  typedef std::pair< Real, Real > RealPair;
 
  class BlockStructure
  {
    public:
      typedef UInt16 BlockType;
      BlockStructure()
        : m_numBlocks( 0 ), m_numRows( 0 ), m_numDynBlocks( 0 ) {}
 
      AGX_FORCE_INLINE void setNumBlocks( BlockType numBlocks ) { m_numBlocks = numBlocks; }
      AGX_FORCE_INLINE void setNumRows( BlockType numRows ) { m_numRows = numRows; }
 
      AGX_FORCE_INLINE BlockType getNumBlocks() const { return m_numBlocks; }
      AGX_FORCE_INLINE BlockType getNumRows() const { return m_numRows; }
 
      AGX_FORCE_INLINE BlockType& blocks() { return m_numBlocks; }
      AGX_FORCE_INLINE BlockType& rows() { return m_numRows; }
      AGX_FORCE_INLINE BlockType  blocks() const { return m_numBlocks; }
      AGX_FORCE_INLINE BlockType  rows() const { return m_numRows; }
 
      AGX_FORCE_INLINE BlockType& dynBlocks() { return m_numDynBlocks; }
      AGX_FORCE_INLINE BlockType  dynBlocks() const { return m_numDynBlocks; }
      AGX_FORCE_INLINE BlockType  kinBlocks() const { return (BlockType)(m_numBlocks - m_numDynBlocks); }
 
      AGX_FORCE_INLINE void reset() { m_numBlocks = m_numRows = m_numDynBlocks = 0; }
 
    private:
      BlockType m_numBlocks;
      BlockType m_numRows;
      BlockType m_numDynBlocks;
  };
 
  class CALLABLE AGXPHYSICS_EXPORT ConstraintImplementation
  {
    public:
      enum Tags
      {
        NONE                  = 0,
        MANY_BODY             = (1<<0), 
        DEFAULT_BINARY_TAG    = NONE,
        DEFAULT_MANY_BODY_TAG = MANY_BODY
      };
 
      class AGXPHYSICS_EXPORT ConstrainedBodiesState
      {
        public:
          class AGXPHYSICS_EXPORT BodiesInfo
          {
            public:
              BodiesInfo();
 
              agx::UInt& numDynamicBodies();
 
              agx::UInt  numDynamicBodies() const;
 
              agx::UInt& numNonStaticBodies();
 
              agx::UInt  numNonStaticBodies() const;
 
            private:
              agx::UInt m_numDynamicBodies;
              agx::UInt m_numNonStaticBodies;
          };
 
        public:
          ConstrainedBodiesState();
 
          void reset( agx::UInt numBodies );
 
          agx::Bool increment( agx::RigidBody* rb );
 
          agx::Bool finalize( agx::Bool valid );
 
          const BodiesInfo& getOriginalInfo() const;
 
          const BodiesInfo& getSolverInfo() const;
 
          const agx::Int32Vector& getPermutation() const;
 
          agx::Int32Vector& getPermutation();
 
          const agx::Int32Vector& getBodyIndices() const;
 
          agx::Int32Vector& getBodyIndices();
 
          const agx::ConstraintRigidBodyContainer& getSolverBodies() const;
 
          agx::UInt getNumRecurrentMergedBodies() const;
 
        private:
          BodiesInfo m_originalInfo;
          BodiesInfo m_solverInfo;
          ConstraintRigidBodyContainer m_bodies;
          Int32Vector m_permutation;
          Int32Vector m_bodyIndices;
          UInt m_numRecurrentMergedBodies;
      };
 
      #if !defined(SWIGPYTHON)
      class AGXPHYSICS_EXPORT BodyView
      {
        public:
          BodyView( const ConstrainedBodiesState::BodiesInfo& info,  const ConstraintRigidBodyContainer& bodies );
 
          BodyView( const BodyView& other );
 
          agx::UInt getNumDynamicBodies() const;
 
          agx::UInt getNumNonStaticBodies() const;
 
          agx::UInt getNumBodies() const;
 
          agx::RigidBody* getBodyAt( agx::UInt i );
 
          const agx::RigidBody* getBodyAt( agx::UInt i ) const;
 
          const ConstraintRigidBodyContainer& getBodies() const;
 
          BodyView& operator = ( const BodyView& ) = delete;
 
        private:
          const ConstrainedBodiesState::BodiesInfo& m_info;
          const ConstraintRigidBodyContainer& m_bodies;
      };
 
      class AGXPHYSICS_EXPORT SolverBodyView : public BodyView
      {
        public:
          SolverBodyView( ConstrainedBodiesState& state );
 
          SolverBodyView( const SolverBodyView& other );
 
          const ConstrainedBodiesState& getState() const;
 
          ConstrainedBodiesState& getState();
 
          agx::RigidBody* getPermutedBodyAt( agx::UInt i );
 
          const agx::RigidBody* getPermutedBodyAt( agx::UInt i ) const;
 
        private:
          inline SolverBodyView& operator = ( const SolverBodyView& ) { agxAbort(); return *this; }
 
        private:
          ConstrainedBodiesState& m_state;
      };
      #endif
 
    public:
      ConstraintImplementation();
 
      virtual ~ConstraintImplementation();
 
      #if !defined(SWIGPYTHON)
      BodyView getUserView() const;
 
      SolverBodyView getSolverView() const;
      #endif
 
      // ****************************************************************************
      // Callbacks from solvers *****************************************************
      // ****************************************************************************
 
      virtual bool updateValid();
 
      virtual void prepare();
 
      virtual size_t updateJacobian( agx::Jacobian6DOFElement* jacobians );
 
      virtual int getViolation( agx::Real* g, int row );
 
      virtual void getEpsilon( agx::Real* epsilon, agx::Real h );
 
      virtual void getParameters( agx::Real* violation,
                                  agx::Real* epsilon,
                                  agx::Real* damping,
                                  bool* holonomic,
                                  agx::Real* velocity,
                                  agx::RangeReal* bounds,
                                  int8_t* indexSetState,
                                  agx::Real h,
                                  agx::Real minPerturbation = agx::Real( 1E-10 ) );
 
      virtual int getConstraintVelocity( agx::Real* v, int row );
 
      virtual void getBounds( agx::RangeReal* bounds, agx::Real dt ) const;
 
      CALLABLE_IGNORE
      virtual void updateComplianceMatrix( LSquareComplianceMatrix matrix ) const;
 
      void onDirectComplianceMatrix( Real* data, unsigned int ld ) const;
 
      virtual void postSolveCallback( const agx::Jacobian6DOFElement* jacobians, const agx::Real* solution, agx::Real dt );
 
      void postSolveIndexSetCallback( const int8_t* state );
 
      virtual void preSystemCallback( DynamicsSystem* system );
 
      virtual void postSystemCallback( DynamicsSystem* system );
 
      void sort();
 
      void reset();
 
      virtual void addNlCallbacks( agx::SparseRangeReal& sparseRangeReal, const NlmcpCallbackSolverData& solverData ) const;
 
      // ****************************************************************************
 
      // ****************************************************************************
      // Methods used by the solvers ************************************************
      // ****************************************************************************
 
      AGX_FORCE_INLINE agx::UInt getJacobianRowUsage() const { return m_numRows; }
 
      AGX_FORCE_INLINE const BlockStructure& getBlockStructure() const { return m_blockStructure; }
 
      AGX_FORCE_INLINE void setBlockRowIndex( agx::UInt blockRowIndex ) { m_blockRowIndex = blockRowIndex; }
 
      AGX_FORCE_INLINE agx::UInt getBlockRowIndex() const { return m_blockRowIndex; }
 
      AGX_FORCE_INLINE bool getSolveIterative() const { return m_solveType == Constraint::ITERATIVE; }
 
      AGX_FORCE_INLINE bool getSolveBoth() const { return m_solveType == Constraint::DIRECT_AND_ITERATIVE; }
 
      AGX_FORCE_INLINE void setSolveType( Constraint::SolveType type ) { m_solveType = type; }
 
      AGX_FORCE_INLINE Constraint::SolveType getSolveType() const { return m_solveType; }
 
      AGX_FORCE_INLINE bool isImpacting() const { return m_localState.Is( IMPACTING ); }
 
      AGX_FORCE_INLINE void incrementConstraintCount( RigidBody* rb ) const { agxAssert( rb ); if (rb) rb->incrementNumConstraints(); }
 
      AGX_FORCE_INLINE int getTag() const { return m_tag; }
 
#ifndef SWIG
      void* customData;  
#endif
 
      // ****************************************************************************
 
      // ****************************************************************************
      // Callbacks from the simulation during add and remove of this constraint *****
      // ****************************************************************************
 
      virtual void addNotification() {}
 
      virtual void removeNotification() {}
 
      // ****************************************************************************
 
      // ****************************************************************************
      // Methods used by non-implementation constraints but not the 'end user' ******
      // ****************************************************************************
 
      AGX_FORCE_INLINE agx::ElementaryConstraint::RefContainer& getElementaryConstraints() { return m_ec; }
      AGX_FORCE_INLINE const agx::ElementaryConstraint::RefContainer& getElementaryConstraints() const { return m_ec; }
 
      AGX_FORCE_INLINE agx::ElementaryConstraint::RefContainer& getSecondaryConstraints() { return m_sc; }
      AGX_FORCE_INLINE const agx::ElementaryConstraint::RefContainer& getSecondaryConstraints() const { return m_sc; }
 
      AGX_FORCE_INLINE ConstraintImplPtrVector& getSubConstraints() { return m_subConstraints; }
      AGX_FORCE_INLINE const ConstraintImplPtrVector& getSubConstraints() const { return m_subConstraints; }
 
      // ****************************************************************************
      // Methods used by 'the user' *************************************************
      // ****************************************************************************
 
      virtual bool rebind() { return false; }
 
      virtual AGX_FORCE_INLINE bool getEnable() const { return m_enable; }
 
      virtual void setEnable(bool enable);
 
      AGX_FORCE_INLINE bool getActive() const { return m_enable && m_localState.Is( VALID ); }
 
      AGX_FORCE_INLINE bool isValid() const { return m_localState.Is( VALID ); }
 
      // ****************************************************************************
 
      // ****************************************************************************
      // Methods for user API, these should not be used internally ******************
      // ****************************************************************************
 
      agx::Real calculateViolationVectorAverageSize();
 
      RealVector calculateCurrentViolation() const;
 
      virtual void setEnableComputeForces( agx::Bool enable );
 
      virtual agx::Bool getEnableComputeForces() const;
 
      virtual agx::Bool getLastForce( agx::UInt bodyIndex, agx::Vec3& retForce, agx::Vec3& retTorque, agx::Bool giveForceAtCm = false ) const;
 
      virtual agx::Bool getLastForce( const agx::RigidBody* rb, agx::Vec3& retForce, agx::Vec3& retTorque, agx::Bool giveForceAtCm = false ) const;
 
      virtual agx::Bool getLastLocalForce( agx::UInt bodyIndex, agx::Vec3& retForce, agx::Vec3& retTorque, agx::Bool giveForceAtCm = false ) const;
 
      virtual agx::Bool getLastLocalForce( const agx::RigidBody* rb, agx::Vec3& retForce, agx::Vec3& retTorque, agx::Bool giveForceAtCm = false ) const;
 
      void setEnableLinearization(bool enable);
 
      bool getEnableLinearization() const;
 
      virtual agx::Attachment* userAPIgetAttachment( const agx::RigidBody* /*rb*/ ) const { return nullptr; }
 
      virtual agx::Attachment* userAPIgetAttachment( agx::UInt /*index*/ ) const { return nullptr; }
 
      virtual agx::AttachmentPair* getAttachmentPair() const { return nullptr; }
 
      virtual bool addElementaryConstraint( const agx::String& name, agx::ElementaryConstraint* elementaryConstraint );
 
      virtual bool addSecondaryConstraint( const agx::String& name, agx::ElementaryConstraint* secondaryConstraint );
 
      virtual bool removeElementaryConstraint( agx::ElementaryConstraint* elementaryConstraint );
 
      virtual bool removeSecondaryConstraint( agx::ElementaryConstraint* secondaryConstraint );
 
      virtual bool removeElementaryConstraint( const agx::String& name );
 
      virtual bool removeSecondaryConstraint( const agx::String& name );
 
      virtual agx::ElementaryConstraint* getElementaryConstraint( const agx::UInt index ) const;
 
      virtual agx::ElementaryConstraint* getElementaryConstraint( const agx::String& name ) const;
 
      virtual agx::ElementaryConstraint* getSecondaryConstraint( const agx::UInt index ) const;
 
      virtual agx::ElementaryConstraint* getSecondaryConstraint( const agx::String& name ) const;
 
      template< typename T >
      T* findSecondaryConstraintGivenType( const agx::UInt count = 0 ) const;
 
      agx::Motor1D* getMotor1D( agx::UInt number = 0 ) const;
 
      agx::Lock1D* getLock1D( agx::UInt number = 0 ) const;
 
      agx::Range1D* getRange1D( agx::UInt number = 0 ) const;
 
      agx::FrictionController* getFrictionController( agx::UInt number = 0 ) const;
 
      virtual agx::Real getAngle( agx::UInt /*dof*/ = 0 ) const { return 0; }
 
      virtual agx::Real getCurrentSpeed( agx::UInt /*dof*/ = 0 ) const { return 0; }
 
      agx::RegularizationParameters* userAPIgetRegularizationParameters( agx::UInt i ) const;
 
      agx::Real userAPIgetCompliance( agx::UInt dof ) const;
 
      void userAPIsetCompliance( agx::Real compliance, int dof );
 
      agx::Real userAPIgetDamping( agx::UInt dof ) const;
 
      void userAPIsetDamping( agx::Real damping, int dof );
 
      void userAPIsetForceRange( agx::RangeReal forceRange, agx::Int dof );
 
      agx::RangeReal userAPIgetForceRange( agx::UInt dof ) const;
 
      agx::Real userAPIgetCurrentForce( agx::UInt dof ) const;
 
      agx::UInt calculateNumActiveRows() const;
 
 
      // **************************************************************************
 
      DOXYGEN_START_INTERNAL_BLOCK()
 
      
      agx::RealValarray getCompliance() const;
 
      agx::Referenced* getInternalData() const;
 
      void setInternalData( agx::Referenced* data );
 
      void clearRigidBodies();
 
      void setRigidBodies( agx::RigidBody* rb1, agx::RigidBody* rb2 = nullptr );
 
      void setRigidBodies( std::initializer_list<agx::RigidBody*> bodies );
 
      void addRigidBody( agx::RigidBody* rb );
 
      AGX_FORCE_INLINE agxSDK::SimulationProxy* getSimulationProxy() { return m_simulationProxy; }
      AGX_FORCE_INLINE const agxSDK::SimulationProxy* getSimulationProxy() const { return m_simulationProxy; }
 
      agx::Physics::ConstraintForcesPtr getForceData();
      void setForceData(agx::Physics::ConstraintForcesPtr forceData);
 
      virtual void storeLightData( agxStream::StorageStream& ) const {}
      virtual void restoreLightData( agxStream::StorageStream& ) {}
 
      DOXYGEN_END_INTERNAL_BLOCK()
 
    protected:
      enum State : Int32
      {
        VALID = 1 << 0,                     // Replaces m_valid.
        IMPACTING = 1 << 1,                 // Replaces m_impactState.
        SUPPORTS_COMPLIANCE_MATRIX = 1 << 2 // Set if this constraint supports write access to the compliance matrix
                                            // allocated in the system matrix.
      };
 
      using LocalState = agx::BitState<State, Int32>;
 
    protected:
      friend class Constraint;
      void setSimulation( agxSDK::Simulation* simulation );
 
      void setSupportsComplianceMatrix( Bool supportsComplianceMatrix );
 
    protected:
      ElementaryConstraint::RefContainer  m_ec;                         
      ElementaryConstraint::RefContainer  m_sc;                         
      ConstraintRigidBodyContainer        m_bodies;                     
      ConstrainedBodiesState              m_constrainedBodiesState;     
      mutable ConstraintImplPtrVector     m_subConstraints;             
      UInt                                m_numRows;                    
      BlockStructure                      m_blockStructure;             
      UInt                                m_blockRowIndex;              
      int                                 m_tag;                        
    private:
      agxSDK::SimulationProxyRef          m_simulationProxy;            
      Physics::ConstraintForcesPtr        m_forceData;
      ref_ptr<Referenced>                 m_internalData;
 
    protected:
      Constraint::SolveType               m_solveType;                  
      bool                                m_enable;                     
      bool                                m_enableLinearization;        
      LocalState                          m_localState;                 
  };
 
  class AGXPHYSICS_EXPORT HighLevelConstraintImplementation : public ConstraintImplementation, public virtual agxStream::Serializable
  {
    public:
      HighLevelConstraintImplementation();
      virtual ~HighLevelConstraintImplementation();
 
      virtual bool updateValid() override;
 
      virtual void prepare() override;
 
      virtual size_t updateJacobian( agx::Jacobian6DOFElement* jacobians ) override;
 
      virtual void postSolveCallback( const agx::Jacobian6DOFElement* jacobians, const agx::Real* solution, agx::Real dt ) override;
 
      virtual bool rebind() override;
 
      virtual void computeForces( const agx::Jacobian6DOFElement* jacobians );
 
      virtual agx::Bool computeForces( const agx::Jacobian6DOFElement* jacobians, agx::Vec3Vector& result ) const;
 
      virtual void setEnableComputeForces( agx::Bool enable ) override;
 
      virtual agx::Bool getEnableComputeForces() const override;
 
      virtual agx::Bool getLastForce( agx::UInt bodyIndex, agx::Vec3& retForce, agx::Vec3& retTorque, agx::Bool giveForceAtCm = false ) const override;
 
      virtual agx::Bool getLastForce( const RigidBody* rb, Vec3& retForce, Vec3& retTorque, agx::Bool giveForceAtCm = false ) const override;
 
      virtual agx::Bool getLastLocalForce( agx::UInt bodyIndex, agx::Vec3& retForce, agx::Vec3& retTorque, agx::Bool giveForceAtCm = false ) const override;
 
      virtual agx::Bool getLastLocalForce( const agx::RigidBody* rb, agx::Vec3& retForce, agx::Vec3& retTorque, agx::Bool giveForceAtCm = false ) const override;
 
      // **************************************************************************
      // Internal utility methods for one or two body constraints *****************
      // **************************************************************************
 
      typedef agx::Bool (*CreateElementaryConstraintsFunction)( HighLevelConstraintImplementation* );
 
      bool construct( RigidBody* rb1, Frame* rb1AttachmentFrame, RigidBody* rb2, Frame* rb2AttachmentFrame, CreateElementaryConstraintsFunction = nullptr );
      bool setupOneOrTwoBodySystem( RigidBodyAttachment* a1, RigidBodyAttachment* a2 );
      bool validateRigidBodiesAndAttachments( RigidBody* rb1, Frame* rb1AttachmentFrame, RigidBody* rb2, Frame* rb2AttachmentFrame );
 
      virtual agx::Attachment* userAPIgetAttachment( const agx::RigidBody* rb ) const override;
 
      virtual agx::Attachment* userAPIgetAttachment( agx::UInt index ) const override;
 
      virtual agx::AttachmentPair* getAttachmentPair() const override;
 
      virtual agx::Real getAngle( agx::UInt dof = 0 ) const override;
 
      bool bodiesValid() const;
 
      template< typename T >
      T* getAttachment( agx::UInt index );
 
      template< typename T >
      const T* getAttachment( agx::UInt index ) const;
 
      AGXSTREAM_DECLARE_ABSTRACT_SERIALIZABLE( agx::HighLevelConstraintImplementation );
 
      virtual void restore( agxStream::InputArchive& in ) override;
      virtual void store( agxStream::OutputArchive& out ) const override;
 
      virtual void storeLightData( agxStream::StorageStream& str ) const override;
      virtual void restoreLightData( agxStream::StorageStream& str ) override;
 
    protected:
      AttachmentPair m_attachmentPair;
      Vec3Vector     m_lastForces;
  };
 
  AGX_FORCE_INLINE UInt& ConstraintImplementation::ConstrainedBodiesState::BodiesInfo::numDynamicBodies()
  {
    return m_numDynamicBodies;
  }
 
  AGX_FORCE_INLINE UInt ConstraintImplementation::ConstrainedBodiesState::BodiesInfo::numDynamicBodies() const
  {
    return m_numDynamicBodies;
  }
 
  AGX_FORCE_INLINE UInt& ConstraintImplementation::ConstrainedBodiesState::BodiesInfo::numNonStaticBodies()
  {
    return m_numNonStaticBodies;
  }
 
  AGX_FORCE_INLINE UInt ConstraintImplementation::ConstrainedBodiesState::BodiesInfo::numNonStaticBodies() const
  {
    return m_numNonStaticBodies;
  }
 
  inline const ConstraintImplementation::ConstrainedBodiesState::BodiesInfo& ConstraintImplementation::ConstrainedBodiesState::getOriginalInfo() const
  {
    return m_originalInfo;
  }
 
  inline const ConstraintImplementation::ConstrainedBodiesState::BodiesInfo& ConstraintImplementation::ConstrainedBodiesState::getSolverInfo() const
  {
    return m_solverInfo;
  }
 
  inline const Int32Vector& ConstraintImplementation::ConstrainedBodiesState::getPermutation() const
  {
    return m_permutation;
  }
 
  inline Int32Vector& ConstraintImplementation::ConstrainedBodiesState::getPermutation()
  {
    return m_permutation;
  }
 
  inline const Int32Vector& ConstraintImplementation::ConstrainedBodiesState::getBodyIndices() const
  {
    return m_bodyIndices;
  }
 
  inline Int32Vector& ConstraintImplementation::ConstrainedBodiesState::getBodyIndices()
  {
    return m_bodyIndices;
  }
 
  inline const ConstraintRigidBodyContainer& ConstraintImplementation::ConstrainedBodiesState::getSolverBodies() const
  {
    return m_bodies;
  }
#if !defined(SWIGPYTHON)
  AGX_FORCE_INLINE ConstraintImplementation::BodyView::BodyView(const ConstrainedBodiesState::BodiesInfo &info, const ConstraintRigidBodyContainer &bodies) : m_info(info), m_bodies(bodies)
  {
  }
 
  AGX_FORCE_INLINE ConstraintImplementation::BodyView::BodyView( const BodyView& other ) : m_info( other.m_info ), m_bodies( other.m_bodies )
  {
  }
 
  AGX_FORCE_INLINE UInt ConstraintImplementation::BodyView::getNumDynamicBodies() const
  {
    return m_info.numDynamicBodies();
  }
 
  AGX_FORCE_INLINE UInt ConstraintImplementation::BodyView::getNumNonStaticBodies() const
  {
    return m_info.numNonStaticBodies();
  }
 
  AGX_FORCE_INLINE UInt ConstraintImplementation::BodyView::getNumBodies() const
  {
    return m_bodies.size();
  }
  AGX_FORCE_INLINE RigidBody* ConstraintImplementation::BodyView::getBodyAt( agx::UInt i )
  {
    agxAssert( i < getNumBodies() ); return m_bodies[ i ];
  }
 
  AGX_FORCE_INLINE const RigidBody* ConstraintImplementation::BodyView::getBodyAt( agx::UInt i ) const
  {
    agxAssert( i < getNumBodies() ); return m_bodies[ i ];
  }
 
  AGX_FORCE_INLINE const ConstraintRigidBodyContainer& ConstraintImplementation::BodyView::getBodies() const
  {
    return m_bodies;
  }
 
  AGX_FORCE_INLINE ConstraintImplementation::SolverBodyView::SolverBodyView( ConstrainedBodiesState& state )
    : BodyView( state.getSolverInfo(), state.getSolverBodies() ), m_state( state )
  {
  }
 
  AGX_FORCE_INLINE ConstraintImplementation::SolverBodyView::SolverBodyView( const SolverBodyView& other )
    : BodyView( other.m_state.getSolverInfo(), other.m_state.getSolverBodies() ), m_state( other.m_state )
  {
  }
 
  AGX_FORCE_INLINE const ConstraintImplementation::ConstrainedBodiesState& ConstraintImplementation::SolverBodyView::getState() const
  {
    return m_state;
  }
 
  AGX_FORCE_INLINE ConstraintImplementation::ConstrainedBodiesState& ConstraintImplementation::SolverBodyView::getState()
  {
    return m_state;
  }
 
  AGX_FORCE_INLINE const RigidBody* ConstraintImplementation::SolverBodyView::getPermutedBodyAt( UInt i ) const
  {
    agxAssert( i < m_state.getPermutation().size() );
    UInt permutedIndex = (UInt)m_state.getPermutation()[ i ];
    agxAssert( permutedIndex < getNumBodies() );
    return getBodyAt( permutedIndex );
  }
 
  AGX_FORCE_INLINE RigidBody* ConstraintImplementation::SolverBodyView::getPermutedBodyAt( UInt i )
  {
    agxAssert( i < m_state.getPermutation().size() );
    UInt permutedIndex = (UInt)m_state.getPermutation()[ i ];
    agxAssert( permutedIndex < getNumBodies() );
    return getBodyAt( permutedIndex );
  }
#endif
  template< typename T >
  inline T* ConstraintImplementation::findSecondaryConstraintGivenType( UInt count /*= 0*/ ) const
  {
    UInt currentNumber = 0;
    for ( UInt i = 0, num = m_sc.size(); i < num; ++i ) {
      T* ec = dynamic_cast< T* >( m_sc[ i ].get() );
      if ( ec != nullptr && currentNumber == count )
        return ec;
      currentNumber += ec != nullptr;
    }
 
    return nullptr;
  }
 
  inline void ConstraintImplementation::preSystemCallback( DynamicsSystem* /*system*/ ) {}
 
  inline void ConstraintImplementation::postSystemCallback( DynamicsSystem* /*system*/ ) {}
 
  inline AttachmentPair* HighLevelConstraintImplementation::getAttachmentPair() const
  {
    return const_cast< AttachmentPair* >( &m_attachmentPair );
  }
 
  template< typename T >
  AGX_FORCE_INLINE T* HighLevelConstraintImplementation::getAttachment( UInt index )
  {
    return m_attachmentPair[ index ] != nullptr ? m_attachmentPair[ index ]->as< T >() : nullptr;
  }
 
  template< typename T >
  AGX_FORCE_INLINE const T* HighLevelConstraintImplementation::getAttachment( UInt index ) const
  {
    return m_attachmentPair[ index ] != nullptr ? m_attachmentPair[ index ]->as< T >() : nullptr;
  }
 
  AGX_FORCE_INLINE bool HighLevelConstraintImplementation::bodiesValid() const
  {
    return m_attachmentPair[ 0 ] != nullptr && !m_attachmentPair[ 0 ]->objectDeleted() &&
           m_attachmentPair[ 1 ] != nullptr && !m_attachmentPair[ 1 ]->objectDeleted();
  }
 
  AGX_FORCE_INLINE Real getAngle( const Vec3& a, const Vec3& b, const Vec3& c )
  {
    return std::atan2( c * b, c * a );
  }
 
  AGXPHYSICS_EXPORT void addSecondaryConstraints1DOF( agx::HighLevelConstraintImplementation* constraint, agx::Angle* angle );
 
  AGXPHYSICS_EXPORT void addSecondaryConstraints2DOF( agx::HighLevelConstraintImplementation* constraint, agx::SeparationAngle* sepAngle, agx::RotationalAngle* rotAngle );
} // namespace agx
 
#ifdef _MSC_VER
# pragma warning(pop)
#endif
 
 
#endif /* _CONSTRAINTIMPLEMENTATION_H_ */