path_utils.h File Reference

Utility functions for path interpolation and exception handling. More...

#include <algorithm>
#include <concepts>
#include <stdexcept>
#include <vector>
#include "core/config.h"

Include dependency graph for path_utils.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Classes
class	math::PathException
	Exception class for handling path-related errors. More...

Namespaces
namespace	math

Concepts
concept	Interpolatable
	Concept for types that can be interpolated.

Functions
template<Interpolatable T>
void	getPositionStatic (T &coord, const std::vector< T > &coords)
	Interpolates a static position from a list of coordinates.
template<Interpolatable T>
void	getPositionLinear (RealType t, T &coord, const std::vector< T > &coords)
	Performs linear interpolation between coordinate points.
template<Interpolatable T>
void	getPositionCubic (RealType t, T &coord, const std::vector< T > &coords, const std::vector< T > &dd)
	Performs cubic spline interpolation between coordinate points.
template<Interpolatable T>
void	finalizeCubic (const std::vector< T > &coords, std::vector< T > &dd)
	Finalizes cubic spline interpolation by calculating second derivatives.

Detailed Description

Utility functions for path interpolation and exception handling.

The cubic interpolation functions are based on methods described in "Numerical Recipes in C, Second Edition" by Press et al., but the code here is distinct from the original.

Definition in file path_utils.h.

Function Documentation

finalizeCubic()

template<Interpolatable T>

void finalizeCubic	(	const std::vector< T > &	coords,
		std::vector< T > &	dd
	)

Finalizes cubic spline interpolation by calculating second derivatives.

Template Parameters

T	The type of the coordinate, which must satisfy the Interpolatable concept.

Parameters

coords	A vector of coordinates for which second derivatives will be calculated.
dd	The output vector that will store the calculated second derivatives.

Exceptions

PathException

if there are fewer than two points for interpolation.

Definition at line 180 of file path_utils.h.

{
    using index_type = typename std::vector<T>::size_type;
    const index_type size = coords.size();
    if (size < 2)
    {
        throw math::PathException("Not enough points for cubic interpolation");
    }
 
    std::vector<T> tmp(size);
    dd.resize(size);
 
    dd.front() = 0;
    dd.back() = 0;
 
    for (index_type i = 1; i + 1 < size; ++i)
    {
        const index_type next_index = i + 1;
        const index_type prev_index = i - 1;
 
        const T yrd = coords[next_index] - coords[i];
        const T yld = coords[i] - coords[prev_index];
        const RealType xrd = coords[next_index].t - coords[i].t;
        const RealType xld = coords[i].t - coords[prev_index].t;
        const RealType iw = coords[next_index].t - coords[prev_index].t;
 
        if (iw <= EPSILON)
        {
            dd[i] = 0.0;
            tmp[i] = 0.0;
            continue;
        }
 
        const T yrd_xrd = (xrd <= EPSILON) ? (yrd * 0.0) : (yrd / xrd);
        const T yld_xld = (xld <= EPSILON) ? (yld * 0.0) : (yld / xld);
 
        const RealType si = xld / iw;
        const T p = dd[prev_index] * si + 2.0;
        dd[i] = (si - 1.0) / p;
        tmp[i] = ((yrd_xrd - yld_xld) * 6.0 / iw - tmp[prev_index] * si) / p;
    }
 
    for (index_type i = size - 1; i-- > 0;)
    {
        dd[i] = dd[i] * dd[i + 1] + tmp[i];
    }
}

References EPSILON.

Referenced by math::RotationPath::finalize().

Here is the caller graph for this function:

getPositionCubic()

template<Interpolatable T>

void getPositionCubic	(	RealType	t,
		T &	coord,
		const std::vector< T > &	coords,
		const std::vector< T > &	dd
	)

Performs cubic spline interpolation between coordinate points.

The method used for calculating the spline is from "Numerical Recipes in C."

Template Parameters

T	The type of the coordinate, which must satisfy the Interpolatable concept.

Parameters

t	The interpolation factor (usually time) to determine the position.
coord	The output coordinate that will be interpolated.
coords	A vector of coordinates to interpolate between.
dd	A vector of second derivatives used in the cubic interpolation.

Exceptions

PathException

if the list of coordinates is empty.

Definition at line 134 of file path_utils.h.

{
    if (coords.empty())
    {
        throw math::PathException("coord list empty during GetPositionCubic");
    }
 
    auto xrp = std::ranges::upper_bound(coords, t, {}, &T::t);
 
    if (xrp == coords.begin())
    {
        coord = *xrp;
    }
    else if (xrp == coords.end())
    {
        coord = *(xrp - 1);
    }
    else
    {
        using index_type = typename std::vector<T>::size_type;
        const auto right_index = static_cast<index_type>(xrp - coords.begin());
        const auto left_index = right_index - 1;
 
        const RealType xrd = coords[right_index].t - t;
        const RealType xld = t - coords[left_index].t;
        const RealType iw = coords[right_index].t - coords[left_index].t;
        const RealType iws = iw * iw / 6.0;
        const RealType a = xrd / iw;
        const RealType b = xld / iw;
        const RealType c = (a * a * a - a) * iws;
        const RealType d = (b * b * b - b) * iws;
 
        coord = coords[left_index] * a + coords[right_index] * b + dd[left_index] * c + dd[right_index] * d;
    }
    coord.t = t;
}

Referenced by math::Path::getPosition(), and math::RotationPath::getPosition().

Here is the caller graph for this function:

getPositionLinear()

template<Interpolatable T>

void getPositionLinear	(	RealType	t,
		T &	coord,
		const std::vector< T > &	coords
	)

Performs linear interpolation between coordinate points.

Template Parameters

T	The type of the coordinate, which must satisfy the Interpolatable concept.

Parameters

t	The interpolation factor (usually time) to determine the position.
coord	The output coordinate that will be interpolated.
coords	A vector of coordinates to interpolate between.

Exceptions

PathException

if the list of coordinates is empty.

Definition at line 90 of file path_utils.h.

{
    if (coords.empty())
    {
        throw math::PathException("coord list empty during GetPositionLinear");
    }
 
    auto xrp = std::ranges::upper_bound(coords, t, {}, &T::t);
 
    if (xrp == coords.begin())
    {
        coord = *xrp;
    }
    else if (xrp == coords.end())
    {
        coord = *(xrp - 1);
    }
    else
    {
        using index_type = typename std::vector<T>::size_type;
        const auto right_index = static_cast<index_type>(xrp - coords.begin());
        const auto left_index = right_index - 1;
 
        const RealType iw = coords[right_index].t - coords[left_index].t;
        const RealType rw = (coords[right_index].t - t) / iw;
        const RealType lw = 1 - rw;
 
        coord = coords[right_index] * lw + coords[left_index] * rw;
    }
    coord.t = t;
}

Referenced by math::Path::getPosition(), and math::RotationPath::getPosition().

Here is the caller graph for this function:

getPositionStatic()

template<Interpolatable T>

void getPositionStatic	(	T &	coord,
		const std::vector< T > &	coords
	)

Interpolates a static position from a list of coordinates.

Template Parameters

T	The type of the coordinate, which must satisfy the Interpolatable concept.

Parameters

coord	The output coordinate to be set.
coords	A vector of coordinates from which the first will be selected.

Exceptions

PathException

if the list of coordinates is empty.

Definition at line 71 of file path_utils.h.

{
    if (coords.empty())
    {
        throw math::PathException("coord list empty during GetPositionStatic");
    }
    coord = coords[0];
}

Referenced by math::Path::getPosition(), and math::RotationPath::getPosition().

Here is the caller graph for this function: