TrackUtils.h

Go to the documentation of this file.

/*
Copyright 2007-2025. Algoryx Simulation AB.
 
All AGX source code, intellectual property, documentation, sample code,
tutorials, scene files and technical white papers, are copyrighted, proprietary
and confidential material of Algoryx Simulation AB. You may not download, read,
store, distribute, publish, copy or otherwise disseminate, use or expose this
material unless having a written signed agreement with Algoryx Simulation AB, or having been
advised so by Algoryx Simulation AB for a time limited evaluation, or having purchased a
valid commercial license from Algoryx Simulation AB.
 
Algoryx Simulation AB disclaims all responsibilities for loss or damage caused
from using this software, unless otherwise stated in written agreements with
Algoryx Simulation AB.
*/
 
#pragma once
 
#include <agxVehicle/Track.h>
 
#include <agxCollide/Box.h>
 
#include <agxRender/Color.h>
 
namespace agxVehicle::utils
{
  inline void createGeometryBox( const TrackNode& node )
  {
    agxCollide::GeometryRef geometry = new agxCollide::Geometry( new agxCollide::Box( node.getHalfExtents() ) );
    agx::AffineMatrix4x4 transform = agx::AffineMatrix4x4::translate( 0, 0, node.getHalfExtents().z() );
    geometry->setLocalTransform( transform );
    node.getRigidBody()->add( geometry );
  }
 
  template<typename ContainerT>
  IndexRangeContainer splitNonconsecutive( const ContainerT& container )
  {
    if ( container.size() < 2 )
      return IndexRangeContainer{ IndexRange( 0, container.size() ) };
 
    agxAssert( std::is_sorted( container.begin(), container.end() ) );
 
    IndexRangeContainer result{ IndexRange( 0, 1 ) };
    for ( agx::UInt i = 1; i < container.size(); ++i ) {
      if ( container[ i ] == container[ i - 1 ] + typename ContainerT::value_type( 1 ) )
        ++result.back().end();
      else
        result.push_back( agx::IndexRange( i, i + 1 ) );
    }
 
    return result;
  }
 
  template<typename ContainerT>
  IndexRangeContainer splitNonconsecutive( const Track& track, const ContainerT& container )
  {
    auto result = splitNonconsecutive( container );
 
    agxAssert( !result.empty() );
    if ( result.size() == 1 && result.front().empty() )
      return IndexRangeContainer{};
 
    // Transform ranges from indices to container values. E.g.,
    // 2 4 7 [9 10 11 12] 18
    // index: 3  ...  6
    // range original:    begin = 3, end = 7
    // range transformed: begin = 9, end = 13
    for ( agx::UInt i = 0; i < result.size(); ++i ) {
      auto& range     = result[ i ];
      const auto size = range.size();
      range.begin()   = container[ range.begin() ];
      range.end()     = range.begin() + size;
    }
 
    if ( result.size() < 2 )
      return result;
 
    // Including the last range in the first if the first node belongs
    // to the first range and the last node to the last.
    const auto numNodes = track.nodes().size();
    if ( result.front().begin() == agx::UInt( 0 ) && result.back().end() == numNodes ) {
      result.front().begin() = result.back().begin();
      result.pop_back();
    }
 
    return result;
  }
 
  template<typename T>
  inline T prevIndex( T index, T size )
  {
    agxAssert( size > 0 && index != agx::InvalidIndex );
    return index == 0 ? size - 1 : index - 1;
  }
 
  template<typename T>
  inline T nextIndex( T index, T size )
  {
    agxAssert( size > 0 && index != agx::InvalidIndex );
    return ( index + 1 ) % size;
  }
 
