RigidBody.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_RIGIDBODY_H
#define AGX_RIGIDBODY_H
 
#include <agx/agx.h>
#include <agx/Frame.h>
#include <agx/MassProperties.h>
#include <agx/BitArray.h>
 
#include <agx/Physics/RigidBodyEntity.h>
 
#include <agxCollide/Geometry.h>
 
#include <agx/RigidBodyState.h>
#include <agx/SetVector.h>
 
#include <agx/macros.h>
 
#ifdef _MSC_VER
#  pragma warning(push)
#  pragma warning(disable:4355) //Disable 'this' : used in base member initializer list warning
#endif
 
namespace agx
{
  class DynamicsSystem;
  class InternalData;
 
  AGX_DECLARE_POINTER_TYPES( RigidBody );
  AGX_DECLARE_VECTOR_TYPES( RigidBody );
  typedef agx::SetVector<ref_ptr<RigidBody> >    RigidBodyRefSetVector;
 
  class CALLABLE AGXPHYSICS_EXPORT RigidBody : public agx::Referenced, public virtual agxStream::Serializable
  {
    public:
      enum MotionControl
      {
        STATIC     = 1, 
        KINEMATICS = 2, 
        DYNAMICS   = 3  
      };
 
    public:
      RigidBody( const agx::Name& name = "" );
 
      RigidBody( agxCollide::Geometry* geometry );
 
      RigidBody *clone( bool shallowCopyRenderData=false ) const;
 
      agx::Frame* getFrame();
 
      const agx::Frame* getFrame() const;
 
      void setTransform( const agx::AffineMatrix4x4& matrix );
 
      const agx::AffineMatrix4x4& getLocalTransform() const;
 
      void setLocalTransform( const agx::AffineMatrix4x4& matrix );
 
      agx::Vec3 getLocalPosition() const;
 
      void setLocalPosition( const agx::Vec3& p );
 
      void setLocalPosition( agx::Real x, agx::Real y, agx::Real z );
 
      void setRotation( const agx::Quat& q );
 
      void setRotation( const agx::EulerAngles& e);
 
      void setRotation( const agx::OrthoMatrix3x3& m );
 
      agx::Quat getLocalRotation() const;
 
      void setLocalRotation( const agx::Quat& q );
 
      void setLocalRotation( const agx::EulerAngles& e );
 
      agx::Frame* getParentFrame();
 
      const agx::Frame* getParentFrame() const;
 
      bool setParentFrame( agx::Frame* frame );
 
      void setPosition( const agx::Vec3& p );
 
      void setPosition( agx::Real x, agx::Real y, agx::Real z );
 
      agx::Frame* getCmFrame();
 
      const agx::Frame* getCmFrame() const;
 
      const agx::AffineMatrix4x4& getTransform() const;
 
      CALLABLE_UNIT("m/s")
      agx::Vec3 getPosition() const;
 
      agx::Quat getRotation() const;
 
      void setVelocity( const agx::Vec3& velocity );
 
      void setVelocity( agx::Real vx, agx::Real vy, agx::Real vz );
 
      void setAngularVelocity( const agx::Vec3& angularVelocity );
 
      void setAngularVelocity( agx::Real vx, agx::Real vy, agx::Real vz );
 
      agx::Vec3 getVelocity() const;
 
      agx::Vec3 getAngularVelocity() const;
 
      agx::Vec3 getModelVelocity( const agx::Vec3& relPos = agx::Vec3() ) const;
 
      agx::Vec3 getModelAcceleration( const agx::Vec3& relPos = agx::Vec3() ) const;
 
      void setEnable( bool enable );
 
      bool getEnable() const;
 
      bool isEnabled() const;
 
      agx::RigidBody::MotionControl getMotionControl() const;
 
      void setMotionControl( agx::RigidBody::MotionControl control );
 
      agx::DynamicsSystem* getSystem();
 
      const agx::DynamicsSystem* getSystem() const;
 
      agx::Vec3 getLastForce() const;
 
      agx::Vec3 getLastTorque() const;
 
      agx::PropertyContainer* getPropertyContainer() const;
 
      bool hasPropertyContainer() const;
 
      void setPropertyContainer( agx::PropertyContainer* container );
 
      const agx::Name& getName() const;
 
      void setName( const agx::Name& name );
 
      agx::UInt32 getId() const;
 
      const agx::AffineMatrix4x4& getCmTransform() const;
 
      void setCmTransform( const agx::AffineMatrix4x4& matrix, bool synchronizeModel = true );
 
      agx::Vec3 getCmLocalTranslate() const;
 
      void setCmLocalTranslate(const agx::Vec3& translate);
 
      agx::Vec3 getCmPosition() const;
 
      void setCmPosition( const agx::Vec3& p, bool synchronizeModel = true );
 
