API Reference

Core Concepts

Core C++20 concepts defining the manifold interface hierarchy.

HasMetric concept and batched variant for manifolds with a Riemannian inner product.

HasDistance concept for manifolds with geodesic distance.

HasGeodesic concept for manifolds with geodesic interpolation and exp/log maps.

Retraction concept for generalized exponential/logarithmic maps.

Manifolds

Sphere

template<int Dim = 2, typename MetricT = IdentityMetric<detail::sphere_ambient_v<Dim>>, typename RetractionT = SphereExponentialMap, typename SamplerT = StochasticSampler> class Sphere

The n-sphere \( S^n \) parameterized by dimension, metric and retraction.

This class composes a metric policy and a retraction policy to form a complete RiemannianManifold. Points are represented as unit vectors in \( \mathbb{R}^{n+1} \); tangent vectors live in the orthogonal complement of the base point.

Template Parameters:

Dim – Intrinsic dimension (e.g. 2 for \( S^2 \)), or Eigen::Dynamic for runtime sizing. Defaults to 2 (the classical round 2-sphere).
MetricT – Metric policy (default: ConstantSPDMetric<Dim+1> identity).
RetractionT – Retraction policy (default: SphereExponentialMap).

Metric delegates

inline Scalar inner(const Point &p, const Tangent &u, const Tangent &v) const: Riemannian inner product at \( p \).

inline Scalar norm(const Point &p, const Tangent &v) const: Riemannian norm at \( p \).

inline Eigen::MatrixXd inner_matrix(const Point &p, const Eigen::MatrixXd &U, const Eigen::MatrixXd &V) const: Batched inner product \(U^\top M(p)\, V\) when the metric provides it.

Retraction delegates

inline Point exp(const Point &p, const Tangent &v) const

Exponential map (or retraction) \( \exp_p(v) \).

Parameters:

p – Base point on the sphere.
v – Tangent vector at \( p \).

Returns:

The resulting point on the sphere.

inline Tangent log(const Point &p, const Point &q) const

Logarithmic map (or inverse retraction) \( \log_p(q) \).

Parameters:

p – Base point on the sphere.
q – Target point on the sphere.

Returns:

Tangent vector at \( p \) such that \( \exp_p(v) \approx q \).

Derived operations

inline Scalar distance(const Point &p, const Point &q) const

Geodesic distance \( d(p, q) \) via the midpoint approximation.

Parameters:

p – First point on the sphere.
q – Second point on the sphere.

Returns:

The geodesic distance.

inline Scalar injectivity_radius() const

Injectivity radius of the round n-sphere: \( \pi \).

Returns the topological injectivity radius for the default round (identity) metric. For anisotropic custom metrics the effective radius is smaller: \( \pi / \sqrt{\lambda_{\max}(A)} \) where \( \lambda_{\max} \) is the largest eigenvalue of the weight matrix. This value is an upper bound; discrete_geodesic uses it for step capping and may take extra retries if the true radius is smaller.

inline Point geodesic(const Point &p, const Point &q, Scalar t) const

Geodesic interpolation between \( p \) and \( q \) at parameter \( t \).

Parameters:

p – Start point.
q – End point.
t – Interpolation parameter in \( [0, 1] \).

Returns:

The interpolated point on the sphere.

Public Types

using Scalar = double: Scalar type.

using Point = Eigen::Vector<double, Ambient>: Point type (unit vector in \( \mathbb{R}^{n+1} \)).

using Tangent = Eigen::Vector<double, Ambient>: Tangent vector type.

Public Functions

inline bool has_riemannian_log_runtime() const

Runtime query: is log the Riemannian logarithm of the metric?

Only when the metric is the identity ConstantSPDMetric<Ambient> AND the retraction is the true exponential map does grad((1/2) d^2)(x) = -log_x(q) hold exactly. Other metrics and any projection retraction fall back to finite-difference natural gradient.

inline Sphere(): Static-dim default constructor: default-construct metric/retraction.

inline explicit Sphere(MetricT metric, RetractionT retraction = {}): Static-dim constructor with explicit metric and retraction policies.

inline explicit Sphere(int n): Dynamic-dim constructor: takes the intrinsic dimension \( n \) of \( S^n \).

inline Sphere(int n, MetricT metric, RetractionT retraction = {}): Dynamic-dim constructor with explicit metric and retraction.

inline int dim() const: Return the intrinsic dimension \( n \) of \( S^n \).

inline Point random_point() const

Sample a uniformly random point on \( S^n \).

Draws n+1 uniform samples from the sampler via sample_box, applies the Box-Muller transform to produce standard normal variates, and normalizes the resulting vector to project onto the sphere. This is mathematically equivalent to Marsaglia’s method but uses the configurable sampler policy instead of a thread-local RNG.

Returns:: A unit vector in \( \mathbb{R}^{n+1} \).

inline Tangent project(const Point &p, const Tangent &v) const

Project a vector onto the tangent space at \( p \).

Parameters:

p – Base point on the sphere.
v – Ambient vector to project.

Returns:

The tangential component of \( v \).

Public Static Attributes

static constexpr int Ambient = detail::sphere_ambient_v<Dim>: Ambient dimension (Dim + 1 for static, Eigen::Dynamic otherwise).

using geodex::SphereRoundMetric = IdentityMetric<3>

The standard round (bi-invariant) metric on \( S^2 \).

The inner product is the ambient Euclidean dot product restricted to tangent vectors: \( \langle u, v \rangle_p = u \cdot v \). Zero-storage stateless metric.

struct SphereExponentialMap

True exponential and logarithmic maps on \( S^n \) (round geometry).

retract(p, v) computes \( \exp_p(v) = \cos(\|v\|)\, p + \sin(\|v\|)\, v / \|v\| \)
inverse_retract(p, q) computes \( \log_p(q) \) via the arc-length formula

Both methods are polymorphic in the point type, so the same retraction struct serves every \( S^n \).

Public Functions

template<typename Point> inline EIGEN_STRONG_INLINE Point retract (const Point &p, const Point &v) const

Exponential map \( \exp_p(v) \) on \( S^n \).

Parameters:

p – Base point on the sphere.
v – Tangent vector at \( p \).

Returns:

The resulting point on the sphere.

template<typename Point> inline EIGEN_STRONG_INLINE Point inverse_retract (const Point &p, const Point &q) const

Logarithmic map \( \log_p(q) \) on \( S^n \).

Parameters:

p – Base point on the sphere.
q – Target point on the sphere.

Returns:

Tangent vector at \( p \) such that \( \exp_p(v) = q \).

struct SphereProjectionRetraction

First-order projection retraction on \( S^n \).

retract(p, v) computes \( R_p(v) = (p + v) / \|p + v\| \)
inverse_retract(p, q) projects \( q - p \) onto \( T_p S^n \)

Public Functions

template<typename Point> inline EIGEN_STRONG_INLINE Point retract (const Point &p, const Point &v) const

Projection retraction: normalize \( p + v \).

Parameters:

p – Base point on the sphere.
v – Tangent vector at \( p \).

Returns:

The retracted point on the sphere.

template<typename Point> inline EIGEN_STRONG_INLINE Point inverse_retract (const Point &p, const Point &q) const

Inverse projection retraction.

Parameters:

p – Base point on the sphere.
q – Target point on the sphere.

Returns:

Tangent vector at \( p \) approximating \( \log_p(q) \).

Euclidean

template<int Dim = Eigen::Dynamic, typename MetricT = EuclideanStandardMetric<Dim>, typename SamplerT = StochasticSampler> class Euclidean

Euclidean manifold \( \mathbb{R}^n \) parameterized by dimension and metric policy.

Supports both compile-time fixed dimension (e.g., Euclidean<3>) and runtime dynamic dimension (Euclidean<Eigen::Dynamic>). The exp/log maps are trivial (addition/subtraction) since the space is flat.

Template Parameters:

Dim – Compile-time dimension, or Eigen::Dynamic.
MetricT – Metric policy (default: EuclideanStandardMetric).
SamplerT – Sampler policy for random_point() (default: StochasticSampler).

Metric delegates

inline Scalar inner(const Point &p, const Tangent &u, const Tangent &v) const: Riemannian inner product at \( p \).

inline Scalar norm(const Point &p, const Tangent &v) const: Riemannian norm at \( p \).

inline Eigen::MatrixXd inner_matrix(const Point &p, const Eigen::MatrixXd &U, const Eigen::MatrixXd &V) const: Batched inner product \(U^\top M(p)\, V\) when the metric provides it.

Exp / Log

inline Point exp(const Point &p, const Tangent &v) const

Exponential map: \( \exp_p(v) = p + v \).

Parameters:

p – Base point.
v – Tangent vector.

Returns:

The resulting point.

inline Tangent log(const Point &p, const Point &q) const

Logarithmic map: \( \log_p(q) = q - p \).

