AddedMassInteraction.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/StrongInteraction.h>
#include <agx/agx_valarray_types.h>
 
namespace agx
{
  AGX_DECLARE_POINTER_TYPES( AddedMassInteraction );
 
  class AGXPHYSICS_EXPORT AddedMassInteraction : public agx::StrongInteraction
  {
    public:
      class AGXPHYSICS_EXPORT Matrix6x6
      {
        public:
          Matrix6x6();
 
          Matrix6x6( const Matrix6x6& other );
 
          Matrix6x6& operator=( const Matrix6x6& other );
 
          const agx::Real* operator[]( size_t i ) const;
 
          agx::Real* operator[]( size_t i );
 
          const agx::Real& at( size_t i, size_t j ) const;
 
          agx::Real operator()(size_t i, size_t j) const;
 
          agx::Real& operator()(size_t i, size_t j);
 
 
          void set( size_t i, size_t j, agx::Real value );
 
 
          agx::Real& at( size_t i, size_t j );
 
          Matrix6x6 transposed() const;
 
          agx::Matrix3x3 block( size_t i, size_t j ) const;
 
          void block( size_t i, size_t j, const agx::Matrix3x3& matrix );
 
          Matrix6x6 transform( const agx::Matrix3x3& left, const agx::Matrix3x3& right ) const;
 
          void multiply( const agx::Vec3 v1, const agx::Vec3 v2, agx::Real* result ) const;
 
          void reset();
 
        public:
          void store( agxStream::OutputArchive& out ) const;
 
          void restore( agxStream::InputArchive& in );
 
        private:
          agx::Real m_data[ 6 ][ 6 ];
      };
 
 
      
 
      class AGXPHYSICS_EXPORT RigidBodyStorage
      {
        public:
          RigidBodyStorage( agx::RigidBody* rb );
 
          agx::RigidBody* getRigidBody() const;
 
          agx::Physics::RigidBodyPtr getRigidBodyEntity() const;
 
          agx::Bool getValid() const;
 
          const agx::AddedMassInteraction::Matrix6x6& getBlock() const;
 
          agx::AddedMassInteraction::Matrix6x6& getBlock();
 
        public:
          void store( agxStream::OutputArchive& out ) const;
 
          void restore( agxStream::InputArchive& in );
 
        private:
          agx::RigidBodyObserver m_rb;
          agx::Bool m_hadValidRb;
          agx::AddedMassInteraction::Matrix6x6 m_block;
      };
 
    public:
      AddedMassInteraction( agx::RigidBody* rb1, agx::RigidBody* rb2 = nullptr );
 
      const agx::AddedMassInteraction::RigidBodyStorage* getRigidBody1Storage() const;
 
      agx::AddedMassInteraction::RigidBodyStorage* getRigidBody1Storage();
 
      const agx::AddedMassInteraction::RigidBodyStorage* getRigidBody2Storage() const;
 
      agx::AddedMassInteraction::RigidBodyStorage* getRigidBody2Storage();
 
      const agx::AddedMassInteraction::Matrix6x6& getOffDiagonalBlock() const;
 
      agx::AddedMassInteraction::Matrix6x6& getOffDiagonalBlock();
 
      void translateWorldDiagonalBlock( const agx::Vec3 cmOffset,
                                        agx::AddedMassInteraction::Matrix6x6& matrix ) const;
 
      void translateWorldDiagonalBlock( const agx::Vec3 cmOffsetRb1,
                                        const agx::Vec3 cmOffsetRb2,
                                        agx::AddedMassInteraction::Matrix6x6& matrix ) const;
 
    public:
      virtual agx::RigidBody* getRigidBody1() const override;
 
      virtual agx::RigidBody* getRigidBody2() const override;
 
      AGXSTREAM_DECLARE_SERIALIZABLE( agx::AddedMassInteraction );
 
    protected:
 
      AddedMassInteraction();
 
      virtual ~AddedMassInteraction();
 
      virtual void prepare() override;
 
      virtual void writeMatrixData( agx::StrongInteraction::MatrixData rb1DiagonalBlock,
                                    agx::StrongInteraction::MatrixData rb2DiagonalBlock,
                                    agx::StrongInteraction::MatrixData offDiagonalBlock ) override;
 
