Constraint.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/ElementaryConstraint.h>
#include <agx/Frame.h>
#include <agx/Interaction.h>
#include <agx/SetVector.h>
 
#if defined(_MSC_VER)
#pragma warning( push )
#pragma warning( disable : 4589 ) // Disable: 4589: Constructor of abstract class 'agx::FrictionModel' ignores initializer for virtual base class 'agxStream::Serializable'
#endif
 
 
namespace agxSDK
{
  class Simulation;
}
 
namespace agxRender
{
  class RenderManager;
}
 
namespace agx
{
  // forward declaration
  class Constraint;
  class DynamicsSystem;
  class ConstraintImplementation;
  class DebugRendererCache;
  class Attachment;
  class AttachmentPair;
  class RigidBodyAttachment;
  class RigidBody;
  class RegularizationParameters;
 
  AGX_DECLARE_POINTER_TYPES( Constraint );
  AGX_DECLARE_VECTOR_TYPES( Constraint );
  typedef agx::SetVector<ref_ptr<Constraint> >   ConstraintRefSetVector;
 
 
  class CALLABLE AGXPHYSICS_EXPORT Constraint : public agx::Interaction
  {
    public:
      enum SolveType
      {
        DIRECT = 1,                                 
        ITERATIVE = (1<<2),                         
        DIRECT_AND_ITERATIVE = (DIRECT | ITERATIVE) 
      };
 
    public:
      virtual agx::Bool rebind();
 
      agx::UInt getNumBodies() const;
 
      agx::RigidBody* getBodyAt( agx::UInt i );
 
      const agx::RigidBody* getBodyAt( agx::UInt i ) const;
 
      virtual void setCompliance( agx::Real compliance, agx::Int dof );
 
      void setCompliance( agx::Real compliance );
 
      virtual void setElasticity(agx::Real elasticity, agx::Int dof);
 
      virtual void setElasticity(agx::Real elasticity);
 
 
      virtual agx::Real getCompliance( agx::UInt dof ) const;
 
      virtual agx::Real getElasticity(agx::UInt dof) const;
 
      virtual void setDamping( agx::Real damping, agx::Int dof );
 
      void setDamping( agx::Real damping );
 
      virtual Real getDamping( agx::UInt dof ) const;
 
      void setForceRange( agx::RangeReal forceRange );
 
      void setForceRange( agx::RangeReal forceRange, agx::Int dof );
 
      void setForceRange( agx::Real lower, agx::Real upper, agx::Int dof );
 
      agx::RangeReal getForceRange( agx::UInt dof = 0 ) const;
 
      agx::Real getCurrentForce( agx::UInt dof ) const;
 
      virtual agx::RegularizationParameters* getRegularizationParameters( agx::UInt i );
 
      virtual const agx::RegularizationParameters* getRegularizationParameters( agx::UInt i ) const;
 
      agx::Bool getValid() const;
 
      virtual void render( class agxRender::RenderManager* mgr, float scale ) const = 0;
 
      agx::ConstraintImplementation* getRep();
 
      #ifndef SWIG
      const agx::ConstraintImplementation* getRep() const;
      #endif
 
      agx::UInt32 getEntityId() const;
 
      virtual void setEnable( agx::Bool enable );
 
      agx::Bool getEnable() const;
 
      agx::Bool isEnabled() const;
 
      virtual int getNumDOF() const = 0;
 
      void setSolveType( agx::Constraint::SolveType solveType );
 
      agx::Constraint::SolveType getSolveType() const;
 
      agx::RigidBodyAttachment* getAttachment( const agx::RigidBody* rb ) const;
 
      agx::Attachment* getAttachment( agx::UInt i ) const;
 
      agx::AttachmentPair* getAttachmentPair() const;
 
      void setEnableDebugRendering( agx::Bool enable );
 
      bool getEnableDebugRendering() const;
 
      void setEnableComputeForces( agx::Bool enable );
 
      agx::Bool getEnableComputeForces() const;
 
      agx::Bool getLastForce( agx::UInt bodyIndex, agx::Vec3& retForce, agx::Vec3& retTorque, agx::Bool giveForceAtCm = false ) const;
 
      agx::Bool getLastForce( const agx::RigidBody* rb, agx::Vec3& retForce, agx::Vec3& retTorque, agx::Bool giveForceAtCm = false ) const;
 
      agx::Bool getLastLocalForce( agx::UInt bodyIndex, agx::Vec3& retForce, agx::Vec3& retTorque, agx::Bool giveForceAtCm = false ) const;
 
      agx::Bool getLastLocalForce( const agx::RigidBody* rb, agx::Vec3& retForce, agx::Vec3& retTorque, agx::Bool giveForceAtCm = false ) const;
 
      RealVector calculateCurrentViolation() const;
 
      void setEnableLinearization(bool enable);
 
      bool getEnableLinearization() const;
 
      agx::Bool addElementaryConstraint( const agx::Name& name, agx::ElementaryConstraint* elementaryConstraint );
 