Parameters:

p – Base point.
q – Target point.

Returns:

The tangent vector from \( p \) to \( q \).

Derived operations

inline Scalar distance(const Point &p, const Point &q) const

Geodesic distance via the midpoint approximation.

Parameters:

p – First point.
q – Second point.

Returns:

The distance \( d(p, q) \).

inline Scalar injectivity_radius() const

Injectivity radius of \( \mathbb{R}^n \): \( \infty \).

Euclidean space is flat, so the injectivity radius is infinite regardless of the metric. Anisotropic custom metrics change geodesic directions but not the fact that the space is simply connected and geodesically complete.

inline Point geodesic(const Point &p, const Point &q, Scalar t) const

Geodesic interpolation: \( (1 - t) p + t q \).

Parameters:

p – Start point.
q – End point.
t – Interpolation parameter in \( [0, 1] \).

Returns:

The interpolated point.

Public Types

using Scalar = double: Scalar type.

using Point = Eigen::Vector<double, Dim>: Point type.

using Tangent = Eigen::Vector<double, Dim>: Tangent vector type.

Public Functions

inline bool has_riemannian_log_runtime() const

Runtime query: is log the Riemannian logarithm of the metric?

True only when the metric is the identity ConstantSPDMetric<Dim> (the standard Euclidean dot product). Non-identity SPDs live under a different inner product, so discrete_geodesic falls back to finite differences for those cases.

inline Euclidean(): Fixed-dimension constructor with default bounds \([-1, 1]^n\).

inline explicit Euclidean(MetricT metric)

Fixed-dimension constructor with custom metric.

Parameters:: metric – The metric policy instance.

inline explicit Euclidean(int n)

Dynamic-dimension constructor.

Parameters:: n – The dimension of the space.

inline Euclidean(int n, MetricT metric)

Dynamic-dimension constructor with custom metric.

Parameters:

n – The dimension of the space.
metric – The metric policy instance.

inline void set_sampling_bounds(const Eigen::VectorXd &lo, const Eigen::VectorXd &hi)

Set the sampling bounds.

Values outside these bounds are never returned by random_point(), but exp/log/metric operations remain unchanged (bounds are a sampler concern, not a topological one).

inline int dim() const: Return the dimension of the space.

inline Point random_point() const

Sample a point uniformly in \([\mathrm{lo}, \mathrm{hi}]^n\) (default \([-1, 1]^n\)).

Uses the configured SamplerT (default: StochasticSampler) to draw uniform box samples and linearly rescales to the sampling bounds. Pass HaltonSampler via the template parameter for deterministic low-discrepancy sampling.

inline Tangent project(const Point&, const Tangent &v) const

Project an ambient vector onto the tangent space at \( p \).

The tangent space of \( \mathbb{R}^n \) is \( \mathbb{R}^n \) everywhere, so the projection is the identity.

template<int Dim = Eigen::Dynamic> using geodex::EuclideanStandardMetric = IdentityMetric<Dim>

The standard Euclidean metric on \( \mathbb{R}^n \).

The inner product is the standard dot product: \( \langle u, v \rangle = u \cdot v \). Zero-storage stateless metric.

Torus

template<int Dim = Eigen::Dynamic, typename MetricT = TorusFlatMetric<Dim>, typename SamplerT = StochasticSampler> class Torus

Flat torus \( T^n \) parameterized by dimension and metric policy.

Points are represented as angles in \( [0, 2\pi)^n \). The exp map wraps to \( [0, 2\pi) \) and the log map wraps differences to \( [-\pi, \pi) \).

Template Parameters:

Dim – Compile-time dimension, or Eigen::Dynamic.
MetricT – Metric policy (default: TorusFlatMetric).
SamplerT – Sampler policy for random_point() (default: StochasticSampler).

Metric delegates

inline Scalar inner(const Point &p, const Tangent &u, const Tangent &v) const: Riemannian inner product at \( p \).

inline Scalar norm(const Point &p, const Tangent &v) const: Riemannian norm at \( p \).

inline Eigen::MatrixXd inner_matrix(const Point &p, const Eigen::MatrixXd &U, const Eigen::MatrixXd &V) const: Batched inner product \(U^\top M(p)\, V\) when the metric provides it.

Exp / Log

inline Point exp(const Point &p, const Tangent &v) const

Exponential map: \( \exp_p(v) = \mathrm{wrap}(p + v) \).

Parameters:

p – Base point.
v – Tangent vector.

Returns:

The resulting point, wrapped to \( [0, 2\pi)^n \).

inline Tangent log(const Point &p, const Point &q) const

Logarithmic map: shortest-path tangent vector from \( p \) to \( q \).

Parameters:

p – Base point.
q – Target point.

Returns:

The wrapped difference in \( [-\pi, \pi)^n \).

Derived operations

inline Scalar distance(const Point &p, const Point &q) const

Geodesic distance via the midpoint approximation.

Parameters:

p – First point.
q – Second point.

Returns:

The distance \( d(p, q) \).

inline Scalar injectivity_radius() const

Injectivity radius of \( T^n \): \( \pi \) (half the period).

Returns the topological value for the default identity metric and period \( 2\pi \). For anisotropic custom metrics the effective radius is \( \pi / \sqrt{\lambda_{\max}(A)} \). This value is an upper bound; discrete_geodesic may take extra retries if the true radius is smaller.

inline Point geodesic(const Point &p, const Point &q, Scalar t) const

Geodesic interpolation between \( p \) and \( q \) at parameter \( t \).

Parameters:

p – Start point.
q – End point.
t – Interpolation parameter in \( [0, 1] \).

Returns:

The interpolated point, wrapped to \( [0, 2\pi)^n \).

Public Types

using Scalar = double: Scalar type.

using Point = Eigen::Vector<double, Dim>: Point type (angles in \( [0, 2\pi)^n \)).

using Tangent = Eigen::Vector<double, Dim>: Tangent vector type.

Public Functions

inline bool has_riemannian_log_runtime() const

Runtime query: is log the Riemannian logarithm of the metric?

Torus topology has trivial exp/log (addition/wrapping), which is the Riemannian log exactly when the metric is the identity (standard flat metric). Anisotropic SPDs are still flat in the mathematical sense but their geodesics are reparameterized, so we mark them as not log-compatible and discrete_geodesic uses finite differences.

inline Torus(): Fixed-dimension constructor.

inline explicit Torus(MetricT metric)

Fixed-dimension constructor with custom metric.

Parameters:: metric – The metric policy instance.

inline explicit Torus(int n)

Dynamic-dimension constructor.

Parameters:: n – The dimension of the torus.

inline Torus(int n, MetricT metric)

Dynamic-dimension constructor with custom metric.

Parameters:

n – The dimension of the torus.
metric – The metric policy instance.

inline int dim() const: Return the dimension of the torus.

inline Point random_point() const

Sample a uniformly random point in \( [0, 2\pi)^n \).

Returns:: A random point on the torus.

inline Tangent project(const Point&, const Tangent &v) const

Project an ambient vector onto the tangent space at \( p \).

The tangent space of \( T^n \) is \( \mathbb{R}^n \) everywhere, so the projection is the identity.

template<int Dim = Eigen::Dynamic> using geodex::TorusFlatMetric = IdentityMetric<Dim>

Standard flat metric on \( T^n \).

The inner product is the standard dot product: \( \langle u, v \rangle = u \cdot v \). Zero-storage stateless metric.

SE(2)

template<typename MetricT = SE2LeftInvariantMetric, typename RetractionT = SE2ExponentialMap, typename SamplerT = StochasticSampler> class SE2

The special Euclidean group \( \mathrm{SE}(2) = \mathbb{R}^2 \rtimes \mathrm{SO}(2) \).

Poses are represented as \( (x, y, \theta) \) with \( \theta \in [-\pi, \pi) \). The manifold is parameterized by a metric policy and a retraction policy, following the same design as Sphere and Torus.

Template Parameters:

MetricT – Metric policy (default: SE2LeftInvariantMetric).
RetractionT – Retraction policy (default: SE2ExponentialMap).
SamplerT – Sampler policy for random_point() (default: StochasticSampler).

Metric delegates

inline Scalar inner(const Point &p, const Tangent &u, const Tangent &v) const: Riemannian inner product at \( p \).

inline Scalar norm(const Point &p, const Tangent &v) const: Riemannian norm at \( p \).

inline Eigen::MatrixXd inner_matrix(const Point &p, const Eigen::MatrixXd &U, const Eigen::MatrixXd &V) const: Batched inner product \(U^\top M(p)\, V\) when the metric provides it.

Retraction delegates

inline Point exp(const Point &p, const Tangent &v) const

Exponential map (or retraction) \( \exp_p(v) \).

Parameters:

p – Base pose.
v – Tangent vector.

Returns:

Resulting pose on SE(2).

inline Tangent log(const Point &p, const Point &q) const