      virtual void writeRhs( agx::Physics::RigidBodyPtr rb1,
                             agx::Real* rb1Rhs,
                             agx::Physics::RigidBodyPtr rb2,
                             agx::Real* rb2Rhs,
                             agx::Bool isRestingSolve ) override;
 
    private:
      struct Transformed
      {
        AddedMassInteraction::Matrix6x6 rb1DiagonalBlock;
        AddedMassInteraction::Matrix6x6 rb2DiagonalBlock;
        AddedMassInteraction::Matrix6x6 offDiagonalBlock;
 
        void reset()
        {
          rb1DiagonalBlock.reset();
          rb2DiagonalBlock.reset();
          offDiagonalBlock.reset();
        }
      };
 
    private:
      agx::AddedMassInteraction::RigidBodyStorage m_rb1Storage;
      agx::AddedMassInteraction::RigidBodyStorage m_rb2Storage;
      agx::AddedMassInteraction::Matrix6x6        m_offDiagonalBlock;
      Transformed                                 m_transformed;
  };
 
 
 
 
 
  AGXPHYSICS_EXPORT agx::Bool equivalent( const agx::AddedMassInteraction::Matrix6x6& lhs, const agx::AddedMassInteraction::Matrix6x6& rhs, agx::Real eps = agx::RealEpsilon );
  AGXPHYSICS_EXPORT agx::Bool equalsZero( const agx::AddedMassInteraction::Matrix6x6& matrix, agx::Real eps = agx::RealEpsilon );
 
  AGX_FORCE_INLINE const Real* AddedMassInteraction::Matrix6x6::operator[]( size_t i ) const
  {
    agxAssert( i < 6 );
    return m_data[ i ];
  }
 
  AGX_FORCE_INLINE Real* AddedMassInteraction::Matrix6x6::operator[]( size_t i )
  {
    agxAssert( i < 6 );
    return m_data[ i ];
  }
 
  AGX_FORCE_INLINE Real AddedMassInteraction::Matrix6x6::operator()(size_t i, size_t j) const
  {
    agxAssert(j < 6);
    return (*this)[i][j];
  }
 
  AGX_FORCE_INLINE Real& AddedMassInteraction::Matrix6x6::operator()(size_t i, size_t j)
  {
    agxAssert(j < 6);
    return (*this)[i][j];
  }
 
  AGX_FORCE_INLINE const Real& AddedMassInteraction::Matrix6x6::at(size_t i, size_t j ) const
  {
    agxAssert( j < 6 );
    return (*this)[ i ][ j ];
  }
 
  AGX_FORCE_INLINE void AddedMassInteraction::Matrix6x6::set( size_t i, size_t j, agx::Real value )
  {
    agxAssert( i < 6 );
    agxAssert( j < 6 );
    (*this)[ i ][ j ] = value;
  }
 
 
  AGX_FORCE_INLINE Real& AddedMassInteraction::Matrix6x6::at( size_t i, size_t j )
  {
    agxAssert( j < 6 );
    return (*this)[ i ][ j ];
  }
 
  AGX_FORCE_INLINE const AddedMassInteraction::Matrix6x6& AddedMassInteraction::RigidBodyStorage::getBlock() const
  {
    return m_block;
  }
 
  AGX_FORCE_INLINE AddedMassInteraction::Matrix6x6& AddedMassInteraction::RigidBodyStorage::getBlock()
  {
    return m_block;
  }
 
  AGX_FORCE_INLINE const AddedMassInteraction::Matrix6x6& AddedMassInteraction::getOffDiagonalBlock() const
  {
    return m_offDiagonalBlock;
  }
 
  AGX_FORCE_INLINE AddedMassInteraction::Matrix6x6& AddedMassInteraction::getOffDiagonalBlock()
  {
    return m_offDiagonalBlock;
  }
 
  inline std::ostream& operator<< (std::ostream& os, const agx::AddedMassInteraction::Matrix6x6& m)
  {
    os << "{" << std::endl;
    for (size_t row = 0; row < 6; ++row) {
      os << "\t";
      for (size_t col = 0; col < 6; ++col)
        os << m(row, col) << " ";
      os << std::endl;
    }
    os << "}" << std::endl;
    return os;
  }
 
}