      agx::Bool addSecondaryConstraint( const agx::Name& name, agx::ElementaryConstraint* secondaryConstraint );
 
      agx::Bool removeElementaryConstraint( agx::ElementaryConstraint* elementaryConstraint );
 
      agx::Bool removeSecondaryConstraint( agx::ElementaryConstraint* secondaryConstraint );
 
      agx::Bool removeElementaryConstraint( const agx::Name& name );
 
      agx::Bool removeSecondaryConstraint( const agx::Name& name );
 
      agx::ElementaryConstraint* getElementaryConstraint( const agx::UInt index ) const;
 
      agx::ElementaryConstraint* getElementaryConstraintGivenName( const agx::Name& name ) const;
 
      agx::ElementaryConstraint* getSecondaryConstraint( const agx::UInt index ) const;
 
      agx::ElementaryConstraint* getSecondaryConstraintGivenName( const agx::Name& name ) const;
 
      agx::UInt getNumElementaryConstraints() const;
 
      agx::UInt getNumSecondaryConstraints() const;
 
      agx::UInt calculateNumActiveRows() const;
 
      static agx::Bool calculateFramesFromWorld( agx::Vec3 worldPoint,
                                                 agx::Vec3 worldAxis,
                                                 const agx::RigidBody* rb1,
                                                 agx::Frame* rb1Frame,
                                                 const agx::RigidBody* rb2,
                                                 agx::Frame* rb2Frame );
 
      static agx::Bool calculateFramesFromWorld( agx::Vec3 worldPoint,
                                                 agx::Vec3 worldAxis,
                                                 agx::Vec3 secondWorldAxis,
                                                 const agx::RigidBody* rb1,
                                                 agx::Frame* rb1Frame,
                                                 const agx::RigidBody* rb2,
                                                 agx::Frame* rb2Frame );
 
      static agx::Bool calculateFramesFromBody( agx::Vec3 bodyPoint,
                                                agx::Vec3 bodyAxis,
                                                const agx::RigidBody* body,
                                                agx::Frame* bodyFrame,
                                                const agx::RigidBody* otherBody,
                                                agx::Frame* otherFrame );
 
      static agx::Bool calculateFramesFromBody( agx::Vec3 bodyPoint,
                                                agx::Vec3 bodyAxis,
                                                agx::Vec3 secondBodyAxis,
                                                const agx::RigidBody* body,
                                                agx::Frame* bodyFrame,
                                                const agx::RigidBody* otherBody,
                                                agx::Frame* otherFrame );
 
      template<typename T>
      static agx::ref_ptr<T> createFromWorld( agx::Vec3 worldPoint,
                                              agx::Vec3 worldAxis,
                                              agx::RigidBody* rb1,
                                              agx::RigidBody* rb2 = nullptr );
 
      template<typename T>
      static agx::ref_ptr<T> createFromBody( agx::Vec3 bodyPoint,
                                             agx::Vec3 bodyAxis,
                                             agx::RigidBody* rb1,
                                             agx::RigidBody* rb2 = nullptr );
 
      template<typename T>
      T *as();
 
      template<typename T>
      const T *as() const;
 
      AGXSTREAM_DECLARE_ABSTRACT_SERIALIZABLE( agx::Constraint );
 
      void store( agxStream::OutputArchive& out ) const override;
      void restore( agxStream::InputArchive& in ) override;
 
      virtual void storeLightData( agxStream::StorageStream& str ) const override;
      virtual void restoreLightData( agxStream::StorageStream& str ) override;
 
    protected:
      Constraint();
      virtual ~Constraint();
 
      friend class DynamicsSystem;
      virtual void setSystem( agx::DynamicsSystem* ) {}
 
      friend class agxSDK::Simulation;
      virtual void setSimulation( agxSDK::Simulation* simulation ) override;
      /*
      Called when this Constraint is added to a simulation
      (given that it not already is in the simulation).
      */
      virtual void addNotification() override;
      virtual void removeNotification() override;
 
#ifndef SWIG
      virtual void preSystemCallback( agx::DynamicsSystem* dynamicsSystem ) override;
      virtual void postSystemCallback( agx::DynamicsSystem* dynamicsSystem ) override;
#endif
 
      void setRep( agx::ConstraintImplementation* _rep );
 
    protected:
      agx::ConstraintImplementation* m_implementation;
 
    private:
      DOXYGEN_START_INTERNAL_BLOCK()
      friend class InternalData;
      agx::Referenced* getInternalData() const;
 
      void setInternalData( agx::Referenced* data );
      DOXYGEN_END_INTERNAL_BLOCK()
 
    private:
      agx::Bool m_debugRenderingEnable;
  };
 
  typedef agx::TargetSpeedController Motor1D;
  typedef agx::LockController        Lock1D;
  typedef agx::RangeController       Range1D;
  typedef agx::ScrewController       Screw1D;
 
  AGX_DECLARE_POINTER_TYPES(Constraint1DOF);
  AGX_DECLARE_VECTOR_TYPES(Constraint1DOF);
 