Logarithmic map (or inverse retraction) \( \log_p(q) \).

Parameters:

p – Base pose.
q – Target pose.

Returns:

Tangent vector at \( p \) pointing toward \( q \).

Derived operations

inline Scalar distance(const Point &p, const Point &q) const: Geodesic distance \( d(p, q) \) via the midpoint approximation.

inline Point geodesic(const Point &p, const Point &q, Scalar t) const

Geodesic interpolation between \( p \) and \( q \) at parameter \( t \).

Parameters:

p – Start pose.
q – End pose.
t – Interpolation parameter in \( [0, 1] \).

Returns:

The interpolated pose.

Public Types

using Scalar = double: Scalar type.

using Point = Eigen::Vector3d: Pose \( (x, y, \theta) \).

using Tangent = Eigen::Vector3d: Tangent vector \( (v_x, v_y, \omega) \).

Public Functions

inline bool has_riemannian_log_runtime() const

Runtime query: is the currently-configured metric the bi-invariant Lie group metric (unit weights on SE2LeftInvariantMetric paired with the true SE2ExponentialMap)?

Only in this case is the Lie-group log the Riemannian logarithm of the metric, so discrete_geodesic can safely take the log direction as the natural gradient. discrete_geodesic calls this method to activate the fast path on a per-call basis — anisotropic SE2 metrics fall through to finite differences.

Because SE2LeftInvariantMetric::weights_ is a runtime value, this check cannot be made at compile time.

SE2() = default

Default constructor.

Users must call set_sampling_bounds() before using random_point() if the default \([0,10]^2 \times [-\pi,\pi)\) is unsuitable.

inline explicit SE2(MetricT metric)

Construct with explicit metric.

Parameters:: metric – The metric policy instance.

inline SE2(const Eigen::Vector3d &lo, const Eigen::Vector3d &hi)

Construct with sampling bounds.

Parameters:

lo – Lower sampling bounds \((x_\min, y_\min, \theta_\min)\).
hi – Upper sampling bounds \((x_\max, y_\max, \theta_\max)\).

inline SE2(MetricT metric, const Eigen::Vector3d &lo, const Eigen::Vector3d &hi)

Construct with explicit metric and sampling bounds.

Parameters:

metric – The metric policy instance.
lo – Lower sampling bounds \((x_\min, y_\min, \theta_\min)\).
hi – Upper sampling bounds \((x_\max, y_\max, \theta_\max)\).

inline SE2(MetricT metric, RetractionT retraction, const Eigen::Vector3d &lo, const Eigen::Vector3d &hi)

Construct with explicit metric, retraction, and sampling bounds.

Parameters:

metric – The metric policy instance.
retraction – The retraction policy instance.
lo – Lower sampling bounds \((x_\min, y_\min, \theta_\min)\).
hi – Upper sampling bounds \((x_\max, y_\max, \theta_\max)\).

inline void set_sampling_bounds(const Eigen::Vector3d &lo, const Eigen::Vector3d &hi): Set the sampling bounds.

inline int dim() const: Return the intrinsic dimension (always 3).

inline Point random_point() const

Sample a random pose uniformly in the sampling bounds.

Returns:: A random pose \( (x, y, \theta) \).

inline Tangent project(const Point&, const Tangent &v) const

Project an ambient vector onto the tangent space at \( p \).

The tangent space of SE(2) is \( \mathbb{R}^3 \) (the Lie algebra \( \mathfrak{se}(2) \)), so the projection is the identity.

Configuration Space

template<typename BaseManifoldT, typename MetricT> class ConfigurationSpace

A configuration space that combines a base manifold’s topology with a custom Riemannian metric.

Only topology operations (exp, log, random_point, dim) are delegated to the base manifold. All geometry (inner, norm, distance) comes from MetricT. The base manifold’s own metric is never called by this class — it exists only because the base must be a complete RiemannianManifold (which requires exp/log).

Template Parameters:

BaseManifoldT – The base manifold type (must provide exp, log, dim, random_point).
MetricT – The metric policy type (must provide inner and norm).

Topology — delegated to base manifold

inline int dim() const: Return the intrinsic dimension.

inline Point random_point() const: Sample a random point from the base manifold.

inline Point exp(const Point &p, const Tangent &v) const: Exponential map (or retraction) from the base manifold.

inline Tangent log(const Point &p, const Point &q) const: Logarithmic map (or inverse retraction) from the base manifold.

inline Tangent project(const Point &p, const Tangent &v) const

Project an ambient vector onto the tangent space at \( p \).

Delegates to the base manifold’s projection.

Geometry — delegated to custom metric

inline Scalar inner(const Point &p, const Tangent &u, const Tangent &v) const: Riemannian inner product from the custom metric.

inline Scalar norm(const Point &p, const Tangent &v) const: Riemannian norm from the custom metric.

inline Eigen::MatrixXd inner_matrix(const Point &p, const Eigen::MatrixXd &U, const Eigen::MatrixXd &V) const

Batched inner product \(U^\top M(p)\, V\) when the custom metric provides it.

Forwards to the metric’s inner_matrix. This is the main performance hook for kinetic-energy configuration spaces, where the expensive mass matrix evaluation is amortized over all \(d^2\) entries of the tangent-metric tensor in a single call.

Derived operations

inline Scalar distance(const Point &p, const Point &q) const: Geodesic distance via the midpoint approximation.

inline Scalar injectivity_radius() const: Injectivity radius — forwarded from the metric if available.

inline Point geodesic(const Point &p, const Point &q, Scalar t) const: Geodesic interpolation between two points.

Public Types

using Scalar = typename BaseManifoldT::Scalar: Scalar type from the base manifold.

using Point = typename BaseManifoldT::Point: Point type from the base manifold.

using Tangent = typename BaseManifoldT::Tangent: Tangent vector type from the base manifold.

Public Functions

inline bool has_riemannian_log_runtime() const

Runtime query: is log the Riemannian logarithm of the custom metric?

Always returns false. The whole purpose of ConfigurationSpace is to overlay a custom metric on a base manifold — the base’s log is the Riemannian log of the base’s native metric, not of the custom metric. This forces discrete_geodesic to use the finite-difference natural gradient, which correctly follows the energy-minimizing curve under the custom metric.

Warning

Setting InterpolationSettings::force_log_direction = true on a ConfigurationSpace bypasses this and uses the base manifold’s log for direction. The resulting path follows the base metric’s geodesic, not the custom metric’s. Only use when the base log is a reasonable approximation.

inline ConfigurationSpace(BaseManifoldT base, MetricT metric)

Construct with a base manifold and a metric.

Parameters:

base – The base manifold instance.
metric – The metric policy instance.

inline const BaseManifoldT &base() const: Access the base manifold.

inline const MetricT &metric() const: Access the metric.

Metrics

template<int Dim = Eigen::Dynamic> class IdentityMetric

Stateless identity metric: \( \langle u, v \rangle_p = u \cdot v \).

A zero-storage alternative to ConstantSPDMetric<Dim> with the identity weight matrix. Uses O(1) memory instead of O(n^2) for the identity-metric case. All manifold default-metric aliases (SphereRoundMetric, TorusFlatMetric, EuclideanStandardMetric) point here.

Template Parameters:: Dim – Compile-time vector dimension, or Eigen::Dynamic.

Public Functions

inline double inner(const Eigen::Vector<double, Dim>&, const Eigen::Vector<double, Dim> &u, const Eigen::Vector<double, Dim> &v) const

Compute the inner product \( \langle u, v \rangle = u \cdot v \).

Parameters:

u – First tangent vector.
v – Second tangent vector.

Returns:

The inner product value.

inline double norm(const Eigen::Vector<double, Dim> &p, const Eigen::Vector<double, Dim> &v) const

Compute the norm \( \|v\| = \sqrt{v \cdot v} \).

Parameters:

p – Base point (unused).
v – Tangent vector.

Returns:

The norm value.

inline Eigen::MatrixXd inner_matrix(const Eigen::Vector<double, Dim>&, const Eigen::MatrixXd &U, const Eigen::MatrixXd &V) const: Batched inner product: \( U^\top V \).

inline Eigen::MatrixXd weight_matrix() const

Identity weight matrix (runtime-sized identity, for API compatibility).

Returns a dynamically-sized identity matrix of the given dimension. Provided so that has_riemannian_log_runtime() can call weight_matrix() uniformly on any metric. Unlike ConstantSPDMetric, this allocates only when called — which should be rare.

template<int Dim = Eigen::Dynamic> class ConstantSPDMetric

Point-independent Riemannian metric defined by a constant SPD matrix.

The inner product is \( \langle u, v \rangle_p = u^\top A \, v \) where \( A \) is a symmetric positive-definite matrix. The metric does not depend on the base point \( p \).

This is a manifold-agnostic metric: the same class works for spheres, Euclidean spaces, tori, or any manifold whose tangent vectors are Eigen column vectors.

Template Parameters:: Dim – Compile-time dimension, or Eigen::Dynamic.

Public Functions

inline ConstantSPDMetric()

Default: identity weight matrix (only for static Dim).

The default metric is the standard ambient inner product, which is the “natural” choice for every built-in manifold (Sphere/Euclidean/Torus). For Dim == Eigen::Dynamic the size is unknown at compile time, so use the int n constructor below instead.

inline explicit ConstantSPDMetric(int n)

Dynamic-size identity factory (size n×n).

Parameters:: n – Dimension of the SPD matrix.

inline explicit ConstantSPDMetric(const Eigen::Matrix<double, Dim, Dim> &A)

Construct with a given SPD weight matrix.

Parameters:: A – Symmetric positive-definite matrix defining the metric.

inline double inner(const Eigen::Vector<double, Dim>&, const Eigen::Vector<double, Dim> &u, const Eigen::Vector<double, Dim> &v) const

Compute the inner product \( \langle u, v \rangle = u^\top A \, v \).

Parameters:

u – First tangent vector.
v – Second tangent vector.

Returns:

The inner product value.

inline double norm(const Eigen::Vector<double, Dim> &p, const Eigen::Vector<double, Dim> &v) const

Compute the norm \( \|v\| = \sqrt{v^\top A \, v} \).

Parameters:

p – Base point.
v – Tangent vector.

Returns:

The norm value.

inline Eigen::MatrixXd inner_matrix(const Eigen::Vector<double, Dim>&, const Eigen::MatrixXd &U, const Eigen::MatrixXd &V) const

Batched inner product: \(U^\top A\, V\) in a single matrix multiply.

Provides the HasBatchInnerMatrix fast path for algorithms that evaluate a tangent-metric tensor in a basis (e.g., natural_gradient_fd). For ConstantSPDMetric this is a simple linear-algebra shortcut; the bigger win is for point-dependent metrics like KineticEnergyMetric where the expensive mass matrix is evaluated once instead of \(d^2\) times.

inline const Eigen::Matrix<double, Dim, Dim> &weight_matrix() const: Access the weight matrix.

class SE2LeftInvariantMetric

Left-invariant metric on SE(2).

The inner product is constant (left-invariant): \( \langle u, v \rangle = w_x u_x v_x + w_y u_y v_y + w_\theta u_\theta v_\theta \). The weights \( (w_x, w_y, w_\theta) \) allow anisotropic cost, e.g. penalizing lateral motion for car-like robots.

Implementation: this is ConstantSPDMetric<3> with A = diag(w_x, w_y, w_\theta). The weights_ field is preserved alongside the base metric so that SE2::has_riemannian_log_runtime() can quickly check unit weights without inspecting the full SPD matrix.

Public Functions

inline SE2LeftInvariantMetric(): Construct with unit weights (isotropic).

inline SE2LeftInvariantMetric(double wx, double wy, double wtheta)

Construct with explicit weights.

Parameters:

wx – Translational weight in x.
wy – Translational weight in y.
wtheta – Rotational weight.

inline const Eigen::Vector3d &weights() const: Access the diagonal weight vector \( (w_x, w_y, w_\theta) \).

inline double inner(const Eigen::Vector3d &p, const Eigen::Vector3d &u, const Eigen::Vector3d &v) const

Compute the inner product via the wrapped ConstantSPDMetric.

Parameters:

p – Base point (unused for a constant metric).
u – First tangent vector.
v – Second tangent vector.

Returns:

The inner product value.

inline Eigen::MatrixXd inner_matrix(const Eigen::Vector3d &p, const Eigen::MatrixXd &U, const Eigen::MatrixXd &V) const

Batched inner product via the wrapped ConstantSPDMetric.

Parameters:

p – Base point.
U – Matrix whose columns are tangent vectors.
V – Matrix whose columns are tangent vectors.

Returns:

\( U^\top A \, V \).

inline double norm(const Eigen::Vector3d &p, const Eigen::Vector3d &v) const

Compute the norm \( \|v\| = \sqrt{\langle v, v \rangle} \).

Parameters:

p – Base point.
v – Tangent vector.

Returns:

The norm value.

Public Static Functions

static inline SE2LeftInvariantMetric car_like(const double turning_radius, const double lateral_penalty = 100.0)

Create a car-like metric with a specified effective turning radius.

The metric \( g = \mathrm{diag}(w_x, w_y, w_\theta) \) with high \( w_y \) suppresses lateral motion. The effective minimum turning radius of the resulting geodesic is approximately \( r_{\mathrm{eff}} \approx \sqrt{w_\theta / w_x} \).

Note

Soft constraint only: the metric penalizes but does not forbid motions violating the turning radius. Near start/goal the planner may produce in-place rotations.

Parameters:

turning_radius – Desired effective turning radius.
lateral_penalty – Weight for lateral (y) motion — higher values more strongly suppress sideslip (default 100.0).

Returns:

An SE2LeftInvariantMetric configured for car-like behavior.

template<typename MassMatrixFn> class KineticEnergyMetric

Kinetic energy metric where the inner product is defined by a configuration-dependent mass matrix.

The inner product at configuration \( q \) is: \( \langle u, v \rangle_q = u^\top M(q) \, v \) where \( M(q) \) is a symmetric positive-definite mass matrix returned by the user-provided callable.

Template Parameters:: MassMatrixFn – A callable type with signature auto operator()(const Point& q) -> Eigen::MatrixXd (or fixed-size matrix).

Public Functions

inline explicit KineticEnergyMetric(MassMatrixFn fn)

Construct with a mass matrix function.

Parameters:: fn – Callable returning the SPD mass matrix at a given configuration.

template<typename Point, typename Tangent> inline double inner(const Point &q, const Tangent &u, const Tangent &v) const

Compute the inner product \( \langle u, v \rangle_q = u^\top M(q) \, v \).

Parameters:

q – Configuration point.
u – First tangent vector.
v – Second tangent vector.

Returns:

The inner product value.

template<typename Point, typename Tangent> inline double norm(const Point &q, const Tangent &v) const

Compute the norm \( \|v\|_q = \sqrt{v^\top M(q) \, v} \).

Parameters:

q – Configuration point.
v – Tangent vector.

Returns:

The norm value.

template<typename Point> inline Eigen::MatrixXd inner_matrix(const Point &q, const Eigen::MatrixXd &U, const Eigen::MatrixXd &V) const

Batched inner product: \(U^\top M(q)\, V\) computed with a single call to the mass-matrix function.

This is the performance-critical path for natural_gradient_fd when the mass matrix is expensive to compute (e.g., forward kinematics for a manipulator): instead of calling mass_matrix_fn_(q) for every scalar \(G_{ij} = \langle e_i, e_j\rangle_q\), we call it once and form the entire \(d\times d\) tensor in a single matmul.

inline double injectivity_radius() const: Return the injectivity radius \( \infty \) (assumes flat topology).

template<typename MassMatrixFn, typename PotentialFn> class JacobiMetric

Jacobi metric conformally scaling a kinetic energy metric by the available kinetic energy \( H - P(q) \).

The inner product at configuration \( q \) is: \( \langle u, v \rangle_q = 2\,(H - P(q))\, u^\top M(q) \, v \) where \( H \) is the total energy, \( P(q) \) is the potential, and \( M(q) \) is the mass matrix. Geodesics of this metric are the natural motions of the mechanical system (Maupertuis’ principle).

Implementation: this is a thin composition of KineticEnergyMetric (the mass matrix) and WeightedMetric (the configuration-dependent scaling). The inner, inner_matrix, and norm methods forward to the composed metric — no duplicated mass-matrix or potential-evaluation logic.

Template Parameters:

MassMatrixFn – Callable returning the SPD mass matrix at \( q \).
PotentialFn – Callable returning the scalar potential \( P(q) \).

Public Functions

inline JacobiMetric(MassMatrixFn mass_fn, PotentialFn pot_fn, double H)

Construct a Jacobi metric.

Parameters:

mass_fn – Callable returning the SPD mass matrix.
pot_fn – Callable returning the potential energy.
H – Total energy (must satisfy \( H > P(q) \) everywhere on the path).

template<typename Point, typename Tangent> inline double inner(const Point &q, const Tangent &u, const Tangent &v) const

Compute the inner product \( 2(H - P(q))\, u^\top M(q) \, v \).

Parameters:

q – Configuration point.
u – First tangent vector.
v – Second tangent vector.

Returns:

The inner product value.

template<typename Point, typename Tangent> inline double norm(const Point &q, const Tangent &v) const

Compute the norm \( \|v\|_q = \sqrt{2(H - P(q))\, v^\top M(q) \, v} \).

Parameters:

q – Configuration point.
v – Tangent vector.

Returns:

The norm value.

template<typename Point> inline Eigen::MatrixXd inner_matrix(const Point &q, const Eigen::MatrixXd &U, const Eigen::MatrixXd &V) const: Batched inner product: \(U^\top \bigl(2(H - P(q)) M(q)\bigr) V\) computed with a single evaluation of \(M(q)\) and \(P(q)\) through the wrapped WeightedMetric.

inline double total_energy() const: Return the total energy \( H \) set at construction.

template<typename JacobianFn, typename TaskMetricFn> class PullbackMetric

Pullback metric that maps a task-space metric to configuration space via the Jacobian.

The inner product at configuration \( q \) is: \( \langle u, v \rangle_q = u^\top J(q)^\top G_X(q) \, J(q) \, v + \lambda \, u^\top v \) where \( J(q) \) is the task-space Jacobian, \( G_X(q) \) is the task-space metric (SPD), and \( \lambda \) is an optional regularization parameter ensuring positive-definiteness even at singularities.

Template Parameters:

JacobianFn – Callable returning the Jacobian matrix at \( q \).
TaskMetricFn – Callable returning the task-space SPD metric at \( q \).

Public Functions

inline PullbackMetric(JacobianFn jac_fn, TaskMetricFn task_fn, double lambda = 0.0)

Construct a pullback metric.

Parameters:

jac_fn – Callable returning the Jacobian.
task_fn – Callable returning the task-space metric.
lambda – Regularization (default 0).

template<typename Point, typename Tangent> inline double inner(const Point &q, const Tangent &u, const Tangent &v) const

Compute the inner product.

Parameters:

q – Configuration point.
u – First tangent vector.
v – Second tangent vector.

Returns:

The inner product value.

template<typename Point, typename Tangent> inline double norm(const Point &q, const Tangent &v) const

Compute the norm.

Parameters:

q – Configuration point.
v – Tangent vector.

Returns:

The norm value.

template<typename Point> inline Eigen::MatrixXd inner_matrix(const Point &q, const Eigen::MatrixXd &U, const Eigen::MatrixXd &V) const: Batched inner product: \(U^\top (J^\top G J + \lambda I)\, V\) with a single pair of Jacobian / task-metric evaluations.

inline double lambda() const: Access the regularization parameter.

template<typename MetricT, typename AlphaT = double> class WeightedMetric

Metric wrapper that scales another metric by a constant or configuration-dependent factor \(\alpha\).

The inner product is: \( \langle u, v \rangle_q = \alpha(q) \cdot \langle u, v \rangle^{\mathrm{base}}_q \)

The alpha parameter can be either:

a constant double (e.g. WeightedMetric{base, 3.0})
a callable Fn(q) -> double for configuration-dependent scaling (used by JacobiMetric, region-avoiding metrics, etc.).

Template Parameters:

MetricT – The base metric type.
AlphaT – Scaling factor type — either double or a callable.

Public Functions

inline WeightedMetric(MetricT base, AlphaT alpha)

Construct a weighted metric.

Parameters:

base – The base metric to scale.
alpha – Scaling factor — constant double or callable Fn(q) -> double.

template<typename Point, typename Tangent> inline double inner(const Point &q, const Tangent &u, const Tangent &v) const

Compute the scaled inner product \(\alpha(q) \langle u, v \rangle^{\mathrm{base}}_q\).

Parameters:

q – Configuration point.
u – First tangent vector.
v – Second tangent vector.

Returns:

The scaled inner product value.

template<typename Point, typename Tangent> inline double norm(const Point &q, const Tangent &v) const

Compute the scaled norm \(\|v\|_q = \sqrt{\alpha(q) \langle v, v \rangle^{\mathrm{base}}_q}\).

Parameters:

q – Configuration point.
v – Tangent vector.

Returns:

The scaled norm value.

template<typename Point> inline Eigen::MatrixXd inner_matrix(const Point &q, const Eigen::MatrixXd &U, const Eigen::MatrixXd &V) const: Batched inner product: forwards to the base metric’s inner_matrix when available and scales the result by \(\alpha(q)\).

inline double injectivity_radius() const: Forward injectivity radius from the base metric — only valid for constant-scalar \(\alpha\) (a config-dependent alpha breaks the uniform-scaling guarantee).

inline const MetricT &base() const: Access the base metric.

inline const AlphaT &alpha() const: Access the scaling factor.

template<typename BaseMetricT, typename SDFFn> class SDFConformalMetric

Conformal metric that scales a base metric by an obstacle proximity field.

The inner product is: \( \langle u, v \rangle_q = c(q) \cdot \langle u, v \rangle^{\mathrm{base}}_q \)

where the conformal factor is: \( c(q) = 1 + \kappa \exp(-\beta \cdot \mathrm{sdf}(q)) \)

This makes the metric expensive near obstacles (low SDF) and unmodified far away (high SDF), causing geodesics to naturally maintain clearance. With multiple obstacles, geodesics follow channels equidistant from obstacle surfaces.

The SDF callable should return a smooth signed distance field where positive values indicate free space. For circular obstacles, use log-sum-exp smooth-min to avoid gradient discontinuities at Voronoi boundaries. For grid-based distance transforms, use bilinear interpolation.

Template Parameters:

BaseMetricT – The base metric type (e.g., SE2LeftInvariantMetric).
SDFFn – Callable with signature double(const Point&) returning the signed distance to the nearest obstacle surface.

Public Functions

inline SDFConformalMetric(BaseMetricT base, SDFFn sdf, double kappa = 5.0, double beta = 3.0)

Construct an SDF-based conformal metric.

Parameters:

base – The base metric to scale.
sdf – Callable returning signed distance (positive = free space).
kappa – Strength of obstacle repulsion (higher = more clearance).
beta – Falloff rate (higher = tighter to obstacle surfaces).

template<typename Point, typename Tangent> inline double inner(const Point &q, const Tangent &u, const Tangent &v) const: Compute the scaled inner product \( c(q) \langle u, v \rangle^{\mathrm{base}}_q \).

template<typename Point, typename Tangent> inline double norm(const Point &q, const Tangent &v) const: Compute the scaled norm \( \|v\|_q = \sqrt{c(q)} \|v\|^{\mathrm{base}}_q \).

template<typename Point> inline Eigen::MatrixXd inner_matrix(const Point &q, const Eigen::MatrixXd &U, const Eigen::MatrixXd &V) const: Batched inner product scaled by the conformal factor.

inline const BaseMetricT &base() const: Access the base metric.

inline const SDFFn &sdf() const: Access the SDF callable.

inline double kappa() const: Get the strength parameter.

inline double beta() const: Get the falloff rate parameter.

template<typename Point> inline double conformal_factor(const Point &q) const

Evaluate the conformal factor at a configuration.

Parameters:: q – Configuration point.
Returns:: \( c(q) = 1 + \kappa \exp(-\beta \cdot \mathrm{sdf}(q)) \).

Collision

class DistanceGrid

A 2D precomputed distance transform with bilinear interpolation.

Stores obstacle distances at regular grid points. Queried in world coordinates (meters); returns interpolated distance to nearest obstacle. Positive = free space, zero/negative = obstacle.

Public Functions

inline DistanceGrid(const int width, const int height, const double resolution, std::vector<double> data)

Construct from raw data.

Parameters:

width – Grid width in cells.
height – Grid height in cells.
resolution – Meters per cell.
data – Row-major distance values: data[r * width + c].

inline bool load(const std::string &filename)

Load from the geodex distance transform file format.

Format: width height resolution on the first line, followed by height * width double values in row-major order.

inline double distance_at(const double x_m, const double y_m) const: Query distance at world coordinates using bilinear interpolation.

inline void distance_at_batch(const double *x, const double *y, double *out, const int n) const

Batch query: evaluate distance at N world-coordinate points.

On ARM NEON and x86 SSE2, processes 2 points per iteration with vectorized bilinear interpolation math. Grid value gathering remains scalar on both architectures.

inline int width() const: Grid width in cells.

inline int height() const: Grid height in cells.

inline double resolution() const: Cell size in meters.

inline const std::vector<double> &data() const: Raw distance data array.

class GridSDF

SDF callable wrapping a DistanceGrid for use with SDFConformalMetric.

Extracts (x, y) from the configuration point and queries the grid.

Public Functions

inline explicit GridSDF(const DistanceGrid *grid): Construct from a DistanceGrid pointer.

template<typename Point> inline double operator()(const Point &q) const: Evaluate grid-interpolated signed distance at (x, y).

template<typename SDFType> class InflatedSDF

Wraps any SDF and subtracts a constant inflation radius.

Equivalent to Minkowski expansion of all obstacles by inflation. Useful for point-robot queries that need to account for robot radius.

Public Functions

inline InflatedSDF(SDFType sdf, const double inflation): Construct with a base SDF and inflation radius.

template<typename Point> inline double operator()(const Point &q) const: Evaluate inflated signed distance at (x, y).

