agxSDK/EnergyManager.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/agxPhysics_export.h>
#include <agx/RigidBody.h>
 
#include <agx/Physics/BinaryConstraintEntity.h>
#include <agx/Physics/ManyBodyConstraintEntity.h>
#include <agx/Physics/ContactConstraintEntity.h>
#include <agx/Physics/ConstraintRowEntity.h>
#include <agx/Jacobian.h>
#include <agx/Constraint.h>
#include <agxSDK/StepEventListener.h>
 
#include <agxSDK/ConstraintEnergyData.h>
 
// Forward declarations
namespace agxSDK
{
  class Simulation;
  typedef agx::observer_ptr< Simulation > SimulationObserver;
}
 
namespace agxSDK
{
  AGX_DECLARE_POINTER_TYPES( EnergyManager );
 
  class AGXPHYSICS_EXPORT EnergyManager : public agxStream::Serializable, public agx::Referenced
  {
 
  public:
    AGXSTREAM_DECLARE_SERIALIZABLE( agxSDK::EnergyManager );
 
  public:
 
    void add( const agx::RigidBody* rigidBody );
 
    void add( const agx::Constraint* constraint );
 
    void remove( const agx::RigidBody* rigidBody );
 
    void remove( const agx::Constraint* constraint );
 
    agx::Real getPotentialChange( const agx::RigidBody* rb ) const;
 
    agx::Real getDissipation( const agx::RigidBody* rb ) const;
 
    agx::Real getPotentialChange( const agx::Constraint* constraint ) const;
 
    agx::Real getDissipation( const agx::Constraint* constraint ) const;
 
    agx::Real getPotentialChange( const agx::ElementaryConstraint* constraint ) const;
 
    agx::Real getDissipation( const agx::ElementaryConstraint* constraint ) const;
 
    agx::Real getPower( const agx::ElementaryConstraint* motor ) const;
 
    agx::Real getKineticEnergyChange( const agx::RigidBody* rb ) const;
 
    agx::Real getTranslationalEnergyChange( const agx::RigidBody* rb ) const;
 
    agx::Real getRotationalEnergyChange( const agx::RigidBody* rb ) const;
 
    static agx::Real getKineticEnergy( const agx::RigidBody* rb );
 
    static agx::Real getTranslationalEnergy( const agx::RigidBody* rb );
 
    static agx::Real getRotationalEnergy( const agx::RigidBody* rb );
 
    DOXYGEN_START_INTERNAL_BLOCK()
    void calculateWorkFromExternalForces( const agx::Physics::RigidBodyData& rigidBody );
    void calculateWorkFromBinaryConstraints( const agx::Physics::BinaryConstraintData& binaryConstraint,
                                             const agx::Physics::ConstraintRowData& binaryConstraintRow,
                                             const agxData::Array< agx::Jacobian6DOFElement >& binaryConstraintJacobian,
                                             const agx::Physics::RigidBodyData& rigidBody );
    void calculateWorkFromManyBodyConstraints( const agx::Physics::ManyBodyConstraintData& manyBodyConstraint,
                                               const agx::Physics::ConstraintRowData& manyBodyConstraintRow,
                                               const agxData::Array< agx::Jacobian6DOFElement >& manyBodyConstraintJacobian,
                                               const agx::Physics::RigidBodyData& rigidBody );
 
    void updateData();
 
    void resetEnergy();
 
 
  protected:
    // Default constructor
    EnergyManager() = default;
 
    virtual ~EnergyManager();
 
  private:
    class RigidBodyData
    {
    public:
      RigidBodyData( const agx::RigidBody* rb );
 
      agx::Vec3 velocity() const;
      agx::Vec3 angularVelocity() const;
 
      agx::Bool update();
 
    private:
      agx::Vec3 m_velocity;
      agx::Vec3 m_angularVelocity;
      agx::RigidBodyConstObserver m_rb;
    };
 
    struct EnergyChange
    {
      EnergyChange( agx::Real initValue = 0 )
        : potential( initValue ), dissipation( initValue )
      {
      }
 
      agx::Real potential;
      agx::Real dissipation;
 
      void reset()
      {
        potential = 0;
        dissipation = 0;
      };
    };
 
    using RigidBodyDataContainer = agx::HashVector< const agx::RigidBody*, EnergyManager::RigidBodyData >;
    using EnergyChangeBodyContainer = agx::HashVector< const agx::RigidBody*, EnergyManager::EnergyChange >;
    using EnergyChangeConstraintContainer = agx::HashVector< const agx::ConstraintImplementation*, EnergyManager::EnergyChange >;
    using EnergyChangeScContainer = agx::HashVector< const agx::ElementaryConstraint*, EnergyChange >;
    using RigidBodyConstRefVector = agx::Vector< agx::RigidBodyConstRef >;
    using ConstraintConstRefVector = agx::Vector< agx::ConstraintConstRef >;
 
    AGX_DECLARE_POINTER_TYPES( UpdateListener );
 
    class UpdateListener : public agxSDK::StepEventListener
    {
    public:
      UpdateListener( EnergyManager* energyManager );
      virtual void pre( const agx::TimeStamp& t ) override;
 
    protected:
      virtual ~UpdateListener();
 
    private:
      EnergyManagerObserver m_energyManager;
    };
 
  private:
    friend class Simulation;
 
    EnergyManager( agxSDK::Simulation* simulation );
 
    void setSimulation( agxSDK::Simulation* simulation );
 
    void clear();
 
    template<class T>
    void calculateEnergyForElementaryConstraint( agx::Real& potential, agx::Real& dissipation,
                                                 ConstraintEnergyData* data, agx::UInt numRows, agx::Bool isActive,
                                                 agx::UInt& rowIndex, const T& rbIndices,
                                                 const agx::Physics::ConstraintRowData& binaryConstraintRow,
                                                 agx::UInt& gIndex,
                                                 const agxData::Array< agx::Jacobian6DOFElement >& binaryConstraintJacobian,
                                                 const agx::Physics::RigidBodyData& rigidBody );
 
    void calculateWorkOnBody( agx::Real& potential, agx::Real& dissipation, const EnergyManager::RigidBodyData& rbData,
                              const agx::Vec3& v, const agx::Vec3& w, agx::Bool isHolonomic, agx::Real lambdaDot,
                              agx::Real tau, const agx::Jacobian6DOFElement& G, agx::Real lambda );
 
    DOXYGEN_END_INTERNAL_BLOCK()
 
  private:
    RigidBodyDataContainer          m_rbData;
    EnergyChangeBodyContainer       m_energyChangeBodies;
    EnergyChangeConstraintContainer m_energyChangeConstraints;
    EnergyChangeScContainer         m_energyChangeSc;
 
    RigidBodyConstRefVector         m_rigidBodies;
    ConstraintConstRefVector        m_constraints;
 
    agxSDK::SimulationObserver      m_simulation;
    UpdateListenerRef               m_updateListener;
  };
}