  inline TrackNodeRange createRangeExcludingNodeIndex( const Track& track, agx::UInt nodeIndex, agx::Int signedNumNodes )
  {
    const auto totalNumNodes = track.nodes().size();
    if ( totalNumNodes < 1 )
      return track.nodes();
 
    const agx::UInt numNodes = signedNumNodes < 0 ?
                                  (agx::UInt)-signedNumNodes :
                                  (agx::UInt)signedNumNodes;
    agx::UInt beginIndex = agx::InvalidIndex;
    agx::UInt endIndex   = agx::InvalidIndex;
    if ( signedNumNodes < 0 ) {
      beginIndex = nodeIndex > numNodes ?
                     nodeIndex - numNodes :
                     nodeIndex + totalNumNodes - numNodes;
      endIndex   = nodeIndex;
    }
    else {
      beginIndex = ( nodeIndex + 1 ) % totalNumNodes;
      endIndex = ( nodeIndex + numNodes + 1 ) % totalNumNodes;
    }
 
    return TrackNodeRange( track.getIterator( beginIndex ), track.getIterator( endIndex ) );
  }
 
  inline agx::AffineMatrix4x4 calculateNodeTransform( const agx::Vec3& position,
                                                      const agx::Vec3& direction,
                                                      const agx::Vec3& wheelPlaneNormal )
  {
    auto q = agx::Quat();
    q     *= agx::Quat::rotate( q * agx::Vec3::Z_AXIS(), direction );
    q     *= agx::Quat::rotate( q * agx::Vec3::Y_AXIS(), wheelPlaneNormal );
    q     *= agx::Quat::rotate( q * agx::Vec3::X_AXIS(), ( wheelPlaneNormal ^ direction ).normal() );
 
    return agx::AffineMatrix4x4{ q, position };
  }
 
  inline agx::Real getNodeWheelOverlapThreshold( const Track& track )
  {
    const auto trackThickness = track.getRoute()->getNodeThickness();
    const auto trackNodeLength = track.getRoute()->getNodeLength();
 
    return trackThickness + 0.25 * trackNodeLength;
  }
 
  AGXVEHICLE_EXPORT Track::TrackWheelContainer getWheelsAlongTrack( const Track& track );
 
  AGXVEHICLE_EXPORT bool isTrackSegment( const Track& track, const agx::RigidBody& rb );
 
  AGXVEHICLE_EXPORT const TrackWheel* findWheel( const Track& track, const agx::RigidBody& rb );
 
  AGXVEHICLE_EXPORT void debugRender( const WheelTangentData& tangent,
                                      const agxRender::Color& tangentColor = agxRender::Color::OrangeRed(),
                                      const agxRender::Color& upColor = agxRender::Color::GreenYellow(),
                                      agx::Real scale = 1.0 );
 
 
  AGXVEHICLE_EXPORT void setTrackStiffnessFromCompliance( TrackProperties* properties, agx::Real compliance, agx::Real nodeLength );
 
  AGXVEHICLE_EXPORT void setTrackStiffnessFromCompliance( TrackProperties* properties, const agx::RealVector& compliance, agx::Real nodeLength );
 
  struct AGXVEHICLE_EXPORT WheelOverlap
  {
    bool isOverlapping() const;
 
    operator bool() const;
 
    const TrackWheel* wheel = nullptr;
  };
 
  struct AGXVEHICLE_EXPORT WheelSegmentOverlap : WheelOverlap
  {
    static WheelSegmentOverlap test( const TrackWheel& wheel,
                                     const agx::Vec3& sBegin,
                                     const agx::Vec3& sEnd,
                                     agx::Real additionalRadius = 0.0 );
 
    static WheelSegmentOverlap test( const TrackWheel& wheel,
                                     const WheelTangentData& tangent,
                                     agx::Real additionalRadius = 0.0 );
 
    void render() const;
 
    agx::Vec3 entryPoint;
    agx::Vec3 exitPoint;
  };
 
  struct AGXVEHICLE_EXPORT WheelPointOverlap : WheelOverlap
  {
    static WheelPointOverlap test( const TrackWheel& wheel,
                                   const agx::Vec3& point,
                                   agx::Real additionalRadius = 0.0 );
 