inline double inflation() const: Get the inflation radius.

inline const SDFType &base() const: Get the underlying base SDF.

class PolygonFootprint

A convex polygon footprint represented as body-frame perimeter samples.

Samples are stored in SoA layout (separate x[] and y[] arrays) padded to an even count for NEON 2-wide processing. The transform() method rotates and translates all samples to world frame using a single sincos call and NEON vectorized rotation.

Note

Only convex polygons are supported. For non-convex shapes, interior sampling would be needed (future work).

Public Functions

inline PolygonFootprint(const std::vector<Eigen::Vector2d> &vertices, const int samples_per_edge)

Construct from polygon vertices with a specified number of samples per edge.

Vertices define a convex polygon in the body frame, centered at the origin. Samples are placed uniformly along each edge, including the start vertex but excluding the end vertex (to avoid duplicates at corners).

inline int sample_count() const: Total number of perimeter samples (padded to even for NEON).

inline int sample_count_raw() const: Actual (unpadded) number of perimeter samples.

inline const double *body_x() const: Body-frame x-coordinates (SoA, padded).

inline const double *body_y() const: Body-frame y-coordinates (SoA, padded).

inline double bounding_radius() const: Bounding radius from origin (for early-out tests).

inline void transform(const double x, const double y, const double theta, double *wx, double *wy) const

Transform body-frame samples to world frame at pose (x, y, theta).

SIMD strategy:

One sincos(theta) call shared across all samples
NEON 2-wide: rotate + translate 2 points per iteration
For a rectangle with samples_per_edge=4: 16 samples → 8 iterations

Parameters:

x – World x-coordinate of the polygon center.
y – World y-coordinate of the polygon center.
theta – Rotation angle (radians).
wx – [out] World-frame x-coordinates (must have capacity >= sample_count()).
wy – [out] World-frame y-coordinates (must have capacity >= sample_count()).

Public Static Functions

static inline PolygonFootprint rectangle(const double half_length, const double half_width, const int samples_per_edge)

Convenience factory for a rectangular footprint.

Parameters:

half_length – Half-extent along the local x-axis.
half_width – Half-extent along the local y-axis.
samples_per_edge – Number of sample points per edge.

class FootprintGridChecker

Polygon-vs-grid collision checker with SIMD acceleration.

Precomputes a polygon footprint as body-frame perimeter samples. At query time, transforms all samples to world frame and batch-queries the distance grid. Returns either a binary collision result or a continuous signed distance field (minimum grid distance across all samples minus safety margin).

Thread-safe: uses thread_local scratch buffers for OMPL’s parallel motion validation.

Public Functions

inline FootprintGridChecker(const DistanceGrid *grid, PolygonFootprint footprint, const double safety_margin = 0.0)

Construct a footprint checker.

Parameters:

grid – Pointer to the distance grid (must outlive this object).
footprint – Polygon footprint with precomputed body-frame samples.
safety_margin – Extra clearance subtracted from distances (default 0).

inline bool is_valid(const Eigen::Vector3d &q) const: Binary collision test. Returns true if the footprint is collision-free.

template<typename Point> inline double operator()(const Point &q) const

SDF: minimum grid distance across all perimeter samples minus safety margin.

Positive = clear, negative = collision. Suitable for use with SDFConformalMetric to bias planning away from obstacles.

inline const DistanceGrid *grid() const: Get the underlying distance grid.

inline const PolygonFootprint &footprint() const: Get the polygon footprint.

inline double safety_margin() const: Get the safety margin.

class CircleSDF

Signed distance to a single circular obstacle.

Returns positive values in free space and negative inside the circle: \( \mathrm{sdf}(q) = \|q_{xy} - c\| - r \)

Public Functions

template<typename Point> inline double operator()(const Point &q) const: Evaluate signed distance from point to circle boundary.

inline double cx() const: X-coordinate of the circle center.

inline double cy() const: Y-coordinate of the circle center.

inline double radius() const: Radius of the circle.

inline double radius_sq() const: Squared radius of the circle.

class CircleSmoothSDF

Log-sum-exp smooth-min SDF over a collection of circular obstacles.

Produces a smooth approximation to the minimum signed distance across all circles. The smoothing parameter beta controls how closely the smooth-min approximates the hard minimum (higher values give a tighter approximation but sharper gradient transitions).

The numerically stable computation is:

\[ \mathrm{sdf}(q) = -\frac{1}{\beta}\left(m + \ln\sum_i \exp(-\beta\,d_i - m)\right), \quad m = \max_i(-\beta\,d_i) \]

Public Functions

inline CircleSmoothSDF(std::vector<CircleSDF> circles, const double beta = 20.0): Construct from a list of circles and smoothing parameter.

template<typename Point> inline double operator()(const Point &q) const: Evaluate smooth signed distance at point (x, y).

template<typename Point> inline bool is_free(const Point &q) const: Check if a point is outside all circles (binary collision test).

inline const std::vector<CircleSDF> &circles() const: Get the underlying circle obstacles.

inline double beta() const: Get the log-sum-exp smoothing parameter.

struct RectObstacle: An oriented rectangle obstacle defined by center, rotation, and half-extents.

class RectSmoothSDF

Fused SDF + log-sum-exp for oriented rectangles with SIMD acceleration.

SIMD strategy (ARM NEON, 2-wide float64):

SoA layout: cache-line-friendly, NEON-loadable
2-wide processing: 2 obstacles per iteration, branchless via vbslq
Bounding sphere early-out: skip obstacles where center distance exceeds diagonal + skip_dist (where exp(-beta*d) ~ 0)
Inflation applied inside the SIMD loop (no separate wrapper needed)
fast_exp_neon for the log-sum-exp accumulation

Scalar fallback provided for platforms without SIMD. x86 SSE2 path uses 2-wide float64 processing with optional SSE4.1/FMA.

Public Functions

inline RectSmoothSDF(std::vector<RectObstacle> obstacles, const double beta = 20.0, const double inflation = 0.0): Construct from obstacles with smoothing and optional inflation.

template<typename Point> inline double operator()(const Point &q) const: Evaluate smooth signed distance at point (x, y).

inline const std::vector<RectObstacle> &obstacles() const: Get the rectangle obstacles.

inline double beta() const: Get the log-sum-exp smoothing parameter.

inline double inflation() const: Get the inflation radius.

inline bool geodex::collision::rects_overlap(const RectObstacle &a, const RectObstacle &b)

Separating Axis Theorem (SAT) overlap test for two oriented rectangles.

Returns:: true if the rectangles overlap (collide).

Sampling

class StochasticSampler

Pseudo-random sampler wrapping std::mt19937.

Default construction uses a shared thread_local Mersenne-Twister generator, preserving the pre-refactor per-manifold random_point() semantics and zero-cost instantiation. Passing an explicit seed creates an owned generator with a reproducible sequence — useful for tests and benchmarks.

Public Functions

StochasticSampler() = default: Default: share a thread-local generator.

inline explicit StochasticSampler(std::uint64_t seed): Seeded: own a generator for reproducible sequences.

inline void seed(std::uint64_t s): Reseed the sampler; transitions to an owned generator.

inline void sample_box(const int d, Eigen::Ref<Eigen::VectorXd> out): Fill out[0..d-1] with uniform values in \([0, 1)\).

class HaltonSampler

Halton low-discrepancy sampler (deterministic quasi-random).

Produces a fully reproducible quasi-random sequence by advancing an internal 1-based index. Coordinates are computed per dimension via detail::van_der_corput with the first d primes. Maximum supported dimension is detail::halton_primes.size() (30).

Public Functions

HaltonSampler() = default: Default: start the sequence at index 1.

inline explicit HaltonSampler(std::uint64_t start_index): Start the sequence at an explicit index.

inline void seed(std::uint64_t s): Reset the sequence index.

inline void sample_box(const int d, Eigen::Ref<Eigen::VectorXd> out): Fill out[0..d-1] with the next Halton sample.

template<typename S> concept Sampler: #include <sampler.hpp>

Concept: a type that fills a length-d vector with uniform samples in \([0, 1)\).

template<typename S> concept SeedableSampler: #include <sampler.hpp>

Concept: a Sampler that also supports explicit reseeding.

Heuristics

struct EuclideanHeuristic

Euclidean (L2) heuristic between coordinate vectors.

Computes the chord distance \( \|a - b\|_2 \) between two points. Admissible for any manifold where geodesic distance >= chord distance.

Public Functions

template<typename PointA, typename PointB> inline auto operator()(const PointA &a, const PointB &b) const -> double

Compute \( \|a - b\|_2 \).

Parameters:

a – First point.
b – Second point.

Returns:

The Euclidean distance.

Algorithms

Warning

doxygenfunction: Unable to resolve function “geodex::distance_midpoint” with arguments None in doxygen xml output for project “geodex” from directory: ../build/docs/doxygen/xml. Potential matches:

- template<RiemannianManifold M> auto distance_midpoint(const M &m, const typename M::Point &a, const typename M::Point &b) -> typename M::Scalar
- template<typename BaseM, typename MetricT> auto distance_midpoint(const ConfigurationSpace<BaseM, MetricT> &m, const Eigen::Vector3d &a, const Eigen::Vector3d &b) -> double
- template<typename MetricT, typename RetractionT, typename SamplerT> auto distance_midpoint(const SE2<MetricT, RetractionT, SamplerT> &m, const Eigen::Vector3d &a, const Eigen::Vector3d &b) -> double

struct InterpolationSettings

Settings for the discrete geodesic walk.

Each iteration takes a Riemannian step of length \(\min(\texttt{step\_size}, \text{remaining distance})\) in the descent direction. Iteration count and returned-path size therefore scale as approximately \(\text{initial\_distance} / \texttt{step\_size}\), so step_size also functions as the effective path resolution (the maximum Riemannian distance between consecutive returned points).

Fast path: the algorithm first tries the Riemannian logarithm as the descent direction, exploiting the identity \(\nabla_g(\tfrac{1}{2}\, d_g^2(\cdot, q))(x) = -\log_x^g(q)\) which holds on any Riemannian manifold when x is strictly inside q’s injectivity radius and log is the Riemannian logarithm of the metric in use. A progress check on each step verifies that the proposed point actually decreased the distance to target. When the check fails (e.g., the retraction is not the true exponential map, or the metric differs from the one implied by the retraction), the algorithm falls back for that step only to a central finite-difference natural gradient computed from the manifold’s inner product.

Public Members

double step_size = 0.5: Max Riemannian step per iteration; also the effective path resolution.

double convergence_tol = 1e-4: Absolute stop threshold on \(|\log(\text{current}, \text{target})|_R\).

double convergence_rel = 1e-3: Relative stop threshold: also stop when distance drops below convergence_rel * initial_distance.

int max_steps = 100: Maximum number of successful gradient-descent steps.

double fd_epsilon = 0.0

Central finite-difference step for the fallback gradient.

Set to 0 to auto-select as max(1e-8, 1e-5 * max(1, initial_distance)).

double distortion_ratio = 1.5

Max ratio \(|\log(\text{current}, \text{proposed})|_R / \text{step\_used}\) before the retraction is considered to have over-shot.

If violated, the step cap is halved and the iteration retries. This guards against retractions that blow up under curvature and is usually 1.5 (modest slack over a perfect isometry).

double growth_factor = 1.5

Factor by which the current step cap grows back toward step_size after each successful iteration.

Set to 1.0 to disable growth.

double min_step_size = 1e-12: Floor below which the step cap triggers a StepShrunkToZero failure.

double gradient_eps = 1e-12: Riemannian-norm threshold below which the gradient is considered vanished (triggers GradientVanished).

double cut_locus_eps = 1e-10: \(|\log|_R\) threshold that, combined with a nonzero ambient gap, flags a cut-locus situation (e.g., antipodal points on the sphere where log returns zero).

bool force_log_direction = false

Force the log-based direction even when is_riemannian_log() is false.

When true, the algorithm always uses log(current, target) as the descent direction and never falls back to the FD natural gradient. The metric’s norm and distance still control step sizing and convergence. This produces smoother paths (no FD oscillation) but the path follows the base retraction’s geodesic rather than the true Riemannian geodesic of the configured metric.

double fd_midpoint_guard_tau = 0.25

Relative-error threshold above which the midpoint distance surrogate used by the FD gradient is deemed unreliable and falls back to |log(a,b)|_R.

natural_gradient_fd samples \(d^2(p \pm h\,e_i, q)\) via a third-order-accurate midpoint formula (see distance_midpoint). For a true Riemannian midpoint we have \(\log_m(a) = -\log_m(b)\), so the quantity \(\|v_{ma} + v_{mb}\|_m / \|v_{mb} - v_{ma}\|_m\) is zero. When this ratio exceeds tau, the midpoint is considered unreliable (non-Riemannian retraction, cut locus, or non-smoothness between the samples) and the FD sample falls back to |log|_R for the entire basis direction. The count of fallbacks is reported on InterpolationResult::fd_midpoint_fallbacks. Set to 0.0 to force via-log for every sample. Default 0.25.

enum class geodex::InterpolationStatus

Termination status for the discrete geodesic walk.

Values:

enumerator Converged: Distance to target fell below convergence tolerance.

enumerator MaxStepsReached: Iteration budget exhausted without reaching tolerance.

enumerator GradientVanished: Riemannian gradient norm is ~0 at a non-target point.

enumerator CutLocus

log collapsed to ~0 while start and target are distinct (e.g.

antipodal on a sphere).

enumerator StepShrunkToZero: Distortion halving drove step size below min_step_size.

enumerator DegenerateInput: start == target on entry; returned a single-point path immediately.

template<typename PointT> struct InterpolationResult

Output of the discrete geodesic interpolation.

Template Parameters:: PointT – Point type of the manifold (e.g. Eigen::Vector3d).

Public Members

std::vector<PointT> path: Sequence of iterates from start toward target (always starts with start).

InterpolationStatus status = InterpolationStatus::Converged : Termination reason — always check this before using the path for downstream work.

int iterations = 0: Number of successful gradient steps taken (distortion retries do not count).

int distortion_halvings = 0: Number of times the step cap was halved due to distortion / progress failure.

int fd_midpoint_fallbacks = 0

Number of FD basis directions whose midpoint distance surrogate was rejected by the runtime guard and replaced with |log|_R for that sample.

Summed across all iterations of the walk. A nonzero value is a strong signal that the manifold/retraction/metric combination is producing a midpoint that is not the Riemannian midpoint (e.g. projection retraction on an anisotropic metric, SE(2) Euler retraction for large Δθ, cut-locus crossings, or a non-smooth metric feature within ~h of the midpoint).

double initial_distance = 0.0: Riemannian distance from start to target at entry.

double final_distance = 0.0: Riemannian distance from the final iterate to target at exit.

template<RiemannianManifold M> struct InterpolationCache

Reusable scratch buffers for discrete_geodesic.

Passing a cache to discrete_geodesic eliminates per-iteration heap allocations (for fixed-size manifolds) and per-call allocation beyond the first call (for dynamic-size manifolds). Intended for use in hot loops such as steering functions in sampling-based planners:

geodex::InterpolationCache<Sphere<>> cache;
for (auto& edge : edges) {
  auto r = geodex::discrete_geodesic(sphere, edge.a, edge.b, settings, &cache);
  ...
}

Users who do not need this optimization can omit the cache argument and a stack-local one will be used automatically.

Public Types

using Point = typename M::Point: Manifold point type.

using Tangent = typename M::Tangent: Manifold tangent vector type.

Public Functions

inline void reset(int ambient, int d)

Resize all buffers for the given ambient and intrinsic dimensions.

Parameters:

ambient – Ambient-space dimension of a tangent vector (typically Tangent::size()).
d – Intrinsic manifold dimension (manifold.dim()).

Public Members

Tangent v_fd: Scratch FD-path ambient tangent (natural gradient reconstructed in ambient space).

Eigen::Matrix<double, N, Eigen::Dynamic, 0, MaxN, MaxN> basis_mat: Basis matrix (FD path): columns are tangent basis vectors at the current point.

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, 0, MaxN, MaxN> G: Metric tensor \(G_{ij} = \langle e_i, e_j\rangle_p\) in the current basis.

Eigen::Matrix<double, Eigen::Dynamic, 1, 0, MaxN, 1> grad: Coordinate-space gradient of \(\tfrac{1}{2}\, d^2(\cdot, \text{target})\).

Eigen::Matrix<double, Eigen::Dynamic, 1, 0, MaxN, 1> alpha: Natural-gradient coefficients \(\alpha = -G^{-1} g\).

template<RiemannianManifold M> auto geodex::discrete_geodesic(const M &manifold, const typename M::Point &start, const typename M::Point &target, InterpolationSettings settings = {}, InterpolationCache<M> *cache = nullptr) -> InterpolationResult<typename M::Point>

Walk from start toward target via Riemannian natural gradient descent.

The algorithm iteratively descends on \(\varphi(x) = \tfrac{1}{2}\, d^2(x, \text{target})\) with step length capped by settings.step_size per iteration. Each iteration tries the Riemannian logarithm direction first (exploiting \(\nabla\varphi = -\log_x(\text{target})\) on any Riemannian manifold away from the cut locus) and verifies via a progress check. When the log direction does not produce enough distance decrease (e.g., because the retraction is a projection, or the metric is custom), the algorithm falls back for that step to a central finite-difference natural gradient computed from the manifold’s inner product.

