SoilParticleAggregate.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 <agxTerrain/PrimaryActiveZone.h>
#include <agxTerrain/SoilSimulationInterface.h>
#include <agx/RigidBody.h>
#include <agx/LockJoint.h>
#include <agx/Plane.h>
 
namespace agxTerrain
{
  class Shovel;
  class ActiveZone;
  class TerrainMaterial;
  class AggregateContactGenerator;
  class ExternalDeformer;
 
  typedef agx::Physics::GranularBodyPtr SoilParticlePtr;
  typedef agx::Physics::GranularBodyPtrVector SoilParticlePtrVector;
  typedef agx::HashSet<agx::Vec3i> VoxelHashSet;
 
  AGX_DECLARE_POINTER_TYPES( SoilParticleAggregate );
  AGX_DECLARE_VECTOR_TYPES(SoilParticleAggregate);
 
  class AGXTERRAIN_EXPORT SoilParticleAggregate : public agx::Referenced
  {
    public:
      SoilParticleAggregate( agx::RigidBody* connectedBody = nullptr,
                             bool isDeformerAggregate = false,
                             const agx::Name& tag = "" );
 
      agx::RigidBody* getInnerBody() const;
 
      const agx::RigidBody* getContactingWedgeBody() const;
 
      agx::RigidBodyPtrVector getWedgeBodies(bool onlyEnabled=true) const;
 
      agx::ConstraintPtrVector getWedgeLockJoints(bool onlyEnabled=true) const;
 
      const agxCollide::Geometry* getInnerGeometry() const;
 
      const agxCollide::Geometry* getContactingWedgeGeometry() const;
 
      const agx::LockJoint* getInnerWedgeLockJoint() const;
 
      void setMaterial( agx::Material* material );
 
      agx::Material* getMaterial() const;
 
      agx::Real getMass() const;
 
      const SoilParticlePtrVector& getInnerBodyParticles() const;
 
      const SoilParticlePtrVector& getWedgeBodyParticles() const;
 
      agx::Vec3iVector getWedgeBodyVoxels() const;
 
      agx::Real getTotalWedgeBodyMass() const;
 
      agx::Real getTotalAggregateMass() const;
 
      agx::Real getTotalSoilBulkVolume(agx::Real packingFraction=0.67) const;
 
      agx::Real getTotalParticleVolume() const;
 
      agx::Real getInnerParticleVolume() const;
 
      agx::Real getWedgeParticleVolume() const;
 
      agx::Vec3 getInnerParticleMomentum() const;
      agx::Vec3 getInnerParticleAngularMomentum() const;
 
      agx::Vec3 getInnerCachedMomentum() const;
      agx::Vec3 getInnerCachedAngularMomentum() const;
 
    public:
      DOXYGEN_START_INTERNAL_BLOCK()
 
      
      void addNotification( agxSDK::Simulation* simulation );
 
      void removeNotification( agxSDK::Simulation* simulation );
 
      void onPreCollide( Terrain* terrain,
                         Shovel* shovel,
                         bool enableBodies );
 
      void onPre( Terrain* terrain, Shovel* shovel, ActiveZone* activeZone);
 
      void onDeformerPre(Terrain* terrain, ActiveZone* activeZone);
 
      void onPost();
 
      void onEnableChange( bool enable );
 
      static agx::Plane transformPlaneToWorld(const agx::Frame* localFrame, const agx::Plane& localPlane);
 
      static agx::Vector<agx::Plane> transformPlanesToWorld( const agx::Frame* localFrame, const agx::Vector<agx::Plane>& localPlanes );
 
      void setConnectedInnerBody(agx::RigidBody* connectedInnerBody);
 
      agx::RigidBody* getInnerOrConnectedBody() const;
 
      bool isConnectedAggregate() const;
 
      void updateAggregateCacheData( agxTerrain::Shovel* shovel );
 
      void clearAggregateData();
 
      void disableWedgeAggregates();
 
      void renderWegePositions( const agx::Vec3& origin,
                                const agxRender::Color& color );
 
      void renderWedgeVelocityNormals( agx::Real normalLength = agx::Real( 1.0 ) );
 
    public:
      struct AGXTERRAIN_EXPORT WedgePartitionData
      {
        agx::RigidBodyRef wedgeBody;
        agx::LockJointRef lock;
 
        agx::Vec3 lockFramePosition;
        agx::Vec3 lockPlaneNormal;
 
        SoilParticlePtrVector splitWedgeParticles;
        VoxelHashSet voxels;
 
        agx::Vec3 wedgeBodyMomentum;
        agx::Vec3 wedgeBodyAngularMomentum;
 
        using RealArray = std::array<agx::Real, 6>;
        RealArray violations;
      };
 
      class AGXTERRAIN_EXPORT SoilWedgeContainer
      {
      public:
        SoilWedgeContainer( size_t numWedges );
 
        void init( agx::RigidBody* innerBody,
                   const agx::Name& tag );
 
        void addNotification( agxSDK::Simulation* simulation );
 
        void removeNotification( agxSDK::Simulation* simulation );
 
        void onEnableChange(bool enable);
 
        void setMaterial( agx::Material* material );
 
        void calculatePartitionPlaneData( Shovel* shovel );
 
        void splitWedgeParticles( const Shovel* shovel,
                                  const SoilParticlePtrVector& soilParticles );
 
        void splitWedgeVoxels( const Terrain* terrain,
                               const Shovel* shovel,
                               VoxelHashSet& voxels );
 