    agx::Real depth = 0.0;
    agx::Vec3 normal{};
  };
 
  struct AGXVEHICLE_EXPORT Tan
  {
    enum Location
    {
      BELOW = -1,  // Inside the segment and on the wheel-facing side.
      OUTSIDE = 0, // Not within the begin - end interval.
      ABOVE = 1    // Inside the segment and on the outer side.
    };
 
    static bool inside( const agx::Vec3& point, const WheelTangentData& tangent );
 
    static Location findLocation( const agx::Vec3& point, const WheelTangentData& tangent );
  };
 
  class OneOffCircularIndexView
  {
    public:
      struct iterator
      {
        iterator( size_t index1,
                  size_t index2,
                  size_t size,
                  size_t numLoops = 0 )
          : m_index1( index1 )
          , m_index2( index2 )
          , m_numLoops( 2 * numLoops )
          , m_size( size )
        {
        }
 
        iterator( const iterator& ) = delete;
        iterator& operator=( const iterator& ) = delete;
 
        iterator& operator++()
        {
          m_index1 = nextIndex( m_index1 );
          m_index2 = nextIndex( m_index2 );
          return *this;
        }
 
        std::pair<size_t, size_t> operator*() const
        {
          return { m_index1, m_index2 };
        }
 
        bool operator==( const iterator& other ) const
        {
          return this->m_index1 == other.m_index1 &&
                 this->m_index2 == other.m_index2 &&
                 this->m_numLoops == other.m_numLoops;
        }
 
        bool operator!=( const iterator& other ) const
        {
          return !( ( *this ) == other );
        }
 
      private:
        inline size_t nextIndex( size_t index )
        {
          m_numLoops += ( index + 1 ) == m_size;
          return ( index + 1 ) % m_size;
        }
 
      private:
        size_t m_index1;
        size_t m_index2;
        size_t m_numLoops;
        size_t m_size;
      };
 
    public:
      OneOffCircularIndexView( size_t size )
        : m_size( size )
      {
      }
 
      iterator begin() const
      {
        return { 0, 1, m_size };
      }
 
      iterator end() const
      {
        return { 0, 1, m_size, 1 };
      }
 
    private:
      size_t m_size = 0;
  };
 
  template<typename ContainerT>
  class OneOffCircularView
  {
    public:
      struct iterator
      {
        using reference = typename ContainerT::reference;
        using pair = std::pair<reference, reference>;
 
        iterator( size_t index1,
                  size_t index2,
                  ContainerT& container,
                  size_t numLoops = 0 )
          : m_it( index1, index2, container.size(), numLoops )
          , m_container( container )
        {
        }
 
        iterator( const iterator& ) = delete;
        iterator& operator=( const iterator& ) = delete;
 
        iterator& operator++()
        {
          ++m_it;
          return *this;
        }
 
        pair operator*() const
        {
          const auto [index1, index2] = *m_it;
          return { (reference)m_container[ index1 ], (reference)m_container[ index2 ] };
        }
 
        bool operator==( const iterator& other ) const
        {
          return this->m_it == other.m_it;
        }
 
        bool operator!=( const iterator& other ) const
        {
          return !( ( *this ) == other );
        }
 
        private:
          OneOffCircularIndexView::iterator m_it;
          ContainerT& m_container;
      };
 
    public:
      OneOffCircularView( ContainerT& container )
        : m_container( container )
      {
      }
 
      iterator begin()
      {
        return { 0, 1, m_container };
      }
 
      iterator end()
      {
        return { 0, 1, m_container, 1 };
      }
 
    private:
      ContainerT& m_container;
  };
 
  template<typename T>
  auto circularView( const T& foo )
  {
    if constexpr ( std::is_integral_v<T> )
      return OneOffCircularIndexView{ (size_t)foo };
    else
      return OneOffCircularView{ foo };
  }
}