math::Path Class Reference

Represents a path with coordinates and allows for various interpolation methods. More...

#include "path.h"

Public Types
enum class	InterpType : std::uint8_t { INTERP_STATIC , INTERP_LINEAR , INTERP_CUBIC }
	Types of interpolation supported by the Path class. More...

Public Member Functions
	Path (const InterpType type=InterpType::INTERP_STATIC) noexcept
	Constructs a Path object with a specified interpolation type.
	~Path ()=default
	Path (const Path &)=delete
	Path (Path &&)=delete
Path &	operator= (const Path &)=delete
Path &	operator= (Path &&)=delete
void	addCoord (const Coord &coord) noexcept
	Adds a coordinate to the path.
void	finalize ()
	Finalizes the path, preparing it for interpolation.
InterpType	getType () const noexcept
	Retrieves the current interpolation type of the path.
const std::vector< Coord > &	getCoords () const noexcept
	Gets the list of coordinates in the path.
Vec3	getPosition (RealType t) const
	Retrieves the position at a given time along the path.
Vec3	getVelocity (RealType t) const
	Retrieves the velocity at a given time along the path.
void	setInterp (InterpType settype) noexcept
	Changes the interpolation type.

Detailed Description

Represents a path with coordinates and allows for various interpolation methods.

Definition at line 30 of file path.h.

Member Enumeration Documentation

InterpType

enum class math::Path::InterpType : std::uint8_t

strong

Types of interpolation supported by the Path class.

Enumerator
INTERP_STATIC	Hold the first coordinate for all query times.
INTERP_LINEAR	Linearly interpolate between neighboring coordinates.
INTERP_CUBIC	Cubically interpolate between neighboring coordinates.

Definition at line 36 of file path.h.

        {
            INTERP_STATIC, ///< Hold the first coordinate for all query times.
            INTERP_LINEAR, ///< Linearly interpolate between neighboring coordinates.
            INTERP_CUBIC ///< Cubically interpolate between neighboring coordinates.
        };

Constructor & Destructor Documentation

Path() [1/3]

math::Path::Path ( const InterpType  type = InterpType::INTERP_STATIC )

explicitnoexcept

Constructs a Path object with a specified interpolation type.

Parameters

type	The interpolation type (default is INTERP_STATIC).

Definition at line 48 of file path.h.

: _type(type) {}

~Path()

math::Path::~Path ( )

default

Path() [2/3]

math::Path::Path ( const Path &  )

delete

Path() [3/3]

math::Path::Path ( Path &&  )

delete

Member Function Documentation

addCoord()

void math::Path::addCoord ( const Coord &  coord )

noexcept

Adds a coordinate to the path.

Parameters

coord

The coordinate to be added.

Definition at line 27 of file path.cpp.

    {
        auto comp = [](const Coord& a, const Coord& b) { return a.t < b.t; };
 
        const auto iter = std::ranges::lower_bound(_coords, coord, comp);
        _coords.insert(iter, coord);
        _final = false;
    }

References a, b, and max.

Referenced by fers_get_interpolated_motion_path(), and serial::xml_parser_utils::parseMotionPath().

Here is the caller graph for this function:

finalize()

void math::Path::finalize

(

)

Finalizes the path, preparing it for interpolation.

Definition at line 147 of file path.cpp.

    {
        if (!_final)
        {
            switch (_type)
            {
            case InterpType::INTERP_STATIC:
            case InterpType::INTERP_LINEAR:
                break;
            case InterpType::INTERP_CUBIC:
                finalizeCubic<Coord>(_coords, _dd);
                break;
            }
            _final = true;
        }
    }

References INTERP_CUBIC, INTERP_LINEAR, INTERP_STATIC, and max.

Referenced by fers_get_interpolated_motion_path(), and serial::xml_parser_utils::parseMotionPath().

Here is the caller graph for this function:

getCoords()

const std::vector< Coord > & math::Path::getCoords ( ) const

noexcept

Gets the list of coordinates in the path.

Returns

A constant reference to the vector of coordinates.

Definition at line 84 of file path.h.

{ return _coords; }

Referenced by serial::kml_generator_utils::generateDynamicPathKml(), serial::kml_generator_utils::generateTrackEndpointsKml(), and serial::xml_serializer_utils::serializeMotionPath().

Here is the caller graph for this function:

getPosition()

Vec3 math::Path::getPosition

(

RealType

t

)

const

Retrieves the position at a given time along the path.

Parameters

t	The time parameter at which to get the position.

Returns

The interpolated position as a Vec3 object.

Exceptions

PathException

If finalize() has not been called before this method.

Definition at line 36 of file path.cpp.

    {
        if (!_final)
        {
            LOG(Level::FATAL, "Finalize not called before GetPosition");
            throw PathException("Finalize not called before GetPosition");
        }
 
        Coord coord{};
        switch (_type)
        {
        case InterpType::INTERP_STATIC:
            getPositionStatic(coord, _coords);
            break;
        case InterpType::INTERP_LINEAR:
            getPositionLinear(t, coord, _coords);
            break;
        case InterpType::INTERP_CUBIC:
            getPositionCubic(t, coord, _coords, _dd);
            break;
        }
        return coord.pos;
    }