      agx::Quat getCmRotation() const;
 
      void setCmRotation( const agx::Quat& q);
 
      void setCmRotation( const agx::EulerAngles& e);
 
      void setCmRotation( const agx::OrthoMatrix3x3& m);
 
      void setInertiaTensor( const agx::SPDMatrix3x3& inertiaTensor, const agx::AffineMatrix4x4& cmLocalTransform = agx::AffineMatrix4x4(), bool autogenerateCmOffset = false, bool autogenerateInertia = false );
 
      void setInertiaTensor( const agx::Vec3& inertiaDiagonal, const agx::AffineMatrix4x4& cmLocalTransform = agx::AffineMatrix4x4(), bool autogenerateCmOffset = false, bool autogenerateInertia = false );
 
      bool addAttachment( agx::Frame* frame, const agx::String& name );
 
      bool removeAttachment( const agx::String& name );
 
      Frame* getAttachment( const agx::String& name );
 
      const agx::MassProperties* getMassProperties() const;
 
      agx::MassProperties* getMassProperties();
 
      void setForce( const agx::Vec3& force );
 
      void setForce( agx::Real fx, agx::Real fy, agx::Real fz );
 
      void setTorque( const agx::Vec3& torque );
 
      void setTorque( agx::Real tx, agx::Real ty, agx::Real tz );
 
      void addForce( const agx::Vec3& force );
 
      void addForce( agx::Real fx, agx::Real fy, agx::Real fz );
 
      void addForceAtPosition( const agx::Vec3& force, const agx::Vec3& position );
 
      void addForceAtPosition( agx::Real fx, agx::Real fy, agx::Real fz, agx::Real px, agx::Real py, agx::Real pz );
 
      void addForceAtLocalPosition( const agx::Vec3& force, const agx::Vec3& position );
 
      void addForceAtLocalPosition( agx::Real fx, agx::Real fy, agx::Real fz, agx::Real px, agx::Real py, agx::Real pz );
 
      void addForceAtLocalCmPosition( const agx::Vec3& force, const agx::Vec3& position );
 
      void addForceAtLocalCmPosition( agx::Real fx, agx::Real fy, agx::Real fz, agx::Real px, agx::Real py, agx::Real pz );
 
      void addTorque( const agx::Vec3& torque );
 
      void addTorque( agx::Real tx, agx::Real ty, agx::Real tz );
 
      void addLocalTorque( agx::Real tx, agx::Real ty, agx::Real tz );
 
      void addLocalTorque( const agx::Vec3& torque );
 
      bool add( agxCollide::Geometry* geometry , const agx::AffineMatrix4x4& localTransform, bool incrementalMassCalculation = false );
 
      bool add( agxCollide::Geometry* geometry, bool incrementalMassCalculation = false );
 
      bool remove( agxCollide::Geometry* geometry, bool incrementalMassCalculation = false );
 
      const agxCollide::Geometry* getGeometry(const agx::Name& name) const;
 
      agxCollide::Geometry* getGeometry(const agx::Name& name);
 
      const agxCollide::GeometryRefVector& getGeometries() const;
 
      void moveTo( const agx::AffineMatrix4x4& target, agx::Real t );
 
      void moveTo( const agx::Vec3& targetPosition, const agx::Quat& targetRotation, agx::Real t );
 
      void updateMassProperties();
 
      void updateMassProperties( agx::UInt32 mask );
 
      agx::Real calculateMass() const;
 
      agx::Vec3 calculateCenterOfMassOffset( agx::Real mass = agx::Real() ) const;
 
      void setLinearVelocityZeroDamping( const agx::Vec3f& dir );
 
      void setAngularVelocityZeroDamping( const agx::Vec3f& dir );
 
      agx::Vec3f getLinearVelocityZeroDamping() const;
 
      agx::Vec3f getAngularVelocityZeroDamping() const;
 
      void setLinearVelocityDamping( float damping );
 
      void setLinearVelocityDamping( const agx::Vec3f& damping );
 
      void setAngularVelocityDamping( float damping );
 
      void setAngularVelocityDamping( const agx::Vec3f& damping );
 
      void setVelocityDamping( float damping );
 
      agx::Vec3f getLinearVelocityDamping() const;
 
      agx::Vec3f getAngularVelocityDamping() const;
 
      agx::Vec3 getLinearMomentum() const;
 
      agx::Vec3 getAngularMomentum() const;
 
      void setHandleAsParticle( bool handleAsParticle );
 
      bool getHandleAsParticle() const;
 
      agx::Vec3 getAcceleration() const;
 
      agx::Vec3 getAngularAcceleration() const;
 
      bool hasConstraints() const;
 
      agx::UInt16 getNumConstraints() const;
 
      agx::Referenced* getCustomData() const;
 
      void setCustomData( agx::Referenced* data );
 
      agx::Physics::RigidBodyPtr getEntity() const;
 
      void setEntity( agx::Physics::RigidBodyPtr entity );
 
      void transfer( agxData::EntityStorage* storage );
 
      agx::Vec3 getForce() const;
 
      agx::Vec3 getTorque() const;
 
      bool isPowerlineBody() const;
 
      void setIsPowerlineBody(bool isHydraulicBody);
 
#ifndef SWIG
      inline static bool shouldIgnoreGravity(agx::RigidBodyState state)
      {
        return !state.enabled() ||
          state.motionControl() != agx::RigidBody::DYNAMICS ||
          state.powerline();
      }
#endif
 
      
 
      AGXSTREAM_DECLARE_SERIALIZABLE( agx::RigidBody );
 
      bool hasValidEntityIndex() const;
 
    protected:
      friend class DynamicsSystem;
      friend class GeometryListener;
      friend class ConstraintImplementation;
 
      virtual ~RigidBody();
 
      // Never use these
      RigidBody( const agx::RigidBody& );
      RigidBody( const agx::Name& name, agx::Physics::RigidBodyPtr entity );
      RigidBody& operator = ( const agx::RigidBody& ) { return *this; }
 
      void setId( agx::UInt32 id );
 
      void setSystem( agx::DynamicsSystem* system );
 
      void incrementalMassPropertyUpdate( agx::UInt32 mask, int sign, const agxCollide::Geometry* geometry );
 
    private:
      void init( const agx::Name& name );
 
      void incrementNumConstraints();
      void decrementNumConstraints();
      void resetNumConstraints();
 
      void removeAllGeometries();
      void appendGeometry( agxCollide::Geometry* geometry );
      void removeGeometry( agx::Physics::GeometryPtr before, agx::Physics::GeometryPtr toBeRemoved );
 
      DOXYGEN_START_INTERNAL_BLOCK()
      friend class InternalData;
      agx::Referenced* getInternalData() const;
 
      void setInternalData( agx::Referenced* data );
      DOXYGEN_END_INTERNAL_BLOCK()
 
    private:
      // Low level data.
      Physics::RigidBodyRef m_entity;
 
      mutable PropertyContainerRef m_propertyContainer;
 
      DynamicsSystem* m_mainSystem;
 
      FrameRef m_frame;
      FrameRef m_cmFrame;
 
      MassProperties m_massProperties;
 
      agxCollide::GeometryRefVector m_geometries;
 
      typedef HashTable<agx::String, FrameRef > FrameTable;
      FrameTable m_namedAttachments;
 
      agx::ref_ptr<agx::Referenced> m_customData;
      agx::ref_ptr<agx::Referenced> m_internalData;
  };
 
  AGX_FORCE_INLINE bool RigidBody::getEnable() const
  {
    return m_entity.state().enabled();
  }
 
  AGX_FORCE_INLINE bool RigidBody::isEnabled() const
  {
    return m_entity.state().enabled();
  }
 
  AGX_FORCE_INLINE RigidBody::MotionControl RigidBody::getMotionControl() const
  {
    return (MotionControl)m_entity.state().motionControl();
  }
 
  AGX_FORCE_INLINE bool RigidBody::hasPropertyContainer() const
  {
    return ( m_propertyContainer.isValid() );
  }
 
  AGX_FORCE_INLINE agx::PropertyContainer* RigidBody::getPropertyContainer() const
  {
    if ( m_propertyContainer == nullptr )
      m_propertyContainer = new agx::PropertyContainer;
    return m_propertyContainer;
  }
 
  inline Physics::RigidBodyPtr RigidBody::getEntity() const
  {
    return m_entity;
  }
 
  AGX_FORCE_INLINE UInt32 RigidBody::getId() const
  {
    return m_entity.id();
  }
 
  AGX_FORCE_INLINE Frame* RigidBody::getFrame()
  {
    return m_frame.get();
  }
 
  AGX_FORCE_INLINE const Frame* RigidBody::getFrame() const
  {
    return m_frame.get();
  }
 
  AGX_FORCE_INLINE Frame* RigidBody::getCmFrame()
  {
    return m_cmFrame.get();
  }
 
  AGX_FORCE_INLINE const Frame* RigidBody::getCmFrame() const
  {
    return m_cmFrame.get();
  }
 
  AGX_FORCE_INLINE const AffineMatrix4x4& RigidBody::getCmTransform() const
  {
    return m_entity.cmTransform();
  }
 
  AGX_FORCE_INLINE Vec3 RigidBody::getVelocity() const
  {
    return m_entity.velocity();
  }
 
  AGX_FORCE_INLINE Vec3 RigidBody::getAngularVelocity() const
  {
    return m_entity.angularVelocity();
  }
 