        const agx::RigidBody* getContactingWedgeBody() const;
 
        void updatePartitionBodyMomentum(
          agx::RigidBody* partitionBody,
          agx::Real totalMass,
          const agx::Matrix3x3& invWorldInertiaTensor );
 
        void updateCachedWedgeMomentum();
 
        void computeLockViolations();
 
        void synchronizeLockJoints(Terrain* terrain, Shovel* shovel);
 
        void clearParticles( size_t reserve );
 
        void clearVoxelsAndParticles();
 
        agx::Real getTotalWedgeMass() const;
 
        void setSinglePartitionMode();
 
        void setDeformerWedgeMode();
 
        bool isWedgePartitionBody( const agx::RigidBody* body ) const;
 
        const WedgePartitionData& getPartitionData( const agx::RigidBody* partitionBody ) const;
 
        const agx::Vec3& getCuttingVectorFlat() const;
 
        const agx::Vec3& getCutTopVector() const;
 
        const agx::Vec3& getForwardDiggingPlane() const;
 
        inline const WedgePartitionData& operator[] ( size_t index ) const {
          return m_wedgePartitions[ index ];
        }
 
        inline WedgePartitionData& operator[] ( size_t index ) {
          return m_wedgePartitions[ index ];
        }
 
        size_t size() const noexcept
        {
          return m_wedgePartitions.size();
        }
 
      protected:
        agx::Vec3 m_cuttingVectorFlat;
        agx::Vec3 m_topCutVector;
        agx::Vec3 m_forwardDiggingPlane;
 
        std::vector<WedgePartitionData> m_wedgePartitions;
      };
 
      SoilWedgeContainer* getSoilWedgeContainer();
 
      DOXYGEN_END_INTERNAL_BLOCK()
 
    protected:
      virtual ~SoilParticleAggregate();
 
      agxCollide::Geometry* _getInnerGeometry() const;
 
      agxCollide::Geometry* _getWedgeGeometry() const;
 
    private:
      void calculateWedgeVoxels(Terrain* terrain, ActiveZone* activeZone);
 
      VoxelHashSet filterWedgeVoxelsSplittingPlane( Terrain* terrain,
                                                    ActiveZone* activeZone,
                                                    const VoxelHashSet& wedgeVoxels );
 
      void updateDynamicProperties( const SoilParticlePtrVector& soilParticles,
                                    agx::RigidBody* rb,
                                    VoxelHashSet& voxels,
                                    Terrain* terrain,
                                    Shovel* shovel );
 
      void computeParticlesInActiveZone(  Shovel* shovel,
                                          ActiveZone* activeZone);
 
      void sortParticlesBySeparatingPlane( agx::ParticleSystem* soilParticleSystem,
                                           ActiveZone* activeZone,
                                           SoilParticlePtrVector& innerParticles,
                                           SoilParticlePtrVector& wedgeParticles,
                                           bool onlyMidpoint=false);
 
      void computeParticlesInInnerShape( Shovel* shovel,
                                         PrimaryActiveZone* activeZone,
                                         SoilParticlePtrVector& innerParticles,
                                         agx::Real radiusMultiplier);
 
      SoilParticlePtrVector computeParticlesInWedgeShape( ActiveZone* activeZone,
                                                          SoilParticlePtrVector& wedgeParticleCandidates );
 
      void computeParticlesAboveShovel( Shovel* shovel,
                                        SoilParticlePtrVector& wedgeParticles);
 
      bool isPointAbovePlane( const agx::Plane& plane,
                              const agx::Vec3& point,
                              agx::Real threshold = 0.0 ) const;
 
      bool isSphereIntersectingPlane( const agx::Plane& plane,
                                      const agx::Vec3& point,
                                      agx::Real radius ) const;
 
      bool shouldSplitWedgeAggregate( const Shovel* shovel );
 
      void clearParticles( size_t particleStorageSize );
 
      void debugRenderParticles( agxTerrain::Shovel* shovel,
                                 bool shouldSplit );
 
      void debugRenderVoxels( agxTerrain::Shovel* shovel,
                              agxTerrain::Terrain* terrain,
                              bool shouldSplit );
 
    private:
      agx::RigidBodyRef   m_innerBody;
 
      agx::Vec3           m_innerBodyCachedMomentum;
      agx::Vec3           m_innerBodyCachedAngularMomentum;
 
      agx::Vec3           m_innerParticleMomentum;
      agx::Vec3           m_innerParticleAngularMomentum;
 
      SoilParticlePtrVector m_innerParticles;
      SoilParticlePtrVector m_wedgeParticles;
      VoxelHashSet          m_wedgeVoxels;
      agx::Real             m_wedgeVoxelsVolume;
 
      SoilWedgeContainer     m_soilWedgeContainer;
      agx::RigidBodyObserver m_connectedInnerBody;
  };
 
 
 
  inline bool SoilParticleAggregate::isPointAbovePlane( const agx::Plane& plane,
                                                        const agx::Vec3& point,
                                                        agx::Real threshold ) const
  {
    return plane.signedDistanceToPoint(point) > threshold;
  }
 
 
 
  inline bool SoilParticleAggregate::isSphereIntersectingPlane( const agx::Plane& plane,
                                                                const agx::Vec3& point,
                                                                agx::Real radius ) const
  {
    return std::abs(plane.signedDistanceToPoint(point)) < radius;
  }
 
 
 
 
}