The returned InterpolationResult carries the path, a termination status enum, iteration count, and the initial/final distances — allowing callers to distinguish successful convergence from MaxStepsReached, CutLocus, GradientVanished, StepShrunkToZero, and DegenerateInput.

Walk semantics: iteration count and path size both scale as \(\approx \text{initial\_distance} / \texttt{step\_size}\). Reduce step_size for higher path resolution.

Note

See Kyaw, P. T., & Kelly, J. (2026). Geometry-Aware Sampling-Based Motion Planning on Riemannian Manifolds. arXiv:2602.00992. The identity \(\nabla_g(\tfrac{1}{2}\, d_g^2(\cdot, q))(x) = -\log_x^g(q)\) is standard; see Sakai, Riemannian Geometry, §IV.5 and do Carmo, Riemannian Geometry, Ch 13 Prop 3.6.

Template Parameters:

M – A type satisfying RiemannianManifold.

Parameters:

manifold – The manifold instance.
start – Starting point.
target – Target point to walk toward.
settings – Algorithm parameters.
cache – Optional reusable cache. If null, a stack-local one is used.

Returns:

An InterpolationResult carrying the path and termination diagnostics.

template<typename M> concept HasBatchInnerMatrix

#include <metric.hpp>

A manifold that exposes a batched inner-product, computing \(U^\top M(p) V\) in one call.

This is an optional optimization hook for point-dependent metrics where the expensive part is evaluating the metric tensor \(M(p)\) (e.g., forward kinematics for a kinetic-energy metric). Providing inner_matrix allows algorithms that compute a \(d \times d\) metric tensor in a tangent basis (natural_gradient_fd is the canonical consumer) to evaluate \(M(p)\) once instead of \(d^2\) times.

Algorithms that only need pointwise evaluation use the scalar inner path and do not require metrics to provide inner_matrix.

OMPL Integration

template<typename ManifoldT> class GeodexStateSpace : public ompl::base::StateSpace

OMPL state space adapter for geodex Riemannian manifolds.

Wraps a geodex RiemannianManifold as an ompl::base::StateSpace, delegating distance, interpolation, and sampling to the manifold’s operations. States store ambient-space coordinates as raw double* arrays.

Template Parameters:: ManifoldT – A type satisfying geodex::RiemannianManifold.

Public Types

using Point = typename ManifoldT::Point: Manifold point type.

using Tangent = typename ManifoldT::Tangent: Manifold tangent type.

using StateType = GeodexState<ManifoldT>: OMPL state type.

Public Functions

inline GeodexStateSpace(ManifoldT manifold, ob::RealVectorBounds bounds)

Construct a state space from a manifold and bounds.

Parameters:

manifold – The geodex manifold instance.
bounds – Axis-aligned bounds for the ambient coordinates.

inline const ManifoldT &getManifold() const: Access the underlying geodex manifold.

inline const ob::RealVectorBounds &getBounds() const: Access the coordinate bounds.

inline void setCollisionResolution(double resolution)

Set the minimum collision checking resolution in coordinate distance.

When set to a positive value, validSegmentCount() ensures at least ceil(coord_distance / resolution) checks along each edge, independent of OMPL’s longestValidSegmentFraction. This prevents thin walls from being missed when getMaximumExtent() is large.

Parameters:: resolution – Minimum distance (meters) between collision checks. Use 0.0 to disable (OMPL default only).

inline double getCollisionResolution() const: Get the collision checking resolution.

inline void setInterpolationMode(const InterpolationMode mode)

Set the interpolation strategy.

Parameters:: mode – The desired interpolation mode.

inline InterpolationMode getInterpolationMode() const: Get the current interpolation mode.

inline void setInterpolationSettings(const InterpolationSettings &settings)

Set the interpolation settings for discrete geodesic computation.

Parameters:: settings – The settings (step_size, convergence_tol, max_steps, etc.).

inline const InterpolationSettings &getInterpolationSettings() const: Get the current interpolation settings.

inline void setGeodesicStepSize(const double step_size)

Convenience: set the step size for discrete geodesic interpolation.

Controls the maximum Riemannian distance between consecutive waypoints in the cached geodesic path. Smaller values increase resolution (and computation cost). Default is 0.5.

Parameters:: step_size – Maximum step size per iteration.

inline unsigned int getDimension() const override: Return the ambient dimension of the state space.

inline double getMaximumExtent() const override

Return the maximum extent of the state space.

Todo:: Validate this fix properly. Current approach (max of coordinate diagonal and Riemannian corner-to-corner distance) fixes the connect-loop hang with anisotropic metrics, but the corner-to-corner Riemannian distance may not be the true maximum extent (angle wrapping, non-diagonal pairs, configuration-dependent metrics). Needs formal analysis and tests across different manifold types.

inline double getMeasure() const override: Return the volume of the bounding box.

inline void enforceBounds(ob::State *state) const override: Clamp state coordinates to the bounds.

inline bool satisfiesBounds(const ob::State *state) const override: Check whether all coordinates satisfy the bounds.

inline void copyState(ob::State *destination, const ob::State *source) const override: Copy state coordinates.

inline double distance(const ob::State *state1, const ob::State *state2) const override: Compute the geodesic distance between two states.

inline bool equalStates(const ob::State *state1, const ob::State *state2) const override: Check whether two states are equal (within tolerance).

inline void interpolate(const ob::State *from, const ob::State *to, double t, ob::State *state) const override

Geodesic interpolation between two states.

For manifolds where is_riemannian_log() returns true (identity metric with matching retraction), uses the direct geodesic(p, q, t) — zero overhead. For non-flat metrics, computes a discrete geodesic via Riemannian natural gradient descent, caches the path, and serves subsequent lookups via arc-length parameterization. The cache is keyed on the (from, to) state pair; sequential calls with the same pair (as in DiscreteMotionValidator::checkMotion) amortize the computation.

Parameters:

from – Start state.
to – End state.
t – Interpolation parameter in [0, 1].
state – Output state.

inline ob::StateSamplerPtr allocDefaultStateSampler() const override: Allocate the default state sampler.

inline ob::State *allocState() const override: Allocate a new state.

inline void freeState(ob::State *state) const override: Free a state.

inline double *getValueAddressAtIndex(ob::State *state, unsigned int index) const override: Get a pointer to the coordinate at the given index.

inline void copyToReals(std::vector<double> &reals, const ob::State *source) const override: Copy state coordinates to a vector of doubles.

inline void copyFromReals(ob::State *destination, const std::vector<double> &reals) const override: Set state coordinates from a vector of doubles.

inline const GeodesicPathCache<ManifoldT> &getGeodesicCache() const

Read-only access to the internal cached discrete-geodesic path.

Exposed so GeodexOptimizationObjective can compute the integrated arc cost for the last interpolate() pair without recomputing. The cache holds at most one (s1, s2) pair at a time; callers must check matches() before using it.

inline void ensureGeodesicCached(const Point &from, const Point &to) const

Populate the cache for the given endpoint pair (or no-op on hit).

Used by the optimization objective when it needs the arc cost for a pair that is not yet cached. Uses the state space’s configured InterpolationSettings.

inline unsigned int validSegmentCount(const ob::State *s1, const ob::State *s2) const override

Number of collision checks for a motion between two states.

Returns the maximum of OMPL’s default segment count and a coordinate-distance-based count derived from collision_resolution_.

template<typename ManifoldT> class GeodexState : public ompl::base::State

State type for GeodexStateSpace, storing ambient-space coordinates.

Wraps a raw double* array with Eigen map accessors for convenient conversion between OMPL states and geodex manifold points.

Template Parameters:: ManifoldT – The geodex manifold type.

Public Functions

inline auto asEigen() const: Read-only Eigen map of the state coordinates.

inline auto asEigen(): Mutable Eigen map of the state coordinates.

Public Members

double *values = nullptr: Raw coordinate array (owned by the state space).

template<typename ManifoldT> class GeodexStateSampler : public ompl::base::StateSampler

State sampler for GeodexStateSpace.

Provides uniform, near-uniform, and Gaussian sampling on the manifold by sampling tangent vectors and applying the exponential map.

Template Parameters:: ManifoldT – The geodex manifold type.

Public Functions

inline explicit GeodexStateSampler(const ob::StateSpace *space)

Construct a sampler for the given state space.

Parameters:: space – The GeodexStateSpace to sample from.

inline void sampleUniform(ob::State *state) override: Sample a state uniformly within the bounds.

inline void sampleUniformNear(ob::State *state, const ob::State *near, double distance) override

Sample a state uniformly near another state at a given distance.

Parameters:

state – Output state.
near – Reference state to sample around.
distance – Desired geodesic distance from near.

inline void sampleGaussian(ob::State *state, const ob::State *mean, double stdDev) override

Sample a state from a Gaussian distribution centered at mean.

Parameters:

state – Output state.
mean – Center of the Gaussian.
stdDev – Standard deviation in the tangent space.