  AGX_FORCE_INLINE Vec3 RigidBody::getCmLocalTranslate() const
  {
    return getCmFrame()->getLocalTranslate();
  }
 
  AGX_FORCE_INLINE Vec3 RigidBody::getCmPosition() const
  {
    return getCmTransform().getTranslate();
  }
 
  AGX_FORCE_INLINE Quat RigidBody::getCmRotation() const
  {
    return getCmTransform().getRotate();
  }
 
  AGX_FORCE_INLINE MassProperties* RigidBody::getMassProperties()
  {
    return &m_massProperties;
  }
 
  AGX_FORCE_INLINE const MassProperties* RigidBody::getMassProperties() const
  {
    return &m_massProperties;
  }
 
  AGX_FORCE_INLINE bool RigidBody::getHandleAsParticle() const
  {
    return m_entity.state().handleAsParticle();
  }
 
  AGX_FORCE_INLINE Vec3 RigidBody::getAcceleration() const
  {
    return m_entity.linearAcceleration();
  }
 
  AGX_FORCE_INLINE Vec3 RigidBody::getAngularAcceleration() const
  {
    return m_entity.angularAcceleration();
  }
 
  AGX_FORCE_INLINE bool RigidBody::hasConstraints() const
  {
    return m_entity.numConstraints() > 0;
  }
 
  AGX_FORCE_INLINE UInt16 RigidBody::getNumConstraints() const
  {
    return m_entity.numConstraints();
  }
 
  AGX_FORCE_INLINE Referenced* RigidBody::getCustomData() const
  {
    return m_customData.get();
  }
 
  AGX_FORCE_INLINE const agx::AffineMatrix4x4& RigidBody::getTransform() const
  {
    return m_entity.modelTransform();
  }
 
  AGX_FORCE_INLINE agx::Vec3 RigidBody::getPosition() const
  {
    return m_entity.modelTransform().getTranslate();
  }
 
  AGX_FORCE_INLINE agx::Quat RigidBody::getRotation() const
  {
    return m_entity.modelTransform().getRotate();
  }
 
  inline Referenced* RigidBody::getInternalData() const
  {
    return m_internalData;
  }
 
  inline void RigidBody::setVelocity( agx::Real vx, agx::Real vy, agx::Real vz )
  {
    setVelocity(agx::Vec3(vx,vy,vz));
  }
 
  inline void RigidBody::setAngularVelocity( agx::Real vx, agx::Real vy, agx::Real vz )
  {
    setAngularVelocity(agx::Vec3(vx,vy,vz));
  }
 
  inline void RigidBody::addForce( agx::Real fx, agx::Real fy, agx::Real fz )
  {
    addForce( agx::Vec3(fx,fy,fz) );
  }
 
  inline void RigidBody::addForceAtPosition( agx::Real fx, agx::Real fy, agx::Real fz, agx::Real px, agx::Real py, agx::Real pz )
  {
    addForceAtPosition( agx::Vec3(fx,fy,fz), agx::Vec3(px,py,pz) );
  }
 
  inline void RigidBody::addForceAtLocalPosition( agx::Real fx, agx::Real fy, agx::Real fz, agx::Real px, agx::Real py, agx::Real pz )
  {
    addForceAtLocalPosition( agx::Vec3(fx,fy,fz), agx::Vec3(px,py,pz) );
  }
 
  inline void RigidBody::addForceAtLocalCmPosition( agx::Real fx, agx::Real fy, agx::Real fz, agx::Real px, agx::Real py, agx::Real pz )
  {
    addForceAtLocalCmPosition( agx::Vec3(fx,fy,fz), agx::Vec3(px,py,pz) );
  }
 
  inline void RigidBody::addTorque( agx::Real tx, agx::Real ty, agx::Real tz )
  {
    addTorque( agx::Vec3(tx,ty,tz) );
  }
 
  inline void RigidBody::addLocalTorque( agx::Real tx, agx::Real ty, agx::Real tz )
  {
    addLocalTorque( agx::Vec3(tx, ty, tz) );
  }
 
  inline void RigidBody::setForce( agx::Real fx, agx::Real fy, agx::Real fz )
  {
    setForce( agx::Vec3(fx,fy,fz) );
  }
 
  inline void RigidBody::setTorque( agx::Real tx, agx::Real ty, agx::Real tz )
  {
    setTorque( agx::Vec3(tx,ty,tz) );
  }
 
  inline bool RigidBody::isPowerlineBody() const
  {
    return m_entity.state().powerline();
  }
 
  inline void RigidBody::setIsPowerlineBody(bool isHydraulicBody)
  {
    m_entity.state().setPowerline(isHydraulicBody);
  }
 
} // namespace agx
 
#ifdef _MSC_VER
# pragma warning(pop)
#endif
 
#endif