  class CALLABLE AGXPHYSICS_EXPORT Constraint1DOF : public agx::Constraint
  {
    public:
      static agx::Constraint1DOF* safeCast( const agx::Constraint* constraint );
 
    public:
      agx::Lock1D* getLock1D();
 
      const agx::Lock1D* getLock1D() const;
 
      agx::Motor1D* getMotor1D();
 
      const agx::Motor1D* getMotor1D() const;
 
      agx::ElectricMotorController* getElectricMotorController();
 
      const agx::ElectricMotorController* getElectricMotorController() const;
 
      agx::Range1D* getRange1D();
 
      const agx::Range1D* getRange1D() const;
 
      agx::FrictionController* getFrictionController();
 
      const agx::FrictionController* getFrictionController() const;
 
      agx::Real getAngle() const;
 
      agx::Real getCurrentSpeed() const;
 
    protected:
      Constraint1DOF();
      virtual ~Constraint1DOF();
  };
 
 
 
  AGX_DECLARE_POINTER_TYPES(Constraint2DOF);
 
  class CALLABLE AGXPHYSICS_EXPORT Constraint2DOF : public agx::Constraint
  {
    public:
      static agx::Constraint2DOF* safeCast( const agx::Constraint* constraint );
 
    public:
      enum DOF
      {
        FIRST = 0,
        SECOND
      };
 
      agx::Lock1D* getLock1D( agx::Constraint2DOF::DOF dof );
 
      const agx::Lock1D* getLock1D( agx::Constraint2DOF::DOF dof ) const;
 
      agx::Motor1D* getMotor1D( agx::Constraint2DOF::DOF dof );
 
      const agx::Motor1D* getMotor1D( agx::Constraint2DOF::DOF dof ) const;
 
      agx::ElectricMotorController* getElectricMotorController( agx::Constraint2DOF::DOF dof );
 
      const agx::ElectricMotorController* getElectricMotorController( agx::Constraint2DOF::DOF dof ) const;
 
      agx::Range1D* getRange1D( agx::Constraint2DOF::DOF dof );
 
      const agx::Range1D* getRange1D( agx::Constraint2DOF::DOF dof ) const;
 
      agx::FrictionController* getFrictionController( agx::Constraint2DOF::DOF dof );
 
      const agx::FrictionController* getFrictionController( agx::Constraint2DOF::DOF dof ) const;
 
      agx::Screw1D* getScrew1D();
 
      const agx::Screw1D* getScrew1D() const;
 
      agx::Real getAngle( agx::Constraint2DOF::DOF dof ) const;
 
      agx::Real getCurrentSpeed( agx::Constraint2DOF::DOF dof ) const;
 
    protected:
      Constraint2DOF();
      virtual ~Constraint2DOF();
  };
 
  DOXYGEN_START_INTERNAL_BLOCK()
  class AGXPHYSICS_EXPORT ConstraintFrame
  {
    public:
      ConstraintFrame();
      virtual ~ConstraintFrame();
 
      Vec3& operator [] ( const unsigned int& i );
      const Vec3& operator [] ( const unsigned int& i ) const;
 
      void setTranslate( const Vec3& translate );
      const Vec3& getTranslate() const;
 
    protected:
      Vec3 m_data[ 4 ];
  };
  DOXYGEN_END_INTERNAL_BLOCK()
 
  inline ConstraintImplementation* Constraint::getRep()
  {
    return m_implementation;
  }
 
  inline const ConstraintImplementation* Constraint::getRep() const
  {
    return m_implementation;
  }
 
  template<typename T>
  ref_ptr<T> Constraint::createFromWorld( Vec3 worldPoint, Vec3 worldAxis, RigidBody* rb1, RigidBody* rb2 /* = nullptr */ )
  {
    FrameRef f1 = new Frame();
    FrameRef f2 = new Frame();
    Constraint::calculateFramesFromWorld( worldPoint, worldAxis, rb1, f1, rb2, f2 );
    ref_ptr<T> constraint = new T( rb1, f1, rb2, f2 );
    return constraint;
  }
 
  template<typename T>
  ref_ptr<T> Constraint::createFromBody( Vec3 bodyPoint, Vec3 bodyAxis, RigidBody* rb1, RigidBody* rb2 /* = nullptr */ )
  {
    FrameRef f1 = new Frame();
    FrameRef f2 = new Frame();
    Constraint::calculateFramesFromBody( bodyPoint, bodyAxis, rb1, f1, rb2, f2 );
    ref_ptr<T> constraint = new T( rb1, f1, rb2, f2 );
    return constraint;
  }
 
  template<typename T>
  T *Constraint::as()
  {
    return dynamic_cast<T*>(this);
  }
 
  template<typename T>
  const T *Constraint::as() const
  {
    return dynamic_cast<const T*>(this);
  }
 
} //namespace agx
 
#if defined(_MSC_VER)
#pragma warning( pop )
#endif