References getPositionCubic(), getPositionLinear(), getPositionStatic(), INTERP_CUBIC, INTERP_LINEAR, INTERP_STATIC, LOG, and max.

Referenced by fers_get_interpolated_motion_path(), and serial::kml_generator_utils::generateDynamicPathKml().

Here is the call graph for this function:

Here is the caller graph for this function:

getType()

InterpType math::Path::getType ( ) const

noexcept

Retrieves the current interpolation type of the path.

Returns

The interpolation type of the path.

Definition at line 77 of file path.h.

{ return _type; }

Referenced by serial::xml_serializer_utils::serializeMotionPath().

Here is the caller graph for this function:

getVelocity()

Vec3 math::Path::getVelocity

(

RealType

t

)

const

Retrieves the velocity at a given time along the path.

Parameters

t	The time parameter at which to get the velocity.

Returns

The interpolated velocity as a Vec3 object.

Exceptions

PathException

If finalize() has not been called before this method.

Definition at line 60 of file path.cpp.

    {
        if (!_final)
        {
            LOG(Level::FATAL, "Finalize not called before GetVelocity");
            throw PathException("Finalize not called before GetVelocity");
        }
 
        if (_coords.empty())
        {
            return {0, 0, 0};
        }
 
        switch (_type)
        {
        case InterpType::INTERP_STATIC:
            return {0, 0, 0};
 
        case InterpType::INTERP_LINEAR:
            {
                auto xrp = std::ranges::upper_bound(_coords, t, {}, &Coord::t);
                auto idx = std::distance(_coords.begin(), xrp);
 
                // Clamp to valid segments
                if (idx <= 0)
                    idx = 1;
                if (static_cast<std::size_t>(idx) >= _coords.size())
                    idx = static_cast<decltype(idx)>(_coords.size() - 1);
 
                if (idx < 1 || static_cast<std::size_t>(idx) >= _coords.size())
                    return {0, 0, 0}; // Should not happen if size >= 1 and clamp works
 
                const auto right_idx = static_cast<std::size_t>(idx);
                const auto left_idx = right_idx - 1;
 
                const auto& p1 = _coords[left_idx];
                const auto& p2 = _coords[right_idx];
                const RealType dt = p2.t - p1.t;
 
                if (dt <= EPSILON)
                    return {0, 0, 0};
 
                return (p2.pos - p1.pos) / dt;
            }
 
        case InterpType::INTERP_CUBIC:
            {
                auto xrp = std::ranges::upper_bound(_coords, t, {}, &Coord::t);
                std::ptrdiff_t xri = 0;
                if (xrp == _coords.begin())
                    xri = 1;
                else if (xrp == _coords.end())
                    xri = static_cast<std::ptrdiff_t>(_coords.size() - 1);
                else
                    xri = std::distance(_coords.begin(), xrp);
 
                if (xri < 1 || static_cast<std::size_t>(xri) >= _coords.size())
                    return {0, 0, 0};
 
                const auto right_idx = static_cast<std::size_t>(xri);
                const auto left_idx = right_idx - 1;
 
                const RealType h = _coords[right_idx].t - _coords[left_idx].t;
                if (h <= EPSILON)
                    return {0, 0, 0};
 
                const RealType a = (_coords[right_idx].t - t) / h;
                const RealType b = (t - _coords[left_idx].t) / h;
 
                // Derivative coefficients
                // da/dt = -1/h
                // db/dt = 1/h
                // dc/dt = -h/6 * (3a^2 - 1)
                // dd/dt = h/6 * (3b^2 - 1)
 
                const RealType da = -1.0 / h;
                const RealType db = 1.0 / h;
                const RealType dc = -h / 6.0 * (3.0 * a * a - 1.0);
                const RealType dd_coeff = h / 6.0 * (3.0 * b * b - 1.0);
 
                return _coords[left_idx].pos * da + _coords[right_idx].pos * db + _dd[left_idx].pos * dc +
                    _dd[right_idx].pos * dd_coeff;
            }
        }
        return {0, 0, 0};
    }

References a, b, EPSILON, INTERP_CUBIC, INTERP_LINEAR, INTERP_STATIC, LOG, max, and math::Coord::t.

Referenced by fers_get_interpolated_motion_path().

Here is the caller graph for this function:

operator=() [1/2]

Path & math::Path::operator= ( const Path &  )

delete

operator=() [2/2]

Path & math::Path::operator= ( Path &&  )

delete

setInterp()

void math::Path::setInterp ( InterpType  settype )

noexcept

Changes the interpolation type.

Parameters

settype

The new interpolation type to be used.

Definition at line 164 of file path.cpp.

    {
        _type = settype;
        _final = false;
    }

References max.

Referenced by fers_get_interpolated_motion_path(), and serial::xml_parser_utils::parseMotionPath().

Here is the caller graph for this function:

---

The documentation for this class was generated from the following files:

• packages/libfers/src/math/path.h
• packages/libfers/src/math/path.cpp