Tutorials

C++ Tutorials

• tutorial_agxPlot.cpp
• tutorial_agxTerrain.cpp
• tutorial_agxTerrain_ClamShellBucket.cpp
• tutorial_ballJoint_secondaryConstraints.cpp
• tutorial_basicSimulation.cpp
• tutorial_beam.cpp
• tutorial_bodies.cpp
• tutorial_bulldozer_with_terrain.cpp
• tutorial_cable.cpp
• tutorial_cableWindAndWater.cpp
• tutorial_cable_damage.cpp
• tutorial_cable_tunneling_guard.cpp
• tutorial_car.cpp
• tutorial_carSteering.cpp
• tutorial_centerOfBuoyancy.cpp
• tutorial_collisionDetection.cpp
• tutorial_constraints.cpp
• tutorial_contactForces.cpp
• tutorial_contactMeasurement.cpp
• tutorial_convex.cpp
• tutorial_conveyorBelts.cpp
• tutorial_create_oriented_bounding_volume.cpp
• tutorial_customConstraints.cpp
• tutorial_customMergeSplitAlgorithm.cpp
• tutorial_driveTrain.cpp
• tutorial_driveTrain_ElectricMotor.cpp
• tutorial_driveTrain_combustionEngine.cpp
• tutorial_energyManager.cpp
• tutorial_fracture.cpp
• tutorial_frictionModels.cpp
• tutorial_gamePad.cpp
• tutorial_granularBodies.cpp
• tutorial_gravityFields.cpp
• tutorial_hydraulics.cpp
• tutorial_hydraulics_coupling.cpp
• tutorial_hydrodynamics.cpp
• tutorial_imu.cpp
• tutorial_io.cpp
• tutorial_lidar.cpp
• tutorial_listeners.cpp
• tutorial_machine_reconfiguration.cpp
• tutorial_material.cpp
• tutorial_mergeSplitHandler.cpp
• tutorial_mergedBody.cpp
• tutorial_particleSystem.cpp
• tutorial_pidController.cpp
• tutorial_pointcloud_over_ros2.cpp
• tutorial_python_excavator_controller.cpp
• tutorial_rigidBodyEmitter.cpp
• tutorial_robot_pick_and_place.cpp
• tutorial_ros2.cpp
• tutorial_statistics.cpp
• tutorial_suctionGripper.cpp
• tutorial_terrainPager_tileEvents.cpp
• tutorial_threadAffinity.cpp
• tutorial_threads.cpp
• tutorial_tire.cpp
• tutorial_trackedVehicle.cpp
• tutorial_tree.cpp
• tutorial_trimesh.cpp
• tutorial_wind.cpp
• tutorial_wire1.cpp
• tutorial_wireLink.cpp
• tutorial_wireWindAndWater.cpp

tutorial_agxPlot.cpp

/*
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.
*/
 
/*
 
This tutorial demonstrates how to use the agxPlot for a simple to use plotting through a Web interface.
 
*/
 
#include <agx/version.h>
#include <agx/Logger.h>
 
#include <agx/config/AGX_USE_WEBPLOT.h>
 
#if AGX_USE_WEBPLOT()
 
#include <agxCollide/Box.h>
#include <agxUnit/UnitMacros.h>
#include <agx/Hinge.h>
 
#include <agxOSG/utils.h>
#include <agxOSG/ExampleApplication.h>
#include <agxSDK/Simulation.h>
 
#include <agxPlot/System.h>
#include <agxPlot/FilePlot.h>
 
#include <agxPlot/WebPlot.h>
 
static bool g_isUnitTest = false;
 
 
namespace
{
 
  // Class that will be used to generate data for the plot
  class AngleDataGenerator : public agxPlot::RealCallbackDataGenerator
  {
  public:
    AngleDataGenerator( agx::Hinge *hinge )
      : m_hinge( hinge )
    {
    }
 
    // This just return a real value
    agx::Real get( )
    {
      agx::Real result = m_hinge->getAngle();
      return result;
    }
 
  private:
    agx::HingeRef m_hinge;
  };
 
  // Class that will be used to generate data for the plot
  class SpeedDataGenerator : public agxPlot::DataGenerator
  {
  public:
    SpeedDataGenerator(agx::Hinge* hinge)
      : m_hinge(hinge)
    {
    }
 
    // By using the base class, we can also indicate if there is a value or not
    agxPlot::CallbackDataGenerator::Result getValue()
    {
      agxPlot::CallbackDataGenerator::Result result;
      
      if (m_hinge) {
        result.value = m_hinge->getCurrentSpeed();
        result.hasValue = true;
      }
      return result;
    }
 
  private:
    agx::HingeRef m_hinge;
  };
 
 
}
 
osg::Group* buildTutorial1(agxSDK::Simulation* simulation, agxOSG::ExampleApplication*)
{
  osg::Group* root = new osg::Group();
 
  auto body = new agx::RigidBody();
  body->setRotation(agx::EulerAngles(0, agx::degreesToRadians(90), 0));
  auto box = new agxCollide::Geometry(new agxCollide::Box(0.1, 0.1, 1));
  body->add(box);
  agxOSG::createVisual(body, root);
  
  simulation->add(body);
 
  auto frame = new agx::Frame();
  frame->setLocalTranslate(0, 0, 1);
  frame->setLocalRotate(agx::EulerAngles(agx::degreesToRadians(90), 0, 0));
  auto hinge = new agx::Hinge(body, frame);
  simulation->add(hinge);
 
  // Get the reference to the plot system
  auto plot = simulation->getPlotSystem();
 
  // We can use a plot time series from the simulation(for the x axis)
  auto time_data = simulation->getTimeDataSeries();
 
  // Create a DataSeries where the data(from the angle callback class)
  auto angle_series = new agxPlot::DataSeries(new AngleDataGenerator(hinge), "Angle");
 
  // Create a DataSeries where the data(from the speed callback class)
  auto speed_series = new agxPlot::DataSeries(new SpeedDataGenerator(hinge), "Speed");
 
  // Specify the units
  angle_series->setUnit("radians");
  speed_series->setUnit("rad/s");
 
  // Create a Curve that will present the time(x) and angle(y)
  auto angle_curve = new agxPlot::Curve(
    time_data,
    angle_series,
    "Hinge angle"
    );
 
  // Create a Curve that will present the time(x) and speed(y)
  auto speed_curve = new agxPlot::Curve(
    time_data,
    speed_series,
    "Hinge speed"
  );
 
  // Set a specific colors of the curves
  angle_curve->setColor(agxRender::Color::Red());
  speed_curve->setColor(agxRender::Color::Blue());
 
  // Create a window with a name.If the window(Box)
  // already exists, we will get a reference to that window.
  auto plot_window = plot->getOrCreateWindow("Box");
 
  // Add the curves to the window
  plot_window->add(angle_curve);
  plot_window->add(speed_curve);
 
  // We can plot to a file if we want to.
  // The file will be overwritten everytime we restart the script.
  // plot.add(agxPlot.FilePlot("plot.dat"))
 
  // Here we create a WebPlot output that will open a browser
  if (!g_isUnitTest)
  {
    auto webplot = new agxPlot::WebPlot(true);
 
    // Add the webplot output to the plot system
    plot->add(webplot);
 
    // The rest is handled by the Simulation.
  }
 
  return root;
}
 
int main(int argc, char** argv)
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial agxPlot\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  g_isUnitTest = argc >= 2; // Do not show plots if we sent in arguments - this is only for the internal testing framework at Algoryx.
 
 
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene(buildTutorial1, '1');
 
 
    if (application->init(argc, argv))
    {
      return application->run();
    }
  }
  catch (std::exception& e) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}
 
#else
 
 
int main(int, char**)
{
 
  LOGGER_WARNING() << LOGGER_STATE(agx::Notify::PRINT_NONE) << "\n" <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial agxPlot\n" <<
    "\t--------------------------\n" << LOGGER_ENDL() <<
    "AGX Dynamics built without support for agxPlot" << LOGGER_ENDL();
 
  return 0;
}
#endif

tutorial_agxTerrain.cpp

/*
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.
*/
 
 
/*
 
Setup and use the agxTerrain::Terrain class which is a generic deformable terrain simulating soil mechanics.
 
*/
 
#include <agx/config/AGX_USE_AGXTERRAIN.h>
#if AGX_USE_AGXTERRAIN()
#include <agx/version.h>
#include <agx/SystemInformation.h>
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/TerrainVoxelRenderer.h>
#include <agxOSG/utils.h>
#include <agxUtil/StepEventCallback.h>
#include <agxTerrain/Terrain.h>
#include <agxTerrain/TerrainMaterialLibrary.h>
#include "terrain_shovel_utils.h"
 
using namespace agx;
 
namespace
{
  template<size_t N>
  size_t find(const char(&vs)[N], int v)
  {
    if ( v < 0 || v > std::numeric_limits<char>::max() ) {
      return agx::InvalidIndex;
    }
 
    auto c = static_cast<char>( v );
    auto b = std::begin(vs);
    auto e = std::end(vs);
    auto it = std::find(b, e, c);
    if ( it != e ) {
      return it - b;
    }
    else {
      return agx::InvalidIndex;
    }
  }
 
  class RenderToggler : public agxSDK::GuiEventListener
  {
  public:
    RenderToggler(agxOSG::TerrainVoxelRenderer* renderer, agxOSG::SceneDecorator* text)
      : m_renderer(renderer), m_state(0), m_text(text)
    {
      m_state.set(HEIGHTS, true);
      m_state.set(SOIL_PARTICLES, true);
      this->updateState();
    }
 
    virtual bool keyboard(int key, unsigned int /*modKeyMask*/, float /*x*/, float /*y*/, bool keydown)
    {
      if ( keydown ) {
        return false;
      }
 
      const char keys[] = { 'a', 'z', 's', 'x' };
      size_t index = find(keys, key);
      if ( index == agx::InvalidIndex ) {
        return false;
      }
 
      m_state.set(index, !m_state.test(index));
 
      this->updateState();
 
      return true;
    }
 
  protected:
    void updateState()
    {
      m_renderer->setRenderVoxelSolidMass(m_state.test(SOLID_VOXELS));
      m_text->setText(1, agx::String::format("RenderSolidVoxel[a]: %d", (int)m_state.test(SOLID_VOXELS)));
 
      m_renderer->setRenderVoxelFluidMass(m_state.test(FLUID_VOXELS));
      m_text->setText(2, agx::String::format("RenderFluidVoxel[z]: %d", (int)m_state.test(FLUID_VOXELS)));
 
      m_renderer->setRenderSoilParticles(m_state.test(SOIL_PARTICLES));
      m_text->setText(3, agx::String::format("RenderSoilParticles[s]: %d", (int)m_state.test(SOIL_PARTICLES)));
 
      m_renderer->setRenderHeightField(m_state.test(HEIGHTS));
      m_text->setText(4, agx::String::format("RenderHeightField[x]: %d", (int)m_state.test(HEIGHTS)));
    }
 
  private:
    enum { SOLID_VOXELS, FLUID_VOXELS, SOIL_PARTICLES, HEIGHTS };
    agxOSG::TerrainVoxelRendererObserver m_renderer;
    agx::BitSet_small<int> m_state;
    agxOSG::SceneDecorator* m_text;
  };
 
  class ShovelDriver : public agxSDK::StepEventListener
  {
  public:
    ShovelDriver( RigidBody* shovelBody )
      : StepEventListener( StepEventListener::POST_STEP )
      , m_shovelBody(shovelBody)
    {  }
 
    virtual void post( const agx::TimeStamp& t ) override
    {
      if ( t < 0.5 )
        m_shovelBody->setVelocity(agx::Quat(agx::PI / 6.0, agx::Vec3::Y_AXIS()) * agx::Vec3(1.0, 0.0, 0.0));
      else if ( t < 7.5 )
        m_shovelBody->setVelocity(1.0, 0.0, 0.0);
      else if ( t < 8 )
        m_shovelBody->setVelocity(agx::Quat(-agx::PI / 6.0, agx::Vec3::Y_AXIS()) * agx::Vec3(1.0, 0.0, 0.0));
      else if ( t < 14 )
        m_shovelBody->setVelocity(-1.0, 0.0, 0.0);
      else
        m_shovelBody->setVelocity(0.0, 0.0, 0.0);
    }
 
  private:
    agx::RigidBody* m_shovelBody;
  };
 
  class ShovelKeyboardListener : public agxSDK::GuiEventListener
  {
  public:
    ShovelKeyboardListener(agxSDK::Simulation* simulation,
                           agx::RigidBody* shovelBody,
                           agx::Vec3 offset,
                           float maxSpeed = 1.0,
                           float maxRotSpeed = (float)agx::PI_4 )
    : m_maxSpeed(maxSpeed)
    , m_maxRotSpeed(maxRotSpeed)
  {
      auto kinematicSphere = new agxCollide::Geometry(new agxCollide::Sphere(0.5));
      kinematicSphere->setEnableCollisions(false);
      m_kinematicBody = new agx::RigidBody(kinematicSphere);
      m_kinematicBody->setMotionControl(RigidBody::KINEMATICS);
      m_kinematicBody->setPosition(shovelBody->getFrame()->transformPointToWorld(offset));
      m_kinematicBody->setRotation(shovelBody->getRotation());
      simulation->add(m_kinematicBody);
      m_joint = new agx::LockJoint(m_kinematicBody, shovelBody);
      m_joint->setEnableComputeForces(true);
      simulation->add(m_joint);
      setName("keyboard listener");
  }
 
  bool keyboard(int key, unsigned modkeyMask, float /*x*/, float /*y*/, bool keyDown) {
 
    bool handled = false;
 
    if ( !keyDown ) {
      m_kinematicBody->setVelocity(0, 0, 0);
      m_kinematicBody->setAngularVelocity(0, 0, 0);
      return handled;
    }
 
    switch ( key )
    {
    case 'a':
      m_kinematicBody->setVelocity(-m_maxSpeed, 0, 0);
      break;
    case 'z':
      m_kinematicBody->setVelocity(m_maxSpeed, 0, 0);
      break;
    case 's':
      m_kinematicBody->setVelocity(0, m_maxSpeed, 0);
      break;
    case 'x':
      m_kinematicBody->setVelocity(0, -m_maxSpeed, 0);
      break;
    case GuiEventListener::KEY_Page_Up:
      if ( modkeyMask == GuiEventListener::MODKEY_LEFT_ALT )
        m_kinematicBody->setAngularVelocity(0, m_maxRotSpeed, 0);
      else
        m_kinematicBody->setVelocity(0, 0, m_maxSpeed);
      break;
    case GuiEventListener::KEY_Page_Down:
      if ( modkeyMask == GuiEventListener::MODKEY_LEFT_ALT )
        m_kinematicBody->setAngularVelocity(0, -m_maxRotSpeed, 0);
      else
        m_kinematicBody->setVelocity(0, 0, -m_maxSpeed);
      break;
    default:
      break;
    }
 
    return handled;
  }
 
  agx::LockJoint* getLock()  {
    return m_joint;
  }
 
private:
  agx::LockJointRef m_joint;
  agx::RigidBodyRef m_kinematicBody;
  float m_maxSpeed;
  float m_maxRotSpeed;
};
 
 
  void createPile( agxTerrain::Terrain* terrain,
                agx::Real length,
                agx::Real width,
                agx::Real height,
                const agx::Vec3& direction,
                const agx::Vec3& location,
                agx::Real compaction )
  {
    int numLenPoints = int(( length ) / terrain->getElementSize()) + 1;
    int numWidthPoints = int(( width ) / terrain->getElementSize()) + 1;
 
    agx::Vec3 widthDirection = agx::Vec3(0, 0, 1).cross(direction);
    widthDirection.normalize();
 
    auto interface = terrain->getTerrainGridControl();
 
    agx::Real l_a_inc = agx::PI / numLenPoints;
    agx::Real w_a_inc = agx::PI / numWidthPoints;
    agx::Real numWidthPoints_2 = numWidthPoints * 0.5;
    agx::Real numLenPoints_2 = numLenPoints * 0.5;
 
    for ( int l=0; l < numLenPoints; l++ )
      for ( int w=0; w < numLenPoints; w++ )
      {
        agx::Real w_inc = int( w - numWidthPoints_2 ) * terrain->getElementSize();
        agx::Real l_inc = int( l - numLenPoints_2 ) * terrain->getElementSize();
        agx::Vec2i gridPoint = terrain->getClosestGridPoint(location + widthDirection * w_inc + direction * l_inc );
        agx::Real h = height * sin( l_a_inc * l ) * cos( w_a_inc * w - agx::PI_2 );
        interface->addSolidOccupancyInColumnToHeight( gridPoint, h, static_cast<float>(compaction) );
      }
  }
 
  //class ShovelDriver : public agxSDK::StepEventListener
  //{
  //public:
  //  ShovelDriver(RigidBody* shovelBody)
  //    : StepEventListener(StepEventListener::POST_STEP)
  //    , m_shovelBody(shovelBody)
  //  {
  //  }
 
  //  virtual void post(const agx::TimeStamp& t) override
  //  {
  //    if (t < 0.5)
  //      m_shovelBody->setVelocity(agx::Quat(agx::PI / 6.0, agx::Vec3::Y_AXIS()) * agx::Vec3(1.0, 0.0, 0.0));
  //    else if (t < 7.5)
  //      m_shovelBody->setVelocity(1.0, 0.0, 0.0);
  //    else if (t < 8)
  //      m_shovelBody->setVelocity(agx::Quat(-agx::PI / 6.0, agx::Vec3::Y_AXIS()) * agx::Vec3(1.0, 0.0, 0.0));
  //    else if (t < 14)
  //      m_shovelBody->setVelocity(-1.0, 0.0, 0.0);
  //    else
  //      m_shovelBody->setVelocity(0.0, 0.0, 0.0);
  //  }
 
  //private:
  //  agx::RigidBody* m_shovelBody;
  //};
 
  class MeasurementListener : public agxSDK::StepEventListener
  {
  public:
    MeasurementListener(agxOSG::ExampleApplication* application, agxTerrain::Terrain* terrain)
      : m_sd(application->getSceneDecorator())
      , m_soilSimulationInterface(terrain->getSoilSimulationInterface())
    {
    }
 
    virtual void post(const agx::TimeStamp& t) override
    {
      m_sd->setText(0, agx::String::format("Time:                          %d", (int)t));
      m_sd->setText(1, agx::String::format("# Particles:                   %d", m_soilSimulationInterface->getNumSoilParticles()));
      m_sd->setText(2, agx::String::format("# Particles-particle contacts: %d", m_soilSimulationInterface->getParticleParticleContacts().size()));
      m_sd->setText(3, agx::String::format("# Particles-geometry contacts: %d", m_soilSimulationInterface->getParticleGeometryContacts().size()));
    }
  private:
    agxOSG::SceneDecorator* m_sd;
    agxTerrain::SoilSimulationInterface* m_soilSimulationInterface;
  };
}
 
 
 
osg::Group* buildTutorial1( agxSDK::Simulation *simulation, agxOSG::ExampleApplication*  application )
{
  osg::Group* root = new osg::Group();
 
  size_t resolutionX = 51;
  size_t resolutionY = 51;
  agx::Real elementSize = 0.2;
  agx::Real maximumDepth = 2.5;
 
  agxTerrain::TerrainRef terrain = new agxTerrain::Terrain( resolutionX, resolutionY, elementSize, maximumDepth );
  simulation->add( terrain );
 
  terrain->loadLibraryMaterial( "dirt_1" );
 
  agx::MaterialRef terrainMaterial = new agx::Material( "GroundMaterial" );
  simulation->add(terrainMaterial);
 
  terrain->setMaterial( terrainMaterial );
 
  agx::MaterialRef shovelMaterial = new agx::Material("ShovelMaterial");
 
  // Create a simple bulldozer blade using a two-body shovel constructed from two boxes
  agx::Vec3 shovelSize{ 0.366, 1.6, 0.01 };
  agx::Real shovelAngle = agx::PI * 55.0 / 180.0;
  auto shovel = createSimpleShovelBody(simulation, root, shovelSize, shovelAngle);
  agxUtil::setBodyMaterial(shovel, shovelMaterial);
 
  agx::Vec3 cuttingPoint1 = agx::Vec3{ 2 * shovelSize.x() * std::cos(shovelAngle), -shovelSize.y(), 2 * shovelSize.x() * ( -0.5 - std::sin(shovelAngle) ) };
  agx::Vec3 cuttingPoint2 = cuttingPoint1 + agx::Vec3{ 0.0, 2 * shovelSize.y(), 0.0 };
  agx::Vec3 topPoint1     = agx::Vec3{ 0.0, -shovelSize.y(), shovelSize.x() };
  agx::Vec3 topPoint2     = agx::Vec3{ 0.0, shovelSize.y(), shovelSize.x() };
  agx::Line cuttingEdge { cuttingPoint1, cuttingPoint2 };
  agx::Line topEdge { topPoint1, topPoint2 };
  agx::Vec3 cuttingVector{ cos( shovelAngle ), 0.0, -sin( shovelAngle ) };
  agxTerrain::ShovelRef terrainShovel { new agxTerrain::Shovel{ shovel, topEdge, cuttingEdge, cuttingVector } };
  simulation->add( terrainShovel );
  terrainShovel->getSettings()->setVerticalBladeSoilMergeDistance(0.4);
  terrainShovel->getAdvancedSettings()->setNoMergeExtensionDistance(0.5);
  terrainShovel->getSettings()->setPenetrationForceScaling(0);
 
  agxOSG::createVisual(shovel, root);
 
  // Set the initial position of the shovel
  shovel->setPosition(agx::Vec3(-5.5, 0.0, shovelSize.x() * ( 1 + 2 * sin(shovelAngle) ) + 0.15));
 
  agx::ref_ptr<ShovelDriver> driver = new ShovelDriver( shovel );
  simulation->add(driver, 51);
 
  const agx::UInt numCores = agx::SystemInformation::getNumCpu();
  agx::setNumThreads(numCores / 2 - 1);
 
  agxOSG::TerrainVoxelRendererRef renderer = new agxOSG::TerrainVoxelRenderer(terrain, root);
  renderer->setRenderVoxelSolidMass(false);
  renderer->setRenderVoxelFluidMass(false);
  renderer->setRenderVoxelBoundingBox(false);
  renderer->setRenderHeightField(true);
  renderer->setRenderVelocityField(false);
  renderer->setVelocityFieldLineColor(Vec4(1.0, 1.0, 0, 1));
  renderer->setRenderSoilParticles(true);
  simulation->add(renderer);
  simulation->add(new RenderToggler(renderer, application->getSceneDecorator()));
 
  return root;
}
 
osg::Group* buildTutorial2(agxSDK::Simulation *simulation, agxOSG::ExampleApplication*  application)
{
  osg::Group* root = new osg::Group();
 
  size_t resolutionX = 71;
  size_t resolutionY = 71;
  agx::Real elementSize = 0.2;
  agx::Real maximumDepth = 2.5;
 
  agxTerrain::TerrainRef terrain = new agxTerrain::Terrain(resolutionX, resolutionY, elementSize, maximumDepth);
  simulation->add(terrain);
 
  terrain->loadLibraryMaterial("dirt_1");
 
  auto compactionProperties = terrain->getTerrainMaterial()->getCompactionProperties();
 
  compactionProperties->setCompactionTimeRelaxationConstant(0.5);
 
  compactionProperties->setCompressionIndex(0.11);
 
  compactionProperties->setAngleOfReposeCompactionRate(50.0);
 
 
  auto bulkProperties = terrain->getTerrainMaterial()->getBulkProperties();
  bulkProperties->setMaximumDensity(bulkProperties->getDensity() * 2.0);
 
  bulkProperties->setSwellFactor(1.25);
 
  agx::MaterialRef terrainMaterial = new agx::Material("GroundMaterial");
  simulation->add(terrainMaterial);
 
  terrain->setMaterial(terrainMaterial);
 
  agx::Real length = 6.0, width = 6.0, height = 2.5;
  agx::Real compaction = 0.5;
  agx::Vec3 location = agx::Vec3(0, 0, 0);
  createPile( terrain,
              length,
              width,
              height,
              agx::Vec3(1, 0, 0),
              location,
              compaction );
 
  agx::MaterialRef deformerMaterial = new agx::Material("DeformerMaterial");
  simulation->add(deformerMaterial);
 
  auto sphereGeometry = new agxCollide::Geometry(new agxCollide::Sphere(0.5));
  auto sphereBody = new agx::RigidBody(sphereGeometry);
  sphereGeometry->setMaterial(deformerMaterial);
  sphereBody->getMassProperties()->setMass(5e3);
  sphereBody->setPosition(agx::Vec3(0.6, -0.25, 4.0));
  simulation->add( sphereBody );
  agxOSG::createVisual( sphereBody, root );
  sphereBody->setAngularVelocityDamping(agx::Vec3f(0.75,0.75,0.75));
 
  auto boxGeometry = new agxCollide::Geometry(new agxCollide::Box( agx::Vec3(0.5)));
  auto boxBody = new agx::RigidBody(boxGeometry);
  boxGeometry->setMaterial(deformerMaterial);
  boxBody->getMassProperties()->setMass(1e5);
  boxBody->setPosition(agx::Vec3(-0.85, -0.5, 4.0));
  simulation->add(boxBody);
  agxOSG::createVisual(boxBody, root);
 
  agxOSG::TerrainVoxelRendererRef renderer = new agxOSG::TerrainVoxelRenderer(terrain, root);
  renderer->setRenderCompaction(true, agx::RangeReal(0.85, 1.15));
  renderer->setRenderVoxelSolidMass(false);
  renderer->setRenderVoxelFluidMass(false);
  renderer->setRenderVoxelBoundingBox(false);
  renderer->setRenderHeightField(true);
  renderer->setRenderSoilParticles(true);
  simulation->add(renderer);
  simulation->add(new RenderToggler(renderer, application->getSceneDecorator()));
 
  return root;
}
 
 
 
osg::Group* buildTutorial3(agxSDK::Simulation *simulation, agxOSG::ExampleApplication*  application)
{
  osg::Group* root = new osg::Group();
 
  size_t resolutionX = 81;
  size_t resolutionY = 81;
  agx::Real elementSize = 0.2;
  agx::Real maximumDepth = 2.5;
 
  agxTerrain::TerrainRef terrain = new agxTerrain::Terrain(resolutionX, resolutionY, elementSize, maximumDepth);
  simulation->add(terrain);
 
  terrain->loadLibraryMaterial("dirt_1");
  terrain->getTerrainMaterial()->getBulkProperties()->setYoungsModulus(1e11);
 
  terrain->getProperties()->setDeleteSoilParticlesOutsideBounds(true);
 
  agx::Real length = 0.65, width = 0.6, height = 0.45, thickness = 0.05, topFraction = 0.25;
  auto bucketShovelData = createSimpleBucket(simulation,
                                             root,
                                             length,
                                             width,
                                             height,
                                             thickness,
                                             topFraction);
 
  agxTerrain::ShovelRef terrainShovel{
    new agxTerrain::Shovel{
    bucketShovelData.shovelBody,
    bucketShovelData.topEdge,
    bucketShovelData.cuttingEdge,
    bucketShovelData.cuttingDirection }
  };
 
  terrainShovel->getRigidBody()->setPosition({ 0, 0, 1.0 });
  terrainShovel->getRigidBody()->setRotation({ 0, -agx::PI / 10.0, 0 });
  simulation->add(terrainShovel);
 
  agx::Real plength = 2.5, pwidth = 2.5, pheight = 1.25;
  agx::Real compaction1 = 1.0;
  agx::Vec3 location = agx::Vec3(-4, 0, 0);
  createPile(terrain,
             plength,
             pwidth,
             pheight,
             agx::Vec3(1, 0, 0),
             location,
             compaction1);
 
  agx::Real compaction2 = 0.5;
  agx::Vec3 location2 = agx::Vec3(0, -4, 0);
  createPile(terrain,
             plength,
             pwidth,
             pheight,
             agx::Vec3(1, 0, 0),
             location2,
             compaction2);
 
  agx::Real compaction3 = 1.5;
  agx::Vec3 location3 = agx::Vec3(0, 4, 0);
  createPile(terrain,
             plength,
             pwidth,
             pheight,
             agx::Vec3(1, 0, 0),
             location3,
             compaction3);
 
  agx::MaterialRef shovelMaterial = new agx::Material("ShovelMaterial");
  simulation->add(shovelMaterial);
  agxUtil::setBodyMaterial(terrainShovel->getRigidBody(), shovelMaterial);
 
  agx::MaterialRef terrainMaterial = new agx::Material("TerrainMaterial");
  simulation->add(terrainMaterial);
  terrain->setMaterial(terrainMaterial);
 
  auto shovelTerrainContactMaterial = simulation->getMaterialManager()->getContactMaterialOrCreateImplicit(shovelMaterial, terrainMaterial);
  shovelTerrainContactMaterial->setYoungsModulus(1e8);
  simulation->add(shovelTerrainContactMaterial);
  agxUtil::setBodyMaterial(terrainShovel->getRigidBody(), shovelMaterial);
 
  agx::Material* particleMaterial = terrain->getMaterial(agxTerrain::Terrain::MaterialType::PARTICLE);
  auto shovelParticleContactMaterial = simulation->getMaterialManager()->getContactMaterialOrCreateImplicit(shovelMaterial, particleMaterial);
  shovelParticleContactMaterial->setYoungsModulus(1e8);
  shovelParticleContactMaterial->setFrictionCoefficient(0.3);
  shovelParticleContactMaterial->setRollingResistanceCoefficient(0.1);
  shovelParticleContactMaterial->setRestitution(0.0);
  simulation->add(shovelParticleContactMaterial);
  simulation->add(terrainShovel);
 
  auto listener = new ShovelKeyboardListener( simulation, terrainShovel->getRigidBody(), Vec3(0.7, 0.0, -0.0), 1.0 );
  simulation->add(listener);
 
  agx::Vec4f textColor = agxRender::Color::Yellow();
  application->getSceneDecorator()->setText(1, "--- Shovel Controls ---", textColor);
  application->getSceneDecorator()->setText(2, "'a'        : Forward", textColor);
  application->getSceneDecorator()->setText(3, "'z'        : Backwards", textColor);
  application->getSceneDecorator()->setText(4, "'s'        : Right", textColor);
  application->getSceneDecorator()->setText(5, "'x'        : Left", textColor);
  application->getSceneDecorator()->setText(6, "'Page Up'  : Move Up", textColor);
  application->getSceneDecorator()->setText(7, "'Page Down': Move Down", textColor);
  application->getSceneDecorator()->setText(8, "'Page Up + Left Alt'  : Rotate Up", textColor);
  application->getSceneDecorator()->setText(9, "'Page Down + Left Alt': Rotate Down", textColor);
 
  agxUtil::StepEventCallback::post( [ application, textColor, terrainShovel, listener ](agxSDK::Simulation* /*simulation*/)
  {
    agx::Vec3 force, torque;
    listener->getLock()->getLastForce( terrainShovel->getRigidBody(), force, torque );
    application->getSceneDecorator()->setText( 10, agx::String::format( "Shovel Force: %6.2f %6.2f %6.2f  N", force.x(), force.y(), force.z() ), textColor );
  }, simulation);
 
 
  agxOSG::TerrainVoxelRendererRef renderer = new agxOSG::TerrainVoxelRenderer(terrain, root);
  renderer->setRenderCompaction(true, agx::RangeReal(0.5, 1.5));
  renderer->setRenderVoxelSolidMass(false);
  renderer->setRenderVoxelFluidMass(false);
  renderer->setRenderVoxelBoundingBox(false);
  renderer->setRenderHeightField(true);
  renderer->setRenderSoilParticles(true);
  simulation->add(renderer);
 
  agx::Vec3 eye    = agx::Vec3(1.1446339652418182E+01, -4.0189526421746642E+00, 8.6762060961937877E+00);
  agx::Vec3 center = agx::Vec3(1.6077577107657193E+00, -7.9424674741572898E-01, 1.9513089499363527E+00);
  agx::Vec3 up = agx::Vec3(-5.1512025829023400E-01, 1.7720716398098579E-01, 8.3859927291455416E-01);
  application->setCameraHome(eye, center, up);
 
  return root;
}
 
osg::Group* buildTutorial4(agxSDK::Simulation *simulation, agxOSG::ExampleApplication*  application)
{
  osg::Group* root = new osg::Group();
 
  // Create a height field that will be used to construct the terrain
  // Note that the element size(size / resolution) has to be identical
  // in both dimensions for usage in terrain.
  size_t resolution = 250;
  agx::Real elementSize = 0.2;
  agx::Real size = elementSize * static_cast<agx::Real>(resolution - 1);
  agx::Real height = 1.0;
 
  // Helper functions for constructing the height field surface
  auto hfIndexToPosition = []( size_t index, size_t resolution, agx::Real size ) {
    return ( (static_cast<agx::Real>( index ) / ( static_cast<agx::Real>( resolution )) - 0.5) * size );
  };
  agxCollide::HeightFieldRef heightField = new agxCollide::HeightField(resolution, resolution, size, size);
 
  // Set the height field heights
  auto heightFunction = []( agx::Real x, agx::Real y, agx::Real h, agx::Real size ) {
    return h * std::sin( x * ( 2 * agx::PI / size ) + agx::PI ) * cos( y * agx::PI / size );
  };
  for ( size_t i = 0; i < resolution; i++)
  {
    for ( size_t j = 0; j < resolution; j++)
    {
      heightField->setHeight(i, j, heightFunction(hfIndexToPosition(i, resolution, size), hfIndexToPosition(j, resolution, size), height, size));
    }
  }
 
  // Create the terrain from a height field, set maximum depth to 5m and add it to the simulation
  agxTerrain::TerrainRef terrain = agxTerrain::Terrain::createFromHeightField(heightField, 5.0);
  simulation->add(terrain);
 
  agx::Vec3 shovelSize{0.366, 1.6, 0.01};
  agx::Real shovelAngle = agx::PI * 55.0 / 180.0;
  auto shovel = createSimpleShovelBody(simulation, root, shovelSize, shovelAngle);
 
  simulation->add(shovel);
 
  terrain->loadLibraryMaterial("dirt_1");
 
  auto particleParticleCm = terrain->getContactMaterial(agxTerrain::Terrain::MaterialType::PARTICLE, agxTerrain::Terrain::MaterialType::PARTICLE);
  agx::MaterialRef shovelMaterial = new agx::Material("shovel_material");
  auto particleMaterial = terrain->getMaterial(agxTerrain::Terrain::MaterialType::PARTICLE);
  agx::ContactMaterialRef shovelParticleContactMaterial = new agx::ContactMaterial(shovelMaterial, particleMaterial);
  shovelParticleContactMaterial->setYoungsModulus(particleParticleCm->getYoungsModulus());
  shovelParticleContactMaterial->setRestitution(particleParticleCm->getRestitution());
  shovelParticleContactMaterial->setFrictionCoefficient(particleParticleCm->getFrictionCoefficient());
  shovelParticleContactMaterial->setRollingResistanceCoefficient(particleParticleCm->getRollingResistanceCoefficient());
  simulation->add(shovelParticleContactMaterial);
 
  auto terrainMaterial = terrain->getMaterial(agxTerrain::Terrain::MaterialType::TERRAIN);
  agx::ContactMaterialRef shovelTerrainContactMaterial = new agx::ContactMaterial(shovelMaterial, terrainMaterial);
  shovelTerrainContactMaterial->setYoungsModulus(1e11);
  shovelTerrainContactMaterial->setFrictionCoefficient(0.4);
  simulation->add(shovelTerrainContactMaterial);
 
  agxUtil::setBodyMaterial(shovel, shovelMaterial);
 
  auto cuttingPoint1 = agx::Vec3(2 * shovelSize.x() * cos(shovelAngle), -shovelSize.y(), 2 * shovelSize.x() * (-0.5 - std::sin(shovelAngle)));
  auto cuttingPoint2 = cuttingPoint1 + agx::Vec3(0.0, 2.0 * shovelSize.y(), 0.0);
  auto topPoint1 = agx::Vec3(0.0, -shovelSize.y(), shovelSize.x());
  auto topPoint2 = agx::Vec3(0.0, shovelSize.y(), shovelSize.x());
  auto cuttingEdge = agx::Line(cuttingPoint1, cuttingPoint2);
  auto topEdge = agx::Line(topPoint1, topPoint2);
  auto forwardVector = agx::Vec3(cos(shovelAngle), 0.0, -std::sin(shovelAngle));
 
  agxTerrain::ShovelRef terrainShovel = new agxTerrain::Shovel(shovel, topEdge, cuttingEdge, forwardVector);
  terrainShovel->getSettings()->setVerticalBladeSoilMergeDistance(0.3);
  terrainShovel->getAdvancedSettings()->setNoMergeExtensionDistance(0.5);
  simulation->add(terrainShovel);
 
  shovel->setPosition(-12.0, 0.0, shovelSize.x() * (1 + 2 * std::sin(shovelAngle)) + heightFunction(-12.0, 0.0, height, size) - 0.1);
 
  application->getSceneDecorator()->setBackgroundColor(agx::Vec3(agxRender::Color::Black().x(), agxRender::Color::Black().y(), agxRender::Color::Black().z()));
 
  agxOSG::TerrainVoxelRendererRef renderer = new agxOSG::TerrainVoxelRenderer(terrain, root);
  renderer->setRenderHeights(true, agx::RangeReal(-1.25, 1.25));
  renderer->setRenderVoxelSolidMass(false);
  renderer->setRenderVoxelFluidMass(false);
  renderer->setRenderHeightField(true);
  renderer->setRenderVoxelBoundingBox(false);
  renderer->setRenderSoilParticles(true);
  simulation->add(renderer);
 
  class BladeDriver : public agxSDK::StepEventListener
  {
  private:
    agxTerrain::ShovelRef m_terrainShovel;
    agx::Vec3 m_bladeOffset;
    agx::Real m_height;
    agx::Real m_size;
 
  public:
    BladeDriver(agxTerrain::ShovelRef terrainShovel, agx::Real height, agx::Real size, agx::Vec3 shovelSize, agx::Real shovelAngle)
    {
      m_terrainShovel = terrainShovel;
      m_bladeOffset = agx::Vec3(shovelSize.x() * 2 * cos(shovelAngle), 0.0, -shovelSize.x() * (1 + 2 * sin(shovelAngle)));
      m_height = height;
      m_size = size;
    }
 
    agx::Vec3 getVelocity(agx::Vec3 pos)
    {
      auto heightFunction = [](agx::Real x, agx::Real y, agx::Real height, agx::Real size) {
        return height * std::sin(x * (2 * agx::PI / size) + agx::PI) * std::cos(y * agx::PI / size);
      };
 
      auto heightFunctionGradX = [](agx::Real x, agx::Real y, agx::Real height, agx::Real size) {
        return 2 * agx::PI * height * std::cos((2 * agx::PI * x / size) + agx::PI) * std::cos(y * agx::PI / size) / size;
      };
 
      agx::Vec3 cuttingEdgePosition = pos + m_bladeOffset;
      agx::Vec3 vel;
      if (cuttingEdgePosition.x() < 0)
      {
        if (cuttingEdgePosition.z() > 0)
        {
          vel = agx::Vec3(1.0, 0.0, heightFunctionGradX(cuttingEdgePosition.x(), 0.0, m_height, m_size)).normal();
        }
        else
        {
          vel = agx::Vec3(1.0, 0.0, 0.0);
        }
      }
      else if (cuttingEdgePosition.x() >= 0)
      {
        if (heightFunction(cuttingEdgePosition.x(), cuttingEdgePosition.y(), m_height, m_size) < -0.1)
        {
          vel = agx::Vec3(1.0, 0.0, heightFunctionGradX(cuttingEdgePosition.x(), 0.0, m_height, m_size)).normal();
        }
        else
        {
          vel = agx::Vec3(1.0, 0.0, 0.0);
        }
      }
      return vel;
    }
 
    void post(const agx::TimeStamp& t)
    {
      if (t < 24)
      {
        m_terrainShovel->getRigidBody()->setVelocity(this->getVelocity(m_terrainShovel->getRigidBody()->getPosition()));
      }
      else
      {
        m_terrainShovel->getRigidBody()->setVelocity(0.0, 0.0, 0.0);
      }
    }
  };
 
  agx::ref_ptr<BladeDriver> bladeDriver = new BladeDriver(terrainShovel, height, size, shovelSize, shovelAngle);
  simulation->add(bladeDriver);
 
  agx::ref_ptr<MeasurementListener> measurementListener = new MeasurementListener(application, terrain);
  simulation->add(measurementListener);
 
  auto eye = agx::Vec3(4.2554700794164848E+00, -1.8111588117481372E+01, 6.3025642249031311E+00);
  auto center = agx::Vec3(-9.4897856997624352E-01, -2.0127647520494536E+00, 7.1454264792766731E-01);
  auto up = agx::Vec3(-8.7901903076223503E-02, 3.0116556248169890E-01, 9.4951174790555393E-01);
  application->setCameraHome(eye, center, up);
 
  const agx::UInt numCores = agx::SystemInformation::getNumCpu();
  agx::setNumThreads(numCores / 2);
 
  return root;
}
 
osg::Group* buildTutorial5(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  osg::Group* root = new osg::Group();
 
  size_t resolutionX = 51;
  size_t resolutionY = 51;
  agx::Real elementSize = 0.2;
  agx::Real maximumDepth = 2.5;
 
  agxTerrain::TerrainRef terrain = new agxTerrain::Terrain(resolutionX, resolutionY, elementSize, maximumDepth);
  simulation->add(terrain);
 
  terrain->loadLibraryMaterial("dirt_1");
 
  agx::MaterialRef terrainMaterial = new agx::Material("GroundMaterial");
  simulation->add(terrainMaterial);
 
  terrain->setMaterial(terrainMaterial);
 
  agx::MaterialRef shovelMaterial = new agx::Material("ShovelMaterial");
 
  // Create a simple bulldozer blade using a two-body shovel constructed from two boxes
  agx::Vec3 shovelSize{ 0.366, 1.6, 0.01 };
  agx::Real shovelAngle = agx::PI * 55.0 / 180.0;
  auto shovel = createSimpleShovelBody(simulation, root, shovelSize, shovelAngle);
  agxUtil::setBodyMaterial(shovel, shovelMaterial);
 
  agx::Vec3 cuttingPoint1 = agx::Vec3{ 2 * shovelSize.x() * std::cos(shovelAngle), -shovelSize.y(), 2 * shovelSize.x() * (-0.5 - std::sin(shovelAngle)) };
  agx::Vec3 cuttingPoint2 = cuttingPoint1 + agx::Vec3{ 0.0, 2 * shovelSize.y(), 0.0 };
  agx::Vec3 topPoint1 = agx::Vec3{ 0.0, -shovelSize.y(), shovelSize.x() };
  agx::Vec3 topPoint2 = agx::Vec3{ 0.0, shovelSize.y(), shovelSize.x() };
  agx::Line cuttingEdge{ cuttingPoint1, cuttingPoint2 };
  agx::Line topEdge{ topPoint1, topPoint2 };
  agx::Vec3 cuttingVector{ cos(shovelAngle), 0.0, -sin(shovelAngle) };
  agxTerrain::ShovelRef terrainShovel{ new agxTerrain::Shovel{ shovel, topEdge, cuttingEdge, cuttingVector } };
  simulation->add(terrainShovel);
  terrainShovel->getSettings()->setVerticalBladeSoilMergeDistance(0.4);
  terrainShovel->getAdvancedSettings()->setNoMergeExtensionDistance(0.5);
  terrainShovel->getSettings()->setPenetrationForceScaling(0);
 
  agxOSG::createVisual(shovel, root);
 
  // Set the initial position of the shovel
  shovel->setPosition(agx::Vec3(-5.5, 0.0, shovelSize.x() * (1 + 2 * sin(shovelAngle)) + 0.15));
 
  agx::ref_ptr<ShovelDriver> driver = new ShovelDriver(shovel);
  simulation->add(driver, 51);
 
  const agx::UInt numCores = agx::SystemInformation::getNumCpu();
  agx::setNumThreads(numCores / 2 - 1);
 
  /*
  Create and add a new material to the terrain.
  */
  auto sandTerrainMaterial = terrain->getLibraryMaterial("sand_1");
  agxCollide::GeometryRef sandRegion = new agxCollide::Geometry(new agxCollide::Box(agx::Vec3(2, 2, 2)));
  sandRegion->setPosition(agx::Vec3(0, 0, 0)); // place in the middle.
  terrain->addTerrainMaterial(sandTerrainMaterial, sandRegion);
 
  agxOSG::TerrainVoxelRendererRef renderer = new agxOSG::TerrainVoxelRenderer(terrain, root);
  renderer->setRenderVoxelSolidMass(false);
  renderer->setRenderVoxelFluidMass(false);
  renderer->setRenderVoxelBoundingBox(false);
  renderer->setRenderHeightField(true);
  renderer->setRenderVelocityField(false);
  renderer->setVelocityFieldLineColor(Vec4(1.0, 1.0, 0, 1));
  renderer->setRenderSoilParticles(true);
  renderer->setRenderTerrainMaterials(true);
  renderer->setRenderDefaultTerrainMaterial(false); // To be able to visualize where we put the added terrain material.
  simulation->add(renderer);
  simulation->add(new RenderToggler(renderer, application->getSceneDecorator()));
 
  return root;
}
 
osg::Group* buildTutorial6(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  osg::Group* root = new osg::Group();
 
  agx::Real size = 25;
  agx::Real elementSize = 0.2;
  int resolution = (int)std::floor((double)size / elementSize) + 1;
  Real depth = 0.1;
 
  agxTerrain::TerrainRef terrain = new agxTerrain::Terrain(resolution, resolution, elementSize, depth);
 
  // Define a height function lambda
  auto heightFunction = [](double x, double y, double h, double size) {
    double height = 0.0;
 
    x /= (size / 2.0);
    y /= (size / 2.0);
    x += 0.55;
 
    double r = std::sqrt(x * x + y * y);
    double theta = std::atan2(x, y);
    double theta_threshold = M_PI / 12.0;
    double theta_min = 50.0 * M_PI / 180.0;
    double theta_max = M_PI - theta_min;
    double deltaR = 0.2;
    double R = 1.0;
    double n = 1.0;
 
    if (r > R - deltaR && theta > theta_min && theta < theta_max) {
      if (r < R) {
        height = h * std::pow(std::sin((r + deltaR - R) / (2 * deltaR) * M_PI), n);
      }
      else {
        height = h;
      }
    }
 
    if (theta < (theta_min + theta_threshold)) {
      theta = theta - theta_min;
      height *= std::sin(theta / (2 * theta_threshold) * M_PI);
    }
 
    if (theta > (theta_max - theta_threshold)) {
      theta = theta_max - theta;
      height *= std::sin(theta / (2 * theta_threshold) * M_PI);
    }
 
    return height;
  };
  terrain->setEnableMinimumHeights(true);
  for (int x = 0; x < resolution; x++)
    for (int y = 0; y < resolution; y++)
    {
      auto ti = agx::Vec2i(x, y);
      terrain->setMinimumHeight(ti, - heightFunction((double)x, (double)y, 4, size));
      terrain->setHeight(ti, heightFunction((double)x, (double)y, 4, size));
    }
  simulation->add(terrain);
 
  agxOSG::TerrainVoxelRendererRef renderer = new agxOSG::TerrainVoxelRenderer(terrain, root);
  renderer->setRenderVoxelSolidMass(false);
  renderer->setRenderVoxelFluidMass(false);
  renderer->setRenderVoxelBoundingBox(false);
  renderer->setRenderHeightField(true);
  renderer->setRenderVelocityField(false);
  renderer->setVelocityFieldLineColor(Vec4(1.0, 1.0, 0, 1));
  renderer->setRenderSoilParticles(true);
  renderer->setRenderTerrainMaterials(true);
  renderer->setRenderDefaultTerrainMaterial(false); // To be able to visualize where we put the added terrain material.
  simulation->add(renderer);
  simulation->add(new RenderToggler(renderer, application->getSceneDecorator()));
 
  return root;
}
 
 
 
osg::Group* buildTutorial7( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  // This scene shows how to add a bound on a dynamic
  // body that moves over particles, preventing them
  // from merging with the terrain. We also add an index bound
  // at the end of the particle row to prevent merging at the end.
 
  osg::Group* root = new osg::Group();
 
  // Construct a Terrain with 51-by-51 elements, element size 0.1 m, max depth 2.5 m.
  int resX = 51;
  int resY = 51;
  double elementSize = 0.1;
  double depth = 2.5;
  agxTerrain::TerrainRef terrain = new agxTerrain::Terrain( resX, resY, elementSize, depth );
  simulation->add( terrain );
 
  // Add a moving kinematic rigid body.
  agxCollide::GeometryRef box_geom = new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.25, 0.25, 0.25 ) ) );
  agx::RigidBodyRef rb = new agx::RigidBody( box_geom );
  simulation->add( rb );
  agxOSG::createVisual( rb, root );
 
  rb->setMotionControl( agx::RigidBody::KINEMATICS );
  rb->setVelocity( agx::Vec3( 0.25, 0, 0 ) );
  rb->setPosition( agx::Vec3( 0.0, 0.0, 0.5 ) );
 
  // Add a 3D forbidden bound attached to the frame of the rigid body.
  agx::Vec3 halfVec( 0.5, 2.5, 0.5 );
  agx::AffineMatrix4x4 localTransform( agx::EulerAngles( 0, 0, 0 ), agx::Vec3( 0, 0, -0.5 ) );
  // Create the bound and use the agx::Frame of the RigidBody as the parent transform via rb->getFrame.
  agxTerrain::IForbiddenBoundRef fb = new agxTerrain::ForbiddenBound( agxCollide::BoundingAABB( -halfVec, halfVec ),
                                                                      localTransform,
                                                                      rb->getFrame()
  );
  terrain->addForbiddenBound( fb );
 
  // Add a 3D index bound to the terrain that covers the left half of the terrain.
  agxTerrain::IForbiddenBoundRef ib = new agxTerrain::ForbiddenIndexBound( agx::Bound3i( agx::Vec3i( 30, 0, -5 ),
                                                                                         agx::Vec3i( 45, 51, 5 ) ) );
  terrain->addForbiddenBound( ib );
 
  // Add particles under the forbidden bound.
  auto si = terrain->getSoilSimulationInterface();
  for ( int x = 22; x < 30; ++x ) {
    for ( int y = 5; y < 45; ++y ) {
      agx::Vec3 p = terrain->getSurfacePositionWorld( { x, y } );
      si->createSoilParticle( p + agx::Vec3( 0, 0, terrain->getElementSize() / 2 ) );
    }
  }
 
  // Setup a renderer for the terrain to render the height field.
  auto renderer = new agxOSG::TerrainVoxelRenderer( terrain, root );
  simulation->add( renderer );
 
  // Enable debug rendering for showcasing the forbidden bounds.
  application->setEnableDebugRenderer( true );
 
  return root;
}
 
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial agxTerrain\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene(buildTutorial1, '1');
    application->addScene( buildTutorial2, '2' );
    application->addScene( buildTutorial3, '3' );
    application->addScene( buildTutorial4, '4' );
    application->addScene( buildTutorial5, '5');
    application->addScene( buildTutorial6, '6');
    application->addScene( buildTutorial7, '7' );
 
 
    if ( application->init( argc, argv ) )
    {
      return application->run();
    }
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}
#else
 
#include <agx/Logger.h>
#include <agx/version.h>
int main(int , char** )
{
 
  LOGGER_WARNING() << LOGGER_STATE(agx::Notify::PRINT_NONE) << "\n" <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial agxTerrain\n" <<
    "\t--------------------------\n" << LOGGER_ENDL() <<
    "AGX Dynamics built without support for agxTerrain" << LOGGER_ENDL();
 
  return 0;
}
 
#endif

tutorial_agxTerrain_ClamShellBucket.cpp

/*
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.
*/
 
 
/*
 
Setup and use the agxTerrain::Terrain class which is a generic deformable terrain simulating soil mechanics.
 
*/
 
#include <agx/config/AGX_USE_AGXTERRAIN.h>
#if AGX_USE_AGXTERRAIN()
#include <agx/version.h>
#include <agx/SystemInformation.h>
#include <agx/Hinge.h>
#include <agx/DistanceJoint.h>
#include <agx/LockJoint.h>
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/TerrainVoxelRenderer.h>
#include <agxOSG/utils.h>
#include <agxTerrain/Terrain.h>
#include <agxTerrain/TerrainMaterialLibrary.h>
#include <agxTerrain/TerrainToolUtils.h>
#include <agxTerrain/ClamShellBucket.h>
#include "terrain_shovel_utils.h"
 
using namespace agx;
 
namespace
{
  class ClamShellBucketAssembly : public agxSDK::Assembly
  {
  public:
    ClamShellBucketAssembly( agx::Material* material)
      : Assembly()
    {
      this->setName( "ClamShellAssembly" );
 
      const auto segmentSize = agx::Vec3( 0.035, 1.0, 0.1 );
      const auto numSegments = 20;
      std::vector<std::pair<agx::Vec3, agx::Real>> shovelSizes;
      shovelSizes.reserve( numSegments );
      agx::Real deltaAngle = 180.0 / (numSegments - 1);
      for (int i = 0; i < numSegments; ++i)
        shovelSizes.push_back( { segmentSize, i * deltaAngle }  );
 
      auto bucketData = agxTerrain::createCustomBucketData( shovelSizes, true );
      bucket1 = new agxTerrain::Shovel( bucketData.shovelBody,
                                        bucketData.topEdge,
                                        bucketData.cuttingEdge,
                                        bucketData.cuttingDirection );
      bucket1->getRigidBody()->setRotation( agx::EulerAngles( 0, agx::PI * 1.5, 0.0 ) );
      bucket1->getRigidBody()->setName( "Bucket1" );
 
      auto bucketData2 = agxTerrain::createCustomBucketData( shovelSizes, true );
      bucket2 = new agxTerrain::Shovel( bucketData2.shovelBody,
                                        bucketData2.topEdge,
                                        bucketData2.cuttingEdge,
                                        bucketData2.cuttingDirection );
      bucket2->getRigidBody()->setRotation( agx::EulerAngles( 0, agx::PI * 1.5, agx::PI ) );
      bucket2->getRigidBody()->setName( "Bucket2" );
 
      clamShellBucket = new agxTerrain::ClamShellBucket( bucket1, bucket2 );
      this->add( clamShellBucket );
 
      double radius = 0.15;
      agxCollide::Geometry* cylinderGeom = new agxCollide::Geometry( new agxCollide::Cylinder( radius, segmentSize.y() ) );
      cylinderBody = new agx::RigidBody( cylinderGeom );
      cylinderBody->setPosition( agx::Vec3( 0, 0, radius ) );
      cylinderBody->setName( "CylinderBody" );
      this->add( cylinderBody );
 
      agx::HingeFrame hf1 = agx::HingeFrame( cylinderBody->getPosition(),
                                             agx::Vec3( 0, 1, 0 ) );
      bucketHinge1 = createBucketHingeJoint( hf1, cylinderBody, bucket1->getRigidBody() );
      bucketHinge1->setName( "BucketHinge1" );
      this->add( bucketHinge1 );
 
      agx::HingeFrame hf2 = agx::HingeFrame( cylinderBody->getPosition(),
                                             agx::Vec3( 0, -1, 0 ) );
      bucketHinge2 = createBucketHingeJoint( hf2, cylinderBody, bucket2->getRigidBody() );
      bucketHinge2->setName( "BucketHinge2" );
      this->add( bucketHinge2 );
 
      const auto connectorBodySize = agx::Vec3( 1.0, 0.15, 0.25 );
      connectorBody = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( connectorBodySize ) ) );
      connectorBody->setPosition( agx::Vec3( 0, 0, 1.5 ) );
      connectorBody->setName( "ConnectorBody" );
      this->add( connectorBody );
 
      agx::LockJointRef lockJoint = new agx::LockJoint( connectorBody, cylinderBody );
      this->add( lockJoint );
 
      const auto topCraneBodySize = agx::Vec3( 1.0, 4.0, 0.25 );
      topCraneBody = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( topCraneBodySize ) ) );
      topCraneBody->setPosition( agx::Vec3( 0, -topCraneBodySize.y(), 5 ) );
      topCraneBody->setName( "TopBodyCrane" );
      this->add( topCraneBody );
 
      distanceJoint1 = createCraneDistanceJoint( connectorBody,
                                                 agx::Vec3( connectorBodySize.x(), 0, connectorBodySize.z() ),
                                                 topCraneBody,
                                                 agx::Vec3( topCraneBodySize.x(), topCraneBodySize.y(), -topCraneBodySize.z() ) );
      distanceJoint1->setName( "DistanceJoint1" );
      this->add( distanceJoint1 );
 
      distanceJoint2 = createCraneDistanceJoint( connectorBody,
                                                 agx::Vec3( -connectorBodySize.x(), 0, connectorBodySize.z() ),
                                                 topCraneBody,
                                                 agx::Vec3( -topCraneBodySize.x(), topCraneBodySize.y(), -topCraneBodySize.z() ) );
      distanceJoint2->setName( "DistanceJoint2" );
      this->add( distanceJoint2 );
 
      topHubBody = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Cylinder(1.0, 0.5) ) );
      topHubBody->setMotionControl( agx::RigidBody::STATIC );
      topHubBody->setPosition( topCraneBody->getPosition() + agx::Vec3( 0, -4, 0 ) );
      topHubBody->setRotation( agx::EulerAngles( agx::PI_2, 0, 0 ) );
      topHubBody->setName( "TopHubBody" );
      this->add( topHubBody );
 
      agx::HingeFrame topHf = agx::HingeFrame( topCraneBody->getPosition() + agx::Vec3(0, -4, 0 ), agx::Vec3( 0, 0, 1 ) );
      topBodyHinge = new agx::Hinge( topHf, topCraneBody, topHubBody );
      topBodyHinge->getMotor1D()->setEnable( true );
      topBodyHinge->getMotor1D()->setLockedAtZeroSpeed( true );
      topBodyHinge->setName( "TopHubBodyHinge" );
      this->add( topBodyHinge );
 
      // Disable collisions internally in the clamshell structure
      agxUtil::setEnableCollisions( cylinderBody, bucket1->getRigidBody(), false );
      agxUtil::setEnableCollisions( cylinderBody, bucket2->getRigidBody(), false );
      agxUtil::setEnableCollisions( cylinderBody, connectorBody, false );
      agxUtil::setEnableCollisions( topHubBody, topCraneBody, false );
 
      // Apply the supplied shovel material on all bodies
      for (auto& body : getRigidBodies())
        agxUtil::setBodyMaterial( body, material );
 
      // Increase the connector body mass to push the center of mass of
      // the bucket higher to dampen oscillations when bucket is loaded.
      connectorBody->getMassProperties()->setMass( connectorBody->getMassProperties()->getMass() * 5 );
 
      // Increase the base compliance of all constraints
      for (auto& constaint : getConstraints())
        constaint->setCompliance( 1E-12 );
    }
 
    agx::Hinge* createBucketHingeJoint( agx::HingeFrame& frame, agx::RigidBody* body1, agx::RigidBody* body2 )
    {
      auto bucketHinge = new agx::Hinge( frame, body1, body2 );
      bucketHinge->getMotor1D()->setEnable( true );
      bucketHinge->getMotor1D()->setLockedAtZeroSpeed( true );
      bucketHinge->getRange1D()->setEnable( true );
      bucketHinge->getRange1D()->setRange( { 0, agx::PI } );
      bucketHinge->getRange1D()->setCompliance( 1E-14 );
 
      return bucketHinge;
    }
 
    agx::DistanceJoint* createCraneDistanceJoint( agx::RigidBody* body1,
                                                  const agx::Vec3& offset1,
                                                  agx::RigidBody* body2,
                                                  const agx::Vec3& offset2 )
    {
      auto distanceJoint = new agx::DistanceJoint( body1, offset1, body2, offset2 );
      distanceJoint->getLock1D()->setEnable( false );
      distanceJoint->getMotor1D()->setEnable( true );
      distanceJoint->getMotor1D()->setLockedAtZeroSpeed( true );
      distanceJoint->setEnableComputeForces( true );
 
      return distanceJoint;
    }
 
    // Reset all velocities for the active motors in the asssembly
    void resetVelocities()
    {
      distanceJoint1->getMotor1D()->setSpeed( 0 );
      distanceJoint2->getMotor1D()->setSpeed( 0 );
      bucketHinge1->getMotor1D()->setSpeed( 0 );
      bucketHinge2->getMotor1D()->setSpeed( 0 );
      topBodyHinge->getMotor1D()->setSpeed( 0 );
    }
 
    void setDistanceJointSpeed( agx::Real speed )
    {
      distanceJoint1->getMotor1D()->setSpeed( speed );
      distanceJoint2->getMotor1D()->setSpeed( speed );
    }
 
    void setBucketHingeJointSpeed( agx::Real speed )
    {
      bucketHinge1->getMotor1D()->setSpeed( speed );
      bucketHinge2->getMotor1D()->setSpeed( speed );
    }
 
    void setTopBodyHingeSpeed( agx::Real speed )
    {
      topBodyHinge->getMotor1D()->setSpeed( speed );
    }
 
    agx::Real getDistanceJointForce()
    {
      return distanceJoint1->getMotor1D()->getCurrentForce()
             + distanceJoint1->getMotor1D()->getCurrentForce();
    }
 
    agx::Real getSoilMassInBucket()
    {
      return bucket1->getSoilParticleAggregate()->getMass() + bucket2->getSoilParticleAggregate()->getMass();
    }
 
    agx::Real getClamShellBodyMass()
    {
      return bucket1->getRigidBody()->getMassProperties()->getMass()
        + bucket2->getRigidBody()->getMassProperties()->getMass()
        + connectorBody->getMassProperties()->getMass()
        + cylinderBody->getMassProperties()->getMass();
    }
 
  public:
    agx::DistanceJointRef distanceJoint1;
    agx::DistanceJointRef distanceJoint2;
    agx::HingeRef bucketHinge1;
    agx::HingeRef bucketHinge2;
    agx::HingeRef topBodyHinge;
    agx::RigidBodyRef connectorBody;
    agx::RigidBodyRef cylinderBody;
    agx::RigidBodyRef topCraneBody;
    agx::RigidBodyRef topHubBody;
    agxTerrain::ShovelRef bucket1;
    agxTerrain::ShovelRef bucket2;
    agxTerrain::ClamShellBucketRef clamShellBucket;
  };
 
 
  class ClamShellKeyboardListener : public agxSDK::GuiEventListener
  {
  public:
    ClamShellKeyboardListener( ClamShellBucketAssembly* clamShell,
                               float maxSpeed = 1.5,
                               float maxRotSpeed = (float)agx::PI_4 )
      : m_clamShellAssembly(clamShell)
      ,  m_maxSpeed( maxSpeed )
      , m_maxRotSpeed( maxRotSpeed )
    {
      setName( "keyboard listener" );
    }
 
    bool keyboard( int key, unsigned /*modkeyMask*/, float /*x*/, float /*y*/, bool keyDown) {
 
      bool handled = false;
 
      if (!keyDown) {
        m_clamShellAssembly->resetVelocities();
        return handled;
      }
 
      switch (key)
      {
      case 'a':
        m_clamShellAssembly->setDistanceJointSpeed( -m_maxSpeed );
        break;
      case 'z':
        m_clamShellAssembly->setDistanceJointSpeed( m_maxSpeed );
        break;
      case 's':
        m_clamShellAssembly->setBucketHingeJointSpeed( -m_maxRotSpeed );
        break;
      case 'x':
        m_clamShellAssembly->setBucketHingeJointSpeed( m_maxRotSpeed );
        break;
      case GuiEventListener::KEY_Page_Up:
        m_clamShellAssembly->setTopBodyHingeSpeed( -m_maxRotSpeed / 4 );
        break;
      case GuiEventListener::KEY_Page_Down:
        m_clamShellAssembly->setTopBodyHingeSpeed( m_maxRotSpeed / 4 );
        break;
      default:
        break;
      }
 
      return handled;
    }
 
  private:
    ClamShellBucketAssembly* m_clamShellAssembly;
 
    float m_maxSpeed;
    float m_maxRotSpeed;
  };
 
  class MeasurementListener : public agxSDK::StepEventListener
  {
  public:
    MeasurementListener( agxOSG::ExampleApplication* application,
                         ClamShellBucketAssembly* assembly)
      : m_sd( application->getSceneDecorator() )
      , m_assembly( assembly )
    {
    }
 
    virtual void post( const agx::TimeStamp& t ) override
    {
      const auto color = agxRender::Color::Yellow();
      m_sd->setText( 0, agx::String::format( "Time:                  %7.4f", t ), color );
      m_sd->setText( 1, agx::String::format( "ClamShell is closed:   %d",        m_assembly->clamShellBucket->isClosed() ), color );
      m_sd->setText( 2, agx::String::format( "Soil Mass in Bucket    %7.4f ton", m_assembly->getSoilMassInBucket() / 1e3 ), color );
      m_sd->setText( 3, agx::String::format( "ClamShell Body Mass    %7.4f ton", m_assembly->getClamShellBodyMass() / 1e3 ), color );
      m_sd->setText( 4, agx::String::format( "Crane Load             %7.4f kN",  m_assembly->getDistanceJointForce() / 1e3 ), color );
    }
  private:
    agxOSG::SceneDecorator*  m_sd;
    ClamShellBucketAssembly* m_assembly;
  };
}
 
 
 
osg::Group* buildTutorial1( agxSDK::Simulation *simulation, agxOSG::ExampleApplication*  application )
{
  osg::Group* root = new osg::Group();
 
  size_t resolutionX = 240;
  size_t resolutionY = 240;
  agx::Real elementSize = 0.25;
  agx::Real maximumDepth = 1.0;
 
  agxTerrain::TerrainRef terrain = new agxTerrain::Terrain( resolutionX, resolutionY, elementSize, maximumDepth );
  simulation->add( terrain );
 
  terrain->loadLibraryMaterial( "dirt_1" );
 
  agx::MaterialRef terrainMaterial = new agx::Material( "GroundMaterial" );
  simulation->add(terrainMaterial);
 
  terrain->setMaterial( terrainMaterial );
 
  agx::MaterialRef shovelMaterial = new agx::Material("ShovelMaterial");
  shovelMaterial->getBulkMaterial()->setDensity( 7850 ); // Set density of steel
 
  auto clamShellAssembly = new ClamShellBucketAssembly(shovelMaterial);
  simulation->add( clamShellAssembly );
 
  auto node = agxOSG::createVisual( clamShellAssembly, root );
  agxOSG::setDiffuseColor( node, agxRender::Color( 0.878f, 0.678f, 0.071f, 1.0f ) );
 
  auto shovelTerrainContactMaterial = simulation->getMaterialManager()->getContactMaterialOrCreateImplicit( shovelMaterial, terrainMaterial );
  shovelTerrainContactMaterial->setYoungsModulus( 1e8 );
  shovelTerrainContactMaterial->setFrictionCoefficient( 0.4 );
  simulation->add( shovelTerrainContactMaterial );
 
  agx::Material* particleMaterial = terrain->getMaterial( agxTerrain::Terrain::MaterialType::PARTICLE );
  auto shovelParticleContactMaterial = simulation->getMaterialManager()->getContactMaterialOrCreateImplicit( shovelMaterial, particleMaterial );
  shovelParticleContactMaterial->setYoungsModulus( 1e8 );
  shovelParticleContactMaterial->setFrictionCoefficient( 0.3 );
  shovelParticleContactMaterial->setRollingResistanceCoefficient( 0.1 );
  shovelParticleContactMaterial->setRestitution( 0.0 );
  simulation->add( shovelParticleContactMaterial );
 
  // Set the initial position of the ClamShellBucket structure
  clamShellAssembly->setPosition(agx::Vec3(0, 0, 8.0));
 
  // Setup keyboard listener for controling the shovel
  auto keyListener = new ClamShellKeyboardListener( clamShellAssembly );
  simulation->add( keyListener );
 
  // Construct keyboard listener to print information about the excavation to screen
  auto mListener = new MeasurementListener( application, clamShellAssembly );
  simulation->add( mListener );
 
  const agx::UInt numCores = agx::SystemInformation::getNumCpu();
  agx::setNumThreads(numCores / 2 - 1);
 
  int startIndex = 8;
  agx::Vec4f textColor = agxRender::Color::Yellow();
  application->getSceneDecorator()->setText( startIndex + 1, "--- Bucket Controls ---", textColor );
  application->getSceneDecorator()->setText( startIndex + 2, "'a'        : Raise ClamShell", textColor );
  application->getSceneDecorator()->setText( startIndex + 3, "'z'        : Lower ClamShell", textColor );
  application->getSceneDecorator()->setText( startIndex + 4, "'s'        : Close ClamShell", textColor );
  application->getSceneDecorator()->setText( startIndex + 5, "'x'        : Open ClamShell", textColor );
  application->getSceneDecorator()->setText( startIndex + 6, "'Page Up'  : Rotate Crane Right", textColor );
  application->getSceneDecorator()->setText( startIndex + 7, "'Page Down': Rotate Crane Left", textColor );
 
  agxOSG::TerrainVoxelRendererRef renderer = new agxOSG::TerrainVoxelRenderer(terrain, root);
  renderer->setRenderVoxelSolidMass(false);
  renderer->setRenderVoxelFluidMass(false);
  renderer->setRenderVoxelBoundingBox(false);
  renderer->setRenderHeightField(true);
  renderer->setRenderVelocityField(false);
  renderer->setVelocityFieldLineColor(Vec4(1.0, 1.0, 0, 1));
  renderer->setRenderSoilParticles(true);
  simulation->add(renderer);
 
  return root;
}
 
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial agxTerrain\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene(buildTutorial1, '1');
 
    if ( application->init( argc, argv ) )
    {
      return application->run();
    }
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}
#else
 
#include <agx/Logger.h>
#include <agx/version.h>
int main(int , char** )
{
 
  LOGGER_WARNING() << LOGGER_STATE(agx::Notify::PRINT_NONE) << "\n" <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial agxTerrain\n" <<
    "\t--------------------------\n" << LOGGER_ENDL() <<
    "AGX Dynamics built without support for agxTerrain" << LOGGER_ENDL();
 
  return 0;
}
 
#endif

tutorial_ballJoint_secondaryConstraints.cpp

/*
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.
*/
 
/*
 
This tutorial will show the basics of using a cone limit, as well as other secondary constraints
on the ball joint.
 
*/
 
#include <agxOSG/ExampleApplication.h>
 
#include <agxIO/ImageReaderPNG.h>
 
#include <agxUtil/HeightFieldGenerator.h>
#include <agxUtil/agxUtil.h>
 
#include <agx/Logger.h>
 
#include "tutorialUtils.h"
#include <agxUnit/UnitMacros.h>
#include <agx/Hinge.h>
#include <agx/CylindricalJoint.h>
#include <agx/BallJoint.h>
 
#include <agxRender/RenderSingleton.h>
 
using namespace agx;
using namespace agxOSG;
 
 
osg::Group* buildTutorial1( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/)
{
  // Let's create a static sphere as the base of our ball joint.
  RigidBodyRef base = new RigidBody("base");
  Real radius = 0.5;
  base->add(new agxCollide::Geometry(new agxCollide::Sphere(radius)));
  simulation->add(base);
  base->setMotionControl(agx::RigidBody::STATIC);
 
  // Next we create a cylinder rod to connect to the sphere
  RigidBodyRef rod = new RigidBody("rod");
  agxCollide::GeometryRef rodGeom = new agxCollide::Geometry(new agxCollide::Cylinder(0.1, 1));
  // Rotate the rod so that it points in the z-direction and then move it so that the bottom of the
  // rod is at the center of the sphere
  rodGeom->setRotation(EulerAngles(agx::PI_2, 0, 0));
  rodGeom->setPosition(0, 0, 0.5);
  rod->add(rodGeom);
  simulation->add(rod);
  
  // Disable collisions between base and rod
  agxUtil::setEnableCollisions(base, rod, false);
 
  // Create visuals for the bodies
  osg::Group* root = new osg::Group;
  agxOSG::createVisual(base, root);
  agxOSG::createVisual(rod, root);
 
  // Next we want to connect the two bodies with a ball joint
  // To do that we need one frame for each body. 
  FrameRef f1 = new Frame();
  FrameRef f2 = new Frame();
  // The order of how the bodies are added to the ball joint does matter. The second body will be seen as the "base"
  // body, which the cone limit will be attached to. The first body can move relative to the second one.
  BallJointRef ballJoint = new BallJoint(rod, f1, base, f2);
  simulation->add(ballJoint);
 
  // Next, we must enable the cone limit. By default the limit is PI/4 in both directions, so let's change that too.
  // Setting two different values will give an elliptic cone limit, and two similar one will give a circular cone 
  // limit.
  ballJoint->getConeLimit()->setEnable(true);
  ballJoint->getConeLimit()->setLimitAngles(PI / 4, PI / 3);
 
  // One important thing to note is that the limit does not know what geometry it is attached to, it will only limit
  // the movement of the relative coordinate systems.
 
  // Lastly, let's give the rod a little velocity so that it falls over and we can see it stop on the limit.
  rod->setAngularVelocity(0.2, 0.2, 0);
 
  return root;
}
 
 
 
osg::Group* buildTutorial2(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/)
{
  // Set up a similar scene as in the previous tutorial, with a ball and a rod attached to it with a ball joint.
  RigidBodyRef base = new RigidBody("base");
  Real radius = 0.5;
  base->add(new agxCollide::Geometry(new agxCollide::Sphere(radius)));
  simulation->add(base);
  base->setMotionControl(agx::RigidBody::STATIC);
 
  RigidBodyRef rod = new RigidBody("rod");
  agxCollide::GeometryRef rodGeom = new agxCollide::Geometry(new agxCollide::Cylinder(0.1, 1));
  rodGeom->setRotation(EulerAngles(agx::PI_2, 0, 0));
  rodGeom->setPosition(0, 0, 0.5);
  rod->add(rodGeom);
  simulation->add(rod);
 
  agxUtil::setEnableCollisions(base, rod, false);
 
  osg::Group* root = new osg::Group;
  agxOSG::createVisual(base, root);
  agxOSG::createVisual(rod, root);
 
  // Set up ball joint
  FrameRef f1 = new Frame();
  FrameRef f2 = new Frame();
  BallJointRef ballJoint = new BallJoint(rod, f1, base, f2);
  simulation->add(ballJoint);
 
  // Enable cone limit
  ballJoint->getConeLimit()->setEnable(true);
  ballJoint->getConeLimit()->setLimitAngles(PI / 4, PI / 3);
 
  // Connected to the cone limit, there exists two FrictionControllers that can be used to set friction on the cone 
  // limit. One controls the friction for sliding along the limit, and the other for rotating on the limit (in this
  // case the rod rotating around its own z-axis on the limit). These FrictionControllers can be accessed in the 
  // following way:
  FrictionControllerRef limitFrictionController = ballJoint->getConeLimitFrictionControllerLimit();
  FrictionControllerRef rotationalFrictionController = ballJoint->getConeLimitFrictionControllerRotational();
 
  // They work similar to any other friction controller, we enable them and set friction coefficients as:
  limitFrictionController->setEnable(true);
  limitFrictionController->setFrictionCoefficient(0.1);
  rotationalFrictionController->setEnable(true);
  rotationalFrictionController->setFrictionCoefficient(0.1);
 
  // However, since you might want to set the friction coefficients to the same value for these controllers,
  // there exists a utility function to set them both at the same time. Also, there is a function to enable/disable
  // them both.
  ballJoint->setEnableConeLimitFriction(true);
  ballJoint->setConeLimitFrictionCoefficients(0.1);
 
  // If we now give the rod a little bit of velocity so that it falls over, we should see that it slows down as
  // it slides along the limit.
  rod->setAngularVelocity(0.2, 0.2, 0);
 
  return root;
}
 
 
 
osg::Group* buildTutorial3(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/)
{
  // Set up a similar scene as in the previous tutorial, with a ball and a rod attached to it with a ball joint.
  RigidBodyRef base = new RigidBody("base");
  Real radius = 0.5;
  base->add(new agxCollide::Geometry(new agxCollide::Sphere(radius)));
  simulation->add(base);
  base->setMotionControl(agx::RigidBody::STATIC);
 
  RigidBodyRef rod = new RigidBody("rod");
  agxCollide::GeometryRef rodGeom = new agxCollide::Geometry(new agxCollide::Cylinder(0.1, 1));
  rodGeom->setRotation(EulerAngles(agx::PI_2, 0, 0));
  rodGeom->setPosition(0, 0, 0.5);
  rod->add(rodGeom);
  simulation->add(rod);
 
  agxUtil::setEnableCollisions(base, rod, false);
 
  osg::Group* root = new osg::Group;
  agxOSG::createVisual(base, root);
  agxOSG::createVisual(rod, root);
 
  // Set up ball joint
  FrameRef f1 = new Frame();
  FrameRef f2 = new Frame();
  BallJointRef ballJoint = new BallJoint(rod, f1, base, f2);
  simulation->add(ballJoint);
 
  // There are three FrictionControllers outside of the ones connected to the cone limit. These work in 
  // the same way as a normal rotational FrictionController (that you might see on a hinge for example).
  // The only difference is that the ball joint has three rotational degrees of freedom, and thus we
  // must have one FrictionController for each of them. The FrictionControllers can be accessed separately,
  // for each direction, as:
  FrictionControllerRef frictionControllerU = ballJoint->getRotationalFrictionControllerU();
  FrictionControllerRef frictionControllerV = ballJoint->getRotationalFrictionControllerV();
  FrictionControllerRef frictionControllerN = ballJoint->getRotationalFrictionControllerN();
 
  // However, there exists some utility functions on the ball joint, to make it easier to use them.
  // To enable all of them:
  ballJoint->setEnableRotationalFriction(true);
  // To set the same friction coefficient to all of them:
  ballJoint->setRotationalFrictionCoefficients(0.3);
  // To read the friction coefficients from the controllers:
  RealVector frictionCoeffs = ballJoint->getRotationalFrictionCoefficients();
 
  // Giving the rod some velocity, we should now see that it does not move as much as without the
  // friction controllers.
  rod->setAngularVelocity(0.2, 0.2, 0);
 
  return root;
}
 
 
 
osg::Group* buildTutorial4(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  // Set up a similar scene as in the previous tutorial, but let us use boxes instead, since it is much
  // easier to see when they rotate
  RigidBodyRef base = new RigidBody("base");
  Real side = 0.3;
  base->add(new agxCollide::Geometry(new agxCollide::Box(side, side, side)));
  base->setPosition(0, 0, -side);
  simulation->add(base);
  base->setMotionControl(agx::RigidBody::STATIC);
 
  RigidBodyRef rod = new RigidBody("rod");
  agxCollide::GeometryRef rodGeom = new agxCollide::Geometry(new agxCollide::Box(0.1, 0.1, 0.5));
  rodGeom->setPosition(0, 0, 0.5);
  rod->add(rodGeom);
  simulation->add(rod);
  agxUtil::setEnableCollisions(base, rod, false);
 
  // We enable debug rendering, since it makes it easier to see the bodies moving
  application->setEnableDebugRenderer(true);
 
  // Set up ball joint
  FrameRef f1 = new Frame();
  FrameRef f2 = new Frame();
  f2->setTranslate(0, 0, side);
  BallJointRef ballJoint = new BallJoint(rod, f1, base, f2);
  simulation->add(ballJoint);
 
  // Now, let us enable the twist range. The twist range set a range on how much the "rod" body can rotate around 
  // its own z-axis.
  TwistRangeControllerRef twistRange = ballJoint->getTwistRangeController();
  twistRange->setEnable(true);
  // By default, the range is not set, so let us set it to some value.
  // Note that min value must be larger than -pi and largest value smaller than pi.
  // Setting the angles to the same value will create a lock
  twistRange->setRange(-PI_4, PI_4);
 
  // NOTE! The TwistRangeController fails when the z-axis of the frame is exactly opposite the z-axis of the reference 
  // frame. Normally, this constraint is intended to be used together with the cone limit, where this cannot happen, 
  // but in this example it is not possible for it to go to that position, so it is fine to use it without the
  // cone limit.
 
  // Now, let's also create a box, which we will attach to the other end of the rod
  RigidBodyRef hingeBox = new RigidBody("hingeBox");
  hingeBox->add(new agxCollide::Geometry(new agxCollide::Box(0.2, 0.2, 0.2)));
  simulation->add(hingeBox);
  hingeBox->setMotionControl(agx::RigidBody::STATIC);
  agxUtil::setEnableCollisions(hingeBox, rod, false);
  hingeBox->setPosition(0, 0, 1.2);
 
  // Create hinge
  HingeFrame hingeFrame = HingeFrame(Vec3(0, 0, 1), Vec3::Z_AXIS());
  HingeRef hinge = new Hinge(hingeFrame, rod, hingeBox);
  simulation->add(hinge);
 
  // Enable the motor on the hinge and set a velocity. Since we don't want this motor to be stronger than our
  // twist range controller, we make sure to make it a bit weaker.
  auto motor = hinge->getMotor1D();
  motor->setEnable(true);
  motor->setSpeed(-0.5);
  motor->setForceRange(-100, 100);
 
  // When we start the simulation, we can see that the "rod" is allowed to rotate until it reaches the end of the range.
  // The TwistRangeController is very similar to a range on a hinge, except that there is a limitation of how large the
  // twist range can be.
 
  return new osg::Group();
}
 
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout <<
                "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
                "\tBall Joint Limits tutorial\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene(buildTutorial1, '1');
    application->addScene(buildTutorial2, '2');
    application->addScene(buildTutorial3, '3');
    application->addScene(buildTutorial4, '4');
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;               
    return 1;
  }
 
  return 0;
}

tutorial_basicSimulation.cpp

/*
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.
*/
#include <agx/version.h>
#include <agxSDK/Simulation.h>
 
int main( int /*argc*/, char** /*argv*/ )
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tBasic simulation tutorial\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
 
    // Create a new simulation. This object will create all the fundamental components needed
    // for a physics simulation - such as a DynamicsSystem (including the default solver),
    // Space (for collision handling) and the event system.
    std::cerr << "Creating a simulation" << std::endl;
    agxSDK::SimulationRef mySimulation = new agxSDK::Simulation();
 
    // Create a new rigid body.
    std::cerr << "Creating a body" << std::endl;
    agx::RigidBodyRef rb = new agx::RigidBody();
 
    // Add the rigid body to the simulation.
    std::cerr << "Adding body to the simulation" << std::endl;
    mySimulation->add( rb );
 
    // Default time step is 1/60 s (60 Hz). Set the simulation frequency to 100 Hz.
    // Dynamics and time stepping is part of the dynamics system, and the dynamics
    // system includes a so called "time governor" to handle time.
    agx::Real dt = 1.0f / 100;
    mySimulation->getDynamicsSystem()->getTimeGovernor()->setTimeStep( dt );
 
    // Step for three seconds in 100 Hz.
    std::cerr << "Step system 3 seconds forward in time" << std::endl;
    for ( agx::Real t = 0; t <= 3; t += dt )
      mySimulation->stepForward();
 
    // The for-loop above can be replaced by a call to:
    // mySimulation->stepTo( 3 );
    // Step system forward (with the time step as specified in dt)
    // for three (3) seconds forward.
    mySimulation->stepTo( mySimulation->getTimeStamp() + 3 );
 
    // Clean the simulation object (remove bodies, constraints, geometries, listeners, assemblies)
    // Not really necessary, will be done when constructor for Simulation is executed.
    mySimulation->cleanup( agxSDK::Simulation::CLEANUP_ALL );
 
    // Create a new simulation.
    mySimulation = new agxSDK::Simulation();
 
    // The default solver can run on several threads. Make sure the number of threads doesn't exceeds the
    // number of physical cores available.
    agx::setNumThreads( 4 );
  }
  catch(std::runtime_error& e)
  {
    std::cerr << "Caught exception: " << e.what() << std::endl;
  }
  
  // This call should be done before exit of main scope to prevent hanging threads.
  // Will stop running threads, deallocate all memory used by AGX in Singletons, loaded plug-ins etc...
  std::cerr << "Shutting down AGX" << std::endl;
 
  agx::shutdown();
 
  return 0;
}

tutorial_beam.cpp

/*
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.
*/
 
/*
 
Demonstrates how to use the agxModel::Beam class for simulating deformable beams.
 
*/
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/WireRenderer.h>
#include <agxOSG/utils.h>
 
#include <agxUtil/StepEventCallback.h>
 
#include <agxModel/Beam.h>
 
#include <agxWire/Wire.h>
 
#include <agxRender/RenderSingleton.h>
 
#include <agx/version.h>
 
#include <agxCollide/Box.h>
#include <agxCollide/Cylinder.h>
 
namespace utils
{
  template<typename T>
  void createVisual( T obj,
                     osg::Group* root,
                     agxRender::Color color = agxRender::Color::RosyBrown() )
  {
    agxOSG::setDiffuseColor( agxOSG::createVisual( obj, root ), color );
  }
}
 
osg::Group* buildTutorial1( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  auto root = new osg::Group();
 
  // The material (agx::Material) of the beam carries density.
  // A material may be shared between many beam instances.
  agx::MaterialRef steelBeamMaterial = new agx::Material( "Steel" );
  steelBeamMaterial->getBulkMaterial()->setDensity( 7800.0 );
 
  // Beam properties has the other material parameters, such as Young's
  // modulus, Poisson's ratio and damping time. The beam properties may
  // be shared between several beam models.
  agxModel::BeamModelPropertiesRef steelBeamProperties = new agxModel::BeamModelProperties();
  steelBeamProperties->setYoungsModulus( 210.0E9 );
  steelBeamProperties->setPoissonsRatio( 0.310 );
 
  // Beam models describes the geometry and by that indirectly the material/stiffness
  // properties of a beam. Beam models may be shared between many beam instances and
  // carries the beam model properties instance created above.
 
  // Rectangular model:
  //     Width: 0.13 m
  //     Height: 0.15 m
  agxModel::RectangularBeamRef steelRectangularModel = new agxModel::RectangularBeam( /* width */ 0.13,
                                                                                      /* height */0.15,
                                                                                      /* properties */ steelBeamProperties );
  // Hollow rectangular model:
  //     Width: 0.14 m
  //     Height: 0.17 m
  //     Thickness: 0.007 m
  agxModel::HollowRectangularBeamRef steelHollowRectangularModel = new agxModel::HollowRectangularBeam( /* width */ 0.14,
                                                                                                        /* height */ 0.17,
                                                                                                        /* thickness */ 7.0E-3,
                                                                                                        /* properties */ steelBeamProperties );
  // I-beam model:
  //     Width: 0.10 m
  //     Height: 0.14 m
  //     Flange thickness: 0.007 m
  //     Web thickness: 0.009 m
  agxModel::IBeamRef steelIBeamModel = new agxModel::IBeam( /* width */ 0.10,
                                                            /* height */ 0.14,
                                                            /* flange thickness */ 7.0E-3,
                                                            /* web thickness */ 9.0E-3,
                                                            /* properties */ steelBeamProperties );
  // Circular model:
  //     Radius: 0.08 m
  agxModel::CircularBeamRef steelCircularModel = new agxModel::CircularBeam( /* radius */ 0.08,
                                                                             /* properties */ steelBeamProperties );
  // Hollow circular model:
  //     Radius: 0.08 m
  //     Thickness: 0.012 m
  agxModel::HollowCircularBeamRef steelHollowCircularModel = new agxModel::HollowCircularBeam( /* radius */ 0.08,
                                                                                               /* thickness */ 12.0E-3,
                                                                                               /* properties */ steelBeamProperties );
 
  // A beam creates its segments along the local z axis of the beam. Each segment (rigid body)
  // is oriented with the beam frame which is +z forward, +y right and +x up.
  //
  //            x
  //            |  z
  //            | /
  // BEAM FRAME |/____ y
  //
  // The start of the first segment is at the origin of the beam frame and the end position
  // of the last segment is at (0, 0, length) of the beam frame, where 'length' is given
  // when the beam is constructed. I.e., creating and initializing a beam, without changing
  // the beam frame, will create a beam along the z axis in world with the beam start position
  // at the world origin and end position at world (0, 0, length). Up of the beam will be along
  // world -x and right direction along world -y.
  //
  // Fortunately there are utility methods that calculates the transform (given start position
  // and beam forward in world), rotation (given beam forward in world) and complete initialization
  // given start-, end positions and desired beam up direction (in world frame).
 
  using BeamModelPtrVector = agx::Vector<agxModel::BeamModel*>;
  BeamModelPtrVector steelBeamModels {
    steelRectangularModel,
    steelHollowRectangularModel,
    steelIBeamModel,
    steelCircularModel,
    steelHollowCircularModel
  };
 
  const agx::Real beamLength = 6.0;
  agx::Real yOffset = 0.0;
  for ( auto steelBeamModel : steelBeamModels ) {
    const auto beamColor = agxRender::Color::getColor( agx::hash( steelBeamModel->getClassName() ) );
 
    // 10 segment beam, along world x. Setting rotation and
    // position explicitly.
    agxModel::BeamRef steelBeam_10 = new agxModel::Beam( steelBeamModel, 10u, beamLength );
    // Rotate from +z (default) to +x. Note that rotating the
    // beam to, e.g., +y will result in the beam up to be along
    // -x and right along +z, see utility methods below if this
    // is confusing.
    steelBeam_10->setRotation( agx::Quat::rotate( agx::Vec3::Z_AXIS(), agx::Vec3::X_AXIS() ) );
    steelBeam_10->setPosition( 0.0, yOffset, 0.0 );
    steelBeam_10->setMaterial( steelBeamMaterial );
    // Initializes the beam and adds all bodies and constraints to the simulation.
    simulation->add( steelBeam_10 );
    // Attaching begin (cantilever).
    simulation->add( steelBeam_10->attachBegin().release() );
    utils::createVisual( steelBeam_10, root, beamColor );
 
    yOffset += 0.5;
 
    // 15 segment beam, along world x. Setting rotation using utility method
    // calculateRotation given beam world forward and beam world up.
    agxModel::BeamRef steelBeam_15 = new agxModel::Beam( steelBeamModel, 15u, beamLength );
    steelBeam_15->setRotation( agxModel::Beam::calculateRotation( /* world beam forward */ agx::Vec3::X_AXIS(),
                                                                  /* world beam up */ agx::Vec3::Z_AXIS() ) );
    steelBeam_15->setPosition( 0, yOffset, 0 );
    steelBeam_15->setMaterial( steelBeamMaterial );
    simulation->add( steelBeam_15 );
    simulation->add( steelBeam_15->attachBegin().release() );
    utils::createVisual( steelBeam_15, root, beamColor );
 
    yOffset += 0.5;
 
    // 20 segment beam, along world x. Setting transform using utility method
    // calculate transform given beam start position, beam forward and beam
    // up axis, all in world coordinate frame.
    agxModel::BeamRef steelBeam_20 = new agxModel::Beam( steelBeamModel, 20u, beamLength );
    steelBeam_20->setTransform( agxModel::Beam::calculateTransform( /* world start position */ agx::Vec3( 0, yOffset, 0 ),
                                                                    /* world beam forward */ agx::Vec3::X_AXIS(),
                                                                    /* world beam up */ agx::Vec3::Z_AXIS() ) );
    steelBeam_20->setMaterial( steelBeamMaterial );
    simulation->add( steelBeam_20 );
    simulation->add( steelBeam_20->attachBegin().release() );
    utils::createVisual( steelBeam_20, root, beamColor );
 
    yOffset += 0.5;
 
    // 25 segment beam, along world x. Initializing the beam using utility method
    // create given start-, end positions, model, resolution and up axis (all in
    // world coordinate frame).
    agxModel::BeamRef steelBeam_25 = agxModel::Beam::create( /* world start position */ agx::Vec3( 0, yOffset, 0 ),
                                                             /* world end position */ agx::Vec3( beamLength, yOffset, 0 ),
                                                             /* beam model */ steelBeamModel,
                                                             /* beam resolution */ 25u,
                                                             /* world beam up */ agx::Vec3::Z_AXIS() );
    steelBeam_25->setMaterial( steelBeamMaterial );
    simulation->add( steelBeam_25 );
    simulation->add( steelBeam_25->attachBegin().release() );
    utils::createVisual( steelBeam_25, root, beamColor );
 
    yOffset += 1.0;
  }
 
  // Printing mass and resolution of all beams in the simulation.
  const auto beams = agxModel::Beam::findAll( simulation );
  for ( const auto beam : beams )
    std::cout << beam->getModel()->getClassName() <<
                 ": Mass = " << beam->getMass() << " kg, Resolution = " << beam->getNumSegments() << std::endl;
 
  auto cameraData = application->getCameraData();
  cameraData.eye = agx::Vec3( -7.5046, -3.6649, 3.8471 );
  cameraData.center = agx::Vec3( 4.1953, 5.8053, 0.2871 );
  cameraData.up = agx::Vec3( 0.1990, 0.1198, 0.9727 );
  cameraData.nearClippingPlane = 0.1;
  cameraData.farClippingPlane = 5000;
  application->applyCameraData( cameraData );
 
  application->getSceneDecorator()->setBackgroundColor( agxRender::Color::SkyBlue(), agxRender::Color::DodgerBlue() );
 
  return root;
}
 
namespace utils
{
  agxSDK::AssemblyRef createGuideSupport( agx::Real radius,
                                          agx::UInt resolution,
                                          agx::Real L,
                                          agx::Real a,
                                          agx::Bool guideIsAttachmentParent,
                                          osg::Group* root )
  {
    agxSDK::AssemblyRef assembly = new agxSDK::Assembly();
 
    const auto supportRadius = 0.85 * radius;
    const auto guideThickness = 5.0E-2 * radius;
    const auto supportThickness = 5.0E-2 * supportRadius;
    const auto supportResolution = agx::UInt( agx::Real( resolution ) * a + 0.5 );
 
    agxModel::BeamModelPropertiesRef properties = new agxModel::BeamModelProperties( 190.0E9,
                                                                                     0.31 );
    agxModel::HollowCircularBeamRef guideModel = new agxModel::HollowCircularBeam( radius,
                                                                                   guideThickness,
                                                                                   properties );
    agxModel::HollowCircularBeamRef supportModel = new agxModel::HollowCircularBeam( supportRadius,
                                                                                     supportThickness,
                                                                                     properties );
 
    agxModel::BeamRef guideBeam = agxModel::Beam::create( agx::Vec3( 0, 0, 0 ),
                                                          agx::Vec3( 0, 0, L ),
                                                          guideModel,
                                                          resolution );
    guideBeam->setName( "guide" );
    assembly->add( guideBeam );
 
    const auto supportBeginPosition = guideBeam->getBeginPosition() -
                                      ( 1.0 - a ) * L * agx::Vec3::X_AXIS();
    auto attachmentGuideSegment = guideBeam->getSegment( a * L );
    const auto attachmentGuideFrame = attachmentGuideSegment->getRigidBody()->getFrame();
    // Distance along the local z axis of the guide beam segment that
    // we're attaching the support beam.
    const auto localAttachmentLength = a * L -
                                       // Rest length to the start of the segment.
                                       guideBeam->findRestLengthTo( attachmentGuideSegment );
    const auto supportEndPosition = attachmentGuideFrame->transformPointToWorld( guideModel->getRadius(),
                                                                                 0.0,
                                                                                 localAttachmentLength );
 
    agxModel::BeamRef supportBeam = agxModel::Beam::create( supportBeginPosition,
                                                            supportEndPosition,
                                                            supportModel,
                                                            supportResolution );
    supportBeam->setName( "support" );
    assembly->add( supportBeam );
 
    agxUtil::setEnableCollisions( guideBeam, supportBeam, false );
 
    agx::LockJointRef guideSupportAttachment = nullptr;
    // If true, we create the attachment given 'guideBeam', so the
    // attachment frames will align with the guide beam frame.
    if ( guideIsAttachmentParent ) {
      guideSupportAttachment = guideBeam->attach( attachmentGuideSegment,
                                                  agx::Vec3( guideModel->getRadius(),
                                                             0,
                                                             localAttachmentLength ),
                                                  // Support beam last segment rigid body is
                                                  // attached to the guide beam.
                                                  supportBeam->getSegments().back()->getRigidBody() );
      // Applying constraint properties (compliance and damping) of the
      // guide beam model to the attachment, but twice as stiff when the
      // attachment can be assumed to be more rigid.
      guideBeam->calculateSegmentStiffnessDamping().apply( guideSupportAttachment, 2.0 );
    }
    // If false, we create the attachment given 'supportBeam' and
    // the attachment frames will align with the support beam frame.
    else {
      guideSupportAttachment = supportBeam->attachEnd( attachmentGuideSegment->getRigidBody() );
      // Applying constraint properties (compliance and damping) of the
      // support beam model to the attachment, but twice as stiff when the
      // attachment can be assumed to be more rigid.
      supportBeam->calculateSegmentStiffnessDamping().apply( guideSupportAttachment, 2.0 );
    }
    guideSupportAttachment->setName( "attachment" );
    assembly->add( guideSupportAttachment );
 
    agxOSG::setDiffuseColor( agxOSG::createVisual( guideBeam, root ), agxRender::Color::Goldenrod() );
    agxOSG::setDiffuseColor( agxOSG::createVisual( supportBeam, root ), agxRender::Color::LightSkyBlue() );
 
    return assembly;
  }
}
 
osg::Group* buildTutorial2( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  auto root = new osg::Group();
 
  agxCollide::GeometryRef ground = new agxCollide::Geometry( new agxCollide::Box( 5.0, 5.0, 0.15 ) );
  ground->setPosition( 0, 0, -0.15 );
  simulation->add( ground );
  utils::createVisual( ground, root, agxRender::Color::DarkGreen() );
 
  // Adding 10 000 kg payload to either begin or end of a beam. The payload
  // is attached to the beam with a modified lock joint (as ball joint).
  const auto addPayload = [simulation, root]( agxModel::Beam* beam, agx::Bool atBegin )
  {
    agx::RigidBodyRef payload = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 0.25,
                                                                                                   0.25,
                                                                                                   0.25 ) ) );
    agxModel::BeamSegment* segment = ( atBegin ?
                                         beam->getSegments().front() :
                                         beam->getSegments().back() );
    const auto payloadPosition = ( atBegin ? segment->getBeginPosition() : segment->getEndPosition() ) -
                                 1.0 * agx::Vec3::Z_AXIS();
    payload->setPosition( payloadPosition );
    payload->getMassProperties()->setMass( 10.0E3 );
    payload->setLinearVelocityDamping( 0.3f );
    simulation->add( payload );
    utils::createVisual( payload, root, agxRender::Color::Black() );
 
    agx::LockJointRef attachment = beam->attach( segment, atBegin, payload );
    // Disabling rotational part of the lock joint, making it a ball joint.
    attachment->getElementaryConstraint( 1u )->setEnable( false );
    simulation->add( attachment );
  };
 
  // Beam properties used in all beams.
  agxModel::BeamModelPropertiesRef beamProperties = new agxModel::BeamModelProperties( 120.0E9, 0.33 );
 
  // Creating tilted circular beam with 10 segments, starting at ground.
  agxModel::BeamRef beamStartGround = agxModel::Beam::create( agx::Vec3( 0, -2, 0 ),
                                                              agx::Vec3( 4, -2, 4 ),
                                                              new agxModel::CircularBeam( 0.10, beamProperties ),
                                                              10u );
  simulation->add( beamStartGround );
  utils::createVisual( beamStartGround, root, agxRender::Color::AliceBlue() );
 
  // Attaching begin of the beam with ground (world) and disable collisions.
  // We're applying the stiffness properties of the model to the attachment
  // which makes the slope/deflection of the beam less dependent on the
  // resolution.
  agx::LockJointRef beamStartGroundAttachment = beamStartGround->attachBegin( nullptr, true );
  //beamStartGroundAttachment->setCompliance( 1.0E-12 );
  // The beam doesn't add or track the attachment, only creates it, so
  // we have to add it to the simulation.
  simulation->add( beamStartGroundAttachment );
  beamStartGround->setEnableCollisions( ground, false );
 
  // Adding payload at end of the beam.
  addPayload( beamStartGround, false );
 
  // Similar setup as beamStartGround but this beam has 30 segments and
  // ends at ground.
  agxModel::BeamRef beamEndGround = agxModel::Beam::create( agx::Vec3( 4, -1, 4 ),
                                                            agx::Vec3( 0, -1, 0 ),
                                                            beamStartGround->getModel(),
                                                            30u );
  simulation->add( beamEndGround );
  utils::createVisual( beamEndGround, root, agxRender::Color::AliceBlue() );
 
  // Attaching end of the beam with ground (world) and disable collisions.
  agx::LockJointRef beamEndGroundAttachment = beamEndGround->attachEnd( nullptr, true );
  //beamEndGroundAttachment->setCompliance( 1.0E-12 );
  simulation->add( beamEndGroundAttachment );
  beamEndGround->setEnableCollisions( ground, false );
 
  // Adding payload to the begin of the beam.
  addPayload( beamEndGround, true );
 
  // Creating a guide with two beams attached with ground and each other.
  auto guideSupportAssembly = utils::createGuideSupport( /* radius */ 0.08,
                                                         /* resolution */ 20u,
                                                         /* length */ 2.5,
                                                         /* length scale support attachment */ 0.70,
                                                         /* guide beam is parent in attachment */ false,
                                                         root );
  simulation->add( guideSupportAssembly );
 
  auto guideBeam = guideSupportAssembly->getAssembly( "guide" )->asSafe<agxModel::Beam>();
  auto supportBeam = guideSupportAssembly->getAssembly( "support" )->asSafe<agxModel::Beam>();
 
  agx::MaterialRef guideMaterial = new agx::Material( "guide" );
  guideMaterial->getBulkMaterial()->setDensity( 2700 );
  guideBeam->setMaterial( guideMaterial );
  supportBeam->setMaterial( guideMaterial );
 
  // Positioning before attaching the beams to the ground/world.
  guideSupportAssembly->setPosition( -1, 2, 0 );
  simulation->add( guideBeam->attachBegin().release() );
  simulation->add( supportBeam->attachBegin().release() );
  guideBeam->setEnableCollisions( ground, false );
  supportBeam->setEnableCollisions( ground, false );
 
  agx::RigidBodyRef heavyBox = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 1.5, 2.0, 1.5 ) ) );
  heavyBox->setPosition( guideBeam->getEndPosition() + agx::Vec3( 4, 0, 1 ) );
  heavyBox->setRotation( agx::Quat::rotate( agx::degreesToRadians( -20 ), agx::Vec3::Y_AXIS() ) );
  simulation->add( heavyBox );
  utils::createVisual( heavyBox, root, agxRender::Color::GhostWhite() );
 
  agx::MaterialRef heavyBoxMaterial = new agx::Material( "HeavyBox" );
  heavyBoxMaterial->getBulkMaterial()->setDensity( 1500 );
  agxUtil::setBodyMaterial( heavyBox, heavyBoxMaterial );
 
  agxWire::WireRef wire = new agxWire::Wire( 0.02, 2.0, false );
  wire->add( new agxWire::BodyFixedNode( heavyBox, 0, 0, 1.5 ) );
  wire->add( new agxWire::BodyFixedNode( nullptr, heavyBox->getFrame()->transformPointToWorld( 0, 0, 5.0 ) ) );
  simulation->add( wire );
  simulation->add( new agxOSG::WireRenderer( wire, root ) );
 
  auto guideHeavyBoxContactMaterial = simulation->getMaterialManager()->getOrCreateContactMaterial( guideMaterial, heavyBoxMaterial );
  guideHeavyBoxContactMaterial->setYoungsModulus( 100.0E9 );
  guideHeavyBoxContactMaterial->setUseContactAreaApproach( true );
 
  auto cameraData = application->getCameraData();
  cameraData.eye = agx::Vec3( -7.5857, -19.7662, 9.2489 );
  cameraData.center = agx::Vec3( 0.5826, -0.4139, 2.5559 );
  cameraData.up = agx::Vec3( 0.1333, 0.2732, 0.9527 );
  cameraData.nearClippingPlane = 0.1;
  cameraData.farClippingPlane = 5000;
  application->applyCameraData( cameraData );
  
  application->getSceneDecorator()->setBackgroundColor( agxRender::Color::SkyBlue(), agxRender::Color::DodgerBlue() );
 
  return root;
}
 
osg::Group* buildTutorial3( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  auto root = new osg::Group();
 
  // Outward angle of the supports. Different angles makes the
  // impact of the large sphere different. Try e.g., 0, 30, 45, 90.
  const auto supportOutwardRotationDeg = 30.0;
 
  // Ground "hill" box rotated 20 degrees.
  agxCollide::GeometryRef ground = new agxCollide::Geometry( new agxCollide::Box( 8, 3, 0.25 ) );
  ground->setPosition( 0, 0, -0.25 );
  ground->setRotation( agx::Quat::rotate( agx::degreesToRadians( -20.0 ), agx::Vec3::Y_AXIS() ) );
  simulation->add( ground );
  agxOSG::createVisual( ground, root );
 
  // Creating the first guide with support. The attachment constraint between
  // the guide beam and the support beam is created using the guide beam frame.
  auto guide1Assembly = utils::createGuideSupport( 0.07, 30u, 2.5, 0.6, true, root );
  auto guide1 = guide1Assembly->getAssembly( "guide" )->asSafe<agxModel::Beam>();
  auto support1 = guide1Assembly->getAssembly( "support" )->asSafe<agxModel::Beam>();
  guide1Assembly->setPosition( 0, 1.0, 0 );
  guide1Assembly->setRotation( agx::Quat::rotate( agx::degreesToRadians( -supportOutwardRotationDeg ), agx::Vec3::Z_AXIS() ) *
                               guide1Assembly->getRotation() *
                               ground->getRotation() );
 
  // Creating the second guide with support. The attachment constraint between
  // the guide beam and the support beam is created using the support beam frame.
  auto guide2Assembly = utils::createGuideSupport( 0.07, 30u, 2.5, 0.6, false, root );
  auto guide2 = guide2Assembly->getAssembly( "guide" )->asSafe<agxModel::Beam>();
  auto support2 = guide2Assembly->getAssembly( "support" )->asSafe<agxModel::Beam>();
  guide2Assembly->setPosition( 0, -1.0, 0 );
  guide2Assembly->setRotation( agx::Quat::rotate( agx::degreesToRadians( supportOutwardRotationDeg ), agx::Vec3::Z_AXIS() ) *
                               guide2Assembly->getRotation() *
                               ground->getRotation() );
 
  simulation->add( guide1Assembly );
  simulation->add( guide2Assembly );
 
  // Debug rendering the attachment constraint frames.
  auto guideSupportAttachmentA = guide1Assembly->getConstraint( "attachment" );
  auto guideSupportAttachmentB = guide2Assembly->getConstraint( "attachment" );
  agxRender::debugRenderConstraintFrames( guideSupportAttachmentA, 0.85f, nullptr, true );
  agxRender::debugRenderConstraintFrames( guideSupportAttachmentB, 0.85f, nullptr, true );
 
  // Attaching the guides to the ground/world and disable collisions.
  simulation->add( guide1->attachBegin().release() );
  simulation->add( guide2->attachBegin().release() );
  simulation->add( support1->attachBegin().release() );
  simulation->add( support2->attachBegin().release() );
  guide1->setEnableCollisions( ground, false );
  guide2->setEnableCollisions( ground, false );
  support1->setEnableCollisions( ground, false );
  support2->setEnableCollisions( ground, false );
 
  // Creating the bug sphere to roll down the hill towards the guides.
  agx::RigidBodyRef sphere = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Sphere( 2.0 ) ) );
  sphere->setPosition( ground->getFrame()->transformPointToWorld( 5.0, 0, 2.0 + 0.25 ) );
  simulation->add( sphere );
  agxOSG::setDiffuseColor( agxOSG::createVisual( sphere, root ), agxRender::Color::Black() );
 
  // The beams has default properties, create new and assign to the models.
  agxModel::BeamModelPropertiesRef guideProperties = new agxModel::BeamModelProperties( 190.0E9, 0.31 );
  guide1->getModel()->setProperties( guideProperties );
  guide2->getModel()->setProperties( guideProperties );
  support1->getModel()->setProperties( guideProperties );
  support2->getModel()->setProperties( guideProperties );
 
  agx::MaterialRef guideMaterial = new agx::Material( "guide" );
  guideMaterial->getBulkMaterial()->setDensity( 2700 );
  guide1->setMaterial( guideMaterial );
  guide2->setMaterial( guideMaterial );
  support1->setMaterial( guideMaterial );
  support2->setMaterial( guideMaterial );
 
  agx::MaterialRef sphereMaterial = new agx::Material( "cylinder" );
  agxUtil::setBodyMaterial( sphere, sphereMaterial );
 
  auto contactMaterial = simulation->getMaterialManager()->getOrCreateContactMaterial( guideMaterial, sphereMaterial );
  contactMaterial->setYoungsModulus( 4.0E10 );
  contactMaterial->setRestitution( 0.0 );
  contactMaterial->setUseContactAreaApproach( true );
 
  auto cameraData = application->getCameraData();
  cameraData.eye = agx::Vec3( -8.3369, -13.6958, 3.6122 );
  cameraData.center = agx::Vec3( 1.3613, -0.9364, 1.9912 );
  cameraData.up = agx::Vec3( 0.0056, 0.1218, 0.9925 );
  cameraData.nearClippingPlane = 0.1;
  cameraData.farClippingPlane = 5000;
  application->applyCameraData( cameraData );
 
  application->getSceneDecorator()->setBackgroundColor( agxRender::Color::SkyBlue(), agxRender::Color::DodgerBlue() );
 
  return root;
}
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout << "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial Beam (agxModel::Beam)\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
    application->addScene( buildTutorial3, '3' );
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_bodies.cpp

/*
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.
*/
 
#include <agxOSG/ExampleApplication.h>
 
#include <agx/version.h>
#include <agx/ForceField.h>
#include "tutorialUtils.h"
 
/*
 
Demonstrates how to use RigidBodies for simulation:
 
- Add/remove bodies, geometries to a simulation.
- Set linear/angular velocity on bodies.
- Set mass properties: mass/inertia.
- Demonstrates the use of model/CM coordinate systems.
- Create bodies with multiple geometries.
- Use agx::Frame to tranform vectors and points
- Use ForceFields to affect bodies.
 
*/
 
osg::Group* buildTutorial1( agxSDK::Simulation *simulation, agxOSG::ExampleApplication * /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create the dynamic rigid body (carries mass properties and interface to dynamic properties)
  agx::RigidBodyRef dynamicRB = new agx::RigidBody();
 
  // Without a geometry, dynamicRB will just fall in the gravitational field. So create a geometry
  // and add that geometry to the rigid body.
  agxCollide::GeometryRef dynamicRBGeometry = new agxCollide::Geometry();
 
  // The geometry object is a place holder for shapes. Shapes (that aren't internal, i.e., belongs
  // to the same body) can collide with each other.
 
  // Create a box shape, half extent 0.5x0.5x0.5 m (gives dimension 1x1x1 m).
  agxCollide::BoxRef dynamicRBBox = new agxCollide::Box( agx::Vec3( 0.5, 0.5, 0.5 ) );
 
  // Add the shape to geometry. The local transform of the geometry is identity at this point, which
  // means that the center of the box will coincide with the center of mass of the rigid body.
  dynamicRBGeometry->add( dynamicRBBox );
 
  // Add the geometry to the rigid body, telling that if this geometry collides with other geometries,
  // this rigid body will take the forces from these collisions.
  dynamicRB->add( dynamicRBGeometry );
 
  // As a last step we have to add the rigid body to the simulation.
  simulation->add( dynamicRB );
 
  // For rendering, associate visual geometries to the physical geometries in this rigid body
  agxOSG::createVisual( dynamicRB, root );
 
  // Create the static rigid body, same procedure as above, just some other dimensions
  agx::RigidBodyRef staticRB = new agx::RigidBody();
  staticRB->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 5.0, 5.0, 0.2 ) ) ) );
  simulation->add( staticRB );
 
  // We haven't flagged the rigid body to be static yet, by default it's dynamic. To change and to
  // set this flag is easy, in AGX it's called motion control.
  staticRB->setMotionControl( agx::RigidBody::STATIC );
 
  // For rendering, associate visual geometries to the physical geometries in this rigid body
  agxOSG::createVisual( staticRB, root );
 
  // Both rigid bodies have the same position right now, so position the dynamic rigid body a bit
  // above the static body.
 
  // Default view when you run this tutorial is (before the camera has been moved/rotated):
  // positive x: right
  // positive y: into your screen
  // positive z: up
 
  dynamicRB->setPosition( agx::Vec3( 0, 0, 5 ) );
  // And just move the static body down so that the surface is at exactly z = 0. Remember that the
  // thickness, half extent, of the static body is 2 decimeters.
  staticRB->setPosition( agx::Vec3( 0, 0, -0.2 ) );
 
  return root;
}
 
osg::Group* buildTutorial2( agxSDK::Simulation *simulation, agxOSG::ExampleApplication * /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create the exact same scene as in tutorial 1.1 (the short way) ********************************
  agx::RigidBodyRef dynamicRB = new agx::RigidBody();
  dynamicRB->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.5, 0.5, 0.5 ) ) ) );
  simulation->add( dynamicRB );
 
  agx::RigidBodyRef staticRB = new agx::RigidBody();
  staticRB->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 5.0, 5.0, 0.2 ) ) ) );
  staticRB->setMotionControl( agx::RigidBody::STATIC );
  simulation->add( staticRB );
 
  agxOSG::createVisual( dynamicRB, root );
  agxOSG::createVisual( staticRB, root );
 
  dynamicRB->setPosition( agx::Vec3( 0, 0, 5 ) );
  staticRB->setPosition( agx::Vec3( 0, 0, -0.2 ) );
  // *********************************************************************************************
 
  // Set the initial linear velocity of the dynamic body to some value
  dynamicRB->setVelocity( agx::Vec3( -2, 0, 0 ) );
  // Set the initial angular velocity of the dynamic body to some value
  dynamicRB->setAngularVelocity( agx::Vec3( 0, 1.5, 0 ) );
 
  // Also, change the rotation of the static body.
  // Set the rotation to be 45 degrees rotated about the world y axis (i.e., around the axis going
  // into the screen).
  agx::Quat newRotation( agx::degreesToRadians( 45 ), agx::Vec3( 0, 1, 0 ) );
  staticRB->setRotation( newRotation );
  
  return root;
}
 
osg::Group* buildTutorial3( agxSDK::Simulation *simulation, agxOSG::ExampleApplication * /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create a large static box (no rigid body, only geometry)
  agx::Vec3 boxGeometryHalfExtent( 10, 10, 0.2 );
  agxCollide::GeometryRef boxGeometry = new agxCollide::Geometry( new agxCollide::Box( boxGeometryHalfExtent ) );
 
  // We don't have to explicitly associate a geometry to a rigid body.
  // Add the geometry to the simulation.
  simulation->add( boxGeometry );
 
  // Fix the position of the geometry in world coordinates so that the surface is at z = 0.
  // (Makes it easier to position other objects on this surface.)
  boxGeometry->setPosition( boxGeometry->getPosition() - agx::Vec3( 0, 0, boxGeometryHalfExtent.z() ) );
 
  // Create visual representation of this geometry
  agxOSG::createVisual( boxGeometry, root );
 
  // Create the large sphere body
  agx::RigidBodyRef largeSphereRB = new agx::RigidBody();
 
  // Create the geometry sphere and add it to largeSphereRB
  agx::Real largeSphereRadius = 1.5;
  largeSphereRB->add( new agxCollide::Geometry( new agxCollide::Sphere( largeSphereRadius ) ) );
 
  // Position the large sphere to the right (+x) on the surface
  largeSphereRB->setPosition( agx::Vec3( 9, 0, largeSphereRadius ) );
 
  // Create the small sphere body
  agx::RigidBodyRef smallSphereRB = new agx::RigidBody();
 
  // Create the geometry sphere and add it to largeSphereRB
  agx::Real smallSphereRadius = 1.0;
  smallSphereRB->add( new agxCollide::Geometry( new agxCollide::Sphere( smallSphereRadius ) ) );
 
  // Position the large sphere to the left (-x) on the surface
  smallSphereRB->setPosition( agx::Vec3(-9, 0, smallSphereRadius ) );
 
  // Create visual representation and add the bodies to the simulation
  agxOSG::createVisual( largeSphereRB, root );
  agxOSG::createVisual( smallSphereRB, root );
 
  simulation->add( largeSphereRB );
  simulation->add( smallSphereRB );
 
  // Set the large sphere mass to 1 kg (Note that the inertia tensor will be recalculated given this mass).
  largeSphereRB->getMassProperties()->setMass( 1.0 );
 
  // Set the small sphere mass to 100 kg
  smallSphereRB->getMassProperties()->setMass( 100.0 );
 
  // The inertia in mass properties is given in the body coordinate frame. Given this, the interface often
  // only wants the diagonal (agx::Vec3), but it's possible to specify the inertia as a symmetric positive definite
  // matrix (agx::SPDMatrix3x3).
 
  // The inertia of a sphere is a diagonal matrix with the value:
  // I_sphere = 2/5 * mass * radius ^ 2
  // Which is 2/5 for the large sphere at this point. Set the diagonal to the half of that:
  largeSphereRB->getMassProperties()->setInertiaTensor( agx::Vec3( 1.0/5, 1.0/5, 1.0/5 ) );
 
  // Set the inertia of the small sphere to something large:
  smallSphereRB->getMassProperties()->setInertiaTensor( agx::Vec3( 100, 100, 100 ) );
 
  // Set the velocities so that the bodies will collide near world origin:
  largeSphereRB->setVelocity( agx::Vec3(-10, 0, 0 ) );
  smallSphereRB->setVelocity( agx::Vec3( 10, 0, 0 ) );
 
  return root;
}
 
osg::Group* buildTutorial4( agxSDK::Simulation *simulation, agxOSG::ExampleApplication *application )
{
  osg::Group* root = new osg::Group();
 
  // Create a large static box (no rigid body, only geometry)
  agx::Vec3 boxGeometryHalfExtent( 10, 10, 0.2 );
  agxCollide::GeometryRef boxGeometry = new agxCollide::Geometry( new agxCollide::Box( boxGeometryHalfExtent ) );
 
  // We don't have to explicitly associate a geometry to a rigid body.
  // Add the geometry to the simulation.
  simulation->add( boxGeometry );
 
  // Fix the position of the geometry in world coordinates so that the surface is at z = 0.
  // (Makes it easier to position other objects on this surface.)
  boxGeometry->setPosition( boxGeometry->getPosition() - agx::Vec3( 0, 0, boxGeometryHalfExtent.z() ) );
 
  // Create visual representation of this geometry
  agxOSG::createVisual( boxGeometry, root );
 
  // Create a dynamic rigid body (box, 1x1x3 m)
  agx::Vec3 dynamicsHalfExtent( 0.5, 0.5, 1.5 );
  agx::RigidBodyRef rb1 = new agx::RigidBody();
  rb1->add( new agxCollide::Geometry( new agxCollide::Box( dynamicsHalfExtent ) ) );
  simulation->add( rb1 );
  agxOSG::createVisual( rb1, root );
 
  // Above, we've added a geometry blindly to this rigid body.
  // We want to move the model frame origin to the bottom of this rigid body box.
  // To do this we simply add an offset to the geometry, position it 1.5 m up in
  // local body coordinates.  
  rb1->getGeometries().front()->setLocalPosition( agx::Vec3( 0, 0, dynamicsHalfExtent.z() ) );
 
  // The operation above moves the center of mass as well, but it's possible to tell
  // the body to recalculate the center of mass position by:
  rb1->updateMassProperties( agx::MassProperties::CM_OFFSET );
 
  // Now when we have that the model frame origin is on the bottom surface of the box
  // we can position this dynamic body precisely on the static geometry surface by setting:
  rb1->setPosition( agx::Vec3( 0, 0, 0 ) );
 
  // With this configuration, this call is equivalent to the one above (set position via center of mass frame):
  rb1->setCmPosition( agx::Vec3( 0, 0, dynamicsHalfExtent.z() ) );
 
  // Create another dynamic rigid body (box, 1x1x3 m)
  agx::RigidBodyRef rb2 = new agx::RigidBody();
  rb2->add( new agxCollide::Geometry( new agxCollide::Box( dynamicsHalfExtent ) ) );
  simulation->add( rb2 );
  agxOSG::createVisual( rb2, root );
 
  // Move the model origin as above.
  rb2->getGeometries().front()->setLocalPosition( agx::Vec3( 0, 0, dynamicsHalfExtent.z() ) );
 
  // Instead of calculating the correct CM offset we move the center of mass position to the top surface
  // of the box and one meter to the right (positive x).
  // Note that these operations are made in model/body frame coordinate system.
  rb2->getCmFrame()->setLocalTranslate( agx::Vec3( 1.0, 0, 2 * dynamicsHalfExtent.z() ) );
 
  // Last, position rb2 to the right, in world coordinates.
  rb2->setPosition( agx::Vec3( 3, 0, 0 ) );
 
  // Note that the box on the right will show a "strange behavior". It's not strange given how we've modeled
  // it, with the center of mass located above, outside that body.
  // Enabling debug rendering.
  application->setEnableDebugRenderer( true );
 
  return root;
}
 
osg::Group* buildTutorial5( agxSDK::Simulation *simulation, agxOSG::ExampleApplication * /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create a static box geometry as in tutorial 1.4.
  agx::Vec3 boxGeometryHalfExtent( 10, 10, 0.2 );
  agxCollide::GeometryRef boxGeometry = new agxCollide::Geometry( new agxCollide::Box( boxGeometryHalfExtent ) );
  boxGeometry->setPosition( boxGeometry->getPosition() - agx::Vec3( 0, 0, boxGeometryHalfExtent.z() ) );
  simulation->add( boxGeometry );
  agxOSG::createVisual( boxGeometry, root );
 
  // Create the rigid body
  agx::RigidBodyRef theABody = new agx::RigidBody();
 
  // When we add geometries, we only want the rigid body to update its mass and inertia tensor. I.e., the
  // center of mass position will not be recalculated before we tell it to do so.
  theABody->getMassProperties()->setAutoGenerateMask( agx::MassProperties::MASS | agx::MassProperties::INERTIA );
 
  /* We'll add geometries to theABody looking like:
       _
      /_\
     /   \
    We start with the box:
     /
    / 
   */
  agx::Vec3 sideHalfExtent( 0.3, 0.3, 1.5 );
  agxCollide::GeometryRef side1Geometry = new agxCollide::Geometry();
  side1Geometry->add( new agxCollide::Box( sideHalfExtent ) );
  // Add this geometry to the body:
  theABody->add( side1Geometry );
 
  // Rotate side1Geometry, say 25 degrees around body y:
  side1Geometry->setLocalRotation( agx::Quat( agx::degreesToRadians( 25 ), agx::Vec3( 0, 1, 0 ) ) );
  // And position it to the left (-x):
  agx::Vec3 sideLeftRightGeometryOffset( -1.0, 0, 0 );
  side1Geometry->setLocalPosition( sideLeftRightGeometryOffset );
 
  /* Same procedure for the other side:
       \ 
        \ . 
  */
  agxCollide::GeometryRef side2Geometry = new agxCollide::Geometry();
  side2Geometry->add( new agxCollide::Box( sideHalfExtent ) );
  theABody->add( side2Geometry );
 
  // The rotation is -25 degrees for this geometry.
  side2Geometry->setLocalRotation( agx::Quat( -agx::degreesToRadians( 25 ), agx::Vec3( 0, 1, 0 ) ) );
  // And it should be offset to the opposite side.
  side2Geometry->setLocalPosition( -sideLeftRightGeometryOffset );
 
  // Create the top of the A.
  agx::Vec3 topHalfExtent( 0.6, 0.3, 0.3 );
  agxCollide::GeometryRef topGeometry = new agxCollide::Geometry();
  topGeometry->add( new agxCollide::Box( topHalfExtent ) );
  theABody->add( topGeometry );
 
  // The position should be slightly up (+z)
  topGeometry->setLocalPosition( agx::Vec3( 0, 0, 1.2 ) );
 
  // The final geometry, transforming the body from looking as a capital Lambda to capital Alpha (or simply A)
  agx::Vec3 middleHalfExtent( 1.2, 0.3, 0.3 );
  agxCollide::GeometryRef middleGeometry = new agxCollide::Geometry();
  middleGeometry->add( new agxCollide::Box( middleHalfExtent ) );
  theABody->add( middleGeometry );
 
  // Re-calculate the CM position, now that we've modeled the A.
  theABody->updateMassProperties( agx::MassProperties::AUTO_GENERATE_ALL );
 
  // Position it to slightly above the static geometry.
  theABody->setPosition( agx::Vec3( 0, 0, 2.0 ) );
 
  simulation->add( theABody );
 
  agxOSG::createVisual( theABody, root );
 
  return root;
}
 
osg::Group* buildTutorial6( agxSDK::Simulation *simulation, agxOSG::ExampleApplication *application )
{
  osg::Group* root = new osg::Group();
 
  // Create a static box geometry as in tutorial 1.4.
  agx::Vec3 boxGeometryHalfExtent( 10, 10, 0.2 );
  agxCollide::GeometryRef boxGeometry = new agxCollide::Geometry( new agxCollide::Box( boxGeometryHalfExtent ) );
  boxGeometry->setPosition( boxGeometry->getPosition() - agx::Vec3( 0, 0, boxGeometryHalfExtent.z() ) );
  simulation->add( boxGeometry );
  agxOSG::createVisual( boxGeometry, root );
 
  // Create a dynamic rigid body box
  agx::Vec3 boxHalfExtent( 1, 1, 1 );
  agx::RigidBodyRef box1Body = new agx::RigidBody();
  box1Body->add( new agxCollide::Geometry( new agxCollide::Box( boxHalfExtent ) ) );
 
 
  // Rotate this body some degrees about all axes:
  agx::Quat boxRotate = agx::Quat( agx::degreesToRadians( 15 ), agx::Vec3( 1, 0, 0 ) ) *
                        agx::Quat( agx::degreesToRadians(  34  ), agx::Vec3( 0, 1, 0 ) ) *
                        agx::Quat( agx::degreesToRadians(  5  ), agx::Vec3( 0, 0, 1 ) );
  box1Body->setRotation( boxRotate );
 
  // Position box1Body at some  position:
  box1Body->setPosition( agx::Vec3( 6, 4, 3 ) );
 
  // Create another dynamic rigid body box of the same size as the one above.
  agx::RigidBodyRef box2Body = new agx::RigidBody();
  box2Body->add( new agxCollide::Geometry( new agxCollide::Box( boxHalfExtent ) ) );
 
  // We want to position this body mathematically on the surface of the first box.
  // The rotation is equal:
  box2Body->setRotation( boxRotate );
 
  // The position can be hard, but we can use box1Body model frame to position box2Body
  // on box1Body right surface (+ 2 * x_half_extent in model coordinate frame).
  agx::Vec3 box1RightSurfaceLocalPoint( 2 * boxHalfExtent.x(), 0, 0 );
  // We're after a point in world coordinates, so make sure the correct method is used:
  agx::Vec3 box2Position = box1Body->getFrame()->transformPointToWorld( box1RightSurfaceLocalPoint );
  // Equivalent to the row just above is (transforming as vector instead):
  // agx::Vec3 box2Position = box1Body->getPosition() + box1Body->getFrame()->transformVectorToWorld( box1RightSurfaceLocalPoint );
 
  // Set the position to box2Body:
  box2Body->setPosition( box2Position );
 
  // Create visuals and add the bodies to the simulation:
  agxOSG::createVisual( box1Body, root );
  agxOSG::createVisual( box2Body, root );
 
  simulation->add( box1Body );
  simulation->add( box2Body );
 
  application->setEnableDebugRenderer( true );
 
  return root;
}
 
 
#define LEAF_SIZE 1.0
 
 
class LeafForceField : public agx::ForceField
{
public:
  LeafForceField(const agx::Vec3& dropZone ) :  m_torqueFactor(LEAF_SIZE*-0.0011), m_boyancyFactor(LEAF_SIZE*-0.2) 
  {
    m_dropZone = dropZone;
  }
  void updateForce(agx::DynamicsSystem *);
 
  static void initBody(agx::RigidBody *body);
 
private:
  static agx::Vec3 m_dropZone;
  agx::Real m_torqueFactor, m_boyancyFactor;
};
 
agx::Vec3 LeafForceField::m_dropZone;
 
// Reset transformation, age and velocity for a body
void LeafForceField::initBody(agx::RigidBody *body)
{
  body->getMassProperties()->setMass( agx::random(0.1, 0.3) );
  body->getMassProperties()->setInertiaTensor( 
    agx::Vec3( (0.1*0.1+LEAF_SIZE*LEAF_SIZE)*0.01, 
    2*LEAF_SIZE*LEAF_SIZE*0.01,
    (0.1*0.1+LEAF_SIZE*LEAF_SIZE)*0.01));
 
  agx::EulerAngles euler( agx::degreesToRadians( agx::random( 5.0, 35.0 ) ),
    agx::degreesToRadians( agx::random( 5.0, 35.0 ) ),
    agx::degreesToRadians( agx::random( 5.0, 35.0 ) ));
  body->setRotation( euler );
 
  body->setPosition( agx::Vec3( agx::random( -m_dropZone[0], m_dropZone[0] ), 
    agx::random( -m_dropZone[1], m_dropZone[1] ), 
    agx::random( -m_dropZone[2]*0.1, m_dropZone[2]*0.1 ) ) );
 
  agx::Vec3 v;
  v.random(-0.1, 0.1);
  body->setVelocity( v );
 
  v.random(-0.1, 0.1);
 
  body->setAngularVelocity( v );
}
 
 
void LeafForceField::updateForce(agx::DynamicsSystem *system)
{
  // Loop through all bodies
  agx::RigidBodyRefVector& bodies = system->getRigidBodies();
  for(agx::RigidBodyRefVector::iterator it = bodies.begin(); it != bodies.end(); ++it)
  {
    agx::RigidBody *body = (*it).get();
 
    // Only affect the ones which has a propertyDouble named "createTime"
    if (!body->hasPropertyContainer() || !body->getPropertyContainer()->hasPropertyDouble("createTime") )
      continue;
 
    agx::AffineMatrix4x4 m = body->getTransform();
    agx::Vec3 v = body->getVelocity();
 
    // Get the "up" vector for the body,
    // Which is the 3:rd column in the matrix
    agx::Vec3 N;
    N[0] = m(0,2);
    N[1] = m(1,2);
    N[2] = m(2,2);
 
    // Buoyancy is Normal dot Velocity
    agx::Real boyancy = N*v;
 
    // We need the sign
    agx::Real sign = agx::sign(boyancy);
 
    // Magnitude of boyancy is v^2*boyancyFactor (area really)
    boyancy *=boyancy*sign*m_boyancyFactor;
    agx::Vec3 force = N*boyancy;
 
    // Torque is Normal cross velocity * torqueFactor (also area property)
    agx::Vec3 torque = (N ^ v)*(m_torqueFactor*sign);
 
    body->addForce( force );
    body->addTorque( torque );
  }
}
 
 
class AgeStepListener : public agxSDK::StepEventListener
{
public:
 
  AgeStepListener( agx::Real maxAge ) : m_maxAge(maxAge) {}
 
  void post(const agx::TimeStamp& t)
  {
    agx::RigidBodyRefVector& bodies = getSimulation()->getDynamicsSystem()->getRigidBodies();
 
    for(agx::RigidBodyRefVector::iterator it = bodies.begin(); it != bodies.end(); ++it)
    {
      agx::RigidBody *body = (*it).get();
      if ( body->hasPropertyContainer() && body->getPropertyContainer()->hasPropertyDouble("createTime") )
      {
        double createTime = body->getPropertyContainer()->getPropertyDouble("createTime");
        double age = t - createTime;
 
        // is it time to re-initialize body?
        if (age > m_maxAge)
        {
          LeafForceField::initBody(body);
          createTime = t;
          body->getPropertyContainer()->setPropertyDouble("createTime", createTime);
        }
      }
    }
  }
private:
  agx::Real m_maxAge;
};
 
 
osg::Group* buildTutorial7( agxSDK::Simulation *simulation, agxOSG::ExampleApplication *application )
{
  osg::Group* root = new osg::Group();
 
  
  srand(0);
 
  // Add the leaf force field
  simulation->add( new LeafForceField(agx::Vec3(4,4,8)) );
 
  // Make sure leafs does not collide with each other
  simulation->getSpace()->setEnablePair(1,1,false);
  
  for(size_t i=0; i < 500; i++)
  {
    // Create a dynamic rigid body box
    agx::Vec3 boxHalfExtent( 0.2, 0.2, 0.01 );
    agx::RigidBodyRef box1Body = new agx::RigidBody();
    double createTime = agx::random(0.0, 10.0);
    
    // Add a double value property indicating when the leaf was created
    box1Body->getPropertyContainer()->addPropertyDouble("createTime", -createTime);
 
    agxCollide::GeometryRef boxGeometry = new agxCollide::Geometry( new agxCollide::Box( boxHalfExtent ) );
    boxGeometry->addGroup( 1 );
    box1Body->add( boxGeometry );
 
    // Initialize transformation and velocity
    LeafForceField::initBody(box1Body);
 
    // Create visuals and add the bodies to the simulation:
    agxOSG::createVisual( box1Body, root );
 
    simulation->add( box1Body );
  }
 
  // Add the listener that will reset leafs when they are old enough,
  simulation->add( new AgeStepListener(10) );
 
  application->setEnableDebugRenderer( false );
 
  return root;
}
 
agx::RigidBodyRef createBodyWithTwoSpheres()
{
  agx::RigidBodyRef body = new agx::RigidBody();
 
  agxCollide::GeometryRef sphere1 = new agxCollide::Geometry(new agxCollide::Sphere(0.5));
  body->add(sphere1);
 
  agxCollide::GeometryRef sphere2 = new agxCollide::Geometry(new agxCollide::Sphere(0.5));
  body->add(sphere2, agx::AffineMatrix4x4::translate(-1, 0, 0));
 
  return body;
}
 
 
osg::Group* buildTutorial8(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/)
{
  osg::Group* root = new osg::Group();
 
  agxCollide::GeometryRef ground = new agxCollide::Geometry(new agxCollide::Box(7, 7, 0.1));
  ground->setPosition(0, 0, -0.1);
  simulation->add(ground);
  agxOSG::createVisual(ground, root);
 
  // Create a rigid body with a pair of spheres with the massproperties enabled 
  // for both of the spheres
  agx::RigidBodyRef body1 = createBodyWithTwoSpheres();
  simulation->add(body1);
  osg::Group* node1 = agxOSG::createVisual(body1, root);
  agxOSG::setDiffuseColor(node1, agxRender::Color::OrangeRed());
  body1->setPosition(-2, 0, 2);
  body1->setRotation(agx::EulerAngles(-1, -1, -1));
 
  // Create another rigidbody, now this time disable the mass properties
  // for one of the spheres
  agx::RigidBodyRef body2 = createBodyWithTwoSpheres();
  simulation->add(body2);
  osg::Group* node2 = agxOSG::createVisual(body2, root);
  agxOSG::setDiffuseColor(node2, agxRender::Color::BlueViolet());
  body2->setPosition(2, 0, 2);
  body2->setRotation(agx::EulerAngles(1, 1, 1));
 
  // Disable mass properties for one of the geometries
  body2->getGeometries()[1]->setEnableMassProperties(false);
 
  // Force update of massproperties for the rigidbody.
  body2->updateMassProperties();
 
 
 
  
 
 
  return root;
}
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout <<
                "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
                "\tRigidBody tutorial 1\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
    application->addScene( buildTutorial3, '3' );
    application->addScene( buildTutorial4, '4' );
    application->addScene( buildTutorial5, '5' );
    application->addScene( buildTutorial6, '6' );
    application->addScene(buildTutorial7, '7');
    application->addScene( buildTutorial8, '8' );
 
    
    
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;               
    return 1;
  }
 
  return 0;
}

tutorial_bulldozer_with_terrain.cpp

/*
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.
*/
 
 
 
/*
This tutorial demonstrates how to use the AGX C++ API to model, configure and parameterize a bulldozer on deformable
terrain for control with a GamePad.
 
In this tutorial we first generate a deformable terrain and then load a CAT D10T bulldozer from an existing AGX-model.
We then locate all the actuators (constraints) by name and couple them to a keyboard control and a gamepad control.
We also create the tracks on the bulldozer and three shovels, one main shovel in the front and two rippers in the back.
Finally the contact materials between track, terrain, shovel and terrain particles are created.
 
All parameters we define in this tutorial are read from the json file bulldozer_catd10t.json
 
The CAT D10T has the following components:
    Drivetrain:
 
                                             engine
                                                ^
                                                |
                                                |
                                         torque_converter
                                                ^
                                                |
                                                |
                                            gear_box
                                                ^
                                                |
                                                |
                                           differential
                                                ^
                    LEFT TRACK                  |                 RIGHT TRACK
                                                |
    sprocket_actuator <---- brake <-- clutch <--------> clutch --> brake ----> sprocket_actuator
 
    Default Controls:
        Keyboard:
            - Parking brake toggle: 'p'
            - Increase gear:        'd'
            - Decrease gear:        'c'
            - Increase target rpm:  Arrow key up
            - Decrease target rpm:  Arrow key down
            - Steer left:           Arrow key left
            - Steer right:          Arrow key right
            - Shovel lift:          'a'
            - Shovel lower:         'z'
            - Shovel tilt up:       's'
            - Shovel tilt down:     'x'
            - Ripper lift up:       Home key
            - Ripper lift down:     End key
            - Ripper pitch in:      Page down key
            - Ripper pitch out:     Delete key
 
        Gamepad (Xbox like):
            - Toggle parking brake:           B button
            - Increase gear:                  Right bumber (RB)
            - Decrease gear:                  Left bumper (LB)
            - Increase target rpm:            Up on D-pad
            - Decrease target rpm:            Down on D-pad
            - Right track brake:              Right trigger (RT)
            - Left track brake:               Left trigger (LT)
            - Lower Shovel:                   Forward on Right Joystick
            - Raise Shovel:                   Backward on Right Joystick
            - Twist Shovel Left:              Left on Right Joystick
            - Twist Shovel Right:             Right on Right Joytick
            - Adjust Tilt of Blade Forward:   X button
            - Adjust Tilt of Blade Backward:  Y button
            - Lower Ripper:                   Stick up on Left Joystick
            - Raise Ripper:                   Stick down on Left Joystick
            - Pitch Ripper Inward:            Stick left on Left Joystick
            - Pitch Ripper Outward:           Stick right on Left Joystick
*/
 
#include <iostream>
 
#include <agx/config/AGX_USE_AGXTERRAIN.h>
#include <agx/config/AGX_USE_OIS.h>
#include <agx/config/AGX_USE_WEBPLOT.h>
 
// This tutorial requires that AGX is built with OIS enabled and agxTerrain enabled.
#if AGX_USE_OIS() && AGX_USE_AGXTERRAIN()
 
#include <signal.h>
#include <cmath>
#include <functional>
#include <unordered_map>
 
#include <agx/config.h>
#include <agxIO/Environment.h>
#include <agxIO/FileSystem.h>
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/ReaderWriter.h>
#include <agxOSG/Node.h>
#include <agxOSG/utils.h>
#include <agxOSG/TerrainVoxelRenderer.h>
 
#include <agxUtil/StepEventCallback.h>
 
#include <agx/Json.h>
#include <agx/Logger.h>
#include <agx/version.h>
#include <agx/Interrupt.h>
#include <agxUnit/UnitMacros.h>
#include <agx/Windows.h>
 
#include <agxCFG/utils.h>
#include <agxRender/Color.h>
#include <agxModel/PidController1D.h>
#include <agxModel/ControllerHandler.h>
 
#include <agx/Material.h>
#include <agx/Line.h>
#include <agx/Hinge.h>
#include <agx/Prismatic.h>
#include <agxVehicle/Track.h>
#include <agxCollide/Cylinder.h>
 
#include <agxDriveTrain/CombustionEngine.h>
#include <agxDriveTrain/TorqueConverter.h>
#include <agxDriveTrain/GearBox.h>
#include <agxDriveTrain/Differential.h>
#include <agxDriveTrain/DryClutch.h>
#include <agxDriveTrain/Brake.h>
#include <agxDriveTrain/EnergyManager.h>
#include <agxPowerLine/Actuator1DOF.h>
 
#include <agxCollide/HeightField.h>
#include <agxTerrain/Terrain.h>
 
#include <agxIO/Joystick.h>
 
#include <agxPlot/WebPlot.h>
 
static bool g_isUnitTest = false;
 
 
namespace {
 
#ifdef _MSC_VER
  BOOL WINAPI sighandler(DWORD /*dwCtrlType*/)  //  control signal type
  {
    std::cout << "Caught: CTRL-C or closing window. Shutting down agx." << std::endl; // Not using LOGGER here, might interfere with shutting down and lead to crash.
    throw agx::InterruptException();
    //return TRUE;
  }
#else
  void sighandler(int /*sig*/)
  {
    std::cout << "Caught: CTRL-C or closing window. Shutting down agx." << std::endl; // Not using LOGGER here, might interfere with shutting down and lead to crash.
    throw agx::InterruptException();
  }
#endif
}
 
namespace Utilities {
 
  struct Foundation
  {
    agxSDK::Simulation* simulation;
    agxOSG::ExampleApplication* application;
    osg::Group* root;
 
    Foundation(
      agxSDK::Simulation* simulation,
      agxOSG::ExampleApplication* application,
      osg::Group* root)
      : simulation(simulation)
      , application(application)
      , root(root)
    {
    }
  };
 
  std::pair<double, double> find_size(agx::RigidBodyRef rb) {
    if (rb == nullptr) {
      throw std::invalid_argument("RigidBody is null");
    }
 
    double radius = -1.0f;
    double height = -1.0f;
    const auto& geometries = rb->getGeometries();
 
    for (const auto& geometry : geometries) {
      if (!geometry->getEnableCollisions()) {
        continue;
      }
 
      for (const auto& shape : geometry->getShapes()) {
        // Check if the shape is a cylinder using dynamic_cast
        auto cylinder = dynamic_cast<agxCollide::Cylinder*>(shape.get());
        if (cylinder) {
          radius = std::max(radius, cylinder->getRadius());
          height = std::max(height, cylinder->getHeight());
        }
      }
    }
 
    if (radius > 0) {
      return std::make_pair(radius, height);
    }
    else {
      throw std::runtime_error("Radius and height not found in body: " + rb->getName());
    }
  }
 
  void readSettings(const std::string& path, agxJson::Value& settings)
  {
    agxJson::Reader jsonReader;
    auto filePathContainer = agxIO::Environment::instance()->getFilePath(agxIO::Environment::RESOURCE_PATH);
    auto filePath = filePathContainer.find(path);
 
    std::ifstream file_stream(filePath);
    if (!file_stream.is_open()) {
      throw std::runtime_error("We can't open the bulldozer json configuration file at: " + path);
    }
 
    std::stringstream buffer;
    buffer << file_stream.rdbuf();
 
    bool status = jsonReader.parse(buffer.str(), settings);
    if (!status)
      throw std::runtime_error("We can't parse the supplied bulldozer json configuration file at: " + path);
  }
 
  agx::Vec3 jsonArrayToVec(const agxJson::Value& array)
  {
    return agx::Vec3(array[0].asDouble(), array[1].asDouble(), array[2].asDouble());
  }
 
  agx::RealVector jsonArrayToArray(const agxJson::Value& array)
  {
    //std::vector<double> vec;
    agx::RealVector vec;
    if (array.isArray())
    {
      for (auto d : array)
      {
        vec.push_back(d.asDouble());
      }
    }
    else
    {
      vec.push_back(array.asDouble());
    }
    return vec;
  }
 
  agx::Hinge* getHingeOnBody(agx::RigidBody* rigid_body, agx::ConstraintRefSetVector constraints) {
    std::vector<agx::Hinge*> filtered_hinges;
 
    for (auto& constraint : constraints) {
      // Check if the current constraint involves the provided rigid body
      if ((constraint->getBodyAt(0) == rigid_body || constraint->getBodyAt(1) == rigid_body)
        && dynamic_cast<agx::Hinge*>(constraint.get()) != nullptr) {
        filtered_hinges.push_back(dynamic_cast<agx::Hinge*>(constraint.get()));
      }
    }
 
    // Ensure that exactly one hinge is associated with the rigid body
    assert(filtered_hinges.size() == 1);
    return filtered_hinges.front();
  }
 
  agx::Hinge* toHinge(agx::Constraint1DOFRef constraint)
  {
    auto hinge = dynamic_cast<agx::Hinge*>(constraint.get());
    assert(hinge);
    return hinge;
  }
 
  agx::Material* getOrCreateMaterial(agxSDK::Simulation* simulation, const std::string& name)
  {
    auto material = simulation->getMaterial(name);
    if (material == nullptr)
    {
      material = new agx::Material(name);
      simulation->add(material);
    }
    return material;
  }
 
 
  // Reports true if currentTime is within a 60:ths of a second from time
  bool inTime(double currentTime, double time)
  {
    return std::abs(currentTime - time) <= 1.0 / 60;
  }
 
  // Calculate the height at a specific (x, y) location on a height field to create a hill-like structure.
  double hillHeightFunction(double x, double y, double maxHeight, double size)
  {
    double height = 0;
 
    // Normalize the coordinates to a[-1, 1] range, centered on the field
    x /= (size / 2.0);
    y /= (size / 2.0);
 
    // Adjust the x-coordinate to shift the hill's center (use normalized coordinate between [-1, 1])
    x += 0.55;
 
    double r = std::sqrt(x * x + y * y);
    double theta = std::atan2(x, y);
    const double theta_threshold = M_PI / 12.0;
    const double theta_min = 50.0 * M_PI / 180.0;
    const double theta_max = M_PI - theta_min;
    const double deltaR = 0.15;
    const double R = 1.25;
    const int n = 1;
 
    if (r > R - deltaR && theta > theta_min && theta < theta_max) {
      if (r < R) {
        height = maxHeight * std::pow(std::sin((r + deltaR - R) / (2 * deltaR) * M_PI), n);
      }
      else {
        height = maxHeight;
      }
    }
 
    if (theta < (theta_min + theta_threshold)) {
      theta = theta - theta_min;
      height *= std::sin(theta / (2 * theta_threshold) * M_PI);
    }
 
    if (theta > (theta_max - theta_threshold)) {
      theta = theta_max - theta;
      height *= std::sin(theta / (2 * theta_threshold) * M_PI);
    }
 
    return height;
  }
 
  void createPile(agxTerrain::Terrain* terrain,
    double length = 4.0,
    double width = 2.0,
    double height = 2.0,
    float compaction = 1.0,
    const agx::Vec3& direction = agx::Vec3(1, 0, 0),
    const agx::Vec3& location = agx::Vec3(0, 0, 0))
  {
    int numLenPoints = static_cast<int>((length) / terrain->getElementSize()) + 1;
    int numWidthPoints = static_cast<int>((width) / terrain->getElementSize()) + 1;
 
    agx::Vec3 normalizedDirection = direction.normal();
    agx::Vec3 widthDirection = agx::Vec3(0, 0, 1).cross(normalizedDirection).normal();
 
    auto interface = terrain->getTerrainGridControl();
 
    double l_a_inc = M_PI / numLenPoints;
    double w_a_inc = M_PI / numWidthPoints;
 
    for (int n = 0; n < numLenPoints; ++n) {
      for (int w = 0; w < numWidthPoints; ++w) {
        double w_inc = (w - numWidthPoints / 2.0) * terrain->getElementSize();
        double l_inc = n * terrain->getElementSize();
 
        auto gridPoint = terrain->getClosestGridPoint(location + widthDirection * w_inc + normalizedDirection * l_inc);
        double h = height * std::sin(l_a_inc * n) * std::cos(w_a_inc * w - M_PI / 2);
        interface->addSolidOccupancyInColumnToHeight(gridPoint, h, compaction);
      }
    }
  }
 
  agxTerrain::Terrain* createTerrain(Foundation& foundation, agxJson::Value& terrain_settings)
  {
    const double size = terrain_settings["size"].asDouble();
    const double elementSize = terrain_settings["element_size"].asDouble();
    const double height = terrain_settings["bump_height"].asDouble();
    const int resolution = static_cast<int>(std::floor(size / elementSize)) + 1;
    agxCollide::HeightFieldRef heightField = new agxCollide::HeightField(resolution, resolution, size, size);
 
    // Convert index to position
    auto hfIndexToPosition = [resolution, size](int index) {
      return ((double)index / (double)resolution - 0.5) * size;
    };
 
    agx::RealVector heights(resolution * resolution);
    for (int i = 0; i < resolution; ++i) {
      for (int j = 0; j < resolution; ++j) {
        heights[i + resolution * j] = hillHeightFunction(hfIndexToPosition(i), hfIndexToPosition(j), height, size);
      }
    }
    heightField->setHeights(heights);
 
    auto terrain = agxTerrain::Terrain::createFromHeightField(heightField, terrain_settings["maximum_depth"].asDouble());
 
    // Setup material parameter
    terrain->loadLibraryMaterial("DIRT_1");
    terrain->getTerrainMaterial()->getCompactionProperties()->setAngleOfReposeCompactionRate(
      terrain_settings["angle_of_repose_compaction_rate"].asDouble());
 
    terrain->getProperties()->setAvalancheDecayFraction(terrain_settings["avalanche_decay_fraction"].asDouble());
    terrain->getProperties()->setAvalancheMaxHeightGrowth(terrain_settings["avalanche_max_height_growth"].asDouble());
    terrain->getProperties()->setMaximumParticleActivationVolume(terrain_settings["maximum_particle_activation_volume"].asDouble());
    terrain->getProperties()->setPenetrationForceVelocityScaling(terrain_settings["penetration_force_velocity_scaling"].asDouble());
 
    terrain->setMaterial(getOrCreateMaterial(foundation.simulation, "Terrain"));
 
    auto gridDataInterface = terrain->getTerrainGridControl();
 
    float elevationCompaction = terrain_settings["elevation"]["compaction"].asFloat();
    // Set compaction on the elevation on the far end
    for (size_t i = 0; i < terrain->getResolutionX(); ++i) {
      for (size_t j = 0; j < terrain->getResolutionY(); ++j) {
        agx::Vec2i terrainIndex((agx::Int)i, (agx::Int)j);
        int z = gridDataInterface->findSurfaceIndex(terrainIndex);
        if (z > 0) {
          int curr_z = z;
          while (curr_z > 0) {
            gridDataInterface->setCompaction(terrainIndex, curr_z, elevationCompaction);
            --curr_z;
          }
        }
      }
    }
 
    // Create a soft wall
    auto pile = terrain_settings["pile"];
    agx::Vec3 pile_location(jsonArrayToVec(pile["location"]));
    agx::Vec3 pile_direction(jsonArrayToVec(pile["direction"]));
    createPile(terrain,
      pile["length"].asDouble(),
      pile["width"].asDouble(),
      pile["height"].asDouble(),
      pile["compaction"].asFloat(),
      pile_direction, pile_location);
 
    terrain->getProperties()->setDeleteSoilParticlesOutsideBounds(true);
 
    return terrain;
  }
 
  agxOSG::TerrainVoxelRenderer* attachTerrainGridTexture(agxTerrain::Terrain* terrain, osg::Group* root, float grid_size) {
    // Create a TerrainVoxelRenderer
    auto renderer = new agxOSG::TerrainVoxelRenderer(terrain, root); // Assuming root() is a function that returns the root node
    osg::Node* node = renderer->getNode();
 
    // Load texture
    auto filePathContainer = agxIO::Environment::instance()->getFilePath(agxIO::Environment::RESOURCE_PATH);
    auto filePath = filePathContainer.find("data/textures/grid.png");
    osg::ref_ptr<osg::Texture2D> texture = agxOSG::createTexture(filePath);
 
    // Calculate the scale of the grid based on the size of the terrain
    float length = (float)std::max(terrain->getSize().x(), terrain->getSize().y());
    // Assuming there are 20 squares per tile
    float scale = length / (20.0f * grid_size);
 
    // Apply the texture with the calculated scale
    agxOSG::setTexture(node, texture.get(), true, agxOSG::TextureMode::DIFFUSE_TEXTURE, scale, scale);
 
    return renderer;
  }
 
  agxOSG::TerrainVoxelRenderer* createTerrainVoxelRenderer(agxTerrain::Terrain* terrain, osg::Group* root, const agxJson::Value& terrain_settings)
  {
    agxOSG::TerrainVoxelRenderer* renderer = nullptr;
 
    if (terrain_settings["use_grid_texture"].asBool()) {
      float grid_resolution = terrain_settings["grid_cell_texture_size"].asFloat();
      renderer = attachTerrainGridTexture(terrain, root, grid_resolution);
    }
    else {
      renderer = new agxOSG::TerrainVoxelRenderer(terrain, root); // Assuming root() is defined elsewhere
    }
 
    renderer->setRenderCompaction(true, agx::RangeReal(0.85, 1.15));
    renderer->setRenderVoxelFluidMass(false);
    renderer->setRenderSoilParticlesMesh(true);
    renderer->setRenderVoxelSolidMass(false);
    renderer->setRenderCompaction(true);
    renderer->setRenderVoxelBoundingBox(false);
    renderer->setRenderVelocityField(false);
    renderer->setVelocityFieldLineColor(agx::Vec4(0, 0, 1, 1));
    renderer->setRenderSoilParticles(false);
 
    return renderer;
  }
 
  void setupCamera(Foundation foundation) {
    auto cameraData = foundation.application->getCameraData();
    cameraData.eye = agx::Vec3(-9.99429, -3.27762e+01, 9.606);
    cameraData.center = agx::Vec3(-7.1831e-01, 1.1284e-02, 1.47957);
    cameraData.up = agx::Vec3(6.32905e-02, 2.2320e-01, 9.727e-01);
    cameraData.nearClippingPlane = 0.1f;
    cameraData.farClippingPlane = 5000.0f;
    foundation.application->applyCameraData(cameraData);
  }
 
  void setConstraintTargetSpeed(double speed, agx::Constraint1DOF* constraint) {
    if (!constraint) {
      return; // Safety check for null pointer
    }
 
    auto motor = constraint->getMotor1D();
    auto lock = constraint->getLock1D();
 
    bool lockWasEnabled = lock->getEnable();
 
    motor->setEnable(std::abs(speed) > 0);
    motor->setSpeed(speed);
 
    lock->setEnable(!motor->getEnable());
 
    if (!lockWasEnabled) {
      lock->setPosition(constraint->getAngle());
    }
  }
 
  agx::ContactMaterial* createAndAddContactMaterial(agxSDK::Simulation* sim, const agxJson::Value& contactMaterialData, agx::RigidBody* chassis) {
    assert(sim != nullptr);
 
    auto material1 = getOrCreateMaterial(sim, contactMaterialData["material1"].asString());
    auto material2 = getOrCreateMaterial(sim, contactMaterialData["material2"].asString());
 
    auto contactMaterial = sim->getMaterialManager()->getOrCreateContactMaterial(material1, material2);
    contactMaterial->setFrictionCoefficient(contactMaterialData["friction_coefficient_primary_direction"].asDouble(), agx::ContactMaterial::PRIMARY_DIRECTION);
    contactMaterial->setFrictionCoefficient(contactMaterialData["friction_coefficient_secondary_direction"].asDouble(), agx::ContactMaterial::SECONDARY_DIRECTION);
    contactMaterial->setRestitution(contactMaterialData["restitution"].asDouble());
    contactMaterial->setYoungsModulus(contactMaterialData["youngs_modulus"].asDouble());
    contactMaterial->setSurfaceViscosity(contactMaterialData["surface_viscosity_primary_direction"].asDouble(), agx::ContactMaterial::PRIMARY_DIRECTION);
    contactMaterial->setSurfaceViscosity(contactMaterialData["surface_viscosity_secondary_direction"].asDouble(), agx::ContactMaterial::SECONDARY_DIRECTION);
    contactMaterial->setDamping(2 * sim->getTimeStep());
    contactMaterial->setUseContactAreaApproach(contactMaterialData["use_contact_area_approach"].asBool());
 
    if (contactMaterialData.hasMember("adhesion")) {
      auto adhesion = jsonArrayToArray(contactMaterialData["adhesion"]);
      contactMaterial->setAdhesion(adhesion[0], adhesion[1]);
    }
 
    agx::FrictionModel* model = nullptr;
    auto solver = agx::FrictionModel::SPLIT;
 
    std::string frictionModel = contactMaterialData["friction_model"].asString();
    if (frictionModel.find("agx.FrictionModel.DIRECT_AND_ITERATIVE") != std::string::npos) {
      solver = agx::FrictionModel::DIRECT_AND_ITERATIVE;
    }
    else if (frictionModel.find("agx.FrictionModel.DIRECT") != std::string::npos) {
      solver = agx::FrictionModel::DIRECT;
    }
    else if (frictionModel.find("agx.FrictionModel.ITERATIVE") != std::string::npos) {
      solver = agx::FrictionModel::ITERATIVE;
    }
 
    if (frictionModel.find("IterativeProjectedConeFriction") != std::string::npos) {
      model = new agx::IterativeProjectedConeFriction(solver);
    }
    else if (frictionModel.find("OrientedScaleBoxFrictionModel") != std::string::npos) {
      assert(chassis != nullptr);
      model = new agx::OrientedScaleBoxFrictionModel(chassis->getFrame(),
        agx::Vec3::X_AXIS(),
        solver);
    }
    else if (frictionModel.find("ConstantNormalForceOrientedBoxFrictionModel") != std::string::npos) {
      assert(chassis != nullptr);
      double normal_force_scale = contactMaterialData["friction_model_normal_force_scale"].asDouble();
      model = new agx::ConstantNormalForceOrientedBoxFrictionModel(
        normal_force_scale * chassis->getMassProperties()->getMass(),
        chassis->getFrame(),
        agx::Vec3::X_AXIS(),
        solver,
        true);
    }
 
    if (model) {
      contactMaterial->setFrictionModel(model);
    }
 
    return contactMaterial;
  }
 
  agx::ContactMaterial* createAndAddParticleContactMaterial(agxSDK::Simulation* sim, const agxJson::Value& contactMaterialData, agxTerrain::Terrain* terrain) {
    assert(sim != nullptr && terrain != nullptr);
 
    auto shovelMaterial = getOrCreateMaterial(sim, contactMaterialData["material1"].asString());
    auto particleMaterial = terrain->getMaterial(agxTerrain::Terrain::MaterialType::PARTICLE);
    sim->add(particleMaterial);
 
    auto shovelParticleContactMaterial = sim->getMaterialManager()->getContactMaterialOrCreateImplicit(shovelMaterial, particleMaterial);
    shovelParticleContactMaterial->setRestitution(contactMaterialData["restitution"].asDouble());
    shovelParticleContactMaterial->setFrictionCoefficient(contactMaterialData["friction_coefficient"].asDouble());
    shovelParticleContactMaterial->setRollingResistanceCoefficient(contactMaterialData["rolling_resistance"].asDouble());
    shovelParticleContactMaterial->setYoungsModulus(contactMaterialData["youngs_modulus"].asDouble());
 
    sim->add(shovelParticleContactMaterial);
 
    return shovelParticleContactMaterial;
  }
 
  agx::RigidBody* getShovelBody(const std::string& name, agxSDK::Assembly* assembly)
  {
    // Return the body of an observer attached to the shovel body.
    agx::RigidBody* body = assembly->getObserverFrame(name + "UpperLeft")->getRigidBody();
    return body;
  }
 
  agx::Line getShovelTopEdge(const std::string& name, agxSDK::Assembly* assembly)
  {
    auto body = Utilities::getShovelBody(name, assembly);
    auto upperLeft = body->getFrame()->transformPointToLocal(assembly->getObserverFrame(name + "UpperLeft")->getPosition());
    auto upperRight = body->getFrame()->transformPointToLocal(assembly->getObserverFrame(name + "UpperRight")->getPosition());
    auto topEdge = agx::Line(upperRight, upperLeft);
    return topEdge;
  }
 
  agx::Line getShovelCuttingEdge(const std::string& name, agxSDK::Assembly* assembly)
  {
    auto body = Utilities::getShovelBody(name, assembly);
    auto lowerLeft = body->getFrame()->transformPointToLocal(assembly->getObserverFrame(name + "LowerLeft")->getPosition());
    auto lowerRight = body->getFrame()->transformPointToLocal(assembly->getObserverFrame(name + "LowerRight")->getPosition());
    auto cuttingEdge = agx::Line(lowerRight, lowerLeft);
    return cuttingEdge;
  }
 
  agx::Vec3 getShovelCuttingDirection(const std::string& name, agxSDK::Assembly* assembly)
  {
    auto body = Utilities::getShovelBody(name, assembly);
    auto cuttingDirection = body->getFrame()->transformVectorToLocal(assembly->getObserverFrame(name + "CuttingDirection")->transformVectorToWorld(agx::Vec3(1, 0, 0))).normal();
    return cuttingDirection;
  }
}
 
 
AGX_DECLARE_POINTER_TYPES(Track);
 
class Track : public agxVehicle::Track
{
public:
 
  Track(agx::RigidBody* sprocket,
    agx::Hinge* sprocket_hinge,
    std::vector<agx::RigidBodyRef> rollers,
    // sprocket radius and width of track
    std::pair<double, double> size,
    const agxJson::Value& trackSettings);
 
  void trackSetup(agxSDK::Simulation* simulation, const agxJson::Value& trackSettings);
 
  agx::Hinge* sprocket_hinge() const {
    return m_sprocket_hinge;
  }
 
  void createVisual(osg::Group* root);
 
  double velocity() const {
    double angular_velocity = m_sprocket_hinge->getMotor1D()->getSpeed();
    return angular_velocity * m_sprocket_radius;
  }
 
private:
  agx::RigidBodyRef m_sprocket;
  double m_sprocket_radius;
  double m_track_width;
  agx::Hinge* m_sprocket_hinge;
};
 
AGX_DECLARE_POINTER_TYPES(Bulldozer);
 
class ControllerMotorData;
// The action called when a button is pressed. Contains data about constraints and value to set
// and the duration the button has been pressed
using Action = std::function<void(bool, bool, agx::Real)>;
 
class EngineState {
public:
  // Constructor
  EngineState()
    : rightTrackBrake(0.0),
    leftTrackBrake(0.0),
    parkBrake(true) {}
 
  // Getters
  // From right trigger (0-1)
  agx::Real getRightBrake() const { return rightTrackBrake; }
  // From left trigger (0-1)
  agx::Real getLeftBrake() const { return leftTrackBrake; }
  bool getParkBrake() const { return parkBrake; }
 
  // Setters
  void setRightBrake(agx::Real value) { rightTrackBrake = value; }
  void setLeftBrake(agx::Real value) { leftTrackBrake = value; }
  void toggleParkBrake() { parkBrake = !parkBrake; }
 
private:
  agx::Real rightTrackBrake;
  agx::Real leftTrackBrake;
  bool parkBrake;
};
 
class Bulldozer;
 
class ThrottleController {
public:
  ThrottleController(Bulldozer* bulldozer, agx::Real p, agx::Real i, agx::Real d);
  void setTargetRPM(agx::Real targetRPM);
  double getCurrentRPM() const;
  void update(agx::Real dt);
  ~ThrottleController();
 
private:
  agxModel::PidController1D* m_pidController;
  Bulldozer* m_bulldozer;
  agx::Real m_targetRPM;
};
 
enum class Location {
  LEFT,
  RIGHT,
  BOTH
};
 
class Bulldozer : public agxSDK::Assembly
{
public:
 
  Bulldozer(Utilities::Foundation &foundation, const std::string& path, const agxJson::Value& settings);
 
  void updateDrivetrain();
 
  osg::Group* root()
  {
    return m_foundation.root;
  }
 
  agxSDK::Simulation* simulation()
  {
    return m_foundation.simulation;
  }
 
  Track* getTrack(int index) { return m_tracks[index]; }
 
  agx::RigidBodyRef getChassisBody() { return m_modelComponents ["ChassisBody"]; }
 
  const EngineState& getEngineState() const {
    return m_engineState;
  }
 
  agxDriveTrain::CombustionEngine* getEngine() const {
    return m_engine;
  };
 
  agx::Real getSpeed() {
    auto const forward = getChassisBody()->getFrame()->transformVectorToLocal(agx::Vec3::X_AXIS());
    return getChassisBody()->getVelocity() * getChassisBody()->getFrame()->transformVectorToWorld(forward);
  }
 
  bool getParkingBrake() const {
    return m_engineState.getParkBrake();
  }
 
  agx::Real getGearRatio() const {
    return m_gearBox->getCurrentGearRatio();
  }
 
  agxTerrain::Shovel* getShovel() const {
    return m_shovel;
  }
 
  agx::Real getTargetRpm() const {
    return m_targetRPM;
  }
 
  ~Bulldozer();
 
  // These get functions are added to be able to access and iterate over different parts of the model in a convenient way.
  // For performance sensitive parts, consider handling the return values in a different way to avoid heap allocations.
 
  // \return A vector with track constraints.
  std::vector<agx::Constraint1DOFRef> getTrackConstraints() const {
    return {
      m_actuatorConstraints.at("LeftSprocketHinge"),
      m_actuatorConstraints.at("RightSprocketHinge")
    };
  }
 
  // \return A vector with constraints controlling the shovel lift.
  std::vector<agx::Constraint1DOFRef> getShovelLiftConstraints() const {
    return {
      m_actuatorConstraints.at("LeftLiftPrismatic"),
      m_actuatorConstraints.at("RightLiftPrismatic")
    };
  }
 
  // \return A vector with constraints controlling the shovel tilt.
  std::vector<agx::Constraint1DOFRef> getShovelTiltConstraints() const {
    return {
      m_actuatorConstraints.at("LeftTiltPrismatic"),
      m_actuatorConstraints.at("RightTiltPrismatic")
    };
  }
 
  // \return A vector with constraints controlling the ripper lift.
  std::vector<agx::Constraint1DOFRef> getRipperLiftConstraints() const {
    return {
      m_actuatorConstraints.at("LeftRipperLiftPrismatic"),
      m_actuatorConstraints.at("RightRipperLiftPrismatic")
    };
  }
 
  // \return A vector with constraints controlling the ripper pitch.
  std::vector<agx::Constraint1DOFRef> getRipperPitchConstraints() const {
    return {
      m_actuatorConstraints.at("LeftRipperPitchPrismatic"),
      m_actuatorConstraints.at("RightRipperPitchPrismatic")
    };
  }
 
  std::vector<agx::Constraint1DOFRef> getConstraintsFromTag(const std::string& tag) const;
 
  Action* createAction(const std::string& tag);
 
private:
  void initializeBodiesAndConstraints();
 
  void createTracks();
 
  void createDriveTrain();
  void buildDrivetrainForTrack(agxDriveTrain::Differential* differential, agxPowerLine::Actuator* actuator, Location location, agxJson::Value drivetrainSettings);
  void setTrackDrivetrainState(agx::Real input, agxDriveTrain::DryClutch* clutch, agxDriveTrain::Brake* brake);
 
  void createAndAddShovels(const agxJson::Value& shovel_settings);
 
  agxTerrain::Shovel* createShovel(const std::string &name, const agxJson::Value& shovel_settings);
 
private:
  Utilities::Foundation m_foundation;
 
  agxJson::Value m_settings;
 
  // Your data members here
  std::vector<TrackRef> m_tracks;
 
  agx::Vec3 m_forward;
 
  EngineState m_engineState;
 
  agx::Real m_targetRPM;
  agx::Real m_maxRPM;
  agx::Real m_idleRPM;
  agxDriveTrain::CombustionEngineRef m_engine;
  ThrottleController* m_throttleController;
  agx::Real m_throttleStep;
  agxPowerLine::PowerLineRef m_powerline;
  agxDriveTrain::TorqueConverterRef m_torqueConverter;
  agxDriveTrain::GearBoxRef m_gearBox;
  agx::Vector<agxPowerLine::ActuatorRef> m_actuators;
  agx::RealVector m_steeringLimits;
  agx::Vector<agxDriveTrain::DryClutch*> m_clutches;
  agx::Vector<agxDriveTrain::Brake*> m_brakes;
  agx::Vector<agxDriveTrain::Shaft*> m_actuatorShafts;
  agxTerrain::ShovelRef m_shovel;
 
  // this is the map with constraints we want to control. It links tags to a constraints.
  std::unordered_map<std::string, agx::Constraint1DOFRef> m_actuatorConstraints = {
    {"LeftLiftPrismatic", nullptr},
    {"RightLiftPrismatic", nullptr},
    {"LeftTiltPrismatic", nullptr},
    {"RightTiltPrismatic", nullptr},
    {"LeftRipperLiftPrismatic", nullptr},
    {"RightRipperLiftPrismatic", nullptr},
    {"LeftRipperPitchPrismatic", nullptr},
    {"RightRipperPitchPrismatic", nullptr},
    {"LeftSprocketHinge", nullptr},
    {"RightSprocketHinge", nullptr} };
 
 
 
  // This is a map with bodies we need to setup shovels and tracks. This highlights the important bodies in the agx-model.
  std::unordered_map<std::string, agx::RigidBodyRef> m_modelComponents  = {
    {"ChassisBody", nullptr},
    {"Shovel", nullptr},
    {"LeftRipper", nullptr},
    {"RightRipper", nullptr},
    {"LeftSprocket", nullptr},
    {"RightSprocket", nullptr},
    {"LeftRearRollerWheel", nullptr},
    {"LeftFrontRollerWheel", nullptr},
    {"RightRearRollerWheel", nullptr},
    {"RightFrontRollerWheel", nullptr} };
};
 
#if AGX_USE_WEBPLOT()
namespace {
 
  // Classes that will be used to generate data for the plot
  class EngineRpmDataGenerator : public agxPlot::RealCallbackDataGenerator
  {
  public:
    EngineRpmDataGenerator(agxDriveTrain::CombustionEngine* engine)
      : m_engine(engine)
    {
    }
 
    // This just return a real value
    agx::Real get()
    {
      agx::Real result = m_engine->getRPM();
      return result;
    }
 
  private:
    agxDriveTrain::CombustionEngineRef m_engine;
  };
 
  class EngineTargetRpmDataGenerator : public agxPlot::RealCallbackDataGenerator
  {
  public:
    EngineTargetRpmDataGenerator(Bulldozer* bulldozer)
      : m_bulldozer(bulldozer)
    {
    }
 
    // This just return a real value
    agx::Real get()
    {
      return m_bulldozer->getTargetRpm();
    }
 
  private:
    BulldozerRef m_bulldozer;
  };
 
  class EnginePowerDataGenerator : public agxPlot::RealCallbackDataGenerator
  {
  public:
    EnginePowerDataGenerator(agxDriveTrain::CombustionEngine* engine)
      : m_engine(engine)
    {
    }
 
    // This just return a real value
    agx::Real get()
    {
      return agxDriveTrain::EnergyManager::getPower(m_engine)/1000.0;
    }
 
  private:
    agxDriveTrain::CombustionEngineRef m_engine;
  };
}
#endif // webplot
 
class EngineController {
public:
  EngineController(BulldozerRef bulldozer) : m_bulldozer(bulldozer) {}
 
  void updateEngine() {
    m_bulldozer->updateDrivetrain();
  }
 
 
private:
  BulldozerRef m_bulldozer;
};
 
class ControllerMotorData {
 
public:
 
  ControllerMotorData(std::string name,
    agx::RealVector maxSpeed,
    agx::Real accelerationTime,
    agx::Real decelerationRate,
    agx::Real deadZone,
    bool invert,
    std::vector<agx::Constraint1DOFRef> constraints) :
      name(name),
      maxSpeed(maxSpeed),
      accelerationTime(accelerationTime),
      decelerationRate(decelerationRate),
      deadZone(deadZone),
      invert(invert),
      constraints(constraints)
  {
    targetSpeed.resize(maxSpeed.size(), 0.0);
    if (maxSpeed[0] == 0)
      maxSpeed[0] = 1;
  }
 
  ControllerMotorData(std::string name,
    agx::RealVector maxSpeed,
    std::vector<agx::Constraint1DOFRef> constraints) :
    name(name), maxSpeed(maxSpeed), accelerationTime(0), decelerationRate(0), deadZone(0), invert(false), constraints(constraints)
  {
    targetSpeed.resize(maxSpeed.size(), 0.0);
  }
 
  ControllerMotorData() : name(""), maxSpeed(0), accelerationTime(0), decelerationRate(0), deadZone(0), invert(false)
  {
  }
 
  const std::string& getName() const { return name; }
 
  const agx::RealVector& getMaxSpeed() const { return maxSpeed; }
 
  double getAccelerationTime() const { return accelerationTime; }
 
  double getDecelerationRate() const { return decelerationRate; }
 
  const std::vector<agx::Constraint1DOFRef>& getConstraints() const { return constraints; }
 
  double getDeadZone() const { return deadZone; }
 
  const std::vector<double>& getTargetSpeed() const { return targetSpeed; }
 
  void setTargetSpeed(std::vector<double>& target) { targetSpeed = target; }
 
  void addTargetSpeed(const std::vector<double>& target)
  {
    for (size_t i = 0; i < target.size(); i++)
      targetSpeed[i] += target[i];
  }
 
  std::string getId()
  {
    return constraints.front()->getName() + ":" + constraints.back()->getName();
  }
 
  void updateSpeed(agx::Real time, double value = 1.0) {
    double sign = invert ? -1 : 1;
    value = abs(value) > deadZone ? std::copysign(1.0, value) * (abs(value) - deadZone) : 0;
    for (size_t i = 0; i < maxSpeed.size(); ++i) {
      if (accelerationTime == 0)
        targetSpeed[i] = sign * value * maxSpeed[i];
      else
        targetSpeed[i] = sign * value * std::min(time / accelerationTime, 1.0) * maxSpeed[i];
    }
  }
 
  void reset() {
    for (size_t i = 0; i < targetSpeed.size(); i++)
      targetSpeed[i] = 0;
  }
 
  void tick() {
    for (size_t i = 0; i < maxSpeed.size(); ++i) {
      if (decelerationRate > 1)
      {
        targetSpeed[i] /= decelerationRate;
        if (std::abs(maxSpeed[i]) < 1.0E-3) {
          targetSpeed[i] = 0.0;
        }
      }
      else
      {
        targetSpeed[i] = 0.0;
      }
    }
  }
 
private:
 
  const std::string name;
  const agx::RealVector maxSpeed;
  const agx::Real accelerationTime;
  const agx::Real decelerationRate;
  const agx::Real deadZone;
  const bool invert;
  const std::vector<agx::Constraint1DOFRef> constraints;
 
  std::vector<agx::Real> targetSpeed;
};
 
typedef std::shared_ptr<ControllerMotorData> ControllerMotorDataPtr;
 
class Motors {
public:
  Motors()
  {
  }
 
  void insert(std::vector<agx::Constraint1DOFRef> constraints)
  {
    for (auto constraint : constraints) {
      m_constraintMap.insert({ constraint, 0.0 });
    }
  }
 
  void reset() {
    for (auto& pair : m_constraintMap) {
      pair.second = 0;
    }
  }
 
  void setConstraintTargetSpeed() {
    for (auto& pair : m_constraintMap) {
      Utilities::setConstraintTargetSpeed(pair.second, pair.first);
    }
  }
 
 
  // Add target speeds to the given motors
  void addTargetSpeed(ControllerMotorDataPtr motorData, const std::vector<double>& target)
  {
    size_t i = 0;
    // One constraint may be controlled by several motor controllers at the same time.
    // We loop over all constraints for this motor controller and sum the target values for each constraint.
    for (auto constraint : motorData->getConstraints() ) {
      if (m_constraintMap.find(constraint) != m_constraintMap.end())
      {
        double value = target.size() > i ? target[i] : target.back();
        m_constraintMap.find(constraint)->second += value;
        i++;
      }
    }
  }
 
private:
  std::map<agx::Constraint1DOFRef, double> m_constraintMap;
};
typedef std::shared_ptr<Motors> MotorsPtr;
 
class Axis {
public:
  Axis(std::string name, int key, ControllerMotorDataPtr motorData) :
    m_name(name), m_key(key), m_motorData(motorData), m_isKeyDown(false), m_isKeyUp(false), m_down(false), m_timeDown(0), m_timeAtKeyDown(0), m_action(nullptr), m_value(0) {
  };
 
  Axis(std::string name, int key, ControllerMotorDataPtr motorData, Action* action) :
    m_name(name), m_key(key), m_motorData(motorData), m_isKeyDown(false), m_isKeyUp(false), m_down(false), m_timeDown(0), m_timeAtKeyDown(0), m_action(action), m_value(0) {
  };
 
  std::string getName() const { return m_name; }
  int getKey() const { return m_key; }
  bool getDown() const { return m_down; }
  agx::Real getTimeDown() const { return m_timeDown; }
  agx::Real getValue() const { return m_value; }
 
  ControllerMotorDataPtr getData() { return m_motorData; }
 
  Action* getAction() { return m_action.get(); }
 
  void tick(agx::Real timestamp) {
    if (getData() == nullptr)
      return;
 
    if (m_down)
    {
      m_timeDown = (timestamp - m_timeAtKeyDown);
      getData()->updateSpeed(m_timeDown, m_value);
    }
    else
    {
      getData()->tick();
    }
  }
 
  void processKeyboardData(agx::Real timestamp, agx::Real value) {
    m_value = value;
    if (getData() == nullptr)
      return;
 
    bool keyDown = (abs(value) - m_motorData->getDeadZone()) > 0;
    m_isKeyDown = keyDown && !m_down;
    m_isKeyUp = !keyDown && m_down;
    m_down = keyDown;
 
    if (m_isKeyDown) {
      m_timeAtKeyDown = timestamp;
      m_timeDown = 0.0;
    }
    else {
      m_timeDown = (timestamp - m_timeAtKeyDown);
    }
  }
 
private:
  const std::string m_name;
  const int m_key;
  ControllerMotorDataPtr m_motorData;
  bool m_isKeyDown = false;
  bool m_isKeyUp = true;
  bool m_down = false;
  agx::Real m_timeDown;
  agx::Real m_timeAtKeyDown;
  std::unique_ptr<Action> m_action;
 
  double m_value;
};
 
typedef std::shared_ptr<Axis> AxisPtr;
 
class Button {
public:
  Button(std::string name, int key, ControllerMotorDataPtr motorData) :
    m_name(name), m_key(key), m_motorData(motorData), m_isKeyDown(false), m_isKeyUp(false), m_down(false), m_timeDown(0), m_timeAtKeyDown(0), m_action(nullptr){
  };
 
  Button(std::string name, int key, ControllerMotorDataPtr motorData, Action* action) :
    m_name(name), m_key(key), m_motorData(motorData), m_isKeyDown(false), m_isKeyUp(false), m_down(false), m_timeDown(0), m_timeAtKeyDown(0), m_action(action) {
  };
 
  std::string getName() const { return m_name; }
  int getKey() const { return m_key; }
  bool getDown() const { return m_down; }
  agx::Real getTimeDown() const { return m_timeDown; }
  bool getKeyIsPressed() const { return m_isKeyDown; }
  ControllerMotorDataPtr getData() { return m_motorData; }
 
  Action* getAction() { return m_action.get(); }
 
  void tick(agx::Real timestamp) {
    if (getData() == nullptr)
      return;
 
    if (m_down)
    {
      m_timeDown = (timestamp - m_timeAtKeyDown);
      getData()->updateSpeed(m_timeDown);
    }
    else
    {
      getData()->tick();
    }
  }
 
  void processKeyboardData(agx::Real timestamp, unsigned /*modkeyMask*/, float /*x*/, float /*y*/, bool keyDown) {
 
    m_isKeyDown = keyDown && !m_down;
    m_isKeyUp = !keyDown && m_down;
    m_down = keyDown;
 
    if (m_isKeyDown) {
      m_timeAtKeyDown = timestamp;
      m_timeDown = 0.0;
    }
    else {
      m_timeDown = (timestamp - m_timeAtKeyDown);
    }
  }
 
private:
  const std::string m_name;
  const int m_key;
  ControllerMotorDataPtr m_motorData;
  bool m_isKeyDown = false;
  bool m_isKeyUp = true;
  bool m_down = false;
  agx::Real m_timeDown;
  agx::Real m_timeAtKeyDown;
  std::unique_ptr<Action> m_action;
};
 
typedef std::shared_ptr<Button> ButtonPtr;
 
 
/*
This listener will override the virtual methods from the baseclass to recieve calls when the gamepad buttons,
axis and pov are changed.
*/
class SimulationJoystickListener : public agxIO::JoystickListener {
 
public:
 
  SimulationJoystickListener(Utilities::Foundation foundation, BulldozerRef bulldozer, const agxJson::Value& controll_settings) :
    JoystickListener(), m_foundation(foundation), m_bulldozer(bulldozer)
  {
    size_t index = 0;
    // Loop over all gamepad controls in the settings
    for (auto& gamepad_control : controll_settings) {
 
      auto name = controll_settings.getMemberNames()[index];
      index++;
 
      if (name.rfind("_comment", 0) == 0)
        continue;
 
      // Get the constraints that the button/axis/pov control
      ControllerMotorDataPtr motorData = nullptr;
      if (name.rfind("tracks", 0) != 0) {
        std::vector<agx::Constraint1DOFRef> constraints = m_bulldozer->getConstraintsFromTag(name);
        motorData = std::make_shared<ControllerMotorData>(
          name,
          Utilities::jsonArrayToArray(gamepad_control["max_speed"]),
          gamepad_control["acceleration_time"].asDouble(),
          gamepad_control["deceleration_rate"].asDouble(),
          gamepad_control["deadzone"].asDouble(),
          gamepad_control["invert"].asBool(),
          constraints);
      }
      auto action = m_bulldozer->createAction(name);
 
      if (gamepad_control.hasMember("axis") && m_axisMap.find(gamepad_control["axis"].asString()) != m_axisMap.end()) {
        int symbol = m_axisMap.find(gamepad_control["axis"].asString())->second;
        AxisPtr axis = std::make_shared<Axis>(name, symbol, motorData, action);
        m_axisMotorMap.insert(std::make_pair(symbol, axis));
      }
      else if (gamepad_control.hasMember("button") && m_buttonMap.find(gamepad_control["button"].asString()) != m_buttonMap.end()) {
        int symbol = m_buttonMap.find(gamepad_control["button"].asString())->second;
        ButtonPtr button = std::make_shared<Button>(name, symbol, motorData, action);
        m_buttonMotorMap.insert(std::make_pair(symbol, button));
      }
      else if (gamepad_control.hasMember("pov") && m_povMap.find(gamepad_control["pov"].asString()) != m_povMap.end()) {
        int symbol = (int)m_povMap.find(gamepad_control["pov"].asString())->second;
        ButtonPtr button = std::make_shared<Button>(name, symbol, motorData, action);
        m_povMotorMap.insert(std::make_pair(symbol, button));
      }
    }
 
    createStepEventCallbackUpdater();
  }
 
  virtual ~SimulationJoystickListener() {}
 
  void  addNotification()
  {
    // Important: Call the base class addNotification method when you are done!
    JoystickListener::addNotification();
  }
 
  void  removeNotification()
  {
    // Important: Call the base class removeNotification method when you are done!
    JoystickListener::removeNotification();
  }
 
  // Called when a button value is changed
  bool buttonChanged(const agxIO::JoystickState&, int button, bool down) override {
    bool handled = false;
    if (m_buttonMotorMap.find(button) == m_buttonMotorMap.end())
      return handled;
 
    auto b = m_buttonMotorMap.find(button)->second;
    // process the key event and set e.g. the pressed duration and what time it was pressed or released
    b->processKeyboardData(m_foundation.simulation->getTimeStamp(), 0, 0, 0, down);
 
    if (b->getData() == nullptr) {
      auto action = b->getAction();
      (*action)(b->getKeyIsPressed(), b->getDown(), 0);
    }
 
    // update the speed of the constraints on this button
    if (b->getDown() && b->getData())
      b->getData()->updateSpeed(b->getTimeDown());
    return handled;
  }
 
  // Called when an axis is changed
  bool axisUpdate(const agxIO::JoystickState& arg, int axis) override {
 
    auto val = getManager()->normalize(arg.axes[axis]);
 
    bool handled = false;
    if (m_axisMotorMap.find(axis) == m_axisMotorMap.end())
      return handled;
 
    auto a = m_axisMotorMap.find(axis)->second;
    // process the axis event and store the state of the axis
    a->processKeyboardData(m_foundation.simulation->getTimeStamp(), val);
 
    if (a->getData() == nullptr) {
      auto action = a->getAction();
      (*action)(0, 0, a->getValue());
    }
 
    // update the speed of the constraints on this axis
    if (a->getData() && a->getDown())
      a->getData()->updateSpeed(a->getTimeDown(), val);
    return handled;
  }
 
  // Called when the POV button is changed
  bool povMoved(const agxIO::JoystickState& arg, int pov) override {
 
    auto val = arg.pov[pov];
 
    bool handled = false;
    // process the state of all pov buttons
    for (auto pair : m_povMotorMap)
    {
      auto button = pair.second;
      // process the pov event and store the the state of the POV
      button->processKeyboardData(m_foundation.simulation->getTimeStamp(), 0, 0, 0, (pair.first & val));
 
      if (button->getData() == nullptr) {
        auto action = button->getAction();
        (*action)(button->getKeyIsPressed(), button->getDown(), 0);
      }
 
      // update the speed of the constraints on this POV
      if (button->getData() && button->getDown())
        button->getData()->updateSpeed(button->getTimeDown());
    }
    return handled;
  }
 
  // If the gamepad/joystick has a slider that has changed value, this method will be called.
  bool sliderMoved(const agxIO::JoystickState&, int /*index*/) override {
    return false;
  }
 
private:
 
  void createStepEventCallbackUpdater()
  {
    auto motors = std::make_shared<Motors>();
    std::map<std::string, MotorsPtr> constraintMap;
    for (auto pair : m_buttonMotorMap)
      if (pair.second->getData())
        motors->insert(pair.second->getData()->getConstraints());
    for (auto pair : m_povMotorMap)
      if (pair.second->getData())
        motors->insert(pair.second->getData()->getConstraints());
    for (auto pair : m_axisMotorMap)
      if (pair.second->getData())
        motors->insert(pair.second->getData()->getConstraints());
 
    std::shared_ptr<EngineController> engineControl = std::make_shared<EngineController>(m_bulldozer);
 
    agxUtil::StepEventCallback::pre([this, motors, engineControl](agxSDK::Simulation* simulation)
      {
        motors->reset();
 
        // Here we loop over all our keys and calculate the new target speed for the constraints coupled to each key
        // Note that one constraint may be coupled to multiple keys and therefore we can't directly update the constraint
        // but instead have to accumulate the speed and update the constraint in a second loop.
        auto processKeyData = [motors, simulation](std::map<int, ButtonPtr>& keyMotorMap)
        {
          // Loop over all buttons/pov controls
          for (auto& keyData : keyMotorMap)
          {
            // Notify the button/pov controller that another time step is processed.
            keyData.second->tick(simulation->getTimeStamp());
 
            // Update the target speed of all actuator constraints
            if (keyData.second->getData())
              motors->addTargetSpeed(keyData.second->getData(), keyData.second->getData()->getTargetSpeed());
          }
        };
 
        processKeyData(m_buttonMotorMap);
        processKeyData(m_povMotorMap);
 
        // Loop over all axis controls
        for (auto& axisData : m_axisMotorMap)
        {
          axisData.second->tick(simulation->getTimeStamp());
 
          if (axisData.second->getData())
            motors->addTargetSpeed(axisData.second->getData(), axisData.second->getData()->getTargetSpeed());
        }
 
        // Update the constrain speed with respect to calculated speeds.
        motors->setConstraintTargetSpeed();
 
        engineControl->updateEngine();
 
      }, m_foundation.simulation);
  }
 
private:
  Utilities::Foundation m_foundation;
  BulldozerRef m_bulldozer;
 
  std::map<int, ButtonPtr> m_buttonMotorMap;
  std::map<int, AxisPtr> m_axisMotorMap;
  std::map<int, ButtonPtr> m_povMotorMap;
 
  // A map between the json string and the joystick states
  std::map<std::string, int> m_axisMap{
        { "Gamepad.Axis.LeftVertical",  0       },
        { "Gamepad.Axis.LeftHorizontal",  1     },
        { "Gamepad.Axis.RightVertical",  2      },
        { "Gamepad.Axis.RightHorizontal",  3    },
        { "Gamepad.Axis.LeftTrigger",  4        },
        { "Gamepad.Axis.RightTrigger",  5       }
  };
 
  std::map<std::string, int> m_buttonMap{
        { "Gamepad.Button.A",  8                                      },
        { "Gamepad.Button.B",  9                                      },
        { "Gamepad.Button.X",  10                                     },
        { "Gamepad.Button.Y",  11                                     },
        { "Gamepad.Button.Start",  0                                  },
        { "Gamepad.Button.Back",  1                                   },
        { "Gamepad.Button.LeftBumper",  4                             },
        { "Gamepad.Button.RightBumper",  5                            },
        { "Gamepad.Button.LeftStick",  2                              },
        { "Gamepad.Button.RightStick",  3                             }
  };
 
  std::map<std::string, int> m_povMap{
        { "Gamepad.Pov.North",  agxIO::JoystickState::NORTH         },
        { "Gamepad.Pov.East",  agxIO::JoystickState::EAST           },
        { "Gamepad.Pov.South",  agxIO::JoystickState::SOUTH         },
        { "Gamepad.Pov.West",  agxIO::JoystickState::WEST           },
        { "Gamepad.Pov.Centered",  agxIO::JoystickState::CENTERED   },
        { "Gamepad.Pov.NorthEast",  agxIO::JoystickState::NORTHEAST },
        { "Gamepad.Pov.SouthEast",  agxIO::JoystickState::SOUTHEAST },
        { "Gamepad.Pov.NorthWest",  agxIO::JoystickState::NORTHWEST },
        { "Gamepad.Pov.SouthWest",  agxIO::JoystickState::SOUTHWEST }
  };
 
};
 
class KeyboardListener : public agxSDK::GuiEventListener
{
public:
 
  KeyboardListener(Utilities::Foundation foundation, BulldozerRef bulldozer, agxJson::Value &keyboard_settings) :
    m_foundation(foundation), m_bulldozer(bulldozer)
  {
    setName("Keyboard listener");
 
    m_turnValueLeft = keyboard_settings["tracks_turn_left"]["value"].asDouble();
    m_turnValueRight = keyboard_settings["tracks_turn_right"]["value"].asDouble();
 
    size_t index = 0;
    for (auto& key_controll : keyboard_settings) {
 
      auto name = keyboard_settings.getMemberNames()[index];
      index++;
 
      if (name.rfind("_comment", 0) == 0 || !key_controll.hasMember("key"))
        continue;
 
      int asciiKey = 0;
      if (m_keyMap.find(key_controll["key"].asString()) != m_keyMap.end())
      {
        agxSDK::GuiEventListener::KeySymbol keySymbol = m_keyMap.find(key_controll["key"].asString())->second;
        asciiKey = (int)keySymbol;
      }
      else if (key_controll["key"].asString().rfind("ord('", 0) == 0)
      {
        // this is a key e.g., a-z
        asciiKey = (int)static_cast<unsigned char>(key_controll["key"].asString()[5]);
      }
 
      if (name.rfind("tracks_", 0) == 0) {
        ButtonPtr button = std::make_shared<Button>(name, asciiKey, nullptr, m_bulldozer->createAction(name));
        m_keyMotorMap.insert(std::make_pair(asciiKey, button));
      }
      else {
 
        std::vector<agx::Constraint1DOFRef> constraints = m_bulldozer->getConstraintsFromTag(name);
 
        ControllerMotorDataPtr motorData = std::make_shared<ControllerMotorData>(
          name,
          Utilities::jsonArrayToArray(key_controll["max_speed"]),
          key_controll["acceleration_time"].asDouble(),
          key_controll["deceleration_rate"].asDouble(),
          key_controll["deadzone"].asDouble(),
          key_controll["invert"].asBool(),
          constraints);
 
        ButtonPtr button = std::make_shared<Button>(name, asciiKey, motorData);
        m_keyMotorMap.insert(std::make_pair(asciiKey, button));
      }
    }
 
    createStepEventCallbackUpdater();
  }
 
  bool keyboard(int key, unsigned modkeyMask, float x, float y, bool keyDown) {
 
    bool handled = false;
 
    if (m_keyMotorMap.find(key) == m_keyMotorMap.end())
      return handled;
 
    auto button = m_keyMotorMap.find(key)->second;
    button->processKeyboardData(getSimulation()->getTimeStamp(), modkeyMask, x, y, keyDown);
 
    if (button->getData() == nullptr) {
      agx::Real value = 0;
      if (button->getName() == "tracks_turn_right")
        value = m_turnValueRight;
      else if (button->getName() == "tracks_turn_left")
        value = m_turnValueLeft;
      auto action = button->getAction();
      (*action)(button->getKeyIsPressed(), button->getDown(), value);
    }
    else if (button->getDown())
      button->getData()->updateSpeed(button->getTimeDown());
 
    return handled;
  }
 
private:
 
  void createStepEventCallbackUpdater()
  {
 
    auto motors = std::make_shared<Motors>();
    std::map<std::string, MotorsPtr> constraintMap;
    for (auto pair : m_keyMotorMap)
      if (pair.second->getData() != nullptr)
        motors->insert(pair.second->getData()->getConstraints());
 
    agxUtil::StepEventCallback::pre([this, motors](agxSDK::Simulation* /*simulation*/)
      {
        motors->reset();
 
        // Here we loop over all our keys and calculate the new target speed for the constraints coupled to each key
        // Note that one constraint may be coupled to multiple keys and therefore we can't directly update the constraint
        // but instead have to accumulate the speed and update the constraint in a second loop.
        for (auto keyData : m_keyMotorMap)
        {
          if (keyData.second->getData() == nullptr) // Have no constraint coupled.
            continue;
 
          if (!keyData.second->getDown())
            keyData.second->getData()->tick();
          // The constraint map contains one controller for each pair of prismatic joints (shovel lift/tilt etc..)
          // Here we combine the imput from all buttons on a specific pair of prismatics.
          motors->addTargetSpeed(keyData.second->getData(), keyData.second->getData()->getTargetSpeed());
        }
 
        motors->setConstraintTargetSpeed();
 
        m_bulldozer->updateDrivetrain();
      }, m_foundation.simulation);
  }
 
private:
  Utilities::Foundation m_foundation;
  BulldozerRef m_bulldozer;
 
  std::map<int, ButtonPtr> m_keyMotorMap;
  agx::Real m_turnValueRight;
  agx::Real m_turnValueLeft;
 
  // A map between the json string and the KeySymbol enum
  std::map<std::string, agxSDK::GuiEventListener::KeySymbol> m_keyMap {
      {"Input.KEY_Home", agxSDK::GuiEventListener::KEY_Home},
      {"Input.KEY_Left", agxSDK::GuiEventListener::KEY_Left},
      {"Input.KEY_Up", agxSDK::GuiEventListener::KEY_Up},
      {"Input.KEY_Right", agxSDK::GuiEventListener::KEY_Right},
      {"Input.KEY_Down", agxSDK::GuiEventListener::KEY_Down},
      {"Input.KEY_Prior", agxSDK::GuiEventListener::KEY_Prior},
      {"Input.KEY_Page_Up", agxSDK::GuiEventListener::KEY_Page_Up},
      {"Input.KEY_Next", agxSDK::GuiEventListener::KEY_Next},
      {"Input.KEY_Page_Down", agxSDK::GuiEventListener::KEY_Page_Down},
      {"Input.KEY_End", agxSDK::GuiEventListener::KEY_End},
      {"Input.KEY_Begin", agxSDK::GuiEventListener::KEY_Begin}
  };
};
 
Track::Track(agx::RigidBody* sprocket,
  agx::Hinge* sprocket_hinge,
  std::vector<agx::RigidBodyRef> rollers,
  std::pair<double, double> size,
  const agxJson::Value& trackSettings)
  : agxVehicle::Track(trackSettings["number_of_nodes"].asInt(), size.second, trackSettings["thickness"].asDouble()),
  m_sprocket(sprocket),
  m_sprocket_hinge(sprocket_hinge)
{
  m_sprocket_radius = size.first;
  m_track_width = size.second;
 
  #ifdef AGX_DEBUG
  // Verify sprocket material
  for (auto& geometry : m_sprocket->getGeometries()) {
    if (geometry->getEnableCollisions() && geometry->getMaterial()->getName() != "Unknown Material") {
      auto wheel_material = geometry->getMaterial();
      assert(wheel_material->getName() == "Wheel" && "Name of sprocket material must be Wheel.");
    }
  }
  #endif
 
  // Create transform
  agx::AffineMatrix4x4 transform;
  transform.setRotate(agx::EulerAngles(agx::PI_2, 0, agx::PI_2));
  transform.setTranslate(agx::Vec3(0, 0, 0));
 
  // Create sprocket wheel
  auto sprocket_wheel = new agxVehicle::TrackWheel(agxVehicle::TrackWheel::SPROCKET, m_sprocket_radius, m_sprocket, transform);
  add(sprocket_wheel);
 
  // Create roller wheels
  std::vector<agxVehicle::TrackWheelRef> rollers_wheel;
  rollers_wheel.reserve(rollers.size());
  for (size_t i = 0; i < rollers.size(); ++i) {
    auto wheel = new agxVehicle::TrackWheel(agxVehicle::TrackWheel::IDLER, m_sprocket_radius, rollers[i], transform);
    add(wheel);
    rollers_wheel.push_back(wheel);
  }
}
 
void Track::trackSetup(agxSDK::Simulation *simulation, const agxJson::Value& trackSettings)
{
  // Initialize material
  auto material = simulation->getMaterial("Track");
  if (material == nullptr) {
    material = new agx::Material("Track");
    material->getBulkMaterial()->setDensity(1800);
    simulation->add(material);
  }
  // Set material
  setMaterial(material);
 
  if (trackSettings["move_nodes_to_wheels"].asBool()) {
    for (auto& wheel : getWheels()) {
      wheel->setEnableProperty(agxVehicle::TrackWheel::MOVE_NODES_TO_WHEEL, true);
      wheel->setEnableProperty(agxVehicle::TrackWheel::MOVE_NODES_TO_ROTATION_PLANE, true);
    }
  }
 
  if (trackSettings["sprocket_hinge_direct_solver"].asBool()) {
    m_sprocket_hinge->setSolveType(agx::Constraint::DIRECT);
  }
 
  auto track_properties = getProperties();
  std::vector<std::pair<agx::Real, agx::Hinge::DOF>> hinge_compliance = {
      {4.0E-09, agx::Hinge::TRANSLATIONAL_1},
      {1.0E-11, agx::Hinge::TRANSLATIONAL_2},
      {4.0E-09, agx::Hinge::TRANSLATIONAL_3},
      {2.0E-07, agx::Hinge::ROTATIONAL_1},
      {2.0E-09, agx::Hinge::ROTATIONAL_2}
  };
 
  double default_hinge_damping = 2.0 * simulation->getTimeStep();
  std::vector<std::pair<agx::Real, agx::Hinge::DOF>> hinge_damping = {
      {default_hinge_damping, agx::Hinge::TRANSLATIONAL_1},
      {0.65 * default_hinge_damping, agx::Hinge::TRANSLATIONAL_2},
      {default_hinge_damping, agx::Hinge::TRANSLATIONAL_3},
      {default_hinge_damping, agx::Hinge::ROTATIONAL_1},
      {default_hinge_damping, agx::Hinge::ROTATIONAL_2}
  };
 
  for (const auto& c : hinge_compliance) {
    track_properties->setHingeCompliance(c.first, c.second);
  }
 
  for (const auto& c : hinge_damping) {
    track_properties->setHingeDamping(c.first, c.second);
  }
 
  auto merge_properties = getInternalMergeProperties();
  if (trackSettings["enable_merge"].asBool()) {
    merge_properties->setEnableMerge(true);
    merge_properties->setNumNodesPerMergeSegment(trackSettings["num_nodes_per_merge_segment"].asInt());
    merge_properties->setEnableLockToReachMergeCondition(false);
    merge_properties->setLockToReachMergeConditionCompliance(trackSettings["merge_condition_compliance"].asDouble());
    merge_properties->setContactReduction(agxVehicle::TrackInternalMergeProperties::AGGRESSIVE);
  }
 
  track_properties->setStabilizingHingeFrictionParameter(trackSettings["stabilizing_hinge_friction"].asDouble());
  track_properties->setMinStabilizingHingeNormalForce(trackSettings["stabilizing_hinge_normal_force"].asDouble());
 
  setProperties(track_properties);
}
 
void Track::createVisual(osg::Group *root)
{
  if (root != nullptr) {
    for (size_t i = 0; i < getNumNodes(); ++i) {
      auto nodeRigidBody = getNode(i)->getRigidBody();
      auto n = agxOSG::createVisual(nodeRigidBody, root);
      agxRender::Color color(100.0f / 255.0f, 100.0f / 255.0f, 100.0f / 255.0f, 1.0f);
      agxOSG::setDiffuseColor(n, color);
    }
  }
}
 
Bulldozer::Bulldozer(Utilities::Foundation &foundation, const std::string& path, const agxJson::Value& settings) :
  m_foundation(foundation), m_settings(settings), m_engineState()
{
  auto filePathContainer = agxIO::Environment::instance()->getFilePath(agxIO::Environment::RESOURCE_PATH);
  auto filePath = filePathContainer.find(path);
 
  auto success = agxOSG::readFile(filePath, simulation(), root(), this);
  if (!success)
    throw std::runtime_error("We can't open the agx bulldozer model: " + path);
 
  if (getRigidBody("Ground") != nullptr)
  {
    remove(getRigidBody("Ground"), true);
  }
 
  // Initialize bodies and constraints
  initializeBodiesAndConstraints();
 
  // create the three shovels (shovel, left and right ripper) and add them to the assembly
  createAndAddShovels(settings["shovel_settings"]);
 
  // Initialize the track
  createTracks();
 
  // Create drivetrain
  createDriveTrain();
}
 
void Bulldozer::updateDrivetrain() {
  // The engine throttle(controlling the RPM) is controlled via a PID controller to keep a constant RPM
  m_throttleController->update(simulation()->getTimeStamp());
 
  // Activate the brake
  // If parking brake is enaged, then just use max brake.
  if (m_engineState.getParkBrake()) {
    m_engineState.setLeftBrake(1);
    m_engineState.setRightBrake(1);
    m_engine->setEnable(true);
  }
 
  m_throttleController->setTargetRPM(m_targetRPM);
  setTrackDrivetrainState(m_engineState.getRightBrake(), m_clutches[int(Location::RIGHT)], m_brakes[int(Location::RIGHT)]);
  setTrackDrivetrainState(m_engineState.getLeftBrake(), m_clutches[int(Location::LEFT)], m_brakes[int(Location::LEFT)]);
}
 
Bulldozer::~Bulldozer()
{
  delete m_throttleController;
}
 
std::vector<agx::Constraint1DOFRef> Bulldozer::getConstraintsFromTag(const std::string& tag) const
{
  if (tag.rfind("shovel_lift", 0) == 0)
  {
    return getShovelLiftConstraints();
  }
  else if (tag.rfind("shovel_tilt", 0) == 0)
  {
    return getShovelTiltConstraints();
  }
  else if (tag.rfind("shovel_twist", 0) == 0)
  {
    auto vec1 = getShovelLiftConstraints();
    auto vec2 = getShovelTiltConstraints();
    vec1.insert(vec1.end(), vec2.begin(), vec2.end());
    return vec1;
  }
  else if (tag.rfind("ripper_lift", 0) == 0)
  {
    return getRipperLiftConstraints();
  }
  else if (tag.rfind("ripper_pitch", 0) == 0)
  {
    return getRipperPitchConstraints();
  }
  return std::vector<agx::Constraint1DOFRef>();
}
 
Action* Bulldozer::createAction(const std::string& tag)
{
  if (tag == "tracks_park_brake")
  {
    return new Action([this](bool isPressed, bool /*isDown*/, agx::Real /*value*/)
      {
        if (isPressed) {
          m_engineState.toggleParkBrake();
          if (!m_engineState.getParkBrake()) {
            m_engineState.setRightBrake(0);
            m_engineState.setLeftBrake(0);
          }
        }
      });
  }
  else if (tag == "tracks_gear_increment") {
    return new Action([this](bool isPressed, bool /*isDown*/, agx::Real /*value*/)
      {
        if (isPressed)
          m_gearBox->gearUp();
      });
  }
  else if (tag == "tracks_gear_decrement") {
    return new Action([this](bool isPressed, bool /*isDown*/, agx::Real /*value*/)
      {
        if (isPressed)
          m_gearBox->gearDown();
      });
  }
  else if (tag == "tracks_turn_right") {
    return new Action([this](bool /*isPressed*/, bool isDown, agx::Real value)
      {
        if (isDown)
          m_engineState.setRightBrake(value);
        else
          m_engineState.setRightBrake(0.0);
      });
  }
  else if (tag == "tracks_turn_left") {
    return new Action([this](bool /*isPressed*/, bool isDown, agx::Real value)
      {
        if (isDown)
          m_engineState.setLeftBrake(value);
        else
          m_engineState.setLeftBrake(0.0);
      });
  }
  else if (tag == "tracks_throttle_inc") {
    return new Action([this](bool isPressed, bool /*isDown*/, agx::Real /*value*/)
      {
        if (isPressed) {
          auto new_rpm = m_targetRPM + m_throttleStep;
          m_targetRPM = agx::clamp(new_rpm, m_idleRPM, m_maxRPM);
        }
      });
  }
  else if (tag == "tracks_throttle_dec") {
    return new Action([this](bool isPressed, bool /*isDown*/, agx::Real /*value*/)
      {
        if (isPressed) {
          auto new_rpm = m_targetRPM - m_throttleStep;
          m_targetRPM = agx::clamp(new_rpm, m_idleRPM, m_maxRPM);
        }
      });
  }
  else if (tag == "tracks_right_brake") {
    return new Action([this](bool /*isPressed*/, bool /*isDown*/, agx::Real value)
      {
        m_engineState.setRightBrake(value);
      });
  }
  else if (tag == "tracks_left_brake") {
    return new Action([this](bool /*isPressed*/, bool /*isDown*/, agx::Real value)
      {
        m_engineState.setLeftBrake(value);
      });
  }
  else if (tag == "shovel_twist")
  {
    return new Action([](bool /*isPressed*/, bool /*isDown*/, agx::Real /*value*/)
    {
    });
  }
  else
  {
    return nullptr;
  }
}
 
void Bulldozer::createAndAddShovels(const agxJson::Value& shovel_settings)
{
 
  m_shovel = createShovel("Shovel", shovel_settings);
  add(m_shovel);
  auto leftRipper = createShovel("LeftRipper", shovel_settings);
  add(leftRipper);
  auto rightRipper = createShovel("RightRipper", shovel_settings);
  add(rightRipper);
}
 
agxTerrain::Shovel* Bulldozer::createShovel(const std::string &name, const agxJson::Value& shovel_settings)
{
  // Create a shovel using the parameters for the specific bulldozer
  auto terrain_shovel = new agxTerrain::Shovel(Utilities::getShovelBody(name, this),
    Utilities::getShovelTopEdge(name, this),
    Utilities::getShovelCuttingEdge(name, this),
    Utilities::getShovelCuttingDirection(name, this));
 
  // Set the shovel settings
  terrain_shovel->getSettings()->setVerticalBladeSoilMergeDistance(shovel_settings["vertical_blade_soil_merge_distance"].asDouble());
  terrain_shovel->getAdvancedSettings()->setNoMergeExtensionDistance(shovel_settings["merge_extension_distance"].asDouble());
  terrain_shovel->getSettings()->setPenetrationDepthThreshold(shovel_settings["penetration_depth_threshold"].asDouble());
  terrain_shovel->getSettings()->setPenetrationForceScaling(shovel_settings["penetration_force_scaling"].asDouble());
 
#ifdef AGX_DEBUG
  // Ensure the collision geometries in the shovel have the correct material
  auto it = std::find_if(terrain_shovel->getRigidBody()->getGeometries().begin(),
    terrain_shovel->getRigidBody()->getGeometries().end(),
    [](const auto& geometry) { return geometry->getEnableCollisions(); });
  auto shovel_material = it->get()->getMaterial();
  assert(shovel_material->getName() == "Shovel" && "Is the material on the shovel defined? Expected Shovel.");
#endif
 
  return terrain_shovel;
}
 
void Bulldozer::createTracks()
{
  auto trackSettings = m_settings["track_settings"];
  auto size = Utilities::find_size(m_modelComponents ["LeftSprocket"]);
  auto track = new Track(m_modelComponents ["LeftSprocket"],
    Utilities::toHinge(m_actuatorConstraints["LeftSprocketHinge"]),
    std::vector<agx::RigidBodyRef>{ m_modelComponents ["LeftRearRollerWheel"], m_modelComponents ["LeftFrontRollerWheel"] },
    size,
    trackSettings);
  track->trackSetup(m_foundation.simulation, trackSettings);
  m_tracks.push_back(track);
  add(track);
 
  track = new Track(m_modelComponents ["RightSprocket"],
    Utilities::toHinge(m_actuatorConstraints["RightSprocketHinge"]),
    std::vector<agx::RigidBodyRef>{ m_modelComponents ["RightRearRollerWheel"], m_modelComponents ["RightFrontRollerWheel"] },
    size,
    trackSettings);
  track->trackSetup(m_foundation.simulation, trackSettings);
  m_tracks.push_back(track);
  add(track);
}
 
void Bulldozer::buildDrivetrainForTrack(agxDriveTrain::Differential* differential, agxPowerLine::Actuator* actuator, Location location, agxJson::Value drivetrainSettings) {
  std::string side = "Left_";
  if (location == Location::RIGHT)
    side = "Right_";
 
  agxDriveTrain::ShaftRef differentialShaft = new agxDriveTrain::Shaft();
  differential->connect(agxPowerLine::OUTPUT, agxPowerLine::INPUT, differentialShaft);
 
  agxDriveTrain::DryClutchRef clutch = new agxDriveTrain::DryClutch();
  clutch->setTorqueCapacity(drivetrainSettings["clutch_torque_capacity"].asDouble());
  clutch->setEngage(false);
  clutch->connect(agxPowerLine::INPUT, agxPowerLine::OUTPUT, differentialShaft);
 
  agxDriveTrain::ShaftRef clutchBrakeShaft = new agxDriveTrain::Shaft();
  clutchBrakeShaft->getRotationalDimension()->getOrReserveBody()->setName(side + "clutch_brake_shaft");
  clutch->connect(agxPowerLine::OUTPUT, agxPowerLine::INPUT, clutchBrakeShaft);
 
  agxDriveTrain::BrakeRef brake = new agxDriveTrain::Brake();
  brake->setTorqueCapacity(drivetrainSettings["brake_torque_capacity"].asDouble());
  clutchBrakeShaft->connect(brake);
 
  agxDriveTrain::ShaftRef actuatorShaft = new agxDriveTrain::Shaft();
  actuatorShaft->getRotationalDimension()->getOrReserveBody()->setName(side + "actuator_shaft");
  brake->connect(actuatorShaft);
 
  brake->connect(actuatorShaft);
  actuator->connect(actuatorShaft);
 
  m_clutches[int(location)] = clutch;
  m_brakes[int(location)] = brake;
  m_actuatorShafts[int(location)] = actuatorShaft;
}
 
 
void Bulldozer::setTrackDrivetrainState(agx::Real input, agxDriveTrain::DryClutch* clutch, agxDriveTrain::Brake* brake) {
  if (0 <= input && input <= m_steeringLimits[0]) {  // Full speed ahead
    clutch->setFraction(1);
    brake->setFraction(0);
  }
  else if (m_steeringLimits[0] < input && input <= m_steeringLimits[1]) {  // Start to disengage the clutch
    auto clutch_fraction = (input - m_steeringLimits[0]) / (m_steeringLimits[1] - m_steeringLimits[0]);
    clutch->setFraction(clutch_fraction);
    brake->setFraction(0);
  }
  else if (m_steeringLimits[1] < input && input <= m_steeringLimits[2]) {  // Dead zone, clutch disengaged, no brake
    clutch->setFraction(0);
    brake->setFraction(0);
  }
  else if (m_steeringLimits[2] < input && input <= 1) {  // Start to apply brake
    clutch->setFraction(0);
    auto brake_fraction = (input - m_steeringLimits[2]) / (1 - m_steeringLimits[2]);
    brake->setFraction(brake_fraction);
  }
}
 
 
void Bulldozer::createDriveTrain() {
  auto drivetrainSettings = m_settings["drivetrain_settings"];
  m_powerline = new agxPowerLine::PowerLine;
  add(m_powerline);
 
  // Engine
  auto engineParameters = agxDriveTrain::CombustionEngineParameters();
  agxDriveTrain::CombustionEngine::readLibraryProperties(drivetrainSettings["engine"].asString(), engineParameters);
  m_targetRPM = engineParameters.maxTorqueRPM;
  m_idleRPM = engineParameters.idleRPM;
  m_maxRPM = engineParameters.maxRPM;
 
  m_engine = new agxDriveTrain::CombustionEngine(engineParameters);
  m_engine->setEnable(true);
 
  m_steeringLimits = Utilities::jsonArrayToArray(drivetrainSettings["steering_limits"]);
 
  agx::Real kp = 0.0004;
  agx::Real ki = 0.0005;
  agx::Real kd = 1e-06;
 
  m_throttleController = new ThrottleController(this, kp, ki, kd);
  m_throttleController->setTargetRPM(m_targetRPM);
  m_throttleStep = drivetrainSettings["throttle_step"].asDouble();
 
  agx::RealVector centralGearRatios = Utilities::jsonArrayToArray(drivetrainSettings["gear_box"]["gear_ratios"]);
 
  m_gearBox = new agxDriveTrain::GearBox();
  m_gearBox->setGearRatios(centralGearRatios);
  m_gearBox->setGear(drivetrainSettings["gear_box"]["start_gear_index"].asInt());
 
  for (auto track : m_tracks) {
    m_actuators.push_back(new agxPowerLine::RotationalActuator(track->sprocket_hinge()));
    track->sprocket_hinge()->getMotor1D()->setEnable(false);
  }
 
  m_powerline->add(m_engine);
  simulation()->add(m_powerline);
 
  agxDriveTrain::ShaftRef engineTorqueConverterShaft = new agxDriveTrain::Shaft();
  m_engine->connect(engineTorqueConverterShaft);
 
  auto body = engineTorqueConverterShaft->getRotationalDimension()->getOrReserveBody();
  body->setAngularVelocityDamping(0.5);
  body->setInertiaTensor(agx::Vec3(10, 10, 10));
 
  // Torque converter
  m_torqueConverter = new agxDriveTrain::TorqueConverter();
  m_torqueConverter->setPumpDiameter(drivetrainSettings["torque_converter_pump_diameter"].asDouble());
  m_torqueConverter->enableLockUp(false);
 
  agx::Real scale = 2.0;
  agx::RealPairVector velRatio_efficiency;
  velRatio_efficiency.push_back(agx::RealPair(0.0, 2.1 * scale));
  velRatio_efficiency.push_back(agx::RealPair(0.5, 1.5 * scale));
  velRatio_efficiency.push_back(agx::RealPair(0.95, 0.7 * scale));
  velRatio_efficiency.push_back(agx::RealPair(1.0, 0.0));
  velRatio_efficiency.push_back(agx::RealPair(1.1, 1.0 * scale));
  velRatio_efficiency.push_back(agx::RealPair(1.25, 1.5 * scale));
  velRatio_efficiency.push_back(agx::RealPair(1.5, 1.5 * scale));
  m_torqueConverter->setEfficiencyTable(velRatio_efficiency);
 
  m_torqueConverter->connect(agxPowerLine::INPUT, agxPowerLine::OUTPUT, engineTorqueConverterShaft);
 
  agxDriveTrain::ShaftRef torqueConverterGearShaft = new agxDriveTrain::Shaft();
  m_torqueConverter->connect(agxPowerLine::OUTPUT, agxPowerLine::INPUT, torqueConverterGearShaft);
 
  m_gearBox->connect(agxPowerLine::INPUT, agxPowerLine::OUTPUT, torqueConverterGearShaft);
 
  agxDriveTrain::ShaftRef gearDifferentialShaft = new agxDriveTrain::Shaft();
  m_gearBox->connect(agxPowerLine::OUTPUT, agxPowerLine::INPUT, gearDifferentialShaft);
 
  agxDriveTrain::DifferentialRef differential = new agxDriveTrain::Differential();
  differential->setLock(true);
  differential->connect(agxPowerLine::INPUT, agxPowerLine::OUTPUT, gearDifferentialShaft);
 
  m_clutches = { nullptr, nullptr };
  m_brakes = { nullptr, nullptr };
  m_actuatorShafts = { nullptr, nullptr };
 
  // Connect the drive train to each sprocket hinge
  buildDrivetrainForTrack(differential, m_actuators[int(Location::RIGHT)], Location::RIGHT, drivetrainSettings);
  buildDrivetrainForTrack(differential, m_actuators[int(Location::LEFT)], Location::LEFT, drivetrainSettings);
}
 
 
void Bulldozer::initializeBodiesAndConstraints()
{
  // Filter and store rigid bodies and constraints
  // Assuming getRigidBodies and getConstraints are member functions returning std::vector
  for (auto& rb : getRigidBodies()) {
    if (m_modelComponents .find(rb->getName()) != m_modelComponents .end()) {
      m_modelComponents [rb->getName()] = rb;
    }
  }
  for (auto& c : getConstraints()) {
    if (m_actuatorConstraints.find(c->getName()) != m_actuatorConstraints.end()) {
      m_actuatorConstraints[c->getName()] = dynamic_cast<agx::Constraint1DOF*>(c.get());
    }
  }
  m_actuatorConstraints["LeftSprocketHinge"] = Utilities::getHingeOnBody(m_modelComponents ["LeftSprocket"], getConstraints());
  m_actuatorConstraints["RightSprocketHinge"] = Utilities::getHingeOnBody(m_modelComponents ["RightSprocket"], getConstraints());
 
  // disable some motors that are part if the original agx file
  auto hinge = Utilities::getHingeOnBody(m_modelComponents ["LeftRearRollerWheel"], getConstraints());
  hinge->getMotor1D()->setEnable(false);
  hinge = Utilities::getHingeOnBody(m_modelComponents ["LeftFrontRollerWheel"], getConstraints());
  hinge->getMotor1D()->setEnable(false);
  hinge = Utilities::getHingeOnBody(m_modelComponents ["RightRearRollerWheel"], getConstraints());
  hinge->getMotor1D()->setEnable(false);
  hinge = Utilities::getHingeOnBody(m_modelComponents ["RightFrontRollerWheel"], getConstraints());
  hinge->getMotor1D()->setEnable(false);
}
 
ThrottleController::ThrottleController(Bulldozer* bulldozer, agx::Real p, agx::Real i, agx::Real d)
  : m_bulldozer(bulldozer), m_targetRPM(0) {
  m_pidController = new agxModel::PidController1D();
  m_pidController->setGains(p, i, d);
  m_pidController->setIntegralWindupClamping(0, 1);
  m_pidController->setSetPoint(m_targetRPM);
 
}
 
void ThrottleController::setTargetRPM(agx::Real targetRPM) {
  m_targetRPM = targetRPM;
}
 
agx::Real ThrottleController::getCurrentRPM() const {
  return m_bulldozer->getEngine()->getRPM();
}
 
void ThrottleController::update(agx::Real t) {
  m_pidController->setSetPoint(m_targetRPM);
  m_pidController->update(t, getCurrentRPM());
  m_bulldozer->getEngine()->setThrottle(m_pidController->getManipulatedVariable());
}
 
ThrottleController::~ThrottleController() {
  delete m_pidController;
}
 
// An empty scene for agxViewer.
osg::Group* buildBulldozerScene(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  // Execute the script that build our model.
  // The function buildScene1 that can be found in the script will construct terrain and place a bulldozer with shovels and tracks on the terrain.
  // Constraints, RigidBodies, geometries materials etc. can later be accessed through names via the Simulation object.
  agx::setNumThreads(4);
  auto solver = simulation->getSolver();
  solver->setNumPPGSRestingIterations(10);
 
  // The sceen root node
  osg::Group* root = new osg::Group();
  // A help struct to access simulation, application and root classes from objects
  auto foundation = Utilities::Foundation(simulation, application, root);
 
  Utilities::setupCamera(foundation);
 
  // Read all settings from the external json-file
  agxJson::Value settings;
  Utilities::readSettings("data/models/bulldozer_d10t.json", settings);
 
  // Create the terrain
  auto terrain = Utilities::createTerrain(foundation, settings["terrain_settings"]);
  simulation->add(terrain);
 
  // Create the terrain renderer (this will create a grid texture or a durt texture depending on the terrain settings)
  auto renderer = Utilities::createTerrainVoxelRenderer(terrain, root, settings["terrain_settings"]);
  simulation->add(renderer);
 
  // Create and add the bulldozer to the scene
  BulldozerRef bulldozer = new Bulldozer(foundation, "data/models/bulldozer_d10t.agx", settings);
  simulation->add(bulldozer);
 
  // We have to create the visual representation of the tracks after the bulldozer has been added to the simulation
  bulldozer->getTrack(0)->createVisual(root);
  bulldozer->getTrack(1)->createVisual(root);
 
  bulldozer->setPosition(-2, 0, 1.6);
 
  agxDriveTrain::EnergyManager::add(bulldozer->getEngine());
 
  // Setup the contact materials
  // Create a slippery contact material between the track and the bogie
  Utilities::createAndAddContactMaterial(simulation, settings["contact_material_settings"]["track_bogie"], bulldozer->getChassisBody());
  // Create a high friction contact material between the track and the track wheels
  Utilities::createAndAddContactMaterial(simulation, settings["contact_material_settings"]["track_wheel"], bulldozer->getChassisBody());
  // Create a material between the track and the terrain
  Utilities::createAndAddContactMaterial(simulation, settings["contact_material_settings"]["track_terrain"], bulldozer->getChassisBody());
  // Create a contact material between the shovel and terrain particles
  Utilities::createAndAddParticleContactMaterial(simulation, settings["contact_material_settings"]["shovel_particles"], terrain);
 
  // Create a sensor manager.
  // This will initialize the joystick/gamepad using the current graphic window.
  agxIO::JoystickManagerRef manager = new agxIO::JoystickManager(application->getHWND());
  simulation->add(manager);
 
  // If we do not have a device connected we will display a text.
  if (!manager->valid())
  {
    LOGGER_WARNING() << "No Gamepad or Joystick detected" << std::endl << LOGGER_END();
    LOGGER_INFO() << "Use keyboard controls" << std::endl << LOGGER_END();
    auto keyboardListener = new KeyboardListener(foundation, bulldozer, settings["keyboard_control"]);
    simulation->add(keyboardListener);
  }
  else
  {
 
    auto gamepad_settings = settings["gamepad_controls"];
    agx::ref_ptr<SimulationJoystickListener> gamepadListener = new SimulationJoystickListener(foundation, bulldozer, gamepad_settings);
    // Add the listener to the manager
    manager->add(gamepadListener);
  }
 
  agxUtil::StepEventCallback::post([application, bulldozer, terrain](agxSDK::Simulation*) {
    // Add screen text
    auto sd = application->getSceneDecorator();
    sd->clearText();
    sd->setText(1, agx::String::format("Speed:              %8.2f km / h", 3.6 * bulldozer->getSpeed()));
    agx::String parkBrakeString = "Parking brake:          ";
    parkBrakeString += bulldozer->getParkingBrake() ? "ON" : "OFF";
    sd->setText(2, parkBrakeString);
    sd->setText(3, agx::String::format("Gear ratio:           %4.0f", bulldozer->getGearRatio()));
    sd->setText(4, agx::String::format("Engine RPM:         %8.2f", bulldozer->getEngine()->getRPM()));
    sd->setText(6, agx::String::format("Mass in shovel:        %5.2f kg", bulldozer->getShovel()->getDynamicMass()));
    sd->setText(7, agx::String::format("Shovel Deadload:       %5.2f %%", bulldozer->getShovel()->getDeadLoadFraction() * 100.0));
    sd->setText(8, agx::String::format("Last Excavated Volume: %5.2f m3", terrain->getLastExcavatedVolume()));
  }, simulation);
 
#if AGX_USE_WEBPLOT()
 
  // Add plots
  auto plot = simulation->getPlotSystem();
  auto timeData = simulation->getTimeDataSeries();
  auto engineRpmSeries = new agxPlot::DataSeries(new EngineRpmDataGenerator(bulldozer->getEngine()), "RPM");
  auto engineTargetRpmSeries = new agxPlot::DataSeries(new EngineTargetRpmDataGenerator(bulldozer), "RPM");
  auto enginePowerSeries = new agxPlot::DataSeries(new EnginePowerDataGenerator(bulldozer->getEngine()), "Power");
 
  // Specify the units
  engineRpmSeries->setUnit("rpm");
  engineTargetRpmSeries->setUnit("RPM");
  enginePowerSeries->setUnit("kW");
 
  // Create curves
  auto engineRpmCurve = new agxPlot::Curve(timeData, engineRpmSeries, "Engine rpm" );
  auto engineTargetRpmCurve = new agxPlot::Curve(timeData, engineTargetRpmSeries, "Target rpm");
  auto enginePowerCurve = new agxPlot::Curve(timeData, enginePowerSeries, "Engine power");
 
  // Create a window
  auto plotWindow = plot->getOrCreateWindow("Engine rpm");
 
  // Add the curves to the window
  plotWindow->add(engineRpmCurve);
  plotWindow->add(engineTargetRpmCurve);
 
  // New plot
  plotWindow = plot->getOrCreateWindow("Engine Power");
  plotWindow->add(enginePowerCurve);
 
  if (!g_isUnitTest)
  {
    // Add the webplot output to the plot system
    auto webplot = new agxPlot::WebPlot(true);
    plot->add(webplot);
  }
#endif // webplot
 
  return root;
}
 
int main(int argc, char** argv)
{
  agx::AutoInit agxInit;
 
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial bulldozer_with_terrain \n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  g_isUnitTest = argc >= 2; // Do not show plots if we sent in arguments - this is only for the internal testing framework at Algoryx.
 
#ifdef _MSC_VER
  // Handle ctrl-c
  if (SetConsoleCtrlHandler((PHANDLER_ROUTINE)sighandler, TRUE) == FALSE)
  {
    // unable to install handler...
    // display message to the user
    LOGGER_WARNING() << "Unable to install signal handler!" << std::endl << LOGGER_END();
    return 1;
  }
#else
  // signal(SIGABRT, &sighandler);
  signal(SIGTERM, &sighandler);
  signal(SIGINT, &sighandler);
#endif
 
  int result = 1;
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
    agxIO::ArgumentParserRef arguments = new agxIO::ArgumentParser(argc, argv);
 
    if (application->init(arguments)) {
 
      application->addScene(buildBulldozerScene, '1');
 
      result = application->run();
 
    }
  }
  catch (std::exception& e) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
  }
 
  return result;
}
 
 
#else
 
int main(int, char**) {
  std::cout << "This tutorial requires agxTerrain and agxSensor" << std::endl;
  return 0;
}
 
#endif // AGX_USE_OIS() && AGX_USE_AGXTERRAIN()

tutorial_cable.cpp

/*
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.
*/
 
 
/*
 
The agxCable module is used to simulate cables, hoses and dress packs. These are
long structures with circular cross sections that can bend, stretch and twist.
The agxCable namespace provides the Cable class which uses a lumped elements
approach to model such structures. By using agxCable::Cable it is possible to
create simulations with cable structures which behave realistically in contacts
and interaction with other simulated objects. This can be used for training
operations of assembling scenarios, measurement of load/tear/wear of cables in a
robot scenario etc.
 
agxCable is similar to agxWire, but there are some differences. The cables
support torsion and plasticity, they have a fixed resolution and realistic self
interaction. The wires on the other hand have dynamic resolution and can therefore
handle larger tensions and longer distances. Wires also support operations such as cut and
merge, and can be spooled in and out of winches.
 
 
 This tutorial demonstrates how to:
 
- Create and route a cable.
- Attach cable to objects.
- Set cable properties.
- Inspect dynamic state.
- Control initialization.
- Detect cable collisions.
- Attach objects on initialized cable.
- Using rebind to create a wire with a non-straight relaxed configuration.
- Control the precise initial location and orientation of segments.
 
*/
 
 
#include <agxCable/Cable.h>
#include <agxCable/Node.h>
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/Node.h>
#include <agxOSG/utils.h>
 
#include <agxCollide/Geometry.h>
#include <agxCollide/Shape.h>
#include <agxCollide/Box.h>
#include <agxCollide/Cylinder.h>
 
#include <agx/Hinge.h>
#include <agx/version.h>
 
 
#include <tuple>
 
static bool g_isUnitTest = false;
 
 
 
 
 
// None-cable related helper functions.
namespace
{
 
  std::tuple<agx::RigidBodyRef, agxCollide::GeometryRef> createBody(
      agxSDK::Simulation* simulation, osg::Group* root, agxCollide::Shape* shape,
      agx::String const & name = "", agx::RigidBody::MotionControl motionControl = agx::RigidBody::DYNAMICS)
  {
    agx::RigidBodyRef body = new agx::RigidBody(name);
    agxCollide::GeometryRef geometry = new agxCollide::Geometry(shape);
    body->add(geometry);
    body->setMotionControl(motionControl);
    simulation->add(body);
    agxOSG::createVisual(body, root);
    return std::make_tuple(body,geometry);
  }
}
 
 
 
osg::Group* buildTutorial1(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  osg::Group* root = new osg::Group();
  std::cout << "Tutorial 1 - Basics of creating a cable." << std::endl;
 
  agx::Vec3 groundHe(agx::Real(1.7), agx::Real(1.1), agx::Real(0.5));
  ::createBody(simulation, root, new agxCollide::Box(groundHe), "Ground", agx::RigidBody::STATIC);
 
 
  /*
  Cables in AGX are created as instances of the agxCable::Cable class. A
  fundamental property of a cable is its radius, so that is the first argument
  to the constructor. The second argument is the resolution we want. The cable
  implementation is based on a lumped elements method, which means that the
  continuous cable is modeled as a collection of discrete elements, called
  segments, that are linked together. The resolution specifies the number of
  segments that should be created per unit length.
 
  In this tutorial we will use the SI unit system, so the radius is expressed in
  meters and the resolution in segments per meter.
 
  Using these two arguments we can create a handle to a cable object. The rest
  of the cable definition, such as length and route, is configured later.
  */
  agx::Real const radius(0.04);
  agx::Real const resolution(5);
  agxCable::CableRef cable = new agxCable::Cable(radius, resolution);
 
  /*
  Once the handle has been created we can start routing the cable. In the
  simplest case this is done by declaring points in the world that the cable
  should pass through. Cable segments will be created between these points
  automatically according to the previously specified resolution.
 
  The points are created in the form of FreeNodes that are added to the cable.
  By adding two non-coinciding FreeNodes we get a straight cable that extends
  from  the first point to the second. We call such a pair a 'leg' of the
  cable. Nodes added to the cable in this fashion are called routing nodes.
  */
  cable->add(new agxCable::FreeNode(agx::Vec3(agx::Real(-1.0), agx::Real(0.0), agx::Real(1.0))));
  cable->add(new agxCable::FreeNode(agx::Vec3(agx::Real( 1.0), agx::Real(0.0), agx::Real(1.0))));
 
  /*
  By adding a third FreeNode to the cable we get a new straight cable section,
  possibly consisting of several segments, extending from the position of the
  second FreeNode to the position of the third. This forms the second leg of the
  cable.
 
  The segments does not always perfectly divide the length of a leg, causing the
  cable to be created in a slightly stretched or compressed state. As the
  simulation runs the cable will strive to restore its rest length and to
  straighten out any bends that the routing created. Ways to control the initial
  state are demonstrated in the tutorial titled "Routing parameters" found later
  in this file.
  */
  cable->add(new agxCable::FreeNode(agx::Vec3(agx::Real( 1.5), agx::Real(0.0), agx::Real(1.5))));
 
  /*
  The construction of the cable is finalized when it is added to the simulation.
  Nodes added after this call will not be included.
  */
  simulation->add(cable);
 
  /*
  We enable debug rendering in order to see the cable. This makes it possible
  to see how the cable has been split into segments. In Tutorial 4 we learn how
  to do our own rendering of the cable segments.
 
  Notice that there are a bit of overlap between the cable segments. Each
  segment is modeled with a capsule and the two facing hemispheres overlap. The
  advantage of this, compared to using cylinders, is that the cable remains
  smooth when bent.
  */
  application->setEnableDebugRenderer(true);
  application->setEnableOSGRenderer(false);
  if (!g_isUnitTest) {
    application->setAutoStepping(false);
  }
 
  return root;
}
 
 
 
 
namespace tutorial2_helpers
{
  std::tuple<agx::RigidBodyRef, agx::RigidBodyRef, agx::RigidBodyRef>
  createScene(agxSDK::Simulation* simulation, osg::Group* root,
              agx::Real baseLength, agx::Real armLength,
              agx::Real baseRadius, agx::Real armRadius,
              agx::Real machineHeight)
  {
    agx::Real baseHalfLength(baseLength / 2);
    agx::Real armHalfLength(armLength / 2);
 
    // Create the bodies and geometries we need.
 
    agx::RigidBodyRef ground;
    agxCollide::GeometryRef groundGeom;
    std::tie(ground, groundGeom) = createBody(
          simulation, root,
          new agxCollide::Box(agx::Real(10), agx::Real(5), agx::Real(0.5)),
          "Ground", agx::RigidBody::STATIC);
 
    agx::RigidBodyRef wall;
    agxCollide::GeometryRef wallGeom;
    std::tie(wall, wallGeom) = createBody(
          simulation, root,
          new agxCollide::Box(agx::Real(10), agx::Real(0.5), agx::Real(5)),
          "Wall", agx::RigidBody::STATIC);
 
    agx::RigidBodyRef base;
    agxCollide::GeometryRef baseGeom;
    std::tie(base, baseGeom) = createBody(
          simulation, root,
          new agxCollide::Cylinder(baseRadius, baseLength));
 
    agx::RigidBodyRef arm;
    agxCollide::GeometryRef armGeom;
    std::tie(arm, armGeom) = createBody(
          simulation, root,
          new agxCollide::Cylinder(armRadius, armLength));
 
    wall->setPosition(agx::Real(0.0), agx::Real(5.5), agx::Real(5.5));
    base->setPosition(agx::Real(0.0), baseHalfLength, machineHeight);
    arm->setPosition(armHalfLength, agx::Real(0.0), machineHeight);
    arm->setRotation(agx::EulerAngles(agx::Real(0.0), agx::Real(0.0), -agx::PI_2));
 
    simulation->getSpace()->setEnableCollisions(wallGeom, baseGeom, false);
    simulation->getSpace()->setEnableCollisions(baseGeom, armGeom, false);
 
 
    // Create hinges holding and driving the two moving parts.
 
 
    // Wall to base.
    agx::Frame* f1 = new agx::Frame();
    agx::Frame* f2 = new agx::Frame();
    agx::Constraint::calculateFramesFromWorld(machineHeight * agx::Z_AXIS, agx::Y_AXIS, wall, f1, base, f2);
    agx::HingeRef constraint1 = new agx::Hinge(wall, f1, base, f2);
    simulation->add(constraint1);
 
    // Base to arm.
    agx::Frame* f3 = new agx::Frame();
    agx::Frame* f4 = new agx::Frame();
    agx::Constraint::calculateFramesFromWorld(machineHeight * agx::Z_AXIS, agx::Z_AXIS, base, f3, arm, f4);
    agx::HingeRef constraint2 = new agx::Hinge(base, f3, arm, f4);
    simulation->add(constraint2);
 
    // Function that creates a StepEventListener that makes the given hinge
    // swing back and forth between the given minAngle and maxAngle.
    auto createHingeOperator = [](agx::Hinge* hinge, agx::Real minAngle, agx::Real maxAngle, agx::Real speed, agxSDK::Simulation* simulation)
    {
      class HingeOperator : public agxSDK::StepEventListener
      {
        public:
          HingeOperator(agx::Hinge* hinge, agx::Real minAngle, agx::Real maxAngle, agx::Real speed)
            : m_hinge(hinge)
            , m_minAngle(minAngle)
            , m_maxAngle(maxAngle)
            , m_speed(speed)
            , m_ignoreUntil(2.0)
          {
          }
 
          virtual void pre(agx::TimeStamp const & time) override
          {
            if (time > m_ignoreUntil)
            {
              if (m_hinge->getMotor1D()->getSpeed() >=agx::Real(0.0))
              { // Going forward, check if we've reached max angle.
                if (m_hinge->getAngle() > m_maxAngle)
                { // Max angle reached, turn around.
                  m_hinge->getMotor1D()->setSpeed(-m_speed);
                  m_ignoreUntil = time + agx::Real(1);
                }
                else
                { // Still some way to go. Make sure we're going in the right direction.
                  m_hinge->getMotor1D()->setSpeed(m_speed);
                }
              }
              else if (m_hinge->getMotor1D()->getSpeed() < agx::Real(0.0))
              { // Going backwards, check if we're reached min angle.
                if (m_hinge->getAngle() < m_minAngle)
                { // Min angle reached, turn around.
                  m_hinge->getMotor1D()->setSpeed(m_speed);
                  m_ignoreUntil = time + agx::Real(1);
                }
                else
                { // Still some way to go. Make sure we're going in the right direction.
                  m_hinge->getMotor1D()->setSpeed(-m_speed);
                }
              }
            }
          }
        protected:
          virtual ~HingeOperator() {}
        private:
          agx::HingeRef m_hinge;
          agx::Real m_minAngle;
          agx::Real m_maxAngle;
          agx::Real m_speed;
          agx::Real m_ignoreUntil;
      };
 
      hinge->getMotor1D()->setEnable(true);
      hinge->getMotor1D()->setSpeed(agx::Real(0.0));
      simulation->add(new HingeOperator(hinge, minAngle, maxAngle, speed));
    };
 
    createHingeOperator(constraint1, agx::Real(-3.0), agx::Real(0.4), agx::Real(1.0), simulation);
    createHingeOperator(constraint2, agx::Real(-0.5), agx::Real(1.5), agx::Real(1.0), simulation);
 
    return std::make_tuple(ground, base, arm); 
  }
}
 
 
/*
Tutorial 2 - Attaching the cable to rigid bodies.
Demonstrates how to attach the cable to an articulated arm so that the cable
follows the motions of the arm.
 
When reading this tutorial it is recommended to have the initial state of the
created scene shown by running the tutorial so that the environment and the
resulting cable routing can been seen.
*/
osg::Group* buildTutorial2(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  std::cout << "Tutorial 2 - Attaching the cable to rigid bodies." << std::endl;
  osg::Group* root = new osg::Group();
 
  using namespace tutorial2_helpers;
 
  /*
  We will create a mechanics device consisting of two cylinders hinged together.
  These parameters control the size and position of the device, and also help
  when routing the cable along the device.
  */
  agx::Real baseRadius(1);
  agx::Real armRadius(0.5);
  agx::Real baseLength(5);
  agx::Real armLength(5);
  agx::Real baseHalfLength(baseLength / 2);
  agx::Real armHalfLength(armLength / 2);
  agx::Real machineHeight(7);
 
  /*
  Create a simple mechanical device, including an equally simple controller to
  move it around. Returned are the bodies that we wish to attach the cable to.
  The device consists of a base that can rotate and an arm attached to the base
  that can swing back and forth.
  */
  agx::RigidBodyRef ground;
  agx::RigidBodyRef base;
  agx::RigidBodyRef arm;
  std::tie(ground, base, arm) = createScene(simulation, root, baseLength, armLength, baseRadius, armRadius, machineHeight);
 
 
  /*
  We create the cable handle in much the same way as in Tutorial 1. We use a
  slightly lower resolution since this time we will create a longer cable in a
  larger scene.
  */
  agx::Real radius(0.05);
  agx::Real resolution(3);
  agxCable::CableRef cable = new agxCable::Cable(radius, resolution);
 
  /*
  The goal is to route the cable from a fixed point on the floor up to the base
  of the articulated arm and then follow along the extent of the arm out to the
  tip. Some slack is added along the way to give the arm room to move without
  stretching the cable.
  */
 
  /*
  The first step is to create the start of the cable, which should be attached
  to the floor. This is done using a node type called BodyFixedNode. To create a
  BodyFixedNode we need a RigidBody; the body that the cable attaches to, and a
  transformation. The transformation specifies where on the body that the cable
  is attached.
 
  Here we place the attachment point a bit along the negative X axis, a shorter
  bit along the positive Y axis and just a tad up along the Z axis. In the
  scene, assuming the default camera orientation, this places the attachment
  point on the surface of the floor slightly to the left of the device.
  */
  agx::AffineMatrix4x4 startTransform;
  startTransform.setTranslate(agx::Real(-1.5), agx::Real(1.0), agx::Real(0.5)+radius);
  cable->add(new agxCable::BodyFixedNode(ground, startTransform));
 
  /*
  The BodyFixedNode created above is placed one cable radius above the surface of
  the ground. The segment starting at the routing node is placed so that the
  cylinder part of that segment's capsule starts at the routing node position.
  The first hemisphere of that capsule extends outside of this point. If the
  BodyFixedNode had been placed on the surface of the ground then the hemisphere
  would intersect deeply with the ground. By placing the BodyFixedNode one cable
  radius above the surface the capsule is placed so that the hemisphere just
  about touches the ground.
  */
 
  /*
  Following the fixed starting node we add a bunch of FreeNodes before adding
  the next BodyFixedNode. This causes a bit of extra cable to be created between
  the two BodyFixedNodes so that the bodies can move in relation to each other
  without the cable becoming stretched.
 
  A BodyFixedNode is not only fixed with respect to the body, but also with
  respect to the cable. That is, the cable cannot slide along the body.
  */
  cable->add(new agxCable::FreeNode(agx::Vec3(agx::Real(-3.0),  agx::Real(1.0), agx::Real(1.0))));
  cable->add(new agxCable::FreeNode(agx::Vec3(agx::Real(-3.0),  agx::Real(0.0), agx::Real(1.0))));
  cable->add(new agxCable::FreeNode(agx::Vec3(agx::Real(-3.0), agx::Real(-1.0), agx::Real(1.0))));
  cable->add(new agxCable::FreeNode(agx::Vec3(agx::Real(-1.2),  agx::Real(0.0), agx::Real(0.6))));
  cable->add(new agxCable::FreeNode(agx::Vec3(agx::Real(-1.0),  agx::Real(4.0), agx::Real(0.6))));
 
  /*
  The second BodyFixedNode is attached to the base of the device. The thing to
  note here is that the translation specified in the transformation is relative
  to the body and not the world. To attach the BodyFixedNode on the surface of
  the device part we do not need to know where the device is with respect to the
  world origin, but we do need to know the shape of the part itself. The key
  parameter here is 'baseRadius'.
 
  It is the center of the cable that becomes locked to the body, so we must also
  take the radius of the cable into account.
 
  The translate by a large fraction of the base half length along the Y axis
  places the attachment near the far end of the base.
 
  As the base moves the attachment point moves with it and the cable will
  follow.
  */
  agx::AffineMatrix4x4 middleTransform;
  middleTransform.setTranslate(-baseRadius-radius, agx::Real(0.9)*baseHalfLength, agx::Real(0.0));
  cable->add(new agxCable::BodyFixedNode(base, middleTransform));
 
  /*
  Again, a FreeNode is added to get some slack in the cable between BodyFixedNodes.
  */
  cable->add(new agxCable::FreeNode(agx::Vec3(agx::Real(-baseRadius-radius), agx::Real(2.5), machineHeight + baseRadius)));
 
  /*
  In addition to translation a transformation can describe rotation. The
  property of the cable that is specified with this rotation is the vector that
  the cable should follow at the attachment point. The default is the Z axis, so
  by not specifying any rotational part for the previous BodyFixedNodes we got a
  cable that leave the attachment point straight up.
 
  In this case we want the cable to enter and exit the attachment horizontally
  instead, along the negative Y axis. We do this by specifying a rotation that
  rotates the Z axis; the default exit direction, to the Y axis; the wanted exit
  direction.
 
  The translational part is the same as last time, placing the attachment point
  on the leftmost side of the base.
  */
  agx::AffineMatrix4x4 baseEndTransform;
  baseEndTransform.setTranslate(-baseRadius-radius, -baseHalfLength, agx::Real(0.0));
  baseEndTransform.setRotate(agx::Z_AXIS, agx::Y_AXIS * agx::Real(-1));
  cable->add(new agxCable::BodyFixedNode(base, baseEndTransform));
 
  /*
  Yet another FreeNode so we get a bit of extra cable to work with when the
  simulation starts.
  */
  cable->add(new agxCable::FreeNode(agx::Vec3(-baseRadius, agx::Real(-1.0), machineHeight - agx::Real(2)*baseRadius)));
 
 
  /*
  Here we demonstrate that attachment transformations are not only relative to
  the position of the body but the entire body transformation. We want to attach
  the cable on the arm part of the device to the right of the base part, which
  is along the positive X axis in the world. However, since the point is
  specified in the arm's local coordinate system, which has been rotated with
  respect to the world, we must give a point that offset along the negative Y
  axis. Cylinders in AGX are naturally aligned along the Y axis so moving along
  that axis moves along the cylinder, which is what we want.
 
  We still move by the combined radius to reach the surface of the cylinder. The
  Z axis was not moved by the arm's original transformation, so negative Z moves
  us downwards.
  */
  agx::AffineMatrix4x4 armTransform;
  armTransform.setTranslate(agx::Real(0.0), -armHalfLength+baseRadius, -armRadius-radius);
  armTransform.setRotate(agx::Z_AXIS, agx::Y_AXIS);
  cable->add(new agxCable::BodyFixedNode(arm, armTransform));
 
  /*
  More cable slack. This should be familiar by now.
  */
  cable->add(new agxCable::FreeNode(agx::Vec3(armHalfLength, agx::Real(0.5), machineHeight - agx::Real(4)*armRadius)));
 
 
  /*
  The final BodyFixedNode attaches the end of the cable to the tip of the arm.
  We want the cable to initially point upwards so we do not need to specify a
  rotational part in the transformation. As the arm moves around the world the
  attachment transformation moves with it and the cable follows.
  */
  agx::AffineMatrix4x4 armEndTransform;
  armEndTransform.setTranslate(armRadius+radius, armHalfLength, agx::Real(0.0));
  cable->add(new agxCable::BodyFixedNode(arm, armEndTransform));
 
  /*
  Add the cable to the simulation and we're done.
  */
  simulation->add(cable);
 
  /*
  Well, done with the scene configuration at least. But we also want to see the
  cable. While we could use the debug rendering here as well, it is fairly slow
  and really meant for debugging. We can instead use the rendering facilities
  built into the tutorial framework to create rendering objects for us. In
  Tutorial 4 we learn how to do our own rendering of the cable segments.
  */
  auto cableNode = agxOSG::createVisual(cable, root);
  agxOSG::setDiffuseColor(cableNode, agx::Vec4f(0, 1, 0,   1));
  application->setEnableDebugRenderer(false);
  application->setEnableOSGRenderer(true);
 
  return root;
}
 
 
 
osg::Group* buildTutorial3(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  std::cout << "Tutorial 3 - Cable properties." << std::endl;
  osg::Group* root = new osg::Group();
 
  // To avoid placing multiple cables at the same place. This is updated
  // for every cable created.
  agx::Vec3 worldOffset(agx::Real(0.0));
 
  // The size of the boxes holding each cable.
  agx::Real baseHe(0.5);
 
  /*
  Helper function that creates a horizontal cable attached at one end to a
  static box. Consecutive calls will create the box/cable pair next to the
  previous box/cable pair.
 
  By specifying different values for youngsModulus the different cables will
  bend differently due to their own weight.
  */
  auto createBendTest = [simulation, root, baseHe, &worldOffset](agx::Real youngsModulus)
  {
    // The cable is created the same way as before.
    agxCable::CableRef cable = new agxCable::Cable(agx::Real(0.05), agx::Real(5));
 
    /*
    The cable properties are managed by a class called CableProperties. We ask
    the cable for its CableProperties instance.
    */
    agxCable::CableProperties* properties = cable->getCableProperties();
 
 
    /*
    On the CableProperties instance we can call setYoungsModulus to set the
    stiffness of the cable. A high Young's modulus makes the cable rigid and a
    not quite as high Young's modulus makes it pliable. These numbers are often
    very large.
 
    We can set different Young's modulus for different directions of the cable.
    A list of available directions is given in CableProperties.h. The BEND
    direction controls how difficult it is to bend the cable.
    */
    properties->setYoungsModulus(youngsModulus, agxCable::BEND);
 
    // Create a base body, a BodyFixedNode attached to it and a FreeNode some distance away from the base along the X axis.
    agx::RigidBodyRef base;
    agxCollide::GeometryRef baseGeom;
    std::tie(base, baseGeom) = ::createBody(simulation, root, new agxCollide::Box(baseHe, baseHe, baseHe), "Base", agx::RigidBody::STATIC);
    base->setPosition(worldOffset);
    cable->add(new agxCable::BodyFixedNode(base,
        agx::AffineMatrix4x4::rotate(agx::Z_AXIS, agx::X_AXIS) *
        agx::AffineMatrix4x4::translate(baseHe * agx::X_AXIS)));
    cable->add(new agxCable::FreeNode(worldOffset + agx::Real(10.0)*agx::X_AXIS));
    simulation->add(cable);
 
    // The next cable should be created a bit to the side of this one.
    worldOffset += agx::Real(2.5)*baseHe*agx::Y_AXIS;
 
    // Add basic rendering.
    auto cableNode = agxOSG::createVisual(cable, root);
    agxOSG::setDiffuseColor(cableNode, agx::Vec4f(0, 1, 0,   1));
  };
 
 
 
  /*
  Create a set of horizontal cables with increasing Young's modulus in the BEND
  direction. The cables with the lowest Young's modulus will become bent due to
  their own weight while the cables with higher Young's modulus will be able to
  maintain roughly their original shape.
  */
  // Nearest camera, low Young's modulus, high deformation.
  createBendTest(1e10);
  createBendTest(2e10);
  createBendTest(5e10);
  createBendTest(1e11);
  createBendTest(2e11);
  createBendTest(5e11);
  // Furthest from camera, high Young's modulus, low deformation.
 
 
 
 
  /*
  Helper function that creates a vertical cable attached to a static body at the
  top. By specifying different values for Young's modulus in the STRETCH
  direction the cables will stretch by a different amount even when influenced by
  the same load, in this case the weight of the cable itself.
  */
  auto createStretchTest = [simulation, root, baseHe, &worldOffset](agx::Real youngsModulus, agx::Real resolution) -> agxCable::CableRef
  {
    // Cable created as before.
    agxCable::CableRef cable = new agxCable::Cable(agx::Real(0.05), resolution);
 
    /*
    We use the CableProperties instance to set Young's modulus in the STRETCH
    direction as well.
    */
    cable->getCableProperties()->setYoungsModulus(youngsModulus, agxCable::STRETCH);
 
    // Create a base body, a BodyFixedNode attached to it and a FreeNode some distance away along the negative Z axis.
    agx::RigidBodyRef base;
    agxCollide::GeometryRef baseGeom;
    std::tie(base, baseGeom) = ::createBody(simulation, root, new agxCollide::Box(baseHe, baseHe, baseHe), "Base", agx::RigidBody::STATIC);
    base->setPosition(worldOffset);
    agx::AffineMatrix4x4 rotateDown = agx::AffineMatrix4x4::rotate(agx::Z_AXIS, -agx::Z_AXIS);
    agx::AffineMatrix4x4 translateToBottom = agx::AffineMatrix4x4::translate(-baseHe * agx::Z_AXIS);
    cable->add(new agxCable::BodyFixedNode(base, rotateDown * translateToBottom));
    cable->add(new agxCable::FreeNode(worldOffset - agx::Real(10) * agx::Z_AXIS));
    simulation->add(cable);
 
    worldOffset += agx::Real(2.5) * baseHe * agx::X_AXIS;
 
    // Add basic rendering.
    auto cableNode = agxOSG::createVisual(cable, root);
    agxOSG::setDiffuseColor(cableNode, agx::Vec4f(0, 1, 0,   1));
 
    return cable;
  };
 
 
  /*
  Create a set of vertically hanging cables with increasing Young's modulus in
  the STRETCH direction. The leftmost cable stretches quite a bit and reveal a
  limitation of the implementation. The segment geometries are not resized to
  reflect the increased distance to the neighbors which results in gaps in the
  collision geometry. This can easily lead to other objects tunneling through
  the cable.
  */
  worldOffset = worldOffset + agx::Real(15) * agx::X_AXIS;
  // Leftmost, low Young's modulus, large deformation.
  createStretchTest(1e5, agx::Real(5));
  createStretchTest(2e5, agx::Real(5));
  createStretchTest(5e5, agx::Real(5));
  createStretchTest(1e6, agx::Real(5));
  createStretchTest(2e6, agx::Real(5));
  createStretchTest(5e6, agx::Real(5));
  // Rightmost, high Young's modulus, low deformation.
 
 
 
  /*
  The resolution of the cable can have some effect on the elasticity of the
  cable. Below a set of cables with the same Young's modulus but varying
  resolution are created. As the simulation runs we see that the cable with the
  highest resolution stretches the most. This is at least in part due to the
  overlap between consecutive cable segments. The more segments there are the
  more overlaps there are and the heavier the cable becomes.
  */
  worldOffset = worldOffset + agx::Real(10) * agx::X_AXIS;
  // Nearest camera
  createStretchTest(agx::Real(5e5), agx::Real(0.2));
  createStretchTest(agx::Real(5e5), agx::Real(0.5));
  createStretchTest(agx::Real(5e5), agx::Real(1));
  createStretchTest(agx::Real(5e5), agx::Real(2));
  createStretchTest(agx::Real(5e5), agx::Real(5));
  createStretchTest(agx::Real(5e5), agx::Real(10));
  // Furthest from camera.
 
 
 
 
  /*
  Multiple cables can share the same CableProperties object. This makes it
  possible to easily update the properties for many cables at once.
  */
 
 
  /*
  We first create three similar cables. Two off these will share cable
  properties and the third will use its own.
  */
  worldOffset += agx::Real(5) * agx::X_AXIS;
  agxCable::CableRef cable1 = createStretchTest(agx::Real(5e5), agx::Real(5));
  agxCable::CableRef cable2 = createStretchTest(agx::Real(5e5), agx::Real(5));
  agxCable::CableRef cable3 = createStretchTest(agx::Real(5e5), agx::Real(5));
 
  /*
  Sharing is done by getting the cable properties from one cable and setting it
  to the other.
  */
  agxCable::CableProperties* sharedProperties = cable1->getCableProperties();
  cable2->setCableProperties(sharedProperties);
 
  /*
  By setting parameters in the shared CableProperties instance both cables are
  updated.
  */
  sharedProperties->setYoungsModulus(agx::Real(1e5), agxCable::STRETCH);
 
  /*
  Both cables are updated even if the setting is done through one of the cables.
  The code below will first change both cable1 and cable2 to to have Young's
  modulus 1e6 and then change both of them to have 5e5.
  */
  cable1->getCableProperties()->setYoungsModulus(agx::Real(1e6), agxCable::STRETCH); // This write is lost because
  cable2->getCableProperties()->setYoungsModulus(agx::Real(1e7), agxCable::STRETCH); // this write overwrites it.
 
 
  /*
  The third cable does not participate in the sharing and its properties remain
  unchanged throughout its lifetime.
  */
 
 
  application->setEnableDebugRenderer(false);
  application->setEnableOSGRenderer(true);
 
  return root;
}
 
 
 
 
osg::Group* buildTutorial4(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  std::cout << "Tutorial 4 - Iterating over a cable." << std::endl;
  osg::Group* root = new osg::Group();
 
  // We start with a scene similar to the one built in the first tutorial.
  createBody(simulation, root, new agxCollide::Box(agx::Real(3), agx::Real(2), agx::Real(0.1)), "Ground", agx::RigidBody::STATIC);
  agx::Real radius(0.04);
  agx::Real resolution(5);
  agxCable::CableRef cable = new agxCable::Cable(radius, resolution);
  cable->add(new agxCable::FreeNode(agx::Vec3(agx::Real(0.0), agx::Real(0.0), agx::Real(1.0))));
  cable->add(new agxCable::FreeNode(agx::Vec3(agx::Real(0.9), agx::Real(0.0), agx::Real(1.0))));
  cable->add(new agxCable::FreeNode(agx::Vec3(agx::Real(1.8), agx::Real(0.0), agx::Real(1.0))));
  simulation->add(cable);
 
 
  /*
  The individual segments of the cable can be accessed through an iterator. It
  is important that the cable has been added to the simulation first because
  that triggers the cable finalization that creates the segments that we wish
  to iterate over.
  */
  unsigned counter(0);
  agxCable::CableIterator iterator = cable->begin();
  while (!iterator.isEnd()) {
    /*
    The iterator provides access to various information related to the current
    segment. One such piece of information is the begin- and end positions of the
    segment. As the simulation runs these can be used to track the motion of the
    cable.
    */
    agx::Vec3 beginPosition = iterator->getBeginPosition();
    agx::Vec3 endPosition = iterator->getEndPosition();
 
    ++counter;
    std::cout << "Segment " << counter << " extends from " << beginPosition.x() << " to " << endPosition.x() << "." << std::endl;
 
    /*
    Another thing available from the iterator is the geometry that is used for
    collision detection with this segment of the cable. We can use those to
    create rendering for our cable.
 
    Here we set the color of the entire cable to green.
    */
    agxOSG::GeometryNode* segmentRenderer = agxOSG::createVisual(iterator->getGeometry(), root);
    agxOSG::setDiffuseColor(segmentRenderer, agx::Vec4f(0, 1, 0,  1));
 
    /*
    When we are done with the current segment we can move to the next using the
    advance method. Once we have processed the last segment the iterator's isEnd
    method will begin to return true.
 
    One should never call any method other than isEnd() on an iterator that
    returns true from isEnd().
    */
    ++iterator;
  }
 
 
  /*
  Using the graphics nodes created above we can update the color of each segment
  according to the tension in that segment. We use a step event listener for
  that.
  */
  class TensionRenderer : public agxSDK::StepEventListener
  {
    public:
      TensionRenderer(agxCable::Cable* cable, osg::Group* root)
        : m_cable(cable)
        , m_root(root)
      {
      }
 
      virtual void post(agx::TimeStamp const & /*time*/) override
      {
        // The cable and its iterator conforms to the standard container/iterator
        // idiom, so we can iterate over the cable using a for-each loop.
        for (auto segment : *m_cable)
        {
          /*
          The getTension method on the iterator returns the tension at the beginning
          of the segment. The first segment of the cable is not attached to anything,
          so it will always have zero tension.
          */
          agx::Real tension = segment->getStretchTension();
 
          /*
          We color the segment according to a linear green-to-red gradient over
          tensions from 0 to 100.
          */
          agxOSG::GeometryNode* node = agxOSG::findGeometryNode(segment->getGeometry(), m_root);
          agxOSG::setDiffuseColor(node, agx::Vec4f(float(tension/agx::Real(100)), float(1-(tension/100)), 0, 1));
 
          ++segment;
        }
      }
    private:
      agxCable::Cable* m_cable;
      osg::Group* m_root;
  };
 
  simulation->add(new TensionRenderer(cable, root));
 
 
 
  /*
  The cable create above is a bit short and doesn't contain many segments. That
  makes the gradient difficult to see. Below a longer cable with more segments
  is created.
  */
  agxCable::CableRef longerCable = new agxCable::Cable(radius, resolution);
  longerCable->add(new agxCable::FreeNode(agx::Vec3(agx::Real(-6.0), agx::Real(1.0), agx::Real(0.3))));
  longerCable->add(new agxCable::FreeNode(agx::Vec3(agx::Real( 6.0), agx::Real(1.0), agx::Real(0.3))));
  simulation->add(longerCable);
  for (auto segment : *longerCable)
  {
    agxOSG::createVisual(segment->getGeometry(), root);
  }
  class BendTensionRenderer : public agxSDK::StepEventListener
  {
    public:
      BendTensionRenderer(agxCable::Cable* cable, osg::Group* root)
        : m_cable(cable)
        , m_root(root)
      {
      }
 
      virtual void post(agx::TimeStamp const & /*time*/) override
      {
        for (auto segment : *m_cable)
        {
          /*
          Bend tension is the tension in the bend direction. Plain getTension
          returns the tensile tension.
          */
          agx::Real tension = segment->getBendTension();
          agx::Real tensionRatio = tension / agx::Real(300);
          agxOSG::GeometryNode* node = agxOSG::findGeometryNode(segment->getGeometry(), m_root);
          agxOSG::setDiffuseColor(node, agx::Vec4f((float)tensionRatio, float(1-tensionRatio), 0, 1));
        }
      }
 
    private:
      agxCable::Cable* m_cable;
      osg::Group* m_root;
  };
 
  simulation->add(new BendTensionRenderer(longerCable, root));
 
  application->setEnableDebugRenderer(false);
  application->setEnableOSGRenderer(true);
 
  return root;
}
 
 
void createCableRenderer(agxCable::Cable* cable, const agx::Vec3f& color, osg::Group* root)
{
  for (auto segment : *cable)
  {
    agxOSG::GeometryNode* segmentRenderer = agxOSG::createVisual(segment->getGeometry(), root);
    agxOSG::setDiffuseColor(segmentRenderer, agx::Vec4f(color, 1));
  }
}
 
 
 
osg::Group* buildTutorial5(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  std::cout << "Tutorial 5 - Routing parameters." << std::endl;
  osg::Group* root = new osg::Group();
 
  agx::Real centerLine(1.0);
 
  agx::Real radius(0.04);
  agx::Real resolution(3);
 
 
  /*
  This tutorial creates two cables, and some visual aid, from equivalent sets
  of routing nodes and discusses ways of making them different. The following
  lists the X coordinates for the four routing nodes created for each cable.
  */
  auto x = {0.0, 1.0, 1.1, 1.25, 2.0};
 
 
  // A few marker making showing where the routing nodes are along the X axis.
  for (auto xPos : x) {
    simulation->add(new agxCollide::Geometry(
        new agxCollide::Sphere(agx::Real(0.5) * radius),
        agx::AffineMatrix4x4::translate(xPos, agx::Real(0.0), centerLine - agx::Real(1.3) * radius)));
  }
 
 
  /*
  We start with a basic cable created the regular way, placed a bit up for
  identification purposes. By inspecting the rendered result we notice that the
  actual resolution of the cable is much higher than what we requested. This is
  because the segmentation algorithm applies a number of routing and error
  criteria to the cable during initialization. One such criteria is that the
  resolution must be high enough to resolve all the routing nodes. In other
  words, the segments may not be longer than the shortest distance between two
  consecutive routing nodes. The shortest distance in the routing nodes listed
  below is 0.1 meter, which corresponds to a resolution of 10 segments per
  meter.
 
  The second criteria relates to routing errors. There may not be any reasonable
  sized segment length that perfectly divides all cable legs and any mismatch
  shows up as gaps or extra overlaps between cable segments. The segmentation
  algorithm may chose to increase the resolution further in order to reduce this
  error. In this case the resolution is increased to 20.5 segments per meter,
  resulting in a cable with 41 segments.
  */
  agxCable::CableRef cable1 = new agxCable::Cable(radius, resolution);
  for (auto xPos : x) {
    cable1->add(new agxCable::FreeNode(agx::Vec3(xPos,  agx::Real(0.0), centerLine + agx::Real(2.3)*radius)));
  }
  simulation->add(cable1);
  std::cout << "Cable created with " << cable1->getNumSegments() << " segments and resolution " << cable1->getResolution() << "." << std::endl;
 
 
  /*
  Sometimes we can accept a larger routing error if that is required to reduce
  the number of cable segments to a manageable level. The tool used for that is
  the tryInitialize member function. Its usage is demonstrated below.
  */
 
  /*
  We create the new cable and add the route nodes as before. This cable is
  placed below the first cable.
  */
  agxCable::CableRef cable2 = new agxCable::Cable(radius, resolution);
  for (auto xPos : x) {
    cable2->add(new agxCable::FreeNode(agx::Vec3(xPos,  agx::Real(0.0), centerLine)));
  }
 
  /*
  Instead of adding the cable to the simulation immediately, which would
  finalize the cable with the current configuration, we make a
  test-initialization by calling tryInitialize on the cable. This will gather
  some information on how the cable would be configured if it were to be added
  to the simulation right now.
  */
  agxCable::RouteInitializationReport report = cable2->tryInitialize();
 
  /*
  We get back a report detailing the result of the routing. In the report we can
  check the number of nodes created and the maximum error, i.e., the largest
  overlap or gap for the segment-segment pairs, in the cable. We see that the
  error is fairly small at 2.4 millimeters.
  */
  std::cout << "\nFirst initialization attempt resulted in " << report.getNumSegments() <<
               " nodes and error " << report.getActualError() <<
               ". Resolution is " << cable2->getResolution() << std::endl;
 
  /*
  To reduce the resolution of the cable somewhat we can increase the maximum
  allowed routing error. This is done by calling tryInitialize with the new
  error bound. In this case the length of the shortest cable leg is 0.1 meters
  and the second longest 0.15 meters. 0.1 does not evenly divide 0.15, which is
  the reason for the increase in resolution from 10, corresponding to the
  shortest leg, to 20.5. The remainder 0.05, which became the cable error, was
  considered too large by the segmentation algorithm and the resolution
  increased.
 
  We can say that we accept this error by passing a number slightly
  higher than 0.05 to tryInitialize. This will cause the segmentation algorithm
  to accept the initial resolution dictated by the the length of the shortest
  cable leg.
  */
  report = cable2->tryInitialize(agx::Real(0.051));
 
 
  /*
  Inspection of the new report reveal that only 20 segments were created and
  that the error is larger than before. In the rendered scene we can see the
  error as a gap causing a notch in the cable. We can visually compare the
  resolution of the two cables in the rendering and also see how the extra
  segment created due to the increased resolution fit right in the gap.
 
  In this case there were only a single segment in the problematic leg. If the
  routing nodes are placed so that more segments fit in each leg then the error
  is distributed over all segments and thus less apparent.
  */
  std::cout << "\nSecond initialization attempt resulted in " << report.getNumSegments() <<
               " nodes and error " << report.getActualError() <<
               ". Resolution is " << cable2->getResolution() << std::endl;
 
  /*
  The configuration generated by the last call to tryInitialize is stored and
  adding the cable to the simulation will finalize the initialization with
  the number of nodes and error reported by tryInitialize.
  */
  simulation->add(cable2);
 
 
  /*
  An important take-away from this tutorial is that the length of the shortest
  cable leg can be important for the final cable resolution, in particular if the
  length of the leg is short compared to the resolution of th cable.
  */
 
  application->setEnableDebugRenderer(true);
  application->setEnableOSGRenderer(false);
  if (!g_isUnitTest) {
    application->setAutoStepping(false);
  }
 
  return root;
}
 
 
 
osg::Group* buildTutorial6(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  std::cout << "Tutorial 6 - Detecting collisions." << std::endl;
  osg::Group* root = new osg::Group();
 
  agx::Real radius(0.08);
  agx::Real resolution(5);
 
  agx::Vec3f red(1, 0, 0);
  agx::Vec3f green(0,1,0);
 
  // Create a body on the left.
  agx::RigidBodyRef leftBody;
  agxCollide::GeometryRef leftBodyGeom;
  std::tie(leftBody, leftBodyGeom) = ::createBody(simulation, root, new agxCollide::Box(agx::Real(1), agx::Real(1), agx::Real(1)), "LeftBody", agx::RigidBody::STATIC);
  leftBody->setPosition(agx::Real(-5), agx::Real(0), agx::Real(0));
 
  // Create a cable that will collide with the leftmost body.
  agxCable::CableRef leftCable = new agxCable::Cable(radius, resolution);
  leftCable->setName("LeftCable");
  leftCable->add(new agxCable::FreeNode(agx::Vec3(agx::Real(-5), agx::Real(0), agx::Real(5))));
  leftCable->add(new agxCable::FreeNode(agx::Vec3(agx::Real(-1), agx::Real(0), agx::Real(5))));
  simulation->add(leftCable);
  createCableRenderer(leftCable, red, root);
 
 
  // Create a body to the right.
  agx::RigidBodyRef rightBody;
  agxCollide::GeometryRef rightBodyGeometry;
  std::tie(rightBody, rightBodyGeometry) = ::createBody(simulation, root, new agxCollide::Box(agx::Real(1), agx::Real(1), agx::Real(1)), "RightBody", agx::RigidBody::STATIC);
  rightBody->setPosition(agx::Real(5), agx::Real(0), agx::Real(0));
 
  // Create a cable that will collide with the rightmost body.
  agxCable::CableRef rightCable = new agxCable::Cable(radius, resolution);
  rightCable->setName("RightCable");
  rightCable->add(new agxCable::FreeNode(agx::Vec3(agx::Real(5), agx::Real(0), agx::Real(2))));
  rightCable->add(new agxCable::FreeNode(agx::Vec3(agx::Real(1), agx::Real(0), agx::Real(2))));
  simulation->add(rightCable);
  createCableRenderer(rightCable, green, root);
 
 
  // Contacts are reported by the simulation using contact event listeners.
  class CableContactFinder : public agxSDK::ContactEventListener
  {
    public:
      virtual KeepContactPolicy impact(agx::TimeStamp const & /*time*/, agxCollide::GeometryContact* contact) override
      {
        /*
        From the GeometryContact 'contact' we can read various information about
        the two colliding entities.
        */
        agxCollide::Geometry* geometry1 = contact->getGeometryPair().first;
        agxCollide::Geometry* geometry2 = contact->getGeometryPair().second;
        agx::RigidBody* body1 = geometry1->getRigidBody();
        agx::RigidBody* body2 = geometry2->getRigidBody();
        agx::Name bodyName1 = body1->getName();
        agx::Name bodyName2 = body2->getName();
 
        /*
        Using a static helper method in agxCable::Cable we can find the cable, if
        any, that a particular geometry is part of.
        */
        agxCable::Cable* cable1 = agxCable::Cable::getCableForGeometry(geometry1);
        agxCable::Cable* cable2 = agxCable::Cable::getCableForGeometry(geometry2);
 
        /*
        Depending on the null-ness of the return value from getCableForGeometry
        we can determine if the geometry was part of a cable or not, and if
        there is a cable we can access it directly.
        */
        if (cable1 == nullptr && cable2 == nullptr)
          std::cout << "Collision between body " << bodyName1 << " and body " << bodyName2 << '.' << std::endl;
        else if (cable1 != nullptr && cable2 == nullptr)
          std::cout << "Collision between cable " << cable1->getName() << " and body " << bodyName2 << '.' << std::endl;
        else if (cable1 == nullptr && cable2 != nullptr)
          std::cout << "Collision between cable " << cable2->getName() << " and body " << bodyName1 << '.' << std::endl;
        else if (cable1 != nullptr && cable2 != nullptr)
          std::cout << "Collision between cable " << cable1->getName() << " and cable " << cable2->getName() << '.' << std::endl;
 
        return KEEP_CONTACT;
      }
  };
 
  simulation->add(new CableContactFinder());
 
 
  /*
  When running the tutorial, notice that the contact listener may be called
  multiple times per cable-geometry contact. There will be one call for every
  cable segment that touches another geometry, and a cable has multiple
  segments. Since the segments are geometries themselves, we will also get a
  call for every cable segment that impacts another cable segment, regardless of
  whether the segments belong to the same or different cables. A special case is
  two adjacent segments on the same cable. Such segments never cause collisions.
 
  There are other ways to control the collision event handling, such as
  collision groups and execute filters. See separate tutorials such as
  tutorial7_Kinematic_bodies_Contact_listener.agxPy and
  tutorial_listeners.cpp.
  */
 
 
  /*
  A floor is used to prevent the cables from falling indefinitely. Use mouse
  picking (Ctrl+LeftMouseButton) to bring the two cables together.
  */
  agx::RigidBodyRef floor;
  agxCollide::GeometryRef floorGeom;
  std::tie(floor, floorGeom) = ::createBody(simulation, root, new agxCollide::Box(agx::Real(20), agx::Real(20), agx::Real(1)), "Floor", agx::RigidBody::STATIC);
  floor->setPosition(agx::Real(-10) * agx::Z_AXIS);
 
  application->setEnableDebugRenderer(false);
  application->setEnableOSGRenderer(true);
 
  return root;
}
 
 
 
 
osg::Group* buildTutorial7(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  std::cout << "Tutorial 7 - Attaching objects along a cable after it has been initialized." << std::endl;
  osg::Group* root = new osg::Group();
 
 
  // Create a cable.
  agx::Real radius(0.05);
  agx::Real resolution(2);
  agx::Real length(5);
  agxCable::CableRef cable = new agxCable::Cable(radius, resolution);
  cable->add(new agxCable::FreeNode(agx::Vec3(agx::Real(0.0), agx::Real(0.0), agx::Real(0.0))));
  cable->add(new agxCable::FreeNode(agx::Vec3(length, agx::Real(0.0), agx::Real(0.0))));
  simulation->add(cable);
  createCableRenderer(cable, agx::Vec3f(1, 0, 0), root);
 
  /*
  Create the object that is to be attached to the cable. Attaching does not
  place the object on the cable, but instead maintains the relative positions of
  the cable segment and the object as the simulation progresses. We therefore
  place the object close to the cable right from the start.
  */
  agx::RigidBodyRef object = new agx::RigidBody("Attachment");
  agx::Vec3 objectHe(agx::Real(3)*radius, agx::Real(0.7)*radius, agx::Real(0.7)*radius);
  object->add(new agxCollide::Geometry(new agxCollide::Box(objectHe)));
  object->setPosition(agx::Real(0.05)*length, agx::Real(0.0), agx::Real(2)*radius);;
  simulation->add(object);
  osg::Group* osgNode = agxOSG::createVisual(object, root);
  agxOSG::setDiffuseColor(osgNode, agx::Vec4f(0,1,0,1));
 
  /*
  In order to attach the object to the cable we need to know which cable segment
  we wish to attach to. The closest one is often a good idea, but it is not
  required. In this case we placed the object very close to the start of the
  cable so we want to attach to the first segment.
  */
  agxCable::CableIterator segment = cable->begin();
 
  /*
  The last piece of information we need to proved is the attachment point on the
  object. This is the point that will stay fixed relative to the cable. The
  object will be able to rotate round this point.
 
  The point is given in the object's local coordinate frame here we select a
  point that is at one of the corners of the object.
  */
  agx::Vec3 objectAttachPoint(objectHe.x(), -objectHe.y(), -objectHe.z());
 
  /*
  The actual attaching is done by calling cable:attach, passing in everything
  we just created as parameters. From this point on the object will follow the
  cable.
  */
  cable->attach(segment, object, objectAttachPoint);
 
 
  /*
  The attachment created above is called a point attachment. So named because a
  single point on the object is attached to the cable. Sometimes we need to
  completely attach the object to the cable, not allowing any relative motion at
  all. This is called a rigid attachment.
 
  A rigid attachment needs to know, in addition to the attachment point, the
  orientation that the object should have relative to the cable. Both the point
  and the orientation are expressed as a transformation matrix.
 
  The matrix describe how to transform the object's coordinate frame to the
  attachment point, along with a rotation that aligns Z axis with the cable.
 
  An example demonstrates.
  */
 
 
 
  // Create a new object to be attached to the cable.
  agx::RigidBodyRef rigidObject = new agx::RigidBody("RigidAttachment");
  agx::Vec3 rigidObjectHe(agx::Real(3)*radius, agx::Real(0.7)*radius, agx::Real(0.7)*radius);
  rigidObject->add(new agxCollide::Geometry(new agxCollide::Box(rigidObjectHe)));
  rigidObject->setPosition(agx::Real(0.5)*length, agx::Real(0.0), agx::Real(2)*radius);
  simulation->add(rigidObject);
  osg::Group* rigidOsgNode = agxOSG::createVisual(rigidObject, root);
  agxOSG::setDiffuseColor(rigidOsgNode, agx::Vec4f(0, 0, 1, 1));
 
 
  /*
  We want to attach the new object further along the cable and need to find an
  appropriate segment to attach the cable to. This is easy in this case since
  the cable has been laid out along a straight line.
  */
  agxCable::CableIterator rigidSegment = cable->begin();
  while (rigidSegment->getEndPosition().x() < rigidObject->getPosition().x())
    ++rigidSegment;
 
  /*
  We need a transformation matrix that describes how the object should be
  attached to the cable. It consists of two parts: a translational part and a
  rotational part.
 
  The translational part identifies the pivot point in the object. This is the
  point that the object will rotate about in order to restore the directional
  requirement, if that were to ever become violated.
 
  The rotational part specifies the direction that the cable should have past
  the object. By default this is along the Z axis. In this case we want the
  cable to lie along the X axis instead. We therefore supply a transformation
  that rotates the Z axis (the default) to the X axis (what we want).
  */
  agx::AffineMatrix4x4 translate = agx::AffineMatrix4x4::translate(objectHe.x(), agx::Real(0), agx::Real(0));
  agx::AffineMatrix4x4 rotate = agx::AffineMatrix4x4::rotate(agx::Z_AXIS, agx::X_AXIS);
  agx::AffineMatrix4x4 transform = rotate * translate;
 
  /*
  Lastly, the object is attached to the cable using cable:attach in much the
  same way as before. The only difference being that we pass an AffineMatrix4x4
  as the last argument instead of a Vec3.
  */
  cable->attach(rigidSegment, rigidObject, transform);
 
  agx::RigidBodyRef floor;
  agxCollide::GeometryRef floorGeom;
  std::tie(floor, floorGeom) = ::createBody(simulation, root, new agxCollide::Box(agx::Real(5), agx::Real(5), agx::Real(0.3)), "Floor", agx::RigidBody::STATIC);
  floor->setPosition(agx::Real(2.5), agx::Real(0), agx::Real(-0.5));
 
  application->setEnableDebugRenderer(false);
  application->setEnableOSGRenderer(true);
 
  return root;
}
 
 
osg::Group* buildTutorial8(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  std::cout << "Tutorial 8 - Using rebind to create a spiral." << std::endl;
  osg::Group* root = new osg::Group();
 
  // Create a cable.
  agx::Real radius(0.05);
  agx::Real resolution(10);
  agxCable::CableRef cable = new agxCable::Cable(radius, resolution);
 
  agxCable::CableProperties* properties = cable->getCableProperties();
  properties->setYoungsModulus(4E09, agxCable::ALL_DIRECTIONS);
 
  agx::Real spiralHeight = 1.0;
  agx::Real angleResolution = 40;
  agx::Real heightResolution = 10;
  agx::Real spiralRadius = 0.5;
 
  // Position the cable somewhere a little bit up from the ground
  cable->setPosition(0, 0, spiralHeight / 2);
 
  agx::Real zStart = -0.5 * spiralHeight;
  agx::Real zEnd = 0.5 * spiralHeight;
  agx::Real z = zStart;
 
  agx::Real angleStep = 360.0 / angleResolution;
  agx::Real heightStep = 0.2*radius + spiralHeight / (angleResolution * heightResolution);
 
  // Create FreeNode in a spiral shape
  while (z < zEnd)
  {
    for (auto a = 0.0; a < 360 - angleStep; a += angleStep)
    {
      auto x = spiralRadius * std::cos(agx::degreesToRadians(a));
      auto y = spiralRadius * std::sin(agx::degreesToRadians(a));
      z = z + heightStep;
 
      cable->add(new agxCable::FreeNode(x, y, z));
    }
  }
 
  simulation->add(cable);
 
  // Cable is routed, now we tell the cable that the current state, is the resting state.
  cable->rebind();
 
  createCableRenderer(cable, agx::Vec3f(1, 0, 0), root);
 
  agx::RigidBodyRef floor;
  agxCollide::GeometryRef floorGeom;
  std::tie(floor, floorGeom) = ::createBody(simulation, root, new agxCollide::Box(agx::Real(5), agx::Real(5), agx::Real(0.3)), "Floor", agx::RigidBody::STATIC);
  floor->setPosition(agx::Real(2.5), agx::Real(0), agx::Real(-0.5));
 
  application->setEnableDebugRenderer(false);
  application->setEnableOSGRenderer(true);
 
  return root;
}
 
 
 
osg::Group* buildTutorial9(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  std::cout << "Tutorial 9 - Control the precise initial location and orientation of segments." << std::endl;
  osg::Group* root = new osg::Group();
 
  const agx::Real radius = agx::Real(0.04);
  const agx::Real resolution = agx::Real(3.0);
 
 
  /*
  To place each cable segment we need to provide two pieces of information:
  - The position where the segment should start.
  - The orientation that the cable should have.
 
  This information may come from a spline, a visual cable routing tool or an
  external segmentation algorithm.
 
  The placement can also be supplied in the form of an AffineMatrix4x4, which is
  demonstrated later.
  */
  struct Placement {
    agx::Vec3 pos;
    agx::Quat rot;
  };
 
 
  {
    /*
    Here we list the placements for each cable segment. The position marks the
    starting position of the segment and the rotation describe how the segment
    should be rotated in relation to it's neutral orientation, with is along the
    Z axis.
    */
    agx::Vector<Placement> placements = {
      {agx::Vec3(agx::Real(0.0), agx::Real(0.0), agx::Real(0.0)), agx::Quat(agx::Z_AXIS, agx::X_AXIS)},
      {agx::Vec3(agx::Real(0.4), agx::Real(0.0), agx::Real(0.0)), agx::Quat()},
      {agx::Vec3(agx::Real(0.4), agx::Real(0.0), agx::Real(0.4)), agx::Quat(agx::Z_AXIS, -agx::X_AXIS)}
 
      /*
      Opposed to the automatic segmentation algorithm, there is no special
      handling of the last routing node. When using automatic segmentation the
      cable always ends where the last routing node is placed. When using direct
      segment placements the last segment begin at the last routing node and
      extends one segment length along the direction specified by the rotation
      given to that routing node.
      */
    };
 
    /*
    We enable direct cable segment placing by passing an IdentityRoute to the
    cable constructor. The passed resolution is used to determine the length of
    each cable segment and for error reporting, it has no other influence on the
    initialization of the cable.
    */
    agxCable::CableRef cable = new agxCable::Cable(radius, new agxCable::IdentityRoute(resolution));
 
    /*
    When the cable has been created we can start adding routing nodes.
    */
    for (auto& placement : placements) {
      /*
      The position is configured as usual, by passing it to the node constructor.
      */
      agxCable::FreeNodeRef node = new agxCable::FreeNode(placement.pos);
 
      /*
      The rotation is configured by setting the rotation of the RigidBody that
      the node contains.
      */
      node->getRigidBody()->setRotation(placement.rot);
      cable->add(node);
    }
 
    /*
    The placement can also be set using setTransform, as exemplified here, or
    any other means that AGX provides to set the position and orientation of a
    RigidBody.
    */
    agxCable::FreeNodeRef node = new agxCable::FreeNode(agx::Vec3());
    node->getRigidBody()->setTransform(agx::AffineMatrix4x4::rotate(-M_PI_4, agx::Y_AXIS) *
                                       agx::AffineMatrix4x4::translate(agx::Real(0.1), agx::Real(0.0), agx::Real(0.4)));
    cable->add(node);
 
    /*
    As usual, the cable is finalized when added to the simulation. Nodes added
    after this point will not be included in the cable.
    */
    simulation->add(cable);
    agxOSG::createVisual(cable, root);
 
    /*
    We can use the initialization report to review some information about the
    cable. The number of segments and the cable resultion are trivially
    determined from the input to the cable creation. The error is a measure of
    how mismatched the end of one segment is in relation to the beginning of the
    next one.
    */
    agxCable::RouteInitializationReport report = cable->getInitializationReport();
 
    std::cout << "\nInitialization resulted in " << report.getNumSegments() <<
      " nodes and error " << report.getActualError() <<
      ". Resolution is " << cable->getResolution() << std::endl;
  }
 
 
  {
    /*
    The full transformation of the cable segments are important because the
    simulation includes cable twist. Here we create a cable with two segments on
    a straight line but with the second segment twisted in relation to the first
    one.
    */
    agx::Vector<Placement> placements = {
      {agx::Vec3(agx::Real(1.5), agx::Real(0.0), agx::Real(0.0)), agx::Quat()},
      {agx::Vec3(agx::Real(1.5), agx::Real(0.0), agx::Real(0.333)), agx::Quat(agx::X_AXIS, agx::Y_AXIS)}
    };
 
    agxCable::CableRef cable = new agxCable::Cable(radius, new agxCable::IdentityRoute(resolution));
    for (auto& placement : placements) {
      agxCable::FreeNodeRef node = new agxCable::FreeNode(placement.pos);
      node->getRigidBody()->setRotation(placement.rot);
      cable->add(node);
    }
 
    simulation->add(cable);
    agxOSG::createVisual(cable, root);
 
    /*
    Some visual aid showing the segments' global transformations is added to
    make the relative rotation apparent. By starting the simulation paused, pass
    '--startPaused' on the command line, and stepping one frame at the time by
    typing 'r' we see that the simulation removes the relative rotation very
    quickly with high rotational velocities as a result.
    */
    for (auto segment : cable->getSegments()) {
      agxOSG::createAxes(segment->getRigidBody(), nullptr, root, 0.4f);
    }
  }
 
 
  {
    /*
    BodyFixedNodes can also be added with direct segment placing, as
    demonstrated here. First we create a body to which the cable should be
    attached.
    */
    agx::RigidBodyRef attachedBody;
    agxCollide::GeometryRef attachedGeometry;
    std::tie(attachedBody, attachedGeometry) = createBody(simulation, root, new agxCollide::Sphere(agx::Real(0.2)), "Attached body");
 
    attachedBody->setPosition(agx::Real(3.9), agx::Real(0.0), agx::Real(1.05));
 
    /*
    These routing placements will route the cable right past the body.
    */
    agx::Vector<Placement> placements = {
      {agx::Vec3(agx::Real(3.0), agx::Real(0.0), agx::Real(0.0)), agx::Quat()},
      {agx::Vec3(agx::Real(3.0), agx::Real(0.0), agx::Real(0.4)), agx::Quat()},
      {agx::Vec3(agx::Real(3.0), agx::Real(0.0), agx::Real(0.8)), agx::Quat(agx::Z_AXIS, agx::X_AXIS)},
      {agx::Vec3(agx::Real(3.33), agx::Real(0.0), agx::Real(0.8)), agx::Quat(agx::Z_AXIS, agx::X_AXIS)},
 
      /*
      This is the segment that we wish to attach to the body.
      */
      {agx::Vec3(agx::Real(3.66), agx::Real(0.0), agx::Real(0.8)), agx::Quat(agx::Z_AXIS, agx::X_AXIS)},
 
      {agx::Vec3(agx::Real(4.0), agx::Real(0.0), agx::Real(0.8)), agx::Quat(agx::Z_AXIS, agx::X_AXIS)}
    };
 
    agxCable::CableRef cable = new agxCable::Cable(radius, new agxCable::IdentityRoute(resolution));
    for (auto& placement : placements) {
      agxCable::RoutingNodeRef node;
      if (agx::equivalent(placement.pos.x(), agx::Real(3.66))) {
        /*
        The relative translate we pass to the BodyFixedNode constructor define
        where, in the body's coordinate system, the cable should be
        attached. This does not mean that the cable will pass through this
        point, it merely defines the attachment points for the constraint that
        will be created between the body and the cable segment. The constraint
        will be created so that the initial relative transformation between the
        body and the cable segment is the constraint's rest state.
        */
        node = new agxCable::BodyFixedNode(attachedBody, agx::Real(-0.24) * agx::Z_AXIS);
 
        /*
        The BodyFixedNode constructor doesn't take a position argument so we
        instead set it on the RigidBody held by the routing node.
        */
        node->getRigidBody()->setPosition(placement.pos);
      }
      else {
        /*
        All other nodes are created as regular FreeNodes, just like before.
        */
        node = new agxCable::FreeNode(placement.pos);
      }
      node->getRigidBody()->setRotation(placement.rot);
      cable->add(node);
    }
 
    simulation->add(cable);
    agxOSG::createVisual(cable, root);
  }
 
 
  application->setEnableDebugRenderer(false);
  application->setEnableOSGRenderer(true);
 
  return root;
}
 
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial Cable\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
    application->addScene(buildTutorial1, '1');
    application->addScene(buildTutorial2, '2');
    application->addScene(buildTutorial3, '3');
    application->addScene(buildTutorial4, '4');
    application->addScene(buildTutorial5, '5');
    application->addScene(buildTutorial6, '6');
    application->addScene(buildTutorial7, '7');
    application->addScene(buildTutorial8, '8');
    application->addScene(buildTutorial9, '9');
 
    if ( application->init( argc, argv ) )
    {
      agxIO::ArgumentParser* args = application->getArguments();
      if (args->read("--isFileSanityUnitTest") || args->read("--agxOnly"))
      {
        g_isUnitTest = true;
      }
      return application->run();
    }
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_cableWindAndWater.cpp

/*
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.
*/
 
#include <agxOSG/ExampleApplication.h>
 
#include <agxModel/WindAndWaterController.h>
 
#include <agxCable/Cable.h>
#include <agxCable/Node.h>
 
#include <agxRender/Color.h>
 
#include "tutorialUtils.h"
 
/*
 
This tutorial illustrates how to achieve wind/water effects together with cables.
 
*/
 
namespace
{
 
  agxOSG::GeometryNode* createWaterVisual( agxCollide::Geometry* waterGeometry, osg::Group* root )
  {
    agxOSG::GeometryNode* node = agxOSG::createVisual( waterGeometry, root );
 
    const agx::Vec4f color = agxRender::Color::DeepSkyBlue();
 
    agxOSG::setDiffuseColor( node, color );
    agxOSG::setAmbientColor( node, agx::Vec4f( agx::Real( 0.2f ) ) );
    agxOSG::setSpecularColor( node, agx::Vec4f( agx::Real( 0.4f ) ) );
    agxOSG::setShininess( node, 120 );
    agxOSG::setAlpha( node, float( 0.3 ) );
 
    return node;
  }
}
 
osg::Group* buildTutorial1( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group( );
 
  // Create material to define the density of water.
  agx::MaterialRef waterMaterial = new agx::Material( "waterMaterial" );
  waterMaterial->getBulkMaterial( )->setDensity( agx::Real( 1000 ) );
 
  // Let the water geometry to be an box of size 20x20x10 meters.
  agxCollide::GeometryRef waterGeometry = new agxCollide::Geometry( new agxCollide::Box( 10, 2, 5 ) );
  // Surface of water at z = 0.
  waterGeometry->setPosition( 0, 0, -5 );
  waterGeometry->setMaterial( waterMaterial );
  ::createWaterVisual( waterGeometry, root );
 
  // Mark as water geometry.
  agxModel::WindAndWaterControllerRef controller = new agxModel::WindAndWaterController( );
  controller->addWater( waterGeometry );
  simulation->add( controller );
  simulation->add( waterGeometry );
 
  /* Three cables with different hydrodynamic coefficients are added to the simulation.
     The coefficients can be set both by cable and by segment (shape). The priority is:
          1. If a segment has individual coefficients these will be used.
          2. If a segment has no individual coefficients the coefficient for the cable it belong to will be used.
          3. If a segment has no individual coefficients and the cable it belongs to has no coefficients the default values will be used.*/
 
 
  // Cable 1 will experience hydrodynamics. Since no parameters are set the hydrodynamic coefficients will
  // have default values.
  agxCable::CableRef cable1 = new agxCable::Cable( 0.02, 2 );
  cable1->add( new agxCable::FreeNode( agx::Vec3( -8, 0, -5 ) ) );
  cable1->add( new agxCable::FreeNode( agx::Vec3( 8, 0, -5 ) ) );
  simulation->add( cable1 );
  agxOSG::setDiffuseColor( agxOSG::createVisual( cable1, root ), agxRender::Color::Red() );
 
  // Cable 2 will have zero drag. This means that is will only experience buoyancy.
  agxCable::CableRef cable2 = new agxCable::Cable( 0.02, 2 );
  cable2->add( new agxCable::FreeNode( agx::Vec3( -8, 0.5, -5 ) ) );
  cable2->add( new agxCable::FreeNode( agx::Vec3( 8, 0.5, -5 ) ) );
  simulation->add( cable2 );
  agxOSG::setDiffuseColor( agxOSG::createVisual( cable2, root ), agxRender::Color::Green() );
 
  // We change the drag parameters to zero for the entire cable.
  agxModel::HydrodynamicsParametersRef hydrodynamicParameters = controller->getOrCreateHydrodynamicsParameters( cable2 );
  hydrodynamicParameters->setCoefficient( agxModel::WindAndWaterParameters::PRESSURE_DRAG, 0 );
  hydrodynamicParameters->setCoefficient( agxModel::WindAndWaterParameters::VISCOUS_DRAG, 0 );
 
  //Cable 3 will have zero drag for some of the segments by setting the coefficients by segment.
  agxCable::CableRef cable3 = new agxCable::Cable( 0.02, 2 );
  cable3->add( new agxCable::FreeNode( agx::Vec3( -8, -0.5, -5 ) ) );
  cable3->add( new agxCable::FreeNode( agx::Vec3( 8, -0.5, -5 ) ) );
  simulation->add( cable3 );
  agxOSG::setDiffuseColor( agxOSG::createVisual( cable3, root ), agxRender::Color::DarkBlue() );
 
  // By iterating over the segments of the cable the drag coefficients can be set per segment.
  size_t numSegments = cable3->getNumSegments();
  int counter( 0 );
 
  agxCable::CableIterator iterator = cable3->begin();
  while ( !iterator.isEnd() && counter < static_cast<agx::Real>(numSegments) * 0.5 ) // One half of the segments will hade drag coefficients set to zero.
  {
    agxCollide::ShapeRef shape = iterator.get()->getRigidBody()->getGeometries()[0]->getShape();
    agxModel::HydrodynamicsParametersRef segmentParameters = controller->getOrCreateHydrodynamicsParameters( shape );
    segmentParameters->setCoefficient( agxModel::WindAndWaterParameters::PRESSURE_DRAG, 0 );
    segmentParameters->setCoefficient( agxModel::WindAndWaterParameters::VISCOUS_DRAG, 0 );
 
    ++counter;
    iterator++;
  }
 
  return root;
}
 
osg::Group* buildTutorial2( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group( );
 
  // Create a WindAndWaterController.
  agxModel::WindAndWaterControllerRef controller = new agxModel::WindAndWaterController( );
  simulation->add( controller );
 
  // Create a static box that will hold the end of each cable.
  agx::RigidBodyRef box = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 0.5, 0.5, 0.5 ) ) );
  box->setPosition( 0, 0, 5 );
  box->setMotionControl( agx::RigidBody::STATIC );
  agxOSG::setDiffuseColor( agxOSG::createVisual( box, root ), agxRender::Color::DarkBlue() );
  simulation->add( box );
 
  // Cable 1 is connected to the box. Aerodynamics are disabled (default) which means that the cable will fall
  // without resistance.
  agxCable::CableRef cable1 = new agxCable::Cable( 0.02, 2 );
  cable1->add( new agxCable::BodyFixedNode( box, agx::Vec3( 0, -0.3, -0.5 ) ) );
  cable1->add( new agxCable::FreeNode( agx::Vec3( 20, -0.3, 0 ) ) );
  simulation->add( cable1 );
  agxOSG::setDiffuseColor( agxOSG::createVisual( cable1, root ), agxRender::Color::Green() );
 
  // Cable 2 is connected to the box. Aerodynamics are enabled and pressure drag is set to 1.
  agxCable::CableRef cable2 = new agxCable::Cable( 0.02, 2 );
  cable2->add( new agxCable::BodyFixedNode( box, agx::Vec3( 0, 0, -0.5 ) ) );
  cable2->add( new agxCable::FreeNode( agx::Vec3( 20, 0, 0 ) ) );
  simulation->add( cable2 );
  agxOSG::setDiffuseColor( agxOSG::createVisual( cable2, root ), agxRender::Color::Red() );
  controller->setEnableAerodynamics( cable2, true );
  controller->getOrCreateAerodynamicsParameters( cable2 )->setCoefficient( agxModel::WindAndWaterParameters::Coefficient::PRESSURE_DRAG, agx::Real( 1 ) );
 
  // Cable 3 is connected to the box. Aerodynamics are enabled for some of the segments and pressure drag is set to 1 for the entire cable.
  agxCable::CableRef cable3 = new agxCable::Cable( 0.02, 2 );
  cable3->add( new agxCable::BodyFixedNode( box, agx::Vec3( 0, 0.3, -0.5 ) ) );
  cable3->add( new agxCable::FreeNode( agx::Vec3( 20, 0.3, 0 ) ) );
  simulation->add( cable3 );
  agxOSG::setDiffuseColor( agxOSG::createVisual( cable3, root ), agxRender::Color::Orange() );
 
  size_t numSegments = cable3->getNumSegments();
  int counter( 0 );
  agxCable::CableIterator iterator = cable3->begin();
  while ( !iterator.isEnd() && counter < static_cast<agx::Real>(numSegments) * 0.5 ) // One half of the segments will have aerodynamics enabled.
  {
    agx::RigidBodyRef rb = iterator.get()->getRigidBody();
    controller->setEnableAerodynamics( rb, true );
 
    ++counter;
    iterator++;
  }
  controller->getOrCreateAerodynamicsParameters( cable3 )->setCoefficient( agxModel::WindAndWaterParameters::Coefficient::PRESSURE_DRAG, agx::Real( 1 ) );
 
  return root;
}
 
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout << "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
               "\tTutorial cable - wind and water\n" <<
               "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_cable_damage.cpp

/*
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.
*/
 
 
/*
 
Demonstrates how to use the cable damage class in the cable module to estimate
damage inflicted on a cable during simulation.
 
*/
 
 
#include <agx/agx.h>
#include <agx/version.h>
 
#include <agxCollide/Geometry.h>
#include <agxCollide/Box.h>
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/CableDamageRenderer.h>
#include <agxOSG/Node.h>
#include <agxOSG/utils.h>
 
#include <agxIO/ArgumentParser.h>
 
#include <agxCable/Cable.h>
#include <agxCable/Node.h>
#include <agxCable/CableDamage.h>
#include <agxUtil/StepEventCallback.h>
 
 
 
osg::Group* buildTutorial1(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  std::cout << "Tutorial 1 - Basic usage.\n";
 
  osg::Group* root = new osg::Group();
 
  // Create an obstacle that the cable will fall onto.
  agxCollide::GeometryRef obstacle = new agxCollide::Geometry(
      new agxCollide::Box(agx::Real(0.3), agx::Real(5.0), agx::Real(0.3)));
  obstacle->setPosition(agx::Real(1.5), agx::Real(0.0), agx::Real(0.8));
  simulation->add(obstacle);
  agxOSG::createVisual(obstacle, root);
 
  // Create the cable a bit above the obstacle. For more information on cable
  // creation see tutorial_cable.cpp and the cable chapter in the user manual.
  agx::Real radius(0.02);
  agx::Real resolution(5);
  agxCable::CableRef cable = new agxCable::Cable(radius, resolution);
  cable->add(new agxCable::FreeNode(agx::Real(-5.0), agx::Real(0.0), agx::Real(2.0)));
  cable->add(new agxCable::BodyFixedNode(nullptr, agx::Vec3(agx::Real(5.0), agx::Real(0.0), agx::Real(2.0))));
  simulation->add(cable);
 
  // Create the cable damage object and add it to our cable instance. After the
  // cable damage object has been created it will compute a new damage
  // estimation every time step.
  agxCable::CableDamageRef damage = new agxCable::CableDamage();
  cable->addComponent(damage);
 
  // There are three main sources of damage to a cable: tension, deformation,
  // and contact forces, where deformation are comprised of current deformation
  // and deformation rate. Moreover, the tension and deformation damage
  // estimation is computed separately for stretch, bend, and twist, giving a
  // total of eleven values that are weighted together. By specifying these
  // eleven weights we can model how sensitive this particular cable is to
  // various forms of deformation.
 
 
  // The deformation weight estimates damage caused by the current deformation
  // of the cable. For a stationary cable at rest this value will remain
  // constant over time.
 
  // The stretch of a cable is measured in elongation per unit length, and the
  // rate is a measure of how fast that elongation is increasing. We want the
  // cable we're modeling here to be very sensitive to stretching, so we set
  // that weight relatively high.
  damage->setStretchDeformationWeight(agx::Real(100.0));
 
  // The bend of a cable measures its curvature; the inverse of the radius of
  // the circle that follows a particular section of the cable. Cables are often
  // less sensitive to bending compared to stretching, so we set a lower value
  // here.
  damage->setBendDeformationWeight(agx::Real(1.0));
 
  // Twist measures the number of radians the cable is twisted per unit length.
  // The scene we've created for this tutorial doesn't produce much twisting of
  // the cable, so the damage estimation reported for twisting will be small.
  damage->setTwistDeformationWeight(agx::Real(30.0));
 
  // The rate damage estimates damage caused as the cable is being deformed. The
  // faster the deformation happens, the larger the reported damage becomes, and
  // the larger the deformation the more damage is reported per unit rate.
  damage->setStretchRateWeight(agx::Real(100.0));
  damage->setBendRateWeight(agx::Real(1.0));
  damage->setTwistRateWeight(agx::Real(30.0));
 
 
  // While the rate damage sources captures damages caused by motion within the
  // cable, the tension sources captures damages caused by tension in the cable.
  damage->setStretchTensionWeight(agx::Real(1e-3));
  damage->setBendTensionWeight(agx::Real(0.02));
  damage->setTwistTensionWeight(agx::Real(0.04));
 
 
  // The final damage source if contact forces. Normal and friction forces are
  // treated separately.
  damage->setNormalForceWeight(agx::Real(2e-4));
  damage->setFrictionForceWeight(agx::Real(2.0));
 
 
  // The damage model includes the concept of "small safe deformations". A
  // deformation smaller than a set threshold produces no deformation or
  // deformation rate damage. Tension and contact forces are not effected by
  // this setting.
  damage->setStretchThreshold(agx::Real(0.0));
  damage->setBendThreshold(agx::Real(0.1));
  damage->setTwistThreshold(agx::Real(0.4));
 
 
 
 
  // Now that the damage estimation has been configured it's time to simulate a
  // bit.
  simulation->stepTo(agx::Real(1.5));
 
  // Damage is computed per cable segment and we get access to the damage
  // contribution from the last time step by calling 'getCurrentDamages' on the
  // cable damage instance.
  const agxCable::SegmentDamageVector& segmentDamages = damage->getCurrentDamages();
 
  // We can inspect a particular segment's damage by indexing into the vector.
  const agxCable::SegmentDamage& segmentDamage = segmentDamages[29];
  std::cout << "Total damage:    " << segmentDamage.total() << '\n';
  std::cout << "Stretch tension: " << segmentDamage.stretchTension() << '\n';
  std::cout << "Stretch rate:    " << segmentDamage.stretchRate() << '\n';
  std::cout << "Bend tension:    " << segmentDamage.bendTension() << '\n';
  std::cout << "Bend rate:       " << segmentDamage.bendRate() << '\n';
  std::cout << "Twist tension:   " << segmentDamage.twistTension() << '\n';
  std::cout << "Twist rate:      " << segmentDamage.twistRate() << '\n';
  std::cout << '\n';
 
  // The sum of all time steps' damage contributions is available as well.
  const agxCable::SegmentDamageVector& accumulatedSegmentDamages = damage->getAccumulatedDamages();
  const agxCable::SegmentDamage accumulatedSegmentDamage = accumulatedSegmentDamages[29];
  std::cout << "Total damage:    " << accumulatedSegmentDamage.total() << '\n';
  std::cout << "Stretch tension: " << accumulatedSegmentDamage.stretchTension() << '\n';
  std::cout << "Stretch rate:    " << accumulatedSegmentDamage.stretchRate() << '\n';
  std::cout << "Bend tension:    " << accumulatedSegmentDamage.bendTension() << '\n';
  std::cout << "Bend rate:       " << accumulatedSegmentDamage.bendRate() << '\n';
  std::cout << "Twist tension:   " << accumulatedSegmentDamage.twistTension() << '\n';
  std::cout << "Twist rate:      " << accumulatedSegmentDamage.twistRate() << '\n';
  std::cout << '\n';
 
 
  // A damage renderer is available for demonstration purposes. This makes it
  // possible to see the cable when running this tutorial. The cable segments
  // are colored based on the current total damage as a color gradient from
  // green to red as the estimated damage goes from zero to the given maximum.
  // The color for cable segments with a damage estimate larger than the maximum will
  // be clamped to full red.
  agx::Real damageMaximum(50);
  agxOSG::CableDamageRendererRef renderer = new agxOSG::CableDamageRenderer(damage, damageMaximum, root);
  renderer->post(simulation->getTimeStamp()); // Update colors for the current time step.
  simulation->add(renderer);
 
  // Create a step event callback that writes the largest current damages to the screen.
  agxUtil::StepEventCallback::post([application, damage](agxSDK::Simulation*) {
    agxOSG::SceneDecorator* text = application->getSceneDecorator();
 
    // The damage sources can be accessed not only by name, but also by index.
    // The range of valid indices are found in the agxCable::DamageTypes
    // namespace. A loop over all damage sources within a loop over all the
    // segments makes it possible to find the largest damage contribution for
    // each damage source and cable segment.
    const agxCable::SegmentDamageVector& damages = damage->getCurrentDamages();
    agxCable::SegmentDamage largest;
    for (auto& d: damages) {
      for (int i = 0; i < agxCable::DamageTypes::NUM_CABLE_DAMAGE_TYPES; ++i) {
        largest[i] = std::max(largest[i], d[i]);
      }
    }
 
    int i(0);
    text->setText(i++, agx::String::format("Stretch:"));
    text->setText(i++, agx::String::format("  Deformation: %f", largest.stretchDeformation()));
    text->setText(i++, agx::String::format("  Tension:     %f", largest.stretchTension()));
    text->setText(i++, agx::String::format("  Rate:        %f", largest.stretchRate()));
    text->setText(i++, agx::String::format("Bend:"));
    text->setText(i++, agx::String::format("  Deformation: %f", largest.bendDeformation()));
    text->setText(i++, agx::String::format("  Tension:     %f", largest.bendTension()));
    text->setText(i++, agx::String::format("  Rate:        %f", largest.bendRate()));
    text->setText(i++, agx::String::format("Twist"));
    text->setText(i++, agx::String::format("  Deformation: %f", largest.twistDeformation()));
    text->setText(i++, agx::String::format("  Tension:     %f", largest.twistTension()));
    text->setText(i++, agx::String::format("  Rate:        %f", largest.twistRate()));
    text->setText(i++, agx::String::format("Contacts:"));
    text->setText(i++, agx::String::format("  Normal:      %f", largest.contactNormalForce()));
    text->setText(i++, agx::String::format("  Friction:    %f", largest.contactFrictionForce()));
    i++;
    text->setText(i++, agx::String::format("Total:         %f", largest.total()));
  }, simulation);
 
  // A step with the damage and text renderers in place so they have had a
  // change to render the first frame.
  simulation->stepForward();
 
  return root;
}
 
 
 
int main(int argc, char** argv)
{
  agx::AutoInit init;
 
  std::cout << '\t' << agxGetLibraryName() << ' ' << agxGetVersion() << ' ' << agxGetCompanyName() << "(C)\n "
            << "\tTutorial cable damage\n" << "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication();
 
    application->addScene(buildTutorial1, '1');
 
    if (application->init(argc, argv)) {
      return application->run();
    }
  }
  catch (std::exception& e) {
    std::cerr << "\nMain caught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_cable_tunneling_guard.cpp

/*
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.
*/
 
/*
 
Demonstrates how to use the cable tunneling guard class to help prevent tunneling in cable<->cable interactions.
 
*/
 
#include <agx/agx.h>
#include <agx/version.h>
 
#include <agxCollide/Box.h>
#include <agxCollide/Geometry.h>
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/Node.h>
#include <agxOSG/utils.h>
 
#include <agxIO/ArgumentParser.h>
 
#include <agxCable/Cable.h>
#include <agxCable/CableTunnelingGuard.h>
#include <agxCable/Node.h>
#include <agxUtil/StepEventCallback.h>
 
namespace tutorial_helpers
{
  agxSDK::Assembly *create_swinging_cable_assembly(agx::Vec3 position = agx::Vec3(0, 0, 0), agx::Real resolution = 8)
  {
    auto assembly = new agxSDK::Assembly();
 
    agxCable::CableRef fixedCable = new agxCable::Cable(0.01, resolution);
    fixedCable->setName("Fixed cable");
    fixedCable->add(new agxCable::BodyFixedNode(nullptr, position + agx::Vec3(0, -2, 0)));
    fixedCable->add(new agxCable::BodyFixedNode(nullptr, position + agx::Vec3(0, 2, 0)));
    assembly->add(fixedCable);
 
    agxCable::CableRef swingingCable = new agxCable::Cable(0.01, resolution);
    swingingCable->setName("Swinging cable");
    swingingCable->add(new agxCable::BodyFixedNode(nullptr, position + agx::Vec3(0, 0, 2)));
    swingingCable->add(new agxCable::FreeNode(position + agx::Vec3(2, 0, 0)));
    assembly->add(swingingCable);
 
    return assembly;
  }
 
  agxSDK::Assembly *create_falling_cable_assembly(agx::Vec3 position = agx::Vec3(0, 0, 0), agx::Real resolution = 8)
  {
    auto assembly = new agxSDK::Assembly();
 
    agxCable::CableRef fixedCable = new agxCable::Cable(0.01, resolution);
    fixedCable->setName("Fixed cable");
    fixedCable->add(new agxCable::BodyFixedNode(nullptr, position + agx::Vec3(0, -2, 0)));
    fixedCable->add(new agxCable::BodyFixedNode(nullptr, position + agx::Vec3(0, 2, 0)));
    assembly->add(fixedCable);
 
    agxCable::CableRef fallingCable = new agxCable::Cable(0.01, resolution);
    fallingCable->setName("Falling cable");
    fallingCable->add(new agxCable::FreeNode(position + agx::Vec3(-0.5, 0, 1)));
    fallingCable->add(new agxCable::FreeNode(position + agx::Vec3(0.5, 0, 1)));
    assembly->add(fallingCable);
 
    return assembly;
  }
 
  void createHelperText(agxOSG::SceneDecorator *sd)
  {
    sd->setText(0, "Press e to pause/resume the simulation.");
    sd->setText(1, "Press r to step the simulation when paused.");
    sd->setText(2, "Press g to enable/disable rendering of the tunneling guard hulls.");
  }
}
 
osg::Group *buildTutorial1(agxSDK::Simulation *simulation, agxOSG::ExampleApplication *application)
{
  using tutorial_helpers::create_swinging_cable_assembly;
  std::cout << "Tutorial 1 - Basic usage\n";
 
  osg::Group *root = new osg::Group();
 
  /*
  To showcase a simple use case for the CableTunnelingGuard component we will construct a scene consisting of two cables
  set to collide. One cable will be suspended by both ends while another is suspended only in one end, creating a
  swinging motion towards the fixed cable.
  */
  agxSDK::AssemblyRef cables = create_swinging_cable_assembly();
 
  /*
  To help alliviate this problem the CableTunnelingGuard component can be used on either of the cables in this scenario.
  It will create scaled sensor geometries around the cable which themselves come in contact with other cables,
  potentially (if a contact in the next step seems resonable) adding a geometry contact to the solver. This contact will
  act as context for the solver to use in the timestep, making sure they collide properly.
  */
  agxCable::Cable::asCable(cables->getAssembly("Swinging cable"))
      ->addComponent(new agxCable::CableTunnelingGuard(
          4));  // Feel free to remove this line to check that the cables would tunnel otherwise.
 
  simulation->add(cables);
 
  // We also visualise the cables so that we can se the component in action
  agxOSG::createVisual(cables, root);
 
  tutorial_helpers::createHelperText(application->getSceneDecorator());
  return root;
}
 
osg::Group *buildTutorial2(agxSDK::Simulation *simulation, agxOSG::ExampleApplication *application)
{
  using tutorial_helpers::create_swinging_cable_assembly;
 
  std::cout << "Tutorial 2 - Hull Scale\n";
 
  osg::Group *root = new osg::Group();
 
  // We create three sets of swinging cables to show the difference in using different hull scales
  agxSDK::Assembly *smallScale = create_swinging_cable_assembly(agx::Vec3(-2, 0, 0));
  agxSDK::Assembly *largeScale = create_swinging_cable_assembly(agx::Vec3(0, 0, 0));
  agxSDK::Assembly *twoSmallScales = create_swinging_cable_assembly(agx::Vec3(2, 0, 0));
 
  agxCable::Cable::asCable(smallScale->getAssembly("Swinging cable"))
      ->addComponent(new agxCable::CableTunnelingGuard(4));
 
  agxCable::Cable::asCable(largeScale->getAssembly("Swinging cable"))
      ->addComponent(new agxCable::CableTunnelingGuard(8));
 
  // Different components hulls can interact with each other, effectively extending the range of both. In the case were
  // these guards have differing parameters, they are mediated as described in the documentation
  agxCable::Cable::asCable(twoSmallScales->getAssembly("Swinging cable"))
      ->addComponent(new agxCable::CableTunnelingGuard(4));
  agxCable::Cable::asCable(twoSmallScales->getAssembly("Fixed cable"))
      ->addComponent(new agxCable::CableTunnelingGuard(4));
 
  simulation->add(smallScale);
  simulation->add(largeScale);
  simulation->add(twoSmallScales);
  agxOSG::createVisual(smallScale, root);
  agxOSG::createVisual(largeScale, root);
  agxOSG::createVisual(twoSmallScales, root);
 
  tutorial_helpers::createHelperText(application->getSceneDecorator());
  return root;
}
 
osg::Group *buildTutorial3(agxSDK::Simulation *simulation, agxOSG::ExampleApplication *application)
{
  std::cout << "Tutorial 3 - Angle Threshold.\n";
 
  osg::Group *root = new osg::Group();
 
  // We once again use a fixed cable to test the components against
  agxCable::CableRef fixedCable = new agxCable::Cable(0.01, 2);
  fixedCable->add(new agxCable::BodyFixedNode(nullptr, agx::Vec3(-0.5, 0, 0)));
  fixedCable->add(new agxCable::BodyFixedNode(nullptr, agx::Vec3(0.5, 0, 0)));
  simulation->add(fixedCable);
  agxOSG::createVisual(fixedCable, root);
 
  // When the ends of a cable are tunneling this may be resolved by changing the angle threshold parameter
  agxCable::CableRef verticallyFallingCable = new agxCable::Cable(0.01, 2);
  verticallyFallingCable->add(new agxCable::FreeNode(agx::Vec3(0.25, 0, 0.7)));
  verticallyFallingCable->add(new agxCable::FreeNode(agx::Vec3(0.25, 0, 0.4)));
  agxCable::CableTunnelingGuardRef guard = new agxCable::CableTunnelingGuard(8);
  // Allow predictions for contacts at approaching angles above 0 radians at the ends of the cables
  guard->setAngleThreshold(0);
  verticallyFallingCable->addComponent(guard);
  simulation->add(verticallyFallingCable);
  agxOSG::setDiffuseColor(agxOSG::createVisual(verticallyFallingCable, root), agxRender::Color::Green());
 
  // The same scenario without adjustment produces tunneling
  agxCable::CableRef verticallyFallingCableDefaultAngleThreshold = new agxCable::Cable(0.01, 2);
  verticallyFallingCableDefaultAngleThreshold->add(new agxCable::FreeNode(agx::Vec3(-0.25, 0, 0.7)));
  verticallyFallingCableDefaultAngleThreshold->add(new agxCable::FreeNode(agx::Vec3(-0.25, 0, 0.4)));
  guard = new agxCable::CableTunnelingGuard(8);
  verticallyFallingCableDefaultAngleThreshold->addComponent(
      guard);  // Add with default parameters which sets angle threshold to PI_2*0.9
  simulation->add(verticallyFallingCableDefaultAngleThreshold);
  agxOSG::setDiffuseColor(agxOSG::createVisual(verticallyFallingCableDefaultAngleThreshold, root),
                          agxRender::Color::Red());
 
  // Reducing this threshold does also produce potentially unwanted effects
  // This cable will fall next to the other end but because of the lower angle threshhold, it will create a contact and
  // react to the fixed cable (having predicted a contact due to the high velocity of the falling cable)
  agxCable::CableRef nearMissCableLowAngle = new agxCable::Cable(0.01, 2);
  guard = new agxCable::CableTunnelingGuard(8);
  guard->setAngleThreshold(0.5);  // Allow at 0.5 radians instead
  nearMissCableLowAngle->addComponent(guard);
  nearMissCableLowAngle->add(new agxCable::FreeNode(agx::Vec3(0.54, -0.2, 1)));
  nearMissCableLowAngle->add(new agxCable::FreeNode(agx::Vec3(0.54, 0.2, 1)));
  simulation->add(nearMissCableLowAngle);
  agxOSG::setDiffuseColor(agxOSG::createVisual(nearMissCableLowAngle, root), agx::Vec4f(1, 0, 0, 1));
 
  tutorial_helpers::createHelperText(application->getSceneDecorator());
  return root;
}
 
osg::Group *buildTutorial4(agxSDK::Simulation *simulation, agxOSG::ExampleApplication *application)
{
  using tutorial_helpers::create_falling_cable_assembly;
  std::cout << "Tutorial 4 - Leniency\n";
 
  osg::Group *root = new osg::Group();
 
  // Here we demonstrate a clear case where leniency is needed to prevent tunneling in a system but we also show an
  // example of the side-effects caused by increasing it
  agxSDK::Assembly *softCables = create_falling_cable_assembly(agx::Vec3(-0.6, 0, 0), 16);
  agxSDK::Assembly *softCablesWithLeniency = create_falling_cable_assembly(agx::Vec3(0.6, 0, 0), 16);
 
  // Softer cables will in this case produce an oscillating motion after the intial impact which the guard fails to
  // predict contacts for Higher resolution cables also exhibit more complex movements which in turn is harder to
  // correctly predict, causing more failure cases without leniency (or debouncing, see next tutorial)
  for (auto cable : softCables->getAssemblies())
    agxCable::Cable::asCable(cable)->getCableProperties()->setYoungsModulus(1e8, agxCable::BEND);
 
  agxCable::Cable::asCable(softCables->getAssembly("Falling cable"))
      ->addComponent(new agxCable::CableTunnelingGuard(8));
 
  for (auto cable : softCablesWithLeniency->getAssemblies())
    agxCable::Cable::asCable(cable)->getCableProperties()->setYoungsModulus(1e8, agxCable::BEND);
 
  auto guard = new agxCable::CableTunnelingGuard(8);
  // Increasing the leniency however will create contacts in less certain scenarios, saving the cables from tunneling in
  // this scenario
  guard->setLeniency(1);
  agxCable::Cable::asCable(softCablesWithLeniency->getAssembly("Falling cable"))->addComponent(guard);
 
  simulation->add(softCables);
  simulation->add(softCablesWithLeniency);
  agxOSG::setDiffuseColor(agxOSG::createVisual(softCables->getAssembly("Falling cable"), root),
                          agxRender::Color::Red());
  agxOSG::createVisual(softCables->getAssembly("Fixed cable"), root);
  agxOSG::setDiffuseColor(agxOSG::createVisual(softCablesWithLeniency->getAssembly("Falling cable"), root),
                          agxRender::Color::Green());
  agxOSG::createVisual(softCablesWithLeniency->getAssembly("Fixed cable"), root);
 
  tutorial_helpers::createHelperText(application->getSceneDecorator());
  return root;
}
 
osg::Group *buildTutorial5(agxSDK::Simulation *simulation, agxOSG::ExampleApplication *application)
{
  using tutorial_helpers::create_falling_cable_assembly;
  std::cout << "Tutorial 5 - Debounce Steps.\n";
 
  osg::Group *root = new osg::Group();
 
  // Here we demonstrate a very similiar case to the previous tutorial but use debouncing to prevent the resulting
  // tunneling instead
  agxSDK::Assembly *softCables = create_falling_cable_assembly(agx::Vec3(-0.6, 0, 0), 16);
  agxSDK::Assembly *softCablesWithDebounce = create_falling_cable_assembly(agx::Vec3(0.6, 0, 0), 16);
 
  // We once again use softer and higher resolution cables to get a more unpredictable behaviour around the time of
  // impact, requiring additional parametrisation to solve the resulting tunneling
  for (auto cable : softCables->getAssemblies())
    agxCable::Cable::asCable(cable)->getCableProperties()->setYoungsModulus(1e8, agxCable::BEND);
 
  agxCable::Cable::asCable(softCables->getAssembly("Falling cable"))
      ->addComponent(new agxCable::CableTunnelingGuard(8));
 
  for (auto cable : softCablesWithDebounce->getAssemblies())
    agxCable::Cable::asCable(cable)->getCableProperties()->setYoungsModulus(1e8, agxCable::BEND);
 
  auto guard = new agxCable::CableTunnelingGuard(8);
  // Forcing the guard to continue adding contacts between geometries 3 steps after an intial added contacts allows the
  // guard to catch the unpredictable behaviour which occurs around the impact
  guard->setDebounceSteps(3);
  agxCable::Cable::asCable(softCablesWithDebounce->getAssembly("Falling cable"))->addComponent(guard);
 
  simulation->add(softCables);
  simulation->add(softCablesWithDebounce);
  agxOSG::setDiffuseColor(agxOSG::createVisual(softCables->getAssembly("Falling cable"), root),
                          agxRender::Color::Red());
  agxOSG::createVisual(softCables->getAssembly("Fixed cable"), root);
  agxOSG::setDiffuseColor(agxOSG::createVisual(softCablesWithDebounce->getAssembly("Falling cable"), root),
                          agxRender::Color::Green());
  agxOSG::createVisual(softCablesWithDebounce->getAssembly("Fixed cable"), root);
 
  tutorial_helpers::createHelperText(application->getSceneDecorator());
  return root;
}
 
int main(int argc, char **argv)
{
  agx::AutoInit init;
 
  std::cout << '\t' << agxGetLibraryName() << ' ' << agxGetVersion() << ' ' << agxGetCompanyName() << "(C)\n "
            << "\tTutorial cable tunneling guard\n"
            << "\t--------------------------------\n\n"
            << std::endl;
 
  try
  {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication();
 
    application->addScene(buildTutorial1, '1');
    application->addScene(buildTutorial2, '2');
    application->addScene(buildTutorial3, '3');
    application->addScene(buildTutorial4, '4');
    application->addScene(buildTutorial5, '5');
 
    if (application->init(argc, argv))
    {
      return application->run();
    }
  }
  catch (std::exception &e)
  {
    std::cerr << "\nMain caught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_car.cpp

/*
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.
*/
 
/*
 
This tutorials shows how to build a simple car with keyboard interaction.
 
*/
 
#include <agxOSG/ExampleApplication.h>
 
#include <agxIO/ImageReaderPNG.h>
 
#include <agxUtil/HeightFieldGenerator.h>
#include <agxUtil/agxUtil.h>
 
#include <agx/Logger.h>
 
#include "tutorialUtils.h"
#include <agx/Hinge.h>
#include <agx/CylindricalJoint.h>
 
using namespace agx;
using namespace agxOSG;
 
 
class CarWheel : public agxSDK::Assembly
{
  public:
    enum GeometryType { CYLINDER = 0, SPHERE, DOUBLE_SPHERE };
 
    CarWheel( RigidBody* carBody, const agx::String& bodyAttachmentName, Real radius, const Vec3& suspensionAxis, const Vec3& drivingAxis, osg::Group* root, GeometryType geometryType = SPHERE )
      : m_body( carBody ), m_bodyAttachmentName( bodyAttachmentName ), m_radius( radius ), m_suspensionAxis( suspensionAxis ),
        m_drivingAxis( drivingAxis ), m_root( root ), m_geometryType( geometryType )
    {
      create();
    }
 
    virtual void addNotification( agxSDK::Simulation* ) override
    {
    }    
 
    RigidBody* getWheel() { return m_wheel; }
    RigidBody* getWheelSteeringAttachment() { return m_wheelSteeringAttachment; }
    const RigidBody* getBody() const { return m_body; }
 
    Hinge* getDrivingHinge() { return m_drivingHinge; }
    Hinge* getSteeringHinge() { return m_steeringHinge; }
    CylindricalJoint* getSuspension() { return m_steeringAndSuspension; }
 
    void setSuspension( RangeReal range, Real lockCompliance, Real lockDamping, Real position = 0 )
    {
      agxAssert( getSuspension() );
      getSuspension()->getRange1D( Constraint2DOF::FIRST )->setEnable( true );
      getSuspension()->getRange1D( Constraint2DOF::FIRST )->setRange( range );
 
      getSuspension()->getLock1D( Constraint2DOF::FIRST )->setEnable( true );
      getSuspension()->getLock1D( Constraint2DOF::FIRST )->setPosition( position );
      getSuspension()->getLock1D( Constraint2DOF::FIRST )->getRegularizationParameters()->setCompliance( lockCompliance );
      getSuspension()->getLock1D( Constraint2DOF::FIRST )->getRegularizationParameters()->setDamping( lockDamping );
    }
 
  private:
    void create()
    {
      // Create the wheel body and geometry given specified type.
      m_wheel = new RigidBody();
      if ( m_geometryType == CYLINDER ) {
        m_wheel->add( new agxCollide::Geometry( new agxCollide::Cylinder( m_radius, 0.3 ) ), AffineMatrix4x4::rotate( Vec3( 0, 1, 0 ), m_drivingAxis ) );
      }
      else if ( m_geometryType == SPHERE ) {
        m_wheel->add( new agxCollide::Geometry( new agxCollide::Sphere( m_radius ) ) );
      }
      else if ( m_geometryType == DOUBLE_SPHERE ){
        m_wheel->add( new agxCollide::Geometry( new agxCollide::Sphere( m_radius ) ) );
        m_wheel->add( new agxCollide::Geometry( new agxCollide::Sphere( m_radius ) ), AffineMatrix4x4::translate( m_drivingAxis * 0.15 ) );
      }
      createVisual( m_wheel, m_root.get() );
 
      // Extra body in order to get suspension.
      m_wheelSteeringAttachment = new RigidBody();
      m_wheelSteeringAttachment->getMassProperties()->setMass( 45 );
 
      // Add the wheel and steering attachment bodies to this assembly.
      add( m_wheel );
      add( m_wheelSteeringAttachment );
 
      // Create the suspension frame and rotate the z axis to the given suspension axis.
      FrameRef suspensionFrame = new Frame();
      suspensionFrame->setLocalRotate( Quat( AffineMatrix4x4::rotate( Vec3( 0, 0, 1 ), m_suspensionAxis ) ) );
      m_wheelSteeringAttachment->addAttachment( suspensionFrame, "suspension" );
      m_wheelSteeringAttachment->addAttachment( suspensionFrame, "steering" );
 
      // Create the driving frame and rotate the z axis to the given driving axis.
      FrameRef drivingFrame = new Frame();
      drivingFrame->setLocalRotate( Quat( AffineMatrix4x4::rotate( Vec3( 0, 0, 1 ), m_drivingAxis ) ) );
      m_wheelSteeringAttachment->addAttachment( drivingFrame, "driving" );
 
      // Position the wheel frame some distance away from the wheel/steering attachment.
      Real distFromAttachment = 0.05;
      FrameRef wheelFrame = new Frame();
      wheelFrame->setLocalMatrix( AffineMatrix4x4::rotate( Vec3( 0, 0, 1 ), m_drivingAxis ) * AffineMatrix4x4::translate( -m_drivingAxis * distFromAttachment ) );
      m_wheel->addAttachment( wheelFrame, "driving" );
 
      // Wheel/steering attachment reference position, origin.
      m_wheelSteeringAttachment->setPosition( Vec3( 0, 0, 0 ) );
      // The position of the wheel is a given distance in the driving axis direction.
      // The two attachment frames should be on the same position now.
      m_wheel->setPosition( m_drivingAxis * distFromAttachment );
 
      // Attach the body to the steering.
      m_steeringHinge = new Hinge( m_body, m_body->getAttachment( m_bodyAttachmentName ), m_wheelSteeringAttachment, m_wheelSteeringAttachment->getAttachment( "steering" ) );
      m_steeringHinge->setCompliance( 1E2 );
      m_steeringHinge->getMotor1D()->getRegularizationParameters()->setCompliance( 1E-10 );
      m_steeringHinge->getMotor1D()->setForceRange( RangeReal( 2E3 ) );
      add( m_steeringHinge );
 
      // Attach the body to the suspension.
      m_steeringAndSuspension = new CylindricalJoint( m_body, m_body->getAttachment( m_bodyAttachmentName ), m_wheelSteeringAttachment, m_wheelSteeringAttachment->getAttachment( "suspension" ) );
      add( m_steeringAndSuspension );
 
      // Attach the wheel to the body (steering/wheel attachment body).
      m_drivingHinge = new Hinge( m_wheel, m_wheel->getAttachment( "driving" ), m_wheelSteeringAttachment, m_wheelSteeringAttachment->getAttachment( "driving" ) );
      add( m_drivingHinge );
    }
 
    using Assembly::addNotification;
    using Assembly::removeNotification;
 
  private:
    RigidBodyRef  m_body;
    agx::String   m_bodyAttachmentName;
 
    Real                                m_radius;
    Vec3                                m_suspensionAxis;
    Vec3                                m_drivingAxis;
    osg::observer_ptr< osg::Group >  m_root;
    GeometryType                        m_geometryType;
 
    RigidBodyRef        m_wheel;
    RigidBodyRef        m_wheelSteeringAttachment;
    HingeRef            m_steeringHinge;
    HingeRef            m_drivingHinge;
    CylindricalJointRef m_steeringAndSuspension;
};
 
typedef ref_ptr< CarWheel > CarWheelRef;
 
class CarController : public agxSDK::StepEventListener
{
  public:
    CarController( CarWheel* backRight, CarWheel* backLeft, CarWheel* frontRight, CarWheel* frontLeft, Real maxTorque, int keyAccelerate, int keyBrake, int keyGearUp, int keyGearDown, int keySteerLeft, int keySteerRight )
      : m_backRight( backRight ), m_backLeft( backLeft ), m_frontRight( frontRight ), m_frontLeft( frontLeft ), m_maxTorque( maxTorque ),
        m_gearRatio( 8, Real( 0 ) ), m_differentialRatio( 4.1 ), m_gasPedalInput( 0 ), m_brakePedalInput( 0 ), m_steeringInput( 0 )
    {
      setMask( PRE_STEP );
 
      // Keyboard callback. Pedal input 0 or 1, brake pedal input 0 or 1, number of gears + 2 (neutral and reverse) and keyboard input.
      m_input = new CarControllerInput( m_gasPedalInput, m_brakePedalInput, m_steeringInput, int(m_gearRatio.size()), keyAccelerate, keyBrake, keyGearUp, keyGearDown, keySteerLeft, keySteerRight );
 
      m_backRight->getDrivingHinge()->getMotor1D()->setEnable( true );
      m_backLeft->getDrivingHinge()->getMotor1D()->setEnable( true );
      m_backRight->getDrivingHinge()->getMotor1D()->setSpeed( -100 );
      m_backLeft->getDrivingHinge()->getMotor1D()->setSpeed( 100 );
 
      // Ferrari Enzo?
      m_gearRatio[ 0 ] = 0; // Neutral
      m_gearRatio[ 1 ] = 3.15;
      m_gearRatio[ 2 ] = 2.18;
      m_gearRatio[ 3 ] = 1.57;
      m_gearRatio[ 4 ] = 1.19;
      m_gearRatio[ 5 ] = 0.94;
      m_gearRatio[ 6 ] = 0.76;
      m_gearRatio[ 7 ] = 4.1; // Reverse
    }
 
    virtual void addNotification() override
    {
      getSimulation()->add( m_input );
    }
 
    virtual void removeNotification() override
    {
      getSimulation()->remove( m_input );
    }
 
    virtual void pre( const agx::TimeStamp& ) override
    {
      // Computes the torque to the back wheels given gear and current rpm (differential).
      computeAndSetTorque( m_backRight->getDrivingHinge(), m_backLeft->getDrivingHinge() );
      // Computes the current brake torque (if brake pedal is down).
      computeAndSetBrake( m_frontRight->getDrivingHinge(), m_frontLeft->getDrivingHinge() );
 
      // Updates the steering.
      updateSteering( m_frontRight->getSteeringHinge(), m_frontLeft->getSteeringHinge() );
 
      m_input->update( 0, 0 );
 
      // Render current gear and speed.
      std::stringstream ss;
      ss << "Speed: " << int( m_frontLeft->getBody()->getVelocity().length() * 3.6 ) << " km/h";
      agxRender::RenderProxy *proxy = getSimulation()->getRenderManager()->acquireText( ss.str().c_str(), agx::Vec3(0.01, 0.025,0));
      proxy->setColor( Vec3( 1, 0, 0) );
    }
 
  private:
    void computeAndSetTorque( Hinge* h1, Hinge* h2 ) const
    {
      agxAssert( m_input->getGear() < m_gearRatio.size() && m_gearRatio[ m_input->getGear() ] >= 0 && m_differentialRatio > 0 );
 
      // Current gear ratio given current gear.
      Real gearRatio = m_gearRatio[ m_input->getGear() ];
      // Set the maximum angular speed to the motors for this gear.
      m_backRight->getDrivingHinge()->getMotor1D()->setSpeed( -getMaxWheelRPM( m_input->getGear() ) );
      m_backLeft->getDrivingHinge()->getMotor1D()->setSpeed( getMaxWheelRPM( m_input->getGear() ) );
 
      // Differential calculations.
      Real omegaTot = std::abs( h1->getCurrentSpeed() ) + std::abs( h2->getCurrentSpeed() );
      // The wheels aren't moving
      if ( agx::equalsZero( omegaTot ) ) {
        h1->getMotor1D()->setForceRange( RangeReal( 0 ) );
        h2->getMotor1D()->setForceRange( RangeReal( 0 ) );
        return;
      }
      Real omega0 = 0.5 * omegaTot / ( gearRatio * m_differentialRatio );
      Real taue = m_gasPedalInput * getTorque( omega0 );
      Real taud = gearRatio * m_differentialRatio * taue;
 
      Real t1 = taud * std::abs( h1->getCurrentSpeed() ) / omegaTot;
      Real t2 = taud * std::abs( h2->getCurrentSpeed() ) / omegaTot;
 
      h1->getMotor1D()->setForceRange( RangeReal( t1 ) );
      h2->getMotor1D()->setForceRange( RangeReal( t2 ) );
    }
 
    void computeAndSetBrake( Hinge* h1, Hinge* h2 ) const
    {
      Real maxBrakeTorque = 2E3;
      Real brakeTorque = maxBrakeTorque * m_brakePedalInput;
      h1->getLock1D()->setEnable( m_brakePedalInput > 0 );
      h2->getLock1D()->setEnable( m_brakePedalInput > 0 );
      h1->getLock1D()->setPosition( h1->getAngle() );
      h2->getLock1D()->setPosition( h2->getAngle() );
      h1->getLock1D()->setForceRange( RangeReal( brakeTorque ) );
      h2->getLock1D()->setForceRange( RangeReal( brakeTorque ) );
    }
 
    void updateSteering( Hinge* h1, Hinge* h2 ) const
    {
      Real maxSteeringSpeed = 2;
 
      if ( agx::equalsZero( m_steeringInput ) ) {
        h1->getMotor1D()->setSpeed( -agx::sign( h1->getAngle() ) * 10 * std::abs( h1->getAngle() ) );
        h2->getMotor1D()->setSpeed( -agx::sign( h2->getAngle() ) * 10 * std::abs( h2->getAngle() ) );
        return;
      }
 
      Real bodySpeed = m_frontLeft->getBody()->getVelocity().length();
 
                        Real steeringSpeed = agx::clamp( maxSteeringSpeed / std::sqrt( agx::Real(0.5) * bodySpeed ), agx::Real(0.1), maxSteeringSpeed );
 
      h1->getMotor1D()->setSpeed( m_steeringInput * steeringSpeed );
      h2->getMotor1D()->setSpeed( m_steeringInput * steeringSpeed );
    }
 
    Real getTorque( Real /*omega0*/ ) const
    {
      // For now, we always deliver maximum torque.
      return m_maxTorque;
    }
 
    Real getMaxWheelRPM( size_t gear ) const
    {
      // Reverse?
      int sgn = int(2 * ( gear != m_gearRatio.size() - 1 ) - 1);
      return m_gearRatio[ gear ] > 0 ? sgn * 200 / m_gearRatio[ gear ] : 0;
    }
 
  private:
    class CarControllerInput : public agxSDK::GuiEventListener
    {
      public:
        CarControllerInput( Real& gasPedalInput, Real& brakePedalInput, Real& steeringInput, int numGearsIncReverse, int keyAccelerate, int keyBrake, int keyGearUp, int keyGearDown, int keySteerLeft, int keySteerRight )
          : m_gasPedalInput( gasPedalInput ), m_brakePedalInput( brakePedalInput ), m_steeringInput( steeringInput ), m_numGears( numGearsIncReverse ),
            m_keyAccelerate( keyAccelerate ), m_keyBrake( keyBrake ), m_keyGearUp( keyGearUp ), m_keyGearDown( keyGearDown ),
            m_keySteerLeft( keySteerLeft ), m_keySteerRight( keySteerRight ), m_gear( 0 ), m_steeringReleased( true )
        {
          setMask( ActivationMask(KEYBOARD | UPDATE) );
        }
 
        virtual bool keyboard( int key, unsigned int , float , float , bool keydown )
        {
          bool handled = false;
 
          if ( key == m_keyAccelerate ) {
            m_gasPedalInput = (Real)keydown;
            handled = true;
          }
          else if ( key == m_keyBrake ) {
            m_brakePedalInput = (Real)keydown;
            handled = true;
          }
          else if ( keydown && key == m_keyGearUp ) {
            m_gear = std::min( m_gear + 1, m_numGears - 2 );
            handled = true;
          }
          else if ( keydown && key == m_keyGearDown ) {
            m_gear = std::max( m_gear - 1, -1 );
            handled = true;
          }
          else if ( key == m_keySteerLeft ) {
            m_steeringReleased = !keydown;
            m_steeringInput = -Real( keydown );
            handled = true;
          }
          else if ( key == m_keySteerRight ) {
            m_steeringReleased = !keydown;
            m_steeringInput = Real( keydown );
            handled = true;
          }
 
          return handled;
        }
 
        virtual void update( float , float )
        {
          std::stringstream gTxt;
          gTxt << "Gear:  " << ( m_gear == 0 ? "N" : m_gear < 0 ? "R" : "" );
          if ( m_gear > 0 )
            gTxt << m_gear;
          agxRender::RenderProxy *proxy = getSimulation()->getRenderManager()->acquireText( gTxt.str().c_str(), agx::Vec3(0.01, 0.01,0));
          proxy->setColor( agx::Vec3(1, 0, 0) );
        }
 
        size_t getGear() const
        {
          if ( m_gear < 0 )
            return m_numGears - 1;
          return m_gear;
        }
 
        bool getSteeringReleased() const
        {
          return m_steeringReleased;
        }
      protected:
        virtual ~CarControllerInput() {}
 
      private:
        CarControllerInput& operator=(const CarControllerInput&) {return *this;}
 
      private:
        Real& m_gasPedalInput;
        Real& m_brakePedalInput;
        Real& m_steeringInput;
        int m_numGears;
        int m_keyAccelerate;
        int m_keyBrake;
        int m_keyGearUp;
        int m_keyGearDown;
        int m_keySteerLeft;
        int m_keySteerRight;
        int m_gear;
        bool m_steeringReleased;
    };
 
    typedef ref_ptr< CarControllerInput > CarControllerInputRef;
 
  private:
    CarWheelRef m_backRight;
    CarWheelRef m_backLeft;
    CarWheelRef m_frontRight;
    CarWheelRef m_frontLeft;
 
    Real m_maxTorque;
 
    RealVector m_gearRatio;
    Real m_differentialRatio;
    Real m_gasPedalInput;
    Real m_brakePedalInput;
    Real m_steeringInput;
 
    CarControllerInputRef m_input;
};
 
osg::Group* buildTutorialCar( agxSDK::Simulation *simulation, agxOSG::ExampleApplication *application )
{
  LOGGER_WARNING() << LOGGER_STATE( agx::Notify::PRINT_NONE ) << __FUNCTION__ << std::endl << LOGGER_END();
 
  osg::Group *root = new osg::Group;
 
  // Create ground box (static box)
  RigidBodyRef ground = new RigidBody();
  ground->setName( "ground" );
  // Add geometry with shape box. Size 100x100 m.
  ground->add( new agxCollide::Geometry( new agxCollide::Box( Vec3( 50, 50, 0.5 ) ) ) );
  // Set to static.
  ground->setMotionControl( RigidBody::STATIC );
  ground->setPosition( Vec3( 0, 0, -0.5 ) );
 
  createVisual( ground, root );
 
  simulation->add( ground );
 
  enum eWheel { RIGHT_BACK = 0, LEFT_BACK, RIGHT_FRONT, LEFT_FRONT };
 
  // Half extents of the car.
  Vec3 carBodyDim = Vec3( 2.2, 0.75, 0.45 );
  // How far from the back the rear wheels are, in m.
  Real rearOffset = 0.55;
  // How far from the front of the car body the front wheels are, in m.
  Real frontOffset = 0.72;
  // Radius of the wheels, in m.
  Real wheelRadius = 0.36;
  // It's possible to choose car wheel shape type:
  // - SPHERE: Single sphere
  // - DOUBLE_SPHERE: Two spheres to get two contact points at certain conditions
  // - CYLINDER: Cylinder
  CarWheel::GeometryType geometryType = CarWheel::SPHERE;
 
  // Create car body.
  RigidBodyRef carBody = new RigidBody();
  carBody->add( new agxCollide::Geometry( new agxCollide::Box( carBodyDim ) ) );
  createVisual( carBody, root );
 
  simulation->add( carBody );
 
  // Let the weight be 1.3 tons.
  carBody->getMassProperties()->setMass( 1.3E3 );
 
  // Move the center of mass/gravity forward and down (down prevents overturning)
  carBody->getCmFrame()->setLocalTranslate( Vec3( 0.5, 0, -0.5 ) );
 
  // Create attachment frames for the wheels. I.e., here will the constraints between the CarWheel and
  // the car body be positioned.
  FrameRef rightBackWheelAttachment = new Frame();
  rightBackWheelAttachment->setLocalTranslate( Vec3( -carBodyDim.x() + rearOffset, -carBodyDim.y(), -carBodyDim.z() ) );
 
  FrameRef leftBackWheelAttachment = new Frame();
  leftBackWheelAttachment->setLocalTranslate( Vec3( -carBodyDim.x() + rearOffset, carBodyDim.y(), -carBodyDim.z() ) );
 
  FrameRef rightFrontWheelAttachment = new Frame();
  rightFrontWheelAttachment->setLocalTranslate( Vec3( carBodyDim.x() - frontOffset, -carBodyDim.y(), -carBodyDim.z() ) );
 
  FrameRef leftFrontWheelAttachment = new Frame();
  leftFrontWheelAttachment->setLocalTranslate( Vec3( carBodyDim.x() - rearOffset, carBodyDim.y(), -carBodyDim.z() ) );
 
  carBody->addAttachment( rightBackWheelAttachment,  "rightBack" );
  carBody->addAttachment( leftBackWheelAttachment,   "leftBack" );
  carBody->addAttachment( rightFrontWheelAttachment, "rightFront" );
  carBody->addAttachment( leftFrontWheelAttachment,  "leftFront" );
 
  carBody->setPosition( Vec3( 0, 0, 4 ) );
 
  CarWheelRef wheels[4];
 
  // Create the wheels - given car body, the attachment name, radius, axis of the suspension,
  //                     axis of driving, root node for rendering and the shape type described above.
  wheels[ RIGHT_BACK ] = new CarWheel( carBody, "rightBack", wheelRadius, Vec3( 0, 0, 1 ), Vec3( 0, -1, 0 ), root, geometryType );
  wheels[ RIGHT_BACK ]->setPosition( rightBackWheelAttachment->getTranslate() );
 
  wheels[ LEFT_BACK ] = new CarWheel( carBody, "leftBack", wheelRadius, Vec3( 0, 0, 1 ), Vec3( 0, 1, 0 ), root, geometryType );
  wheels[ LEFT_BACK ]->setPosition( leftBackWheelAttachment->getTranslate() );
 
  wheels[ RIGHT_FRONT ] = new CarWheel( carBody, "rightFront", wheelRadius, Vec3( 0, 0, 1 ), Vec3( 0, -1, 0 ), root, geometryType );
  wheels[ RIGHT_FRONT ]->setPosition( rightFrontWheelAttachment->getTranslate() );
 
  wheels[ LEFT_FRONT ] = new CarWheel( carBody, "leftFront", wheelRadius, Vec3( 0, 0, 1 ), Vec3( 0, 1, 0 ), root, geometryType );
  wheels[ LEFT_FRONT ]->setPosition( leftFrontWheelAttachment->getTranslate() );
 
  // The CarWheel class is an agx::Assembly, add the assemblies to simulation and disable collisions
  // between the car body geometries and the wheel geometries.
  for ( int i = 0; i < 4; ++i ) {
    simulation->add( wheels[ i ] );
    agxUtil::setEnableCollisions( carBody, wheels[ i ]->getWheel(), false );
  }
 
  // Set the suspension. Let the suspension range go from -0.3 m to 2 m.
  RangeReal suspensionRange( -0.3, 2 );
  // Suspension compliance is the inverse spring constant.
  Real suspensionCompliance = 9.9E-6;
  // Viscosity of the suspension.
  Real suspensionDamping = 6.0 / 60;
 
  for ( int i = 0; i < 4; ++i ) {
    wheels[ i ]->setSuspension( suspensionRange, suspensionCompliance, suspensionDamping );
  }
 
  // Lock the back wheels steering hinge.
  wheels[ RIGHT_BACK ]->getSteeringHinge()->getLock1D()->setEnable( true );
  wheels[ LEFT_BACK ]->getSteeringHinge()->getLock1D()->setEnable( true );
 
  // Set the range for front wheels.
  RangeReal steeringRange( M_PI_4 );
  wheels[ RIGHT_FRONT ]->getSteeringHinge()->getRange1D()->setEnable( true );
  wheels[ RIGHT_FRONT ]->getSteeringHinge()->getRange1D()->setRange( steeringRange );
 
  wheels[ LEFT_FRONT ]->getSteeringHinge()->getRange1D()->setEnable( true );
  wheels[ LEFT_FRONT ]->getSteeringHinge()->getRange1D()->setRange( steeringRange );
 
  // Enable the motor for steering.
  wheels[ RIGHT_FRONT ]->getSteeringHinge()->getMotor1D()->setEnable( true );
  wheels[ LEFT_FRONT ]->getSteeringHinge()->getMotor1D()->setEnable( true );
  wheels[ RIGHT_FRONT ]->getSteeringHinge()->getMotor1D()->setForceRange( RangeReal( 1E3 ) );
  wheels[ LEFT_FRONT ]->getSteeringHinge()->getMotor1D()->setForceRange( RangeReal( 1E3 ) );
 
  // Material setup.
  MaterialRef groundMaterial = new Material( "groundMaterial" );
  ground->getGeometries().front().get()->setMaterial( groundMaterial );
  simulation->add( groundMaterial );
 
  MaterialRef backWheelMaterial = new Material( "backWheelMaterial" );
  MaterialRef frontWheelMaterial = new Material( "frontWheelMaterial" );
  simulation->add( backWheelMaterial );
  simulation->add( frontWheelMaterial );
  for ( int i = 0; i < 2; ++i ) {
    agxUtil::setBodyMaterial( wheels[ 0 + i ]->getWheel(), backWheelMaterial );
    agxUtil::setBodyMaterial( wheels[ 2 + i ]->getWheel(), frontWheelMaterial );
  }
 
  // Create contact materials.
  ContactMaterialRef backWheelGroundContact = new ContactMaterial( backWheelMaterial, groundMaterial );
  ContactMaterialRef frontWheelGroundContact = new ContactMaterial( frontWheelMaterial, groundMaterial );
  simulation->add( backWheelGroundContact );
  simulation->add( frontWheelGroundContact );
 
  backWheelGroundContact->setFrictionCoefficient( 2.0 );
  frontWheelGroundContact->setFrictionCoefficient( 1.8 );
 
  // How "deformable" the wheels are is controlled via Young's modulus.
  backWheelGroundContact->setYoungsModulus( 1E7 );
  frontWheelGroundContact->setYoungsModulus( 1E7 );
 
  // Set mass to the wheels. Let the wheels take some of the mass from the car body in order to get more
  // town force (or model the car in some other way).
  for ( int i = 0; i < 4; ++i )
    wheels[ i ]->getWheel()->getMassProperties()->setMass( 90 );
 
  // Maximum torque the motor can deliver.
  Real maxTorque = 4E2;
  // Add the car controller given the four wheels, max torque and keyboard controls.
  // 'a': forward (remember to switch gears!)
  // 'z': brake
  // 's': gear up
  // 'x': gear down
  // Left: turn left
  // Right: turn right
  simulation->add( new CarController( wheels[ RIGHT_BACK ], wheels[ LEFT_BACK ], wheels[ RIGHT_FRONT ], wheels[ LEFT_FRONT ], maxTorque,
                                      'a', 'z', 's', 'x', agxSDK::GuiEventListener::KEY_Left, agxSDK::GuiEventListener::KEY_Right ) );
 
  std::cerr << "\n\n";
  std::cerr << "Key binding for controlling the car: \n\n";
  std::cerr << "  \'a\': forward (remember to switch gears!)\n";
  std::cerr << "  \'z\': brake\n";
  std::cerr << "  \'s\': gear up\n";
  std::cerr << "  \'x\': gear down\n";
  std::cerr << "  Left: turn left\n";
  std::cerr << "  Right: turn right\n" << std::endl;
 
  // Debug rendering enabled to visualize the constraints.
  application->setEnableDebugRenderer( true );
 
 
  return root;
}
 
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout <<
                "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
                "\tCar tutorial\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorialCar, '1' );
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;               
    return 1;
  }
 
  return 0;
}

tutorial_carSteering.cpp

/*
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.
*/
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/utils.h>
 
#include <agxCollide/Cylinder.h>
#include <agxCollide/Plane.h>
#include <agxCollide/Box.h>
 
#include <agxVehicle/WheelJoint.h>
#include <agxVehicle/export.h>
#include <agxVehicle/Steering.h>
 
#include <agxUtil/agxUtil.h>
 
#include <agx/Logger.h>
#include <agx/version.h>
 
using namespace agx;
using namespace agxVehicle;
 
 
typedef agx::Vector<WheelJointRef> WheelJointRefVector;
typedef agx::Vector<RigidBodyRef> WheelRefVector;
 
class SteeringController : public agxSDK::StepEventListener
{
public:
  SteeringController(WheelJointRefVector& wheelJoints, SteeringRef steering, agxOSG::SceneDecorator* senceDecorator)
    : m_backRight(wheelJoints[3]), m_backLeft(wheelJoints[2]), m_frontRight(wheelJoints[1]), m_frontLeft(wheelJoints[0]), m_steering(steering),
    m_senceDecorator(senceDecorator)
  {
    // Set steering lock of the back wheels to be enabled.
    m_backLeft->getLock1D(WheelJoint::STEERING)->setEnable(true);
    m_backRight->getLock1D(WheelJoint::STEERING)->setEnable(true);
 
    // Set forward driving speeds on the four wheels.
    for (int i = 0; i < 4; ++i)
    {
      wheelJoints[i]->getMotor1D(WheelJoint::WHEEL)->setEnable(true);
      wheelJoints[i]->getMotor1D(WheelJoint::WHEEL)->setSpeed(5.0);
    }
 
    m_senceDecorator->setText(0, agx::String::format("Steering: %s", m_steering->getName().c_str()));
    setMask(PRE_STEP);
  }
 
  virtual void pre(const agx::TimeStamp& t) override
  {
    const Real maxSteeringAngle = 1.0;
    if (t < 1.0) 
    {
      m_steering->setSteeringAngle(0.0);
    }
    else if (t < 2.0) 
    {
      m_steering->setSteeringAngle(maxSteeringAngle * (t - 1.0));
    }
    else
    {
      m_steering->setSteeringAngle(maxSteeringAngle);
    }
    m_senceDecorator->setText(2, agx::String::format("Left-front wheel:    %.2f deg", radiansToDegrees(m_frontLeft->getAngle())));
    m_senceDecorator->setText(3, agx::String::format("Right-front wheel:   %.2f deg", radiansToDegrees(m_frontRight->getAngle())));
  }
 
  typedef ref_ptr< SteeringController > SteeringControllerRef;
 
private:
  WheelJointRef m_backRight;
  WheelJointRef m_backLeft;
  WheelJointRef m_frontRight;
  WheelJointRef m_frontLeft;
  SteeringRef m_steering;
  agxOSG::SceneDecorator* m_senceDecorator;
};
  
 
 
osg::Group* buildTutorialCar( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application, osg::Group* root, 
  WheelRefVector& wheels, WheelJointRefVector& wheelJoints)
{
  LOGGER_WARNING() << LOGGER_STATE( agx::Notify::PRINT_NONE ) << __FUNCTION__ << std::endl << LOGGER_END();
 
  enum eWheel { RIGHT_BACK = 0, LEFT_BACK, RIGHT_FRONT, LEFT_FRONT };
 
  // Create ground
  agxCollide::GeometryRef ground = new agxCollide::Geometry(new agxCollide::Plane(Vec3::Z_AXIS(), Vec3(0, 0, 0)));
  ground->setName("ground");
  agxOSG::createVisual(ground, root);
  simulation->add(ground);
 
  Vec3 carHalfLengths = Vec3(2.3, 0.8, 0.5);
  Real wheelRadius = 0.4;
 
  // Clearance between wheel and chassis.
  Real clearance = 0.2;
 
  // Create chassis body.
  RigidBodyRef chassisBody = new RigidBody();
  chassisBody->add(new agxCollide::Geometry(new agxCollide::Box(carHalfLengths)));
  agxOSG::setDiffuseColor(agxOSG::createVisual(chassisBody, root), agxRender::Color::DarkGoldenrod());
  simulation->add(chassisBody);
 
  chassisBody->setPosition(Vec3(0, 0, carHalfLengths.z() + wheelRadius));
 
  FrameRef chassiFrame = new Frame();
  chassiFrame = chassisBody->getFrame();
  Vec3 steeringAxle = chassiFrame->transformVectorToWorld(Vec3::Z_AXIS());
  Vec3 wheelAxle = chassiFrame->transformVectorToWorld(Vec3::Y_AXIS());
 
  agxOSG::createAxes(chassisBody, 0, root, 0.5f);
 
  chassisBody->getMassProperties()->setMass(1.5E3);
 
  // Create attachment frames for the wheels, i.e., where the constraints between the wheels and
  // the chassis body will be positioned.
  FrameRef rightBackWheelAttachment = new Frame();
  rightBackWheelAttachment->setLocalTranslate(Vec3(-carHalfLengths.x() + wheelRadius, -carHalfLengths.y() - clearance, -carHalfLengths.z()));
 
  FrameRef leftBackWheelAttachment = new Frame();
  leftBackWheelAttachment->setLocalTranslate(Vec3(-carHalfLengths.x() + wheelRadius, carHalfLengths.y() + clearance, -carHalfLengths.z()));
 
  FrameRef rightFrontWheelAttachment = new Frame();
  rightFrontWheelAttachment->setLocalTranslate(Vec3(carHalfLengths.x() - wheelRadius, -carHalfLengths.y() - clearance, -carHalfLengths.z()));
 
  FrameRef leftFrontWheelAttachment = new Frame();
  leftFrontWheelAttachment->setLocalTranslate(Vec3(carHalfLengths.x() - wheelRadius, carHalfLengths.y() + clearance, -carHalfLengths.z()));
 
  chassisBody->addAttachment(rightBackWheelAttachment, "rightBack");
  chassisBody->addAttachment(leftBackWheelAttachment, "leftBack");
  chassisBody->addAttachment(rightFrontWheelAttachment, "rightFront");
  chassisBody->addAttachment(leftFrontWheelAttachment, "leftFront");
 
  // Create the wheels - given car body, the attachment name, radius, axis of the suspension,
  //                     axis of driving, root node for rendering and the shape type described above.
  wheels[RIGHT_BACK] = new RigidBody();
  wheels[RIGHT_BACK]->add(new agxCollide::Geometry(new agxCollide::Cylinder(0.4, 0.2)));
  wheels[RIGHT_BACK]->setPosition(rightBackWheelAttachment->getTranslate());
 
  wheels[LEFT_BACK] = new RigidBody();
  wheels[LEFT_BACK]->add(new agxCollide::Geometry(new agxCollide::Cylinder(0.4, 0.2)));
  wheels[LEFT_BACK]->setPosition(leftBackWheelAttachment->getTranslate());
 
  wheels[RIGHT_FRONT] = new RigidBody();
  wheels[RIGHT_FRONT]->add(new agxCollide::Geometry(new agxCollide::Cylinder(0.4, 0.2)));
  wheels[RIGHT_FRONT]->setPosition(rightFrontWheelAttachment->getTranslate());
 
  wheels[LEFT_FRONT] = new RigidBody();
  wheels[LEFT_FRONT]->add(new agxCollide::Geometry(new agxCollide::Cylinder(0.4, 0.2)));
  wheels[LEFT_FRONT]->setPosition(leftFrontWheelAttachment->getTranslate());
 
  // Add the wheels to simulation and disable collisions
  // between the car body geometries and the wheel geometries.
  for ( int i = 0; i < 4; ++i )
  {
    agxOSG::setDiffuseColor(agxOSG::createVisual(wheels[i], root), agxRender::Color::Wheat());
    simulation->add(wheels[i]);
    agxUtil::setEnableCollisions(chassisBody, wheels[i], false);
  }
 
  // Create the four wheelJoints.
  Vec3 rightBackPointWorld = wheels[RIGHT_BACK]->getPosition();
  WheelJointFrame rightBackWheelJointFrame = WheelJointFrame(rightBackPointWorld, wheelAxle, steeringAxle);
  WheelJointRef rightBackWheelJoint = new WheelJoint(rightBackWheelJointFrame, wheels[RIGHT_BACK], chassisBody);
 
  Vec3 leftBackPointWorld = wheels[LEFT_BACK]->getPosition();
  WheelJointFrame leftBackWheelJointFrame = WheelJointFrame(leftBackPointWorld, wheelAxle, steeringAxle);
  WheelJointRef leftBackWheelJoint = new WheelJoint(leftBackWheelJointFrame, wheels[LEFT_BACK], chassisBody);
 
  Vec3 leftFrontPointWorld = wheels[LEFT_FRONT]->getPosition();
  WheelJointFrame leftFrontWheelJointFrame = WheelJointFrame(leftFrontPointWorld, wheelAxle, steeringAxle);
  WheelJointRef leftFrontWheelJoint = new WheelJoint(leftFrontWheelJointFrame, wheels[LEFT_FRONT], chassisBody);
 
  Vec3 rightFrontPointWorld = wheels[RIGHT_FRONT]->getPosition();
  WheelJointFrame rightFrontWheelJointFrame = WheelJointFrame(rightFrontPointWorld, wheelAxle, steeringAxle);
  WheelJointRef rightFrontWheelJoint = new WheelJoint(rightFrontWheelJointFrame, wheels[RIGHT_FRONT], chassisBody);
 
  simulation->add(rightBackWheelJoint);
  simulation->add(leftBackWheelJoint);
  simulation->add(rightFrontWheelJoint);
  simulation->add(leftFrontWheelJoint);
 
  wheelJoints[0] = leftFrontWheelJoint;
  wheelJoints[1] = rightFrontWheelJoint;
  wheelJoints[2] = leftBackWheelJoint;
  wheelJoints[3] = rightBackWheelJoint;
 
  // Material setup.
  MaterialRef groundMaterial = new Material( "groundMaterial" );
  ground->setMaterial( groundMaterial );
  simulation->add( groundMaterial );
 
  MaterialRef backWheelMaterial = new Material( "backWheelMaterial" );
  MaterialRef frontWheelMaterial = new Material( "frontWheelMaterial" );
  simulation->add( backWheelMaterial );
  simulation->add( frontWheelMaterial );
 
  for ( int i = 0; i < 2; ++i ) 
  {
    agxUtil::setBodyMaterial(wheels[0 + i], backWheelMaterial);
    agxUtil::setBodyMaterial(wheels[2 + i], frontWheelMaterial);
  }
 
  // Create contact materials.
  ContactMaterialRef backWheelGroundContact = new ContactMaterial(backWheelMaterial, groundMaterial);
  ContactMaterialRef frontWheelGroundContact = new ContactMaterial(frontWheelMaterial, groundMaterial);
  simulation->add(backWheelGroundContact);
  simulation->add(frontWheelGroundContact);
 
  backWheelGroundContact->setFrictionCoefficient(2.0);
  frontWheelGroundContact->setFrictionCoefficient(1.8);
 
  // How "deformable" the wheels are is controlled via Young's modulus.
  backWheelGroundContact->setYoungsModulus(1E7);
  frontWheelGroundContact->setYoungsModulus(1E7);
 
  // Set mass to the wheels. 
  for (int i = 0; i < 4; ++i)
    wheels[i]->getMassProperties()->setMass(90);
 
  // Debug rendering enabled to visualize the constraints.
  application->setEnableDebugRenderer(true);
 
  return root;
}
 
 
 
osg::Group* buildAckermannSteer(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  LOGGER_WARNING() << LOGGER_STATE(agx::Notify::PRINT_NONE) << __FUNCTION__ << std::endl << LOGGER_END();
 
  osg::Group* root = new osg::Group;
 
  WheelJointRefVector wheelJoints;
  wheelJoints.resize(4);
 
  WheelRefVector wheels;
  wheels.resize(4);
  buildTutorialCar(simulation, application, root, wheels, wheelJoints);
 
  SteeringRef ackermann = new Ackermann(wheelJoints[0], wheelJoints[1]);
  ackermann->setName("Ackermann");
  simulation->add(ackermann);
 
  auto sd = application->getSceneDecorator();
  simulation->add(new SteeringController(wheelJoints, ackermann, sd));
 
  return root;
}
 
 
osg::Group* buildRackPinionSteer(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  LOGGER_WARNING() << LOGGER_STATE(agx::Notify::PRINT_NONE) << __FUNCTION__ << std::endl << LOGGER_END();
 
  osg::Group* root = new osg::Group;
 
  WheelJointRefVector wheelJoints;
  wheelJoints.resize(4);
 
  WheelRefVector wheels;
  wheels.resize(4);
  buildTutorialCar(simulation, application, root, wheels, wheelJoints);
 
  SteeringRef rackPinion = new RackPinion(wheelJoints[0], wheelJoints[1]);
  rackPinion->setName("RackPinion");
  simulation->add(rackPinion);
 
  auto sd = application->getSceneDecorator();
  simulation->add(new SteeringController(wheelJoints, rackPinion, sd));
  
  return root;
}
 
 
 
osg::Group* buildBellCrankSteer(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  LOGGER_WARNING() << LOGGER_STATE(agx::Notify::PRINT_NONE) << __FUNCTION__ << std::endl << LOGGER_END();
 
  osg::Group* root = new osg::Group;
 
  WheelJointRefVector wheelJoints;
  wheelJoints.resize(4);
 
  WheelRefVector wheels;
  wheels.resize(4);
 
  buildTutorialCar(simulation, application, root, wheels, wheelJoints);
 
  SteeringRef bellCrank = new BellCrank(wheelJoints[0], wheelJoints[1]);
  bellCrank->setName("BellCrank");
  simulation->add(bellCrank);
 
  auto sd = application->getSceneDecorator();
  simulation->add(new SteeringController(wheelJoints, bellCrank, sd));
 
  return root;
}
 
 
osg::Group* buildDavisSteer(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  LOGGER_WARNING() << LOGGER_STATE(agx::Notify::PRINT_NONE) << __FUNCTION__ << std::endl << LOGGER_END();
 
  osg::Group* root = new osg::Group;
 
  WheelJointRefVector wheelJoints;
  wheelJoints.resize(4);
 
  WheelRefVector wheels;
  wheels.resize(4);
 
  buildTutorialCar(simulation, application, root, wheels, wheelJoints);
 
  SteeringRef davis = new Davis(wheelJoints[0], wheelJoints[1]);
  davis->setName("Davis");
  simulation->add(davis);
 
  auto sd = application->getSceneDecorator();
  simulation->add(new SteeringController(wheelJoints, davis, sd));
 
  return root;
}
 
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout <<
                "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
                "\tTutorial: car steering \n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene(buildAckermannSteer, '1');
    application->addScene(buildBellCrankSteer, '2');
    application->addScene(buildRackPinionSteer, '3');
    application->addScene(buildDavisSteer, '4');
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;               
    return 1;
  }
 
  return 0;
}

tutorial_centerOfBuoyancy.cpp

/*
Copyright 2007-2025. Algoryx Simulation AB.
 
All AGX source code, intellectual property, documentation, sample code,
tutorials, scene files and technical white papers, is 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.
*/
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/utils.h>
#include <agxUtil/TrimeshReaderWriter/TrimeshReaderWriter.h>
#include <agxModel/WindAndWaterController.h>
 
#include "tutorialUtils.h"
 
namespace
{
  agxOSG::GeometryNode* createWaterVisual(agxCollide::Geometry* waterGeometry, osg::Group* root)
  {
    agxOSG::GeometryNode* node = agxOSG::createVisual(waterGeometry, root);
 
    const agx::Vec4f color = agxRender::Color::DeepSkyBlue();
    float alpha(0.3f);
 
    agxOSG::setDiffuseColor(node, color);
    agxOSG::setAmbientColor(node, agx::Vec4f(0.2f));
    agxOSG::setSpecularColor(node, agx::Vec4f(0.4f));
    agxOSG::setShininess(node, 120.0f);
    agxOSG::setAlpha(node, alpha);
 
    return node;
  }
 
}
 
 
 
class WaveWaterWrapper : public agxModel::WaterWrapper
{
public:
  WaveWaterWrapper()
    : agxModel::WaterWrapper() {}
 
  agx::Real findHeightFromSurface(const agx::Vec3& worldPoint, const agx::Vec3& /*upVector*/, const agx::TimeStamp& t) const override
  {
    agx::Real surfaceLevel = -agx::Real(2) + agx::Real(0.5) * std::sin(agx::Real(0.5) * worldPoint.x() + agx::Real(0.6) * t) +
      agx::Real(0.25) * std::cos(agx::Real(0.6) * worldPoint.y() + agx::Real(0.3) * worldPoint.x() + agx::Real(1.45) * t);
 
    return worldPoint.z() - surfaceLevel;
  }
};
 
 
 
class CenterOfBuoyancyListener : public agxSDK::StepEventListener
{
public:
  CenterOfBuoyancyListener(
    agxCollide::Geometry* refGeom, 
    agxCollide::Geometry* visualGeom, 
    agxModel::WindAndWaterControllerRef waterController) 
    : m_refGeom(refGeom), m_visualGeom(visualGeom), m_waterController(waterController)
  {
  }
 
  virtual void pre(agx::TimeStamp const& /*time*/) override
  {
    // Get center of buoyancy for the given geometry. The value of the Vec3 will be overwritten
    // if there is a value to set for center of buoyancy.
    agx::Vec3 cob;
    m_waterController->getCenterOfBuoyancy(m_refGeom, cob);
    // Move our visual body to that position
    m_visualGeom->setPosition(cob);
  }
 
private:
  agxCollide::Geometry* m_refGeom;
  agxCollide::Geometry* m_visualGeom;
  agxModel::WindAndWaterControllerRef m_waterController;
};
 
 
 
osg::Group* buildTutorial1(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  // First we must set up a scene with water and with something in the water.
  osg::Group* root = new osg::Group();
  // Create two water geometries of size 50x50x5 meters.
  agxCollide::GeometryRef waterGeometry = new agxCollide::Geometry(new agxCollide::Box(50, 50, 5));
  agx::Vec3 position(0, 0, -5);
  waterGeometry->setPosition(position);
  createWaterVisual(waterGeometry, root);
 
  // Create a wind- and watercontroller and add the water geometry and a water wrapper
  agxModel::WindAndWaterControllerRef controller = new agxModel::WindAndWaterController();
  controller->addWater(waterGeometry);
  controller->setWaterWrapper(waterGeometry, new WaveWaterWrapper());
 
  simulation->add(controller);
  simulation->add(waterGeometry);
 
  // We create a sphere to drop into the water
  agx::MaterialRef sphereMaterial = new agx::Material("sphereMaterial");
  sphereMaterial->getBulkMaterial()->setDensity(600);
  agxCollide::GeometryRef sphereGeometry = new agxCollide::Geometry(new agxCollide::Sphere(1));
  agx::RigidBodyRef sphere = new agx::RigidBody(sphereGeometry);
  agxUtil::setBodyMaterial(sphere, sphereMaterial);
  simulation->add(sphere);
 
  // The center of buoyancy calculations are off by default, so first we have to enable them
  // for our geometry.
  controller->setEnableCenterOfBuoyancy(sphereGeometry, true);
  
  // To get the center of buoyancy, you do: 
  agx::Vec3 centerOfBuoyancy;
  controller->getCenterOfBuoyancy(sphereGeometry, centerOfBuoyancy);
  // The Vec3 we send into getCenterOfBuoyancy will be overwritten with the center of buoyancy
  // position, if it is possible to set it. If the geometry is not in the water, there is no
  // value for center of buoyancy to set, and the Vec3 will remain unchanged. getCenterOfBuoyancy
  // returns true when it has set the center of buoyancy, and false otherwise.
  // In this case our geometry starts outside the water, so we have no center of buoyancy yet.
 
  // To be able to visualize where the center of buoyancy is in each time step, we create a small
  // sphere that we position with a step event listener that reads the center of buoyancy value
  // at each time step.
  agxCollide::GeometryRef cobGeometry = new agxCollide::Geometry(new agxCollide::Sphere(0.05));
  cobGeometry->setPosition(sphere->getPosition());
  simulation->add(cobGeometry);
  cobGeometry->setEnableCollisions(false);
  // Create and add the listener to the simulaiton
  simulation->add(new CenterOfBuoyancyListener(sphereGeometry, cobGeometry, controller));
 
  // Enable debug rendering and move the camera a bit
  application->setEnableDebugRenderer(true);
  agxOSG::CameraData cameraData = application->getCameraData();
  cameraData.eye = agx::Vec3(-1.25, -25, 0);
  cameraData.center = agx::Vec3(-5, 15, -7);
  cameraData.up = agx::Vec3(0, 0, 1);
  application->applyCameraData(cameraData);
 
  return root;
}
 
 
osg::Group* buildTutorial2(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  // First we must set up a scene with water and with something in the water.
  osg::Group* root = new osg::Group();
  agxCollide::GeometryRef waterGeometry = new agxCollide::Geometry(new agxCollide::Box(100, 100, 20));
  agx::Vec3 position(0, 0, -5);
  waterGeometry->setPosition(position);
  ::createWaterVisual(waterGeometry, root);
 
  // Create a wind- and watercontroller and add the water geometry and a water wrapper
  agxModel::WindAndWaterControllerRef controller = new agxModel::WindAndWaterController();
  controller->addWater(waterGeometry);
  controller->setWaterWrapper(waterGeometry, new WaveWaterWrapper());
  simulation->add(controller);
  simulation->add(waterGeometry);
 
  // We use the mesh of a ships hull to see what the center of buoyancy is for a more complex shape
  agx::MaterialRef shipMaterial = new agx::Material("shipMaterial");
  shipMaterial->getBulkMaterial()->setDensity(600);
  // We scale the ship down a bit, and rotate it to be correctly oriented
  auto rotateAndScale = agx::Matrix3x3(agx::EulerAngles(agx::PI_2, 0, 0)) * agx::Matrix3x3(agx::Vec3(0.1, 0.1, 0.1));
  // Read the obj-file for the ship
  agxCollide::TrimeshRef shipTrimesh = agxUtil::TrimeshReaderWriter::createTrimesh(
    "models/shipHull.obj", agxCollide::Trimesh::REMOVE_DUPLICATE_VERTICES, rotateAndScale);
  agxCollide::GeometryRef shipGeometry = new agxCollide::Geometry(shipTrimesh);
  agx::RigidBodyRef ship = new agx::RigidBody(shipGeometry);
  agxUtil::setBodyMaterial(ship, shipMaterial);
  simulation->add(ship);
 
  // The center of buoyancy calculations are off by default, so first we have to enable them
  // for our geometry.
  controller->setEnableCenterOfBuoyancy(shipGeometry, true);
 
  // Next we create a sphere to just visualize the center of buoyancy
  agxCollide::GeometryRef cobGeometry = new agxCollide::Geometry(new agxCollide::Sphere(0.1));
  cobGeometry->setPosition(ship->getPosition());
  simulation->add(cobGeometry);
  cobGeometry->setEnableCollisions(false);
  // We add a listener that moves the sphere each timestep, to the position of center of buoyancy.
  simulation->add(new CenterOfBuoyancyListener(shipGeometry, cobGeometry, controller));
 
  // Enable debug rendering and move the camera a bit
  application->setEnableDebugRenderer(true);
  agxOSG::CameraData cameraData = application->getCameraData();
  cameraData.eye = agx::Vec3(14, -50, 2);
  cameraData.center = agx::Vec3(-13, 15, -6);
  cameraData.up = agx::Vec3(0, 0, 1);
  application->applyCameraData(cameraData);
 
  return root;
}
 
 
 
int main(int argc, char** argv)
{
  agx::AutoInit agxInit;
  std::cout << "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial hydrodynamics\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene(buildTutorial1, '1');
    application->addScene(buildTutorial2, '2');
 
    if (application->init(argc, argv))
      return application->run();
  }
  catch (std::exception& e) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_collisionDetection.cpp

/*
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.
*/
 
 
/*
 
Demonstrates triangle mesh collision detection.
 
*/
 
#include <agx/Logger.h>
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/Node.h>
#include <agxOSG/utils.h>
 
#include <agx/RigidBody.h>
#include <agxCollide/Box.h>
#include <agxCollide/Trimesh.h>
#include <agxUtil/TrimeshReaderWriter/TrimeshReaderWriter.h>
 
#include <agxRender/RenderManager.h>
#include <iomanip>
#include <agx/version.h>
#include <agx/Hinge.h>
 
AGX_DECLARE_POINTER_TYPES(VelocityPrinter);
class VelocityPrinter : public agxSDK::StepEventListener
{
public:
  VelocityPrinter(agxOSG::ExampleApplication* app, int textStartRow = 0) :
    agxSDK::StepEventListener(agxSDK::StepEventListener::POST_STEP),
    m_app(app), m_textStartRow(textStartRow)
  {
    agxAssert(m_textStartRow >= 0);
  }
 
  // Override from StepEventListener. Called each time step at post (after solver/integrator).
  virtual void post(const agx::TimeStamp&) override
  {
    if (m_app && m_app->getSceneDecorator()) { // Observer pointer might point to deallocated app.      
      int textRowCount = 0;
      const int w = 7; // Width.
      for (size_t i = 0; i < m_bodies.size(); ++i) {
        if (m_bodies[i].isValid()) { // Observer pointer might point to deallocated body.
          agx::Vec3 v = m_bodies[i]->getVelocity();
          agx::Vec3 omega = m_bodies[i]->getAngularVelocity();
          std::stringstream ss;
          ss.precision(3);
          ss << std::fixed << "Body " << std::setw(2) << i << ": " << m_bodies[i]->getName()
            << ": Velocity: (" << std::setw(w) << v.x() << std::setw(w) << v.y() << std::setw(w) << v.z() << ") m/s"
            << "; Angular velocity : (" << std::setw(w) << omega.x() << std::setw(w) << omega.y() << std::setw(w) << omega.z() << ") rad/s";
          m_app->getSceneDecorator()->setText(textRowCount + m_textStartRow, ss.str());
          textRowCount++;
        }
      }
    }
  }
 
    // Add a body to be printed.
    void addBody(agx::RigidBodyRef body)
  {
    m_bodies.push_back(body);
  }
 
protected:
  virtual ~VelocityPrinter() {}
 
protected:
  agx::observer_ptr<agxOSG::ExampleApplication> m_app;
  agx::RigidBodyObserverVector m_bodies;
  int m_textStartRow;
};
 
 
osg::Group* buildTutorial1(agxSDK::Simulation* sim, agxOSG::ExampleApplication* app)
{
  osg::Group* root = new osg::Group();
 
  uint32_t defaultMeshFlag = agxCollide::Trimesh::REMOVE_DUPLICATE_VERTICES; // Merges duplicates. Not needed here, but good practice.
 
  agx::MaterialRef material = new agx::Material("GearMaterial");
  auto contactMaterial = sim->getMaterialManager()->getOrCreateContactMaterial(material, material);
  contactMaterial->setRestitution(0);
 
  // Load a gear from an obj-file.
  const agx::Matrix3x3 gearScaling = agx::Matrix3x3(agx::Vec3(0.02)); // Scale mesh in-data.
  agxCollide::TrimeshRef gearMesh = agxUtil::TrimeshReaderWriter::createTrimesh("data/models/gear1.obj", defaultMeshFlag, gearScaling);
  assert(gearMesh);
  auto gearGeometry = new agxCollide::Geometry(gearMesh);
  gearGeometry->setMaterial(material);
 
  agx::RigidBodyRef gear = new agx::RigidBody(gearGeometry);
  gear->setName("gear");
  sim->add(gear);
  agxOSG::createVisual(gear, root);
  gear->setPosition(0.1, 0, 0.4); // Move gear above bowl.
 
  // Load a bowl from an obj-file.
  agx::Matrix3x3 bowlScaling = agx::Matrix3x3(agx::Vec3(0.1)); // Scale mesh in-data.
  agxCollide::TrimeshRef bowlMesh = agxUtil::TrimeshReaderWriter::createTrimesh("data/models/bowl.obj", defaultMeshFlag, bowlScaling);
  assert(bowlMesh);
  auto bowlGeometry = new agxCollide::Geometry(bowlMesh);
  bowlGeometry->setMaterial(material);
 
  agx::RigidBodyRef bowl = new agx::RigidBody(bowlGeometry);
  bowl->setName("bowl");
  sim->add(bowl);
  agxOSG::createVisual(bowl, root);
  bowl->setPosition(0, 0, -0.1);
  
  // Let bowl be kinematic, and rotate with constant speed.
  bowl->setMotionControl(agx::RigidBody::KINEMATICS); 
  bowl->setAngularVelocity(0, 0, 0.1);
 
  // Scene description.
  app->getSceneDecorator()->setText(0, "Gear resting in bowl. Press 'g' to toggle debug rendering, 'G' to toggle OSG rendering.");
 
  // A velocity printer for the two bodies.
  VelocityPrinterRef velocityPrinter = new VelocityPrinter(app, 2);
  sim->add(velocityPrinter);
  velocityPrinter->addBody(gear);
  velocityPrinter->addBody(bowl);
  velocityPrinter->post(0); // Printout also before taking first time-step.
 
  // Better debug view for contacts.
  app->setEnableDebugRenderer(true); // Toggle by pressing 'g'.
  app->setEnableOSGRenderer(false); // Toggle by pressing 'G'.  
  // All of these are toggled by pressing cursor-left or cursor-right.
  app->getSimulation()->getRenderManager()->disableFlags(agxRender::RENDER_BATCH_CONTACTS); 
  app->getSimulation()->getRenderManager()->disableFlags(agxRender::RENDER_BATCH_BODIES);
  app->getSimulation()->getRenderManager()->disableFlags(agxRender::RENDER_BATCH_WIRES);
 
  // Some nice initial camera setting (looking at the scene from slightly above).
  agx::Vec3 eye = agx::Vec3(-3.0832610934950855E-001, -1.5247185050586647E+000, 1.0936281422567065E+000);
  agx::Vec3 center = agx::Vec3(3.1770658032579266E-002, -8.2853946902834041E-002, 1.7500546587327595E-001);
  agx::Vec3 up = agx::Vec3(1.0626159313065037E-001, 5.1633116377248522E-001, 8.4977091215379164E-001);
  app->setCameraHome(eye, center, up);
 
  return root;
}
 
 
osg::Group* buildTutorial2(agxSDK::Simulation* sim, agxOSG::ExampleApplication* app)
{
  osg::Group* root = new osg::Group();
 
  uint32_t defaultMeshFlag = agxCollide::Trimesh::REMOVE_DUPLICATE_VERTICES; // Merges duplicates. Not needed here, but good practice.
 
  // Load a gear from an obj-file.
  const agx::Matrix3x3 gearScaling = agx::Matrix3x3(agx::Vec3(0.02)); // Scale mesh in-data.
  agxCollide::TrimeshRef gearMesh = agxUtil::TrimeshReaderWriter::createTrimesh("data/models/gear1.obj", defaultMeshFlag, gearScaling);
  // We cannot use the same agxCollide::Trimesh in several agxCollide::Geometries 
  // (and cannot use the same agxCollide::Geometry in several agx::RigidBodies).
  // However, we can make a shallow copy, so that the trimeshes share their mesh data.
  agxCollide::TrimeshRef gearMesh2 = gearMesh->shallowCopy();
 
  agx::RigidBodyRef rb1 = new agx::RigidBody(new agxCollide::Geometry(gearMesh));
  agx::RigidBodyRef rb2 = new agx::RigidBody(new agxCollide::Geometry(gearMesh2));
 
  sim->add(rb1);
  sim->add(rb2);
 
  agxOSG::createVisual(rb1, root);
  agxOSG::createVisual(rb2, root);
 
  rb2->setPosition(0.53, 0, 0);
  rb2->setRotation(agx::EulerAngles(agx::PI_2, 0.2, 0));
  rb1->setRotation(agx::EulerAngles(agx::PI_2, 0.2, 0));
 
 
  // Attach the two bodies each at a hinge in the world.
  agx::HingeRef hinge1 = new agx::Hinge(rb1, new agx::Frame(agx::AffineMatrix4x4()));
  agx::HingeRef hinge2 = new agx::Hinge(rb2, new agx::Frame(agx::AffineMatrix4x4()));
  sim->add(hinge1);
  sim->add(hinge2);
 
  // Give one hinge a motor with simple controller with a target speed.
  hinge1->getMotor1D()->setEnable(true);
  hinge1->getMotor1D()->setSpeed(0.2);
 
  // Scene description.
  app->getSceneDecorator()->setText(0, "One gear driven by a motor, the other by collisions.");
  app->getSceneDecorator()->setText(1, "Note the difference in angular velocity due to non-perfect gear arrangement - this kind ");
  app->getSceneDecorator()->setText(2, "of behavior can only be simulated with collisions.");
  app->getSceneDecorator()->setText(3, "");
  app->getSceneDecorator()->setText(4, "Press 'g' to toggle debug rendering, 'G' to toggle OSG rendering.");
 
  // A velocity printer for the two bodies.
  VelocityPrinterRef velocityPrinter = new VelocityPrinter(app, 5);
  sim->add(velocityPrinter);
  velocityPrinter->addBody(rb1);
  velocityPrinter->addBody(rb2);
  velocityPrinter->post(0); // Printout also before taking first time-step.
 
  // Some nice initial camera setting (looking at the scene from slightly above).
  agx::Vec3 eye = agx::Vec3(2.7230274677276611E-001, -2.3059253137839035E+000, 1.7681157588958740E-001);
  agx::Vec3 center = agx::Vec3(2.7230274677276611E-001, -7.3146820068359375E-003, 1.7681157588958740E-001);
  agx::Vec3 up = agx::Vec3(0.0000000000000000E+000, 0.0000000000000000E+000, 1.0000000000000000E+000);
  app->setCameraHome(eye, center, up);
 
  return root;
}
 
 
int main(int argc, char** argv)
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tCollision detection tutorial\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene(buildTutorial1, '1');
    application->addScene(buildTutorial2, '2');
 
    if (application->init(argc, argv))
      return application->run();
  }
  catch (std::exception& e) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_constraints.cpp

/*
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.
*/
 
/*
 
This tutorial demonstrates how to create and use various constraints available in AGX: Distance, BallJoint, CylindricalJoint, Hinge, Prismatic, LockJoint
 
*/
 
#include <agxOSG/ExampleApplication.h>
 
#include <agx/version.h>
#include <agx/DistanceJoint.h>
#include <agx/Hinge.h>
#include <agx/Prismatic.h>
#include <agx/CylindricalJoint.h>
#include <agx/LockJoint.h>
#include <agx/BallJoint.h>
#include <agx/UniversalJoint.h>
#include <agx/PrismaticUniversalJoint.h>
 
#include <agxUtil/agxUtil.h>
 
#include <agxUtil/TrimeshReaderWriter/TrimeshReaderWriter.h>
 
#include <agxSDK/GeometryFilter.h>
 
#include "tutorialUtils.h"
 
 
#ifdef __clang__
# pragma clang diagnostic ignored "-Winvalid-source-encoding"
#endif
 
 
// Utility function to create default scene
typedef std::pair< agx::RigidBody*, agx::RigidBody* > RigidBodyPtrPair;
 
RigidBodyPtrPair createDefaultScene( const agx::Vec3& posRB1, const agx::Vec3& posRB2, agxSDK::Simulation* simulation, osg::Group* root, bool createGroundBox = true )
{
  if ( createGroundBox ) {
    // Create a static box geometry as in tutorial 1.4.
    agx::Vec3 boxGeometryHalfExtent( 15, 15, 0.5 );
    agxCollide::GeometryRef boxGeometry = new agxCollide::Geometry( new agxCollide::Box( boxGeometryHalfExtent ) );
    boxGeometry->setPosition( boxGeometry->getPosition() - agx::Vec3( 0, 0, boxGeometryHalfExtent.z() ) );
    simulation->add( boxGeometry );
    boxGeometry->setName( "boxGeometry" );
    agxOSG::createVisual( boxGeometry, root );
  }
 
  agx::Vec3 boxHalfExtent( 1, 1, 1 );
  agx::RigidBodyRef rb1 = new agx::RigidBody();
  agx::RigidBodyRef rb2 = new agx::RigidBody();
  rb1->add( new agxCollide::Geometry( new agxCollide::Box( boxHalfExtent ) ) );
  rb2->add( new agxCollide::Geometry( new agxCollide::Box( boxHalfExtent ) ) );
  simulation->add( rb1 );
  simulation->add( rb2 );
  agxOSG::createVisual( rb1, root );
  agxOSG::createVisual( rb2, root );
 
  rb1->setPosition( posRB1 );
  rb2->setPosition( posRB2 );
 
  return std::make_pair( rb1, rb2 );
}
 
osg::Group* buildTutorial1( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  RigidBodyPtrPair rbPair = createDefaultScene( agx::Vec3( -4, 2, 7 ), agx::Vec3( 0, 2, 7 ), simulation, root );
  agx::RigidBodyRef rb1 = rbPair.first;
  agx::RigidBodyRef rb2 = rbPair.second;
 
  //
  //    RB1----distance constraint----RB2
  //
  //  _____________ground box______________
 
  // There are several ways to create a distance joint/constraint.
 
  // First, distance constraint between rb1 and rb2, attached at (1, 0, 0) on rb1 (local frame) and (0, 0, -1) on rb2 (local frame).
  agx::DistanceJointRef d1 = new agx::DistanceJoint( rb1, agx::Vec3( 1, 0, 0 ), rb2, agx::Vec3( 0, 0, -1 ) );
  // Add the distance joint to the simulation.
  simulation->add( d1 );
  // Make rb2 static.
  rb2->setMotionControl( agx::RigidBody::STATIC );
 
  // The distance joint is built of the constraint controllers seen in for example hinge and prismatic.
  // This gives you a wide range of applications for this joint. Note that the Lock1D is enabled
  // by default. Examples:
  //
  // 1. The distance joint may be shorter than its rest length, but not longer:
  d1->getLock1D()->setForceRange( -agx::Infinity, 0 );
 
  // 2. Use the range to describe a distance joint that is "flexible" between -0.5 to 0.5 of the defined
  //    rest length.
  // Disable the lock when we want to use the range instead.
  //d1->getLock1D()->setEnable( false );
  //d1->getRange1D()->setEnable( true );
  //agx::Real d1RestLength = rb1->getFrame()->transformPointToWorld( agx::Vec3( 1, 0, 0 ) ).distance( rb2->getFrame()->transformPointToWorld( agx::Vec3( 0, 0, -1 ) ) );
  //d1->getRange1D()->setRange( d1RestLength - agx::Real( 0.5 ), d1RestLength + agx::Real( 0.5 ) );
 
  // 3. Use the motor to drive the distance joint.
  //d1->getLock1D()->setEnable( false );
  //d1->getMotor1D()->setEnable( true );
  //d1->getMotor1D()->setSpeed( agx::Real( -0.5 ) );
  //d1->getMotor1D()->setForceRange( -1E5, 1E5 );
 
  // Create two more rigid bodies.
  rbPair = createDefaultScene( agx::Vec3( -4, -2, 7 ), agx::Vec3( 0, -2, 7 ), simulation, root, false );
  agx::RigidBodyRef rb3 = rbPair.first;
  agx::RigidBodyRef rb4 = rbPair.second;
 
  // A rigid body can have attachments frames. It's possible to attach constraints to a given attachment frame.
  // Create attachment frames, position them as the local attachment points in d1.
  agx::FrameRef rb3AttFrame = new agx::Frame();
  agx::FrameRef rb4AttFrame = new agx::Frame();
 
  // Set local translate of the frames - same values as in d1 (i.e., this is in body coordinate frame)
  rb3AttFrame->setLocalTranslate( agx::Vec3( 1, 0, 0 ) );
  rb4AttFrame->setLocalTranslate( agx::Vec3( 0, 0, -1 ) );
 
  // Add the frames to the bodies.
  rb3->addAttachment( rb3AttFrame, "DistanceJointAttachment" );
  rb4->addAttachment( rb4AttFrame, "DistanceJointAttachment" );
 
  // Create the distance joint given these attachment frames.
  agx::DistanceJointRef d2 = new agx::DistanceJoint( rb3, rb3AttFrame, rb4, rb4AttFrame );
  // Equivalent expression:
  // agx::DistanceJointRef d2 = new agx::DistanceJoint( rb3, rb3->getAttachment( "DistanceJointAttachment" ), rb4, rb4->getAttachment( "DistanceJointAttachment" ) );
 
  // Add the distance joint to the simulation.
  simulation->add( d2 );
 
  // Instead of making rb4 static, attach it to the world with a distance joint.
  // From top of rb4 local(0,0,1) to world(0,-2,10) in world.
  agx::DistanceJointRef d3 = new agx::DistanceJoint( rb4, agx::Vec3( 0, 0, 1 ), agx::Vec3( 0, -2, 10 ) );
  simulation->add( d3 );
 
  // Debug rendering enabled to visualize the constraints.
  application->setEnableDebugRenderer( true );
 
  return root;
}
 
osg::Group* buildTutorial2( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  RigidBodyPtrPair rbPair = createDefaultScene( agx::Vec3( -4, 0, 7 ), agx::Vec3( 0, 0, 7 ), simulation, root );
  agx::RigidBodyRef rb1 = rbPair.first;
  agx::RigidBodyRef rb2 = rbPair.second;
 
  // Create a hinge, given a hinge frame (world coordinates).
  // First, create a hinge frame.
  agx::HingeFrame hf1;
  // The center of a hinge is the point (in world coordinates) where the axis is located.
  // We want to attach rb2 to the world.
  hf1.setCenter( rb2->getPosition() + rb2->getFrame()->transformVectorToWorld( agx::Vec3( 1, 0, 0 ) ) );
  // Let the axis be the world y axis (into the screen).
  hf1.setAxis( agx::Vec3( 0, 1, 0 ) );
 
  agx::HingeRef h1 = new agx::Hinge( hf1, rb2 );
  simulation->add( h1 );
 
  // Now rb2 is attached to the world and may rotate around world y.
 
  // Create a hinge between rb1 and rb2 with attachment frames.
  // Create the attachment frames.
  agx::FrameRef rb1AttFrame = new agx::Frame();
  agx::FrameRef rb2AttFrame = new agx::Frame();
 
  // When using frames, the axis is defined to be along the z axis of the attachment frame.
  // Which means that we have to rotate both the attachment frames to get the wanted configuration.
  // The bodies hasn't been rotated, so their local frame is aligned with the world frame. But
  // remember that the attachment frame operations is in local frame.
  // Rotate the frames -90 degrees about x (gives that the z axis is aligned with world y in this case).
  rb1AttFrame->setRotate( agx::Quat( -M_PI_2, agx::Vec3( 1, 0, 0 ) ) );
  rb2AttFrame->setRotate( agx::Quat( -M_PI_2, agx::Vec3( 1, 0, 0 ) ) );
 
  // We want the center of the hinge to be exactly between the two bodies. Take the vector between
  // the two bodies and transform them to local coordinate frame.
  agx::Vec3 localTranslateRB1Frame = rb1->getFrame()->transformVectorToLocal( rb2->getPosition() - rb1->getPosition() );
  agx::Vec3 localTranslateRB2Frame = rb2->getFrame()->transformVectorToLocal( rb1->getPosition() - rb2->getPosition() );
 
  // Position the attachment frames half the distance of the vectors.
  rb1AttFrame->setLocalTranslate( localTranslateRB1Frame * 0.5 );
  rb2AttFrame->setLocalTranslate( localTranslateRB2Frame * 0.5 );
 
  // Add the attachments to the bodies.
  rb1->addAttachment( rb1AttFrame, "HingeAttachment" );
  rb2->addAttachment( rb2AttFrame, "HingeAttachment" );
 
  // Create the hinge and add it to the simulation.
  agx::HingeRef h2 = new agx::Hinge( rb1, rb1AttFrame, rb2, rb2AttFrame );
  simulation->add( h2 );
 
  // Compliance is proportional to the inverse of the, more well known, spring constant.
  // It's possible to simulate constraints with infinite spring constant, but - for stability
  // reasons - it's good practice to choose as small spring constant (large compliance) as
  // possible for the current system simulated.
 
  // For both hinges, let the spring constant be infinite, i.e., zero compliance.
  h1->setCompliance( 0 );
  h2->setCompliance( 0 );
 
  // Mass ratios are often a problem for physics simulations like this. So if we have a constraint
  // between two bodies, and the mass ratio m1/m2 is very small or very large, the solvers often
  // has problem solving the constraints in a correct way. We can try to change the masses so that
  // rb1 has a large mass in comparison to rb2.
 
  // This is 10 000 000 tons (ten million tons)
  rb1->getMassProperties()->setMass( 1.0E10 );
  // This is 10 kg. So a ten million ton box is hanging in on a 10 kg box.
  rb2->getMassProperties()->setMass( 1.0E1 );
 
  // Disable collisions between rb1 and rb2.
  rb1->getGeometries().front()->setEnableCollisions( rb2->getGeometries().front(), false );
 
  // Debug rendering enabled to visualize the constraints.
  application->setEnableDebugRenderer( true );
 
  // Hold left CTRL key and left mouse click-hold on an object in the scene to "mouse pick".
 
  return root;
}
 
// Hinge- and Prismatic constraints are both Constraint1DOF - meaning that
// only one of the six degrees of freedom is free.
class Constraint1DMotorKeyboardHandler : public agxSDK::GuiEventListener
{
  public:
    Constraint1DMotorKeyboardHandler( agx::Constraint1DOF* constraint, agx::Real speed, int keyPositive, int keyNegative )
      : m_constraint( constraint ), m_speed( speed ), m_keyPositive( keyPositive ), m_keyNegative( keyNegative )
    {
      // We're only interested in keyboard events.
      setMask( KEYBOARD );
 
      // Motor1D, Range1D and Lock1D are by default disabled. Make sure that the motor is enabled.
      if ( m_constraint.isValid() )
        m_constraint->getMotor1D()->setEnable( true );
    }
 
    virtual bool keyboard( int key, unsigned int /*modKeyMask*/, float /*x*/, float /*y*/, bool keydown )
    {
      bool handled = false;
 
      if ( !m_constraint.isValid() )
        return false;
 
      // If someone holds "key positive", set positive speed to the motor.
      if ( key == m_keyPositive ) {
        m_constraint->getMotor1D()->setSpeed( m_speed * int( keydown ) );
        handled = true;
      }
      // If someone holds "key negative", set negative speed to the motor.
      else if ( key == m_keyNegative ) {
        m_constraint->getMotor1D()->setSpeed( -m_speed * int( keydown ) );
        handled = true;
      }
 
      return handled;
    }
 
  private:
    agx::observer_ptr< agx::Constraint1DOF > m_constraint;
    agx::Real m_speed;
    int m_keyPositive;
    int m_keyNegative;
};
 
osg::Group* buildTutorial3( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  RigidBodyPtrPair rbPair = createDefaultScene( agx::Vec3( -4, -2, 7 ), agx::Vec3( 0, -2, 7 ), simulation, root );
  agx::RigidBodyRef rb1 = rbPair.first;
  agx::RigidBodyRef rb2 = rbPair.second;
 
  // Create attachment frames like in the earlier tutorials, but this time let the local z axis be
  // aligned with world z for the hinge (i.e., rotate 90 degrees about y)
  agx::FrameRef rb1AttFrame = new agx::Frame();
  agx::FrameRef rb2AttFrame = new agx::Frame();
 
  rb1AttFrame->setRotate( agx::Quat( M_PI_2, agx::Vec3( 0, 1, 0 ) ) );
  rb2AttFrame->setRotate( agx::Quat( M_PI_2, agx::Vec3( 0, 1, 0 ) ) );
 
  agx::Vec3 localTranslateRB1Frame = rb1->getFrame()->transformVectorToLocal( rb2->getPosition() - rb1->getPosition() );
  agx::Vec3 localTranslateRB2Frame = rb2->getFrame()->transformVectorToLocal( rb1->getPosition() - rb2->getPosition() );
 
  // Let the hinge center be in the middle of rb1 and rb2.
  rb1AttFrame->setLocalTranslate( localTranslateRB1Frame * 0.5 );
  rb2AttFrame->setLocalTranslate( localTranslateRB2Frame * 0.5 );
 
  // Add the attachments to the bodies.
  rb1->addAttachment( rb1AttFrame, "HingeAttachment" );
  rb2->addAttachment( rb2AttFrame, "HingeAttachment" );
 
  // Create the hinge and add it to the simulation.
  agx::HingeRef h = new agx::Hinge( rb1, rb1AttFrame, rb2, rb2AttFrame );
  // Set the compliance of this hinge to some small value.
  h->setCompliance( 1.0E-11 );
  simulation->add( h );
 
  // Create the keyboard callback to control the hinge motor (class above this function).
  simulation->add( new Constraint1DMotorKeyboardHandler( h, 2.0, agxSDK::GuiEventListener::KEY_Up, agxSDK::GuiEventListener::KEY_Down ) );
 
  // Now, create a lock joint (constraining all six degrees of freedom) (rb2 to world) to
  // prevent the system from falling down.
  // LockJoint given one body will lock the body, given current transform, to the world. If the
  // body is moved after the creation of the lock, the body will move back to the position it
  // had when the lock joint was made.
  agx::LockJointRef l = new agx::LockJoint( rb2 );
  // A lock joint has six compliance parameters, one for each degree of freedom. The same holds
  // for all constraints, each degree of freedom the constraint constrains, has a compliance
  // parameter. The index of the compliance actually differs from constraint to constraint,
  // but for the lock joint it's trivial. Compliance index:
  // 0 - spatial, world x
  // 1 - spatial, world y
  // 2 - spatial, world z
  // 3 - rotational, about world x
  // 4 - rotational, about world y
  // 5 - rotational, about world z
  // First, set all equations to have compliance 1E-10.
  l->setCompliance( 1.0E-10 );
  // The hinge axis is set to be about world x, so relax this equation (i.e., the constraint may
  // violate this degree of freedom by some controlled amount). So when the speed of the motor
  // is non-zero, the locked body (rb2), may rotate by some amount around its x axis.
  l->setCompliance( 2.0E-7, 3 );
  simulation->add( l );
 
  // Create two new rigid bodies and attachment frames.
  rbPair = createDefaultScene( agx::Vec3( -4, 2, 7 ), agx::Vec3( 0, 2, 7 ), simulation, root );
  agx::RigidBodyRef rb3 = rbPair.first;
  agx::RigidBodyRef rb4 = rbPair.second;
 
  agx::FrameRef rb3AttFrame = new agx::Frame();
  agx::FrameRef rb4AttFrame = new agx::Frame();
 
  rb3AttFrame->setRotate( agx::Quat( M_PI_2, agx::Vec3( 0, 1, 0 ) ) );
  rb4AttFrame->setRotate( agx::Quat( M_PI_2, agx::Vec3( 0, 1, 0 ) ) );
 
  agx::Vec3 localTranslateRB3Frame = rb3->getFrame()->transformVectorToLocal( rb4->getPosition() - rb3->getPosition() );
  agx::Vec3 localTranslateRB4Frame = rb4->getFrame()->transformVectorToLocal( rb3->getPosition() - rb4->getPosition() );
 
  // Let the hinge center be in the middle of rb1 and rb2.
  rb3AttFrame->setLocalTranslate( localTranslateRB3Frame * 0.5 );
  rb4AttFrame->setLocalTranslate( localTranslateRB4Frame * 0.5 );
 
  // Add the attachments to the bodies.
  rb3->addAttachment( rb3AttFrame, "PrismaticAttachment" );
  rb4->addAttachment( rb4AttFrame, "PrismaticAttachment" );
 
  // Create a prismatic constraint - equal construction as for the hinge.
  // (Actually - when using attachment frames, the construction of the
  // constraints are the same for all constraints.)
  agx::PrismaticRef p = new agx::Prismatic( rb3, rb3AttFrame, rb4, rb4AttFrame );
  // Let the compliance be the same as for the hinge above.
  p->setCompliance( 1.0E-11 );
  simulation->add( p );
 
  // Create and add the motor controller for p.
  simulation->add( new Constraint1DMotorKeyboardHandler( p, -5.0, agxSDK::GuiEventListener::KEY_Left, agxSDK::GuiEventListener::KEY_Right ) );
 
  // We don't want the motor to drive the two boxes together, and not too far away
  // from each other, so enable the prismatic range to constrain the min and max distance rb3
  // and rb4 may be from each other. (This is also possible to enable for hinges,
  // but for hinges it's the angle the range constraint is working on. E.g., the
  // angle between the two bodies may only be between -pi/4 to pi/4.)
 
  // All secondary constraints are disabled by default, so start by enabling the range.
  p->getRange1D()->setEnable( true );
  // This range may apply forces between -inf to +inf (i.e., no boundary).
  // Note: This is the default values for the force range.
  p->getRange1D()->setForceRange( agx::RangeReal( -std::numeric_limits<agx::Real>::infinity(), std::numeric_limits<agx::Real>::infinity() ) );
  // We don't want the motor and the range to fight each other with the same force limits,
  // the motor should stop when we reach the limit. So set the force range to something
  // smaller than infinity.
  p->getMotor1D()->setForceRange( agx::RangeReal( -1.0E6, 1.0E6 ) );
  // Set the range (in meters for the prismatic, and when created, the constraint angle is zero).
  // So let the rb3 and rb4 move at maximum 1 meter towards each other and at maximum 5 meters
  // away from each other.
  // Note: The sign here and for the motor is dependent in which order one pass the bodies to
  //       the constraint constructor.
  p->getRange1D()->setRange( agx::RangeReal( -5.0, 1.0 ) );
 
  // The prismatic axis is aligned with world x axis. Above, for the hinge, we relaxed
  // the rotational degree of freedom around x, for the lock on rb2. Here, we're
  // interested in relaxing the spatial x degree of freedom to get the same bouncy
  // behavior when enabling the motor. So, lock rb4 to the world.
  agx::LockJointRef l2 = new agx::LockJoint( rb4 );
  l2->setCompliance( 1.0E-10 );
  // Spatial x is index 0 (see list above).
  l2->setCompliance( 2.0E-7, 0 );
  simulation->add( l2 );
 
  // Debug rendering enabled to visualize the constraints.
  application->setEnableDebugRenderer( true );
 
  std::cout << "Press left/right to operate prismatic constraint\n";
  std::cout << "Press up/down to operate hinge constraint\n" << std::endl;
 
  return root;
}
 
/*
testing for cable simulation
*/
 
class CableMoveKeyboardHandler : public agxSDK::GuiEventListener
{
  public:
    CableMoveKeyboardHandler( agx::RigidBody* firstBody)
      : m_firstBody( firstBody )
    {
      // We're only interested in keyboard events.
      setMask( KEYBOARD );
    }
 
    virtual bool keyboard( int key, unsigned int /*modKeyMask*/, float /*x*/, float /*y*/, bool /*keydown*/ ) override
    {
      bool handled = false;
 
      if ( !m_firstBody.isValid() )
        return false;
 
      if ( key == agxSDK::GuiEventListener::KEY_Up ) {
        m_firstBody->setVelocity(m_firstBody->getVelocity() + agx::Vec3(0, 0, 1));
        handled = true;
      }
      else if ( key == agxSDK::GuiEventListener::KEY_Down ) {
        m_firstBody->setVelocity(m_firstBody->getVelocity() + agx::Vec3(0, 0, -1));
        handled = true;
      }
      else if ( key == agxSDK::GuiEventListener::KEY_Left ) {
        m_firstBody->setVelocity(m_firstBody->getVelocity() + agx::Vec3(1, 0, 0));
        handled = true;
      }
      else if ( key == agxSDK::GuiEventListener::KEY_Right ) {
        m_firstBody->setVelocity(m_firstBody->getVelocity() + agx::Vec3(-1, 0, 0));
        handled = true;
      }
 
      return handled;
    }
 
  private:
    agx::RigidBodyObserver m_firstBody;
};
osg::Group* buildTutorial4( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  // Create a static box geometry as in tutorial 1.4.
  agx::Vec3 boxGeometryHalfExtent( 15, 15, 0.5 );
  agxCollide::GeometryRef boxGeometry = new agxCollide::Geometry( new agxCollide::Box( boxGeometryHalfExtent ) );
  boxGeometry->setPosition( boxGeometry->getPosition() - agx::Vec3( 0, 0, boxGeometryHalfExtent.z() ) );
  simulation->add( boxGeometry );
  boxGeometry->setName( "boxGeometry" );
  agxOSG::createVisual( boxGeometry, root );
 
  // add cylinders and joints
  const agx::Vec3 startPos = agx::Vec3( 0, 0, 1.0 );
  const size_t numSegments = 30;
  const agx::Real radius = agx::Real( 0.2 );
  const agx::Real height = agx::Real( 0.5 );
 
  agx::RigidBodyRef prevRb = 0L;
  for( size_t i = 0; i < numSegments; ++i ) {
    // Create the dynamic rigid body (carries mass properties and interface to dynamic properties)
    agx::RigidBodyRef dynamicRb = new agx::RigidBody();
 
    // Without a geometry, dynamicRb will just fall in the gravitational field. So create a geometry
    // and add that geometry to the rigid body.
    agxCollide::GeometryRef dynamicRbGeometry = new agxCollide::Geometry();
 
    //agxCollide::CylinderRef dynamicRbCylinder = new agxCollide::Cylinder( radius, height );
    agxCollide::CapsuleRef dynamicRbCylinder = new agxCollide::Capsule( radius, height );
 
    dynamicRbGeometry->add( dynamicRbCylinder );
 
    // Add the geometry to the rigid body, telling that if this geometry collides with other geometries,
    // this rigid body will take the forces from these collisions.
    dynamicRb->add( dynamicRbGeometry );
 
    // coordinate system for cylinder in RS (origin is cylinder bottom center) is different from Agx(origin is cylinder center)
    // For both to conform to interafce regulation (see IPhysicsEngineBase::CreateCylinder(...)), a 90 deg rotation around x needs to be adjusted for Agx, and a half height translation is needed for RS;
    //dynamicRbGeometry->setLocalRotation( agx::EulerAngles( agx::Vec3( agx::PI_2, 0, 0 ), agx::EulerAngles::XYZs ) );
    dynamicRbGeometry->setLocalRotation( agx::Quat( agx::PI_2, agx::Vec3::X_AXIS() ) );
 
    agxOSG::createVisual( dynamicRbGeometry, root );
 
    // Position the body.
    agx::Vec3 pos = startPos + agx::Vec3( 0, 0, static_cast<agx::Real>(i) * height );
    dynamicRb->setPosition( pos );
 
    // As a last step we have to add the rigid body to the simulation.
    simulation->add( dynamicRb );
 
    // Previous body defined, disable collisions and create the constraint (ball joint).
    if ( prevRb != 0L ) {
      agxUtil::setEnableCollisions( prevRb, dynamicRb, false );
 
      // We use ball joints, so the orientation of the frame isn't important.
      // Position of the constraint exactly on top of prevRb and exactly at
      // bottom of dynamicRb.
      agx::FrameRef prevRbFrame = new agx::Frame();
      agx::FrameRef dynamicRbFrame = new agx::Frame();
 
      prevRbFrame->setTranslate( 0, 0, agx::Real( 0.5 ) * height );
      dynamicRbFrame->setTranslate( 0, 0, -agx::Real( 0.5 ) * height );
 
      agx::BallJointRef ballJoint = new agx::BallJoint( prevRb, prevRbFrame, dynamicRb, dynamicRbFrame );
      ballJoint->setCompliance( agx::Real( 1E-7 ) );
 
      simulation->add( ballJoint );
    }
 
    prevRb = dynamicRb;
  }
 
  // Create rigid body that we can control by assigning velocity (a kinematic rigid body).
  agx::RigidBodyRef body = new agx::RigidBody;
  simulation->add(body);
  body->setTransform( prevRb->getTransform() );
  body->setMotionControl( agx::RigidBody::KINEMATICS );
  agx::LockJointRef lock = new agx::LockJoint( body, prevRb );
  // Let the constraint be a little relaxed/spongy.
  lock->setCompliance( agx::Real( 1E-5 ) );
  simulation->add( lock );
 
  // Create the keyboard callback to control the movement
  simulation->add( new CableMoveKeyboardHandler( body ) );
 
  // Debug rendering enabled to visualize the constraints.
  application->setEnableDebugRenderer( true );
 
  return root;
}
 
class LiftListener : public agxSDK::ContactEventListener
{
  public:
    LiftListener(agx::RigidBody* rb1, agx::RigidBody* rb2 )
      : m_rb1( rb1 ), m_rb2( rb2 )
    {
      setMask(CONTACT);
    }
 
    virtual KeepContactPolicy contact( const agx::TimeStamp&, agxCollide::GeometryContact* ) override;
 
  private:
    agx::RigidBodyRef m_rb1;
    agx::RigidBodyRef m_rb2;
};
 
agxSDK::ContactEventListener::KeepContactPolicy LiftListener::contact( const agx::TimeStamp& t, agxCollide::GeometryContact* )
{
  std::cout << "Time: " << t << std::endl;
  if ( t < agx::Real( 3.51667 + 3.0/60.0 ) )
    return KEEP_CONTACT;
 
  m_rb1->setVelocity( agx::Vec3( 0, 0, 0.2 ) );
  m_rb2->setVelocity( agx::Vec3( 0, 0, 0.2 ) );
 
  std::cout << "Lifting" << std::endl;
 
  return KEEP_CONTACT;
}
 
osg::Group* buildTutorial5( agxSDK::Simulation *simulation, agxOSG::ExampleApplication *application )
{
  osg::Group* root = new osg::Group();
 
  // Create a static box geometry as in tutorial 1.4.
  agx::Vec3 boxGeometryHalfExtent( 3, 3, 0.5 );
  agxCollide::GeometryRef boxGeometry = new agxCollide::Geometry( new agxCollide::Box( boxGeometryHalfExtent ) );
  boxGeometry->setPosition( boxGeometry->getPosition() - agx::Vec3( 0, 0, boxGeometryHalfExtent.z() ) );
  simulation->add( boxGeometry );
  boxGeometry->setName( "boxGeometry" );
  agxOSG::createVisual( boxGeometry, root );
 
  agx::Real radius = 0.1;
 
  agx::MaterialRef material = new agx::Material("lift");
  material->getSurfaceMaterial()->setRoughness( 30 );
 
  agx::RigidBodyRef body = new agx::RigidBody;
  body->setPosition( agx::Vec3(0,0, 1 ));
  body->setRotation( agx::Quat( M_PI/2, agx::Vec3(1,0,0)));
 
  body->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( radius, 1, radius ) )) );
  agxOSG::createVisual( body, root );
  simulation->add( body );
  body->getMassProperties()->setMass( 10 );
  //(body->getGeometries().front())->setMaterial( material );
 
  agx::Real width = 0.05;
  agx::Real height = 0.1;
  agx::Real length = 0.4;
 
  // Gripper 1
  agx::RigidBodyRef grip1 = new agx::RigidBody;
  grip1->setPosition( agx::Vec3(-0.2,0.5, 0.3 ));
  grip1->setMotionControl( agx::RigidBody::KINEMATICS);
  grip1->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3(length, width, height) )) );
  agxOSG::createVisual( grip1, root );
  simulation->add( grip1 );
  grip1->setVelocity(agx::Vec3(0,-0.1, 0));
  (grip1->getGeometries().front())->setMaterial( material );
 
  // Gripper 2
  agx::RigidBodyRef grip2 = new agx::RigidBody;
  grip2->setPosition( agx::Vec3(-0.2,-0.5, 0.3 ));
  grip2->setMotionControl( agx::RigidBody::KINEMATICS);
  grip2->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3(length, width, height) )) );
  agxOSG::createVisual( grip2, root );
  simulation->add( grip2 );
  grip2->setVelocity(agx::Vec3(0,0.1, 0));
  grip2->getGeometries().front()->setMaterial( material );
 
  agx::ref_ptr<LiftListener> lift = new LiftListener( grip1, grip2 );
  lift->setFilter( new agxSDK::GeometryFilter( grip1->getGeometries().front().get() ));
  simulation->add(  lift );
 
  // Debug rendering enabled to visualize the constraints.
  application->setEnableDebugRenderer( true );
 
  return root;
}
 
osg::Group* buildTutorial6( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  // create two objects, 8 meters away from each other along world 's x axis
  RigidBodyPtrPair rbPair = createDefaultScene( agx::Vec3( -4, 0, 15 ), agx::Vec3( 4, 0, 15 ), simulation, root, false );
  agx::RigidBodyRef rb1 = rbPair.first;
  agx::RigidBodyRef rb2 = rbPair.second;
 
  agx::LockJointRef h0 = new agx::LockJoint(rb1);
  h0->setCompliance( agx::Real( 1E-11 ) );
  simulation->add( h0 );
 
  // Create joints between rb1 and rb2 using attachment frames.
  agx::FrameRef rb1AttFrame = new agx::Frame();
  agx::FrameRef rb2AttFrame = new agx::Frame();
 
  // Position the attachment frames
  // frame 1 is -5 meters along body1's z axis
  // frame 2 is 5 meters along body2's z axis
  // so that when joint takes effect, body 1 is 10 meters above the body 2
  rb1AttFrame->setLocalTranslate( agx::Vec3( 0, 0, -5 ) );
  rb2AttFrame->setLocalTranslate( agx::Vec3( 0, 0,  5 ) );
 
  {
#if 1
    rb2AttFrame->setLocalTranslate( agx::Vec3( 0, 0, 5 ) );
    rb2AttFrame->setLocalRotate( agx::Quat( agx::PI_4, agx::Vec3::X_AXIS() ) );
    // Create hinge between rb2 and the world;
    // The joint should also keep rb2 at the attachment frame (rb2AttFrame) in world coordinates. But it DOES NOT!
    agx::FrameRef worldFrame = new agx::Frame();
    worldFrame->setLocalTranslate( agx::Vec3( 0, 0, 15 ) );
    agx::HingeRef h3 = new agx::Hinge( rb2, rb2AttFrame, 0L, 0L );
    simulation->add( h3 );
#else
    // Add the attachments to the bodies.
    rb1->addAttachment( rb1AttFrame, "LockAttachment" );
    rb2->addAttachment( rb2AttFrame, "LockAttachment" );
 
#endif
  }
#if 0
  {
          // Create the lock and add it to the simulation.
          agx::LockJointRef h1 = new agx::LockJoint( rb1, rb1AttFrame, rb2, rb2AttFrame );
    h1->setCompliance( 1E-11 );
          simulation->add( h1 );
  }
#endif
 
#if 0
  {
          // Create the hinge and add it to the simulation.
          agx::HingeRef h2 = new agx::Hinge( rb1, rb1AttFrame, rb2, rb2AttFrame );
          simulation->add( h2 );
  }
#endif
 
#if 0
  {
          // Create the hinge and add it to the simulation.
          agx::BallJointRef h3 = new agx::BallJoint( rb1, rb1AttFrame, rb2, rb2AttFrame );
          simulation->add( h3 );
  }
#endif
 
#if 0
  {
          // Create the hinge and add it to the simulation.
          agx::PrismaticRef h3 = new agx::Prismatic( rb1, rb1AttFrame, rb2, rb2AttFrame );
 
          // All secondary constraints are disabled by default, so start by enabling the range.
          h3->getRange1D()->setEnable( true );
          h3->getRange1D()->setRange(-5, 5);
 
          simulation->add( h3 );
  }
#endif
 
  // Debug rendering enabled to visualize the constraints.
  application->setEnableDebugRenderer( true );
 
  // Hold left CTRL key and left mouse click-hold on an object in the scene to "mouse pick".
 
  return root;
}
 
class Beam : public agxSDK::Assembly
{
  public:
    Beam( const agx::Vec3& dimension, size_t numSegments, agx::Real mass, agx::Real compliance, osg::Group* root )
    {
      create( dimension, numSegments, mass, compliance, root );
    }
 
    agx::RigidBody* getFirstBody() { return m_first; }
    agx::RigidBody* getLastBody() { return m_last; }
 
    void addNotification( agxSDK::Simulation* simulation ) override
    {
      // This beam can't collide with it self.
      simulation->getSpace()->setEnablePair( agx::UInt32(ms_index - 1), agx::UInt32(ms_index - 1), false );
    }
    using agxSDK::Assembly::addNotification;
 
  private:
    void create( const agx::Vec3& dimension, size_t numSegments, agx::Real mass, agx::Real compliance, osg::Group* root )
    {
      agx::Real dl = dimension.x() / static_cast<agx::Real>( numSegments - 1 );
      // Initial position in assembly frame.
      agx::Vec3 position( -0.5 * dimension.x(), 0, 0 );
      // Mass for each box.
      agx::Real dMass = mass / static_cast<agx::Real>(numSegments);
      // Lock constraint damping.
      agx::Real damping = 3.0 / 60;
      agx::RigidBodyRef rb = 0L;
      agx::RigidBodyRef prev = 0L;
      for ( size_t i = 0; i < numSegments; ++i ) {
        rb = new agx::RigidBody();
        rb->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( dl, dimension.y(), dimension.z() ) * 0.5 ) ) );
        rb->getGeometries().front()->addGroup( agx::UInt32(ms_index) );
        rb->setPosition( position );
        rb->getMassProperties()->setMass( dMass );
        agxOSG::createVisual( rb, root );
        add( rb );
 
        if ( prev == 0L ) {
          m_first = rb;
        }
        else {
          // Lock prev to rb.
          agx::LockJointRef lock = new agx::LockJoint( prev, rb );
          lock->setCompliance( compliance );
          for ( int j = 0; j < agx::LockJoint::NUM_DOF; ++j )
            lock->getRegularizationParameters( j )->setDamping( damping );
          add( lock );
        }
        prev = rb;
 
        position.x() += dl;
      }
      m_last = prev;
      ++ms_index;
    }
 
    agx::RigidBodyRef m_first;
    agx::RigidBodyRef m_last;
    static size_t ms_index;
};
 
class Wheel : public agxSDK::Assembly
{
  public:
    Wheel( agx::Real radius, agx::Real thickness, size_t numSegments, agx::Real mass, agx::Real compliance, agx::Real damping, osg::Group* root )
    {
      create( radius, thickness, numSegments, mass, compliance, damping, root );
    }
 
    void addNotification( agxSDK::Simulation* simulation ) override
    {
      // This beam can't collide with it self.
      simulation->getSpace()->setEnablePair( agx::UInt32(ms_index - 1), agx::UInt32(ms_index - 1), false );
    }
    using agxSDK::Assembly::addNotification;
 
  private:
    void create( agx::Real radius, agx::Real thickness, size_t numSegments, agx::Real mass, agx::Real compliance, agx::Real damping, osg::Group* root )
    {
      agxAssert( numSegments > 1 );
      agx::Real dPhi = 2 * M_PI / static_cast<agx::Real>(numSegments - 1);
      agx::Real dMass = mass / static_cast<agx::Real>(numSegments);
      agx::Vec3 position( 0, 0, -radius );
      agx::Real xDim = 2 * M_PI * radius / static_cast<agx::Real>( numSegments - 1 );
      agx::AffineMatrix4x4 transform = agx::AffineMatrix4x4::translate( position );
      agx::RigidBodyRef prev = 0L;
      agx::RigidBodyRef first = 0L;
      for ( size_t i = 0; i < numSegments; ++i ) {
        agx::RigidBodyRef rb = new agx::RigidBody();
        rb->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( xDim, 2 * thickness, thickness ) * 0.5 ) ) );
        rb->getGeometries().front()->addGroup( agx::UInt32(ms_index) );
        rb->setTransform( transform );
        rb->getMassProperties()->setMass( dMass );
        agxOSG::createVisual( rb, root );
        transform *= agx::AffineMatrix4x4::rotate( dPhi, agx::Vec3( 0, 1, 0 ) );
        add( rb );
        if ( first == 0L ) {
          first = rb;
        }
        else {
          // Lock prev to rb.
          agx::LockJointRef lock = new agx::LockJoint( prev, rb );
          lock->setCompliance( compliance );
          for ( int j = 0; j < agx::LockJoint::NUM_DOF; ++j )
            lock->getRegularizationParameters( j )->setDamping( damping );
          add( lock );
        }
        prev = rb;
      }
      // Lock first to last.
      agx::LockJointRef lock = new agx::LockJoint( first, prev );
      lock->setCompliance( compliance );
      for ( int j = 0; j < agx::LockJoint::NUM_DOF; ++j )
        lock->getRegularizationParameters( j )->setDamping( damping );
      add( lock );
      ++ms_index;
    }
 
    static size_t ms_index;
};
 
size_t Beam::ms_index = 1;
size_t Wheel::ms_index = 123456;
 
osg::Group* buildTutorial7( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Dimension: [ length, width, height ]
  agx::Vec3 dimension( 20, 0.8, 0.8 );
  agx::Real totalMass = 1E3;
  agx::Real compliance = 2E-9;
  // Number of boxes representing the beam.
  size_t numSegments = 100;
 
  // Beam class above is an assembly. An beam will be created along x axis, center of beam at the origin.
  agx::ref_ptr<Beam> beam1 = new Beam( dimension, numSegments, totalMass, compliance, root );
  simulation->add( beam1 );
 
  beam1->setPosition( agx::Vec3( 0, 0, 0 ) );
 
  agx::LockJointRef attLock = new agx::LockJoint( beam1->getFirstBody() );
  attLock->setCompliance( 1E-12 );
  simulation->add( attLock );
  attLock = new agx::LockJoint( beam1->getLastBody() );
  attLock->setCompliance( 1E-12 );
  simulation->add( attLock );
 
  compliance = 1E-9;
  totalMass = 12E3;
  agx::ref_ptr<Beam> beam2 = new Beam( dimension, numSegments, totalMass, compliance, root );
  simulation->add( beam2 );
 
  // Rotate this beam 90 degrees about z.
  beam2->setRotation( agx::Quat( M_PI_2, agx::Vec3( 0, 0, 1 ) ) );
  beam2->setPosition( agx::Vec3( 0, 0.5, 5 ) );
 
  return root;
}
 
class Tutorial58KeyboardController : public agxSDK::GuiEventListener
{
  public:
    Tutorial58KeyboardController( agx::Hinge* h1, agx::Hinge* h2, agx::Real motorForce, agx::Real motorSpeed )
      : m_h1( h1 ), m_h2( h2 ), m_motorForce( motorForce ), m_motorSpeed( motorSpeed )
    {
      setMask( KEYBOARD );
    }
 
    bool keyboard( int key, unsigned int , float , float , bool keydown )
    {
      bool handled = false;
 
      if ( key == agxSDK::GuiEventListener::KEY_Left ) {
        m_h1->getMotor1D()->setSpeed( m_motorSpeed * keydown );
        m_h2->getMotor1D()->setSpeed(-m_motorSpeed * keydown );
        m_h1->getMotor1D()->setForceRange( agx::RangeReal( m_motorForce * keydown ) );
        m_h2->getMotor1D()->setForceRange( agx::RangeReal( m_motorForce * keydown ) );
        handled = true;
      }
      else if ( key == agxSDK::GuiEventListener::KEY_Right ) {
        m_h1->getMotor1D()->setSpeed(-m_motorSpeed * keydown );
        m_h2->getMotor1D()->setSpeed( m_motorSpeed * keydown );
        m_h1->getMotor1D()->setForceRange( agx::RangeReal( m_motorForce * keydown ) );
        m_h2->getMotor1D()->setForceRange( agx::RangeReal( m_motorForce * keydown ) );
        handled = true;
      }
 
      return handled;
    }
 
  private:
    agx::observer_ptr< agx::Hinge > m_h1;
    agx::observer_ptr< agx::Hinge > m_h2;
    agx::Real m_motorForce;
    agx::Real m_motorSpeed;
};
 
osg::Group* buildTutorial8( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Dimension: [ length, width, height ]
  agx::Vec3 dimension( 20, 0.8, 0.8 );
  agx::Real totalMass = 1E3;
  agx::Real compliance = 3E-10;
  // Number of boxes representing the beam.
  size_t numSegments = 500;
 
  // Beam class above is an assembly. An beam will be created along x axis, center of beam at the origin.
  agx::ref_ptr<Beam> beam1 = new Beam( dimension, numSegments, totalMass, compliance, root );
  simulation->add( beam1 );
 
  beam1->setPosition( agx::Vec3( 0, 0, 0 ) );
 
#if 0
  agx::LockJointRef l = new agx::LockJoint( beam1->getFirstBody() );
  l->setCompliance( 1E-10 );
  simulation->add( l );
  l = new agx::LockJoint( beam1->getLastBody() );
  l->setCompliance( 1E-10 );
  simulation->add( l );
#else
  agx::HingeRef att1 = new agx::Hinge( agx::HingeFrame( beam1->getFirstBody()->getPosition(), agx::Vec3( 1, 0, 0 ) ), beam1->getFirstBody() );
  att1->setCompliance( 1E-12 );
  simulation->add( att1 );
  agx::HingeRef att2 = new agx::Hinge( agx::HingeFrame( beam1->getLastBody()->getPosition(), agx::Vec3( 1, 0, 0 ) ), beam1->getLastBody() );
  att2->setCompliance( 1E-12 );
  simulation->add( att2 );
 
  att1->getMotor1D()->setEnable( true );
  att1->getMotor1D()->setForceRange( agx::RangeReal( 0 ) );
  att2->getMotor1D()->setEnable( true );
  att2->getMotor1D()->setForceRange( agx::RangeReal( 0 ) );
 
  agx::Real motorForce = 6E8;
  agx::Real motorSpeed = 5;
 
  simulation->add( new Tutorial58KeyboardController( att1, att2, motorForce, motorSpeed ) );
 
  // Add some info text
  simulation->add( new agxOSG::TextEventListener("Rotate beam with LEFT/RIGHT keys", agx::Vec2(0.1, 0.1), agx::Vec3(1,0,0)));
 
#endif
 
  return root;
}
 
osg::Group* buildTutorial9( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  //simulation->setUniformGravity( agx::Vec3() );
 
  agx::ref_ptr<Wheel> wheel1 = new Wheel( 2, 0.5, 50, 2E2, 1E-6, 2.5 / 60, root );
  simulation->add( wheel1 );
 
  agx::ref_ptr<Wheel> wheel2 = new Wheel( 2, 0.5, 50, 2E2, 1E-6, 2.5 / 60, root );
  wheel2->setPosition( agx::Vec3( 0, 0, 5 ) );
  wheel2->setRotation( agx::Quat( M_PI_2, agx::Vec3( 0, 0, 1 ) ) );
  simulation->add( wheel2 );
 
  agx::RigidBodyRef plane = new agx::RigidBody();
  plane->setMotionControl( agx::RigidBody::STATIC );
  plane->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 25, 25, 1 ) ) ) );
  agxOSG::createVisual( plane, root );
 
  plane->setPosition( agx::Vec3( 0, 0, -5 ) );
 
  simulation->add( plane );
 
  return root;
}
 
osg::Group* buildTutorial10( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  // No gravity
  simulation->setUniformGravity( agx::Vec3() );
 
  agxCollide::GeometryRef boxGeometry1 = new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.5, 0.5, 0.5 ) ) );
  agx::RigidBodyRef body1 = new agx::RigidBody();
  body1->setPosition( agx::Vec3( 1.5, 0, 0 ) );
  body1->add( boxGeometry1 );
  agxOSG::createVisual( boxGeometry1, root );
  simulation->add( body1 );
 
  // Lock body to the world
  agx::LockJointRef lock1 = new agx::LockJoint(body1);
  simulation->add( lock1 );
 
  // Loosen up the constraint for translation
  lock1->setCompliance( 1E-1, agx::LockJoint::TRANSLATIONAL_1 );
  lock1->setCompliance( 1E-1, agx::LockJoint::TRANSLATIONAL_2 );
  lock1->setCompliance( 1E-1, agx::LockJoint::TRANSLATIONAL_3 );
 
  agxCollide::GeometryRef boxGeometry2 = new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.5, 0.5, 0.5 ) ) );
  agx::RigidBodyRef body2 = new agx::RigidBody();
  body2->add( boxGeometry2 );
  body2->setPosition( agx::Vec3( -1.5, 0, 0 ) );
  agxOSG::createVisual( boxGeometry2, root );
  simulation->add( body2 );
 
  // Lock body to the world
  agx::LockJointRef lock2 = new agx::LockJoint(body2);
  simulation->add( lock2 );
 
  // Loosen up the constraint for rotation
  lock2->setCompliance( 1E-1, agx::LockJoint::ROTATIONAL_1 );
  lock2->setCompliance( 1E-1, agx::LockJoint::ROTATIONAL_2 );
  lock2->setCompliance( 1E-1, agx::LockJoint::ROTATIONAL_3 );
 
  // Debug rendering enabled to visualize the constraints.
  application->setEnableDebugRenderer( true );
 
  // Add some info text
  simulation->add( new agxOSG::TextEventListener( "Pick bodies with CTRL+left mouse", agx::Vec2( 0.1, 0.1 ), agx::Vec3( 1, 0, 0 ) ) );
 
  return root;
}
 
osg::Group* buildTutorial11( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  agx::RigidBodyRef rb1 = new agx::RigidBody();
  agx::RigidBodyRef rb2 = new agx::RigidBody();
 
  rb1->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.2, 0.2, 0.2 ) ) ) );
  rb2->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.2, 0.2, 0.2 ) ) ) );
 
  agxOSG::createVisual( rb1, root );
  agxOSG::createVisual( rb2, root );
 
  simulation->add( rb1 );
  simulation->add( rb2 );
 
  rb1->setPosition( agx::Vec3( -1, 0, 0 ) );
  rb2->setPosition( agx::Vec3(  1, 0, 0 ) );
 
  // Attachment frames in body coordinates.
  agx::FrameRef rb1Frame = new agx::Frame();
  agx::FrameRef rb2Frame = new agx::Frame();
 
  // Prismatic axis is by definition the z-axis of the attachment frames.
 
  // Rotate to x for both frames and offset the frames to be at -0.25 (for rb1) and +0.25 (for rb2)
  // in world frame. I.e., -1 + 0.75 and 1 - 0.75.
  rb1Frame->setLocalMatrix( agx::AffineMatrix4x4::rotate( agx::Vec3( 0, 0, 1 ), agx::Vec3( 1, 0, 0 ) ) * agx::AffineMatrix4x4::translate(  0.75, 0, 0 ) );
  rb2Frame->setLocalMatrix( agx::AffineMatrix4x4::rotate( agx::Vec3( 0, 0, 1 ), agx::Vec3( 1, 0, 0 ) ) * agx::AffineMatrix4x4::translate( -0.75, 0, 0 ) );
 
  agx::PrismaticRef prismatic = new agx::Prismatic( rb1, rb1Frame, rb2, rb2Frame );
  simulation->add( prismatic );
 
  std::cout << "Initial angle: " << prismatic->getAngle() << std::endl;
 
  return root;
}
 
class ElementaryConstraintCurrentForceReader : public agxSDK::StepEventListener
{
  public:
    ElementaryConstraintCurrentForceReader( agx::ElementaryConstraint* ec, agxOSG::SceneDecorator* sd, size_t index )
      : agxSDK::StepEventListener( agxSDK::StepEventListener::POST_STEP ), m_ec( ec ), m_sd( sd ), m_index( index )
    {
      update();
    }
 
  protected:
    virtual void post( const agx::TimeStamp& ) override
    {
      update();
    }
 
  private:
    void update()
    {
      if ( m_ec == 0L || m_sd == 0L )
        return;
 
      m_sd->setText( (int)m_index, agx::String::format( "Applied force by %s: %3.3f Nm", m_ec->getName().c_str(), m_ec->getCurrentForce() ) );
    }
 
  private:
    agx::observer_ptr< agx::ElementaryConstraint > m_ec;
    osg::observer_ptr< agxOSG::SceneDecorator >    m_sd;
    size_t                                         m_index;
};
 
osg::Group* buildTutorial12( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  // Gravity is irrelevant in this tutorial.
  simulation->setUniformGravity( agx::Vec3() );
 
  // Creating an 1 x 1 x 1 meter box.
  agx::RigidBodyRef rb = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.5, 0.5, 0.5 ) ) ) );
  simulation->add( rb );
  agxOSG::createVisual( rb, root );
 
  // Hinge the body to world. Axis along world y.
  agx::FrameRef rbFrame = new agx::Frame();
  agx::FrameRef worldFrame = new agx::Frame();
  agx::Constraint::calculateFramesFromWorld( agx::Vec3(), agx::Vec3::Y_AXIS(), rb, rbFrame, 0L, worldFrame );
  agx::HingeRef hinge = new agx::Hinge( rb, rbFrame, 0L, worldFrame );
  simulation->add( hinge );
 
  // Enable the default motor.
  hinge->getMotor1D()->setEnable( true );
  hinge->getMotor1D()->setSpeed( agx::Real( 1 ) );
 
  // Create a new target speed controller (same type as Motor1D). But first
  // we have to define in which degree of freedom it's working and couple
  // it to a constraint via shared data.
 
  // By default:
  // The hinge should have (at least) one angle.
  agxAssert( hinge->getAttachmentPair()->getAngle( 0 ) != 0L );
  // The default angle type should be rotational.
  agxAssert( hinge->getAttachmentPair()->getAngle( 0 )->getType() == agx::Angle::ROTATIONAL );
  // The default angle axis should be z-axis (constraint n-axis).
  agxAssert( hinge->getAttachmentPair()->getAngle( 0 )->getAxis() == agx::Angle::N );
 
  // Note that this object isn't used.
  // We can create the controller data given the default angle. But it is a
  // bit unsafe since it depends on the current implementation/structure of
  // the hinge.
  agx::ConstraintAngleBasedData controllerDataWithDefaultAngle( hinge->getAttachmentPair(), hinge->getAttachmentPair()->getAngle( 0 ) );
 
  // Instead we could create and add a new angle with the same configuration
  // as the default.
  agx::AngleRef myAngle = new agx::RotationalAngle( agx::Angle::N );
  agx::ConstraintAngleBasedData controllerDataWithMyAngle( hinge->getAttachmentPair(), myAngle );
 
  // Create the target speed (friction) controller.
  agx::TargetSpeedControllerRef frictionController = new agx::TargetSpeedController( controllerDataWithMyAngle );
 
  // We can add a range as well because we can. Works on same angle and is activated
  // after 1.5 revolutions.
  agx::RangeControllerRef rangeController = new agx::RangeController( controllerDataWithMyAngle );
 
  // Add the controllers to the hinge.
  hinge->addSecondaryConstraint( "FrictionController", frictionController );
  hinge->addSecondaryConstraint( "RedundantRange", rangeController );
 
  // By default the controller is disabled. Enable it and set parameters.
  frictionController->setEnable( true );
  frictionController->setSpeed( agx::Real( 0 ) );
  // Let 123 Newton meters be the internal resistance.
  frictionController->setForceRange( -agx::Real( 123 ), agx::Real( 123 ) );
 
  rangeController->setEnable( true );
  rangeController->setCompliance( agx::Real( 1E-10 ) );
  rangeController->setRange( -agx::Real( 3 ) * agx::PI, agx::Real( 3 ) * agx::PI );
 
  // Showing the current force applied by the controllers, upper left corner of screen.
  simulation->add( new ElementaryConstraintCurrentForceReader( hinge->getMotor1D(), application->getSceneDecorator(), 0 ) );
  simulation->add( new ElementaryConstraintCurrentForceReader( frictionController, application->getSceneDecorator(), 1 ) );
 
  return root;
}
 
osg::Group* buildTutorial13( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Creating an 0.2 x 0.2 x 3 meter box.
  agx::RigidBodyRef rb = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.1, 0.1, 1.5 ) ) ) );
  simulation->add( rb );
  agxOSG::createVisual( rb, root );
 
  // Creating a cylindrical joint, axis along world z.
  agx::FrameRef rbFrame = new agx::Frame();
  agx::FrameRef worldFrame = new agx::Frame();
  agx::CylindricalJointRef cylindrical = new agx::CylindricalJoint( rb, rbFrame, 0L, worldFrame );
 
  simulation->add( cylindrical );
 
  // The screw controller is constraining the relation between translational- and
  // rotational movement. E.g., after the object has completed three full revolutions
  // it should have traveled one meter.
 
  // The controlling parameter is called 'lead', and the lead is;
  //   the distance along the screw's axis that is covered by one complete rotation of the screw (360 degrees)
  cylindrical->getScrew1D()->setEnable( true );
  cylindrical->getScrew1D()->setLead( agx::Real( 0.01 ) );
 
  // Now, when the object falls in gravity it will start to rotate. For every
  // centimeter it goes down it has to complete one revolution.
 
  return root;
}
 
osg::Group* buildTutorial14( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Creating an 0.2 x 0.2 x 3 meter box.
  agx::RigidBodyRef rb = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.1, 0.1, 1.5 ) ) ) );
  simulation->add( rb );
  agxOSG::createVisual( rb, root );
 
  // Attach the body with a hinge in the world frame
  agx::FrameRef rbHingeFrame = new agx::Frame();
  agx::Hinge* hinge = new agx::Hinge(rb, rbHingeFrame);
  simulation->add(hinge);
 
  // Set a speed to the hinge motor
  hinge->getMotor1D()->setEnable(true);
  hinge->getMotor1D()->setSpeed(0.5);
 
  // Set a speed to the hinge motor
  hinge->getMotor1D()->setEnable(true);
 
  // Creating another 0.2 x 0.2 x 3 meter box.
  agx::RigidBodyRef rb2 = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.1, 0.1, 1.5 ) ) ) );
  simulation->add( rb2 );
  agxOSG::createVisual( rb2, root );
 
  //Position the second body above the first
  rb2->setPosition( 0, 0, 3.0 );
 
  // Creating a universal joint, axis along world z.
  agx::FrameRef rbFrame = new agx::Frame();
  rbFrame->setTranslate(0,0,1.5);
  agx::FrameRef rb2Frame = new agx::Frame();
  rb2Frame->setTranslate(0,0,-1.5);
  agx::UniversalJointRef universal = new agx::UniversalJoint( rb, rbFrame, rb2, rb2Frame );
 
  simulation->add( universal );
 
  // Disable collisions between the two bodies
  agxUtil::setEnableCollisions(rb, rb2, false);
 
  // The Universal Joint constrains all 3 spatial dof's. And one rotational dof.
  // The 2 remaining dofs can be controlled with controllers
 
  // Enable the two motors
  universal->getMotor1D( agx::UniversalJoint::ROTATIONAL_CONTROLLER_1 )->setEnable( true );
  universal->getMotor1D( agx::UniversalJoint::ROTATIONAL_CONTROLLER_2 )->setEnable( true );
 
  // Set a speed to the two motors
  universal->getMotor1D( agx::UniversalJoint::ROTATIONAL_CONTROLLER_1 )->setSpeed( 0.5 );
  universal->getMotor1D( agx::UniversalJoint::ROTATIONAL_CONTROLLER_2 )->setSpeed( 0.2 );
 
  return root;
}
 
osg::Group* buildTutorial15( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Creating an 0.2 x 0.2 x 3 meter box.
  agx::RigidBodyRef rb = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.1, 0.1, 1.5 ) ) ) );
  simulation->add( rb );
  agxOSG::createVisual( rb, root );
 
  // Attach the body with a hinge in the world frame
  agx::FrameRef rbHingeFrame = new agx::Frame();
  agx::Hinge* hinge = new agx::Hinge(rb, rbHingeFrame);
  simulation->add(hinge);
 
  // Set a speed to the hinge motor
  hinge->getMotor1D()->setEnable(true);
  hinge->getMotor1D()->setSpeed(0.5);
 
  // Creating another 0.2 x 0.2 x 3 meter box.
  agx::RigidBodyRef rb2 = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.1, 0.1, 1.5 ) ) ) );
  simulation->add( rb2 );
  agxOSG::createVisual( rb2, root );
 
  //Position the second body above the first
  rb2->setPosition( 0, 0, 3.0 );
 
  // Give it a push so that it falls over
  rb2->addForce(1000,0,0);
 
  // Creating a universal joint, axis along world z.
  agx::FrameRef rbFrame = new agx::Frame();
  rbFrame->setTranslate(0,0,1.5);
  agx::FrameRef rb2Frame = new agx::Frame();
  rb2Frame->setTranslate(0,0,-1.5);
  agx::UniversalJointRef universal = new agx::UniversalJoint( rb, rbFrame, rb2, rb2Frame );
 
  simulation->add( universal );
 
  // Disable collisions between the two bodies
  agxUtil::setEnableCollisions(rb, rb2, false);
 
  // The Universal Joint constrains all 3 spatial dof's. And one rotational dof.
  // The 2 remaining dofs can be controlled with controllers
 
  // Enable the ranges
  universal->getRange1D( agx::UniversalJoint::ROTATIONAL_CONTROLLER_1 )->setEnable( true );
  universal->getRange1D( agx::UniversalJoint::ROTATIONAL_CONTROLLER_2 )->setEnable( true );
 
  // Enable the ranges
  universal->getRange1D( agx::UniversalJoint::ROTATIONAL_CONTROLLER_1 )->setRange( -agx::PI*0.3, agx::PI*0.3 );
  universal->getRange1D( agx::UniversalJoint::ROTATIONAL_CONTROLLER_2 )->setRange( -agx::PI*0.3, agx::PI*0.3 );
 
  return root;
}
 
osg::Group* buildTutorial16( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Creating an 0.2 x 0.2 x 3 meter box.
  agx::RigidBodyRef rb = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.1, 0.1, 1.5 ) ) ) );
  simulation->add( rb );
  agxOSG::createVisual( rb, root );
 
  // Attach the body with a hinge in the world frame
  agx::FrameRef rbHingeFrame = new agx::Frame();
  agx::Hinge* hinge = new agx::Hinge(rb, rbHingeFrame);
  simulation->add(hinge);
 
  // Set a speed to the hinge motor
  hinge->getMotor1D()->setEnable(true);
  hinge->getMotor1D()->setSpeed(0.5);
 
  // Creating another 0.2 x 0.2 x 3 meter box.
  agx::RigidBodyRef rb2 = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.1, 0.1, 1.5 ) ) ) );
  simulation->add( rb2 );
  agxOSG::createVisual( rb2, root );
 
  //Position the second body above the first
  rb2->setPosition( 0, 0, 3.0 );
 
  // Give it a push so that it falls over
  rb2->addForce(1000,0,0);
 
  // Creating a prismatic universal joint, axis along world z.
  agx::FrameRef rbFrame = new agx::Frame();
  rbFrame->setTranslate(0,0,1.5);
  agx::FrameRef rb2Frame = new agx::Frame();
  rb2Frame->setTranslate(0,0,-1.5);
  agx::PrismaticUniversalJointRef universal = new agx::PrismaticUniversalJoint( rb, rbFrame, rb2, rb2Frame );
 
  simulation->add( universal );
 
  // Disable collisions between the two bodies
  agxUtil::setEnableCollisions(rb, rb2, false);
 
  // Add a torus to lean against
  agxCollide::Shape* mesh = agxUtil::TrimeshReaderWriter::createTrimesh( "models/torusRound.obj", 0, agx::Matrix3x3(agx::Vec3(1,1,1)) );
  agxCollide::Geometry* meshGeom = new agxCollide::Geometry(mesh, agx::AffineMatrix4x4::rotate( agx::PI*0.5, agx::Vec3(1,0,0)));
  meshGeom->setPosition(agx::Vec3(0,0,1.6));
  simulation->add(meshGeom);
 
 
  // The remaining spatial dof can also be controlled with a controller
  universal->getRange1D( agx::PrismaticUniversalJoint::TRANSLATIONAL_CONTROLLER_1 )->setEnable(true);
  universal->getRange1D( agx::PrismaticUniversalJoint::TRANSLATIONAL_CONTROLLER_1 )->setRange(-1,1);
 
  return root;
}
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout <<
                "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
                "\tConstraints tutorial\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
    application->addScene( buildTutorial3, '3' );
    application->addScene( buildTutorial4, '4' );
    application->addScene( buildTutorial5, '5' );
    application->addScene( buildTutorial6, '6' );
    application->addScene( buildTutorial7, '7' );
    application->addScene( buildTutorial8, '8' );
    application->addScene( buildTutorial9, '9' );
    application->addScene( buildTutorial10, '0' );
    application->addScene( buildTutorial11, '!' );
    application->addScene( buildTutorial12, '"' );
    application->addScene( buildTutorial13, '#' );
    application->addScene( buildTutorial14, '%' );
    application->addScene( buildTutorial15, '&' );
    application->addScene( buildTutorial16, '/' );
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_contactForces.cpp

/*
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.
*/
 
 
/*
 
Demonstrates how to access contact forces from a simulation.
 
*/
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/utils.h>
 
#include <agxUtil/StepEventCallback.h>
#include <agxRender/Color.h>
#include <agx/version.h>
#include <agxCollide/Box.h>
#include <agx/ContactForceReader.h>
 
osg::Group* buildTutorial1( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group;
 
  agx::RigidBodyRef ground = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 2.0, 2.0, 0.5 ) ) );
  ground->setPosition( 0, 0, -0.5 );
  ground->setMotionControl( agx::RigidBody::STATIC );
  simulation->add( ground );
  agxOSG::setDiffuseColor( agxOSG::createVisual( ground, root ),
                           agxRender::Color::DarkBlue() );
 
  agx::RigidBodyRef box = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 0.5, 0.5, 0.5 ) ) );
  box->setPosition( 0, 0, 1.0 );
  simulation->add( box );
  agxOSG::setDiffuseColor( agxOSG::createVisual( box, root ),
                           agxRender::Color::LightGreen() );
 
  // Let box mass be 100 kg and gravity magnitude 10 for the
  // expected resting normal force to be 1 000 N.
  box->getMassProperties()->setMass( 100.0 );
  simulation->setUniformGravity( -10.0 * agx::Vec3::Z_AXIS() );
 
  // Displaying number of contact points, impact state, current- and
  // expected normal force of the box <-> ground interaction.
  auto displayNormalForce = [application]( agxSDK::Simulation* simulation )
  {
    auto sceneDecorator = application->getSceneDecorator();
    sceneDecorator->clearText();
    sceneDecorator->setText( 0, "Tutorial contact forces: Normal force." );
 
    const auto& geometryContacts = simulation->getSpace()->getGeometryContacts();
    if ( geometryContacts.empty() ) {
      sceneDecorator->setText( 1, "  - The box is currently not in contact with the ground." );
    }
    else {
      // Assuming only one contact when the box is interacting with the ground.
      auto geometryContact = geometryContacts.front();
      // Dynamic bodies are prioritized and assigned as first geometry/rigid body
      // in the geometry contact. Hence we know that 'box' is at index 0 and
      // 'ground' at index 1.
      //
      // When the box is resting on ground we expect the normal force to be:
      //   F_n = mass * |gravity|
      agx::Real expectedRestingNormalForce = geometryContact->rigidBody( 0 )->getMassProperties()->getMass() *
                                             simulation->getUniformGravity().length();
      agx::Real currentNormalForce{ 0 };
      for ( const auto& p : geometryContact->points() )
        currentNormalForce += p.getNormalForceMagnitude();
 
      sceneDecorator->setText( 1, agx::String::format( "  - Number of contact points:      %d", geometryContact->points().size() ) );
      sceneDecorator->setText( 2, agx::String::format( "  - Contains impacts:              %s", geometryContact->hasImpactingPoints() ?
                                                                                                  "true" :
                                                                                                  "false" ) );
      sceneDecorator->setText( 3, agx::String::format( "  - Current normal force:          %.10f N", currentNormalForce ) );
      sceneDecorator->setText( 4, agx::String::format( "  - Expected resting normal force: %.10f N", expectedRestingNormalForce ) );
    }
  };
  agxUtil::StepEventCallback::post( displayNormalForce, simulation );
  displayNormalForce( simulation );
 
  return root;
}
 
osg::Group* buildTutorial2( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group;
 
  agx::RigidBodyRef ground = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 10.0, 2.0, 0.5 ) ) );
  ground->setPosition( 8, 0, -0.5 );
  ground->setMotionControl( agx::RigidBody::STATIC );
  simulation->add( ground );
  agxOSG::setDiffuseColor( agxOSG::createVisual( ground, root ),
                           agxRender::Color::DarkBlue() );
 
  agx::RigidBodyRef box = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 0.5, 0.5, 0.5 ) ) );
  box->setPosition( 0, 0, 0.499 );
  simulation->add( box );
  agxOSG::setDiffuseColor( agxOSG::createVisual( box, root ),
                           agxRender::Color::LightGreen() );
 
  // Let box mass be 100 kg and gravity magnitude 10 for the
  // expected resting normal force to be 1 000 N.
  box->getMassProperties()->setMass( 100.0 );
  simulation->setUniformGravity( -10.0 * agx::Vec3::Z_AXIS() );
 
  // Create contact material between box and ground with 0.5 friction
  // coefficient and give the box some initial sliding speed.
  agx::MaterialRef groundMaterial = new agx::Material( "ground" );
  agx::MaterialRef boxMaterial = new agx::Material( "box" );
  agxUtil::setBodyMaterial( ground, groundMaterial );
  agxUtil::setBodyMaterial( box, boxMaterial );
 
  auto contactMaterial = simulation->getMaterialManager()->getOrCreateContactMaterial( groundMaterial, boxMaterial );
  contactMaterial->setFrictionCoefficient( 0.5 );
 
  // Displaying state sliding or sticking given current friction force,
  // current friction force and "theoretical" max friction force given
  // resting normal force.
  auto displayFrictionForce = [application, contactMaterial]( agxSDK::Simulation* simulation )
  {
    auto sceneDecorator = application->getSceneDecorator();
    sceneDecorator->clearText();
    sceneDecorator->setText( 0, "Tutorial contact forces: Friction force." );
 
    const auto& geometryContacts = simulation->getSpace()->getGeometryContacts();
    if ( geometryContacts.empty() ) {
      sceneDecorator->setText( 1, "  - The box is currently not in contact with the ground." );
    }
    else {
      // Expected and current normal force as in the "Normal forces" tutorial above.
      auto geometryContact = geometryContacts.front();
      agx::Real expectedRestingNormalForce = geometryContact->rigidBody( 0 )->getMassProperties()->getMass() *
                                             simulation->getUniformGravity().length();
 
      // Collecting current friction forces (magnitudes, i.e., ignoring sign).
      agx::Real currentFrictionForce{ 0 };
      for ( const auto& p : geometryContact->points() )
        currentFrictionForce += p.getTangentialForceMagnitude();
 
      // Given friction force: F_friction <= friction_coefficient * F_normal
      // the box is sliding when F_friction == friction_coefficient * F_normal and
      // sticking/not sliding when F_friction < friction_coefficient * F_normal.
      agx::Real maxFrictionForce = contactMaterial->getFrictionCoefficient() *
                                   expectedRestingNormalForce;
      sceneDecorator->setText( 1, agx::String::format( "  - Sliding:                    %s", agx::equivalent( maxFrictionForce,
                                                                                                              currentFrictionForce,
                                                                                                              1.0E-4 ) ?
                                                                                               "true" :
                                                                                               "false" ) );
      sceneDecorator->setText( 2, agx::String::format( "  - Current friction force:     %.5f N", currentFrictionForce ) );
      sceneDecorator->setText( 3, agx::String::format( "  - Max resting friction force: %.5f N", maxFrictionForce ) );
    }
  };
  agxUtil::StepEventCallback::post( displayFrictionForce, simulation );
 
  // Relaxing the system since we haven't calculated the theoretical
  // resting overlap between the ground and the box.
  simulation->stepTo( 1 );
 
  box->setVelocity( 10, 0, 0 );
 
  return root;
}
 
osg::Group* buildTutorial3( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group;
 
  agx::RigidBodyRef ground = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 4.0, 4.0, 0.5 ) ) );
  ground->setName( "Ground" );
  ground->setPosition( 0, 0, -0.5 );
  ground->setMotionControl( agx::RigidBody::STATIC );
  simulation->add( ground );
  agxOSG::setDiffuseColor( agxOSG::createVisual( ground, root ),
                           agxRender::Color::DarkBlue() );
 
  agx::RigidBodyRef box = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 0.25, 0.25, 1.25 ) ) );
  box->setName( "Box" );
  box->setPosition( 0, 0, 1.75 );
  box->setRotation( agx::Quat::rotate( agx::degreesToRadians( 17.0 ), agx::Vec3::Y_AXIS() ) );
  simulation->add( box );
  agxOSG::setDiffuseColor( agxOSG::createVisual( box, root ),
                           agxRender::Color::LightGreen() );
 
  agx::MaterialRef groundMaterial = new agx::Material( "ground" );
  agx::MaterialRef boxMaterial = new agx::Material( "box" );
  agxUtil::setBodyMaterial( ground, groundMaterial );
  agxUtil::setBodyMaterial( box, boxMaterial );
 
  // Creating contact material with restitution 0.5 and some allowed
  // overlap (5 mm) for the contact points to not come and go when
  // the box is falling over.
  auto contactMaterial = simulation->getMaterialManager()->getOrCreateContactMaterial( groundMaterial, boxMaterial );
  contactMaterial->setRestitution( 0.5 );
  contactMaterial->setAdhesion( 0.0, 5.0E-3 );
 
  // Displaying geometry contact state and the state each individual
  // contact point (is impacting, normal force and friction force).
  auto displayContactPointState = [application]( agxSDK::Simulation* simulation )
  {
    auto sceneDecorator = application->getSceneDecorator();
    sceneDecorator->clearText();
    sceneDecorator->setText( 0, "Tutorial contact forces: Friction force." );
 
    const auto& geometryContacts = simulation->getSpace()->getGeometryContacts();
    if ( geometryContacts.empty() ) {
      sceneDecorator->setText( 1, "  - The box is currently not in contact with the ground." );
    }
    else {
      auto geometryContact = geometryContacts.front();
      sceneDecorator->setText( 1, "  - Geometry contact state:" );
      sceneDecorator->setText( 2, agx::String::format( "    * Contains impacting contact points: %s", geometryContact->hasImpactingPoints() ?
                                                                                                        "true" :
                                                                                                        "false" ) );
      sceneDecorator->setText( 3, agx::String::format( "    * Interacting bodies:                %s <-> %s", geometryContact->rigidBody( 0 )->getName().c_str(),
                                                                                                             geometryContact->rigidBody( 1 )->getName().c_str() ) );
      sceneDecorator->setText( 4, agx::String::format( "    * Number of contact points:          %d", geometryContact->points().size() ) );
      for ( agx::UInt i = 0u; i < geometryContact->points().size(); ++i ) {
        const auto& p = geometryContact->points()[ i ];
        sceneDecorator->setText( 4 + (int)i + 1, agx::String::format( "      o Point %d: Is impacting [%s], Normal force [%4.2f N], Friction force [%4.2f N]",
                                                                      (int)i + 1,
                                                                      p.hasState( agxCollide::ContactPoint::IMPACTING ) ? "true" : "false",
                                                                      p.getNormalForceMagnitude(),
                                                                      p.getTangentialForceMagnitude() ),
                                 p.hasState( agxCollide::ContactPoint::IMPACTING ) ?
                                   agxRender::Color::MediumVioletRed() :
                                   agxRender::Color::LightGreen() );
      }
    }
  };
  agxUtil::StepEventCallback::post( displayContactPointState, simulation );
  displayContactPointState( simulation );
 
  return root;
}
 
osg::Group* buildTutorial4(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  osg::Group* root = new osg::Group;
 
  agx::RigidBodyRef ground = new agx::RigidBody(new agxCollide::Geometry(new agxCollide::Box(4.0, 4.0, 0.5)));
  ground->setName("Ground");
  ground->setPosition(0, 0, -0.5);
  ground->setMotionControl(agx::RigidBody::STATIC);
  simulation->add(ground);
  agxOSG::setDiffuseColor(agxOSG::createVisual(ground, root),
    agxRender::Color::DarkBlue());
 
  agx::RigidBodyRef box = new agx::RigidBody(new agxCollide::Geometry(new agxCollide::Box(0.25, 0.25, 1.25)));
  box->setName("Box");
  box->setPosition(0, 0, 1.75);
  box->setRotation(agx::Quat::rotate(agx::degreesToRadians(17.0), agx::Vec3::Y_AXIS()));
  simulation->add(box);
  agxOSG::setDiffuseColor(agxOSG::createVisual(box, root),
    agxRender::Color::LightGreen());
 
  agx::MaterialRef groundMaterial = new agx::Material("ground");
  agx::MaterialRef boxMaterial = new agx::Material("box");
  agxUtil::setBodyMaterial(ground, groundMaterial);
  agxUtil::setBodyMaterial(box, boxMaterial);
 
  // Creating contact material with restitution 0.5 and some allowed
  // overlap (5 mm) for the contact points to not come and go when
  // the box is falling over.
  auto contactMaterial = simulation->getMaterialManager()->getOrCreateContactMaterial(groundMaterial, boxMaterial);
  contactMaterial->setRestitution(0.5);
  contactMaterial->setAdhesion(0.0, 5.0E-3);
 
  agx::ContactForceReaderRef contactForceReader = new agx::ContactForceReader(simulation);
 
  // Use a ContactForceReader to extract the sum of normal and frictional forces between two rigid bodies:
  auto displayContactForces = [application](agxSDK::Simulation*, agx::ContactForceReaderRef contactForceReader, agx::RigidBody* ground, agx::RigidBody* box)
  {
    auto sceneDecorator = application->getSceneDecorator();
    sceneDecorator->clearText();
    sceneDecorator->setText(0, "Tutorial contact forces: ContactForceReader Friction force.");
 
    // Get the total normal force between the two rigid bodies:
    agx::Vec3 normalForce = contactForceReader->getNormalContactForces(ground, box);
 
    // Get the total frictional force between the two rigid bodies
    agx::Vec3 frictionForce = contactForceReader->getTangentialContactForces(ground, box);
 
    sceneDecorator->setText(1, agx::String::format("  - Current normal force:          %.10f N", normalForce.length()));
    sceneDecorator->setText(2, agx::String::format("  - Current friction force:          %.10f N", frictionForce.length()));
 
  };
  agxUtil::StepEventCallback::post(displayContactForces, simulation, contactForceReader, ground, box);
  displayContactForces(simulation, contactForceReader, ground, box);
 
  return root;
}
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n" <<
    "\tContact forces tutorial\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
    application->addScene(buildTutorial3, '3');
    application->addScene( buildTutorial4, '4' );
 
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_contactMeasurement.cpp

/*
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.
*/
 
 
/*
 
Demonstrates triangle mesh collision energy messurement.
 
*/
 
#include <agx/Logger.h>
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/Node.h>
#include <agxOSG/utils.h>
 
#include <agx/RigidBody.h>
#include <agxCollide/Box.h>
#include <agxCollide/Trimesh.h>
#include <agxModel/ContactMeasurement.h>
#include <agxUtil/TrimeshReaderWriter/TrimeshReaderWriter.h>
 
#include <agxRender/RenderManager.h>
#include <iomanip>
#include <agx/version.h>
#include <agx/Hinge.h>
#include <agxUtil/StepEventCallback.h>
#include <agx/RigidBodyEmitter.h>
 
osg::Group* buildTutorial1(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  osg::Group* root = new osg::Group;
 
  agxCollide::GeometryRef floorGeometry = new agxCollide::Geometry(new agxCollide::Box(10.0, 10.0, 0.2));
  agx::RigidBodyRef floor = new agx::RigidBody(floorGeometry);
  floor->setPosition(0.0, 0.0, -0.2);
  floor->setMotionControl(agx::RigidBody::STATIC);
  simulation->add(floor);
  agxOSG::setDiffuseColor(agxOSG::createVisual(floor, root),
    agxRender::Color::DarkBlue());
 
  agxCollide::GeometryRef boxGeometry = new agxCollide::Geometry(new agxCollide::Box(1.0, 1.0, 1.0));
  agx::RigidBodyRef box = new agx::RigidBody(boxGeometry);
  box->setPosition(1.0, 0.0, 5.0);
  box->setName("box");
  simulation->add(box);
  agx::RigidBodyEmitter::setRigidBodyIsEmitted(box, true);
  agxOSG::setDiffuseColor(agxOSG::createVisual(box, root),
    agxRender::Color::LightGreen());
 
  simulation->add(new agxModel::ContactMeasurement(false));
 
  // Displaying maximum impact energy, kinetic energy and mass of box
  auto displayMaxImpactEnergy = [application]( agxSDK::Simulation* simulation)
  {
    auto sceneDecorator = application->getSceneDecorator();
    sceneDecorator->clearText();
    sceneDecorator->setText( 0, "Tutorial contact measurement: Maximum impact energy." );
 
    // When the box is hitting the ground we expect the max impact to be:
    // kineticEnergyPostImpact = kineticEnergyPreImpact - maxImpactEnergy
    auto box = simulation->getRigidBody("box");
    auto boxMass = box->getMassProperties()->getMass();
    auto boxVelocity = box->getVelocity();
 
 
    sceneDecorator->setText( 1, agx::String::format( "  - Maximum impact energy :      %.10f J", agxModel::ContactMeasurement::getListener(simulation)->getMaxImpactEnergy(box) ) );
    sceneDecorator->setText( 2, agx::String::format( "  - Kinetic energy        :      %.10f J", (boxMass * boxVelocity.length2()) / 2));
    sceneDecorator->setText( 3, agx::String::format( "  - Box mass              :      %.10f Kg", boxMass));
  };
 
  agxUtil::StepEventCallback::post(displayMaxImpactEnergy, simulation );
  displayMaxImpactEnergy( simulation );
 
  return root;
}
 
int main(int argc, char** argv)
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tContact measurement tutorial\n" <<
    "\t-------------------------------------\n\n" << std::endl;
 
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene(buildTutorial1, '1');
 
    if (application->init(argc, argv))
      return application->run();
  }
  catch (std::exception& e) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_convex.cpp

/*
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.
*/
 
/*
 
This tutorial demonstrates how to use the Convex shape for collision detection.
 
*/
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/GeometryNode.h>
#include <agxOSG/ReaderWriter.h>
#include <agxIO/MeshReader.h>
#include <agxCollide/ConvexFactory.h>
 
#include <agxUtil/ConvexReaderWriter/ConvexReaderWriter.h>
 
#include <agxCollide/BasicPrimitiveTests.h>
#include <agxCollide/Line.h>
#include <agxCollide/LineClipping.h>
#include <agxCollide/PolygonClipping.h>
#include <agxCollide/Trimesh.h>
 
#include <agxRender/RenderSingleton.h>
#include <agx/Logger.h>
 
#include "tutorialUtils.h"
 
#include <ostream>
 
 
//
// Create vertices/indices to form a pyramid
//
void createPyramid(double base, double height, agx::Vec3Vector& vertices, agx::UInt32Vector& indices)
{
  vertices.clear();
  indices.clear();
 
  vertices.push_back(agx::Vec3(-base, -base, 0.0)); // four base corners
  vertices.push_back(agx::Vec3(-0.4, 0.4, 0.0));
  vertices.push_back(agx::Vec3(base, base, 0.0));
  vertices.push_back(agx::Vec3(base, -base, 0.0));
  vertices.push_back(agx::Vec3(0.0, 0.0, height));    // pyramid top
 
  // The triangles.
  indices.push_back(1);
  indices.push_back(0);
  indices.push_back(4);
 
  indices.push_back(2);
  indices.push_back(1);
  indices.push_back(4);
 
  indices.push_back(3);
  indices.push_back(2);
  indices.push_back(4);
 
  indices.push_back(0);
  indices.push_back(3);
  indices.push_back(4);
 
  indices.push_back(0);
  indices.push_back(1);
  indices.push_back(3);
 
  indices.push_back(3);
  indices.push_back(1);
  indices.push_back(2);
}
 
osg::Group* buildTutorial1( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // A Convex can be generated in a few different ways
 
  // =========================================================
  // Directly from a set of vertices known to be convex:
  // =========================================================
 
  agx::Vec3Vector vertices;
  agx::UInt32Vector indices;
 
  createPyramid( 0.4, 0.8, vertices, indices );
 
  // Create the collision shape
  agxCollide::ConvexRef convexMesh = new agxCollide::Convex( &vertices, &indices, "handmade pyramid" );
 
  // Create the rigid body and add the convex mesh to it
  agx::RigidBodyRef convexRigidBody = new agx::RigidBody(new agxCollide::Geometry(convexMesh));
  convexRigidBody->setPosition(-2, 0, 0.1);
  convexRigidBody->setName( "fromAConvexMesh" );
  agxOSG::createVisual( convexRigidBody, root );
  agxOSG::addText(convexRigidBody->getGeometries()[0], root, "Created from a convex mesh", agx::Vec3(0, 0, 1), agxRender::Color::Yellow());
 
  simulation->add( convexRigidBody );
 
  // =========================================================
  // From a set of points:
  // =========================================================
  vertices.clear();
 
  srand(5); // Deterministic result
  for(size_t i=0; i < 30; i++)
  {
    auto v = agx::Vec3::random( agx::Vec3(0, 0, 0), agx::Vec3(1, 1, 1) );
    vertices.push_back(v);
  }
 
  // Create the collision shape
  agxCollide::ConvexRef convexMesh1 = agxUtil::ConvexReaderWriter::createConvex(vertices);
 
  // Create the rigid body and add the convex mesh to it
  agx::RigidBodyRef convexRigidBody1 = new agx::RigidBody(new agxCollide::Geometry(convexMesh1));
  convexRigidBody1->setPosition(0, 0, 0.1);
  convexRigidBody1->setName("createConvexFromRandomPoints");
  agxOSG::createVisual(convexRigidBody1, root);
  agxOSG::addText(convexRigidBody1->getGeometries()[0], root, "Created from random points", agx::Vec3(0, 0, 1), agxRender::Color::Yellow());
 
  simulation->add( convexRigidBody1 );
 
 
  // =========================================================
  // From a mesh file:
  // =========================================================
 
  // Create the collision shape
  agxCollide::ConvexRef convexMesh2 = agxUtil::ConvexReaderWriter::createConvex("models/torusForCollision.obj");
 
  // Create the rigid body and add the convex mesh to it
  agx::RigidBodyRef convexRigidBody2 = new agx::RigidBody(new agxCollide::Geometry(convexMesh2));
  convexRigidBody2->setPosition(4, 0, 2);
  convexRigidBody2->setName("CreatedFromMeshFile");
  agxOSG::createVisual(convexRigidBody2, root);
  agxOSG::addText(convexRigidBody2->getGeometries()[0], root, "Created from Mesh file", agx::Vec3(0, 0, 1), agxRender::Color::Yellow());
 
  simulation->add(convexRigidBody2);
 
  // Create the static rigid body (a floor onto which all other objects will fall)
  agx::RigidBodyRef staticRB = new agx::RigidBody();
  staticRB->setMotionControl( agx::RigidBody::STATIC );
  agxCollide::GeometryRef staticBodyGeometry = new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 15.0, 15.0, 0.2 ) ) );
  agxOSG::setDiffuseColor(agxOSG::createVisual(staticBodyGeometry, root), agxRender::Color::DarkBlue());
 
  staticRB->add( staticBodyGeometry );
  simulation->add(staticRB);
 
  return root;
}
 
 
osg::Group* buildTutorial2( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
 
  std::cerr << "Creating Convex Decompositions" << std::endl;
  for (size_t i = 0; i < 5; i++)
  {
    size_t resolution = 5+i*10;
 
    agxCollide::ConvexRefVector result;
    agx::Timer timer(true);
    // Create the collision shape
    if (!agxUtil::ConvexReaderWriter::createConvexDecomposition("models/torusForCollision.obj", result, resolution))
      LOGGER_ERROR() << "Error creating convex decomposition" << LOGGER_ENDL();
 
    timer.stop();
    std::cout << "\tResolution " << resolution << " Time: " << timer.getTime() / 1000 << " Seconds." << std::endl;
    agx::RigidBodyRef convexRigidBody = new agx::RigidBody();
    for (auto c : result)
    {
      auto geometry = new agxCollide::Geometry(c);
      convexRigidBody->add(geometry);
    }
    // Create the rigid body and add the handmade triangle mesh to it.
    convexRigidBody->setPosition(-5.0+static_cast<agx::Real>(i)*3.0, 0, 2);
    convexRigidBody->setName("ConvexDecomposition");
    agxOSG::setDiffuseColor(agxOSG::createVisual(convexRigidBody, root), agxRender::Color::getColor(i));
    agxOSG::addText(convexRigidBody->getGeometries()[0], root, agx::String::format("ConvexDecomposition Resolution: %d", resolution), agx::Vec3(0, 0, 1), agxRender::Color::Yellow());
 
    simulation->add(convexRigidBody);
  }
  // Create the static rigid body (a floor onto which all other objects will fall)
  agx::RigidBodyRef staticRB = new agx::RigidBody();
  staticRB->setMotionControl(agx::RigidBody::STATIC);
  agxCollide::GeometryRef staticBodyGeometry = new agxCollide::Geometry(new agxCollide::Box(agx::Vec3(20.0, 15.0, 0.2)));
  agxOSG::setDiffuseColor(agxOSG::createVisual(staticBodyGeometry, root), agxRender::Color::DarkBlue());
 
  staticRB->add(staticBodyGeometry);
  simulation->add(staticRB);
 
  return root;
}
 
namespace {
 
  struct Settings {
    int resolution = 0;
    double alpha = 0;
    double beta = 0;
    double concavity = 0;
    int numVertices = 0;
 
    Settings(int res, double a, double b, double c, int n) :
      resolution(res),
      alpha(a),
      beta(b),
      concavity(c),
      numVertices(n)
    {
 
    }
 
    Settings(const Settings& s) :
      resolution(s.resolution),
      alpha(s.alpha),
      beta(s.beta),
      concavity(s.concavity),
      numVertices(s.numVertices)
    {
 
    }
  };
 
  std::ostream& operator <<(std::ostream& os, const Settings& s)
  {
    os << agx::String::format("Resolution=%d, alpha=%f, beta=%f, concavity=%f, maxVertices=%d", s.resolution, s.alpha, s.beta, s.concavity, s.numVertices);
    return os;
  }
}
 
class ConvexCooker : public agx::BasicThread, public agx::Referenced
{
public:
  ConvexCooker(const agxIO::MeshReader* reader, const Settings& s) : BasicThread(), m_reader(reader), m_settings(s), m_time(0)
  {
  }
 
  double getTime() const
  {
    return m_time;
  }
 
  Settings getSettings() const
  {
    return m_settings;
  }
  agxCollide::ConvexRefVector getConvexShapes()
  {
    if (!m_factory)
      return agxCollide::ConvexRefVector();
 
    return m_factory->getConvexShapes();
  }
 
  void run() override
  {
    agx::Thread::registerAsAgxThread();
    m_factory = new agxCollide::ConvexFactory();
 
    agxCollide::ConvexFactory::VHACDParametersRef parameters = new agxCollide::ConvexFactory::VHACDParameters();
    parameters->resolution = m_settings.resolution;
    parameters->alpha = m_settings.alpha;
    parameters->beta = m_settings.beta;
    parameters->concavity = m_settings.concavity;
    parameters->maxNumVerticesPerConvex = m_settings.numVertices;
 
    agx::Timer timer(true);
    m_factory->build(m_reader, parameters, agx::Matrix3x3(agx::EulerAngles(agx::PI_2, 0, 0)) * agx::Matrix3x3(agx::Vec3(10, 10, 10)));
    timer.stop();
    m_time = timer.getTime() / 1000;
    agx::Thread::unregisterAsAgxThread();
 
  }
 
private:
  agxCollide::ConvexFactoryRef m_factory;
  const agxIO::MeshReader* m_reader;
  const Settings& m_settings;
  double m_time;
};
typedef agx::ref_ptr<ConvexCooker> ConvexCookerRef;
 
 
osg::Group* buildTutorial3(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/)
{
  osg::Group* root = new osg::Group();
 
 
  agxIO::MeshReaderRef meshReader = new agxIO::MeshReader();
  if (!meshReader->readFile("models/bunny.obj"))
    LOGGER_ERROR() << "Error reading mesh model" << LOGGER_ENDL();
 
  agxCollide::ConvexFactoryRef factory = new agxCollide::ConvexFactory();
 
 
 
  std::vector<Settings> settings = {
    Settings({5,0,0,0.1,10}),
    Settings({20,0.01,0.01,0.05,1000}),
    Settings({30,0.001,0.01,0.01,1000})
  };
 
  agx::Vector<ConvexCookerRef> cookers;
 
  std::cerr << "Creating Convex Decompositions..." << std::endl;
  for (const auto& s : settings)
  {
    auto cooker = new ConvexCooker(meshReader, s);
    cookers.push_back(cooker);
    cooker->start();
  }
 
  // Waiting for the threads to finalize
  std::for_each(cookers.begin(), cookers.end(), [](ConvexCooker* c) { c->join(); } );
 
  int i = 0;
  for (auto t : cookers)
  {
    std::cerr << t->getSettings() << ": " << t->getTime() << std::endl;
 
    agx::RigidBodyRef convexRigidBody = new agx::RigidBody();
    for (auto c : t->getConvexShapes())
    {
      auto geometry = new agxCollide::Geometry(c);
      convexRigidBody->add(geometry);
    }
    // Create the rigid body and add the handmade triangle mesh to it.
    convexRigidBody->setPosition(-5.0 + i * 3.0, 0, 2);
    convexRigidBody->setName("ConvexDecomposition");
    agxOSG::setDiffuseColor(agxOSG::createVisual(convexRigidBody, root), agxRender::Color::getColor(i));
    std::stringstream str;
    str << t->getSettings() << std::ends;
    agxOSG::addText(convexRigidBody->getGeometries()[0], root, str.str(), agx::Vec3(0, 0, 1), agxRender::Color::Yellow());
 
    simulation->add(convexRigidBody);
 
    i++;
  }
  cookers.clear();
 
  // Create the static rigid body (a floor onto which all other objects will fall)
  agx::RigidBodyRef staticRB = new agx::RigidBody();
  staticRB->setMotionControl(agx::RigidBody::STATIC);
  agxCollide::GeometryRef staticBodyGeometry = new agxCollide::Geometry(new agxCollide::Box(agx::Vec3(20.0, 15.0, 0.2)));
  agxOSG::setDiffuseColor(agxOSG::createVisual(staticBodyGeometry, root), agxRender::Color::DarkBlue());
 
  staticRB->add(staticBodyGeometry);
  simulation->add(staticRB);
 
  return root;
}
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial Convex\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
    application->addScene( buildTutorial3, '3' );
 
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}
 

tutorial_conveyorBelts.cpp

/*
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.
*/
 
/*
 
Demonstrates various ways of modelling conveyorbelts using AGX Dynamics
 
*/
 
#include <agxCollide/Geometry.h>
#include <agxCollide/Box.h>
#include <agxCollide/Sphere.h>
#include <agx/RigidBody.h>
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/Node.h>
#include <agxOSG/utils.h>
#include <agx/Physics/GranularBodySystem.h>
#include <agxRender/RenderSingleton.h>
#include <agxUtil/TrimeshReaderWriter/TrimeshReaderWriter.h>
#include <agx/version.h>
 
/*
First example deriving from agxCollide::Geometry and implement the calculateSurfaceVelocity method!
In this example we try to generate a surface "field" specified with points which the objects should try to follow.
 
This is very similar to the agxModell:SurfaceConveyorBelt class which can be used as a Geometry.
*/
AGX_DECLARE_POINTER_TYPES(ConveyorBelt);
class ConveyorBelt : public agxCollide::Geometry
{
public:
  ConveyorBelt(const agx::Vec3Vector& points) : Geometry(), m_points(points), m_speed(0)
  {
    if (points.size() > 0) {
      m_unitDirs.reserve(points.size());
      for (size_t i = 0; i < points.size() - 1; ++i)  {
        agx::Vec3 unitDir = m_points[i + 1] - m_points[i];
        unitDir.normalize();
        m_unitDirs.push_back(unitDir);
        agxRender::RenderSingleton::instance()->addStatic(m_points[i], 0.02, agx::Vec4f(1, 0, 0, 1));
        agxRender::RenderSingleton::instance()->addStatic(m_points[i], m_points[i]+ unitDir * 0.2, 0.01, agx::Vec4f(1, 1, 0, 1));
      }
      {
        agx::Vec3 unitDir = m_points.front() - m_points.back();
        unitDir.normalize();
        m_unitDirs.push_back(unitDir);
        agxRender::RenderSingleton::instance()->addStatic(m_points.back(), 0.02, agx::Vec4f(1, 0, 0, 1));
        agxRender::RenderSingleton::instance()->addStatic(m_points.back(), m_points.back() + unitDir * 0.2, 0.01, agx::Vec4f(1, 1, 0, 1));        
      }
    }
    this->setSurfaceVelocity(agx::Vec3f(1.0f, 0.0f, 0.0f)); // Have to do this to work around some optimization later.
  }
 
  agx::Vec3f calculateSurfaceVelocity( const agxCollide::LocalContactPoint& point, size_t /*index*/ ) const override
  {
 
    if (m_points.size() > 2 && m_speed != agx::Real(0)) {
      // Find closest point. \todo This can be sped up.
      const agx::Vec3 p = point.point();
      size_t closestIndex = 0;
      agx::Real closestDist2 = agx::RealMax;
      for (size_t i = 0; i < m_points.size(); ++i) {
        agx::Real dist2 = p.distance2(m_points[i]);
        if (dist2 < closestDist2) {
          closestDist2 = dist2;
          closestIndex = i;
        }
      }
 
      // Now compute surface velocity given closest point.
      // We want the following behavior:
      // If we are closer to a point just before the current one in the old direction, then we take the old direction.
      // If we are closer to a point just after the current one in the new direction, then we take the new direction.
      const agx::Real eps(1e-5);
      const size_t indexBefore = (closestIndex == 0) ? m_points.size() - 1 : closestIndex - 1;
      const agx::Vec3& oldDir = m_unitDirs[indexBefore];
      const agx::Vec3& newDir = m_unitDirs[closestIndex];
      const agx::Real oldDist2 = (p - (m_points[closestIndex] - oldDir * eps)).length2();
      const agx::Real newDist2 = (p - (m_points[closestIndex] + newDir * eps)).length2();
      const agx::Vec3& dir = (oldDist2 < newDist2) ? oldDir : newDir;
 
      agx::Vec3f v = agx::Vec3f(dir * m_speed);
      return v;
    }
    else {
      return agx::Vec3f(0.0f);
    }
  }
 
  const agx::Vec3Vector& getPoints() const
  {
    return m_points;
  }
 
  const agx::Vec3Vector& getDistances() const
  {
    return m_unitDirs;
  }
 
  void setSpeed(agx::Real speed)
  {
    m_speed = speed;
  }
 
  agx::Real getSpeed() const
  {
    return m_speed;
  }
 
 
private:
  virtual ~ConveyorBelt()
  {
  }
  agx::Vec3Vector m_points;
  agx::Vec3Vector m_unitDirs;
  agx::Real m_speed;
};
 
 
// Create a "ConveyorBelt" with a number of points. The shape of the conveyor is a Box.
ConveyorBeltRef createConveyorBelt()
{
  agx::Vec3Vector points;
  points.push_back(agx::Vec3(0, 0, 0));
  points.push_back(agx::Vec3(0, -1, 0));
  points.push_back(agx::Vec3(1, -1, 0));
  points.push_back(agx::Vec3(2, -1, 0));
  points.push_back(agx::Vec3(2, -2, 0));
  points.push_back(agx::Vec3(1, -2, 0));
  points.push_back(agx::Vec3(0, -2, 0));
  points.push_back(agx::Vec3(-1, -2, 0));
  points.push_back(agx::Vec3(-2, -2, 0));
  points.push_back(agx::Vec3(-2, -1, 0));
  points.push_back(agx::Vec3(-2, 0, 0));
  points.push_back(agx::Vec3(-2, 1, 0));
  points.push_back(agx::Vec3(-2, 2, 0));
  points.push_back(agx::Vec3(-1, 2, 0));
  points.push_back(agx::Vec3(0, 2, 0));
  points.push_back(agx::Vec3(1, 2, 0));
  points.push_back(agx::Vec3(2, 2, 0));
  points.push_back(agx::Vec3(2, 1, 0));
  points.push_back(agx::Vec3(1, 1, 0));
  points.push_back(agx::Vec3(0, 1, 0));
 
  ConveyorBeltRef conveyorBelt = new ConveyorBelt(points);
  conveyorBelt->setSpeed(1);
 
  // The shape for this conveyor is a Box.
  conveyorBelt->add( new agxCollide::Box( agx::Vec3(3, 3, 0.1)) );
  return conveyorBelt;
}
 
// Create some materials to be used in this example
agx::MaterialRef createMaterials(agxSDK::Simulation* sim)
{
  agx::MaterialRef mat = new agx::Material("mat");
  sim->add(mat);
  agx::ContactMaterialRef cm = new agx::ContactMaterial(mat, mat);
  sim->add(cm);
  agx::FrictionModelRef fm = new agx::IterativeProjectedConeFriction();
  cm->setFrictionModel(fm);
  cm->setFrictionCoefficient(1.0);
  return mat;
}
 
osg::Group* ConveyorBeltWithSurfaceVelocity( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*app*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create a static body to act as a ground
  agx::RigidBodyRef groundBody = new agx::RigidBody;
  groundBody->setMotionControl(agx::RigidBody::STATIC);
  
  // Create our ConveyorBelt class (which is a geometry)
  ConveyorBeltRef conveyorBelt = createConveyorBelt();
 
  // Add the ConveyorBelt geometry to the body
  groundBody->add(conveyorBelt);
 
  // Add to simulation and make it visible
  simulation->add(groundBody);
  agxOSG::createVisual(conveyorBelt, root);
 
  agx::MaterialRef mat = createMaterials(simulation);
  conveyorBelt->setMaterial(mat);
 
  // Now create a few objects that will move around on this Conveyor.
  {
    agx::RigidBodyRef body = new agx::RigidBody;
    agxCollide::GeometryRef geom = new agxCollide::Geometry( new agxCollide::Box( agx::Vec3(0.2)));
    body->add( geom );
    simulation->add(body);
    geom->setPosition( agx::Vec3(-1, 0, 0.3 ));
    geom->setMaterial(mat);
    agxOSG::createVisual(body, root);
  }
 
  {
    agx::RigidBodyRef body = new agx::RigidBody;
    agxCollide::GeometryRef geom = new agxCollide::Geometry( new agxCollide::Sphere( 0.2));
    body->add( geom );
    simulation->add(body);
    geom->setPosition( agx::Vec3(1, 0, 0.3 ));
    geom->setMaterial(mat);
    agxOSG::createVisual(body, root);
  }
  return root;
}
 
 
// Create another scene with the same conveyor but with more objects moving around, including particles.
osg::Group* ConveyorBeltWithSurfaceVelocityAndParticles( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*app*/ )
{
  osg::Group* root = new osg::Group();
  agx::RigidBodyRef groundBody = new agx::RigidBody;
  ConveyorBeltRef conveyorBelt = createConveyorBelt();
  groundBody->add( conveyorBelt );
  simulation->add(groundBody);
  groundBody->setMotionControl(  agx::RigidBody::STATIC );
  agxOSG::createVisual(conveyorBelt, root);
  agx::MaterialRef mat = createMaterials(simulation);
  conveyorBelt->setMaterial(mat);
 
  // Particle system
  agx::ParticleSystemRef system = new agx::Physics::GranularBodySystem();
  system->setParticleRadius((agx::Real)0.05);
  system->setParticleMass((agx::Real)0.1);
  agxOSG::createVisual(system, root);
  system->setMaterial(mat);
 
  // Create emitter
  agxCollide::GeometryRef sphere = new agxCollide::Geometry(new agxCollide::Sphere(0.1));
  sphere->setPosition(0, 0, 0.5);
  agx::ParticleEmitterRef emitter = new agx::ParticleEmitter(system);
  emitter->setGeometry(sphere);
  emitter->setRate(10);
  emitter->setLife(1000);
 
  simulation->add(system);
  simulation->add(emitter);
 
  return root;
}
 
// This class calculates a circular surface speed on a geometry
// Outside a radius of 0.8 the speed should be zero
class CircularConveyorBelt : public agxCollide::Geometry
{
public:
  CircularConveyorBelt()
  {}
 
  agx::Vec3f calculateSurfaceVelocity(const agxCollide::LocalContactPoint& point, size_t /*index*/) const override
  {
    double speed = point.point().length();
 
    // Cut of speed if distance from origo is equal to or larger than 0.8m
    if (speed >= 0.8)
      speed = 0;
 
    // Calculate the tangent direction
    agx::Vec3 direction = point.point();
    direction[2] = 0;
    direction.normalize();
 
    // Rotate 90 deg in clockwise direction: swap x and y and negate x.
    direction.set(direction[1], -direction[0], 0);
    
    return agx::Vec3f(direction * speed);
  }
};
 
osg::Group* CircularConveyorBeltWithSurfaceVelocityAndParticles(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*app*/)
{
  osg::Group* root = new osg::Group();
  
  // Create a static ground body
  agx::RigidBodyRef groundBody = new agx::RigidBody;
  groundBody->setMotionControl(agx::RigidBody::STATIC);
 
  // Create our "Conveyor" which is a geometry with a custom calculateSurfaceSpeed
  agx::ref_ptr<CircularConveyorBelt> conveyorBelt = new CircularConveyorBelt();
  
  // The shape of the conveyor should be a box
  conveyorBelt->add(new agxCollide::Box(2, 2, 0.1));
  groundBody->add(conveyorBelt);
  simulation->add(groundBody);
  agxOSG::createVisual(conveyorBelt, root);
 
  agx::MaterialRef mat = createMaterials(simulation);
  conveyorBelt->setMaterial(mat);
 
  // Particle system
  agx::ParticleSystemRef system = new agx::Physics::GranularBodySystem();
  system->setParticleRadius((agx::Real)0.05);
  system->setParticleMass((agx::Real)0.1);
  agxOSG::createVisual(system, root);
  system->setMaterial(mat);
 
  // Create emitter
  agxCollide::GeometryRef sphere = new agxCollide::Geometry(new agxCollide::Sphere(0.1));
  sphere->setPosition(0, 0, 0.5);
  agx::ParticleEmitterRef emitter = new agx::ParticleEmitter(system);
  emitter->setGeometry(sphere);
  emitter->setRate(10);
  emitter->setLife(1000);
 
  simulation->add(system);
  simulation->add(emitter);
 
  return root;
}
 
// This class calculates a Curved surface speed on a geometry
// Outside a radius of 1.6 the speed should be zero
class CurveConveyorBelt : public agxCollide::Geometry
{
public:
  CurveConveyorBelt()
  {}
 
  agx::Vec3f calculateSurfaceVelocity(const agxCollide::LocalContactPoint& point, size_t /*index*/) const override
  {
    agx::Vec3 localOrigin = getPosition();
    double speed = (point.point() - localOrigin).length();
 
    // Cut of speed if distance from origo is equal to or larger than 1.6m
    if (speed >= 1.6)
      speed = 0;
 
    // Calculate the tangent direction
    agx::Vec3 direction = point.point() - localOrigin;
    direction[2] = 0;
    direction.normalize();
 
    // Rotate 90 deg in clockwise direction: swap x and y and negate x.
    direction.set(direction[1], -direction[0], 0);
 
    return agx::Vec3f(direction * speed);
  }
};
 
osg::Group* CurvedConveyorBeltWithSurfaceVelocityAndParticles(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*app*/)
{
  osg::Group* root = new osg::Group();
 
  agx::MaterialRef mat = createMaterials(simulation);
 
  // Create linear conveyor
  agx::RigidBodyRef convBody = new agx::RigidBody;
  convBody->setMotionControl(agx::RigidBody::STATIC);
  agxCollide::GeometryRef convGeo = new agxCollide::Geometry(new agxCollide::Box(0.4, 1, 0.1));
  convGeo->setSurfaceVelocity(agx::Vec3f(0.0, -1.0, 0.0));
  convBody->add(convGeo);
  convBody->setPosition(2.2, 0, 0);
  simulation->add(convBody);
  agxOSG::createVisual(convBody, root);
  convGeo->setMaterial(mat);
 
  // Create a static ground body
  agx::RigidBodyRef groundBody = new agx::RigidBody;
  groundBody->setMotionControl(agx::RigidBody::STATIC);
 
  // Create our "Conveyor Curve" which is a geometry with a custom calculateSurfaceSpeed
  agx::ref_ptr<CurveConveyorBelt> conveyorBelt = new CurveConveyorBelt();
  conveyorBelt->setSurfaceVelocity(agx::Vec3f(1, 1, 1));
 
  // The shape of the conveyor should be a curve, let's read a meshfile:
  agxCollide::TrimeshRef curveMesh = agxUtil::TrimeshReaderWriter::createTrimesh("models/conveyorCurve_r1.6.stl");
  conveyorBelt->add(curveMesh);
  groundBody->add(conveyorBelt);
  groundBody->setPosition(1.0, -1.0, 0);
  simulation->add(groundBody);
  agxOSG::createVisual(conveyorBelt, root);
 
 
  conveyorBelt->setMaterial(mat);
 
  // Create a kinematic body
  agx::RigidBodyRef canBody = new agx::RigidBody;
  agxCollide::GeometryRef canGeo = new agxCollide::Geometry(new agxCollide::Box(0.05, 0.05, 0.05));
  canGeo->setMaterial(mat);
  canBody->add(canGeo);
  canBody->setPosition(2.2, 0.8, 0.3);
  simulation->add(canBody);
  agxOSG::createVisual(canBody, root);
 
  // Particle system
  agx::ParticleSystemRef system = new agx::Physics::GranularBodySystem();
  system->setParticleRadius((agx::Real)0.05);
  system->setParticleMass((agx::Real)0.1);
  agxOSG::createVisual(system, root);
  system->setMaterial(mat);
 
  // Create emitter
  agxCollide::GeometryRef sphere = new agxCollide::Geometry(new agxCollide::Sphere(0.05));
  sphere->setPosition(2.2, 0.4, 0.4);
  agx::ParticleEmitterRef emitter = new agx::ParticleEmitter(system);
  emitter->setGeometry(sphere);
  emitter->setRate(10);     // particles per second
  emitter->setLife(1000);
 
  simulation->add(system);
  simulation->add(emitter);
 
  return root;
}
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout << "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial conveyorBelts\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
    application->addScene(ConveyorBeltWithSurfaceVelocity, '1');
    application->addScene(ConveyorBeltWithSurfaceVelocityAndParticles, '2');
    application->addScene(CircularConveyorBeltWithSurfaceVelocityAndParticles, '3');
    application->addScene(CurvedConveyorBeltWithSurfaceVelocityAndParticles, '4');
 
    if (application->init( argc, argv ))
      return application->run();
  } catch (std::exception& e)
  {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}
 

tutorial_create_oriented_bounding_volume.cpp

/*
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.
*/
 
 
 
/*
 
Demonstrates how to create and use oriented bounding volumes as proxy for mesh objects
 
*/
 
 
#include <agx/agx.h>
#include <agx/version.h>
 
#include <agxCollide/Geometry.h>
#include <agxCollide/Box.h>
#include <agxUtil/TrimeshHelperFunctions.h>
#include <agxUtil/TrimeshReaderWriter/TrimeshReaderWriter.h>
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/CableDamageRenderer.h>
#include <agxOSG/Node.h>
#include <agxOSG/utils.h>
 
#include <agxIO/ArgumentParser.h>
 
#include <agxCable/Cable.h>
#include <agxCable/Node.h>
#include <agxCable/CableDamage.h>
#include <agxUtil/StepEventCallback.h>
 
 
 
void createBoundingVolume(
  agxSDK::Simulation *simulation, 
  osg::Group *root, 
  const agxCollide::CollisionMeshData* mesh_data, 
  agx::Vec3 translate, 
  std::function< bool(const agx::Vec3Vector&, agx::Vec2&, agx::AffineMatrix4x4&, agxUtil::ShapeOrientation::Orientation) >& computeBOV, 
  std::function< agxCollide::Shape*(double, double) >& createShape,
  agxUtil::ShapeOrientation::Orientation orientation = agxUtil::ShapeOrientation::MINIMIZE_VOLUME)
{
  // Create a trimesh with the original mesh model
  agxCollide::TrimeshRef trimesh = new agxCollide::Trimesh(&mesh_data->getVertices(), 
                                                           &mesh_data->getIndices(), "", 
                                                           agxCollide::Trimesh::NO_WARNINGS | agxCollide::Trimesh::REMOVE_DUPLICATE_VERTICES);
 
  auto transform_bunny = agx::AffineMatrix4x4::rotate(agx::EulerAngles(agx::PI_2, 0, 0)) * agx::AffineMatrix4x4::translate(translate);
  auto geom1 = new agxCollide::Geometry(trimesh, transform_bunny);
  osg::Node* node = agxOSG::createVisual(geom1, root);
  agxOSG::setDiffuseColor(node, agxRender::Color::Red());
  simulation->add(geom1);
 
  // Prepare the arguments
  auto radiusHeight = agx::Vec2();
  auto localTransform = agx::AffineMatrix4x4();
 
  // Compute the bounding volume
  agxVerify(computeBOV(mesh_data->getVertices(), radiusHeight, localTransform, orientation));
 
  // Create the shape based on the result
  auto boundingVolumeShape = createShape(radiusHeight[0], radiusHeight[1]);
 
  // Create a geometry with the shape including the localTransformation from the BOV calculation
  // plus the transformation for the original mesh model
  auto geom = new agxCollide::Geometry(boundingVolumeShape, localTransform * transform_bunny);
  simulation->add(geom);
 
  node = agxOSG::createVisual(geom, root);
  agxOSG::forceWireFrameModeOn(node);
  agxOSG::setDiffuseColor(node, agxRender::Color::Yellow());
}
 
osg::Group* buildTutorial1(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  std::cout << "Creating oriented bounding volumes from mesh shapes\n";
 
  osg::Group* root = new osg::Group();
 
  std::string filename = "models/bunny.obj";
  double scale = 4;
 
  agxCollide::Trimesh* trimesh = agxUtil::TrimeshReaderWriter::createTrimesh(
    filename,
    agxCollide::Trimesh::REMOVE_DUPLICATE_VERTICES | agxCollide::Trimesh::NO_WARNINGS,
    agx::Matrix3x3(agx::Vec3(scale)));
  
  agxVerify(trimesh);
 
  //  Create a ground plane
  auto ground = new agxCollide::Geometry(new agxCollide::Box(10, 2, 0.03));
  ground->setPosition(0, 0, 0);
  auto node = agxOSG::createVisual(ground, root);
  agxOSG::setDiffuseColor(node, agxRender::Color::White());
  agxOSG::setShininess(node, 128);
  simulation->add(ground);
 
  //  Rotate so that the bunny is standing up in Z.
  auto rotateX = agx::AffineMatrix4x4::rotate(agx::EulerAngles(agx::PI_2, 0, 0));
 
  auto mesh_geom = new agxCollide::Geometry(trimesh, rotateX);
  node = agxOSG::createVisual(mesh_geom, root);
  agxOSG::setDiffuseColor(node, agxRender::Color::Red());
  simulation->add(mesh_geom);
 
  // Get the mesh dataand vertices from the mesh shape
  auto mesh_data = trimesh->getMeshData();
  const agx::Vec3Vector& vertices = mesh_data->getVertices();
 
  // Prepare result variables
  // Contains the resulting transform of the oriented box
  auto transform = agx::AffineMatrix4x4();
 
  // Contains the size of the oriented box
  auto halfExtents = agx::Vec3();
  agxVerify(agxUtil::computeOrientedBox(vertices, halfExtents, transform));
 
  // Create a box primitive based on the computed size
  auto box = new agxCollide::Box(halfExtents);
 
  // Create a box with the same rotation as the mesh bunny
  auto box_geometry = new agxCollide::Geometry(box, transform * rotateX);
  simulation->add(box_geometry);
 
  auto geom_node = agxOSG::createVisual(box_geometry, root);
  agxOSG::setDiffuseColor(geom_node, agxRender::Color::Yellow());
  agxOSG::setEnableWireFrameMode(geom_node, true);
 
  auto axes_node = agxOSG::createAxes(transform * rotateX, nullptr, 0.5);
  root->addChild(axes_node);
  
 
  std::function<bool(const agx::Vec3Vector&, agx::Vec2&, agx::AffineMatrix4x4&, agxUtil::ShapeOrientation::Orientation )> 
    computeOrientedCylinder = [](const agx::Vec3Vector& vertices, 
                                 agx::Vec2& radiusHeight, 
                                 agx::AffineMatrix4x4& localRotation, 
                                 agxUtil::ShapeOrientation::Orientation orientation) -> bool
  { 
    return agxUtil::computeOrientedCylinder(vertices, radiusHeight, localRotation, orientation); 
  };
 
  std::function<bool(const agx::Vec3Vector&, agx::Vec2&, agx::AffineMatrix4x4&, agxUtil::ShapeOrientation::Orientation)> 
    computeOrientedCapsule = [](const agx::Vec3Vector& vertices, 
                                agx::Vec2& radiusHeight, 
                                agx::AffineMatrix4x4& localRotation, 
                                agxUtil::ShapeOrientation::Orientation orientation) -> bool
  { 
    return agxUtil::computeOrientedCapsule(vertices, radiusHeight, localRotation, orientation); 
  };
 
  std::function<agxCollide::Shape* (double, double)> 
    createCylinder = [](double radius,
                        double height) -> agxCollide::Shape*
  {
    return new agxCollide::Cylinder(radius, height);
  };
 
  std::function<agxCollide::Shape* (double, double)> 
    createCapsule = [](double radius, 
                       double height) -> agxCollide::Shape*
  {
    return new agxCollide::Capsule(radius, height);
  };
 
  // Create an oriented capsule, compute primary direction that results in the minimal volume of a cylinder
  createBoundingVolume(simulation, root, mesh_data, agx::Vec3(1, 0, 0), computeOrientedCylinder, createCylinder);
 
  // Create a oriented cylinder but along the local X axis of the oriented box
  createBoundingVolume(simulation, root, mesh_data, agx::Vec3(2, 0, 0), computeOrientedCylinder, createCylinder, agxUtil::ShapeOrientation::X_AXIS);
 
  // Create an oriented capsule, compute primary direction that results in the minimal volume(of a cylinder)
  createBoundingVolume(simulation, root, mesh_data, agx::Vec3(3, 0, 0), computeOrientedCapsule, createCapsule);
 
  // Create an oriented capsule, along the Z axis of the oriented box
  createBoundingVolume(simulation, root, mesh_data, agx::Vec3(4, 0, 0), computeOrientedCapsule, createCapsule, agxUtil::ShapeOrientation::Z_AXIS);
 
  auto sd = application->getSceneDecorator();
  sd->setBackgroundColor(agxRender::Color::White());
  sd->setEnableShadows(true);
  sd->setShadowMethod(agxOSG::SceneDecorator::SOFT_SHADOWMAP);
 
  return root;
}
 
 
 
int main(int argc, char** argv)
{
  agx::AutoInit init;
 
  std::cout << '\t' << agxGetLibraryName() << ' ' << agxGetVersion() << ' ' << agxGetCompanyName() << "(C)\n "
            << "\tTutorial oriented bounding volumes\n" << "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication();
 
    application->addScene(buildTutorial1, '1');
 
    if (application->init(argc, argv)) {
      return application->run();
    }
  }
  catch (std::exception& e) {
    std::cerr << "\nMain caught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_customConstraints.cpp

/*
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.
*/
 
#include <agxOSG/ExampleApplication.h>
 
#include <agxUtil/agxUtil.h>
 
#include <agx/Logger.h>
#include <agx/Constraint.h>
#include <agx/ElementaryConstraint.h>
#include <agx/ConstraintImplementation.h>
#include <agx/LockJoint.h>
#include <agx/LSquareComplianceMatrix.h>
 
#include <agxRender/RenderSingleton.h>
 
#include "tutorialUtils.h"
 
 
 
 
using namespace agx;
using namespace agxOSG;
 
 
// Function that calls the create method in MyHingeImplementation.
Bool createFunc( HighLevelConstraintImplementation* implementation );
 
class MyHingeImplementation : public HighLevelConstraintImplementation
{
  public:
    MyHingeImplementation( RigidBody* rb1, RigidBody* rb2 )
    {
      // Don't construct if rb1 is zero or if rb1 = rb2.
      if ( rb1 == 0L || rb1 == rb2 )
        return;
 
      // As commented in the interface class, the hinge axis is defined to be world y for this hinge.
      const Vec3 worldHingeAxis( 0, 1, 0 );
      // Calculate the anchor point in world, defined to be between the two bodies.
      const Vec3 relAnchorPosition = ( rb1->getPosition() + (rb2 == 0L ? rb1->getPosition() : rb2->getPosition()) ) * Real( 0.5 );
 
      // General interface for high level constraints are rigid bodies and frames. For this
      // constraint to fit in with the other constraints, create two frames (seen as external)
      // and two constraint attachment frames (seen as internal).
      FrameRef frame1 = new Frame();
      FrameRef frame2 = new Frame();
 
      // Utility function that transforms/configures the frames given world position and world hinge axis.
      // The z-axis is by definition the hinge axis.
      Constraint::calculateFramesFromWorld( relAnchorPosition, worldHingeAxis, rb1, frame1, rb2, frame2 );
 
      // Utility method to set up one or two body constraints. This method makes sure all
      // containers etc are initialized for the solvers.
      bool constructSuccess = construct( rb1, frame1, rb2, frame2, createFunc );
 
      if ( !constructSuccess ) {
        LOGGER_ERROR() << "Unable to create MyHinge" << std::endl << LOGGER_END();
      }
    }
 
    Bool create()
    {
      return createElementaryConstraints() && createSecondaryConstraints();
    }
 
    Bool createElementaryConstraints()
    {
      std::cout << "Constructing elementary primary constraints." << std::endl;
 
      // It's possible to construct a hinge with three components. One spherical (ball joint) and two
      // dot one constraints. Spherical is defined to keep two points at the same position, i.e., p1 = p2.
      // Dot1 constraints is defined to keep two vectors orthogonal, i.e., v1 * v2 = 0.
      // In our case we want the N vectors of both attachment frames to be parallel, i.e., N1 * U2 = 0 and
      // N1 * V2 = 0.
 
      // First, create the spherical constraint given the vector from center of mass to anchor point (world coordinates) and the current
      // position of the relative anchor point in world coordinates.
 
      AttachmentPair* ap = getAttachmentPair();
 
      // First, create the spherical constraint given the vector from center of mass to anchor point (world coordinates) and the current
      // position of the relative anchor point in world coordinates.
      addElementaryConstraint( "SR", new SphericalRel( ElementaryConstraintData( ap ) ) );
 
      // Create the two dot ones as defined above.
      addElementaryConstraint( "D1_VN", new Dot1( Dot1Data( ap, Attachment::V, Attachment::N ) ) );
      addElementaryConstraint( "D1_UN", new Dot1( Dot1Data( ap, Attachment::U, Attachment::N ) ) );
 
      return true;
    }
 
    Bool createSecondaryConstraints()
    {
      // Construct secondary controllers. They will act on the constraint n axis (z axis).
      // Hinges have rotation about that axis, so we have to create a 'rotational angle'
      // that counts the rotation about the hinge axis.
 
      AttachmentPair* ap = getAttachmentPair();
 
      // Create the rotational angle, defined about constraint n axis.
      RotationalAngleRef rotAngle = new RotationalAngle( Angle::N );
 
      // Range controller, disabled by default.
      addSecondaryConstraint( "RR", new RangeController( ConstraintAngleBasedData( ap, rotAngle ) ) );
      // Target speed controller, disabled by default.
      addSecondaryConstraint( "MR", new TargetSpeedController( ConstraintAngleBasedData( ap, rotAngle ) ) );
      // Lock controller, disabled by default.
      addSecondaryConstraint( "LR", new LockController( ConstraintAngleBasedData( ap, rotAngle ) ) );
 
      return true;
    }
 
    virtual void render( agxRender::RenderManager *mgr, float scale ) const
    {
      if ( !isValid() )
        return;
 
      const RigidBodyAttachment* a1 = getAttachment< RigidBodyAttachment >( 0 );
      const RigidBodyAttachment* a2 = getAttachment< RigidBodyAttachment >( 1 );
 
      // Lines from model center of each body to attachment points.
      agxRender::RenderProxy* line1 = mgr->acquireLine( m_bodies.front()->getPosition(), a1->get( Attachment::ANCHOR_POS ) );
      agxRender::RenderProxy* line2 = mgr->acquireLine( m_bodies.back()->getPosition(), a2->get( Attachment::ANCHOR_POS ) );
      // Line from rb1 to its attachment, red color.
      if ( line1 != 0L )
        line1->setColor( Vec3( 1, 0, 0 ) );
      // The other one green.
      if ( line2 != 0L )
        line2->setColor( Vec3 (0, 1, 0) );
 
      agxRender::RenderProxy* att1 = mgr->acquireSphere( scale * 0.2f );
      agxRender::RenderProxy* att2 = mgr->acquireSphere( scale * 0.1f );
 
      if ( line1 != 0L )
        att1->setColor( line1->getColor() );
      if ( line2 != 0L )
        att2->setColor( line2->getColor() );
 
      // Position the spheres at the attachment points.
      att1->setTransform( AffineMatrix4x4::translate( a1->get( Attachment::ANCHOR_POS ) ) );
      att2->setTransform( AffineMatrix4x4::translate( a2->get( Attachment::ANCHOR_POS ) ) );
 
      // Let the axis be blue and use rb1's definition of the axis.
      agxRender::RenderProxy* axis = mgr->acquireLine( a1->get( Attachment::ANCHOR_POS ) - a1->get( Attachment::N ),
                                                       a1->get( Attachment::ANCHOR_POS ) + a1->get( Attachment::N ) );
      axis->setColor( Vec3( 0, 0, 1) );
    }
 
    virtual ~MyHingeImplementation() {}
};
 
Bool createFunc( HighLevelConstraintImplementation* implementation )
{
  return dynamic_cast< MyHingeImplementation* >( implementation )->create();
}
 
class MyHinge : public Constraint
{
  public:
    MyHinge( RigidBody* rb1, RigidBody* rb2 )
    {
      m_implementation = new MyHingeImplementation( rb1, rb2 );
    }
 
    virtual int getNumDOF() const override { return 5; }
 
  protected:
    virtual void render( agxRender::RenderManager *mgr, float scale=1.0f ) const override
    {
      if ( m_implementation != 0L )
        static_cast< const MyHingeImplementation* >( m_implementation )->render( mgr, scale );
    }
 
    // Destructor has to handle implementation.
    virtual ~MyHinge()
    {
      if ( m_implementation != 0L )
      {
        delete m_implementation;
        m_implementation = 0L;
      }
    }
};
 
 
osg::Group* buildTutorialMyHinge1( agxSDK::Simulation *simulation, agxOSG::ExampleApplication *application )
{
  LOGGER_WARNING() << LOGGER_STATE( Notify::PRINT_NONE ) << __FUNCTION__ << std::endl << LOGGER_END();
 
  osg::Group *root = new osg::Group;
 
  // Create ground box.
  RigidBodyRef ground = new RigidBody();
  ground->setMotionControl( RigidBody::STATIC );
  ground->add( new agxCollide::Geometry( new agxCollide::Box( Vec3( 10, 10, 0.3 ) ) ) );
  ground->setPosition( Vec3( 0, 0, -0.3 ) );
  agxOSG::createVisual( ground, root );
 
  // Create two dynamic rigid bodies.
  RigidBodyRef rb1 = new RigidBody();
  RigidBodyRef rb2 = new RigidBody();
 
  rb1->add( new agxCollide::Geometry( new agxCollide::Box( Vec3( 1, 1, 1 ) ) ) );
  rb2->add( new agxCollide::Geometry( new agxCollide::Box( Vec3( 1, 1, 1 ) ) ) );
  agxOSG::createVisual( rb1, root );
  agxOSG::createVisual( rb2, root );
 
  // Add the bodies to simulation.
  simulation->add( ground );
  simulation->add( rb1 );
  simulation->add( rb2 );
 
  // Position the bodies.
  rb1->setPosition( Vec3( -4, 0, 4 ) );
  rb2->setPosition( Vec3(  4, 0, 5 ) );
 
  // Create our hinge...
  ref_ptr< MyHinge > myHinge = new MyHinge( rb1, rb2 );
  // ...and add it to the simulation.
  simulation->add( myHinge );
 
  // Via the interface class we get some stuff for free. Such as:
  // myHinge->setCompliance <- index 0, 1, 2 is the spherical, i.e., how stiff the translation should be, 3 is 'firstDot1' (N1 * U2) and 4 is 'secondDot1' (N1 * V2)
  // myHinge->getCompliance
  // myHinge->setDamping
  // myHinge->getDamping
  // myHinge->setSolveType <- solvers that supports this (DIRECT, ITERATIVE or DIRECT_AND_ITERATIVE)
  // myHinge->setEnable
  // myHinge->getEnable
  // ...and more.
 
  // Enable debug rendering by default.
  application->setEnableDebugRenderer( true );
 
  return root;
}
 
class MyElementaryDistanceData : public ElementaryConstraintData
{
  public:
    MyElementaryDistanceData( const Vec3& point1, const Vec3& point2 )
      : p1( point1 ), p2( point2 ) {}
    MyElementaryDistanceData( const MyElementaryDistanceData& other )
      : p1( other.p1 ), p2( other.p2 ) {}
 
    const Vec3& p1;
    const Vec3& p2;
 
    virtual ~MyElementaryDistanceData() {}
  private:
    MyElementaryDistanceData& operator=(const MyElementaryDistanceData&) {return *this;}
 
};
 
class MyElementaryDistance : public ElementaryConstraintNData< 1, MyElementaryDistanceData >
{
  public:
    MyElementaryDistance( const MyElementaryDistanceData& data )
      // Base class needs to know the number of rows this elementary constraints have and regularization parameters
      // and forces for each of those rows (convenient interface, so we don't have to implement getCompliance etc).
      : ElementaryConstraintNData< 1, MyElementaryDistanceData >( data, true )
    {
      // Calculate the initial length.
      m_restLength = ( m_data.p1 - m_data.p2 ).length();
      m_currentLength = m_restLength;
    }
 
    virtual ~MyElementaryDistance() {}
 
    virtual UInt getJacobian( Jacobian6DOFElement* G, UInt numBlocks, UInt row, GWriteState::Enum writeState ) override
    {
      // Calculate the direction.
      Vec3 k = m_data.p1 - m_data.p2;
      // Update the current length and normalize the direction vector.
      m_currentLength = k.normalize();
 
      // If G[0] is valid, push k, so the complete row for the first body becomes:
      // G[0] = [ k_1 k_2 k_3 0 0 0 ]
      // Zeros for rotation, i.e., this row can't make the constrained bodies to start rotate and the distance
      // preserved is from center of the bodies. To include rotation, we need additional information; from where
      // to where this distance constraint goes between the bodies. G becomes:
      // G[0] = [ k_1 k_2 k_3 vk_1 vk_2 vk_3 ] where,
      // vk = v1 x k (v1 cross k) and v1 is the vector from center of mass to the attachment point on that body.
 
      // The solver assumes a few things and to preserve direction of forces and signs we
      // have to obey writeState.
 
      // Normal write state. I.e., if two bodies (numBlocks > 1) write two rows.
      if ( writeState == GWriteState::NORMAL ) {
        G[ row + 0 ].set(  k, Vec3() );
        G[ row + 1 ].set( -k, Vec3() );
      }
      // If first body in the constraint is static, this state will be called.
      // I.e., write jacobian as seen from the second body.
      else if ( writeState == GWriteState::IGNORE_FIRST ) {
        agxAssert( numBlocks == 1 );
        G[ row + 0 ].set( -k, Vec3() );
      }
      // If first body in the constraint is kinematic, this state will be called.
      // I.e., both jacobians must be written but the AGX solvers preferes
      // dynamic bodies first - meaning second body is dynamic. This method
      // wouldn't be called if e.g., both were static or kinematic, static etc.
      // One has to be dynamic, otherwise the constraint is ignored.
      else {
        agxAssert( writeState == GWriteState::WRITE_SECOND_FIRST );
        G[ row + 0 ].set( -k, Vec3() );
        G[ row + 1 ].set(  k, Vec3() );
      }
 
      // Return the number of rows used in 'G'.
      return numBlocks;
    }
 
    virtual UInt getViolation( Real* g, UInt row ) override
    {
      g[ row ] = m_currentLength - m_restLength;
      return 1;
    }
 
  protected:
    Real m_restLength;                                      
    Real m_currentLength;                                   
};
 
class MyDistanceJointImplementation : public ConstraintImplementation
{
  public:
    MyDistanceJointImplementation( RigidBody* rb1, RigidBody* rb2 )
      : m_distance( 0L )
    {
      if ( rb1 == 0L || rb2 == 0L )
        return;
 
      // Creates data used by solvers.
      createData( rb1, rb2 );
      // Create the elementary distance constraint.
      m_distance = new MyElementaryDistance( MyElementaryDistanceData( m_p1, m_p2 ) );
      // Add it to the elementary constraints vector. The contents in m_ec etc. will be deleted during destruct of base class.
      m_ec.push_back( m_distance );
 
      // Call updateValid to update the valid flag.
      agxAssert( updateValid() );
    }
 
    virtual ~MyDistanceJointImplementation() {}
 
    virtual void prepare() override
    {
      agxAssert( m_bodies.size() == 2 );
 
      // Make sure ConstraintImplementation::prepare is called as well. Base prepare collects additional
      // information about this constraint, essential for the solvers.
      ConstraintImplementation::prepare();
 
      m_p1 = m_bodies.front()->getPosition();
      m_p2 = m_bodies.back()->getPosition();
 
      //if ( m_distance )
      //  std::cout << "Force: " << m_distance->getCurrentForce() << " N" << std::endl;
    }
 
    void render( agxRender::RenderManager *mgr, float /*scale*/ ) const
    {
      if ( !isValid() )
        return;
 
      agxRender::RenderProxy* line = mgr->acquireLine( m_p1, m_p2 );
      line->setColor( Vec3( 0, 1, 0 ) );
    }
 
  protected:
    MyDistanceJointImplementation() : m_distance( 0L ) {}
 
    void createData( RigidBody* rb1, RigidBody* rb2 )
    {
      // Valid rigid bodies, add them to m_bodies.
      m_bodies.push_back( rb1 );
      m_bodies.push_back( rb2 );
 
      // Update p1 and p2.
      prepare();
    }
 
  protected:
    Vec3 m_p1;  
    Vec3 m_p2;  
    MyElementaryDistance* m_distance;
};
 
class MyDistanceJoint : public Constraint
{
  public:
    MyDistanceJoint( RigidBody* rb1, RigidBody* rb2 )
    {
      m_implementation = new MyDistanceJointImplementation( rb1, rb2 );
    }
 
  protected:
    // Default constructor for implementations of this.
    MyDistanceJoint() {}
 
    // Destructor has to handle implementation.
    virtual ~MyDistanceJoint()
    {
      if ( m_implementation )
      {
        delete m_implementation;
        m_implementation = 0;
      }
    }
 
    virtual int getNumDOF() const override { return 1; }
 
    virtual void render( agxRender::RenderManager *mgr, float scale=1.0f ) const override
    {
      static_cast< const MyDistanceJointImplementation* >( m_implementation )->render( mgr, scale );
    }
};
 
osg::Group* buildTutorialMyDistanceJoint1( agxSDK::Simulation *simulation, agxOSG::ExampleApplication *application )
{
  LOGGER_WARNING() << LOGGER_STATE( Notify::PRINT_NONE ) << __FUNCTION__ << std::endl << LOGGER_END();
 
  osg::Group *root = new osg::Group;
 
  // Create ground box.
  RigidBodyRef ground = new RigidBody();
  ground->setMotionControl( RigidBody::STATIC );
  ground->add( new agxCollide::Geometry( new agxCollide::Box( Vec3( 10, 10, 0.3 ) ) ) );
  ground->setPosition( Vec3( 0, 0, -0.3 ) );
  agxOSG::createVisual( ground, root );
 
  // Create two dynamic rigid bodies.
  RigidBodyRef rb1 = new RigidBody();
  RigidBodyRef rb2 = new RigidBody();
 
  rb1->add( new agxCollide::Geometry( new agxCollide::Box( Vec3( 1, 1, 1 ) ) ) );
  rb2->add( new agxCollide::Geometry( new agxCollide::Box( Vec3( 1, 1, 1 ) ) ) );
  agxOSG::createVisual( rb1, root );
  agxOSG::createVisual( rb2, root );
 
  // Add the bodies to simulation.
  simulation->add( ground );
  simulation->add( rb1 );
  simulation->add( rb2 );
 
  // Position the bodies.
  rb1->setPosition( Vec3(  0, 0, 10 ) );
  rb2->setPosition( Vec3( -2, 0,  3 ) );
 
  // Create our distance joint.
  ref_ptr< MyDistanceJoint > distanceJoint = new MyDistanceJoint( rb1, rb2 );
  simulation->add( distanceJoint );
 
  // Lock rb1 to world.
  LockJointRef lock = new LockJoint( rb1 );
  simulation->add( lock );
 
  // Enable debug rendering by default.
  application->setEnableDebugRenderer( true );
 
  return root;
}
 
class MyAdvancedElementaryDistance : public MyElementaryDistance
{
  public:
    MyAdvancedElementaryDistance( const Vec3& p1, const Vec3& p2 )
      : MyElementaryDistance( MyElementaryDistanceData( p1, p2 ) ), m_desiredSpeed( 0 ) {}
 
    virtual ~MyAdvancedElementaryDistance() {}
 
    virtual UInt getViolation( Real* g, UInt row ) override
    {
      // If holonomic (motor disabled), use base.
      if ( getRegularizationParameters()->isHolonomic() )
        return MyElementaryDistance::getViolation( g, row );
      else
        g[ row ] = 0;
 
      return 1;
    }
 
    virtual UInt getVelocity( Real* v, UInt row ) const override
    {
      // Motor not active, velocity is zero.
      if ( getRegularizationParameters()->isHolonomic() )
        v[ row ] = 0;
      // Motor active, push desired speed.
      else
        v[ row ] = m_desiredSpeed;
 
      return 1;
    }
 
    virtual UInt getBounds( RangeReal* bounds, UInt row, Real dt ) const override
    {
      // If motor is enabled and we have finite bounds, push our force range.
      // Note: The force bounds used by the solver has unit impulse (i.e., force * dt)
      if ( !getRegularizationParameters()->isHolonomic() && !getForceRange().isInfinite() )
        bounds[ row + 0 ] = getForceRange() * dt;
      // Else infinite bounds.
      else
        bounds[ row + 0 ] = RangeReal();
 
      // Number of rows.
      return UInt( 1 );
    }
 
    void setDesiredSpeed( Real desiredSpeed )
    {
      m_desiredSpeed = desiredSpeed;
    }
 
    Real getDesiredSpeed() const { return m_desiredSpeed; }
 
    void updateRestLength()
    {
      m_restLength = (m_data.p1 - m_data.p2).length();
    }
 
  protected:
    Real m_desiredSpeed;
    RangeReal m_forceRange;
};
 
class MyAdvancedDistanceJointImplementation : public MyDistanceJointImplementation
{
  public:
    MyAdvancedDistanceJointImplementation( RigidBody* rb1, RigidBody* rb2 )
      : m_advancedDistance( 0L )
    {
      if ( rb1 == 0L || rb2 == 0L )
        return;
 
      // Create solver data.
      createData( rb1, rb2 );
 
      // Create the advanced elementary distance constraint.
      m_distance = m_advancedDistance = new MyAdvancedElementaryDistance( m_p1, m_p2 );
      // Add to elementary constraints vector.
      m_ec.push_back( m_distance );
 
      agxAssert( updateValid() );
    }
 
    void setMotorEnable( bool enable )
    {
      if ( enable ) {
        m_advancedDistance->getRegularizationParameters( 0 )->setType( RegularizationParameters::NONHOLONOMIC );
      }
      else {
        m_advancedDistance->getRegularizationParameters( 0 )->setType( RegularizationParameters::HOLONOMIC );
        m_advancedDistance->updateRestLength();
      }
    }
 
    void setDesiredSpeed( Real desiredSpeed )
    {
      m_advancedDistance->setDesiredSpeed( desiredSpeed );
    }
 
    void setForceRange( Real lower, Real upper )
    {
      m_advancedDistance->setForceRange( lower, upper );
    }
 
  protected:
    MyAdvancedElementaryDistance* m_advancedDistance;
};
 
class MyAdvancedDistanceJoint : public MyDistanceJoint
{
  public:
    MyAdvancedDistanceJoint( RigidBody* rb1, RigidBody* rb2 )
    {
      m_implementation = new MyAdvancedDistanceJointImplementation( rb1, rb2 );
    }
 
    void setMotorEnable( bool enable )
    {
      static_cast< MyAdvancedDistanceJointImplementation* >( m_implementation )->setMotorEnable( enable );
    }
 
    void setDesiredSpeed( Real desiredSpeed )
    {
      static_cast< MyAdvancedDistanceJointImplementation* >( m_implementation )->setDesiredSpeed( desiredSpeed );
    }
 
    void setForceRange( Real lower, Real upper )
    {
      static_cast< MyAdvancedDistanceJointImplementation* >( m_implementation )->setForceRange( lower, upper );
    }
 
    virtual int getNumDOF() const override { return 1; }
 
  protected:
    virtual ~MyAdvancedDistanceJoint()
    {
      if (m_implementation)
      {
        delete m_implementation;
        m_implementation = 0;
      }
    }
 
};
 
class MyDistanceJointKeyboardController : public agxSDK::GuiEventListener
{
  public:
    MyDistanceJointKeyboardController( MyAdvancedDistanceJoint* distanceJoint, Real speed, int keyIn, int keyOut )
      : m_distanceJoint( distanceJoint ), m_speed( speed ), m_keyIn( keyIn ), m_keyOut( keyOut )
    {
      setMask( KEYBOARD );
    }
 
    bool keyboard( int key, unsigned int , float , float , bool keydown )
    {
      bool handled = key == m_keyIn || key == m_keyOut;
      if ( !handled || !m_distanceJoint.isValid() )
        return false;
 
      m_distanceJoint->setMotorEnable( keydown );
      m_distanceJoint->setDesiredSpeed( (key == m_keyOut) * m_speed - (key == m_keyIn) * m_speed );
 
      return true;
    }
 
  private:
    observer_ptr< MyAdvancedDistanceJoint > m_distanceJoint;
    Real m_speed;
    int m_keyIn;
    int m_keyOut;
};
 
osg::Group* buildTutorialMyDistanceJoint2( agxSDK::Simulation *simulation, agxOSG::ExampleApplication *application )
{
  LOGGER_WARNING() << LOGGER_STATE( Notify::PRINT_NONE ) << __FUNCTION__ << std::endl << LOGGER_END();
 
  osg::Group *root = new osg::Group;
 
  // Create ground box.
  RigidBodyRef ground = new RigidBody();
  ground->setMotionControl( RigidBody::STATIC );
  ground->add( new agxCollide::Geometry( new agxCollide::Box( Vec3( 10, 10, 0.3 ) ) ) );
  ground->setPosition( Vec3( 0, 0, -0.3 ) );
  agxOSG::createVisual( ground, root );
 
  // Create two dynamic rigid bodies.
  RigidBodyRef rb1 = new RigidBody();
  RigidBodyRef rb2 = new RigidBody();
 
  rb1->add( new agxCollide::Geometry( new agxCollide::Box( Vec3( 1, 1, 1 ) ) ) );
  rb2->add( new agxCollide::Geometry( new agxCollide::Box( Vec3( 1, 1, 1 ) ) ) );
  agxOSG::createVisual( rb1, root );
  agxOSG::createVisual( rb2, root );
 
  // Add the bodies to simulation.
  simulation->add( ground );
  simulation->add( rb1 );
  simulation->add( rb2 );
 
  // Position the bodies.
  rb1->setPosition( Vec3( 0, 0, 10 ) );
  rb2->setPosition( Vec3( 0, 0,  3 ) );
 
  // Create our distance joint.
  ref_ptr< MyAdvancedDistanceJoint > distanceJoint = new MyAdvancedDistanceJoint( rb1, rb2 );
  simulation->add( distanceJoint );
 
  // The advanced version of the distance joint can be motor controlled. Define the
  // maximum forces this motor may apply when active (default: -infinity to infinity).
  distanceJoint->setForceRange( Real( -1E5 ), Real( 1E5 ) );
 
  // Use our MyHinge and the lock controller in it to lock the
  // top rigid body in world.
  ref_ptr< MyHinge > hinge = new MyHinge( rb1, 0L );
  // Since we don't have interface to access the lock we have to fetch it given its name.
  LockController* lock = LockController::safeCast( hinge->getSecondaryConstraintGivenName( "LR" ) );
  if ( lock != 0L )
    lock->setEnable( true );
 
  simulation->add( hinge );
 
  // Create keyboard callback to control our advanced distance joint.
  int keyIn  = agxSDK::GuiEventListener::KEY_Up;
  int keyOut = agxSDK::GuiEventListener::KEY_Down;
  Real speed = 0.7;
  simulation->add( new MyDistanceJointKeyboardController( distanceJoint, speed, keyIn, keyOut ) );
 
  agxRender::RenderSingleton::instance()->addStatic( "Press directional key 'up' or 'down' to control the distance joint.", Vec2( 0.01, 0.02 ), Vec4f( 0.5f, 0, 1, 1 ) );
 
  // Enable debug rendering by default.
  application->setEnableDebugRenderer( true );
 
  return root;
}
 
Bool createMyLock( HighLevelConstraintImplementation* myLockImplementation )
{
  auto ap = myLockImplementation->getAttachmentPair();
  myLockImplementation->addElementaryConstraint( "Spatial",
                                                 new SphericalRel( ElementaryConstraintData( ap ) ) );
  myLockImplementation->addElementaryConstraint( "Rotational",
                                                 new QuatLock( QuatLockData( ap ) ) );
  return true;
}
 
class MyLockJointImplementation : public HighLevelConstraintImplementation
{
  public:
    MyLockJointImplementation( RigidBody* rb1,
                               Vec3 rb1LocalPosition,
                               Vec3 rb1LocalAxis,
                               RigidBody* rb2 = nullptr )
    {
      FrameRef f1 = new Frame();
      FrameRef f2 = new Frame();
      Constraint::calculateFramesFromBody( rb1LocalPosition,
                                           rb1LocalAxis,
                                           rb1,
                                           f1,
                                           rb2,
                                           f2 );
      auto success = construct( rb1,
                                f1,
                                rb2,
                                f2,
                                createMyLock );
      if ( !success )
        LOGGER_ERROR() << "Unable to initialize MyLockJoint." << LOGGER_ENDL();
 
      for ( size_t i = 0u; i < sizeof( m_offDiagonalParameters ) / sizeof( Real ); ++i )
        m_offDiagonalParameters[ i ] = 0.0;
 
      // Enable callbacks from the direct solver with read/write access
      // to our compliance matrix.
      setSupportsComplianceMatrix( true );
    }
 
    Real getOffDiagonalParameter( UInt row, UInt col )
    {
      return m_offDiagonalParameters[ calculateIndexAndValidate( row, col ) ];
    }
 
    void setOffDiagonalParameter( UInt row, UInt col, Real value )
    {
      // The interface is, naturally, positive values, but we
      // write it as negative in updateComplianceMatrix.
      if ( value < 0.0 )
        LOGGER_ERROR() << "Off diagonal value must be >= 0." << LOGGER_ENDL();
 
      m_offDiagonalParameters[ calculateIndexAndValidate( row, col ) ] = value;
    }
 
  public:
    void updateComplianceMatrix( LSquareComplianceMatrix matrix ) const override
    {
      // Looping over lower triangle, off-diagonal elements. Inner loop
      // breaks when row >= col.
      for ( UInt32 row = 1u; row < 6u; ++row ) {
        for ( UInt32 col = 0u; col < row && col < 5u; ++col ) {
          matrix[ row ][ col ] = -m_offDiagonalParameters[ calculateIndex( row, col ) ];
        }
      }
    }
 
  private:
    template<typename T>
    T calculateIndex( T row, T col ) const
    {
      // Assuming row is > 0 here and col < row.
      return (T)( row * ( row - 1 ) / 2 + col );
    }
 
    template<typename T>
    T calculateIndexAndValidate( T row, T col ) const
    {
      if ( row >= 6 || col >= 6 )
        LOGGER_ERROR() << "Row and column index out of range: (" << row << ", " << col << ") >= (6, 6)" << LOGGER_ENDL();
      if ( row == col )
        LOGGER_ERROR() << "Access values along diagonal using the regular API." << LOGGER_ENDL();
 
      // Transform from upper to lower triangle.
      if ( col > row )
        std::swap( col, row );
 
      return calculateIndex( row, col );
    }
 
  private:
    // Additional parameters of the off-diagonal elements. The compliance matrix is required
    // to be symmetric so we "only" have to store an additional 15 parameters.
    Real m_offDiagonalParameters[ (6 * 6 - 6) / 2 ];
};
 
class MyBeam : public Constraint
{
  public:
    MyBeam( Vec3 halfExtents, UInt numSegments )
    {
      // Constraints may have a tree structure, this is the
      // root constraint (can be empty) and all valid sub-constraints
      // of this root will be propagated to the solver. We
      // do getRep()->getSubConstraints().push_back( subConstraint )
      // to add a constraint.
      setRep( new ConstraintImplementation() );
 
      const auto segmentHalfExtents = Vec3( halfExtents.x() / static_cast<agx::Real>(numSegments),
                                            halfExtents.y(),
                                            halfExtents.z() );
      auto position = Vec3( segmentHalfExtents.x(), 0, 0 );
      for ( UInt i = 0u; i < numSegments; ++i ) {
        // Create and position segment rigid body.
        RigidBodyRef segment = new RigidBody( new agxCollide::Geometry( new agxCollide::Box( segmentHalfExtents ) ) );
        segment->setPosition( position );
        position.x() += 2.0 * segmentHalfExtents.x();
 
        RigidBody* prevSegment = m_segments.empty() ?
                                   nullptr :
                                   m_segments.back();
 
        if ( prevSegment == nullptr ) {
          // If this is the first segment, create a "normal" lock
          // joint where the first segment is attached in world.
          m_worldLock = new LockJoint( segment, Vec3( -segmentHalfExtents.x(), 0, 0 ) );
        }
        else {
          // Create our custom lock between prevSegment and segment.
          // Constraint axis is along the beam (x axis).
          MyLockJointImplementation* lock = new MyLockJointImplementation( prevSegment,
                                                                           Vec3( segmentHalfExtents.x(), 0, 0 ),
                                                                           Vec3::X_AXIS(),
                                                                           segment );
          // Add the lock and disable collisions between the segments.
          getRep()->getSubConstraints().push_back( lock );
          agxUtil::setEnableCollisions( prevSegment, segment, false );
        }
 
        // The segments are added and removed from the simulation
        // in addNotification and removeNotification.
        m_segments.push_back( segment );
      }
    }
 
    void createVisual( osg::Group* root )
    {
      for ( UInt i = 0u; i < m_segments.size(); ++i )
        agxOSG::setDiffuseColor( agxOSG::createVisual( m_segments[ i ],
                                                              root ),
                                 agxRender::Color::getColor( i ) );
    }
 
    void setComplianceMatrixValue( Real compliance, int dof = -1 )
    {
      compliance = std::max( compliance, 0.0 );
      visit( [dof, compliance]( MyLockJointImplementation* lock )
             {
               lock->userAPIsetCompliance( compliance, dof );
             } );
    }
 
    void setComplianceMatrixValue( UInt row, UInt col, Real value )
    {
      if ( row == col ) {
        setComplianceMatrixValue( value, (int)row );
        return;
      }
 
      visit( [row, col, value]( MyLockJointImplementation* lock )
             {
               lock->setOffDiagonalParameter( row, col, value );
             } );
    }
 
  public:
    virtual void addNotification() override
    {
      getRep()->getSimulationProxy()->add( m_worldLock );
      for ( auto segment : m_segments )
        getRep()->getSimulationProxy()->add( segment );
    }
 
    virtual void removeNotification() override
    {
      getRep()->getSimulationProxy()->remove( m_worldLock );
      for ( auto rb : m_segments )
        getRep()->getSimulationProxy()->remove( rb );
    }
 
    virtual void render( agxRender::RenderManager* mgr, float scale ) const override
    {
      visit( [mgr, scale]( MyLockJointImplementation* lock )
             {
               agxRender::debugRenderConstraintFrames( lock, scale, mgr );
             } );
    }
 
    virtual int getNumDOF() const override
    {
      return 0;
    }
 
  protected:
    virtual ~MyBeam()
    {
      while ( !getRep()->getSubConstraints().empty() ) {
        delete getRep()->getSubConstraints().back();
        getRep()->getSubConstraints().pop_back();
      }
      delete getRep();
      setRep( nullptr );
    }
 
  private:
    void visit( std::function<void( MyLockJointImplementation* )> visitor ) const
    {
      for ( UInt i = 0u; i < getRep()->getSubConstraints().size(); ++i )
        visitor( static_cast<MyLockJointImplementation*>( getRep()->getSubConstraints()[ i ] ) );
    }
 
  private:
    RigidBodyRefVector m_segments;
    LockJointRef m_worldLock;
};
 
using MyBeamRef = ref_ptr<MyBeam>;
 
osg::Group* buildTutorialMyLockJointComplianceMatrix( agxSDK::Simulation* simulation,
                                                      agxOSG::ExampleApplication* /*application*/ )
{
  LOGGER_WARNING() << LOGGER_STATE( Notify::PRINT_NONE ) << __FUNCTION__ << std::endl << LOGGER_END();
 
  osg::Group* root = new osg::Group;
 
  MyBeamRef beam = new MyBeam( Vec3( 3, 0.25, 0.25 ), 10 );
  beam->createVisual( root );
  simulation->add( beam );
 
  // Make the beam bend in gravity.
  beam->setComplianceMatrixValue( 1.0E-6, LockJoint::ROTATIONAL_1 );
  beam->setComplianceMatrixValue( 1.0E-6, LockJoint::ROTATIONAL_2 );
  beam->setComplianceMatrixValue( 5.0E-6, LockJoint::ROTATIONAL_3 );
 
  // Note: This matrix isn't positive definite.
  beam->setComplianceMatrixValue( (UInt)LockJoint::ROTATIONAL_3,
                                  (UInt)LockJoint::ROTATIONAL_2,
                                  1.0E-3 );
 
  return root;
}
 
int main( int argc, char** argv )
{
  AutoInit agxInit;
  std::cout <<
                "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
                "\tTutorial custom constraints\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorialMyHinge1, '1' );
    application->addScene( buildTutorialMyDistanceJoint1, '2' );
    application->addScene( buildTutorialMyDistanceJoint2, '3' );
    application->addScene( buildTutorialMyLockJointComplianceMatrix, '4' );
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_customMergeSplitAlgorithm.cpp

/*
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.
*/
 
/*
 
Demonstrates how to use custom merge and split algoritms together with the Adaptive Model Order Reduction (AMOR) system.
 
*/
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/utils.h>
 
#include <agxSDK/Assembly.h>
#include <agxSDK/MergeSplitHandler.h>
#include <agxSDK/MergeSplitUtils.h>
 
#include <agxUtil/StepEventCallback.h>
 
#include <agx/LockJoint.h>
#include <agx/Hinge.h>
 
#include <agxCollide/Box.h>
 
#include <agxRender/RenderSingleton.h>
#include <agx/version.h>
 
namespace tutorial1
{
  class BoxContactMergeSplitAlgorithm : public agxSDK::MergeSplitAlgorithm
  {
    public:
      BoxContactMergeSplitAlgorithm();
 
    protected:
      virtual ~BoxContactMergeSplitAlgorithm();
 
      virtual void onPreStepActions( agxSDK::MergeSplitActionContainer& actions ) override;
 
      virtual void onPostSolveActions( const agxSDK::MergeSplitPostSolveData& postSolveData,
                                       agxSDK::MergeSplitActionContainer& actions ) override;
  };
 
  BoxContactMergeSplitAlgorithm::BoxContactMergeSplitAlgorithm()
    : MergeSplitAlgorithm()
  {
    setEnableSerialization( false );
  }
 
  BoxContactMergeSplitAlgorithm::~BoxContactMergeSplitAlgorithm()
  {
  }
 
  void BoxContactMergeSplitAlgorithm::onPreStepActions( agxSDK::MergeSplitActionContainer& actions )
  {
    // All, current, contacts in the simulation.
    const auto& geometryContacts = getMergeSplitHandler()->getSimulation()->getSpace()->getGeometryContacts();
    for ( const auto geometryContact : geometryContacts ) {
      // Ignore invalid and disabled contacts.
      if ( !geometryContact->isValid() || !geometryContact->enabled() )
        continue;
 
      // Merged state collecting data regarding current state (merged or not etc.)
      // and finds allowed actions (given agxSDK::MergeSplitProperties merge and split enabled).
      agxSDK::MergedState mergedState( geometryContact->geometry( 0 ), geometryContact->geometry( 1 ), getMergeSplitHandler() );
 
      // We're only interested in the geometry contact if we may split at least
      // one of the objects.
      if ( !mergedState.getAllowedActions().Is( agxSDK::MergedState::SPLIT_FIRST | agxSDK::MergedState::SPLIT_SECOND ) )
        continue;
 
      // Split when the geometry contact state is 'impact'. The state is 'impact'
      // the first time (after last separation) the two geometries overlap.
      if ( geometryContact->state() == agxCollide::GeometryContact::IMPACT_STATE )
        actions.split( mergedState );
    }
  }
 
  void BoxContactMergeSplitAlgorithm::onPostSolveActions( const agxSDK::MergeSplitPostSolveData& postSolveData,
                                                          agxSDK::MergeSplitActionContainer& actions )
  {
    // 'postSolveData' contains all the nodes/edges in the interaction graph.
    // Find all geometry contacts and calculate the contact state.
    for ( agx::UInt i = 0u; i < postSolveData.graphNode.instance.size(); ++i ) {
      // Ignoring nodes that aren't contacts.
      if ( postSolveData.getType( i ) != agx::GraphNode::GEOMETRY_CONTACT )
        continue;
 
      // Creates merged state given the geometry contact.
      const auto mergedState = postSolveData.createMergedState( i, *getMergeSplitHandler() );
 
      // We want to merge, check so that we may merge the objects involved.
      if ( !mergedState.getAllowedActions().Is( agxSDK::MergedState::MERGE ) )
        continue;
 
      // Fetch the geometry contact data object. This object
      // contains the bodies, contact points etc..
      auto geometryContact = postSolveData.asGeometryContact( i );
 
      // We're only handling contacts with two bodies.
      if ( !geometryContact.body1() || !geometryContact.body2() )
        continue;
 
      // We're only merging geometry contacts with 4 contact points!
      if ( geometryContact.points().size() != 4 )
        continue;
 
      const agx::RigidBody* rb1   = geometryContact.body1().model();
      const agx::RigidBody* rb2   = geometryContact.body2().model();
      const agx::Vec3 relVel      = rb1->getVelocity() - rb2->getVelocity();
 
      // Function that calculates the speed at a contact point, along a given direction (normal or tangent).
      auto calculateSpeed = [ & ]( const agx::Vec3& position, const agx::Vec3f& dir ) -> agx::Real
      {
        return agxSDK::MergeSplitUtils::calculateImpactSpeed( position, dir, relVel, rb1->getCmPosition(), rb1->getAngularVelocity(), rb2->getCmPosition(), rb2->getAngularVelocity() );
      };
 
      agx::Real maxSpeedInContact = -agx::Infinity;
      // Loop over all contact points and calculate impact and sliding
      // speed given the normal and tangents in the contact.
      for ( auto contactPoint : geometryContact.points() ) {
        if ( !contactPoint.enabled() )
          continue;
 
        maxSpeedInContact = std::max( agx::absolute( calculateSpeed( contactPoint.point(), contactPoint.normal() ) ), maxSpeedInContact );
        maxSpeedInContact = std::max( agx::absolute( calculateSpeed( contactPoint.point(), contactPoint.tangentU() ) ), maxSpeedInContact );
        maxSpeedInContact = std::max( agx::absolute( calculateSpeed( contactPoint.point(), contactPoint.tangentV() ) ), maxSpeedInContact );
      }
 
      // If the maximum speed in the four contact points is less than
      // some value - merge the two objects.
      if ( maxSpeedInContact < 0.3 )
        actions.merge( mergedState, new agx::MergedBody::GeometryContactEdgeInteraction( geometryContact ) );
    }
  }
}
 
osg::Group* buildTutorial1( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create a static ground body.
  agx::RigidBodyRef ground = new agx::RigidBody( "ground" );
  ground->add( new agxCollide::Geometry( new agxCollide::Box( 25, 25, 0.5 ) ) );
  ground->setPosition( 0, 0, -0.5 );
  ground->setMotionControl( agx::RigidBody::STATIC );
  simulation->add( ground );
 
  agxOSG::setDiffuseColor( agxOSG::createVisual( ground, root ), agxRender::Color::Green() );
 
  // Spawn boxes at some random rate.
  // The random seed is set to a fixed value for our internal determinism-testing.
  srand(5);
  // Feel free to choose a non-fixed random seed for "real" randomness by uncommenting this line:
  //srand((unsigned int)time(nullptr));
  agx::Real firstSpawnTime = 0.5;
  agxUtil::StepEventCallback::preCollide( [ root ]( agxSDK::Simulation* simulation, agx::Real& timeToSpawn )
  {
    if ( simulation->getTimeStamp() < timeToSpawn )
      return;
 
    agx::RigidBodyRef box = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 0.5, 0.5, 0.5 ) ) );
    box->setPosition( agx::Vec3::random( agx::Vec3( -1, -1, 1.5 ), agx::Vec3( 1, 1, 2.5 ) ) );
    box->setVelocity( agx::Vec3::random( agx::Vec3( 0, 0, 2 ), agx::Vec3( 0, 0, 3 ) ) );
    box->setAngularVelocity( agx::Vec3::random( agx::Vec3( -1, -1, -1 ), agx::Vec3( 1, 1, 1 ) ) );
    simulation->add( box );
 
    osg::Group* node = agxOSG::createVisual( box, root );
    agxOSG::setDiffuseColor( node, agxRender::Color::Yellow() );
 
    // Enable merge and split for the box.
    agxSDK::MergeSplitHandler::getOrCreateProperties( box )->setEnableMergeSplit( true );
 
    timeToSpawn += agx::random( 1.0, 4.0 );
  }, simulation, firstSpawnTime );
 
  // Enable the merge-split handler.
  simulation->getMergeSplitHandler()->setEnable( true );
 
  // Remove all other merge split algorithms. We only want our own!
  auto algorithms = simulation->getMergeSplitHandler()->getAlgorithms();
  for ( auto algorithm : algorithms )
    simulation->getMergeSplitHandler()->remove( algorithm );
 
  agxAssert( simulation->getMergeSplitHandler()->getAlgorithms().empty() );
  // Add our specialized box merge split algorithm.
  simulation->getMergeSplitHandler()->add( new tutorial1::BoxContactMergeSplitAlgorithm() );
 
  // Enable merge for ground.
  agxSDK::MergeSplitHandler::getOrCreateProperties( ground )->setEnableMerge( true );
 
  return root;
}
 
namespace tutorial2
{
  class InclinationTestMergeSplitAlgorithm : public agxSDK::MergeSplitAlgorithm
  {
    public:
      InclinationTestMergeSplitAlgorithm();
 
    protected:
      virtual ~InclinationTestMergeSplitAlgorithm();
 
      virtual void onPreStepActions( agxSDK::MergeSplitActionContainer& actions ) override;
  };
 
  InclinationTestMergeSplitAlgorithm::InclinationTestMergeSplitAlgorithm()
    : MergeSplitAlgorithm()
  {
    setEnableSerialization( false );
  }
 
  InclinationTestMergeSplitAlgorithm::~InclinationTestMergeSplitAlgorithm()
  {
  }
 
  void InclinationTestMergeSplitAlgorithm::onPreStepActions( agxSDK::MergeSplitActionContainer& actions )
  {
    // All constraints in the simulation.
    const auto& constraints = getMergeSplitHandler()->getSimulation()->getConstraints();
    for ( const auto& constraint : constraints ) {
      // Check for the hinge.
      const agx::Hinge* hinge = constraint->asSafe<agx::Hinge>();
      if ( hinge == nullptr || hinge->getNumBodies() > 1 )
        continue;
 
      const agx::RigidBody* ground = constraint->getBodyAt( 0 );
      // We're only interested when an object is merged to ground, i.e.,
      // ground has to have an agx::MergedBody associated to it.
      const agx::MergedBody* mergedBody = agx::MergedBody::getActive( ground );
      if ( mergedBody == nullptr )
        return;
 
      // Collect all edge interactions connected to ground. When an objects
      // is merged to 'ground' (in this scene) it appears as a GeometryContactEdgeInteraction.
      agx::MergedBody::EdgeInteractionRefContainer edges;
      agx::MergedBody::EdgeInteractionVisitor edgeVisitor = [ &edges ]( agx::MergedBody::EdgeInteraction* edge )
      {
        agxAssert( edge->isTagged( agx::MergedBody::EdgeInteraction::CONTACT ) );
        edges.push_back( edge );
      };
      mergedBody->traverse( ground, edgeVisitor );
 
      // Loop over all edges connected to 'ground' and split when the
      // gravity force exceeds the friction force stored in the edge
      // when the objects merged.
      for ( auto edge : edges ) {
        // We're looking for the object merged to ground: 'otherRb'.
        agx::RigidBody* otherRb = edge->getRigidBody1() == ground ? edge->getRigidBody2() : edge->getRigidBody1();
 
        // Not that we're only handling edges tagged as contact.
        if ( otherRb == nullptr || !edge->isTagged( agx::MergedBody::EdgeInteraction::CONTACT ) )
          continue;
 
        // The merged state. We use this to, e.g., determine allowed actions (may split or merge etc.)
        // and internally, to understand how to merge or split the objects.
        agxSDK::MergedState state( edge->getRigidBody1(), edge->getRigidBody2(), getMergeSplitHandler() );
 
        // Calculate the gravity force acting on the merged object resting on 'ground'.
        const agx::Vec3 gravity      = getMergeSplitHandler()->getSimulation()->getGravityField()->calculateGravity( otherRb->getPosition() );
        const agx::Vec3 gravityForce = otherRb->getMassProperties()->getMass() * gravity;
 
        // Utility function that calculates the strength of the merged contact -
        // i.e., the amount of force needed to split the edge.
        const auto edgeStrength = agxSDK::MergeSplitUtils::calculateContactForceFrame( otherRb,
                                                                                       edge->as<agx::MergedBody::GeometryContactEdgeInteraction>(),
                                                                                       getMergeSplitHandler() );
 
        // When 'otherRb' were merged to 'ground' the current interaction forces were
        // stored in the edge. This utility function checks if an external force is
        // large enough to split the object, given the active edges connected to it.
        auto result = agxSDK::MergeSplitUtils::shouldSplitGivenExternalForce( edgeStrength,
                                                                              otherRb,
                                                                              ground,
                                                                              gravityForce,
                                                                              agx::Vec3(),
                                                                              gravity,
                                                                              *getMergeSplitHandler() );
 
        // If the result is 'should split', split/remove the edge given the state.
        if ( result.shouldSplit )
          actions.split( state, edge );
      }
    }
  }
}
 
osg::Group* buildTutorial2( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  // Number of boxes to place on ground.
  const agx::UInt numBoxes = 20;
 
  // Preparing on-screen text.
  application->getSceneDecorator()->setFontSize( 0.01f );
  application->getSceneDecorator()->setText( 0, "Merged boxes are blue.", agxRender::Color::LightBlue() );
  application->getSceneDecorator()->setText( 1, "Free boxes are yellow.", agxRender::Color::Yellow() );
  for ( agx::UInt i = 0; i < numBoxes; ++i )
    application->getSceneDecorator()->setText( 2 + int( i ), "" );
 
  // Create ground and hinge it in world.
  agx::RigidBodyRef ground = new agx::RigidBody( "ground" );
  ground->add( new agxCollide::Geometry( new agxCollide::Box( 45, 2.0 * 0.5 * agx::Real( numBoxes ), 0.5 ) ) );
  ground->setPosition( 12, 0, -0.5 );
  ground->getMassProperties()->setMass( 1.0E7 );
  simulation->add( ground );
 
  agxOSG::setDiffuseColor( agxOSG::createVisual( ground, root ), agxRender::Color::Green() );
 
  agx::HingeRef groundHinge = agx::Constraint::createFromBody<agx::Hinge>( agx::Vec3(), agx::Vec3::Y_AXIS(), ground );
  groundHinge->setCompliance( 0 );
  groundHinge->getMotor1D()->setCompliance( 2.0E-10 );
  groundHinge->getMotor1D()->setEnable( true );
  simulation->add( groundHinge );
 
  // Ground material to handle the different friction coefficients for all boxes.
  agx::MaterialRef groundMaterial = new agx::Material( "groundMaterial" );
  agxUtil::setBodyMaterial( ground, groundMaterial );
 
  // Creates an explicit contact material between the box and ground.
  // Sets friction coefficient from 0.7 to 0.45.
  auto createMaterial = [ & ]( agx::RigidBody* rb, agx::UInt index )
  {
    agx::MaterialRef material = new agx::Material( agx::String::format( "boxMaterial_%d", index ) );
    agxUtil::setBodyMaterial( rb, material );
 
    auto contactMaterial = simulation->getMaterialManager()->getOrCreateContactMaterial( groundMaterial, material );
    contactMaterial->setFrictionCoefficient( 0.7 - ( 0.7 - 0.45 ) * agx::Real( index ) / ( numBoxes - 1 ) );
 
    application->getSceneDecorator()->setText( 2 + int( index ), agx::String::format( "%s friction coefficient: %.2f", rb->getName().c_str(), contactMaterial->getFrictionCoefficient() ) );
  };
 
  agx::Vec3 boxPosition( 10, -0.5 * 2.0 * numBoxes + 1.0, 1.5 );
  for ( agx::UInt i = 0; i < numBoxes; ++i ) {
    // Create a box and drop it onto the ground.
    agx::RigidBodyRef box = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 0.5, 0.5, 0.5 ) ) );
    box->setName( agx::String::format( "Box %d", i + 1 ) );
    box->setPosition( boxPosition );
    simulation->add( box );
 
    osg::Group* node = agxOSG::createVisual( box, root );
    agxOSG::setDiffuseColor( node, agxRender::Color::Yellow() );
 
    // Creates contact material and assign friction coefficient.
    createMaterial( box, i );
 
    // Enable merge and split for the box.
    agxSDK::MergeSplitHandler::getOrCreateProperties( box )->setEnableMergeSplit( true );
 
    boxPosition += agx::Vec3( 0, 2.0, 0 );
 
    // In post-step, sets color given merged state.
    agxUtil::StepEventCallback::post( []( agxSDK::Simulation* /*simulation*/, agx::RigidBody* box, osg::Group* node )
    {
      if ( agx::MergedBody::getActive( box ) != nullptr )
        agxOSG::setDiffuseColor( node, agxRender::Color::LightBlue() );
      else
        agxOSG::setDiffuseColor( node, agxRender::Color::Yellow() );
    }, simulation, box, node );
  }
 
  // Enable the merge-split handler.
  simulation->getMergeSplitHandler()->setEnable( true );
 
  // Enable merge for ground.
  agxSDK::MergeSplitHandler::getOrCreateProperties( ground )->setEnableMerge( true );
 
  // Add our split algorithm to the handler.
  simulation->getMergeSplitHandler()->add( new tutorial2::InclinationTestMergeSplitAlgorithm() );
 
  const agx::Real hingeSpeed = agx::Real( 0.15 );
  // Handles speed of the motor from a pre-collide step event.
  agxUtil::StepEventCallback::preCollide( [ groundHinge, hingeSpeed ]( agxSDK::Simulation* simulation )
  {
    // Start after 1.5 seconds when the objects are merged.
    if ( simulation->getTimeStamp() >= 1.5 && groundHinge->getMotor1D()->getSpeed() == agx::Real( 0 ) )
      groundHinge->getMotor1D()->setSpeed( hingeSpeed );
    // When the angle is more than 35 degrees, change speed of the motor.
    else if ( std::abs( groundHinge->getAngle() ) > 35.0 * agx::DEG_TO_RAD && agx::sign( groundHinge->getAngle() ) == agx::sign( groundHinge->getMotor1D()->getSpeed() ) )
      groundHinge->getMotor1D()->setSpeed( -groundHinge->getMotor1D()->getSpeed() );
  }, simulation );
 
  return root;
}
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout << "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
               "\tTutorial custom merge-split algorithms\n" <<
               "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_driveTrain.cpp

/*
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.
*/
 
 
 
/*
 
This file contains a collection of tutorials for the AGX DriveTrain library. It
describes the basics of how the library works and exemplifies this using a
handful of examples.
 
The tutorial creates no graphics window. All state inspection is instead done
using prints to the console.
 
*/
 
#include <agx/Prismatic.h>
#include <agx/version.h>
 
#include <agx/config/AGX_USE_WEBPLOT.h>
 
#include <agxCollide/Box.h>
 
#include <agxDriveTrain/Gear.h>
#include <agxDriveTrain/TorqueConverter.h>
#include <agxDriveTrain/GearBox.h>
#include <agxDriveTrain/Shaft.h>
#include <agxDriveTrain/Engine.h>
#include <agxDriveTrain/DryClutch.h>
#include <agxDriveTrain/Clutch.h>
#include <agxDriveTrain/Differential.h>
 
#include <agxIO/FileSystem.h>
 
#include <agxSDK/Simulation.h>
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/Node.h>
 
#include <agxPowerLine/Actuator1DOF.h>
#include <agxPowerLine/Sides.h>
 
#include <agxPlot/System.h>
#include <agxPlot/FilePlot.h>
#if AGX_USE_WEBPLOT()
#include <agxPlot/WebPlot.h>
#endif
 
#include <agxVehicle/WheelJoint.h>
#include <agxVehicle/WheelJointConstraintGeometry.h>
 
#include <chrono>
#include <thread>
 
 
namespace
{
  class DriveTrainShaftRPMListener : public agxPlot::DataListener
  {
  public:
    DriveTrainShaftRPMListener( agxDriveTrain::Shaft* shaft )
      : m_shaft( shaft )
    {
    }
 
    virtual agxPlot::DataGenerator::Result getValue( )
    {
      agxPlot::DataGenerator::Result result;
      if ( m_shaft == nullptr )
      {
        result.hasValue = false;
        return result;
      }
      result.hasValue = true;
      result.value = m_shaft->getRPM( );
      return result;
    }
 
  private:
    agxDriveTrain::Shaft* m_shaft;
 
  };
 
 
 
  class DriveTrainTorqueListener : public agxPlot::DataListener
  {
  public:
    DriveTrainTorqueListener( agxPowerLine::Connector* connector )
      : m_connector(connector)
    {
    }
 
    virtual agxPlot::DataGenerator::Result getValue( )
    {
      agxPlot::DataGenerator::Result result;
      if (m_connector == nullptr)
      {
        result.hasValue = false;
        return result;
      }
      result.hasValue = true;
      if (m_connector->getConstraint()->getEnable())
        result.value = m_connector->getConstraint()->getCurrentForce();
      else
        result.value = m_connector->getOutputConnections().front()->getDimension()->getOutputLoad();
      return result;
    }
 
  private:
    agxPowerLine::Connector* m_connector;
  };
 
 
 
  class DriveTrainRotationalActuatorRelativeRPMListener : public agxPlot::DataListener
  {
  public:
    DriveTrainRotationalActuatorRelativeRPMListener(agxPowerLine::RotationalActuator* rotationalActuator)
      : m_rotationalActuator(rotationalActuator)
    {
    }
 
    virtual agxPlot::DataGenerator::Result getValue()
    {
      agxPlot::DataGenerator::Result result;
      if (m_rotationalActuator == nullptr)
      {
        result.hasValue = false;
        return result;
      }
      result.hasValue = true;
      result.value = agxPowerLine::RotationalUnit::convertAngularVelocityToRPM( m_rotationalActuator->calculateRelativeGradient() );
      return result;
    }
 
  private:
    agxPowerLine::RotationalActuator* m_rotationalActuator;
  };
 
 
 
  class DriveTrainEngineThrottleListener : public agxPlot::DataListener
  {
  public:
    DriveTrainEngineThrottleListener( agxDriveTrain::PidControlledEngine* engine )
      : m_engine( engine )
    {
    }
 
    virtual agxPlot::DataGenerator::Result getValue( )
    {
      agxPlot::DataGenerator::Result result;
      if ( m_engine == nullptr )
      {
        result.hasValue = false;
        return result;
      }
      result.hasValue = true;
      result.value = m_engine->getThrottle() * agx::Real(100);
      return result;
    }
 
  private:
    agxDriveTrain::PidControlledEngine* m_engine;
  };
 
 
 
  class DriveTrainEngineAppliedTorqueListener : public agxPlot::DataListener
  {
  public:
    DriveTrainEngineAppliedTorqueListener(agxDriveTrain::PidControlledEngine* engine)
      : m_engine(engine)
    {
    }
 
    virtual agxPlot::DataGenerator::Result getValue()
    {
      agxPlot::DataGenerator::Result result;
      if (m_engine == nullptr)
      {
        result.hasValue = false;
        return result;
      }
      result.hasValue = true;
      result.value = m_engine->getPowerGenerator()->getPowerTimeIntegralLookupTable()->lookupCurrent() * m_engine->getThrottle();
      return result;
    }
 
  private:
    agxDriveTrain::PidControlledEngine* m_engine;
  };
}
 
 
 
void runTutorial1(bool /*showPlots*/)
{
  std::cout << "\nTutorial 1: Demonstrating basic drive train component creation and assembly." << std::endl;
 
  agxSDK::SimulationRef simulation = new agxSDK::Simulation();
 
  // A DriveTrain system is always part of a PowerLine. A PowerLine consists of
  // a graph of Units and Connectors. In the AGX world, a Unit represents one
  // or more bodies and a Connector represents one or more constraints. For
  // the drive train, a shaft is an example of a Unit, while gears,
  // and viscous couplings (torque converter, clutch) are Connectors.
  agxPowerLine::PowerLineRef powerLine = new agxPowerLine::PowerLine();
 
  // The power line has to be part of some simulation to work. It is not
  // necessary to add each and every component to the simulation, the power
  // line will handle that. Instead the component must be made part of the
  // power line. More on this later in this tutorial.
  simulation->add(powerLine);
 
  /*
  The two most fundamental parts of the DriveTrain is the Shaft and the Gear.
  Let's create two shafts and connect them with a gear.
  */
 
  // The shaft can be considered a 1DOF rigid body with inertia, angular
  // velocity and angle (position) as scalars.
  agxDriveTrain::ShaftRef shaft1 = new agxDriveTrain::Shaft();
  agxDriveTrain::ShaftRef shaft2 = new agxDriveTrain::Shaft();
 
  // The Gear is a connector that creates a constraint that constrains
  // the angular velocity between two shafts (Non-holonomic constraint)
  agxDriveTrain::GearRef gearA = new agxDriveTrain::Gear();
 
  // Use the connect member function to connect the first shaft to the gear,
  // and then the gear to the second shaft.
  shaft1->connect(gearA);
  gearA->connect(shaft2);
 
  // The agxPowerLine::OUTPUT of shaft1 is connected to the agxPowerLine::INPUT
  // side of the gearA, and the agxPowerLine::OUTPUT side of the gearA is
  // connected to the agxPowerLine::INPUT side of shaft2. A similar system can
  // be made in a more detailed manner, where we also control what side of the
  // shafts and gear we choose to connect what. See below for creation and
  // assembly of a similar system, with shaft3, shaft4 and gearB.
  agxDriveTrain::ShaftRef shaft3 = new agxDriveTrain::Shaft();
  agxDriveTrain::ShaftRef shaft4 = new agxDriveTrain::Shaft();
  agxDriveTrain::GearRef gearB = new agxDriveTrain::Gear();
 
  //Connect output from shaft3 to input of gearB,  just as shaft1 is connected to gearA
  gearB->connect(agxPowerLine::INPUT, agxPowerLine::OUTPUT, shaft3);
 
  // With the extra control we have when specifying both the connector and unit
  // side, we can connect shaft4 "backwards" by attaching the gear's agxPowerLine::OUTPUT to the
  // agxPowerLine::OUTPUT side of the shaft. This will cause shaft4 to rotate in the opposite
  // direction of shaft3.
  gearB->connect(agxPowerLine::OUTPUT, agxPowerLine::OUTPUT, shaft4);
 
  // The final step is to add references of our connected graphs of units and
  // connector to the power line. When a Unit is added the power line will
  // traverse the graph and add all Units and Connectors it finds along the way.
  // Since  our components are connected in two separate graphs, two calls to
  // PowerLine::add is needed to simulate them all.
  powerLine->add(shaft1); // Will also add gearA and shaft2 to the power line, and also to the simulation.
  powerLine->add(shaft3); // Will also add gearB and shaft4 to the power line, and also to the simulation.
 
  // Let's set an initial angular velocity on shaft1 and shaft3...
  agx::Real initialAngularVelocity = agx::Real(2);
  shaft1->getRotationalDimension()->setGradient(initialAngularVelocity);
  shaft3->getRotationalDimension()->setGradient(initialAngularVelocity);
 
  // ...and then step the system.
  //
  // The expected result is that angular velocity of shaft1 is equivalent to the
  // angular velocity of shaft 2, which is 1 rad/s (keeping the angular momentum
  // of the system when accelerating shafts 2 and 4), and that the angular
  // velocity of shaft3 is equivalent to the angular velocity of shaft 4 with a
  // flipped sign.
  simulation->stepTo(simulation->getTimeStamp() + agx::Real(0.5));
 
  std::cout << "Angular velocities shaft 1, 2, 3 & 4:" << std::endl;
  std::cout << " Shaft1  :  " << shaft1->getRotationalDimension()->getGradient() << " rad/s." << std::endl;
  std::cout << " Shaft2  :  " << shaft2->getRotationalDimension()->getGradient() << " rad/s." << std::endl;
  std::cout << " Shaft3  :  " << shaft3->getRotationalDimension()->getGradient() << " rad/s." << std::endl;
  std::cout << " Shaft4  :  " << shaft4->getRotationalDimension()->getGradient() << " rad/s." << std::endl;
}
 
 
 
void runTutorial2(bool showPlots)
{
  std::cout << "\nTutorial 2: Demonstrating creation and configuration of an Engine." << std::endl;
 
  agxSDK::SimulationRef simulation = new agxSDK::Simulation();
 
  agxPowerLine::PowerLineRef powerLine = new agxPowerLine::PowerLine();
 
  simulation->add(powerLine);
 
  // The PidControlledEngine contains a Shaft, so it is a 1DOF rigid body, with
  // inertia, angular velocity and angle (position) as scalars.
  agxDriveTrain::PidControlledEngineRef engine = new agxDriveTrain::PidControlledEngine();
 
  // Create table of maximum produced torque at specific RPMs in an
  // RPM-to-torque table. Note that the two first entries contain the same
  // torque, 100 RPM and 400 RPM both deliver 700 Nm. This leads to making the
  // curve flat before 400 RPM, always being able to deliver 700 Nm.
  agx::Vector<std::pair<agx::Real, agx::Real> > rpm_torque;
  rpm_torque.push_back(std::make_pair(agx::Real(100),  agx::Real(700)));
  rpm_torque.push_back(std::make_pair(agx::Real(400),  agx::Real(700)));
  rpm_torque.push_back(std::make_pair(agx::Real(700),  agx::Real(800)));
  rpm_torque.push_back(std::make_pair(agx::Real(900),  agx::Real(1300)));
  rpm_torque.push_back(std::make_pair(agx::Real(1000), agx::Real(1400)));
  rpm_torque.push_back(std::make_pair(agx::Real(1200), agx::Real(1600)));
  // This configuration is designed  so that after 1600RPM the engine gets weaker
  // and maximum possible delivered torque will decrease after 1600RPM.
  rpm_torque.push_back(std::make_pair(agx::Real(1600), agx::Real(1700)));
  rpm_torque.push_back(std::make_pair(agx::Real(1900), agx::Real(1000)));
  // At RPM's higher than 2000 RPM we design the engine to have no torque available.
  // Internal friction is at equilibrium with the produced torque.
  rpm_torque.push_back(std::make_pair(agx::Real(2000), agx::Real(0)));
  rpm_torque.push_back(std::make_pair(agx::Real(2001), agx::Real(0)));
 
  // Now add the table to the engine.
  engine->setRPMTorqueTable(rpm_torque);
 
  // If the rpm_torque table is not configured, a default value will be used:
  // Go through the engine power generator to configure the default value of torque.
  agx::Real defaultTorque = agx::Real(50);
  engine->getPowerGenerator()->getPowerTimeIntegralLookupTable()->setDefaultValue(defaultTorque);
 
  // The engine being named 'PidControlled' means that it has an internal object
  // handling throttle given the PidControl implementation. AGX is not using any
  // PidController by default. A ThrottleController can be set to the
  // PidControlledEngine instead of controlling it by doing
  // PidControlledEngine::setThrottle( agx::Real throttle ). However, in this
  // example we are satisfied with applying full throttle from start and
  // forward. 0 is no throttle 1 (100%) is maximum throttle.
  engine->setThrottle( agx::Real(1));
 
  // The engine must also be turned on.
  engine->ignition(true);
 
  //Let's create some heavily damped object, to drive with engine
  agxDriveTrain::ShaftRef heavilyDampedShaft = new agxDriveTrain::Shaft( );
  heavilyDampedShaft->getRotationalDimension()->setVelocityDamping( agx::Real(1000.0) );
  heavilyDampedShaft->setInertia( agx::Real( 100 ) );
 
 
  // Create a gear with 100 in gear ratio.
  agxDriveTrain::GearRef gear100 = new agxDriveTrain::Gear( agx::Real( 100 ) );
 
  // Connect the engine to the shaft using the gear
  engine->connect(gear100);
  gear100->connect(heavilyDampedShaft);
 
  // Add the connected graph to the power line.
  powerLine->add(engine);
 
  // Create a plot showing the engine RPM and the torque exposed to the gear.
  // See tutorial_plot for details.
  if (showPlots) {
    agxPlot::System* plotSystem = simulation->getPlotSystem();
    agxPlot::DataSeries* timeStampsSeries = simulation->getTimeDataSeries();
    agxPlot::DataSeriesRef rpmSeries = new agxPlot::DataSeries(new DriveTrainShaftRPMListener(engine), "RPM");
    agxPlot::DataSeriesRef torqueSeries = new agxPlot::DataSeries(new DriveTrainTorqueListener(gear100), "Torque");
    rpmSeries->setUnit("RPM");
    torqueSeries->setUnit("Nm");
    agxPlot::CurveRef rpmCurve = new agxPlot::Curve(timeStampsSeries, rpmSeries, "Engine RPM");
    agxPlot::CurveRef gearTorqueCurve = new agxPlot::Curve(timeStampsSeries, torqueSeries, "Gear torque");
    agxPlot::Window* plotWindow = plotSystem->getOrCreateWindow("RPM");
    plotWindow->add(rpmCurve);
    plotWindow = plotSystem->getOrCreateWindow("Torque");
    plotWindow->add(gearTorqueCurve);
    plotSystem->add(new agxPlot::FilePlot("EngineDrivingHeavyShaft.dat"));
#if AGX_USE_WEBPLOT()
    plotSystem->add(new agxPlot::WebPlot(true));
#endif
  }
 
 
  simulation->stepTo( agx::Real( 0.5 ) );
 
  std::cout << " Engine RPM after 0.5 seconds  :  " << engine->getRPM( ) << " RPM." << std::endl;
  std::cout << " Gear torque exposure after 0.5 seconds  :  " << gear100->getConstraint()->getCurrentForce() << " RPM." << std::endl;
 
 
  simulation->stepTo(agx::Real(5));
 
 
  std::cout << " Engine RPM after 5.0 seconds  :  " << engine->getRPM( ) << " RPM." << std::endl;
  std::cout << " Gear torque exposure after 5.0 seconds  :  " << gear100->getConstraint( )->getCurrentForce( ) << " RPM." << std::endl;
 
  simulation->stepTo( agx::Real( 10 ) );
 
 
  std::cout << " Engine RPM after 10.0 seconds  :  " << engine->getRPM( ) << " RPM." << std::endl;
  std::cout << " Gear torque exposure after 10.0 seconds  :  " << gear100->getConstraint( )->getCurrentForce( ) << " RPM." << std::endl;
 
  std::cout << "See web plot or 'EngineDrivingHeavyShaft.dat' for full picture of engine RPM and gear torque" << std::endl;
}
 
 
void runTutorial3(bool showPlots)
{
  std::cout << "\nTutorial 3: Demonstrating creation and configuration of an RotationalActuator." << std::endl;
 
  agxSDK::SimulationRef simulation = new agxSDK::Simulation();
 
  agxPowerLine::PowerLineRef powerLine = new agxPowerLine::PowerLine();
 
  simulation->add(powerLine);
 
  //Let's create a constant torque engine
  agxDriveTrain::PidControlledEngineRef engine = new agxDriveTrain::PidControlledEngine();
  agx::Real defaultTorque = agx::Real(50); // in W/s
  engine->getPowerGenerator( )->getPowerTimeIntegralLookupTable( )->setDefaultValue( defaultTorque );
  engine->setThrottle(agx::Real(1));
  engine->ignition(true);
 
  // Create one heavy object, called ground
  agx::RigidBodyRef ground = new agx::RigidBody("Ground");
  ground->getMassProperties()->setMass(agx::Real(1E8));
  simulation->add(ground);
 
  // Create a light object, called propeller
  agx::RigidBodyRef propeller = new agx::RigidBody("Propeller");
  propeller->getMassProperties()->setMass(agx::Real(1E1));
  simulation->add(propeller);
 
  //hinge the two bodies
  agx::FrameRef groundFrame = new agx::Frame();
  agx::FrameRef propellerFrame = new agx::Frame();
  agx::Constraint::calculateFramesFromWorld(agx::Vec3(0, 0, 0), agx::Vec3::Z_AXIS(), ground, groundFrame, propeller, propellerFrame);
  agx::HingeRef hinge = new agx::Hinge(ground, groundFrame, propeller, propellerFrame);
  simulation->add(hinge);
 
  // Set zero gravity, so that nothing will fall.
  simulation->setUniformGravity(agx::Vec3(0, 0, 0));
 
  // Create at rotational actuator using the hinge
  // The rotational actuator works as a Unit, and is possible to connect to, just as any shaft
  agxPowerLine::RotationalActuatorRef rotationalActuator = new agxPowerLine::RotationalActuator(hinge);
 
  // Create a gear and connect the engine to the rotational actuator
  agxDriveTrain::GearRef gear = new agxDriveTrain::Gear();
  engine->connect(gear);
  gear->connect(rotationalActuator);
 
  // Add the connected graph to the power line.
  powerLine->add(engine);
 
  // Create a plot showing the engine RPM and the torque exposed to the gear.
  // See tutorial_plot for details.
  if (showPlots) {
    agxPlot::System* plotSystem = simulation->getPlotSystem();
    agxPlot::DataSeries* timeStampsSeries = simulation->getTimeDataSeries();
    agxPlot::DataSeriesRef rpmSeries = new agxPlot::DataSeries(new DriveTrainShaftRPMListener(engine), "RPM");
    agxPlot::DataSeriesRef torqueSeries = new agxPlot::DataSeries(new DriveTrainTorqueListener(gear), "Torque");
    agxPlot::DataSeriesRef relRpmSeries = new agxPlot::DataSeries(new DriveTrainRotationalActuatorRelativeRPMListener(rotationalActuator), "RPM");
 
    rpmSeries->setUnit("RPM");
    torqueSeries->setUnit("Nm");
    relRpmSeries->setUnit("RPM");
 
    agxPlot::CurveRef rpmCurve = new agxPlot::Curve(timeStampsSeries, rpmSeries, "Engine RPM");
    agxPlot::CurveRef gearTorqueCurve = new agxPlot::Curve(timeStampsSeries, torqueSeries, "Gear torque");
    agxPlot::CurveRef relRpmCurve = new agxPlot::Curve(timeStampsSeries, relRpmSeries, "Rotational Actuator RPM");
    agxPlot::Window* plotWindow = plotSystem->getOrCreateWindow();
    plotWindow->add(rpmCurve);
    plotWindow->add(gearTorqueCurve);
    plotWindow->add(relRpmCurve);
    plotSystem->add(new agxPlot::FilePlot("EngineDrivingRotationalActuator.dat"));
#if AGX_USE_WEBPLOT()
    plotSystem->add(new agxPlot::WebPlot(true));
#endif
  }
 
  simulation->stepTo(agx::Real(0.5));
 
  std::cout << " Engine RPM after 0.5 seconds  :  " << engine->getRPM() << " RPM." << std::endl;
  std::cout << " Gear torque exposure after 0.5 seconds  :  " << gear->getConstraint()->getCurrentForce() << " Nm." << std::endl;
  std::cout << " RotationalActuator RPM after 0.5 seconds  :  " << agxPowerLine::RotationalUnit::convertAngularVelocityToRPM( rotationalActuator->calculateRelativeGradient() ) << " RPM." << std::endl;
 
 
  simulation->stepTo(agx::Real(5));
 
 
  std::cout << " Engine RPM after 5 seconds  :  " << engine->getRPM() << " RPM." << std::endl;
  std::cout << " Gear torque exposure after 5 seconds  :  " << gear->getConstraint()->getCurrentForce() << " Nm." << std::endl;
  std::cout << " RotationalActuator RPM after 5 seconds  :  " << agxPowerLine::RotationalUnit::convertAngularVelocityToRPM(rotationalActuator->calculateRelativeGradient()) << " RPM." << std::endl;
 
  simulation->stepTo(agx::Real(10));
 
  std::cout << " Engine RPM after 10 seconds  :  " << engine->getRPM() << " RPM." << std::endl;
  std::cout << " Gear torque exposure after 10 seconds  :  " << gear->getConstraint()->getCurrentForce() << " Nm." << std::endl;
  std::cout << " RotationalActuator RPM after 10 seconds  :  " << agxPowerLine::RotationalUnit::convertAngularVelocityToRPM(rotationalActuator->calculateRelativeGradient()) << " RPM." << std::endl;
 
 
  std::cout << "See web plot or 'EngineDrivingRotationalActuator.dat' for full picture of RPM and torque" << std::endl;
}
 
namespace
{
  agxDriveTrain::PidControlledEngineRef createConfiguredEngine()
  {
    // Let's create a engine with the same specifications as in scene 2.
    agxDriveTrain::PidControlledEngineRef engine = new agxDriveTrain::PidControlledEngine();
    agx::Vector<std::pair<agx::Real, agx::Real> > rpm_torque;
    rpm_torque.push_back(std::make_pair(agx::Real(100), agx::Real(700)));
    rpm_torque.push_back(std::make_pair(agx::Real(400), agx::Real(700)));
    rpm_torque.push_back(std::make_pair(agx::Real(700), agx::Real(800)));
    rpm_torque.push_back(std::make_pair(agx::Real(900), agx::Real(1300)));
    rpm_torque.push_back(std::make_pair(agx::Real(1000), agx::Real(1400)));
    rpm_torque.push_back(std::make_pair(agx::Real(1200), agx::Real(1600)));
    rpm_torque.push_back(std::make_pair(agx::Real(1600), agx::Real(1700)));
    rpm_torque.push_back(std::make_pair(agx::Real(1900), agx::Real(1000)));
    rpm_torque.push_back(std::make_pair(agx::Real(2000), agx::Real(0)));
    rpm_torque.push_back(std::make_pair(agx::Real(2001), agx::Real(0)));
    engine->setRPMTorqueTable(rpm_torque);
 
    engine->ignition(true);
    return engine;
  }
 
  agxPowerLine::RotationalActuatorRef createRotationalActuatorAndHingedSystem(agxSDK::Simulation* simulation)
  {
    agx::RigidBodyRef ground = new agx::RigidBody("Ground");
    ground->getMassProperties()->setMass(agx::Real(1E8));
    simulation->add(ground);
    agx::RigidBodyRef propeller = new agx::RigidBody("Propeller");
    propeller->getMassProperties()->setMass(agx::Real(1E1));
    simulation->add(propeller);
    agx::FrameRef groundFrame = new agx::Frame();
    agx::FrameRef propellerFrame = new agx::Frame();
    agx::Constraint::calculateFramesFromWorld(agx::Vec3(0, 0, 0), agx::Vec3::Z_AXIS(), ground, groundFrame, propeller, propellerFrame);
    agx::HingeRef hinge = new agx::Hinge(ground, groundFrame, propeller, propellerFrame);
    simulation->add(hinge);
    agxPowerLine::RotationalActuatorRef rotationalActuator = new agxPowerLine::RotationalActuator(hinge);
 
    return rotationalActuator;
  }
}
 
void runTutorial4(bool showPlots)
{
  std::cout << "\nTutorial 4: Demonstrating RpmController for Engine." << std::endl;
 
  agxSDK::SimulationRef simulation = new agxSDK::Simulation( );
 
  agxPowerLine::PowerLineRef powerLine = new agxPowerLine::PowerLine( );
 
  simulation->add( powerLine );
 
  // Create and configure an engine the same way as in scene 2.
  agxDriveTrain::PidControlledEngineRef engine = ::createConfiguredEngine();
 
  // Lets create the same RotationalActuator as in scene 3 and connect it to this engine
  agxPowerLine::RotationalActuatorRef rotationalActuator = createRotationalActuatorAndHingedSystem(simulation);
 
  agxDriveTrain::GearRef gear = new agxDriveTrain::Gear();
  engine->connect(gear);
  gear->connect(rotationalActuator);
 
  simulation->setUniformGravity( agx::Vec3( 0, 0, 0 ) );
 
  powerLine->add( engine );
 
  // Get the hinge from the rotationalActuator
  agx::Hinge* hinge = static_cast<agx::Hinge*>(rotationalActuator->getConstraint());
 
  // Enable the hinge motor
  hinge->getMotor1D()->setEnable(true);
  // Set the desired speed to zero
  hinge->getMotor1D()->setSpeed(agx::Real(0));
  // Set the maximum hinge motor force to 20Nm.
  agx::Real hingeFrictionForce = agx::Real(20);
  hinge->getMotor1D()->setForceRange(agx::RangeReal(hingeFrictionForce));
 
  agx::Real targetRpm = agx::Real(1000);
  //Some controller specific values.
  agx::Real pGain = agx::Real(0.5);
  agx::Real dGain = agx::Real(0.5);
  agx::Real iGain = agx::Real(0.05);
  agx::Real iRange = agx::Real(1100);
  agxDriveTrain::RpmControllerRef rpmController = new agxDriveTrain::RpmController( engine, targetRpm, pGain, dGain, iGain, iRange );
  engine->setThrottleCalculator( rpmController );
 
  // Create a plot showing the engine RPM and the torque exposed to the gear.
  // See tutorial_plot for details.
  if (showPlots) {
    agxPlot::System* plotSystem = simulation->getPlotSystem( );
    agxPlot::DataSeries* timeStampsSeries = simulation->getTimeDataSeries( );
    agxPlot::DataSeriesRef rpmSeries = new agxPlot::DataSeries( new DriveTrainShaftRPMListener( engine ), "RPM" );
    agxPlot::DataSeriesRef torqueSeries = new agxPlot::DataSeries( new DriveTrainTorqueListener( gear ), "Torque" );
    agxPlot::DataSeriesRef throttleSeries = new agxPlot::DataSeries( new DriveTrainEngineThrottleListener( engine ), "%" );
 
    rpmSeries->setUnit( "RPM" );
    torqueSeries->setUnit( "Nm" );
    throttleSeries->setUnit( "%" );
 
    agxPlot::CurveRef rpmCurve = new agxPlot::Curve( timeStampsSeries, rpmSeries, "Engine RPM" );
    agxPlot::CurveRef gearTorqueCurve = new agxPlot::Curve( timeStampsSeries, torqueSeries, "Gear torque" );
    agxPlot::CurveRef throttleCurve = new agxPlot::Curve( timeStampsSeries, throttleSeries, "Engine throttle %" );
    agxPlot::Window* plotWindow = plotSystem->getOrCreateWindow( );
    plotWindow->add( rpmCurve );
    plotWindow->add( gearTorqueCurve );
    plotWindow->add( throttleCurve );
    plotSystem->add( new agxPlot::FilePlot( "EngineThrottle.dat" ) );
#if AGX_USE_WEBPLOT()
    plotSystem->add(new agxPlot::WebPlot(true));
#endif
  }
 
 
  simulation->stepTo( agx::Real( 0.5 ) );
 
  std::cout << " Engine RPM after 0.5 seconds  :  " << engine->getRPM( ) << " RPM." << std::endl;
  std::cout << " Gear torque exposure after 0.5 seconds  :  " << gear->getConstraint( )->getCurrentForce( ) << " Nm." << std::endl;
  std::cout << " Engine throttle 0.5 seconds  :  " << engine->getThrottle()*agx::Real(100) << "%" << std::endl;
 
 
  simulation->stepTo( agx::Real( 5 ) );
 
 
  std::cout << " Engine RPM after 5 seconds  :  " << engine->getRPM( ) << " RPM." << std::endl;
  std::cout << " Gear torque exposure after 5 seconds  :  " << gear->getConstraint( )->getCurrentForce( ) << " Nm." << std::endl;
  std::cout << " Engine throttle 5 seconds  :  " << engine->getThrottle( )*agx::Real( 100 ) << "%" << std::endl;
 
 
  simulation->stepTo( agx::Real( 10 ) );
 
  std::cout << " Engine RPM after 10 seconds  :  " << engine->getRPM( ) << " RPM." << std::endl;
  std::cout << " Gear torque exposure after 10 seconds  :  " << gear->getConstraint( )->getCurrentForce( ) << " Nm." << std::endl;
  std::cout << " Engine throttle 10 seconds  :  " << engine->getThrottle( )*agx::Real( 100 ) << "%" << std::endl;
 
  simulation->stepTo(agx::Real(20));
 
  std::cout << " Engine RPM after 20 seconds  :  " << engine->getRPM() << " RPM." << std::endl;
  std::cout << " Gear torque exposure after 20 seconds  :  " << gear->getConstraint()->getCurrentForce() << " Nm." << std::endl;
  std::cout << " Engine throttle 20 seconds  :  " << engine->getThrottle()*agx::Real(100) << "%" << std::endl;
 
 
  std::cout << "See web plot or 'EngineThrottle.dat' for full picture of RPM, torque and engine throttle." << std::endl;
}
 
namespace
{
  void createRpmControllerAndInternalRotationalActuatorResistance(agxDriveTrain::PidControlledEngine* engine, agxPowerLine::RotationalActuator* rotationalActuator, agx::Real constantFrictionForce)
  {
    // Get the hinge from the rotationalActuator
    agx::Hinge* hinge = static_cast<agx::Hinge*>(rotationalActuator->getConstraint());
 
    // Enable the hinge motor
    hinge->getMotor1D()->setEnable(true);
    // Set the desired speed to zero
    hinge->getMotor1D()->setSpeed(agx::Real(0));
    // Set the maximum hinge motor force to 20Nm.
    hinge->getMotor1D()->setForceRange(agx::RangeReal(constantFrictionForce));
 
    agx::Real targetRpm = agx::Real(1000);
    //Some controller specific values.
    agx::Real pGain = agx::Real(0.5);
    agx::Real dGain = agx::Real(0.5);
    agx::Real iGain = agx::Real(0.05);
    agx::Real iRange = agx::Real(1100);
    agxDriveTrain::RpmControllerRef rpmController = new agxDriveTrain::RpmController(engine, targetRpm, pGain, dGain, iGain, iRange);
    engine->setThrottleCalculator(rpmController);
 
  }
}
 
void runTutorial5(bool showPlots)
{
  std::cout << "\nTutorial 5: Demonstrating TorqueConverter functionality." << std::endl;
  // A torque converter is a type of fuild coupling and used to transfer power in a drive train, instead of using a
  // clutch. It has a pump, which recieves torque from the engine and will rotate, in turn pushing the fluid in the
  // torque converter to the turbine, which will make anything connected to the turbine rotate as well. Braking,
  // and stopping the turbine from spinning, will not force the pump to stop and thus the engine can keep on spinning.
 
  agxSDK::SimulationRef simulation = new agxSDK::Simulation();
 
  agxPowerLine::PowerLineRef powerLine = new agxPowerLine::PowerLine();
 
  simulation->add(powerLine);
  simulation->setUniformGravity(agx::Vec3(0, 0, 0));
 
  // Create and configure an engine the same way as in scene 2.
  agxDriveTrain::PidControlledEngineRef engine = ::createConfiguredEngine();
 
  // Lets create the same RotationalActuator as in scene 3 and connect it to this engine
  agxPowerLine::RotationalActuatorRef rotationalActuator = createRotationalActuatorAndHingedSystem(simulation);
 
  // Create an rpm controller for the engine, and
  agx::Real constantFrictionForce = agx::Real(20);
  createRpmControllerAndInternalRotationalActuatorResistance(engine, rotationalActuator, constantFrictionForce);
 
  agxDriveTrain::TorqueConverterRef torqueConverter = new agxDriveTrain::TorqueConverter();
 
  agx::RealPairVector velRatio_efficiency;
  velRatio_efficiency.push_back(agx::RealPair(agx::Real(0.0), agx::Real(2.5)));
  velRatio_efficiency.push_back(agx::RealPair(agx::Real(0.5), agx::Real(1.8)));
  velRatio_efficiency.push_back(agx::RealPair(agx::Real(0.95), agx::Real(0.8)));
  velRatio_efficiency.push_back(agx::RealPair(agx::Real(1.0), agx::Real(0.0)));
  velRatio_efficiency.push_back(agx::RealPair(agx::Real(1.1), agx::Real(1.0)));
  velRatio_efficiency.push_back(agx::RealPair(agx::Real(1.25), agx::Real(1.7)));
  velRatio_efficiency.push_back(agx::RealPair(agx::Real(1.5), agx::Real(1.7)));
  torqueConverter->setEfficiencyTable(velRatio_efficiency);
 
  // Connect the engine to the rotational actuator, in the same way we used a gear in previous scenes.
  engine->connect(torqueConverter);
  torqueConverter->connect(rotationalActuator);
 
  powerLine->add(engine);
 
  // It is also possible to lock the torque converter, making the input and output of it always rotate at the same
  // speed. Normally, in cars, this is used when the vehicle is traveling at a stable velocity. The locking up of
  // the torque converter happens over a short time period and is started calling:
  torqueConverter->enableLockUp(true);
 
  // When braking, gassing up quickly and shifting gear, the lock should be disengaged. This does not happen
  // automatically, but can be done by calling:
  torqueConverter->enableLockUp(false);
 
  // Create a plot showing the engine RPM and the torque exposed to the gear.
  // See tutorial_plot for details.
  if (showPlots) {
    agxPlot::System* plotSystem = simulation->getPlotSystem();
    agxPlot::DataSeries* timeStampsSeries = simulation->getTimeDataSeries();
    agxPlot::DataSeriesRef rpmSeries = new agxPlot::DataSeries(new DriveTrainShaftRPMListener(engine), "RPM");
    agxPlot::DataSeriesRef torqueSeries = new agxPlot::DataSeries(new DriveTrainTorqueListener(torqueConverter), "Torque");
    agxPlot::DataSeriesRef relRpmSeries = new agxPlot::DataSeries(new DriveTrainRotationalActuatorRelativeRPMListener(rotationalActuator), "RPM");
    agxPlot::DataSeriesRef engineTorqueSeries = new agxPlot::DataSeries(new DriveTrainEngineAppliedTorqueListener(engine), "Torque");
 
    rpmSeries->setUnit("RPM");
    torqueSeries->setUnit("Nm");
    relRpmSeries->setUnit("RPM");
    engineTorqueSeries->setUnit("Nm");
 
    agxPlot::CurveRef rpmCurve = new agxPlot::Curve(timeStampsSeries, rpmSeries, "Engine RPM");
    agxPlot::CurveRef gearTorqueCurve = new agxPlot::Curve(timeStampsSeries, torqueSeries, "Converter torque");
    agxPlot::CurveRef relRpmCurve = new agxPlot::Curve(timeStampsSeries, relRpmSeries, "Hinge RPM");
    agxPlot::CurveRef engineTorqueCurve = new agxPlot::Curve(timeStampsSeries, engineTorqueSeries, "Engine torque");
 
    agxPlot::Window* plotWindow = plotSystem->getOrCreateWindow();
    plotWindow->add(rpmCurve);
    plotWindow->add(gearTorqueCurve);
    plotWindow->add(relRpmCurve);
    plotWindow->add(engineTorqueCurve);
    plotSystem->add(new agxPlot::FilePlot("TorqueConverter.dat"));
#if AGX_USE_WEBPLOT()
    plotSystem->add(new agxPlot::WebPlot(true));
#endif
  }
 
  simulation->stepTo(agx::Real(0.5));
 
  std::cout << " Engine RPM after 0.5 seconds  :  " << engine->getRPM() << " RPM." << std::endl;
  std::cout << " Converter torque after 0.5 seconds  :  " << torqueConverter->getTurbineTorque() << " Nm." << std::endl;
  std::cout << " RotationalActuator RPM after 0.5 seconds  :  " << agxPowerLine::RotationalUnit::convertAngularVelocityToRPM(rotationalActuator->calculateRelativeGradient()) << " RPM.\n" << std::endl;
 
  simulation->stepTo(agx::Real(2));
 
  std::cout << " Engine RPM after 2 seconds  :  " << engine->getRPM() << " RPM." << std::endl;
  std::cout << " Converter torque after 2 seconds  :  " << torqueConverter-> getTurbineTorque() << " Nm." << std::endl;
  std::cout << " RotationalActuator RPM after 2 seconds  :  " << agxPowerLine::RotationalUnit::convertAngularVelocityToRPM(rotationalActuator->calculateRelativeGradient()) << " RPM.\n" << std::endl;
 
  simulation->stepTo(agx::Real(4));
 
  std::cout << " Engine RPM after 4 seconds  :  " << engine->getRPM() << " RPM." << std::endl;
  std::cout << " Converter torque after 4 seconds  :  " << torqueConverter->getTurbineTorque() << " Nm." << std::endl;
  std::cout << " RotationalActuator RPM after 4 seconds  :  " << agxPowerLine::RotationalUnit::convertAngularVelocityToRPM(rotationalActuator->calculateRelativeGradient()) << " RPM.\n" << std::endl;
 
  simulation->stepTo(agx::Real(6));
 
  std::cout << " Engine RPM after 6 seconds  :  " << engine->getRPM() << " RPM." << std::endl;
  std::cout << " Converter torque after 6 seconds  :  " << torqueConverter->getTurbineTorque() << " Nm." << std::endl;
  std::cout << " RotationalActuator RPM after 6 seconds  :  " << agxPowerLine::RotationalUnit::convertAngularVelocityToRPM(rotationalActuator->calculateRelativeGradient()) << " RPM.\n" << std::endl;
 
  std::cout << "See web plot or 'TorqueConverter.dat' for full picture of RPM and torque." << std::endl;
}
 
void runTutorial6(bool showPlots)
{
  std::cout << "\nTutorial 6: Demonstrating Clutch functionality." << std::endl;
 
  agxSDK::SimulationRef simulation = new agxSDK::Simulation();
 
  agxPowerLine::PowerLineRef powerLine = new agxPowerLine::PowerLine();
 
  simulation->add(powerLine);
  simulation->setUniformGravity(agx::Vec3(0, 0, 0));
 
  // Create and configure an engine the same way as in scene 2.
  agxDriveTrain::PidControlledEngineRef engine = ::createConfiguredEngine();
 
  // Lets create the same RotationalActuator as in scene 3 and connect it to this engine
  agxPowerLine::RotationalActuatorRef rotationalActuator = createRotationalActuatorAndHingedSystem(simulation);
 
  // Create an rpm controller for the engine, and
  agx::Real constantFrictionForce = agx::Real(20);
  createRpmControllerAndInternalRotationalActuatorResistance(engine, rotationalActuator, constantFrictionForce);
 
  agxDriveTrain::ClutchRef clutch = new agxDriveTrain::Clutch();
 
  // Connect the engine to the rotational actuator, in the same way we used a gear in previous scenes.
  engine->connect(clutch);
  clutch->connect(rotationalActuator);
 
  powerLine->add(engine);
 
 
  // Create a plot showing the engine RPM and the torque exposed to the gear.
  // See tutorial_plot for details.
  if (showPlots) {
    agxPlot::System* plotSystem = simulation->getPlotSystem();
    agxPlot::DataSeries* timeStampsSeries = simulation->getTimeDataSeries();
    agxPlot::DataSeriesRef rpmSeries = new agxPlot::DataSeries(new DriveTrainShaftRPMListener(engine), "RPM");
    agxPlot::DataSeriesRef torqueSeries = new agxPlot::DataSeries(new DriveTrainTorqueListener(clutch), "Torque");
    agxPlot::DataSeriesRef relRpmSeries = new agxPlot::DataSeries(new DriveTrainRotationalActuatorRelativeRPMListener(rotationalActuator), "RPM");
    agxPlot::DataSeriesRef engineTorqueSeries = new agxPlot::DataSeries(new DriveTrainEngineAppliedTorqueListener(engine), "Torque");
 
    rpmSeries->setUnit("RPM");
    torqueSeries->setUnit("Nm");
    relRpmSeries->setUnit("RPM");
    engineTorqueSeries->setUnit("Nm");
 
    agxPlot::CurveRef rpmCurve = new agxPlot::Curve(timeStampsSeries, rpmSeries, "Engine RPM");
    agxPlot::CurveRef gearTorqueCurve = new agxPlot::Curve(timeStampsSeries, torqueSeries, "Clutch torque");
    agxPlot::CurveRef relRpmCurve = new agxPlot::Curve(timeStampsSeries, relRpmSeries, "Hinge RPM");
    agxPlot::CurveRef engineTorqueCurve = new agxPlot::Curve(timeStampsSeries, engineTorqueSeries, "Engine torque");
 
    agxPlot::Window* plotWindow = plotSystem->getOrCreateWindow();
    plotWindow->add(rpmCurve);
    plotWindow->add(gearTorqueCurve);
    plotWindow->add(relRpmCurve);
    plotWindow->add(engineTorqueCurve);
    plotSystem->add(new agxPlot::FilePlot("Clutch.dat"));
#if AGX_USE_WEBPLOT()
    plotSystem->add(new agxPlot::WebPlot(true));
#endif
  }
 
 
  for (size_t i = 0; i < 600; ++i)
  {
    if (i % 100 == 0)
    {
      std::cout << " Engine RPM after" << agx::Real(i) / agx::Real(60) << " seconds  :  " << engine->getRPM() << " RPM." << std::endl;
      std::cout << " Clutch after" << agx::Real(i) / agx::Real(60) << " seconds  :  " << clutch->getConstraint()->getCurrentForce() << " Nm." << std::endl;
      std::cout << " RotationalActuator RPM after" << agx::Real(i) / agx::Real(60) << " seconds  :  " << agxPowerLine::RotationalUnit::convertAngularVelocityToRPM(rotationalActuator->calculateRelativeGradient()) << " RPM." << std::endl;
      std::cout << " Engine torque applied after" << agx::Real(i) / agx::Real(60) << " seconds  :  " << engine->getPowerGenerator()->getPowerTimeIntegralLookupTable()->lookupCurrent() << " RPM." << std::endl;
    }
    clutch->setEfficiency(std::min(std::pow(agx::Real(i) / agx::Real(500), 2.0), agx::Real(1)));
    simulation->stepForward();
  }
 
}
 
 
void runTutorial7(bool showPlots)
{
  std::cout << "\nTutorial 7: Demonstrating GearBox functionality." << std::endl;
 
  agxSDK::SimulationRef simulation = new agxSDK::Simulation();
 
  agxPowerLine::PowerLineRef powerLine = new agxPowerLine::PowerLine();
 
  simulation->add(powerLine);
  simulation->setUniformGravity(agx::Vec3(0, 0, 0));
 
  // Create and configure an engine the same way as in scene 2.
  agxDriveTrain::PidControlledEngineRef engine = ::createConfiguredEngine();
 
  // Lets create the same RotationalActuator as in scene 3 and connect it to this engine
  agxPowerLine::RotationalActuatorRef rotationalActuator = createRotationalActuatorAndHingedSystem(simulation);
 
  // Create an rpm controller for the engine, and
  agx::Real constantFrictionForce = agx::Real(20);
  createRpmControllerAndInternalRotationalActuatorResistance(engine, rotationalActuator, constantFrictionForce);
 
  agxDriveTrain::GearBoxRef gearBox = new agxDriveTrain::GearBox();
 
  agx::RealVector gearRatios;
 
  // Define six gears
  gearRatios.push_back(agx::Real(10));
  gearRatios.push_back(agx::Real(7));
  gearRatios.push_back(agx::Real(5));
  gearRatios.push_back(agx::Real(3.5));
  gearRatios.push_back(agx::Real(2));
  gearRatios.push_back(agx::Real(1));
 
  gearBox->setGearRatios(gearRatios);
 
  // Connect the engine to the rotational actuator, in the same way we used a gear in previous scenes.
  engine->connect(gearBox);
  gearBox->connect(rotationalActuator);
 
  powerLine->add(engine);
 
 
  // Create a plot showing the engine RPM and the torque exposed to the gear.
  // See tutorial_plot for details.
  if (showPlots) {
    agxPlot::System* plotSystem = simulation->getPlotSystem();
    agxPlot::DataSeries* timeStampsSeries = simulation->getTimeDataSeries();
    agxPlot::DataSeriesRef rpmSeries = new agxPlot::DataSeries(new DriveTrainShaftRPMListener(engine), "RPM");
    agxPlot::DataSeriesRef torqueSeries = new agxPlot::DataSeries(new DriveTrainTorqueListener(gearBox), "Torque");
    agxPlot::DataSeriesRef relRpmSeries = new agxPlot::DataSeries(new DriveTrainRotationalActuatorRelativeRPMListener(rotationalActuator), "RPM");
 
    rpmSeries->setUnit("RPM");
    torqueSeries->setUnit("Nm");
    relRpmSeries->setUnit("RPM");
 
    agxPlot::CurveRef rpmCurve = new agxPlot::Curve(timeStampsSeries, rpmSeries, "Engine RPM");
    agxPlot::CurveRef gearTorqueCurve = new agxPlot::Curve(timeStampsSeries, torqueSeries, "GearBox torque");
    agxPlot::CurveRef relRpmCurve = new agxPlot::Curve(timeStampsSeries, relRpmSeries, "Hinge RPM");
 
    agxPlot::Window* plotWindow = plotSystem->getOrCreateWindow();
    plotWindow->add(rpmCurve);
    plotWindow->add(gearTorqueCurve);
    plotWindow->add(relRpmCurve);
    plotSystem->add(new agxPlot::FilePlot("GearBox.dat"));
#if AGX_USE_WEBPLOT()
    plotSystem->add(new agxPlot::WebPlot(true));
#endif
  }
 
 
  gearBox->setGear(1);
  for (size_t i = 1; i < 600; ++i)
  {
    if (i % 100 == 0)
    {
      int gear = static_cast<int>(i) / 100;
      std::cout << "using gear [" << i << "] with gear ratio " << gearBox->getGearRatio(gear) << std::endl;
      gearBox->setGear(gear);
      std::cout << " Engine RPM after" << agx::Real(i) / agx::Real(60) << " seconds  :  " << engine->getRPM() << " RPM." << std::endl;
      std::cout << " GearBox after" << agx::Real(i) / agx::Real(60) << " seconds  :  " << gearBox->getConstraint()->getCurrentForce() << " Nm." << std::endl;
      std::cout << " RotationalActuator RPM after" << agx::Real(i) / agx::Real(60) << " seconds  :  " << agxPowerLine::RotationalUnit::convertAngularVelocityToRPM(rotationalActuator->calculateRelativeGradient()) << " RPM." << std::endl;
 
    }
 
    simulation->stepForward();
  }
 
}
 
void runTutorial8(bool showPlots)
{
  std::cout << "\nTutorial 8: Demonstrating Differential functionality." << std::endl;
 
  agxSDK::SimulationRef simulation = new agxSDK::Simulation();
 
  agxPowerLine::PowerLineRef powerLine = new agxPowerLine::PowerLine();
 
  simulation->add(powerLine);
  simulation->setUniformGravity(agx::Vec3(0, 0, 0));
 
  // Create and configure an engine the same way as in scene 2.
  agxDriveTrain::PidControlledEngineRef engine = ::createConfiguredEngine();
 
  // Lets create the same RotationalActuator as in scene 3 and connect it to this engine
  agxPowerLine::RotationalActuatorRef rotationalActuator = createRotationalActuatorAndHingedSystem(simulation);
 
  // Create an rpm controller for the engine, and
  agx::Real constantFrictionForce = agx::Real(20);
  createRpmControllerAndInternalRotationalActuatorResistance(engine, rotationalActuator, constantFrictionForce);
 
  agxDriveTrain::DifferentialRef differential = new agxDriveTrain::Differential();
 
  // Lets create the same RotationalActuator as in scene 3 and connect it to this engine
  agxPowerLine::RotationalActuatorRef rotationalActuator2 = createRotationalActuatorAndHingedSystem(simulation);
 
  // Connect the engine to the differential
  differential->connect(agxPowerLine::INPUT, agxPowerLine::OUTPUT, engine);
 
  // connect the two rotational actuators to the output side of the differential
  differential->connect(agxPowerLine::OUTPUT, agxPowerLine::INPUT, rotationalActuator);
  differential->connect(agxPowerLine::OUTPUT, agxPowerLine::INPUT, rotationalActuator2);
 
  powerLine->add(engine);
 
 
  // Create a plot showing the engine RPM and the torque exposed to the gear.
  // See tutorial_plot for details.
  if (showPlots) {
    agxPlot::System* plotSystem = simulation->getPlotSystem();
    agxPlot::DataSeries* timeStampsSeries = simulation->getTimeDataSeries();
    agxPlot::DataSeriesRef rpmSeries = new agxPlot::DataSeries(new DriveTrainShaftRPMListener(engine), "RPM");
    agxPlot::DataSeriesRef relRpmSeries = new agxPlot::DataSeries(new DriveTrainRotationalActuatorRelativeRPMListener(rotationalActuator), "RPM");
    agxPlot::DataSeriesRef relRpm2Series = new agxPlot::DataSeries(new DriveTrainRotationalActuatorRelativeRPMListener(rotationalActuator2), "RPM");
 
    rpmSeries->setUnit("RPM");
    relRpmSeries->setUnit("RPM");
    relRpm2Series->setUnit("RPM");
 
    agxPlot::CurveRef rpmCurve = new agxPlot::Curve(timeStampsSeries, rpmSeries, "Engine RPM");
    agxPlot::CurveRef relRpmCurve = new agxPlot::Curve(timeStampsSeries, relRpmSeries, "Hinge with friction RPM");
    agxPlot::CurveRef relRpm2Curve = new agxPlot::Curve(timeStampsSeries, relRpm2Series, "Hinge without friction RPM");
 
    agxPlot::Window* plotWindow = plotSystem->getOrCreateWindow();
    plotWindow->add(rpmCurve);
    plotWindow->add(relRpmCurve);
    plotWindow->add(relRpm2Curve);
 
    //plotWindow = plotSystem->getOrCreateWindow("Fraction");
 
    plotSystem->add(new agxPlot::FilePlot("Differential.dat"));
#if AGX_USE_WEBPLOT()
    plotSystem->add(new agxPlot::WebPlot(true));
#endif
  }
 
 
  for (size_t i = 0; i < 600; ++i)
  {
    if (i % 100 == 0)
    {
      std::cout << " Engine RPM after" << agx::Real(i) / agx::Real(60) << " seconds  :  " << engine->getRPM() << " RPM." << std::endl;
      std::cout << " RotationalActuator with friction RPM after" << agx::Real(i) / agx::Real(60) << " seconds  :  " << agxPowerLine::RotationalUnit::convertAngularVelocityToRPM(rotationalActuator->calculateRelativeGradient()) << " RPM." << std::endl;
      std::cout << " RotationalActuator without friction RPM after" << agx::Real(i) / agx::Real(60) << " seconds  :  " << agxPowerLine::RotationalUnit::convertAngularVelocityToRPM(rotationalActuator2->calculateRelativeGradient()) << " RPM." << std::endl;
 
    }
 
    simulation->stepForward();
  }
 
}
 
void runTutorial9(bool showPlots)
{
  std::cout << "\nTutorial 9: Demonstrating Dry Clutch functionality." << std::endl;
 
  agxSDK::SimulationRef simulation = new agxSDK::Simulation();
 
  agxPowerLine::PowerLineRef powerLine = new agxPowerLine::PowerLine();
 
  simulation->add(powerLine);
  simulation->setUniformGravity(agx::Vec3(0, 0, 0));
 
  // Create and configure an engine the same way as in scene 2.
  agxDriveTrain::PidControlledEngineRef engine = ::createConfiguredEngine();
 
  // Lets create the same RotationalActuator as in scene 3 and connect it to this engine
  agxPowerLine::RotationalActuatorRef rotationalActuator = createRotationalActuatorAndHingedSystem(simulation);
 
  // Create an rpm controller for the engine, and
  agx::Real constantFrictionForce = agx::Real(20);
  createRpmControllerAndInternalRotationalActuatorResistance(engine, rotationalActuator, constantFrictionForce);
 
  agxDriveTrain::DryClutchRef dryClutch = new agxDriveTrain::DryClutch();
  dryClutch->setManualMode(true);
  // Note that: the user is recommended to set the appropriate torque capacity according to their clutch and engine setup.
  dryClutch->setTorqueCapacity(1000);
  dryClutch->setAutoLock(true);
 
  // Connect the engine to the rotational actuator, in the same way we used a gear in previous scenes.
  engine->connect(dryClutch);
  dryClutch->connect(rotationalActuator);
 
  powerLine->add(engine);
 
  // Create a plot showing the engine RPM and the torque exposed to the gear.
  // See tutorial_plot for details.
  if (showPlots) {
    agxPlot::System* plotSystem = simulation->getPlotSystem();
    agxPlot::DataSeries* timeStampsSeries = simulation->getTimeDataSeries();
    agxPlot::DataSeriesRef rpmSeries = new agxPlot::DataSeries(new DriveTrainShaftRPMListener(engine), "RPM");
    agxPlot::DataSeriesRef relRpmSeries = new agxPlot::DataSeries(new DriveTrainRotationalActuatorRelativeRPMListener(rotationalActuator), "RPM");
    agxPlot::DataSeriesRef engineTorqueSeries = new agxPlot::DataSeries(new DriveTrainEngineAppliedTorqueListener(engine), "Torque");
    agxPlot::DataSeriesRef torqueSeries = new agxPlot::DataSeries(new DriveTrainTorqueListener(dryClutch), "Torque");
 
    rpmSeries->setUnit("RPM");
    relRpmSeries->setUnit("RPM");
    engineTorqueSeries->setUnit("Nm");
    torqueSeries->setUnit("Nm");
 
    agxPlot::CurveRef rpmCurve = new agxPlot::Curve(timeStampsSeries, rpmSeries, "Engine RPM");
    agxPlot::CurveRef relRpmCurve = new agxPlot::Curve(timeStampsSeries, relRpmSeries, "Shaft RPM");
    agxPlot::CurveRef engineTorqueCurve = new agxPlot::Curve(timeStampsSeries, engineTorqueSeries, "Engine torque");
    agxPlot::CurveRef gearTorqueCurve = new agxPlot::Curve(timeStampsSeries, torqueSeries, "Dry clutch torque");
 
    agxPlot::Window* plotWindow = plotSystem->getOrCreateWindow();
    plotWindow->add(rpmCurve);
    plotWindow->add(relRpmCurve);
    plotWindow->add(engineTorqueCurve);
    plotWindow->add(gearTorqueCurve);
    plotSystem->add(new agxPlot::FilePlot("DryClutch.dat"));
#if AGX_USE_WEBPLOT()
    plotSystem->add(new agxPlot::WebPlot(true));
#endif
  }
 
 
  for (size_t i = 0; i < 600; ++i)
  {
    if (i % 100 == 0)
    {
      std::cout << " Engine RPM after" << agx::Real(i) / agx::Real(60) << " seconds  :  " << engine->getRPM() << " RPM." << std::endl;
      std::cout << " Dry clutch torque after" << agx::Real(i) / agx::Real(60) << " seconds  :  " << dryClutch->getTorque() << " Nm." << std::endl;
      std::cout << " RotationalActuator RPM after" << agx::Real(i) / agx::Real(60) << " seconds  :  " << agxPowerLine::RotationalUnit::convertAngularVelocityToRPM(rotationalActuator->calculateRelativeGradient()) << " RPM." << std::endl;
      std::cout << " Engine torque applied after" << agx::Real(i) / agx::Real(60) << " seconds  :  " << engine->getPowerGenerator()->getPowerTimeIntegralLookupTable()->lookupCurrent() << " RPM." << std::endl;
    }
    dryClutch->setFraction(std::min(std::pow(agx::Real(i) / agx::Real(500), 2.0), agx::Real(1)));
    simulation->stepForward();
  }
}
 
 
 
void runTutorial10(bool showPlots)
{
  std::cout << "\nTutorial 10: Demonstrating how to connect a WheelJoint to a drive-train." << std::endl;
 
  // In tutorial 3 we learned how to power a hinge with an engine using a
  // RotationalActuator. Many vehicle simulations use WheelJoint instead of
  // Hinge to connect the wheels to the vehicle. A RotationalActuator by itself
  // cannot be connected to a WheelJoint because a WheelJoint contains multiple
  // free degrees of freedom and we must tell the Actuator which of then it
  // should act on. A WheelJoint contains WHEEL, for rotating the wheels,
  // STEERING, for turning the wheels, and SUSPENSION, for translating the
  // wheels up and down. An engine is typically connected to the WHEEL degree
  // of freedom, making the wheels spin and the vehicle move forward when
  // power is delivered from the engine to the wheels.
 
  // First we create the regular objects required in most tutorials.
  agxSDK::SimulationRef simulation = new agxSDK::Simulation();
  agxPowerLine::PowerLineRef powerLine = new agxPowerLine::PowerLine();
  simulation->add(powerLine);
 
  // Create a vehicle model. Keeping it simple since this is not the focus for
  // this tutorial. See tutorial_wheel_joint.agxPy for a tutorial on how
  // WheelJoints are created and configured.
  agx::RigidBodyRef chassis = new agx::RigidBody("Chassis");
  chassis->getMassProperties()->setMass(agx::Real(1000));
  simulation->add(chassis);
  agx::RigidBodyRef wheel = new agx::RigidBody("Wheel");
  wheel->getMassProperties()->setMass(agx::Real(10));
  simulation->add(wheel);
  agxVehicle::WheelJointFrame wheelFrame(agx::Vec3(), agx::Vec3::Y_AXIS(), agx::Vec3::Z_AXIS());
  agxVehicle::WheelJointRef wheelJoint = new agxVehicle::WheelJoint(wheelFrame, wheel, chassis);
  simulation->add(wheelJoint);
 
  // A simple engine model is used, since the engine is not the focus of this
  // tutorial.
  agxDriveTrain::FixedVelocityEngineRef engine = new agxDriveTrain::FixedVelocityEngine();
  engine->setTargetRpm(agx::Real(1500));
  powerLine->add(engine);
 
  agxVehicle::WheelJointConstraintGeometryRef wheelGeometry = new agxVehicle::WheelJointConstraintGeometry(
      wheelJoint, agxVehicle::WheelJoint::WHEEL);
  agxPowerLine::RotationalActuatorRef actuator = new agxPowerLine::RotationalActuator(wheelJoint, wheelGeometry);
  powerLine->add(actuator);
  engine->connect(actuator);
 
  if (showPlots)
  {
    agxPlot::System* plotSystem = simulation->getPlotSystem();
    agxPlot::DataSeries* timeStampsSeries = simulation->getTimeDataSeries();
    agxPlot::DataSeriesRef engineRpmSeries = new agxPlot::DataSeries(
        new DriveTrainShaftRPMListener(engine), "Engine RPM");
    agxPlot::DataSeriesRef wheelRpmSeries = new agxPlot::DataSeries(
        new DriveTrainRotationalActuatorRelativeRPMListener(actuator), "Wheel RPM");
 
    engineRpmSeries->setUnit("RPM");
    wheelRpmSeries->setUnit("RPM");
 
    agxPlot::CurveRef engineRpmCurve = new agxPlot::Curve(timeStampsSeries, engineRpmSeries, "Engine RPM");
    agxPlot::CurveRef wheelRpmCurve = new agxPlot::Curve(timeStampsSeries, wheelRpmSeries, "Wheel RPM");
    agxPlot::Window* plotWindow = plotSystem->getOrCreateWindow();
    plotWindow->add(engineRpmCurve);
    plotWindow->add(wheelRpmCurve);
    plotSystem->add(new agxPlot::FilePlot("RotationalActuatorDrivingWheel.dat"));
#if AGX_USE_WEBPLOT()
    plotSystem->add(new agxPlot::WebPlot(true));
#endif
  }
 
  simulation->stepTo(agx::Real(0.1));
 
  std::cout << " Engine RPM after 0.1 seconds  :  " << engine->getRPM() << " RPM." << std::endl;
  std::cout << " Wheel RPM after 0.1 seconds  :  " << agxPowerLine::RotationalUnit::convertAngularVelocityToRPM(actuator->calculateRelativeGradient()) << " RPM." << std::endl;
 
  simulation->stepTo(agx::Real(0.2));
 
  std::cout << " Engine RPM after 0.2 seconds  :  " << engine->getRPM() << " RPM." << std::endl;
  std::cout << " Wheel RPM after 0.2 seconds  :  " << agxPowerLine::RotationalUnit::convertAngularVelocityToRPM(actuator->calculateRelativeGradient()) << " RPM." << std::endl;
 
  simulation->stepTo(agx::Real(1.0));
 
  std::cout << " Engine RPM after 1 second  :  " << engine->getRPM() << " RPM." << std::endl;
  std::cout << " Wheel RPM after 1 second  :  " << agxPowerLine::RotationalUnit::convertAngularVelocityToRPM(actuator->calculateRelativeGradient()) << " RPM." << std::endl;
 
  if (showPlots)
  {
 
    // This tutorial run for a short amount of time, which means the plot window
    // we open may not have had time to get the plot date before we start
    // tearing down everything. Wait a bit to give it a chance to read the data.
    using namespace std::chrono_literals;
    std::this_thread::sleep_for(1s);
    simulation->stepForward();
  }
 
  std::cout << "See web plot or 'RotationalActuatorDrivingWheel.dat' for full picture of RPM and torque" << std::endl;
}
 
int main(int argc, char** /*argv*/)
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial Hydraulics\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  bool showPlots = argc <= 1; // Do not show plots if we sent in arguments - this is only for the internal testing framework at Algoryx.
 
  runTutorial1(showPlots);
  runTutorial2(showPlots);
  runTutorial3(showPlots);
  runTutorial4(showPlots);
  runTutorial5(showPlots);
  runTutorial6(showPlots);
  runTutorial7(showPlots);
  runTutorial8(showPlots);
  runTutorial9(showPlots);
  runTutorial10(showPlots);
 
  return 0;
}

tutorial_driveTrain_ElectricMotor.cpp

/*
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.
*/
 
 
/*
This file contains a collection of tutorials for the ElectricMotor, in AGX
DriveTrain. It describes the the basics of creating an electric motor and
how this can be use together with other drive train components in two simple
examples.
 
The tutorial creates no graphics window. All state inspection is instead done
using prints to the console.
*/
 
#include <agxDriveTrain/ElectricMotor.h>
#include <agxDriveTrain/Shaft.h>
#include <agxDriveTrain/Gear.h>
 
#include <agx/version.h>
 
 
 
void runTutorial1()
{
  std::cout << "\nTutorial 1: Demonstrating basic electric motor creation.\n\n";
 
  agxSDK::SimulationRef simulation = new agxSDK::Simulation();
 
  // An ElectricMotor is part of a DriveTrain and a DriveTrain system is always
  // part of a PowerLine. A PowerLine consists of a graph of Units and Connectors,
  // where a Unit represents one or more bodies and a Connector represents one or
  // more constraints. An electric motor is in AGX a Unit that is a source of
  // power.
  agxPowerLine::PowerLineRef powerLine = new agxPowerLine::PowerLine();
 
  // The power line has to be part of some simulation to work. It is not
  // necessary to add each and every component to the simulation, the power
  // line will handle that. Instead the component must be made part of the
  // power line.
  simulation->add(powerLine);
 
  // When creating an ElectricMotor the resistance, torque constant, emf constant,
  // and inductance of the motor must be known.
  // The resistance describes the electrical resistance of the motor.
  agx::Real resistance = agx::Real(1.0);
 
  // The EMF constant, or back EMF constant, can be described by the back EMF voltage
  // of the motor divided by the angular velocity of the motor.
  agx::Real emfConstant = agx::Real(1.0);
 
  // The torque constant can be described by the torque produced divided by the
  // armature current of the motor.
  agx::Real torqueConstant = agx::Real(1.0);
 
  // Next we set the inductance of the motor
  agx::Real inductance = agx::Real(0.05);
 
  // Now we create the motor
  agxDriveTrain::ElectricMotorRef motor = new agxDriveTrain::ElectricMotor(resistance, torqueConstant, emfConstant,
                                            inductance);
 
  // Since the voltage of the motor is set to zero when it is created, the voltage
  // needs to be set to some other value to start the motor spinning.
  agx::Real voltage = agx::Real(5.0);
  motor->setVoltage(voltage);
 
  // Next step is to add the motor to the power line.
  powerLine->add(motor);
 
  // Initially the angular velocity of the motror should be zero
  std::cout << "Time(s)\tAngular velocity(rad/s)" << std::endl;
  std::cout << "0 \t" << motor->getAngularVelocity()  << std::endl;
 
  // Finally we step in the simulation to see how the angular velocity increases
  // with time, unil it reaches maximum velocity.
  simulation->stepTo(agx::Real(5.0));
  std::cout << 5.0 << " \t" << motor->getAngularVelocity() << std::endl;
 
  for (int i = 15; i <= 60; i += 15) {
    simulation->stepTo(agx::Real(i));
    std::cout << i << " \t" << motor->getAngularVelocity() << std::endl;
  }
}
 
 
void runTutorial2()
{
  std::cout << "\nTutorial 2: Demonstrating using electric motor with other drive train components.\n\n";
 
  // First we create the simulation, power line and electrical motor
  agxSDK::SimulationRef simulation = new agxSDK::Simulation();
  agxPowerLine::PowerLineRef powerLine = new agxPowerLine::PowerLine();
  simulation->add(powerLine);
 
  agx::Real resistance = agx::Real(1.0);
  agx::Real emfConstant = agx::Real(1.0);
  agx::Real torqueConstant = agx::Real(1.0);
  agx::Real inductance = agx::Real(0.05);
 
  agxDriveTrain::ElectricMotorRef motor = new agxDriveTrain::ElectricMotor(resistance, torqueConstant, emfConstant,
                                            inductance);
 
  // Next, set the voltage to some value other than zero
  agx::Real voltage = agx::Real(5.0);
  motor->setVoltage(voltage);
 
  // Then we create a shaft to connect to the electric motor
  agxDriveTrain::ShaftRef shaft = new agxDriveTrain::Shaft();
 
  // Create a gear to connect the shaft to the motor, with gear constant 10
  agxDriveTrain::GearRef gear = new agxDriveTrain::Gear(agx::Real(10.0));
 
  // The motor is then connected to the shaft, with the gear
  motor->connect(gear);
  gear->connect(shaft);
 
  // Add the motor to the power line
  powerLine->add(motor);
 
  // Finally we step the simulation and see that the shaft has an RPM 10x larger
  // than the motor. We also see the torque exposure to the gear at different
  // time steps.
  simulation->stepTo(agx::Real(0.5));
 
  std::cout << " Engine RPM after 0.5 seconds  :  " << motor->getRPM() << " RPM." << std::endl;
  std::cout << " Shaft RPM after 0.5 seconds  :  " << shaft->getRPM() << " RPM." << std::endl;
  std::cout << " Gear torque exposure after 0.5 seconds  :  " << gear->getConstraint()->getCurrentForce() << " Nm." << std::endl;
 
 
  simulation->stepTo(agx::Real(5));
 
  std::cout << " Engine RPM after 5.0 seconds  :  " << motor->getRPM() << " RPM." << std::endl;
  std::cout << " Shaft RPM after 5.0 seconds  :  " << shaft->getRPM() << " RPM." << std::endl;
  std::cout << " Gear torque exposure after 5.0 seconds  :  " << gear->getConstraint()->getCurrentForce() << " Nm." << std::endl;
 
  simulation->stepTo(agx::Real(10));
 
  std::cout << " Engine RPM after 10.0 seconds  :  " << motor->getRPM() << " RPM." << std::endl;
  std::cout << " Shaft RPM after 10.0 seconds  :  " << shaft->getRPM() << " RPM." << std::endl;
  std::cout << " Gear torque exposure after 10.0 seconds  :  " << gear->getConstraint()->getCurrentForce() << " Nm." << std::endl;
 
}
 
 
 
int main(int /*argc*/, char** /*argv*/)
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial Electric Motor\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  runTutorial1();
  runTutorial2();
 
  return 0;
}

tutorial_driveTrain_combustionEngine.cpp

/*
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.
*/
 
/*
Demonstrates the use of the Combustion engine in a drivetrain.
*/
 
#include <agx/config/AGX_USE_WEBPLOT.h>
 
#include <agxOSG/ExampleApplication.h>
 
#include <agxDriveTrain/CombustionEngine.h>
#include <agxDriveTrain/DryClutch.h>
#include <agxDriveTrain/Shaft.h>
#include <agxDriveTrain/Gear.h>
#include <agxDriveTrain/Brake.h>
 
#include <agxPowerLine/Actuator1DOF.h>
 
#include <agxCollide/Cylinder.h>
#include <agxCollide/Plane.h>
#include <agxCollide/Box.h>
 
#include <agxUtil/agxUtil.h>
 
#include <agx/version.h>
 
#include "tutorialUtils.h"
 
#include <agxPlot/System.h>
#if AGX_USE_WEBPLOT()
#include <agxPlot/WebPlot.h>
#endif
 
 
agx::Real v_max = 100;
agx::Real ms_to_kph = 3.6;
agx::Real kph_to_ms = 1.0 / 3.6;
 
struct CarConfig
{
  agx::Real width = 1.6;
  agx::Real length = 4.6;
  agx::Real height = 1.0;
 
  struct Wheel
  {
    agx::Real width = 0.2;
    agx::Real radius = 0.3262;
    agx::Real mass = 10;
    agx::Real inertia = 1.0;
  } wheel;
 
  struct Chassis {
    agx::Real mass = 1500;
  } chassis;
 
  struct Engine {
    agx::Real crankShaftInertia = 0.2;
  } engine;
 
  struct DryClutch
  {
    agx::Real torqueCapacity = 300;
    struct Duration
    {
      agx::Real engage = 1.5;
      agx::Real disengage = 0.2;
    } duration;
  } dryClutch;
 
  agx::Vector<agx::Real> gearRatios = { 0.0, 3.583, 2.050, 1.267, 0.838, 0.617, 0.510, -2.833 };
  agx::Real gearShiftDuration = 0.5;
  agx::Vector<agx::Real> maxSpeedsPerGear = { 16, 32, 48, 64, 80, v_max };
  agx::Vector<agx::Real> minSpeedsPerGear = { 8, 24, 34, 40, 56, 70 };
 
  struct Differential
  {
    agx::Real finalDriveRatio = 10;
  } differential;
 
  struct Material
  {
    agx::Real dampingRate = 2.0;
    agx::Real friction = 1.0;
    agx::FrictionModel::SolveType solverType = agx::FrictionModel::DIRECT;
    agx::Real restitution = 0.0;
    agx::Bool useContactArea = true;
    agx::Real youngsModulus = 1e10;
    agx::Real surfaceViscosity = 1e-3;
  } material;
 
  agx::Real gravity = -10;
  agx::Real timeStep = 1.0 / 140.0;
 
  agx::Vector<agx::String> DrivingStatus = { "Parked", "Idling", "Acclerating", "Braking", "Reversing" };
 
  struct Park
  {
    agx::Real duration = 0.1;
  } park;
 
  struct Idle
  {
    agx::Real duration = 0.1;
  } idle;
 
  struct Reverse
  {
    agx::Real duration = 8.0;
    struct ThrottleAdjust
    {
      agx::Real targetPos = 1.0;
      agx::Real duration = 0.5;
    } throttleAdjust;
  } reverse;
 
  struct Accelerate
  {
    agx::Real targetSpeed = v_max * kph_to_ms;
    struct ThrottleAdjust
    {
      agx::Real targetPos = 1.0;
      agx::Real duration = 0.5;
    } throttleAdjust;
  } accelerate;
 
  struct Brake
  {
    agx::Real targetSpeed = 1e-3;
    agx::Real torque = 1000;
  } brake;
} carConfig;
 
 
struct Car
{
  agx::RigidBodyRef body;
  agx::HingeRef back_left;
  agx::HingeRef back_right;
  agx::HingeRef front_left;
  agx::HingeRef front_right;
 
  agxDriveTrain::CombustionEngineRef engine;
  agxDriveTrain::DryClutchRef clutch;
  agxDriveTrain::ShaftRef inputShaft;
  agxDriveTrain::HolonomicGearRef gearBox;
  agxDriveTrain::ShaftRef outputShaft;
  agxDriveTrain::HolonomicGearRef differential;
  agxDriveTrain::BrakeRef brake;
  agxPowerLine::RotationalActuatorRef leftWheel;
  agxPowerLine::RotationalActuatorRef rightWheel;
};
 
 
struct Action
{
  agx::String name;
  std::function<void()> func;
};
 
 
// Help classes and functions for tutorial 3
class VehicleControl : public agxSDK::StepEventListener
{
  public:
    VehicleControl(Car car, agxOSG::SceneDecorator* sd) :
      m_carBody(car.body), m_engine(car.engine), m_clutch(car.clutch), m_brake(car.brake), m_gearBox(car.gearBox),  m_sceneDecorator(sd)
    {
      m_actions = {
        {"Parked", std::bind(&VehicleControl::park, this)},
        {"Idling", std::bind(&VehicleControl::idle, this)},
        {"Accelerating", std::bind(&VehicleControl::accelerate, this)},
        {"Braking", std::bind(&VehicleControl::brake, this)},
        {"Reversing", std::bind(&VehicleControl::reverse, this)},
      };
      m_sceneDecorator->setFontSize(float(0.028));
    }
 
    void pre(const agx::TimeStamp& t)
    {
      logVehicleData(t);
      updateStatus();
 
      addDisplayStates();
    }
 
 
    void logVehicleData(agx::Real t)
    {
      m_t = t;
      m_carSpeed = m_carBody->getVelocity().x() * ms_to_kph;
      m_engineRPM = m_engine->getRPM();
      m_engineTorque = m_engine->getIndicatedTorque();
      m_clutchEngaged = m_clutch->isEngaged();
    }
 
    void addDisplayStates()
    {
      m_sceneDecorator->setText(0, agx::String::format("Time:  %.2f s", m_t), agxRender::Color::Cyan());
      m_sceneDecorator->setText(2, agx::String::format("Car speed:  %.2f km/h", m_carSpeed), agxRender::Color::Cyan());
      m_sceneDecorator->setText(4, agx::String::format("Engine RPM:  %.2f RPM", m_engineRPM), agxRender::Color::Cyan());
      m_sceneDecorator->setText(6, agx::String::format("Current gear: %d", m_currentGear), agxRender::Color::Cyan());
      m_sceneDecorator->setText(8, agx::String::format("Engine torque:  %.2f Nm", m_engineTorque), agxRender::Color::Cyan());
      m_sceneDecorator->setText(10, agx::String::format("Clutch engaged: %d",  m_clutchEngaged), agxRender::Color::Cyan());
    }
 
    void updateStatus()
    {
      // Find the action corresponding to the current status
      auto action = std::find_if(m_actions.begin(), m_actions.end(),
        [&](const Action& a) { return a.name == m_currentStatus; });
 
      // Perform the action and update the status
      if (action != m_actions.end())
      {
        action->func();
      }
      else
      {
        std::cerr << "Error: unknown status '" << m_currentStatus << "'" << std::endl;
      }
    };
 
    void park()
    {
      if (!m_isParking)
      {
        m_isParking = true;
        // Neutral gear
        m_currentGear = 0;
        setGear();
        releaseThrottle();
        m_engine->setEnable(false);
 
        m_startTime = m_t;
 
        m_actionDuration = carConfig.park.duration;
 
      }
 
      if ((m_t - m_startTime) > m_actionDuration)
      {
        m_currentStatus = "Idling";
        m_isParking = false;
      }
    };
 
    void idle()
    {
      // Make sure that the following actions will only execute one time, once it starts to idle.
      if (!m_isIdling)
      {
        m_isIdling = true;
        m_engine->setEnable(true);
        m_clutch->setEngage(false);
 
        // idle with 1st gear.
        m_currentGear = 1;
        setGear();
 
        // log the start time of idling
        m_startTime = m_t;
 
        // log the idling duration
        m_actionDuration = carConfig.idle.duration;
      }
 
      // Switch current driving status to "Accelerating", once the idling duration is elapsed.
      if (m_t - m_startTime > m_actionDuration)
      {
        m_currentStatus = "Accelerating";
        m_isIdling = false;
 
        // log the start time of accelerating
        m_startTime = m_t;
      }
 
    };
 
    void accelerate()
    {
      // perform accelerating maneuver.
 
      // If car speed reaches to the target speed of 100km / h, then finish acceleration.
      if (m_carSpeed > carConfig.accelerate.targetSpeed* ms_to_kph)
      {
        m_currentStatus = "Braking";
      }
 
      if (m_carSpeed > m_maxSpeedsPerGear[m_currentGear - 1])
      {
        shiftUp();
      }
      else
      {
        agx::Bool isGearShifting = (m_t - m_gearShiftStartTime < m_gearShiftDuration);
 
        if (isGearShifting && m_gearShiftStartTime > 0)
        {
          releaseThrottle();
          disengageClutch();
        }
        else
        {
          if (!m_clutch->isEngaged())
          {
            engageClutch();
            m_targetPos = carConfig.accelerate.throttleAdjust.targetPos;
            m_adjustDuration = carConfig.accelerate.throttleAdjust.duration;
          }
          agx::Real currentPos = computeThrottlePos();
          applyThrottle(currentPos);
        }
      }
    };
 
    void brake()
    {
      if (!m_isBraking)
      {
        m_isBraking = true;
        releaseThrottle();
        disengageClutch();
      }
 
      applyBrake();
 
      // Keep braking, if speed is lower than the minimum speed limit per gear.
      // gear shift down.
      try {
        if (abs(m_carSpeed) < m_minSpeedsPerGear[m_currentGear - 1] && m_currentGear <= (int) m_minSpeedsPerGear.size())
        {
          shiftDown();
        }
      }
      catch (std::out_of_range& /*e*/) {
        // When reversing: currentGear is 7, it will be out-of-range.
        // Here it is designed by purpose for holding the reversing gear to be 7 until it brakes to stop.
      }
 
      // Car already stopped, so finish braking oepration.
      if (abs(m_carSpeed) < carConfig.brake.targetSpeed)
      {
        releaseBrake();
        m_isBraking = false;
        if (m_currentGear == 7)
          m_currentStatus = "Parked";
        else
          m_currentStatus = "Reversing";
        return;
      }
 
    };
 
    void reverse()
    {
      if (!m_isReversing)
      {
        m_isReversing = true;
 
        // reversing gear 7
        m_currentGear = 7;
        setGear();
 
        if (!m_clutch->isEngaged())
          engageClutch();
 
        // log the start time of reversing
        m_startTime = m_t;
 
        m_actionDuration = carConfig.reverse.duration;
        m_adjustDuration = carConfig.reverse.throttleAdjust.duration;
        m_targetPos = carConfig.reverse.throttleAdjust.targetPos;
      }
 
      agx::Real currentPos = computeThrottlePos();
      applyThrottle(currentPos);
 
      if ((m_t - m_startTime) > m_actionDuration)
      {
        m_currentStatus = "Braking";
        m_isReversing = false;
        return;
      }
    };
 
 
    agx::Real getGearRatio()
    {
      return m_gearRatios[m_currentGear];
    }
 
    void setGear()
    {
      agx::Real gearRatio = getGearRatio();
      m_gearBox->setGearRatio(gearRatio);
      m_gearBox->rebind();
    }
 
    void shiftUp()
    {
      releaseThrottle();
      disengageClutch();
      m_currentGear = agx::clamp<int>((int)m_currentGear + 1, 0, 6);
      setGear();
      m_gearShiftStartTime = m_t;
      m_startTime = m_t + m_gearShiftDuration;
    }
 
    void shiftDown()
    {
      m_currentGear = agx::clamp<int>((int)m_currentGear - 1, 1, (int)m_currentGear - 1);
      setGear();
    }
 
    void engageClutch()
    {
      m_clutch->setEngage(true);
      m_clutch->setEngageTimeConstant(m_engageDuration);
    }
 
    void disengageClutch()
    {
      m_clutch->setEngage(false);
      m_clutch->setDisengageTimeConstant(m_disengageDuration);
 
    }
 
    void applyThrottle(agx::Real targetPos)
    {
      m_engine->setThrottle(targetPos);
    }
 
    void releaseThrottle()
    {
      m_engine->setThrottle(0.0);
    }
 
    void applyBrake()
    {
      m_brake->setEngage(true);
    }
 
    void releaseBrake()
    {
      m_brake->setEngage(false);
    }
 
    agx::Real computeThrottlePos()
    {
      agx::Real elapsedTime = m_t - m_startTime;
      agx::Real duration = m_adjustDuration;
      agx::Real targetPos = m_targetPos;
 
      return agx::clamp(targetPos * elapsedTime / duration, 0.0, targetPos);
    }
 
  private :
    agx::RigidBody* m_carBody;
    agxDriveTrain::CombustionEngine* m_engine;
    agxDriveTrain::DryClutch* m_clutch;
    agxDriveTrain::Brake* m_brake;
    agxDriveTrain::HolonomicGear* m_gearBox;
 
    agx::Vector<agx::Real> m_gearRatios = carConfig.gearRatios;
    agx::Vector<agx::Real> m_maxSpeedsPerGear = carConfig.maxSpeedsPerGear;
    agx::Vector<agx::Real> m_minSpeedsPerGear = carConfig.minSpeedsPerGear;
    agx::Real m_gearShiftDuration = carConfig.gearShiftDuration;
    agx::Real m_engageDuration = carConfig.dryClutch.duration.engage;
    agx::Real m_disengageDuration = carConfig.dryClutch.duration.disengage;
 
    agx::Bool m_isParking = false;
    agx::Bool m_isIdling = false;
    agx::Bool m_isBraking = false;
    agx::Bool m_isReversing = false;
 
    agx::Real m_gearShiftStartTime = 0.0;
    agx::Real m_actionDuration = 0.0;
    agx::String m_currentStatus = "Parked";
 
    agx::Real m_t =  0.0;
    agx::Real m_startTime = 0.0;
    agx::Int m_currentGear = 0;
 
    agx::Real m_adjustDuration = 0.0;
    agx::Real m_targetPos = 0.0;
    agx::Real m_carSpeed = 0.0;
    agx::Real m_engineRPM = 0.0;
    agx::Real m_engineTorque = 0.0;
 
    agx::Bool m_clutchEngaged;
 
    agx::Vector<Action> m_actions;
    agxOSG::SceneDecorator* m_sceneDecorator;
};
 
 
class DriveTrainCarSpeedListener : public agxPlot::DataListener
{
public:
  DriveTrainCarSpeedListener(agx::RigidBody* body)
    : m_body(body)
  {
 
  }
 
  virtual agxPlot::DataGenerator::Result getValue()
  {
    agxPlot::DataGenerator::Result result;
    if (m_body == nullptr)
    {
      result.hasValue = false;
      return result;
    }
    result.hasValue = true;
    result.value = m_body->getVelocity().x() * ms_to_kph;
    return result;
  }
 
private:
  agx::RigidBody* m_body;
 
};
 
 
// Creates car wheel
agx::HingeRef createWheel(agx::Vec3 position, agx::RigidBodyRef carBody, agx::MaterialRef material,
                          agxSDK::SimulationRef sim, osg::Group* root)
{
  agx::Real rad = carConfig.wheel.radius;
  agx::Real width = carConfig.wheel.width;
  agx::Real mass = carConfig.wheel.mass;
  agx::Real inertia = carConfig.wheel.inertia;
 
  agxCollide::GeometryRef tireGeom = new agxCollide::Geometry(new agxCollide::Cylinder(rad, width));
  tireGeom->setMaterial(material);
  agx::RigidBodyRef tire = new agx::RigidBody();
  tire->add(tireGeom);
  tire->setPosition(position);
 
  // Set wheel mass and inertia.
  tire->getMassProperties()->setMass(mass);
  tire->getMassProperties()->setInertiaTensor(agx::Vec3(1e-3, inertia, 1e-5));
 
  agxOSG::createVisual(tire, root);
  sim->add(tire);
 
 
  agx::FrameRef f1 = new agx::Frame();
  agx::FrameRef f2 = new agx::Frame();
  agx::Constraint::calculateFramesFromWorld(tire->getPosition(), agx::Vec3::Y_AXIS(), tire, f1, carBody, f2);
  agx::HingeRef hinge = new agx::Hinge(tire, f1, carBody, f2);
  sim->add(hinge);
 
  agxUtil::setEnableCollisions(tire, carBody, false);
 
  return hinge;
}
 
 
// Creates the car, returns the hinge we want to drive with the engine
Car createCar(agxSDK::SimulationRef sim, osg::Group* root)
{
 
  Car car;
 
  agx::Real halfLength = carConfig.length / 2.0;
  agx::Real halfWidth = carConfig.width / 2.0;
  agx::Real halfHeight = carConfig.height / 2.0;
 
  agx::Real wheelPosX = halfLength - carConfig.wheel.radius;
  agx::Real wheelPosY = halfWidth + carConfig.wheel.width;
  agx::Real wheelPosZ = carConfig.wheel.radius;
 
  agx::Real frictionCoef = carConfig.material.friction;
  agx::Real restitution = carConfig.material.restitution;
  agx::Real youngsModulus = carConfig.material.youngsModulus;
  agx::FrictionModel::SolveType solverType = carConfig.material.solverType;
 
  agx::MaterialRef tireMaterial = new agx::Material("tireMaterial");
  agx::MaterialRef groundMaterial = new agx::Material("groundMaterial");
  agx::ContactMaterialRef contactMaterial = new agx::ContactMaterial(tireMaterial, groundMaterial);
  sim->add(contactMaterial);
  contactMaterial->setFrictionCoefficient(frictionCoef, agx::ContactMaterial::PRIMARY_DIRECTION);
  contactMaterial->setFrictionCoefficient(frictionCoef, agx::ContactMaterial::SECONDARY_DIRECTION);
  contactMaterial->setRestitution(restitution);
  contactMaterial->setYoungsModulus(youngsModulus);
  agx::IterativeProjectedConeFrictionRef friction = new agx::IterativeProjectedConeFriction();
  friction->setSolveType(solverType);
  contactMaterial->setFrictionModel(friction);
 
  agxCollide::GeometryRef floorGeom = new agxCollide::Geometry(new agxCollide::Plane(0.0, 0.0, 1.0, 0.0));
  floorGeom->setMaterial(groundMaterial);
  agx::RigidBodyRef floor = new agx::RigidBody();
  floor->setMotionControl(agx::RigidBody::STATIC);
  floor->add(floorGeom);
  sim->add(floor);
 
  agx::RigidBodyRef carBody = new agx::RigidBody(new agxCollide::Geometry(new agxCollide::Box(halfLength, halfWidth, halfHeight)));
  carBody->setPosition(0.0, 0.0, carConfig.height);
  sim->add(carBody);
  agxOSG::createVisual(carBody, root);
 
  carBody->getMassProperties()->setMass(carConfig.chassis.mass);
 
  car.body = carBody;
 
  agx::HingeRef back_left = createWheel(agx::Vec3(-wheelPosX, wheelPosY, wheelPosZ), carBody, tireMaterial, sim, root);
  agx::HingeRef back_right = createWheel(agx::Vec3(-wheelPosX, -wheelPosY, wheelPosZ), carBody, tireMaterial, sim, root);
  agx::HingeRef front_left = createWheel(agx::Vec3(wheelPosX, wheelPosY, wheelPosZ), carBody, tireMaterial, sim, root);
  agx::HingeRef front_right = createWheel(agx::Vec3(wheelPosX, -wheelPosY, wheelPosZ), carBody, tireMaterial, sim, root);
 
  car.back_left = back_left;
  car.back_right = back_right;
  car.front_left = front_left;
  car.front_right = front_right;
 
  return car;
}
 
 
 
osg::Group* runTutorial1(agxSDK::Simulation* sim, agxOSG::ExampleApplication*)
{
  osg::Group* root = new osg::Group();
 
  std::cout << "\nTutorial 1: Demonstrating basic combustion engine creation.\n\n";
 
 
  // A CombustionEngine is part of a DriveTrain and a DriveTrain system is always
  // part of a PowerLine. A PowerLine consists of a graph of Units and Connectors,
  // where a Unit represents one or more bodies and a Connector represents one or
  // more constraints. An combustion engine is in AGX a Unit that is a source of
  // power.
  agxPowerLine::PowerLineRef powerLine = new agxPowerLine::PowerLine();
 
  // The power line has to be part of some simulation to work. It is not necessary to
  // add each and every component to the simulation, the power line will handle that.
  // Instead the component must be made part of the power line.
  sim->add(powerLine);
 
  // When creating a CombustionEngine, the combustion engine parameters must be
  // known.Here, the user can use the program pre-set combustion engine
  // parameters as well as customize their paramters to suit their specific
  // needs.
  // In this tutorial, the general procedure of customizing the engine
  // parameters is presented.
 
  agxDriveTrain::CombustionEngineParameters engineParameters = agxDriveTrain::CombustionEngineParameters();
  engineParameters.displacementVolume = 0.003;
  engineParameters.maxTorque = 300;
  engineParameters.maxTorqueRPM = 1500;
  engineParameters.maxPowerRPM = 2500;
  engineParameters.idleRPM = 900;
 
  // Now we create the engine
  agxDriveTrain::CombustionEngineRef engine = new agxDriveTrain::CombustionEngine(engineParameters);
 
  // Next step is to add the engine to the power line.
  powerLine->add(engine);
 
  // Since the engine is by default not running when it is created, it must be started.
  // This is done by calling the setEnable-method in the engine and setting it to true. If
  // one wants to turn off the engine the same method is called and set to false instead.
  engine->setEnable(true);
 
  // Initially the angular velocity of the engine should be 0.0
  std::cout << "Time(s)\tVelocity(RPM)\tThrottle" << std::endl;
  std::cout << "0 \t" << engine->getRPM() << "\t\t" << engine->getThrottle() << std::endl;
 
  // After started the angular velocity of the engine builds up to the idle RPM
  sim->stepTo(agx::Real(1.0));
  std::cout << "1.0 \t" << engine->getRPM() << "\t\t" << engine->getThrottle() << std::endl;
 
  // Now we increase the throttle opening to 20%. Accordingly, the engine speed
  // will build up.
  engine->setThrottle(agx::Real(0.2));
  sim->stepTo(agx::Real(10.0));
  std::cout << "10.0 \t" << engine->getRPM() << "\t\t" << engine->getThrottle() << std::endl;
 
  return root;
}
 
 
osg::Group* runTutorial2(agxSDK::Simulation* sim, agxOSG::ExampleApplication*)
{
  osg::Group* root = new osg::Group();
 
  std::cout << "\nTutorial 2: Demonstrating connecting combustion engine to other drive train components.\n\n";
 
  // Create power line and engine
  agxPowerLine::PowerLineRef powerLine = new agxPowerLine::PowerLine();
  sim->add(powerLine);
 
 
  // Using the parameters for a Saab engine parameter.
  agxDriveTrain::CombustionEngineParameters saabEngineParameters;
  if (!agxDriveTrain::CombustionEngine::readLibraryProperties("Saab-B234i", saabEngineParameters))
    LOGGER_ERROR() << "Unable to read Saab9000 Engine parameters" << LOGGER_ENDL();
 
  agxDriveTrain::CombustionEngineRef engine = new agxDriveTrain::CombustionEngine(saabEngineParameters);
  powerLine->add(engine);
 
  //Create a shaft to be driven by the engine
  agxDriveTrain::ShaftRef shaft = new agxDriveTrain::Shaft();
 
  // Create a gear with a gear ratio of 10
  agxDriveTrain::GearRef gear = new agxDriveTrain::Gear(agx::Real(10));
 
  // Connect the engine to the gearbox
  engine->connect(gear);
  gear->connect(shaft);
 
  // Start the engine
  engine->setEnable(true);
 
  // After starting the engineand set the throttle to 100 % direct.
  // Note that 100 % throttle means the throttle is fully open to its maximum,
  // which is also a parameter that can be tuned.
  engine->setThrottle(agx::Real(1.0));
 
  // Step the simulation and print the velocity of the shaft.
  sim->stepTo(agx::Real(3.0));
  std::cout << "Time(s)\tVelocity(RPM)\tThrottle" << std::endl;
  std::cout << 3.0 << "\t" << shaft->getRPM() << "\t\t" << engine->getThrottle() << std::endl;
 
  // # Release the throttle and see that the velocity decreases to the idleRPM of
  // Sabb engine divided by the gear ratio, which would be 1000 / 10 = 100RPM
  engine->setThrottle(agx::Real(0.0));
  sim->stepTo(agx::Real(30.0));
  std::cout << 30.0 << "\t" << shaft->getRPM() << "\t\t" << engine->getThrottle() << std::endl;
 
  return root;
}
 
 
 
osg::Group* runTutorial3(agxSDK::Simulation *sim, agxOSG::ExampleApplication * app)
{
  osg::Group* root = new osg::Group();
  std::cout << "\nTutorial 3: Drive a car with the combustion engine.\n\n";
 
  agx::Real crankShaftInertia = carConfig.engine.crankShaftInertia;
  agx::Real clutchTorqueCapacity = carConfig.dryClutch.torqueCapacity;
 
  // Set the time step size.
  sim->setTimeStep(carConfig.timeStep);
 
  // Set the gravity
  sim->setUniformGravity((agx::Vec3(0, 0, carConfig.gravity)));
 
  // Create a car
  Car car = createCar(sim, root);
 
  // Create a power line
  agxPowerLine::PowerLineRef powerLine = new agxPowerLine::PowerLine();
  sim->add(powerLine);
 
  // Create the engine and start it
  // Using the parameters for a Saab engine parameter.
  agxDriveTrain::CombustionEngineRef engine = new agxDriveTrain::CombustionEngine();
  if (!engine->loadLibraryProperties("Saab-B234i"))
    LOGGER_ERROR() << "Unable to load parameters for Saab9000" << LOGGER_ENDL();
 
  powerLine->add(engine);
  engine->setEnable(true);
  engine->setInertia(crankShaftInertia);
 
  // Create dry clutch,
  // add it to the powerline.
  agxDriveTrain::DryClutchRef dryClutch = new agxDriveTrain::DryClutch();
  dryClutch->setTorqueCapacity(clutchTorqueCapacity);
  dryClutch->setAutoLock(true);
  powerLine->add(dryClutch);
 
  // Create the gearbox input shaft,
  // add it to the powerline.
  agxDriveTrain::ShaftRef inputShaft = new agxDriveTrain::Shaft();
  inputShaft->getRotationalDimension()->setName("inputShaft");
  powerLine->add(inputShaft);
 
  // Create a gear box, so as to slower down the car speed.
  agxDriveTrain::HolonomicGearRef gearBox = new agxDriveTrain::HolonomicGear();
  powerLine->add(gearBox);
 
  // Create the gearbox output shaft,
  // add it to the powerline.
  agxDriveTrain::ShaftRef outputShaft = new agxDriveTrain::Shaft();
  outputShaft->getRotationalDimension()->setName("outputShaft");
  powerLine->add(outputShaft);
 
  // Create a differential.
  agxDriveTrain::HolonomicGearRef differential = new agxDriveTrain::HolonomicGear();
  powerLine->add(differential);
  differential->setGearRatio(carConfig.differential.finalDriveRatio);
 
  // Create a brake
  agxDriveTrain::BrakeRef brake = new agxDriveTrain::Brake();
  powerLine->add(brake);
 
  // Create a actuator to connect the engine to one of the wheels.
  // For now we only drive one wheel.
  agxPowerLine::RotationalActuatorRef leftWheel = new agxPowerLine::RotationalActuator(car.front_left);
  powerLine->add(leftWheel);
 
  // Create a actuator to connect the engine to one of the wheels.
  // For now we only drive one wheel.
  agxPowerLine::RotationalActuatorRef rightWheel = new agxPowerLine::RotationalActuator(car.front_right);
  powerLine->add(rightWheel);
 
  // Connect the engine to the actuator, with the gear
  engine->connect(dryClutch);
  dryClutch->connect(inputShaft);
  inputShaft->connect(gearBox);
  gearBox->connect(outputShaft);
  outputShaft->connect(differential);
  differential->connect(brake);
  brake->connect(leftWheel);
  leftWheel->connect(rightWheel);
 
  // Add the powerline components into the car namespace.
  car.engine = engine;
  car.clutch = dryClutch;
  car.inputShaft = inputShaft;
  car.gearBox = gearBox;
  car.outputShaft = outputShaft;
  car.differential = differential;
  car.brake = brake;
  car.leftWheel = leftWheel;
  car.rightWheel = rightWheel;
 
  // Create a scene decorator.
  auto sd = app->getSceneDecorator();
 
  // Add a listener to show the engine velocity and throttle.
  VehicleControl* vehicleControl = new VehicleControl(car, sd);
  sim->add(vehicleControl);
 
  // Switch the showPlot on/off to check the car speed.
  agx::Bool showPlot = false;
  if (showPlot)
  {
    agxPlot::System* plotSystem = sim->getPlotSystem();
    agxPlot::DataSeries* timeStampsSeries = sim->getTimeDataSeries();
    agxPlot::DataSeriesRef carSpeedSeries = new agxPlot::DataSeries(new DriveTrainCarSpeedListener(car.body), "Car speed (km/h)");
 
    agxPlot::CurveRef carSpeedCurve = new agxPlot::Curve(timeStampsSeries, carSpeedSeries, "Car speed");
 
    agxPlot::Window* plotWindow = plotSystem->getOrCreateWindow();
    plotWindow->add(carSpeedCurve);
#if AGX_USE_WEBPLOT()
    plotSystem->add(new agxPlot::WebPlot(true));
#endif
  }
 
  return root;
}
 
 
 
int main(int argc, char** argv)
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial Combustion Engine\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
    /*runTutorial1();
    runTutorial2();*/
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
    application->addScene(runTutorial1);
    application->addScene(runTutorial2);
    application->addScene(runTutorial3);
 
    if (application->init(argc, argv))
      return application->run();
  }
  catch (std::exception& e) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_energyManager.cpp

/*
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.
*/
 
/*
 
Demonstrates how to use the EnergyManager to measure energies in a system.
 
*/
 
#include <agxOSG/ExampleApplication.h>
 
#include <agx/Hinge.h>
#include <agx/Prismatic.h>
 
#include <agxPowerLine/PowerLine.h>
#include <agxPowerLine/Actuator1DOF.h>
#include <agxDriveTrain/ElectricMotor.h>
#include <agxDriveTrain/EnergyManager.h>
 
#include <agxSDK/EnergyManager.h>
 
#include <agxUtil/StepEventCallback.h>
 
#include "tutorialUtils.h"
 
osg::Group* buildTutorial1( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
  auto energyManager = simulation->getEnergyManager();
 
  // Setup the pendulum
  agx::Real pendulumLength = 1.0;
 
  agx::RigidBodyRef sphere = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Sphere(0.1) ) );
  sphere->setPosition( pendulumLength, 0, 0 );
 
  // Setting damping will give some dissipation for the rigid body.
  sphere->setLinearVelocityDamping( 0.2f );
 
  agx::HingeFrame frame( agx::Vec3(), agx::Vec3::Y_AXIS() );
  agx::HingeRef hinge = new agx::Hinge( frame, sphere );
 
  simulation->add( sphere );
  simulation->add( hinge );
 
  agxOSG::setDiffuseColor( agxOSG::createVisual( sphere, root ), agxRender::Color::ForestGreen() );
 
  // Add the hinge and the sphere to the EnergyManager.
  energyManager->add( hinge );
  energyManager->add( sphere );
 
  // Lets print out the energies so we can see them.
  auto printEnergy = [application, energyManager, sphere, hinge]( agxSDK::Simulation* /*simulation*/ ) {
 
    // Kinetic energy of the sphere.
    auto rbKineticEnergy = agxSDK::EnergyManager::getKineticEnergy( sphere );
    application->getSceneDecorator()->setText( 0, agx::String::format( "Sphere kinetic energy:        %5.2f J",
                                                                       rbKineticEnergy ) );
 
    // Here we instead get the change of kinetic energy during the previous time step. 
    auto rbKineticEnergyChange = energyManager->getKineticEnergyChange( sphere );
    application->getSceneDecorator()->setText( 1, agx::String::format( "Sphere kinetic energy change: %5.2f J/dt",
                                                                       rbKineticEnergyChange ) );
 
    // The change of gravity potential for the sphere.
    auto rbPotentialChange = energyManager->getPotentialChange( sphere );
    application->getSceneDecorator()->setText( 2, agx::String::format( "Sphere potential change:      %5.2f J/dt",
                                                                       rbPotentialChange ) );
 
    // The dissipation of a rigid body will come from velocity damping and external forces.
    auto rbDissipation = energyManager->getDissipation( sphere );
    application->getSceneDecorator()->setText( 3, agx::String::format( "Sphere dissipation:           %5.2f J/dt",
                                                                       rbDissipation ) );
 
    // Some energy will be stored in the hinge. Here we get the change in potential.
    auto hingePotentialChange = energyManager->getPotentialChange( hinge );
    application->getSceneDecorator()->setText( 4, agx::String::format( "Hinge potential change: %8.5f J/dt",
                                                                       hingePotentialChange ) );
 
    // There will also be some dissipation in the hinge.
    auto hingeDissipation = energyManager->getDissipation( hinge );
    application->getSceneDecorator()->setText( 5, agx::String::format( "Hinge dissipation:      %8.5f J/dt",
                                                                       hingeDissipation ) );
  };
 
  agxUtil::StepEventCallback::post( printEnergy, simulation );
 
  // Call this one before simulation starts to get the text in place.
  printEnergy( simulation );
 
  application->setEnableDebugRenderer( true );
 
  return root;
}
 
 
 
osg::Group* buildTutorial2( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
  auto energyManager = simulation->getEnergyManager();
 
  // Setup a prismatic with a motor and a range.
  agx::RigidBodyRef box = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 0.1, 0.1, 0.1 ) ) );
  agx::PrismaticRef prismatic = new agx::Prismatic( agx::Vec3::Z_AXIS(), box );
 
  auto motor = prismatic->getMotor1D();
  motor->setEnable( true );
  motor->setSpeed( 0.1 );
 
  auto range = prismatic->getRange1D();
  range->setEnable( true );
 
  range->setRange( 0, 0.5 );
 
  simulation->add( box );
  simulation->add( prismatic );
 
  agxOSG::setDiffuseColor( agxOSG::createVisual( box, root ), agxRender::Color::DarkCyan() );
 
  // Add the prismatic to the EnergyManager. 
  energyManager->add( prismatic );
 
  // Lets print out the energies so we can see them.
  auto printEnergy = [application, energyManager, motor]( agxSDK::Simulation* /*simulation*/ ) {
 
    // Power of the motor
    auto motorPower = energyManager->getPower( motor );
    application->getSceneDecorator()->setText( 0, agx::String::format( "Motor power: %5.2f J/s", motorPower ) );
 
    // Motor1D does not store any potential energy and therefore this call will always return zero.
    // auto potentialChangeMotor = energyManager->getPotentialChange( motor );
 
    // The dissipation for the motor. The dissipation will be the work done by the motor and will be positive when it's
    // working with the motion and negative when working against the motion.
    auto motorDissipation = energyManager->getDissipation( motor );
    application->getSceneDecorator()->setText( 1, agx::String::format( "Motor work:  %5.2f J/dt", motorDissipation ) );
  };
 
  agxUtil::StepEventCallback::post( printEnergy, simulation );
 
  // Call this one before simulation starts to get the text in place.
  printEnergy( simulation );
 
  application->setEnableDebugRenderer( true );
 
  return root;
}
 
 
 
osg::Group* buildTutorial3( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  // Set up a scene where an electric motors drives a sphere around the z-axis.
  agxPowerLine::PowerLineRef powerLine = new agxPowerLine::PowerLine();
  simulation->add( powerLine );
 
  agx::RigidBodyRef sphere = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Sphere( 0.1 ) ) );
  sphere->setPosition( 0.5, 0, 0 );
  agxOSG::setDiffuseColor( agxOSG::createVisual( sphere, root ), agxRender::Color::Violet() );
 
  agx::HingeFrame hingeFrame( agx::Vec3(), agx::Vec3::Z_AXIS() );
  agx::HingeRef hinge = new agx::Hinge( hingeFrame, sphere );
 
  simulation->add( sphere );
  simulation->add( hinge );
 
  agx::Real resistance = 1.0;
  agx::Real emfConstant = 1.0;
  agx::Real torqueConstant = 1.0;
  agx::Real inductance = 1.0;
  agxDriveTrain::ElectricMotorRef motor = new agxDriveTrain::ElectricMotor( resistance, torqueConstant, emfConstant,
                                                                            inductance );
 
  agxPowerLine::RotationalActuatorRef rotationalActuator = new agxPowerLine::RotationalActuator( hinge );
  motor->connect( rotationalActuator );
 
  powerLine->add( motor );
  motor->setVoltage( 2.0 );
 
  // Add the motor to the EnergyManager. Since we already added it to a simulation there is no need for the second 
  // parameter.
  agxDriveTrain::EnergyManager::add( motor/*, simulation*/ );
 
  // Lets print out the power and other useful values.
  auto printEnergy = [application, motor]( agxSDK::Simulation* /*simulation*/ ) {
 
    // Angular velocity
    auto angularVelocity = motor->getAngularVelocity();
    application->getSceneDecorator()->setText( 0, agx::String::format( "Angular velocity: %8.6f rad/s", 
                                                                       angularVelocity ) );
    // Power of the motor
    auto motorPower = agxDriveTrain::EnergyManager::getPower( motor );
    application->getSceneDecorator()->setText( 1, agx::String::format( "Motor power:      %8.6f J/s", motorPower ) );
 
    // The work done by the motor since the previous time step.
    auto motorWork = agxDriveTrain::EnergyManager::getWorkDone( motor );
    application->getSceneDecorator()->setText( 2, agx::String::format( "Motor work:       %8.6f J/dt", motorWork ) );
  };
 
  agxUtil::StepEventCallback::post( printEnergy, simulation );
 
  // Call this one before simulation starts to get the text in place.
  printEnergy( simulation );
 
  application->setEnableDebugRenderer( true );
 
  return root;
}
 
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tEnergyManager tutorial\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
    application->addScene( buildTutorial3, '3' );
 
    if ( application->init( argc, argv ) )
      return application->run();
  } catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_fracture.cpp

/*
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.
*/
 
/*
 
Demonstrates the agxModel::FractureGenerator class for splitting obects during simulation. 
 
*/
 
#include <agxUnit/UnitMacros.h>
 
#include <agxOSG/utils.h>
#include <agxOSG/ExampleApplication.h>
 
#include <agx/Logger.h>
#include <agx/version.h>
 
#include <agxOSG/GeometryNode.h>
#include <agxOSG/ReaderWriter.h>
 
#include <agxCollide/VoronoiGenerator.h>
#include <agxCollide/Box.h>
#include <agxCollide/Sphere.h>
#include <agxCollide/Cylinder.h>
#include <agxCollide/Convex.h>
#include <agxRender/RenderManager.h>
#include <agxUtil/ConvexReaderWriter/ConvexReaderWriter.h>
#include <agxModel/FractureGenerator.h>
#include <agxWire/Wire.h>
#include <agxOSG/WireRenderer.h>
 
#include <agxRender/RenderSingleton.h>
 
using namespace agxOSG;
using namespace agx;
 
osg::Group* buildTutorial5(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/)
{
  LOGGER_WARNING() << LOGGER_STATE(agx::Notify::PRINT_NONE) << "\nCreating Scene " << __FUNCTION__ << std::endl << LOGGER_END();
  osg::Group* root = new osg::Group;
 
  agx::setNumThreads(8);
 
  agx::MaterialRef matIter = new agx::Material("mat");
  simulation->add(matIter);
  agx::ContactMaterialRef cm = simulation->getMaterialManager()->getOrCreateContactMaterial(matIter, matIter);
  agx::IterativeProjectedConeFrictionRef model = new agx::IterativeProjectedConeFriction();
  model->setSolveType(agx::FrictionModel::ITERATIVE);
  cm->setFrictionModel(model);
  cm->setRestitution(0);
  cm->setFrictionCoefficient(0.9);
 
  // Create floor  box
  Vec3 floorVec = agx::Vec3(30, 15, .5);
  agxCollide::GeometryRef geomPlane = new agxCollide::Geometry(new agxCollide::Box(floorVec));
  geomPlane->setPosition(Vec3(0, 0, -.5));
  simulation->add(geomPlane);
  geomPlane->setMaterial(matIter);
  agxOSG::createVisual(geomPlane, root);
 
  // Add ball with wire
  Real ballRadius = 0.75;
  agxCollide::GeometryRef geom = new agxCollide::Geometry(new agxCollide::Sphere(ballRadius));
  geom->setMaterial(matIter);
  agx::RigidBodyRef body = new agx::RigidBody(geom);
  simulation->add(body);
  body->setPosition(Vec3(8, 0, 10));
  body->getMassProperties()->setMass(5e5); // 1 ton
  //body->setVelocity(Vec3(-20, 0, 5));
  body->setVelocity(Vec3(0, 0, -5));
  Vec4f rockColor = Vec4f(0, 0, 0, 1.0);
  agxOSG::GeometryNode* rockNode = agxOSG::createVisual(geom, root);
  agxOSG::setDiffuseColor(rockNode, rockColor);
 
  //
  // Create wire to attach ball to
  //
  agx::RigidBodyRef staticWireHolder = new agx::RigidBody(new agxCollide::Geometry(new agxCollide::Sphere(0.1)));
  staticWireHolder->setMotionControl(RigidBody::STATIC);
  simulation->add(staticWireHolder);
  staticWireHolder->setPosition(Vec3(1, 0, 10));
 
  agx::Real wireRadius(0.025);                 // - Radius of the wire.
  agx::Real wireResolutionPerUnitLength(0.1); // - Number of mass elements, used by the wire, per unit length.
  agxWire::WireRef wire = new agxWire::Wire(wireRadius, wireResolutionPerUnitLength, false);
  agx::MaterialRef wireMaterial = new agx::Material("WireMaterial");
  wireMaterial->getWireMaterial()->setYoungsModulusStretch(200E14);
  wire->setMaterial(wireMaterial);
 
  wire->add(new agxWire::BodyFixedNode(staticWireHolder, 0, 0, 0));
  wire->add(new agxWire::BodyFixedNode(body, 0, 0, ballRadius));
  simulation->add(wire);
  agxOSG::WireRendererRef wireRenderer = new agxOSG::WireRenderer(wire, root);
  simulation->add(wireRenderer);
  wireRenderer->setColor(agx::Vec4f(.75f, .75f, 0, 1));
 
  //
  // Create fracture listener
  //
  agxModel::FractureGeneratorRef fracture = new agxModel::FractureGenerator();
  fracture->setFractureMinimumSize(.1);
  fracture->setNumFractureFragmentsInterval(50, 100);
  simulation->add(fracture);
 
  agx::Vec4f wallColor = agxRender::Color::Burlywood();
 
  // Create a breakable walls
  Vec3 wallWec = agx::Vec3(0.1, 6, 4);
  size_t numWalls = 3;
  for (size_t i = 0; i < numWalls; ++i)
  {
    agxCollide::GeometryRef wallGeom = new agxCollide::Geometry(new agxCollide::Box(wallWec));
    wallGeom->setPosition(Vec3(-4 + Real(i), 0, wallWec.z()));
    simulation->add(wallGeom);
    wallGeom->setMaterial(matIter);
    agxOSG::GeometryNode* wallnode1 = agxOSG::createVisual(wallGeom, root);
    agxOSG::setDiffuseColor(wallnode1, wallColor);
    fracture->addBreakable(wallGeom);
  }
 
  fracture->addBreaker(geom);
 
  agxModel::FractureGenerator::NewBodyCallback callback = [wallColor, root](agxModel::FractureGenerator*, agxCollide::Geometry*, agx::RigidBody* rb)
  {
    agxOSG::setDiffuseColor(agxOSG::createVisual(rb, root), wallColor);
  };
 
  fracture->setNewBodyCallback(callback);
 
  return root;
}
 
osg::Group*  buildTutorial4(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/)
{
  LOGGER_WARNING() << LOGGER_STATE(agx::Notify::PRINT_NONE) << "\nCreating Scene " << __FUNCTION__ << std::endl << LOGGER_END();
  osg::Group* root = new osg::Group;
 
  agx::setNumThreads(8);
 
  agx::MaterialRef  matIter = new agx::Material("mat");
  simulation->add(matIter);
  agx::ContactMaterialRef cm = simulation->getMaterialManager()->getOrCreateContactMaterial(matIter, matIter);
  agx::IterativeProjectedConeFrictionRef model = new agx::IterativeProjectedConeFriction();
  model->setSolveType(agx::FrictionModel::ITERATIVE);
  cm->setFrictionModel(model);
 
  // Create floor  box
  Vec3 floorVec = agx::Vec3(40, 40, .5);
  agxCollide::GeometryRef geomPlane = new agxCollide::Geometry(new agxCollide::Box(floorVec));
  geomPlane->setPosition(Vec3(0, 0, -.5));
  simulation->add(geomPlane);
  geomPlane->setMaterial(matIter);
  agxOSG::createVisual(geomPlane, root);
 
  // Create fracture listener
  agxModel::FractureGeneratorRef fracture = new agxModel::FractureGenerator();
  fracture->setFractureMinimumSize(.1);
  fracture->setNumFractureFragmentsInterval(10, 20);
  simulation->add(fracture);
  fracture->addBreaker(geomPlane);
  fracture->setAddFractureFragmentsToGenerator(false);
 
  Vec4f rockColor = Vec4f(0.1f, 0.1f, 0.1f, 1);
 
  // Load all filenames in the convex stones folder
  std::vector<std::string> rocks = {
    "data\\models\\convex_stones\\convex_rock2.obj",
    "data\\models\\convex_stones\\convex_rock3.obj",
    "data\\models\\convex_stones\\convex_rock4.obj",
    "data\\models\\convex_stones\\convex_rock5.obj",
    "data\\models\\convex_stones\\convex_rock6.obj"
  };
 
  agx::Real height = 15;
 
  for (size_t i = 0; i < 10; ++i)
  {
    std::string sRock = rocks[agx::random<int>(0, static_cast<int>(rocks.size() - 1))];
    Real scale = 0.001;
    agx::Matrix3x3 mscale = agx::Matrix3x3(Vec3(scale, scale, scale));
    agxCollide::ConvexRef rockConvex = agxUtil::ConvexReaderWriter::createFromConvex(sRock, mscale);
    agxCollide::GeometryRef rockRef = new agxCollide::Geometry(rockConvex);
    rockRef->setMaterial(matIter);
    agx::RigidBodyRef rbBody = new agx::RigidBody(rockRef);
    simulation->add(rbBody);
    Vec3 spawnVec = floorVec; spawnVec.z() = 0;
    rbBody->setPosition(Vec3::random(-spawnVec / 2, spawnVec / 2) + Vec3(0, 0, height));
    rbBody->setRotation(agx::EulerAngles::random(Vec3(0, 0, 0), Vec3(2.0 * M_PI, 2.0 * M_PI, 2.0 * M_PI)));
    fracture->addBreakable(rockRef);
    agxOSG::setDiffuseColor(agxOSG::createVisual(rbBody, root), rockColor);
  }
 
  agxModel::FractureGenerator::NewBodyCallback callback = [rockColor, root](agxModel::FractureGenerator*, agxCollide::Geometry*, agx::RigidBody* rb)
  {
    agxOSG::setDiffuseColor(agxOSG::createVisual(rb, root), rockColor);
  };
 
  fracture->setNewBodyCallback(callback);
 
  return root;
}
 
osg::Group*  buildTutorial3(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/)
{
  LOGGER_WARNING() << LOGGER_STATE(agx::Notify::PRINT_NONE) << "\nCreating Scene " << __FUNCTION__ << std::endl << LOGGER_END();
  osg::Group* root = new osg::Group;
 
  agx::setNumThreads(8);
 
  agx::MaterialRef  matIter = new agx::Material("mat");
  simulation->add(matIter);
  agx::ContactMaterialRef cm = simulation->getMaterialManager()->getOrCreateContactMaterial(matIter, matIter);
  agx::IterativeProjectedConeFrictionRef model = new agx::IterativeProjectedConeFriction();
  model->setSolveType(agx::FrictionModel::ITERATIVE);
  cm->setFrictionModel(model);
  cm->setRestitution(0);
  cm->setFrictionCoefficient(0.9);
 
  // Create floor  box
  Vec3 floorVec = agx::Vec3(20, 20, .5);
  agxCollide::GeometryRef geomPlane = new agxCollide::Geometry(new agxCollide::Box(floorVec));
  geomPlane->setPosition(Vec3(0, 0, -.5));
  simulation->add(geomPlane);
  geomPlane->setMaterial(matIter);
  agxOSG::createVisual(geomPlane, root);
 
  // Create fracture listener
  agxModel::FractureGeneratorRef fracture = new agxModel::FractureGenerator();
  fracture->setFractureMinimumSize(.5);
  fracture->setNumFractureFragmentsInterval(2, 5);
  simulation->add(fracture);
  fracture->addBreaker(geomPlane);
  fracture->setAddFractureFragmentsToGenerator(true);
 
  Vec4f rockColor = agxRender::Color::SlateGray();
  agx::Real height = 10;
  Real scale = 0.002;
  agx::Matrix3x3 mscale = agx::Matrix3x3(Vec3(scale, scale, scale));
  agx::String rock = "data\\models\\convex_stones\\convex_rock2.obj";
  agxCollide::ConvexRef rockConvex = agxUtil::ConvexReaderWriter::createFromConvex(rock, mscale);
  agxCollide::GeometryRef rockRef = new agxCollide::Geometry(rockConvex);
  rockRef->setMaterial(matIter);
  agx::RigidBodyRef rbBody = new agx::RigidBody(rockRef);
  simulation->add(rbBody);
  rbBody->setPosition(Vec3(0, 0, height));
  fracture->addBreakable(rockRef);
  rbBody->setRotation(agx::EulerAngles(0, M_PI/2, 0));
  agxOSG::setDiffuseColor(agxOSG::createVisual(rockRef, root), rockColor);
 
  agxModel::FractureGenerator::NewBodyCallback callback = [rockColor, root](agxModel::FractureGenerator*, agxCollide::Geometry*, agx::RigidBody* rb)
  {
    agxOSG::setDiffuseColor(agxOSG::createVisual(rb, root), rockColor);
  };
 
  fracture->setNewBodyCallback(callback);
 
  return root;
}
 
osg::Group*  buildTutorial2(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/)
{
  LOGGER_WARNING() << LOGGER_STATE(agx::Notify::PRINT_NONE) << "\nCreating Scene " << __FUNCTION__ << std::endl << LOGGER_END();
  osg::Group* root = new osg::Group;
 
  agx::setNumThreads(8);
 
  agx::MaterialRef  matIter = new agx::Material("mat");
  simulation->add(matIter);
  agx::ContactMaterialRef cm = simulation->getMaterialManager()->getOrCreateContactMaterial(matIter, matIter);
  agx::IterativeProjectedConeFrictionRef model = new agx::IterativeProjectedConeFriction();
  model->setSolveType(agx::FrictionModel::ITERATIVE);
  cm->setFrictionModel(model);
  cm->setRestitution(0);
  cm->setFrictionCoefficient(0.9);
 
  // Create floor  box
  Vec3 floorVec = agx::Vec3(10, 10, .5);
  agxCollide::GeometryRef geomPlane = new agxCollide::Geometry(new agxCollide::Box(floorVec));
  geomPlane->setPosition(Vec3(0, 0, -.5));
  simulation->add(geomPlane);
  geomPlane->setMaterial(matIter);
  agxOSG::createVisual(geomPlane, root);
 
  // Create fracture listener
  agxModel::FractureGeneratorRef fracture = new agxModel::FractureGenerator();
  fracture->setFractureMinimumSize(.1);
  fracture->setNumFractureFragmentsInterval(10, 20);
  simulation->add(fracture);
  fracture->addBreaker(geomPlane);
  fracture->setAddFractureFragmentsToGenerator(true);
 
  Vec4f coneColor = agxRender::Color::Brown();
  agx::Real height = 10;
  Real scale = 3;
  agx::Matrix3x3 mscale = agx::Matrix3x3(Vec3(scale, scale, scale));
  agxCollide::ConvexRef rockConvex = agxUtil::ConvexReaderWriter::createFromConvex("data/models/cone.obj", mscale);
  agxCollide::GeometryRef rockRef = new agxCollide::Geometry(rockConvex);
  rockRef->setMaterial(matIter);
  agx::RigidBodyRef rbBody = new agx::RigidBody(rockRef);
  simulation->add(rbBody);
  rbBody->setPosition(Vec3(0, 0, height));
  rbBody->setRotation(agx::EulerAngles(0, M_PI, 0));
  fracture->addBreakable(rockRef);
  agxOSG::setDiffuseColor(agxOSG::createVisual(rbBody, root), coneColor);
 
  agxModel::FractureGenerator::NewBodyCallback callback = [coneColor, root](agxModel::FractureGenerator*, agxCollide::Geometry*, agx::RigidBody* rb)
  {
    agxOSG::setDiffuseColor(agxOSG::createVisual(rb, root), coneColor);
  };
 
  fracture->setNewBodyCallback(callback);
 
  return root;
}
 
osg::Group*  buildTutorial1(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/)
{
  LOGGER_WARNING() << LOGGER_STATE(agx::Notify::PRINT_NONE) << "\nCreating Scene " << __FUNCTION__ << std::endl << LOGGER_END();
  osg::Group* root = new osg::Group;
 
  agx::MaterialRef  matIter = new agx::Material("mat");
  simulation->add(matIter);
  agx::ContactMaterialRef cm = simulation->getMaterialManager()->getOrCreateContactMaterial(matIter, matIter);
  agx::IterativeProjectedConeFrictionRef model = new agx::IterativeProjectedConeFriction();
  model->setSolveType(agx::FrictionModel::ITERATIVE);
  cm->setFrictionModel(model);
 
  // Create box
  agxCollide::GeometryRef geomPlane = new agxCollide::Geometry(new agxCollide::Box(agx::Vec3(15, 15, .5)));
  geomPlane->setPosition(Vec3(0, 0, -.5));
  simulation->add(geomPlane);
  geomPlane->setMaterial(matIter);
  agxOSG::createVisual(geomPlane, root);
 
  agxCollide::VoronoiGeneratorRef generator = new agxCollide::VoronoiGenerator();
  generator->setResolution(0.75);
  auto fractureShape = [generator, simulation, matIter, root](
    const agxCollide::Shape* shapeToFracture,
    const Vec3& posForFragments,
    const Quat& rotForFragments)
  {
    agx::HighAccuracyTimer timer;
    timer.start();
    agxCollide::VoronoiGenerator::VoronoiDiagramData data(generator->generateVoronoiDiagramInShape(shapeToFracture, 20));
    timer.stop();
    std::cout << "3D Voronoi diagram of shape type '"<< shapeToFracture->getTypeName() << " took " << timer.getTime()  << " ms to compute."<< std::endl;
    std::cout << "# Voronoi cells: " << data.size() << std::endl;
 
    Real volume = 0;
    // Create convexes from the generated Voronoi diagram
    for (auto& cell : data)
    {
      agxCollide::ConvexRef convex = agxUtil::ConvexReaderWriter::createConvex(cell.voronoiCellVertices);
      volume += convex->getVolume();
      agxCollide::GeometryRef geom = new agxCollide::Geometry(convex);
      geom->setMaterial(matIter);
      agxOSG::setDiffuseColor(agxOSG::createVisual(geom, root), agx::Vec4f::random(Vec4f(0, 0, 0, 1), Vec4f(1, 1, 1, 1)));
      agx::RigidBodyRef rb = new agx::RigidBody(geom);
      simulation->add(rb);
      rb->setPosition(posForFragments);
      rb->setRotation(rotForFragments);
    }
    Real oldVolume = shapeToFracture->getVolume();
    Real relDiff = ((oldVolume - volume) / oldVolume) * Real(100.0);
    std::cout << "Relative volume difference: " << relDiff << " %" << std::endl;
 
  };
 
  // Create Voronoi diagrams in various shapes
  agxCollide::BoxRef box = new agxCollide::Box(1, 1, 1);
  fractureShape(box, Vec3(-8, 0, 5), agx::Quat());
 
  agxCollide::CylinderRef cylinder = new agxCollide::Cylinder(1.0, 4.0);
  fractureShape(cylinder, Vec3(-4, 0, 5), agx::Quat(Vec3(0, 1, 0), Vec3(0, 1, 1).normal()));
 
  agxCollide::CapsuleRef capsule = new agxCollide::Capsule(1.0, 2.0);
  fractureShape(capsule, Vec3(0, 0, 5), agx::Quat(Vec3(0, 1, 0), Vec3(0, 0, 1)));
 
  agxCollide::SphereRef sphere = new agxCollide::Sphere(1.0);
  fractureShape(sphere, Vec3(4, 0, 5), agx::Quat());
 
  Real scale = 3;
  agx::Matrix3x3 mscale = agx::Matrix3x3(Vec3(scale, scale, scale));
  agxCollide::ConvexRef convex = agxUtil::ConvexReaderWriter::createFromConvex("data/models/cone.obj", mscale);
  fractureShape(convex, Vec3(8, 0, 5), agx::Quat(Vec3(0, 0, 1), Vec3(0, 0, -1)));
 
  return root;
}
 
int main(int argc, char** argv)
{
  agx::AutoInit agxInit;
  int status = 0;
 
  LOGGER_WARNING() << LOGGER_STATE(agx::Notify::PRINT_NONE) << "\n" <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tRigidBody demonstrator\n" <<
    "\t--------------------------------\n\n" << LOGGER_END();
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene(buildTutorial1, '1');
    application->addScene(buildTutorial2, '2');
    application->addScene(buildTutorial3, '3');
    application->addScene(buildTutorial4, '4');
    application->addScene(buildTutorial5, '5');
 
    if (application->init(argc, argv)) {
      status = application->run();
    }
  }
  catch (std::exception& e) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return status;
}

tutorial_frictionModels.cpp

/*
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.
*/
 
/*
 
Demonstrates how to use different friction models in a simulation.
 
*/
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/utils.h>
 
#include <agx/version.h>
#include <agx/RigidBody.h>
#include <agxRender/RenderSingleton.h>
#include <agx/Logger.h>
 
#include <agxUtil/agxUtil.h>
 
#include <agxCollide/Box.h>
#include <agxStream/InputArchive.h>
#include <agxStream/OutputArchive.h>
 
using namespace agx;
 
osg::Group *buildTutorial1( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group *root = new osg::Group;
 
  LOGGER_WARNING() << LOGGER_STATE( agx::Notify::PRINT_NONE ) << __FUNCTION__ << std::endl << LOGGER_END();
 
  // Create ground box.
  RigidBodyRef ground = new RigidBody();
  ground->setMotionControl( RigidBody::STATIC );
  ground->add( new agxCollide::Geometry( new agxCollide::Box( Vec3( 10, 10, 0.3 ) ) ) );
  ground->setPosition( Vec3( 0, 0, -0.3 ) );
  agxOSG::createVisual( ground, root );
 
  // Create a dynamic box.
  RigidBodyRef box = new RigidBody();
  box->add( new agxCollide::Geometry( new agxCollide::Box( Vec3( 1, 1, 1 ) ) ) );
  box->setPosition( Vec3( 0, 0, 5 ) );
  agxOSG::createVisual( box, root );
 
  // Create and assign ground material.
  MaterialRef groundMaterial = new Material( "groundMaterial" );
  ground->getGeometries().front()->setMaterial( groundMaterial );
 
  // Create and assign box material.
  MaterialRef boxMaterial = new Material( "boxMaterial" );
  box->getGeometries().front()->setMaterial( boxMaterial );
 
  // Add the bodies to the simulation.
  simulation->add( ground );
  simulation->add( box );
 
  // Create explicit contact material. It's only possible to assign
  // new friction models through explicit contact materials.
  ContactMaterial* groundBoxContactMaterial = simulation->getMaterialManager()->getOrCreateContactMaterial( groundMaterial, boxMaterial );
 
  /* 
  * Box Friction Model 
  * This friction model has static bounds through the solve stage.
  * E.g., stack of boxes, with a naive implementation of this friction model the bottom
  * box wont feel the weight of the other boxes. The normal force will be constant,
  * specified by the user.
  
  * This implementation of the box friction model estimates the normal force in two
  * different ways:
  *   1. If the contact is new (i.e., never been solved before), the normal
  *      force is estimated by the impact (relative) speed.
  *  2. If the contact is old (i.e., been solved before), the last normal force
  *     is used to set the bounds. Which means that this friction model, in theory,
  *     could get the correct normal forces during stacking.
  * See tutorials below for custom implementations of this.
  */
  BoxFrictionModelRef boxFriction = new BoxFrictionModel();
 
 /*
  * Scale Box Friction Model 
  * Callbacks from the NLMCP(Non - Linear Mixed Complementarity Problem)
  * solver with the current normal force between objects.
  *
  * The friction box is by default aligned with the world axes.
  * It's possible to implement 'calculateTangentPlane' to align the box is some other direction.
  * This friction model supports primary and secondary direction friction coefficients.
  * Scale box friction is computationally more expensive than for example box friction,
  * but with more realistic dry friction, so think carefully where to use this friction model.
  */
  ScaleBoxFrictionModelRef scaleBoxFriction = new ScaleBoxFrictionModel();
 
  /*
  * Iterative Projected Cone Friction(IPC friction) model.
  * This is the default friction model, with solve type = SPLIT.
  * Given the normal force, this friction model projects
  * the friction forces onto the friction cone, which means that the
  * alignment of the friction box can be neglected.
  *
  * If the solve type includes direct solves, this friction model
  * uses scale box friction during the direct solve, and if
  * possible, iterative projection after the last direct solve.
  */
  IterativeProjectedConeFrictionRef ipcFriction = new IterativeProjectedConeFriction();
 
  enum FrictionModelType { BOX, SCALE_BOX, ITERATIVE_PROJECTED };
  FrictionModelType modelType = ITERATIVE_PROJECTED;
 
  if ( modelType == BOX )
    groundBoxContactMaterial->setFrictionModel( boxFriction );
  else if ( modelType == SCALE_BOX )
    groundBoxContactMaterial->setFrictionModel( scaleBoxFriction );
  else if ( modelType == ITERATIVE_PROJECTED )
    groundBoxContactMaterial->setFrictionModel( ipcFriction );
 
  // Default solve type is SPLIT. 
  //
  // It's possible to change the solve type. To use the DIRECT:
  ipcFriction->setSolveType( FrictionModel::DIRECT );
 
  // Solve type can be: DIRECT, ITERATIVE, SPLIT and DIRECT_AND_ITERATIVE. The normal
  // forces will be solved according to the friction model solve type.
  FrictionModel::SolveType solveType = FrictionModel::DIRECT;
 
  groundBoxContactMaterial->getFrictionModel()->setSolveType( solveType );
 
  return root;
}
 
class GivenNormalForceBoxFrictionModel : public BoxFrictionModel
{
  public:
    GivenNormalForceBoxFrictionModel()
      : BoxFrictionModel(), m_normalForce( 0 ) {}
 
    void setNormalForce( Real normalForce )
    {
      m_normalForce = normalForce;
    }
 
    Real getNormalForce() const { return m_normalForce; }
 
    virtual void calculateTangentPlane( const agxCollide::Geometry* geometry1, const agxCollide::Geometry* geometry2, const Vec3& point, const Vec3& normal, const Real depth, Vec3& ret_u, Vec3& ret_v ) const override
    {
      FrictionModel::calculateTangentPlane( geometry1, geometry2, point, normal, depth, ret_u, ret_v );
      renderDirection( point, ret_u, ret_v );
    }
 
    virtual Real calculateNormalForce( const agxCollide::Geometry* /*geometry1*/, const agxCollide::Geometry* /*geometry2*/, const Vec3& /*point*/, const Vec3& /*normal*/, const Real /*depth*/, const Real /*dt*/, const size_t numPoints ) const override
    {
      // Two geometries, one point in world, one normal and a depth is the definition of one contact point.
      // I.e., this method will be called once per contact point in the geometry contact.
 
      // This is the normal force the friction constraint will use during this time step.
      // Note: This is only valid for box friction models.
      return getNormalForce() / Real( numPoints );
    }
    AGXSTREAM_DECLARE_SERIALIZABLE( GivenNormalForceBoxFrictionModel );
 
  protected:
    virtual ~GivenNormalForceBoxFrictionModel() {}
 
    void renderDirection( const Vec3& point, const Vec3& u, const Vec3& v ) const
    {
      agxRender::RenderSingleton::instance()->add( point, point + u, Real( 0.05 ), Vec4f( 0, 1, 0, 1 ) );
      agxRender::RenderSingleton::instance()->add( point, point + v, Real( 0.05 ), Vec4f( 1, 0, 0, 1 ) );
    }
 
  protected:
    Real m_normalForce;
};
 
class OrientedBoxGivenNormalForceBoxFrictionModel : public GivenNormalForceBoxFrictionModel
{
  public:
    OrientedBoxGivenNormalForceBoxFrictionModel( RigidBody* rb, const Vec3& primaryDirection )
      : m_rb( rb ), m_primaryDirection( primaryDirection ) {}
 
  protected:
    OrientedBoxGivenNormalForceBoxFrictionModel() : m_rb( 0L ), m_primaryDirection() {}
    virtual ~OrientedBoxGivenNormalForceBoxFrictionModel() {}
 
    virtual void calculateTangentPlane( const agxCollide::Geometry* geometry1, const agxCollide::Geometry* geometry2, const Vec3& point, const Vec3& normal, const Real depth, Vec3& ret_u, Vec3& ret_v ) const override
    {
      agxAssert( m_rb.isValid() );
 
      // Reference primary direction in world coordinates.
      ret_u = m_rb->getFrame()->transformVectorToWorld( m_primaryDirection );
 
      // Project u onto the plane defined by the normal.
      ret_u -= normal * ( normal * ret_u );
 
      // u and n could be parallel from the beginning, if so ret_u is zero, use default tangent plane.
      Real uLength = ret_u.normalize();
      if ( agx::equalsZero( uLength ) ) {
        FrictionModel::calculateTangentPlane( geometry1, geometry2, point, normal, depth, ret_u, ret_v );
        return;
      }
 
      // Calculate secondary direction orthogonal to primary direction and normal. (primary is already orthogonal to the normal)
      ret_v = normal ^ ret_u;
 
      // Isn't necessary.
      ret_v.normalize();
 
      renderDirection( point, ret_u, ret_v );
    }
 
    AGXSTREAM_DECLARE_SERIALIZABLE( OrientedBoxGivenNormalForceBoxFrictionModel );
 
  protected:
    observer_ptr< RigidBody > m_rb;
    Vec3 m_primaryDirection;
};
 
using namespace agxStream;
 
AGXSTREAM_INSTANTIATE_STORAGE( GivenNormalForceBoxFrictionModel )
 
void GivenNormalForceBoxFrictionModel::store( OutputArchive& out ) const 
{
  BoxFrictionModel::store( out );
  out << agxStream::out( "normalForce", m_normalForce );
}
 
void GivenNormalForceBoxFrictionModel::restore( InputArchive& in ) 
{
  BoxFrictionModel::restore( in );
  in >> agxStream::in( "normalForce", m_normalForce );
}
 
AGXSTREAM_INSTANTIATE_STORAGE( OrientedBoxGivenNormalForceBoxFrictionModel )
 
void OrientedBoxGivenNormalForceBoxFrictionModel::store( OutputArchive& out ) const 
{
  GivenNormalForceBoxFrictionModel::store( out );
  out << agxStream::out( "body", m_rb.get() );
  out << agxStream::out( "primaryDirection", m_primaryDirection );
}
 
void OrientedBoxGivenNormalForceBoxFrictionModel::restore( InputArchive& in ) 
{
  GivenNormalForceBoxFrictionModel::restore( in );
  RigidBody* rb = 0L;
  in >> agxStream::in( "body", rb );
  m_rb = rb;
  in >> agxStream::in( "primaryDirection", m_primaryDirection );
}
 
osg::Group *buildTutorial2( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* application )
{
  osg::Group *root = new osg::Group;
 
  LOGGER_WARNING() << LOGGER_STATE( agx::Notify::PRINT_NONE ) << __FUNCTION__ << std::endl << LOGGER_END();
 
  // Create ground box.
  RigidBodyRef ground = new RigidBody();
  ground->setMotionControl( RigidBody::STATIC );
  ground->add( new agxCollide::Geometry( new agxCollide::Box( Vec3( 10, 10, 0.3 ) ) ) );
  ground->setPosition( Vec3( 0, 0, -0.3 ) );
  agxOSG::createVisual( ground, root );
 
  // Create two dynamic boxes.
  RigidBodyRef box1 = new RigidBody();
  box1->add( new agxCollide::Geometry( new agxCollide::Box( Vec3( 1, 1, 1 ) ) ) );
  box1->setPosition( Vec3( -5, 0, 5 ) );
  agxOSG::createVisual( box1, root );
 
  RigidBodyRef box2 = new RigidBody();
  box2->add( new agxCollide::Geometry( new agxCollide::Box( Vec3( 1, 1, 1 ) ) ) );
  box2->setPosition( Vec3( 5, 0, 5 ) );
  agxOSG::createVisual( box2, root );
 
  // Create and assign ground material.
  MaterialRef groundMaterial = new Material( "groundMaterial" );
  ground->getGeometries().front()->setMaterial( groundMaterial );
 
  // Create and assign box materials.
  MaterialRef box1Material = new Material( "box1Material" );
  box1->getGeometries().front()->setMaterial( box1Material );
 
  MaterialRef box2Material = new Material( "box2Material" );
  box2->getGeometries().front()->setMaterial( box2Material );
 
  // Add the bodies to the simulation.
  simulation->add( ground );
  simulation->add( box1 );
  simulation->add( box2 );
 
  // Create explicit contact material. It's only possible to assign
  // new friction models through explicit contact materials.
  ContactMaterial* groundBox1ContactMaterial = simulation->getMaterialManager()->getOrCreateContactMaterial( groundMaterial, box1Material );
  ContactMaterial* groundBox2ContactMaterial = simulation->getMaterialManager()->getOrCreateContactMaterial( groundMaterial, box2Material );
  simulation->add( groundBox1ContactMaterial );
  simulation->add( groundBox2ContactMaterial );
 
  // Assign friction model to box1, without tangent plane updater. Box2 gets that.
  GivenNormalForceBoxFrictionModel* box1FrictionModel = new GivenNormalForceBoxFrictionModel();
  // Set the normal force = gravity * mass.
  box1FrictionModel->setNormalForce( Real( 9.8 ) * box1->getMassProperties()->getMass() );
  groundBox1ContactMaterial->setFrictionModel( box1FrictionModel );
 
  // With box2 we update the tangent plane given a primary direction in body coordinates.
  // Let body x direction be the primary.
  OrientedBoxGivenNormalForceBoxFrictionModel* box2FrictionModel = new OrientedBoxGivenNormalForceBoxFrictionModel( box2, Vec3( 1, 0, 0 ) );
  box2FrictionModel->setNormalForce( Real( 9.8 ) * box2->getMassProperties()->getMass() );
  groundBox2ContactMaterial->setFrictionModel( box2FrictionModel );
 
  application->setEnableDebugRenderer( true );
 
  return root;
}
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tFriction model tutorial\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;               
    return 1;
  }
 
  return 0;
}

tutorial_gamePad.cpp

/*
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.
*/
 
/*
Tutorial demonstrating how to use the Joystick/Gamepad API to access gamepad data.
 
*/
 
#include <agx/config/AGX_USE_OIS.h>
#if AGX_USE_OIS()
 
#include <agxOSG/ExampleApplication.h>
#include <agx/version.h>
 
#include <agx/RigidBody.h>
#include <agx/Prismatic.h>
 
#include <agxCollide/Geometry.h>
#include <agxCollide/Box.h>
#include <agxCollide/Cylinder.h>
#include <agxOSG/utils.h>
#include <agxOSG/Text.h>
#include <agxRender/Color.h>
 
#include <agx/Logger.h>
 
using namespace agx;
using namespace agxOSG;
 
 
#include <agxIO/Joystick.h>
#include <agxIO/DeviceManager.h>
 
 
struct XYAxis {
  XYAxis(agx::PrismaticRef x, agx::PrismaticRef y, osg::Group* root)
  {
    m_axes[0] = x;
    m_axes[1] = y;
 
    for (int i = 0; i < 2; i++)
    {
      m_text[i] = new agxOSG::Text();
      m_text[i]->setCharacterSize(0.2f);
      m_text[i]->setAlignment(agxOSG::Text::CENTER_TOP);
      m_text[i]->setAxisAlignment(agxOSG::Text::XZ_PLANE);
      m_text[i]->setPosition(agx::Vec3(0, 0, 0.4 - 0.2 * i));
      m_text[i]->setText(agx::String::format("Value: %.2f", 0.f));
 
      m_text[i]->setColor(agxRender::Color::Yellow());
      root->addChild(m_text[i]);
    }
  }
 
  void setValue(int index, double value)
  {
    if (index < 2 && index >= 0)
    {
      m_axes[index]->getLock1D()->setPosition(value);
      m_text[index]->setText(agx::String::format("Value: %.2f", value));
    }
  }
 
  ~XYAxis()
  {
    m_axes[0] = nullptr;
    m_axes[1] = nullptr;
  }
 
  osg::ref_ptr<agxOSG::Text> m_text[2];
  agx::PrismaticRef m_axes[2];
};
 
typedef agx::Vector<XYAxis> AxisVector;
 
 
struct Button
{
  Button(agxOSG::GeometryNode* node, int i) : m_node(node)
  {
    setPressed(false);
 
    m_text = new agxOSG::Text();
    m_text->setCharacterSize(0.2f);
    m_text->setAlignment(agxOSG::Text::CENTER_TOP);
    m_text->setAxisAlignment(agxOSG::Text::XZ_PLANE);
    m_text->setPosition(agx::Vec3(0, -0.11, 0.08));
    m_text->setText(agx::String::format("%d", i));
 
    m_text->setColor(agxRender::Color::Yellow());
    m_node->addChild(m_text);
  }
 
  void setPressed(bool flag)
  {
    if (flag)
      agxOSG::setDiffuseColor(m_node, agxRender::Color::Green());
    else
      agxOSG::setDiffuseColor(m_node, agxRender::Color::Red());
  }
 
  osg::ref_ptr<agxOSG::GeometryNode> m_node;
  osg::ref_ptr<agxOSG::Text> m_text;
};
 
typedef agx::Vector<Button> ButtonVector;
 
/*
This listener will override the virtual methods from the baseclass to recieve calls when the gamepad buttons,
axis and pov are changed.
*/
class SimulationJoystickListener : public agxIO::JoystickListener {
 
public:
 
  SimulationJoystickListener(AxisVector& axes, XYAxis& pov, ButtonVector& buttons) :
    JoystickListener(),
    m_axes(axes),
    m_pov(pov),
    m_buttons(buttons)
  {
    using agxIO::JoystickState;
 
    auto diag = std::sin(agx::PI / 4);
    std::map<int, agx::Vec2> direction = {
         {JoystickState::CENTERED, agx::Vec2(0, 0)},
         {JoystickState::NORTH, agx::Vec2(0, 1)},
         {JoystickState::SOUTH, agx::Vec2(0, -1)},
         {JoystickState::EAST, agx::Vec2(-1, 0)},
         {JoystickState::WEST, agx::Vec2(1, 0)},
         {JoystickState::NORTHEAST, agx::Vec2(-diag, diag)},
         {JoystickState::SOUTHEAST, agx::Vec2(-diag, -diag)},
         {JoystickState::NORTHWEST, agx::Vec2(diag, diag)},
         {JoystickState::SOUTHWEST, agx::Vec2(diag, -diag)},
    };
 
    m_direction = std::move(direction);
  }
 
  virtual ~SimulationJoystickListener() {}
 
  void  addNotification()
  {
    // Important: Call the base class addNotification method when you are done!
    JoystickListener::addNotification();
  }
 
  void  removeNotification()
  {
    // Important: Call the base class removeNotification method when you are done!
    JoystickListener::removeNotification();
  }
 
  // Called when a button value is changed
  bool buttonChanged(const agxIO::JoystickState&, int button, bool down) override {
    m_buttons[button].setPressed(down);
    return false;
  }
 
  // Called when an axis is changed
  bool axisUpdate(const agxIO::JoystickState& arg, int axis) override {
 
    auto val = getManager()->normalize(arg.axes[axis]);
    m_axes[axis / 2].setValue(axis % 2, val);
 
    return false;
  }
 
  // Called when the POV button is changed
  bool povMoved(const agxIO::JoystickState& arg, int pov) override {
 
    auto val = arg.pov[pov];
 
    auto dir = m_direction[val];
 
    m_pov.setValue(0, -dir[1]);
    m_pov.setValue(1, -dir[0]);
 
    return false;
  }
 
  // If the gamepad/joystick has a slider that has changed value, this method will be called.
  bool sliderMoved(const agxIO::JoystickState&, int index) override {
    std::cout << "sliderMoved: index = " << index << std::endl;
    return false;
  }
 
protected:
  AxisVector m_axes;
  XYAxis m_pov;
  std::map<int, agx::Vec2> m_direction;
  ButtonVector m_buttons;
};
 
// Create a button, really just a geometry with a default color
Button createButton(int i, agx::Vec3 position, agxSDK::Simulation* simulation, osg::Group* root)
{
  auto cylinder = new agxCollide::Geometry(new agxCollide::Cylinder(0.2, 0.1));
  cylinder->setPosition(position);
  cylinder->setEnableCollisions(false);
  auto node = agxOSG::createVisual(cylinder, root);
  simulation->add(cylinder);
 
  return Button(node, i);
}
 
// Create a two bodies, each constrained with a prismatic joint.
// One for the X-axis and one for Z.
// When an axis is changed, this will be reflected in the position of this joint.
// This will move the bodies in X and Z.
XYAxis createAxis(agxRender::Color color, agx::Vec3 position, agxSDK::Simulation* simulation, osg::Group* root)
{
 
  // Create a background cylinder with a texture
  auto cylinder = new agxCollide::Geometry(new agxCollide::Cylinder(1, 0.1));
  cylinder->setPosition(position);
  cylinder->setEnableCollisions(false);
 
  simulation->add(cylinder);
  auto cylNode = agxOSG::createVisual(cylinder, root);
  agxOSG::setDiffuseColor(cylNode, agxRender::Color::White());
  agxOSG::setTexture(cylNode, "textures/grid.png");
 
  // Create a box body that will be moved using the two orthogonal Prismatic joints
  double size = 0.1;
  auto body = new agx::RigidBody();
  body->add(new agxCollide::Geometry(new agxCollide::Box(size, size, size)));
  body->getGeometries()[0]->setEnableCollisions(false);
  body->setPosition(position);
 
  auto node = agxOSG::createVisual(body, root);
  agxOSG::setDiffuseColor(node, color);
  simulation->add(body);
 
  // We need a intermediate body.
  auto centerBody = new agx::RigidBody();
  simulation->add(centerBody);
  centerBody->setPosition(position);
 
  agx::FrameRef frame1 = new agx::Frame();
  frame1->setLocalRotate(agx::EulerAngles(agx::PI_2, 0, 0));
 
  // Connect the two bodies with a prismatic joint moving along X
  agx::PrismaticRef x = new agx::Prismatic(centerBody, new agx::Frame(), body, frame1);
  simulation->add(x);
  x->getLock1D()->setEnable(true);
 
  agx::FrameRef frame2 = new agx::Frame();
  frame2->setLocalRotate(agx::EulerAngles(0, agx::PI_2, 0));
 
  // Create a second prismatic between the world and the center body along Z
  agx::PrismaticRef y = new agx::Prismatic(centerBody, frame2);
  simulation->add(y);
  y->getLock1D()->setEnable(true);
 
  osg::MatrixTransform* textRoot = new osg::MatrixTransform();
  textRoot->setMatrix(osg::Matrix::translate(position.x(), position.y(), position.z() + 1.1));
  root->addChild(textRoot);
 
  return XYAxis(x, y, textRoot);
}
 
osg::Group* buildTutorial1(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  osg::Group* root = new osg::Group();
 
 
  double size = agxIO::JoystickState::NUM_AXES * 4;
  agxCollide::GeometryRef background = new agxCollide::Geometry(new agxCollide::Box(size, 0.1, size));
  background->setPosition(0, 0.4, 0);
  simulation->add(background);
  auto node = agxOSG::createVisual(background, root);
  agxOSG::setDiffuseColor(node, agxRender::Color::White());
 
  // Create a sensor manager.
  // This will initialize the joystick/gamepad using the current graphic window.
  agxIO::JoystickManagerRef manager = new agxIO::JoystickManager(application->getHWND());
  simulation->add(manager);
 
  // If we do not have a device connected we will display a text.
  if (!manager->valid())
    application->getSceneDecorator()->setText(4, "No Gamepad or Joystick detected", agxRender::Color::Yellow());
 
  AxisVector axes;
 
  double delta = 2;
 
  // For each axis, connect to a prismatic joint so we can move in X/Z.
  agx::Vec3 leftPosition(-agxIO::JoystickState::NUM_AXES / 2 * 2, 0, 0);
 
  for (int i = 0; i < agxIO::Joystick::State::NUM_AXES; i++)
  {
    agx::Vec3 position = leftPosition + agx::Vec3(i * delta, 0, 0);
    axes.push_back(createAxis(agxRender::Color::Red(), position, simulation, root));
  }
 
  // Create another one for the POV button.
  auto pov = createAxis(agxRender::Color::Blue(), leftPosition - agx::Vec3(0, 0, 3), simulation, root);
 
  // Create a number of buttons. We do not know how many the device has.
  // Whenever a button is pressed, the button geometry will change color
  ButtonVector buttons;
  const int numButtons = 13;
  delta = std::abs(leftPosition.x()) * 2 / numButtons;
  for (int i = 0; i < numButtons; i++)
  {
    auto position = leftPosition + agx::Vec3(delta * i, 0, 3);
    auto button = createButton(i, position, simulation, root);
    buttons.push_back(button);
  }
 
  // Create our listener that listens to Gamepad events.
  // Connect with all the axes, pov and buttons that we have created
  agx::ref_ptr<SimulationJoystickListener> listener = new SimulationJoystickListener(axes, pov, buttons);
 
  // Add the listener to the manager
  manager->add(listener);
 
  // Setup some graphical stuff.
  application->getSceneDecorator()->setEnableShadows(true);
  application->getSceneDecorator()->setShadowMethod(agxOSG::SceneDecorator::SOFT_SHADOWMAP);
 
  // Position camera
  auto cam_eye = agx::Vec3(-2.155E-01, -1.487E+01, -3.7E+00);
  auto cam_center = agx::Vec3(-4.90E-01, 5.5E-01, -6.3E-01);
  auto up = agx::Vec3(0.0082, -0.1952, 0.9807);
  application->setCameraHome(cam_eye, cam_center, up);
 
  // Explicitly position light sources
  application->getSceneDecorator()->setEnableCalculateLightPositions(agxOSG::SceneDecorator::LIGHT0, false);
  application->getSceneDecorator()->setEnableCalculateLightPositions(agxOSG::SceneDecorator::LIGHT1, false);
 
  agx::Vec4 eye(-0.2581, -15.6644, -5.26, 1);
  agx::Vec3 center(-0.2719, -14.7023, -4.98773);
  agx::Vec3 direction = (eye.asVec3() - center).normal();
 
  agxOSG::LightSource light0 = application->getSceneDecorator()->getLightSource(agxOSG::SceneDecorator::LIGHT0);
  light0.setPosition(eye);
  light0.setDirection(direction);
 
  agxOSG::LightSource light1 = application->getSceneDecorator()->getLightSource(agxOSG::SceneDecorator::LIGHT0);
  light1.setPosition(eye);
 
  return root;
}
 
int main(int argc, char** argv)
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tGamePad Tutorial\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene(buildTutorial1, '1');
 
    if (application->init(argc, argv))
      return application->run();
  }
  catch (std::exception& e) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}
 
#else
#include <agx/agx.h>
#include <agx/version.h>
#include <iostream>
int main(int, char**)
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tGamePad Tutorial\n" <<
    "\t--------------------------------\n\n" << std::endl;
  std::cout << "Build without agxSensor support" << std::endl;
}
#endif // AGX_USE_OIS()

tutorial_granularBodies.cpp

/*
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.
*/
 
/*
 
Demonstrates the Granular simulation model
 
*/
 
 
#include <agxOSG/ExampleApplication.h>
 
#include <agx/version.h>
#include <agx/Physics/GranularBodySystem.h>
#include <agx/ParticleEmitter.h>
#include <agx/Prismatic.h>
#include <agx/SystemInformation.h>
#include <agxUtil/StepEventCallback.h>
#include <agxRender/RenderSingleton.h>
#include "tutorialUtils.h"
 
#include <agxOSG/ReaderWriter.h>
#include <json/json.h>
namespace
{
  class MotorStepListener : public agxSDK::StepEventListener
  {
  public:
    MotorStepListener(agx::Prismatic* prismatic, agx::Real speed, agx::Real interval)
      : m_prismatic(prismatic),
      m_speed(speed),
      m_interval(interval),
      m_last(0)
    {
      setMask(MotorStepListener::PRE_STEP);
    }
 
    void pre(const agx::TimeStamp& t)
    {
      if (t - m_last >= m_interval)
      {
        m_last = t;
        m_speed = -m_speed;
        m_prismatic->getMotor1D()->setSpeed(m_speed);
      }
    }
 
    agx::Prismatic* m_prismatic;
    agx::Real m_speed;
    agx::Real m_interval;
    agx::Real m_last;
  };
}
 
 
 
osg::Group* buildTutorial1(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  std::cout << "Tutorial 1: Basic example of GranularBodySystem." << std::endl;
 
  osg::Group* root = new osg::Group;
 
  // Set maximum number of threads according to physical cores ( given that HT/SMT is enabled ).
  const agx::UInt numCores = agx::SystemInformation::getNumCpu();
  agx::setNumThreads(numCores / 2);
 
  // Use the parallel PGS solver for best performance using multiple threads.
  simulation->getDynamicsSystem()->getSolver()->setUseParallelPgs(true);
 
  // Enable 32bit float solver for granular bodies to increase performance.
  simulation->getDynamicsSystem()->getSolver()->setUse32bitGranularBodySolver(true);
 
  // Create a floor geometry.
  agxCollide::GeometryRef floorGeometry = new agxCollide::Geometry(new agxCollide::Box(10, 10, 0.2));
  agx::RigidBodyRef floor = new agx::RigidBody(floorGeometry);
  floor->setPosition(0, 0, -0.2);
  floor->setMotionControl(agx::RigidBody::STATIC);
  simulation->add(floor);
  agxOSG::createVisual(floor, root);
 
  // Create and configure the particle system.
  agx::Physics::GranularBodySystemRef granularBodySystem = new agx::Physics::GranularBodySystem();
  simulation->add(granularBodySystem);
 
  // Create two materials so we can define the interaction parameters.
  agx::MaterialRef pelletmat = new agx::Material("pellet");
  agx::MaterialRef floormat = new agx::Material("floor");
 
  // Setup the material properties.
  pelletmat->getBulkMaterial()->setDensity(2500);
  agx::ContactMaterialRef c = new agx::ContactMaterial(pelletmat, floormat);
  c->setYoungsModulus(1.1e+11);
  c->setFrictionCoefficient(0.5);
  c->setRollingResistanceCoefficient(0.3);
  simulation->add(c);
 
  agx::ContactMaterialRef c2 = new agx::ContactMaterial(pelletmat, pelletmat);
  c2->setYoungsModulus(1.1e+11);
  c2->setFrictionCoefficient(0.5);
  c2->setRollingResistanceCoefficient(0.3);
  simulation->add(c2);
 
  // Specify the default radius of each granular body.
  granularBodySystem->setParticleRadius(0.08);
 
  // Set the default material of all the granular bodies.
  granularBodySystem->setMaterial(pelletmat);
 
  // Set the material of the floor.
  floorGeometry->setMaterial(floormat);
 
  // Spawn particles in a spaced lattice pattern in the container 3D bound.
  agx::Bound3 containerBound = agx::Bound3(agx::Vec3(-1.0, -1.0, 3), agx::Vec3(1.0, 1.0, 6));
  granularBodySystem->spawnParticlesInBound(containerBound,
    granularBodySystem->getParticleRadius(),
    agx::Vec3(2.0 * granularBodySystem->getParticleRadius()),
    0.3 * granularBodySystem->getParticleRadius());
 
  // We can loop over the particles and do stuff( set position, velocity, material ) etc.
  auto particles = granularBodySystem->getParticles();
  for (size_t i = 0; i < particles.size(); ++i)
  {
    agx::Physics::GranularBodyPtr gPtr = particles[i];
    gPtr->setVelocity(agx::Vec3(0, 0, 0));
    gPtr->setMaterial(pelletmat->getEntity());
    gPtr->setRadius(0.08);
    gPtr->setColor(agxRender::Color::Orange());
    // We can also adjust motion control for particles. ( DYNAMICS or KINEMATICS )
    gPtr.state().setMotionControl(agx::Physics::GranularBodySystem::MotionControl::DYNAMICS);
    gPtr.state().setEnabled(true);
  }
 
  std::cout << "Particle system now has " << granularBodySystem->getNumParticles() << " particles" << std::endl;
  application->getSceneDecorator()->setText(1, agx::String::format("particles: %d", granularBodySystem->getNumParticles()));
 
  // Create a visual representation for the granular.
  agxOSG::createVisual(granularBodySystem, root);
 
  // Declare sizes for funnel container, conveyor and scraper
  agx::Vec3 funnelSize = agx::Vec3(2 * 0.8, 0.1, 3 * 0.8);
  agx::Vec3 conveyorSize = agx::Vec3(4, 1, 0.1);
  agx::Vec3 scraperSize = agx::Vec3(0.5, 0.1, 0.2);
 
  // Now build a funnel container for all the granulars to fall down into.
  agxSDK::AssemblyRef containerAssembly = new agxSDK::Assembly();
 
  agx::Vec3 funnelPos = agx::Vec3(-1, -1, 4) + agx::Vec3(funnelSize.x() * (0.6),
    -funnelSize.y() * 1.9,
    funnelSize.z() * (-.2));
  agx::Real rotation = 20;
 
  agxCollide::GeometryRef w1 = new agxCollide::Geometry(new agxCollide::Box(funnelSize));
  w1->setPosition(funnelPos.x(), funnelPos.y(), funnelPos.z());
  w1->setRotation(agx::EulerAngles(agx::degreesToRadians(rotation), 0, 0));
  containerAssembly->add(w1);
 
  agxCollide::GeometryRef w2 = new agxCollide::Geometry(new agxCollide::Box(funnelSize));
  w2->setPosition(-funnelPos.y(), funnelPos.x(), funnelPos.z());
  w2->setRotation(agx::EulerAngles(agx::degreesToRadians(rotation), 0, agx::degreesToRadians(90)));
  containerAssembly->add(w2);
 
  agxCollide::GeometryRef w3 = new agxCollide::Geometry(new agxCollide::Box(funnelSize));
  w3->setPosition(funnelPos.y(), -funnelPos.x(), funnelPos.z());
  w3->setRotation(agx::EulerAngles(agx::degreesToRadians(rotation), 0, agx::degreesToRadians(-90)));
  containerAssembly->add(w3);
 
  agxCollide::GeometryRef w4 = new agxCollide::Geometry(new agxCollide::Box(funnelSize));
  w4->setPosition(funnelPos.x(), -funnelPos.y(), funnelPos.z());
  w4->setRotation(agx::EulerAngles(agx::degreesToRadians(rotation), 0, agx::degreesToRadians(180)));
  containerAssembly->add(w4);
 
  simulation->add(containerAssembly);
 
  // Create a visual for the funnel
  osg::Group* node = nullptr;
  node = agxOSG::createVisual(containerAssembly, root);
  agxOSG::setDiffuseColor(node, agx::Vec4f(0.2f, 0.4f, 1.0f, 1.0f));
  agxOSG::setShininess(node, 120);
  agxOSG::setAlpha(node, 0.2f);
 
  // Create a conveyor belt below the funnel container and scraper body which will move
  // back and forth over the conveyor belt which will displace the granular material out
  // to the sides from the middle of the conveyor belt.
  agxSDK::AssemblyRef assembly = new agxSDK::Assembly();
 
  agxCollide::GeometryRef conveyor = new agxCollide::Geometry(new agxCollide::Box(conveyorSize[0],
    conveyorSize[1],
    conveyorSize[2]));
  conveyor->addGroup("conveyor");  // so we can disable collisions between the "scraper" and the "conveyor"
  conveyor->setPosition(0, 0, 0);
  conveyor->setSurfaceVelocity(agx::Vec3f(1, 0, 0));
  assembly->add(conveyor);
 
  agxCollide::GeometryRef conveyor2 = new agxCollide::Geometry(new agxCollide::Box(conveyorSize[0] - scraperSize[0],
    conveyorSize[2],
    conveyorSize[2] * 3));
  conveyor2->addGroup("conveyor");
  conveyor2->setPosition(-scraperSize[0], conveyorSize[1] - conveyorSize[2], conveyorSize[2] * 4);
  assembly->add(conveyor2);
 
  agxCollide::GeometryRef conveyor3 = new agxCollide::Geometry(new agxCollide::Box(conveyorSize[0] - scraperSize[0],
    conveyorSize[2],
    conveyorSize[2] * 3));
  conveyor3->addGroup("conveyor");
  conveyor3->setPosition(-scraperSize[0], -conveyorSize[1] + conveyorSize[2], conveyorSize[2] * 4);
  assembly->add(conveyor3);
 
  agxCollide::GeometryRef conveyor4 = new agxCollide::Geometry(new agxCollide::Box(conveyorSize[2],
    conveyorSize[1],
    conveyorSize[2] * 4));
  conveyor4->addGroup("conveyor");
  conveyor4->setPosition(conveyorSize[0] + conveyorSize[2], 0, conveyorSize[2] * 4);
  assembly->add(conveyor4);
 
  // Create the scraper geometry
  agxCollide::GeometryRef scraperGeometry = new agxCollide::Geometry(new agxCollide::Box(scraperSize[0],
    scraperSize[1],
    scraperSize[2]));
  scraperGeometry->addGroup("scraper");
  agx::RigidBodyRef scraper = new agx::RigidBody(scraperGeometry);
  assembly->add(scraper);
  scraper->setPosition(conveyorSize[0] - scraperSize[0], 0, scraperSize[2] + conveyorSize[2]);
 
  assembly->setPosition(conveyorSize[0] / 2, 0, 0);
 
  node = agxOSG::createVisual(scraper, root);
  agxOSG::setAlpha(node, 1.0);
  agxOSG::setDiffuseColor(node, agx::Vec4f(0.0, 0.0, 0.0, 1.0));
  agxOSG::setShininess(node, 120);
 
  // Create a prismatic and a motor listener for that will move the the scraper geometry back and forth over the conveyor belt
  agx::FrameRef f1 = new agx::Frame();
  f1->setLocalRotate(agx::EulerAngles(0, agx::degreesToRadians(90), agx::degreesToRadians(90)));
  agx::PrismaticRef prismatic = new agx::Prismatic(scraper, f1);
  prismatic->getRange1D()->setEnable(true);
  prismatic->setSolveType(agx::Constraint::DIRECT_AND_ITERATIVE);
  prismatic->getRange1D()->setRange(-conveyorSize[1] + scraperSize[1], conveyorSize[1] - scraperSize[1]);
  assembly->add(prismatic);
 
  agx::Real speed = 1.0;
  agx::Real interval = 3.0;
  agxSDK::StepEventListenerRef listener = new MotorStepListener(prismatic, interval, speed);
 
  // Enable the motor and set the initial speed
  prismatic->getMotor1D()->setEnable(true);
  prismatic->getMotor1D()->setSpeed(speed);
 
  simulation->add(listener);
 
  // Now disable collisions between the two named groups "conveyor" and "scraper"
  simulation->getSpace()->setEnablePair("conveyor", "scraper", false);
 
  // Create a visual for the created assembly
  node = agxOSG::createVisual(assembly, root);
  agxOSG::setDiffuseColor(node, agx::Vec4f(0.6f, 0.6f, 0.6f, 1.0f));
  agxOSG::setShininess(node, 120);
  agxOSG::setAlpha(node, 0.3f);
 
  simulation->add(assembly);
 
  return root;
}
 
 
 
namespace
{
  class ParticleEmitterMover : public agxSDK::StepEventListener
  {
  public:
    ParticleEmitterMover(agx::ParticleEmitter* emitter, agxCollide::Geometry* geom, agx::Real speed = 4.0)
      : m_emitter(emitter), m_emitterGeom(geom), m_speed(speed)
    {
      setMask(ParticleEmitterMover::PRE_STEP);
    }
 
    // Called before each time step
    void pre(const agx::TimeStamp& t)
    {
      if (m_emitter == nullptr || m_emitterGeom == nullptr) {
        getSimulation()->remove(this);
        return;
      }
 
      // Set the rotation of the emitter with respect to time
      m_emitterGeom->setRotation(agx::EulerAngles(0, 0, (agx::PI / 6) * sin(2 * t)));
 
      // Give the particles an initial velocity in the outward direction of the emitter. We can transform the
      // initial velocity of the particles to the local transform space of the emitter geometry in order to
      // change the velocity direction according to the emitter rotation.
      m_emitter->setVelocity(m_emitterGeom->getTransform().transform3x3(agx::Vec3(0, -1, 0)) * m_speed);
    }
 
    agx::ParticleEmitterObserver m_emitter;
    agxCollide::GeometryObserver m_emitterGeom;
    agx::Real m_speed;
  };
}
 
 
 
osg::Group* buildTutorial2(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/)
{
  std::cout << "Tutorial 2: Particle emitter with granular <-> rigid body interaction" << std::endl;
  osg::Group* root = new osg::Group;
 
  // Set maximum number of threads according to physical cores ( given that HT/SMT is enabled ).
  const agx::UInt numCores = agx::SystemInformation::getNumCpu();
  agx::setNumThreads(numCores / 2);
 
  // Use the parallel PGS solver for best performance using multiple threads
  simulation->getDynamicsSystem()->getSolver()->setUseParallelPgs(true);
 
  // Enable 32bit float solver for granular bodies to increase performance
  simulation->getDynamicsSystem()->getSolver()->setUse32bitGranularBodySolver(true);
 
  // Create two materials so we can define the interaction parameters
  agx::MaterialRef pelletmat = new agx::Material("pellet");
  agx::MaterialRef bodymat = new agx::Material("body");
 
  // Setup the material properties
  pelletmat->getBulkMaterial()->setDensity(2500);
  agx::ContactMaterialRef c = new agx::ContactMaterial(pelletmat, bodymat);
  c->setYoungsModulus(1.1e+11);
  c->setFrictionCoefficient(0.5);
  c->setRollingResistanceCoefficient(0.3);
  simulation->add(c);
 
  agx::ContactMaterialRef c2 = new agx::ContactMaterial(pelletmat, pelletmat);
  c2->setYoungsModulus(1.1e+11);
  c2->setFrictionCoefficient(0.5);
  c2->setRollingResistanceCoefficient(0.3);
  simulation->add(c2);
 
  // Create a geometry with a box shape as our floor. We create a box instead
  // of a plane because OSG currently don't know abut the particles and
  // therefore cannot set a view frustum than includes them and because
  // 'agxOSG::createVisual' for a plane appears to do nothing.
  agxCollide::GeometryRef floorGeometry = new agxCollide::Geometry(new agxCollide::Box(agx::Vec3(5, 5, 0.1)));
  floorGeometry->setMaterial(bodymat);
  simulation->add(floorGeometry);
  agxOSG::createVisual(floorGeometry, root);
 
  // Create a granular body system.
  agx::Physics::GranularBodySystemRef granularBodySystem = new agx::Physics::GranularBodySystem();
  simulation->add(granularBodySystem);
 
  // Set default radius of new particles
  granularBodySystem->setParticleRadius(0.04);
 
  // Create the visual representation of the GranularBodySystem.
  agxOSG::createVisual(granularBodySystem, root);
 
  // We will be using an emitter to create a steady flow of particles. The
  // emitter spawns particles randomly inside the provided geometry.
  agxCollide::GeometryRef spawnGeom = new agxCollide::Geometry(new agxCollide::Cylinder(0.15, 0.1));
  spawnGeom->setPosition(0, 2, 1);
  simulation->add(spawnGeom);
  agxOSG::createVisual(spawnGeom, root);
 
  // Disable collision between the geometry and the created particles.
  granularBodySystem->setEnableCollisions(spawnGeom, false);
 
  // Create a particle emitter and pass it the spawn geometry we just created.
  agx::ParticleEmitterRef emitter = new agx::ParticleEmitter(granularBodySystem);
  emitter->setSeed(0);
  emitter->setGeometry(spawnGeom);
 
  // Create a distribution with varying sizes and assign the distribution table to the emitter.
  agx::Emitter::DistributionTableRef distribution = new agx::Emitter::DistributionTable(agx::Emitter::QUANTITY_MASS);
  distribution->addModel(new agx::ParticleEmitter::DistributionModel(0.04, pelletmat, 0.6));
  distribution->addModel(new agx::ParticleEmitter::DistributionModel(0.09, pelletmat, 0.3));
  distribution->addModel(new agx::ParticleEmitter::DistributionModel(0.06, pelletmat, 0.1));
  emitter->setDistributionTable(distribution);
  simulation->add(emitter);
 
  // Set the rate parameter from the emitter.
  emitter->setRate(250);
 
  // Set the 'life' parameter of created particles (seconds).
  emitter->setLife(3.0);
 
  // Create a listener that will rotate the emitter.
  simulation->add(new ParticleEmitterMover(emitter, spawnGeom, 4.0));
 
  // Create some boxes to hit with the created granular bodies.
  agx::Vec3 boxSixe{ 0.1, 0.1, 0.3 };
  agx::Real spacing = 0.5;
  agx::Real z = 0.3;
  int numX = 8, numY = 4;
 
  agx::Real startX = -numX / 2 * spacing + boxSixe[0] * 2.5;
  agx::Real startY = 0;
  for (int xi = 0; xi < numX; xi++)
  {
    for (int yi = 0; yi < numY; yi++)
    {
      agx::Vec3 pos(startX + xi * spacing,
        startY + yi * spacing,
        z);
      agxCollide::GeometryRef geometry = new agxCollide::Geometry(new agxCollide::Box(boxSixe));
      geometry->setMaterial(bodymat);
      agx::RigidBodyRef body = new agx::RigidBody(geometry);
      body->setPosition(pos + agx::Vec3(0, 0, 0.1));
      simulation->add(body);
      agxOSG::setDiffuseColor(agxOSG::createVisual(body, root),
        agxRender::Color::DarkOrange());
    }
  }
 
  return root;
}
 
 
 
osg::Group* buildTutorial3(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/)
{
  std::cout << "Tutorial 3: GranularBody contact data extraction." << std::endl;
 
  osg::Group* root = new osg::Group();
 
  // Set maximum number of threads according to physical cores ( given that HT/SMT is enabled ).
  const agx::UInt numCores = agx::SystemInformation::getNumCpu();
  agx::setNumThreads(numCores / 2);
 
  // Use the parallel PGS solver for best performance using multiple threads.
  simulation->getDynamicsSystem()->getSolver()->setUseParallelPgs(true);
 
  // Enable 32bit float solver for granular bodies to increase performance.
  simulation->getDynamicsSystem()->getSolver()->setUse32bitGranularBodySolver(true);
 
  // Create a floor geometry.
  agxCollide::GeometryRef floorGeometry = new agxCollide::Geometry(new agxCollide::Box(10, 10, 1.0));
  agx::RigidBodyRef floor = new agx::RigidBody(floorGeometry);
  floor->setPosition(0, 0, -6);
  floor->setMotionControl(agx::RigidBody::STATIC);
  simulation->add(floor);
  agxOSG::createVisual(floor, root);
 
  // Create and configure the particle system.
  agx::Physics::GranularBodySystemRef granularBodySystem = new agx::Physics::GranularBodySystem();
  simulation->add(granularBodySystem);
  agxOSG::createVisual(granularBodySystem, root);
 
  // Create 2 rows of particles in the horizontal x - direction direction.
  auto createParticleRow = [granularBodySystem](agx::Real radius,
    agx::Real height,
    agx::Real increment,
    int numParticles)
  {
    agx::Vec3 startPos = agx::Vec3(increment * radius * (numParticles / 2) - radius, 0, height);
    for (int i = 0; i < numParticles; i++)
    {
      auto p = granularBodySystem->createParticle();
      p->setPosition(agx::Vec3(i * increment * radius, 0, 0) - startPos);
      p->setRadius(radius);
      p->setAngularVelocity(agx::Vec3(0, 0, agx::PI_2));
    }
  };
 
  agx::Real radius = 0.4;
  agx::Real increment = 2.05;
  int numParticles = 6;
  createParticleRow(radius, 0.5, increment, numParticles);
  createParticleRow(radius, 5.0, increment, numParticles);
 
  // Function that collects all particle-particle contacts, parses data and computes the relative
  // velocity in the normal direction.
  auto parseParticleParticleContacts = [granularBodySystem, radius](agxSDK::Simulation* s)
  {
    // Get the particle < ->particle contacts from the Space objects.
    auto particleParticleContacts = s->getSpace()->getParticleParticleContacts();
    for (auto& c : particleParticleContacts)
    {
      // Get particle ids from the contact.
      agx::Index particle1ID = c->getParticleId1();
      agx::Index particle2ID = c->getParticleId2();
 
      // Get normals and tangents. These are all in agx::Vec3f format they
      // need to be converted to agx::Vec3 in order to be multiplied with
      // 'agx::Vec3' from methods like 'getVelocity' or 'getPosition'.
      agx::Vec3f normal = c->getNormal();
      agx::Vec3f tu = c->getTangentU();
      agx::Vec3f tv = c->getTangentV();
 
      //Extract contact point and depth.
      agx::Vec3 point = c->getPoint();
      agx::Real depth = c->getDepth();
 
      // Extract contact force. Note that this will be zero if read in
      // the 'pre-step' phase of the simulation. Forces are available
      // in the 'post-step' phase of the simulation which is executed
      // after the solver.
      agx::Vec3 f = c->getLocalForce();
 
      // Get particles from id.
      agx::Physics::GranularBodyPtr p1 = granularBodySystem->getParticle(particle1ID);
      agx::Physics::GranularBodyPtr p2 = granularBodySystem->getParticle(particle2ID);
 
      // Calculate relative velocity in normal direction.
      agx::Vec3 v1 = p1->getVelocity();
      agx::Vec3 v2 = p2->getVelocity();
 
      // Compute relative velocity of the particles relative to particle 1
      // note that normals are in a Vec3f format and not Vec3 and is thus
      // incompatible with regular multiplication. Thus we convert to Vec3 first.
      agx::Vec3 n = agx::Vec3(normal);
      agx::Vec3 rel21 = n * ((v2 - v1) * n);
 
      // Only print the relative speed between the particles if it is above a certain threshold.
      if (rel21.length() > 1E-1)
      {
        agx::Real t = s->getTimeStamp();
        std::cout << agx::String::format("t=%5.2f particle: %d <-> particle: %d relative velocity: %5.2f m/s, force: %5.2f N",
          t, particle1ID, particle2ID, rel21.length(), f.length()) << std::endl;
      }
 
      // Draw a single sphere at the contact point between the granulars and also visualize
      // the contact tangents where radius and size is modified by contact depth.
      agxRender::RenderSingleton::instance()->add(point, radius + depth, agxRender::Color::Red());
      agxRender::RenderSingleton::instance()->add(point, point + agx::Vec3(tu), radius / 2 + depth, agxRender::Color::Blue());
      agxRender::RenderSingleton::instance()->add(point, point + agx::Vec3(tv), radius / 2 + depth, agxRender::Color::Blue());
    }
  };
 
  // We use a utility function to execute the contact parsing function in the "post" stage
  // of the simulation where the solver has computed contact forces and integrated the system.
  agxUtil::StepEventCallback::post(parseParticleParticleContacts, simulation);
 
  auto parseParticleGeometryContacts = [granularBodySystem, radius](agxSDK::Simulation* s)
  {
    // Get the particle <-> geometry contacts from the Space objects.
    auto particleGeometryContacts = s->getSpace()->getParticleGeometryContacts();
    for (auto& c : particleGeometryContacts)
    {
      // Get particle id from the contact.
      agx::Index particleID = c->getParticleId();
 
      // Get normals and tangents. These are all in agx::Vec3f format
      // they need to be converted to agx::Vec3 in order to be multiplied with
      // agx::Vec3 from methods like 'getVelocity' or 'getPosition'.
      agx::Vec3f normal = c->getNormal();
      agx::Vec3f tu = c->getTangentU();
      agx::Vec3f tv = c->getTangentV();
 
      // Extract contact point and depth
      agx::Vec3 point = c->getPoint();
      agx::Real depth = c->getDepth();
 
      // Extract contact force. Note that this will be zero if read in the
      // 'pre-step' phase of the simulation. Forces are available in the
      // 'post-step' phase of the simulation which is executed after the solver.
      agx::Vec3 f = c->getLocalForce();
 
      // Get ParticlePtr / GranularBodyPtr from id by using the GranularBodySystem.
      agx::Physics::GranularBodyPtr p = granularBodySystem->getParticle(particleID);
 
      // Get the geometry from the contact and then the rigid body.
      agx::Index geometryID = c->getGeometryId();
      agxCollide::Geometry* geom = s->getSpace()->getGeometryWithEntityId(geometryID);
      agx::RigidBody* rigidBody = geom->getRigidBody();
 
      // Compute the relative velocity of the objects relative object 1
      // note that normals are in a Vec3f format and not Vec3 and is thus
      // incompatible with regular multiplication. Thus we convert to Vec3 first.
      agx::Vec3 n = agx::Vec3(normal);
      agx::Vec3 v1 = p->getVelocity();
      agx::Vec3 v2 = rigidBody->getVelocity();
      agx::Vec3 rel21 = n * ((v2 - v1) * n);
 
      // Only print the relative speed between the particles if it is above a certain threshold.
      if (rel21.length() > 1E-1)
      {
        agx::Real t = s->getTimeStamp();
        std::cout << agx::String::format("t=%5.2f particle: %d <-> geometry: %d relative velocity: %5.2f m/s, force: %5.2f N",
          t, particleID, geometryID, rel21.length(), f.length()) << std::endl;
      }
 
      // Draw a single sphere at the contact point between the granulars and floor and also
      // visualize the contact tangents where radius and size is modified by contact depth.
      agxRender::RenderSingleton::instance()->add(point, radius, agxRender::Color::Green());
      agxRender::RenderSingleton::instance()->add(point, point + agx::Vec3(tu), radius / 2 + depth, agxRender::Color::Blue());
      agxRender::RenderSingleton::instance()->add(point, point + agx::Vec3(tv), radius / 2 + depth, agxRender::Color::Blue());
    }
  };
 
  // We use a utility function to execute the contact parsing function in the "post" stage
  // of the simulation where the solver has computed contact forces and integrated the system.
  agxUtil::StepEventCallback::post(parseParticleGeometryContacts, simulation);
 
  return root;
}
 
 
 
class AgeKinematic : public agxSDK::StepEventListener
{
public:
  AgeKinematic(agx::ParticleSystem* ps);
 
  void pre(const agx::TimeStamp& time) override;
 
private:
  agx::ParticleSystem* m_ps;
};
 
 
AgeKinematic::AgeKinematic(agx::ParticleSystem* ps) : m_ps(ps)
{
 
}
 
void AgeKinematic::pre(const agx::TimeStamp& /*time*/)
{
  agxData::Buffer* particleLifeBuffer = m_ps->getResource<agxData::Buffer>("Particle.life");
  agxData::Array<double> particleLifeArray = particleLifeBuffer->getArray<double>();
 
  agxData::Buffer* particleVelocityBuffer = m_ps->getResource<agxData::Buffer>("Particle.velocity");
  agxData::Array<agx::Vec3> velocityArray = particleVelocityBuffer->getArray<agx::Vec3>();
 
  agxData::Buffer* particleAngularVelocityBuffer = m_ps->getResource<agxData::Buffer>("Particle.angularVelocity");
  agxData::Array<agx::Vec3> angularVelocityArray = particleAngularVelocityBuffer->getArray<agx::Vec3>();
 
  agxData::Buffer* particleStateBuffer = m_ps->getResource<agxData::Buffer>("Particle.state");
  agxData::Array<agx::ParticleState> particleStateArray = particleStateBuffer->getArray<agx::ParticleState>();
 
  // For all particles that has a lifetime < 2 * timestep: Make them Kinematic.
  auto dt = getSimulation()->getTimeStep() * 2;
  for (size_t i = 0; i < particleStateArray.size(); i++)
  {
    if (particleLifeArray[i] < dt)
    {
      particleStateArray[i].setMotionControl(agx::RigidBody::KINEMATICS);
      particleLifeArray[i] = agx::Infinity;
      velocityArray[i].set(0, 0, 0);
      angularVelocityArray[i].set(0, 0, 0);
    }
  }
}
 
#include <agxUtil/Spline.h>
#include <agx/SplineJoint.h>
#include <agxUtil/TrimeshReaderWriter/TrimeshReaderWriter.h>
#include <agxIO/agxIO.h>
 
// Read json data and return a spline from the specified points
agxUtil::Spline* readSplineJson(const std::string& points_json_data)
{
  agxJson::Reader reader;
  agxJson::Value points;
  bool result = reader.parse(points_json_data, points);
  if (!result) {
    LOGGER_WARNING() << agx::String::format("Unable to read json data: %s", reader.getFormattedErrorMessages().c_str()) << LOGGER_ENDL();
    return nullptr;
  }
 
  agxUtil::Spline* spline = new agxUtil::CardinalSpline();
  for (agxJson::ArrayIndex i = 0; i < points.size(); ++i)
  {
    agxJson::Value& point = points[i];
    agxAssert(point.size() == 3);
 
    agx::Vec3 p;
    for (auto idx = 0; idx < 3; idx++)
      p[idx] = point[idx].asDouble();
 
    spline->add(p, 0);
  }
 
  return spline;
}
 
// based on points specified in a json string, create a SplineJoint that will attach the toolBody to the world
agx::Constraint* createSplineJoint(const std::string& points_json_data, agx::RigidBody* toolBody)
{
  // Create the spline joint from the points
  agxUtil::Spline* spline = readSplineJson(points_json_data);
 
  const auto& points = spline->getPoints();
  const auto& first_pos = points[0];
 
  toolBody->setPosition(first_pos);
 
  // Orient the body related to the joint
  agx::FrameRef frame = new agx::Frame(agx::AffineMatrix4x4::rotate(agx::PI_2, agx::Vec3::Z_AXIS()));
 
  auto joint = new agx::SplineJoint(toolBody, frame, spline);
 
  // Enable the motor and set a speed
  auto motor = joint->getMotor1D(agx::Constraint2DOF::FIRST);
  motor->setEnable(true);
  motor->setSpeed(0.05);
  motor->setForceRange(-100, 100);
  auto range = joint->getRange1D(agx::Constraint2DOF::FIRST);
  range->setEnable(true);
  range->setForceRange(-1000, 1000);
  range->setRange(-0.2, 0.2);
 
  auto lock = joint->getLock1D(agx::Constraint2DOF::SECOND);
  lock->setEnable(true);
 
  return joint;
}
 
// Specify the density of the granulars
static const struct GranularMaterialSpecification
{
  const double density = 2000;
} g_granularMaterialSpecification;
 
const double g_timeStep = 1.0 / 2100;
 
// Specify the contact material for the granular against other granulars
static const struct GranularMaterialInteractionSpecification
{
  const double YM = 2.1e+11;
  const double frictionCoefficient = 1;
  const double rollingResistance = 1;
  const double restitution = 2.0;
  const double damping = 4.5 * (1.0 / g_timeStep);
  const double adhesiveForce = 10;
  const double adhesiveOverlap = 1e-4;
} g_materialInteractionSpecification;
 
// Setup the granular emitter
static const struct EmitterSpecification {
  const double radius = 0.15e-3;
  const double rate = 0.0009 * 1E3 / (60 * 60);  // 0.0009 ton / h
  const double maximumQuantity = agx::Infinity;
  const agx::Vec3 velocity = agx::Vec3(0, 0.2, 0);
  const double lifeTime = 0.065;
} g_emitterSpecification;
 
// Create the material for granulars
std::pair<agx::Material*, agx::Material*> createMaterials(agxSDK::Simulation* simulation)
{
  // Create two materials so we can define the interaction parameters
  auto granular_material = new agx::Material("Granular");
  auto ground_material = new agx::Material("Ground");
 
  // Set density if the material coupled to the granular material
  granular_material->getBulkMaterial()->setDensity(g_granularMaterialSpecification.density);
 
  // Material configuration for granular - ground floor
  auto c = simulation->getMaterialManager()->getOrCreateContactMaterial(granular_material, granular_material);
  c->setYoungsModulus(g_materialInteractionSpecification.YM);
  c->setFrictionCoefficient(g_materialInteractionSpecification.frictionCoefficient);
  c->setRollingResistanceCoefficient(g_materialInteractionSpecification.rollingResistance);
  c->setRestitution(g_materialInteractionSpecification.restitution);
  c->setDamping(g_materialInteractionSpecification.damping);
 
  c->setAdhesion(g_materialInteractionSpecification.adhesiveForce, g_materialInteractionSpecification.adhesiveOverlap);
 
  // Material configuration for granular - granular interaction
  auto c2 = simulation->getMaterialManager()->getOrCreateContactMaterial(granular_material, ground_material);
  c2->setYoungsModulus(g_materialInteractionSpecification.YM);
  c2->setFrictionCoefficient(g_materialInteractionSpecification.frictionCoefficient);
  c2->setRollingResistanceCoefficient(g_materialInteractionSpecification.rollingResistance);
  c2->setRestitution(g_materialInteractionSpecification.restitution);
  c2->setDamping(g_materialInteractionSpecification.damping);
  c2->setAdhesion(g_materialInteractionSpecification.adhesiveForce, g_materialInteractionSpecification.adhesiveOverlap);
 
  return std::make_pair(granular_material, ground_material);
}
 
// Create and return a granular system from the specification
agx::Physics::GranularBodySystem* createGranularBodySystem(agxSDK::Simulation* simulation, osg::Group* root, agx::RigidBody* nozzle_body, agx::Material* granular_material)
{
  auto granularBodySystem = new agx::Physics::GranularBodySystem();
 
  // Set the material of all the granular bodies
  granularBodySystem->setMaterial(granular_material);
 
  auto table = new agx::Emitter::DistributionTable(agx::Emitter::QUANTITY_MASS);
  table->addModel(new agx::ParticleEmitter::DistributionModel(g_emitterSpecification.radius, granular_material, 1));
 
  // Create a visual representation for the Granulars
  agxOSG::createVisual(granularBodySystem, root);
  simulation->add(granularBodySystem);
 
  auto emitter = new agx::ParticleEmitter(granularBodySystem, agx::Emitter::QUANTITY_MASS);
  emitter->setLife(g_emitterSpecification.lifeTime);
 
  // Set the previously declared distribution table to the emitter.
  emitter->setDistributionTable(table);
 
  // Set the previously declared distribution table to the emitter.
  emitter->setDistributionTable(table);
 
  emitter->setRate(g_emitterSpecification.rate);
 
  emitter->setMaximumEmittedQuantity(g_emitterSpecification.maximumQuantity);
 
  // Create spawn geometry for the emitter.The particles will be created inside the bound of the shape in the geometry.
  emitter->setGeometry(nozzle_body->getGeometries()[0]);
 
  // Set an initial velocity downward to reduced initial collisions create in the spawn geometry.
  emitter->setVelocity(g_emitterSpecification.velocity);
 
  simulation->add(emitter);
 
  return granularBodySystem;
}
 
// Specify the points along which a spline joint will be created
static const std::string s_points_json_data = "[[-0.5383610006568054, 0.07257386344015329, 0.2612060291523281], [-0.5418047474216708, 0.11293615062691789, 0.26370108581085105], [-0.5361500496970344, 0.14519659816770528, 0.2652300372674672], [-0.4775519268170531, 0.2080014742821952, 0.26620497184289876], [-0.3964428295845093, 0.27186709252238345, 0.26621986020922855], [-0.3176976805866512, 0.3171006043823843, 0.2661171216674311], [-0.27655114555090565, 0.3241443742601609, 0.2701447747685833], [-0.2274695051932803, 0.31130002896729714, 0.2668435478351895], [-0.21083479463568902, 0.3024535683707251, 0.26533277559175883], [-0.20934248531798608, 0.2888885696911075, 0.263967521222618], [-0.22866360309227196, 0.2540214836548071, 0.26167469092540563], [-0.3008557485211213, 0.1496838681729756, 0.2507133475332961], [-0.32996967457399284, 0.09763065730373077, 0.24863848352199786], [-0.3631376428639147, 0.05981029822581489, 0.25300237978599127], [-0.3924501514037914, 0.047172225366720985, 0.24857640003099735], [-0.4268284901932429, 0.047616282428914934, 0.25091825113762367], [-0.4654995140423077, 0.0468375557720938, 0.25370482194536853], [-0.5162173618987876, 0.04642251652096434, 0.2576855869280641], [-0.5368906411728297, 0.06920317649666592, 0.26626008087213454]]";
 
// This tutorial demonstrates:
//
// Creating a SplineJoint and attaching an emitter body so that it moves along the spline
// Creating a step eventlistener that will put all "dead" particles to sleep (becomes Kinematic).
// This will increase the speed dramatically but other particles will still be able to collide with them.
osg::Group* buildTutorial4(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  std::cout << "Tutorial 4: Make particles kinematic along spline joint." << std::endl;
 
  osg::Group* root = new osg::Group();
 
  // Set maximum number of threads according to physical cores ( given that HT/SMT is enabled ).
  const agx::UInt numCores = agx::SystemInformation::getNumCpu();
  agx::setNumThreads(numCores / 2);
 
  simulation->setTimeStep(g_timeStep);
 
  // Create materials for the ground and the granular
  std::pair<agx::Material*, agx::Material*> materials = createMaterials(simulation);
  agx::Material* granular_material = materials.first;
  agx::Material* ground_material = materials.second;
 
  // Create a floor onto which the particles will collide and rest
  auto floor = new agxCollide::Geometry(new agxCollide::Box(0.3, 0.3, 0.005));
  floor->setMaterial(ground_material);
 
  floor->setPosition(-0.4, 0.2, 0.24);
  simulation->add(floor);
  agxOSG::createVisual(floor, root);
 
  // Create the "Emitter body"
  auto nozzle = new agx::RigidBody();
  nozzle->setRotation(agx::EulerAngles(-agx::PI_2, 0, 0));
  auto nozzle_geom = new agxCollide::Geometry(new agxCollide::Box(0.0035, 0.0015, 0.0015));
  nozzle_geom->setEnableCollisions(false);
 
  nozzle->add(nozzle_geom);
  simulation->add(nozzle);
  agxOSG::createVisual(nozzle, root);
 
  // Now create a SplineJoint and attach the emitter body
  simulation->add(createSplineJoint(s_points_json_data, nozzle));
 
  // Create a granular system
  auto granular_system = createGranularBodySystem(simulation, root, nozzle, granular_material);
 
  simulation->add(new AgeKinematic(granular_system));
 
  auto solver = simulation->getSolver();
 
  // We need very few iterations here.
  solver->setNumPPGSRestingIterations(5);
 
  // Setup the camera
  auto cameraData = application->getCameraData();
  cameraData.eye = agx::Vec3(-0.5968, 0.0156, 0.2695);
  cameraData.center = agx::Vec3(-0.3705, 0.3325, 0.2341);
  cameraData.up = agx::Vec3(0.0851, 0.0504, 0.9951);
  cameraData.nearClippingPlane = 0.01;
  cameraData.farClippingPlane = 50;
  application->applyCameraData(cameraData);
 
  return root;
}
 
 
int main(int argc, char** argv)
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial particle system\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene(buildTutorial1, '1');
    application->addScene(buildTutorial2, '2');
    application->addScene(buildTutorial3, '3');
    application->addScene(buildTutorial4, '4');
 
    if (application->init(argc, argv))
      return application->run();
  }
  catch (std::exception& e) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_gravityFields.cpp

/*
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.
*/
 
#include <agxOSG/ExampleApplication.h>
 
#include <agx/version.h>
#include <agx/LockJoint.h>
#include <agxSDK/GeometryFilter.h>
 
#include "tutorialUtils.h"
 
#include <agxIO/ImageValueInterpreter.h>
 
agxSDK::Assembly *createPyramid(const agx::Vec3& center, const agx::Vec3& boxesHalfExtent, int numBoxesAtBottomRow, agx::Real boxMass = -1)
{
  agxSDK::Assembly *parent = new agxSDK::Assembly();
 
  agx::Real eps = 0;
  agx::Real boxHeight = boxesHalfExtent.z();
  agx::Real boxLength = boxesHalfExtent.x();
  agx::Vec3 currentPos(-boxLength * numBoxesAtBottomRow, 0.0, boxHeight);
  for (int i = numBoxesAtBottomRow; i > 0; --i) {
    for (int j = i; j > 0; --j) {
      
      agxCollide::GeometryRef boxGeometry = new agxCollide::Geometry(new agxCollide::Box(boxesHalfExtent));
      agx::RigidBodyRef rb = new agx::RigidBody(boxGeometry);
      rb->setPosition(center + currentPos);
      parent->add(rb);
 
      if (boxMass > 0)
        rb->getMassProperties()->setMass(boxMass);
      currentPos.x() += (agx::Real)2.0 * boxLength + eps;
    }
    currentPos.x() = boxLength * (-i + 1);
    currentPos.z() += (agx::Real)2.0 * boxHeight + eps;
  }
 
  return parent;
}
 
#include <agx/UniformGravityField.h>
 
osg::Group* buildTutorial1( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create a pyramid of boxes
  agx::Vec3 boxGeometryHalfExtent( 3, 3, 0.5 );
  agxCollide::GeometryRef boxGeometry = new agxCollide::Geometry( new agxCollide::Box( boxGeometryHalfExtent ) );
  boxGeometry->setPosition( boxGeometry->getPosition() - agx::Vec3( 0, 0, boxGeometryHalfExtent.z() ) );
  simulation->add( boxGeometry );
  agxOSG::createVisual( boxGeometry, root );
 
  // Create a rigid body sphere
  agxSDK::AssemblyRef parent = createPyramid(agx::Vec3(0, 0, 0), agx::Vec3((agx::Real)0.2), 6, 0.1);
  agxOSG::createVisual(parent, root);
 
  simulation->add( parent );
 
  // 1: We can set a uniform gravity directly like this:
  simulation->setUniformGravity( agx::Vec3(0, 0, -9.81));
 
  // Or we can create a uniform gravity field also:
  agx::UniformGravityFieldRef gravityField = new agx::UniformGravityField();
  gravityField->setGravity(agx::Vec3(0, 0, -9.81));
 
  // 2: Exactly the same result as in 1
  simulation->setGravityField(gravityField);
  
  return root;
}
 
#include <agx/PointGravityField.h>
 
osg::Group* buildTutorial2( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create a rigid body sphere
  agxSDK::AssemblyRef parent = createPyramid(agx::Vec3(0, 0, 0), agx::Vec3((agx::Real)0.2), 6, 0.1);
  agxOSG::createVisual(parent, root);
 
  simulation->add(parent);
  
  // Now we want to create a Point gravity field instead:
  agx::PointGravityFieldRef gravityField = new agx::PointGravityField();
  
  // We specify the center of gravity:
  gravityField->setCenter(agx::Vec3(0, 0, 0));
 
  // And the magnitude of the gravity. The PointGravity will then calculate a gravity based on the position
  // of the center of mass of all bodies and particles
  gravityField->setGravity(9.81);
 
  // Register the point gravity field with the simulation:
  simulation->setGravityField(gravityField);
 
  return root;
}
 
#include <agx/CustomGravityField.h>
 
class MyGravityField : public agx::CustomGravityField
{
public:
  MyGravityField() {}
 
 
  virtual agx::Vec3 calculateGravity(const agx::Vec3& position) const
  {
    return position*-1;
  }
 
protected:
  ~MyGravityField() {}
 
};
 
 
osg::Group* buildTutorial3(agxSDK::Simulation *simulation, agxOSG::ExampleApplication* /*application*/)
{
  osg::Group* root = new osg::Group();
 
  // Create a rigid body sphere
  agxSDK::AssemblyRef parent = createPyramid(agx::Vec3(0, 0, 0), agx::Vec3((agx::Real)0.2), 6, 0.1);
  agxOSG::createVisual(parent, root);
 
  simulation->add(parent);
 
  // Now we want to create our own gravity field
  agx::GravityFieldRef gravityField = new MyGravityField();
 
  // Register the point gravity field with the simulation:
  simulation->setGravityField(gravityField);
 
  return root;
}
 
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout <<
                "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
                "\nGravityModel tutorial\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
    application->addScene( buildTutorial3, '3', false ); // Cannot test this in unittest as it will trash determinism (no serialization for custom gravity)
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;               
    agx::shutdown();
    return 1;
  }
 
  agx::shutdown();
  return 0;
}
 

tutorial_hydraulics.cpp

/*
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.
*/
 
/*
 
This file contains a collection of tutorials for the AGX Hydraulics library. It
describes the basics of how the library works and exemplifies this using a
handful of examples.
 
The tutorial creates no graphics window. All state inspection is instead done
using prints to the console.
 
*/
 
#include <agxIO/FileSystem.h>
 
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/Node.h>
 
#include <agxSDK/Simulation.h>
 
#include <agxDriveTrain/deprecated/HighLevelDriveTrain.h>
#include <agxDriveTrain/Engine.h>
 
#include <agxHydraulics/ConstantFlowValve.h>
#include <agxHydraulics/hydraulicUtils.h>
#include <agxHydraulics/Pipe.h>
#include <agxHydraulics/Pump.h>
#include <agxHydraulics/PistonActuator.h>
#include <agxHydraulics/ReliefValve.h>
#include <agxHydraulics/SpoolValve.h>
#include <agxHydraulics/hydraulicUtils.h>
 
#include <agxCollide/Box.h>
 
#include <agx/Prismatic.h>
#include <agx/version.h>
 
 
 
void runTutorial1()
{
  std::cout << "\nTutorial 1: Demonstrating basic hydraulic component creation and assembly." << std::endl;
 
  agxSDK::SimulationRef simulation = new agxSDK::Simulation();
 
  // Shorter names for the input and output sides, to cut down on the writing
  // required when connecting components.
  agxPowerLine::Side INPUT = agxPowerLine::INPUT;
  agxPowerLine::Side OUTPUT = agxPowerLine::OUTPUT;
 
  // A collection of parameters. We're using SI units here, but any system will
  // work. It is important to understand how the units relate to each other.
  // The unit of pressure is force over area, so expressing lengths in meters
  // and forces in newtons causes the pressure unit to be in N/m^2, or pascals
  // (Pa). The selection is made for an entire simulation, so if geometries for
  // rigid bodies are configured in meters, then pipe lengths and areas have to
  // be in units of meters as well.
  agx::Real pipeDiameter(0.03);
  agx::Real pipeArea = agxHydraulics::utils::diameterToArea(pipeDiameter);
  agx::Real shortPipeLength(0.5);
  agx::Real mediumPipeLength(1.0);
  agx::Real longPipeLength(5.0);
  agx::Real fluidDensity(800);
 
  // A hydraulics system is always part of a PowerLine. A PowerLine consists of
  // a graph of Units and Connectors. In the AGX world, a Unit represents one
  // or more bodies and a Connector represents one or more constraints. For
  // hydraulics, a pipe is an example of a Unit, while couplings, junctions,
  // pumps and motors are Connectors.
  agxPowerLine::PowerLineRef powerLine = new agxPowerLine::PowerLine();
 
  // The power line has to be part of some simulation to work. It is not
  // necessary to add each and every component to the simulation, the power
  // line will handle that. Instead the component must be made part of the
  // power line. More on this later in this tutorial.
  simulation->add(powerLine);
 
  // The most basic hydraulic component is a pipe. A pipe carries fluid and has
  // two sides, the input and the output. Positive flow is defined as fluid
  // entering through the input and exiting through the output. Naturally, a
  // pipe is said to have negative flow when the fluid goes the other
  // direction. A pipe is a subclass of Unit.
  agxHydraulics::PipeRef shortPipe = new agxHydraulics::Pipe(shortPipeLength, pipeArea, fluidDensity);
  agxHydraulics::PipeRef mediumPipe = new agxHydraulics::Pipe(mediumPipeLength, pipeArea, fluidDensity);
  agxHydraulics::PipeRef longPipe = new agxHydraulics::Pipe(longPipeLength, pipeArea, fluidDensity);
 
  // Pipes, and other PowerLine Units, are connected using Connectors.
  // Different Connectors perform different functions. The most basic hydraulic
  // Connector is the FlowConnector. It simply ensures that all fluid that
  // enters the Connector from some of the connected pipes must exit through
  // the other connected pipes. It models a basic pipe junction.
  agxHydraulics::FlowConnectorRef shortMediumCoupling = new agxHydraulics::FlowConnector();
 
  // Pipes are connected to a Connector using the Connector::connect method.
  // Just as with Units, a Connector has an input and an output side. For a
  // FlowConnector the Connector side doesn't matter, but it is important to
  // connect the pipe to the correct side of a pump or motor. However, a
  // logical layout is helpful when reasoning about the system.
  shortMediumCoupling->connect(INPUT, OUTPUT, shortPipe);
  shortMediumCoupling->connect(OUTPUT, INPUT, mediumPipe);
 
  // There is a quick-connect method available on pipes that creates the Flow-
  // Connector implicitly, if required. This will connect the output end of
  // 'mediumPipe' to the input end of 'longPipe'.
  mediumPipe->connect(longPipe);
 
  // The FlowConnector can be retrieved by asking either pipe for it. The same
  // connector will be returned on both of the following calls.
  agxHydraulics::FlowConnectorRef mediumLongCoupling = mediumPipe->getOutputFlowConnector();
  mediumLongCoupling = longPipe->getInputFlowConnector();
 
  // The final step before we have a complete hydraulics system is to add the
  // Unit/Connector graph to the power line. When a Unit is added the power
  // line will traverse the graph and add all Units and Connectors it finds
  // along the way. Since all our components are connected in a single graph, a
  // singe call to PowerLine::add is sufficient to add them all.
  powerLine->setSource(shortPipe);
 
  // We now have a complete hydraulics system ready to run. However, not much
  // is going to happen since we don't have any pressure generation yet. For
  // now we just give one of a pipes an initial flow rate.
  //
  // As previously noted, a Unit contains AGX bodies. However, the bodies are
  // managed by PhysicalDimensions which only represent one degree of
  // freedom (DoF) each. For a PhysicalDimension the position is named "value"
  // and the velocity is named "gradient", and they are both scalars.
  //
  // For the FlowDimension the value is the volume of fluid, in units of volume.
  // The gradient is the flow rate in units of volume per time.
  agx::Real initialFlowRate(0.01);
  shortPipe->getFlowDimension()->setGradient(initialFlowRate);
 
  // Let's step the system. The expected result is that the flow in the short
  // pipe will propagate into the other pipes. The flow rate after a few time
  // steps will be smaller than the value we set because of losses in the pipes
  // and the fact that some of the energy added to the fluid in the short pipe
  // is spent accelerating the fluid in the connected pipes.
  simulation->stepTo(simulation->getTimeStamp() + agx::Real(0.5));
 
  // The FlowUnit, a base class of Pipe, provides inspection methods so we can
  // get the current flow rate without having to deal with low-level details
  // such as PhysicalDimensions and gradients.
  //
  // The flow rates in all the pipes does not have to be exactly the same. The
  // toolkit models flexibility of the pipes and compressibility of the fluid as
  // constraint regularizations. The effect of this is that the total flow
  // into a FlowConnector may be larger or smaller than the total flow out.
  // What actually happen in the simulation is that the junction flexes.
  //
  // The pipe train we've created has two open ends: the input to the short
  // pipe and the output of the long pipe. All unconnected pipe ends are
  // assumed to be connected to an implicit tank from which any amount of fluid
  // can be fetched and to which any amount of fluid can be deposited.
  std::cout << "Flow rates throug the three pipes:" << std::endl;
  std::cout << " Short pipe flow rate:  " << shortPipe->getFlowRate() << " m^3/s." << std::endl;
  std::cout << " Medium pipe flow rate: " << mediumPipe->getFlowRate() << " m^3/s." << std::endl;
  std::cout << " Long pipe flow rate:   " << longPipe->getFlowRate() << " m^3/s." << std::endl;
 
  // A single FlowConnector can connect more than two pipes so that a
  // multi-pipe junction is formed. The constraint in the junction will ensure
  // that fluid that flows through the junction is conserved. That is, all
  // fluid that enters the junctions through the pipes either leave the junction
  // or be stored in the junction itself due to the flexibility.
  agxHydraulics::PipeRef secondLongPipe = new agxHydraulics::Pipe(longPipeLength, pipeArea, fluidDensity);
  mediumLongCoupling->connect(OUTPUT, INPUT, secondLongPipe);
 
  // Another jolt of flow rate to the short pipe and a few more simulation steps.
  shortPipe->getFlowDimension()->setGradient(initialFlowRate);
  simulation->stepTo(simulation->getTimeStamp() + agx::Real(0.5));
 
  // The output produced by the following shows that a FlowConnector only deals
  // with the total flow in and out of the junction. All the flow that comes
  // from the medium pipe is split over the two long pipes, but is it not split
  // evenly. The first long pipe already had some non-zero flow rate from
  // previously while the new pipe starts from zero. The junction has only a
  // single pressure to work with and that pressure is applied to all connected
  // pipes.
  //
  // However, losses in pipes are flow rate dependent so over time the greater
  // loss in the high flow rate pipe will act to equiliberate the flow rates.
  std::cout << "Flow rates after connecting the second long pipe:" << std::endl;
  std::cout << " Short pipe flow rate:       " << shortPipe->getFlowRate() << " m^3/s." << std::endl;
  std::cout << " Medium pipe flow rate:      " << mediumPipe->getFlowRate() << " m^3/s." << std::endl;
  std::cout << " Long pipe flow rate:        " << longPipe->getFlowRate() << " m^3/s." << std::endl;
  std::cout << " Second long pipe flow rate: " << secondLongPipe->getFlowRate() << " m^3/s." << std::endl;
  std::cout << " Sum flow in long pipes:     " << longPipe->getFlowRate()+secondLongPipe->getFlowRate() <<" m^3/s." << std::endl;
}
 
 
 
void runTutorial2()
{
  std::cout << "\nTutorial 2: Demonstrating pressure generation using an engine and a pump." << std::endl;
 
  agxSDK::SimulationRef simulation = new agxSDK::Simulation();
 
  //using namespace agxPowerLine::Side;
  agxPowerLine::Side INPUT = agxPowerLine::INPUT;
  agxPowerLine::Side OUTPUT = agxPowerLine::OUTPUT;
 
  // See tutorial 1 above.
  agx::Real pipeDiameter(0.03);
  agx::Real pipeArea = agxHydraulics::utils::diameterToArea(pipeDiameter);
  agx::Real shortPipeLength(0.5);
  agx::Real mediumPipeLength(1.0);
  agx::Real longPipeLength(5.0);
  agx::Real fluidDensity(800);
 
  // Parameters new to this tutorial.
  agx::Real engineTorque(100); // The torque delivered by the engine.
  agx::Real pumpDisplacement(0.1); // The volume of fluid displaced per radian of revolution of the engine.
 
  // Power line object created and added to the simulation as in tutorial 1.
  agxPowerLine::PowerLineRef powerLine = new agxPowerLine::PowerLine();
  simulation->add(powerLine);
 
  // The pump needs a RotationalDimension to attach to, and some kind of power
  // generation is required in order to drive everything. A
  // agxDriveTrain::Engine provides both of these. Here we create an engine
  // that is powered by a constant torque regardless of the angular velocity.
  agxDriveTrain::EngineRef engine = new agxDriveTrain::Engine();
  agxPowerLine::PowerGenerator* powerGenerator = engine->getPowerGenerator();
  agxPowerLine::PowerTimeIntegralLookupTable* curve = powerGenerator->getPowerTimeIntegralLookupTable();
  curve->insertValue(agx::Real(0), engineTorque); // The zero is irrelevant because we add a single value to the curve.
  engine->setThrottle(agx::Real(1));
 
  // A Unit can be added to the PowerLine before the rest of the Unit/Connector
  // graph is created. Connecting a Unit that has been added to a PowerLine to
  // a Unit that has not been added will cause the second Unit to be added as
  // well. In tutorial 1 we learned that adding a Unit will add all connected
  // Units as well, so the entire graph behind the newly connected Unit will be
  // added along with it.
  powerLine->add(engine);
 
  // A pump is a Connector that connects a RotationalDimension and a
  // FlowDimension. Whenever there is rotation there will also be flow. The
  // pump displacement is the ratio between the flow rate and the rotational
  // velocity. The higher the displacement, the more flow will be produced for
  // a given angular velocity. That is, the ratio 10:1, or displacement 10,
  // means that a rotational velocity of 1 produces a flow rate of 10.
  //
  // The inverse relationship holds for the torque-to-pressure ratio. The
  // pressure created by the pump is the torque applied on the pump shaft
  // divided by the pump displacement.
  agxHydraulics::PumpRef pump = new agxHydraulics::Pump(pumpDisplacement);
 
  // The rest of the hydraulics system is created as in tutorial 1.
  agxHydraulics::PipeRef shortPipe = new agxHydraulics::Pipe(shortPipeLength, pipeArea, fluidDensity);
  agxHydraulics::FlowConnectorRef shortMediumConnector = new agxHydraulics::FlowConnector();
  agxHydraulics::PipeRef mediumPipe = new agxHydraulics::Pipe(mediumPipeLength, pipeArea, fluidDensity);
  agxHydraulics::FlowConnectorRef mediumLongConnector = new agxHydraulics::FlowConnector();
  agxHydraulics::PipeRef longPipe = new agxHydraulics::Pipe(longPipeLength, pipeArea, fluidDensity);
 
  // The pump is used just like the FlowConnector, with the exception that the
  // Unit connected to the pump's input should have a RotationalDimension.
  pump->connect(INPUT, OUTPUT, engine);
  pump->connect(OUTPUT, INPUT, shortPipe);
 
  // The rest of the hydraulics system is connected as was described in tutorial 1.
  shortMediumConnector->connect(INPUT, OUTPUT, shortPipe);
  shortMediumConnector->connect(OUTPUT, INPUT, mediumPipe);
  mediumLongConnector->connect(INPUT, OUTPUT, mediumPipe);
  mediumLongConnector->connect(OUTPUT, INPUT, longPipe);
 
  // When the the simulation is run we will get an acceleration of both the
  // engine and the fluid until the torque available is completely consumed by
  // the frictional losses within the pipes.
  simulation->stepTo(simulation->getTimeStamp() + agx::Real(20));
 
  // The pressures of the hydraulics system can be read at the connections. In
  // the resulting output we can clearly see the pressure drop as we move further
  // away from the pump. We also see that the pressure at the pump follows the
  // definition of displacement. The engine supplies 100 Nm and the displacement
  // is 0.1. The pressure in the pump therefore becomes 100 / 0.1 = 1000.
  std::cout << "Connector pressures with pump at start:" << std::endl;
  std::cout << " Pump pressure:   " << pump->getPressure() << std::endl;
  std::cout << " Short-to-medium: " << shortMediumConnector->getPressure() << std::endl;
  std::cout << " Medium-to-long:  " << mediumLongConnector->getPressure() << std::endl;
  std::cout << " Flow rate: " << longPipe->getFlowRate() << std::endl;
 
  // The simulation has no notion of vacuum and will report negative pressure
  // whenever a negative pressure is required in order to maintain consistency
  // of the system. One such low pressure point is the inlet to the pump. This
  // is best illustrated by adding a new pipe before the pump. We call this the
  // tank pipe since it transports fluid from the tank to the pump.
  agxHydraulics::PipeRef tankPipe = new agxHydraulics::Pipe(shortPipeLength, pipeArea, fluidDensity);
  agxHydraulics::FlowConnectorRef tankShortConnector  = new agxHydraulics::FlowConnector();
  tankShortConnector->connect(INPUT, OUTPUT, tankPipe);
  tankShortConnector->connect(OUTPUT, INPUT, shortPipe);
  simulation->stepTo(simulation->getTimeStamp() + agx::Real(10));
 
  std::cout << "Connector pressures with tank pipe at start:" << std::endl;
  std::cout << " Pump inlet pressure:  " << tankShortConnector->getPressure() << std::endl;
  std::cout << " Pump pressure:        " << pump->getPressure() << std::endl;
  std::cout << " Pump outlet pressure: " << shortMediumConnector->getPressure() << std::endl;
  std::cout << " Medium-to-long:       " << mediumLongConnector->getPressure() << std::endl;
  std::cout << " Flow rate: " << longPipe->getFlowRate() << std::endl;
 
  // Negative pressures are usually not present in the real world. And what is
  // actually important in the simulation is pressure gradients. In order to
  // produce a positive flow in the tank pipe the pressure at the input must be
  // greater than the pressure at the output. The input side is not connected
  // to anything and the output side is connected to the pump, so the pump
  // alone will have to produce the pressure gradient and the only thing it can
  // do is to reduce its local pressure to something lower than the pressure at
  // the unconnected input side. Pressures at unconnected pipe ends are zero by
  // default, so the end result is that the pressure at the output end must be
  // negative.
  //
  // In the real world a pressure gradient over the tank pipe can be created
  // without negative pressures because there is a positive pressure source at
  // the input side as well. We call this the tank pressure. Any pipe end not
  // connected to anything is implicitly connected to the tank and will be
  // subjected to the tank pressure. The tank pressure is set globally.
  agxHydraulics::utils::setTankPressure(agx::Real(1e5));
  simulation->stepTo(simulation->getTimeStamp() + agx::Real(10));
 
  std::cout << "Connector pressures with tank pipe at start and tank pressure:" << std::endl;
  std::cout << " Pump inlet pressure:  " << tankShortConnector->getPressure() << std::endl;
  std::cout << " Pump pressure:        " << pump->getPressure() << std::endl;
  std::cout << " Pump outlet pressure: " << shortMediumConnector->getPressure() << std::endl;
  std::cout << " Medium-to-long:       " << mediumLongConnector->getPressure() << std::endl;
  std::cout << " Flow rate: " << longPipe->getFlowRate() << std::endl;
 
  // We now get pressures around the tank pressure instead of around zero and
  // negative pressures are avoided.
  //
  // Notice that the pressure reported by the pump is not the total pressure in
  // the pipe that the pump is connected to (the short pipe), but the pressure
  // that the pump adds. The total pressure gradient over the short pipe is the
  // pressure at the input connector plus the pump pressure minus the pressure
  // at the output connector.
  //    P_g = P_in + P_pump - P_out.
  // This positive value is the pressure loss over the short pipe.
  //
  // Setting a tank pressure is no guarantee that there will never be negative
  // pressures in the system. Any sudden pressure changes anywhere in the
  // system may cause sudden fluid accelerations, which in turn requires large
  // pressure gradients. Possible larger than the tank pressure. In the real
  // world the pressure would not even reach vacuum. Instead the water would
  // start gassing at the surface and even boil at ambient temperature, causing
  // cavitation within the fluid. This leads to reduced pump efficiency and
  // possibly damage to the pump. The AGX hydraulics toolkit has not been
  // designed to handle these situations.
 
 
  // A concept related to pressures is load. A pressure is a type of load, but
  // just as a power line may contain PhysicalDimensions other than
  // FlowDimensions, it may contain loads other than pressures. Torque is an
  // example of another type of load. In general, a load is the influence that
  // a Connector has over a PhysicalDimension. Given a PhysicalDimension, we
  // can ask it for its input or output load. The input load is the sum of
  // loads applied on the PhysicalDimension from all Connectors to which the
  // PhysicalDimension's Unit was connected using INPUT. For a pipe end
  // that is only connected to a FlowConnector this will equal the pressure of
  // that FlowConnector.
  //
  // It's more complicated when we consider the short pipe in the example since
  // it has been connected to both the 'pump' and the 'tankShortConnector'
  // using INPUT. The input load then becomes the sum of the two. We say
  // that the short pipe input has two pressure sources.
  //
  // A single Connector can apply loads of different kinds on different
  // PhysicalDimensions. When the short pipe computes its input load the pump
  // will report its influence as a pressure. When the engine computes its
  // output load the same pump will report a torque instead. The ratio between
  // these two values is exactly the pump displacement.
  //
  // Have a look at the output from the following.
  std::cout << "Loads:" << std::endl;
  std::cout << " TankPipe in:    " << tankPipe->getFlowDimension()->getInputLoad() << std::endl;
  std::cout << " TankPipe out:   " << tankPipe->getFlowDimension()->getOutputLoad() << std::endl;
  std::cout << " ShortPipe in:   " << shortPipe->getFlowDimension()->getInputLoad() << std::endl;
  std::cout << " ShortPipe out:  " << shortPipe->getFlowDimension()->getOutputLoad() << std::endl;
  std::cout << " MediumPipe in:  " << mediumPipe->getFlowDimension()->getInputLoad() << std::endl;
  std::cout << " MediumPipe out: " << mediumPipe->getFlowDimension()->getOutputLoad() << std::endl;
  std::cout << " LongPipe in:    " << longPipe->getFlowDimension()->getInputLoad() << std::endl;
  std::cout << " LongPipe out:   " << longPipe->getFlowDimension()->getOutputLoad() << std::endl;
 
  // The input load to the tank pipe is 0 because it has no Connectors attached
  // to that side. The tank pressure does not originate from a Connector and
  // only Connectors are considered when computing loads. The output load of
  // the tank pipe is the same as the pump inlet pressure printed previously. A
  // new number that shows up is the short pipe input load, and the pump
  // pressure printed previously is nowhere to be seen in the loads. Some of
  // the pressure generated by the pump is not usable to push fluid through the
  // short pipe. It is lost due to the flow resistance in the tank pipe
  // attached to the same input. The intuitive understanding is that the lost
  // pressure was required to move fluid through the tank pipe. Therefore we
  // get an input load at the short pipe that is slightly less than the sum of
  // the tank pressure and the pressure delivered by the pump.
  //
  // There are no additional complexities along the rest of the hydraulics
  // line, so the remaining loads equal the pressure in the connected
  // Connectors. Notice that the output load of one pipe is the same as the
  // input load of the next. The only addition is the 0 at the end, which
  // represents the end of the hydraulics line being implicitly connected to
  // the tank.
}
 
 
 
namespace Tutorial_3
{
  class Mechanics
  {
    public:
      Mechanics(agxSDK::Simulation* simulation)
      {
        this->createFloor(simulation);
        this->createCylinderBodies(simulation);
        this->createPrismatic(simulation);
      }
 
      agx::RigidBody* getBase()
      {
        return m_base;
      }
 
      agx::RigidBody* getHead()
      {
        return m_head;
      }
 
      agx::Prismatic* getJoint()
      {
        return m_prismatic;
      }
 
    private:
      void createFloor(agxSDK::Simulation* simulation)
      {
        agx::Vec3 he(agx::Real(10), agx::Real(10), agx::Real(0.2));
        agx::Vec3 pos(-he.z() * agx::Z_AXIS);
        m_floor = new agx::RigidBody("Floor");
        m_floor->add(new agxCollide::Geometry(new agxCollide::Box(he)));
        m_floor->setPosition(pos);
        m_floor->setMotionControl(agx::RigidBody::STATIC);
        simulation->add(m_floor);
      }
 
      void createCylinderBodies(agxSDK::Simulation* simulation)
      {
        agx::Vec3 baseHe(agx::Real(0.4));
        agx::Vec3 headHe(agx::Real(0.2));
 
        agx::Vec3 basePos(baseHe.z() * agx::Z_AXIS);
        agx::Vec3 headPos(agx::Real(3)*basePos + headHe.z() * agx::Z_AXIS);
 
        m_base = new agx::RigidBody("Base");
        m_head = new agx::RigidBody("Head");
 
        m_base->add(new agxCollide::Geometry(new agxCollide::Box(baseHe)));
        m_head->add(new agxCollide::Geometry(new agxCollide::Box(headHe)));
 
        m_base->setPosition(basePos);
        m_head->setPosition(headPos);
 
        simulation->add(m_base);
        simulation->add(m_head);
      }
 
      void createPrismatic(agxSDK::Simulation* simulation)
      {
        // The order that the bodies is given to the prismatic constructor is
        // significant. The prismatic's forward direction is defined as the
        // vector pointing from the second body towards the first. When the
        // hydraulic cylinder receives fluid at its input, then the first body
        // will move along that vector.
        m_prismatic = new agx::Prismatic(m_head, new agx::Frame(), m_base, new agx::Frame());
        simulation->add(m_prismatic);
      }
 
    private:
      agx::RigidBodyRef m_floor;
      agx::RigidBodyRef m_base;
      agx::RigidBodyRef m_head;
 
      agx::PrismaticRef m_prismatic;
  };
}
 
 
 
 
 
void runTutorial3()
{
  std::cout << "\nTutorial 3: Connection to mechanical system and more hydraulic components." << std::endl;
 
  agxSDK::SimulationRef simulation = new agxSDK::Simulation();
 
  //using namespace agxPowerLine::Side;
  agxPowerLine::Side INPUT = agxPowerLine::INPUT;
  agxPowerLine::Side OUTPUT = agxPowerLine::OUTPUT;
 
  using namespace Tutorial_3;
 
  // Create a mechanical system consisting of two boxes joined by a prismatic.
  // A static box serves as the floor.
  Mechanics mechanics(simulation);
 
  // Some pipe parameters.
  agx::Real pipeLength(1);
  agx::Real pipeArea(agxHydraulics::utils::diameterToArea(0.05));
  agx::Real fluidDensity(800);
 
  // These two areas define the pressure-to-force ratios of the cylinder. Each
  // area is the effective area that the fluid in one of the cylinder chambers
  // has in contact with the piston. The barrel area is the area of the front
  // chamber. When there is high pressure at the input then the magnitude of
  // the force acting to extend the cylinder is |F_ext| = P_in*barrelArea. The
  // piston area is the area of the rear chamber. When there is high pressure
  // at the output of the cylinder then the magnitude of the force acting to
  // compress the cylinder is |F_com| = P_out*pistonArea.
  //
  // The total force, along the direction of the cylinder, is the difference
  // between the two. |F| = P_in*barrelArea - P_out*pistonArea.
  //
  // A second effect of the areas is that the ratio between flow rates at the
  // input and output is the same as the ratio between the areas. For example,
  // if the piston area is half of the barrel area, then the flow rate at the
  // output will be half that of the flow rate at the input.
  agx::Real barrelArea(agxHydraulics::utils::diameterToArea(0.04));
  agx::Real pistonArea(agxHydraulics::utils::diameterToArea(0.03));
 
  agxPowerLine::PowerLineRef powerLine = new agxPowerLine::PowerLine();
  simulation->add(powerLine);
 
  // A pair of pipes to connect the cylinder to.
  agxHydraulics::PipeRef inputPipe = new agxHydraulics::Pipe(pipeLength, pipeArea, fluidDensity);
  powerLine->add(inputPipe);
  agxHydraulics::PipeRef outputPipe = new agxHydraulics::Pipe(pipeLength, pipeArea, fluidDensity);
 
 
  // The range of the hydraulic cylinder is defined by the range of the
  // prismatic from which it is created. The volumes of the two chambers is
  // calculated from the range and it is therefore important that it is set
  // appropriately.
  agx::RangeReal cylinderRange;
  cylinderRange.lower() = mechanics.getHead()->getPosition().z() - mechanics.getBase()->getPosition().z();
  cylinderRange.upper() = cylinderRange.lower() + agx::Real(4);
  mechanics.getJoint()->getRange1D()->setRange(cylinderRange);
  mechanics.getJoint()->getRange1D()->setEnable(true);
 
  // The cylinder itself is an instance of a PistonActuator. An Actuator is
  // something that converts motion in the power line into motion of mechanical
  // bodies. All Actuators are created from a regular AGX constraint that
  // defines how the power line may influence the mechanical bodies. The
  // PistonActuator can be created from a Prismatic or a DistanceJoint, and
  // pressure in the PistonActuator will make the mechanical bodies move in the
  // same way as if the Motor1D of the constraint had been used.
  agxHydraulics::PistonActuatorRef cylinder =
      new agxHydraulics::PistonActuator(mechanics.getJoint(), barrelArea, pistonArea, fluidDensity);
 
  // The cylinder is functionally very similar to a Unit and is connected to
  // its input and output pipes in the same way as Units are connected. Here we
  // use the quick connect form.
  inputPipe->connect(cylinder);
  cylinder->connect(outputPipe);
 
  // We now have a functional system that includes some bodies, a constraint, a
  // hydraulic cylinder and a pair of pipes. We can run this scene as is, but
  // not much will happen since there is nothing to power the hydraulics and
  // the cylinder will rest on its lower bound. All pressures will equal the
  // tank pressure that was set in the second tutorial and there will be no
  // flow.
  simulation->stepTo(simulation->getTimeStamp() + agx::Real(1));
 
  std::cout << "At rest:" << std::endl;
  std::cout << "  Input pressure:   " << inputPipe->getOutputFlowConnector()->getPressure() << std::endl;
  std::cout << "  Output pressure:  " << outputPipe->getInputFlowConnector()->getPressure() << std::endl;
  std::cout << "  Input flow rate:  " << inputPipe->getFlowRate() << std::endl;
  std::cout << "  Output flow rate: " << outputPipe->getFlowRate() << std::endl;
 
 
  // While the normal way to produce pressure is by using a pump driven by an
  // engine, an alternative approach that can be used when the details of
  // pressure generation are unimportant is to use a constant flow valve. A
  // constant flow valve is a component that restricts flow through a pipe to
  // some maximum configured flow rate.
  agx::Real targetFlowRate(0.002);
  agxHydraulics::ConstantFlowValveRef flowRateControl =
      new agxHydraulics::ConstantFlowValve(pipeLength, pipeArea, fluidDensity, targetFlowRate);
 
  // The constant flow valve is connected in the same way as any other FlowUnit,
  // by connecting it to a FlowConnector.
  agxHydraulics::FlowConnectorRef controlOutput = new agxHydraulics::FlowConnector();
  controlOutput->connect(INPUT, OUTPUT, flowRateControl);
  controlOutput->connect(OUTPUT, INPUT, inputPipe);
 
  // Normally, a constant flow valve may only act to restrict the flow rate of
  // a pipe. It models a restriction and not a pump so it should not be able to
  // increase the flow rate. It will therefore have no effect on the current
  // system.
  simulation->stepTo(simulation->getTimeStamp() + agx::Real(1));
 
  std::cout << "With passive constant flow valve:" << std::endl;
  std::cout << "  Input pressure:   " << inputPipe->getOutputFlowConnector()->getPressure() << std::endl;
  std::cout << "  Output pressure:  " << outputPipe->getInputFlowConnector()->getPressure() << std::endl;
  std::cout << "  Input flow rate:  " << inputPipe->getFlowRate() << std::endl;
  std::cout << "  Output flow rate: " << outputPipe->getFlowRate() << std::endl;
 
  // However, in situations where the details of pressure generation are
  // unimportant it can be configured to act in a pump-like manner, adding
  // energy to the system in order to reach the target flow rate.
  flowRateControl->setAllowPumping(true);
 
  // The constant flow valve will now apply whatever pressure is necessary to
  // reach and maintain the target flow rate. This can produce some very large
  // pressures during the acceleration phase.
  simulation->stepForward();
 
  std::cout << "One time step of active constant flow valve:" << std::endl;
  std::cout << "  Input pressure:   " << inputPipe->getOutputFlowConnector()->getPressure() << std::endl;
  std::cout << "  Output pressure:  " << outputPipe->getInputFlowConnector()->getPressure() << std::endl;
  std::cout << "  Input flow rate:  " << inputPipe->getFlowRate() << std::endl;
  std::cout << "  Output flow rate: " << outputPipe->getFlowRate() << std::endl;
 
  // Too high pressures can damage sensitive hydraulics components, which will
  // require some sort of protection. A common way of guarding such components
  // from damage due to high pressure is by the use of a relief valve.
 
  // First we stop all flow and other velocities so that the experiment can be
  // repeated.
  flowRateControl->setEnable(false);
  flowRateControl->getFlowDimension()->setGradient(agx::Real(0));
  inputPipe->getFlowDimension()->setGradient(agx::Real(0));
  cylinder->getFrontChamber()->getFlowDimension()->setGradient(agx::Real(0));
  cylinder->getRearChamber()->getFlowDimension()->setGradient(agx::Real(0));
  outputPipe->getFlowDimension()->setGradient(agx::Real(0));
  mechanics.getBase()->setVelocity(agx::Vec3());
  mechanics.getHead()->setVelocity(agx::Vec3());
  simulation->stepTo(simulation->getTimeStamp() + agx::Real(1));
 
  // A relief valve is a hydraulic component that monitors the local pressure
  // and opens a drain to tank whenever the pressure becomes too high. It has
  // three configuration parameters: the pressure at which the drain start to
  // open, called the cracking pressure; the pressure at which the drain is
  // fully open, called the fullyOpenPressure; and the maximum area that the
  // drain can open, called the fully open area.
  //
  // Here we set these to a few times larger than the tank pressure, but less
  // than the pressure observed in the print out.
  agx::Real crackingPressure(6e5);
  agx::Real fullyOpenPressure(9e5);
  agx::Real fullyOpenArea(1e-4);
  agxHydraulics::ReliefValveRef reliefValve = new agxHydraulics::ReliefValve(crackingPressure, fullyOpenPressure, fullyOpenArea);
 
  // The relief valve should be inserted between the pressure generator (the
  // constant flow valve) and the component we wish to protect (the cylinder).
  // This will require some restructuring and an additional FlowConnector.
 
  // First we disconnect the cylinder input pipe from the FlowConnector and
  // connect the relief valve in its place.
  controlOutput->disconnect(inputPipe);
  controlOutput->connect(OUTPUT, INPUT, reliefValve);
 
  // The we create the new cylinder input pipe FlowConnector and use it to
  // connect the relief valve and the input pipe.
  agxHydraulics::FlowConnectorRef inpipeInput = new agxHydraulics::FlowConnector();
  inpipeInput->connect(INPUT, OUTPUT, reliefValve);
  inpipeInput->connect(OUTPUT, INPUT, inputPipe);
 
  // Enabling the pump again is now safe since the cylinder is protected by the
  // relief valve.
  flowRateControl->setEnable(true);
 
 
  // Stepping again, we will see that the input pressure this time is lower
  // than before. The flow rates are also lower because we have less pressure
  // available to accelerate the fluid.
  simulation->stepForward();
 
  std::cout << "One time step of active constant flow valve with relief valve:" << std::endl;
  std::cout << "  Input pressure:   " << inputPipe->getOutputFlowConnector()->getPressure() << std::endl;
  std::cout << "  Output pressure:  " << outputPipe->getInputFlowConnector()->getPressure() << std::endl;
  std::cout << "  Input flow rate:  " << inputPipe->getFlowRate() << std::endl;
  std::cout << "  Output flow rate: " << outputPipe->getFlowRate() << std::endl;
 
 
  // Stepping a bit more we find that, given time, the flow rate into the
  // cylinder eventually reaches the target flow rate.
  simulation->stepTo(simulation->getTimeStamp() + agx::Real(1));
 
  std::cout << "With relief valve:" << std::endl;
  std::cout << "  Input pressure:   " << inputPipe->getOutputFlowConnector()->getPressure() << std::endl;
  std::cout << "  Output pressure:  " << outputPipe->getInputFlowConnector()->getPressure() << std::endl;
  std::cout << "  Input flow rate:  " << inputPipe->getFlowRate() << std::endl;
  std::cout << "  Output flow rate: " << outputPipe->getFlowRate() << std::endl;
 
}
 
 
 
void runTutorial4()
{
  std::cout << "\nTutorial 4: Flow control using spool valve." << std::endl;
 
  agxSDK::SimulationRef simulation = new agxSDK::Simulation();
 
  //using namespace agxPowerLine::Side;
  agxPowerLine::Side INPUT = agxPowerLine::INPUT;
  agxPowerLine::Side OUTPUT = agxPowerLine::OUTPUT;
 
  // This example demonstrates how to use a spool valve to construct a circuit
  // that can control the direction of flow through a sub circuit using a four
  // way/two position spool valve. We call the two flow directions 'forward'
  // and 'backward'. Two pipes are created with the same names. We will
  // configure the system so that the two pipes always have flow rates with
  // opposite signs. When we have positive flow in the 'forward' pipe then we
  // say that the circuit is configured for 'forward' flow. Similarly, when the
  // 'backward' pipe has positive flow then the circuit is configured for
  // 'backward' flow.
  //
  // We also create a pump pipe and a tank pipe. These pipes will always have
  // positive flow.
  //
  // The circuit layout is depicted in the diagram below. The arrow inside the
  // pipes denote positive flow direction, pointing from the pipe's input side
  // to the output side. The box in the middle represents the spool valve. At
  // any given time the spool valve is configured to either send fluid from
  // from the pump to the forward pipe, or to the backward pipe.
  //
  // Notice the the output sides of the forward and backward pipes are
  // connected so that the sign of the flow is reversed.
  //
  //   -----------      =======    --------------
  //   | Pump => |------|-----|----| Forward => |------
  //   -----------      |-----|    --------------     |
  //                    |=====|                       |
  //   -----------      |-\ /-|    ---------------    |
  //   | <= Tank |------|-/ \-|----| Backward => |-----
  //   -----------      |=====|    ---------------
 
 
  // Some pipe parameters.
  agx::Real pipeLength(1);
  agx::Real pipeArea(agxHydraulics::utils::diameterToArea(0.05));
  agx::Real fluidDensity(800);
 
  // We start with a basic setup where a constant flow valve supplies the flow.
  // and a relief valve limits the pressure sent into the circuit.
  agxPowerLine::PowerLineRef powerLine = new agxPowerLine::PowerLine();
  simulation->add(powerLine);
 
  agx::Real targetFlowRate(0.001);
  agxHydraulics::ConstantFlowValveRef pump = new agxHydraulics::ConstantFlowValve(pipeLength, pipeArea, fluidDensity, targetFlowRate);
 
  pump->getFlowDimension()->setName("PressureSource");
 
  pump->setAllowPumping(true);
  powerLine->add(pump);
 
  agx::Real crackingPressure(1e3);
  agx::Real fullyOpePressure(1.1e3);
  agxHydraulics::ReliefValveRef relief = new agxHydraulics::ReliefValve(crackingPressure, fullyOpePressure, pipeArea);
  pump->connect(relief);
 
  // The rest of the pipes are created.
  agxHydraulics::PipeRef pumpPipe = new agxHydraulics::Pipe(pipeLength, pipeArea, fluidDensity);
  agxHydraulics::PipeRef forwardPipe = new agxHydraulics::Pipe(pipeLength, pipeArea, fluidDensity);
  agxHydraulics::PipeRef backwardPipe = new agxHydraulics::Pipe(pipeLength, pipeArea, fluidDensity);
  agxHydraulics::PipeRef tankPipe = new agxHydraulics::Pipe(pipeLength, pipeArea, fluidDensity);
 
  pumpPipe->getFlowDimension()->setName("Pump");
  forwardPipe->getFlowDimension()->setName("Forward");
  backwardPipe->getFlowDimension()->setName("Backward");
  tankPipe->getFlowDimension()->setName("Tank");
 
  relief->connect(pumpPipe);
 
  // The class to use then a spool valve is needed is the SpoolValve.
  // It manages a collection of internal units and connectors that are
  // manipulated based on the operations performed on the spool valve.
  agxHydraulics::SpoolValveRef spool = new agxHydraulics::SpoolValve();
 
  // Connect the spool valve to the pipes in the same way as Connectors are.
  // The diagram in the initial comment is used as a guide to get the sides
  // right.
  spool->connect(INPUT, OUTPUT, pumpPipe);
  spool->connect(OUTPUT, INPUT, forwardPipe);
  spool->connect(OUTPUT, INPUT, backwardPipe);
  spool->connect(INPUT, INPUT, tankPipe);
 
  // The forward and backward pipes are connected using a regular pipe coupling.
  // Notice that they are connected OUTPUT to OUTPUT.
  agxHydraulics::FlowConnectorRef coupling = new agxHydraulics::FlowConnector();
  coupling->connect(INPUT, OUTPUT, forwardPipe);
  coupling->connect(OUTPUT, OUTPUT, backwardPipe);
 
  // We have now created the circuit, but not yet configured the spool valve.
  // We want a four way/two position spool valve, but the SpoolValve
  // is more general than that. It can create arbitrary connection patterns
  // between the connected units. The default connection pattern is the empty
  // set, i.e., no unit is connected to any other unit. If we simulate this
  // system we will get no flow.
  simulation->stepTo(simulation->getTimeStamp() + agx::Real(1));
  std::cout << "= Flow is blocked at spool valve." << std::endl;
  std::cout << "  Pump flow rate:     " << pumpPipe->getFlowRate() << std::endl;
  std::cout << "  Forward flow rate:  " << forwardPipe->getFlowRate() << std::endl;
  std::cout << "  Backward flow rate: " << backwardPipe->getFlowRate() << std::endl;
  std::cout << "  Tank flow rate:     " << tankPipe->getFlowRate() << std::endl;
 
  // A coupling between two units in a spool valve is called a link. A link is
  // created by calling SpoolValve::link and passing in the two units
  // that should be linked.
  spool->link(pumpPipe, forwardPipe);
  spool->link(tankPipe, backwardPipe);
 
  // The spool valve have now been configured for forward operation of the
  // circuit, visualized as the parallel lines in the diagram. Stepping the
  // simulation now will produce positive flow in the forward pipe and negative
  // flow in the backward pipe.
  simulation->stepTo(simulation->getTimeStamp() + agx::Real(3));
  std::cout << "= Forward flow." << std::endl;
  std::cout << "  Pump flow rate:     " << pumpPipe->getFlowRate() << std::endl;
  std::cout << "  Forward flow rate:  " << forwardPipe->getFlowRate() << std::endl;
  std::cout << "  Backward flow rate:" << backwardPipe->getFlowRate() << std::endl;
  std::cout << "  Tank flow rate:     " << tankPipe->getFlowRate() << std::endl;
 
  // To reverse the flow through the circuit we swap the links, linking the
  // pump to the backward pipe instead, and the tank to the forward pipe.
  //
  // Simply calling link twice again will not produce the wanted behavior.
  // Links are not one-to-one mappings, but many-to-many. When a already linked
  // unit is linked to a third unit then the new unit is included in the same,
  // pre-existing link set. The effect is the same as if the three units had
  // been connected using a FlowConnector. In our case, if we had issued the
  // two link calls now then we would create a fully connected junction.
  //
  // To reconfigure the spool valve from scratch we must first unlink
  // everything. Here it is enough to unlink the pump and the tank pipes
  // because a unit that becomes the sole member of a link set will be unlinked
  // automatically.
  spool->unlink(pumpPipe);
  spool->unlink(tankPipe);
 
  // Now the spool valve is in the same state as initially. Stepping will stop
  // all flow.
  simulation->stepTo(simulation->getTimeStamp() + agx::Real(3));
  std::cout << "= Flow is blocked at spool valve." << std::endl;
  std::cout << "  Pump flow rate:     " << pumpPipe->getFlowRate() << std::endl;
  std::cout << "  Forward flow rate:  " << forwardPipe->getFlowRate() << std::endl;
  std::cout << "  Backward flow rate: " << backwardPipe->getFlowRate() << std::endl;
  std::cout << "  Tank flow rate:     " << tankPipe->getFlowRate() << std::endl;
 
 
  // We can now configure the spool valve for backward operation by linking the
  // units according to the crossed lines in the diagram.
  spool->link(pumpPipe, backwardPipe);
  spool->link(tankPipe, forwardPipe);
 
  // Now, after simulating a while, we will get positive flow in the backward
  // pipe and negative flow in the forward pipe. We call this the backward
  // operation of the circuit.
  simulation->stepTo(simulation->getTimeStamp() + agx::Real(3));
  std::cout << "= Backward flow." << std::endl;
  std::cout << "  Pump flow rate:     " << pumpPipe->getFlowRate() << std::endl;
  std::cout << "  Forward flow rate: " << forwardPipe->getFlowRate() << std::endl;
  std::cout << "  Backward flow rate: " << backwardPipe->getFlowRate() << std::endl;
  std::cout << "  Tank flow rate:     " << tankPipe->getFlowRate() << std::endl;
 
 
  // The spool valve is not limited to one-to-one or input-to-output couplings.
  // By unlinking forward pipe and linking the pump to the tank we get the
  // following configuration.
  //
  //   -----------      =========    --------------
  //   | Pump => |------|-\   |-|----| Forward => |------
  //   -----------      |  \    |    --------------     |
  //                    |   |   |                       |
  //   -----------      |   |   |    ---------------    |
  //   | <= Tank |------|-------|----| Backward => |-----
  //   -----------      =========    ---------------
  //
  // With this layout, the fluid flowing from the pump is split and can go
  // either to the tank or to the backward pipe. However, the forward pipe is
  // blocked at the spool valve so the backward pipe cannot accept any flow.
  // All flow from the pump goes immediately to tank.
  spool->unlink(forwardPipe);
  spool->link(tankPipe, backwardPipe);
 
  simulation->stepTo(simulation->getTimeStamp() + agx::Real(3));
  std::cout << "= Flow to tank." << std::endl;
  std::cout << "  Pump flow rate:     " << pumpPipe->getFlowRate() << std::endl;
  std::cout << "  Forward flow rate:  " << forwardPipe->getFlowRate() << std::endl;
  std::cout << "  Backward flow rate: " << backwardPipe->getFlowRate() << std::endl;
  std::cout << "  Tank flow rate:     " << tankPipe->getFlowRate() << std::endl;
 
  // It is recommended that that helper classes are created for specific types
  // of spool valves to simplify usage. An example of that is given in the next
  // tutorial.
}
 
 
 
namespace tutorial_5
{
  class FourWayThreePositionSpoolValve
  {
    public:
      FourWayThreePositionSpoolValve(
          agxHydraulics::FlowUnit* in1, agxPowerLine::Side in1Side,
          agxHydraulics::FlowUnit* in2, agxPowerLine::Side in2Side,
          agxHydraulics::FlowUnit* out1, agxPowerLine::Side out1Side,
          agxHydraulics::FlowUnit* out2, agxPowerLine::Side out2Side) :
        m_in1(in1), m_in2(in2),
        m_out1(out1), m_out2(out2),
        m_spool(new agxHydraulics::SpoolValve())
      {
        bool connected = true;
        connected &= m_spool->connect(agxPowerLine::INPUT, in1Side, in1);
        connected &= m_spool->connect(agxPowerLine::INPUT, in2Side, in2);
        connected &= m_spool->connect(agxPowerLine::OUTPUT, out1Side, out1);
        connected &= m_spool->connect(agxPowerLine::OUTPUT, out2Side, out2);
        agxVerify(connected);
 
        this->linkNone();
      }
 
      void linkNone()
      {
        m_spool->unlink(m_in1);
        m_spool->unlink(m_in2);
        agxVerify(m_spool->getNumLinks() == size_t(0));
      }
 
      void linkParallel()
      {
        this->linkNone();
        m_spool->link(m_in1, m_out1);
        m_spool->link(m_in2, m_out2);
        agxVerify(m_spool->getNumLinks() == size_t(2));
      }
 
      void linkCross()
      {
        this->linkNone();
        m_spool->link(m_in1, m_out2);
        m_spool->link(m_in2, m_out1);
        agxVerify(m_spool->getNumLinks() == size_t(2));
      }
 
    private:
      agxHydraulics::FlowUnit* m_in1;
      agxHydraulics::FlowUnit* m_in2;
      agxHydraulics::FlowUnit* m_out1;
      agxHydraulics::FlowUnit* m_out2;
      agxHydraulics::SpoolValveRef m_spool;
  };
}
 
 
void runTutorial5()
{
  std::cout << "\nTutorial 5: Flow control using wrapped spool valve." << std::endl;
 
  agxSDK::SimulationRef simulation = new agxSDK::Simulation();
 
  //using namespace agxPowerLine::Side;
  agxPowerLine::Side INPUT = agxPowerLine::INPUT;
  agxPowerLine::Side OUTPUT = agxPowerLine::OUTPUT;
 
  // Some pipe parameters.
  agx::Real pipeLength(1);
  agx::Real pipeArea(agxHydraulics::utils::diameterToArea(0.05));
  agx::Real fluidDensity(800);
 
  // We start with a basic setup where a constant flow valve supplies the flow.
  agxPowerLine::PowerLineRef powerLine = new agxPowerLine::PowerLine();
  simulation->add(powerLine);
 
  agx::Real targetFlowRate(0.001);
  agxHydraulics::ConstantFlowValveRef pump = new agxHydraulics::ConstantFlowValve(pipeLength, pipeArea, fluidDensity, targetFlowRate);
 
  pump->getFlowDimension()->setName("PressureSource");
 
  pump->setAllowPumping(true);
  powerLine->add(pump);
 
  agx::Real crackingPressure(1e3);
  agx::Real fullyOpenPressure(1.1e3);
  agxHydraulics::ReliefValveRef relief = new agxHydraulics::ReliefValve(crackingPressure, fullyOpenPressure, pipeArea);
  pump->connect(relief);
 
  // The rest of the pipes are created.
  agxHydraulics::PipeRef pumpPipe = new agxHydraulics::Pipe(pipeLength, pipeArea, fluidDensity);
  agxHydraulics::PipeRef forwardPipe = new agxHydraulics::Pipe(pipeLength, pipeArea, fluidDensity);
  agxHydraulics::PipeRef backwardPipe = new agxHydraulics::Pipe(pipeLength, pipeArea, fluidDensity);
  agxHydraulics::PipeRef tankPipe = new agxHydraulics::Pipe(pipeLength, pipeArea, fluidDensity);
 
  pumpPipe->getFlowDimension()->setName("Pump");
  forwardPipe->getFlowDimension()->setName("Forward");
  backwardPipe->getFlowDimension()->setName("Backward");
  tankPipe->getFlowDimension()->setName("Tank");
 
  relief->connect(pumpPipe);
 
  tutorial_5::FourWayThreePositionSpoolValve spool(
        pumpPipe, OUTPUT,
        tankPipe, INPUT,
        forwardPipe, INPUT,
        backwardPipe, INPUT);
 
  agxHydraulics::FlowConnectorRef coupling = new agxHydraulics::FlowConnector();
  coupling->connect(INPUT, OUTPUT, forwardPipe);
  coupling->connect(OUTPUT, OUTPUT, backwardPipe);
 
  // Stepping with an closed spool valve. No flow is possible.
  simulation->stepTo(simulation->getTimeStamp() + agx::Real(3));
  std::cout << "= Flow blocked at spool valve." << std::endl;
  std::cout << "  Pump flow rate:      " << pumpPipe->getFlowRate() << std::endl;
  std::cout << "  Forward flow rate:   " << forwardPipe->getFlowRate() << std::endl;
  std::cout << "  Backward flow rate:  " << backwardPipe->getFlowRate() << std::endl;
  std::cout << "  Tank flow rate:      " << tankPipe->getFlowRate() << std::endl;
 
  // Stepping with the spool valve in the parallel mode. Produces positive flow
  // in the circuit.
  spool.linkParallel();
  simulation->stepTo(simulation->getTimeStamp() + agx::Real(3));
  std::cout << "= Forward flow." << std::endl;
  std::cout << "  Pump flow rate:      " << pumpPipe->getFlowRate() << std::endl;
  std::cout << "  Forward flow rate:   " << forwardPipe->getFlowRate() << std::endl;
  std::cout << "  Backward flow rate: " << backwardPipe->getFlowRate() << std::endl;
  std::cout << "  Tank flow rate:      " << tankPipe->getFlowRate() << std::endl;
 
  // Stepping with closed spool valve again. No flow is possible.
  spool.linkNone();
  simulation->stepTo(simulation->getTimeStamp() + agx::Real(3));
  std::cout << "= Flow blocked at spool valve." << std::endl;
  std::cout << "  Pump flow rate:      " << pumpPipe->getFlowRate() << std::endl;
  std::cout << "  Forward flow rate:   " << forwardPipe->getFlowRate() << std::endl;
  std::cout << "  Backward flow rate:  " << backwardPipe->getFlowRate() << std::endl;
  std::cout << "  Tank flow rate:      " << tankPipe->getFlowRate() << std::endl;
 
  // Stepping with the spool valve in the crossed mode. Produces negative flow
  // in the circuit.
  spool.linkCross();
  simulation->stepTo(simulation->getTimeStamp() + agx::Real(3));
  std::cout << "= Backward flow." << std::endl;
  std::cout << "  Pump flow rate:      " << pumpPipe->getFlowRate() << std::endl;
  std::cout << "  Forward flow rate:  " << forwardPipe->getFlowRate() << std::endl;
  std::cout << "  Backward flow rate:  " << backwardPipe->getFlowRate() << std::endl;
  std::cout << "  Tank flow rate:      " << tankPipe->getFlowRate() << std::endl;
 
}
 
 
int main(int /*argc*/, char** /*argv*/)
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial Hydraulics\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
 
  runTutorial1();
  runTutorial2();
  runTutorial3();
  runTutorial4();
  runTutorial5();
 
  return 0;
}

tutorial_hydraulics_coupling.cpp

/*
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.
*/
 
 
/*
 
This is a demo of how to couple an external simulation to AGX hydraulics and
power line. The scene contains three main parts: an engine, a hydraulic
cylinder, and a propeller. The hydraulics is fully inside AGX, but the engine
and propeller is shared between AGX and an external simulation. The external
simulation provides torques and effective inertias for the shared bodies and
AGX replies with the resulting angular velocities of those bodies.
 
The code consists of a number of parts: interface classes, external
simulation, scene setup, hydraulic cylinder creation, operator event
scheduling, and data collection specification. Search for these names in ALL
CAPS to find the code in question. Each name may appear multiple times.
 
The example creates two data files on disk: 'dimensions.dot' and
'plotData.dat'. The first one shows a graphical layout of the power line
system, including hydraulics, and the second is a recording of the data that
is printed on screen.
 
*/
 
#include <agxSDK/Simulation.h>
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/Node.h>
#include <agxOSG/utils.h>
 
#include <agx/BallJoint.h>
#include <agx/DistanceJoint.h>
#include <agx/Hinge.h>
#include <agx/LockJoint.h>
#include <agx/Prismatic.h>
#include <agx/RigidBody.h>
#include <agx/version.h>
 
#include <agxCollide/Geometry.h>
#include <agxCollide/Box.h>
 
#include <agxDriveTrain/Clutch.h>
#include <agxDriveTrain/deprecated/HighLevelDriveTrain.h>
 
#include <agxPowerLine/StepEventDotGraphWriter.h>
 
#include <agxHydraulics/ReliefValve.h>
#include <agxHydraulics/PistonActuator.h>
#include <agxHydraulics/hydraulicUtils.h>
#include <agxHydraulics/VariableDisplacementPump.h>
 
#include <agxControl/Action.h>
 
#include <agxUnit/UnitMacros.h>
 
#include "HydraulicUtils.h"
 
 
 
// INTERFACE CLASSES
/*
 * External interface classes. These are used by the external simulation to
 * communicate with AGX. Their purpose is only to make the interface clearer in
 * the example, an application may of course call methods on the various components
 * directly.
 */
 
 
class RotationalUnitInterface
{
  public:
    void configure(agx::Real torque, agx::Real inertia);
 
    agx::Real getAngularVelocity() const;
 
  protected:
    RotationalUnitInterface(agxPowerLine::RotationalUnit* unit);
 
  private:
    agxPowerLine::RotationalUnitRef m_unit;
};
 
 
class EngineInterface : public RotationalUnitInterface
{
  public:
    EngineInterface(agxDriveTrain::PidControlledEngine* engine);
};
 
 
class PropellerInterface : public RotationalUnitInterface
{
  public:
    PropellerInterface(agxDriveTrain::deprecated::FixedGear* propeller);
};
 
 
class ClutchInterface
{
  public:
    ClutchInterface(agxDriveTrain::deprecated::Clutch* clutch);
 
    void configure(agx::Real engagement);
 
    agxDriveTrain::deprecated::Clutch* getClutchConnector( );
 
  private:
    agxDriveTrain::deprecated::ClutchRef m_clutch;
};
 
 
class GearBoxInterface
{
  public:
    GearBoxInterface(agxDriveTrain::deprecated::HighLevelGearBox* gearBox);
 
    void configure(agx::Real gear);
 
    agxDriveTrain::deprecated::HighLevelGearBox* getGearBox();
 
  private:
    agxDriveTrain::deprecated::HighLevelGearBoxRef m_gearBox;
};
 
 
 
// EXTERNAL SIMULATION
 
class External : public agxSDK::StepEventListener
{
  public:
    External(agxSDK::Simulation* simulation,
      EngineInterface engine, PropellerInterface propeller,
      ClutchInterface pumpClutch, GearBoxInterface pumpGearBox,
      ClutchInterface propellerClutch, GearBoxInterface propellerGearBox);
 
    virtual void pre(const agx::TimeStamp& time) override;
    virtual void post(const agx::TimeStamp& time) override;
 
  protected:
    virtual void updateEngineAgxState();
 
    virtual agx::Real calculateEngineTorque();
 
    virtual agx::Real calculateEngineInertia();
 
    virtual void collectEngineAgxState();
 
 
    virtual void updatePropellerAgxState();
 
    virtual agx::Real calculatePropellerTorque();
 
    virtual agx::Real calculatePropellerInertia();
 
    virtual void collectPropellerAgxState();
 
  private:
    EngineInterface m_engine;
    PropellerInterface m_propeller;
 
    ClutchInterface m_pumpClutch;
    GearBoxInterface m_pumpGearBox;
 
    ClutchInterface m_propellerClutch;
    GearBoxInterface m_propellerGearBox;
 
    agx::Real m_lastEngineVelocity;
 
    // The angular velocity that AGX produced for the propeller during the previous time step.
    agx::Real m_lastPropellerVelocity;
};
 
 
 
 
 
 
/*
 * Scene construction classes. A bunch of setup code that creates the scene, with
 * all the bodies and hydraulics and power line and such.
 */
 
 
 
struct Foundation
{
  agxSDK::Simulation* simulation;
  agxOSG::ExampleApplication* application;
  osg::Group* root;
  StatsPrinter* printer;
  bool writeToDisk;
};
 
 
class Scene
{
  public:
    Scene();
    void construct(const Foundation& foundation);
 
  public:
    agxPowerLine::PowerLineRef powerLine;
 
    // Power generation.
    agxDriveTrain::PidControlledEngineRef engine;
 
    // Hydraulics path.
    agxDriveTrain::deprecated::ClutchRef pumpClutch;
    agxDriveTrain::deprecated::HighLevelGearBoxRef pumpGearBox;
    agxHydraulics::VariableDisplacementPumpRef pump;
    agxHydraulics::ReliefValveRef relief;
    agxHydraulics::PipeRef pumpPipe;
    agxHydraulics::PipeRef tankPipe;
    agxHydraulics::PistonActuatorRef piston;
 
    // Mechanical system.
    agx::RigidBodyRef base;
    agx::RigidBodyRef lowerArm;
    agx::RigidBodyRef upperArm;
    agx::HingeRef baseJoint;
    agx::HingeRef armJoint;
 
    // Propeller path.
    agxDriveTrain::deprecated::ClutchRef propellerClutch;
    agxDriveTrain::deprecated::HighLevelGearBoxRef propellerGearBox;
    agxDriveTrain::deprecated::FixedGearRef propeller;
 
    // Hydraulics parameters. Configure these in the Scene constructor code.
    agx::Real pipeLength;
    agx::Real pipeArea;
    agx::Real fluidDensity;
    agx::Real barrelArea;
    agx::Real pistonArea;
 
    // Mechanics parameters. Configure these in the Scene constructor code.
    agx::Real baseHe;
    agx::Real baseAttachOffset;
    agx::Real armHalfLength;
    agx::Real armHalfWidth;
    agx::Vec3 armHe;
 
 
  private:
    void constructMechanics(const Foundation& foundation);
 
    void constructPowerLine(const Foundation& foundation);
};
 
 
 
class DistanceScene
{
  public:
    DistanceScene(const Foundation& foundation);
 
  private:
    Scene m_scene;
    agx::DistanceJointRef m_distanceJoint;
};
 
 
class PrismaticScene
{
  public:
    PrismaticScene(const Foundation& foundation);
 
  private:
    Scene m_scene;
    agx::RigidBodyRef m_upperAttachBody;
    agx::RigidBodyRef m_lowerAttachBody;
    agx::BallJointRef m_upperBall;
    agx::BallJointRef m_lowerBall;
    agx::LockJointRef m_upperLock;
    agx::LockJointRef m_lowerLock;
    agx::PrismaticRef m_prismatic;
};
 
 
 
agx::RigidBodyRef createBox(const agx::Vec3& halfExtents, const agx::Vec3& position, const char* name, const Foundation& foundation)
{
  agx::RigidBodyRef body = new agx::RigidBody(name);
  body->add(new agxCollide::Geometry(new agxCollide::Box(halfExtents)));
  body->setPosition(position);
  agxOSG::createVisual(body, foundation.root);
  foundation.simulation->add(body);
  return body;
}
 
 
 
 
/*
 * Implementation of external interface classes.
 */
 
 
RotationalUnitInterface::RotationalUnitInterface(agxPowerLine::RotationalUnit* unit) : m_unit(unit)
{
}
 
 
void RotationalUnitInterface::configure(agx::Real torque, agx::Real inertia)
{
  // The way external torque is applied depends on whether the unit is an engine or not.
  agxDriveTrain::Engine* engine = dynamic_cast<agxDriveTrain::Engine*>(m_unit.get());
  if (engine != nullptr) {
    // We have a power generator, i.e. this is an engine, so pass the wanted torque
    // to it and let the power line implementation handle the rest.
    agxPowerLine::PowerGenerator* powerGenerator = engine->getPowerGenerator();
    agxPowerLine::LookupTable* curve = powerGenerator->getPowerTimeIntegralLookupTable();
    curve->clear();
    // The 0, RPM when the engine should produce the given torque, doesn't matter
    // since we only add a single value. The given torque will be applied unconditionally..
    curve->insertValue(agx::Real(0), torque);
  } else {
    // This is not an engine, so we just add a load directly to the RotationalDimension.
    agxPowerLine::RotationalDimension* dimension = m_unit->getRotationalDimension();
    dimension->addLoad(torque);
  }
 
  // We only pass a single scalar here. The power line framework will find the
  // full inertia tensor that produces the provided inerta when rotated around
  // the rotational unit's axis of rotation.
  agxPowerLine::RotationalDimension* dimension = m_unit->getRotationalDimension();
  dimension->setInertia(inertia);
}
 
 
agx::Real RotationalUnitInterface::getAngularVelocity() const
{
  // A PhysicalDimension, which RotationalDimension inherits from, defines a value
  // that represents the dimension's current position. The value for a RotationalDimension
  // is the current angle. The gradient of the angle is the angular velocity.
  return m_unit->getRotationalDimension()->getGradient();
}
 
 
 
 
EngineInterface::EngineInterface(agxDriveTrain::PidControlledEngine* engine) :
    RotationalUnitInterface(engine)
{
}
 
 
PropellerInterface::PropellerInterface(agxDriveTrain::deprecated::FixedGear* propeller) :
    RotationalUnitInterface(propeller)
{
}
 
 
 
ClutchInterface::ClutchInterface(agxDriveTrain::deprecated::Clutch* clutch) : m_clutch(clutch)
{
}
 
 
void ClutchInterface::configure(agx::Real engagement)
{
  // This is a work-around because of a bug in the AGX clutch. Here we try to
  // map the [0..1] engagement range to some sane constraint compliance range.
  agx::Real looseness = agx::Real(1) - engagement;
  agx::Real complianceAtLoose(1E2);
  agx::Real compliance = complianceAtLoose * std::pow(looseness, agx::Real(10));
  m_clutch->setEfficiency(std::max(compliance, agx::RealEpsilon));
}
 
 
agxDriveTrain::deprecated::Clutch* ClutchInterface::getClutchConnector( )
{
  return m_clutch;
}
 
 
GearBoxInterface::GearBoxInterface(agxDriveTrain::deprecated::HighLevelGearBox* gearBox) : m_gearBox(gearBox)
{
}
 
 
void GearBoxInterface::configure(agx::Real gear)
{
  // The 'gear' argument is a real because of limitations of the AGX action
  // scheduling system.
  m_gearBox->setGear((int)gear);
}
 
 
agxDriveTrain::deprecated::HighLevelGearBox* GearBoxInterface::getGearBox()
{
  return m_gearBox;
}
 
 
 
// OPERATOR EVENT SCHEDULING
 
void createClutchEvent(
  agxSDK::Simulation* simulation,
  agx::Real time,
  agx::Real fraction,
  ClutchInterface* clutch)
{
  // Create an Action that runs the ClutchInterface::configure method at the
  // given simulation time, passing in the given clutch setting.
  agxControl::Callback1ActionImplementation<agx::Real>* configureClutch =
    new agxControl::Callback1ActionImplementation<agx::Real>(
      time, &ClutchInterface::configure, clutch, fraction);
  // Read the last line as "At time 'time', call 'ClutchInterface::configure'
  // on the object pointed to by 'clutch', and pass the value 'fraction' as the
  // only argument.".
 
  simulation->add(new agxControl::CallbackAction(configureClutch));
}
 
 
void createGearChangeEvent(
  agxSDK::Simulation* simulation,
  agx::Real time,
  agx::Real gear,
  GearBoxInterface* gearBox)
{
  // See comments in createClutchEvent. The workings are similar.
  agxControl::Callback1ActionImplementation<agx::Real>* setGear =
    new agxControl::Callback1ActionImplementation<agx::Real>(
      time, &GearBoxInterface::configure, gearBox, gear);
 
  simulation->add(new agxControl::CallbackAction(setGear));
}
 
 
 
void buildEventList(agxSDK::Simulation* simulation,
  ClutchInterface* pumpClutch, ClutchInterface* propellerClutch,
  GearBoxInterface* /*pumpGearBox*/, GearBoxInterface* /*propellerGearBox*/)
{
 
  // Currently we do not switch gear, need a relief valve to reduce pressure for the hydraulic system. Otherwise spikes will appear
 
  agx::Real time;
 
  // Enable clutch for pump at 10s.
  time = agx::Real(10.0);
  createClutchEvent(simulation, time, agx::Real(1), pumpClutch);
 
  // Switch gear at 13s.
  //time = agx::Real(13);
  //createGearChangeEvent(simulation, time, agx::Real(1), pumpGearBox);
 
 
 
  // Enable clutch for propeller at 20s.
  time = agx::Real(20.0);
  createClutchEvent(simulation, time, agx::Real(1), propellerClutch);
 
  // Switch gear for propeller at 20s.
  // time = agx::Real(20);
  //createGearChangeEvent(simulation, time, agx::Real(1), propellerGearBox);
}
 
 
 
 
 
 
// EXTERNAL SIMULATION
 
External::External(agxSDK::Simulation* simulation,
    EngineInterface engine, PropellerInterface propeller,
    ClutchInterface pumpClutch, GearBoxInterface pumpGearBox,
    ClutchInterface propellerClutch, GearBoxInterface propellerGearBox)
:
    agxSDK::StepEventListener(PRE_STEP | POST_STEP),
    m_engine(engine), m_propeller(propeller),
    m_pumpClutch(pumpClutch), m_pumpGearBox(pumpGearBox),
    m_propellerClutch(propellerClutch), m_propellerGearBox(propellerGearBox),
    m_lastEngineVelocity(0), m_lastPropellerVelocity(0)
{
  buildEventList(simulation, &m_pumpClutch, &m_propellerClutch, &m_pumpGearBox, &m_propellerGearBox);
}
 
 
void External::pre(const agx::TimeStamp& /*time*/)
{
  // Before the time step we run the external simulation and pass the resulting
  // torques and inertias to AgX. This section can be replaced by any kind of
  // simulation, as long as the Interface classe's 'configure' methods are called
  // at the end.
  this->updateEngineAgxState();
  this->updatePropellerAgxState();
}
 
 
void External::post(const agx::TimeStamp& /*time*/)
{
  // After the time step we read the resuling angular velocities.
  this->collectEngineAgxState();
  this->collectPropellerAgxState();
}
 
 
void External::updateEngineAgxState()
{
  // Run the external engine simulation.
  agx::Real newTorque = this->calculateEngineTorque();
  agx::Real newInertia = this->calculateEngineInertia();
 
  // Pass the new torque and effective inertia for the engine to AgX.
  m_engine.configure(newTorque, newInertia);
}
 
 
agx::Real External::calculateEngineTorque()
{ // EXTERNAL SIMULATION
  // An example of an external simulation. A simple RPM controller that applies
  // maximum torque until the engine is approaching the target angular velocity.
  // There is a linear drop-off in torque as the velocity approaches the target
  // velocity and zero torque when faster than the target.
 
  // The maximum torque that the engine can deliver.
  agx::Real maxTorque(1000);
 
  // The velocity at which the delivered toque starts to decrease.
  agx::Real cutoffVelocity(100);
 
  // The maximum velocity. No toque will be delivered when the engine is rotating
  // faster than this.
  agx::Real maxVelocity(150);
 
  if (m_lastEngineVelocity < cutoffVelocity)
    return maxTorque;
 
  // Linear interpolation of (maxTorque->0) as velocity goes (cutoffVelocity->maxVelocity).
  agx::Real cutoffOvershoot = m_lastEngineVelocity - cutoffVelocity;
  agx::Real cutoffRange = maxVelocity - cutoffVelocity;
  agx::Real progress = agx::clamp(cutoffOvershoot / cutoffRange, agx::Real(0), agx::Real(1));
  return maxTorque - progress*maxTorque;
}
 
 
agx::Real External::calculateEngineInertia()
{ // EXTERNAL SIMULATION
  // An example of a very simple external simulation computing the effective inertia
  // of the engine.
  return agx::Real(20);
}
 
 
void External::collectEngineAgxState()
{ // EXTERNAL SIMULATION
  // Read the resulting engine velocity produced by AGX and store it in preparation
  // for the next external simulation step.
  m_lastEngineVelocity = m_engine.getAngularVelocity();
}
 
 
void External::updatePropellerAgxState()
{
  // Run the external propeller simulation.
  agx::Real newTorque = this->calculatePropellerTorque();
  agx::Real newInertia = this->calculatePropellerInertia();
 
  // Pass the new torque and effective inertia for the propeller to AgX.
  m_propeller.configure(newTorque, newInertia);
}
 
 
agx::Real External::calculatePropellerTorque()
{ // EXTERNAL SIMULATION
  // An example of a counter torque simulation. The resistance to rotation produced
  // by the propeller is some quadratic function on the propeller angular velocity.
  agx::Real scale(3e3);
  return -scale * m_lastPropellerVelocity * m_lastPropellerVelocity;
}
 
 
agx::Real External::calculatePropellerInertia()
{ // EXTERNAL SIMULATION
  // An example of an effective inertia calculation. Effective inertia increases
  // linearly with propeller angular velocity.
  agx::Real baseInertia(500);
  agx::Real scale(3);
  return baseInertia + scale*m_lastPropellerVelocity;
}
 
 
void External::collectPropellerAgxState()
{ // EXTERNAL SIMULATION
  // Read the resulting propeller velocity produced by AGX and store it in preparation
  // for the next external simulation step.
  m_lastPropellerVelocity = m_propeller.getAngularVelocity();
}
 
 
 
/*
 * Implementation of scene construction classes.
 */
 
 
Scene::Scene() :
  pipeLength(1),
  pipeArea(agxHydraulics::utils::diameterToArea(0.05)),
  fluidDensity(800),
  barrelArea(agxHydraulics::utils::diameterToArea(0.1)),
  pistonArea(agxHydraulics::utils::diameterToArea(0.08)),
  baseHe(0.5),
  baseAttachOffset(agx::Real(0.8)*baseHe),
  armHalfLength(2.0),
  armHalfWidth(0.1),
  armHe(armHalfLength, armHalfWidth, armHalfWidth)
{
}
 
 
void Scene::construct(const Foundation& foundation)
{
  foundation.printer->addCustomGauge(std::bind(&agxSDK::Simulation::getTimeStamp, foundation.simulation), "Time", "s");
 
  this->constructMechanics(foundation);
  this->constructPowerLine(foundation);
 
  External* external = new External(foundation.simulation,
    EngineInterface(engine), PropellerInterface(propeller),
    ClutchInterface(pumpClutch), GearBoxInterface(pumpGearBox),
    ClutchInterface(propellerClutch), GearBoxInterface(propellerGearBox));
  foundation.simulation->add(external);
}
 
 
void Scene::constructMechanics(const Foundation& foundation)
{ // SCENE SETUP
 
  // Create a static body that the crane is attached to.
  base = createBox(agx::Vec3(baseHe), agx::Vec3(), "Base", foundation);
  base->setMotionControl(agx::RigidBody::STATIC);
 
  // Create the lower vertical segment of the crane.
  lowerArm = createBox(armHe, agx::Vec3(baseAttachOffset, agx::Real(0), baseAttachOffset+armHalfLength), "Lower arm", foundation);
  lowerArm->setRotation(agx::EulerAngles(agx::Real(0), agx::Real(0.5)*agx::PI, agx::Real(0)));
 
  // Create the upper angled segment of the crane.
  agx::Real upperArmAngle(agx::PI_4);
  agx::Vec3 circleCenter(baseAttachOffset, agx::Real(0), baseAttachOffset+agx::Real(2)*armHalfLength);
  agx::Vec3 circleDirection(std::cos(-upperArmAngle), agx::Real(0), std::sin(-upperArmAngle));
  agx::Vec3 upperArmPos = circleCenter + armHalfLength*circleDirection;
  upperArm = createBox(armHe, upperArmPos, "Upper arm", foundation);
  upperArm->setRotation(agx::EulerAngles(agx::Real(0), agx::Real(0.25)*agx::PI, agx::Real(0)));
 
  // Disable collisions for neighboring bodies.
  foundation.simulation->getSpace()->setEnableCollisions(base->getGeometries()[0], lowerArm->getGeometries()[0], false);
  foundation.simulation->getSpace()->setEnableCollisions(lowerArm->getGeometries()[0], upperArm->getGeometries()[0], false);
 
  // Join the two segments of the crane with a hinge.
  agx::FrameRef lowerArmJointFrame = new agx::Frame();
  agx::FrameRef upperArmJointFrame = new agx::Frame();
  agx::Constraint::calculateFramesFromBody(
    agx::Vec3(-armHalfLength, agx::Real(0), agx::Real(0)), agx::Y_AXIS,
    lowerArm, lowerArmJointFrame,
    upperArm, upperArmJointFrame);
  armJoint = new agx::Hinge(lowerArm, lowerArmJointFrame, upperArm, upperArmJointFrame);
  foundation.simulation->add(armJoint);
 
  // Attach the lower segment of the crane to the base with a locked hinge. Future
  // version of this demo may add hydraulic cylinder control of the lower arm
  // as well.
  agx::FrameRef baseJointFrame = new agx::Frame();
  agx::FrameRef baseArmFrame = new agx::Frame();
  agx::Constraint::calculateFramesFromBody(
    agx::Vec3(baseAttachOffset, agx::Real(0), baseAttachOffset), agx::Y_AXIS,
    base, baseJointFrame,
    lowerArm, baseArmFrame);
  baseJoint = new agx::Hinge(base, baseJointFrame, lowerArm, baseArmFrame);
  baseJoint->getLock1D()->setEnable(true);
  foundation.simulation->add(baseJoint);
}
 
 
void Scene::constructPowerLine(const Foundation& foundation)
{
  // SCENE SETUP
 
 /*
  * Power generation creation.
  */
 
 
  // The power line object is the coordinator for the power line. It contains
  // both the engine, hydraulics path, and propeller path.
  powerLine = new agxPowerLine::PowerLine();
  foundation.simulation->add(powerLine);
 
  // Create the engine. We create an empty power curve since the torque produced
  // by the engine is determined by the external simulation.
  engine = new agxDriveTrain::PidControlledEngine();
 
  engine->getRotationalDimension()->setName("Engine");
 
  agxPowerLine::PowerGenerator* powerGenerator = engine->getPowerGenerator();
  agxPowerLine::LookupTable* powerCurve = powerGenerator->getPowerTimeIntegralLookupTable();
  powerCurve->insertValue(agx::Real(0), agx::Real(0));
  engine->setThrottle(agx::Real(1));
  engine->ignition(true);
  powerLine->setSource(engine);
 
 
  /*
   * Hydraulics path creation.
   */
 
  // A clutch is a rotational unit that can loosen the connection between two
  // other rotational units. See the comment in ClutchInterface::configure.
  pumpClutch = new agxDriveTrain::deprecated::Clutch();
  pumpClutch->setEfficiency(agx::Real(1e100));
 
  pumpClutch->getRotationalDimension()->setName("PumpClutch");
 
  // A gear box scales the rotational velocity from input side to the output side
  // according to some gearing ratio. A gear box can have multiple gearing ratios
  // that a user can switch between during runtime.
  pumpGearBox = new agxDriveTrain::deprecated::HighLevelGearBox();
 
  pumpGearBox->getRotationalDimension(agxPowerLine::INPUT)->setName("PumpGearBox");
 
  agx::RealVector pumpGears;
  agx::Real strongGearRatio(100);
  agx::Real weakGearRatio(500);
  pumpGears.push_back(agx::Real(0));
  pumpGears.push_back(agx::Real(strongGearRatio));
  pumpGears.push_back(agx::Real(weakGearRatio));
  pumpGearBox->setGearRatios(pumpGears);
  pumpGearBox->setGear(0);
 
 
  // These are the parameters for the pump.
  agx::Real cutoffPressure(1e6); // The pressure at which the pump efficiency starts to drop significantly.
  agx::Real deadheadPressure(1.1e6); // The pressure at which the pump reaches zero efficiency.
  agx::Real internalLeakage(1e-11); // Control the efficiency loss for pressures below the deadheadPressure.
  agx::Real flowRatio(5e-3); // The conversion ratio from angular velocity to flow rate.
  agx::Real compensatorArea(1e-5); // Control the responsiveness of the efficiency.
 
  pump = new agxHydraulics::VariableDisplacementPump(
    cutoffPressure, deadheadPressure, internalLeakage, flowRatio, compensatorArea);
 
  // Pipes are used to connect hydraulics components to each other.
  pumpPipe = new agxHydraulics::Pipe(pipeLength, pipeArea, fluidDensity);
 
  pumpPipe->getFlowDimension()->setName("Pump pipe");
 
  relief = new agxHydraulics::ReliefValve(agx::Real(1.5e6), agx::Real(1.7e6), pipeArea);
 
  relief->getInputChamber()->getFlowDimension()->setName("Relief valve");
 
  tankPipe = new agxHydraulics::Pipe(pipeLength, pipeArea, fluidDensity);
 
  tankPipe->getFlowDimension()->setName("Tank pipe");
 
  // Connect the components together. Notice how the pump is used as a bridge
  // between the rotational part and the hydraulic part of the power line. We
  // don't create the rest of the hydraulics system here because this demo is
  // intended to contain multiple scenes with varying contents of the hydraulics.
  // The creation of the hydraulics systems is done by the sub-classes of Scene.
  agxVerify(engine->connect(pumpClutch));
  agxVerify(pumpClutch->connect(pumpGearBox));
  agxVerify(pumpGearBox->connect(pump));
  agxVerify(pump->connect(relief));
  agxVerify(relief->connect(pumpPipe));
 
 
  /*
   * Propeller path creation.
   */
 
  propellerClutch = new agxDriveTrain::deprecated::Clutch( );
  propellerClutch->setEfficiency(agx::Real(1e100));
 
  propellerClutch->getRotationalDimension()->setName("PropellerClutch");
 
  propellerGearBox = new agxDriveTrain::deprecated::HighLevelGearBox();
 
  propellerGearBox->getRotationalDimension(agxPowerLine::INPUT)->setName("PropellerGearBox");
 
  agx::RealVector propellerGears;
  propellerGears.push_back(agx::Real(0));
  propellerGears.push_back(agx::Real(10));
  propellerGears.push_back(agx::Real(500));
  propellerGearBox->setGearRatios(propellerGears);
  propellerGearBox->setGear(0);
 
  //
  propeller = new agxDriveTrain::deprecated::FixedGear();
 
  propeller->getRotationalDimension(agxPowerLine::INPUT)->setName("Propeller");
 
 
  agxVerify(engine->connect(propellerClutch));
  agxVerify(propellerClutch->connect(propellerGearBox));
  agxVerify(propellerGearBox->connect(propeller));
 
 
  foundation.printer->addPressureUnit(&agxHydraulics::utils::pascalToPsi, "PSI");
  foundation.printer->addPressureUnit(&agxHydraulics::utils::pascalToBar, "BAR");
  // foundation.printer->setPressurePlotUnit(&agxHydraulics::utils::pascalToPsi, "PSI");
  foundation.printer->setPressurePlotUnit(&agxHydraulics::utils::pascalToBar, "BAR");
 
  foundation.printer->addFlowRateUnit(&agxHydraulics::utils::cubicMetersPerSecondToLitersPerMinute, "l/min");
  foundation.printer->addFlowRateUnit(&agxHydraulics::utils::cubicMetersPerSecondToGallonsPerMinute, "gpm");
  // foundation.printer->setFlowRatePlotUnit(&agxHydraulics::utils::cubicMetersPerSecondToGallonsPerMinute, "gpm");
  foundation.printer->setFlowRatePlotUnit(&agxHydraulics::utils::cubicMetersPerSecondToLitersPerMinute, "l/m");
 
  foundation.printer->addPressureGauge(pump, "Pump");
  foundation.printer->addFlowRateGauge(pumpPipe, "Pump");
  foundation.printer->addCustomGauge(std::bind(&agxPowerLine::TranslationalDimension::getValue, pump->getPoppetDimension()), "PumpActivation", "");
 
  foundation.printer->addCustomGauge(std::bind(&agxDriveTrain::PidControlledEngine::getTorque, engine.get()), "Engine", "Nm");
  foundation.printer->addCustomGauge(std::bind(&agxPowerLine::RotationalDimension::getOutputLoad, engine->getRotationalDimension()), "EngineOutput", "Nm");
  foundation.printer->addCustomGauge(std::bind(&agxPowerLine::RotationalDimension::getGradient, engine->getRotationalDimension()), "Engine", "rpm", &agxHydraulics::utils::radiansPerSecondToRevolutionsPerMinute);
  foundation.printer->addCustomGauge(std::bind(&agxPowerLine::RotationalDimension::getGradient, pumpClutch->getRotationalDimension()), "PumpClutch", "rpm", &agxHydraulics::utils::radiansPerSecondToRevolutionsPerMinute);
  foundation.printer->addCustomGauge(std::bind(&agxPowerLine::RotationalDimension::getGradient, pumpGearBox->getRotationalDimension(agxPowerLine::INPUT)), "PumpGearBox", "rpm", &agxHydraulics::utils::radiansPerSecondToRevolutionsPerMinute);
  foundation.printer->addCustomGauge(std::bind(&agxPowerLine::RotationalDimension::getGradient, propellerClutch->getRotationalDimension()), "PropellerClutch", "rpm", &agxHydraulics::utils::radiansPerSecondToRevolutionsPerMinute);
  foundation.printer->addCustomGauge(std::bind(&agxPowerLine::RotationalDimension::getGradient, propellerGearBox->getRotationalDimension(agxPowerLine::INPUT)), "PropellerGearBox", "rpm", &agxHydraulics::utils::radiansPerSecondToRevolutionsPerMinute);
  foundation.printer->addCustomGauge(std::bind(&agxPowerLine::RotationalDimension::getGradient, propeller->getRotationalDimension(agxPowerLine::INPUT)), "Propeller", "rpm", &agxHydraulics::utils::radiansPerSecondToRevolutionsPerMinute);
  foundation.printer->addCustomGauge(std::bind(&agxPowerLine::RotationalDimension::getInputTorque, propeller->getRotationalDimension(agxPowerLine::INPUT)), "Propeller", "Nm");
 
  // Restore this line to genereate .dot files showing the circuit layout every time step.
  if (foundation.writeToDisk)
  {
    agxPowerLine::StepEventDotGraphWriterRef dotWriter = new agxPowerLine::StepEventDotGraphWriter(powerLine);
    dotWriter->registerUnit(agxHydraulics::FlowDimension::getStaticTypeName().c_str(), "m^3/s");
    foundation.simulation->add(dotWriter);
  }
}
 
 
 
 
 
DistanceScene::DistanceScene(const Foundation& foundation)
{
  // SCENE SETUP
  m_scene.construct(foundation);
 
  m_distanceJoint = new agx::DistanceJoint(
    m_scene.lowerArm, m_scene.armHalfLength*agx::X_AXIS,
    m_scene.upperArm, m_scene.armHalfLength*agx::X_AXIS);
  m_distanceJoint->getRange1D()->setRange(agx::Real(0.5), agx::Real(6.0));
  m_distanceJoint->getRange1D()->setEnable(true);
  m_distanceJoint->getLock1D()->setEnable(false);
  foundation.simulation->add(m_distanceJoint);
 
  m_scene.piston = new agxHydraulics::PistonActuator(
    m_distanceJoint, m_scene.barrelArea, m_scene.pistonArea, m_scene.fluidDensity);
 
  bool success = true;
  m_scene.pumpPipe->connect(m_scene.piston); agxVerify(success);
  m_scene.piston->connect(m_scene.tankPipe); agxVerify(success);
 
  foundation.printer->addFlowRateGauge(m_scene.piston->getRearChamber(), "RearChamber");
  foundation.printer->addFlowRateGauge(m_scene.piston->getFrontChamber(), "FrontChamber");
  foundation.printer->addCustomGauge(std::bind(&agxHydraulics::PistonActuator::getPistonPosition, m_scene.piston), "Piston", "m");
}
 
 
 
PrismaticScene::PrismaticScene(const Foundation& foundation)
{
  // Scene creating cylinder from agx::Prismatic is not yet implemented.
  m_scene.construct(foundation);
}
 
 
 
/*
 * Scene contruction functions.
 */
 
 
 
osg::Group* buildDistanceScene(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  agxIO::ArgumentParser* arguments = application->getArguments();
  const bool isUnitTest = arguments->find("--isFileSanityUnitTest") != -1;
  const bool writeToDisk = !isUnitTest;
  osg::Group* root = new osg::Group();
  StatsPrinter* printer = new StatsPrinter(simulation, application, writeToDisk);
  Foundation foundation = {simulation, application, root, printer, writeToDisk};
  DistanceScene scene(foundation);
  return root;
}
 
 
 
 
int main(int argc, char** argv)
{
  agx::AutoInit agxInit;
 
  LOGGER_WARNING() << LOGGER_STATE( agx::Notify::PRINT_NONE ) << "\n" <<
  "                  " << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
  "      External coupling demo application.\n" <<
  "      -----------------------------------------------------\n\n"  << LOGGER_END();
 
  {
    try {
      agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
      application->addScene( buildDistanceScene, '1' );
      if (application->init(argc, argv)) {
        return application->run();
      }
     }
     catch ( std::exception& e ) {
       std::cerr << "\nCaught exception: " << e.what() << std::endl;
       return 1;
     }
  }
 
 return 0;
}

tutorial_hydrodynamics.cpp

/*
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.
*/
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/PressureFieldRenderer.h>
#include <agxOSG/WireRenderer.h>
 
#include <agxModel/WindAndWaterController.h>
 
#include <agxUtil/TrimeshReaderWriter/TrimeshReaderWriter.h>
 
#include <agxWire/Wire.h>
 
#include <agx/LockJoint.h>
 
#include <agxRender/RenderSingleton.h>
 
#include <agxCollide/Trimesh.h>
#include <agxCollide/Plane.h>
 
#include <agxIO/ImageReaderPNG.h>
#include <agxIO/MeshReader.h>
#include <agxOSG/ReaderWriter.h>
#include "tutorialUtils.h"
 
 
namespace
{
 
 
  agxOSG::GeometryNode* createWaterVisual( agxCollide::Geometry* waterGeometry, osg::Group* root )
  {
    agxOSG::GeometryNode* node = agxOSG::createVisual( waterGeometry, root );
 
    const agx::Vec4f color = agxRender::Color::DeepSkyBlue();
    float alpha( 0.3f );
 
    agxOSG::setDiffuseColor(node, color );
    agxOSG::setAmbientColor(node, agx::Vec4f( 0.2f ) );
    agxOSG::setSpecularColor(node , agx::Vec4f( 0.4f ) );
    agxOSG::setShininess(node, 120.0f );
    agxOSG::setAlpha(node, alpha );
 
    return node;
  }
 
  agxSDK::AssemblyRef createPyramid( agx::UInt numSpheresBase, agx::Real sphereRadius, agx::Material* material, osg::Group* root )
  {
    agxSDK::AssemblyRef assembly = new agxSDK::Assembly();
 
    agx::Real eps = agx::Real( 0.01 );
    agx::Vec3 pos( -sphereRadius * static_cast<agx::Real>(numSpheresBase), 0, sphereRadius );
    for ( agx::UInt dir = 0; dir < 2; ++dir ) {
      for ( agx::UInt o = numSpheresBase; o > 0; --o ) {
        for ( agx::UInt i = o; i > 0; --i ) {
          for ( agx::UInt j = i; j > 0; --j ) {
            agx::RigidBodyRef sphere = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Sphere( sphereRadius ) ) );
            sphere->setPosition( pos );
 
            agxUtil::setBodyMaterial( sphere, material );
            agxOSG::setDiffuseColor( agxOSG::createVisual( sphere, root ), agxRender::Color::OrangeRed() );
 
            assembly->add( sphere );
 
            pos.x() += agx::Real( 2 ) * sphereRadius + eps;
          }
          pos.x()  = sphereRadius * ( -agx::Real( i ) + 1 );
          pos.z() += std::sqrt( agx::Real( 3 ) ) * sphereRadius + eps;
        }
        pos.x()  = sphereRadius * ( -agx::Real( o ) + 1 );
        pos.y() += agx::Real( 2 * int( dir ) - 1 ) * agx::Real( 2 ) * sphereRadius + eps;
        pos.z() = sphereRadius;
      }
      pos.x() = -sphereRadius * static_cast<agx::Real>(numSpheresBase);
      pos.y() = agx::Real( 2 ) * sphereRadius + eps;
    }
 
    return assembly;
  }
 
  void scaleMesh( const agx::Vec3Vector& vertices, const agx::Vec3 scale )
  {
    for ( agx::UInt i = 0, numVertices = vertices.size(); i < numVertices; ++i )
      vertices[ i ] = agx::Vec3::mul( vertices[ i ], scale );
  }
 
  class Printer : public agxSDK::StepEventListener
  {
    public:
      Printer( agxOSG::ExampleApplication* app ) : agxSDK::StepEventListener( PRE_COLLIDE | LAST_STEP ), m_app( app ) {}
 
    protected:
      void preCollide( const agx::TimeStamp& )
      {
        m_timer.reset( true );
      }
 
      void last( const agx::TimeStamp& )
      {
        if ( getSimulation() == nullptr )
          return;
 
        m_timer.stop();
        if ( m_app != nullptr ) {
          // Time in milliseconds.
          agx::Real time = m_timer.getTime();
          m_app->getSceneDecorator()->setText( 0, agx::String::format( "Time to step forward: %6.2f ms (%6.1f fps)",
                                                                       time,
                                                                       agx::Real( 1E3 ) / time ) );
          m_app->getSceneDecorator()->setText( 1, agx::String::format( "Number of objects:    %d",
                                                                       getSimulation()->getDynamicsSystem()->getRigidBodies().size() ) );
        }
      }
 
    protected:
      agx::Timer m_timer;
      agx::observer_ptr< agxOSG::ExampleApplication > m_app;
  };
}
 
osg::Group* buildTutorial1( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  // Create material to define the density of water.
  agx::MaterialRef waterMaterial = new agx::Material( "waterMaterial" );
  waterMaterial->getBulkMaterial()->setDensity( agx::Real( 1000 ) );
 
  // Let the water geometry to be an box of size 60x60x30 meters.
  agxCollide::GeometryRef waterGeometry = new agxCollide::Geometry( new agxCollide::Box( 30, 30, 15 ) );
  // Surface of water at z = 0.
  waterGeometry->setPosition( 0, 0, -15 );
  waterGeometry->setMaterial( waterMaterial );
  ::createWaterVisual( waterGeometry, root );
 
  // Mark as water geometry. Contacts will be disabled for the water geometry, i.e.,
  // no "normal" contacts will be created, only hydrodynamic forces.
  agxModel::WindAndWaterControllerRef controller = new agxModel::WindAndWaterController();
  controller->addWater( waterGeometry );
  simulation->add( controller );
 
  simulation->add( waterGeometry );
 
  // Sphere density at 400 kg/m^3.
  agx::MaterialRef spheresMaterial = new agx::Material( "spheresMaterial" );
  spheresMaterial->getBulkMaterial()->setDensity( agx::Real( 400 ) );
 
  agxSDK::AssemblyRef pyramid = ::createPyramid( 3, agx::Real( 0.25 ), spheresMaterial, root );
  pyramid->setPosition( 0, 0, 4 );
  simulation->add( pyramid );
 
  // Choose ITERATIVE solver for contacts between the spheres - less exact, but faster
  // (contacts are not main focus here, but hydrodynamics).
  // Note also: these contact material settings only apply for contacts between rigid bodies,
  // not for interaction between rigid bodies and wind or water -
  // for these, there are hydrodynamic and aerodynamic parameters (introduced later in this file in scene 3).
  agx::ContactMaterialRef sphereSphereContactMaterial = simulation->getMaterialManager()->getOrCreateContactMaterial( spheresMaterial, spheresMaterial );
  sphereSphereContactMaterial->setFrictionModel( new agx::IterativeProjectedConeFriction( agx::FrictionModel::ITERATIVE ) );
 
  simulation->add( new Printer( application ) );
 
  return root;
}
 
osg::Group* buildTutorial2( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  agx::MaterialRef waterMaterial = new agx::Material( "waterMaterial" );
  waterMaterial->getBulkMaterial()->setDensity( agx::Real( 1000 ) );
 
  agxCollide::GeometryRef waterGeometry = new agxCollide::Geometry( new agxCollide::Box( 30, 30, 15 ) );
  waterGeometry->setPosition( 0, 0, -15 );
  waterGeometry->setMaterial( waterMaterial );
  ::createWaterVisual( waterGeometry, root );
 
  // Create controller and mark as water geometry.
  agxModel::WindAndWaterControllerRef controller = new agxModel::WindAndWaterController();
  controller->addWater( waterGeometry );
  simulation->add( controller );
 
  simulation->add( waterGeometry );
 
  agx::MaterialRef meshMaterial = new agx::Material( "meshMaterial" );
  meshMaterial->getBulkMaterial()->setDensity( agx::Real( 400 ) );
 
  // To make our system tests that test determinism happy.
  std::srand( 4 );
 
  const agx::UInt numLayers = 3;
  const agx::UInt numPerLayer = 8;
  agx::Vec3 pos( -agx::Real( 1.2 * numPerLayer ), -agx::Real( 0.5 ) * numPerLayer, 4 );
  for ( agx::UInt layer = 0; layer < numLayers; ++layer ) {
    for ( agx::UInt i = 0; i < numPerLayer; ++i ) {
      agx::RigidBodyRef torus = new agx::RigidBody( new agxCollide::Geometry( agxUtil::TrimeshReaderWriter::createTrimesh( "models/torusForCollision.obj" ) ) );
      torus->setPosition( pos );
      torus->setRotation( agx::Quat( agx::PI_2 * agx::random( agx::Real( 0.8 ), agx::Real( 1.2 ) ), agx::Vec3::X_AXIS() ) );
      agxUtil::setBodyMaterial( torus, meshMaterial );
      agxOSG::setDiffuseColor( agxOSG::createVisual( torus, root ), agxRender::Color::IndianRed() );
      simulation->add( torus );
 
      pos.x() += agx::Real( 3 ) * agx::random( agx::Real( 0.7 ), agx::Real( 1.3 ) );
      pos.y() += agx::Real( 1 ) * agx::random( agx::Real( 0.5 ), agx::Real( 1.5 ) );
    }
    pos.x()  = -agx::Real( 1.2 * numPerLayer );
    pos.y()  = -agx::Real( 0.5 ) * numPerLayer + static_cast<agx::Real>(layer) + 1;
    pos.z() += agx::Real( 1 ) * agx::random( agx::Real( 0.5 ), agx::Real( 1.5 ) );
  }
 
  // Choose ITERATIVE solver for contacts between the meshes - less exact, but faster
  // (contacts are not main focus here, but hydrodynamics).
  // Note also: these contact material settings only apply for contacts between rigid bodies,
  // not for interaction between rigid bodies and wind or water -
  // for these, there are hydrodynamic and aerodynamic parameters (introduced later in this file in scene 3).
  agx::ContactMaterialRef meshMeshContactMaterial = simulation->getMaterialManager()->getOrCreateContactMaterial( meshMaterial, meshMaterial );
  meshMeshContactMaterial->setFrictionModel( new agx::IterativeProjectedConeFriction( agx::FrictionModel::ITERATIVE ) );
 
  simulation->add( new Printer( application ) );
 
  return root;
}
 
namespace
{
  class WaveGenerator : public agxSDK::StepEventListener
  {
    public:
      WaveGenerator( agxCollide::HeightField* hf );
 
      virtual void preCollide( const agx::TimeStamp& ) override;
 
    protected:
      virtual ~WaveGenerator();
 
    protected:
      agxCollide::HeightFieldObserver m_hf;
      agx::RealVector m_heights;
      osg::ref_ptr<agxOSG::GeometryNode> m_hfNode;
  };
 
  typedef agx::ref_ptr<WaveGenerator> WaveGeneratorRef;
 
  WaveGenerator::WaveGenerator( agxCollide::HeightField* hf )
    : agxSDK::StepEventListener( agxSDK::StepEventListener::PRE_COLLIDE ), m_hf( hf )
  {
    // Local storage of the heights.
    m_heights.resize( m_hf->getResolutionX() * m_hf->getResolutionY() );
  }
 
  WaveGenerator::~WaveGenerator()
  {
  }
 
  void WaveGenerator::preCollide( const agx::TimeStamp& t )
  {
    if ( m_hf == nullptr ) {
      getSimulation()->remove( this );
      return;
    }
 
    // Update waves. Rather explicit, but two waves. One going along
    // world x and the other at some angle in the xy-plane.
    for ( unsigned int i = 0; i < m_hf->getResolutionY(); ++i ) {
      for ( unsigned int j = 0; j < m_hf->getResolutionX(); ++j ) {
        agx::UInt index = i * m_hf->getResolutionX() + j;
        m_heights[ index ] = agx::Real( 2 ) * std::sin( agx::Real( 0.5 ) * j + agx::Real( 0.6 ) * t ) +
                             agx::Real( 0.5 ) * std::cos( agx::Real( 0.6 ) * i + agx::Real( 0.3 ) * j + agx::Real( 1.45 ) * t );
      }
    }
 
    m_hf->setHeights( m_heights );
  }
 
  class ShipStaticThrusters : public agxSDK::StepEventListener
  {
    public:
      ShipStaticThrusters( agx::RigidBody* ship, const agx::Vec3& localPosition, const agx::Vec3& localForce );
 
      virtual void preCollide( const agx::TimeStamp& ) override;
 
    private:
      agx::RigidBodyObserver m_ship;
      agx::Vec3 m_localPosition;
      agx::Vec3 m_localForce;
  };
 
  ShipStaticThrusters::ShipStaticThrusters( agx::RigidBody* ship, const agx::Vec3& localPosition, const agx::Vec3& localForce )
    : agxSDK::StepEventListener( agxSDK::StepEventListener::PRE_COLLIDE ), m_ship( ship ), m_localPosition( localPosition ),
      m_localForce( localForce )
  {
  }
 
  void ShipStaticThrusters::preCollide( const agx::TimeStamp& )
  {
    if ( m_ship == nullptr ) {
      getSimulation()->remove( this );
      return;
    }
 
    agxRender::RenderSingleton::instance()->add( m_ship->getFrame()->transformPointToWorld( m_localPosition ),
                                                 0.3,
                                                 agxRender::Color::Red() );
    agxRender::RenderSingleton::instance()->add( m_ship->getFrame()->transformPointToWorld( m_localPosition ),
                                                 m_ship->getFrame()->transformPointToWorld( m_localPosition ) +
                                                   m_ship->getFrame()->transformVectorToWorld( m_localForce.normal() ),
                                                 0.2,
                                                 agxRender::Color::White() );
 
    m_ship->addForceAtPosition( m_ship->getFrame()->transformVectorToWorld( m_localForce ),
                                m_ship->getFrame()->transformPointToWorld( m_localPosition ) );
  }
 
  agx::RigidBodyRef createShip( osg::Group* root )
  {
    // Read the ship .obj-file from disk.
    agxIO::MeshReaderRef meshReader = new agxIO::MeshReader();
    meshReader->readFile( "models/ship.obj" );
    // Correcting scale because file scale is centimeters.
    ::scaleMesh( meshReader->getVertices(), agx::Vec3( 0.01 ) );
 
    // Material defining the floating ability of the ship.
    agx::MaterialRef shipMaterial = new agx::Material( "shipMaterial" );
    shipMaterial->getBulkMaterial()->setDensity( 250 );
 
    // Creating the ship rigid body with a trimesh shape.
    agx::RigidBodyRef ship = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Trimesh( &meshReader->getVertices(),
                                                                                                    &meshReader->getIndices(),
                                                                                                    "ship" ) ) );
    ship->setPosition( agx::Vec3( 0, 0, 2 ) );
    // The ship default orientation is y up and along z, so rotate 90 degrees
    // about x to our world y axis, 0 degrees about y and 90 degrees about
    // z so the ship heading is along positive x.
    ship->setRotation( agx::Quat( agx::PI_2, agx::Vec3::X_AXIS(),
                                  agx::Real( 0 ), agx::Vec3::Y_AXIS(),
                                  agx::PI_2, agx::Vec3::Z_AXIS() ) );
    agxUtil::setBodyMaterial( ship, shipMaterial );
    agxOSG::setDiffuseColor( agxOSG::createVisual( ship, root ), agxRender::Color::IndianRed() );
 
    return ship;
  }
}
 
osg::Group* buildTutorial3( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  // Pressure field renderer render the state after a simulation step.
  // Tell simulation to integrate transforms of the bodies first in
  // a step-forward instead of last, this is only for pressure field rendering.
  simulation->setPreIntegratePositions( true );
 
  agx::MaterialRef waterMaterial = new agx::Material( "waterMaterial" );
  waterMaterial->getBulkMaterial()->setDensity( agx::Real( 1000 ) );
 
  // Create a height field, resolution 64x64 vertices and 250x250 meters.
  agxCollide::HeightFieldRef heightField = new agxCollide::HeightField( 32, 32, agx::Real( 125 ), agx::Real( 125 ), agx::Real( 25 ) );
  agxCollide::GeometryRef waterGeometry = new agxCollide::Geometry( heightField );
  waterGeometry->setPosition( 0, 0, 0 );
  waterGeometry->setMaterial( waterMaterial );
 
  agxModel::WindAndWaterControllerRef controller = new agxModel::WindAndWaterController();
  controller->addWater( waterGeometry );
  simulation->add( controller );
 
  simulation->add( waterGeometry );
 
  agx::RigidBodyRef ship = ::createShip( root );
  simulation->add( ship );
 
  // Hydrodynamic settings for the ship.
 
  // We know that the ship has one geometry and one shape (see ::createShip),
  // otherwise we would have to loop over the geometries and their shapes and apply settings per shape.
  agxCollide::Shape* shipShape = ship->getGeometries()[0]->getShapes()[0];
 
  // First, a pressure field renderer, purely for visualization.
  controller->registerPressureFieldRenderer( new agxOSG::PressureFieldRenderer( root ), shipShape );
 
  // These parameters are defined per shape (or wire or cable):
  agxModel::HydrodynamicsParameters* shipHydroParams = controller->getOrCreateHydrodynamicsParameters(shipShape);
  // The parameters are based on the ship's desired behavior, and can be configured by testing against the ship's
  // desired max speed, turning radius, and height in water at zero and max speed.
  agx::Real viscousDrag(0.15);
  agx::Real pressureDrag(1);
  agx::Real lift(0.01);
 
  shipHydroParams->setCoefficient(agxModel::WindAndWaterParameters::Coefficient::VISCOUS_DRAG, viscousDrag);
  shipHydroParams->setCoefficient(agxModel::WindAndWaterParameters::Coefficient::PRESSURE_DRAG, pressureDrag);
  shipHydroParams->setCoefficient(agxModel::WindAndWaterParameters::Coefficient::LIFT, lift);
 
 
  // The same parameters can be applied for aerodynamic effects.
  // They will only have some percent of the effect of the hydrodynamic ones due to the lower density of water,
  // but cannot be neglected. For more on aerodynamics, see tutorial_wind.
  // The difference is that we first have to enable aerodynamics for the body, since computing aerodynamics for
  // all bodies in the simulation might be wasteful. Set the parameter in the following line to true for doing that
  // (note that this model has >27000 triangles and might thus not run in real-time on all systems):
  bool useAeroDynamics = false; // Change to true for using aerodynamics. See notes above.
  if (useAeroDynamics) {
    controller->setEnableAerodynamics(ship, true);
 
    // Then, similar to hydro, the settings per shape:
    agxModel::AerodynamicsParameters* shipAeroParams = controller->getOrCreateAerodynamicsParameters(shipShape);
 
    // Setting the same values here as at hydro - might differ in real-world use cases.
    shipAeroParams->setCoefficient(agxModel::WindAndWaterParameters::Coefficient::VISCOUS_DRAG, viscousDrag);
    shipAeroParams->setCoefficient(agxModel::WindAndWaterParameters::Coefficient::PRESSURE_DRAG, pressureDrag);
    shipAeroParams->setCoefficient(agxModel::WindAndWaterParameters::Coefficient::LIFT, lift);
  }
 
 
  // Uncomment the following line to enable added mass calculations, which models the added
  // mass of water being moved by the ship.
  // Note that the first time when the file is generated it will take very long time! Probably over 3 hours.
  // Note also that based on these pre-calculations, the added mass will be recalculated each time step
  // based on the ship's relative velocity (this will be fast though).
  // shipHydroParams->initializeAddedMassStorage( "shipAddedMassStorage.dat" );
 
 
  // Let's see how the spheres behave in the waves as well.
  // We set only density here, no added mass, and leave pressure drag, viscous drag and lift at their
  // default values by not setting them.
  agx::MaterialRef spheresMaterial = new agx::Material( "spheresMaterial" );
  spheresMaterial->getBulkMaterial()->setDensity( agx::Real( 400 ) );
  agxSDK::AssemblyRef pyramid = ::createPyramid( 2, agx::Real( 0.25 ), spheresMaterial, root );
  pyramid->setPosition( 0, -10, 4 );
  simulation->add( pyramid );
 
  // Choose ITERATIVE solver for contacts between the spheres - less exact, but faster
  // (contacts are not main focus here, but hydrodynamics).
  // Note also: these contact material settings only apply for contacts between rigid bodies,
  // not for interaction between rigid bodies and wind or water -
  // for these, there are hydrodynamic and aerodynamic parameters (introduced above).
  agx::ContactMaterialRef sphereSphereContactMaterial = simulation->getMaterialManager()->getOrCreateContactMaterial( spheresMaterial, spheresMaterial );
  sphereSphereContactMaterial->setFrictionModel( new agx::IterativeProjectedConeFriction( agx::FrictionModel::ITERATIVE ) );
 
  // Create the visual representation of the height field).
  ::createWaterVisual(waterGeometry, root);
 
  // Create the wave generator
  WaveGenerator* waveGenerator = new ::WaveGenerator( heightField );
  // Run pre-collide one time so the water isn't flat before stepping the first time.
  waveGenerator->preCollide( 0 );
  simulation->add( waveGenerator );
 
  simulation->add( new Printer( application ) );
 
  simulation->add( new ShipStaticThrusters( ship, agx::Vec3( 0, 0, -15 ), agx::Vec3( 0, 0, 2E5 ) ) );
 
  return root;
}
 
osg::Group* buildTutorial4( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create material to define the density of water.
  agx::MaterialRef waterMaterial = new agx::Material( "waterMaterial" );
  waterMaterial->getBulkMaterial()->setDensity( agx::Real( 1000 ) );
 
  // Let the water geometry to be a plane with normal in z-direction, centered on point 5,5,5.
  agxCollide::GeometryRef waterGeometry = new agxCollide::Geometry( new agxCollide::Plane( agx::Vec3::Z_AXIS(), agx::Vec3( 5, 5, 5 ) ) );
  // Reset water position.
  waterGeometry->setPosition( 0, 0, -15 );
  waterGeometry->setMaterial( waterMaterial );
 
  // Create the visual representation of the height field).
  ::createWaterVisual( waterGeometry, root );
 
  agxModel::WindAndWaterControllerRef controller = new agxModel::WindAndWaterController();
  controller->addWater( waterGeometry );
  simulation->add( controller );
 
  simulation->add( waterGeometry );
 
  // Sphere density at 400 kg/m^3.
  agx::MaterialRef spheresMaterial = new agx::Material( "spheresMaterial" );
  spheresMaterial->getBulkMaterial()->setDensity( agx::Real( 400 ) );
 
  agxSDK::AssemblyRef pyramid = ::createPyramid( 3, agx::Real( 0.25 ), spheresMaterial, root );
  pyramid->setPosition( 0, 0, 4 );
  simulation->add( pyramid );
 
  // Choose ITERATIVE solver for contacts between the spheres - less exact, but faster
  // (contacts are not main focus here, but hydrodynamics).
  // Note also: these contact material settings only apply for contacts between rigid bodies,
  // not for interaction between rigid bodies and wind or water -
  // for these, there are hydrodynamic and aerodynamic parameters (introduced in scene 3).
  agx::ContactMaterialRef sphereSphereContactMaterial = simulation->getMaterialManager()->getOrCreateContactMaterial( spheresMaterial, spheresMaterial );
  sphereSphereContactMaterial->setFrictionModel( new agx::IterativeProjectedConeFriction( agx::FrictionModel::ITERATIVE ) );
 
  return root;
}
 
namespace
{
  class MyHydroSimulation : public agxSDK::StepEventListener
  {
    public:
      MyHydroSimulation( const agxCollide::Geometry* waterGeometry, WaveGenerator* waveGenerator, agx::RigidBody* ship, osg::Group* root );
 
      void preCollide( const agx::TimeStamp& ) override;
 
    private:
      const agxCollide::Shape* getShipShape() const;
 
    private:
      agxCollide::GeometryConstRef m_waterGeometry;
      WaveGeneratorRef m_waveGenerator;
      agxModel::WindAndWaterControllerRef m_hydrodynamicsModule;
      agx::RigidBodyObserver m_shipInAgX;
      agx::Real m_time;
  };
 
  MyHydroSimulation::MyHydroSimulation( const agxCollide::Geometry* waterGeometry,
                                        WaveGenerator* waveGenerator,
                                        agx::RigidBody* ship,
                                        osg::Group* root )
    : agxSDK::StepEventListener( agxSDK::StepEventListener::PRE_COLLIDE )
    , m_waterGeometry( waterGeometry )
    , m_waveGenerator( waveGenerator )
    , m_hydrodynamicsModule( new agxModel::WindAndWaterController() )
    , m_shipInAgX( ship )
    , m_time( 0 )
  {
    // Create a pressure field renderer for the ship.
    if ( root != nullptr )
      m_hydrodynamicsModule->registerPressureFieldRenderer( new agxOSG::PressureFieldRenderer( root ), getShipShape() );
  }
 
  const agxCollide::Shape* MyHydroSimulation::getShipShape() const
  {
    return m_shipInAgX != nullptr &&
           !m_shipInAgX->getGeometries().empty() ?
             m_shipInAgX->getGeometries()[ 0 ]->getShapes()[ 0 ] :
             nullptr;
  }
 
  void MyHydroSimulation::preCollide( const agx::TimeStamp& )
  {
    // Ship has been deleted for unknown reason...
    if ( m_shipInAgX == nullptr ) {
      getSimulation()->remove( this );
      return;
    }
 
    // Update waves.
    if ( m_waveGenerator != nullptr )
      m_waveGenerator->preCollide( m_time );
 
    // Calculate forces acting on the ship.
    if ( m_hydrodynamicsModule != nullptr ) {
      // NOTE THAT ADDED MASS CALCULATIONS ARE DISABLED BY DEFAULT AND
      // REQUIRES A FILE TO READ OR WRITE (ONLY ONCE THOUGH) STATIC DATA.
 
      // Uncomment the following line to enable added mass calculations.
      // (Note that the first time when the file is generated it will take very long time! Probably over 3 hours.)
       // m_hydrodynamicsModule->getOrCreateHydrodynamicsParameters( getShipShape() )->initializeAddedMassStorage( "shipAddedMassStorage.dat" );
 
      // Assume gravity is the same as in AgX.
      agxModel::WindAndWaterController::DynamicsData dynamicsData = m_hydrodynamicsModule->calculateDynamicsData( m_waterGeometry,
                                                                                                                  m_shipInAgX,
                                                                                                                  getSimulation()->getUniformGravity(),
                                                                                                                  nullptr,
                                                                                                                  true );
      m_shipInAgX->addForce( dynamicsData.worldForce );
      m_shipInAgX->addTorque( dynamicsData.worldTorque );
 
      // 'dynamicsData' contains full added mass and inertia matrices, when
      // input to AGX is the scale of the current mass and inertia matrices:
      //   M_final = (1 + addedMassCoefficients) * M_original
      // There's an utility method that calculates these coefficients.
      agx::Vec3 addedMassCoeffs;
      agx::Vec3 addedInertiaCoeffs;
      dynamicsData.getAddedMassAndInertiaDiagonalCoefficients( m_shipInAgX, addedMassCoeffs, addedInertiaCoeffs );
 
      // Currently the added mass and inertia are assumed to be diagonal, and
      // must be given in rigid body frame.
      m_shipInAgX->getMassProperties()->setMassCoefficients( addedMassCoeffs );
      m_shipInAgX->getMassProperties()->setInertiaTensorCoefficients( addedInertiaCoeffs );
    }
 
    m_time += getSimulation()->getTimeStep();
  }
}
 
osg::Group* buildTutorial5( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Pressure field renderer render the state after a simulation step.
  // Tell simulation to integrate transforms of the bodies first in
  // a step-forward instead of last.
  simulation->setPreIntegratePositions( true );
 
  agx::MaterialRef waterMaterial = new agx::Material( "waterMaterial" );
  waterMaterial->getBulkMaterial()->setDensity( agx::Real( 1000 ) );
 
  // Create a height field, resolution 64x64 vertices and 250x250 meters.
  agxCollide::HeightFieldRef heightField = new agxCollide::HeightField( 32, 32, agx::Real( 125 ), agx::Real( 125 ), agx::Real( 25 ) );
  agxCollide::GeometryRef waterGeometry = new agxCollide::Geometry( heightField );
  waterGeometry->setPosition( 0, 0, 0 );
  waterGeometry->setMaterial( waterMaterial );
 
  // Create the visual representation of the height field).
  ::createWaterVisual(waterGeometry, root);
 
  // Create the wave generator (and at the same time visual representation of the height field).
  ::WaveGenerator* waveGenerator = new ::WaveGenerator( heightField );
  // Run pre-collide one time so the water isn't flat before stepping the first time.
  waveGenerator->preCollide( 0 );
 
  agx::RigidBodyRef ship = ::createShip( root );
  simulation->add( ship );
 
  simulation->add( new MyHydroSimulation( waterGeometry, waveGenerator, ship, root ) );
 
  return root;
}
 
namespace
{
  class MyHydroSimulationAndStepper : public agxSDK::StepEventListener
  {
    public:
      MyHydroSimulationAndStepper( const agxCollide::Geometry* waterGeometry,
                                   const agx::RigidBody* ship,
                                   agx::RigidBody* kinematicBody,
                                   agx::LockJoint* lock,
                                   WaveGenerator* waveGenerator,
                                   osg::Group* root );
 
      void preCollide( const agx::TimeStamp& ) override;
 
    private:
      const agxCollide::Shape* getShipShape() const;
 
    private:
      agxCollide::GeometryConstRef m_waterGeometry;
      WaveGeneratorRef m_waveGenerator;
      agxModel::WindAndWaterControllerRef m_hydrodynamicsModule;
      agx::RigidBodyConstObserver m_shipInAgXDontTouch;
      agx::RigidBodyRef m_kinematicBody;
      agx::LockJointRef m_lock;
      agx::Real m_time;
      agx::Vec3 m_gravity;
  };
 
  MyHydroSimulationAndStepper::MyHydroSimulationAndStepper( const agxCollide::Geometry* waterGeometry
                                                            , const agx::RigidBody* ship,
                                                            agx::RigidBody* kinematicBody,
                                                            agx::LockJoint* lock,
                                                            WaveGenerator* waveGenerator,
                                                            osg::Group* root )
    : agxSDK::StepEventListener( agxSDK::StepEventListener::PRE_COLLIDE ), m_waterGeometry( waterGeometry ),
      m_waveGenerator( waveGenerator ), m_hydrodynamicsModule( new agxModel::WindAndWaterController() ), m_shipInAgXDontTouch( ship ),
      m_kinematicBody( kinematicBody ), m_lock( lock ), m_time( 0 ), m_gravity( 0, 0, -agx::GRAVITY_ACCELERATION )
  {
    // Enable API access of the computed forces.
    if ( m_lock != nullptr )
      m_lock->setEnableComputeForces( true );
 
    // Create a pressure field renderer for the ship.
    if ( root != nullptr )
      m_hydrodynamicsModule->registerPressureFieldRenderer( new agxOSG::PressureFieldRenderer( root ), getShipShape() );
  }
 
  const agxCollide::Shape* MyHydroSimulationAndStepper::getShipShape() const
  {
    return m_shipInAgXDontTouch != nullptr && !m_shipInAgXDontTouch->getGeometries().empty() ?
             m_shipInAgXDontTouch->getGeometries()[ 0 ]->getShapes()[ 0 ] :
             nullptr;
  }
 
  void MyHydroSimulationAndStepper::preCollide( const agx::TimeStamp& )
  {
    // Ship has been deleted for unknown reason...
    if ( m_shipInAgXDontTouch == nullptr || m_kinematicBody == nullptr ) {
      getSimulation()->remove( this );
      return;
    }
 
    // Update water geometry and waves.
    if ( m_waveGenerator != nullptr )
      m_waveGenerator->preCollide( m_time );
 
    const agx::Real timeStep = getSimulation()->getTimeStep();
    agx::Vec3 totalForce;
    agx::Vec3 totalTorque;
 
    // Gravity force.
    totalForce += m_shipInAgXDontTouch->getMassProperties()->getMass() * m_gravity;
 
    // Hydrodynamic forces.
    if ( m_hydrodynamicsModule != nullptr ) {
      agxModel::WindAndWaterController::DynamicsData dynamicsData = m_hydrodynamicsModule->calculateDynamicsData( m_waterGeometry,
                                                                                                                  m_shipInAgXDontTouch,
                                                                                                                  m_gravity,
                                                                                                                  nullptr,
                                                                                                                  true );
      totalForce  += dynamicsData.worldForce;
      totalTorque += dynamicsData.worldTorque;
    }
 
    // Forces from the AGX simulation (contacts, cranes, wires etc etc).
    if ( m_lock != nullptr ) {
      agx::Vec3 forceFromAgX;
      agx::Vec3 torqueFromAgX;
      m_lock->getLastForce( m_kinematicBody, forceFromAgX, torqueFromAgX );
 
      totalForce  += forceFromAgX;
      totalTorque += torqueFromAgX;
    }
 
    agx::Matrix3x3 shipR( m_shipInAgXDontTouch->getRotation() );
    agx::Matrix3x3 shipRT = shipR.transpose();
 
    agx::Matrix3x3 shipWorldMassMatrix    = shipRT * agx::Matrix3x3( agx::Vec3( m_shipInAgXDontTouch->getMassProperties()->getMass() ) ) * shipR;
    agx::Matrix3x3 shipWorldInertiaMatrix = shipRT * agx::Matrix3x3( m_shipInAgXDontTouch->getMassProperties()->getPrincipalInertiae() ) * shipR;
 
    // Integrate velocity.
    agx::Vec3 linearVelocity  = m_kinematicBody->getVelocity() + shipWorldMassMatrix.inverse() * totalForce * timeStep;
    agx::Vec3 angularVelocity = m_kinematicBody->getAngularVelocity() + shipWorldInertiaMatrix.inverse() * totalTorque * timeStep;
 
    // Integrate position.
    agx::Vec3 position    = m_kinematicBody->getPosition() + linearVelocity * timeStep;
 
    // Integrate orientation.
    agx::Vec3 ww          = angularVelocity;
    agx::Real alpha       = agx::Real( 0.5 ) * timeStep * ww.normalize();
    agx::Quat orientation = m_kinematicBody->getRotation() * agx::Quat( agx::Vec4( ww * std::sin( alpha ), std::cos( alpha ) ) );
 
    // The velocity of the kinematic body has to be provided to AGX in order
    // for AGX to calculate correct interaction forces with other objects.
    // RigidBody::moveTo calculats the velocity so that the kinematic body
    // will be at given position/orientation after next stepForward.
    m_kinematicBody->moveTo( position, orientation, timeStep );
 
    m_time += timeStep;
  }
}
 
osg::Group* buildTutorial6( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  // Pressure field renderer render the state after a simulation step.
  // Tell simulation to integrate transforms of the bodies first in
  // a step-forward instead of last.
  simulation->setPreIntegratePositions( true );
 
  agx::MaterialRef waterMaterial = new agx::Material( "waterMaterial" );
  waterMaterial->getBulkMaterial()->setDensity( agx::Real( 1000 ) );
 
  // Create a height field, resolution 64x64 vertices and 250x250 meters.
  agxCollide::HeightFieldRef heightField = new agxCollide::HeightField( 32, 32, agx::Real( 125 ), agx::Real( 125 ), agx::Real( 25 ) );
  agxCollide::GeometryRef waterGeometry = new agxCollide::Geometry( heightField );
  waterGeometry->setPosition( 0, 0, 0 );
  waterGeometry->setMaterial( waterMaterial );
 
  // Create the visual representation of the height field).
  ::createWaterVisual(waterGeometry, root);
 
  // Create the wave generator
  ::WaveGenerator* waveGenerator = new ::WaveGenerator( heightField );
  // Run pre-collide one time so the water isn't flat before stepping the first time.
  waveGenerator->preCollide( 0 );
 
  agx::RigidBodyRef ship = ::createShip( root );
  simulation->add( ship );
 
  // The ship should be dynamic in AgX, feeling forces from other objects.
  ship->setMotionControl( agx::RigidBody::DYNAMICS );
 
  // Create kinematic body at center of mass of the ship.
  agx::RigidBodyRef kinematicShipAttachmentBody = new agx::RigidBody( "kinematicShipAttachmentBody" );
  kinematicShipAttachmentBody->setMotionControl( agx::RigidBody::KINEMATICS );
  kinematicShipAttachmentBody->setPosition( ship->getCmPosition() );
 
  simulation->add( kinematicShipAttachmentBody );
 
  // No gravity in AGX, it's handled in MyHydroSimulationAndStepper.
  simulation->setUniformGravity( agx::Vec3() );
 
  // Lock the two bodies together. Center at ship center of mass.
  agx::LockJointRef lock = agx::Constraint::createFromBody< agx::LockJoint >( agx::Vec3(), agx::Vec3::Z_AXIS(), kinematicShipAttachmentBody, ship );
  // Let the lock be stiff, but too stiff could result in unstable simulations!
  lock->setCompliance( agx::Real( 8E-12 ) );
  simulation->add( lock );
 
  simulation->add( new MyHydroSimulationAndStepper( waterGeometry, ship, kinematicShipAttachmentBody, lock, waveGenerator, root ) );
 
  application->getSceneDecorator()->setFontSize( 0.012f );
  application->getSceneDecorator()->setText( 0, "Caution: Co-simulation in progress - integration is made outside of AGX Dynamics, hence applying", agxRender::Color::Red() );
  application->getSceneDecorator()->setText( 1, "         stiff forces (e.g., picking) might result in an unstable simulation!", agxRender::Color::Red() );
 
  return root;
}
 
 
class GuiListener : public agxSDK::GuiEventListener
{
 
public:
  GuiListener(agxOSG::ExampleApplication* app,
              agx::RigidBody* body,
              agx::Motor1D* left,
              agx::Motor1D* right,
              agx::Motor1D* leftRear,
              agx::Motor1D* rightRear,
              agx::Motor1D* rotator,
              agxWire::Wire* wire)
    : m_app(app)
    , m_body(body)
    , m_left(left)
    , m_right(right)
    , m_leftRear(leftRear)
    , m_rightRear(rightRear)
    , m_rotator(rotator)
    , m_wire(wire)
  {
    m_app->getSceneDecorator()->setText(4, agx::String::format("a/z - Right wings up/down"));
    m_app->getSceneDecorator()->setText(5, agx::String::format("s/x - Left wings up/down"));
    m_app->getSceneDecorator()->setText(6, agx::String::format("up/down - Increase/decrease speed"));
  }
 
  bool keyboard(int key, unsigned int, float, float, bool down)
  {
    if (down && key == 'z') {
      m_left->setSpeed(-0.5);
      m_leftRear->setSpeed(-0.5);
      return true;
    }
    if (!down && key == 'z') {
      m_left->setSpeed(0);
      m_leftRear->setSpeed(0);
      return true;
    }
    if (down && key == 'a') {
      m_left->setSpeed(0.5);
      m_leftRear->setSpeed(0.5);
      return true;
    }
    if (!down && key == 'a') {
      m_left->setSpeed(0);
      m_leftRear->setSpeed(0);
      return true;
    }
    if (down && key == 'x') {
      m_right->setSpeed(-0.5);
      m_rightRear->setSpeed(-0.5);
      return true;
    }
    if (!down && key == 'x') {
      m_right->setSpeed(0);
      m_rightRear->setSpeed(0);
      return true;
    }
    if (down && key == 's') {
      m_right->setSpeed(0.5);
      m_rightRear->setSpeed(0.5);
      return true;
    }
    if (!down && key == 's') {
      m_right->setSpeed(0);
      m_rightRear->setSpeed(0);
      return true;
    }
 
    if (down && key == agxSDK::GuiEventListener::KEY_Down)
    {
      agx::Real s = m_rotator->getSpeed()*0.99;
      m_rotator->setSpeed(s);
      std::cerr << "Speed: " << m_rotator->getSpeed() << std::endl;
      return true;
    }
    if (down && key == agxSDK::GuiEventListener::KEY_Up)
    {
      agx::Real s = std::min(0.7, m_rotator->getSpeed()*1.01);
      m_rotator->setSpeed(s);
      std::cerr << "Speed: " << m_rotator->getSpeed() << std::endl;
      return true;
    }
 
    return false;
 
  }
 
  void update(float, float)
  {
    agx::Real depth = m_body->getPosition().z();
    agx::Vec3 vel = m_body->getVelocity();
    agx::Real knots = vel.length()*1.94384;
    agx::Real tension = m_wire->getTension(2).smoothed;
    m_app->getSceneDecorator()->setText(0, agx::String::format("Depth: %6.2f m", depth));
    m_app->getSceneDecorator()->setText(1, agx::String::format("Speed: %6.2f knots", knots));
    m_app->getSceneDecorator()->setText(2, agx::String::format("Tension: %6.2f kN", tension / 1000));
  }
 
  agxOSG::ExampleApplication* m_app;
  agx::RigidBody* m_body;
  agx::Motor1D* m_left;
  agx::Motor1D* m_right;
  agx::Motor1D* m_leftRear;
  agx::Motor1D* m_rightRear;
  agx::Motor1D* m_rotator;
  agxWire::Wire *m_wire;
};
 
 
 
class PressureVisitor : public agxSDK::AssemblyVisitor
{
  public:
    PressureVisitor(agxModel::WindAndWaterController* controller, osg::Group* root)
      : m_controller(controller)
      , m_root(root)
    {
    }
 
 
    void visit(agxCollide::Geometry* geom) override
    {
      agxCollide::ShapeRefVector shapes = geom->getShapes();
      for (size_t i = 0; i<shapes.size(); i++)
      {
        m_controller->registerPressureFieldRenderer(new agxOSG::PressureFieldRenderer(m_root), shapes[i]);
      }
    }
 
    using agxSDK::AssemblyVisitor::visit;
 
  private:
    agxModel::WindAndWaterController* m_controller;
    osg::Group* m_root;
};
 
 
osg::Group* buildTutorial7(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  osg::Group* root = new osg::Group();
 
 
  // Create material to define the density of water.
  agx::MaterialRef waterMaterial = new agx::Material("waterMaterial");
  waterMaterial->getBulkMaterial()->setDensity(agx::Real(1000));
 
  // Let the water geometry to be an box of size 140x140x60 meters.
  agxCollide::GeometryRef waterGeometry = new agxCollide::Geometry(new agxCollide::Box(70, 70, 30));
 
  // Surface of water at z = 0.
  waterGeometry->setPosition(0, 0, -30);
  waterGeometry->setMaterial(waterMaterial);
 
  // Create the visual representation of the height field).
  ::createWaterVisual(waterGeometry, root);
 
  agxModel::WindAndWaterControllerRef controller = new agxModel::WindAndWaterController();
  controller->addWater(waterGeometry);
  simulation->add(controller);
 
  simulation->add(waterGeometry);
 
  agxCollide::GeometryRef bottomGeometry = new agxCollide::Geometry(new agxCollide::Plane(agx::Vec3::Z_AXIS(), agx::Vec3(0, 0, -30)));
  simulation->add(bottomGeometry);
 
  agxSDK::AssemblyRef submarine = new agxSDK::Assembly;
  agx::String modelPath = "data/models/submarine.agx";
  if (!agxOSG::readFile(modelPath, simulation, root, submarine))
  {
    LOGGER_ERROR() << "Unable to load model: " << modelPath << LOGGER_ENDL();
  }
 
  // Find various objects in the loaded simulation
  agx::RigidBody* leftWing = simulation->getRigidBody("LeftWing");
  agx::RigidBody* rightWing = simulation->getRigidBody("RightWing");
  agx::RigidBody* leftWingRear = simulation->getRigidBody("LeftWingRear");
  agx::RigidBody* rightWingRear = simulation->getRigidBody("RightWingRear");
  agx::RigidBody* body = simulation->getRigidBody("Body");
  agx::Hinge* leftWingHinge = simulation->getConstraint<agx::Hinge>("LeftWingHinge");
  agx::Hinge* rightWingHinge = simulation->getConstraint<agx::Hinge>("RightWingHinge");
  agx::Hinge* leftWingHingeRear = simulation->getConstraint<agx::Hinge>("LeftWingHingeRear");
  agx::Hinge* rightWingHingeRear = simulation->getConstraint<agx::Hinge>("RightWingHingeRear");
  agxCollide::Geometry* bodyGeometry = simulation->getGeometry("Body");
  agxCollide::Geometry* wireConnection = simulation->getGeometry("WireConnection");
 
  // Make sure we found them
  if (!leftWing)
    LOGGER_ERROR() << "Unable to find body \'LeftWing\'" << LOGGER_ENDL();
  if (!rightWing)
    LOGGER_ERROR() << "Unable to find body \'RightWing\'" << LOGGER_ENDL();
  if (!body)
    LOGGER_ERROR() << "Unable to find body \'Body\'" << LOGGER_ENDL();
  if (!leftWingHinge)
    LOGGER_ERROR() << "Unable to find Hinge \'LeftWingHinge\'" << LOGGER_ENDL();
  if (!rightWingHinge)
    LOGGER_ERROR() << "Unable to find Hinge \'RightWingHinge\'" << LOGGER_ENDL();
  if (!wireConnection)
    LOGGER_ERROR() << "Unable to find Geometry \'WireConnection\'" << LOGGER_ENDL();
  if (!rightWingHingeRear)
    LOGGER_ERROR() << "Unable to find Hinge \'RightWingHingeRear\'" << LOGGER_ENDL();
  if (!leftWingHingeRear)
    LOGGER_ERROR() << "Unable to find Hinge \'LeftWingHingeRear\'" << LOGGER_ENDL();
  if (!bodyGeometry)
    LOGGER_ERROR() << "Unable to find Geometry \'Body\'" << LOGGER_ENDL();
 
  // Set viscous drag for all shapes of the relevant bodies.
  // Pressure drag and lift can be set the same way.
  agxModel::WindAndWaterParameters::setHydrodynamicCoefficient(controller, leftWing, agxModel::WindAndWaterParameters::VISCOUS_DRAG, 0.01);
  agxModel::WindAndWaterParameters::setHydrodynamicCoefficient(controller, leftWingRear, agxModel::WindAndWaterParameters::VISCOUS_DRAG, 0.01);
  agxModel::WindAndWaterParameters::setHydrodynamicCoefficient(controller, rightWing, agxModel::WindAndWaterParameters::VISCOUS_DRAG, 0.01);
  agxModel::WindAndWaterParameters::setHydrodynamicCoefficient(controller, rightWingRear, agxModel::WindAndWaterParameters::VISCOUS_DRAG, 0.01);
  agxModel::WindAndWaterParameters::setHydrodynamicCoefficient(controller, body, agxModel::WindAndWaterParameters::VISCOUS_DRAG, 0.01);
 
  // Disable the collision for the wire (its a box on the front of the submarine).
  wireConnection->setEnableCollisions(waterGeometry, false);
  submarine->setPosition(-45, -45, 0);
 
  agx::RigidBodyRef ship = new agx::RigidBody();
  agxCollide::GeometryRef shipGeom = new agxCollide::Geometry(new agxCollide::Box(1, 1, 1));
  shipGeom->setEnableCollisions(false);
  ship->add(shipGeom);
  simulation->add(ship);
 
  agxOSG::createVisual(shipGeom, root);
 
  simulation->setPreIntegratePositions(true);
  ship->setPosition(-20, -40, 0);
  agx::FrameRef frame = new agx::Frame();
  frame->setLocalTranslate(20, 40, 0);
  agx::HingeRef hinge = new agx::Hinge(ship, frame);
  simulation->add(hinge);
  hinge->getMotor1D()->setEnable(true);
  hinge->getMotor1D()->setSpeed(0.45);
 
  agxWire::WireRef wire = new agxWire::Wire(0.01, 2, false);
  wire->add(new agxWire::BodyFixedNode(ship, 0, 0, 0));
  wire->add(new agxWire::BodyFixedNode(body, wireConnection->getLocalPosition()));
 
  simulation->add(wire);
 
  agxOSG::WireRendererRef wireRenderer = new agxOSG::WireRenderer(wire, root);
  simulation->add(wireRenderer);
 
  simulation->add(new GuiListener(application, body, leftWingHinge->getMotor1D(), rightWingHinge->getMotor1D(),
    leftWingHingeRear->getMotor1D(), rightWingHingeRear->getMotor1D(), hinge->getMotor1D(), wire));
 
  PressureVisitor visitor(controller, root);
  submarine->traverse(&visitor);
 
  return root;
}
 
namespace
{
  class CustomWaterFlowGenerator : public agxModel::WindAndWaterController::WaterFlowGenerator
  {
  public:
    CustomWaterFlowGenerator( agx::Real flowCoefficient, agx::Vec3 center )
      : m_flowCoefficient( flowCoefficient ), m_center( center )
    {
    }
 
    // Prepare is called before a geometry is about to be handled. This is part of the
    // WaterFlowGenerator interface but is not used in this tutorial. For an example of
    // of how to use this for shadowing, look at tutorial_wind.cpp where this method is
    // used in a custom wind generator that shares the same interface.
    /*virtual void prepare( const agxCollide::Geometry* ) override
    {
    }*/
 
    // Return flow velocity at worldPoint.
    virtual agx::Vec3 getVelocity( const agx::Vec3& worldPoint ) const override
    {
      agx::Vec3 positionFromCenter = worldPoint - m_center;
      agx::Real xVelocity = positionFromCenter.x() - 2 * positionFromCenter.y();
      agx::Real yVelocity = 2 * positionFromCenter.x() + positionFromCenter.y();
      return m_flowCoefficient * agx::Vec3( xVelocity, yVelocity, 0 );
    }
 
    AGXSTREAM_DECLARE_SERIALIZABLE( CustomWaterFlowGenerator );
 
  protected:
    CustomWaterFlowGenerator() {}
 
  private:
    agx::Real m_flowCoefficient;
    agx::Vec3 m_center;
  };
 
  // Serialization.
  void CustomWaterFlowGenerator::store( agxStream::OutputArchive& out ) const
  {
    out << agxStream::out( "flowCoefficient", m_flowCoefficient );
    out << agxStream::out( "center", m_center );
  }
 
  void CustomWaterFlowGenerator::restore( agxStream::InputArchive& in )
  {
    in >> agxStream::in( "flowCoefficient", m_flowCoefficient );
    in >> agxStream::in( "center", m_center );
  }
}
 
// Introducing water flow generators
osg::Group* buildTutorial8( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create two water geometries of size 60x60x30 meters. Since no material is used to set the density
  // it will have the default value of 1000 kg per cubic meter.
  agxCollide::GeometryRef waterGeometry1 = new agxCollide::Geometry( new agxCollide::Box( 10, 10, 5 ) );
  // Surface of water at z = 0.
  agx::Vec3 position1( -11, 0, -5 );
  waterGeometry1->setPosition( position1 );
 
  // Create the visual representation of the height field).
  ::createWaterVisual( waterGeometry1, root );
 
  agxCollide::GeometryRef waterGeometry2 = new agxCollide::Geometry( new agxCollide::Box( 10, 10, 5 ) );
  // Surface of water at z = 0.
  agx::Vec3 position2( 11, 0, -5 );
  waterGeometry2->setPosition( position2 );
 
  // Create the visual representation of the height field).
  ::createWaterVisual( waterGeometry2, root );
 
  // Create a wind- and watercontroller and add the water geometries and two flow generators. One for each water geometry.
  agxModel::WindAndWaterControllerRef controller = new agxModel::WindAndWaterController();
  controller->addWater( waterGeometry1 );
  controller->setWaterFlowGenerator( waterGeometry1, new agxModel::ConstantWaterFlowGenerator( agx::Vec3( 0, 1, 0 ) ) );
  controller->addWater( waterGeometry2 );
  controller->setWaterFlowGenerator( waterGeometry2, new ::CustomWaterFlowGenerator( agx::Real( 0.3 ), position2 ) );
 
  simulation->add( controller );
  simulation->add( waterGeometry1 );
  simulation->add( waterGeometry2 );
 
  // Set density to make the spheres buoyant
  agx::MaterialRef sphereMaterial = new agx::Material( "sphereMaterial" );
  sphereMaterial->getBulkMaterial()->setDensity( 600 );
 
  // Create a grid of spheres for both of the water geometries
  for ( int i = 0; i < 2; i++ ) {
    for ( int j = 0; j < 2; j++ ) {
      agxCollide::GeometryRef sphereGeometry1 = new agxCollide::Geometry( new agxCollide::Sphere( 0.25 ) );
      agx::RigidBodyRef sphere1 = new agx::RigidBody( sphereGeometry1 );
      sphere1->setPosition( position1 + agx::Vec3( -2.5 + i * 5, -2.5 + j * 5, 5 ) );
      agxUtil::setBodyMaterial( sphere1, sphereMaterial );
      agxOSG::createVisual( sphereGeometry1, root );
      simulation->add( sphere1 );
 
      agxCollide::GeometryRef sphereGeometry2 = new agxCollide::Geometry( new agxCollide::Sphere( 0.25 ) );
      agx::RigidBodyRef sphere2 = new agx::RigidBody( sphereGeometry2 );
      sphere2->setPosition( position2 + agx::Vec3( -1 + i * 1, -1 + j * 1, 5 ) );
      agxUtil::setBodyMaterial( sphere2, sphereMaterial );
      agxOSG::createVisual( sphereGeometry2, root );
      simulation->add( sphere2 );
    }
  }
 
  return root;
}
 
class WaveWaterWrapper : public agxModel::WaterWrapper
{
  public:
    WaveWaterWrapper()
      : agxModel::WaterWrapper() {}
 
    agx::Real findHeightFromSurface( const agx::Vec3& worldPoint, const agx::Vec3& /*upVector*/, const agx::TimeStamp& t ) const override
    {
      agx::Real surfaceLevel = - agx::Real( 2 ) + std::sin( agx::Real( 0.5 ) * worldPoint.x() + agx::Real( 0.6 ) * t ) +
        agx::Real( 0.5 ) * std::cos( agx::Real( 0.6 ) * worldPoint.y() + agx::Real( 0.3 ) * worldPoint.x() + agx::Real( 1.45 ) * t );
 
      return worldPoint.z() - surfaceLevel;
    }
};
 
// Tutorial 9 - Custom water wrapper that enables water surfaces other than that of the water geometry.
osg::Group* buildTutorial9( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create two water geometries of size 60x60x30 meters. Since no material is used to set the density
  // it will have the default value of 1000 kg per cubic meter.
  agxCollide::GeometryRef waterGeometry = new agxCollide::Geometry( new agxCollide::Box( 50, 50, 5 ) );
  // Surface of water at z = 0.
  agx::Vec3 position( 0, 0, -5 );
  waterGeometry->setPosition( position );
 
  // Create the visual representation of the height field).
  ::createWaterVisual( waterGeometry, root );
 
  // Create a wind- and watercontroller and add the water geometry and a water wrapper.
  agxModel::WindAndWaterControllerRef controller = new agxModel::WindAndWaterController();
  controller->addWater( waterGeometry );
  controller->setWaterWrapper( waterGeometry, new WaveWaterWrapper() );
 
  simulation->add( controller );
  simulation->add( waterGeometry );
 
  // Set density to make the spheres buoyant
  agx::MaterialRef sphereMaterial = new agx::Material( "sphereMaterial" );
  sphereMaterial->getBulkMaterial()->setDensity( 600 );
 
  // Create a number of buoyant spheres
  for( int i = 0; i < 10; i++ ) {
    agxCollide::GeometryRef sphereGeometry = new agxCollide::Geometry( new agxCollide::Sphere( 0.25 ) );
    agx::RigidBodyRef sphere = new agx::RigidBody( sphereGeometry );
    sphere->setPosition( agx::Vec3( -10 + i * 2, 0, 0 ) );
    agxUtil::setBodyMaterial( sphere, sphereMaterial );
    agxOSG::createVisual( sphereGeometry, root );
    simulation->add( sphere );
  }
 
  return root;
}
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout << "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
               "\tTutorial hydrodynamics\n" <<
               "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
    application->addScene( buildTutorial3, '3' );
    application->addScene( buildTutorial4, '4' );
    application->addScene( buildTutorial5, '5' );
    application->addScene( buildTutorial6, '6' );
    application->addScene( buildTutorial7, '7' );
    application->addScene( buildTutorial8, '8' );
    application->addScene( buildTutorial9, '9' );
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_imu.cpp

/*
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.
*/
#include <agx/version.h>
 
#include <agxCollide/Box.h>
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/utils.h>
 
#include <agxPlot/DebugPlot.h>
 
#include <agxSensor/Accelerometer.h>
#include <agxSensor/Environment.h>
#include <agxSensor/Gyroscope.h>
#include <agxSensor/IMU.h>
#include <agxSensor/IMUModelAccelerometerAttachment.h>
#include <agxSensor/IMUModelGyroscopeAttachment.h>
#include <agxSensor/IMUModelMagnetometerAttachment.h>
#include <agxSensor/Magnetometer.h>
#include <agxSensor/UniformMagneticField.h>
 
#include <agxSDK/Simulation.h>
 
#include <agxUtil/StepEventCallback.h>
 
#include <tuple>
 
static bool g_isUnitTest = false;
 
osg::Group* buildTutorial1(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*app*/)
{
  auto root = new osg::Group();
 
  // Create an Environment and associate it with the Simulation.
  agxSensor::Environment* environment = agxSensor::Environment::getOrCreate(simulation);
 
  // Configure the magnetic field of the environment to that of a point on the Earth's surface.
  const agx::Vec3 magneticFieldVector = agx::Vec3(-19.462e-6, 44.754e-6, 7.8426e-6);
  environment->setMagneticField(new agxSensor::UniformMagneticField(magneticFieldVector));
 
  // Create the slope (the angled floor) as a static rigid body.
  agx::RigidBodyRef slope = new agx::RigidBody(new agxCollide::Geometry(
                                               new agxCollide::Box(10.0, 10.0, 0.5)));
  slope->setRotation(agx::EulerAngles(0.0, 3.14 * 0.2, 0.0));
  slope->setPosition(0.0, 0.0, -2.2);
  slope->setMotionControl(agx::RigidBody::STATIC);
 
  // Add the slope to the Simulation, Environment (optionally) and visualization.
  simulation->add(slope);
  environment->add(slope);
  agxOSG::createVisual(slope, root);
 
  // Create the bottom floor, for the IMU to settle on, as a static rigid body.
  agx::RigidBodyRef floor = new agx::RigidBody(new agxCollide::Geometry(
                                                 new agxCollide::Box(10.0, 10.0, 0.5)));
  floor->setPosition(18.0, 0.0, -8.0);
  floor->setMotionControl(agx::RigidBody::STATIC);
 
  // Add the bottom floor to the Simulation, Environment (optionally) and visualization.
  simulation->add(floor);
  environment->add(floor);
  agxOSG::createVisual(floor, root);
 
  // Create a regular dynamic rigid body to attach the IMU to.
  agx::RigidBodyRef imuBody = new agx::RigidBody(new agxCollide::Geometry(
                                                 new agxCollide::Box(1.0, 1.0, 1.0)));
 
  // Angle the IMU rigid body such that it slides, and not rolls, down the slope.
  imuBody->setRotation(slope->getRotation());
 
  // Add the IMU body to the Simulation, Environment (optionally) and visualization.
  simulation->add(imuBody);
  environment->add(imuBody);
  agxOSG::createVisual(imuBody, root);
 
  // Visualize some axes on the IMU body to display the measurement axes.
  agxOSG::createAxes(imuBody, nullptr, root, 4.0);
 
  // Create the IMU sub-sensor models for an accelerometer ...
  agxSensor::AccelerometerModelRef accelerometerModel = new agxSensor::AccelerometerModel();
 
  // ... a gyroscope ...
  agxSensor::GyroscopeModelRef gyroscopeModel = new agxSensor::GyroscopeModel(
    agxSensor::TriaxialRange(),
    agxSensor::TriaxialCrossSensitivity(0.01),
    agx::Vec3( 3.0 ),
    agxSensor::ITriaxialSignalSystemNodeRefVector{
      new agxSensor::GyroscopeLinearAccelerationEffects(agx::Vec3(2.62*1e-4)),
      new agxSensor::TriaxialSpectralGaussianNoise(agx::Vec3(1.75*1e-4))
    }
  );
 
  // ... and a magnetometer.
  agxSensor::MagnetometerModelRef magnetometerModel = new agxSensor::MagnetometerModel(
    agxSensor::TriaxialRange(),
    agxSensor::TriaxialCrossSensitivity(0.01),
    agx::Vec3(0.0),
    agxSensor::ITriaxialSignalSystemNodeRefVector{
      new agxSensor::TriaxialGaussianNoise(agx::Vec3(4.5e-7))
    }
  );
 
  // Create an IMU with the specified sensor attachments and attach it to the body's frame.
  agxSensor::IMURef imu = new agxSensor::IMU(imuBody->getFrame(), new agxSensor::IMUModel(
    agxSensor::IMUModelSensorAttachmentRefVector{
      new agxSensor::IMUModelAccelerometerAttachment(agx::AffineMatrix4x4(), accelerometerModel),
      new agxSensor::IMUModelGyroscopeAttachment(agx::AffineMatrix4x4(), gyroscopeModel),
      new agxSensor::IMUModelMagnetometerAttachment(agx::AffineMatrix4x4(), magnetometerModel)
    }
  ));
 
  // Add the IMU to the Environment.
  environment->add(imu);
 
  // Add a 9 DoF output to the IMU, and store the auto-assigned id.
  struct NineDoF
  {
    struct {
      agx::Real x, y, z;
    } accelerometer;
    struct {
      agx::Real x, y, z;
    } gyroscope;
    struct {
      agx::Real x, y, z;
    } magnetometer;
  };
 
  size_t outputId = 0u; // (Default) 0 is considered "invalid".
  std::tie(outputId, std::ignore) = imu->getOutputHandler()
                                       ->add<NineDoF,
                                             agxSensor::IMUOutput::ACCELEROMETER_X_F64,
                                             agxSensor::IMUOutput::ACCELEROMETER_Y_F64,
                                             agxSensor::IMUOutput::ACCELEROMETER_Z_F64,
                                             agxSensor::IMUOutput::GYROSCOPE_X_F64,
                                             agxSensor::IMUOutput::GYROSCOPE_Y_F64,
                                             agxSensor::IMUOutput::GYROSCOPE_Z_F64,
                                             agxSensor::IMUOutput::MAGNETOMETER_X_F64,
                                             agxSensor::IMUOutput::MAGNETOMETER_Y_F64,
                                             agxSensor::IMUOutput::MAGNETOMETER_Z_F64>()
                                       .value_or(std::make_pair(0u, nullptr));
 
  if (!g_isUnitTest) {
    // Add a post-step event callback to fetch the IMU measurements.
    agxUtil::StepEventCallback::post([](agxSDK::Simulation* /* simulation */,
                                        agxSensor::IMU* imu,
                                        size_t outputId)
    {
      agxSensor::BinaryOutputView<NineDoF> output = imu->getOutputHandler()
                                                       ->view<NineDoF>(outputId);
 
      // Plot the IMU sub-sensor measurement magnitudes using the DebugPlot utility.
      for (const NineDoF& v : output) {
        agx::Real accelerometerMagnitude = agx::Vec3{
          v.accelerometer.x,
          v.accelerometer.y,
          v.accelerometer.z
        }.length();
        agxPlot::DebugPlot::add(accelerometerMagnitude, "Accelerometer",
                                "Inertial Measurement Unit");
 
        agx::Real gyroscopeMagnitude = agx::Vec3{
          v.gyroscope.x,
          v.gyroscope.y,
          v.gyroscope.z
        }.length();
        agxPlot::DebugPlot::add(gyroscopeMagnitude, "Gyroscope", "Inertial Measurement Unit");
 
        agx::Real magnetometerMagnitude = agx::Vec3{
          v.magnetometer.x,
          v.magnetometer.y,
          v.magnetometer.z
        }.length();
        agxPlot::DebugPlot::add(magnetometerMagnitude, "Magnetometer", "Inertial Measurement Unit");
      }
    },
    simulation,
    imu,
    outputId);
  }
 
  return root;
}
 
osg::Group* buildTutorial2(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*app*/)
{
  auto root = new osg::Group();
 
  // Create an Environment and associate it with the Simulation.
  agxSensor::Environment* environment = agxSensor::Environment::getOrCreate(simulation);
 
  // Create the slope (the angled floor) as a static rigid body.
  agx::RigidBodyRef slope = new agx::RigidBody(new agxCollide::Geometry(
                                                new agxCollide::Box(10.0, 10.0, 0.5)));
  slope->setRotation(agx::EulerAngles(0.0, 3.14 * 0.2, 0.0));
  slope->setPosition(0.0, 0.0, -2.2);
  slope->setMotionControl(agx::RigidBody::STATIC);
 
  // Add the slope to the Simulation, Environment (optionally) and visualization.
  simulation->add(slope);
  environment->add(slope);
  agxOSG::createVisual(slope, root);
 
  // Create the bottom floor, for the accelerometer to settle on, as a static rigid body.
  agx::RigidBodyRef floor = new agx::RigidBody(new agxCollide::Geometry(
                                                 new agxCollide::Box(10.0, 10.0, 0.5)));
  floor->setPosition(18.0, 0.0, -8.0);
  floor->setMotionControl(agx::RigidBody::STATIC);
 
  // Add the bottom floor to the Simulation, Environment (optionally) and visualization.
  simulation->add(floor);
  environment->add(floor);
  agxOSG::createVisual(floor, root);
 
  // Create a regular dynamic rigid body to attach the accelerometer to.
  agx::RigidBodyRef accelerometerBody = new agx::RigidBody(new agxCollide::Geometry(
                                                             new agxCollide::Box(1.0, 1.0, 1.0)));
 
  // Angle the accelerometer rigid body such that it slides, and not rolls, down the slope.
  accelerometerBody->setRotation(slope->getRotation());
 
  // Add the accelerometer body to the Simulation, Environment (optionally) and visualization.
  simulation->add(accelerometerBody);
  environment->add(accelerometerBody);
  agxOSG::createVisual(accelerometerBody, root);
 
  // Visualize some axes on the accelerometer body to display the measurement axes.
  agxOSG::createAxes(accelerometerBody, nullptr, root, 4.0);
 
  // Create an accelerometer with the default parameters and attach it to the body's frame.
  agxSensor::AccelerometerRef accelerometer = new agxSensor::Accelerometer(
                                                accelerometerBody->getFrame(),
                                                new agxSensor::AccelerometerModel());
 
  // Add the accelerometer to the Environment
  environment->add(accelerometer);
 
  // Add a three-axis output to the accelerometer, and store the auto-assigned id.
  struct XYZ
  {
    agx::Real x, y, z;
  };
 
  size_t outputId = 0u; // (Default) 0 is considered "invalid".
  std::tie(outputId, std::ignore) = accelerometer->getOutputHandler()
                                                 ->add<XYZ,
                                                       agxSensor::TriaxialOutput::X_VALUE_F64,
                                                       agxSensor::TriaxialOutput::Y_VALUE_F64,
                                                       agxSensor::TriaxialOutput::Z_VALUE_F64>()
                                                 .value_or(std::make_pair(0u, nullptr));
 
  if (!g_isUnitTest) {
    // Add a post-step event callback to fetch the accelerometer measurements.
    agxUtil::StepEventCallback::post([](agxSDK::Simulation* /* simulation */,
                                        agxSensor::Accelerometer* accelerometer,
                                        size_t outputId)
    {
      agxSensor::BinaryOutputView<XYZ> output = accelerometer->getOutputHandler()
                                                             ->view<XYZ>(outputId);
 
      // Plot the accelerometer measurement values using the DebugPlot utility.
      for (const XYZ& v : output) {
        agxPlot::DebugPlot::add(v.x, "X", "Accelerometer");
        agxPlot::DebugPlot::add(v.y, "Y", "Accelerometer");
        agxPlot::DebugPlot::add(v.z, "Z", "Accelerometer");
      }
    },
    simulation,
    accelerometer,
    outputId);
  }
 
  return root;
}
 
osg::Group* buildTutorial3(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*app*/)
{
  auto root = new osg::Group();
 
  // Create an Environment and associate it with the Simulation.
  agxSensor::Environment* environment = agxSensor::Environment::getOrCreate(simulation);
 
  // Create the slope (the angled floor) as a static rigid body.
  agx::RigidBodyRef slope = new agx::RigidBody(new agxCollide::Geometry(
                                                new agxCollide::Box(10.0, 10.0, 0.5)));
  slope->setRotation(agx::EulerAngles(0.0, 3.14 * 0.2, 0.0));
  slope->setPosition(0.0, 0.0, -2.2);
  slope->setMotionControl(agx::RigidBody::STATIC);
 
  // Add the slope to the Simulation, Environment (optionally) and visualization.
  simulation->add(slope);
  environment->add(slope);
  agxOSG::createVisual(slope, root);
 
  // Create the bottom floor, for the gyroscope to settle on, as a static rigid body.
  agx::RigidBodyRef floor = new agx::RigidBody(new agxCollide::Geometry(
                                                 new agxCollide::Box(10.0, 10.0, 0.5)));
  floor->setPosition(18.0, 0.0, -8.0);
  floor->setMotionControl(agx::RigidBody::STATIC);
 
  // Add the bottom floor to the Simulation, Environment (optionally) and visualization.
  simulation->add(floor);
  environment->add(floor);
  agxOSG::createVisual(floor, root);
 
  // Create a regular dynamic rigid body to attach the gyroscope to.
  agx::RigidBodyRef gyroscopeBody = new agx::RigidBody(new agxCollide::Geometry(
                                                         new agxCollide::Box(1.0, 1.0, 1.0)));
 
  // Angle the gyroscope rigid body such that it slides, and not rolls, down the slope.
  gyroscopeBody->setRotation(slope->getRotation());
 
  // Add the gyroscope body to the Simulation, Environment (optionally) and visualization.
  simulation->add(gyroscopeBody);
  environment->add(gyroscopeBody);
  agxOSG::createVisual(gyroscopeBody, root);
 
  // Visualize some axes on the gyroscope body to display the measurement axes.
  agxOSG::createAxes(gyroscopeBody, nullptr, root, 4.0);
 
  // Create an gyroscope with the default parameters and attach it to the body's frame.
  agxSensor::GyroscopeRef gyroscope = new agxSensor::Gyroscope(gyroscopeBody->getFrame(),
                                                               new agxSensor::GyroscopeModel());
 
  // Add the gyroscope to the Environment
  environment->add(gyroscope);
 
  // Add disturbances from noise and linear acceleration effects to gyroscope output.
  gyroscope->getOutputHandler()->add(new agxSensor::GyroscopeLinearAccelerationEffects(
                                       agx::Vec3(2.62*1e-4)));
  gyroscope->getOutputHandler()->add(new agxSensor::TriaxialSpectralGaussianNoise(
                                       agx::Vec3(1.75*1e-4)));
 
  // Add a three-axis output to the gyroscope, and store the auto-assigned id.
  struct XYZ
  {
    agx::Real x, y, z;
  };
 
  size_t outputId = 0u; // (Default) 0 is considered "invalid".
  std::tie(outputId, std::ignore) = gyroscope->getOutputHandler()
                                             ->add<XYZ,
                                                   agxSensor::TriaxialOutput::X_VALUE_F64,
                                                   agxSensor::TriaxialOutput::Y_VALUE_F64,
                                                   agxSensor::TriaxialOutput::Z_VALUE_F64>()
                                             .value_or(std::make_pair(0u, nullptr));
 
  if (!g_isUnitTest) {
    // Add a post-step event callback to fetch the gyroscope measurements.
    agxUtil::StepEventCallback::post([](agxSDK::Simulation* /* simulation */,
                                        agxSensor::Gyroscope* gyroscope,
                                        size_t outputId)
    {
      agxSensor::BinaryOutputView<XYZ> output = gyroscope->getOutputHandler()
                                                         ->view<XYZ>(outputId);
 
      // Plot the gyroscope measurement values using the DebugPlot utility.
      for (const XYZ& v : output) {
        agxPlot::DebugPlot::add(v.x, "X", "Gyroscope");
        agxPlot::DebugPlot::add(v.y, "Y", "Gyroscope");
        agxPlot::DebugPlot::add(v.z, "Z", "Gyroscope");
      }
    },
    simulation,
    gyroscope,
    outputId);
  }
 
  return root;
}
 
osg::Group* buildTutorial4(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*app*/)
{
  auto root = new osg::Group();
 
  // Create an Environment and associate it with the Simulation.
  agxSensor::Environment* environment = agxSensor::Environment::getOrCreate(simulation);
 
  // Configure the magnetic field of the environment to that of a point on the Earth's surface.
  const agx::Vec3 magneticFieldVector = agx::Vec3(-19.462e-6, 44.754e-6, 7.8426e-6);
  environment->setMagneticField(new agxSensor::UniformMagneticField(magneticFieldVector));
 
  // Create the slope (the angled floor) as a static rigid body.
  agx::RigidBodyRef slope = new agx::RigidBody(new agxCollide::Geometry(
                                               new agxCollide::Box(10.0, 10.0, 0.5)));
  slope->setRotation(agx::EulerAngles(0.0, 3.14 * 0.2, 0.0));
  slope->setPosition(0.0, 0.0, -2.2);
  slope->setMotionControl(agx::RigidBody::STATIC);
 
  // Add the slope to the Simulation, Environment (optionally) and visualization.
  simulation->add(slope);
  environment->add(slope);
  agxOSG::createVisual(slope, root);
 
  // Create the bottom floor, for the magnetometer to settle on, as a static rigid body.
  agx::RigidBodyRef floor = new agx::RigidBody(new agxCollide::Geometry(
                                                 new agxCollide::Box(10.0, 10.0, 0.5)));
  floor->setPosition(18.0, 0.0, -8.0);
  floor->setMotionControl(agx::RigidBody::STATIC);
 
  // Add the bottom floor to the Simulation, Environment (optionally) and visualization.
  simulation->add(floor);
  environment->add(floor);
  agxOSG::createVisual(floor, root);
 
  // Create a regular dynamic rigid body to attach the magnetometer to.
  agx::RigidBodyRef magnetometerBody = new agx::RigidBody(new agxCollide::Geometry(
                                                          new agxCollide::Box(1.0, 1.0, 1.0)));
 
  // Angle the magnetometer rigid body such that it slides, and not rolls, down the slope.
  magnetometerBody->setRotation(slope->getRotation());
 
  // Add the magnetometer body to the Simulation, Environment (optionally) and visualization.
  simulation->add(magnetometerBody);
  environment->add(magnetometerBody);
  agxOSG::createVisual(magnetometerBody, root);
 
  // Visualize some axes on the magnetometer body to display the measurement axes.
  agxOSG::createAxes(magnetometerBody, nullptr, root, 4.0);
 
  // Create an magnetometer with the default parameters and attach it to the body's frame.
  agxSensor::MagnetometerRef magnetometer = new agxSensor::Magnetometer(
                                              magnetometerBody->getFrame(),
                                              new agxSensor::MagnetometerModel());
 
  // Add the magnetometer to the Environment
  environment->add(magnetometer);
 
  // Add disturbances from noise
  magnetometer->getOutputHandler()->add(new agxSensor::TriaxialGaussianNoise(agx::Vec3(4.5e-7)));
 
  // Add a three-axis output to the magnetometer, and store the auto-assigned id.
  struct XYZ
  {
    agx::Real x, y, z;
  };
 
  size_t outputId = 0u; // (Default) 0 is considered "invalid".
  std::tie(outputId, std::ignore) = magnetometer->getOutputHandler()
                                                ->add<XYZ,
                                                      agxSensor::TriaxialOutput::X_VALUE_F64,
                                                      agxSensor::TriaxialOutput::Y_VALUE_F64,
                                                      agxSensor::TriaxialOutput::Z_VALUE_F64>()
                                                .value_or(std::make_pair(0u, nullptr));
 
  if (!g_isUnitTest) {
    // Add a post-step event callback to fetch the magnetometer measurements.
    agxUtil::StepEventCallback::post([](agxSDK::Simulation* /* simulation */,
                                        agxSensor::Magnetometer* magnetometer,
                                        size_t outputId)
    {
      agxSensor::BinaryOutputView<XYZ> output = magnetometer->getOutputHandler()
                                                            ->view<XYZ>(outputId);
 
      // Plot the magnetometer measurement values using the DebugPlot utility.
      for (const XYZ& v : output) {
        agxPlot::DebugPlot::add(v.x, "X", "Magnetometer");
        agxPlot::DebugPlot::add(v.y, "Y", "Magnetometer");
        agxPlot::DebugPlot::add(v.z, "Z", "Magnetometer");
      }
    },
    simulation,
    magnetometer,
    outputId);
  }
 
  return root;
}
 
int main(int argc, char** argv)
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tLidar tutorial\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  g_isUnitTest = argc >= 2; // Do not show plots if we sent in arguments - this is only for the internal testing framework at Algoryx.
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene(buildTutorial1, '1');
    application->addScene(buildTutorial2, '2');
    application->addScene(buildTutorial3, '3');
    application->addScene(buildTutorial4, '4');
 
    if (application->init(argc, argv))
      return application->run();
  }
  catch (std::exception& e) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_io.cpp

/*
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.
*/
 
/*
 
Demonstrates various ways of reading/write resources from disk and memory.
 
*/
 
#include <agx/Logger.h>
 
#include <agxOSG/ExampleApplication.h>
#include <agxCollide/ConvexFactory.h>
#include <agxIO/ImageReaderPNG.h>
#include <agxIO/ReaderWriter.h>
#include <agxOSG/ReaderWriter.h>
 
#include <agxUtil/HeightFieldGenerator.h>
 
#include <agxCollide/Convex.h>
 
#include "tutorialUtils.h"
 
 
 
 
osg::Group* buildTutorial1( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  /* This is the file we will read.
   AGX uses the following approach for finding all resource files:
 
   AGX_ENVIRONMENT().getFilePath().find( fileName );
 
   This "filepath" can be altered using:
 
   All / will be changed to \ on windows and vice versa.
   When using drive letter just use: c:/path
 
   AGX_ENVIRONMENT().getFilePath().pushbackPath("myDataDir/somedir");
 
   There is an environment variable that is always used:
   AGX_FILE_PATH
 
   can be set as: set AGX_FILE_PATH=c:\mydatadir;c:\otherdatadir
 
   Then at initialization of AGX, the following will be executed, and
   hence reading paths from the environment variable:
 
   AGX_ENVIRONMENT().getFilePath().setFromEnvironmentVariable("AGX_FILE_PATH");
 
   */
 
  // If you are building this executable in a directory other than agx/bin
  // Then you might have to add path to AGX_FILE_PATH to locate the file below:
  // By default current directory, ..\data;..\data\models is searched for models.
  agx::String fileName = "pipe.obj";
 
  // Create a MeshReader for mesh files.
  agxIO::MeshReaderRef meshReader = new agxIO::MeshReader;
 
  // Read the file, returns number of read triangles, 0 means error:
  if (!meshReader->readFile( fileName ))
    LOGGER_ERROR() << "Unable to load file \'" << fileName << "\'" << LOGGER_END();
 
  // Create a ConvexFactory which will transform the read data into Convex Shapes.
  // Note: There are functions in agxUtil/ConvexReaderWriter.h which make creating agxCollide::Convex shapes easier.
  agxCollide::ConvexFactoryRef convexFactory = new agxCollide::ConvexFactory();
 
 
  // Let's create 10 bodies
  for(size_t n=0; n < 10; n++ )
  {
    // Create the dynamic rigid body (carries mass properties and interface to dynamic properties)
    agx::RigidBodyRef dynamicRB = new agx::RigidBody();
 
    // Set the position of the body.
    dynamicRB->setPosition( agx::Vec3( 0,0,agx::Real(n+2)));
 
    // Now tell the ConvexBuilder to build convex meshes with data from the MeshReader:
    size_t num_convex = convexFactory->build( meshReader, new agxCollide::ConvexFactory::VHACDParameters() );
    if (!num_convex) // 0 means no convex was created
    {
      LOGGER_ERROR() << "No Convex shapes could be generated from  \'" << fileName << "\'" << LOGGER_END();
    }
 
    // Get the vector of created Convex Shapes.
    const agxCollide::ConvexRefVector& convexShapes = convexFactory->getConvexShapes();
 
    // For each Shape, create a geometry.
    agxCollide::ConvexRefVector::const_iterator shapeIt = convexShapes.begin();
    for(; shapeIt != convexShapes.end(); ++shapeIt)
    {
      // Get the shape from the iterator.
      agxCollide::Shape* shape = shapeIt->get();
 
      // Create a new geometry
      agxCollide::GeometryRef dynamicRBGeometry = new agxCollide::Geometry();
 
      // Add the shape to the geometry
      dynamicRBGeometry->add( shape );
 
      // Add the geometry to the rigid body, telling that if this geometry collides with other geometries,
      // this rigid body will take the forces from these collisions.
      dynamicRB->add( dynamicRBGeometry );
 
      // Rotate the geometry 90 degrees around Y using an Euler representation (X,Y,Z)
      agx::EulerAngles euler(0,agx::degreesToRadians(90),0);
      dynamicRBGeometry->setRotation( euler );
 
      // Important: We have rotated the geometry, the body will not know about this rotation
      // so the inertia tensor and Center of Mass will be wrong. We have to tell the RigidBody to recalculate
      // Inertia and Center of Mass
      dynamicRB->updateMassProperties(agx::MassProperties::AUTO_GENERATE_ALL);
    }
 
    // As a last step we have to add the rigid body to the simulation.
    // Any geometries associated with the body will also be added to collision space.
    simulation->add( dynamicRB );
 
    // For rendering, associate visual geometries to the physical geometries in this rigid body
    agxOSG::createVisual( dynamicRB, root );
  }
 
  // Create the static rigid body (a floor onto which all other objects will fall)
  agx::RigidBodyRef staticRB = new agx::RigidBody();
  staticRB->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 15.0, 15.0, 0.2 ) ) ) );
  simulation->add( staticRB );
 
  // We haven't flagged the rigid body to be static yet, by default it's dynamic.
  // To change this is easy; in AGX this behavior is called motion control and can be set via a flag.
  staticRB->setMotionControl( agx::RigidBody::STATIC );
 
  // For rendering, associate visual geometries to the physical geometries in this rigid body
  agxOSG::createVisual( staticRB, root );
 
 
  // And just move the static body down so that the surface is at exactly z = 0. Remember that the
  // thickness, half extent, of the static body is 2 decimeters.
  staticRB->setPosition( agx::Vec3( 0, 0, -0.2 ) );
 
  return root;
}
 
osg::Group* buildTutorial2(  agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application  )
{
  application->setEnableDebugRenderer( false );
 
  agx::String filename = "heightfield_bowl.png";
 
  osg::Group* root = new osg::Group;
 
  // Create an image reader
  agxIO::ImageReaderRef ir = new agxIO::ImageReaderPNG();
 
  // Read in the image
  agxIO::ImageRef image = ir->readImage(filename);
 
  // Was reading of image successful?
  if (image)
  {
    agxCollide::GeometryRef heightFieldGeometry;
 
    // Create a geometry
    heightFieldGeometry = new agxCollide::Geometry();
 
 
    agxCollide::HeightFieldRef hf;
 
    // Create a HeightFieldGenerator that will create a HeightField shape from an image
    agxUtil::HeightFieldGeneratorRef hfg = new agxUtil::HeightFieldGenerator();
 
    // Now create the HeightField shape
    //
    const agx::Real realSizeX = 18;
    const agx::Real realSizeY = 18;
    const agx::Real lowestHeight = 0.5;
    const agx::Real highestHeight = 3.0;
    hf = hfg->createHeightFieldFromImage(image, realSizeX, realSizeY, lowestHeight, highestHeight);
 
    // Add the height field shape to the Geometry
    heightFieldGeometry->add(hf);
 
    // Having a body is not necessary for a geometry that should be static (non-movable).
    // But we have to create one just to simplify the rendering of it (we use the same
    // utility functions for creating all visual geometry, and it requires a body
    agx::RigidBodyRef heightFieldBody = new agx::RigidBody;
    heightFieldBody->setMotionControl(agx::RigidBody::STATIC);
    heightFieldBody->add( heightFieldGeometry );
 
    // Add the body to the simulation
    simulation->add( heightFieldBody );
    agxOSG::createVisual( heightFieldBody, root );
 
    // move it downwards a bit
    heightFieldBody->setPosition( agx::Vec3(0,0,-highestHeight));
 
  }
  else
    LOGGER_ERROR() << "Unable to read image: \'" << filename << "\'" << LOGGER_END();
 
 
 
  // Create a bunch of spheres that will be dropped onto the height field
  size_t nX=10;
  size_t nY=10;
  agx::Real radius = 0.2;
  for ( size_t i = 0; i < nX; ++i )
    for ( size_t j = 0; j < nY; ++j ) {
      agx::RigidBodyRef sphereBody = new agx::RigidBody;
      agxCollide::GeometryRef geometry = new agxCollide::Geometry(new agxCollide::Sphere(radius));
      sphereBody->add(geometry);
      sphereBody->setPosition( agx::Vec3(-(agx::Real(nX)*0.5*radius*2.1) + agx::Real(i) * radius * 2.1,
                                         -(agx::Real(nY)*0.5*radius*2.1) + agx::Real(j) * radius * 2.1,
                                         1.0 ));
      simulation->add( sphereBody );
      agxOSG::createVisual( sphereBody, root, true );
 
    }
 
 
  return root;
}
 
osg::Group* buildTutorial4(  agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application  )
{
 
  application->setEnableDebugRenderer( true );
 
 
 
  agx::String tmpFileName = agxIO::FileSystem::getTmpFilename();
  agx::String asciiFilename = tmpFileName+ ".aagx"; // Ascii
  agx::String binaryFilename = tmpFileName + ".agx";  // Binary
 
  osg::Group* root = new osg::Group;
 
 
  // Create a kinematic body (no geometry)
  agx::RigidBodyRef kinematicBody = new agx::RigidBody;
  kinematicBody->setMotionControl( agx::RigidBody::KINEMATICS );
  kinematicBody->setPosition( agx::Vec3(0,0,2 ) );
  simulation->add( kinematicBody );
 
  // Create a Dynamic Cylinder
  agx::RigidBodyRef cylinder = new agx::RigidBody;
  cylinder->add( new agxCollide::Geometry( new agxCollide::Cylinder( 0.05, 0.5 )));
  cylinder->setRotation( agx::Quat( M_PI_2, agx::Vec3(1,0,0)));
  cylinder->setPosition( agx::Vec3(0,0,0.5) );
  simulation->add( cylinder );
  cylinder->getGeometries().front()->addGroup( 2 );
 
  agxOSG::createVisual( cylinder, root );
 
 
  // Create a Dynamic Box
  agx::RigidBodyRef boxBody = new agx::RigidBody;
  boxBody->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3(0.2, 0.2, 0.2) )));
  boxBody->setPosition( agx::Vec3(0,0,1.5) );
  simulation->add( boxBody );
  boxBody->getGeometries().front()->addGroup( 1 );
 
  agxOSG::createVisual( boxBody, root );
 
 
  // Create the static rigid body (a floor onto which all other objects will fall)
  agx::RigidBodyRef staticRB = new agx::RigidBody();
  staticRB->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 3.0, 3.0, 0.02 ) ) ) );
  simulation->add( staticRB );
 
  // Make floor static
  staticRB->setMotionControl( agx::RigidBody::STATIC );
 
  // For rendering, associate visual geometries to the physical geometries in this rigid body
  agxOSG::createVisual( staticRB, root );
 
  // Disable collision in groups 1 and 2.
  simulation->getSpace()->setEnablePair( 1,2, false );
 
 
  // Now write the simulation to a file, the extension determines if it is a binary dump or an XML
  // binary is a lot faster and results in a smaller file.
  agxIO::writeFile( asciiFilename, simulation );
 
  // Clean the simulation from bodies, geometries etc.
  simulation->cleanup( agxSDK::Simulation::CleanupSelectionMask(agxSDK::Simulation::SYSTEM |  agxSDK::Simulation::SPACE |  agxSDK::Simulation::MATERIALS | agxSDK::Simulation::ASSEMBLIES));
   simulation->setDynamicsSystem( new agx::DynamicsSystem );
   simulation->setSpace( new agxCollide::Space );
 
  // If we want to read this file into the simulation again we could do:
  agxOSG::readFile( asciiFilename, simulation, root );
 
 
  return root;
}
 
 
#include <agxStream/RestoreListener.h>
 
class MyRestoreListener : public agxStream::RestoreListener
{
public:
  MyRestoreListener(osg::Group* root, const agx::Uuid& geomUuid, const agx::String& geomName) :
    m_root(root), m_geomUuid(geomUuid), m_geomName(geomName)
  {
  }
 
  // Will be called upon each restored object
  void restore(agxStream::Serializable* obj, const agx::String& className);
 
  osg::ref_ptr<osg::Group> m_root;
  agx::Uuid m_geomUuid;
  agx::String m_geomName;
};
 
 
void MyRestoreListener::restore(agxStream::Serializable* obj, const agx::String& className)
{
 
  // We are only interested in geometries, lets filter those out.
  if (className == "agxCollide::Geometry")
  {
    agxCollide::Geometry* geometry = dynamic_cast<agxCollide::Geometry*>(obj);
    if (geometry)
    {
      std::cerr << "Creating visual for Geometry: " << geometry->getName() << std::endl;
 
      // Create some graphics for each geometry
      agxOSG::createVisual(geometry, m_root);
 
      if (geometry->getName() == m_geomName)
      {
        if (geometry->getUuid() == m_geomUuid)
          std::cerr << "Uuid for restored geometry matches!!: " << m_geomUuid << std::endl;
        else
          std::cerr << "ERROR: Uuid for restored geometry does not match!!" << std::endl;
 
      }
    }
  }
}
 
osg::Group* buildTutorial5(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
 
  application->setEnableDebugRenderer(true);
 
  osg::Group* root = new osg::Group;
 
  // Create a Dynamic Cylinder
  agx::RigidBodyRef cylinder = new agx::RigidBody;
  agxCollide::GeometryRef cylGeom = new agxCollide::Geometry(new agxCollide::Cylinder(0.05, 0.5));
 
  const agx::String cylinderGeometryName = "CylinderGeometry";
  cylGeom->setName(cylinderGeometryName);
 
  // Let's store the uuid (Universally unique identifier) for this geometry.
  agx::Uuid cylinderGeometryUuid = cylGeom->getUuid();
 
 
  cylinder->add(cylGeom);
  cylinder->setRotation(agx::Quat(M_PI_2, agx::Vec3(1, 0, 0)));
  cylinder->setPosition(agx::Vec3(0, 0, 0.5));
  simulation->add(cylinder);
  cylinder->getGeometries().front()->addGroup(2);
 
 
  // Create a Dynamic Box
  agx::RigidBodyRef boxBody = new agx::RigidBody;
  agxCollide::GeometryRef boxGeom = new agxCollide::Geometry(new agxCollide::Box(agx::Vec3(0.2, 0.2, 0.2)));
  boxGeom->setName("BoxGeometry");
  boxBody->add( boxGeom );
  boxBody->setPosition(agx::Vec3(0, 0, 1.5));
  simulation->add(boxBody);
  boxBody->getGeometries().front()->addGroup(1);
 
 
  // Create the static rigid body (a floor onto which all other objects will fall)
  agx::RigidBodyRef staticRB = new agx::RigidBody();
  agxCollide::GeometryRef staticBoxGeom = new agxCollide::Geometry(new agxCollide::Box(agx::Vec3(3.0, 3.0, 0.02)));
  staticBoxGeom->setName("StaticBoxGeometry");
  staticRB->add( staticBoxGeom );
  simulation->add(staticRB);
 
  // Make floor static
  staticRB->setMotionControl(agx::RigidBody::STATIC);
 
  // Disable collision in groups 1 and 2.
  simulation->getSpace()->setEnablePair(1, 2, false);
 
#if 1 // Set to zero if you would like to see the scene before we store/restore it.
 
  // Remove all the visual stuff we added above, we will build it up again during restore
  root->removeChild(0, root->getNumChildren());
 
  // Now serialize to a memory stream
  std::stringstream os(std::ios_base::out | std::ios_base::binary);
 
  // Now write the simulation to the stream. The second boolean argument determines if the
  // serialization format should be binary or ASCII. Use ASCII for faster process and smaller memory footprint.
  bool binaryFormat = true;
  simulation->write(os, binaryFormat);
 
  // Clean the simulation from bodies, geometries etc.
  simulation->cleanup(agxSDK::Simulation::CleanupSelectionMask(agxSDK::Simulation::SYSTEM | agxSDK::Simulation::SPACE | agxSDK::Simulation::MATERIALS | agxSDK::Simulation::ASSEMBLIES));
  simulation->setDynamicsSystem(new agx::DynamicsSystem);
  simulation->setSpace(new agxCollide::Space);
 
  // Restore the simulation from memory
  os.seekg(0);
 
  // We need to get the data into a istream to be able to read from it
  std::stringstream is(std::ios_base::in | std::ios_base::binary);
  is.str(os.str());
 
  // Create and register our restore listener.
  // It will get a callback for each restored object, after it is restored.
  // It will in our case associate each geometry with rendering information.
  simulation->addRestoreListener(new MyRestoreListener(root, cylinderGeometryUuid, cylinderGeometryName));
 
  // Now read from the binary stream. Our restore listener will get callbacks during the process.
  simulation->read(is, binaryFormat);
#endif
  return root;
}
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout <<
                "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
                "\tIO tutorial\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
    application->addScene( buildTutorial4, '4' );
    application->addScene( buildTutorial5, '5' );
 
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_lidar.cpp

/*
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.
*/
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/LidarOutputRenderer.h>
#include <agxOSG/utils.h>
 
#include <agxUtil/StepEventCallback.h>
 
#include <agxSensor/Environment.h>
#include <agxSensor/Lidar.h>
#include <agxSensor/LidarModelOusterOS.h>
#include <agxSensor/RaytraceLambertianOpaqueMaterial.h>
#include <agxSensor/LidarRayPatternSingleRay.h>
#include <agxSensor/RaytraceConfig.h>
 
#include <agxSDK/Simulation.h>
#include <agxSDK/StepEventListener.h>
 
#include <agxRender/RenderManager.h>
#include <agxRender/RenderSingleton.h>
 
#include <agx/version.h>
 
#include <agxCollide/Box.h>
#include <agxCollide/Cylinder.h>
#include <agxCollide/Capsule.h>
#include <agxCollide/Sphere.h>
#include <agxCollide/Cone.h>
#include <agxCollide/HollowCone.h>
#include <agxCollide/HollowCylinder.h>
#include <agxCollide/WireShape.h>
 
namespace tutorial_helpers
{
  agx::RigidBodyRefVector createPrimitiveShapeCircle(agx::Real circleRadius,
                                                     agx::Real scale = 1.0)
  {
    agxCollide::ShapeRefVector shapes = {
      new agxCollide::Box(scale * 0.500, scale * 0.500, scale * 0.500),
      new agxCollide::Box(scale * 0.330, scale * 0.250, scale * 0.850),
      new agxCollide::Box(scale * 0.700, scale * 0.600, scale * 0.300),
      new agxCollide::Cylinder(scale * 0.200, scale * 0.750),
      new agxCollide::Cylinder(scale * 0.530, scale * 1.365),
      new agxCollide::Cylinder(scale * 0.050, scale * 1.665),
      new agxCollide::Capsule(scale * 0.423, scale * 1.389),
      new agxCollide::Capsule(scale * 0.210, scale * 0.450),
      new agxCollide::Capsule(scale * 0.930, scale * 1.030),
      new agxCollide::Sphere(scale * 0.150),
      new agxCollide::Sphere(scale * 0.470),
      new agxCollide::Sphere(scale * 0.820),
      new agxCollide::Cone(scale * 0.050, scale * 0.650, scale * 1.250),
      new agxCollide::Cone(scale * 0.000, scale * 0.750, scale * 0.720),
      new agxCollide::Cone(scale * 0.200, scale * 0.250, scale * 0.420),
      new agxCollide::HollowCone(scale * 0.120, scale * 0.380, scale * 1.310, scale * 0.025),
      new agxCollide::HollowCone(scale * 0.100, scale * 0.480, scale * 1.630, scale * 0.045),
      new agxCollide::HollowCone(scale * 0.300, scale * 0.830, scale * 0.730, scale * 0.140),
      new agxCollide::HollowCylinder(scale * 0.350, scale * 1.250, scale * 0.120),
      new agxCollide::HollowCylinder(scale * 0.720, scale * 0.150, scale * 0.320),
      new agxCollide::HollowCylinder(scale * 0.080, scale * 1.010, scale * 0.015),
      new agxCollide::WireShape(scale * 0.423, scale * 1.389),
      new agxCollide::WireShape(scale * 0.210, scale * 0.450),
      new agxCollide::WireShape(scale * 0.930, scale * 1.030),
    };
 
    agx::RigidBodyRefVector bodies;
    agx::UniformRealGenerator randomReal{ 123u, -0.75, 0.75 };
    agx::AffineMatrix4x4 rbTransform{ agx::Quat(), circleRadius * agx::Vec3::X_AXIS() };
    for (agxCollide::Shape* shape : shapes) {
      agx::RigidBodyRef rb = new agx::RigidBody(new agxCollide::Geometry(shape));
      rb->setMotionControl(agx::RigidBody::KINEMATICS);
      rb->setTransform(rbTransform);
      rb->setAngularVelocity(randomReal(), randomReal(), randomReal());
 
      bodies.push_back(rb);
 
      rbTransform *= agx::AffineMatrix4x4::rotate(0.25, agx::Vec3::Z_AXIS());
    }
 
    return bodies;
  }
 
  class CustomSingleRayLidarModel : public agxSensor::LidarModel
  {
    public:
      CustomSingleRayLidarModel( agx::RangeReal32 range = { 0.0f, 1000.0f } )
        : agxSensor::LidarModel( new agxSensor::LidarRayPatternSingleRay(),
                                 range )
      {
      }
 
      virtual ~CustomSingleRayLidarModel() = default;
  };
}
 
osg::Group* buildTutorial1(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* app)
{
  using namespace tutorial_helpers;
  auto root = new osg::Group();
 
  // Set positions integrated in PRE so that they are "seen" in the Lidar output in the same step.
  simulation->setPreIntegratePositions(true);
 
  // Create an Environment and associate it with the Simulation.
  agxSensor::Environment* environment = agxSensor::Environment::getOrCreate(simulation);
 
  agx::RigidBodyRefVector bodies = createPrimitiveShapeCircle(/*scale*/ 10.0);
  for (agx::RigidBody* rb : bodies) {
    // Notice that the Rigid Bodies are added both to the Simulation and Environment.
    simulation->add(rb);
    agxOSG::createVisual(rb, root);
    environment->add(rb);
  }
 
  // Back wall.
  agx::RigidBodyRef rb = new agx::RigidBody(new agxCollide::Geometry(new agxCollide::Box(40, 0.1, 100)));
  rb->setMotionControl(agx::RigidBody::KINEMATICS);
  rb->setPosition(0.0, 40.0, 0.0);
  simulation->add(rb);
  environment->add(rb);
  agxOSG::createVisual(rb, root);
 
  // Create a Rigid Body to attach the Lidar to.
  agx::RigidBodyRef lidarRb = new agx::RigidBody();
  lidarRb->setMotionControl(agx::RigidBody::KINEMATICS);
  simulation->add(lidarRb);
 
  // Create a Lidar using a pre-defined Lidar Model and add it to the Environment.
  agxSensor::LidarRef lidar = new agxSensor::Lidar(lidarRb->getFrame(),
                                                   new agxSensor::LidarModelOusterOS1());
  environment->add(lidar);
 
  // Create a renderer for the Lidar output.
  auto renderer = new agxOSG::LidarOutputRenderer(lidar);
  root->addChild(renderer);
 
  // Add an output "is hit" 0 or 1. By default, misses (i.e., lidar rays not hitting anything)
  // are removed to reduce the amount of data being syncrhonized. See tutorial 2 for
  // more information about the outputs and how to configure them.
  struct IsHitOutput
  {
    int32_t isHit;
  };
  lidar->getOutputHandler()->add<IsHitOutput,
                                 agxSensor::RtOutput::IS_HIT_I32>();
 
  // Display the number of hits from the rays.
  agxUtil::StepEventCallback::post( []( agxSDK::Simulation* simulation,
                                        agxSensor::LidarRef lidar,
                                        agxOSG::ExampleApplication* app )
  {
    app->getSceneDecorator()->setText( 0,
                                       agx::String::format( "Time stamp: %f, number of points: %d",
                                       simulation->getTimeStamp(),
                                       lidar->getOutputHandler()->view<IsHitOutput>().size() ) );
  },
  simulation,
  lidar,
  app );
 
  return root;
}
 
osg::Group* buildTutorial2( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*app*/ )
{
  auto root = new osg::Group();
 
  // Create the sensor environment.
  auto environment = agxSensor::Environment::getOrCreate( simulation );
 
  // Integrate transforms before collision detection so that the Lidar output is
  // in sync with the geometry/bodies in the simulation.
  simulation->setPreIntegratePositions( true );
 
  // Enable debug rendering for rendering of our additional outputs.
  simulation->getRenderManager()->setEnable( true );
 
  // OS1 model located at world origin.
  agxSensor::LidarRef lidar = new agxSensor::Lidar( new agx::Frame(),
                                                    new agxSensor::LidarModelOusterOS1() );
  environment->add( lidar );
 
  // And the ciricle of primitive shaped bodies, both in simulation (for integration) and
  // in the sensor environment for the lidar to see them.
  auto bodies = tutorial_helpers::createPrimitiveShapeCircle( 25.0, 3.0 );
  for ( const auto& rb : bodies ) {
    simulation->add( rb );
    environment->add( rb );
  }
 
  // Output 1: Hit point (given in lidar frame = world frame in this example) and intensity.
  //           In total four floats, three for XYZ_VEC3_F32 and one for INTENSITY_F32.
  struct HitPointAndIntensity : agx::Vec4f
  {
    agx::Vec3 position() const
    {
      return agx::Vec3{ asVec3() };
    }
 
    float intensity() const
    {
      return w();
    }
  };
 
  // Build the output by fields matching our struct HitPointAndIntensity. The order
  // and sizes are important, but the output handler will issue warning and ignore
  // the output if the size doesn't match ('add' returns nullptr).
  lidar->getOutputHandler()->add<HitPointAndIntensity,
                                 agxSensor::RtOutput::XYZ_VEC3_F32,
                                 agxSensor::RtOutput::INTENSITY_F32>();
 
  // Rendering Output 1: Rays with hits rendered as red lines, hits as small spheres colored red -> green
  //                     given intensity (red low, green high).
  agxUtil::StepEventCallback::post( []( agxSDK::Simulation*, agxSensor::LidarRef lidar )
  {
    auto hitPointAndIntensityView = lidar->getOutputHandler()->view<HitPointAndIntensity>();
    for ( const auto& hitPointAndIntensity : hitPointAndIntensityView ) {
      // Ray hit, line from origin to hit point.
      agxRender::RenderSingleton::instance()->add( agx::Vec3{ 0, 0, 0 },
                                                   hitPointAndIntensity.position(),
                                                   0.003,
                                                   agxRender::Color::Red() );
      // Hit point sphere, color by intensity. Scale the raw intensity
      // to represent something in the range [0, 1] for low to high.
      const float intensityScaled = std::min( 1.5E2f * hitPointAndIntensity.intensity(), 1.0f );
      agxRender::RenderSingleton::instance()->add( hitPointAndIntensity.position(),
                                                   0.012,
                                                   agxRender::Color{ 1.0f - intensityScaled,
                                                                     intensityScaled,
                                                                     0.3f } );
    }
  },
  simulation,
  lidar );
 
  // Output 2: Hit point and normal (both given in the frame of the lidar - world frame in this example).
  //           Six floats in total, three for XYZ_VEC3_F32 and three for NORMAL_VEC3_F32. Since we use
  //           two agx::Vec3f (four floats each) we have to add padding between and after the fields.
  struct HitPointAndNormal
  {
    agx::Vec3f position;
    agx::Vec3f normal;
  };
 
  // Build the output by fields matching our struct HitPointAndNormal. When our HitPointAndNormal is
  // (float[4], float[4]) in size, we have to build our output fields with 4 byte padding after both
  // XYZ_VEC3_F32 and NORMAL_VEC3_F32.
  lidar->getOutputHandler()->add<HitPointAndNormal,
                                 agxSensor::RtOutput::XYZ_VEC3_F32,
                                 agxSensor::RtOutput::PADDING_32,
                                 agxSensor::RtOutput::NORMAL_VEC3_F32,
                                 agxSensor::RtOutput::PADDING_32>();
 
  // Rendering Output 2: Normal direction (blue line) at hit point.
  agxUtil::StepEventCallback::post( []( agxSDK::Simulation*, agxSensor::LidarRef lidar )
  {
    auto hitPointAndNormalView = lidar->getOutputHandler()->view<HitPointAndNormal>();
    for ( const auto& hitPointAndNormal : hitPointAndNormalView ) {
      agxRender::RenderSingleton::instance()->add( agx::Vec3{ hitPointAndNormal.position },
                                                   agx::Vec3{ hitPointAndNormal.position + 0.25f * hitPointAndNormal.normal },
                                                   0.006,
                                                   agxRender::Color::Blue() );
    }
  },
  simulation,
  lidar );
 
  return root;
}
 
namespace myComponents
{
  struct ColorComponent : agxRender::Color
  {
    static constexpr float DefaultValue = 1.0f;
 
    using agxRender::Color::Color;
    using agxRender::Color::operator=;
  };
 
  struct NameComponent : std::string
  {
    static constexpr char DefaultValue{};
 
    using std::string::string;
    using std::string::operator=;
  };
 
  struct MyComponentsInterface
  {
    template<typename T>
    static MyComponentsInterface get( T& instance )
    {
      // Works for agxCollide::Shape, agxCollide::Geometry and agx::RigidBody.
      // Propagation and sharing of id works as for surface materials,
      // Tutorial Lidar 4 - Surface material parameter(s).
      return { agxSensor::manageRtEntityId( instance ) };
    }
 
    MyComponentsInterface() = default;
 
    MyComponentsInterface( agxSensor::RtEntityId entity )
      : m_entity( entity )
    {
    }
 
    std::string getName() const
    {
      // If the data storage doesn't exist, the default value will
      // be returned for m_entity.
      return agxSensor::RtRegistry::storage<NameComponent>().get( m_entity );
    }
 
    void setName( std::string name )
    {
      agxSensor::RtRegistry::getOrCreateStorage<NameComponent>()[ m_entity ] = name;
    }
 
    agxRender::Color getColor() const
    {
      return agxSensor::RtRegistry::storage<ColorComponent>().get( m_entity );
    }
 
    void setColor( agxRender::Color color )
    {
      agxSensor::RtRegistry::getOrCreateStorage<ColorComponent>()[ m_entity ] = color;
    }
 
    private:
      agxSensor::RtEntityId m_entity = agxSensor::NullRtEntityId;
  };
}
 
osg::Group* buildTutorial3( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* app )
{
  auto root = new osg::Group();
 
  // Create the sensor environment.
  auto environment = agxSensor::Environment::getOrCreate( simulation );
 
  // Integrate transforms before collision detection so that the Lidar output is
  // in sync with the geometry/bodies in the simulation.
  simulation->setPreIntegratePositions( true );
 
  // And the ciricle of primitive shaped bodies, both in simulation (for integration) and
  // in the sensor environment for the lidar to see them.
  auto bodies = tutorial_helpers::createPrimitiveShapeCircle( 25.0, 3.0 );
  agxRender::ColorVector colors;
  for ( const auto& rb : bodies ) {
    simulation->add( rb );
    environment->add( rb );
 
    colors.push_back( agxRender::Color::getColor( colors.size() + 17u ) );
    agxOSG::setDiffuseColor( agxOSG::createVisual( rb, root ), colors.back() );
  }
 
  // Two components for each entity, name and color. The interface to them below.
  // Interface to the data - similar to agxSensor::RtSurfaceMaterial.
  using namespace myComponents; // <- go to definition of this namespace for the implementation.
 
  // Set name and color of the entities. The rigid bodies will be the owner
  // but its geometry and shape shares id and data.
  for ( size_t i = 0u; i < bodies.size(); ++i ) {
    const auto& rb = bodies[ i ];
    auto components = MyComponentsInterface::get( rb );
    components.setName( rb->getGeometries()[ 0 ]->getShapes()[ 0 ]->getClassName() );
    components.setColor( colors[ i ] );
 
    // All ids should be the same and not agxSensor::NullRtEntityId. So there shouldn't
    // be any "ERROR:" print outs in the console.
    const auto rbId = agxSensor::getRtEntityId( rb );
    const auto geometryId = agxSensor::getRtEntityId( rb->getGeometries()[ 0 ] );
    const auto shapeId = agxSensor::getRtEntityId( rb->getGeometries()[ 0 ]->getShapes()[ 0 ] );
    if ( rbId == agxSensor::NullRtEntityId )
      std::cout << "ERROR: Unexpected rigid body entity id is agxSensor::NullRtEntityId.\n";
    else if ( rbId != geometryId )
      std::cout << "ERROR: Unexpected geometry entity id is not the same as its rigid body.\n";
    else if ( geometryId != shapeId )
      std::cout << "ERROR: Unexpected shape entity id is not the same as its geometry.\n";
  }
 
  // Create a single ray lidar on a kinematically rotating rigid body at world origin.
  agx::RigidBodyRef lidarRb = new agx::RigidBody( "lidar" );
  lidarRb->setMotionControl( agx::RigidBody::KINEMATICS );
  lidarRb->setAngularVelocity( 0.0, 0.0, 1.0 );
  simulation->add( lidarRb );
 
  // The ray of the single ray pattern generator is along (lidar frame) local z axis.
  // Rotate z axis to x axis from the lidarRb frame to make the ray rotate in the world
  // xy plane.
  agx::FrameRef lidarFrame = new agx::Frame( agx::AffineMatrix4x4::rotate( agx::Vec3::Z_AXIS(),
                                                                           agx::Vec3::X_AXIS() ) );
  lidarFrame->setParent( lidarRb->getFrame() );
  agxSensor::LidarRef lidar = new agxSensor::Lidar( lidarFrame,
                                                    new tutorial_helpers::CustomSingleRayLidarModel() );
  environment->add( lidar );
 
  // The only output we're interested in is the agxSensor::RtOutput::ENTITY_ID_I32, but
  // it may be any number of outputs packed into the custom structure.
  struct HitEntityId
  {
    int32_t entityId;
  };
  lidar->getOutputHandler()->add<HitEntityId,
                                 agxSensor::RtOutput::ENTITY_ID_I32>();
 
  // Render basic lidar outputs.
  auto lidarRenderer = new agxOSG::LidarOutputRenderer( lidar, true );
  lidarRenderer->setHitPointScale( 4.0f );
  root->addChild( lidarRenderer );
 
  // Display hit components given entity id of the configured lidar output. If the ray
  // is hitting an objects, we print "Hit" and name + id in the color of the shape (upper
  // left corner of the screen).
  agxUtil::StepEventCallback::post( []( agxSDK::Simulation* /*simulation*/,
                                        agxOSG::ExampleApplication* app,
                                        agxSensor::LidarRef lidar )
  {
    static const auto noColorColor = agxRender::Color::LightGray();
    auto sd = app->getSceneDecorator();
 
    // The current hit data view of our HitEntityId output.
    auto entityIdView = lidar->getOutputHandler()->view<HitEntityId>();
    // No hit - clear strings and display "No Hit".
    if ( entityIdView.empty() ) {
      sd->setText( 0, "No Hit", noColorColor );
      sd->setText( 1, "", noColorColor );
      sd->setText( 2, "", noColorColor );
    }
    else {
      // Assuming single hit when we only have one ray in our lidar.
      const auto hitEntityId = entityIdView[ 0 ].entityId;
 
      // Load components given entity id. The size of MyComponentsInterface is only our
      // int32_t id, the data is stored in the agxSensor::RtRegistry and fetched when the
      // method of the component is called.
      //
      // In this example all meshes/shapes in the sensor environment has an entity id. Indexing
      // or getting components of agxSensor::NullRtEntityId is valid since the default value is
      // returned and the default value has to be defined in the component (DefaultValue). But
      // entities that doesn't have an id will have agxSensor::MaxRtEntityId (not NullRtEntityId),
      // but since it's the maximum, we can do modulus max to get the default from NullRtEntityId.
      MyComponentsInterface components{ (agxSensor::RtEntityId)( hitEntityId % agxSensor::MaxRtEntityIdIndex ) };
 
      sd->setText( 0, "Hit:", noColorColor );
      sd->setText( 1, agx::String::format( "  name: %s", components.getName().c_str() ), components.getColor() );
      sd->setText( 2, agx::String::format( "  id:   %d", hitEntityId ), components.getColor() );
    }
  },
  simulation,
  app,
  lidar );
 
  return root;
}
 
osg::Group* buildTutorial4( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*app*/ )
{
  auto root = new osg::Group();
 
  // Create the sensor environment.
  auto environment = agxSensor::Environment::getOrCreate( simulation );
 
  // Integrate transforms before collision detection so that the Lidar output is
  // in sync with the geometry/bodies in the simulation.
  simulation->setPreIntegratePositions( true );
 
  /*
  // Create a rigid body with three geometries - geometry 1 (g1) has one cylinder shape (cy1),
  // geometry 2 (g2) has two capsule shapes (ca1, ca2) and geometry 3 (g3) has three sphere
  // shapes (sp1, sp2, sp3).
  //
  //       v-------------- rb --------------v
  //       |                |               |
  //      g1               g2              g3
  //       |              /  \             /|\
  //       |             /    \           / | \
  //      cy1          ca1    ca2      sp1 sp2 sp3
  //
  */
  agx::RigidBodyRef rb = new agx::RigidBody();
  rb->setMotionControl( agx::RigidBody::STATIC );
  rb->setPosition( -0.25, 5, 0 );
 
  agxCollide::GeometryRef g1 = new agxCollide::Geometry();
  g1->add( new agxCollide::Cylinder( 0.5, 1.0 ),
           agx::AffineMatrix4x4::rotate( agx::Vec3::Y_AXIS(),
                                         agx::Vec3::X_AXIS() ) );
 
  agxCollide::GeometryRef g2 = new agxCollide::Geometry();
  g2->add( new agxCollide::Capsule( 0.5, 1.0 - 0.35 ),
           agx::AffineMatrix4x4::rotate( agx::Vec3::Y_AXIS(),
                                         agx::Vec3::X_AXIS() ) *
           agx::AffineMatrix4x4::translate( -0.85, 0, 0 ) );
  g2->add( new agxCollide::Capsule( 0.5, 1.0 - 0.35 ),
           agx::AffineMatrix4x4::rotate( agx::Vec3::Y_AXIS(),
                                         agx::Vec3::X_AXIS() ) *
           agx::AffineMatrix4x4::translate( 0.85, 0, 0 ) );
 
  agxCollide::GeometryRef g3 = new agxCollide::Geometry();
  g3->add( new agxCollide::Sphere( 0.45 ),
           agx::AffineMatrix4x4::translate( -1.0, 0, 0 ) );
  g3->add( new agxCollide::Sphere( 0.45 ),
           agx::AffineMatrix4x4::translate( 0.0, 0, 0 ) );
  g3->add( new agxCollide::Sphere( 0.45 ),
           agx::AffineMatrix4x4::translate( 1.0, 0, 0 ) );
 
  rb->add( g1, agx::AffineMatrix4x4::translate( -3, 0, 0 ) );
  rb->add( g2, agx::AffineMatrix4x4::translate( -0.5, 0, 0 ) );
  rb->add( g3, agx::AffineMatrix4x4::translate( 3, 0, 0 ) );
 
  simulation->add( rb );
  environment->add( rb );
 
  // We've defined a hierarchy where geometries and shapes (and also bodies)
  // can inherit a surface material from a "parent" object, e.g., a geometry
  // of a rigid body or a shape of a geometry. A rigid body may inherit material
  // parameters from, e.g., linked structures such as agxCable::Cable, agxModel::Beam,
  // agxVehicle::Track etc.
  //
  // This assurance function makes sures both instances has a valid surface material
  // or an error is printed.
  const auto assureIdenticalSurfaceMaterial = []( const auto instance1, const auto instance2 )
  {
    // Assure the argments has an entity id, i.e., they can carry data.
    const auto sm1 = agxSensor::RtLambertianOpaqueMaterial::get( instance1 );
    const auto sm2 = agxSensor::RtLambertianOpaqueMaterial::get( instance2 );
    if ( !sm1.isValid() ) {
      std::cout << "ERROR: Surface material for instance \""
                << instance1->template asSafe<agxStream::Serializable>()->getClassName()
                << "\" is invalid.\n";
      return;
    }
    if ( !sm2.isValid() ) {
      std::cout << "ERROR: Surface material for instance \""
                << instance2->template asSafe<agxStream::Serializable>()->getClassName()
                << "\" is invalid.\n";
      return;
    }
 
    if ( sm1 != sm2 ) {
      std::cout << "ERROR: The surface material between instance \""
                << instance1->template asSafe<agxStream::Serializable>()->getClassName()
                << "\" and \""
                << instance2->template asSafe<agxStream::Serializable>()->getClassName()
                << "\" isn't identical.\n";
    }
  };
 
  // Prints an error of the instance (agx::RigidBody, agxCollide::Geometry or
  // agxCollide::Shape) isn't owner of a surface material.
  const auto assureIsOwner = []( const auto instance )
  {
    if ( !agxSensor::isRtMaterialOwner( instance ) )
      std::cout << "ERROR: " << "Instance type "
                << instance->template asSafe<agxStream::Serializable>()->getClassName()
                << " isn't owner of the surface material.\n";
  };
 
  // All - rb, g1, g2, g3, cy1, ca1, ca2, sp1, sp2 and sp3 will all
  // inherit this surface material, 'rb' is the owner.
  agxSensor::RtLambertianOpaqueMaterial::create()
                                        .setReflectivity( 1.0f )
                                        .assignTo( rb );
 
  // Print "ERROR" if a material pair isn't the same. All geometries and
  // shapes should share the same material.
  for ( const auto& geometry : rb->getGeometries() ) {
    assureIdenticalSurfaceMaterial( rb, geometry );
    for ( const auto& shape : geometry->getShapes() ) {
      assureIdenticalSurfaceMaterial( geometry, shape );
      assureIdenticalSurfaceMaterial( rb, shape );
    }
  }
 
  // Middle geometry g2, containing two capsules, reduce the reflectivity
  // by making g2 owner of a new material.
  agxSensor::RtLambertianOpaqueMaterial::create()
                                        .setReflectivity( 0.5f )
                                        .assignTo( g2 );
 
  // Both capsules are now inheriting the material of their geometry g2.
  // I.e., no longer reflectivity 1.0, it's now 0.5 from g2Material.
  for ( const auto& g2Shape : g2->getShapes() )
    assureIdenticalSurfaceMaterial( g2, g2Shape );
 
  // Right capsule ca2 with reduced reflectivity compared to ca1 that
  // is still inheriting its material from g2.
  agxSensor::RtLambertianOpaqueMaterial::create()
                                        .setReflectivity( 0.25f )
                                        .assignTo( g2->getShapes()[ 1 ] );
 
  // Right geometry, containing three spheres, reduce the reflectivity
  // by making g3 owner of a new material.
  agxSensor::RtLambertianOpaqueMaterial::create()
                                        .setReflectivity( 0.15f )
                                        .assignTo( g3 );
 
  // Middle and right sphere both owners of surface materials with
  // custom reflectivity.
  agxSensor::RtLambertianOpaqueMaterial::create()
                                        .setReflectivity( 0.075f )
                                        .assignTo( g3->getShapes()[ 1 ] );
 
  agxSensor::RtLambertianOpaqueMaterial::create()
                                        .setReflectivity( 0.005f )
                                        .assignTo( g3->getShapes()[ 2 ] );
 
  /*
  // Given this initial tree configuration of our rigid body:
  //
  //       v-------------- rb --------------v
  //       |                |               |
  //      g1               g2              g3
  //       |              /  \             /|\
  //       |             /    \           / | \
  //      cy1          ca1    ca2      sp1 sp2 sp3
  //
  // The reflectivity should be reducing from 1.0 at cy1 to 0.005 at sp3. Given
  // our configuration of the surface materials above, we know that:
  */
  // Geometry g1 and its shape cy1 are both inheriting 1.0 reflectivity from rb.
  assureIsOwner( rb );
  assureIdenticalSurfaceMaterial( rb, g1 );
  assureIdenticalSurfaceMaterial( rb, g1->getShapes()[ 0 ] );
 
  // Geometry g2 and its left capsule ca1 are sharing material where g2 is the owner, and
  // its right capsule ca2 has it's own.
  assureIsOwner( g2 );
  assureIdenticalSurfaceMaterial( g2, g2->getShapes()[ 0 ] );
  assureIsOwner( g2->getShapes()[ 1 ] );
 
  // Similar for geometry g3, where its first sphere sp1 is sharing material with g3, and
  // both sp2 and sp3 has their own.
  assureIsOwner( g3 );
  assureIdenticalSurfaceMaterial( g3, g3->getShapes()[ 0 ] );
  assureIsOwner( g3->getShapes()[ 1 ] );
  assureIsOwner( g3->getShapes()[ 2 ] );
 
  agxSensor::LidarRef lidar = new agxSensor::Lidar( new agx::Frame(),
                                                    new agxSensor::LidarModelGeneric360HorizontalSweep( 60.0 ) );
  environment->add( lidar );
 
  // Create a renderer for the Lidar output.
  auto renderer = new agxOSG::LidarOutputRenderer( lidar );
  renderer->setIntensityScale( 10.0f );
  root->addChild( renderer );
 
  // So that we see the output of one turn when the window shows.
  simulation->stepForward();
 
  return root;
}
 
int main(int argc, char** argv)
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tLidar tutorial\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
    // The same return value as agxSensor::RtConfig::isRaytraceSupported() but
    // verifyRaytraceSupported prints warning (once) if not supported before
    // it returns.
    if ( !agxSensor::RtConfig::verifyRaytraceSupported() )
      return 0;
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene(buildTutorial1, '1');
    application->addScene(buildTutorial2, '2');
    application->addScene(buildTutorial3, '3');
    application->addScene(buildTutorial4, '4');
 
    if (application->init(argc, argv))
      return application->run();
  }
  catch (std::exception& e) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_listeners.cpp

/*
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.
*/
 
#include <agxOSG/ExampleApplication.h>
 
#include <agx/version.h>
 
#include <agxSDK/GeometryFilter.h>
 
#include "tutorialUtils.h"
 
 
// Keyboard event listener - inherit from agxSDK::GuiEventListener and
// implement the 'keyboard' method.
class SimpleKeyboardListener : public agxSDK::GuiEventListener
{
  public:
    SimpleKeyboardListener( agx::RigidBody* rb )
      : m_rb( rb )
    {
      // We only want to listen to keyboard events.
      setMask( agxSDK::GuiEventListener::KEYBOARD );
    }
 
    virtual bool keyboard( int key, unsigned int modKeyMask, float /*x*/, float /*y*/, bool keydown )
    {
      // Set handled = true if the key pressed, modKeyMask or keydown was of interest. Otherwise,
      // let it be false. If this method returns false any other keyboard listener executed after
      // this will get the event instead.
      bool handled = false;
 
      // If our rigid body has been deallocated, return directly.
      if ( !m_rb.isValid() )
        return false;
 
      // Angular speed to be set on our kinematic body.
      const agx::Real angularSpeed = 4;
 
      // Check if left CTRL key is down.
      bool leftCtrlDown = (modKeyMask & agxSDK::GuiEventListener::MODKEY_LEFT_CTRL) != 0;
 
      // Switch-case or if-else-if...
      switch ( key )
      {
        case agxSDK::GuiEventListener::KEY_Left:
          // If arrow key left is pressed, or is down, and left CTRL key also is down - set angular
          // velocity about negative y (local frame).
          // angularSpeed * keydown means that the object will rotate as long as you hold the key down.
          if ( leftCtrlDown )
            m_rb->setAngularVelocity( m_rb->getFrame()->transformVectorToWorld( agx::Vec3( 0, -angularSpeed*int( keydown ), 0 ) ) );
          // If the CTRL key isn't down, let it rotate about negative z (local frame)
          else
            m_rb->setAngularVelocity( m_rb->getFrame()->transformVectorToWorld( agx::Vec3( 0, 0, -angularSpeed*int( keydown ) ) ) );
          // We were interested in this event, handled = true.
          handled = true;
          break;
 
        case agxSDK::GuiEventListener::KEY_Right:
          if ( leftCtrlDown )
            m_rb->setAngularVelocity( m_rb->getFrame()->transformVectorToWorld( agx::Vec3( 0, angularSpeed*int( keydown ), 0 ) ) );
          else
            m_rb->setAngularVelocity( m_rb->getFrame()->transformVectorToWorld( agx::Vec3( 0, 0, angularSpeed*int( keydown ) ) ) );
          handled = true;
          break;
 
        case agxSDK::GuiEventListener::KEY_Up:
          m_rb->setAngularVelocity( m_rb->getFrame()->transformVectorToWorld( agx::Vec3( -angularSpeed*int( keydown ), 0, 0 ) ) );
          handled = true;
          break;
 
        case agxSDK::GuiEventListener::KEY_Down:
          m_rb->setAngularVelocity( m_rb->getFrame()->transformVectorToWorld( agx::Vec3(  angularSpeed*int( keydown ), 0, 0 ) ) );
          handled = true;
          break;
 
        case 'j':
          // If j key was pressed (just been pressed), reset the transform.
          if ( keydown ) {
            m_rb->setTransform( agx::AffineMatrix4x4() );
            handled = true;
          }
          break;
      }
 
      return handled;
    }
 
    // Other GUI event callbacks are:
    virtual bool mouseDragged( MouseButtonMask /*buttonMask*/, float /*x*/, float /*y*/ ) { return false; }
    virtual bool mouseMoved( float /*x*/, float /*y*/ ) { return false; }
    virtual bool mouse( MouseButtonMask /*button*/, MouseState /*state*/, float /*x*/, float /*y*/ ) { return false; }
    virtual void update( float /*x*/, float /*y*/ ) {}
 
  private:
    // Observer pointer to the rigid body
    agx::observer_ptr< agx::RigidBody > m_rb;
};
 
osg::Group* buildTutorial1( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create a rigid body cylinder
  agx::Real cylinderRadius = 1;
  agx::Real cylinderHeight = 3.0;
  agx::RigidBodyRef kinematicCylinder = new agx::RigidBody();
  kinematicCylinder->add( new agxCollide::Geometry( new agxCollide::Cylinder( cylinderRadius, cylinderHeight ) ) );
  agxOSG::createVisual( kinematicCylinder, root );
  simulation->add( kinematicCylinder );
 
  // Set motion control for it to be kinematic
  kinematicCylinder->setMotionControl( agx::RigidBody::KINEMATICS );
 
  // Add the keyboard event listener (class above) with kinematicCylinder as argument
  simulation->addEventListener( new SimpleKeyboardListener( kinematicCylinder ) );
 
  return root;
}
 
// This class will remove contacts between two geometries if the two geometries have been in impact
// state more than two times.
class MyContactListener : public agxSDK::ContactEventListener
{
  public:
    MyContactListener()
      : m_impactCounter( 0 )
    {
      // We want to listen to impact, contact and separation events between the two bodies.
      setMask( IMPACT | CONTACT | SEPARATION );
    }
 
    virtual KeepContactPolicy impact( const agx::TimeStamp& /*t*/, agxCollide::GeometryContact* c )
    {
      ++m_impactCounter;
      std::cout << "Impact: " << c->geometry(0)->getName() << " <-> " << c->geometry(1)->getName() << std::endl;
      std::cout << "Number of impacts: " << m_impactCounter;
      if ( m_impactCounter > 2 ) {
        std::cout << " is larger than 2, REMOVING CONTACT IMMEDIATELY!" << std::endl;
        return REMOVE_CONTACT_IMMEDIATELY;
      }
      std::cout << std::endl;
      return KEEP_CONTACT;
    }
 
    virtual KeepContactPolicy contact( const agx::TimeStamp& /*t*/, agxCollide::GeometryContact* c )
    {
      if ( m_impactCounter > 2 )
        return REMOVE_CONTACT_IMMEDIATELY;
      std::cout << "Contact: " << c->geometry(0)->getName() << " <-> " << c->geometry(0)->getName() << std::endl;
      return KEEP_CONTACT;
    }
 
    virtual void separation( const agx::TimeStamp& /*t*/, agxCollide::GeometryPair& cd )
    {
      if ( m_impactCounter > 2 )
        return;
      std::cout << "Separation: " << cd.first->getName() << " <-> " << cd.second->getName() << std::endl;
    }
 
  private:
    size_t m_impactCounter;
};
 
osg::Group* buildTutorial2( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create a static box geometry as in tutorial 1.4.
  agx::Vec3 boxGeometryHalfExtent( 15, 15, 0.5 );
  agxCollide::GeometryRef boxGeometry = new agxCollide::Geometry( new agxCollide::Box( boxGeometryHalfExtent ) );
  boxGeometry->setPosition( boxGeometry->getPosition() - agx::Vec3( 0, 0, boxGeometryHalfExtent.z() ) );
  simulation->add( boxGeometry );
  boxGeometry->setName( "boxGeometry" );
  agxOSG::createVisual( boxGeometry, root );
 
  // Create a rigid body sphere
  agx::RigidBodyRef sphere = new agx::RigidBody();
  agxCollide::GeometryRef sphereGeometry = new agxCollide::Geometry( new agxCollide::Sphere( 2.0 ) );
  sphereGeometry->setName( "sphereGeometry" );
  sphere->add( sphereGeometry );
  simulation->add( sphere );
  agxOSG::createVisual( sphere, root );
 
  sphere->setPosition( agx::Vec3( 0, 0, 8 ) );
 
  // Create the contact event listener (MyContactListener class above).
  // We also pass a geometry filter as argument, meaning that we're only
  // interested in contacts between these two geometries.
  MyContactListener *listener = new MyContactListener();
  listener->setFilter( new agxSDK::GeometryFilter( boxGeometry, sphereGeometry ) );
  simulation->addEventListener(  listener  );
 
  return root;
}
 
class MyStepEventListener : public agxSDK::StepEventListener
{
  public:
    MyStepEventListener( agx::RigidBody* rb, const agx::Vec3& targetPosition )
      : m_rb( rb ), m_targetPosition( targetPosition )
    {
      // Mask to tell which step events we're interested in. Available step events are:
      // Pre-collide: Before collision detection
      // Pre: After collision detection (contact list available) but before the solve stage
      // Post: After solve (transforms and velocities have been updated)
      setMask( PRE_COLLIDE | POST_STEP );
 
      if ( m_rb.isValid() )
        m_rb->setPosition( m_targetPosition );
    }
 
    virtual void preCollide( const agx::TimeStamp& /*t*/ )
    {
      if ( !m_rb.isValid() ) {
        getSimulation()->remove( this );
        return;
      }
 
      // Check if we're close to the target position.
      if ( agx::equalsZero( (m_targetPosition - m_rb->getPosition()).length2() ) ) {
        // Calculate new target position.
        m_targetPosition = m_targetPosition ^ agx::Vec3( 0, 1, 0 );
        // Calculate new target rotation.
        agx::AffineMatrix4x4 targetTransform = agx::AffineMatrix4x4(m_rb->getRotation() * agx::Quat( M_PI, agx::Vec3( agx::sign( m_targetPosition[ 0 ] ), 0, 0 ) ));
        targetTransform.setTranslate( m_targetPosition );
        // For kinematic bodies, moveTo can be used. It means move to this target transform in x seconds (1 second in this case).
        // Note that the kinematic body will not stop at the target transform.
        m_rb->moveTo( targetTransform, 1.0 );
      }
    }
 
    virtual void post( const agx::TimeStamp& /*t*/ )
    {
      std::cout << "Current position: " << m_rb->getPosition() << std::endl;
      std::cout << "Target position:  " << m_targetPosition << std::endl;
      std::cout << "Distance left:    " << ( m_rb->getPosition() - m_targetPosition ).length() << std::endl << std::endl;
    }
 
  private:
    agx::observer_ptr< agx::RigidBody > m_rb;
    agx::Vec3 m_targetPosition;
};
 
osg::Group* buildTutorial3( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create a kinematic box
  agx::RigidBodyRef kinematicBody = new agx::RigidBody();
  kinematicBody->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.5, 0.1, 1.0 ) ) ) );
  kinematicBody->setMotionControl( agx::RigidBody::KINEMATICS );
  simulation->add( kinematicBody );
  agxOSG::createVisual( kinematicBody, root );
 
  // Add the step event listener (class above this function) to simulation
  simulation->add( new MyStepEventListener( kinematicBody, agx::Vec3( -3, 0, -3 ) ) );
 
  return root;
}
 
class OnSensorVelocityScaler : public agxSDK::ContactEventListener
{
  public:
    OnSensorVelocityScaler( agx::Real speedScale )
      : m_speedScale( speedScale )
    {
      // We're interested in impact and separation events.
      setMask( IMPACT | SEPARATION );
    }
 
    virtual KeepContactPolicy impact( const agx::TimeStamp& /*t*/, agxCollide::GeometryContact* c )
    {
      // Find the other geometry (the one that's not a sensor), and store the current velocity.
      if ( c->geometry(0)->isSensor() && c->geometry(1)->getRigidBody() ) {
        m_incidentVelocity = c->geometry(1)->getRigidBody()->getVelocity();
        c->geometry(1)->getRigidBody()->setVelocity( m_incidentVelocity * m_speedScale );
      }
      else if ( c->geometry(1)->isSensor() && c->geometry(0)->getRigidBody() ) {
        m_incidentVelocity = c->geometry(0)->getRigidBody()->getVelocity();
        c->geometry(0)->getRigidBody()->setVelocity( m_incidentVelocity * m_speedScale );
      }
      return KEEP_CONTACT;
    }
 
    virtual void separation( const agx::TimeStamp& /*t*/, agxCollide::GeometryPair& cd )
    {
      // Reset the velocity
      if ( cd.first->isSensor() && cd.second->getRigidBody() )
        cd.second->getRigidBody()->setVelocity( m_incidentVelocity );
      else if ( cd.second->isSensor() && cd.first->getRigidBody() )
        cd.first->getRigidBody()->setVelocity( m_incidentVelocity );
    }
 
  private:
    agx::Vec3 m_incidentVelocity;
    agx::Real m_speedScale;
};
 
osg::Group* buildTutorial4( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create the exact same scene as in tutorial 4.3.
  agx::RigidBodyRef kinematicBody = new agx::RigidBody();
  kinematicBody->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.5, 0.1, 1.0 ) ) ) );
  kinematicBody->setMotionControl( agx::RigidBody::KINEMATICS );
  simulation->add( kinematicBody );
  agxOSG::createVisual( kinematicBody, root );
 
  // Store this so we can position the sensors
  agx::Vec3 initialPathPosition( -3, 0, -3 );
 
  simulation->add( new MyStepEventListener( kinematicBody, initialPathPosition ) );
 
  // Create two sensor spheres (geometries flagged as sensors)
  agx::Real sensorRadius = 0.2;
  agxCollide::GeometryRef sensor1 = new agxCollide::Geometry( new agxCollide::Sphere( sensorRadius ) );
  agxCollide::GeometryRef sensor2 = new agxCollide::Geometry( new agxCollide::Sphere( sensorRadius ) );
  simulation->add( sensor1 );
  simulation->add( sensor2 );
  agxOSG::createVisual( sensor1, root );
  agxOSG::createVisual( sensor2, root );
  // Sensor flag
  sensor1->setSensor( true );
  sensor2->setSensor( true );
 
  // Position the sensors
  sensor1->setPosition( agx::Vec3( 0, 0, initialPathPosition.z() ) );
  sensor2->setPosition( agx::Vec3( 0, 0,-initialPathPosition.z() ) );
 
  // The spheres are now sensors, which means that they will not collide with anything, only send contact events
  // for impact, contact and separation.
 
  // OnSensorVelocityScaler: From sensor impact until separation, the velocity is scaled
  OnSensorVelocityScaler *listener = new OnSensorVelocityScaler( 0.5 );
  listener->setFilter( new agxSDK::GeometryFilter( sensor1 ) );
  simulation->addEventListener( listener );
 
  listener = new OnSensorVelocityScaler( 2.0 );
  listener->setFilter( new agxSDK::GeometryFilter( sensor2 ) );
  simulation->addEventListener( listener );
 
  return root;
}
 
class MyContactEventListener : public agxSDK::ContactEventListener
{
  public:
    typedef agx::VectorPOD< agxCollide::GeometryContact > GeometryContactContainer;
 
  public:
    MyContactEventListener( agxSDK::ExecuteFilter* execFilter )
      : agxSDK::ContactEventListener( agxSDK::ContactEventListener::IMPACT | agxSDK::ContactEventListener::CONTACT )
    {
      // Listen to impact and contact events.
 
      // Assign the given execute filter.
      setFilter( execFilter );
    }
 
    virtual agxSDK::ContactEventListener::KeepContactPolicy impact( const agx::TimeStamp&, agxCollide::GeometryContact* gc )
    {
      // Store the contact.
      return storeContact( gc );
    }
 
    virtual agxSDK::ContactEventListener::KeepContactPolicy contact( const agx::TimeStamp&, agxCollide::GeometryContact* gc )
    {
      // Store the contact.
      return storeContact( gc );
    }
 
    GeometryContactContainer& getContacts() { return m_contacts; }
    const GeometryContactContainer& getContacts() const { return m_contacts; }
 
  protected:
    virtual ~MyContactEventListener() {}
 
    agxSDK::ContactEventListener::KeepContactPolicy storeContact( agxCollide::GeometryContact* gc )
    {
      m_contacts.push_back( *gc );
      return agxSDK::ContactEventListener::KEEP_CONTACT;
    }
 
  protected:
    GeometryContactContainer m_contacts;
};
 
class ContactStepListener : public agxSDK::StepEventListener
{
  public:
    ContactStepListener( MyContactEventListener* contactEvent )
      : agxSDK::StepEventListener( agxSDK::StepEventListener::POST_STEP ),
        m_contactEvent( contactEvent )
    {
    }
 
    virtual void addNotification()
    {
      // Add our contact event listener if it hasn't already been added.
      if ( m_contactEvent && m_contactEvent->getSimulation() == 0L )
        getSimulation()->add( m_contactEvent );
    }
 
    virtual void removeNotification()
    {
      getSimulation()->remove( m_contactEvent );
    }
 
    virtual void post( const agx::TimeStamp& )
    {
      // The contacts collected by our contact event (given filter etc).
      MyContactEventListener::GeometryContactContainer& contacts = m_contactEvent->getContacts();
 
      for ( size_t i = 0; i < contacts.size(); ++i ) {
        agxCollide::GeometryContact gc = contacts[ i ];
 
        // Check so that the geometry contact still is valid!
        if ( !gc.isValid() )
          continue;
 
        // Sum the normal forces.
        agx::Real normalForceSum = 0;
        for ( size_t j = 0; j < gc.points().size(); ++j ) {
          const agxCollide::ContactPoint& point = gc.points()[ j ];
 
          // Fetching the tangential directions. This could be user defined through
          // friction models.
          agx::Vec3 primaryDirection;
          agx::Vec3 secondaryDirection;
          // Could be user defined friction model.
          if ( gc.getMaterial( true )->getFrictionModel() )
            gc.getMaterial( true )->getFrictionModel()->calculateTangentPlane( gc.geometry( 0 ), gc.geometry( 1 ), point.point(), (agx::Vec3)point.normal(), point.depth(), primaryDirection, secondaryDirection );
          // No friction model attached (default is used). Function located in <agx/Attachment.h>.
          else
            agx::Attachment::createAttachmentBase( (agx::Vec3)point.normal(), primaryDirection, secondaryDirection );
 
          // Not that the direction is defined by the collision detection (i.e., the order in which
          // the geometries are stored in the geometry contact).
          std::cout << "Contact point " << j << ":" << std::endl;
          std::cout << "  - Normal force                       = " << agx::Vec3(point.normal())     * point.localForce( agxCollide::ContactPoint::NORMAL_FORCE ) << std::endl;
          std::cout << "  - Primary direction friction force   = " << primaryDirection   * point.localForce( agxCollide::ContactPoint::TANGENTIAL_FORCE_U ) << std::endl;
          std::cout << "  - Secondary direction friction force = " << secondaryDirection * point.localForce( agxCollide::ContactPoint::TANGENTIAL_FORCE_V ) << std::endl;
 
          normalForceSum += point.getNormalForceMagnitude();
        }
 
        std::cout << "Normal force sum: " << normalForceSum << " N. ((m1 + m2)g = " << (1 + 1) * agx::GRAVITY_ACCELERATION << " N.)" << std::endl;
      }
 
      std::cout << std::endl;
 
      // Important to clear the contacts container. It is not
      // valid to store this between time steps.
      contacts.clear();
    }
 
  protected:
    virtual ~ContactStepListener() {}
 
  protected:
    agx::ref_ptr< MyContactEventListener > m_contactEvent;
};
 
osg::Group* buildTutorial5( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create a static box geometry as in tutorial 1.4.
  agx::Vec3 boxGeometryHalfExtent( 15, 15, 0.5 );
  agxCollide::GeometryRef boxGeometry = new agxCollide::Geometry( new agxCollide::Box( boxGeometryHalfExtent ) );
  boxGeometry->setPosition( boxGeometry->getPosition() - agx::Vec3( 0, 0, boxGeometryHalfExtent.z() ) );
  simulation->add( boxGeometry );
  boxGeometry->setName( "boxGeometry" );
  agxOSG::createVisual( boxGeometry, root );
 
  // Create two rigid body boxes stacked on the static box.
  agx::RigidBodyRef box1 = new agx::RigidBody( "box1" );
  agxCollide::GeometryRef box1Geometry = new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.5, 0.5, 0.2 ) ) );
  box1->add( box1Geometry );
  simulation->add( box1 );
  agxOSG::createVisual( box1, root );
 
  agx::RigidBodyRef box2 = new agx::RigidBody( "box2" );
  agxCollide::GeometryRef box2Geometry = new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.5, 0.5, 0.2 ) ) );
  box2->add( box2Geometry );
  simulation->add( box2 );
  agxOSG::createVisual( box2, root );
 
  // Position the boxes.
  box1->setPosition( agx::Vec3( 0, 0, 0.2       - 1 * agx::REAL_SQRT_EPSILON ) );
  box2->setPosition( agx::Vec3( 0, 0, 0.2 + 0.4 - 2 * agx::REAL_SQRT_EPSILON ) );
 
  // Let the boxes masses be 1.
  box1->getMassProperties()->setMass( 1 );
  box2->getMassProperties()->setMass( 1 );
 
  // Add our ContactStepListener, listening to contacts between the static box and box1.
  simulation->add( new ContactStepListener( new MyContactEventListener( new agxSDK::GeometryFilter( boxGeometry, box1Geometry ) ) ) );
 
  return root;
}
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout <<
                "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
                "\tListener tutorial\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
    application->addScene( buildTutorial3, '3' );
    application->addScene( buildTutorial4, '4' );
    application->addScene( buildTutorial5, '5' );
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;               
    return 1;
  }
 
  return 0;
}

tutorial_machine_reconfiguration.cpp

/*
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.
*/
 
 
/*
 
Demonstrates how to use the agxUtil::ReconfigureRequest to change the configuration
of a constrained system. The example used here is a Wheel loader which contains
kinematic loops.
 
*/
 
#include <agx/version.h>
#include <agx/Logger.h>
#include <agx/Constraint.h>
#include <agx/RigidBody.h>
 
#include <agxCollide/Box.h>
#include <agxCollide/Geometry.h>
#include <agxCollide/Space.h>
 
#include <agxIO/Environment.h>
 
#include <agxSDK/Assembly.h>
#include <agxSDK/Collection.h>
#include <agxSDK/Simulation.h>
#include <agxSDK/GuiEventListener.h>
 
#include <agxUtil/ReconfigureRequest.h>
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/utils.h>
#include <initializer_list>
 
 
// Struct to hold information about a range from which a value will be randomized.
// One or more joints will have this value as requested new position.
//
struct RandomPos {
  agx::Real min;
  agx::Real max;
  std::vector<std::string> jointNames;
 
  RandomPos(agx::Real _min, agx::Real _max, std::initializer_list<std::string> names)
    : min(_min), max(_max), jointNames(names)
  {}
};
 
 
// A GuiEventListener that processes the "RandomPos" defined for the current model
// and uses "ReconfigureRequest" to compute new transforms for the bodies in the model.
//
// When manipulating the simulation and assigning new transforms to the bodies, one
// must pay attention to how this affects the constraints that make up the structure.
//
// Constraint controllers can have old positions (rotational or translational) that does
// not match the new transforms and this must be handled to avoid rapid acceleration
// towards the old state.
//
class RandomizePose : public agxSDK::GuiEventListener
{
  public:
 
    RandomizePose(agxSDK::Collection* collection, agx::RigidBody* refBody)
      : m_col(collection), m_refBody(refBody)
    {
    }
 
    // Will get called on key presses
    virtual bool keyboard(int key, unsigned int /*modkey*/, float /*x*/, float /*y*/, bool keydown) override
    {
      bool handled = false;
 
      if (key == 'A' and keydown) {
 
        agxUtil::ConstraintPositionVector cpv;
 
        // Randomize values put input data in right format
        //
        for (const auto& item : m_items) {
          agx::Real value = agx::random(item.min, item.max);
 
          for (const auto& name : item.jointNames) {
            auto joint = m_col->getConstraint(name);
            if (joint) {
              cpv.push_back(agxUtil::ConstraintPosition(joint, value));
            }
            else {
              LOGGER_WARNING() << "Could not locate " << name << LOGGER_ENDL();
            }
          }
        }
 
        agxUtil::BodyTransformVector results;
 
        bool status = m_rr.computeTransforms(m_col, m_refBody, cpv, results);
 
        if (!status) {
          LOGGER_WARNING() << m_rr.getErrorMessage() << LOGGER_ENDL();
        }
        else {
          // Use computed results. So far nothing is changed in the simulation.
          // Now we'll update the bodies with new transforms.
          // We can also have to old velocities cleared since they might not be
          // valid for the new configuration.
          //
          // Note: Make sure to check the API documentation regarding what this method handles
          //       and what it does not do to maintain a stable simulation after bodies
          //       are updated.
          //
          //
          m_rr.applyTransforms(m_col, cpv, results, true);
        }
 
        handled = true;
      }
 
      return handled;
    }
 
    void addRandomization(struct RandomPos rp) {
      m_items.emplace_back(rp);
    }
 
  private:
 
    // The ReconfigureRequest instance is kept as a member so it can be reused to save some time.
    // Creating- and destroying one each time computeTransforms is called adds some overhead.
    agxUtil::ReconfigureRequest   m_rr;
 
    std::vector<struct RandomPos> m_items;     // Container to keep track of what should be randomized
 
    agxSDK::CollectionRef         m_col;       // The collection of items making up the structure that should be reconfigured
    agx::RigidBodyRef             m_refBody;   // The reference body, must be part of the collection
 
};
 
 
 
// This method builds a scene with a box for ground and a wheel loader.
// It also sets up a "RandomizePose" GuiEventListener described above.
//
osg::Group* buildTutorial1(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  application->getSceneDecorator()->setText(0, "Press 'A' to randomize the pose for the machine");
 
  auto root = new osg::Group();
 
  // Locate wheel loader model file
  //
  auto fpc = agxIO::Environment::instance()->getFilePath(agxIO::Environment::RESOURCE_PATH);
  auto modelName = agxIO::FileSystem::convertToNativeFilePath("models/wheel_loader_DL300.agx");
  auto filename = fpc.find(modelName);
 
  if (filename == "") {
    LOGGER_ERROR() << "Could not locate wheel loader file: " << modelName << LOGGER_ENDL();
    return root;
  }
 
  // Load wheel loader into simulation and put all the items in an assembly
  agxSDK::AssemblyRef wheel_loader = new agxSDK::Assembly();
  simulation->read(filename, wheel_loader);
 
  // Create a static ground to prevent the machine from falling
  agxCollide::GeometryRef ground = new agxCollide::Geometry(new agxCollide::Box(12, 12, 0.5));
  ground->setPosition(agx::Vec3(0, 0, -0.55));
  simulation->add(ground);
 
  // Visualization
  agxOSG::createVisual(wheel_loader, root);
  agxOSG::createVisual(ground, root);
 
  // Input to RandomizePose
  agxSDK::CollectionRef col = new agxSDK::Collection();
  col->add(wheel_loader);
 
  // When new transforms are computed, one body is treated as reference.
  // That body will not move. For the wheel loader and the same waist angle,
  // the result will be different if a body in the front- och back part of the
  // machine is used as reference.
  agx::RigidBody* refBody = wheel_loader->getRigidBody("FrontBody");
 
  auto rp = new RandomizePose(col, refBody);
 
  // Which angles (rotational or positional) that should be changed.
  // Note that we want the same value for both front prismatics. Different values for those
  // two would not give a valid pose.
  rp->addRandomization(RandomPos(-0.4, 0.4,   {"WaistHinge"} ));
  rp->addRandomization(RandomPos(-0.8, 0.015, {"LeftLowerPrismatic", "RightLowerPrismatic"}));
  rp->addRandomization(RandomPos(-0.2, 0.2,   {"CenterPrismatic"}));
 
  simulation->add(rp);
 
  return root;
}
 
 
 
int main(int argc, char** argv)
{
  agx::AutoInit agxInit;
  std::cout << "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial Machine Reconfiguration (agxModel::ReconfigureRequest)\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene(buildTutorial1, '1');
 
    if ( application->init(argc, argv))
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_material.cpp

/*
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.
*/
 
#include <agxOSG/ExampleApplication.h>
 
#include <agx/version.h>
#include <agx/LockJoint.h>
#include <agxSDK/GeometryFilter.h>
 
#include "tutorialUtils.h"
 
#include <agxIO/ImageValueInterpreter.h>
osg::Group* buildTutorial1( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create a static box geometry as in tutorial 1.4.
  agx::Vec3 boxGeometryHalfExtent( 15, 15, 0.5 );
  agxCollide::GeometryRef boxGeometry = new agxCollide::Geometry( new agxCollide::Box( boxGeometryHalfExtent ) );
  boxGeometry->setPosition( boxGeometry->getPosition() - agx::Vec3( 0, 0, boxGeometryHalfExtent.z() ) );
  simulation->add( boxGeometry );
  agxOSG::createVisual( boxGeometry, root );
 
  // Create a rigid body sphere
  agx::RigidBodyRef rb = new agx::RigidBody();
  agx::Real sphereRadius = 1.5;
  agxCollide::GeometryRef sphereGeometry = new agxCollide::Geometry( new agxCollide::Sphere( sphereRadius ) );
  rb->add( sphereGeometry );
 
  // Set position to something above the static box
  rb->setPosition( agx::Vec3( 0, 0, 8 ) );
 
  // Create visual representation, and add the rigid body to the simulation
  agxOSG::createVisual( rb, root );
  simulation->add( rb );
 
  // Create a new material with an unique name
  agx::MaterialRef sphereMaterial = new agx::Material( "CylinderMaterial" );
 
  // Add the material to the simulation
  simulation->add( sphereMaterial );
 
  // Default material parameters are something like steel, we want a softer material.
  // Note: The rigid body is still rigid, the geometry will not deform, but the properties of
  //       the material is as if it was deformed.
  sphereMaterial->getBulkMaterial()->setYoungsModulus( 8.0E5 );
 
  // Set the material to the sphere geometry (if a body has multiple geometries, tutorial 1.5
  // for example, each geometry may have a unique material).
  sphereGeometry->setMaterial( sphereMaterial );
 
  // Let the boxGeometry object have the same material
  boxGeometry->setMaterial( sphereMaterial );
 
  return root;
}
 
osg::Group* buildTutorial2( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create a static box geometry as in tutorial 1.4.
  agx::Vec3 boxGeometryHalfExtent( 15, 8, 0.5 );
  agxCollide::GeometryRef boxGeometry = new agxCollide::Geometry( new agxCollide::Box( boxGeometryHalfExtent ) );
  boxGeometry->setPosition( boxGeometry->getPosition() - agx::Vec3( 0, 0, boxGeometryHalfExtent.z() ) );
  simulation->add( boxGeometry );
  agxOSG::createVisual( boxGeometry, root );
 
  // Rotate the box geometry 30 degrees to form an "incline plane".
  boxGeometry->setRotation( agx::Quat( agx::degreesToRadians( 30 ), agx::Vec3( 0, 1, 0 ) ) );
 
  // Create three boxes
  agx::Vec3 boxHalfExtent( 1.5, 1.5, 1.5 );
  agx::RigidBodyRefVector rbs( 3 );
  for ( size_t i = 0; i < rbs.size(); ++i ) {
    rbs[ i ] = new agx::RigidBody();
    rbs[ i ]->add( new agxCollide::Geometry( new agxCollide::Box( boxHalfExtent ) ) );
    // Set the rotation to the same as the box geometry
    rbs[ i ]->setRotation( boxGeometry->getRotation() );
    simulation->add( rbs[ i ] );
    agxOSG::createVisual( rbs[ i ], root );
  }
 
  // The boxes have the correct rotation, set the position to near the upper left edge of the boxGeometry object.
  // We know the dimension of boxGeometry so we can use its frame to transform points form local frame to the world.
  rbs[ 0 ]->setPosition( boxGeometry->getFrame()->transformPointToWorld( agx::Vec3( -boxGeometryHalfExtent.x() + boxHalfExtent.x(),
                                                                                     -5,
                                                                                     boxGeometryHalfExtent.z() + boxHalfExtent.z() ) ) );
  rbs[ 1 ]->setPosition( boxGeometry->getFrame()->transformPointToWorld( agx::Vec3( -boxGeometryHalfExtent.x() + boxHalfExtent.x(),
                                                                                     0,
                                                                                     boxGeometryHalfExtent.z() + boxHalfExtent.z() ) ) );
  rbs[ 2 ]->setPosition( boxGeometry->getFrame()->transformPointToWorld( agx::Vec3( -boxGeometryHalfExtent.x() + boxHalfExtent.x(),
                                                                                     5,
                                                                                     boxGeometryHalfExtent.z() + boxHalfExtent.z() ) ) );
 
  // Create three materials with unique names and add them to the simulation.
  agx::MaterialRef m1 = new agx::Material( "box1Material" );
  rbs[ 0 ]->getGeometries().front()->setMaterial( m1 );
  simulation->add( m1 );
 
  agx::MaterialRef m2 = new agx::Material( "box2Material" );
  rbs[ 1 ]->getGeometries().front()->setMaterial( m2 );
  simulation->add( m2 );
 
  agx::MaterialRef m3 = new agx::Material( "box3Material" );
  rbs[ 2 ]->getGeometries().front()->setMaterial( m3 );
  simulation->add( m3 );
 
  // Roughness is a measure of how rough the surface of the geometry is.
  // Note: This isn't the actual friction coefficient. This value, with
  //       and other parameters, calculates a friction coefficient given
  //       the two materials.
  // To handle the friction coefficient and restitution explicitly, have
  // a look at tutorial 2.2.
  m1->getSurfaceMaterial()->setRoughness( 0.1f );
  m2->getSurfaceMaterial()->setRoughness( 0.4f );
  m3->getSurfaceMaterial()->setRoughness( 0.8f );
 
  // Adhesion is a measure in how sticky a material is (not tangential!).
  // If one for example Throw an object at a wall, and the adhesion is
  // large, the object gets stuck to the wall for some time. This feature
  // is currently not activated.
 
  return root;
}
 
osg::Group* buildTutorial3( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create a static box geometry as in tutorial 1.4.
  agx::Vec3 boxGeometryHalfExtent( 15, 8, 0.5 );
  agxCollide::GeometryRef boxGeometry = new agxCollide::Geometry( new agxCollide::Box( boxGeometryHalfExtent ) );
  boxGeometry->setPosition( boxGeometry->getPosition() - agx::Vec3( 0, 0, boxGeometryHalfExtent.z() ) );
  simulation->add( boxGeometry );
  agxOSG::createVisual( boxGeometry, root );
 
  // Create two spheres
  agx::Real sphereRadius = 1.5;
  agx::RigidBodyRef sphere1 = new agx::RigidBody();
  agxCollide::GeometryRef sphere1Geometry = new agxCollide::Geometry( new agxCollide::Sphere( sphereRadius ) );
  sphere1->add( sphere1Geometry );
  simulation->add( sphere1 );
 
  agx::RigidBodyRef sphere2 = new agx::RigidBody();
  agxCollide::GeometryRef sphere2Geometry = new agxCollide::Geometry( new agxCollide::Sphere( sphereRadius ) );
  sphere2->add( sphere2Geometry );
  simulation->add( sphere2 );
 
  // Create visual representation of the two spheres
  agxOSG::createVisual( sphere1, root );
  agxOSG::createVisual( sphere2, root );
 
  // Create material for boxGeometry
  agx::MaterialRef boxGeometryMat = new agx::Material( "boxGeometryMat" );
  boxGeometry->setMaterial( boxGeometryMat );
  simulation->add( boxGeometryMat );
 
  // Create material for sphere1
  agx::MaterialRef sphere1Mat = new agx::Material( "sphere1Mat" );
  sphere1Geometry->setMaterial( sphere1Mat );
  simulation->add( sphere1Mat );
 
  // Create material for sphere2
  agx::MaterialRef sphere2Mat = new agx::Material( "sphere2Mat" );
  sphere2Geometry->setMaterial( sphere2Mat );
  simulation->add( sphere2Mat );
 
  // Now, each geometry has a material, all with the default values for roughness, Youngs' modulus etc.
  // It's possible to specify what the behavior should be when each pair of these materials collide.
 
  // Lets start with boxGeometryMat and sphere1Mat.
  agx::ContactMaterialRef boxSphere1Contact = new agx::ContactMaterial( boxGeometryMat, sphere1Mat );
  // Add the contact material to the simulation.
  simulation->add( boxSphere1Contact );
 
  // Same procedure for boxGeometryMat and sphere2Mat.
  agx::ContactMaterialRef boxSphere2Contact = new agx::ContactMaterial( boxGeometryMat, sphere2Mat );
  simulation->add( boxSphere2Contact );
 
  // Sphere1 and sphere2 never collides so we don't have to create a contact material for those two.
 
  // Position sphere1 to the left...
  sphere1->setPosition( agx::Vec3( -2, 0, 6 ) );
  // ...and sphere2 4 m to the right of sphere1.
  sphere2->setPosition( agx::Vec3(  2, 0, 6 ) );
 
  // Set infinite friction between sphere1 and boxGeometry.
  boxSphere1Contact->setFrictionCoefficient( std::numeric_limits<agx::Real>::infinity() );
  // And set the restitution to be zero.
  boxSphere1Contact->setRestitution( 0 );
 
  // Let the friction between sphere2 and boxGeometry be small and restitution 0.95.
  boxSphere2Contact->setFrictionCoefficient( 0.02f );
  boxSphere2Contact->setRestitution( 0.95f );
 
  // Finally, set some angular velocity to the spheres.
  sphere1->setAngularVelocity( agx::Vec3( 0, -5, 0 ) );
  sphere2->setAngularVelocity( agx::Vec3( 0,  5, 0 ) );
 
  return root;
}
 
// Class to print rigid body velocity and speed (see tutorial 4 for how to use event handlers)
class PrintRBVelocityAndSpeed : public agxSDK::StepEventListener
{
  public:
    PrintRBVelocityAndSpeed( agx::RigidBody* rb ) : m_rb( rb )
    {
      setMask( PRE_STEP );
    }
 
    virtual void pre( const agx::TimeStamp& /*t*/ )
    {
      if ( !m_rb.isValid() )
        return;
 
      std::cout << "Name: " << m_rb->getName() << ": Speed: " << m_rb->getVelocity().length() << ": Velocity: " << m_rb->getVelocity() << std::endl;
    }
 
  private:
    agx::observer_ptr< agx::RigidBody > m_rb;
};
 
osg::Group* buildTutorial4( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create a static box geometry as in tutorial 1.4.
  agx::Vec3 boxGeometryHalfExtent( 15, 8, 0.5 );
  agxCollide::GeometryRef boxGeometry = new agxCollide::Geometry( new agxCollide::Box( boxGeometryHalfExtent ) );
  boxGeometry->setPosition( boxGeometry->getPosition() - agx::Vec3( 0, 0, boxGeometryHalfExtent.z() ) );
  simulation->add( boxGeometry );
  agxOSG::createVisual( boxGeometry, root );
 
  // Create a material for the box
  //agx::MaterialRef boxGeometryMaterial = new agx::Material( "boxGeometryMaterial" );
  //boxGeometry-> //setMaterial( boxGeometryMaterial );
  //simulation->add( boxGeometryMaterial );
 
  // Set the surface velocity (any object colliding with this material will "slide" so that the relative
  // velocity between boxGeometry and the other objects is the surface velocity). This vector will be
  // projected onto the contact plane and is here given in geometry coordinates.
  // Note: If two or more geometries share the same material they all will have the velocity in their own
  //       local geometry coordinate system.
  agx::Vec3 surfaceVelocity( 2, 2, 0 );
  //boxGeometryMaterial->getSurfaceMaterial()->setVelocity( surfaceVelocity );
  boxGeometry->setSurfaceVelocity( (agx::Vec3f)surfaceVelocity );
 
  // Create two boxes, one with just default material and one with a material that eliminates the y-component of
  // the surface velocity of boxGeometry.
 
  // First the box with the default material.
  agx::Vec3 dynamicBoxHalfExtent( 1, 1, 1 );
  agx::RigidBodyRef rbBox1 = new agx::RigidBody();
  rbBox1->setName( "rbBox1" );
  rbBox1->add( new agxCollide::Geometry( new agxCollide::Box( dynamicBoxHalfExtent ) ) );
  simulation->add( rbBox1 );
  agxOSG::createVisual( rbBox1, root );
 
  agx::RigidBodyRef rbBox2 = new agx::RigidBody();
  rbBox2->setName( "rbBox2" );
  agxCollide::GeometryRef rbBox2Geometry = new agxCollide::Geometry( new agxCollide::Box( dynamicBoxHalfExtent ) );
  rbBox2->add( rbBox2Geometry );
  simulation->add( rbBox2 );
  agxOSG::createVisual( rbBox2, root );
 
  // Create the material with surface velocity = (5, -surfaceVelocity.y(), 0), because the surface velocities of
  // two materials colliding will add the surface velocities. In this case the resulting velocity for rbBox2 will
  // be (2, 2, 0) + (5, -2, 0) = (7, 0, 0).
//   agx::MaterialRef box2GeometryMaterial = new agx::Material( "box2GeometryMaterial" );
//   rbBox2Geometry->setMaterial( box2GeometryMaterial );
//   simulation->add( box2GeometryMaterial );
 
  //  box2GeometryMaterial->getSurfaceMaterial()->setVelocity( agx::Vec3( 5, -surfaceVelocity.y(), 0 ) );
    rbBox2Geometry->setSurfaceVelocity( agx::Vec3f( 5, (float)-surfaceVelocity.y(), 0 ) );
 
  // Position the dynamic boxes
  rbBox1->setPosition( agx::Vec3( -10, -4, 1.5 ) );
  rbBox2->setPosition( agx::Vec3( -10,  2, 2 ) );
 
  // How to use event listeners, see tutorial 4.
  // Will spam cout with speed and velocity of rbBox1 (should converge to speed = sqrt(8) = 2.82842712, and velocity = (2, 2, 0))
  simulation->add( new PrintRBVelocityAndSpeed( rbBox1 ) );
 
  return root;
}
 
// Simple class to spawn boxes on the conveyor belt (see tutorial 4 for event handlers)
// Will spawn a box each x second.
class SimpleBoxSpawner : public agxSDK::StepEventListener
{
  public:
    SimpleBoxSpawner( const agx::Vec3& position, const agx::Vec3& halfExtent, agx::Real time, osg::Group* root )
      : m_position( position ), m_halfExtent( halfExtent ), m_time( time ), m_currentTime( 0 ), m_root( root )
    {
      setMask( PRE_COLLIDE );
    }
 
    virtual void addNotification()
    {
      createAndAdd();
    }
 
    virtual void preCollide( const agx::TimeStamp& /*t*/ )
    {
      if ( m_currentTime > m_time ) {
        createAndAdd();
        m_currentTime = 0;
        return;
      }
 
      m_currentTime += getSimulation()->getDynamicsSystem()->getTimeGovernor()->getTimeStep();
    }
 
  private:
    void createAndAdd()
    {
      agxAssert( getSimulation() && m_root.valid() );
      agx::RigidBody* rb = new agx::RigidBody();
      rb->add( new agxCollide::Geometry( new agxCollide::Box( m_halfExtent ) ) );
 
      // Give the material a unique name
      agx::MaterialRef material = new agx::Material( "" );
      std::stringstream ss;
      ss << "Material" << material->getIndex();
      material->setName( ss.str() );
 
      rb->getGeometries().front()->setMaterial( material );
      agxOSG::createVisual( rb, m_root.get() );
      rb->setPosition( m_position );
      rb->getMassProperties()->setMass( 100 );
      getSimulation()->add( rb );
    }
 
  private:
    agx::Vec3 m_position;
    agx::Vec3 m_halfExtent;
    agx::Real m_time;
    agx::Real m_currentTime;
    osg::observer_ptr< osg::Group > m_root;
};
 
 
 
// Simple class to spawn boxes on the conveyor belt (see tutorial 4 for event handlers)
// Will spawn a box each x second.
class SimpleCylinderSpawner : public agxSDK::StepEventListener
{
public:
  SimpleCylinderSpawner( const agx::Vec3& position, const agx::Real radius, agx::Real height, agx::Real time, osg::Group* root )
    : m_position( position ), m_radius( radius ), m_height( height ), m_time( time ), m_currentTime( 0 ), m_root( root )
  {
    setMask( PRE_COLLIDE );
  }
 
  virtual void addNotification()
  {
    createAndAdd();
  }
 
  virtual void preCollide( const agx::TimeStamp& /*t*/ )
  {
    if ( m_currentTime > m_time ) {
      createAndAdd();
      m_currentTime = 0;
      return;
    }
 
    m_currentTime += getSimulation()->getDynamicsSystem()->getTimeGovernor()->getTimeStep();
  }
 
private:
  void createAndAdd()
  {
    agxAssert( getSimulation() && m_root.valid() );
    agx::RigidBody* rb = new agx::RigidBody();
    rb->add( new agxCollide::Geometry( new agxCollide::Cylinder( m_radius, m_height ) ), agx::AffineMatrix4x4::rotate(agx::PI_2, 1, 0, 0) );
 
    agx::MaterialRef material = new agx::Material("");
 
    // Give material unique name
    std::stringstream ss;
    ss << "Material" << material->getIndex();
    material->setName( ss.str() );
 
    rb->getGeometries().front()->setMaterial( material );
    agxOSG::createVisual( rb, m_root.get() );
    rb->setPosition( m_position );
    rb->getMassProperties()->setMass( 100 );
    getSimulation()->add( rb );
  }
 
private:
  agx::Vec3 m_position;
  agx::Real m_radius;
  agx::Real m_height;
  agx::Real m_time;
  agx::Real m_currentTime;
  osg::observer_ptr< osg::Group > m_root;
};
 
 
class KillerObject : public agxSDK::StepEventListener
{
  public:
    KillerObject( agx::Real z ) : m_z( z ) { setMask( PRE_COLLIDE ); }
 
    virtual void preCollide( const agx::TimeStamp& /*t*/ )
    {
      agx::RigidBodyRefVector& bodies = getSimulation()->getDynamicsSystem()->getRigidBodies();
      agx::RigidBodyRefVector remove;
      for ( size_t i = 0; i < bodies.size(); ++i ) {
        if ( bodies[ i ]->getPosition().z() < m_z )
          remove.push_back( bodies[ i ] );
      }
      while ( !remove.empty() ) {
        getSimulation()->remove( remove.back() );
        remove.pop_back();
      }
    }
 
  private:
    agx::Real m_z;
};
 
osg::Group* buildTutorial5( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create a static box geometry as in tutorial 1.4.
  agx::Vec3 boxGeometryHalfExtent( 15, 10, 0.5 );
  agxCollide::GeometryRef boxGeometry = new agxCollide::Geometry( new agxCollide::Box( boxGeometryHalfExtent ) );
  boxGeometry->setPosition( boxGeometry->getPosition() - agx::Vec3( 0, 0, boxGeometryHalfExtent.z() ) );
  simulation->add( boxGeometry );
  agxOSG::createVisual( boxGeometry, root );
 
  // Create conveyor belt section 1
  agx::Vec3 convSecHalfExtent( 8, 2, 0.5 );
  agx::RigidBodyRef convSec1RB = new agx::RigidBody();
  agxCollide::GeometryRef convSec1Geom = new agxCollide::Geometry( new agxCollide::Box( convSecHalfExtent ) );
  convSec1RB->add( convSec1Geom );
  simulation->add( convSec1RB );
  agxOSG::createVisual( convSec1RB, root );
 
  // Create conveyor belt section 2 as section 1 (same dimensions)
  agx::RigidBodyRef convSec2RB = new agx::RigidBody();
  agxCollide::GeometryRef convSec2Geom = new agxCollide::Geometry( new agxCollide::Box( convSecHalfExtent ) );
  convSec2RB->add( convSec2Geom );
  simulation->add( convSec2RB );
  agxOSG::createVisual( convSec2RB, root );
 
  // Let the mass of the belts be 200 kg each
  convSec1RB->getMassProperties()->setMass( 200 );
  convSec2RB->getMassProperties()->setMass( 200 );
 
  // Rotate section 2 90 degrees about world z
  convSec2RB->setRotation( agx::Quat( agx::PI_2, agx::Vec3( 0, 0, 1 ) ) );
 
  // Create material with surface velocity, where the velocity is non-zero in x in geometry coordinates.
  // We will assign this material to both convSec1Geom and convSec2Geom.
  //agx::MaterialRef convMat = new agx::Material( "convMat" );
  // Set the surface velocity, 3 m/s along x in geometry coordinates. This means that for convSec1 the
  // velocity is along world x and for convSec2 along world y.
  //convMat->getSurfaceMaterial()->setVelocity( agx::Vec3( 3, 0, 0 ) );
 
  // Add the material and assign it to the geometries (order doesn't matter).
  //simulation->add( convMat );
  //convSec1Geom->setMaterial( convMat );
  //convSec2Geom->setMaterial( convMat );
  convSec1Geom->setSurfaceVelocity( agx::Vec3f(3,0,0) );
  convSec2Geom->setSurfaceVelocity( agx::Vec3f(3,0,0) );
 
  // Position the conveyor belt sections
  convSec1RB->setPosition( agx::Vec3( -5.00f, -6, 1.51f ) );
  convSec2RB->setPosition( agx::Vec3(  5.01f,  0, 1.50f ) );
 
  // Lock the two sections to the world (see tutorial 5 for constraints)
  convSec1RB->setMotionControl( agx::RigidBody::STATIC );
  convSec2RB->setMotionControl( agx::RigidBody::STATIC );
  //simulation->add( new agx::LockJoint( convSec1RB ) );
  //simulation->add( new agx::LockJoint( convSec2RB ) );
 
  // This step event listener (class above, for more on event listeners see tutorial 4), will spawn a box
  // from the beginning and one every 5th second.
  simulation->add( new SimpleBoxSpawner( convSec1RB->getPosition() + agx::Vec3( -5, 0, 0.5 + 1.0f ), agx::Vec3( 1, 1, 1 ), 1.5f, root ) );
 
  // Step event listener that checks if a rigid body has passed a given height, if so, the body is removed.
  // So if position.z() < -100, the body is removed.
  simulation->add( new KillerObject( -100 ) );
 
  return root;
}
 
 
osg::Group* buildTutorial6( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create a static box geometry as in tutorial 1.4.
  agx::Vec3 boxGeometryHalfExtent( 15, 10, 0.5 );
  agxCollide::GeometryRef boxGeometry = new agxCollide::Geometry( new agxCollide::Box( boxGeometryHalfExtent ) );
  boxGeometry->setPosition( boxGeometry->getPosition() - agx::Vec3( 0, 0, boxGeometryHalfExtent.z() ) );
  simulation->add( boxGeometry );
  agxOSG::createVisual( boxGeometry, root );
 
  // Create conveyor belt section 1
  agx::Vec3 convSecHalfExtent( 8, 2, 0.5 );
  agx::RigidBodyRef convSec1RB = new agx::RigidBody();
  agxCollide::GeometryRef convSec1Geom = new agxCollide::Geometry( new agxCollide::Box( convSecHalfExtent ) );
  convSec1RB->add( convSec1Geom );
  simulation->add( convSec1RB );
  agxOSG::createVisual( convSec1RB, root );
 
  // Create conveyor belt section 2 as section 1 (same dimensions)
  agx::RigidBodyRef convSec2RB = new agx::RigidBody();
  agxCollide::GeometryRef convSec2Geom = new agxCollide::Geometry( new agxCollide::Box( convSecHalfExtent ) );
  convSec2RB->add( convSec2Geom );
  simulation->add( convSec2RB );
  agxOSG::createVisual( convSec2RB, root );
 
  // Let the mass of the belts be 200 kg each
  convSec1RB->getMassProperties()->setMass( 200 );
  convSec2RB->getMassProperties()->setMass( 200 );
 
  // Rotate section 2 90 degrees about world z
  convSec2RB->setRotation( agx::Quat( agx::PI_2, agx::Vec3( 0, 0, 1 ) ) );
 
  // Create material with surface velocity, where the velocity is non-zero in x in geometry coordinates.
  // We will assign this material to both convSec1Geom and convSec2Geom.
  //agx::MaterialRef convMat = new agx::Material( "convMat" );
  // Set the surface velocity, 3 m/s along x in geometry coordinates. This means that for convSec1 the
  // velocity is along world x and for convSec2 along world y.
  //convMat->getSurfaceMaterial()->setVelocity( agx::Vec3( 3, 0, 0 ) );
 
  // Add the material and assign it to the geometries (order doesn't matter).
  //simulation->add( convMat );
//   convSec1Geom->setMaterial( convMat );
//   convSec2Geom->setMaterial( convMat );
 
  convSec1Geom->setSurfaceVelocity( agx::Vec3f( 3, 0, 0 ) );
  convSec2Geom->setSurfaceVelocity( agx::Vec3f( 3, 0, 0 ) );
 
  // Position the conveyor belt sections
  convSec1RB->setPosition( agx::Vec3( -5.00f, -6, 1.51f ) );
  convSec2RB->setPosition( agx::Vec3(  5.01f,  0, 1.50f ) );
 
  // Lock the two sections to the world (see tutorial 5 for constraints)
  convSec1RB->setMotionControl( agx::RigidBody::STATIC );
  convSec2RB->setMotionControl( agx::RigidBody::STATIC );
  //simulation->add( new agx::LockJoint( convSec1RB ) );
  //simulation->add( new agx::LockJoint( convSec2RB ) );
 
  // This step event listener (class above, for more on event listeners see tutorial 4), will spawn a box
  // from the beginning and one every 5th second.
  simulation->add( new SimpleCylinderSpawner( convSec1RB->getPosition() + agx::Vec3( -5, 0, 0.5f + 1.0f ), 1, 1, 1.5f, root ) );
 
  // Step event listener that checks if a rigid body has passed a given height, if so, the body is removed.
  // So if position.z() < -100, the body is removed.
  simulation->add( new KillerObject( -100 ) );
 
  return root;
}
 
 
/*
  In this tutorial we will demonstrate how to set a per contact point friction coefficient.
  To illustrate this, a texture is used which defines the friction on the large box.
*/
 
class SurfaceTextureListener : public agxSDK::ContactEventListener
{
public:
 
  // Constructor, take a path to a texture which we will use as the "friction texture"
  SurfaceTextureListener( const agx::String& texture, agxOSG::SceneDecorator *decorator ) : m_decorator(decorator)
  {
    setMask( CONTACT|IMPACT);
 
    // Read the "friction texture"
    // Scale the read between 0(0) and 1(255)
    m_textureImage = new agxIO::PNGImageValueInterpreter(0,1);
 
    if (!m_textureImage->read( texture ))
      LOGGER_WARNING() << "Unable to read texture image: " << texture << LOGGER_ENDL();
  }
 
  // Called at first contact between two geometries
  // Just call the "other" contact method, it does all the stuff
  KeepContactPolicy impact(const agx::TimeStamp& ts, agxCollide::GeometryContact* gc)
  {
    return contact( ts, gc);
  }
 
  // Return a friction value based on the world position of the contact point.
  // Make a simple lookup in the texture, assuming the large box is 10x10 large
  agx::Real getFriction( const agx::Vec3& point )
  {
    agxIO::Image *image = m_textureImage->getImage();
    if (!image)
      return 0.5;
 
    unsigned int height = image->getHeight();
    unsigned int width = image->getWidth();
 
    // Well we wont to perspective correct sampling ;-)
    unsigned int iy = (width-1)-agx::clamp<unsigned int>(  (unsigned int)(width*point.y()/10), (unsigned int)0, (width-1));
    unsigned int ix = agx::clamp<unsigned int>((unsigned int)(height*point.x())/10, (unsigned int)0,height-1);
 
    // Now read the friction value scaled between 0 and 1
    agx::Real v = m_textureImage->getValueR(ix,iy);
 
    return v;
  }
 
  /* Here is where everything happens.
 
  We get here for each contact that matches the GeometryFilter that was assigned to this ContactEventListener
 
  1. Take the GeometryContact
  2. For each ContactPoint
      2.1 Do a lookup to get the frictional value for the specified contact point position
      2.2 Set the friction coefficient for the contact point
  3. return KEEP_CONTACT
  */
  KeepContactPolicy contact(const agx::TimeStamp&, agxCollide::GeometryContact* gc)
  {
 
    // Just clear the text in the window
    for(size_t i=0; i < 5; i++)
      m_decorator->setText((int)i, "" );
 
    // For each point in the GeometryContact
    for( size_t i=0; i < gc->points().size(); i++)
    {
      agx::Vec3 position = gc->points()[i].point();
 
      // Here we do the friction lookup.
      agx::Real friction = getFriction(position);
 
      // Update the friction for the contact point for one or both directions
      gc->points()[i].setFrictionCoefficient(friction, agx::ContactMaterial::BOTH_PRIMARY_AND_SECONDARY);
 
      agx::String str = agx::String::format("F(%.2f,%.2f): %.2f", position.x(), position.y(), friction);
 
      if (i < 5)
        m_decorator->setText((int)i, str );
    }
    return KEEP_CONTACT;
  }
 
  agx::ref_ptr<agxIO::PNGImageValueInterpreter> m_textureImage;
  agxOSG::SceneDecorator *m_decorator;
};
 
osg::Group* buildTutorial7( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  // This is the image we will be using as a friction map
  agx::String surfaceTexture = "data/textures/frictionmap.png";
 
  // Create a large static "ground box"
  agx::Vec3 boxGeometryHalfExtent( 5, 5, 0.5 );
  agxCollide::GeometryRef boxGeometry = new agxCollide::Geometry( new agxCollide::Box( boxGeometryHalfExtent ) );
 
  // Position so that lower left corner is at 0,0
  boxGeometry->setPosition( boxGeometryHalfExtent );
 
  simulation->add( boxGeometry );
  agxOSG::GeometryNode *node = agxOSG::createVisual( boxGeometry, root );
  agxOSG::setTexture(node, surfaceTexture);
 
  // Create a contact listener that will update the friction for all contacts on the large static box
  // based on a lookup in the image
  agx::ref_ptr<SurfaceTextureListener> sl = new SurfaceTextureListener(surfaceTexture, application->getSceneDecorator());
 
  // Filter out contacts with the large box only
  sl->setFilter( new agxSDK::GeometryFilter(boxGeometry));
  simulation->add( sl );
 
  // Create a small box which will be dropped on the large
  agx::Vec3 smallBoxHalfExtent( agx::Vec3(.3f) );
  agxCollide::GeometryRef smallBoxGeometry = new agxCollide::Geometry( new agxCollide::Box( smallBoxHalfExtent ) );
  agx::RigidBodyRef smallBox = new agx::RigidBody();
  smallBox->setPosition(boxGeometryHalfExtent+agx::Vec3(0,0,1));
  smallBox->add(smallBoxGeometry);
 
  simulation->add( smallBox );
  agxOSG::createVisual( smallBox, root );
 
  return root;
}
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout <<
                "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
                "\tMaterial tutorial\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
    application->addScene( buildTutorial3, '3' );
    application->addScene( buildTutorial4, '4' );
    application->addScene( buildTutorial5, '5' );
    application->addScene( buildTutorial6, '6' );
    application->addScene( buildTutorial7, '7' );
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    agx::shutdown();
    return 1;
  }
 
  agx::shutdown();
  return 0;
}
 

tutorial_mergeSplitHandler.cpp

/*
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.
*/
 
 
/*
 
Demonstrates the API for Adaptive Model Order Reduction (AMOR) that can automatically merge/split
bodies based on impacts/velocities etc.
 
Useful for large speedup of large simulations.
 
*/
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/utils.h>
 
#include <agxSDK/Assembly.h>
#include <agxSDK/MergeSplitHandler.h>
#include <agxSDK/GeometryFilter.h>
 
#include <agxUtil/StepEventCallback.h>
 
#include <agx/LockJoint.h>
 
#include <agxCollide/Box.h>
#include <agxCollide/BasicPrimitiveTests.h>
 
#include <agxRender/RenderSingleton.h>
#include <agx/version.h>
 
namespace
{
  agx::RigidBodyRef createBox( agx::Vec3 he, agx::Vec3 position, agxRender::Color color, agxSDK::Simulation* simulation, osg::Group* root )
  {
    agx::RigidBodyRef rb = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( he ) ) );
    rb->setPosition( position );
    simulation->add( rb );
    agxOSG::setDiffuseColor( agxOSG::createVisual( rb, root ), color );
    return rb;
  }
 
  agx::Vector< agx::RigidBodyRefVector > createPyramid( const agx::Vec3& center, const agx::Vec3& boxesHalfExtent, agx::UInt numBoxesAtBottomRow, agxSDK::Simulation* simulation, osg::Group* root )
  {
    agx::Vector< agx::RigidBodyRefVector > result;
    agx::Real eps = -1.0E-3;
    agx::Real boxHeight = boxesHalfExtent.z();
    agx::Real boxLength = boxesHalfExtent.x();
    agx::Vec3 currentPos( -boxLength * static_cast<agx::Real>(numBoxesAtBottomRow), 0.0, boxHeight );
    for ( int i = int( numBoxesAtBottomRow ); i > 0; --i ) {
      result.push_back( agx::RigidBodyRefVector() );
      auto& bodies = result.back();
      for ( int j = i; j > 0; --j ) {
        agx::RigidBodyRef box = createBox( boxesHalfExtent, center + currentPos, agxRender::Color::Yellow(), simulation, root );
        currentPos.x() += (agx::Real)2.0 * boxLength + eps;
 
        bodies.push_back( box );
      }
      currentPos.x() = boxLength * ( - i + 1 );
      currentPos.z() += (agx::Real)2.0 * boxHeight + eps;
    }
 
    return result;
  }
}
 
osg::Group* buildTutorial1( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Enable the merge-split handler that manages a set of merged bodies
  // automatically. The bodies will merge when it's considered not moving
  // relative another body. There are configurable thresholds included.
  simulation->getMergeSplitHandler()->setEnable( true );
 
  // Static ground.
  agx::RigidBodyRef ground = new agx::RigidBody( "ground" );
  ground->add( new agxCollide::Geometry( new agxCollide::Box( 10, 10, 0.5 ) ) );
  ground->setPosition( 0, 0, -0.5 );
  ground->setMotionControl( agx::RigidBody::STATIC );
 
  simulation->add( ground );
  agxOSG::createVisual( ground, root );
 
  // Fetch merge-split properties for ground and enable merge.
  agxSDK::MergeSplitProperties* groundMSProps = agxSDK::MergeSplitHandler::getOrCreateProperties( ground );
  // Enabling merge, i.e., this body and other bodies may merge with ground.
  groundMSProps->setEnableMerge( true );
 
  agx::RigidBodyRef rb1 = ::createBox( agx::Vec3( 0.50, 0.5, 0.5 ), agx::Vec3( -2.00, 0, 1 ), agxRender::Color::Red(), simulation, root );
  agx::RigidBodyRef rb2 = ::createBox( agx::Vec3( 0.50, 0.5, 0.5 ), agx::Vec3( -0.50, 0, 0.5 ), agxRender::Color::Green(), simulation, root );
  agx::RigidBodyRef rb3 = ::createBox( agx::Vec3( 1.75, 0.5, 0.5 ), agx::Vec3( -1.25, 0, 8 ), agxRender::Color::Blue(), simulation, root );
 
  rb3->setAngularVelocity( 0, 3.2, 0 );
 
  // Enable merge AND split for the three bodies. I.e., they may merge and they
  // can also be split (e.g., during impacts).
  agxSDK::MergeSplitHandler::getOrCreateProperties( rb1 )->setEnableMergeSplit( true );
  agxSDK::MergeSplitHandler::getOrCreateProperties( rb2 )->setEnableMergeSplit( true );
  agxSDK::MergeSplitHandler::getOrCreateProperties( rb3 )->setEnableMergeSplit( true );
 
  return root;
}
 
osg::Group* buildTutorial2( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create ground body with three box geometries.
  const agx::Vec3 groundBoxesHe( 1, 4, 0.5 );
  agx::RigidBodyRef ground = new agx::RigidBody();
  agxCollide::GeometryRef leftGeometry = new agxCollide::Geometry( new agxCollide::Box( groundBoxesHe ) );
  agxCollide::GeometryRef middleGeometry = new agxCollide::Geometry( new agxCollide::Box( groundBoxesHe ) );
  agxCollide::GeometryRef rightGeometry = new agxCollide::Geometry( new agxCollide::Box( groundBoxesHe ) );
  ground->add( leftGeometry,   agx::AffineMatrix4x4::translate( -2.0 * groundBoxesHe.x(), 0, 0 ) );
  ground->add( middleGeometry, agx::AffineMatrix4x4::translate(                        0, 0, 0 ) );
  ground->add( rightGeometry,  agx::AffineMatrix4x4::translate(  2.0 * groundBoxesHe.x(), 0, 0 ) );
 
  ground->setPosition( 0, 0, -groundBoxesHe.z() );
 
  simulation->add( ground );
 
  agxOSG::setDiffuseColor( agxOSG::createVisual( leftGeometry, root ), agxRender::Color::Green() );
  agxOSG::setDiffuseColor( agxOSG::createVisual( middleGeometry, root ), agxRender::Color::Red() );
  agxOSG::setDiffuseColor( agxOSG::createVisual( rightGeometry, root ), agxRender::Color::Green() );
 
  // Lock ground in world.
  agx::LockJointRef groundWorldLock = agx::Constraint::createFromBody<agx::LockJoint>( agx::Vec3(), agx::Vec3::Z_AXIS(), ground );
  groundWorldLock->setCompliance( 1.0E-11 );
  simulation->add( groundWorldLock );
 
  // Create three box bodies to fall down on the ground body. One on each ground geometry.
  agx::RigidBodyRef box1 = ::createBox( agx::Vec3( 0.5 ), agx::Vec3( -2.0 * groundBoxesHe.x(), 0, 1.5 ), agxRender::Color::Red(), simulation, root );
  agx::RigidBodyRef box2 = ::createBox( agx::Vec3( 0.5 ), agx::Vec3(                        0, 0, 1.5 ), agxRender::Color::Red(), simulation, root );
  agx::RigidBodyRef box3 = ::createBox( agx::Vec3( 0.5 ), agx::Vec3(  2.0 * groundBoxesHe.x(), 0, 1.5 ), agxRender::Color::Red(), simulation, root );
 
  // Enable the merge-split handler.
  simulation->getMergeSplitHandler()->setEnable( true );
 
  // Enable merge for ground rigid body. This property is inherited by
  // all the geometries in the ground body unless a geometry has its own.
  agxSDK::MergeSplitHandler::getOrCreateProperties( ground )->setEnableMerge( true );
 
  // Disable merge for the middle box.
  agxSDK::MergeSplitHandler::getOrCreateProperties( middleGeometry )->setEnableMerge( false );
 
  // Enable merge and split for the three box bodies.
  agxSDK::MergeSplitHandler::getOrCreateProperties( box1 )->setEnableMergeSplit( true );
  agxSDK::MergeSplitHandler::getOrCreateProperties( box2 )->setEnableMergeSplit( true );
  agxSDK::MergeSplitHandler::getOrCreateProperties( box3 )->setEnableMergeSplit( true );
 
  // Update color of the box bodies - red if not merged, green merged.
  auto updateColor = [ root ]( agx::RigidBody* rb, const agxRender::Color& freeColor, const agxRender::Color& mergedColor )
  {
    const agx::Bool isMerged = agx::MergedBody::getActive( rb ) != nullptr;
    if ( isMerged )
      agxOSG::setDiffuseColor( rb, mergedColor, root );
    else
      agxOSG::setDiffuseColor( rb, freeColor, root );
  };
 
  // Post-step event to update color of the bodies.
  agxUtil::StepEventCallback::post( [ updateColor, box1, box2, box3 ]( ... )
  {
    updateColor( box1, agxRender::Color::Red(), agxRender::Color::Green() );
    updateColor( box2, agxRender::Color::Red(), agxRender::Color::Green() );
    updateColor( box3, agxRender::Color::Red(), agxRender::Color::Green() );
  }, simulation );
 
  return root;
}
 
osg::Group* buildTutorial3( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  // Static ground.
  agx::RigidBodyRef ground = new agx::RigidBody( "ground" );
  ground->add( new agxCollide::Geometry( new agxCollide::Box( 15, 25, 0.5 ) ) );
  ground->setPosition( 0, 0, -0.5 );
  ground->setMotionControl( agx::RigidBody::STATIC );
 
  simulation->add( ground );
  agxOSG::createVisual( ground, root );
 
  // Create pyramid with a base of 8 boxes. The rows are returned where rows[ 0 ][ 0 ] being
  // the first (leftmost, bottom) body and rows[ 7 ][ 0 ] the box on the top.
  auto rows = ::createPyramid( agx::Vec3( 0, 0, 0 ), agx::Vec3( 0.5, 0.5, 0.5 ), 8, simulation, root );
 
  agx::MergedBodyRef mergedBody = new agx::MergedBody();
  // Merge first row with ground.
  for ( auto firstRowRb : rows[ 0 ] )
    mergedBody->add( new agx::MergedBody::ContactGeneratorEdgeInteraction( ground, firstRowRb ) );
 
  // Merge each body of the row above with the two neighboring bodies in the
  // row below. Like:
  //    o <- merged with the two below
  //   o o <- merged with the two below, NOT the neighbor on the same row.
  //  o o o
  for ( agx::UInt row = 0; row < rows.size() - 1; ++row ) {
    const auto& thisRow  = rows[ row ];
    const auto& aboveRow = rows[ row + 1 ];
    for ( agx::UInt aboveRbIndex = 0; aboveRbIndex < aboveRow.size(); ++aboveRbIndex ) {
      auto rb = aboveRow[ aboveRbIndex ];
      // 'rb' is resting on the two boxes below - create a contact generator
      // edge which performs collision detection between the objects when
      // they're split.
      for ( agx::UInt thisRowRbIndex = aboveRbIndex; thisRowRbIndex <= aboveRbIndex + 1; ++thisRowRbIndex )
        mergedBody->add( new agx::MergedBody::ContactGeneratorEdgeInteraction( thisRow[ thisRowRbIndex ], rb ) );
    }
  }
 
  // Adding the merged body. After this all the objects added above will be merged.
  simulation->add( mergedBody );
 
  // Enable the merge-split handler.
  simulation->getMergeSplitHandler()->setEnable( true );
 
  // Enable merge for ground.
  agxSDK::MergeSplitHandler::getOrCreateProperties( ground )->setEnableMerge( true );
 
  // Enable merge and split for all bodies in the pyramid.
  for ( const auto& row : rows )
    for ( auto rb : row )
      agxSDK::MergeSplitHandler::getOrCreateProperties( rb )->setEnableMergeSplit( true );
 
  auto sendBullet = [ simulation, root ]()
  {
    agx::RigidBodyRef bullet = ::createBox( agx::Vec3( 0.3, 0.3, 0.3 ), agx::Vec3( 4, -20, 8 ), agxRender::Color::Red(), simulation, root );
    bullet->getMassProperties()->setMass( agx::Real( 1.0E3 ) );
    bullet->setVelocity( ( agx::Vec3( -0.7, 0, 5 ) - bullet->getPosition() ).normal() * agx::Real( 60.0 ) );
    bullet->setAngularVelocity( 5, 5, 5 );
  };
 
  // Send the first bullet towards the pyramid. All the objects are
  // explicitly merged (mergedBody above), which means that the bullet
  // may not split any object included in mergedBody.
  sendBullet();
 
  application->getSceneDecorator()->setFontSize( 0.01f );
  application->getSceneDecorator()->setText( 0, "1. All objects explicitly merged." );
  application->getSceneDecorator()->setText( 1, "" );
  application->getSceneDecorator()->setText( 2, "" );
 
  agxUtil::StepEventCallback::preCollide( [ sendBullet, mergedBody, application ]( agxSDK::Simulation* simulation )
  {
    agx::TimeStamp t = simulation->getTimeStamp();
 
    // Time = 1.0 s: Register the merged body to the handler. After this the objects
    //               in the merged body isn't considered to be explicitly merged anymore.
    //               This means that they may merge and/or split after this.
    if ( agx::equivalent( t, 1.0 ) ) {
      simulation->getMergeSplitHandler()->registerMergedBody( mergedBody );
      application->getSceneDecorator()->setText( 1, "2. Merged body registered to the handler. Automatic split is active." );
    }
    // Time = 2.0 s: Sending a bullet with split enabled.
    else if ( agx::equivalent( t, 2.0 ) ) {
      sendBullet();
      application->getSceneDecorator()->setText( 2, "3. Sending second bullet - it's up to the handle to split the merged objects!" );
    }
  }, simulation );
 
  return root;
}
 
namespace tutorial4
{
  class ExplicitSplitSensorListener : public agxSDK::ContactEventListener
  {
    public:
      ExplicitSplitSensorListener( const agxCollide::Geometry* sensor );
 
    protected:
      virtual agxSDK::ContactEventListener::KeepContactPolicy impact( const agx::TimeStamp& t, agxCollide::GeometryContact* geometryContact ) override;
      virtual void separation( const agx::TimeStamp& t, agxCollide::GeometryPair& geometryPair ) override;
 
    private:
      agxCollide::Geometry* findOtherGeometry( agxCollide::Geometry* geometry1, agxCollide::Geometry* geometry2 ) const;
      void handleGeometry( agxCollide::Geometry* geometry, agx::Bool isImpact ) const;
 
    private:
      agxCollide::GeometryConstObserver m_sensor;
  };
 
  ExplicitSplitSensorListener::ExplicitSplitSensorListener( const agxCollide::Geometry* sensor )
    : agxSDK::ContactEventListener( agxSDK::ContactEventListener::IMPACT | agxSDK::ContactEventListener::SEPARATION,
                                    new agxSDK::GeometryFilter( sensor, nullptr ) ),
      m_sensor( sensor )
  {
  }
 
  agxSDK::ContactEventListener::KeepContactPolicy ExplicitSplitSensorListener::impact( const agx::TimeStamp& /*t*/, agxCollide::GeometryContact* geometryContact )
  {
    handleGeometry( findOtherGeometry( geometryContact->geometry( 0 ), geometryContact->geometry( 1 ) ), true );
    return agxSDK::ContactEventListener::KEEP_CONTACT;
  }
 
  void ExplicitSplitSensorListener::separation( const agx::TimeStamp& /*t*/, agxCollide::GeometryPair& geometryPair )
  {
    handleGeometry( findOtherGeometry( geometryPair.first, geometryPair.second ), false );
  }
 
  agxCollide::Geometry* ExplicitSplitSensorListener::findOtherGeometry( agxCollide::Geometry* geometry1, agxCollide::Geometry* geometry2 ) const
  {
    return geometry2 == m_sensor ? geometry1 : geometry2;
  }
 
  void ExplicitSplitSensorListener::handleGeometry( agxCollide::Geometry* geometry, agx::Bool isImpact ) const
  {
    agxAssert( geometry != nullptr && geometry != m_sensor );
 
    // If the geometry doesn't have properties set or if it hasn't got split enabled, ignore it.
    // NOTE: The getProperties method will first check if the geometry has the properties and if it
    //       doesn't, it will check the rigid body the geometry is part of.
    agxSDK::MergeSplitProperties* properties = agxSDK::MergeSplitHandler::getProperties( geometry );
    if ( properties == nullptr || !properties->getEnableSplit() )
      return;
 
    // If impact:
    //   1. Split the rigid body which the geometry is part of.
    //   2. Disable merge of the object.
    if ( isImpact ) {
      agxSDK::MergeSplitHandler::split( geometry->getRigidBody() );
      properties->setEnableMerge( false );
    }
    // If separation - enable merge.
    else
      properties->setEnableMerge( true );
  }
}
 
osg::Group* buildTutorial4( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  // The random seed is set to a fixed value for our internal determinism-testing.
  srand(5);
  // Feel free to choose a non-fixed random seed for "real" randomness by uncommenting this line:
  //srand((unsigned int)time(nullptr));
 
 
  // Preparing on-screen text.
  application->getSceneDecorator()->setFontSize( 0.01f );
  application->getSceneDecorator()->setText( 0, "Merged boxes are blue.", agxRender::Color::LightBlue() );
  application->getSceneDecorator()->setText( 1, "Free boxes are yellow.", agxRender::Color::Yellow() );
 
  // Static ground.
  agx::RigidBodyRef ground = new agx::RigidBody( "ground" );
  ground->add( new agxCollide::Geometry( new agxCollide::Box( 20, 20, 0.5 ) ) );
  ground->setPosition( 0, 0, -0.5 );
  ground->setMotionControl( agx::RigidBody::STATIC );
 
  simulation->add( ground );
  agxOSG::createVisual( ground, root );
 
  // Create grid of boxes and merge the to ground.
  agx::MergedBodyRef mergedBody = new agx::MergedBody();
  for ( agx::Real x = -18.0; x < 19.0; x += 2.0 ) {
    for ( agx::Real y = -18.0; y < 19.0; y += 2.0 ) {
      agx::RigidBodyRef box = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 0.5, 0.5, 0.5 ) ) );
      box->setPosition( agx::Vec3( x, y, 0.499 ) );
      simulation->add( box );
 
      osg::Group* node = agxOSG::createVisual( box, root );
      agxOSG::setDiffuseColor( node, agxRender::Color::LightBlue() );
 
      // Enable merge and split for the box.
      agxSDK::MergeSplitHandler::getOrCreateProperties( box )->setEnableMergeSplit( true );
 
      // Merge given a contact generator edge.
      mergedBody->add( new agx::MergedBody::ContactGeneratorEdgeInteraction( ground, box ) );
 
      // In post-step, sets color given merged state.
      agxUtil::StepEventCallback::post( []( agxSDK::Simulation* /*simulation*/, agx::RigidBody* box, osg::Group* node )
      {
        if ( agx::MergedBody::getActive( box ) != nullptr )
          agxOSG::setDiffuseColor( node, agxRender::Color::LightBlue() );
        else
          agxOSG::setDiffuseColor( node, agxRender::Color::Yellow() );
      }, simulation, box, node );
    }
  }
 
  // Hand over the merged body to the handler.
  simulation->getMergeSplitHandler()->registerMergedBody( mergedBody );
 
  // Enable the merge-split handler.
  simulation->getMergeSplitHandler()->setEnable( true );
 
  // Enable merge for ground.
  agxSDK::MergeSplitHandler::getOrCreateProperties( ground )->setEnableMerge( true );
 
  // Create the sensor sphere and create our explicit split listener.
  agxCollide::GeometryRef sensor = new agxCollide::Geometry( new agxCollide::Sphere( 3.0 ) );
  sensor->setPosition( 12, 0, 0 );
  sensor->setSensor( true );
  simulation->add( sensor );
 
  simulation->add( new tutorial4::ExplicitSplitSensorListener( sensor ) );
 
  auto* sensorNode = agxOSG::createVisual( sensor, root, 1.0f, false, true );
  agxOSG::setDiffuseColor( sensorNode, agxRender::Color::Red() );
  agxOSG::setAlpha( sensorNode, 0.25f );
 
  // Updates position of the sensor sphere.
  agx::Vec3 velocity = agx::Vec3( 10, 10, 0 );
  agxUtil::StepEventCallback::preCollide( [ sensor ]( agxSDK::Simulation* simulation, agx::Vec3& velocity )
  {
    const agx::Vec3 currentPosition = sensor->getPosition();
    if ( !agxCollide::pointInBox( currentPosition, agx::Vec3( 20, 20, 0.5 ) ) ) {
      auto indexMax = agx::absolute( currentPosition ).maxElement();
      agx::Vec3 normal;
      normal[ indexMax ] = -currentPosition[ indexMax ] / currentPosition[ indexMax ];
      // Rotate the normal by some small, random amount.
      normal = agx::Quat::rotate( agx::random( -5.0 * agx::DEG_TO_RAD, 5.0 * agx::DEG_TO_RAD ), agx::Vec3::Z_AXIS() ) * normal;
      velocity -= 2.0 * velocity * normal * normal;
    }
 
    sensor->setPosition( sensor->getPosition() + simulation->getTimeStep() * velocity );
  }, simulation, velocity );
 
  return root;
}
 
 
 
/*
Tutorial merge-split handler 5.
 
Use merge ignore filters to prevent merges that would otherwise have happened.
*/
osg::Group* buildTutorial5(agxSDK::Simulation* simulation, agxOSG::ExampleApplication*)
{
  osg::Group* root = new osg::Group();
 
  agxSDK::MergeSplitHandler* mergeSplitHandler = simulation->getMergeSplitHandler();
  mergeSplitHandler->setEnable(true);
 
  // A merge ignore filter is a rejection filter that prevents merging. The
  // filter operates on groups and individual MergeSplitProperties can be made
  // part of these groups. Groups are identified either by an UInt32, called the
  // group's ID, or a name. Here we use the value 1 as the ID for one group and
  // the value 2 for another group.
  agx::UInt32 filterGroup1{1};
  agx::UInt32 filterGroup2{2};
 
  agx::Real large = agx::Real(1);
  agx::Real small = agx::Real(0.1);
 
  // To use a group ID we need a MergeSplitProperty, which we can get from a
  // body. So lets create a floor.
  agx::Vec3 floorHe(large, large, small);
  agx::Vec3 floorPos(agx::Real(0), agx::Real(0), -small);
  agxRender::Color floorColor = agxRender::Color::Gray();
  agx::RigidBodyRef floor = createBox(floorHe, floorPos, floorColor, simulation, root);
  floor->setMotionControl(agx::RigidBody::STATIC);
 
  // Merge is enabled for the floor so that other bodies can be merged with it.
  // However, by adding the filter group we can have additional control.
  agxSDK::MergeSplitProperties* floorProperties = agxSDK::MergeSplitHandler::getOrCreateProperties(floor);
  floorProperties->setEnableMerge(true);
  floorProperties->addGroup(filterGroup1);
 
  agx::Vec3 barHe(small, large, small);
  agx::Vec3 barPos(-agx::Real(4) * small, agx::Real(0), small);
  agxRender::Color barColor = agxRender::Color::DarkGray();
 
  // Another body is created on the floor and merge is enabled for this body as
  // well. Since merge is enabled for both the floor and the new body, they will
  // merge.
  agx::RigidBodyRef leftBar = createBox(barHe, barPos, barColor, simulation, root);
  agxSDK::MergeSplitProperties* leftBarProperties = agxSDK::MergeSplitHandler::getOrCreateProperties(leftBar);
  leftBarProperties->setEnableMerge(true);
 
  // Let's create a third body, much like the second one, but this time with a
  // filter group.
  barPos.x() = -barPos.x();
  agx::RigidBodyRef rightBar = createBox(barHe, barPos, barColor, simulation, root);
  agxSDK::MergeSplitProperties* rightBarProperties = agxSDK::MergeSplitHandler::getOrCreateProperties(rightBar);
  rightBarProperties->setEnableMerge(true);
  rightBarProperties->addGroup(filterGroup2);
 
  // We filter away merging between the floor and the bar with the filter group
  // by disabling that group pair on the MergeSplitHandler.
  mergeSplitHandler->setEnableMergePair(filterGroup1, filterGroup2, false);
 
  // We have now created the following system, where * indicate a merge:
  //    ------      ------
  //    |    |      | 2  |
  //    ******      ------
  //  --******---------------
  //  |         1           |
  //  -----------------------
 
  // Next we create a bar spanning the two we already have.
  barHe = agx::Vec3(large, small, small);
  barPos = agx::Vec3(agx::Real(0), agx::Real(0), agx::Real(3) * small);
  agx::RigidBodyRef topBar = createBox(barHe, barPos, barColor, simulation, root);
  agxSDK::MergeSplitProperties* frontBarProperties = agxSDK::MergeSplitHandler::getOrCreateProperties(topBar);
  frontBarProperties->setEnableMerge(true);
 
  // By adding the second filter group to the bar and disabling merging of
  // objects within that group we prevent the top bar from merging with the
  // right bar. There is no filter for the left bar, so it can merge with the
  // new one.
  frontBarProperties->addGroup(filterGroup2);
  mergeSplitHandler->setEnableMergePair(filterGroup2, filterGroup2, false);
 
  // We  have now created the following system.
  //  -----------------------
  //  |         2           |
  //  --******---------------
  //    ******      ------
  //    |    |      | 2  |
  //    ******      ------
  //  --******---------------
  //  |         1           |
  //  -----------------------
  //
  // Notice that the left bar has merged with both the floor and the top bar,
  // forming a merged body that includes both the floor and the top bar despite
  // those two containing groups for which there is a merge ignore filter. That
  // is because the filter is applied against the two bodies being merged, and
  // not against ALL bodies in the merge body.
 
  // Constraints have their own set of filter groups that override the groups
  // for the bodies that the constraint constrains. Let's create some boxes and
  // a constraint.
  agx::Vec3 boxHe(small, small, small);
  agx::Vec3 boxPos(small + small + large, agx::Real(0.0), agx::Real(0.0));
  agx::RigidBodyRef bottomBox = createBox(boxHe, boxPos, barColor, simulation, root);
  bottomBox->setMotionControl(agx::RigidBody::STATIC);
  boxPos.z() += agx::Real(3) * small;
  agx::RigidBodyRef topBox = createBox(boxHe, boxPos, barColor, simulation, root);
  agx::LockJointRef lock = agx::Constraint::createFromBody<agx::LockJoint>(agx::Vec3(), agx::Z_AXIS, bottomBox, topBox);
  simulation->add(lock);
 
  // By adding the disabled filter groups to the two bodies we disable merging
  // based on contacts, but by enabling merge for the constraint and not adding
  // any filter groups to that we allow the system to merge based on the
  // constraint despite.
  agxSDK::MergeSplitHandler::getOrCreateProperties(bottomBox)->addGroup(filterGroup1);
  agxSDK::MergeSplitHandler::getOrCreateProperties(topBox)->addGroup(filterGroup2);
  agxSDK::MergeSplitHandler::getOrCreateProperties(lock)->setEnableMerge(true);
 
  simulation->stepTo(agx::TimeStamp(1.0));
 
  return root;
}
 
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout << "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
               "\tTutorial Merge Split Handler (Adaptive Model Order Reduction)\n" <<
               "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
    application->addScene( buildTutorial3, '3' );
    application->addScene( buildTutorial4, '4' );
    application->addScene( buildTutorial5, '5' );
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_mergedBody.cpp

/*
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.
*/
 
/*
 
Demonstrates the API for merging rigid bodies into a collection acting as one single rigid body.
 
*/
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/utils.h>
 
#include <agxSDK/Assembly.h>
#include <agxSDK/RigidBodyFilter.h>
 
#include <agxUtil/StepEventCallback.h>
 
#include <agx/LockJoint.h>
#include <agx/Hinge.h>
#include <agx/MergedBody.h>
 
#include <agxCollide/Box.h>
 
#include <agxRender/RenderSingleton.h>
#include <agx/version.h>
 
namespace
{
  agxSDK::AssemblyRef createBoxRow( agx::UInt numBoxes, agx::Real offset, osg::Group* root, const agx::Vec4f& color )
  {
    if ( numBoxes < 1 )
      return nullptr;
 
    agxSDK::AssemblyRef assembly = new agxSDK::Assembly();
    agx::Vec3 position = agx::Vec3( agx::Real( -0.5 * ( 1.0 + offset ) ) * agx::Real( numBoxes - 1 ), 0, 0 );
    for ( agx::UInt i = 0; i < numBoxes; ++i ) {
      agx::RigidBodyRef box = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 0.5, 0.5, 0.5 ) ) );
      box->setPosition( position );
      assembly->add( box );
 
      agxOSG::setDiffuseColor( agxOSG::createVisual( box, root ), color );
 
      position.x() += agx::Real( 1.0 ) + offset;
    }
    return assembly;
  }
 
  class OnImpactCallback : public agxSDK::ContactEventListener
  {
    public:
      typedef std::function< void( agxCollide::GeometryContact* ) > Callback;
 
      static void createSplitOnImpact( agx::RigidBody* rbToSplit, agxSDK::Simulation* simulation )
      {
        Callback callback = [rbToSplit, simulation]( agxCollide::GeometryContact* )
        {
          agx::MergedBodyRef mb = agx::MergedBody::getActive( rbToSplit );
          if ( mb == nullptr ) return;
 
          mb->remove( rbToSplit );
          if ( mb->isEmpty() )
            simulation->remove( mb );
        };
        simulation->add( new OnImpactCallback( new agxSDK::RigidBodyFilter( rbToSplit ), callback ) );
      }
 
    public:
      OnImpactCallback( agxSDK::ExecuteFilter* filter, Callback callback )
        : agxSDK::ContactEventListener( agxSDK::ContactEventListener::IMPACT, filter ),
          m_callback( callback )
      {
      }
 
      virtual agxSDK::ContactEventListener::KeepContactPolicy impact(const agx::TimeStamp&, agxCollide::GeometryContact* gc ) override
      {
        if ( m_callback )
          m_callback( gc );
 
        return KEEP_CONTACT;
      }
 
    private:
      Callback m_callback;
  };
}
 
osg::Group* buildTutorial1( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Static ground.
  agx::RigidBodyRef ground = new agx::RigidBody( "ground" );
  ground->add( new agxCollide::Geometry( new agxCollide::Box( 10, 10, 0.5 ) ) );
  ground->setMotionControl( agx::RigidBody::STATIC );
 
  simulation->add( ground );
  agxOSG::createVisual( ground, root );
 
  // Create a row of rigid bodies. These boxes will not be merged.
  agxSDK::AssemblyRef boxRow1 = ::createBoxRow( 5, agx::Real( 0.01 ), root, agxRender::Color::Red() );
  boxRow1->setPosition( 0, 1, 3 );
  boxRow1->setRotation( agx::Quat( agx::PI_4, agx::Vec3::Y_AXIS() ) );
  simulation->add( boxRow1 );
 
  // Create another row of rigid bodies. These boxes will be merged to one rigid body (below).
  agxSDK::AssemblyRef boxRow2 = ::createBoxRow( 5, agx::Real( 0.01 ), root, agxRender::Color::Green() );
  boxRow2->setPosition( 0, -1, 3 );
  boxRow2->setRotation( agx::Quat( agx::PI_4, agx::Vec3::Y_AXIS() ) );
  simulation->add( boxRow2 );
 
  // Create a merged body. This body is similar to an assembly with rigid bodies but
  // properties such as mass, inertia, velocity etc are calculated internally and
  // the solver sees only one rigid body.
  agx::MergedBodyRef mergedBody = new agx::MergedBody();
 
  // Add the 'boxRow2' rigid bodies to the merged body.
  // The merged body must know how a pair of bodies are connected because this
  // defines the behavior when a rigid body is being removed from the merged body.
  // Current set of 'edge interactions':
  //   - MergedBody::EmptyEdgeInteraction( rb1, rb2 ):                 Nothing happens when the edge is removed.
  //
  //   - MergedBody::ContactGeneratorEdgeInteraction( rb1, rb2 ):      When the edge is removed, new contact points will be calculated
  //                                                                   between rb1 and rb2 and if there are overlaps, the contact will be
  //                                                                   added to the solver.
  //
  //  - MergedBody::GeometryContactEdgeInteraction( geometryContact ): Contact data is stored and will be restored when the edge is removed.
  // NOTE: buildTutorial3 visualizes the differences between these edge interactions!
 
  for ( agx::UInt i = 1; i < boxRow2->getRigidBodies().size(); ++i ) {
    // Add given empty edge interaction.
    mergedBody->add( new agx::MergedBody::EmptyEdgeInteraction( boxRow2->getRigidBodies()[ i - 1 ], boxRow2->getRigidBodies()[ i ] ) );
  }
 
  // The merged body has to be added to the simulation.
  simulation->add( mergedBody );
 
  return root;
}
 
osg::Group* buildTutorial2( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Static ground.
  agx::RigidBodyRef ground = new agx::RigidBody( "ground" );
  ground->add( new agxCollide::Geometry( new agxCollide::Box( 10, 10, 0.5 ) ) );
  ground->setPosition( 0, 0, -0.5 );
  ground->setMotionControl( agx::RigidBody::STATIC );
 
  simulation->add( ground );
  agxOSG::createVisual( ground, root );
 
  // A 'split box', so when the merged body collide with this box we will split the merged body.
  agx::RigidBodyRef splitBox = new agx::RigidBody( "splitBox" );
  splitBox->add( new agxCollide::Geometry( new agxCollide::Box( 0.1, 10, 1 ) ) );
  splitBox->add( new agxCollide::Geometry( new agxCollide::Box( 0.3, 10, 0.3 ) ), agx::AffineMatrix4x4::translate( 3.5, 0, -1 + 0.3 ) );
  splitBox->setPosition( 0, 0, 1 );
  splitBox->setMotionControl( agx::RigidBody::STATIC );
 
  simulation->add( splitBox );
  agxOSG::setDiffuseColor( agxOSG::createVisual( splitBox, root ), agxRender::Color::Yellow() );
 
  // Create a row of rigid bodies. These boxes will be merged.
  agxSDK::AssemblyRef boxRow = ::createBoxRow( 9, agx::Real( 0.01 ), root, agxRender::Color::Red() );
  boxRow->setPosition( 0, 1, 3 );
  simulation->add( boxRow );
 
  // Create the merged body and add the box row.
  agx::MergedBodyRef mergedBody = new agx::MergedBody();
  for ( agx::UInt i = 1; i < boxRow->getRigidBodies().size(); ++i ) {
    // Add given empty edge interaction.
    mergedBody->add( new agx::MergedBody::EmptyEdgeInteraction( boxRow->getRigidBodies()[ i - 1 ], boxRow->getRigidBodies()[ i ] ) );
  }
  simulation->add( mergedBody );
 
  ::OnImpactCallback::Callback onImpact = [simulation]( agxCollide::GeometryContact* gc )
  {
    // We've impact with 'splitBox', check for the merged body container in the geometry contact:
    agx::MergedBodyRef mb1 = agx::MergedBody::getActive( gc->rigidBody( 0 ) );
    agx::MergedBodyRef mb2 = agx::MergedBody::getActive( gc->rigidBody( 1 ) );
 
    // Note that the splitBox isn't merged, so one of these should be null.
    agxAssert( mb1 == nullptr || mb2 == nullptr );
 
    agx::MergedBody* mb = mb1 != nullptr ? mb1 : mb2;
    agx::RigidBody* rbToSplit = mb == mb1 ? gc->rigidBody( 0 ) : gc->rigidBody( 1 );
    if ( mb == nullptr )
      return;
 
    // Split/remove.
    mb->remove( rbToSplit );
 
    // Since we're not keeping track of the merged bodies we should
    // remove empty instances from the simulation.
    if ( mb->isEmpty() )
      simulation->remove( mb );
  };
  simulation->add( new ::OnImpactCallback( new agxSDK::RigidBodyFilter( splitBox ), onImpact ) );
 
  return root;
}
 
osg::Group* buildTutorial3( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Static ground.
  agx::RigidBodyRef ground = new agx::RigidBody( "ground" );
  ground->add( new agxCollide::Geometry( new agxCollide::Box( 10, 10, 0.1 ) ) );
  ground->setPosition( 0, 0, -0.1 );
  ground->setMotionControl( agx::RigidBody::STATIC );
 
  simulation->add( ground );
  agxOSG::createVisual( ground, root );
 
  // Our merged body container, including ground this time.
  agx::MergedBodyRef mergedBody = new agx::MergedBody();
  simulation->add( mergedBody );
 
  // Create boxes and place them on the ground and merge them with ground.
  auto createBox = [simulation, root]( agx::Vec3 position, agxRender::Color color ) -> agx::RigidBodyRef
  {
    agx::RigidBodyRef rb = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 0.5, 0.5, 0.5 ) ) );
    rb->setPosition( position );
    simulation->add( rb );
    agxOSG::setDiffuseColor( agxOSG::createVisual( rb, root ), color );
    return rb;
  };
 
  agx::RigidBodyRef rbLeft  = createBox( agx::Vec3( -2, 0, 0.495 ), agxRender::Color::Green() );
  agx::RigidBodyRef rbRight = createBox( agx::Vec3(  2, 0, 0.495 ), agxRender::Color::Red() );
 
  // Merge left box with ground. If 'rbLeft' is overlapping ground, a new geometry
  // contact will be created and added at the time the split happens.
  mergedBody->add( new agx::MergedBody::ContactGeneratorEdgeInteraction( rbLeft, ground ) );
 
  // Merge right box with ground with an empty edge interaction. When this edge is
  // removed (e.g., during an impact event), no new contact will be created until
  // next time collision detection executes.
  mergedBody->add( new agx::MergedBody::EmptyEdgeInteraction( rbRight, ground ) );
 
  // Create boxes to drop onto the left and right box.
  agx::RigidBodyRef rbLeftDrop  = createBox( agx::Vec3( -2, 0, 37.5 ), agxRender::Color::Black() );
  agx::RigidBodyRef rbRightDrop = createBox( agx::Vec3(  2, 0, 37.5 ), agxRender::Color::Black() );
 
  // Will split when something impacts with rbLeft and rbRight.
  ::OnImpactCallback::createSplitOnImpact( rbLeft, simulation );
  ::OnImpactCallback::createSplitOnImpact( rbRight, simulation );
 
  return root;
}
 
osg::Group* buildTutorial4( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Static ground.
  agx::RigidBodyRef ground = new agx::RigidBody( "ground" );
  ground->add( new agxCollide::Geometry( new agxCollide::Box( 10, 10, 0.1 ) ) );
  ground->setPosition( 0, 0, 0.4 );
  ground->setMotionControl( agx::RigidBody::STATIC );
 
  simulation->add( ground );
  agxOSG::createVisual( ground, root );
 
  // Create two bodies and hinge them together.
  agx::RigidBodyRef rb1 = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 0.5, 0.5, 0.5 ) ) );
  agx::RigidBodyRef rb2 = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 0.5, 0.5, 0.5 ) ) );
 
  rb1->setPosition( -1, 0, 2 );
  rb2->setPosition(  1, 0, 2 );
 
  simulation->add( rb1 );
  simulation->add( rb2 );
 
  agxOSG::setDiffuseColor( agxOSG::createVisual( rb1, root ), agxRender::Color::Green() );
  agxOSG::setDiffuseColor( agxOSG::createVisual( rb2, root ), agxRender::Color::Yellow() );
 
  // Hinge rb1 in world.
  agx::HingeRef rb1Hinge = agx::Constraint::createFromBody<agx::Hinge>( agx::Vec3(), agx::Vec3::Y_AXIS(), rb1 );
  simulation->add( rb1Hinge );
 
  // Hinge rb1 to rb2.
  agx::HingeRef rb1Rb2Hinge = agx::Constraint::createFromWorld<agx::Hinge>( 0.5 * ( rb1->getPosition() + rb2->getPosition() ), agx::Vec3::Y_AXIS(), rb1, rb2 );
  simulation->add( rb1Rb2Hinge );
 
  // Create a merged body and add the binary constraint edge ("binary" since one or two body constraint).
  agx::MergedBodyRef mergedBody = new agx::MergedBody();
  simulation->add( mergedBody );
 
  mergedBody->add( new agx::MergedBody::BinaryConstraintEdgeInteraction( rb1Rb2Hinge ) );
 
  agxUtil::StepEventCallback::preCollide( [ rb1Rb2Hinge ]( agxSDK::Simulation* simulation )
  {
    agx::TimeStamp t = simulation->getTimeStamp();
 
    // Find the merged body given the constraint (if merged).
    agx::MergedBodyRef mergedBody = agx::MergedBody::get( rb1Rb2Hinge );
 
    // Split when we've simulated for one second.
    if ( agx::equivalent( t, 1.0 ) && mergedBody != nullptr ) {
      const agx::Bool success = mergedBody->remove( agx::MergedBody::getEdge( rb1Rb2Hinge ) );
      std::cout << "Hinge edge interaction successfully removed? " << ( success ? "Yes!" : "No." ) << std::endl;
 
      // If the merged body is empty after we've removed the edge - remove
      // the merged body from the simulation.
      if ( mergedBody->isEmpty() )
        simulation->remove( mergedBody );
    }
    // Merge again after two seconds.
    else if ( agx::equivalent( t, 2.0 ) && mergedBody == nullptr ) {
      mergedBody = new agx::MergedBody();
      const agx::Bool success = mergedBody->add( new agx::MergedBody::BinaryConstraintEdgeInteraction( rb1Rb2Hinge ) );
      std::cout << "Hinge edge interaction successfully added? " << ( success ? "Yes!" : "No." ) << std::endl;
 
      if ( success )
        simulation->add( mergedBody );
    }
  }, simulation );
 
  return root;
}
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout << "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
               "\tTutorial Merged Bodies\n" <<
               "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
    application->addScene( buildTutorial3, '3' );
    application->addScene( buildTutorial4, '4' );
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_particleSystem.cpp

/*
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.
*/
 
 
/*
 
Demonstrates the use of the particle system API.
 
*/
 
#include <agxOSG/ExampleApplication.h>
 
#include <agx/version.h>
#include <agx/RigidParticleSystem.h>
#include <agx/ParticleEmitter.h>
#include "tutorialUtils.h"
 
#include <agxOSG/ReaderWriter.h>
 
//
// Tutorial 1: Creating a basic particle system which interacts with other geometries
//
osg::Group* buildTutorial1( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* /*application*/ )
{
  std::cout << "\nBuilding Scene 1 - Single pillar of rigid particles" << std::endl;
  std::cout << "Demonstrates how to create and initialize particles one by one."  << std::endl;
  osg::Group *root = new osg::Group;
 
  // Create a geometry with a box shape as our floor. We create a box instead
  // of a plane because OSG currently don't know abut the particles and
  // therefore cannot set a view frustum than includes them and because
  // 'agxOSG.createVisual' for a plane appears to do nothing.
  agxCollide::GeometryRef floorGeometry = new agxCollide::Geometry( new agxCollide::Box( agx::Vec3(5, 5, 0.1)));
  simulation->add(floorGeometry);
  agxOSG::createVisual( floorGeometry, root );
 
  // Create a spherical body that will interact with the pillar of particles.
  agx::RigidBodyRef rb1 = new agx::RigidBody();
  rb1->add(new agxCollide::Geometry(new agxCollide::Sphere(0.2)));
  rb1->setPosition(0,0,1.0);
  rb1->setVelocity( agx::Vec3(0.3, 0.3, 0) );
  simulation->add(rb1);
  agxOSG::createVisual(rb1, root);
 
  // Create a rigid particle system and add to the simulation. A
  // RigidParticleSystem is a type of ParticleSystem where each particle is
  // seen as a hard sphere and a Gauss-Seidel solver is used to resolve
  // overlaps.
  agx::ParticleSystemRef particleSystem = new agx::RigidParticleSystem();
  simulation->add(particleSystem);
 
 
  // Create the visual representation of the ParticleSystem
  agxOSG::createVisual(particleSystem, root);
 
  // Create ten particles and position them one over the other.
  for(size_t i=1; i < 10; i++) {
    // The createParticle() method allocates and initializes a single particle
    // in the particle system. It returns a ParticlePtr which is a index
    // object used to find that particle's slot in the particle storage. It
    // has a set of utility functions that gives access to the particle's
    // attributes, such as position, directly.
    agx::Physics::ParticlePtr particle = particleSystem->createParticle();
    particle->setPosition(agx::Vec3(1, 1, static_cast<agx::Real>(i) * 0.3));
    particle->setRadius(0.1);
  }
  return root;
}
 
//
// Tutorial 2: Creating a particle system with an emitter that emits particles into a scene.
// The particles are given a life after which they will be removed.
//
 
class ParticleEmitterMover : public agxSDK::StepEventListener
{
public:
  ParticleEmitterMover( agx::ParticleEmitter *emitter, agxCollide::Geometry *geom ) : m_emitter(emitter), m_emitterGeom(geom), m_speed(4.0)
  {
    setMask( ParticleEmitterMover::PRE_STEP );
  }
 
  // Called before each time step
  void pre(const agx::TimeStamp& t)
  {
    if (!m_emitter) {
      getSimulation()->remove(this);
      return;
    }
 
    // Rotate the emitter somewhat
    m_emitterGeom->setRotation( agx::EulerAngles(0,0, (agx::PI/8)*sin(t/2) ) );
 
    // Give the particles an initial velocity
    m_emitter->setVelocity(  m_emitterGeom->getTransform().transform3x3( agx::Vec3(0,-1,0) )*m_speed );
  }
 
  agx::ParticleEmitterObserver m_emitter;
  agxCollide::GeometryObserver m_emitterGeom;
  agx::Real m_speed;
 
};
 
 
osg::Group* buildTutorial2( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* /*application*/ )
{
  std::cout << "Demonstrates how to use a particle emitter"  << std::endl;
  osg::Group *root = new osg::Group;
 
  // Create a geometry with a box shape as our floor. We create a box instead
  // of a plane because OSG currently don't know abut the particles and
  // therefore cannot set a view frustum than includes them and because
  // 'agxOSG.createVisual' for a plane appears to do nothing.
  agxCollide::GeometryRef floorGeometry = new agxCollide::Geometry( new agxCollide::Box( agx::Vec3(5, 5, 0.1)));
  simulation->add(floorGeometry);
  agxOSG::createVisual( floorGeometry, root );
 
 
  // Create a rigid particle system and add to the simulation. A
  // RigidParticleSystem is a type of ParticleSystem where each particle is
  // seen as a hard sphere and a Gauss-Seidel solver is used to resolve
  // overlaps.
  agx::ParticleSystemRef particleSystem = new agx::RigidParticleSystem();
  simulation->add(particleSystem);
 
  particleSystem->setParticleRadius( 0.04 );
  // Create the visual representation of the ParticleSystem
  agxOSG::createVisual(particleSystem, root);
 
 
  // We will be using an emitter to create a steady flow of particles. The
  // emitter uses a geometry to know where particles should be created, so we
  // create one.
  agxCollide::GeometryRef spawnGeom = new agxCollide::Geometry(new agxCollide::Cylinder(0.15, 0.1));
  spawnGeom->setPosition(0,2,1);
 
  simulation->add(spawnGeom);
  agxOSG::createVisual(spawnGeom, root);
 
  // Disable collision between the geometry and the particles
  particleSystem->setEnableCollisions( spawnGeom, false );
 
  // Create a particle emitter and pass it the spawn geometry we just created.
  agx::ParticleEmitterRef emitter = new agx::ParticleEmitter( particleSystem );
  emitter->setSeed(0);
  emitter->setGeometry( spawnGeom );
 
  simulation->add(emitter);
  // As with most tasks, the behavior of the emitter is controlled using
  // parameters. There are two parameters in the emitter that we are
  // interested in: rate and life.
 
  // Get the rate parameter from the emitter.
  emitter->setRate( 150 );
 
  // Repeat the parameter procedure with the 'life' parameter.
  emitter->setLife( 3.0 );
 
 
  // Create a listener that will rotate the emitter
  simulation->add( new ParticleEmitterMover(emitter, spawnGeom) );
 
  // Create some boxes to hit with the particles
  agx::Real startX = -1;
  agx::Real startY = 0;
  agx::Real delta = 0.5;
  agx::Real z= 0.3;
  for(size_t i=0; i < 4; i++)
  {
    for(size_t j=0; j < 4; j++)
    {
      agx::Vec3 pos(startX + static_cast<agx::Real>(i) * delta, startY + static_cast<agx::Real>(j) * delta, z);
      agx::RigidBodyRef body = new agx::RigidBody;
      body->setPosition( pos+agx::Vec3(0,0,0.1) );
      body->add( new agxCollide::Geometry( new agxCollide::Box(agx::Vec3(0.1,0.1,0.3) ) ) );
      simulation->add( body );
      agxOSG::createVisual( body, root );
    }
  }
 
  return root;
}
 
class SmokeEffect : public agxSDK::StepEventListener
{
public:
 
  SmokeEffect( agx::ParticleSystem *ps );
 
  void pre(const agx::TimeStamp& t);
 
protected:
  agx::ParticleSystemObserver m_ps;
};
 
SmokeEffect::SmokeEffect( agx::ParticleSystem *ps ) : m_ps(ps)
{
  if (!m_ps->getMaterial())
    m_ps->setMaterial(new agx::Material("ParticleMaterial"));
 
  setMask(SmokeEffect::PRE_STEP);
}
 
void SmokeEffect::pre(const agx::TimeStamp& /*t*/)
{
 
  if (!m_ps)
  {
    getSimulation()->remove(this);
    return;
  }
 
  agx::Real viscosity = 1.8e-5f;
  agx::Real viscosityCoefficient = 6 * agx::PI * viscosity;
 
  agx::Real density = 1.2929f;
  agx::Real densityCoefficient = 0.2f * agx::PI * density;
 
 
  agxData::Buffer *forceBuffer = m_ps->getResource<agxData::Buffer>("Particle.force");
  agxData::Array<agx::Vec3> forces = forceBuffer->getArray<agx::Vec3>();
 
  agxData::Buffer *velocityBuffer = m_ps->getResource<agxData::Buffer>("Particle.velocity");
  agxData::Array<agx::Vec3> velocities = velocityBuffer->getArray<agx::Vec3>();
 
  agx::Real radius = m_ps->getParticleRadius();
  agx::Real mass = m_ps->getParticleMass();
  agx::Vec3 G = getSimulation()->getUniformGravity();
  G=G*(-1)-agx::Vec3(0,0,-0.1);
 
  agx::Vec3 m_wind(0,0,0);
 
  agx::Real four_over_three = 4.0/3.0;
  agx::Real area = agx::PI*radius*radius;
  agx::Real volume = area*radius*four_over_three;
 
  // compute force due to gravity + buoyancy of displacing the fluid that the particle is emerged in.
  agx::Vec3 boyancy = G * density*volume;
 
  agx::Vec3 accel_force = G * (mass - density*volume);
 
  size_t n = forces.size();
  for (size_t i=0; i<n; ++i)
  {
    // compute force due to friction
    agx::Vec3 relative_wind = velocities[i]-m_wind;
    agx::Vec3 wind_force = - relative_wind * area * (viscosityCoefficient + densityCoefficient*relative_wind.length());
 
    agx::Vec3 force = (wind_force+accel_force+boyancy);
    forces[i] += force;
  }
}
 
 
osg::Group* buildTutorial3( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* application )
{
  std::cout << "Demonstrates how to access and add forces to particles by simulating a very simple smoke."  << std::endl;
  osg::Group *root = new osg::Group;
 
  // Create a rigid particle system and add to the simulation. A
  // RigidParticleSystem is a type of ParticleSystem where each particle is
  // seen as a hard sphere and a Gauss-Seidel solver is used to resolve
  // overlaps.
  agx::ParticleSystemRef particleSystem = new agx::RigidParticleSystem();
  simulation->add(particleSystem);
 
//   particleSystem->addCollisionGroup(1);
//   simulation->getSpace()->setEnablePair(1,1,false);
//   agx::Task* collDetTask = particleSystem->getResource<agx::Task>("Update.CollisionDetection.ParticleParticleCollider");
//   if (collDetTask)
//     collDetTask->setEnable(false);
 
  // Add a StepEventListener (implemented above) that will calculate and add forces to the particles
  simulation->add(new SmokeEffect(particleSystem));
 
  particleSystem->setParticleRadius( 0.01 );
  particleSystem->setParticleMass( 0.01 );
  // Create the visual representation of the ParticleSystem
  agxOSG::createVisual(particleSystem, root);
 
 
  // We will be using an emitter to create a steady flow of particles. The
  // emitter uses a geometry to know where particles should be created, so we
  // create one.
  agxCollide::GeometryRef spawnGeom = new agxCollide::Geometry(new agxCollide::Sphere(0.15));
  spawnGeom->setPosition(0,0,0);
 
  simulation->add(spawnGeom);
  agxOSG::createVisual(spawnGeom, root);
 
  // Disable collision between the geometry and the particles
  particleSystem->setEnableCollisions( spawnGeom, false );
 
  // Create a particle emitter and pass it the spawn geometry we just created.
  agx::ParticleEmitterRef emitter = new agx::ParticleEmitter( particleSystem );
  emitter->setGeometry( spawnGeom );
  emitter->setSeed(0);
 
  simulation->add(emitter);
  // As with most tasks, the behavior of the emitter is controlled using
  // parameters. There are two parameters in the emitter that we are
  // interested in: rate and life.
 
  // Get the rate parameter from the emitter.
  emitter->setRate( 600 );
 
  // Repeat the parameter procedure with the 'life' parameter.
  emitter->setLife( 10.0 );
 
  int n = 15;
  agx::Real length = 1.5;
  agx::Real radius = 0.01;
  agx::Real delta = radius*3.8;
  agx::Real xStart = 0.1+-n*(delta+radius*2)/2;
  agx::Real yStart = 0;
 
  for (int i=0; i < n; i++)
  {
    agxCollide::GeometryRef g = new agxCollide::Geometry( new agxCollide::Cylinder(radius,length));
    simulation->add(g);
    agxOSG::createVisual(g,root);
    g->setPosition(xStart+(delta+radius)*i, yStart,0.3);
  }
 
 
  application->setCameraHome( agx::Vec3(-2.08495, 0.963726, 1.50669), agx::Vec3(0,0,0), agx::Vec3(0.442041, -0.320361, 0.837836));
 
  return root;
}
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial particle system\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
    application->addScene( buildTutorial3, '3' );
 
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_pidController.cpp

/*
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.
*/
 
/*
 
Tutorials for agxModel::PidController1D
 
This tutorial contains two examples of PID controlled objects in AGX. One winch and one prismatic.
 
*/
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/utils.h>
#include <agxOSG/WireRenderer.h>
#include <agxSDK/StepEventListener.h>
#include <agxSDK/Simulation.h>
#include <agx/RigidBody.h>
#include <agx/Prismatic.h>
#include <agx/LockJoint.h>
#include <agx/Task.h>
#include <agxCollide/Box.h>
#include <agxCollide/Cylinder.h>
#include <agxWire/Wire.h>
#include <agxWire/Winch.h>
#include <agxModel/ControllerHandler.h>
#include <agxModel/PidController1D.h>
#include <agx/version.h>
 
class PlantWinch : public agxModel::ControllerHandler::Plant
{
public:
  PlantWinch(agxWire::Winch* winch, agx::RigidBody* pendulumWeight) : m_winch(winch), m_pendulumWeight(pendulumWeight)
  {
  }
 
  agx::Real getProcessVariable(const agx::TimeStamp& time) override
  {
    // Update the pendulum base oscillating motion.
    agx::Real winchBaseVelocity = 0.5 * std::cos(1 / 10.0 * time);
    m_winch->getRigidBody()->setVelocity(agx::Vec3(0, 0, winchBaseVelocity));
    // Here the Plant Value is the z position of the load attached to the wire in the winch.
    return m_pendulumWeight->getPosition().z();
  }
 
  void setManipulatedVariable(agx::Real manipulatedVariable) override
  {
     // The PID manipulate variable is set as the winch speed.
    // Negative since a negative error should pull in the weight.
    m_winch->setSpeed(-manipulatedVariable);
  }
 
private:
  agxWire::WinchRef m_winch;
  agx::RigidBodyRef m_pendulumWeight;
};
 
class ShipPositionController : public agxSDK::StepEventListener
{
public:
  ShipPositionController(agx::RigidBodyRef ship, agxModel::PidController1D* pid,
     agxOSG::ExampleApplication* application) :
    m_ship(ship), m_pid(pid), m_application(application)
  {
  }
 
  virtual void pre(agx::TimeStamp const& time) override
  {
    m_ship->setVelocity(agx::Vec3(0, 0, 1.0 * sin(0.2 * 3.14 * time)));
 
    // Print some PID info to screen
    if (m_application) {
      m_application->getSceneDecorator()->setText(1, "Last 10 seconds cost function value " + std::to_string(m_pid->calculateCostFunctionValue(10)));
      m_application->getSceneDecorator()->setText(2, "Current error                       " + std::to_string(m_pid->getError()) + " meters.");
      if (time > 8)
        m_application->getSceneDecorator()->setText(0, "PID is active");
      else
        m_application->getSceneDecorator()->setText(0, "PID deactivated");
      }
  }
 
private:
  agx::RigidBodyRef m_ship;
  agxModel::PidController1DRef m_pid;
  agxOSG::ExampleApplicationObserver m_application;
};
 
agx::RigidBodyRef createReferencePlane(agxSDK::Simulation* simulation, osg::Group* root)
{
  agxCollide::GeometryRef loadReferenceCenterGeometry = new agxCollide::Geometry(new agxCollide::Box(10, 10, 1));
  loadReferenceCenterGeometry->setEnableCollisions(false);
  agx::RigidBodyRef loadReferenceRb = new agx::RigidBody(loadReferenceCenterGeometry);
  loadReferenceRb->setMotionControl(agx::RigidBody::STATIC);
  auto visual = agxOSG::createVisual(loadReferenceRb, root, 0.1f);
  agxOSG::setDiffuseColor(visual, agxRender::Color::AliceBlue());
  agxOSG::setAlpha(visual, 0.2f);
  simulation->add(loadReferenceRb);
  return loadReferenceRb;
}
 
std::tuple<agxWire::WinchRef, agx::RigidBodyRef> createWirePendulumWithWinch(agxSDK::Simulation* simulation, osg::Group* root, const agx::Vec3 &initialPosition)
{
  agx::RigidBodyRef winchBase = new agx::RigidBody("winchBase");
  winchBase->add(new agxCollide::Geometry(new agxCollide::Box(agx::Vec3(1, 1, 1))));
  winchBase->setMotionControl(agx::RigidBody::KINEMATICS);
  agxOSG::setDiffuseColor(agxOSG::createVisual(winchBase, root, 1.0f), agxRender::Color::ForestGreen());
 
  agx::RigidBodyRef pendulumWeight = new agx::RigidBody("pendulumWeight");
  pendulumWeight->add(new agxCollide::Geometry(new agxCollide::Sphere(1)));
  agxOSG::setDiffuseColor(agxOSG::createVisual(pendulumWeight, root, 1.0f), agxRender::Color::IndianRed());
 
  // The initial state
  pendulumWeight->setPosition(initialPosition);
 
  simulation->add(winchBase);
  simulation->add(pendulumWeight);
 
  // Create wire to hang between the winch base and the winch load
  agxWire::WireRef wire = new agxWire::Wire(0.015, 1, false);
  // Attach wire to load
  wire->add(new agxWire::BodyFixedNode(pendulumWeight));
  // Wire nodes
  wire->add(new agxWire::FreeNode(winchBase->getPosition()));
 
  // Create winch
  agxWire::WinchRef winch = new agxWire::Winch(winchBase, agx::Vec3(0, 0, 0), agx::Vec3(0, 0, -1));
  agx::Real pulledInLength = pendulumWeight->getPosition().distance(winchBase->getPosition());
  wire->add(winch, pulledInLength);
  simulation->add(wire);
 
  auto renderer = new agxOSG::WireRenderer(wire, root);
  simulation->add(renderer);
 
  return std::make_tuple(winch, pendulumWeight);
}
 
 
osg::Group* winchPidControlled( agxSDK::Simulation *simulation, agxOSG::ExampleApplication * /*application*/  )
{
  LOGGER_WARNING() << LOGGER_STATE( agx::Notify::PRINT_NONE ) << "\nCreating Scene " << __FUNCTION__ << std::endl << LOGGER_END();
 
  osg::Group *root = new osg::Group;
 
  // The initial state
  const agx::Vec3 initialPosition(-0.5, 0.5, -10);
 
  // Create the Pendulum scene with a load attached to wire in a winch
  agxWire::WinchRef winch;
  agx::RigidBodyRef pendulumWeight;
  std::tie(winch, pendulumWeight) = createWirePendulumWithWinch(simulation, root, initialPosition);
 
  // The Set Point for the controller, 10 meter below the winch.
  agx::Real setPoint = -10;
 
  // Add the a reference plane visualizing the PID controller target z level
  agx::RigidBodyRef loadReferenceRb = createReferencePlane(simulation, root);
  loadReferenceRb->setPosition(0, 0, setPoint);
 
  agxModel::ControllerHandler::PlantRef plant = new PlantWinch(winch, pendulumWeight);
  agx::Real proportionalGain = 2;
  agx::Real integralGain = 1;
  agx::Real derivativeGain = 1;
  agxModel::PidController1DRef pidController = new agxModel::PidController1D();
  pidController->setGains(proportionalGain, integralGain, derivativeGain);
  pidController->setSetPoint(setPoint);
  pidController->setManipulatedVariableLimit(-5, 5);
 
  agxModel::ControllerHandlerRef controller_handler = new agxModel::ControllerHandler(pidController, plant);
 
  simulation->add(controller_handler);
 
  return root;
}
 
 
std::tuple<agx::PrismaticRef, agx::RigidBodyRef, agx::RigidBodyRef> createGangwayWithPrismatic(agxSDK::Simulation* simulation, osg::Group* root)
{
  agx::RigidBodyRef fixPoint = new agx::RigidBody("fixpoint");
  fixPoint->add(new agxCollide::Geometry(new agxCollide::Box(agx::Vec3(3, 3, 5))));
  fixPoint->setMotionControl(agx::RigidBody::KINEMATICS);
  agxOSG::setDiffuseColor(agxOSG::createVisual(fixPoint, root, 1.0f), agxRender::Color::DarkSlateGray());
 
  agx::RigidBodyRef gangway = new agx::RigidBody("gangway");
  gangway->add(new agxCollide::Geometry(new agxCollide::Box(agx::Vec3(8, 1.5, 1.5))));
  auto pillar = new agxCollide::Geometry(new agxCollide::Cylinder(1, 5.5));
  pillar->setPosition(agx::Vec3(-5, 0, -4));
  pillar->setRotation(agx::EulerAngles(3.14 / 2, 0, 0));
  gangway->add(pillar);
  agxOSG::setDiffuseColor(agxOSG::createVisual(gangway, root, 1.0f), agxRender::Color::IndianRed());
 
  agx::RigidBodyRef ship = new agx::RigidBody("ship");
  ship->add(new agxCollide::Geometry(new agxCollide::Box(agx::Vec3(5, 2, 2))));
  ship->setMotionControl(agx::RigidBody::KINEMATICS);
  agxOSG::setDiffuseColor(agxOSG::createVisual(ship, root, 1.0f), agxRender::Color::DarkSlateBlue());
 
  // The initial state
  fixPoint->setPosition(agx::Vec3(9, 0, 2));
  ship->setPosition(agx::Vec3(-8, 0, 0));
  gangway->setPosition(agx::Vec3(0, 0, 8));
 
  pillar->setEnableCollisions(false);
 
  simulation->add(gangway);
  simulation->add(fixPoint);
  simulation->add(ship);
 
  gangway->getMassProperties()->setMass(1);
 
  // Create prismatic
  agx::PrismaticRef slider = new agx::Prismatic(agx::PrismaticFrame(pillar->getPosition(), agx::Vec3(0,0,1)), gangway, ship);
  slider->getMotor1D()->setEnable(true);
 
  simulation->add(slider);
 
  return std::make_tuple(slider, gangway, ship);
}
 
 
osg::Group* prismaticPidControlled(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  LOGGER_WARNING() << LOGGER_STATE(agx::Notify::PRINT_NONE) << "\nCreating Scene " << __FUNCTION__ << std::endl << LOGGER_END();
 
  osg::Group* root = new osg::Group;
 
  // Create the "ship" scene with a gangway between a ship and a fix point in the world
  agx::PrismaticRef slider;
  agx::RigidBodyRef gangway;
  agx::RigidBodyRef ship;
  std::tie(slider, gangway, ship) = createGangwayWithPrismatic(simulation, root);
 
  // Create the plant representing the system that our PID should control.
  agxModel::ControllerHandler::PlantRef plant = new agxModel::ControllerHandler::Plant(
    [gangway](const agx::TimeStamp&)
    {
      // The process variable is the height of the gangway
      return gangway->getPosition().z();
    },
    [slider, simulation](agx::Real mv)
    {
      // In this tutorial we activate the regulation after 8 seconds.
      // Note that the PID gains are calculated from time zero.
      if (simulation->getTimeStamp() > 8)
      {
        // The manipulated variable of this plant is the speed of the prismatic "slider".
        slider->getMotor1D()->setSpeed(mv);
      }
    }
  );
 
  // Set gain constants
  agx::Real proportionalGain = 16;
  agx::Real integralGain = 8;
  agx::Real derivativeGain = 4;
  agxModel::PidController1DRef pidController = new agxModel::PidController1D();
  pidController->setGains(proportionalGain, integralGain, derivativeGain);
 
  // The Set Point for the controller is 0.5 meter above the initial gangway position.
  agx::Real setPoint = gangway->getPosition().z() + 0.5;
  pidController->setSetPoint(setPoint);
 
  // Set the max and min values of speed and acceleration of the prismatic
  pidController->setManipulatedVariableLimit(-1, 1);
  pidController->setManipulatedVariableAccelerationLimit(-2, 2);
 
  agxModel::ControllerHandlerRef controller_handler = new agxModel::ControllerHandler(pidController, plant);
  simulation->add(controller_handler);
 
  // Create a step event listener that moves the ship up and down in a sinus motion
  simulation->add(new ShipPositionController(ship, pidController, application));
 
  return root;
}
 
int main(int argc, char** argv )
{
  agx::AutoInit agxInit;
  int status = 0;
 
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tPID controller\n" <<
    "\tPress 1 to control a winch\n" <<
    "\tPress 2 to control a gangway with a prismatic\n" <<
    "\t--------------------------------\n\n" << std::endl;
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( winchPidControlled, '1' );
    application->addScene( prismaticPidControlled, '2');
 
 
    if ( application->init( argc, argv ) ) {
      status = application->run();
    }
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return status;
}

tutorial_pointcloud_over_ros2.cpp

/*
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.
*/
 
#include <agx/version.h>
#include <agx/Logger.h>
 
#include <agxUnit/UnitMacros.h>
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/LidarOutputRenderer.h>
#include <agxOSG/utils.h>
 
#include <agxCollide/Box.h>
#include <agxCollide/Cylinder.h>
#include <agxCollide/Capsule.h>
#include <agxCollide/Sphere.h>
#include <agxCollide/Cone.h>
#include <agxCollide/HollowCone.h>
#include <agxCollide/HollowCylinder.h>
#include <agxCollide/WireShape.h>
 
#include <agxSensor/Environment.h>
#include <agxSensor/RaytraceConfig.h>
#include <agxSensor/LidarModelOusterOS.h>
#include <agxSensor/Lidar.h>
 
#include <agxROS2/ROS2Util.h>
#include <agxROS2/Publisher.h>
 
namespace helpers
{
    void addPrimitiveShapeCircleToSensorEnvironement(agxSDK::Simulation* simulation,
        osg::Group* root,
        agx::Real circleRadius,
        agx::Real scale = 1.0)
    {
        agxCollide::ShapeRefVector shapes = {
          new agxCollide::Box(scale * 0.500, scale * 0.500, scale * 0.500),
          new agxCollide::Box(scale * 0.330, scale * 0.250, scale * 0.850),
          new agxCollide::Box(scale * 0.700, scale * 0.600, scale * 0.300),
          new agxCollide::Cylinder(scale * 0.200, scale * 0.750),
          new agxCollide::Cylinder(scale * 0.530, scale * 1.365),
          new agxCollide::Cylinder(scale * 0.050, scale * 1.665),
          new agxCollide::Capsule(scale * 0.423, scale * 1.389),
          new agxCollide::Capsule(scale * 0.210, scale * 0.450),
          new agxCollide::Capsule(scale * 0.930, scale * 1.030),
          new agxCollide::Sphere(scale * 0.150),
          new agxCollide::Sphere(scale * 0.470),
          new agxCollide::Sphere(scale * 0.820),
          new agxCollide::Cone(scale * 0.050, scale * 0.650, scale * 1.250),
          new agxCollide::Cone(scale * 0.000, scale * 0.750, scale * 0.720),
          new agxCollide::Cone(scale * 0.200, scale * 0.250, scale * 0.420),
          new agxCollide::HollowCone(scale * 0.120, scale * 0.380, scale * 1.310, scale * 0.025),
          new agxCollide::HollowCone(scale * 0.100, scale * 0.480, scale * 1.630, scale * 0.045),
          new agxCollide::HollowCone(scale * 0.300, scale * 0.830, scale * 0.730, scale * 0.140),
          new agxCollide::HollowCylinder(scale * 0.350, scale * 1.250, scale * 0.120),
          new agxCollide::HollowCylinder(scale * 0.720, scale * 0.150, scale * 0.320),
          new agxCollide::HollowCylinder(scale * 0.080, scale * 1.010, scale * 0.015),
          new agxCollide::WireShape(scale * 0.423, scale * 1.389),
          new agxCollide::WireShape(scale * 0.210, scale * 0.450),
          new agxCollide::WireShape(scale * 0.930, scale * 1.030),
        };
        
        agx::UniformRealGenerator randomReal{ 123u, -0.75, 0.75 };
        agx::AffineMatrix4x4 rbTransform{ agx::Quat(), circleRadius * agx::Vec3::X_AXIS() };
        agxSensor::EnvironmentRef environment = agxSensor::Environment::getOrCreate(simulation);
        for (agxCollide::Shape* shape : shapes) {
            agx::RigidBodyRef rb = new agx::RigidBody(new agxCollide::Geometry(shape));
            rb->setMotionControl(agx::RigidBody::KINEMATICS);
            rb->setTransform(rbTransform);
            rb->setAngularVelocity(randomReal(), randomReal(), randomReal());
            simulation->add(rb);
            agxOSG::createVisual(rb, root);
            environment->add(rb);
            
            rbTransform *= agx::AffineMatrix4x4::rotate(0.25, agx::Vec3::Z_AXIS());
        }
    }
 
    agxSensor::LidarRef createHorizontalSweepLidar(agxSDK::Simulation* simulation, osg::Group* root)
    {
        agx::RigidBodyRef lidarRb = new agx::RigidBody();
        lidarRb->setPosition(0, 0, 0);
        lidarRb->setMotionControl(agx::RigidBody::KINEMATICS);
        simulation->add(lidarRb);
 
        agxSensor::LidarRef lidar = new agxSensor::Lidar(lidarRb->getFrame(),
            new agxSensor::LidarModelOusterOS1());
        agxSensor::Environment::getOrCreate(simulation)->add(lidar);
 
        auto renderer = new agxOSG::LidarOutputRenderer(lidar);
        root->addChild(renderer);
 
        return lidar;
    }
}
 
 
osg::Group* buildTutorial1(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
    auto root = new osg::Group();
 
    if (!agxSensor::RtConfig::verifyRaytraceSupported()) {
        LOGGER_WARNING() << "Raytracing support is needed to run this tutorial." << LOGGER_ENDL();
        LOGGER_WARNING() << "Exiting." << LOGGER_ENDL();
        return root;
    }
 
    // The agxROS2::convertLidarOutput function serves a shorthand for converting point clouds
    // recieved from agxSensor::Lidar instances into agxROS2:sensorMsgs::PointCloud2 instances.
    // To demonstrate this we first need an output from a Lidar instance
 
    // Create an environment and a lidar
    helpers::addPrimitiveShapeCircleToSensorEnvironement(simulation, root, 70, 10);
    auto lidar = helpers::createHorizontalSweepLidar(simulation, root);
 
    // Define an output    
    agxSensor::RtOutputRef lidarOutput = new agxSensor::RtOutput();
    lidarOutput->build<
        agxSensor::RtOutput::XYZ_VEC3_F32,
        agxSensor::RtOutput::INTENSITY_F32,
        agxSensor::RtOutput::NORMAL_VEC3_F32
    >();
    lidar->getOutputHandler()->add(42, lidarOutput);
 
 
    // We step the simulation to make sure lidar has produced data
    simulation->stepForward();
 
    double timestamp = simulation->getTimeStamp();
 
    // To construct a PointCloud2 message a frame id is needed for the header
    // This specifies the coordinate frame in which the data is expressed. The default frame for Rviz2
    // is "map" and as such we use it here as a default value.
    std::string frameId = "map";
 
    // The "isDense" parameter of the utility is used to set the "is_dense" flag of the PointCloud2 message.
    // It denotes if misses are excluded in the message and as such should be set the value of
    // "getEnableRemoveRayMisses" from the output handler of the lidar in question.
    bool isDense = lidar->getOutputHandler()->getEnableRemoveRayMisses();
 
    agxROS2::sensorMsgs::PointCloud2 ros2Message = agxROS2::convertLidarOutput(lidarOutput, timestamp, frameId, isDense);
 
    agx::String text = agx::String::format(
        "ROS2 message created with %d bytes of data from %d points with fields:",
        ros2Message.data.size(), ros2Message.width
    );
    application->getSceneDecorator()->setText(0, text);
 
    // Note that it is standard to represent the x, y, and z values of a point using seperate fields 
    // The utility automatically splits output fields with multiple values into multiple message fields
    // with appropriate names to conform to this.
    int i = 1;
    for (auto& field : ros2Message.fields)
        application->getSceneDecorator()->setText(i++, field.name);
 
    return root;
}
 
osg::Group* buildTutorial2(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
    auto root = new osg::Group();
 
    if (!agxSensor::RtConfig::verifyRaytraceSupported()) {
        LOGGER_WARNING() << "Raytracing support is needed to run this tutorial." << LOGGER_ENDL();
        LOGGER_WARNING() << "Exiting." << LOGGER_ENDL();
        return root;
    }
 
    // The lidar simulation is run during the timestep which means that the sensor envorionment only sees the
    // objects before their positions are integrated from the solver. We can enable "preIntegratePositions" to 
    // make the integrated positions visible to the lidar.
    simulation->setPreIntegratePositions(true);
 
    // Create an environment and a lidar
    helpers::addPrimitiveShapeCircleToSensorEnvironement(simulation, root, 70, 10);
    auto lidar = helpers::createHorizontalSweepLidar(simulation, root);
 
    // Let's create a step event listener which continously publishes the results from the lidar
    class LidarStepPublisher : public agxSDK::StepEventListener
    {
    public:
        LidarStepPublisher(std::string rosTopic, std::string frame, agxSensor::RtOutputHandlerRef lidarOutputHandler)
            : agxSDK::StepEventListener(agxSDK::StepEventListener::POST_STEP)
            , m_topic(rosTopic)
            , m_frame(frame)
            , m_lidarOutputHandler(lidarOutputHandler)
            , m_output(new agxSensor::RtOutput())
            , m_publisher(rosTopic)
        {
            m_output->build< 
                // These fields will be automatically picked up by RViz and used by the PointCloud2 visualizer
                agxSensor::RtOutput::XYZ_VEC3_F32,
                agxSensor::RtOutput::INTENSITY_F32,
                // Distance isn't used by default by RViz but any field from the output will be available
                // to use to color the points
                agxSensor::RtOutput::DISTANCE_F32
            >();
            // We need to supply a unique id for each output (determined from a type hash when done using a struct)
            m_lidarOutputHandler->add(42, m_output);
        }
 
        void post(const agx::TimeStamp& time)
        {
            // No need to send a message of no data has been read
            if (!m_output->hasUnreadData())
                return;
 
            bool isDense = m_lidarOutputHandler->getEnableRemoveRayMisses();
            auto message = agxROS2::convertLidarOutput(m_output, time, m_frame, isDense);
 
            m_publisher.sendMessage(message);
        }
 
    private:
        std::string m_topic;
        std::string m_frame;
        agxSensor::RtOutputHandlerRef m_lidarOutputHandler;
        agxSensor::RtOutputRef m_output;
        agxROS2::sensorMsgs::PublisherPointCloud2 m_publisher;
    };
 
    // We use the "map" frame name as previously done because it is the default frame in RViz
    auto lidarStepPublisher = new LidarStepPublisher("tutorial2", "map", lidar->getOutputHandler());
 
    simulation->add(lidarStepPublisher);
 
    // This scene will now continously publish each steps lidar results which can then be visualized in RViz
    // For a guide on how to use RViz in tandem with agxSensor please refer to the AGX Dynamics user manual
    application->getSceneDecorator()->setText(0, "Publishing point cloud data on topic \"tutorial2\"");
 
    return root;
}
 
int main(int argc, char** argv)
{
    agx::AutoInit agxInit;
    std::cout <<
        "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
        "\tagxSensor + ROS2 tutorial\n" <<
        "\t--------------------------------\n\n" << std::endl;
 
    try {
        if (!agxSensor::RtConfig::verifyRaytraceSupported())
            return 0;
 
        agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
        application->addScene(buildTutorial1, '1');
        application->addScene(buildTutorial2, '2');
 
        if (application->init(argc, argv))
            return application->run();
    }
    catch (std::exception& e) {
        std::cerr << "\nCaught exception: " << e.what() << std::endl;
        return 1;
    }
 
    return 0;
}

tutorial_python_excavator_controller.cpp

/*
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.
*/
 
 
 
/*
 
This tutorial demonstrates how to use the AGX Python API to model, configure and parameterize simulations models
still using C++ for all control/analyzis.
 
We start by loading an existing script that creates an Excavator on a Deformable terrain.
We then locate all the actuators (constraints) by name.
Next we can control either the target SPEED or target POSITION depending on what kind of controller we want.
 
In this example we are using the target position using
explicit PID controllers (agxModel::PidController1D, agxModel::ControllerHandler an agxModel::ControllerHandler::Plant).
 
To move the machine we also queue up a bunch of events that will send signals to the controller making the excavator move.
 
*/
 
#include <signal.h>
#include <iostream>
 
#include <agx/config/AGX_USE_AGXTERRAIN.h>
#include <agx/config/AGX_USE_PYTHON.h>
 
// This tutorial can be built without agxPython and agxTerrain, but for it to actually function
// and not produce runtime errors, those two are required.
#if AGX_USE_PYTHON() && AGX_USE_AGXTERRAIN()
 
#include <agx/Logger.h>
#include <agx/version.h>
#include <agx/Interrupt.h>
#include <agx/Windows.h>
#include <agx/Hinge.h>
#include <agx/Prismatic.h>
 
#include <agxIO/Environment.h>
#include <agxIO/FileSystem.h>
 
#include <agxModel/PidController1D.h>
#include <agxModel/ControllerHandler.h>
 
#include <agxCFG/utils.h>
#include <agxRender/Color.h>
 
#include <agxOSG/Node.h>
#include <agxOSG/ExampleApplication.h>
 
using namespace agxOSG;
 
namespace {
 
#ifdef _MSC_VER
  BOOL WINAPI sighandler(DWORD /*dwCtrlType*/)  //  control signal type
  {
    std::cout << "Caught: CTRL-C or closing window. Shutting down agx." << std::endl; // Not using LOGGER here, might interfere with shutting down and lead to crash.
    throw agx::InterruptException();
    //return TRUE;
  }
#else
  void sighandler(int /*sig*/)
  {
    std::cout << "Caught: CTRL-C or closing window. Shutting down agx." << std::endl; // Not using LOGGER here, might interfere with shutting down and lead to crash.
    throw agx::InterruptException();
  }
#endif
}
 
class ActuatorPlant : public agxModel::ControllerHandler::Plant
{
public:
  ActuatorPlant(agx::Constraint1DOF* constraint1dof) : m_constraint1dof(constraint1dof)
  {
  }
 
  agx::Real getProcessVariable(const agx::TimeStamp& ) override
  {
    return m_constraint1dof->getAngle();
  }
 
  void setManipulatedVariable(agx::Real manipulatedVariable) override
  {
    // The PID manipulate variable is set as the winch speed.
   // Negative since a negative error should pull in the weight.
    m_constraint1dof->getMotor1D()->setSpeed(manipulatedVariable);
  }
 
private:
  agx::Constraint1DOFRef m_constraint1dof;
};
 
 
class ExcavatorController : public agxSDK::StepEventListener
{
public:
 
  enum Constraints
  {
    BucketPrismatic,
    RightSprocketHinge,
    LeftSprocketHinge,
    ArmPrismatic2,
    ArmPrismatic1,
    StickPrismatic,
    CabinHinge
  };
 
  enum ControllerMode
  {
    TARGET_SPEED,
    TARGET_POSITION
  };
 
  ExcavatorController(agxOSG::ExampleApplication *application, ControllerMode mode=TARGET_SPEED);
 
  bool registerConstraint(const std::string& name, Constraints constraint);
 
  agx::Constraint1DOF* getConstraint(Constraints constraint);
 
  const agx::Constraint1DOF* getConstraint(Constraints constraint) const;
 
  bool setSpeed(Constraints constraint, double speed);
 
  double getSpeed(Constraints constraint) const;
 
  double getPosition(Constraints constraint) const;
 
  bool setPosition(Constraints constraint, double position);
 
  double getForce(Constraints constraint) const;
 
 
protected:
  void addNotification() override;
  void pre(const agx::TimeStamp& time) override;
  void post(const agx::TimeStamp& time) override;
 
  std::string constraintToName(Constraints constraint) const;
 
  void setText(int row, const std::string& text, agxRender::Color color=agxRender::Color::White());
  void setupConstraints();
 
  void update(Constraints constraint, double time);
  void updatePositionControllers(double time);
 
private:
  typedef agx::HashTable<Constraints, std::pair<std::string, agx::Constraint1DOFRef>, std::hash<Constraints>> ConstraintMap;
  ConstraintMap m_constraints;
 
  agx::observer_ptr<agxOSG::ExampleApplication> m_application;
  ControllerMode m_controllerMode;
 
  typedef agx::HashTable<Constraints, agxModel::ControllerHandlerRef, std::hash<Constraints>> ConstraintControllerMap;
  ConstraintControllerMap m_controllers;
};
 
 
ExcavatorController::ExcavatorController(agxOSG::ExampleApplication* application, ControllerMode mode) :
  m_application(application), m_controllerMode(mode)
{
  setMask(StepEventListener::PRE_STEP | StepEventListener::POST_STEP);
 
  setupConstraints();
}
 
void ExcavatorController::setupConstraints()
{
  for (auto c : m_constraints)
  {
    auto constraint = c.second.second;
    constraint->getMotor1D()->setEnable(true);
    constraint->getMotor1D()->setLockedAtZeroSpeed(true);
    constraint->getLock1D()->setEnable(false);
 
    agxModel::ControllerHandler::PlantRef plant = new ActuatorPlant(constraint);
 
    agxModel::PidController1DRef pidController = new agxModel::PidController1D();
 
    agx::Real proportionalGain = 2;
    agx::Real integralGain = 1;
    agx::Real derivativeGain = 0.2;
    pidController->setGains(proportionalGain, integralGain, derivativeGain);
    pidController->setSetPoint(0);
 
    pidController->setManipulatedVariableLimit(-0.5, 0.5);
 
    agxModel::ControllerHandlerRef controllerHandler = new agxModel::ControllerHandler(pidController, plant);
 
    m_controllers.insert(c.first, controllerHandler);
  }
}
 
void ExcavatorController::setText(int row, const std::string& text, agxRender::Color color)
{
  m_application->getSceneDecorator()->setText(row, text, color);
}
 
void ExcavatorController::addNotification()
{
  registerConstraint("BucketPrismatic", BucketPrismatic);
  registerConstraint("RightSprocketHinge", RightSprocketHinge);
  registerConstraint("LeftSprocketHinge", LeftSprocketHinge);
  registerConstraint("ArmPrismatic2", ArmPrismatic2);
  registerConstraint("ArmPrismatic1", ArmPrismatic1);
  registerConstraint("StickPrismatic", StickPrismatic);
  registerConstraint("CabinHinge", CabinHinge);
 
  setupConstraints();
}
 
 
bool ExcavatorController::registerConstraint(const std::string& name, Constraints constraint)
{
  auto dof1d_constraint = getSimulation()->getConstraint1DOF(name);
  if (!dof1d_constraint)
  {
    LOGGER_WARNING() << "Constraint " << name << " cannot be located in simulation." << LOGGER_ENDL();
    return false;
  }
  else
    std::cout << "Registered constraint '" << name << "'" << std::endl;
 
  m_constraints.insert(constraint, std::make_pair(name, dof1d_constraint));
 
  return true;
}
 
const agx::Constraint1DOF* ExcavatorController::getConstraint(Constraints constraint) const
{
 
  auto c = m_constraints.get_or_return_default(constraint);
  if (!c.second.isValid()) {
    LOGGER_WARNING() << "Unable to locate a constraint named '" << constraintToName(constraint) << "'" << LOGGER_ENDL();
    return nullptr;
  }
 
  return c.second;
}
 
agx::Constraint1DOF* ExcavatorController::getConstraint(Constraints constraint)
{
  auto c = m_constraints.get_or_return_default(constraint);
  if (!c.second.isValid()) {
    LOGGER_WARNING() << "Unable to locate a constraint named '" << constraintToName(constraint) << "'" << LOGGER_ENDL();
    return nullptr;
  }
 
  return c.second;
}
 
bool ExcavatorController::setSpeed(Constraints constraint, double speed)
{
  auto c = getConstraint(constraint);
  if (!c)
    return false;
 
  c->getMotor1D()->setSpeed(speed);
 
  return true;
}
 
double ExcavatorController::getSpeed(Constraints constraint) const
{
  auto c = getConstraint(constraint);
  if (!c)
    return 0;
 
  return c->getMotor1D()->getSpeed();
}
 
double ExcavatorController::getPosition(Constraints constraint) const
{
  auto c = getConstraint(constraint);
  if (!c)
    return 0;
 
  return c->getAngle();
}
 
bool ExcavatorController::setPosition(Constraints constraint, double pos)
{
 
  // Left and right sprocket are NOT position based, just target speed
  if (constraint == LeftSprocketHinge || constraint == RightSprocketHinge)
  {
    LOGGER_WARNING() << "Ignoring setPosition for sprocket hinges" << LOGGER_ENDL();
    return 0;
  }
 
  auto c = getConstraint(constraint);
  if (!c)
    return 0;
 
  auto controller = m_controllers.get_or_return_default(constraint);
  if (!controller)
    return false;
 
  controller->getController()->setSetPoint(pos);
 
  return true;
}
 
void ExcavatorController::updatePositionControllers(double time)
{
  for (auto c : m_constraints)
  {
    // Neither of the sprockets should be position based.
    if (c.first != LeftSprocketHinge && c.first != RightSprocketHinge)
      update((Constraints)c.first, time);
  }
}
void ExcavatorController::update(Constraints constraint, double time)
{
  auto controller = m_controllers.get_or_return_default(constraint);
 
  controller->post(time);
}
 
double ExcavatorController::getForce(Constraints constraint) const
{
  auto c = getConstraint(constraint);
  if (!c)
    return 0;
 
  return c->getMotor1D()->getCurrentForce();
}
 
std::string ExcavatorController::constraintToName(Constraints constraint) const
{
  auto it = m_constraints.find(constraint);
  if (it == m_constraints.end()) {
    LOGGER_WARNING() << "Unable to locate a registered constraint with index " << constraint << LOGGER_ENDL();
    return "";
  }
 
  return it->second.first;
}
 
void ExcavatorController::pre(const agx::TimeStamp& time)
{
  int start = 1;
  for (auto c : m_constraints)
  {
    auto pos = getPosition(c.first);
    bool isHinge = c.second.second->as<agx::Hinge>() != nullptr;
    const char* title = isHinge ? "angle" : "position";
    const char* unit = isHinge ? "rad" : "m";
    setText(start++, agx::String::format("%s %s: %.1f %s", constraintToName(c.first).c_str(), title, pos, unit), agxRender::Color::White());
  }
 
  // If we are using target speed to control, we do not have
  // to do anything here. TargetSpeed is handled by the constraints TargetSpeedController (Motor1D)
  if (m_controllerMode == TARGET_SPEED) {
  }
  else
  {
    updatePositionControllers(time);
  }
}
 
void ExcavatorController::post(const agx::TimeStamp& time)
{
  int start = (int)m_constraints.size() + 3;
  for (auto c : m_constraints)
  {
    auto force = getForce(c.first);
    bool isHinge = c.second.second->as<agx::Hinge>() != nullptr;
    const char* title = isHinge ? "Torque" : "Force";
    const char* unit = isHinge ? "Nm" : "N";
    setText(start++, agx::String::format("%s %s: %.1f %s", constraintToName(c.first).c_str(), title, force, unit), agxRender::Color::Yellow());
  }
 
  setText(0, agx::String::format("Time: %2.1f", time), agxRender::Color::IndianRed());
}
 
class EventTrigger : public agxSDK::StepEventListener
{
public:
  EventTrigger() {}
 
  void pre(const agx::TimeStamp& time) override
  {
    std::vector<Events::iterator> removeEvents;
 
    for (Events::iterator it=events.begin(); it != events.end(); it++)
    {
      // If the event returns true, it should be removed
      bool remove = (*it)(time);
      if (remove)
        removeEvents.push_back(it);
    }
 
    if (!removeEvents.empty())
      for (auto it : removeEvents)
        events.erase(it);
  }
 
  typedef std::vector<std::function<bool(double)>> Events;
  Events events;
};
 
 
// Reports true if currentTime is within a 60:ths of a second from time
bool inTime(double currentTime, double time)
{
  return std::abs(currentTime - time) <= 1.0 / 60;
}
 
// An empty scene for agxViewer.
osg::Group* buildExcavatorPythonScene(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  bool success;
 
  // Execute the script that build our model.
  // The function buildScene1 that can be found in the script will construct an excavator model.
  // Constraints, RigidBodies, geometries materials etc. can later be accessed through names via the Simulation object.
  osg::Group* parent = application->executePythonScript("data/python/agxTerrain/excavator_365_terrain.agxPy", "buildScene1", success);
 
  // Could we load the script and execute the function that constructed our model?
  // If so, create a controller that will communicate with the constraints of the machine to move it.
  if (success) {
 
    // Here we are going to use the TARGET_POSITION based controller.
    // This mean we are going to use a PID controller to move the constraints to the target positions
    // We could also use the TARGET_SPEED controller (setSpeed())
    agx::ref_ptr<ExcavatorController> excavatorController = new ExcavatorController(application, ExcavatorController::TARGET_POSITION);
    simulation->add(excavatorController);
 
    // Create a trigger object where we queue up a bunch of events that will control the Excavator
    agx::ref_ptr<EventTrigger> trigger = new EventTrigger();
 
    // Register a bunch of events
    trigger->events.push_back([excavatorController](double time) { excavatorController->setPosition(ExcavatorController::BucketPrismatic, std::sin(time) * 0.5); return false;  });
    trigger->events.push_back([excavatorController](double time) { excavatorController->setPosition(ExcavatorController::ArmPrismatic1, std::sin(time) * 0.5); return false; });
    trigger->events.push_back([excavatorController](double time) { excavatorController->setPosition(ExcavatorController::ArmPrismatic2, std::sin(time) * 0.5); return false; });
    trigger->events.push_back([excavatorController](double time) { excavatorController->setPosition(ExcavatorController::CabinHinge, std::sin(time) * 2); return false; });
    trigger->events.push_back([excavatorController](double time) { excavatorController->setPosition(ExcavatorController::StickPrismatic, std::sin(time) * 0.3); return false; });
 
    // Lambda for setting excavator speed
    auto setSpeed = [excavatorController](double speed, double when)
      {
        return [excavatorController, when, speed](double time)
          {
            if (inTime(time, when)) {
              excavatorController->setSpeed(ExcavatorController::LeftSprocketHinge, speed);
              excavatorController->setSpeed(ExcavatorController::RightSprocketHinge, speed);
              return true;
            }
            return false;
          };
      };
 
    trigger->events.push_back(setSpeed(1, 3));
    trigger->events.push_back(setSpeed(-1, 10));
    trigger->events.push_back(setSpeed(0, 15));
 
    simulation->add(trigger);
  }
  return parent;
}
 
int main(int argc, char** argv)
{
  agx::AutoInit agxInit;
 
#ifdef _MSC_VER
  // Handle ctrl-c
  if (SetConsoleCtrlHandler((PHANDLER_ROUTINE)sighandler, TRUE) == FALSE)
  {
    // unable to install handler...
    // display message to the user
    LOGGER_WARNING() << "Unable to install signal handler!" << std::endl << LOGGER_END();
    return 1;
  }
#else
  // signal(SIGABRT, &sighandler);
  signal(SIGTERM, &sighandler);
  signal(SIGINT, &sighandler);
#endif
 
  int result = 1;
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
    agxIO::ArgumentParserRef arguments = new agxIO::ArgumentParser(argc, argv);
 
    if (application->init(arguments)) {
 
      application->addScene(buildExcavatorPythonScene, '1');
 
      result = application->run();
 
    }
  }
  catch (std::exception& e) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
  }
 
  return result;
}
 
#else
 
int main(int, char**)
{
  std::cout << "This tutorial needs agxPython and agxTerrain to function as expected" << std::endl;
  return 0;
}
 
#endif

tutorial_rigidBodyEmitter.cpp

/*
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.
*/
 
#include <agxOSG/ExampleApplication.h>
#include <agx/RigidBodyEmitter.h>
#include <agxControl/EventSensor.h>
#include <agxUtil/agxUtil.h>
#include <agxOSG/GeometryNode.h>
 
#include <agx/Logger.h>
#include <agx/version.h>
 
#include "tutorialUtils.h"
 
 
 
osg::Group* buildTutorial1(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/)
{
  osg::Group* root = new osg::Group();
 
  simulation->getDynamicsSystem()->getSolver()->setUseParallelPgs(true);
 
  simulation->getSpace()->setEnablePair( "Emitter", "Particles", false );
 
  // Create a floor
  agxCollide::GeometryRef floor_geometry = new agxCollide::Geometry(new agxCollide::Box(10.0, 10.0, 0.2));
  agx::RigidBodyRef floor = new agx::RigidBody(floor_geometry);
  floor->setPosition(0.0, 0.0, -0.2);
  floor->setMotionControl(agx::RigidBody::MotionControl::STATIC);
  simulation->add(floor);
  agxOSG::createVisual(floor, root);
  floor_geometry->setEnableCollisions( false );
 
  // setup material properties
  agx::MaterialRef rbMat = new agx::Material("Rb");
  agx::MaterialRef floorMat = new agx::Material("Floor");
 
  rbMat->getBulkMaterial()->setDensity(2500);
  agx::ContactMaterialRef c1 = new agx::ContactMaterial(rbMat, floorMat);
  agx::ContactMaterialRef c2 = new agx::ContactMaterial(rbMat, rbMat);
  c1->setYoungsModulus(2.1e11);
  c1->setFrictionCoefficient(0.7);
 
  c2->setYoungsModulus(2.1e11);
  c2->setFrictionCoefficient(0.7);
 
  floor_geometry->setMaterial(floorMat);
 
  // Create rigid body emitter
  agx::RigidBodyEmitterRef emitter = new agx::RigidBodyEmitter();
  emitter->setRate(30.0);
  emitter->setMaximumEmittedQuantity(200);
  emitter->setQuantity(agx::Emitter::QUANTITY_COUNT);
  emitter->addCollisionGroup( "Particles" );
  agxOSG::createVisual(emitter, root);
 
  agx::Emitter::DistributionTableRef distributionTable = new agx::Emitter::DistributionTable(agx::Emitter::QUANTITY_MASS);
  emitter->setDistributionTable(distributionTable);
 
  // First body model, capsules
  agx::RigidBodyRef bodyTemplate1 = new agx::RigidBody();
  agxCollide::GeometryRef geometryTemplate1 = new agxCollide::Geometry(new agxCollide::Cylinder(0.4, 0.1));
  geometryTemplate1->setMaterial(rbMat);
  bodyTemplate1->add(geometryTemplate1);
  distributionTable->addModel(new agx::RigidBodyEmitter::DistributionModel(bodyTemplate1, 0.3));
 
  // Second body model, boxes
  agx::RigidBodyRef bodyTemplate2 = new agx::RigidBody();
  agxCollide::GeometryRef geometryTemplate2 = new agxCollide::Geometry(new agxCollide::Box(0.1, 0.1, 0.2));
  geometryTemplate2->setMaterial(rbMat);
  bodyTemplate2->add(geometryTemplate2);
  distributionTable->addModel(new agx::RigidBodyEmitter::DistributionModel(bodyTemplate2, 0.7));
 
  // Specify the emit volume in which the bodies are spawned
  agxCollide::GeometryRef emitGeometry = new agxCollide::Geometry(new agxCollide::Sphere(3.0));
  emitGeometry->setPosition(0.0, 0.0, 4.0);
  //emitGeometry->setSensor(true);
  emitGeometry->addGroup( "Emitter" );
  agxOSG::GeometryNode* emitterNode = agxOSG::createVisual(emitGeometry, root);
  agxOSG::setDiffuseColor(emitterNode, agx::Vec4f(0.2f, 0.4f, 1, 1));
  agxOSG::setAlpha(emitterNode, 0.2f);
 
  emitter->setGeometry(emitGeometry);
 
  agxControl::EventSensor* s = agxUtil::createSinkOnGeometry(floor_geometry, true);
  simulation->add(s);
 
  simulation->add(emitGeometry);
  simulation->add(emitter);
 
 
  return root;
}
 
 
 
int main(int argc, char** argv)
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial RigidBodyEmitter\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene(buildTutorial1, '1');
 
    if (application->init(argc, argv))
      return application->run();
  }
  catch (std::exception& e) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}
 

tutorial_robot_pick_and_place.cpp

/*
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.
*/
 
#include <agx/version.h>
#include <agx/Logger.h>
#include <agx/Math.h>
#include <agx/AffineMatrix4x4.h>
#include <agx/Constraint.h>
#include <agx/Prismatic.h>
#include <agx/FrictionModel.h>
 
#include <agxCollide/Box.h>
#include <agxCollide/Geometry.h>
 
#include <agxIO/Environment.h>
 
#include <agxSDK/Assembly.h>
#include <agxSDK/StepEventListener.h>
 
#include <agxModel/UrdfReader.h>
#include <agxModel/KinematicChain.h>
 
#include <agxUtil/agxUtil.h>
 
#include <agxOSG/SceneDecorator.h>
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/utils.h>
 
#include <stdexcept>
 
/*
 
This tutorial shows how a robot can be controlled to pick up some boxes
and place them on top of each other.
 
The tutorial uses two "controllers" for driving the robot motion:
- A high level controller that handles tasks such as open/close gripper and
  computing the trajectory that the robot should follow when moving.
 
- A low level controller that is responsible for tracking target joint angles.
  - Target angles are received from the high level controller.
  - The controller has access to a "force/torque sensor" to estimate the load from what the tool picks up
  - The controller has feedback (can read current joint angles and joint velocities)
 
*/
 
 
/*
A controller that drives the robot to joint angles that are provided externally.
*/
class LowLevelController : public agxSDK::StepEventListener
{
 
  public:
    LowLevelController(agxModel::SerialKinematicChain* chain, agxSDK::Assembly* panda, agxOSG::SceneDecorator* sd = nullptr)
      : agxSDK::StepEventListener(agxSDK::StepEventListener::PRE_STEP), m_chain(chain), m_panda(panda), m_sd(sd)
    {
      auto nj = m_chain->getNumJoints();
      auto nl = m_chain->getNumLinks() - 1; // -1 due to a relative transform was used when making chain
 
      m_desiredQ.resize(nj);
      m_desiredQd.resize(nj);
      m_desiredQdd.resize(nj);
 
      m_actualQ.resize(nj);
      m_actualQd.resize(nj);
 
      m_extForce.resize(nl * 6);
 
      // Configure the lock between the tool and the last link of the robot as sort of a force-torque sensor
      // We'll use that to estimate the load of the item the gripper is holding and take that into account
      // when performing inverse dynamics computations.
      m_sensor = panda->getConstraint("panda_hand_joint");
      m_sensor->setEnableComputeForces(true);
 
      m_toolMass =  panda->getRigidBody("panda_hand")->getMassProperties()->getMass() +
                    panda->getRigidBody("panda_leftfinger")->getMassProperties()->getMass() +
                    panda->getRigidBody("panda_rightfinger")->getMassProperties()->getMass();
    }
 
 
    void pre(const agx::TimeStamp& t) override
    {
      auto endEffector = m_chain->getLinks().back();
 
      auto h = getSimulation()->getTimeStep();
      auto g = getSimulation()->getGravityField()->calculateGravity(endEffector->getPosition());
 
      auto nl = m_chain->getNumLinks() - 1;
 
      // Read "force-torque sensor"
      agx::Vec3 lf1, lf2;
      m_sensor->getLastForce(agx::UInt(0u), lf1, lf2, true);
 
      auto estimated = std::max(lf1[2] / g.length() - m_toolMass, 0.0);
 
      for (auto i = 0u; i < 3; ++i) {
        m_extForce[(nl - 1) * 6 + i] = -g[i] * estimated;
      }
 
      if (m_sd) {
        m_sd->setText(0, agx::String::format("T=%4.2f s", (float) t));
        m_sd->setText(1, agx::String::format("Estimated load: %4.2f", (float) estimated));
      }
 
 
      // Compute wanted joint accelerration to reach desired joint position
      //
      for (auto i = 0u, idx = 0u; i < m_chain->getJoints().size(); ++i) {
        auto joint = m_chain->getJoints()[i]->as<agx::Constraint1DOF>();
 
        if (!joint)
          continue;
 
        // We have feedback and can read/measure the robot state
        m_actualQ[idx]  = joint->getAngle();
        m_actualQd[idx] = joint->getCurrentSpeed();
 
        m_desiredQd[idx]  = (m_desiredQ[idx]  - m_actualQ[idx]) / h;
        m_desiredQdd[idx] = (m_desiredQd[idx] - m_actualQd[idx]) / h;
 
        idx++;
      }
 
      auto status = m_chain->computeInverseDynamics(m_desiredQdd, m_extForce, m_torque);
 
      if (status != agxModel::KinematicChain::SUCCESS) {
        throw std::runtime_error("Inverse dynamics computation failed");
      }
 
      // Apply torque
      //
      for (auto i = 0u, idx=0u; i < m_chain->getJoints().size(); ++i) {
        auto joint = m_chain->getJoints()[i]->as<agx::Constraint1DOF>();
        if (joint) {
          auto motor = joint->getMotor1D();
          motor->setEnable(true);
          motor->setForceRange(m_torque[idx], m_torque[idx]);
 
          idx++;
        }
      }
    }
 
    bool setDesiredQ(const agx::RealVector& q)
    {
      if (q.size() != m_chain->getNumJoints()) {
        return false;
      }
 
      m_desiredQ = q;
 
      return true;
    }
 
  private:
    agx::RealVector m_desiredQ;
    agx::RealVector m_desiredQd;
    agx::RealVector m_desiredQdd;
 
    agx::RealVector m_actualQ;
    agx::RealVector m_actualQd;
 
    agx::RealVector m_extForce;
    agx::RealVector m_torque;
 
    agxModel::SerialKinematicChainRef m_chain;
    agxSDK::AssemblyRef m_panda;
    agx::ConstraintRef  m_sensor;
 
    agxOSG::SceneDecorator* m_sd;
 
    double m_toolMass;
};
 
 
enum TaskCommand { TASK_MOVE, TASK_OPEN, TASK_CLOSE };
 
struct TaskItem {
  TaskCommand cmd;
  double      startTime;
  double      endTime;
  agx::AffineMatrix4x4 from;
  agx::AffineMatrix4x4 to;
 
  TaskItem(TaskCommand c, double t0, double t1)
    : cmd(c), startTime(t0), endTime(t1)
  {}
 
  TaskItem(TaskCommand c, double t0, double t1, agx::AffineMatrix4x4 m0, agx::AffineMatrix4x4 m1)
    : cmd(c), startTime(t0), endTime(t1), from(m0), to(m1)
  {}
};
 
 
/*
A high level controller that handles tasks such as
open or close gripper or compute and feed trajectory data
to the low level controller.
*/
class TaskController : public agxSDK::StepEventListener
{
  public:
    TaskController(agxModel::SerialKinematicChain* chain, agxSDK::Assembly* panda, LowLevelController* llc)
      : agxSDK::StepEventListener(agxSDK::StepEventListener::PRE_COLLIDE), m_chain(chain), m_panda(panda), m_llc(llc)
    {
    }
 
 
    void preCollide(const agx::TimeStamp& t) override
    {
      for (auto taskIndex=0u; taskIndex < m_tasks.size(); ++taskIndex) {
 
        const auto& task = m_tasks[taskIndex];
 
        // No optimization, we look at every task. Completed tasks could e.g. be removed
        if (task.startTime > t || task.endTime < t) {
          continue;
        }
 
        double localT = t - task.startTime;
        double taskDuration = task.endTime - task.startTime;
 
        if (task.cmd == TASK_OPEN || task.cmd == TASK_CLOSE) {
          // The urdf file uses a mimic-feature and this results in a driveline
          // in the simulation and that both fingers move when controlling one of the
          // finger joints.
          auto joint = m_panda->getConstraint("panda_finger_joint1")->as<agx::Prismatic>();
          joint->getLock1D()->setEnable(false);
          joint->getMotor1D()->setEnable(true);
          joint->getMotor1D()->setSpeed(0.05 * (task.cmd==TASK_OPEN ? 1.0 : -1.0));
        }
        else if (task.cmd == TASK_MOVE)  {
 
          // NOTE:
          //  All the trajectory computations should be precomputed, but this
          //  is to keep things simple and for illustrative purposes.
          //
          agx::Vec3 pos0 = task.from.getTranslate();
          agx::Quat rot0 = task.from.getRotate();
          agx::Vec3 aa0 = rot0.getAngle() * rot0.getUnitVector();
 
          agx::Vec3 pos1 = task.to.getTranslate();
          agx::Quat rot1 = task.to.getRotate();
          agx::Vec3 aa1 = rot1.getAngle() * rot1.getUnitVector();
 
          agx::Vec3 posT = pos0 + (pos1 - pos0) * (localT / taskDuration);
          agx::Vec3 aaT  = aa0  + (aa1  - aa0)  * (localT / taskDuration);
 
          agx::Vec3 axis;
          agx::Real angle = aaT.length();
 
          if (agx::equalsZero(angle)) {
            // 0 angle => no rotation => axis can be anything
            axis = agx::Vec3(0,0,1);
          }
          else {
            axis = aaT / angle;
          }
 
          auto targetPose = agx::AffineMatrix4x4(agx::Quat(angle, axis), posT);
 
          agx::RealVector targetAngles, currentAngles;
          currentAngles.resize(m_chain->getNumJoints());
 
          for (auto i = 0u, idx=0u; i < m_chain->getJoints().size(); ++i) {
            auto joint = m_chain->getJoints()[idx]->as<agx::Constraint1DOF>();
            if (joint)
              currentAngles[idx++]  = joint->getAngle();
          }
 
          auto status = m_chain->computeInverseKinematics(targetPose, currentAngles, targetAngles);
 
          if (status != agxModel::KinematicChain::SUCCESS) {
            throw std::runtime_error("Inverse Kinematics computation failed");
          }
 
          // Feed low-level controller with angles
          m_llc->setDesiredQ(targetAngles);
        }
      }
    }
 
    void addTask(TaskItem ti) {
      m_tasks.push_back(ti);
    }
 
  private:
    agxModel::SerialKinematicChainRef m_chain;
    agxSDK::AssemblyRef m_panda;
    agx::ref_ptr<LowLevelController> m_llc;
 
    agx::Vector<TaskItem> m_tasks;
};
 
 
// Helper function that will reposition the links in the body given joint angles q.
//
void setRobotPose(agxModel::SerialKinematicChain* chain, agxSDK::Assembly* panda, agx::RealVector q);
 
// Helper function that adds a set of predefined tasks to the TaskController
void createTaskPlan(TaskController* tc, agx::AffineMatrix4x4 startTransform, agx::Vec3 boxSize, agx::Vec3 boxStack1Pos, agx::Vec3 boxStack2Pos);
 
 
// Build a scene with a robot and add controllers so the robot performs some task
//
osg::Group* buildTutorial1(agxSDK::Simulation* sim, agxOSG::ExampleApplication* app)
{
  auto env = agxIO::Environment::instance();
  osg::Group* root = new osg::Group();
 
  // Contact materials that will be used
  //
  auto mm = sim->getMaterialManager();
  auto boxMaterial = new agx::Material("BoxMaterial");
  auto gripperMaterial = new agx::Material("GripperMaterial");
 
  auto boxbox = mm->getOrCreateContactMaterial(boxMaterial, boxMaterial);
  boxbox->setFrictionModel(new agx::ScaleBoxFrictionModel(agx::FrictionModel::DIRECT));
  boxbox->setFrictionCoefficient(0.5);
 
  auto boxgripper = mm->getOrCreateContactMaterial(boxMaterial, gripperMaterial);
  boxgripper->setFrictionModel(new agx::ScaleBoxFrictionModel(agx::FrictionModel::DIRECT));
  boxgripper->setFrictionCoefficient(0.9);
 
  // Create "ground" and a table/working area
  //
  auto ground = new agxCollide::Geometry(new agxCollide::Box(2, 2, 0.1));
  ground->setPosition(agx::Vec3(0, 0, -0.1));
  sim->add(ground);
  auto groundNode = agxOSG::createVisual(ground, root);
  agxOSG::setDiffuseColor(groundNode, agxRender::Color::Black());
 
  auto tableSize = agx::Vec3(0.3, 0.3, 0.3);
  auto table = new agxCollide::Geometry(new agxCollide::Box(tableSize));
  table->setPosition(agx::Vec3(0.5, 0, -0.2));
  sim->add(table);
  agxOSG::createVisual(table, root);
  agxOSG::setTexture(table, root, "textures/grid.png");
 
  // Place 3 boxes on the table/working area
  //
  agx::Vec3 boxSize = agx::Vec3(0.04, 0.02, 0.03);
 
  auto boxStack1Pos = table->getPosition() + agx::Vec3(-0.5 * tableSize[0], -0.5 * tableSize[1], tableSize[2] + boxSize[2]);
  auto boxStack2Pos = table->getPosition() + agx::Vec3(0.3 * tableSize[0], 0.4 * tableSize[1], tableSize[2] + boxSize[2]);
 
  agxRender::Color boxColors[3] =  {agxRender::Color::Yellow(), agxRender::Color::Orange(), agxRender::Color::Red()};
  agx::Vec3 boxPositions[3] = {boxStack1Pos, boxStack1Pos + agx::Vec3(0, 0, 0.05999), boxStack2Pos};
 
  for (auto idx=0u; idx < 3; ++idx) {
    auto box = new agx::RigidBody();
    box->add(new agxCollide::Geometry(new agxCollide::Box(boxSize)));
    box->setPosition(boxPositions[idx]);
    box->getMassProperties()->setMass(0.4 + idx * 0.1);
 
    agxUtil::setBodyMaterial(box, boxMaterial);
    sim->add(box);
 
    auto boxNode = agxOSG::createVisual(box, root);
    agxOSG::setDiffuseColor(boxNode, boxColors[idx]);
  }
 
  // Load a panda robot from urdf and add to simulation
  //
  auto pandaFile = env->getFilePath().find("models/URDF/panda/panda.urdf");
  auto pandaDir = agxIO::FileSystem::getDirectory(pandaFile) + "/franka_ros-kinetic/";
 
  auto panda = agxModel::UrdfReader::read(pandaFile, pandaDir, nullptr, agxModel::UrdfReader::Settings(false, false, false));
  agxUtil::setBodyMaterial(panda->getRigidBody("panda_rightfinger"), gripperMaterial);
  agxUtil::setBodyMaterial(panda->getRigidBody("panda_leftfinger"), gripperMaterial);
  sim->add(panda);
 
  agx::RigidBody* ee = panda->getRigidBody("panda_hand");
  agx::RigidBody* base = panda->getRigidBody("panda_link0");
  base->setMotionControl(agx::RigidBody::STATIC);
 
  // Disable self-collision
  sim->getSpace()->setEnablePair(panda->getName(), panda->getName(), false);
 
  agxOSG::createVisual(panda, root);
 
  // Tooltip observer
  agx::AffineMatrix4x4 extraOffset = agx::AffineMatrix4x4::translate(0, 0, 0.1034);
  auto observer = new agx::ObserverFrame(ee, extraOffset);
  sim->add(observer);
 
  // Create a kinematic chain
  agxModel::SerialKinematicChainRef chain = new agxModel::SerialKinematicChain(*sim, *base, *ee, &extraOffset);
 
  if (!chain->isValid()) {
    throw std::runtime_error("Kinematic Chain describing robot is not valid.");
  }
 
  // Set robot start pose
  auto readyPose = agx::RealVector {0, -0.3, 0, -2.2, 0, 2.0, agx::PI / 4};
  setRobotPose(chain, panda, readyPose);
 
  // Initial position for camera
  auto eye = agx::Vec3(1.93, -2.78, 0.93);
  auto center = agx::Vec3(0.312, 0, 0.497);
  auto up = agx::Vec3(-0.0251, 0.1424, 0.9895);
  app->setCameraHome(eye, center, up);
 
  // Controllers for the robot
  //
  auto llc = new LowLevelController(chain, panda, app->getSceneDecorator());
  auto tc = new TaskController(chain, panda, llc);
 
  sim->add(llc);
  sim->add(tc);
 
  llc->setDesiredQ(readyPose);
  createTaskPlan(tc, observer->getFrame()->getMatrix(), boxSize, boxStack1Pos, boxStack2Pos);
 
  return root;
}
 
 
void setRobotPose(agxModel::SerialKinematicChain* chain, agxSDK::Assembly* panda, agx::RealVector q)
{
  // The panda is using a tool (panda_hand, panda_rightfinger, panda_leftfinger).
  // Only panda_hand is part of the chain and we'll have to handle repositioning the fingers ourself.
  // This is done by..
  // - Compute relative transform for the fingers to the hand
  // - Compute the new forward position for each link given input joint angles
  // - Update all links in the robot including the hand
  // - Compute transform for the fingers by using hand transform and the finger relative transform
  //
  auto bodies = chain->getLinks();
 
  auto ee = bodies.back();
  auto eeInverse = ee->getFrame()->getLocalMatrix().inverse();
 
  auto finger1 = panda->getRigidBody("panda_rightfinger");
  auto finger2 = panda->getRigidBody("panda_leftfinger");
 
  // Step 1
  auto finger1Rel  = finger1->getFrame()->getLocalMatrix() * eeInverse;
  auto finger2Rel  = finger2->getFrame()->getLocalMatrix() * eeInverse;
 
  // Step 2 - forward computation to get transforms given joint positions
  agx::AffineMatrix4x4Vector transforms;
  auto status = chain->computeForwardKinematicsAll(q, transforms, true);
  if (status != agxModel::KinematicChain::SUCCESS) {
    throw std::runtime_error("Forward computation failed");
  }
 
  // Step 3 - update all links
  for (size_t idx = 0; idx < bodies.size(); ++idx) {
    bodies[idx]->getFrame()->setLocalMatrix(transforms[idx]);
  }
 
  // Step 4
  finger1->getFrame()->setLocalMatrix(finger1Rel * ee->getFrame()->getLocalMatrix());
  finger2->getFrame()->setLocalMatrix(finger2Rel * ee->getFrame()->getLocalMatrix());
}
 
 
void createTaskPlan(TaskController* tc, agx::AffineMatrix4x4 startTransform, agx::Vec3 boxSize, agx::Vec3 boxStack1Pos, agx::Vec3 boxStack2Pos)
{
  /*
  Create a set of "tasks" so that the robot moves the two boxes that are on
  top of each other and places them on the single box so that each box has
  a different orientation.
  */
 
  auto pickupOffset = 0.05;
  auto dropOffset = 0.08;
 
  /*
  Target orientations we'll use when when creating target transforms
    q1: z-axis points straight downwards
    q2: z-axis downwards, x-axis pointing to the right instead of forward
    q3: z-axis forward, x-axis downwards
   */
  auto q1 = agx::Quat(agx::Vec3::Z_AXIS(), -agx::Vec3::Z_AXIS());
  auto q2 = agx::Quat(agx::PI, agx::Vec3(1, 0, 0), agx::PI * 0.5, agx::Vec3(0, 0, 1), 0, agx::Vec3(0, 0, 1));
  auto q3 = agx::Quat(agx::PI, agx::Vec3(1, 0, 0), -agx::PI * 0.5, agx::Vec3(0, 1, 0), agx::PI, agx::Vec3(1, 0, 0));
 
  auto boxZ = boxSize[2] * 2;
  auto yzDiff = boxSize[2] - boxSize[1];
 
  /*
  To keep this simple, robot base transform and world frame are the same.
  If the entire robot is repositioned, by e.g. changing the robot assembly frame, then
  these pX transforms should be updated to be relative the base and not in world coords.
  */
 
  // Start pos
  auto p0 = startTransform;
 
  // Above top box in stack 1, then down
  auto p1 = agx::AffineMatrix4x4(q1, boxStack1Pos + agx::Vec3(0, 0, boxZ + pickupOffset));
  auto p2 = agx::AffineMatrix4x4(q1, boxStack1Pos + agx::Vec3(0, 0, boxZ));
 
  // To the tower, then down
  auto p3 = agx::AffineMatrix4x4(q2, boxStack2Pos + agx::Vec3(0, 0, boxZ + dropOffset));
  auto p4 = agx::AffineMatrix4x4(q2, boxStack2Pos + agx::Vec3(0, 0, boxZ));
 
  // Above bottom box in stack 1, then down
  auto p5 = agx::AffineMatrix4x4(q1, boxStack1Pos + agx::Vec3(0, 0, pickupOffset));
  auto p6 = agx::AffineMatrix4x4(q1, boxStack1Pos);
 
  // To the tower, then down
  auto p7 = agx::AffineMatrix4x4(q3, boxStack2Pos + agx::Vec3(0, 0, boxZ * 2 + yzDiff + dropOffset));
  auto p8 = agx::AffineMatrix4x4(q3, boxStack2Pos + agx::Vec3(0, 0, boxZ * 2 + yzDiff));
 
 
  tc->addTask(TaskItem(TASK_OPEN,   0.0,  1.0));
  tc->addTask(TaskItem(TASK_MOVE,   0.0,  1.0, p0, p1));
  tc->addTask(TaskItem(TASK_MOVE,   1.0,  2.0, p1, p2));
  tc->addTask(TaskItem(TASK_CLOSE,  2.0,  3.0));
  tc->addTask(TaskItem(TASK_MOVE,   3.0,  4.0, p2, p1));
  tc->addTask(TaskItem(TASK_MOVE,   4.0,  6.0, p1, p3));
  tc->addTask(TaskItem(TASK_MOVE,   6.0,  7.0, p3, p4));
  tc->addTask(TaskItem(TASK_OPEN,   7.0,  7.5));
  tc->addTask(TaskItem(TASK_MOVE,   7.5,  9.0, p4, p3));
  tc->addTask(TaskItem(TASK_MOVE,   9.0, 11.0, p3, p5));
  tc->addTask(TaskItem(TASK_MOVE,  11.0, 12.0, p5, p6));
  tc->addTask(TaskItem(TASK_CLOSE, 12.0, 13.0));
  tc->addTask(TaskItem(TASK_MOVE,  13.0, 14.0, p6, p5));
  tc->addTask(TaskItem(TASK_MOVE,  14.0, 16.0, p5, p7));
  tc->addTask(TaskItem(TASK_MOVE,  16.0, 17.0, p7, p8));
  tc->addTask(TaskItem(TASK_OPEN,  17.0, 18.0));
  tc->addTask(TaskItem(TASK_MOVE,  18.0, 19.0, p8, p7));
  tc->addTask(TaskItem(TASK_MOVE,  19.0, 20.0, p7, p0));
}
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
 
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tRobot - pick and place\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication();
 
    application->addScene(buildTutorial1, '1' );
 
    if (application->init(argc, argv))
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_ros2.cpp

/*
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.
*/
 
#include <agx/String.h>
#include <agx/version.h>
#include <agxROS2/AnyMessageBuilder.h>
#include <agxROS2/AnyMessageParser.h>
#include <agxROS2/Publisher.h>
#include <agxROS2/Subscriber.h>
#include <agxOSG/ExampleApplication.h>
 
// Standard library includes.
#include <cstdint>
#include <sstream>
#include <vector>
 
 
namespace tutorial_ros2_helpers
{
  class ScreenLogger
  {
  public:
    ScreenLogger(agxOSG::ExampleApplication* inApplication) : application(inApplication) {}
    void write(const agx::String& text, const agx::Vec4f& color = agx::Vec4f(1.0, 1.0, 1.0, 1.0))
    {
      if (application == nullptr)
        return;
 
      application->getSceneDecorator()->setText(row++, text, color);
    }
  private:
    int row = 0;
    agxOSG::ExampleApplication* application = nullptr;
  };
}
 
 
osg::Group* buildTutorial1(agxSDK::Simulation*, agxOSG::ExampleApplication* application)
{
  using namespace tutorial_ros2_helpers;
  ScreenLogger logger(application);
  static const agx::Vec4f red(1.0, 0.0, 0.0, 1.0);
  static const agx::Vec4f green(0.0, 1.0, 0.0, 1.0);
  static const agx::Vec4f yellow(0.0, 1.0, 1.0, 1.0);
  logger.write("Basic Publisher and Subscriber demo");
  logger.write("");
 
  // Here we create a Publisher and Subscriber for std_msgs::Float32 using topic 'basic_demo'.
  agxROS2::stdMsgs::PublisherFloat32 pub("basic_demo");
  agxROS2::stdMsgs::SubscriberFloat32 sub("basic_demo");
 
  // We send a simple Float32 message.
  agxROS2::stdMsgs::Float32 msgSend{3.1415f};
  logger.write(agx::String::format("[Publisher] Sending message: %f", msgSend.data), yellow);
  pub.sendMessage(msgSend);
 
  // Try receive the message.
  // Normally the below part of the code will run in a separate application and/or computer.
  agxROS2::stdMsgs::Float32 msgReceive;
  if (sub.receiveMessage(msgReceive))
  {
    logger.write(agx::String::format("[Subscriber] Got message: %f", msgReceive.data), green);
  }
  else
  {
    logger.write(
      agx::String("[Subscriber] Something went wrong, did not receive any message."), red);
  }
 
  return new osg::Group();
}
 
 
osg::Group* buildTutorial2(agxSDK::Simulation*, agxOSG::ExampleApplication* application)
{
  using namespace tutorial_ros2_helpers;
  ScreenLogger logger(application);
  static const agx::Vec4f green(0.0, 1.0, 0.0, 1.0);
  static const agx::Vec4f yellow(0.0, 1.0, 1.0, 1.0);
  logger.write("Quality of Service (QOS) demo");
  logger.write("");
 
  // If we need more control over settings such as message durability, history, reliability or
  // history depth, we specify this in a QOS object that is then given to any Publisher and 
  // Subscriber we wish these settings to apply to.
  //
  // Here we demonstrate how QOS settings can be used to support late joining, i.e. Subscribers
  // joining after some messages has been sent can still read those messages.
  agxROS2::QOS qos;
  qos.durability = agxROS2::QOS_DURABILITY::TRANSIENT_LOCAL;
  qos.history = agxROS2::QOS_HISTORY::KEEP_LAST_HISTORY_QOS;
  qos.reliability = agxROS2::QOS_RELIABILITY::RELIABLE;  
  qos.historyDepth = 5;
  logger.write(agx::String::format("[QOS] Setting history depth to: %d", qos.historyDepth));
 
  // Here we create a Publisher and immediately publish 10 messages with data going from 1..10;
  agxROS2::stdMsgs::PublisherInt32 pub_early_sender("qos_demo", qos);
  for (int i = 1; i <= 10; i++)
  {
    pub_early_sender.sendMessage({i});
    logger.write(agx::String::format("[Early Publisher] Sending message: %d", i), yellow);
  }
 
 
  // We now create a Subscriber and try to read the already sent messages.
  // Normally the below part of the code will run in a separate application and/or computer.
  // The historyDepth is set to 5 in the QOS, so we should only get 5 messages.
  agxROS2::stdMsgs::SubscriberInt32 sub_late_joiner("qos_demo", qos);
  logger.write(agx::String::format(
    "[Late Subscriber] Expecing to get %d messages...", qos.historyDepth), green);
 
  // Try receive the messages (expecting 5).
  agxROS2::stdMsgs::Int32 msgReceive;
  while (sub_late_joiner.receiveMessage(msgReceive))
  {
    logger.write(agx::String::format(
      "[Late Subscriber] Got message: %d", msgReceive.data), green);
  }
 
  return new osg::Group();
}
 
 
namespace tutorial_ros2_helpers
{
  struct MyStruct
  {
    int32_t myInt {0};
    std::vector<std::string> myStrings;
    bool myBool {false};
  };
 
  std::string toString(const MyStruct& v)
  {
    std::stringstream ss;
    ss << v.myInt << " " << "{ ";
    for (const auto& s : v.myStrings)
      ss << s << " ";
 
    auto boolToStr = [](bool b) { return b ? "true" : "false"; };
    ss << "} " << boolToStr(v.myBool);
    return ss.str();
  }
}
 
osg::Group* buildTutorial3(agxSDK::Simulation*, agxOSG::ExampleApplication* application)
{
  using namespace tutorial_ros2_helpers;
  ScreenLogger logger(application);
  static const agx::Vec4f red(1.0, 0.0, 0.0, 1.0);
  static const agx::Vec4f green(0.0, 1.0, 0.0, 1.0);
  static const agx::Vec4f yellow(0.0, 1.0, 1.0, 1.0);
  logger.write("AnyMessageBuilder and AnyMessageParser demo");
  logger.write("");
 
  // Here we will send a custom struct type as an agxMsgs::Any message.
  // The serialization / deserialization of the custom struct is done
  // by the AnyMessageBuilder / AnyMessageParser. It supports the most common
  // built in POD types as well as sequences (std::vector).
  //
  // First we create the Publisher and Subscriber of the agx_msgs::Any type.
  agxROS2::agxMsgs::PublisherAny pub("any_msg_demo");
  agxROS2::agxMsgs::SubscriberAny sub("any_msg_demo");
 
  // Now we set up values in our custom struct.
  MyStruct sendStruct;
  sendStruct.myInt = 42;
  sendStruct.myStrings = { "apple", "toast", "quanta" };
  sendStruct.myBool = true;
 
  // We now serialize our custom struct as an agx_msgs::Any message using the AnyMessageBuilder.
  // Notice that the order of building the message matches the custom struct layout.
  agxROS2::AnyMessageBuilder builder;
  builder
    .beginMessage()
    .writeInt32(sendStruct.myInt)
    .writeStringSequence(sendStruct.myStrings)
    .writeBool(sendStruct.myBool);
 
  // Send the message.
  pub.sendMessage(builder.getMessage());
  logger.write(agx::String::format(
    "[Publisher] Sending custom struct: %s", toString(sendStruct).c_str()), yellow);
 
  // Receive the message.
  // Normally the below part of the code will run in a separate application and/or computer.
  agxROS2::agxMsgs::Any receiveMsg;
  if (!sub.receiveMessage(receiveMsg))
  {
    logger.write(
      agx::String("[Subscriber] Something went wrong, did not receive any message."), red);
    return new osg::Group();
  }
 
  // We can now deserialize the agx_msgs::Any message back to our custom struct type using
  // the AnyMessageParser. Notice that the order of reading back the data matches the custom
  // struct layout, this is important.
  MyStruct receiveStruct;
  agxROS2::AnyMessageParser parser;
  parser.beginParse();
  receiveStruct.myInt = parser.readInt32(receiveMsg);
  receiveStruct.myStrings = parser.readStringSequence(receiveMsg);
  receiveStruct.myBool = parser.readBool(receiveMsg);
 
  logger.write(agx::String::format(
    "[Subscriber] Received custom struct: %s", toString(receiveStruct).c_str()), green);
  return new osg::Group();
}
 
 
int main(int argc, char** argv)
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tROS2 tutorial\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene(buildTutorial1, '1');
    application->addScene(buildTutorial2, '2');
    application->addScene(buildTutorial3, '3');
 
    if (application->init(argc, argv))
      return application->run();
  }
  catch (std::exception& e) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_statistics.cpp

/*
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.
*/
 
 
/*
 
Demonstrates how to use the statistics API to access statistics information during runtime.
 
*/
 
#include <agx/version.h>
#include <agxSDK/Simulation.h>
#include <agxCollide/Box.h>
void createScene(agxSDK::Simulation *simulation)
{
  agx::RigidBodyRef dynamicRB = new agx::RigidBody();
 
  agxCollide::GeometryRef dynamicRBGeometry = new agxCollide::Geometry();
  agxCollide::BoxRef dynamicRBBox = new agxCollide::Box(agx::Vec3(0.5, 0.5, 0.5));
  dynamicRBGeometry->add(dynamicRBBox);
  dynamicRB->add(dynamicRBGeometry);
 
  simulation->add(dynamicRB);
 
  agx::RigidBodyRef staticRB = new agx::RigidBody();
  staticRB->add(new agxCollide::Geometry(new agxCollide::Box(agx::Vec3(5.0, 5.0, 0.2))));
  simulation->add(staticRB);
  staticRB->setMotionControl(agx::RigidBody::STATIC);
 
  dynamicRB->setPosition(agx::Vec3(0, 0, 5));
  staticRB->setPosition(agx::Vec3(0, 0, -0.2));
}
 
void printStatistics()
{
  agx::Vector<std::pair<agx::String, agx::String> > entryNames;
 
  // Statistics data is available as a pair of strings: supplier of data and item to measure.
  // Note that it can be timing or countable data
  entryNames.push_back(std::make_pair("Simulation", "Step forward time"));
  entryNames.push_back(std::make_pair("Simulation", "Dynamics-system time"));
  entryNames.push_back(std::make_pair("Simulation", "Collision-detection time"));
  entryNames.push_back(std::make_pair("Simulation", "Pre-collide event time"));
  entryNames.push_back(std::make_pair("Simulation", "Pre-step event time"));
  entryNames.push_back(std::make_pair("Simulation", "Post-step event time"));
  entryNames.push_back(std::make_pair("Simulation", "Triggering contact events"));
  entryNames.push_back(std::make_pair("Simulation", "Culling contacts"));
  entryNames.push_back(std::make_pair("Simulation", "Triggering contact separation events"));
  entryNames.push_back(std::make_pair("Simulation", "Contact reduction"));
  entryNames.push_back(std::make_pair("Simulation", "Committing removed contacts"));
 
  entryNames.push_back(std::make_pair("Space", "Broad-phase time"));
  entryNames.push_back(std::make_pair("Space", "Narrow-phase time"));
  entryNames.push_back(std::make_pair("Space", "Sync bounds time"));
  entryNames.push_back(std::make_pair("Space", "Sync transforms time"));
  entryNames.push_back(std::make_pair("Space", "Num geometries"));
  entryNames.push_back(std::make_pair("Space", "Num geometry-geometry contact points"));
  entryNames.push_back(std::make_pair("Space", "Num geometry-geometry contacts"));
  entryNames.push_back(std::make_pair("Space", "Num narrow phase tests"));
 
  entryNames.push_back(std::make_pair("DynamicsSystem", "Update time"));
  entryNames.push_back(std::make_pair("DynamicsSystem", "Solver time"));
  entryNames.push_back(std::make_pair("DynamicsSystem", "Sabre: Factoring"));
  entryNames.push_back(std::make_pair("DynamicsSystem", "Sabre: Solve"));
 
  entryNames.push_back(std::make_pair("DynamicsSystem", "Num multi-body constraints"));
  entryNames.push_back(std::make_pair("DynamicsSystem", "Num solve islands"));
  entryNames.push_back(std::make_pair("DynamicsSystem", "Num enabled rigid bodies"));
  entryNames.push_back(std::make_pair("DynamicsSystem", "Num binary constraints"));
  entryNames.push_back(std::make_pair("DynamicsSystem", "Num contact constraints"));
 
  // Print the data
  for (auto entry : entryNames) {
    agx::TimingInfo info = agx::Statistics::instance()->getTimingInfo(entry.first, entry.second);
    //
    // info.current contains data for this time step, 
    // info.accumulated contains the total accumulated data since the start of the simulation
    //
    std::cout << entry.first << ":" << entry.second << " " << info.current << " (" << info.accumulated << ")" << std::endl;
  }
}
void buildTutorial1()
{
  agxSDK::SimulationRef simulation = new agxSDK::Simulation();
  createScene(simulation);
 
  agx::Statistics::instance()->setEnable(true);
 
  while (simulation->getTimeStamp() < 2)
  {
    simulation->stepForward();
    printStatistics();
  }
}
 
 
int main()
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tStatistics tutorial\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
 
    buildTutorial1();
 
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;               
    return 1;
  }
 
  return 0;
}

tutorial_suctionGripper.cpp

/*
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.
*/
 
 
/*
 
Demonstrates how to model a suction gripper on a robot using the agxModel::SuctionGripper class.
 
*/
 
 
#include <agxOSG/ExampleApplication.h>
 
#include <agxModel/SuctionGripper.h>
#include "SuctionGripperTutorialHelpers.h"
#include <agxUtil/agxUtil.h>
#include <agxUtil/TrimeshReaderWriter/TrimeshReaderWriter.h>
#include <agxUtil/TrimeshDeformer.h>
#include <agxOSG/utils.h>
#include <agxOSG/SceneDecorator.h>
 
#include <agxSDK/Simulation.h>
#include <agxSDK/GuiEventListener.h>
 
#include <agxCollide/Box.h>
#include <agxCollide/Cylinder.h>
#include <agxCollide/Capsule.h>
 
#include <agxRender/RenderManager.h>
#include <agx/version.h>
#include <agx/LockJoint.h>
#include <agx/Prismatic.h>
#include <agx/Hinge.h>
 
 
agxModel::SuctionCup* createSuctionCup(
  agx::Material* material,
  const agx::Real& radius = agx::Real(0.025),
  const agx::Real& cupHeight = agx::Real(0.01),
  const size_t& sealResolution = 4u)
{
  // Define a local direction, in which the vacuum force acts
  // The suction cup is limited to have a plane
  // including the origin of the cupBody
  auto localCupForceDir = agx::Vec3(0, 0, 1);
  // Create the suction cup with a cylindrical geometry
  auto cupBody = new agx::RigidBody("Cup");
  // We give the cup a cylindrical collision geometry
  // The idea is that the collision with the cylinder will
  // prevent objects to be sucked past the cup plane.
  // The seal sensors (if sealResolution > 0) will also
  // register full seal for objects in this plane,
  // enabling full vacuum force, given the vacuum level.
  auto cupShape = new agxCollide::Cylinder(radius, cupHeight);
  // Which we rotate to have the cylinder end normals
  // orthogonal to the local x-y plane of the cup
  auto rotate = agx::AffineMatrix4x4::rotate(agx::Vec3::Y_AXIS(), localCupForceDir);
  // Move the cylinder to be fully above
  // the cupBody Z = 0 plane,
  // where the line sensor has it origin,
  // and where the seal sensors are placed.
  auto translate = agx::AffineMatrix4x4::translate(localCupForceDir * cupHeight * agx::Real(0.5));
  auto cupGeometry = new agxCollide::Geometry(cupShape, rotate * translate);
  //Give the cup collision geometry a materal
  cupGeometry->setMaterial(material);
  cupBody->add(cupGeometry);
 
  // let the line sensor point in the negative force direction
  auto lineSensorReach = agx::Real(0.1);
  auto lineSensorVector = -lineSensorReach * localCupForceDir;
  agxModel::SuctionCup* cup = new agxModel::SuctionCup(cupBody, radius, cupHeight, sealResolution, lineSensorVector);
 
  return cup;
}
 
void attachSuctionCup(agxModel::SuctionGripper* gripper,
  agxModel::SuctionCup* cup,
  const agx::Vec3& holderLocalTranslate,
  const agx::Vec3& cupLocalTranslate
)
{
  auto lock = gripper->positionAndLockSuctionCup(gripper, cup, holderLocalTranslate, cupLocalTranslate);
  gripper->addSuctionCup(cup);
  gripper->add(lock);
}
 
agx::RigidBody* createHolderBody(const agx::Real& holderOffsetZ = agx::Real(0.025))
{
  auto holderShape = new agxCollide::Cylinder(agx::Real(0.03), holderOffsetZ * agx::Real(2.0));
  auto holderGeometry = new agxCollide::Geometry(holderShape, agx::AffineMatrix4x4::rotate(agx::PI_2, agx::Vec3(1, 0, 0)));
 
  // Create the suction holder body with a cylindrical geometry
  auto holderBody = new agx::RigidBody("Holder");
 
  holderBody->add(holderGeometry);
  holderBody->getFrame()->setLocalTranslate(agx::Vec3(0, 0, holderOffsetZ));
 
  return holderBody;
}
 
agxModel::SuctionGripper* createSuctionGripperOneCup(agxSDK::Simulation* simulation,
                                               osg::Group* root,
                                               agx::Material* cupMaterial)
{
  // Now a simple suction gripper (a holder with one suction cup).
// We define the gripper attached to the world frame
// but it still can move in the gravitational direction
 
  auto holderOffsetZ = agx::Real(0.025);
  auto holderBody = createHolderBody(holderOffsetZ);
  // Create the gripper
  auto gripper = new agxModel::SuctionGripper(holderBody);
 
  gripper->getVacuumSystem()->setDesiredVacuum(agx::Real(0.3));
 
  auto holderBodyOffset = agx::Vec3(0, 0, -holderOffsetZ);
  auto cupOffsetZ = agx::Real(-0.05);
  auto cup = createSuctionCup(cupMaterial);
 
  attachSuctionCup(gripper, cup, holderBodyOffset, agx::Vec3(0, 0, cupOffsetZ));
 
  addDeformableMesh(simulation, root, gripper->getHolderRigidBody(), cup, holderBodyOffset);
 
  agxOSG::createVisual(gripper->getHolderRigidBody(), root);
 
  // Position the gripper in world frame
  gripper->setPosition(agx::Vec3(0, 0, 0.3));
 
  // Add the gripper to the simulation
  simulation->add(gripper);
 
  return gripper;
}
 
agxModel::SuctionGripper* createMultiCupSuctionGripper(agxSDK::Simulation* simulation,
  osg::Group* root,
  agx::Material* cupMaterial)
{
 
  // Now a suction gripper with siz cups
  auto holderOffsetZ = agx::Real(0.025);
  auto holderShape = new agxCollide::Box(agx::Vec3(0.3, 0.1, holderOffsetZ));
  auto holderGeometry = new agxCollide::Geometry(holderShape);
  // Create the suction holder body with a cylindrical geometry
  auto holderBody = new agx::RigidBody("Holder");
 
  holderBody->add(holderGeometry);
  holderBody->getFrame()->setLocalTranslate(agx::Vec3(0, 0, holderOffsetZ));
  // Create the gripper
  auto gripper = new agxModel::SuctionGripper(holderBody);
 
  auto holderBodyOffset = agx::Vec3(0, 0, -holderOffsetZ);
  auto cupOffsetX = agx::Real(0.2);
  auto cupOffsetY = agx::Real(0.1);
  auto cupOffsetZ = agx::Real(-0.05);
 
  auto lipRadius = agx::Real(0.025);
  auto lipHeight = agx::Real(0.03);
 
  auto cup1 = createSuctionCup(cupMaterial, lipRadius, lipHeight);
  attachSuctionCup(gripper, cup1, holderBodyOffset, agx::Vec3(cupOffsetX, cupOffsetY, cupOffsetZ));
  addDeformableMesh(simulation, root, gripper->getHolderRigidBody(), cup1, agx::Vec3(cupOffsetX, cupOffsetY, -holderOffsetZ));
  auto cup2 = createSuctionCup(cupMaterial, lipRadius, lipHeight);
  attachSuctionCup(gripper, cup2, holderBodyOffset, agx::Vec3(cupOffsetX, -cupOffsetY, cupOffsetZ));
  addDeformableMesh(simulation, root, gripper->getHolderRigidBody(), cup2, agx::Vec3(cupOffsetX, -cupOffsetY, -holderOffsetZ));
  auto cup3 = createSuctionCup(cupMaterial, lipRadius, lipHeight);
  attachSuctionCup(gripper, cup3, holderBodyOffset, agx::Vec3(0, cupOffsetY, cupOffsetZ));
  addDeformableMesh(simulation, root, gripper->getHolderRigidBody(), cup3, agx::Vec3(0, cupOffsetY, -holderOffsetZ));
  auto cup4 = createSuctionCup(cupMaterial, lipRadius, lipHeight);
  attachSuctionCup(gripper, cup4, holderBodyOffset, agx::Vec3(0, -cupOffsetY, cupOffsetZ));
  addDeformableMesh(simulation, root, gripper->getHolderRigidBody(), cup4, agx::Vec3(0, -cupOffsetY, -holderOffsetZ));
  auto cup5 = createSuctionCup(cupMaterial, lipRadius, lipHeight);
  attachSuctionCup(gripper, cup5, holderBodyOffset, agx::Vec3(-cupOffsetX, cupOffsetY, cupOffsetZ));
  addDeformableMesh(simulation, root, gripper->getHolderRigidBody(), cup5, agx::Vec3(-cupOffsetX, cupOffsetY, -holderOffsetZ));
  auto cup6 = createSuctionCup(cupMaterial, lipRadius, lipHeight);
  attachSuctionCup(gripper, cup6, holderBodyOffset, agx::Vec3(-cupOffsetX, -cupOffsetY, cupOffsetZ));
  addDeformableMesh(simulation, root, gripper->getHolderRigidBody(), cup6, agx::Vec3(-cupOffsetX, -cupOffsetY, -holderOffsetZ));
 
  // Set some mass to the holder
  gripper->getHolderRigidBody()->getMassProperties()->setMass(3.0);
 
  // Add the gripper to the simulation
  simulation->add(gripper);
 
  // Add visual for the holder body
  agxOSG::createVisual(gripper->getHolderRigidBody(), root);
 
  // Position the gripper in world frame
  gripper->setPosition(agx::Vec3(0, 0, 0.3));
 
  return gripper;
}
 
osg::Group* buildTutorialSingleCupSuctionGripper(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
  setupCamera(application);
 
  osg::Group* root = new osg::Group();
 
  // Setup the simulation environment
  auto cupMaterial = prepareGroundBoxAndMaterials(simulation, root);
 
  auto radius = agx::Real(0.04);
  auto height = agx::Real(0.04);
  auto cupHeight = agx::Real(0.07);
  auto lipHeight = agx::Real(0.01);
  auto sealResolution = size_t(0);
  auto gripper = new agxModel::SingleCupSuctionGripper(sealResolution, radius, height, cupHeight, lipHeight, nullptr, cupMaterial);
  auto geometry = gripper->getTrimeshGeometry();
 
  // Set the world position of the gripper
  gripper->setPosition(agx::Vec3(0, 0, 0.2));
  auto deformer = gripper->getTrimeshDeformer();
  simulation->add(deformer);
  // Add the gripper to the simulation
  simulation->add(gripper);
 
  auto meshNode = agxOSG::createVisual(geometry, root);
  agxOSG::setTexture(meshNode, "textures/plastic1.normal.png");
 
  // OSG updater for the deformable mesh
  auto updater = agxOSG::createVisualUpdater(deformer, root);
  simulation->add(updater);
  // Create visuals for the gripper geometries
  agxOSG::createVisual(gripper->getHolderRigidBody(), root);
 
  // Position the gripper in world frame
  gripper->setPosition(agx::Vec3(0, 0, 0.3));
 
  setupGripperKeyboardController(simulation, gripper);
  setupVaccumLevelPrinter(application, simulation, gripper);
 
  return root;
}
 
osg::Group* buildTutorialSuctionGripperDefaultCup( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
 
  setupCamera(application);
 
  osg::Group* root = new osg::Group();
 
  // Setup the simulation environment
  auto cupMaterial = prepareGroundBoxAndMaterials(simulation, root);
 
  // For the gripper to be valid it require a rigid body
  // to which suction cups can be attached
  auto holderOffsetZ = agx::Real(0.025);
  // The createHolderBody utility function will offset the
  // holder local to the gripper
  auto holderBody = createHolderBody(holderOffsetZ);
  // Create the gripper
  auto gripper = new agxModel::SuctionGripper(holderBody);
 
  // Create the suction cup
  // If nullptr is passed for the cupBody argument
  // a rigid body with cylindrical collision geometry is created
  auto cup = new agxModel::SuctionCup(nullptr);
  // Set the cupMaterial to the cup body geometry
  // for wanted friction model for the cup - object interaction
  cup->getRigidBody()->getGeometries()[0]->setMaterial(cupMaterial);
 
  // Attach the suction cup to the holder body
  // and position the cup in accordingly in the gripper frame
  auto holderBodyOffset = agx::Vec3(0, 0, -holderOffsetZ);
  auto cupLocalPosition = agx::Vec3(0, 0, -0.05);
  attachSuctionCup(gripper, cup, holderBodyOffset, cupLocalPosition);
 
  // Create visuals for the gripper geometries
  agxOSG::createVisual(gripper->getHolderRigidBody(), root);
  agxOSG::createVisual(cup->getRigidBody(), root);
 
  // Position the gripper in world frame
  gripper->setPosition(agx::Vec3(0, 0, 0.3));
 
  // Add the gripper to the simulation
  simulation->add(gripper);
 
  setupGripperKeyboardController(simulation, gripper);
  setupVaccumLevelPrinter(application, simulation, gripper);
 
  return root;
}
 
osg::Group* buildTutorialSuctionGripperSingleCup(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
 
  setupCamera(application);
 
  osg::Group* root = new osg::Group();
 
  auto cupMaterial = prepareGroundBoxAndMaterials(simulation, root);
 
  auto gripper = createSuctionGripperOneCup(simulation, root, cupMaterial);
 
  setupGripperKeyboardController(simulation, gripper);
  setupVaccumLevelPrinter(application, simulation, gripper);
 
  return root;
}
 
 
osg::Group* buildTutorialSuctionGripperMultiCup( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
 
  setupCamera(application);
 
  osg::Group* root = new osg::Group();
 
  // Create a flat box as object to pick up
  auto halfSide = agx::Real(0.5);
  auto halfHeight = agx::Real(0.005);
  auto cupMaterial = prepareGroundBoxAndMaterials(simulation, root, halfSide, halfHeight);
 
  auto gripper = createMultiCupSuctionGripper(simulation, root, cupMaterial);
 
 
  setupGripperKeyboardController(simulation, gripper);
  setupVaccumLevelPrinter(application, simulation, gripper);
  return root;
}
 
osg::Group* buildTutorialSuctionGripperDeformableObject(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application)
{
 
  setupCamera(application);
 
  osg::Group* root = new osg::Group();
 
  createGround(simulation, root);
 
  auto cupMaterial = prepareGroundDeformableAndMaterials(simulation, root, 0.3);
 
  auto gripper = createMultiCupSuctionGripper(simulation, root, cupMaterial);
 
  setupGripperKeyboardController(simulation, gripper);
  setupVaccumLevelPrinter(application, simulation, gripper);
  return root;
}
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout <<
                "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
                "\tTutorial SuctionGripper\n" <<
    "\t--------------------------------\n\n" << std::endl;
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene(buildTutorialSingleCupSuctionGripper, '1');
    application->addScene(buildTutorialSuctionGripperDefaultCup, '2');
    application->addScene(buildTutorialSuctionGripperSingleCup, '3' );
    application->addScene(buildTutorialSuctionGripperMultiCup, '4' );
    application->addScene(buildTutorialSuctionGripperDeformableObject, '5');
 
 
 
 
 
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_terrainPager_tileEvents.cpp

/*
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.
*/
 
/*
 
The agxTerrain::TerrainPager class can be used to have dynamic loading and unloading
of agxTerrain::Terrain instances. A user application using the TerrainPager could be
interested in when terrain is added to or removed from the simulation.
 
To achieve this, Tile Callbacks can be used.
 
In this tutorial, a listener is created that uses tile callbacks that prints some
information when events are triggered.
 
It is assumed that the reader of this tutorial has looked at tutorial_agxTerrain.cpp.
 
*/
 
 
#include <agx/config/AGX_USE_AGXTERRAIN.h>
 
#include <agx/version.h>
#include <agx/Logger.h>
 
#if AGX_USE_AGXTERRAIN()
 
#include <agx/Hinge.h>
 
#include <agxCollide/Box.h>
#include <agxCollide/Geometry.h>
 
#include <agxIO/FileSystem.h>
 
#include <agxSDK/Simulation.h>
 
#include <agxTerrain/Terrain.h>
#include <agxTerrain/TerrainPager.h>
 
#include <agxOSG/ExampleApplication.h>
 
 
#ifdef _MSC_VER
#include <process.h>
#define getpid _getpid
#else
#include <unistd.h>
#endif
 
 
class DemoListener : public agxSDK::StepEventListener
{
  public:
 
    // Constructor, set up callbacks
    DemoListener( agxTerrain::TerrainPager* pager );
 
    // Destructor, removes callback
    ~DemoListener();
 
    // Two different methods used to receive tile callbacks:
    void onLoad( agxTerrain::TileId, agxTerrain::Terrain* );
    void onUnload( agxTerrain::TileId, agxTerrain::Terrain* );
 
    // Example of one function receiving both load and unload callbacks:
    void onLoadAndUnloadCallback( agxTerrain::TileId, agxTerrain::Terrain*, int );
 
    // Override StepEventListener::last. Will be called during Simulation::stepForward
    void last( const agx::TimeStamp& time ) override;
 
  private:
    agx::observer_ptr<agxTerrain::TerrainPager> m_pager;
 
    agxTerrain::TerrainPager::TileCallback m_loadCallback1;
    agxTerrain::TerrainPager::TileCallback m_unloadCallback1;
 
    agxTerrain::TerrainPager::TileCallback m_loadCallback2;
    agxTerrain::TerrainPager::TileCallback m_unloadCallback2;
 
    agx::HashTable< agxTerrain::Terrain*, agxTerrain::TileId > m_activeTiles;
 
    size_t m_lastCount;
 
};
 
 
DemoListener::DemoListener( agxTerrain::TerrainPager* pager ) :
  StepEventListener( agxSDK::StepEventListener::LAST_STEP ), m_pager( pager )
{
  m_lastCount = 0;
 
  // Setup callbacks:
 
  // First set of callbacks: different functions used for each callback type
  m_loadCallback1   = agxTerrain::TerrainPager::TileCallback( std::bind( &DemoListener::onLoad,   this, std::placeholders::_1, std::placeholders::_2 ) );
  m_unloadCallback1 = agxTerrain::TerrainPager::TileCallback( std::bind( &DemoListener::onUnload, this, std::placeholders::_1, std::placeholders::_2 ) );
 
  // Second set of callbacks: To show that multiple callbacks on each event is supported
  // and that name and signature of function that receives the callback is not that important
  // as long as it can be turned into something that is compatible.
  m_loadCallback2   = agxTerrain::TerrainPager::TileCallback( std::bind( &DemoListener::onLoadAndUnloadCallback, this, std::placeholders::_1, std::placeholders::_2,  1 ) );
  m_unloadCallback2 = agxTerrain::TerrainPager::TileCallback( std::bind( &DemoListener::onLoadAndUnloadCallback, this, std::placeholders::_1, std::placeholders::_2, -1 ) );
 
 
  if ( m_pager ) {
    // Note: agx::Event::addCallback uses pointers.
    // Callbacks should be removed from Events before the callbacks are deallocated to avoid problems
    // where something no longer existing is accessed.
    m_pager->tileLoadEvent.addCallback( &m_loadCallback1 );
    m_pager->tileUnloadEvent.addCallback( &m_unloadCallback1 );
 
 
    m_pager->tileLoadEvent.addCallback( &m_loadCallback2 );
    m_pager->tileUnloadEvent.addCallback( &m_unloadCallback2 );
 
  }
  else {
    LOGGER_ERROR() << "DemoListener requires a valid TerrainPager" << LOGGER_ENDL();
    exit(1);
  }
}
 
 
DemoListener::~DemoListener()
{
  // Remove the callbacks we added in the constructor
  if ( m_pager ) {
 
    m_pager->tileLoadEvent.removeCallback( &m_loadCallback1 );
    m_pager->tileUnloadEvent.removeCallback( &m_unloadCallback1 );
 
    m_pager->tileLoadEvent.removeCallback( &m_loadCallback2 );
    m_pager->tileUnloadEvent.removeCallback( &m_unloadCallback2 );
  }
 
}
 
 
void DemoListener::onLoad( agxTerrain::TileId id, agxTerrain::Terrain* t  ) {
 
  assert( m_activeTiles.contains( t ) == false );
  m_activeTiles[t] = id;
  printf("\t+ Tile [%i,%i] was loaded\n", id[0], id[1] );
}
 
 
void DemoListener::onUnload( agxTerrain::TileId id, agxTerrain::Terrain* t ) {
 
  assert( m_activeTiles.contains( t ) == true );
  m_activeTiles.erase( t );
  printf("\t- Tile [%i,%i] was unloaded\n", id[0], id[1] );
}
 
 
void DemoListener::onLoadAndUnloadCallback( agxTerrain::TileId id, agxTerrain::Terrain* /* t */, int num )
{
  // Same function for both callbacks. Num is +1 or -1 depending on callback type.
  printf("\t%+2i, tile id: [%i,%i]\n", num, id[0], id[1] );
}
 
 
void DemoListener::last( const agx::TimeStamp& time ) {
 
  if ( m_activeTiles.size() != m_lastCount ) {
 
    m_lastCount = m_activeTiles.size();
 
    printf("At T=%5.2f, the DemoListener knows about %i Terrain instances\n",
      (float) time, (int) m_activeTiles.size() );
 
  }
}
 
 
 
 
osg::Group* buildTutorial1( agxSDK::Simulation *sim, agxOSG::ExampleApplication* app )
{
  osg::Group* root = new osg::Group();
 
  // To have something dynamic which forces terrain tiles to be loaded and unloaded,
  // we setup a motorized hinge and hinge one body to the world.
  // The body will then move in a fixed circle.
  agx::RigidBody* body = new agx::RigidBody();
  body->setPosition( 8, 8, 4 );
  body->add( new agxCollide::Geometry( new agxCollide::Sphere( 0.3 ) ) );
  sim->add( body );
 
  agx::HingeFrame hFrame;
  hFrame.setAxis( agx::Vec3( 0,0,1 ) );
  hFrame.setCenter( agx::Vec3(0,0,4 ) );
  agx::Hinge* hinge = new agx::Hinge( hFrame, body );
  hinge->getMotor1D()->setEnable( true );
  hinge->getMotor1D()->setSpeed( 0.5 );
  sim->add(hinge);
 
  // The TerrainPager uses a "template terrain". The material settings for the
  // template is used for each tile that is paged in.
  // In this example no material properties are set.
  auto templateTerrain = new agxTerrain::Terrain(5,5,1,1);
 
 
  // The TerrainPager needs a "data source" that defines the height profile of the terrain
  // that should be paged in. Here we use a TerrainRasterizer which uses geometry as height
  // profile source and shoots rays to sample heights.
 
  // The source geometry should be positioned correctly in the world just as if it would
  // have been added to the Simulation.
 
  // Here we just use one box. Multiple geometries can be used including
  // meshes and heightfield(s).
  agxCollide::Box*      box  = new agxCollide::Box( 1000, 1000, 1 );
  agxCollide::Geometry* geom = new agxCollide::Geometry( box );
 
  auto tds = new agxTerrain::TerrainRasterizer( 50 );
  tds->addSourceGeometry( geom );
 
 
  // Here we create the actual TerrainPager.
 
  // Eeach Terrain instance (tile) will be square with side length (tile resolution - 1 ) * ( elementSize ).
  // In this example 25x25 m.
  size_t tileResolution = 51;
  size_t tileOverlap    = 5;
  agx::Real elementSize = 0.5;
  agx::Real maxDepth    = 2.0;
 
  auto tp = new agxTerrain::TerrainPager( tileResolution, tileOverlap, elementSize, maxDepth, agx::Vec3(), agx::Quat(), templateTerrain );
 
  tp->setName("TerrainPager");
 
  tp->setTerrainDataSource( tds );
 
  // When the TerrainPager pages out terrain data to disk, it has to be stored somewhere.
  // Here we create a unique directory name
  agx::String dirPrefix = agx::String::format("terrain-data-%i-", (int) getpid() );
  agx::String tmpDir    = agxIO::FileSystem::mkTmpDirectory( dirPrefix );
 
  if ( tmpDir.length() == 0 ) {
    printf("Failed to created temp directory for terrain data");
    exit(2);
  }
  else {
    printf("Using %s for temp storage of terrain data\n", tmpDir.c_str() );
  }
 
  tp->setFileCacheDirectory( tmpDir );
 
 
  // To know which tiles that are needed, the TerrainPager tracks one or many bodies.
  // Associated with each body are two radii. Tiles within the "required radius" must
  // be present for simulation::stepForward to be able to proceed.
  // Tiles within the "preload radius" will be loaded in the background and inserted
  // into the simulation when ready.
 
  // These radiuses should be adapted to the velocities involved in the scene and the
  // tile sizes so that tiles should be ready and already in the simulation
  // before they become required.
  tp->add( body, 1, 6 );
 
 
  sim->add( tp );
 
  // Here we create and insert the DemoListener into the Simulation.
  // It uses callbacks to be notified when terrain tiles are paged in and out.
  auto dl = new DemoListener( tp );
  sim->add( dl );
 
 
  // This example does not set up any agxOSG visualization for the Terrain instances
  // or the body moving in a circle. Instead, we enable debug rendering to show what
  // is going on.
  app->setEnableDebugRenderer( true );
 
  return root;
}
 
 
 
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
 
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTerrainPager and TileCallbacks\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    agx::shutdown();
    return 1;
  }
 
  return 0;
}
 
#else
// else for if AGX_USE_AGXTERRAIN() test. No agxTerrain support avaiable.
 
int main( int, char** )
{
  LOGGER_WARNING() << LOGGER_STATE(agx::Notify::PRINT_NONE) << "\n" <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTerrainPager and TileCallbacks\n" <<
    "\t--------------------------------\n" << LOGGER_ENDL() <<
    "AGX Dynamics built without support for agxTerrain" << LOGGER_ENDL();
 
  return 0;
}
 
#endif
 

tutorial_threadAffinity.cpp

/*
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.
*/
 
/*
 
Demonstrates how to use the thread API to set affinity for threads.
 
*/
 
 
#include <agx/agx.h>
#include <agx/agx_architecture.h>
#include <agx/version.h>
#include <agx/Thread.h>
 
#include <agxSDK/Simulation.h>
#include <agx/AtomicValue.h>
 
 
const agx::UInt64 ONE = 1u;
 
 
int tutorial_setAffinityAGXThreads()
{
  const unsigned int NUM_THREADS = 4;
  bool values[NUM_THREADS];
  bool ok;
 
 
  // Setup AGX to use NUM_THREADS number of Threads
  agx::setNumThreads(NUM_THREADS);
 
  // affinity mask for main thread: allow to run on all cores
  agx::Thread* t = agx::Thread::getCurrentThread();
  ok = values[0] = t->setThreadAffinity(0xffff);
 
  // Now loop through the AGX threads and set affinity to each thread
  for (auto i = 1u; i < NUM_THREADS; ++i)
  {
    //
    // For windows and linux, bind thread 'i' to logical core 'i'
    // OS X, use affinity tag 'i' for thread 'i'
    //
    // Now this does not automatically mean that each thread will run on separate hardware cores.
    // Depending on the architecture the mask can differ.
    // To understand how your target architecture enumerates its cores you can download
    // and run this tool: https://www.open-mpi.org/projects/hwloc/
    //
    // It will for example on a typical Windows based Intel processor with Hyper threading enabled show that
    // Each hardware core has two logical cores.
    // So to assign each thread to each physical core, you need to skip over one every loop
    //
    values[i] = agx::Thread::getThread(i)->setThreadAffinity(ONE << (agx::UInt64)i);
    ok &= values[i];
  }
 
  for (auto i = 0u; i < NUM_THREADS; ++i)
  {
    std::cerr << agx::String::format("Status for setting affinity for thread %i: %i\n", i, values[i]) << std::endl;
  }
 
  return (ok ? 0 : 1);
 
}
 
 
// Create a custom thread
class MyThread : public agx::BasicThread, public agx::Referenced
{
public:
 
 
  MyThread(unsigned int id ) : m_sum(0), m_id(id)
  {
  }
 
  // Will be called when the main thread calls start()
  void run();
 
 
  virtual ~MyThread() {}
 
  double m_sum;
  unsigned int m_id;
};
 
 
// This method will be called in the new thread after
// someone called start()
void MyThread::run()
{
  // The agx::BasicThread class that the agx::Threads and MyThread are based
  // upon have a method to set Thread Affinity. The affinity affects how
  // the threads are scheduled.
  //
  // Now this does not automatically mean that each thread will run on separate hardware cores.
  // Depending on the architecture the mask can differ.
  // To understand how your target architecture enumerates its cores you can download
  // and run this tool: https://www.open-mpi.org/projects/hwloc/
  //
  // It will for example on a typical Windows based Intel processor with Hyper threading enabled show that
  // Each hardware core has two logical cores.
  // So to assign each thread to each physical core, you need to skip over one every loop
  //
  this->setThreadAffinity(ONE << (agx::UInt64)m_id * 2);
 
  // Now just do some computations for a while.
  while ( m_sum < 1000000000)
  {
    m_sum += std::exp2(2) * std::cos(0.4) * std::exp2(2);
  }
}
 
void tutorial_createThreads()
{
  const unsigned int NUM_THREADS = 6;
 
  agx::Vector<agx::ref_ptr<MyThread>> threads;
 
 
  // Create NUM_THREADS threads
  for (auto i = 0u; i < NUM_THREADS; i++)
  {
    agx::ref_ptr<MyThread> t = new MyThread(i);
    threads.push_back(t);
  }
 
  // Register the main thread as AGX thread also, otherwise the call below returns null
  agx::Thread::registerAsAgxThread();
 
  // affinity mask for main thread
  agx::Thread* t = agx::Thread::getCurrentThread();
 
  // Main thread can run on all cores
  t->setThreadAffinity(0xffff);
 
  std::cerr << "Starting threads" << std::endl;
 
  // Start the threads
  for (auto thread : threads)
    thread->start();
 
  std::cerr << "Waiting for threads" << std::endl;
 
  // Wait for the threads to finish
  for (auto thread : threads)
  {
    thread->join();
  }
 
  std::cerr << "Shutting down" << std::endl;
 
  // Deallocate the threads
  threads.clear();
 
}
 
int main( int, char** )
{
 
  // Remember to initialize AGX if you are using agx objects
  agx::AutoInit init;
 
  // Show how to set affinity on custom threads
  tutorial_createThreads();
 
  // Show how to set Affinity on threads created by agx
  return tutorial_setAffinityAGXThreads();
}

tutorial_threads.cpp

/*
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.
*/
 
/*
 
Demonstrates how to Use the AGX Dynamics API in threads created by the user.
It is important to initialize the threads correctly so that any eventual callbacks is called within the
correct thread.
 
Schematically the following occurs:
 
- We create one Simulation which will be shared among several threads. This might be because you create the simulation in the main thread
but later want to use the Simulation in a GUI thread for example.
Or you create the Simulation in main thread and then want to create a separate Physics thread.
- We create a number of threads sharing this Simulation
- We have to make sure that only one thread at the time uses the Simulation
- We have to call registerAsAgxThread() to be able to use the API within the thread
- We have to make sure that callbacks for the simulation is done in the active thread (setMainWorkThread)
 
*/
 
 
#include <agx/Thread.h>
#include <agx/version.h>
#include <agxSDK/Simulation.h>
#include <agx/AtomicValue.h>
#include <agx/RigidBody.h>
#include <agxCollide/Trimesh.h>
#include <agxUtil/TrimeshReaderWriter/TrimeshReaderWriter.h>
 
 
// A step event listener that will print in which thread it is executed
class MyListener : public agxSDK::StepEventListener
{
public:
  MyListener()
  {}
 
 
  void pre(const agx::TimeStamp&)
  {
 
    // #12 In this callback we print in which thread we are executing, lets just print it once
    // for each thread (hence the HashSet)
    agx::Thread *thread = agx::Thread::getCurrentThread();
 
    if (!m_threads.contains(thread))
    {
      std::cerr << "In a pre-callback. Thread: " << thread << std::endl;
      m_threads.insert(thread);
    }
  }
 
  agx::HashSet<agx::Thread *> m_threads;
};
 
// This thread will use a Simulation and setup the threads
class MyThread : public agx::BasicThread
{
public:
 
  MyThread(agxSDK::Simulation *sim) : m_simulation(sim)
  {
    m_block.reset(); //Next one that comes to m_block.block() will be blocked
  }
 
  // Will be called when the main thread calls start()
  void run();
 
  // Will release anyone waiting for this block
  void release()
  {
    m_block.release();
  }
 
  virtual ~MyThread() {}
 
  agx::Block m_block;
  agxSDK::SimulationRef m_simulation;
};
 
 
// This method will be called in the new thread after
// someone called start()
void MyThread::run()
{
  // #6 We need to do this if we want to use AGX API within this thread.
  // If you forget this call you might get exceptions
  agx::Thread::registerAsAgxThread();
 
  // Print the address of this thread
  agx::Thread *thread = agx::Thread::getCurrentThread();
  std::cerr << "Thread: " << thread << std::endl;
 
  // We will use the already created Simulation.
  // Note, it was created in another thread (Main thread)
  agxSDK::SimulationRef sim = m_simulation;
 
 
  // #7 Wait here until someone calls the release() method (which will be done at #6)
  m_block.block();
  m_block.reset();
 
  // #8 Here we tell the simulation that all callbacks from listeners etc. should
  // occur in THIS thread, not the original thread which created it. If we would
  // drop the line below, we would get a dead lock as main thread is right now waiting at a join() call.
  // It cannot handle any callbacks. So we have to make sure it is this thread
  sim->setMainWorkThread(agx::Thread::getCurrentThread());
 
  // Now just simulate for a while.
  agx::Real t = sim->getTimeStamp();
  while (sim->getTimeStamp() - t < 0.1)
  {
    agx::RigidBodyRef body = new agx::RigidBody();
    sim->add(body);
 
    // #9 This will trigger a call to MyListener::pre()
    sim->stepForward();
    sim->remove(body);
  }
 
  // #10 This will release some things and will tell AGX that this thread is no longer an AGX thread
  agx::Thread::unregisterAsAgxThread();
}
 
// ===================================================
// main function for threadTutorial1
// ===================================================
void threadTutorial1()
{
  std::cerr << "Running " << __FUNCTION__ << std::endl << std::endl;
  // #1 Create one simulation common to all the threads
  agxSDK::SimulationRef  sim = new agxSDK::Simulation();
 
  // #2 Add a listener (as defined above)
  sim->add(new MyListener);
 
  // The thread that creates an agxSDK::Simulation will automatically be made an
  // AGX Thread by internally calling agx::Thread::registerAsAgxThread()
  agx::Thread *thread = agx::Thread::getCurrentThread();
  std::cerr << "Main thread: " << thread << std::endl;
 
  // Create a vector of threads and start them
  agx::Vector<MyThread *> threads;
 
  for (size_t i = 0; i < 4; i++)
  {
    MyThread *t = new MyThread(sim);
 
    // #3 Start the thread, it will wait for us to call release before it actually does anything
    t->start();
 
    threads.push_back(t);
  }
 
  // Release and wait for each thread.
  // We will (and can) only modify an agxSDK::Simulation in one thread at the same time
  for (size_t i = 0; i < threads.size(); i++)
  {
    MyThread *t = threads[i];
 
    // #4 Now release the thread and let it do its work
    t->release();
 
    // #5 Wait for it to finish
    t->join();
  }
 
  // Delete all the threads
  for (size_t i = 0; i < threads.size(); i++)
  {
    MyThread *t = threads[i];
    delete t;
  }
}
 
/*
threadTutorial2() show how to create objects (in this case load mesh files and create geometries/bodies) in
a separate thread, while a simulation is running. Then when the loading is done, the bodies are added to
the simulation.
 
This can be useful when you do not want to interrupt a running simulation by loading/creating objects.
Instead the bodies are added to the simulation at a later stage with minimal overhead.
Loading these 20 objects takes about 80ms (which would have a huge impact on the running simulation)
in comparison adding them takes about 0.3ms which should not be noticeable.
*/
 
 
// Function that load a named mesh and create a rigid body
agx::RigidBody *createBody(const agx::String& filename, size_t& numVertices)
{
  agx::MaterialRef material = new agx::Material("someMaterial");
 
  // Create the trimesh shape from a named obj-file
  agxCollide::TrimeshRef trimeshShape = agxUtil::TrimeshReaderWriter::createTrimesh(filename);
  numVertices = trimeshShape->getNumVertices();
 
  if (!trimeshShape)
  {
    LOGGER_ERROR() << "Error while loading mesh file: " << filename << std::endl << LOGGER_END();
    return nullptr;
  }
 
  // Create the dynamic rigid body (carries mass properties and interface to dynamic properties)
  agx::RigidBody *dynamicRB = new agx::RigidBody();
 
  // Create a new geometry.
  agxCollide::GeometryRef dynamicRBGeometry = new agxCollide::Geometry();
 
  // Disable collision for this geometry, we are not interested in any simulation results here.
  dynamicRBGeometry->setEnableCollisions(false);
  dynamicRBGeometry->setMaterial(material);
 
  // Add the shape to the geometry.
  dynamicRBGeometry->add(trimeshShape);
 
 
  // Add the geometry to the body (although we have disabled collisions because we do not want any overlaps here).
  dynamicRB->add(dynamicRBGeometry);
 
  return dynamicRB;
}
 
// This thread will do the work of loading mesh from file and creating the bodies
class MyLoadingThread : public agx::BasicThread
{
public:
 
  MyLoadingThread(int numObjects, const std::string& filename) : m_numObjects(numObjects), m_filename(filename)
  {
  }
 
  // Will be called when the main thread calls start()
  void run();
 
  virtual ~MyLoadingThread() {}
 
  // Some public attributes for accessing data later
  int m_numObjects;
  std::string m_filename;
  size_t m_numVertices;
  agx::Real m_time;
 
  agx::RigidBodyRefVector m_bodies;
};
 
 
// This method will be called in the new thread after
// someone called start()
void MyLoadingThread::run()
{
  // We need to do this if we want to use AGX API within this thread.
  // If you forget this call you might get exceptions
  agx::Thread::registerAsAgxThread();
 
  // Measure the time it takes to create a bunch of objects
  agx::Timer timer(true);
  for (auto i = 0; i < m_numObjects; i++)
  {
 
    // Here we do the bulk of the work by loading files and creating bodies.
    agx::RigidBodyRef body = createBody(m_filename, m_numVertices);
 
    // We store the created bodies in a vector for later inclusion into a Simulation
    // We cannot add these bodies to the simulation now as that would require a exclusive access to
    // to the simulation. Instead, adding the bodies will be done in the main thread at a later time.
    body->setPosition(i * 3, 0, 0);
    m_bodies.push_back(body);
  }
  timer.stop();
  m_time = timer.getTime();
 
  // This will release some things and will tell AGX that this thread is no longer an AGX thread
  agx::Thread::unregisterAsAgxThread();
}
 
 
 
// ===================================================
// main function for threadTutorial2
// ===================================================
void threadTutorial2()
{
  std::cerr << std::endl << "Running " << __FUNCTION__ << std::endl << std::endl;
 
  // Create a simulation
  agxSDK::SimulationRef simulation = new agxSDK::Simulation();
 
  // Create our loading-thread
  MyLoadingThread myThread(20, "models/torusRound.obj");
 
  // Now start the thread and let it do its work
  myThread.start();
 
  // Meanwhile we continue to simulate
  std::cerr << "Simulating while creating";
  agx::Timer simTimer(true);
  while (simTimer.getTime() < 2000)
  {
    int i = (int)simTimer.getTime();
    simulation->stepForward();
    if ((i % 700) == 0)
      std::cerr << ".";
  }
  std::cerr << std::endl << "Done Simulating " << std::endl;
 
  myThread.join();
 
  std::cerr << "Creating " << myThread.m_bodies.size() << " objects (each with " << myThread.m_numVertices << " num vertices) took " << myThread.m_time << "ms" << std::endl;
 
  agx::Timer timer(true);
 
 
  // Now when the thread is done, we will have our vector of created rigid bodies
  // Here we can add these bodies to the simulation without interrupting the simulation with the
  // loading times for the mesh models...
  for (auto b : myThread.m_bodies)
    simulation->add(b);
  timer.stop();
  std::cerr << "Adding " << myThread.m_bodies.size() << " bodies to simulation took " << timer.getTime() << "ms" << std::endl;
 
  // Now we can continue to do simulation->stepForward() with the new bodies
  std::cerr << "Continue to simulate with the added bodies" << std::endl;
 
  simTimer.reset(true);
  while (simTimer.getTime() < 0.5)
  {
    int i = (int)simTimer.getTime();
    simulation->stepForward();
    if ((i % 700) == 0)
      std::cerr << ".";
  }
  std::cerr << std::endl << "Done Simulating " << std::endl;
}
 
 
int main(int /*argc*/, char** /*argv*/)
{
 
  // Initialize AGX Dynamics
  // When the scope of agxInit ends, AGX Dynamics will be de-initialized
  agx::AutoInit agxInit;
 
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial Threads\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
    threadTutorial1();
 
    threadTutorial2();
  }
  catch (std::exception& e) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_tire.cpp

/*
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.
*/
 
/*
 
Tutorial for using the agxModel::Tire to simulate tires on a vehicle.
 
*/
 
#include <agxOSG/ExampleApplication.h>
 
#include <agxModel/Tire.h>
#include <agxModel/OneBodyTire.h>
#include <agxModel/TwoBodyTire.h>
#include <agxUtil/agxUtil.h>
#include <agxOSG/utils.h>
 
#include <agxSDK/StepEventListener.h>
#include <agxSDK/Simulation.h>
 
#include <agxCollide/Box.h>
#include <agxCollide/Cylinder.h>
#include <agxCollide/HeightField.h>
#include <agxCollide/Capsule.h>
 
#include <agxRender/RenderManager.h>
#include <agx/version.h>
 
 
osg::Group* buildTutorialOneBodyTire( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  const agx::Real groundHalfExtent(15);
 
  // Define materials and contact materials (since only they define the behavior for OneBodyTire)
  agx::MaterialRef groundMaterial = new agx::Material("groundMaterial");
  agx::MaterialRef tireMaterial = new agx::Material("tireMaterial");
  agx::ContactMaterialRef tireOnGround = new agx::ContactMaterial(tireMaterial, groundMaterial);
  simulation->add(tireOnGround); // Has to be added to simulation; will include materials.
  // Let's set a higher friction coefficient in the primary direction for testing.
  tireOnGround->setFrictionCoefficient(0.75, agx::ContactMaterial::PRIMARY_DIRECTION);
  tireOnGround->setFrictionCoefficient(0.5, agx::ContactMaterial::SECONDARY_DIRECTION);
 
  // Tire elasticity can only be modeled using contact materials for OneBodyTire-model.
  tireOnGround->setYoungsModulus(1e8);
 
  // Add a ground to drive on.
  // Straight part (since it should not move, a geometry without a rigid body is enough).
  agxCollide::GeometryRef groundStraight = new agxCollide::Geometry(new agxCollide::Box(groundHalfExtent, groundHalfExtent, 0.5));
  simulation->add(groundStraight);
  groundStraight->setMaterial(groundMaterial);
  groundStraight->setPosition(groundHalfExtent, 0, -0.5);
  agxOSG::createVisual(groundStraight, root);
 
  // Rotated part (since it should not move, a geometry without a rigid body is enough).
  agxCollide::GeometryRef groundRotated = new agxCollide::Geometry(new agxCollide::Box(groundHalfExtent, groundHalfExtent, 0.5));
  simulation->add(groundRotated);
  groundRotated->setMaterial(groundMaterial);
  groundRotated->setTransform(agx::AffineMatrix4x4::rotate(0.2, agx::Vec3(0, 1, 0)) *
    agx::AffineMatrix4x4::translate(-0.9 * groundHalfExtent, 0, 2.5));
  agxOSG::createVisual(groundRotated, root);
 
  // Now a simple "car" (a box with 4 wheels).
  // We define the car in a coordinate system where it "move in the x-coordinate."
  const agx::Vec3 chassisHalfExtents(1.2, 0.8, 0.2);
 
  agxSDK::AssemblyRef car = new agxSDK::Assembly();
 
  // Chassis.
  agx::RigidBodyRef chassis = new agx::RigidBody(new agxCollide::Geometry(new agxCollide::Box(chassisHalfExtents)));
  simulation->add(chassis);
  agxOSG::createVisual(chassis, root);
  car->add(chassis);
 
  // 4 Wheels
  const agx::Real radius(0.5);
  for (int i = 0; i < 4; ++i) {
    agx::Vec3 pos(i < 2 ? -1 : 1, (i % 2 == 1) ? -1 : 1, 0);
    // One body for the wheel itself (as specified by OneBodyTire).
    agx::RigidBodyRef wheel = new agx::RigidBody(new agxCollide::Geometry(new agxCollide::Cylinder(radius, 0.2)));
    simulation->add(wheel);
    agxUtil::setBodyMaterial(wheel, tireMaterial);
    wheel->setPosition(pos);
    agxOSG::createVisual(wheel, root);
    agxModel::OneBodyTireRef tire = new agxModel::OneBodyTire(wheel, radius);
    simulation->add(tire);
    car->add(wheel);
    car->add(tire);
    // The transformation for the hinge is important:
    // First, we rotate the axis from z-coordinate to y.
    // Then, move the attachment point to the position of the wheel.
    // We let the frame of the wheel to be auto-computed (by not specifying it).
    agx::HingeRef hinge = new agx::Hinge(chassis,
      new agx::Frame(agx::AffineMatrix4x4::rotate(agx::PI_2, agx::Vec3(1, 0, 0)) * agx::AffineMatrix4x4::translate(pos)),
      tire->getRigidBody());
 
    simulation->add(hinge);
    agxUtil::setEnableCollisions(wheel, chassis, false);
  }
 
  // Place car above rotated ground. We also turn the car to "move in the y-coordinate".
  car->setTransform(agx::AffineMatrix4x4::rotate(agx::PI_2, agx::Vec3(0, 0, 1)) *
    groundRotated->getTransform() * agx::AffineMatrix4x4::translate(0, 0, 1));
 
  return root;
}
 
osg::Group* buildTutorialTwoBodyTire(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/)
{
  osg::Group* root = new osg::Group();
 
  const agx::Real groundHalfExtent(15);
 
  // Define materials and contact materials (since only they define the behavior for TwoBodyTire)
  agx::MaterialRef groundMaterial = new agx::Material("groundMaterial");
  agx::MaterialRef tireMaterial = new agx::Material("tireMaterial");
  agx::ContactMaterialRef tireOnGround = new agx::ContactMaterial(tireMaterial, groundMaterial);
  simulation->add(tireOnGround); // Has to be added to simulation; will include materials.
                                 // Let's set a higher friction coefficient in the primary direction for testing.
  tireOnGround->setFrictionCoefficient(0.75, agx::ContactMaterial::PRIMARY_DIRECTION);
  tireOnGround->setFrictionCoefficient(0.5, agx::ContactMaterial::SECONDARY_DIRECTION);
 
  // Tire elasticity can only be modeled using contact materials for TwoBodyTire-model.
  tireOnGround->setYoungsModulus(1e8);
 
  // Add a ground to drive on.
  // Straight part (since it should not move, a geometry without a rigid body is enough).
  agxCollide::GeometryRef groundStraight = new agxCollide::Geometry(new agxCollide::Box(groundHalfExtent, groundHalfExtent, 0.5));
  simulation->add(groundStraight);
  groundStraight->setMaterial(groundMaterial);
  groundStraight->setPosition(groundHalfExtent, 0, -0.5);
  agxOSG::createVisual(groundStraight, root);
 
  // Rotated part (since it should not move, a geometry without a rigid body is enough).
  agxCollide::GeometryRef groundRotated = new agxCollide::Geometry(new agxCollide::Box(groundHalfExtent, groundHalfExtent, 0.5));
  simulation->add(groundRotated);
  groundRotated->setMaterial(groundMaterial);
  groundRotated->setTransform(agx::AffineMatrix4x4::rotate(0.2, agx::Vec3(0, 1, 0)) *
    agx::AffineMatrix4x4::translate(-0.9 * groundHalfExtent, 0, 2.5));
  agxOSG::createVisual(groundRotated, root);
 
  // Now a simple "car" (a box with 4 wheels).
  // We define the car in a coordinate system where it "move in the x-coordinate."
  const agx::Vec3 chassisHalfExtents(1.2, 0.8, 0.2);
 
  agxSDK::AssemblyRef car = new agxSDK::Assembly();
 
  // Chassis.
  agx::RigidBodyRef chassis = new agx::RigidBody(new agxCollide::Geometry(new agxCollide::Box(chassisHalfExtents)));
  simulation->add(chassis);
  agxOSG::createVisual(chassis, root);
  car->add(chassis);
 
  // 4 Wheels
  const agx::Real radius(0.5);
  const agx::Real hubRadius(0.25);
  for (int i = 0; i < 4; ++i) {
    agx::Vec3 pos(i < 2 ? -1 : 1, (i % 2 == 1) ? -1 : 1, 0);
    // Two bodies for the wheel itself (as specified by TwoBodyTire) - one hub, one tire.
    agx::RigidBodyRef hubRigidBody = new agx::RigidBody(new agxCollide::Geometry(new agxCollide::Cylinder(hubRadius, 0.2)));
    simulation->add(hubRigidBody);
    agxUtil::setBodyMaterial(hubRigidBody, tireMaterial);
    hubRigidBody->setPosition(pos);
    agxOSG::createVisual(hubRigidBody, root);
    car->add(hubRigidBody);
 
    agx::RigidBodyRef tireRigidBody = new agx::RigidBody(new agxCollide::Geometry(new agxCollide::Cylinder(radius, 0.2)));
    simulation->add(tireRigidBody);
    agxUtil::setBodyMaterial(tireRigidBody, tireMaterial);
    tireRigidBody->setPosition(pos);
    agxOSG::createVisual(tireRigidBody, root);
    car->add(tireRigidBody);
 
    agxModel::TwoBodyTireRef tire = new agxModel::TwoBodyTire(tireRigidBody, radius, hubRigidBody, hubRadius);
    simulation->add(tire);
 
    car->add(tire);
    // The transformation for the hinge is important:
    // First, we rotate the axis from z-coordinate to y.
    // Then, move the attachment point to the position of the tireRigidBody.
    // We let the frame of the tireRigidBody to be auto-computed (by not specifying it).
    agx::HingeRef hinge = new agx::Hinge(chassis,
      new agx::Frame(agx::AffineMatrix4x4::rotate(agx::PI_2, agx::Vec3(1, 0, 0)) * agx::AffineMatrix4x4::translate(pos)),
      tire->getHubRigidBody());
 
    hinge->setSolveType(agx::Constraint::DIRECT_AND_ITERATIVE);
 
    simulation->add(hinge);
    agxUtil::setEnableCollisions(tireRigidBody, chassis, false);
    agxUtil::setEnableCollisions(hubRigidBody, chassis, false);
    // TwoBodyTire will set the collisions between tireRigidBody and hubRigidBody off by itself.
  }
 
  // Place car above rotated ground. We also turn the car to "move in the y-coordinate".
  car->setTransform(agx::AffineMatrix4x4::rotate(agx::PI_2, agx::Vec3(0, 0, 1)) *
    groundRotated->getTransform() * agx::AffineMatrix4x4::translate(0, 0, 1));
 
  return root;
}
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout <<
                "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
                "\tTutorial Tire\n" <<
    "\t--------------------------------\n\n" << std::endl;
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorialOneBodyTire, '1' );
    application->addScene( buildTutorialTwoBodyTire, '2' );
 
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_trackedVehicle.cpp

/*
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.
*/
 
/*
 
Demonstration of the agxModel::Tracks for simulating a tracked vehicle.
 
*/
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/utils.h>
 
#include <agxVehicle/Track.h>
 
#include <agxUtil/agxUtil.h>
#include <agxUtil/StepEventCallback.h>
#include <agxUtil/HeightFieldGenerator.h>
 
#include <agxPlot/DebugPlot.h>
 
#include <agxRender/RenderSingleton.h>
 
#include <agx/version.h>
#include <agx/OrientedFrictionModels.h>
 
#include <agxCollide/Box.h>
#include <agxCollide/Cylinder.h>
#include <agxCollide/Capsule.h>
#include <agxCollide/HeightField.h>
 
#include <agxStream/OutputArchive.h>
#include <agxStream/InputArchive.h>
 
 
namespace utils
{
  AGX_DECLARE_POINTER_TYPES( TrackedVehicle );
 
  class TrackedVehicle : public agxSDK::Assembly
  {
    public:
      TrackedVehicle();
 
      void setChassis( agx::RigidBodyRef chassis );
 
      agx::RigidBodyRef getChassis() const;
 
      void setRightTrack( agxVehicle::TrackRef rightTrack );
 
      agxVehicle::TrackRef getRightTrack() const;
 
      void setLeftTrack( agxVehicle::TrackRef leftTrack );
 
      agxVehicle::TrackRef getLeftTrack() const;
 
 
    private:
      agx::RigidBodyRef m_chassis;
      agxVehicle::TrackRef m_rightTrack;
      agxVehicle::TrackRef m_leftTrack;
  };
 
  TrackedVehicle::TrackedVehicle()
  {
  }
 
  void TrackedVehicle::setChassis( agx::RigidBodyRef chassis )
  {
    if ( m_chassis != nullptr ) {
      LOGGER_WARNING() << "Chassis assigned multiple times. Removing old chassis." << LOGGER_ENDL();
      agxSDK::Assembly::remove( m_chassis );
      m_chassis = nullptr;
    }
 
    m_chassis = chassis;
    agxSDK::Assembly::add( m_chassis );
  }
 
  agx::RigidBodyRef TrackedVehicle::getChassis() const
  {
    return m_chassis;
  }
 
  void TrackedVehicle::setRightTrack( agxVehicle::TrackRef rightTrack )
  {
    if ( m_rightTrack != nullptr ) {
      LOGGER_WARNING() << "Right track assigned multiple times. Removing old right track." << LOGGER_ENDL();
      agxSDK::Assembly::remove( m_rightTrack );
      m_rightTrack = nullptr;
    }
 
    m_rightTrack = rightTrack;
    agxSDK::Assembly::add( m_rightTrack );
  }
 
  agxVehicle::TrackRef TrackedVehicle::getRightTrack() const
  {
    return m_rightTrack;
  }
 
  void TrackedVehicle::setLeftTrack( agxVehicle::TrackRef leftTrack )
  {
    if ( m_leftTrack != nullptr ) {
      LOGGER_WARNING() << "Left track assigned multiple times. Removing old Left track." << LOGGER_ENDL();
      agxSDK::Assembly::remove( m_leftTrack );
      m_leftTrack = nullptr;
    }
 
    m_leftTrack = leftTrack;
    agxSDK::Assembly::add( m_leftTrack );
  }
 
  agxVehicle::TrackRef TrackedVehicle::getLeftTrack() const
  {
    return m_leftTrack;
  }
 
 
 
  AGXSTREAM_INSTANTIATE_STORAGE( TrackedVehicle )
 
  AGX_DECLARE_POINTER_TYPES( TrackedVehicleKeyboardController );
  using Key = agxSDK::GuiEventListener::KeySymbol;
 
  class TrackedVehicleKeyboardController : public agxSDK::GuiEventListener
  {
    public:
      enum Action
      {
        FORWARD, 
        REVERSE, 
        RIGHT,   
        LEFT     
      };
 
    public:
      TrackedVehicleKeyboardController( TrackedVehicle* trackedVehicle );
 
      void bind( Action action,
                 Key key,
                 agx::Real targetSpeed,
                 agx::Real accelerationTime,
                 agx::Real deaccelerationTime );
 
    public:
      virtual bool keyboard( int key, unsigned int modKey, float x, float y, bool down ) override;
 
      virtual void update( float x, float y ) override;
 
    protected:
 
      ~TrackedVehicleKeyboardController();
 
    private:
      struct KeyData
      {
        agx::Real target;
        agx::Real eventStart;
        agx::Real eventTime;
        agx::Real accelerationTime;
        agx::Real deaccelerationTime;
        Action action;
        agx::Bool isDown;
      };
 
      using KeyDataTable = agx::HashVector<int, KeyData>;
 
    private:
      TrackedVehicleObserver m_trackedVehicle;
      KeyDataTable m_keyDataTable;
  };
 
  TrackedVehicleKeyboardController::TrackedVehicleKeyboardController( TrackedVehicle* trackedVehicle )
    : m_trackedVehicle( trackedVehicle )
  {
 
    auto hr = m_trackedVehicle->getRightTrack()->findReferenceWheel()->getConstraint()->as<agx::Hinge>();
    auto hl = m_trackedVehicle->getLeftTrack()->findReferenceWheel()->getConstraint()->as<agx::Hinge>();
    hr->getMotor1D()->setEnable( true );
    hr->getMotor1D()->setLockedAtZeroSpeed( true );
    hl->getMotor1D()->setEnable( true );
    hl->getMotor1D()->setLockedAtZeroSpeed( true );
  }
 
  TrackedVehicleKeyboardController::~TrackedVehicleKeyboardController()
  {
  }
 
  void TrackedVehicleKeyboardController::bind( TrackedVehicleKeyboardController::Action action,
                                               Key key,
                                               agx::Real targetSpeed,
                                               agx::Real accelerationTime,
                                               agx::Real deaccelerationTime )
  {
    m_keyDataTable.insert( key, KeyData
                                {
                                  targetSpeed,
                                  agx::Real( -1 ),
                                  agx::Real( 0 ),
                                  accelerationTime,
                                  deaccelerationTime,
                                  action,
                                  false
                                } );
  }
 
  bool TrackedVehicleKeyboardController::keyboard( int key, unsigned int, float, float, bool down )
  {
    if ( m_trackedVehicle == nullptr )
      return false;
 
    auto dataIt = m_keyDataTable.find( key );
    if ( dataIt == m_keyDataTable.end() )
      return false;
 
    auto& data = dataIt->second;
    if ( down && !data.isDown ) {
      data.eventStart = getSimulation()->getTimeStamp();
      data.eventTime   = agx::Real( 0 );
    }
    else if ( !down ) {
      data.eventStart = getSimulation()->getTimeStamp();
      data.eventTime  = agx::Real( 0 );
    }
 
    data.isDown = down;
 
    return true;
  }
 
  void TrackedVehicleKeyboardController::update( float, float )
  {
    if ( m_trackedVehicle == nullptr ) {
      getSimulation()->remove( this );
      return;
    }
 
    agx::Real speedRight = agx::Real( 0 );
    agx::Real speedLeft  = agx::Real( 0 );
    for ( auto& kv : m_keyDataTable ) {
      auto& data = kv.second;
 
      data.eventTime = agx::clamp( getSimulation()->getTimeStamp() - data.eventStart, agx::Real( 0 ), data.accelerationTime );
 
      agx::Real speed = agx::Real( 0 );
      if ( data.isDown )
        speed = agx::lerp( agx::Real( 0 ), data.target, data.eventTime / data.accelerationTime );
      else
        speed = agx::lerp( data.target, agx::Real( 0 ), data.eventTime / data.deaccelerationTime );
 
      if ( data.action == Action::FORWARD ) {
        speedRight += speed;
        speedLeft  += speed;
      }
      else if ( data.action == Action::REVERSE ) {
        speedRight -= speed;
        speedLeft  -= speed;
      }
      else if ( data.action == Action::LEFT ) {
        speedRight += speed;
        speedLeft  -= speed;
      }
      else if ( data.action == Action::RIGHT ) {
        speedRight -= speed;
        speedLeft  += speed;
      }
    }
 
    m_trackedVehicle->getRightTrack()->findReferenceWheel()->getConstraint()->as<agx::Hinge>()->getMotor1D()->setSpeed( speedRight );
    m_trackedVehicle->getLeftTrack()->findReferenceWheel()->getConstraint()->as<agx::Hinge>()->getMotor1D()->setSpeed( speedLeft );
  }
}
 
osg::Group* buildTutorial1( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  auto root = new osg::Group();
 
  // Create ground geometry height field given gray scale image.
  const auto heightFieldImage    = "textures/terrain_test/terrain_height.png";
  const auto heightFieldTexture  = "textures/terrain_test/terrain_detail.png";
  const auto heightFieldSize     = agx::Vec2( 60.0, 60.0 );
  const auto heightFieldMinMax   = agx::Vec2( -2.0, 2.0 );
  agxCollide::GeometryRef ground = new agxCollide::Geometry( agxCollide::HeightField::createFromFile( heightFieldImage,
                                                                                                      heightFieldSize.x(),
                                                                                                      heightFieldSize.y(),
                                                                                                      heightFieldMinMax.x(),
                                                                                                      heightFieldMinMax.y() ) );
  simulation->add( ground );
 
  auto groundNode = agxOSG::createVisual( ground, root );
  agxOSG::setTexture( groundNode, heightFieldTexture, true );
 
  // Create the tracked vehicle instance - simply an assembly with some convenience methods.
  utils::TrackedVehicleRef trackedVehicle = new utils::TrackedVehicle();
 
  // Create chassis - center of chassis is the model center.
  const agx::Vec3 chassisHalfExtents  = agx::Vec3( 4, 2, 0.8 );
  const agxRender::Color chassisColor = agxRender::Color::Green();
  agx::RigidBodyRef chassis           = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( chassisHalfExtents ) ) );
  trackedVehicle->setChassis( chassis );
 
  agxOSG::setDiffuseColor( agxOSG::createVisual( chassis, root ), chassisColor );
 
  // Track specific parameters.
  const agx::UInt numberOfNodes       = 100;     // Total number of nodes in the track.
  const agx::Real nodeThickness       = 0.075;   // Thickness of each node in the track.
  const agx::Real nodeWidth           = 0.6;     // Width of each node in the track.
  const agx::Real nodeDistanceTension = 5.0E-3;  // The calculated node length is close to ideal, meaning close to zero tension
                                                 // in the tracks if they were simulated without gravity. This distance is an offset
                                                 // how much closer each node will be to each other, resulting in a given initial tension.
  const agx::Real sprocketRadius      = 0.45;    // Radius of the sprocket wheel.
  const agx::Real sprocketHeight      = 0.35;    // Height of the sprocket wheel.
  const agx::Real idlerRadius         = 0.40;    // Radius of the idler wheel.
  const agx::Real idlerHeight         = 0.35;    // Height of the idler wheel.
  const agx::Real rollerRadius        = 0.35;    // Radius of the ground/roller wheel(s).
  const agx::Real rollerHeight        = 0.35;    // Height of the ground/roller wheel(s).
  const agx::Real rollerOffset        = 1.0 *    // Offset between the rollers.
                                        rollerRadius;
  const agx::UInt numRollers          = 6;       // Number of rollers/ground wheels.
 
  // Track wheel rigid body create function.
  const auto createWheelRigidBody = []( agx::Real radius, agx::Real height, const agx::Vec3& position ) -> agx::RigidBodyRef
  {
    agx::RigidBodyRef rb = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Cylinder( radius, height ) ) );
    rb->setPosition( position );
    return rb;
  };
 
  // Track wheel create function given wheel model (sprocket, idler or roller), radius, height and position.
  const auto createTrackWheel = [createWheelRigidBody]( agxVehicle::TrackWheel::Model model,
                                                        agx::Real radius,
                                                        agx::Real height,
                                                        const agx::Vec3& position ) -> agxVehicle::TrackWheelRef
  {
    return new agxVehicle::TrackWheel( model,
                                       radius,
                                       createWheelRigidBody( radius, height, position ),
                                       agx::AffineMatrix4x4::translate( 0, 0, 0 ) );
  };
 
  // Right and left track are identical. This function creates a track with wheels etc.
  const auto createTrack = [&]() -> agxVehicle::TrackRef
  {
    // Create the track instance given number of nodes, node width and thickness and initial tension distance.
    // By default each node will receive a box shape with given width and thickness. The length of the box
    // is calculated given number of nodes, position of the wheels etc.
    agxVehicle::TrackRef track = new agxVehicle::Track( numberOfNodes, nodeWidth, nodeThickness, nodeDistanceTension );
 
    // Add the sprocket - front upper wheel.
    track->add( createTrackWheel( agxVehicle::TrackWheel::Model::SPROCKET,
                                  sprocketRadius,
                                  sprocketHeight,
                                  agx::Vec3( chassisHalfExtents.x() - 6.5 * sprocketRadius,
                                             0,
                                             1.5 * sprocketRadius ) ) );
    // Add front idler.
    track->add( createTrackWheel( agxVehicle::TrackWheel::Model::IDLER,
                                  idlerRadius,
                                  idlerHeight,
                                  agx::Vec3( chassisHalfExtents.x() - 0.9 * idlerRadius,
                                             0,
                                             -0.5 * chassisHalfExtents.z() ) ) );
 
    // Add the ground wheels/rollers.
    agx::Vec3 rollerPosition = track->getWheels().back()->getRigidBody()->getLocalPosition() -
                               agx::Vec3( idlerRadius + rollerRadius + rollerOffset,
                                          0,
                                          2.0 * rollerRadius );
    for ( agx::UInt i = 0; i < numRollers; ++i ) {
      auto roller = createTrackWheel( agxVehicle::TrackWheel::Model::ROLLER,
                                      rollerRadius,
                                      rollerHeight,
                                      rollerPosition );
 
      // First and last roller should split any merged segment along the track - since
      // the track normally curves to the idlers from there (on this vehicle at least).
      if ( i == 0 || i == numRollers - 1 )
        roller->setEnableProperty( agxVehicle::TrackWheel::SPLIT_SEGMENTS, true );
 
      track->add( roller );
 
      rollerPosition.x() -= ( 2.0 * rollerRadius + rollerOffset );
    }
 
    // Add the rear idler - position given the last roller.
    track->add( createTrackWheel( agxVehicle::TrackWheel::Model::IDLER,
                                  idlerRadius,
                                  idlerHeight,
                                  rollerPosition + agx::Vec3( rollerRadius - idlerRadius,
                                                              0,
                                                              2.0 * rollerRadius ) ) );
 
    return track;
  };
 
  // Create and add the tracks to the tracked vehicle.
  trackedVehicle->setRightTrack( createTrack() );
  trackedVehicle->setLeftTrack( createTrack() );
 
  // First, position the tracks slightly outside the chassis given the track width.
  trackedVehicle->getRightTrack()->setPosition( 0, -chassisHalfExtents.y() - 0.6 * nodeWidth, 0 );
  trackedVehicle->getLeftTrack()->setPosition(  0,  chassisHalfExtents.y() + 0.6 * nodeWidth, 0 );
 
  // Disable collisions between the tracks and the chassis.
  trackedVehicle->getRightTrack()->setEnableCollisions( trackedVehicle->getChassis(), false );
  trackedVehicle->getLeftTrack()->setEnableCollisions( trackedVehicle->getChassis(), false );
 
  // Create the hinges between chassis and wheels.
  // Note: The tracks has to be at the correct position/orientation relative to the chassis
  //       when we create these constraints.
  const auto createWheelHinges = []( const agxVehicle::Track* track, agx::RigidBody* chassis, agx::Real hingeWheelOffset )
  {
    for ( auto wheel : track->getWheels() ) {
      auto hinge = wheel->attachConstraint<agx::Hinge>( "hinge", chassis, hingeWheelOffset );
 
      // Add some damping behavior to the ground rollers.
      const auto rollerHingeCompliance = 5.0E-7;
      const auto rollerHingeDamping    = 0.15;
      if ( wheel->getModel() == agxVehicle::TrackWheel::ROLLER ) {
        hinge->setCompliance( rollerHingeCompliance, agx::Hinge::TRANSLATIONAL_1 );
        hinge->setCompliance( rollerHingeCompliance, agx::Hinge::TRANSLATIONAL_2 );
 
        hinge->setDamping( rollerHingeDamping, agx::Hinge::TRANSLATIONAL_1 );
        hinge->setDamping( rollerHingeDamping, agx::Hinge::TRANSLATIONAL_2 );
      }
    }
  };
  createWheelHinges( trackedVehicle->getRightTrack(), trackedVehicle->getChassis(), 0.6 * nodeWidth );
  createWheelHinges( trackedVehicle->getLeftTrack(), trackedVehicle->getChassis(), -0.6 * nodeWidth );
 
  // Changing track properties such as compliance in the hinges between the track nodes
  // and friction coefficient in the hinges to stabilize the tracks when under heavy
  // load, e.g., when the vehicle is turning.
  const auto setTrackProperties = []( const agxVehicle::Track* track )
  {
    track->getProperties()->setHingeCompliance( 4.0E-10 );
    track->getProperties()->setStabilizingHingeFrictionParameter( 1.5 );
 
    // Enabling merging of nodes within the track to reduce the numerical complexity
    // and to heavily reduce contacts between the tracks and other geometries.
    track->getInternalMergeProperties()->setEnableMerge( true );
    track->getInternalMergeProperties()->setNumNodesPerMergeSegment( 4 );
    track->getInternalMergeProperties()->setLockToReachMergeConditionCompliance( 1.0E-8 );
    track->getInternalMergeProperties()->setContactReduction( agxVehicle::TrackInternalMergeProperties::ContactReduction::AGGRESSIVE );
  };
  setTrackProperties( trackedVehicle->getRightTrack() );
  setTrackProperties( trackedVehicle->getLeftTrack() );
 
  // Setting up materials.
  const agx::Real trackDensity                    = 2000.0;
  const agx::Vec2 trackGroundFrictionCoefficients = agx::Vec2( 1000.0, 10 );
  const agx::Vec2 trackGroundSurfaceViscosity     = agx::Vec2( 1.0E-7, 8.0E-6 );
  const agx::Real trackWheelFrictionCoefficient   = 1.0;
  const agx::Real trackWheelSurfaceViscosity      = 1.0E-5;
 
  agx::MaterialRef groundMaterial = new agx::Material( "groundMaterial" );
  agx::MaterialRef trackMaterial  = new agx::Material( "trackMaterial" );
  agx::MaterialRef wheelMaterial  = new agx::Material( "wheelMaterial" );
 
  trackMaterial->getBulkMaterial()->setDensity( trackDensity );
 
  ground->setMaterial( groundMaterial );
 
  trackedVehicle->getRightTrack()->setMaterial( trackMaterial );
  for ( const auto& wheel : trackedVehicle->getRightTrack()->getWheels() )
    agxUtil::setBodyMaterial( wheel->getRigidBody(), wheelMaterial );
 
  trackedVehicle->getLeftTrack()->setMaterial( trackMaterial );
  for ( const auto& wheel : trackedVehicle->getLeftTrack()->getWheels() )
    agxUtil::setBodyMaterial( wheel->getRigidBody(), wheelMaterial );
 
  // Contact material between track nodes and the wheels.
  auto trackWheelContactMaterial = simulation->getMaterialManager()->getOrCreateContactMaterial( trackMaterial, wheelMaterial );
  // Slightly simplified friction model between the wheels and the track
  // where we estimate the normal force constant @ ten times the mass of
  // the chassis. Solved direct.
  auto trackWheelFrictionModel = new agx::ConstantNormalForceOrientedBoxFrictionModel( 10.0 * trackedVehicle->getChassis()->getMassProperties()->getMass(),
                                                                                       trackedVehicle->getChassis()->getFrame(),
                                                                                       agx::Vec3::X_AXIS(),
                                                                                       agx::FrictionModel::DIRECT );
  trackWheelContactMaterial->setFrictionModel( trackWheelFrictionModel );
  // Let the friction coefficient be high and "ease up" the friction constraints
  // by using high surface viscosity. Friction properties doesn't affect the
  // sprockets or idlers since the nodes will merge to them, simulating infinite
  // stick friction against those geared wheel types.
  trackWheelContactMaterial->setFrictionCoefficient( trackWheelFrictionCoefficient );
  trackWheelContactMaterial->setSurfaceViscosity( trackWheelSurfaceViscosity );
  trackWheelContactMaterial->setRestitution( 0.0 );
 
  // Contact material between track nodes and the ground.
  auto trackGroundContactMaterial = simulation->getMaterialManager()->getOrCreateContactMaterial( trackMaterial, groundMaterial );
  // Similar friction model as between the tracks and the wheels but here, between
  // the tracks and ground it's important that the friction box is oriented with
  // the tracked vehicle. We want the traction along the tracks to be very high
  // and the transverse traction slightly lower to make it possible to turn smoothly.
  // This friction model will scale the maximum friction force with the contact point.
  auto trackGroundFrictionModel = new agx::ConstantNormalForceOrientedBoxFrictionModel( 10.0 * trackedVehicle->getChassis()->getMassProperties()->getMass(),
                                                                                        trackedVehicle->getChassis()->getFrame(),
                                                                                        agx::Vec3::X_AXIS(),
                                                                                        agx::FrictionModel::DIRECT,
                                                                                        true );
  trackGroundContactMaterial->setFrictionModel( trackGroundFrictionModel );
  trackGroundContactMaterial->setFrictionCoefficient( trackGroundFrictionCoefficients.x(), agx::ContactMaterial::PRIMARY_DIRECTION );
  trackGroundContactMaterial->setFrictionCoefficient( trackGroundFrictionCoefficients.y(), agx::ContactMaterial::SECONDARY_DIRECTION );
  trackGroundContactMaterial->setSurfaceViscosity( trackGroundSurfaceViscosity.x(), agx::ContactMaterial::PRIMARY_DIRECTION );
  trackGroundContactMaterial->setSurfaceViscosity( trackGroundSurfaceViscosity.y(), agx::ContactMaterial::SECONDARY_DIRECTION );
  trackGroundContactMaterial->setRestitution( 0.0 );
 
  // Position the tracked vehicle (tracks and chassis inherits these transformations).
  trackedVehicle->setPosition( 0, 0, 3 );
 
  // Finally add the tracked vehicle to the simulation. During this step
  // the tracks will be initialized with bodies, constraints etc.
  simulation->add( trackedVehicle );
 
  // Now that the track nodes has been generated we can create visuals for them.
  const auto trackColor   = agxRender::Color::LightGreen();
  const auto wheelColor   = agxRender::Color::LightSeaGreen();
  const auto createVisual = [&]( const agxVehicle::Track* track )
  {
    for ( const auto& wheel : track->getWheels() )
      agxOSG::setDiffuseColor( agxOSG::createVisual( wheel->getRigidBody(), root ), wheelColor );
    for ( const auto& node : track->nodes() )
      agxOSG::setDiffuseColor( agxOSG::createVisual( node->getRigidBody(), root ), trackColor );
  };
  createVisual( trackedVehicle->getRightTrack() );
  createVisual( trackedVehicle->getLeftTrack() );
 
  application->getSceneDecorator()->setBackgroundColor( agxRender::Color::SkyBlue(), agxRender::Color::DodgerBlue() );
 
  utils::TrackedVehicleKeyboardControllerRef keyControl = new utils::TrackedVehicleKeyboardController( trackedVehicle );
  keyControl->bind( utils::TrackedVehicleKeyboardController::Action::FORWARD, utils::Key::KEY_Up,    4.0, 0.5, 0.25 );
  keyControl->bind( utils::TrackedVehicleKeyboardController::Action::REVERSE, utils::Key::KEY_Down,  4.0, 0.5, 0.25 );
  keyControl->bind( utils::TrackedVehicleKeyboardController::Action::RIGHT,   utils::Key::KEY_Right, 2.5, 0.25, 0.15 );
  keyControl->bind( utils::TrackedVehicleKeyboardController::Action::LEFT,    utils::Key::KEY_Left,  2.5, 0.25, 0.15 );
  simulation->add( keyControl );
 
  return root;
}
 
osg::Group* buildTutorial2( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  auto root = new osg::Group();
 
  // Track wheel rigid body create function.
  const auto createWheelRigidBody = []( agx::Real radius, agx::Real height, const agx::Vec3& position ) -> agx::RigidBodyRef
  {
    agx::RigidBodyRef rb = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Cylinder( radius, height ) ) );
    rb->setPosition( position );
    return rb;
  };
 
  // Track wheel create function given wheel model (sprocket, idler or roller), radius, height and position.
  const auto createTrackWheel = [createWheelRigidBody]( agxVehicle::TrackWheel::Model model,
                                                        agx::Real radius,
                                                        agx::Real height,
                                                        const agx::Vec3& position ) -> agxVehicle::TrackWheelRef
  {
    return new agxVehicle::TrackWheel( model,
                                       radius,
                                       createWheelRigidBody( radius, height, position ),
                                       agx::AffineMatrix4x4::translate( 0, 0, 0 ) );
  };
 
  // Two, different thickness values along the track. Every forth node is thicker than the three before.
  const agx::Real defaultThickness = 0.05;
  const agx::Real thickerThickness = 0.09;
 
  agxVehicle::TrackRef track = new agxVehicle::Track( 120, 0.4, defaultThickness, 0.0 );
  track->add( createTrackWheel( agxVehicle::TrackWheel::Model::SPROCKET, 0.5, 0.3, agx::Vec3( 0, 0, 0 ) ) );
  track->add( createTrackWheel( agxVehicle::TrackWheel::Model::ROLLER, 0.6, 0.3, agx::Vec3( 2, 0, 0 ) ) );
  track->add( createTrackWheel( agxVehicle::TrackWheel::Model::ROLLER, 0.7, 0.3, agx::Vec3( 4, 0, 0 ) ) );
  track->add( createTrackWheel( agxVehicle::TrackWheel::Model::ROLLER, 0.6, 0.3, agx::Vec3( 6, 0, 0 ) ) );
  track->add( createTrackWheel( agxVehicle::TrackWheel::Model::IDLER, 0.4, 0.3, agx::Vec3( 8, 0, 0 ) ) );
 
  // Initialization callback when the segmentation of the track has been made.
  // When a callback is given to 'initialize' it's up to this callback to assign
  // geometries/shapes to the nodes. Note that the track has calculated a length
  // of the nodes that has to be taken into account.
  agx::UInt counter = 0;
  track->initialize( [&]( const agxVehicle::TrackNode& node )
  {
    agx::Real heightOffset = 0.0;
    agx::Real thickness = defaultThickness;
    // For every fourth node we add a thicker box.
    if ( ( counter++ % 4 ) == 0 ) {
      thickness = thickerThickness;
      heightOffset = -0.5 * ( thickerThickness - defaultThickness );
    }
    node.getRigidBody()->add( new agxCollide::Geometry( new agxCollide::Box( 0.5 * thickness,
                                                                             0.25,
                                                                             0.5 * node.getLength() ) ),
                              agx::AffineMatrix4x4::translate( heightOffset, 0, node.getHalfExtents().z() ) );
  } );
 
  for ( auto wheel : track->getWheels() ) {
    wheel->attachHinge( "hinge", nullptr, 0.0 );
    agxOSG::setDiffuseColor( agxOSG::createVisual( wheel->getRigidBody(), root ), agxRender::Color::DarkRed() );
  }
 
  for ( auto node : track->nodes() )
    agxOSG::setDiffuseColor( agxOSG::createVisual( node->getRigidBody(), root ), agxRender::Color::DeepSkyBlue() );
 
  track->findReferenceWheel()->getConstraint()->as<agx::Hinge>()->getMotor1D()->setEnable( true );
  track->findReferenceWheel()->getConstraint()->as<agx::Hinge>()->getMotor1D()->setSpeed( 1.5 );
 
  simulation->add( track );
 
  return root;
}
 
osg::Group* buildTutorial3( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  auto root = new osg::Group();
 
  const auto wheelRadius = 0.45;
  const auto wheelHeight = 0.35;
 
  agx::RigidBodyRef wheelRb = new agx::RigidBody( "wheelRb" );
  wheelRb->add( new agxCollide::Geometry( new agxCollide::Cylinder( wheelRadius, wheelHeight ) ) );
 
  agxVehicle::TrackWheelRef wheel = new agxVehicle::TrackWheel( agxVehicle::TrackWheel::SPROCKET,
                                                                wheelRadius,
                                                                wheelRb );
  auto hinge = wheel->attachHinge( "hinge", nullptr, 0.0 );
  hinge->getMotor1D()->setEnable( true );
  hinge->getMotor1D()->setSpeed( 1.0 );
  hinge->getMotor1D()->setForceRange( -1.0E2, 1.0E2 );
 
  simulation->add( wheelRb );
  simulation->add( hinge );
 
  agxOSG::createVisual( wheelRb, root );
 
  const auto debuRenderWheel = []( agxSDK::Simulation*, agxVehicle::TrackWheelRef wheel )
  {
    agxRender::debugRenderFrame( wheel->getTransform(), 0.25, agxRender::Color::Red() );
  };
  agxUtil::StepEventCallback::post( debuRenderWheel, simulation, wheel );
 
  debuRenderWheel( simulation, wheel );
 
  return root;
}
 
namespace utils
{
  class TrackPropertyKeyboardHandler : public agxSDK::GuiEventListener
  {
    public:
      using Base = agxSDK::GuiEventListener;
      using TracksContainer = agx::Vector<agxVehicle::TrackObserver>;
 
    public:
      TrackPropertyKeyboardHandler( TracksContainer tracks, agxOSG::ExampleApplication* application );
 
      virtual bool keyboard( int key, unsigned int modKey, float x, float y, bool down ) override;
      virtual void update( float x, float y ) override;
 
    protected:
      virtual ~TrackPropertyKeyboardHandler();
 
    private:
      using TrackCallback = std::function<void( agxVehicle::Track*, agx::Hinge*, agx::UInt )>;
 
    private:
      void traverse( TrackCallback callback ) const;
 
    private:
      TracksContainer m_tracks;
      agxOSG::ExampleApplication* m_application;
      agxUtil::ExponentialMovingAverageStatistic m_stepForwardTime;
  };
  using TrackPropertyKeyboardHandlerRef = agx::ref_ptr<TrackPropertyKeyboardHandler>;
 
  TrackPropertyKeyboardHandler::TrackPropertyKeyboardHandler( TracksContainer tracks, agxOSG::ExampleApplication* application )
    : Base( Base::ActivationMask( Base::KEYBOARD |
                                  Base::UPDATE ) ),
      m_tracks( tracks ),
      m_application( application ),
      m_stepForwardTime( 0.1, 0.0 )
  {
    agx::Statistics::instance()->setEnable( true );
  }
 
  TrackPropertyKeyboardHandler::~TrackPropertyKeyboardHandler()
  {
  }
 
  bool TrackPropertyKeyboardHandler::keyboard( int key, unsigned int, float, float, bool down )
  {
    if ( m_tracks.empty() || m_tracks.contains( nullptr ) ) {
      getSimulation()->remove( this );
      return false;
    }
 
    const auto keyNumNodesPerSegment = key >= Base::KEY_F1 && key <= Base::KEY_F9;
    const auto handled = keyNumNodesPerSegment;
 
    if ( keyNumNodesPerSegment ) {
      if ( !down ) {
        const agx::UInt numNodesPerSegment = agx::UInt( key + 1 ) - agx::UInt( Base::KEY_F1 );
        traverse( [numNodesPerSegment]( agxVehicle::Track* track, agx::Hinge*, agx::UInt )
        {
          track->getInternalMergeProperties()->setEnableMerge( numNodesPerSegment > 1 );
          track->getInternalMergeProperties()->setNumNodesPerMergeSegment( numNodesPerSegment );
        } );
      }
    }
 
    return handled;
  }
 
  void TrackPropertyKeyboardHandler::update( float, float )
  {
    if ( m_application == nullptr || m_tracks.empty() || m_tracks.contains( nullptr ) )
      return;
 
    const auto stepForwardTime = agx::Statistics::instance()->getTimingInfo( "Simulation", "Step forward time" ).current;
    if ( stepForwardTime > 0.0 )
      m_stepForwardTime.update( stepForwardTime );
 
    const auto refTrack = m_tracks.front();
    const auto mergeEnabled = refTrack->getInternalMergeProperties()->getEnableMerge();
    const auto numMergedNodes = refTrack->getInternalMergeProperties()->getNumNodesPerMergeSegment();
    m_application->getSceneDecorator()->setText( 0,
                                                 agx::String::format( "Step forward time:                            %.1f ms",
                                                                      m_stepForwardTime.get() ) );
    m_application->getSceneDecorator()->setText( 1,
                                                 agx::String::format( "Merge segments active:                        %s",
                                                                      mergeEnabled ? "true" : "false" ),
                                                 mergeEnabled ? agxRender::Color::LightGreen() : agxRender::Color::Red() );
    m_application->getSceneDecorator()->setText( 2,
                                                 agx::String::format( "Number of nodes per merge segment (F1 to F9): %d",
                                                                      numMergedNodes ) );
  }
 
  void TrackPropertyKeyboardHandler::traverse( TrackPropertyKeyboardHandler::TrackCallback callback ) const
  {
    agx::UInt index = 0u;
    for ( auto track : m_tracks ) {
      const auto refWheel = track->findReferenceWheel();
      if ( refWheel == nullptr || refWheel->getConstraint() == nullptr )
        continue;
 
      agx::Hinge* hinge = refWheel->getConstraint()->asSafe<agx::Hinge>();
      if ( hinge != nullptr )
        callback( track, hinge, index++ );
    }
  }
}
 
osg::Group* buildTutorial4( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  auto root = new osg::Group();
 
  agxCollide::GeometryRef ground = new agxCollide::Geometry( agxUtil::HeightFieldGenerator::createHeightFieldFromFile( "textures/terrain_test/terrain_height.png",
                                                                                                                       35,
                                                                                                                       35,
                                                                                                                       -1,
                                                                                                                       5 ) );
  ground->setRotation( agx::Quat::rotate( agx::PI, agx::Vec3::Z_AXIS() ) );
  ground->setPosition( 4, 7, -3 );
  agxOSG::setTexture( agxOSG::createVisual( ground, root ), "textures/terrain_test/terrain_detail.png" );
  simulation->add( ground );
 
  // Creates conveyor given roller radius, conveyor width and thickness,
  // number of rollers and number of nodes in the conveyor.
  const auto createConveyor = []( agx::Real rollerRadius,
                                  agx::Real width,
                                  agx::Real thickness,
                                  agx::UInt numRollers,
                                  agx::UInt numNodes,
                                  agx::Real rollerOffset ) -> agxVehicle::TrackRef
  {
    agxVehicle::TrackRef conveyor = new agxVehicle::Track( numNodes, width, thickness, 1.0E-4 );
    agx::Vec3 rollerPosition      = agx::Vec3( -agx::Real( numRollers - 1 ) * rollerRadius - 0.5 * agx::Real(numRollers) * rollerOffset,
                                               0,
                                               0 );
    for ( agx::UInt i = 0; i < numRollers; ++i ) {
      agx::RigidBodyRef rollerRb = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Capsule( rollerRadius,
                                                                                                          width + rollerRadius ) ) );
      rollerRb->setPosition( rollerPosition );
 
      agxVehicle::TrackWheelRef roller = new agxVehicle::TrackWheel( i == 0 ? agxVehicle::TrackWheel::IDLER :
                                                                     i == numRollers - 1 ?
                                                                       agxVehicle::TrackWheel::SPROCKET :
                                                                       agxVehicle::TrackWheel::ROLLER,
                                                                     rollerRadius,
                                                                     rollerRb );
      conveyor->add( roller );
 
      rollerPosition += ( 2.0 * rollerRadius + rollerOffset ) * agx::Vec3::X_AXIS();
    }
 
    return conveyor;
  };
 
  // Creates visual representation of the conveyor given roller
  // and track colors.
  const auto createConveyorVisual = [root,
                                     application]( agxVehicle::Track& conveyor,
                                                   const agxRender::Color& rollerColor,
                                                   const agxRender::Color& trackColor )
  {
    for ( auto wheel : conveyor.getWheels() )
      agxOSG::setDiffuseColor( agxOSG::createVisual( wheel->getRigidBody(), root, 1.0 ), rollerColor );
 
    for ( auto node : conveyor.nodes() )
      agxOSG::setDiffuseColor( agxOSG::createVisual( node->getRigidBody(),
                                                     (osg::Group*)application->getSceneDecorator()->getNonShadowedScene() ), trackColor );
  };
 
  // Attaches the conveyor rollers to (optional) parent rigid body.
  const auto attachConveryor = []( agxVehicle::Track& conveyor,
                                   agx::RigidBody* parent )
  {
    for ( auto wheel : conveyor.getWheels() ) {
      auto hinge = wheel->attachHinge( "hinge", parent, 0.0 );
      hinge->setEnableComputeForces( true );
      if ( wheel->getModel() == agxVehicle::TrackWheel::SPROCKET )
        hinge->getMotor1D()->setEnable( true );
    }
  };
 
  agxVehicle::TrackPropertiesRef properties = new agxVehicle::TrackProperties();
  properties->setStabilizingHingeFrictionParameter( 4.0E-3 );
  properties->setMinStabilizingHingeNormalForce( 50.0 );
 
  const auto setConveyorProperties = [properties]( agxVehicle::Track& conveyor )
  {
    conveyor.setProperties( properties );
 
    conveyor.getInternalMergeProperties()->setEnableMerge( true );
    conveyor.getInternalMergeProperties()->setNumNodesPerMergeSegment( 8u );
    conveyor.getInternalMergeProperties()->setContactReduction( agxVehicle::TrackInternalMergeProperties::AGGRESSIVE );
 
    //conveyor.setLinearVelocityDamping( agx::Vec3( 0.1 ) );
    //conveyor.setAngularVelocityDamping( agx::Vec3( 0.1 ) );
  };
 
  // Sets given speed to conveyor sprockets.
  const auto setSpeed = []( agxVehicle::Track& conveyor, agx::Real speed )
  {
    for ( auto wheel : conveyor.getWheels() ) {
      if ( wheel->getModel() != agxVehicle::TrackWheel::SPROCKET )
        continue;
 
      auto hinge = wheel->getConstraint()->as<agx::Hinge>();
      hinge->getMotor1D()->setSpeed( speed );
    }
  };
 
  agx::MaterialRef conveyorMaterial = new agx::Material( "conveyor" );
  conveyorMaterial->getBulkMaterial()->setDensity( 650.0 );
 
  auto conveyor1 = createConveyor( 0.1, 2.0, 0.01, 50, 260, 1.0E-3 );
  setConveyorProperties( *conveyor1 );
  conveyor1->setMaterial( conveyorMaterial );
  simulation->add( conveyor1 );
 
  attachConveryor( *conveyor1, nullptr );
  createConveyorVisual( *conveyor1, agxRender::Color::LightGreen(), agxRender::Color::DarkGoldenrod() );
 
  setSpeed( *conveyor1, 6.0 );
 
  auto conveyor2 = createConveyor( 0.1, 2.5, 0.01, 25, 160, 1.0E-3 );
  setConveyorProperties( *conveyor2 );
  conveyor2->setMaterial( conveyorMaterial );
  simulation->add( conveyor2 );
 
  conveyor2->setTransform( agx::AffineMatrix4x4::rotate( agx::degreesToRadians( -20 ), agx::Vec3::Y_AXIS() ) *
                           agx::AffineMatrix4x4::translate( 7.45, 0, 0.85 ) );
  attachConveryor( *conveyor2, nullptr );
  createConveyorVisual( *conveyor2, agxRender::Color::LightGreen(), agxRender::Color::Goldenrod() );
 
  setSpeed( *conveyor2, 6.5 );
 
  agx::MaterialRef boxMaterial = new agx::Material( "box" );
  boxMaterial->getBulkMaterial()->setDensity( 250 );
 
  auto boxConveyorCm = simulation->getMaterialManager()->getOrCreateContactMaterial( boxMaterial, conveyorMaterial );
  boxConveyorCm->setFrictionModel( new agx::ScaleBoxFrictionModel( agx::FrictionModel::DIRECT_AND_ITERATIVE ) );
  boxConveyorCm->setRestitution( 0 );
  boxConveyorCm->setFrictionCoefficient( 1.0 );
  boxConveyorCm->setSurfaceViscosity( 1.0E-4 );
 
  const auto spawnTimeEvery = 2.8;
  const auto spawnPosition  = agx::Vec3( -4, 0, 0.62 );
  agxUtil::StepEventCallback::preCollide( [boxMaterial,
                                           spawnTimeEvery,
                                           spawnPosition,
                                           root]( agxSDK::Simulation* simulation, agx::Real& spawnTimer )
  {
    if ( spawnTimer < spawnTimeEvery ) {
      spawnTimer += simulation->getTimeStep();
      return;
    }
 
    agx::RigidBodyRef box = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 0.5, 0.5, 0.5 ) ) );
    box->setPosition( spawnPosition );
    agxUtil::setBodyMaterial( box, boxMaterial );
 
    simulation->add( box );
    agxOSG::setDiffuseColor( agxOSG::createVisual( box, root ), agxRender::Color::Blue() );
 
    spawnTimer = 0.0;
  }, simulation, agx::Real() );
 
  application->getSceneDecorator()->setBackgroundColor( agxRender::Color::SkyBlue(), agxRender::Color::DodgerBlue() );
  simulation->add( new utils::TrackPropertyKeyboardHandler( utils::TrackPropertyKeyboardHandler::TracksContainer
                                                            {
                                                              conveyor1.get(),
                                                              conveyor2.get()
                                                            }, application ) );
 
  return root;
}
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout << "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial Tracked Vehicle (agxVehicle::Track)\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
    application->addScene( buildTutorial3, '3' );
    application->addScene( buildTutorial4, '4' );
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_tree.cpp

/*
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.
*/
 
/*
 
This file contains a collection of tutorials for agxModel::Tree. It
describes the basics of how use a Tree and what is possible to do
with it.
 
*/
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/utils.h>
 
#include <agxModel/Tree.h>
 
#include <agx/Logger.h>
 
#include <agxCollide/Cylinder.h>
#include <agxCollide/Capsule.h>
#include <agxCollide/Box.h>
#include <agx/version.h>
#include <agxUtil/agxUtil.h>
 
using namespace agx;
 
osg::Group* buildTutorial1(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/)
{
  std::cout << "\nTutorial 1: Basic Tree." << std::endl;
 
  // First we must create the tree base class. The tree keeps track of all the
  // structures that build up the different branches of a tree. Note that
  // everything in a tree consists of branches, even the "stem".
  agxModel::TreeRef tree = new agxModel::Tree();
  // When creating a tree we can get the root of the tree. The root is
  // technically also a branch, but we do not create any geometries for the root.
  // Instead we use the root to attach other branches to it, which will have
  // geometries.
  agxModel::Tree::BranchRef treeRoot = tree->getRoot();
 
  // We create our first branch. The first vector sets the start position and
  // the second the end position. Note that the start position will set the
  // position of the branch rigid body that is created, and the end position
  // will be used to rotate the body to point in that direction. Both
  // positions are global coordinates.
  agxModel::Tree::BranchRef branch1 = new agxModel::Tree::Branch(agx::Vec3(0,0,0), agx::Vec3(0,0,1));
  // We add the branch to the root, which attaches the branch to the root
  // with a lock constraint.
  treeRoot->addBranch(branch1);
 
  // Since creating a branch only creates the rigid body, without it
  // actually containing any geometries, we have to also create
  // the geometries for the branch. Otherwise there will be nothing
  // in the branch that other things can collide with.
  //
  // A branch can hold any geometry you want, but for some tree features,
  // like cutting, it is only fully supported to use a capsule or cylinder.
  // In this example we will use capsules. So, first we must decide on the
  // radius and length of the geometry of our first branch.
  Real r1 = 0.2; //radius of capsule 1
  Real length1 = 1;
  agxCollide::GeometryRef capsule1 = new agxCollide::Geometry(new agxCollide::Capsule(r1, length1));
 
  // Next, we must transform the capsule a bit. Since, by default, a capsule's 
  // length points in the y - direction we rotate it to make it point in the
  // z - direction instead. Then the length of the capsule will point in
  // the same direction as the branch, when added to it. Next we also want
  // to move the capsule, so that it is locked to the root at the start of
  // its length. By default, a capsule is attached at the centre of its
  // body, so moving it half its length in the z - direction will solve that.
  // The following transform will be used to do all that.
  AffineMatrix4x4 transform = AffineMatrix4x4::rotate(Vec3::Y_AXIS(), Vec3::Z_AXIS()) *
                              AffineMatrix4x4::translate(0, 0, length1 / 2);
  // Finally we then add the capsule geometry to the rigid body of the branch, using
  // the local transform we just created to position it correctly.
  branch1->getRigidBody()->add(capsule1, transform);
  // To be able to see the branch body, we create a visual for it also.
  osg::Group* root = new osg::Group();
  agxOSG::createVisual(branch1->getRigidBody(), root);
 
  // Now we want to connect a new branch, to make our tree a bit bigger.
  // This branch will start where the old one ended. Also, instead of
  // it just going straight up, we let it tilt a bit to the side.
  agxModel::Tree::BranchRef branch2 = new agxModel::Tree::Branch(Vec3(0, 0, 1), Vec3(0.1, 0, 2));
  branch1->addBranch(branch2);
  // We let this one have a bit smaller radius, and calculate the length
  // from our branch position vectors.
  Real r2 = 0.15;
  Real length2 = (Vec3(0.1, 0, 2) - Vec3(0, 0, 1)).length();
  agxCollide::GeometryRef capsule2 = new agxCollide::Geometry(new agxCollide::Capsule(r2, length2));
  // Again, we add the geometry to the rigid body of our new branch. We also create
  // the local transform get the branch positioned correctly.
  transform = AffineMatrix4x4::rotate(Vec3::Y_AXIS(), Vec3::Z_AXIS()) *
              AffineMatrix4x4::translate(0, 0, length2/2);
  branch2->getRigidBody()->add(capsule2, transform);
  agxOSG::createVisual(branch2->getRigidBody(), root);
  // Since the capsule geometries of branch 1 and 2 will overlap at bit, we
  // disable the collisions between them so that branch2 will not be pushed
  // out of branch1.
  agxUtil::setEnableCollisions(branch2->getRigidBody(), branch1->getRigidBody(), false);
 
  // Lastly we add a second branch, also attached to the the first branch we created.
  Real r3 = 0.05;
  Real length3 = (Vec3(-0.5, 0, 2) - Vec3(-r1 + r3, 0, 1)).length();
  agxModel::Tree::BranchRef branch3 = new agxModel::Tree::Branch(Vec3(-r1 + r3, 0, 1), Vec3(-0.5, 0, 2));
  branch1->addBranch(branch3);
  transform = AffineMatrix4x4::rotate(Vec3::Y_AXIS(), Vec3::Z_AXIS()) *
              AffineMatrix4x4::translate(0, 0, length3 / 2);
  branch3->getRigidBody()->add(new agxCollide::Geometry(new agxCollide::Capsule(r3, length3)), transform);
  agxOSG::createVisual(branch3->getRigidBody(), root);
  agxUtil::setEnableCollisions(branch3->getRigidBody(), branch1->getRigidBody(), false);
 
  // Now that we have created a small, simple tree, we just need to add it to the 
  // simulation also.
  simulation->add(tree);
  // Since our tree is currently just floating in space, it will just fall unless
  // we attach the root to something.Here we attach it to the world, by sending
  // in "nullptr" as the body to attach to.
  tree->attach(nullptr, new Frame());
 
  // It is possible to cut a tree at any given branch in the tree. When a tree
  // is cut, a new tree will be created where the parts after the cut will be
  // the new tree and the parts before will still be the old one.
  //
  // It is possible to cut the tree in two ways, either by specifying a branch,
  // where it will cut the branch of at the beginningand make the given branch
  // the first one of a new tree, or by specifying a geometry and a point in the
  // world. We will do the second one here. It will try to split the geometry at
  // the given point. Note that this is only supported when using cylinders or
  // capsules as the geometries for the branches.
  //
  // First we want to collect the geometry of the branch we want to split. We just
  // rake the first one in our tree.
  agxCollide::GeometryRef branch_geom = treeRoot->getBranch(0)->getRigidBody()->getGeometries()[0];
  // Next we cut it. For position we choose the position of the geometry, which
  // is in the middle of the capsule we have. This should then split the geometry
  // in our branch in half. The tree that is returned is the new tree, form the
  // parts that are after the cut.
  tree = tree->cut(branch_geom, branch_geom->getPosition());
  // When we cut the tree, the first part of the new tree is a new geometry
  // (since we split the old one in two). To be able to see it, we must
  // therefor create a visual for it.
  agxOSG::createVisual(tree->getRoot()->getBranch(0)->getRigidBody(), root);
 
  return root;
}
 
 
 
agxModel::Tree::BranchRef createBranch(agxModel::Tree::BranchRef branch, Real startRadius, Real endRadius, 
                                        Vec3 startPos, Vec3 endPos, int numSegments)
{
  // Create new branch and add to old one
  agxModel::Tree::BranchRef newBranch = new agxModel::Tree::Branch(startPos, endPos);
  branch->addBranch(newBranch);
 
  // Calculate length of each segment
  Real segLength = (endPos - startPos).length() / numSegments;
  // Set the start z - position of first segment relative rigid body and start radius
  Real posZ = segLength / 2;
  Real r = startRadius;
  // Calculate how much the radius has to change for each segment in the branch
  Real dr = 0;
  if (numSegments > 1) {
    dr = (endRadius - startRadius) / (numSegments - 1);
  }
  // Create the number of segments specified.
  for (int i = 0; i < numSegments; i++) {
    // Each segment has to be moved a bit in the z - direction and rotated to
    // point in the right direction
    AffineMatrix4x4 transform = AffineMatrix4x4::rotate(Vec3::Y_AXIS(), Vec3::Z_AXIS()) *
                                AffineMatrix4x4::translate(0, 0, posZ);
    // Create the capsule geometry with the correct segment radius and length and add
    // it to the rigid body with the correct local transform
    newBranch->getRigidBody()->add(new agxCollide::Geometry(new agxCollide::Capsule(r, segLength)), transform);
    // Update z - position and radius for the next segment
    r += dr;
    posZ += segLength;
  }
 
  return newBranch;
}
 
 
 
osg::Group* buildTutorial2(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/)
{
  std::cout << "\nTutorial 2: Bigger Tree." << std::endl;
 
  // We create the tree and collect the root.
  agxModel::TreeRef tree = new agxModel::Tree();
  agxModel::Tree::BranchRef treeRoot = tree->getRoot();
  osg::Group* root = new osg::Group();
 
  // We use a function to create the branches, in a very similar way that we
  // did in the above tutorial. However, note that we now also set the number of
  // segments for each branch. We do this because we might not want the branch
  // to have a fixed radius the entire way. We also set a start and end
  // radius. This way each internal geometry will become thinnerand thinner
  // to make the branch become thinner further out.
  Real startRadius = 0.2;
  Real endRadius = 0.18;
  Vec3 startPos = Vec3(0, 0, 0);
  Vec3 endPos = Vec3(0, 0, 1);
  int numSegments = 4;
  agxModel::Tree::BranchRef branch = createBranch(treeRoot, startRadius, endRadius, startPos, endPos, numSegments);
  // Add a visual for the entire branch body
  agxOSG::createVisual(branch->getRigidBody(), root);
 
  std::list<agxModel::Tree::BranchRef> branches = {branch};
 
  // Let's make the base branch of the tree a bit longer, and also include small 
  // random elements to make the tree not be completely straight
  agx::Real dr = (endRadius - startRadius) / (numSegments - 1);
  while (endRadius > 0.05) {
    startRadius = endRadius + dr;
    endRadius = startRadius - 0.02;
    startPos = endPos;
    endPos = startPos + Vec3(random<Real>(-0.05, 0.05), random<Real>(-0.05, 0.05), 1);
    branch = createBranch(branch, startRadius, endRadius, startPos, endPos, numSegments);
    agxOSG::createVisual(branch->getRigidBody(), root);
    branches.push_back(branch);
  }
 
  // Now we only have the stem of the tree however. We would of course like it to
  // have some more branches to actually look like a tree.
  startRadius = 0.04;
  endRadius = 0.01;
  numSegments = 5;
  Real length = 2.5;
  for(agxModel::Tree::BranchRef b : branches) {
    // To each of the branches that make up our stem we will add 3 to 7 branches
    // pointing outward.
    int nrBranches = random<int>(3,7);
    for (int i = 0; i<nrBranches; i++) {
      // For each new branch we randomise it to be at any height from 0 to 1m along
      // the stem segment (which is about how long they are)
      startPos = b->getRigidBody()->getFrame()->transformPointToWorld(0, 0, random<Real>(0,1));
      // We make the branch point in a random direction, but still pretty much
      // straight out(in 90 deg angle) from the tree.
      Vec3 new_dir = Quat(EulerAngles(0, PI/2+random<Real>(-0.2,0.2), random<Real>(-PI,PI))) * 
                     Vec3(0, 0, 1);
      endPos = startPos + new_dir * (length + random<Real>(-0.1,0.1));
      // Create the branch
      branch = createBranch(b, startRadius, endRadius, startPos, endPos, numSegments);
      agxOSG::createVisual(branch->getRigidBody(), root);
    }
    // Branches further upp the tree should become a bit shorter
    length = length - 0.3;
  }
  // Add our tree to the simulation
  simulation->add(tree);
  tree->attach(nullptr, new Frame());
  // Remove all contacts between the bodies in the tree by creating adding the tree to a
  // collision group and disable collisions for that group colliding with itself
  tree->addGroupID(123);
  simulation->getSpace()->setEnablePair(123, 123, false);
 
  return root;
}
 
 
 
void randomTreeGenerator(agxModel::Tree::Branch* root, Vec3 startPos, 
  const Vec3& treeDir, Real totalLength, int numSegments, Real startRadius, Real endRadius, 
  int numSubBranches = 2, bool isBranch = false)
{
  if (numSegments == 0 || totalLength < 1E-3 || startRadius < 1E-3 || endRadius < 1E-3)
    return;
 
  agxModel::Tree::BranchRef currBranch = root;
  Real segmentLength = totalLength / numSegments;
  Real dr = (startRadius - endRadius) / numSegments;
  Real currEndRadius = startRadius;
 
  int numInnerSegments = 3;
  // Let branches be more crooked than the stem
  Real branchBend = 0.1;
  if (isBranch)
    branchBend = 0.2;
 
  for (int i = 0; i < numSegments; i++) {
    // Randomize the new branch segment to point in a small deviation from
    // the input tree_dir - direction
    Vec3 newDir = Quat(EulerAngles(random<Real>(-branchBend, branchBend),
                                   random<Real>(-branchBend, branchBend),
                                   random<Real>(-branchBend, branchBend))) * treeDir;
    newDir.normalize();
    // Calculate the end position of the the branch segment
    Vec3 endPos = startPos + newDir * segmentLength;
 
    // Create the new branch segment
    Real currStartRadius = currEndRadius - dr / numInnerSegments;
    currEndRadius -= dr;
    agxModel::Tree::BranchRef newBranch = createBranch(currBranch, currStartRadius, currEndRadius,
                                                       startPos, endPos, numInnerSegments);
 
    // We recursively call this function to create branches in the same way as the stem
    // There will be no branches going out from the first or last segment of a branch.
    if (i > 0 && i < numSegments - 1) {
      // We let the new branch point in a random angle, approximately perpendicular
      // to the branch it is attached to.
      Vec3 localDir = Quat(EulerAngles(0, random<Real>(PI/2 - 0.2, PI/2 + 0.2),
                                        random<Real>(-PI, PI))) * Vec3(0, 0, 1);
      Vec3 tempDir = newBranch->getRigidBody()->getFrame()->transformVectorToWorld(localDir);
      Vec3 segmentPosition = startPos + newDir * random<Real>(0, 0.95 * totalLength / numSegments) +
                             endRadius * tempDir;
      Real newLength = 0.4 * (numSegments - i) * totalLength / numSegments;
      // Branches going out of the tree are scaled with the end radius of the current 
      // branch. To stop the branches from becoming super thin, we limit the radius of
      // the new branches.
      int newNumSegments = std::min(numSegments - i, numSubBranches + 1);
      Real newSegmentsStartRadius = std::max(0.4 * currEndRadius, 0.02);
      Real newSegmentsEndRadius = std::max(0.4 * endRadius, 0.01);
      randomTreeGenerator(newBranch,
        segmentPosition,
        tempDir,
        newLength,
        newNumSegments,
        newSegmentsStartRadius,
        newSegmentsEndRadius,
        numSubBranches,
        true);
    }
    startPos += (newDir * segmentLength);
    currBranch = newBranch;
  }
}
 
 
 
void setConstraintParameters(agxModel::Tree::BranchRef branch, Real r) {
  // Collect the constraint.
  ConstraintRef branchConstraint = branch->getConstraint();
 
  if (branchConstraint != nullptr) {
    // Calculate compliance, depending on the cross section area of the branch
    Real translationalCompliance = 1 / (1500 * 1E6 * PI * r * r);
    // It is should be easier to bend the branch, so we let the compliance in
    // those directions be larger.
    Real rotationalCompliance = translationalCompliance * 10;
    // We let the damping be 12 / (simulation time step)
    Real damping = Real(12) / 60;
 
    // Set compliance in the translational degrees of freedom
    branchConstraint->setCompliance(translationalCompliance, LockJoint::TRANSLATIONAL_1);
    branchConstraint->setCompliance(translationalCompliance, LockJoint::TRANSLATIONAL_2);
    branchConstraint->setCompliance(translationalCompliance, LockJoint::TRANSLATIONAL_3);
    // Set compliance in the rotational degrees of freedom
    branchConstraint->setCompliance(rotationalCompliance, LockJoint::ROTATIONAL_1);
    branchConstraint->setCompliance(rotationalCompliance, LockJoint::ROTATIONAL_2);
    branchConstraint->setCompliance(rotationalCompliance, LockJoint::ROTATIONAL_3);
    // Set the damping
    branchConstraint->setDamping(damping);
  }
}
 
 
 
class BranchListener : public agxModel::Tree::BranchEventListener
{
  public:
    BranchListener(agxSDK::Simulation* simulation, osg::Group* graphicsRoot)
      : m_simulation(simulation), m_graphicsRoot(graphicsRoot) {}
 
    // onCreate is called when a branch is created / added to the simulation
    virtual void onCreate(agxModel::Tree::Branch* branch)
    {
      // We need to set what load in the limit value for each branch. We need
      // a max load both for bending and stretching, and also, a thinner branch
      // should probably be weaker, thus we want the strength of it to be
      // dependent on the radius
      //
      // First, we then want to know what the radius of the branch is. Since
      // the branches get thinner further out, we only check the radius of
      // the first geometry in the branch, since that is where it is attached.
      Real r = 1;
      agxCollide::Shape* shape = branch->getRigidBody()->getGeometries()[0]->getShapes()[0];
      agxCollide::Capsule* c = static_cast<agxCollide::Capsule*>(shape);
      if (c != nullptr)
        r = c->getRadius();
      // Next we set the tensile strength and the bending strength, depending
      // on the cross section area of the branch. It should be easier to bend
      // the branch off than ripping it of, so we let the bending strength be
      // smaller. Try changing these parameters to see the change in how
      // easily the branches break off.
      Real tensileStrength = 100 * 1E6 * PI * r * r;
      Real bendingStrength = 20 * 1E6 * PI * r * r;
      // Then we set the max load for the branch.
      branch->setMaxLoad(tensileStrength, bendingStrength);
 
      // Now we know when we want the branch to break off, but we also
      // want the branches to be able to flex a little bit before we they
      // break off. Therefore we also want to set some of the the compliance and
      // the damping related to the lock constraint holding the branch in place.
      setConstraintParameters(branch, r);
 
      agxOSG::createVisual(branch->getRigidBody(), m_graphicsRoot.get());
    }
 
    // onHighLoad is called when the load on a branch exceeds a given threshold
    virtual void onHighLoad(agxModel::Tree::Branch* branch)
    {
      // When we have a high load on a branch, we simply cut it off
      this->getTree()->cut(branch);
    }
 
    // clone is needed when we cut a tree and the listeners needs to be copied to all new
    // trees created.
    virtual BranchListener* clone()
    {
      // Since we do not do anything special in this listener, creating a new instance
      // if the listener is enought to clone it.
      return new BranchListener(m_simulation, m_graphicsRoot.get());
    }
 
  protected:
    virtual ~BranchListener() {}
 
    observer_ptr< agxSDK::Simulation > m_simulation;
    osg::observer_ptr<osg::Group> m_graphicsRoot;
};
 
 
 
osg::Group* buildTutorial3(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/)
{
  std::cout << "\nTutorial 3: BranchEventListener." << std::endl;
 
  // We create a tree similar to a spruce, using this random tree generator. For more
  // information about how it works, check the code further upp, where the function is
  // defined. It will be 35 m tall and the stem will start at 0.5 m thickness and end
  // at 5 cm thickness.
  agxModel::TreeRef tree = new agxModel::Tree();
  randomTreeGenerator(tree->getRoot(), Vec3(0, 0, 0), Vec3::Z_AXIS(), 35, 40, 0.5, 0.05);
 
  // Next we want to add a BranchEventListener. A BranchEventListener has three
  // important functions: onCreate, onHightLoadand and clone. onCreate is called for
  // each branch when we add the tree with the listener to the simulation, or when
  // we cut the tree and new parts are added. onHighLoad is called, just as the name
  // suggests, when a branch is experiencing a load that is over a certain threshold.
  // clone needs to be implemented for what happens when we clone a tree with a
  // BranchEventListener in it, because we also need to clone the listener. This is
  // done when cutting a tree for example.
  //
  // Just like, for example, StepEvent - or ContactEventListeners, we must create our
  // own class that implements what will happen to out tree when these functions are
  // called. In this case we have implemented one called BranchListener. It will set
  // some parameters for out branches as they are created, and when experiencing a
  // high load, it will cut that branch off. For more details about how it is
  // implemented, see the class further up.
  osg::Group* root = new osg::Group();
  BranchListener* bl = new BranchListener(simulation, root);
  tree->setBranchEventListener(bl);
 
  // Add the tree to the simulation.
  simulation->add(tree);
  // Make the tree not collide with itself
  tree->addGroupID(123);
  simulation->getSpace()->setEnablePair(123, 123, false);
 
  // We want to add a ground for the tree to fall into, so we simply create a box
  // and add it to the simulation.
  agxCollide::GeometryRef groundGeom = new agxCollide::Geometry(new agxCollide::Box(50, 50, 0.2),
    AffineMatrix4x4::translate(0, 0, -0.7));
  RigidBodyRef ground = new RigidBody(groundGeom);
  ground->setMotionControl(RigidBody::STATIC);
  simulation->add(ground);
 
  agxOSG::createVisual(ground, root);
 
  // Attach the tree to the ground
  tree->attach(ground, new Frame());
 
  // Now we want to cut the tree down, to see it fall and hopefully see some branches
  // snap off. We first get the second branch in the tree, which will be the second
  // segment of the stem.
  agxModel::Tree::BranchRef branch = tree->getRoot()->getBranch(0)->getBranch(0);
  // Next we cut the branch. The tree will be cut were the branch starts.
  tree->cut(branch);
 
  return root;
}
 
 
 
osg::Group* buildTutorial4(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/)
{
  std::cout << "\nTutorial 4: Wake up tree." << std::endl;
 
  // Let's create a similar tree as in the last tutorial, with a BranchEventListener
  // also.
  agxModel::TreeRef tree = new agxModel::Tree();
  randomTreeGenerator(tree->getRoot(), Vec3(0, 0, 0), Vec3::Z_AXIS(), 35, 40, 0.5, 0.05);
  osg::Group* root = new osg::Group();
  tree->setBranchEventListener(new BranchListener(simulation, root));
 
  // Add the tree to the simulation.
  simulation->add(tree);
  // Make the tree not collide with itself
  tree->addGroupID(123);
  simulation->getSpace()->setEnablePair(123, 123, false);
  // Attach the tree to the world
  tree->attach(nullptr, new Frame());
 
  // Now we make the tree static, but send in true, as we want the tree to become dynamic
  // when something is in contact with it. Setting it to false would mean it is static
  // until tree->makeDynamic() is called. It is possible to set a TreeExecuteFilter when 
  // calling makeStatic, but not doing so will use the default one, which will make the 
  // tree dynamic at any contact with it. It is possible to create your own TreeExecuteFilter where
  // you filter out the contacts you want.
  tree->makeStatic(true);
 
  // Let's now create an object to drop at our tree, so we have something to wake it up.
  // It will be a simple sphere, which we place above the tree.
  agxCollide::GeometryRef sphereGeom = new agxCollide::Geometry(new agxCollide::Sphere(1),
                                                                AffineMatrix4x4::translate(1, 1, 42));
  RigidBodyRef sphere = new RigidBody(sphereGeom);
  simulation->add(sphere);
  agxOSG::createVisual(sphere, root);
 
  return root;
}
 
 
 
class CustomTreeExecuteFilter : public agxModel::Tree::TreeExecuteFilter
{
  private:
    int m_groupId = 0;
 
  public:
    CustomTreeExecuteFilter(int groupId)
      : agxModel::Tree::TreeExecuteFilter(),
      m_groupId(groupId)
    {
    }
 
    // The match function is what we need to implement to control when our tree wakes
    // up. It should return true when we want the tree to become dynamic and false
    // otherwise. It takes a agxCollide.GeometryContact or agxCollide.GeometryPair as
    // input, so we must implement what happens in both cases.
    bool match(const agxCollide::GeometryContact& contact) const {
      UInt32 treeId = this->getTreeID();
      // We check if either of the geometries involved in a collision has the tree's 
      // collision group id or the other id we specified. If we have both, we can
      // assume that a collision between something in our given collision group has
      // collided with the tree.
      bool collisionWithTree = contact.geometry(0)->hasGroup(treeId) || contact.geometry(1)->hasGroup(treeId);
      bool collisionWithGroup = contact.geometry(0)->hasGroup(m_groupId) || contact.geometry(1)->hasGroup(m_groupId);
      return collisionWithTree && collisionWithGroup;
    }
 
    // Implement the method for agxCollide.GeometryPair. It will basically do the same
    // thing as the other one, which is to check if we have collision between the tree
    // and the selected collision group.
    bool match(const agxCollide::GeometryPair& pair) const {
      UInt32 treeId = this->getTreeID();
      bool collisionWithTree = pair.first->hasGroup(treeId) || pair.second->hasGroup(treeId);
      bool collisionWithGroup = pair.first->hasGroup(m_groupId) || pair.second->hasGroup(m_groupId);
      return collisionWithTree && collisionWithGroup;
    }
 
    using agxSDK::ExecuteFilter::match;
 
    // Just as for the BranchEventListener, TreeExecuteFilter also needs a
    // clone method implemented.
    virtual agxModel::Tree::TreeExecuteFilter* clone() {
      return new CustomTreeExecuteFilter(m_groupId);
    }
};
 
 
 
osg::Group* buildTutorial5(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/)
{
  std::cout << "\nTutorial 5: Custom TreeExecuteFilter." << std::endl;
 
  // Let's create a similar tree as in the last tutorial, with a BranchEventListener
  // also.
  agxModel::TreeRef tree = new agxModel::Tree();
  randomTreeGenerator(tree->getRoot(), Vec3(0, 0, 0), Vec3::Z_AXIS(), 35, 40, 0.5, 0.05);
  osg::Group* root = new osg::Group();
  tree->setBranchEventListener(new BranchListener(simulation, root));
 
  // Add the tree to the simulation.
  simulation->add(tree);
  // Make the tree not collide with itself
  tree->addGroupID(123);
  simulation->getSpace()->setEnablePair(123, 123, false);
  // Attach the tree to the world
  tree->attach(nullptr, new Frame());
 
  // Now we make the tree static again, but instead of using the default TreeExecuteFilter
  // we use a CustomTreeExecuteFilter, which will only make the tree dynamic when in
  // contact with the bodies with the given group id.
  int collisionGroupId = 321;
  tree->makeStatic(true, new CustomTreeExecuteFilter(collisionGroupId));
 
  // We create a sphere again, but this time, we make sure that it is part of our given
  // collision group id.
  agxCollide::GeometryRef sphereGeom = new agxCollide::Geometry(new agxCollide::Sphere(1),
                                                                AffineMatrix4x4::translate(1, 1, 55));
  RigidBodyRef sphere = new RigidBody(sphereGeom);
  simulation->add(sphere);
  agxUtil::addGroup(sphere, collisionGroupId);
  agxOSG::createVisual(sphere, root);
 
  // Just to be sure the tree only becomes dynamic when in contact with our given bodies,
  // we create another sphere, but do not add it to the collision group id. We also place
  // it is a bit closer to the tree, to be sure that it has contact with the tree first.
  agxCollide::GeometryRef sphereGeom2 = new agxCollide::Geometry(new agxCollide::Sphere(1),
                                                                 AffineMatrix4x4::translate(1, 1, 37));
  RigidBodyRef sphere2 = new RigidBody(sphereGeom2);
  simulation->add(sphere2);
  agxOSG::createVisual(sphere2, root);
 
  return root;
}
 
 
 
int main(int argc, char** argv)
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial Tree\n" <<
    "\t--------------------------------\n\n" << std::endl;
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene(buildTutorial1, '1');
    application->addScene(buildTutorial2, '2');
    application->addScene(buildTutorial3, '3');
    application->addScene(buildTutorial4, '4');
    application->addScene(buildTutorial5, '5');
 
    if (application->init(argc, argv))
      return application->run();
  }
  catch (std::exception& e) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_trimesh.cpp

/*
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.
*/
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/GeometryNode.h>
#include <agxOSG/ReaderWriter.h>
 
#include <agxUtil/TrimeshReaderWriter/TrimeshReaderWriter.h>
 
#include <agxCollide/BasicPrimitiveTests.h>
#include <agxCollide/Line.h>
#include <agxCollide/LineClipping.h>
#include <agxCollide/PolygonClipping.h>
#include <agxCollide/Trimesh.h>
 
#include <agxRender/RenderSingleton.h>
#include <agx/Logger.h>
 
#include "tutorialUtils.h"
 
 
osg::Group* buildTutorial1( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // The trimesh is made up of triangles. These are specified in two vectors:
  //   - one vector with vertices
  //   - one vector with indices. 3 indices creates one face.
  //
  // Default mode is triangle lists and three indices are used for every face.
  //
  // E.g:
  //     index vector: 1 2 3 4 5 6 7 8 9
  //
  // Will create three faces, face1(1,2,3), face2(4,5,6) and face3(7,8,9)
  //
  //
  // Triangles can be specified in counter-clockwise and clockwise order.
  // Default is counter-clockwise. TrimeshOptionsFlags is used to change
  // this behavior.
 
 
  agx::Vec3Vector vertices;
  agx::UInt32Vector indices;
 
  //
  // A small pyramid
  //
  vertices.push_back( agx::Vec3( -0.4, -0.4, 0.0 ) ); // four base corners
  vertices.push_back( agx::Vec3( -0.4,  0.4, 0.0 ) );
  vertices.push_back( agx::Vec3(  0.4,  0.4, 0.0 ) );
  vertices.push_back( agx::Vec3(  0.4, -0.4, 0.0 ) );
  vertices.push_back( agx::Vec3(  0.0,  0.0, 0.8 ) );    // pyramid top
 
  // The triangles.
  indices.push_back( 1 );
  indices.push_back( 0 );
  indices.push_back( 4 );
 
  indices.push_back( 2 );
  indices.push_back( 1 );
  indices.push_back( 4 );
 
  indices.push_back( 3 );
  indices.push_back( 2 );
  indices.push_back( 4 );
 
  indices.push_back( 0 );
  indices.push_back( 3 );
  indices.push_back( 4 );
 
  indices.push_back( 0 );
  indices.push_back( 1 );
  indices.push_back( 3 );
 
  indices.push_back( 3 );
  indices.push_back( 1 );
  indices.push_back( 2 );
 
 
  // Create the collision shape
  agxCollide::TrimeshRef triangleMesh = new agxCollide::Trimesh( &vertices, &indices, "handmade pyramid" );
 
  // Create the rigid body and add the handmade triangle mesh to it.
  agx::RigidBodyRef triangleMeshRigidBody = new agx::RigidBody();
  triangleMeshRigidBody->setName( "handmade" );
  triangleMeshRigidBody->add( new agxCollide::Geometry( triangleMesh ) );
 
  // Motion control: STATIC, and graphics.
  triangleMeshRigidBody->setMotionControl( agx::RigidBody::STATIC );
  agxOSG::createVisual( triangleMeshRigidBody, root );
 
  simulation->add( triangleMeshRigidBody );
 
  return root;
}
 
 
osg::Group* buildTutorial2( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  const char* collisionModel = "models/torusRoundForCollision.obj";
  const char* visualModel    = "models/torusRound.obj";
 
  agx::MaterialRef material = new agx::Material("ourMaterial");
 
  // Create the trimesh shape
  agxCollide::TrimeshRef trimeshShape = agxUtil::TrimeshReaderWriter::createTrimesh( collisionModel );
 
  if ( !trimeshShape )
  {
    LOGGER_ERROR() << "Error while loading trimesh" << std::endl << LOGGER_END();
    return root;
  }
 
  // Create the dynamic rigid body (carries mass properties and interface to dynamic properties)
  agx::RigidBodyRef dynamicRB = new agx::RigidBody();
 
  // Set the position of the body.
  dynamicRB->setPosition( agx::Vec3( 0,0,2));
 
  // Create a new geometry.
  agxCollide::GeometryRef dynamicRBGeometry = new agxCollide::Geometry();
  dynamicRBGeometry->setMaterial( material );
 
  // Add the shape to the geometry.
  dynamicRBGeometry->add( trimeshShape);
 
 
  // Add the geometry to the rigid body, telling that if this geometry collides with other geometries,
  // this rigid body will take the forces from these collisions.
  dynamicRB->add( dynamicRBGeometry );
 
 
  // As a last step we have to add the rigid body to the simulation.
  // Any geometries associated with the body will also be added to collision space.
  simulation->add( dynamicRB );
 
  // For rendering, associate visual geometries to the physical geometries in this rigid body
 
  // First, create a GeometryNode that will update the visual node according to geometry position
  agxOSG::GeometryNode *geometryNode = new agxOSG::GeometryNode( dynamicRBGeometry );
 
  // Second, read the visual mesh and add it to the node.
  // OSG can issue a warning due to a missing material in the .obj-file.
  osg::Node* visualTrimesh = agxOSG::readNodeFile( visualModel, false );
 
  if ( visualTrimesh )
    geometryNode->addChild( visualTrimesh );
  else
    LOGGER_ERROR() << "Could not read visual trimesh model" << std::endl << LOGGER_END();
 
  root->addChild( geometryNode );
 
  // Create the static rigid body (a floor onto which all other objects will fall)
  agx::RigidBodyRef staticRB = new agx::RigidBody();
  agxCollide::GeometryRef staticBodyGeometry = new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 15.0, 15.0, 0.2 ) ) );
  staticRB->add( staticBodyGeometry );
  staticBodyGeometry->setMaterial( material );
 
 
  simulation->add( staticRB );
 
  // We haven't flagged the rigid body to be static yet, by default it's dynamic. To change and to
  // set this flag is easy, in AGX it's called motion control.
  staticRB->setMotionControl( agx::RigidBody::STATIC );
 
  // For rendering, associate visual geometries to the physical geometries in this rigid body
  agxOSG::createVisual( staticRB, root );
 
  // And just move the static body down so that the surface is at exactly z = 0. Remember that the
  // thickness, half extent, of the static body is 2 decimeters.
  staticRB->setPosition( agx::Vec3( 0, 0, -0.2 ) );
 
  simulation->add( staticRB );
 
  // Trimeshes can cause many contacts. Contact reduction is enabled.
  // See the documentation for more information about contact reduction.
  // We have now enabled it by specifying our own material, and use that for all of our geometries.
  // So when "ourMaterial" collides with "ourMaterial" the result will be a ContactMaterial which as
  // ContactReduction enabled.
 
  // We also specify the contact reduction bin resolution to 3 (2 or 3 are usually a good numbers).
  simulation->setContactReductionBinResolution( 3 );
 
  return root;
}
 
 
 
osg::Group* buildTutorial3( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  const char* collisionModel = "models/torusRoundForCollision.obj";
 
  agx::MaterialRef material = new agx::Material("ourMaterial");
 
  // Create the first trimesh shape
  agxCollide::TrimeshRef trimeshShape = agxUtil::TrimeshReaderWriter::createTrimesh( collisionModel );
 
  // now make many copies
  size_t numMeshesPerDim = 3;
  agx::Real objectDistance = agx::Real(3.0);
  for (size_t i = 0; i < numMeshesPerDim; ++i) {
    for (size_t j = 0; j < numMeshesPerDim; ++j) {
      // Now we create the shallow copy of the original mesh.
      // Note that the caller to this function takes ownership of the pointer, so we should
      // use a reference pointer (TrimeshRef).
      agxCollide::TrimeshRef trimeshShapeCopy = trimeshShape->shallowCopy();
      agx::RigidBodyRef triangleMeshRigidBody = new agx::RigidBody();
      triangleMeshRigidBody->setName( "MeshCopy" );
      agxCollide::GeometryRef geom = new agxCollide::Geometry( trimeshShapeCopy );
      geom->setMaterial( material );
 
      triangleMeshRigidBody->add( geom );
      // We spread out the copies evenly.
      triangleMeshRigidBody->setPosition(
        (agx::Real(i) - agx::Real(numMeshesPerDim) * 0.5) * objectDistance,
        (agx::Real(j) - agx::Real(numMeshesPerDim) * 0.5) * objectDistance,
        objectDistance);
 
      agxOSG::createVisual( triangleMeshRigidBody, root );
      simulation->add( triangleMeshRigidBody );
    }
  }
 
  // The original (trimeshShape) mesh could be used for something, or not.
  // Here we choose not to use it. Since we hold a reference pointer to it, its
  // memory will be deallocated once this function returns.
  // Its MeshData will be preserved by all its copies though as long as they exist.
 
 
  // All the other functions below are like in buildTutorial3, to create a static body.
 
 
 
  // Create the static rigid body (a floor onto which all other objects will fall)
  agx::RigidBodyRef staticRB = new agx::RigidBody();
  agxCollide::GeometryRef staticGeometry = new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 15.0, 15.0, 0.2 ) ) );
  staticGeometry->setMaterial( material );
  staticRB->add( staticGeometry );
  simulation->add( staticRB );
 
  // We haven't flagged the rigid body to be static yet, by default it's dynamic. To change and to
  // set this flag is easy, in AGX it's called motion control.
  staticRB->setMotionControl( agx::RigidBody::STATIC );
 
  // For rendering, associate visual geometries to the physical geometries in this rigid body
  agxOSG::createVisual( staticRB, root );
 
  // And just move the static body down so that the surface is at exactly z = 0. Remember that the
  // thickness, half extent, of the static body is 2 decimeters.
  staticRB->setPosition( agx::Vec3( 0, 0, -0.2 ) );
 
  simulation->add( staticRB );
 
  // Trimeshes can cause many contacts. Contact reduction is enabled.
  // See the documentation for more information about contact reduction.
  // We have now enabled it by specifying our own material, and use that for all of our geometries.
  // So when "ourMaterial" collides with "ourMaterial" the result will be a ContactMaterial which as
  // ContactReduction enabled.
 
  // We also specify the contact reduction bin resolution to 3 (2 or 3 are usually a good numbers).
  simulation->setContactReductionBinResolution( 3 );
 
  return root;
}
 
 
osg::Group* traversalTutorial1( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group;
 
  // Create a trimesh from vertices and indices, like in the examples above.
  // This time, it is the example from the documentation document about Trimesh traversal.
  agx::Vec3Vector vertices;
  vertices.push_back( agx::Vec3(1, 0, 2 ));   // v0
  vertices.push_back( agx::Vec3(0.5, 0, 1 )); // v1
  vertices.push_back( agx::Vec3(1, 0, 0 ));   // v2
  vertices.push_back( agx::Vec3(0, 0, 0 ));   // v3
  vertices.push_back( agx::Vec3(2, 0, 0 ));   // v4
  vertices.push_back( agx::Vec3(1.5, 0, 1 )); // v5
 
  agx::UInt32Vector indices;
  indices.push_back(2); indices.push_back(5); indices.push_back(1); // t0
  indices.push_back(3); indices.push_back(1); indices.push_back(3); // t1
  indices.push_back(4); indices.push_back(5); indices.push_back(2); // t2
  indices.push_back(0); indices.push_back(1); indices.push_back(5); // t3
 
  // This will issue warnings, since the trimesh is not closed.
  agxCollide::TrimeshRef trimesh = new agxCollide::Trimesh( &vertices, &indices, "handmade");
 
  // As in the examples above...
  agxCollide::GeometryRef geo = new agxCollide::Geometry();
  geo->add( trimesh );
  agx::RigidBodyRef body = new agx::RigidBody();
  body->add(geo);
  body->setMotionControl(agx::RigidBody::STATIC);
  simulation->add(body);
  agxOSG::createVisual(body, root);
 
  // Does this trimesh have any half edge neighbors?
  bool hasHalfEdge = trimesh->hasHalfEdge();
  std::cout << "The trimesh has " << (hasHalfEdge? "a " : "no ") << "half edge structure.\n";
 
  // Is the trimesh closed? (->It has half edge neighbors everywhere).
  bool isClosed = trimesh->isValidAndClosed();
  std::cout << "The trimesh is " << (isClosed? "" : "not ") << "closed.\n";
 
  // Using the agxCollide::Trimesh::Triangle helper class, find the
  // half edge neighbor to a triangle half edge (half edge 1 in triangle 0).
  agxCollide::Trimesh::Triangle triangle = trimesh->getTriangle(0);
  // Is the triangle valid?
  bool triangleValid = triangle.isValid();
  std::cout << "The triangle is " << (triangleValid? "" : "not ") << "valid.\n";
 
  // Get neighboring triangle to edge 1.
  agxCollide::Trimesh::Triangle neighborTriangle = triangle.getHalfEdgePartner(1);
  size_t neighborTriangleIndex = neighborTriangle.getTriangleIndex();
  std::cout << "Triangle 0 has the neighboring triangle with triangle index " <<
    neighborTriangleIndex << " at edge 1.\n"
    << "This triangle is " << (neighborTriangle.isValid() ? "" : "not ") << "valid\n";
 
  // What index does edge 1 in t0 have in the other triangle?
  size_t neighborEdgeIndex = triangle.getHalfEdgePartnerLocalEdgeIndex(1);
  std::cout << "Edge 1 from triangle 0 has the index " << neighborEdgeIndex <<
    " in the triangle with index " << neighborTriangleIndex << "\n";
 
  return root;
}
 
 
namespace {
  class IteratorMover : public agxSDK::GuiEventListener
  {
  public:
    IteratorMover( agxCollide::Geometry* geometry, unsigned int startTriangle, uint_fast8_t startVertex )
      : m_geometry(geometry)
    {
      if (m_geometry) {
        if (m_geometry->getShapes().size()) {
          agxCollide::Trimesh* mesh = dynamic_cast<agxCollide::Trimesh*>(geometry->getShapes()[0].get());
          if (mesh) {
            m_trimesh = mesh;
            // If any of the parameters is invalid at creation, the m_circulator will be invalid.
            m_circulator = mesh->createVertexEdgeCirculator( startTriangle, startVertex );
          }
        }
      }
      // Listen only to keyboard events.
      setMask( GuiEventListener::KEYBOARD );
    }
 
    // Any keyboard events?
    virtual bool keyboard( int key, unsigned /*modkeyMask*/, float /*x*/, float /*y*/, bool keydown ) {
      if ( keydown && ( key == GuiEventListener::KEY_Right ) ) {
        std::cout << "pressed right" << std::endl;
        m_circulator++; // Set the triangle vertex circulator one step forward.
        RenderTriangle();
        return true;
      }
      return false;
    }
 
  private:
    // Render the triangle in debug rendering.
    void RenderTriangle()
    {
      if (m_trimesh && m_circulator.isValid()) {
        agxCollide::Trimesh::Triangle triangle = m_circulator.getTriangle();
        if (!triangle.isValid())
          return;
        agx::Vec3 verts[3] = {triangle.getVertex(0) * m_geometry->getTransform(),
          triangle.getVertex(1) * m_geometry->getTransform(),
          triangle.getVertex(2) * m_geometry->getTransform() };
 
        // All of the rendering done here will only be seen in debug rendering mode.
        agxRender::RenderSingleton::instance()->clearAll();
 
        // Render the triangle .
        agxRender::RenderSingleton::instance()->addStatic( verts[0], verts[1], 0.02, agx::Vec4f(1, 0, 0, 0) );
        agxRender::RenderSingleton::instance()->addStatic( verts[1], verts[2], 0.02, agx::Vec4f(1, 0, 0, 0) );
        agxRender::RenderSingleton::instance()->addStatic( verts[2], verts[0], 0.02, agx::Vec4f(1, 0, 0, 0) );
 
        // Render the normal at the correct edge.
        int index = m_circulator.getLocalEdgeIndex();
        agx::Vec3 normalStart = (verts[index] + verts[(index + 1) % 3]) * 0.5;
        agx::Vec3 normalEnd = normalStart + triangle.getNormal() * m_geometry->getTransform();
        agxRender::RenderSingleton::instance()->addStatic( normalStart, normalEnd, 0.03, agx::Vec4f(1, 0, 0, 0) );
      }
    }
 
    agxCollide::Trimesh::VertexEdgeCirculator m_circulator;
    agxCollide::TrimeshRef m_trimesh;
    agxCollide::GeometryRef m_geometry;
  };
}
 
 
 
namespace {
  class TriangleTraverser : public agxSDK::StepEventListener
  {
  public:
    TriangleTraverser(agxCollide::Geometry* trimeshGeo, agxCollide::Geometry* lineGeo, const agx::Vec4& color, size_t maxIt = 1000):
        agxSDK::StepEventListener(agxSDK::StepEventListener::POST_STEP),
          m_trimeshGeo( trimeshGeo), m_lineGeo( lineGeo ), m_color(color), m_maxIt(maxIt)
        {
          updateValid();
        }
 
  private:
    void updateValid()
    {
      m_valid = (m_lineGeo->getShapes().size() == 1 && m_lineGeo->getShapes()[0]->getType() == agxCollide::Shape::LINE
        && m_trimeshGeo->getShapes().size() == 1 && m_trimeshGeo->getShapes()[0]->getType() == agxCollide::Shape::TRIMESH);
 
      if (!m_valid)
        LOGGER_WARNING() << "TriangleTraverser should have one geometry with exactly one line and one with exactly one trimesh. Ignoring.\n" << LOGGER_END();
    }
 
    // From StepEventlistener. Will be called each time step after the solver updated all bodies transformations.
    virtual void post(const agx::TimeStamp& /*time*/)
    {
      updateValid();
      if (!m_valid)
        return;
      unsigned int triangleIndex;
      agx::Real lineSegmentT;
 
      // Calculate the transformation matrix from the trimesh shape to world.
      // There is one geometry transform, and maybe even a shape transform.
      agx::AffineMatrix4x4 trimeshToWorld = m_trimeshGeo->getTransform();
 
      trimeshToWorld = m_trimeshGeo->getShapes()[0]->getLocalTransform() * trimeshToWorld;
 
      // In order to transform from the world to the trimesh's local frame.
      agx::AffineMatrix4x4 worldToTrimesh = trimeshToWorld.inverse();
 
      // Calculate the transformation matrix from the line shape to world.
      // There is one geometry transform, and maybe even a shape transform.
      agx::AffineMatrix4x4 lineToWorld = m_lineGeo->getTransform();
      lineToWorld = m_lineGeo->getShapes()[0]->getLocalTransform() * lineToWorld;
 
      // Obtaining the line shape.
      const agxCollide::Line* line = dynamic_cast<const agxCollide::Line*>(m_lineGeo->getShapes()[0].get());
      if (!line) {
        LOGGER_WARNING() << "Line shape not found. Ignoring.\n" << LOGGER_END();
        return;
      }
 
      // Obtaining the trimesh shape.
      const agxCollide::Trimesh* trimesh = dynamic_cast<const agxCollide::Trimesh*>(m_trimeshGeo->getShapes()[0].get());
      if (!trimesh) {
        LOGGER_WARNING() << "Trimesh shape not found. Ignoring.\n" << LOGGER_END();
        return;
      }
 
      // The line starting and ending points in world coordinates.
      agx::Vec3 lineStartInWorld = line->getFirstPoint() * lineToWorld;
      agx::Vec3 lineEndInWorld = line->getSecondPoint() * lineToWorld;
 
      agx::Vec3 lineStartInTrimesh = lineStartInWorld * worldToTrimesh;
      agx::Vec3 lineEndInTrimesh = lineEndInWorld * worldToTrimesh;
 
      // Have to test from hand, since agxCollide does not directly give us access to which triangle was hit.
      bool foundPoint = findIntersectionLineSegmentMesh( lineStartInTrimesh, lineEndInTrimesh, trimesh, lineSegmentT, triangleIndex );
 
      if (!foundPoint)
        return; //no intersection
 
      // Calculate the hit point in world coordinates.
      agx::Vec3 hitPointInWorld = lineStartInWorld * (1-lineSegmentT)  + lineEndInWorld * lineSegmentT;
      agxRender::RenderSingleton::instance()->add(hitPointInWorld, agx::Real(0.06), m_color);
 
      // Direction of the line segment in trimesh coordinates.
      agx::Vec3 dirInTrimesh = lineEndInTrimesh - lineStartInTrimesh;
 
      // Direction of the line segment projected onto the xy plane in trimesh coordinates.
      agx::Vec3 dirProjToTrimeshXY( dirInTrimesh[0], dirInTrimesh[1], 0 );
 
      // Traverse triangles
      agx::Vec3 pointInTrimesh = hitPointInWorld * worldToTrimesh;
      size_t numTriangles = 0;
      agxCollide::Trimesh::Triangle triangle = trimesh->getTriangle(triangleIndex);
      size_t edgeIndex = 3; // only on first triangle, on other triangles it will be between 0 and 2
      bool atEndLength = false;
 
      const agx::Real segmentRestLength = (lineEndInTrimesh - pointInTrimesh).length();
 
      agx::Real traversedLength = 0;
 
      // Main traversal loop.
      // The first time, test all three edges, since we might start out on a face.
      // Then, test only the two other edges.
      while (numTriangles < m_maxIt && triangle.isValid() && !atEndLength) {
        agx::Vec3 newPointInTrimesh;
        agx::Real newT = agx::RealMax;
 
        uint_fast8_t newEdgeIndex = 3;
 
        for (uint_fast8_t i = 0; i < 3; ++i) {
          if (i == edgeIndex)
            continue; // Ignore the edge we just arrived at.
          agx::Vec3 tmpPoint;
          agx::Real tmpT=0;
          agx::Vec3 edge = triangle.getEdgeEndVertex(i) - triangle.getEdgeStartVertex(i);
          agx::Vec3 edgeNormal(edge[1], -edge[0], 0); // Is not normalized.
 
          // Find intersection with plane defined z normal and triangle edge.
          if (agxCollide::intersectLineSegmentHyperPlane(pointInTrimesh, lineEndInTrimesh,
            edgeNormal, edgeNormal * triangle.getEdgeStartVertex(i), tmpT, tmpPoint, agx::RealEpsilon )) {
              if (tmpT >= 0 && tmpT < newT) {
                newT = tmpT;
                newEdgeIndex = i;
                newPointInTrimesh = tmpPoint;
              }
          }
        }
 
        // Ending conditions. Right now, the same xy coordinate as the segment end point
        // in trimesh coordinates is used.
        if (newEdgeIndex == 3) {
          // No intersection found. Assume that endPoint can be used.
          atEndLength = true;
        }
        else  {
          if ((newPointInTrimesh - lineEndInTrimesh) * dirProjToTrimeshXY > 0) {
            atEndLength = true; // Surpassed projection of line on xy plane.
          }
        }
 
        if (atEndLength) {
          newPointInTrimesh = lineEndInTrimesh;
        }
 
        // Project new point upwards in z onto triangle plane.
        const agx::Vec3& triangleNormal = triangle.getNormal();
        agx::Real d = (triangle.getEdgeStartVertex(0) - newPointInTrimesh) * triangleNormal;
 
        if (!agx::equalsZero(triangleNormal[2]))
          newPointInTrimesh += agx::Vec3(0, 0, agx::Real(1.0) / triangleNormal[2] * d);
        else {
          LOGGER_WARNING() << "An error occurred while intersecting the ray with triangles:\n"
            << "tested triangle normal is orthogonal to terrain normal.\n" << LOGGER_END();
        }
 
        traversedLength += (newPointInTrimesh - pointInTrimesh).length();
 
        // If we surpassed projection of line on xy plane, we should cut it back.
        if ( traversedLength > segmentRestLength) {
          atEndLength = true;
          agx::Vec3 dir = newPointInTrimesh - pointInTrimesh;
          dir.normalize();
          newPointInTrimesh -= dir * (traversedLength - segmentRestLength);
        }
 
        agxRender::RenderSingleton::instance()->add(pointInTrimesh * trimeshToWorld, newPointInTrimesh * trimeshToWorld, agx::Real(0.03), m_color);
 
        pointInTrimesh = newPointInTrimesh;
 
        if (!atEndLength){
          // Get next Triangle and set the local edge index to the edge corresponding to one in the old triangle.
          edgeIndex = triangle.getHalfEdgePartnerLocalEdgeIndex( newEdgeIndex );
          triangle = triangle.getHalfEdgePartner( newEdgeIndex );
        }
 
        numTriangles++; // traversed another triangle
      }
    }
 
  private:
    agxCollide::GeometryRef m_trimeshGeo;
    agxCollide::GeometryRef m_lineGeo;
    agx::Vec4f m_color;
    size_t m_maxIt;
    bool m_valid;
  };
}
 
 
osg::Group* traversalTutorial2( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
 
  std::cout << "Trimesh traversal along a line.\nPress 'g' to turn on debug rendering.\n" <<
    "Observe how the lines attached to the sphere are projected to the trimesh, beginning from the first intersection.\n";
 
  osg::Group* root = new osg::Group();
 
  // Geometry for the ground.
  agxCollide::GeometryRef trimeshGeo = 0L;
 
  // Load a trimesh shape from an .obj file. Create it as a terrain, with the local z axis pointing upwards.
  // This will give warnings if there are triangles whose normals point downwards with respect to this normal.
  agxCollide::TrimeshRef trimesh =
    agxUtil::TrimeshReaderWriter::createTrimeshTerrain("terrainFromHeightFieldSimplified.obj", agx::Real(1.0));
  if ( trimesh ) {
    trimeshGeo = new agxCollide::Geometry();
    trimeshGeo->add( trimesh );
  }
  else
    LOGGER_ERROR() << "Could not read trimesh" << std::endl << LOGGER_END();
 
  // As in the first examples...
  agx::RigidBodyRef trimeshBody = new agx::RigidBody();
  trimeshBody->setMotionControl( agx::RigidBody::STATIC );
  if ( trimeshGeo ) {
    trimeshBody->add( trimeshGeo );
    agxOSG::createVisual( trimeshBody, root );
  }
  else
    LOGGER_WARNING() << "Static object created without trimesh geometry." << std::endl << LOGGER_END();
 
  simulation->add( trimeshBody );
 
  // Create three agxCollide::Line and connect them to a agxCollide::Sphere.
  // Only the Sphere should be used for collision detection with the trimesh.
  agx::RigidBodyRef sphereBody = new agx::RigidBody();
  agxCollide::SphereRef sphere = new agxCollide::Sphere(0.1);
  agxCollide::GeometryRef sphereGeo = new agxCollide::Geometry(sphere);
  sphereBody->add(sphereGeo);
 
  const agx::Vec3 dirs[3] = {agx::Vec3(1, 0, 0), agx::Vec3(0, 1, 0), agx::Vec3(0, 0, 1)};
  const agx::Real segmentLength = agx::Real(4.0);
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 2; ++j) {
      agxCollide::LineRef lineSegment = (j) ? new agxCollide::Line(agx::Vec3(), dirs[i] * segmentLength) : new agxCollide::Line(agx::Vec3(), -dirs[i] * segmentLength);
      agxCollide::GeometryRef lineGeo = new agxCollide::Geometry( lineSegment );
      lineGeo->setEnableCollisions(trimeshGeo, false);
      lineGeo->setSensor(true);
      simulation->add( new TriangleTraverser(trimeshGeo, lineGeo, agx::Vec4(dirs[i], 1)));
      sphereBody->add( lineGeo );
    }
  }
  sphereBody->setPosition(0, 0.1, 3);
  simulation->add(sphereBody);
  agxOSG::createVisual(sphereBody, root);
 
  return root;
}
 
osg::Group* traversalTutorial3( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
 
  std::cout << "Trimesh traversal along a line.\nPress 'g' to turn on debug rendering.\n" <<
    "Observe how the lines attached to the sphere are projected to the trimesh, beginning from the first intersection.\n";
 
  osg::Group* root = new osg::Group();
 
  // Geometry for the ground.
  agxCollide::GeometryRef trimeshGeo = 0L;
 
  // Load a trimesh shape from an .obj file. Create is as an terrain, with the local z axis pointing upwards.
  // This will give warnings if there are triangles whose normals point downwards with respect to this normal.
  agxCollide::TrimeshRef trimesh =
    agxUtil::TrimeshReaderWriter::createTrimeshTerrain("terrainFromHeightFieldSimplified.obj", agx::Real(1.0));
  if ( trimesh ) {
    trimeshGeo = new agxCollide::Geometry();
    trimeshGeo->add( trimesh );
  }
  else
    LOGGER_ERROR() << "Could not read trimesh" << std::endl << LOGGER_END();
 
  // As in the first examples...
  agx::RigidBodyRef trimeshBody = new agx::RigidBody();
  trimeshBody->setMotionControl( agx::RigidBody::STATIC );
  if ( trimeshGeo ) {
    trimeshBody->add( trimeshGeo );
    agxOSG::createVisual( trimeshBody, root );
  }
  else
    LOGGER_WARNING() << "Static object created without trimesh geometry." << std::endl << LOGGER_END();
 
  simulation->add( trimeshBody );
 
  // Create three agxCollide::Line and connect them to a agxCollide::Sphere.
  // Only the Sphere should be used for collision detection with the trimesh.
  agx::RigidBodyRef sphereBody = new agx::RigidBody();
  agxCollide::SphereRef sphere = new agxCollide::Sphere(0.1);
  agxCollide::GeometryRef sphereGeo = new agxCollide::Geometry(sphere);
  sphereBody->add(sphereGeo);
 
  const agx::Vec3 dirs[3] = {agx::Vec3(1, 0, 0), agx::Vec3(0, 1, 0), agx::Vec3(0, 0, 1)};
  const agx::Real segmentLength = agx::Real(4.0);
  for (int i = 0; i < 3; ++i) {
    {
      // In -y direction.
      agxCollide::LineRef lineSegment = new agxCollide::Line(agx::Vec3(), -dirs[i] * segmentLength);
      agxCollide::GeometryRef lineGeo = new agxCollide::Geometry( lineSegment );
      lineGeo->setEnableCollisions(trimeshGeo, false);
      lineGeo->setSensor(true);
      simulation->add( new TriangleTraverser(trimeshGeo, lineGeo, agx::Vec4(dirs[i], 1)));
      sphereBody->add( lineGeo );
    }
    {
      // In +y direction.
      agxCollide::LineRef lineSegment = new agxCollide::Line(agx::Vec3(), dirs[i] * segmentLength);
      agxCollide::GeometryRef lineGeo = new agxCollide::Geometry( lineSegment );
      lineGeo->setEnableCollisions(trimeshGeo, false);
      lineGeo->setSensor(true);
      simulation->add( new TriangleTraverser(trimeshGeo, lineGeo, agx::Vec4(dirs[i], 1)));
      sphereBody->add( lineGeo );
    }
 
  }
  sphereBody->setPosition(0, 0.1, 3);
  simulation->add(sphereBody);
  agxOSG::createVisual(sphereBody, root);
 
  return root;
}
#include <agxIO/MeshReader.h>
#include <agxUtil/TrimeshHelperFunctions.h>
#include <agxUtil/TrimeshReaderWriter/TrimeshReaderWriter.h>
 
 
 
namespace {
 
  struct Settings {
 
    double reductionRatio = 0;
    double aggressiveness = 0;
 
 
    Settings(double reduction, double agg) :
      reductionRatio(reduction),
      aggressiveness(agg)
    {
 
    }
 
    Settings(const Settings& s) :
      reductionRatio(s.reductionRatio),
      aggressiveness(s.aggressiveness)
    {
 
    }
 
  };
 
  std::ostream& operator <<(std::ostream& os, const Settings& s)
  {
    os << agx::String::format("ReductionRatio=%f, aggressiveness=%f", s.reductionRatio, s.aggressiveness);
    return os;
  }
 
  class MeshReducer : public agx::BasicThread, public agx::Referenced
  {
  public:
    MeshReducer(const agxCollide::MeshData *meshData, const Settings& settings) : BasicThread(), m_meshData(meshData), m_settings(settings), m_time(0)
    {
    }
 
    double getTime() const
    {
      return m_time;
    }
 
    Settings getSettings() const
    {
      return m_settings;
    }
 
    agxCollide::Trimesh* getTrimesh()
    {
      return m_trimesh;
    }
 
    void run() override
    {
      agx::Thread::registerAsAgxThread();
 
      agx::Vec3Vector outVertices;
      agx::UInt32Vector outIndices;
 
      agx::Timer timer(true);
 
      // Do the actual mesh reduction based on the settings
      agxUtil::reduceMesh(m_meshData->getVertices(), m_meshData->getIndices(), outVertices, outIndices, m_settings.reductionRatio, m_settings.aggressiveness);
      timer.stop();
      m_time = timer.getTime();
 
      // Create a new trimesh from the reduced vertex data
      m_trimesh = new agxCollide::Trimesh(&outVertices, &outIndices, "MeshReduction", agxCollide::Trimesh::NO_WARNINGS);
 
      agx::Thread::unregisterAsAgxThread();
    }
 
  private:
    const agxCollide::MeshData* m_meshData;
    agxCollide::TrimeshRef m_trimesh;
    const Settings& m_settings;
    double m_time;
  };
  typedef agx::ref_ptr<MeshReducer> MeshReducerRef;
}
 
osg::Group* reduceMeshTutorial(agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/)
{
  osg::Group* root = new osg::Group();
 
  // Define a matrix to rotate and scale the model
  auto rotateAndScale = agx::Matrix3x3(agx::EulerAngles(agx::PI_2, 0, 0)) * agx::Matrix3x3(agx::Vec3(10, 10, 10));
 
  // First, create a trimesh from which we will create reduced copies
  agxCollide::TrimeshRef originalTrimesh = agxUtil::TrimeshReaderWriter::createTrimesh(
      "models/bunny.obj",
      agxCollide::Trimesh::REMOVE_DUPLICATE_VERTICES,
    rotateAndScale);
 
  if (!originalTrimesh)
    LOGGER_ERROR() << "Error reading mesh model" << LOGGER_ENDL();
 
  // Create a set of settings for the mesh reduction
  std::vector<Settings> settings = {
    Settings({0.99999,7}),
    Settings({0.9,7}),
    Settings({0.6,7}),
    Settings({0.3,7}),
    Settings({0.1,7}),
    Settings({0.05,100}),
    Settings({0.2,10}),
    Settings({0.4,200}),
    Settings({0.8,200}),
  };
 
  agx::Vector<MeshReducerRef> meshReducers;
 
  // Create a bunch of mesh reducers
  std::cerr << "Creating Mesh reducers..." << std::endl;
  for (const auto& s : settings)
  {
    auto reducer = new MeshReducer(originalTrimesh->getMeshData(), s);
    meshReducers.push_back(reducer);
  }
 
  // Start all the threads
  std::for_each(meshReducers.begin(), meshReducers.end(), [](MeshReducer* c) { c->start(); });
 
  // Waiting for the threads to finalize
  std::for_each(meshReducers.begin(), meshReducers.end(), [](MeshReducer* c) { c->join(); });
 
  // Collect the data and create a new rigidbody for each reduced trimesh
  int i = 0;
  for (auto t : meshReducers)
  {
    std::cerr << t->getSettings() << ": " << t->getTime() << " ms." << std::endl;
 
    agx::RigidBodyRef meshRigidBody = new agx::RigidBody();
 
    {
      auto geometry = new agxCollide::Geometry(t->getTrimesh());
      meshRigidBody->add(geometry);
    }
    // Create the rigid body and add the handmade triangle mesh to it.
    meshRigidBody->setPosition(-5.0 + i * 3.0, 0, 2);
    meshRigidBody->setName("MeshReduction");
    agxOSG::setDiffuseColor(agxOSG::createVisual(meshRigidBody, root), agxRender::Color::getColor(i));
 
    std::stringstream str;
    str << t->getSettings() << ": " << t->getTrimesh()->getMeshData()->getIndices().size()/3 << " triangles" << std::ends;
    agxOSG::addText(meshRigidBody->getGeometries()[0], root, str.str(), agx::Vec3(0, 0, 1), agxRender::Color::Yellow());
 
    simulation->add(meshRigidBody);
 
    i++;
  }
  meshReducers.clear();
 
  // Create the static rigid body (a floor onto which all other objects will fall)
  agx::RigidBodyRef staticRB = new agx::RigidBody();
  staticRB->setMotionControl(agx::RigidBody::STATIC);
  agxCollide::GeometryRef staticBodyGeometry = new agxCollide::Geometry(new agxCollide::Box(agx::Vec3(20.0, 15.0, 0.2)));
  agxOSG::setDiffuseColor(agxOSG::createVisual(staticBodyGeometry, root), agxRender::Color::DarkBlue());
 
  staticRB->add(staticBodyGeometry);
  simulation->add(staticRB);
 
  return root;
}
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout <<
    "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
    "\tTutorial Trimesh\n" <<
    "\t--------------------------------\n\n" << std::endl;
 
  try {
 
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
    application->addScene( buildTutorial3, '3' );
    application->addScene( traversalTutorial1, '5' );
    application->addScene( traversalTutorial2, '6' );
    application->addScene(traversalTutorial3, '7');
    application->addScene(reduceMeshTutorial, '8');
 
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}
 

tutorial_wind.cpp

/*
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.
*/
 
 
/*
 
Demonstrates how to use the agxModel::WindAndWaterManager to simulate wind.
 
*/
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/PressureFieldRenderer.h>
#include <agxOSG/WireRenderer.h>
 
#include <agxModel/WindAndWaterController.h>
 
#include <agxUtil/TrimeshReaderWriter/TrimeshReaderWriter.h>
 
#include <agxWire/Wire.h>
 
#include <agx/LockJoint.h>
 
#include <agxRender/RenderSingleton.h>
 
#include <agxCollide/Trimesh.h>
 
#include "tutorialUtils.h"
 
namespace
{
  agxCollide::Shape* getShape( agxCollide::Geometry* geometry )
  {
    return geometry != 0L && geometry->getShapes().size() > 0 ? geometry->getShapes()[ 0 ] : 0L;
  }
 
  agxCollide::Geometry* getGeometry( agx::RigidBody* rb )
  {
    return rb != 0L && rb->getGeometries().size() > 0 ? rb->getGeometries()[ 0 ] : 0L;
  }
 
  agxCollide::Shape* getShape( agx::RigidBody* rb )
  {
    return getShape( getGeometry( rb ) );
  }
 
  class Tutorial1WindControl : public agxSDK::GuiEventListener
  {
    public:
      Tutorial1WindControl( agxModel::ConstantWindGeneratorRef windGenerator, agxOSG::ExampleApplication* app )
        : m_windGenerator( windGenerator ), m_app( app ) { setMask( KEYBOARD | UPDATE ); }
 
      virtual bool keyboard( int key, unsigned int , float , float , bool down ) override
      {
        const agx::Quat dq( agx::Real( 0.01 ), agx::Vec3::Z_AXIS() );
        if ( down && key == agxSDK::GuiEventListener::KEY_Up )
          m_windGenerator->setVelocity( m_windGenerator->getVelocity() + agx::Real( 0.1 ) * m_windGenerator->getVelocity().normal() );
        if ( down && key == agxSDK::GuiEventListener::KEY_Down )
          m_windGenerator->setVelocity( m_windGenerator->getVelocity() - agx::Real( 0.1 ) * m_windGenerator->getVelocity().normal() );
        if ( down && key == agxSDK::GuiEventListener::KEY_Left )
          m_windGenerator->setVelocity( dq * m_windGenerator->getVelocity() );
        if ( down && key == agxSDK::GuiEventListener::KEY_Right )
          m_windGenerator->setVelocity( dq.conj() * m_windGenerator->getVelocity() );
 
        return key == agxSDK::GuiEventListener::KEY_Up || key == agxSDK::GuiEventListener::KEY_Down ||
               key == agxSDK::GuiEventListener::KEY_Left || key == agxSDK::GuiEventListener::KEY_Right;
      }
 
      virtual void update( float , float ) override
      {
        if ( m_app == 0L )
          return;
 
        m_app->getSceneDecorator()->setText( 0, "Change wind speed: Key up and key down." );
        m_app->getSceneDecorator()->setText( 1, "Change wind direction (in xy plane): Key left and key right." );
        m_app->getSceneDecorator()->setText( 2, agx::String::format( "Wind velocity: [%2.3f %2.3f %2.3f] m/s", m_windGenerator->getVelocity().x(), m_windGenerator->getVelocity().y(), m_windGenerator->getVelocity().z() ), agxRender::Color::Red() );
      }
 
    private:
      agxModel::ConstantWindGeneratorRef m_windGenerator;
      agx::observer_ptr< agxOSG::ExampleApplication > m_app;
  };
}
 
osg::Group* buildTutorial1( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  // Pressure field renderer render the state after a simulation step.
  // Tell simulation to integrate transforms of the bodies first in
  // a step-forward instead of last.
  simulation->setPreIntegratePositions( true );
 
  // Create wind and water controller and add it to the simulation.
  agxModel::WindAndWaterControllerRef controller = new agxModel::WindAndWaterController();
  simulation->add( controller );
 
  // Create ground.
  agxCollide::MeshRef groundMesh = agxCollide::HeightField::createFromFile( "textures/heightfield_bowl.png",
                                                                            agx::Real( 125 ), agx::Real( 75 ),
                                                                            agx::Real( -1 ), agx::Real( 7 ) );
  agxCollide::GeometryRef ground = new agxCollide::Geometry( groundMesh );
  simulation->add( ground );
  agxOSG::GeometryNode* groundNode = agxOSG::createVisual( ground, root );
  agxOSG::setDiffuseColor( groundNode, agxRender::Color::LightGreen() );
  agxOSG::setShininess( groundNode, 128.0f );
 
  // Create a sphere and a box.
  agx::RigidBodyRef sphere = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Sphere( agx::Real( 1 ) ) ) );
  agx::RigidBodyRef box    = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( agx::Real( 0.75 ), agx::Real( 0.75 ), agx::Real( 1.75 ) ) ) );
 
  sphere->setPosition( 0, 2, 1 );
  box->setPosition( 0, -2, 2 );
 
  simulation->add( sphere );
  simulation->add( box );
 
  // The box is hanging in a wire.
  agxWire::WireRef boxWire = new agxWire::Wire( agx::Real( 0.01 ), agx::Real( 2 ) );
  boxWire->add( new agxWire::BodyFixedNode( box, agx::Vec3( 0, 0, 1.75 ) ) );
  boxWire->add( new agxWire::BodyFixedNode( 0L, box->getFrame()->transformPointToWorld( agx::Vec3( 0, 0, 6 ) ) ) );
  simulation->add( boxWire );
  simulation->add( new agxOSG::WireRenderer( boxWire, root ) );
 
  // Enable aerodynamics for the dynamic objects.
  controller->setEnableAerodynamics( sphere, true );
  controller->setEnableAerodynamics( box, true );
 
  // Pressure field renderer.
  controller->registerPressureFieldRenderer( new agxOSG::PressureFieldRenderer( root ), ::getShape( sphere ) );
  controller->registerPressureFieldRenderer( new agxOSG::PressureFieldRenderer( root ), ::getShape( box ) );
 
  // Create a constant wind, with a given wind direction and magnitude.
  const agx::Vec3 windVelocity = agx::Vec3( -15, 0, 0 );
  agxModel::ConstantWindGeneratorRef windGenerator = new agxModel::ConstantWindGenerator( windVelocity );
  controller->setWindGenerator( windGenerator );
 
  // Add our "wind keyboard manipulator".
  simulation->add( new ::Tutorial1WindControl( windGenerator, application ) );
 
  return root;
}
 
namespace
{
  class CustomWindGenerator : public agxModel::WindAndWaterController::WindGenerator
  {
    public:
      CustomWindGenerator( const agxCollide::Geometry* shadowingGeometry, const agx::Vec3& halfExtents, const agx::Vec3& windVelocity )
        : m_shadowingGeometry( shadowingGeometry ), m_halfExtents( halfExtents ), m_windVelocity( windVelocity )
      {
      }
 
      // Prepare is called before a geometry is about to be handled.
      virtual void prepare( const agxCollide::Geometry* ) override
      {
        // Update this for each new geometry. Once every time m_shadowingGeometry has been
        // moved is enough, but we don't know that here.
        m_cachedFarPlanePos[ 0 ] = m_shadowingGeometry->getPosition() + m_shadowingGeometry->getFrame()->transformVectorToWorld( agx::Vec3( -m_halfExtents.x(), 0, 0 ) );
        m_cachedFarPlanePos[ 1 ] = m_shadowingGeometry->getPosition() + m_shadowingGeometry->getFrame()->transformVectorToWorld( agx::Vec3( 0, m_halfExtents.y(), 0 ) );
        m_cachedFarPlanePos[ 2 ] = m_shadowingGeometry->getPosition() + m_shadowingGeometry->getFrame()->transformVectorToWorld( agx::Vec3( 0, -m_halfExtents.y(), 0 ) );
        m_cachedFarPlanePos[ 3 ] = m_shadowingGeometry->getPosition() + m_shadowingGeometry->getFrame()->transformVectorToWorld( agx::Vec3( 0, 0, m_halfExtents.z() ) );
      }
 
      // Return wind at worldPoint.
      virtual agx::Vec3 getVelocity( const agx::Vec3& worldPoint ) const override
      {
        return isShadowed( worldPoint ) ? agx::Vec3() : m_windVelocity;
      }
 
      AGXSTREAM_DECLARE_SERIALIZABLE( CustomWindGenerator );
 
    protected:
      CustomWindGenerator() {}
 
    private:
      agx::Bool isShadowed( const agx::Vec3& worldPoint ) const
      {
        return worldPoint.x() < m_cachedFarPlanePos[ 0 ].x() &&
               worldPoint.y() < m_cachedFarPlanePos[ 1 ].y() &&
               worldPoint.y() > m_cachedFarPlanePos[ 2 ].y() &&
               worldPoint.z() < m_cachedFarPlanePos[ 3 ].z();
      }
 
    private:
      agxCollide::GeometryConstRef m_shadowingGeometry;
      agx::Vec3 m_halfExtents;
      agx::Vec3 m_windVelocity;
      agx::Vec3 m_cachedFarPlanePos[ 4 ];
  };
 
  // Serialization.
  void CustomWindGenerator::store( agxStream::OutputArchive& out ) const
  {
    out << agxStream::out( "shadowingGeometry", m_shadowingGeometry.get() );
    out << agxStream::out( "he", m_halfExtents );
    out << agxStream::out( "wv", m_windVelocity );
  }
 
  void CustomWindGenerator::restore( agxStream::InputArchive& in )
  {
    agxCollide::Geometry* shadowingGeometry = 0L;
    in >> agxStream::in( "shadowingGeometry", shadowingGeometry );
    m_shadowingGeometry = shadowingGeometry;
    in >> agxStream::in( "he", m_halfExtents );
    in >> agxStream::in( "wv", m_windVelocity );
  }
}
 
osg::Group* buildTutorial2( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Pressure field renderer render the state after a simulation step.
  // Tell simulation to integrate transforms of the bodies first in
  // a step-forward instead of last.
  simulation->setPreIntegratePositions( true );
 
  // Create wind and water controller and add it to the simulation.
  agxModel::WindAndWaterControllerRef controller = new agxModel::WindAndWaterController();
  simulation->add( controller );
 
  // Create ground.
  agxCollide::GeometryRef ground = new agxCollide::Geometry( new agxCollide::Box( agx::Real( 20 ), agx::Real( 20 ), agx::Real( 0.5 ) ) );
  ground->add( new agxCollide::Box( agx::Real( 0.5 ), agx::Real( 20 ), agx::Real( 2 ) ), agx::AffineMatrix4x4::rotate( agx::degreesToRadians( -60 ), agx::Vec3::Y_AXIS() ) *
                                                                                         agx::AffineMatrix4x4::translate( -20, 0, 0.5 ) );
  ground->setPosition( agx::Real( 0 ), agx::Real( 0 ), agx::Real( -0.5 ) );
  simulation->add( ground );
  agxOSG::createVisual( ground, root );
 
  // Create box which shadows the wind.
  const agx::Vec3 shadowingBoxHalfExtents = agx::Vec3( agx::Real( 0.2 ), agx::Real( 1.5 ), agx::Real( 2 ) );
  agxCollide::GeometryRef shadowingBox = new agxCollide::Geometry( new agxCollide::Box( shadowingBoxHalfExtents ) );
  shadowingBox->setPosition( agx::Real( 10 ), agx::Real( 0 ), agx::Real( shadowingBoxHalfExtents.z() ) );
  simulation->add( shadowingBox );
  agxOSG::setDiffuseColor( agxOSG::createVisual( shadowingBox, root ), agxRender::Color::Red() );
 
  // Create spheres.
  // Completely shadowed.
  agx::RigidBodyRef sphere1 = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Sphere( agx::Real( 0.5 ) ) ) );
  // Left side shadowed.
  agx::RigidBodyRef sphere2 = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Sphere( agx::Real( 0.5 ) ) ) );
  // Right side shadowed.
  agx::RigidBodyRef sphere3 = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Sphere( agx::Real( 0.5 ) ) ) );
  // Completely in wind.
  agx::RigidBodyRef sphere4 = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Sphere( agx::Real( 0.5 ) ) ) );
 
  sphere1->setPosition( agx::Real( 8 ), agx::Real(  0.0 ), agx::Real( 0.5 ) );
  sphere2->setPosition( agx::Real( 8 ), agx::Real(  1.5 ), agx::Real( 0.5 ) );
  sphere3->setPosition( agx::Real( 8 ), agx::Real( -1.5 ), agx::Real( 0.5 ) );
  sphere4->setPosition( agx::Real( 8 ), agx::Real( -3.0 ), agx::Real( 0.5 ) );
 
  simulation->add( sphere1 );
  simulation->add( sphere2 );
  simulation->add( sphere3 );
  simulation->add( sphere4 );
 
  controller->setEnableAerodynamics( sphere1, true );
  controller->setEnableAerodynamics( sphere2, true );
  controller->setEnableAerodynamics( sphere3, true );
  controller->setEnableAerodynamics( sphere4, true );
 
  // Change (lower, default is 1.0) the viscous drag for sphere2.
  agxModel::AerodynamicsParameters* sphere2AeroParameters = controller->getOrCreateAerodynamicsParameters( ::getShape( sphere2 ) );
  sphere2AeroParameters->setCoefficient( agxModel::WindAndWaterParameters::Coefficient::VISCOUS_DRAG, agx::Real( 0.5 ) );
 
  // Pressure field renderer.
  controller->registerPressureFieldRenderer( new agxOSG::PressureFieldRenderer( root ), ::getShape( sphere1 ) );
  controller->registerPressureFieldRenderer( new agxOSG::PressureFieldRenderer( root ), ::getShape( sphere2 ) );
  controller->registerPressureFieldRenderer( new agxOSG::PressureFieldRenderer( root ), ::getShape( sphere3 ) );
  controller->registerPressureFieldRenderer( new agxOSG::PressureFieldRenderer( root ), ::getShape( sphere4 ) );
 
  const agx::Vec3 windVelocity = agx::Vec3( -10, 0, 0 );
  controller->setWindGenerator( new ::CustomWindGenerator( shadowingBox, shadowingBoxHalfExtents, windVelocity ) );
 
  return root;
}
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout << "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
               "\tTutorial hydrodynamics\n" <<
               "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_wire1.cpp

/*
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.
*/
 
/*
 
Tutorial for demonstrating the use of the agxWire::Wire API for simulating wires.
 
*/
 
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/Node.h>
 
#include <agxUtil/Spline.h>
#include <agxUtil/TrimeshReaderWriter/TrimeshReaderWriter.h>
 
#include <agxIO/MeshReader.h>
 
#include <agxWire/Wire.h>
#include <agxWire/WireController.h>
#include <agxWire/EyeNodeArea.h>
#include <agxOSG/WireRenderer.h>
 
#include <agx/version.h>
#include <agx/LockJoint.h>
 
#include <osg/ShapeDrawable>
 
#include "tutorialUtils.h"
 
#include <agxOSG/WireRenderer.h>
#include <agxRender/RenderSingleton.h>
 
#include <agxCollide/Trimesh.h>
 
 
typedef agx::Vector< osg::ref_ptr< osg::MatrixTransform > > MatrixTransformRefVector;
template < typename T >
class RenderWireListener : public agxSDK::StepEventListener
{
  public:
    RenderWireListener( agxWire::Wire* wire, osg::Group* root )
      : m_wire( wire ), m_spline( new T() )
    {
      // Render data update when all positions are updated.
      setMask( POST_STEP );
 
      osg::Cylinder* cylinder = new osg::Cylinder();
      // Reference cylinder set to 1 meter, we'll add parent scale transform.
      cylinder->set( osg::Vec3(), (float)m_wire->getRadius(), 1 );
      // Rotate so that the cylinder is along y axis.
      cylinder->setRotation( osg::Quat( -agx::PI_2, osg::Vec3( 1, 0, 0 ) ) );
 
      m_osgCylinder = new osg::Geode;
      m_osgCylinder->setCullingActive( false );
 
      osg::TessellationHints* th = new osg::TessellationHints();
      th->setDetailRatio( 0.4f );
      osg::ShapeDrawable* drawable = new osg::ShapeDrawable( cylinder, th );
 
      m_osgCylinder->getOrCreateStateSet();
      m_osgCylinder->addDrawable( drawable );
 
      agxOSG::setDiffuseColor( m_osgCylinder.get(), agx::Vec4f( 0.3f, 0.1f, 0.8f, 1 ) );
 
      // We have our own group node for all cylinders, add to root node.
      m_group = new osg::Group;
      m_group->setDataVariance( osg::Object::STATIC );
      root->addChild( m_group.get() );
    }
 
  protected:
    virtual ~RenderWireListener() {}
 
    virtual void addNotification()
    {
      // Render one when this listener is added.
      if( m_wire.isValid() )
        render();
    }
 
    virtual void post( const agx::TimeStamp& )
    {
      // Remove our selfs if the wire has been removed.
      if ( !m_wire.isValid() ) {
        getSimulation()->remove( this );
        return;
      }
 
      render();
    }
 
    void render()
    {
      // Clear the spline.
      m_spline->clear();
 
      size_t objCounter = 0;
      // Check if we have something to render in the wire.
      if ( !m_wire->getRenderListEmpty() ) {
        // The wire have internal structures which could simulate more wire for example. So
        // it's not possible to go over the whole node list and render from that. But the
        // wire has render begin and end iterator, i.e., we should render from begin to
        // end, but not end.
        agxWire::RenderIterator renderIt    = m_wire->getRenderBeginIterator();
        agxWire::RenderIterator renderEndIt = m_wire->getRenderEndIterator();
 
        // Add the control points to the spline. Take some reference maximum tension
        // and scale to the spline. Sliding nodes, i.e., all nodes that is not BODY_FIXED
        // should have tension 1 (maximum) or make some smart spline implementation that
        // prevents jumps.
        const agx::Real refMaxTension = 1E5;
        while ( renderIt != renderEndIt ) {
          const agxWire::Node* node = *renderIt;
 
          // Find tension for body fixed nodes.
          if ( node->getType() == agxWire::Node::BODY_FIXED ) {
            m_spline->add( node->getWorldPosition(), agx::clamp< agx::Real >( m_wire->getTension( node ).prev.raw / refMaxTension, 0, 1 ) );
          }
          // Contact, eye, or some other node type. Set tension to max.
          else
            m_spline->add( node->getWorldPosition(), 0.8 );
 
          ++renderIt;
        }
 
        // Update, calculate tangents for the spline.
        m_spline->updateTangents();
 
        // Configure the cylinders segments.
        const size_t numControlPoints = m_spline->getNumPoints() - 1;
        const size_t numSegments = 40;
        agx::Real dt = agx::Real( 1 ) / numSegments;
        agx::Real start = dt;
        agxUtil::CubicSpline::Point prevPoint = m_spline->getPoints()[ 0 ];
        for ( size_t i = 0; i < numControlPoints; ++i ) {
          for ( agx::Real t = start; t < 1 || agx::equivalent( t, agx::Real( 1 ), agx::RealEpsilon ); t += dt ) {
            agxUtil::CubicSpline::Point point = m_spline->evaluate( i, t );
            bool valid = addCylinder( prevPoint, point, objCounter );
            objCounter += valid;
            prevPoint = point;
          }
          start = 0;
        }
      }
 
      // Clean our group node from old children.
      if ( objCounter < m_group->getNumChildren() ) {
        m_group->removeChildren( (unsigned int)objCounter, m_group->getNumChildren() - (unsigned int)objCounter );
      }
    }
 
    bool addCylinder( const agx::Vec3& from, const agx::Vec3& to, size_t index )
    {
      agx::Real length = from.distance( to );
      if ( length < 1E-3 )
        return false;
      // If we already have a cylinder transform + scale, manipulate it - otherwise create new.
      bool reused = index < m_group->getNumChildren();
      osg::MatrixTransform* worldTransform = reused ? dynamic_cast< osg::MatrixTransform* >( m_group->getChild( (unsigned int)index ) ) : new osg::MatrixTransform;
      assert( worldTransform );
      assert( !reused || worldTransform->getNumChildren() == 1 );
      osg::MatrixTransform* scaleTransform = reused ? dynamic_cast< osg::MatrixTransform* >( worldTransform->getChild( 0 ) ) : new osg::MatrixTransform;
      assert( scaleTransform );
 
      // Our graphics geode is setup to be along y axis, length 1 meter. So scale transform before to get the correct length.
      scaleTransform->setMatrix( osg::Matrix::scale( 1, length, 1 ) );
      worldTransform->setMatrix( AGX_MAT4X4_TO_OSG( agxUtil::calculateCylinderTransform( from, to ) ) );
 
      // new MatrixTransform for scale and world, add to group node.
      if ( !reused ) {
        scaleTransform->addChild( m_osgCylinder.get() );
        worldTransform->addChild( scaleTransform );
        worldTransform->setDataVariance( osg::Object::STATIC );
        worldTransform->computeBound();
        m_group->addChild( worldTransform );
      }
 
      return true;
    }
 
  protected:
    agx::observer_ptr< agxWire::Wire > m_wire;
    osg::ref_ptr< osg::Geode > m_osgCylinder;
    osg::ref_ptr< osg::Group > m_group;
    agxUtil::SplineRef m_spline;
};
 
osg::Group* buildTutorial1( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  agx::Real wireRadius( 0.01 );                 // - Radius of the wire.
  agx::Real wireResolutionPerUnitLength( 0.5 ); // - Number of mass elements, used by the wire, per unit length.
                                                //   Example: Unit length is meter. 0.5 means ONE mass element every two meters,
                                                //            and in total 10 * 0.5 = 5 for the whole wire.
  bool collisionsEnabled = true;                // - By default true. If true, the wire will collide with supported shapes. At this moment
                                                //   supported shapes are: Box, Cylinder and Trimesh, and only one shape per geometry.
 
  // Construct wire. The wire it self is of type agxSDK::StepEventListener, i.e., NOT an agx::Constraint object.
  // The wire it self doesn't have a length at this stage. The length is calculated so that the system is at
  // rest after initialization. Further down in this tutorial file we'll show how to control the total length.
  agxWire::WireRef wire = new agxWire::Wire( wireRadius, wireResolutionPerUnitLength, collisionsEnabled );
 
  // Routing. We have to specify the initial "look" of the wire. This is a simple U like route.
  // See more about the different wire nodes in Tutorial wire 2.
  // Start of the wire at position:
  wire->add( new agxWire::FreeNode( agx::Vec3( -1.665, 0, 3.33 ) ) );
  // Second route point of the wire:
  wire->add( new agxWire::FreeNode( agx::Vec3( -1.665, 0,    0 ) ) );
  // Third route point:
  wire->add( new agxWire::FreeNode( agx::Vec3(  1.665, 0,    0 ) ) );
  // End of the wire (in total, 10 meters of length):
  wire->add( new agxWire::FreeNode( agx::Vec3(  1.665, 0, 3.33 ) ) );
 
  // Add the wire to the simulation. This will initialize the wire, create constraints etc.
  simulation->add( wire );
 
  // Simulate for one second, the wire will straighten out and fall in gravity.
  simulation->stepTo( 1 );
 
  // Remove the wire from the simulation. This will clean the wire to initial state. So if we add the
  // wire again, it will look as initial configuration.
  simulation->remove( wire );
 
  // Add it again, it should have the look of the routing made above.
  simulation->add( wire );
 
  // We only have debug rendering now of the wire. Enable it in the application.
  application->setEnableDebugRenderer( true );
 
  return root;
}
 
osg::Group* buildTutorial2( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  agx::Real wireRadius( 0.01 );                 // - Radius of the wire.
  agx::Real wireResolutionPerUnitLength( 0.5 ); // - Number of mass elements, used by the wire, per unit length.
                                                //   Example: Unit length is meter. 0.5 means ONE mass element every two meters,
                                                //            and in total 10 * 0.5 = 5 for the whole wire.
  bool collisionsEnabled = true;                // - By default true. If true, the wire will collide with supported shapes. At this moment
                                                //   supported shapes are: Box, Cylinder and Trimesh, and only one shape per geometry.
 
  // First wire is routed with free nodes. Free nodes are only used for initial routing,
  // i.e., the nodes are "useless" after the wire has been initialized.
  {
    // Construct with position given in world frame.
    agxWire::FreeNodeRef beginNode = new agxWire::FreeNode( -5, 0, -2 );
    // 10 meters between begin and end.
    agxWire::FreeNodeRef endNode   = new agxWire::FreeNode(  5, 0, -2 );
 
    // Create wire and add the nodes.
    agxWire::WireRef wire = new agxWire::Wire( wireRadius, wireResolutionPerUnitLength, collisionsEnabled );
    wire->add( beginNode );
    wire->add( endNode );
 
    simulation->add( wire );
 
 
    simulation->add( new RenderWireListener< agxUtil::CardinalSpline >( wire, root ) );
 
    // Alternative (more efficient renderer of a wire:
    //simulation->add( new agxOSG::WireRenderer( wire, root ) );
  }
 
  // Second wire is routed with body fixed nodes. These nodes will keep the wire attached to a rigid body.
  // Unlike free nodes, body fixed nodes are valid (updated) during simulation of the wire.
  {
    // Create one rigid body box.
    agx::RigidBodyRef rbBox = new agx::RigidBody();
    rbBox->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.5, 0.5, 0.1 ) ) ) );
    rbBox->setMotionControl( agx::RigidBody::STATIC );
    rbBox->setPosition( agx::Vec3( -5.5, 0, 0 ) );
 
    agxOSG::createVisual( rbBox, root );
    simulation->add( rbBox );
 
    // Construct body fixed node for rbBox. Position of the node is in rigid body model frame coordinates.
    agxWire::BodyFixedNodeRef beginNode = new agxWire::BodyFixedNode( rbBox, agx::Vec3( 0.5, 0, 0 ) );
    // For the other end, create body fixed node attached to world (rigid body = 0). No reference, so
    // position is in world coordinate frame.
    agxWire::BodyFixedNodeRef endNode = new agxWire::BodyFixedNode( 0L, agx::Vec3( 5, 0, 0 ) );
 
    // Create wire and add nodes.
    agxWire::WireRef wire = new agxWire::Wire( wireRadius, wireResolutionPerUnitLength, collisionsEnabled );
    wire->add( beginNode );
    wire->add( endNode );
 
    simulation->add( wire );
 
    simulation->add( new RenderWireListener< agxUtil::CardinalSpline >( wire, root ) );
  }
 
  // Third wire is routed with contact nodes (begin fixed in world).
  {
    // Create static rigid body box. This box will have routed contact nodes on it.
    agx::RigidBodyRef rbBox = new agx::RigidBody();
    rbBox->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.5, 0.5, 0.1 ) ) ) );
    rbBox->setMotionControl( agx::RigidBody::STATIC );
    rbBox->setPosition( agx::Vec3( 2, 0, 1.9 ) );
 
    agxOSG::createVisual( rbBox, root );
    simulation->add( rbBox );
 
    // Begin node attached in world.
    agxWire::BodyFixedNodeRef beginNode = new agxWire::BodyFixedNode( 0L, agx::Vec3( -5, 0, 2 ) );
    // Create two contact nodes on top box edges. Argument is the geometry, and the position must
    // be given in its shape's coordinate frame. The positions will be corrected so the wire is
    // exactly on the surface. Note that it's up to the wire to remove these nodes. I.e., after
    // initialize (or some time steps) of the wire, you will not know if they are still on this wire.
    agxWire::ContactNodeRef boxNode1 = new agxWire::ContactNode( rbBox->getGeometries().front().get(), agx::Vec3( -0.5, 0, 0.1 ) );
    agxWire::ContactNodeRef boxNode2 = new agxWire::ContactNode( rbBox->getGeometries().front().get(), agx::Vec3(  0.5, 0, 0.1 ) );
    // End node is free.
    agxWire::FreeNodeRef endNode = new agxWire::FreeNode( 5, 0, 2 );
 
    // Create wire and add the nodes in the correct order.
    agxWire::WireRef wire = new agxWire::Wire( wireRadius, wireResolutionPerUnitLength, collisionsEnabled );
    wire->add( beginNode );
    wire->add( boxNode1 );
    wire->add( boxNode2 );
    wire->add( endNode );
 
    simulation->add( wire );
 
    simulation->add( new RenderWireListener< agxUtil::CardinalSpline >( wire, root ) );
  }
 
  // Fourth wire has sliding nodes (EyeNode). Begin and end fixed at boxes.
  {
    // Create two static boxes and the ends.
    agx::RigidBodyRef beginBox = new agx::RigidBody();
    beginBox->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.5, 0.5, 0.5 ) ) ) );
    beginBox->setMotionControl( agx::RigidBody::STATIC );
    beginBox->setPosition( agx::Vec3( -5.5, 0, 4 ) );
 
    agxOSG::createVisual( beginBox, root );
    simulation->add( beginBox );
 
    agx::RigidBodyRef endBox = new agx::RigidBody();
    endBox->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.5, 0.5, 0.5 ) ) ) );
    endBox->setMotionControl( agx::RigidBody::STATIC );
    endBox->setPosition( agx::Vec3( 5.5, 0, 4 ) );
 
    agxOSG::createVisual( endBox, root );
    simulation->add( endBox );
 
    // Create the two sliding boxes.
    agx::RigidBodyRef slidingBox1 = new agx::RigidBody();
    slidingBox1->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.3, 0.2, 0.2 ) ) ) );
    slidingBox1->setPosition( agx::Vec3( -2, 0, 4.2 ) );
 
    agxOSG::createVisual( slidingBox1, root );
    simulation->add( slidingBox1 );
 
    agx::RigidBodyRef slidingBox2 = new agx::RigidBody();
    slidingBox2->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.3, 0.2, 0.2 ) ) ) );
    slidingBox2->setPosition( agx::Vec3( 2, 0, 4.2 ) );
 
    agxOSG::createVisual( slidingBox2, root );
    simulation->add( slidingBox2 );
 
    slidingBox1->getMassProperties()->setMass( 80 );
    slidingBox2->getMassProperties()->setMass( 80 );
 
    // Begin node attached to static begin box.
    agxWire::BodyFixedNodeRef beginNode = new agxWire::BodyFixedNode( beginBox, agx::Vec3( 0.5, 0, 0 ) );
    // One eye (sliding) node on box1 body. Position in rigid body model frame coordinate system.
    agxWire::EyeNodeRef eyeBox1 = new agxWire::EyeNode( slidingBox1, agx::Vec3( 0, 0, 0.3 ) );
    // Two eye (sliding) nodes on box2 body. This to preserve geometry of a wire routed through something physical.
    // Argument, after position, is the vector pointing out the other eye node, from the first.
    agxWire::EyeNodeRef eyeBox2 = new agxWire::EyeNode( slidingBox2, agx::Vec3( -0.1, 0, 0.3 ), agx::Vec3( 0.2, 0, 0 ) );
    // End node attached to static end box.
    agxWire::BodyFixedNodeRef endNode = new agxWire::BodyFixedNode( endBox, agx::Vec3( -0.5, 0, 0 ) );
 
    // Create wire and add nodes.
    agxWire::WireRef wire = new agxWire::Wire( wireRadius, wireResolutionPerUnitLength, collisionsEnabled );
    wire->add( beginNode );
    wire->add( eyeBox1 );
    wire->add( eyeBox2 );
    wire->add( endNode );
 
    simulation->add( wire );
 
    // Set some linear and angular velocity to the sliding boxes to show the difference.
    slidingBox1->setVelocity( agx::Vec3( -0.3, 0, 0 ) );
    slidingBox1->setAngularVelocity( agx::Vec3( 0, 0, 0.5 ) );
 
    slidingBox2->setVelocity( agx::Vec3(  0.3, 0, 0 ) );
    slidingBox2->setAngularVelocity( agx::Vec3( 0, 0, 0.5 ) );
 
    simulation->add( new RenderWireListener< agxUtil::CardinalSpline >( wire, root ) );
  }
 
  // We only have debug rendering now of the wire. Enable it in the application.
  application->setEnableDebugRenderer( true );
 
  return root;
}
 
osg::Group* buildTutorial3( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  // Create static boxes.
  agx::RigidBodyRef rbBox1 = new agx::RigidBody();
  rbBox1->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.5, 0.5, 0.5 ) ) ) );
  rbBox1->setMotionControl( agx::RigidBody::STATIC );
  rbBox1->setPosition( agx::Vec3( -3, 0, 5.5 ) );
  simulation->add( rbBox1 );
  agxOSG::createVisual( rbBox1, root );
 
  agx::RigidBodyRef rbBox2 = new agx::RigidBody();
  rbBox2->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.5, 0.5, 0.5 ) ) ) );
  rbBox2->setMotionControl( agx::RigidBody::STATIC );
  rbBox2->setPosition( agx::Vec3(  0, 0, 5.5 ) );
  simulation->add( rbBox2 );
  agxOSG::createVisual( rbBox2, root );
 
  agx::RigidBodyRef rbBox3 = new agx::RigidBody();
  rbBox3->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.5, 0.5, 0.5 ) ) ) );
  rbBox3->setMotionControl( agx::RigidBody::STATIC );
  rbBox3->setPosition( agx::Vec3(  3, 0, 5.5 ) );
  simulation->add( rbBox3 );
  agxOSG::createVisual( rbBox3, root );
 
  // First wire from rbBox1 down some distance. Controllable wire length.
  {
    agx::Real wireLength = 50; // 50 meters of wire (will check so that's the case after initialize).
    agx::Real wireRadius( 0.025 );
    agx::Real wireResolutionPerUnitLength( 1 );
    agxWire::WireRef wire = new agxWire::Wire( wireRadius, wireResolutionPerUnitLength );
 
    // Create winch controller. Attached to rbBox1, translate and normal in rbBox1 coordinate frame.
    // One has to specify in which direction the winch should haul in/out the wire. In this case it's
    // downwards in rbBox1 body frame.
    agxWire::WireWinchControllerRef winch = new agxWire::WireWinchController( rbBox1, agx::Vec3( 0, 0, -0.5 ), agx::Vec3( 0, 0, -1 ) );
    // Set some parameters to the winch.
    agx::Real maxWinchForce = 1E6;      // - Maximum force, in Newtons, the winch can pull in and out wire. This can also be assigned
                                        //   as a range, i.e., [maxToPull maxToPush] where maxToPullIn is a negative value. Default: infinity.
    agx::Real desiredWinchSpeed = 0.5;  // - Winch desired speed in m/s. Positive for out, negative for in.
    agx::Real brakeForce = 0;           // - The winch is also equipped with a brake, decoupled from the motor. Think of a car where you
                                        //   push the gas- and brake pedals at the same time. Can also be assigned as range,
                                        //   similar to force range above. Default: 0.
 
    winch->setForceRange( maxWinchForce );
    winch->setSpeed( desiredWinchSpeed );
    winch->setBrakeForceRange( brakeForce );
 
    // Route the wire.
 
    // Here it's possible to control the total length of the wire. Tell the winch to take the total length of the wire by:
    winch->setAutoFeed( true ); // Auto feed: The winch will automatically feed out wire during routing. Default: false.
 
    // Add the winch to the wire, the second argument is the amount of wire pulled in to the winch at start.
    wire->add( winch, wireLength );
 
    // All wire, 50 meters, is now in the winch. The winch is located at (-3, 0, 5) (box half extents 0.5, position 5.5, i.e., 5.5 - 0.5 = 5).
    // Complete the route with a free node 10 meters from the winch.
    wire->add( new agxWire::FreeNode( -3, 0, -5 ) );
 
    // Add the wire. Will initialize the wire and assign correct parameters to winches etc.
    simulation->add( wire );
 
    // We expect the rest length to be 50 meters and pulled in length 40.
    std::cout << " * Wire 1 *" << std::endl;
    std::cout << "   - Rest length after initialize:             " << wire->getRestLength() << " m." << std::endl;
    std::cout << "   - Winch pulled in length after initialize:  " << wire->getWinchController( 0 )->getPulledInWireLength( ) << " m." << std::endl << std::endl;
  }
 
  // Second wire from rbBox2 down some distance. Controllable wire length inside winch.
  {
    agx::Real wireLengthInsideWinch = 85; // 85 meters of wire will be inside the winch after initialize of the wire.
    agx::Real wireRadius( 0.025 );
    agx::Real wireResolutionPerUnitLength( 1 );
    agxWire::WireRef wire = new agxWire::Wire( wireRadius, wireResolutionPerUnitLength );
 
    // Winch controller attached to rbBox2.
    agxWire::WireWinchControllerRef winch = new agxWire::WireWinchController( rbBox2, agx::Vec3( 0, 0, -0.5 ), agx::Vec3( 0, 0, -1 ) );
    // Default is 'false' for auto feed, this time we don't want to use the auto feed. We want the winch to have 85 meters from start.
    winch->setAutoFeed( false );
 
    // Route the wire.
 
    // Auto feed is false, and we want 85 meters in the winch. Add winch with 85 meters of wire pulled in.
    wire->add( winch, wireLengthInsideWinch );
 
    // Free node 15 meters from the winch. I.e., total rest length will be 100 meters; 85 in winch and 15 outside.
    wire->add( new agxWire::FreeNode( 0, 0, -10 ) );
 
    // Add the wire. Will initialize the wire and assign correct parameters to winches etc.
    simulation->add( wire );
 
    // We expect the rest length to be 100 meters and pulled in length 85.
    std::cout << " * Wire 2 *" << std::endl;
    std::cout << "   - Rest length after initialize:             " << wire->getRestLength() << " m." << std::endl;
    std::cout << "   - Winch pulled in length after initialize:  " << wire->getWinchController( 0 )->getPulledInWireLength( ) << " m." << std::endl << std::endl;
  }
 
  // Third wire from rbBox3 some distance down to winch in world. Auto feed in both winches.
  {
    agx::Real initialLengthInWinch1 = 20;
    agx::Real initialLengthInWinch2 = 10;
    agx::Real wireRadius( 0.025 );
    agx::Real wireResolutionPerUnitLength( 0.333 );
    agxWire::WireRef wire = new agxWire::Wire( wireRadius, wireResolutionPerUnitLength );
 
    // Winch controller attached to rbBox3.
    agxWire::WireWinchControllerRef winch1 = new agxWire::WireWinchController( rbBox3, agx::Vec3( 0, 0, -0.5 ), agx::Vec3( 0, 0, -1 ) );
    // Winch controller attached in world, points up.
    agxWire::WireWinchControllerRef winch2 = new agxWire::WireWinchController( 0L, agx::Vec3( 3, 0, -5 ), agx::Vec3( 0, 0, 1 ) );
 
    // Both with auto feed.
    winch1->setAutoFeed( true );
    winch2->setAutoFeed( true );
 
    // Route the wire.
 
    // From winch 1...
    wire->add( winch1, initialLengthInWinch1 );
    // ...to winch 2.
    wire->add( winch2, initialLengthInWinch2 );
 
    // Add the wire. Will initialize the wire and assign correct parameters to winches etc.
    simulation->add( wire );
 
    // The initial configuration is not fully defined. We could assume that
    // more wire will be in winch 1 (twice as much as in winch 2, since:
    // initialLengthInWinch1 / initialLengthInWinch2 = 2). Also, the routed
    // length does not exceed 30 m which we have in the winches, so rest length
    // should be 30 (initialLengthInWinch1 + initialLengthInWinch2).
    std::cout << " * Wire 3 *" << std::endl;
    std::cout << "   - Rest length after initialize:               " << wire->getRestLength() << " m." << std::endl;
    std::cout << "   - Winch 1 pulled in length after initialize:  " << wire->getWinchController( 0 )->getPulledInWireLength( ) << " m." << std::endl;
    std::cout << "   - Winch 2 pulled in length after initialize:  " << wire->getWinchController( 1 )->getPulledInWireLength( ) << " m." << std::endl << std::endl;
  }
 
  // We only have debug rendering now of the wire. Enable it in the application.
  application->setEnableDebugRenderer( true );
 
  return root;
}
 
osg::Group* buildTutorial4( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  // We want to rotate the long box and the wire boxes as well. agxSDK::Assembly
  // makes the configuration much easier.
  agxSDK::AssemblyRef assembly = new agxSDK::Assembly();
  simulation->add( assembly );
  // Rotate the assembly.
  assembly->setRotation( agx::Quat( agx::PI_4, agx::Vec3( 1, 0, 0 ) ) );
 
  // Create static long box, rotated some degrees for the wires to slide along.
  agx::RigidBodyRef rbBox = new agx::RigidBody();
  rbBox->setMotionControl( agx::RigidBody::STATIC );
  agxCollide::GeometryRef rbBoxGeometry = new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.5, 4, 0.5 ) ) );
  rbBox->add( rbBoxGeometry );
  // Material for rbBox.
  agx::MaterialRef rbBoxMaterial = new agx::Material( "rbBoxMaterial" );
  // Assign friction coefficients for line contacts.
  rbBoxMaterial->getSurfaceMaterial()->setRoughness( 0.8 );
  rbBoxGeometry->setMaterial( rbBoxMaterial );
 
  // rbBox inherits the transform from the assembly.
  assembly->add( rbBox );
  agxOSG::createVisual( rbBox, root );
 
  // Create two identical wires with some offset.
  agxWire::WireRef wires[ 2 ] = { 0L, 0L };
  for ( size_t i = 0; i < 2; ++i ) {
    wires[ i ] = new agxWire::Wire( 0.015, 10 / agx::Real( 8 - 0.4 ) );
 
    // One small box on each side.
    agx::RigidBodyRef rb1 = new agx::RigidBody();
    agx::RigidBodyRef rb2 = new agx::RigidBody();
    rb1->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.2, 0.2, 0.2 ) ) ) );
    rb2->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.2, 0.2, 0.2 ) ) ) );
 
    rb1->setPosition( agx::Vec3( -4, agx::Real(3 * ( 1 - i )), 0.8 ) );
    rb2->setPosition( agx::Vec3(  4, agx::Real(3 * ( 1 - i )), 0.8 ) );
 
    // Mass and damping for the rigid bodies.
    rb1->getMassProperties()->setMass( 200 );
    rb2->getMassProperties()->setMass( 200 );
 
    rb1->setLinearVelocityDamping( 0.5 );
    rb2->setLinearVelocityDamping( 0.5 );
    rb1->setAngularVelocityDamping( 0.5 );
    rb2->setAngularVelocityDamping( 0.5 );
 
    // rb1 and rb2 inherits the transform from the assembly.
    assembly->add( rb1 );
    assembly->add( rb2 );
 
    agxOSG::createVisual( rb1, root );
    agxOSG::createVisual( rb2, root );
 
    // Route.
    wires[ i ]->add( new agxWire::BodyFixedNode( rb1, agx::Vec3(  0.2, 0, 0 ) ) );
    wires[ i ]->add( new agxWire::BodyFixedNode( rb2, agx::Vec3( -0.2, 0, 0 ) ) );
 
    simulation->add( wires[ i ] );
  }
 
  // There are always a material defined in the wire. I.e., it's always defined to do:
  //   wires[ 0 ]->getMaterial()->getBulkMaterial() etc.
  // Change the current material or create a new and assign it to the wire:
  agx::MaterialRef wire1Material = new agx::Material( "wire1Material" );
  // Set high friction coefficients to wire 1 material.
  wire1Material->getSurfaceMaterial()->setRoughness( 1 );
  // Young's modulus is by default ~steel wire. Decrease the value to get more
  // rubber band effect of the wire.
  wire1Material->getWireMaterial()->setYoungsModulusStretch( 1E8 );
  // Assign the material to the wire.
  wires[ 0 ]->setMaterial( wire1Material );
 
  // For wire 2, we'll create an explicit contact material with rbBoxMaterial.
  agx::ContactMaterial* cm = simulation->getMaterialManager()->getOrCreateContactMaterial( rbBoxMaterial, wires[ 1 ]->getMaterial() );
  // Set friction to zero.
  cm->setFrictionCoefficient( 0 );
  wires[ 1 ]->getMaterial()->getBulkMaterial()->setYoungsModulus( 2E8 );
 
  // We only have debug rendering now of the wire. Enable it in the application.
  application->setEnableDebugRenderer( true );
 
  return root;
}
 
osg::Group* buildTutorial5( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* application )
{
  osg::Group* root = new osg::Group();
 
  agx::RigidBodyRef rbBox1 = new agx::RigidBody();
  rbBox1->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.5, 0.5, 0.5 ) ) ) );
  rbBox1->setMotionControl( agx::RigidBody::STATIC );
  rbBox1->setPosition( agx::Vec3( 10, 0, -5 ) );
  simulation->add( rbBox1 );
  agxOSG::createVisual( rbBox1, root );
 
  agx::RigidBodyRef slidingBox = new agx::RigidBody();
  slidingBox->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 0.3, 0.2, 0.2 ) ) ) );
  slidingBox->setPosition( agx::Vec3( -3, 0, 2.5 ) );
  slidingBox->getMassProperties()->setMass( 200 );
  simulation->add( slidingBox );
  agxOSG::createVisual( slidingBox, root );
 
  // Create wire between fixed node in world and static rbBox1. It's valid to have wires with 0
  // mass nodes per meter.
  agxWire::WireRef wire = new agxWire::Wire( 0.01, 0 );
 
  // First node fixed in world.
  wire->add( new agxWire::BodyFixedNode( 0L, agx::Vec3( -5, 0, 5 ) ) );
 
  // Sliding eye node as second.
  agxWire::EyeNodeRef eye = new agxWire::EyeNode( slidingBox, agx::Vec3( 0, 0.25, 0 ) );
  wire->add( eye );
 
  // Last node on static rbBox1.
  wire->add( new agxWire::BodyFixedNode( rbBox1, agx::Vec3( -0.5, 0, 0 ) ) );
 
  simulation->add( wire );
 
  // Material properties of the eye nodes are not managed through standard agx::Materials, instead
  // directly via the material on the node it self.
 
  // Eye nodes can have direction dependent friction. E.g., infinite friction in one direction (of the wire)
  // and some finite in the other.
 
  // Default direction for friction coefficients are BOTH.
  eye->getMaterial()->setFrictionCoefficient( agx::Real( 1.5 ) );
 
  // Disable collision between the wire and the sliding box. setEnableCollisions takes either a
  // geometry or rigid body. setEnableCollisions( false ) will disable collisions for whole line
  // against any other object.
  wire->setEnableCollisions( slidingBox, false );
 
  // We only have debug rendering now of the wire. Enable it in the application.
  application->setEnableDebugRenderer( true );
 
  return root;
}
 
osg::Group* buildTutorial6( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create two static boxes.
  agx::RigidBodyRef rbBox1 = new agx::RigidBody();
  rbBox1->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 1, 1, 1 ) ) ) );
  rbBox1->setMotionControl( agx::RigidBody::STATIC );
  rbBox1->setPosition( agx::Vec3( 0, 0, 5 ) );
  simulation->add( rbBox1 );
  agxOSG::createVisual( rbBox1, root );
 
  agx::RigidBodyRef rbBox2 = new agx::RigidBody();
  rbBox2->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 1, 1, 1 ) ) ) );
  rbBox2->setMotionControl( agx::RigidBody::STATIC );
  rbBox2->setPosition( agx::Vec3( 105, 0, -15 ) );
  simulation->add( rbBox2 );
  agxOSG::createVisual( rbBox2, root );
 
  // Disable line friction against the boxes.
  agx::MaterialRef dynBoxMaterial = new agx::Material( "dynBoxMaterial" );
  dynBoxMaterial->getSurfaceMaterial()->setRoughness( 0 );
 
  // Two dynamic boxes.
  agx::RigidBodyRef dynBox1 = new agx::RigidBody();
  dynBox1->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 1, 1, 1 ) ) ) );
  agxUtil::setBodyMaterial( dynBox1, dynBoxMaterial );
  dynBox1->setPosition( agx::Vec3( 3, 0, 4.5 ) );
  dynBox1->setRotation( agx::Quat( agx::degreesToRadians( 10 ), agx::Vec3( 0, 1, 0 ) ) );
  simulation->add( dynBox1 );
  agxOSG::createVisual( dynBox1, root );
 
  agx::RigidBodyRef dynBox2 = new agx::RigidBody();
  dynBox2->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 1, 1, 1 ) ) ) );
  agxUtil::setBodyMaterial( dynBox2, dynBoxMaterial );
  dynBox2->setPosition( agx::Vec3( 97, 0, -12.7 ) );
  dynBox2->setRotation( agx::Quat( agx::degreesToRadians( 10 ), agx::Vec3( 0, 1, 0 ) ) );
  simulation->add( dynBox2 );
  agxOSG::createVisual( dynBox2, root );
 
  // Create wire between the static boxes. 10 meters between mass nodes on the wire.
  agxWire::WireRef wire = new agxWire::Wire( 0.02, 0.15 );
 
  // Change some material parameters to make it more rubber band like.
  wire->getMaterial()->getWireMaterial()->setYoungsModulusStretch( 1E9 );
  wire->getMaterial()->getWireMaterial()->setYoungsModulusBend( 1 );
  wire->getMaterial()->getBulkMaterial()->setDensity( 1000 );
  wire->getMaterial()->getSurfaceMaterial()->setRoughness( 0 );
 
  wire->add( new agxWire::BodyFixedNode( rbBox1, agx::Vec3(  0, 0, -1 ) ) );
  wire->add( new agxWire::BodyFixedNode( rbBox2, agx::Vec3( -1, 0,  0 ) ) );
 
  simulation->add( wire );
 
  // Add our renderer.
  //simulation->add( new RenderWireListener< agxUtil::CardinalSpline >( wire, root ) );
 
  return root;
}
 
agxSDK::Assembly* createScene7Structure(agxSDK::Simulation *simulation, agxOSG::ExampleApplication* /*application*/, osg::Group* root, const agx::Bool enableDynamicContacts, agx::Vec3 translate)
{
  agxSDK::Assembly* structure = new agxSDK::Assembly();
 
  // Create static inclined plane.
  agx::RigidBodyRef rbBoxPlane = new agx::RigidBody();
  agx::MaterialRef planeMaterial = new agx::Material("PlaneMaterial");
  agxCollide::GeometryRef planeGeometry = new agxCollide::Geometry(new agxCollide::Box(agx::Vec3(20, 20, 0.5)));
  planeGeometry->setName("Plane1");
  agxWire::WireController::instance()->setEnableDynamicWireContacts(planeGeometry, enableDynamicContacts);
  rbBoxPlane->add(planeGeometry, agx::AffineMatrix4x4::rotate(-agx::PI*0.17, agx::Vec3::Y_AXIS()));
  planeGeometry->setMaterial(planeMaterial);
  rbBoxPlane->setMotionControl(agx::RigidBody::STATIC);
  rbBoxPlane->setPosition(agx::Vec3(10, 0, -10));
  simulation->add(rbBoxPlane);
  agxOSG::createVisual(rbBoxPlane, root);
  structure->add(rbBoxPlane);
 
  // Create static inclined plane.
  agx::RigidBodyRef rbBoxPlane2 = new agx::RigidBody();
  agxCollide::GeometryRef planeGeometry2 = new agxCollide::Geometry(new agxCollide::Box(agx::Vec3(20, 20, 0.5)));
  planeGeometry2->setName("Plane2");
  agxWire::WireController::instance()->setEnableDynamicWireContacts(planeGeometry2, enableDynamicContacts);
  rbBoxPlane2->add(planeGeometry2, agx::AffineMatrix4x4::rotate(agx::PI*0.08, agx::Vec3::Y_AXIS()));
  planeGeometry2->setMaterial(planeMaterial);
  rbBoxPlane2->setMotionControl(agx::RigidBody::STATIC);
  rbBoxPlane2->setPosition(agx::Vec3(0, 0, -17));
  simulation->add(rbBoxPlane2);
  agxOSG::createVisual(rbBoxPlane2, root);
  structure->add(rbBoxPlane2);
 
 
  // Create a body from where a wire is routed
  agx::RigidBodyRef rbBoxWinch = new agx::RigidBody();
  rbBoxWinch->add(new agxCollide::Geometry(new agxCollide::Box(agx::Vec3(0.1, 0.1, 0.5))));
  rbBoxWinch->setMotionControl(agx::RigidBody::STATIC);
  rbBoxWinch->setPosition(agx::Vec3(-5, 0, 5.5));
  simulation->add(rbBoxWinch);
  agxOSG::createVisual(rbBoxWinch, root);
  structure->add(rbBoxWinch);
 
 
  // Create a trimesh object
  agx::RigidBodyRef rbPipeMesh = new agx::RigidBody();
  agx::MaterialRef pipeMaterial = new agx::Material("PipeMaterial");
  agx::ref_ptr<agxCollide::Trimesh> pipe = agxUtil::TrimeshReaderWriter::createTrimesh("pipe.obj", 64U, agx::Matrix3x3(agx::Vec3(3, 3, 6)));
  agxAssert(pipe);
  agxCollide::GeometryRef pipeGeom = new agxCollide::Geometry(pipe);
  pipeGeom->setName("Pipe");
 
  agxWire::WireController::instance()->setEnableDynamicWireContacts(pipeGeom, enableDynamicContacts);
 
 
  pipeGeom->setMaterial(pipeMaterial);
  rbPipeMesh->add(pipeGeom, agx::AffineMatrix4x4::rotate(-agx::PI_2, agx::Vec3::X_AXIS()));
  rbPipeMesh->setPosition(agx::Vec3(22.0, -7.0, 2.0));
  simulation->add(rbPipeMesh);
  agxOSG::createVisual(rbPipeMesh, root);
  structure->add(rbPipeMesh);
 
  agx::Real wireLength = 50; // 50 meters of wire (will check so that's the case after initialize).
  agx::Real wireRadius(0.025);
  agx::Real wireResolutionPerUnitLength(1);
 
  agxWire::WireRef wire = new agxWire::Wire(wireRadius, wireResolutionPerUnitLength);
 
  agx::MaterialRef wireMaterial = new agx::Material("WireMaterial");
  wire->setMaterial(wireMaterial);
 
  // Create winch controller. Attached to rbBox1, translate and normal in rbBox1 coordinate frame.
  // One has to specify in which direction the winch should haul in/out the wire. In this case it's
  // downwards in rbBox1 body frame.
  agxWire::WireWinchControllerRef winch = new agxWire::WireWinchController(rbBoxWinch, agx::Vec3(0, 0, -0.5), agx::Vec3(0, 0, -1));
  // Set some parameters to the winch.
  agx::Real maxWinchForce = 1E6;      // - Maximum force, in Newtons, the winch can pull in and out wire. This can also be assigned
                                      //   as a range, i.e., [maxToPull maxToPush] where maxToPullIn is a negative value. Default: infinity.
  agx::Real desiredWinchSpeed = 0.5;  // - Winch desired speed in m/s. Positive for out, negative for in.
  agx::Real brakeForce = 0;           // - The winch is also equipped with a brake, decoupled from the motor. Think of a car where you
                                      //   push the gas- and brake pedals at the same time. Can also be assigned as range,
                                      //   similar to force range above. Default: 0.
 
  winch->setForceRange(maxWinchForce);
  winch->setSpeed(desiredWinchSpeed);
  winch->setBrakeForceRange(brakeForce);
 
  // Route the wire.
 
  // Here it's possible to control the total length of the wire. Tell the winch to take the total length of the wire by:
  winch->setAutoFeed(true); // Auto feed: The winch will automatically feed out wire during routing. Default: false.
 
                            // Add the winch to the wire, the second argument is the amount of wire pulled in to the winch at start.
  wire->add(winch, wireLength);
 
 
  // All wire, 50 meters, is now in the winch. The winch is located at (-3, 0, 5) (box half extents 0.5, position 5.5, i.e., 5.5 - 0.5 = 5).
  // Complete the route with a free node 10 meters from the winch.
  wire->add(new agxWire::FreeNode(agx::Vec3(-5, 0, -15) + translate));
 
  wire->add(new agxWire::BodyFixedNode(rbPipeMesh, 0.5, 5.0, -2.5));
 
  structure->add(wire);
 
  structure->setPosition(translate);
 
  simulation->add(structure);
 
  // Add our renderer.
  agxOSG::WireRendererRef wr = new agxOSG::WireRenderer(wire, root);
 
  wr->setColor(agx::Vec4f(1, 0, 0, 1));
 
  simulation->add(wr);
 
 
  agx::ContactMaterialRef cmWirePipe = simulation->getMaterialManager()->getOrCreateContactMaterial(wireMaterial, pipeMaterial);
  cmWirePipe->setRestitution(0);
  agx::ContactMaterialRef cmPlanePipe = simulation->getMaterialManager()->getOrCreateContactMaterial(planeMaterial, pipeMaterial);
  cmPlanePipe->setFrictionCoefficient(2);
  return structure;
}
 
osg::Group* buildTutorial7(agxSDK::Simulation *simulation, agxOSG::ExampleApplication* application)
{
  osg::Group* root = new osg::Group();
 
 
  //Create rolling trimesh that generates old kinematic wire contacts.
 
  // Use old contact model
  agx::Bool useDynamicContacts = true;
 
  agx::Vec3 translate = agx::Vec3(0, -25, 0);
 
  agxSDK::AssemblyRef s1 = createScene7Structure(simulation, application, root, !useDynamicContacts, -translate);
  simulation->add(s1);
 
  //Create rolling trimesh that generates new kinematic wire contacts that can become dynamic if the tension is low enough.
 
  //Use the dynamic contact model
  agxSDK::AssemblyRef s2 = createScene7Structure(simulation, application, root, useDynamicContacts, translate);
 
  // We only have debug rendering now of the wire.
  // Look at it to see the difference in how the wire is glued to the trimesh in the old kinematic model
  // While it is better dynamic behavior using the
  application->setEnableDebugRenderer(true);
 
  return root;
}
 
agx::Vec3Vector createConvexPoints( agx::Int numPoints, agx::Real radius, agx::Vec3 center, agx::Vec3 scaling )
{
  agx::Vec3Vector convexPoints;
  agx::Real angle = 0;
  agx::Real step = 2 * M_PI / static_cast<agx::Real>(numPoints);
  for( agx::Int i = 0; i < numPoints; i++ ) {
    convexPoints.push_back( agx::Vec3( radius * std::cos( angle ) * scaling.x(), 0, radius * std::sin( angle ) * scaling.z() ) + center );
    angle += step;
  }
  return convexPoints;
}
 
void scaleMesh( const agx::Vec3Vector& vertices, const agx::Vec3 scale )
{
  for( agx::UInt i = 0, numVertices = vertices.size(); i < numVertices; ++i )
    vertices[ i ] = agx::Vec3::mul( vertices[ i ], scale );
}
 
 
osg::Group* buildTutorial8( agxSDK::Simulation *simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Create a static box that will hold the starting point of the two wires.
  agx::RigidBodyRef rbBox = new agx::RigidBody();
  rbBox->add( new agxCollide::Geometry( new agxCollide::Box( agx::Vec3( 1, 1, 1 ) ) ) );
  rbBox->setMotionControl( agx::RigidBody::STATIC );
  simulation->add( rbBox );
  agxOSG::createVisual( rbBox, root );
 
  agx::Real wireRadius = 0.02;
 
  {
    // Create a rigid body that will be associated with the EyeNodeArea. The geometry is created from a mesh model.
    agx::RigidBodyRef eyeNodeAreaBody = new agx::RigidBody( new agxCollide::Geometry( agxUtil::TrimeshReaderWriter::createTrimesh( "models/torusForCollision.obj" ) ) );
    simulation->add( eyeNodeAreaBody );
    eyeNodeAreaBody->setPosition( -5, -5, 0 );
    simulation->add( new agx::LockJoint( eyeNodeAreaBody ) );
    agxOSG::setDiffuseColor( agxOSG::createVisual( eyeNodeAreaBody, root ), agxRender::Color::DarkGreen() );
 
    // Create a weight that will be attached to the end of the wire.
    agx::RigidBodyRef weight = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 0.5, 0.5, 0.5 ) ) );
    weight->setPosition( -15, -10, 0 );
    simulation->add( weight );
    agxOSG::createVisual( weight, root );
 
    // Create a body to be attached to the eye. Is should be placed somewhere inside the constraining area.
    agx::RigidBodyRef eyeBody = new agx::RigidBody();
    simulation->add( eyeBody );
    eyeBody->setPosition( eyeNodeAreaBody->getPosition() );
 
    // Create the wire.
    agxWire::WireRef wire = new agxWire::Wire( wireRadius, 10 );
    wire->add( new agxWire::BodyFixedNode( rbBox, agx::Vec3( -0.25, -( 1 + wireRadius ), 0 ) ) );
    agxWire::EyeNodeRef eyeNode = new agxWire::EyeNode( eyeBody, agx::Vec3( 0, 0, 0 ) );
    wire->add( eyeNode );
    wire->add( new agxWire::BodyFixedNode( weight, agx::Vec3( 0, 0, 0.5 + wireRadius ) ) );
    simulation->add( wire );
 
    //Render the wire
    agxOSG::WireRendererRef wireRenderer = new agxOSG::WireRenderer( wire, root );
    simulation->add( wireRenderer );
 
    // Create a circular area that will define the constraining area. The position and direction will define a plane
    // in body coordinates. The radius then defines the edge of the area in which the eyeNode will exist.
    agxWire::AreaDefinitionRef area = new agxWire::CircularAreaDefinition( eyeNodeAreaBody->getCmLocalTranslate(), 0.8, eyeNodeAreaBody, agx::Vec3( 0, 1, 0 ), eyeNode );
    agxWire::EyeNodeAreaRef eyeNodeArea = new agxWire::EyeNodeArea( area );
    simulation->add( eyeNodeArea );
  }
 
 
  {
    //Read the mesh and scale it to produce a oval mesh.
    agxIO::MeshReaderRef meshReader = new agxIO::MeshReader();
    meshReader->readFile( "models/torusForCollision.obj" );
    agx::Vec3 scaling( 0.5, 1, 1.5 );
    ::scaleMesh( meshReader->getVertices(), scaling );
 
    // Create a rigid body that will be associated with the EyeNodeArea.
    agx::RigidBodyRef eyeNodeAreaBody = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Trimesh( &meshReader->getVertices(), &meshReader->getIndices(), "torus" ) ) );
    simulation->add( eyeNodeAreaBody );
    eyeNodeAreaBody->setPosition( 5, -4, 0 );
    simulation->add( new agx::LockJoint( eyeNodeAreaBody ) );
    agxOSG::setDiffuseColor( agxOSG::createVisual( eyeNodeAreaBody, root ), agxRender::Color::DarkRed() );
 
    // Create a weight that will be attached to the end of the wire.
    agx::RigidBodyRef weight = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 0.5, 0.5, 0.5 ) ) );
    weight->setPosition( 15, -10, 0 );
    simulation->add( weight );
    agxOSG::createVisual( weight, root );
 
    // Create a body to be attached to the eye. Is should be placed somewhere inside the constraining area.
    agx::RigidBodyRef eyeBody = new agx::RigidBody();
    simulation->add( eyeBody );
    eyeBody->setPosition( eyeNodeAreaBody->getPosition() );
 
    // Create the wire
    agxWire::WireRef wire = new agxWire::Wire( wireRadius, 10 );
    wire->add( new agxWire::BodyFixedNode( rbBox, agx::Vec3( 0.25, -( 1 + wireRadius ), 0 ) ) );
    agxWire::EyeNodeRef eyeNode = new agxWire::EyeNode( eyeBody, agx::Vec3( 0, 0, 0 ) );
    wire->add( eyeNode );
    wire->add( new agxWire::BodyFixedNode( weight, agx::Vec3( 0, 0, 0.5 + wireRadius ) ) );
    simulation->add( wire );
 
    // Render the wire.
    agxOSG::WireRendererRef wireRenderer = new agxOSG::WireRenderer( wire, root );
    simulation->add( wireRenderer );
 
    // Create a number of points in body coordinates that will define the edges of the constraining area.
    auto convexPoints = createConvexPoints( 10, 0.8, eyeNodeAreaBody->getCmLocalTranslate(), scaling );
 
    // Create a convex area that will define the constraining area. The position and direction will define a plane
    // in body coordinates. The convex points then defines the edge of the area in which the eyeNode will exist.
    agxWire::AreaDefinitionRef area = new agxWire::ConvexAreaDefinition( eyeNodeAreaBody->getCmLocalTranslate(), convexPoints, eyeNodeAreaBody, agx::Vec3( 0, 1, 0 ), eyeNode );
    agxWire::EyeNodeAreaRef eyeNodeArea = new agxWire::EyeNodeArea( area );
    simulation->add( eyeNodeArea );
  }
 
  return root;
}
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout << "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
               "\tTutorial wire\n" <<
               "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
    application->addScene( buildTutorial3, '3' );
    application->addScene( buildTutorial4, '4' );
    application->addScene( buildTutorial5, '5' );
    application->addScene( buildTutorial6, '6');
    application->addScene( buildTutorial7, '7' );
    application->addScene( buildTutorial8, '8' );
 
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_wireLink.cpp

/*
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.
*/
 
/*
 
Demonstrates how to model composite wires with links.
 
*/
 
#include <agxOSG/ExampleApplication.h>
#include <agxOSG/Node.h>
#include <agxOSG/WireRenderer.h>
#include <agxOSG/utils.h>
 
#include <agxWire/Link.h>
#include <agxWire/Winch.h>
 
#include <agxRender/Color.h>
 
#include <agx/LockJoint.h>
#include <agx/Hinge.h>
 
#include <agxCollide/Box.h>
 
namespace
{
  agxWire::LinkRef createLink()
  {
    return new agxWire::Link( new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 0.3, 0.15, 0.15 ) ) ) );
  }
 
  void addWire( agxWire::Wire* wire, agxSDK::Simulation* simulation, osg::Group* root, const agx::Vec4f& color = agx::Vec4f( 1, 1, 1, 1 ) )
  {
    simulation->add( wire );
    if ( root != 0L ) {
      agxOSG::WireRendererRef wireRenderer = new agxOSG::WireRenderer( wire, root );
      wireRenderer->setColor( color );
      simulation->add( wireRenderer );
    }
  }
}
 
osg::Group* buildScene1( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Two wire segments, radius = 0.015 and resolution per meter = 2.0.
  agxWire::WireRef wire1 = new agxWire::Wire( 0.015, 2.0 );
  agxWire::WireRef wire2 = new agxWire::Wire( 0.015, 2.0 );
 
  // Create and position the link.
  agxWire::LinkRef link = ::createLink();
  link->getRigidBody()->setPosition( 0, 0, 2 );
 
  // Connect the wires.
  agxWire::Link::connect( wire1, agx::Vec3( -0.3, 0, 0 ), link, wire2, agx::Vec3( 0.3, 0, 0 ) );
  // EQUIVALENT:
  //   link->connect( wire1, agx::Vec3( -0.3, 0, 0 ), agxWire::Link::WIRE_END );
  //   link->connect( wire2, agx::Vec3( 0.3, 0, 0 ), agxWire::Link::WIRE_BEGIN );
 
  // Route wire 1 from (-10, 0, 2) to the link.
  wire1->add( new agxWire::FreeNode( -10, 0, 2 ) );
  wire1->add( link );
 
  // Continue route of wire2 from link through some free nodes.
  wire2->add( link );
  wire2->add( new agxWire::FreeNode(  3, 0,  5 ) );
  wire2->add( new agxWire::FreeNode(  6, 0, -3 ) );
  wire2->add( new agxWire::FreeNode( 10, 0,  3 ) );
 
  // Add the wires to the simulation.
  simulation->add( wire1 );
  simulation->add( wire2 );
 
  return root;
}
 
osg::Group* buildScene2( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Three wire segments.
  agxWire::WireRef wire1 = new agxWire::Wire( 0.015, 2.0 );
  agxWire::WireRef wire2 = new agxWire::Wire( 0.045, 2.0 );
  agxWire::WireRef wire3 = new agxWire::Wire( 0.025, 2.0 );
 
  // Create and position the links.
  agxWire::LinkRef beginLink = ::createLink();
  agxWire::LinkRef link12    = ::createLink();
  agxWire::LinkRef link23    = ::createLink();
  agxWire::LinkRef endLink   = ::createLink();
 
  beginLink->getRigidBody()->setPosition( -15, 0, 3 );
  link12->getRigidBody()->setPosition( -5, 0, 0 );
  link23->getRigidBody()->setPosition( 5, 0, 5 );
  endLink->getRigidBody()->setPosition( 12, 0, -3 );
 
  // Link-wire relation setup:
  // Connect wire1 to beginLink...
  beginLink->connect( wire1, agx::Vec3( 0.3, 0, 0 ), agxWire::Link::WIRE_BEGIN );
  // ...and connect wire1 and wire2 to link12...
  agxWire::Link::connect( wire1, agx::Vec3( -0.3, 0, 0 ), link12, wire2, agx::Vec3( 0.3, 0, 0 ) );
  // ...and connect wire2 and wire3 to link23...
  agxWire::Link::connect( wire2, agx::Vec3( -0.3, 0, 0 ), link23, wire3, agx::Vec3( 0.3, 0, 0 ) );
  // ...and finalize the route to the endLink.
  endLink->connect( wire3, agx::Vec3( -0.3, 0, 0 ), agxWire::Link::WIRE_END );
 
  // Routing:
  wire1->add( beginLink );
  wire1->add( link12 );
 
  wire2->add( link12 );
  wire2->add( link23 );
 
  wire3->add( link23 );
  wire3->add( endLink );
 
  simulation->add( wire1 );
  simulation->add( wire2 );
  simulation->add( wire3 );
 
  return root;
}
 
osg::Group* buildScene3( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Two wire segments, radius = 0.015 and resolution per meter = 2.0.
  agxWire::WireRef wire1 = new agxWire::Wire( 0.015, 2.0 );
  agxWire::WireRef wire2 = new agxWire::Wire( 0.015, 2.0 );
 
  // Create and position the link.
  agxWire::LinkRef link = ::createLink();
  link->getRigidBody()->setPosition( 0, 0, 2 );
 
  // Connect wire1 to wire2 via the link.
  agxWire::Link::connect( wire1, agx::Vec3( -0.3, 0, 0 ), link, wire2, agx::Vec3( 0.3, 0, 0 ) );
 
  // Create winch rigid body.
  agx::RigidBodyRef winchRb = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 1, 1, 4 ) ) );
  winchRb->setPosition( agx::Real( -8 ), agx::Real( 0 ), agx::Real( 0 ) );
  winchRb->setMotionControl( agx::RigidBody::STATIC );
  simulation->add( winchRb );
 
  // Important to specify the direction of the winch, "in which direction
  // is the wire paid out?".
  const agx::Real winchPulledInLength = 50;
  agxWire::WinchRef winch = new agxWire::Winch( winchRb, agx::Vec3( 1, 0, 3 ), agx::Vec3::X_AXIS(), winchPulledInLength );
 
  // Add the winch to the wire. 50 meter of wire1 will initially be on the winch.
  wire1->add( winch );
  wire1->add( link );
 
  // Continue route of wire2.
  wire2->add( link );
  wire2->add( new agxWire::FreeNode(  3, 0,  5 ) );
  wire2->add( new agxWire::FreeNode(  6, 0, -3 ) );
  wire2->add( new agxWire::FreeNode( 10, 0,  3 ) );
 
  // Add the wires to the simulation.
  simulation->add( wire1 );
  simulation->add( wire2 );
 
  winch->setSpeed( agx::Real( -0.75 ) );
 
  return root;
}
 
osg::Group* buildScene4( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  agx::RigidBodyRef ground = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 8, 4, 0.5 ) ) );
  ground->setMotionControl( agx::RigidBody::STATIC );
  ground->setPosition( 0, 0, -0.5 );
  simulation->add( ground );
 
  // Three completely pulled in wires and two active.
  agxWire::WireRef pulledInWire1 = new agxWire::Wire( 0.056, 0.5 );
  agxWire::WireRef pulledInWire2 = new agxWire::Wire( 0.035, 0.5 );
  agxWire::WireRef pulledInWire3 = new agxWire::Wire( 0.015, 0.5 );
  agxWire::WireRef activeWire1   = new agxWire::Wire( 0.025, 0.5 );
  agxWire::WireRef activeWire2   = new agxWire::Wire( 0.012, 0.5 );
 
  // Create the winch (in world rb == 0L, at position (-5, 0, 4)).
  agxWire::WinchRef winch = new agxWire::Winch( 0L, agx::Vec3( -10, 0, 2 ), agx::Vec3::X_AXIS() );
 
  // NOTE:
  //   The length of an agxWire::Wire object is defined by its route and the
  //   amount pulled into a winch. I.e., the wire itself doesn't have a predefined
  //   length so the winch takes care of the lengths of uninitialized/non-routed wires.
 
  // Add the inactive segments to the winch, specifying the different lengths of the segments.
  winch->add( pulledInWire1, 150 ); // First pulled in wire with length 150 m.
  winch->add( pulledInWire2,  80 ); // Second pulled in wire with length 80 m.
  winch->add( pulledInWire3, 250 ); // Third pulled in wire with length 250 m.
 
  // Create the links.
  agxWire::LinkRef pulledInLink12 = ::createLink(); // Links pulledInWire1 <-> pulledInWire2.
  agxWire::LinkRef pulledInLink23 = ::createLink(); // Links pulledInWire2 <-> pulledInWire3.
  agxWire::LinkRef pulledInLink31 = ::createLink(); // Links pulledInWire3 <-> activeWire1.
  agxWire::LinkRef activeLink12   = ::createLink(); // Links activeWire1 <-> activeWire2.
 
  // Connect the pulled in wires with the pulled in links.
  agxWire::Link::connect( pulledInWire1, agx::Vec3( -0.3, 0, 0 ), pulledInLink12, pulledInWire2, agx::Vec3( 0.3, 0, 0 ) );
  agxWire::Link::connect( pulledInWire2, agx::Vec3( -0.3, 0, 0 ), pulledInLink23, pulledInWire3, agx::Vec3( 0.3, 0, 0 ) );
  agxWire::Link::connect( pulledInWire3, agx::Vec3( -0.3, 0, 0 ), pulledInLink31, activeWire1, agx::Vec3( 0.3, 0, 0 ) );
 
  // Connect active wire 1 and 2.
  agxWire::Link::connect( activeWire1, agx::Vec3( -0.3, 0, 0 ), activeLink12, activeWire2, agx::Vec3( 0.3, 0, 0 ) );
 
  // Position the active link. The pulled in links will be inactive.
  activeLink12->getRigidBody()->setPosition( 8, 0, 1 );
 
  // Attach first active wire to the which with 50 meter extra pulled in length.
  //winch->attach( activeWire1, 50 );
  activeWire1->add( winch, 5 );
  activeWire1->add( activeLink12 );
 
  // Finalize the route.
  activeWire2->add( activeLink12 );
  activeWire2->add( new agxWire::FreeNode( 10, 0, 4 ) );
 
  // Add the active wires.
  simulation->add( activeWire1 );
  simulation->add( activeWire2 );
 
  winch->setSpeed( agx::Real( 1 ) );
 
  // Define a high-resolution range going from the winch and ends 5 meters along the wire.
  winch->getStopNode()->getHighResolutionRange()->set( agx::Real( 0 ), agx::Real( 5 ), agx::Real( 2 ) );
 
  return root;
}
 
osg::Group* buildScene5( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Wires with identical radius.
  const agx::Real radius = agx::Real( 0.02 );
  agxWire::WireRef wire1 = new agxWire::Wire( radius, agx::Real( 2 ) );
  agxWire::WireRef wire2 = new agxWire::Wire( radius, agx::Real( 2 ) );
  agxWire::WireRef wire3 = new agxWire::Wire( radius, agx::Real( 2 ) );
 
  // Three links, one for each wire.
  agxWire::LinkRef link1 = createLink();
  agxWire::LinkRef link2 = createLink();
  agxWire::LinkRef link3 = createLink();
 
  // Route from the links to some distance away. Offset along y for the three wires.
  const agx::Real yOffset = agx::Real( 1 );
 
  link1->getRigidBody()->setPosition( 0, -yOffset, 0 );
  link2->getRigidBody()->setPosition( 0,        0, 0 );
  link3->getRigidBody()->setPosition( 0,  yOffset, 0 );
 
  wire1->add( link1, agx::Vec3( 0.3, 0, 0 ) );
  wire1->add( new agxWire::FreeNode( 3, -yOffset, 0 ) );
 
  wire2->add( link2, agx::Vec3( 0.3, 0, 0 ) );
  wire2->add( new agxWire::FreeNode( 3,        0, 0 ) );
 
  wire3->add( link3, agx::Vec3( 0.3, 0, 0 ) );
  wire3->add( new agxWire::FreeNode( 3,  yOffset, 0 ) );
 
  ::addWire( wire1, simulation, root, agxRender::Color::Red() );
  ::addWire( wire2, simulation, root, agxRender::Color::Green() );
  ::addWire( wire3, simulation, root, agxRender::Color::Blue() );
 
  // Rendering of the links and attach them in world with a lock joint.
  agxOSG::createVisual( link1->getRigidBody(), root );
  agxOSG::createVisual( link2->getRigidBody(), root );
  agxOSG::createVisual( link3->getRigidBody(), root );
 
  simulation->add( new agx::LockJoint( link1->getRigidBody() ) );
  simulation->add( new agx::LockJoint( link2->getRigidBody() ) );
  simulation->add( new agx::LockJoint( link3->getRigidBody() ) );
 
  // Fetch the link nodes, containing the connection properties.
  agxWire::ILinkNode* link1Node = link1->getConnectingNode( wire1 );
  agxWire::ILinkNode* link2Node = link2->getConnectingNode( wire2 );
  agxWire::ILinkNode* link3Node = link3->getConnectingNode( wire3 );
 
  link1Node->getConnectionProperties()->setBendStiffness( agx::Real( 1.0E12 ) );
  link2Node->getConnectionProperties()->setBendStiffness( agx::Real( 1.0E10 ) );
  link3Node->getConnectionProperties()->setBendStiffness( agx::Real( 1.0E9 ) );
 
  return root;
}
 
osg::Group* buildScene6( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  // Same concept as previous tutorial, but in this we'll change the radius of the wires
  // with constant bend connection stiffness.
  const agx::Real radius = agx::Real( 0.02 );
  agxWire::WireRef wire1 = new agxWire::Wire( agx::Real( 1.0 ) * radius, agx::Real( 2 ) );
  agxWire::WireRef wire2 = new agxWire::Wire( agx::Real( 2.0 ) * radius, agx::Real( 2 ) );
  agxWire::WireRef wire3 = new agxWire::Wire( agx::Real( 3.0 ) * radius, agx::Real( 2 ) );
 
  // Three links, one for each wire.
  agxWire::LinkRef link1 = createLink();
  agxWire::LinkRef link2 = createLink();
  agxWire::LinkRef link3 = createLink();
 
  // Set default wire connection properties of the links. Any
  // connection AFTER this change will inherit these values.
  const agx::Real connectionStiffness = agx::Real( 1.0E11 );
  link1->getDefaultWireConnectionProperties()->setBendStiffness( connectionStiffness );
  link2->getDefaultWireConnectionProperties()->setBendStiffness( connectionStiffness );
  link3->getDefaultWireConnectionProperties()->setBendStiffness( connectionStiffness );
 
  const agx::Real yOffset = agx::Real( 1 );
  link1->getRigidBody()->setPosition( 0, -yOffset, 0 );
  link2->getRigidBody()->setPosition( 0,        0, 0 );
  link3->getRigidBody()->setPosition( 0,  yOffset, 0 );
 
  wire1->add( link1, agx::Vec3( 0.3, 0, 0 ) );
  wire1->add( new agxWire::FreeNode( 5, -yOffset, 0 ) );
 
  wire2->add( link2, agx::Vec3( 0.3, 0, 0 ) );
  wire2->add( new agxWire::FreeNode( 5, 0, 0 ) );
 
  wire3->add( link3, agx::Vec3( 0.3, 0, 0 ) );
  wire3->add( new agxWire::FreeNode( 5, yOffset, 0 ) );
 
  // NOTE: The wires are connected to the links by now, and changing the
  //       default properties now, will not affect the currently attached
  //       wires.
 
  ::addWire( wire1, simulation, root, agxRender::Color::Red() );
  ::addWire( wire2, simulation, root, agxRender::Color::Green() );
  ::addWire( wire3, simulation, root, agxRender::Color::Blue() );
 
  // Rendering of the links and attach them in world.
  agxOSG::createVisual( link1->getRigidBody(), root );
  agxOSG::createVisual( link2->getRigidBody(), root );
  agxOSG::createVisual( link3->getRigidBody(), root );
 
  simulation->add( new agx::LockJoint( link1->getRigidBody() ) );
  simulation->add( new agx::LockJoint( link2->getRigidBody() ) );
  simulation->add( new agx::LockJoint( link3->getRigidBody() ) );
 
  return root;
}
 
osg::Group* buildScene7( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group();
 
  agx::RigidBodyRef ground = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 8, 4, 0.5 ) ) );
  ground->setMotionControl( agx::RigidBody::STATIC );
  ground->setPosition( 0, 0, -0.5 );
  simulation->add( ground );
  agxOSG::createVisual( ground, root );
 
  // Two wires and two links.
  const agx::Real radius = agx::Real( 0.015 );
  agxWire::WireRef wire1 = new agxWire::Wire( radius, agx::Real( 2 ) );
  agxWire::WireRef wire2 = new agxWire::Wire( radius, agx::Real( 2 ) );
  agxWire::LinkRef link1 = ::createLink();
  agxWire::LinkRef link2 = ::createLink();
 
  // Position the links close to each other.
  link1->getRigidBody()->setPosition( -0.3, 0, 1 );
  link2->getRigidBody()->setPosition(  0.3, 0, 1 );
 
  // Route wire1 from left to link1.
  wire1->add( new agxWire::FreeNode( -4, 0, 1 ) );
  wire1->add( link1, agx::Vec3( -0.3, 0, 0 ) );
 
  // Route wire2 from link2 to right.
  wire2->add( link2, agx::Vec3( 0.3, 0, 0 ) );
  wire2->add( new agxWire::FreeNode( 6, 0, 1 ) );
 
  // Connect link1 to link2 with a Hinge.
  link1->connect( link2, agx::Constraint::createFromBody< agx::Hinge >( agx::Vec3( 0.3, 0, 0 ), agx::Vec3::X_AXIS(), link1->getRigidBody(), link2->getRigidBody() ) );
 
  ::addWire( wire1, simulation, root, agxRender::Color::Red() );
  ::addWire( wire2, simulation, root, agxRender::Color::Green() );
 
  agxOSG::setDiffuseColor( agxOSG::createVisual( link1->getRigidBody(), root ), agxRender::Color::DarkRed() );
  agxOSG::setDiffuseColor( agxOSG::createVisual( link2->getRigidBody(), root ), agxRender::Color::DarkGreen() );
 
  // The two links are now connected and interface for this is
  // the same as for wires.
  agxWire::ILinkNode* link1ToLink2Connection = link1->getConnectingNode( link2 );
  agxWire::ILinkNode* link2ToLink1Connection = link2->getConnectingNode( link1 );
 
  // These two nodes aren't the same node. Each link has their own.
  if ( link2ToLink1Connection != link1ToLink2Connection )
    std::cout << "Expected: The nodes are not the same." << std::endl;
  else
    std::cout << "THE NODES ARE SAME?!" << std::endl;
 
  // It's possible to for example find link2ToLink1Connection from link1ToLink2Connection.
  agxWire::ILinkNode* thisIsLink2ToLink1Connection = link1ToLink2Connection->getLinkConnection();
  if ( thisIsLink2ToLink1Connection == link2ToLink1Connection )
    std::cout << "Expected: link2ToLink1Connection FOUND!" << std::endl;
  else
    std::cout << "link2ToLink1Connection LOST?!" << std::endl;
 
  // The hinge we created earlier can be accessed as well.
  agx::Hinge* h1 = link1->getConnectingConstraint( link2 )->as< agx::Hinge >();
  agx::Hinge* h2 = link2->getConnectingConstraint( link1 )->as< agx::Hinge >();
 
  // For convenience, h1 is h2 and this constraint can of course be of any type.
  if ( h1 == h2 )
    std::cout << "Expected: Access to the constraint from both links works!" << std::endl;
  else
    std::cout << "Where are the constraints?!" << std::endl;
 
  // Enable the range.
  h1->getRange1D()->setEnable( true );
  // h1 or h2, doesn't matter, it's the same constraint.
  h2->getRange1D()->setRange( -agx::PI_4, agx::PI_4 );
 
  link1->getRigidBody()->setAngularVelocity( 2.5, 0, 0 );
 
  return root;
}
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
 
  agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
  application->addScene( buildScene1, '1' );
  application->addScene( buildScene2, '2' );
  application->addScene( buildScene3, '3' );
  application->addScene( buildScene4, '4' );
  application->addScene( buildScene5, '5' );
  application->addScene( buildScene6, '6' );
  application->addScene( buildScene7, '7' );
 
  try {
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}

tutorial_wireWindAndWater.cpp

/*
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.
*/
 
#include <agxOSG/ExampleApplication.h>
 
#include <agxModel/WindAndWaterController.h>
 
#include <agxWire/Wire.h>
#include <agxOSG/WireRenderer.h>
 
#include <agxRender/Color.h>
 
#include "tutorialUtils.h"
 
 
namespace
{
  agxOSG::GeometryNode* createWaterVisual( agxCollide::Geometry* waterGeometry, osg::Group* root )
  {
    agxOSG::GeometryNode* node = agxOSG::createVisual( waterGeometry, root );
 
    const agx::Vec4f color = agxRender::Color::DeepSkyBlue();
 
    agxOSG::setDiffuseColor( node, color );
    agxOSG::setAmbientColor( node, agx::Vec4f( 0.2f ) );
    agxOSG::setSpecularColor( node, agx::Vec4f( 0.4f ) );
    agxOSG::setShininess( node, 120 );
    agxOSG::setAlpha( node, float( 0.3 ) );
 
    return node;
  }
}
 
osg::Group* buildTutorial1( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group( );
 
  // Create material to define the density of water.
  agx::MaterialRef waterMaterial = new agx::Material( "waterMaterial" );
  waterMaterial->getBulkMaterial( )->setDensity( agx::Real( 1000 ) );
 
  // Let the water geometry to be an box of size 50x50x20 meters.
  agxCollide::GeometryRef waterGeometry = new agxCollide::Geometry( new agxCollide::Box( 25, 25, 10 ) );
  // Surface of water at z = 0.
  waterGeometry->setPosition( 0, 0, -10 );
  waterGeometry->setMaterial( waterMaterial );
  ::createWaterVisual( waterGeometry, root );
 
  // Mark as water geometry.
  agxModel::WindAndWaterControllerRef controller = new agxModel::WindAndWaterController( );
  controller->addWater( waterGeometry );
  simulation->add( controller );
  simulation->add( waterGeometry );
 
  // Create a static box that will hold the end of each wire.
  agx::RigidBodyRef box = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 0.5, 0.5, 0.5 ) ) );
  box->setPosition( 0, 0, 5 );
  box->setMotionControl( agx::RigidBody::STATIC );
  agxOSG::setDiffuseColor( agxOSG::createVisual( box, root ), agxRender::Color::DarkBlue() );
 
  simulation->add( box );
 
  // Wire 1 is connected to the box.
  agxWire::WireRef wire1 = new agxWire::Wire( 0.02, 2 );
  wire1->add( new agxWire::BodyFixedNode( box, 0, -0.3, -0.5 ) );
  wire1->add( new agxWire::FreeNode( 20, -0.3, 0 ) );
  simulation->add( wire1 );
 
  // Collisions between the wire and the water geometry is disabled. No hydrodynamic calculations will be performed.
  // Default value is true.
  wire1->setEnableCollisions( waterGeometry, false );
  // Aerodynamics are disabled for the wire. Default value is false.
  controller->setEnableAerodynamics( wire1, false );
 
  agxOSG::WireRendererRef wireRenderer1 = new agxOSG::WireRenderer( wire1, root );
  wireRenderer1->setColor( agxRender::Color::Green() );
  simulation->add( wireRenderer1 );
 
  // Wire 2 is connected to the box. Since nothing is said about hydro-/aerodynamics only
  // hydrodynamic calculations will be performed.
  agxWire::WireRef wire2 = new agxWire::Wire( 0.02, 2 );
  wire2->add( new agxWire::BodyFixedNode( box, 0, 0, -0.5 ) );
  wire2->add( new agxWire::FreeNode( 20, 0, 0 ) );
  simulation->add( wire2 );
 
  agxOSG::WireRendererRef wireRenderer2 = new agxOSG::WireRenderer( wire2, root );
  wireRenderer2->setColor( agxRender::Color::Red( ) );
  simulation->add( wireRenderer2 );
 
  // Wire 3 is connected to the box.
  agxWire::WireRef wire3 = new agxWire::Wire( 0.02, 2 );
  wire3->add( new agxWire::BodyFixedNode( box, 0, 0.3, -0.5 ) );
  wire3->add( new agxWire::FreeNode( 20, 0.3, 0 ) );
  simulation->add( wire3 );
 
  // The hydrodynamic coefficient of pressure drag is set to 1.5. Other coefficients that can be set is VISCOUS_DRAG.
  // If nothing is set default values are used.
  // NOTE: Even though it is also possible to set LIFT in these parameters the lift coefficient will be ignored for wires.
  controller->getOrCreateHydrodynamicsParameters( wire3 )->setCoefficient( agxModel::WindAndWaterParameters::Coefficient::PRESSURE_DRAG, agx::Real( 1.5 ) );
 
  agxOSG::WireRendererRef wireRenderer3 = new agxOSG::WireRenderer( wire3, root );
  wireRenderer3->setColor( agxRender::Color::Orange( ) );
  simulation->add( wireRenderer3 );
 
  return root;
}
 
osg::Group* buildTutorial2( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group( );
 
  // Create a WindAndWaterController.
  agxModel::WindAndWaterControllerRef controller = new agxModel::WindAndWaterController( );
  simulation->add( controller );
 
  // Create a static box that will hold the end of each wire.
  agx::RigidBodyRef box = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 0.5, 0.5, 0.5 ) ) );
  box->setPosition( 0, 0, 5 );
  box->setMotionControl( agx::RigidBody::STATIC );
  agxOSG::setDiffuseColor( agxOSG::createVisual( box, root ), agxRender::Color::DarkBlue() );
  simulation->add( box );
 
  // Wire 1 is connected to the box. Aerodynamics are disabled (default) which means that the wire will fall
  // without resistance.
  agxWire::WireRef wire1 = new agxWire::Wire( 0.02, 2 );
  wire1->add( new agxWire::BodyFixedNode( box, 0, -0.3, -0.5 ) );
  wire1->add( new agxWire::FreeNode( 20, -0.3, 0 ) );
  simulation->add( wire1 );
 
  agxOSG::WireRendererRef wireRenderer1 = new agxOSG::WireRenderer( wire1, root );
  wireRenderer1->setColor( agxRender::Color::Green( ) );
  simulation->add( wireRenderer1 );
 
  // Wire 2 is connected to the box. Aerodynamics are enabled and pressure drag is set to 1.
  agxWire::WireRef wire2 = new agxWire::Wire( 0.02, 2 );
  wire2->add( new agxWire::BodyFixedNode( box, 0, 0, -0.5 ) );
  wire2->add( new agxWire::FreeNode( 20, 0, 0 ) );
  simulation->add( wire2 );
  controller->setEnableAerodynamics( wire2, true );
  controller->getOrCreateAerodynamicsParameters( wire2 )->setCoefficient( agxModel::WindAndWaterParameters::Coefficient::PRESSURE_DRAG, agx::Real( 1 ) );
 
  agxOSG::WireRendererRef wireRenderer2 = new agxOSG::WireRenderer( wire2, root );
  wireRenderer2->setColor( agxRender::Color::Red( ) );
  simulation->add( wireRenderer2 );
 
  // Wire3 is connected to the box. Aerodynamics are enabled and pressure drag is set to 2.
  agxWire::WireRef wire3 = new agxWire::Wire( 0.02, 2 );
  wire3->add( new agxWire::BodyFixedNode( box, 0, 0.3, -0.5 ) );
  wire3->add( new agxWire::FreeNode( 20, 0.3, 0 ) );
  simulation->add( wire3 );
  controller->setEnableAerodynamics( wire3, true );
  controller->getOrCreateAerodynamicsParameters( wire3 )->setCoefficient( agxModel::WindAndWaterParameters::Coefficient::PRESSURE_DRAG, agx::Real( 2 ) );
 
  agxOSG::WireRendererRef wireRenderer3 = new agxOSG::WireRenderer( wire3, root );
  wireRenderer3->setColor( agxRender::Color::Orange( ) );
  simulation->add( wireRenderer3 );
 
  return root;
}
 
osg::Group* buildTutorial3( agxSDK::Simulation* simulation, agxOSG::ExampleApplication* /*application*/ )
{
  osg::Group* root = new osg::Group( );
 
  // Create a WindAndWaterController.
  agxModel::WindAndWaterControllerRef controller = new agxModel::WindAndWaterController( );
  simulation->add( controller );
 
  // Create constant wind with a given velocity.
  const agx::Vec3 windVelocity = agx::Vec3( -15, 0, 0 );
  agxModel::ConstantWindGeneratorRef windGenerator = new agxModel::ConstantWindGenerator( windVelocity );
  controller->setWindGenerator( windGenerator );
 
  // Create a static box that will hold the end of each wire.
  agx::RigidBodyRef box = new agx::RigidBody( new agxCollide::Geometry( new agxCollide::Box( 0.5, 0.5, 0.5 ) ) );
  box->setPosition( 0, 0, 5 );
  box->setMotionControl( agx::RigidBody::STATIC );
  agxOSG::setDiffuseColor( agxOSG::createVisual( box, root ), agxRender::Color::DarkBlue( ) );
  simulation->add( box );
 
  // Wire 1 is connected to the box. Aerodynamics are disabled (default) which means that the wire will not
  // be affected by wind.
  agxWire::WireRef wire1 = new agxWire::Wire( 0.02, 2 );
  wire1->add( new agxWire::BodyFixedNode( box, 0, -0.3, -0.5 ) );
  wire1->add( new agxWire::FreeNode( 0, -0.3, -20 ) );
  simulation->add( wire1 );
 
  agxOSG::WireRendererRef wireRenderer1 = new agxOSG::WireRenderer( wire1, root );
  wireRenderer1->setColor( agxRender::Color::Green( ) );
  simulation->add( wireRenderer1 );
 
  // Wire 2 is connected to the box. Aerodynamics are enabled and pressure drag is set to 1.
  agxWire::WireRef wire2 = new agxWire::Wire( 0.02, 2 );
  wire2->add( new agxWire::BodyFixedNode( box, 0, 0, -0.5 ) );
  wire2->add( new agxWire::FreeNode( 0, 0, -20 ) );
  simulation->add( wire2 );
  controller->setEnableAerodynamics( wire2, true );
  controller->getOrCreateAerodynamicsParameters( wire2 )->setCoefficient( agxModel::WindAndWaterParameters::Coefficient::PRESSURE_DRAG, agx::Real( 1 ) );
 
  agxOSG::WireRendererRef wireRenderer2 = new agxOSG::WireRenderer( wire2, root );
  wireRenderer2->setColor( agxRender::Color::Red( ) );
  simulation->add( wireRenderer2 );
 
  // Wire3 is connected to the box. Aerodynamics are enabled and pressure drag is set to 2.
  agxWire::WireRef wire3 = new agxWire::Wire( 0.02, 2 );
  wire3->add( new agxWire::BodyFixedNode( box, 0, 0.3, -0.5 ) );
  wire3->add( new agxWire::FreeNode( 0, 0.3, -20 ) );
  simulation->add( wire3 );
  controller->setEnableAerodynamics( wire3, true );
  controller->getOrCreateAerodynamicsParameters( wire3 )->setCoefficient( agxModel::WindAndWaterParameters::Coefficient::PRESSURE_DRAG, agx::Real( 2 ) );
 
  agxOSG::WireRendererRef wireRenderer3 = new agxOSG::WireRenderer( wire3, root );
  wireRenderer3->setColor( agxRender::Color::Orange( ) );
  simulation->add( wireRenderer3 );
 
  return root;
}
 
 
int main( int argc, char** argv )
{
  agx::AutoInit agxInit;
  std::cout << "\t" << agxGetLibraryName() << " " << agxGetVersion() << " " << agxGetCompanyName() << "(C)\n " <<
               "\tTutorial wire - wind and water\n" <<
               "\t--------------------------------\n\n" << std::endl;
 
  try {
    agxOSG::ExampleApplicationRef application = new agxOSG::ExampleApplication;
 
    application->addScene( buildTutorial1, '1' );
    application->addScene( buildTutorial2, '2' );
    application->addScene( buildTutorial3, '3' );
 
    if ( application->init( argc, argv ) )
      return application->run();
  }
  catch ( std::exception& e ) {
    std::cerr << "\nCaught exception: " << e.what() << std::endl;
    return 1;
  }
 
  return 0;
}